Squashed 'third_party/pico-sdk/' content from commit 2062372d2
Change-Id: Ic20f199d3ed0ea8d3a6a1bbf513f875ec7500cc6
git-subtree-dir: third_party/pico-sdk
git-subtree-split: 2062372d203b372849d573f252cf7c6dc2800c0a
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/src/rp2_common/CMakeLists.txt b/src/rp2_common/CMakeLists.txt
new file mode 100644
index 0000000..4ca55be
--- /dev/null
+++ b/src/rp2_common/CMakeLists.txt
@@ -0,0 +1,77 @@
+option(PICO_NO_FLASH "Default binaries to not not use flash")
+option(PICO_COPY_TO_RAM "Default binaries to Copy code to RAM when booting from flash")
+
+set(CMAKE_EXECUTABLE_SUFFIX .elf)
+
+pico_add_subdirectory(hardware_base)
+pico_add_subdirectory(hardware_claim)
+# HAL items which expose a public (inline) functions/macro API above the raw hardware
+pico_add_subdirectory(hardware_adc)
+pico_add_subdirectory(hardware_clocks)
+pico_add_subdirectory(hardware_dma)
+pico_add_subdirectory(hardware_divider)
+pico_add_subdirectory(hardware_exception)
+pico_add_subdirectory(hardware_flash)
+pico_add_subdirectory(hardware_gpio)
+pico_add_subdirectory(hardware_i2c)
+pico_add_subdirectory(hardware_interp)
+pico_add_subdirectory(hardware_irq)
+pico_add_subdirectory(hardware_pio)
+pico_add_subdirectory(hardware_pll)
+pico_add_subdirectory(hardware_pwm)
+pico_add_subdirectory(hardware_resets)
+pico_add_subdirectory(hardware_rtc)
+pico_add_subdirectory(hardware_spi)
+pico_add_subdirectory(hardware_sync)
+pico_add_subdirectory(hardware_timer)
+pico_add_subdirectory(hardware_uart)
+pico_add_subdirectory(hardware_vreg)
+pico_add_subdirectory(hardware_watchdog)
+pico_add_subdirectory(hardware_xosc)
+
+# Helper functions to connect to data/functions in the bootrom
+pico_add_subdirectory(pico_bootrom)
+pico_add_subdirectory(pico_platform)
+
+if (NOT PICO_BARE_METAL)
+ # NOTE THE ORDERING HERE IS IMPORTANT AS SOME TARGETS CHECK ON EXISTENCE OF OTHER TARGETS
+ pico_add_subdirectory(boot_stage2)
+
+ pico_add_subdirectory(pico_bootsel_via_double_reset)
+ pico_add_subdirectory(pico_multicore)
+ pico_add_subdirectory(pico_unique_id)
+
+ pico_add_subdirectory(pico_bit_ops)
+ pico_add_subdirectory(pico_divider)
+ pico_add_subdirectory(pico_double)
+ pico_add_subdirectory(pico_int64_ops)
+ pico_add_subdirectory(pico_float)
+ pico_add_subdirectory(pico_mem_ops)
+ pico_add_subdirectory(pico_malloc)
+ pico_add_subdirectory(pico_printf)
+
+ pico_add_subdirectory(pico_stdio)
+ pico_add_subdirectory(pico_stdio_semihosting)
+ pico_add_subdirectory(pico_stdio_uart)
+
+ pico_add_subdirectory(cmsis)
+ pico_add_subdirectory(tinyusb)
+ pico_add_subdirectory(pico_stdio_usb)
+
+ pico_add_subdirectory(pico_stdlib)
+
+ pico_add_subdirectory(pico_cxx_options)
+ pico_add_subdirectory(pico_standard_link)
+
+ pico_add_subdirectory(pico_fix)
+
+ pico_add_subdirectory(pico_runtime)
+
+endif()
+
+set(CMAKE_EXECUTABLE_SUFFIX "${CMAKE_EXECUTABLE_SUFFIX}" PARENT_SCOPE)
+
+pico_add_doxygen(${CMAKE_CURRENT_LIST_DIR})
+pico_add_doxygen_exclude(${CMAKE_CURRENT_LIST_DIR}/cmsis)
+
+pico_promote_common_scope_vars()
\ No newline at end of file
diff --git a/src/rp2_common/README.md b/src/rp2_common/README.md
new file mode 100644
index 0000000..b893801
--- /dev/null
+++ b/src/rp2_common/README.md
@@ -0,0 +1,8 @@
+This directory contains libraries specifically targeting the RP2040 or possible future related devices. It is selected when
+`PICO_PLATFORM=rp2040` (the default) is specified for the build
+
+`hardware_` libraries exist for individual hardware components to provide a simple API
+providing a thin abstraction hiding the details of accessing the hardware registers directly.
+
+`pico_` provides higher level functionality you might generally find in say an OS kernel, as well
+as runtime support familiar to most C programmers.
diff --git a/src/rp2_common/boot_stage2/CMakeLists.txt b/src/rp2_common/boot_stage2/CMakeLists.txt
new file mode 100644
index 0000000..a2f3960
--- /dev/null
+++ b/src/rp2_common/boot_stage2/CMakeLists.txt
@@ -0,0 +1,107 @@
+# PICO_CMAKE_CONFIG: PICO_DEFAULT_BOOT_STAGE2_FILE, Default boot stage 2 file to use unless overridden by pico_set_boot_stage2 on the TARGET; this setting is useful when explicitly setting the default build from a per board CMake file, group=build
+# PICO_CMAKE_CONFIG: PICO_DEFAULT_BOOT_STAGE2, Simpler alternative to specifying PICO_DEFAULT_BOOT_STAGE2_FILE where the file is src/rp2_common/boot_stage2/{PICO_DEFAULT_BOOT_STAGE2}.S, default=compile_time_choice, group=build
+
+if (DEFINED ENV{PICO_DEFAULT_BOOT_STAGE2_FILE})
+ set(PICO_DEFAULT_BOOT_STAGE2_FILE $ENV{PICO_DEFAULT_BOOT_STAGE2_FILE})
+ message("Using PICO_DEFAULT_BOOT_STAGE2_FILE from environment ('${PICO_DEFAULT_BOOT_STAGE2_FILE}')")
+elseif (PICO_DEFAULT_BOOT_STAGE2_FILE)
+ # explicitly set, so cache it
+ set(PICO_DEFAULT_BOOT_STAGE2_FILE "${PICO_DEFAULT_BOOT_STAGE2_FILE}" CACHE STRING "boot stage 2 source file" FORCE)
+endif()
+
+set(PICO_BOOT_STAGE2_COMPILE_TIME_CHOICE_NAME compile_time_choice) # local var
+if (NOT PICO_DEFAULT_BOOT_STAGE2_FILE)
+ if (DEFINED ENV{PICO_DEFAULT_BOOT_STAGE2})
+ set(PICO_DEFAULT_BOOT_STAGE2 $ENV{PICO_DEFAULT_BOOT_STAGE2})
+ message("Using PICO_DEFAULT_BOOT_STAGE2 from environment ('${PICO_DEFAULT_BOOT_STAGE2}')")
+ endif()
+ if (NOT DEFINED PICO_DEFAULT_BOOT_STAGE2)
+ set(PICO_DEFAULT_BOOT_STAGE2 ${PICO_BOOT_STAGE2_COMPILE_TIME_CHOICE_NAME})
+ endif()
+ set(PICO_DEFAULT_BOOT_STAGE2 "${PICO_DEFAULT_BOOT_STAGE2}" CACHE STRING "boot stage 2 short name" FORCE)
+ set(PICO_DEFAULT_BOOT_STAGE2_FILE "${CMAKE_CURRENT_LIST_DIR}/${PICO_DEFAULT_BOOT_STAGE2}.S")
+endif()
+
+if (NOT EXISTS ${PICO_DEFAULT_BOOT_STAGE2_FILE})
+ message(FATAL_ERROR "Specified boot stage 2 source '${PICO_DEFAULT_BOOT_STAGE2_FILE}' does not exist.")
+endif()
+pico_register_common_scope_var(PICO_DEFAULT_BOOT_STAGE2_FILE)
+
+# needed by function below
+set(PICO_BOOT_STAGE2_DIR "${CMAKE_CURRENT_LIST_DIR}" CACHE INTERNAL "")
+
+add_library(boot_stage2_headers INTERFACE)
+target_include_directories(boot_stage2_headers INTERFACE ${CMAKE_CURRENT_LIST_DIR}/include)
+
+# by convention the first source file name without extension is used for the binary info name
+function(pico_define_boot_stage2 NAME SOURCES)
+ add_executable(${NAME}
+ ${SOURCES}
+ )
+
+ # todo bit of an abstraction failure - revisit for Clang support anyway
+ if (CMAKE_C_COMPILER_ID STREQUAL "Clang")
+ target_link_options(${NAME} PRIVATE "-nostdlib")
+ else ()
+ target_link_options(${NAME} PRIVATE "--specs=nosys.specs")
+ target_link_options(${NAME} PRIVATE "-nostartfiles")
+ endif ()
+
+ # boot2_helpers include dir
+ target_include_directories(${NAME} PRIVATE ${PICO_BOOT_STAGE2_DIR}/asminclude)
+
+ target_link_libraries(${NAME} hardware_regs boot_stage2_headers)
+ target_link_options(${NAME} PRIVATE "LINKER:--script=${PICO_BOOT_STAGE2_DIR}/boot_stage2.ld")
+ set_target_properties(${NAME} PROPERTIES LINK_DEPENDS ${PICO_BOOT_STAGE2_DIR}/boot_stage2.ld)
+
+ pico_add_dis_output(${NAME})
+ pico_add_map_output(${NAME})
+
+ set(ORIGINAL_BIN ${CMAKE_CURRENT_BINARY_DIR}/${NAME}.bin)
+ set(PADDED_CHECKSUMMED_ASM ${CMAKE_CURRENT_BINARY_DIR}/${NAME}_padded_checksummed.S)
+
+ find_package (Python3 REQUIRED COMPONENTS Interpreter)
+
+ add_custom_target(${NAME}_bin DEPENDS ${ORIGINAL_BIN})
+ add_custom_command(OUTPUT ${ORIGINAL_BIN} DEPENDS ${NAME} COMMAND ${CMAKE_OBJCOPY} -Obinary $<TARGET_FILE:${NAME}> ${ORIGINAL_BIN})
+
+ add_custom_target(${NAME}_padded_checksummed_asm DEPENDS ${PADDED_CHECKSUMMED_ASM})
+ add_custom_command(OUTPUT ${PADDED_CHECKSUMMED_ASM} DEPENDS ${ORIGINAL_BIN}
+ COMMAND ${Python3_EXECUTABLE} ${PICO_BOOT_STAGE2_DIR}/pad_checksum -s 0xffffffff ${ORIGINAL_BIN} ${PADDED_CHECKSUMMED_ASM}
+ )
+
+ add_library(${NAME}_library INTERFACE)
+ add_dependencies(${NAME}_library ${NAME}_padded_checksummed_asm)
+ # not strictly (or indeed actually) a link library, but this avoids dependency cycle
+ target_link_libraries(${NAME}_library INTERFACE ${PADDED_CHECKSUMMED_ASM})
+ target_link_libraries(${NAME}_library INTERFACE boot_stage2_headers)
+
+ list(GET SOURCES 0 FIRST_SOURCE)
+ get_filename_component(BOOT_STAGE2_BI_NAME ${FIRST_SOURCE} NAME_WE)
+
+ # we only set the PICO_BUILD_STAGE2_NAME if it isn't 'compile_time_choice'
+ if (NOT BOOT_STAGE2_BI_NAME STREQUAL PICO_BOOT_STAGE2_COMPILE_TIME_CHOICE_NAME)
+ target_compile_definitions(${NAME} INTERFACE
+ -DPICO_BUILD_BOOT_STAGE2_NAME="${BOOT_STAGE2_BI_NAME}")
+ target_compile_definitions(${NAME}_library INTERFACE
+ -DPICO_BUILD_BOOT_STAGE2_NAME="${BOOT_STAGE2_BI_NAME}")
+ endif()
+endfunction()
+
+macro(pico_set_boot_stage2 TARGET NAME)
+ get_target_property(target_type ${TARGET} TYPE)
+ if ("EXECUTABLE" STREQUAL "${target_type}")
+ set_target_properties(${TARGET} PROPERTIES PICO_TARGET_BOOT_STAGE2 "${NAME}")
+ else()
+ message(FATAL_ERROR "boot stage 2 implementation must be set on executable not library")
+ endif()
+endmacro()
+
+pico_define_boot_stage2(bs2_default ${PICO_DEFAULT_BOOT_STAGE2_FILE})
+
+# Create a new boot stage 2 target using the default implementation for the current build (PICO_BOARD derived)
+function(pico_clone_default_boot_stage2 NAME)
+ pico_define_boot_stage2(${NAME} ${PICO_DEFAULT_BOOT_STAGE2_FILE})
+endfunction()
+
+pico_promote_common_scope_vars()
\ No newline at end of file
diff --git a/src/rp2_common/boot_stage2/asminclude/boot2_helpers/exit_from_boot2.S b/src/rp2_common/boot_stage2/asminclude/boot2_helpers/exit_from_boot2.S
new file mode 100644
index 0000000..6f06fc1
--- /dev/null
+++ b/src/rp2_common/boot_stage2/asminclude/boot2_helpers/exit_from_boot2.S
@@ -0,0 +1,28 @@
+/*
+ * Copyright (c) 2021 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _BOOT2_HELPER_EXIT_FROM_BOOT2
+#define _BOOT2_HELPER_EXIT_FROM_BOOT2
+
+#include "hardware/regs/m0plus.h"
+
+// If entered from the bootrom, lr (which we earlier pushed) will be 0,
+// and we vector through the table at the start of the main flash image.
+// Any regular function call will have a nonzero value for lr.
+check_return:
+ pop {r0}
+ cmp r0, #0
+ beq vector_into_flash
+ bx r0
+vector_into_flash:
+ ldr r0, =(XIP_BASE + 0x100)
+ ldr r1, =(PPB_BASE + M0PLUS_VTOR_OFFSET)
+ str r0, [r1]
+ ldmia r0, {r0, r1}
+ msr msp, r0
+ bx r1
+
+#endif
diff --git a/src/rp2_common/boot_stage2/asminclude/boot2_helpers/read_flash_sreg.S b/src/rp2_common/boot_stage2/asminclude/boot2_helpers/read_flash_sreg.S
new file mode 100644
index 0000000..83698ed
--- /dev/null
+++ b/src/rp2_common/boot_stage2/asminclude/boot2_helpers/read_flash_sreg.S
@@ -0,0 +1,30 @@
+/*
+ * Copyright (c) 2021 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _BOOT2_HELPER_READ_FLASH_SREG
+#define _BOOT2_HELPER_READ_FLASH_SREG
+
+#include "boot2_helpers/wait_ssi_ready.S"
+
+// Pass status read cmd into r0.
+// Returns status value in r0.
+.global read_flash_sreg
+.type read_flash_sreg,%function
+.thumb_func
+read_flash_sreg:
+ push {r1, lr}
+ str r0, [r3, #SSI_DR0_OFFSET]
+ // Dummy byte:
+ str r0, [r3, #SSI_DR0_OFFSET]
+
+ bl wait_ssi_ready
+ // Discard first byte and combine the next two
+ ldr r0, [r3, #SSI_DR0_OFFSET]
+ ldr r0, [r3, #SSI_DR0_OFFSET]
+
+ pop {r1, pc}
+
+#endif
diff --git a/src/rp2_common/boot_stage2/asminclude/boot2_helpers/wait_ssi_ready.S b/src/rp2_common/boot_stage2/asminclude/boot2_helpers/wait_ssi_ready.S
new file mode 100644
index 0000000..2e49b64
--- /dev/null
+++ b/src/rp2_common/boot_stage2/asminclude/boot2_helpers/wait_ssi_ready.S
@@ -0,0 +1,26 @@
+/*
+ * Copyright (c) 2021 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _BOOT2_HELPER_WAIT_SSI_READY
+#define _BOOT2_HELPER_WAIT_SSI_READY
+
+wait_ssi_ready:
+ push {r0, r1, lr}
+
+ // Command is complete when there is nothing left to send
+ // (TX FIFO empty) and SSI is no longer busy (CSn deasserted)
+1:
+ ldr r1, [r3, #SSI_SR_OFFSET]
+ movs r0, #SSI_SR_TFE_BITS
+ tst r1, r0
+ beq 1b
+ movs r0, #SSI_SR_BUSY_BITS
+ tst r1, r0
+ bne 1b
+
+ pop {r0, r1, pc}
+
+#endif
diff --git a/src/rp2_common/boot_stage2/boot2_at25sf128a.S b/src/rp2_common/boot_stage2/boot2_at25sf128a.S
new file mode 100644
index 0000000..be232ff
--- /dev/null
+++ b/src/rp2_common/boot_stage2/boot2_at25sf128a.S
@@ -0,0 +1,285 @@
+// ----------------------------------------------------------------------------
+// Second stage boot code
+// Copyright (c) 2019-2021 Raspberry Pi (Trading) Ltd.
+// SPDX-License-Identifier: BSD-3-Clause
+//
+// Device: Adesto AT25SF128A
+// Based on W25Q080 code: main difference is the QE bit is being set
+// via command 0x31
+//
+// Description: Configures AT25SF128A to run in Quad I/O continuous read XIP mode
+//
+// Details: * Check status register 2 to determine if QSPI mode is enabled,
+// and perform an SR2 programming cycle if necessary.
+// * Use SSI to perform a dummy 0xEB read command, with the mode
+// continuation bits set, so that the flash will not require
+// 0xEB instruction prefix on subsequent reads.
+// * Configure SSI to write address, mode bits, but no instruction.
+// SSI + flash are now jointly in a state where continuous reads
+// can take place.
+// * Jump to exit pointer passed in via lr. Bootrom passes null,
+// in which case this code uses a default 256 byte flash offset
+//
+// Building: * This code must be position-independent, and use stack only
+// * The code will be padded to a size of 256 bytes, including a
+// 4-byte checksum. Therefore code size cannot exceed 252 bytes.
+// ----------------------------------------------------------------------------
+
+#include "pico/asm_helper.S"
+#include "hardware/regs/addressmap.h"
+#include "hardware/regs/ssi.h"
+#include "hardware/regs/pads_qspi.h"
+
+// ----------------------------------------------------------------------------
+// Config section
+// ----------------------------------------------------------------------------
+// It should be possible to support most flash devices by modifying this section
+
+// The serial flash interface will run at clk_sys/PICO_FLASH_SPI_CLKDIV.
+// This must be a positive, even integer.
+// The bootrom is very conservative with SPI frequency, but here we should be
+// as aggressive as possible.
+
+#ifndef PICO_FLASH_SPI_CLKDIV
+#define PICO_FLASH_SPI_CLKDIV 4
+#endif
+#if PICO_FLASH_SPI_CLKDIV & 1
+#error PICO_FLASH_SPI_CLKDIV must be even
+#endif
+
+// Define interface width: single/dual/quad IO
+#define FRAME_FORMAT SSI_CTRLR0_SPI_FRF_VALUE_QUAD
+
+// For W25Q080 this is the "Read data fast quad IO" instruction:
+#define CMD_READ 0xeb
+
+// "Mode bits" are 8 special bits sent immediately after
+// the address bits in a "Read Data Fast Quad I/O" command sequence.
+// On W25Q080, the four LSBs are don't care, and if MSBs == 0xa, the
+// next read does not require the 0xeb instruction prefix.
+#define MODE_CONTINUOUS_READ 0x20
+
+// The number of address + mode bits, divided by 4 (always 4, not function of
+// interface width).
+#define ADDR_L 8
+
+// How many clocks of Hi-Z following the mode bits. For W25Q080, 4 dummy cycles
+// are required.
+#define WAIT_CYCLES 4
+
+// If defined, we will read status reg, compare to SREG_DATA, and overwrite
+// with our value if the SR doesn't match.
+// We do a two-byte write to SR1 (01h cmd) rather than a one-byte write to
+// SR2 (31h cmd) as the latter command isn't supported by WX25Q080.
+// This isn't great because it will remove block protections.
+// A better solution is to use a volatile SR write if your device supports it.
+#define PROGRAM_STATUS_REG
+
+#define CMD_WRITE_ENABLE 0x06
+#define CMD_READ_STATUS 0x05
+#define CMD_READ_STATUS2 0x35
+#define CMD_WRITE_STATUS 0x01
+#define CMD_WRITE_STATUS2 0x31
+#define SREG_DATA 0x02 // Enable quad-SPI mode
+
+// ----------------------------------------------------------------------------
+// Start of 2nd Stage Boot Code
+// ----------------------------------------------------------------------------
+
+.syntax unified
+.cpu cortex-m0plus
+.thumb
+
+.section .text
+
+// The exit point is passed in lr. If entered from bootrom, this will be the
+// flash address immediately following this second stage (0x10000100).
+// Otherwise it will be a return address -- second stage being called as a
+// function by user code, after copying out of XIP region. r3 holds SSI base,
+// r0...2 used as temporaries. Other GPRs not used.
+.global _stage2_boot
+.type _stage2_boot,%function
+.thumb_func
+_stage2_boot:
+ push {lr}
+
+ // Set pad configuration:
+ // - SCLK 8mA drive, no slew limiting
+ // - SDx disable input Schmitt to reduce delay
+
+ ldr r3, =PADS_QSPI_BASE
+ movs r0, #(2 << PADS_QSPI_GPIO_QSPI_SCLK_DRIVE_LSB | PADS_QSPI_GPIO_QSPI_SCLK_SLEWFAST_BITS)
+ str r0, [r3, #PADS_QSPI_GPIO_QSPI_SCLK_OFFSET]
+ ldr r0, [r3, #PADS_QSPI_GPIO_QSPI_SD0_OFFSET]
+ movs r1, #PADS_QSPI_GPIO_QSPI_SD0_SCHMITT_BITS
+ bics r0, r1
+ str r0, [r3, #PADS_QSPI_GPIO_QSPI_SD0_OFFSET]
+ str r0, [r3, #PADS_QSPI_GPIO_QSPI_SD1_OFFSET]
+ str r0, [r3, #PADS_QSPI_GPIO_QSPI_SD2_OFFSET]
+ str r0, [r3, #PADS_QSPI_GPIO_QSPI_SD3_OFFSET]
+
+ ldr r3, =XIP_SSI_BASE
+
+ // Disable SSI to allow further config
+ movs r1, #0
+ str r1, [r3, #SSI_SSIENR_OFFSET]
+
+ // Set baud rate
+ movs r1, #PICO_FLASH_SPI_CLKDIV
+ str r1, [r3, #SSI_BAUDR_OFFSET]
+
+ // Set 1-cycle sample delay. If PICO_FLASH_SPI_CLKDIV == 2 then this means,
+ // if the flash launches data on SCLK posedge, we capture it at the time that
+ // the next SCLK posedge is launched. This is shortly before that posedge
+ // arrives at the flash, so data hold time should be ok. For
+ // PICO_FLASH_SPI_CLKDIV > 2 this pretty much has no effect.
+
+ movs r1, #1
+ movs r2, #SSI_RX_SAMPLE_DLY_OFFSET // == 0xf0 so need 8 bits of offset significance
+ str r1, [r3, r2]
+
+
+// On QSPI parts we usually need a 01h SR-write command to enable QSPI mode
+// (i.e. turn WPn and HOLDn into IO2/IO3)
+#ifdef PROGRAM_STATUS_REG
+program_sregs:
+#define CTRL0_SPI_TXRX \
+ (7 << SSI_CTRLR0_DFS_32_LSB) | /* 8 bits per data frame */ \
+ (SSI_CTRLR0_TMOD_VALUE_TX_AND_RX << SSI_CTRLR0_TMOD_LSB)
+
+ ldr r1, =(CTRL0_SPI_TXRX)
+ str r1, [r3, #SSI_CTRLR0_OFFSET]
+
+ // Enable SSI and select slave 0
+ movs r1, #1
+ str r1, [r3, #SSI_SSIENR_OFFSET]
+
+ // Check whether SR needs updating
+ movs r0, #CMD_READ_STATUS2
+ bl read_flash_sreg
+ movs r2, #SREG_DATA
+ cmp r0, r2
+ beq skip_sreg_programming
+
+ // Send write enable command
+ movs r1, #CMD_WRITE_ENABLE
+ str r1, [r3, #SSI_DR0_OFFSET]
+
+ // Poll for completion and discard RX
+ bl wait_ssi_ready
+ ldr r1, [r3, #SSI_DR0_OFFSET]
+
+ // Send status write command followed by data bytes
+ movs r1, #CMD_WRITE_STATUS2
+ str r1, [r3, #SSI_DR0_OFFSET]
+ str r2, [r3, #SSI_DR0_OFFSET]
+
+ bl wait_ssi_ready
+ ldr r1, [r3, #SSI_DR0_OFFSET]
+ ldr r1, [r3, #SSI_DR0_OFFSET]
+ ldr r1, [r3, #SSI_DR0_OFFSET]
+
+ // Poll status register for write completion
+1:
+ movs r0, #CMD_READ_STATUS
+ bl read_flash_sreg
+ movs r1, #1
+ tst r0, r1
+ bne 1b
+
+skip_sreg_programming:
+
+ // Disable SSI again so that it can be reconfigured
+ movs r1, #0
+ str r1, [r3, #SSI_SSIENR_OFFSET]
+#endif
+
+// Currently the flash expects an 8 bit serial command prefix on every
+// transfer, which is a waste of cycles. Perform a dummy Fast Read Quad I/O
+// command, with mode bits set such that the flash will not expect a serial
+// command prefix on *subsequent* transfers. We don't care about the results
+// of the read, the important part is the mode bits.
+
+dummy_read:
+#define CTRLR0_ENTER_XIP \
+ (FRAME_FORMAT /* Quad I/O mode */ \
+ << SSI_CTRLR0_SPI_FRF_LSB) | \
+ (31 << SSI_CTRLR0_DFS_32_LSB) | /* 32 data bits */ \
+ (SSI_CTRLR0_TMOD_VALUE_EEPROM_READ /* Send INST/ADDR, Receive Data */ \
+ << SSI_CTRLR0_TMOD_LSB)
+
+ ldr r1, =(CTRLR0_ENTER_XIP)
+ str r1, [r3, #SSI_CTRLR0_OFFSET]
+
+ movs r1, #0x0 // NDF=0 (single 32b read)
+ str r1, [r3, #SSI_CTRLR1_OFFSET]
+
+#define SPI_CTRLR0_ENTER_XIP \
+ (ADDR_L << SSI_SPI_CTRLR0_ADDR_L_LSB) | /* Address + mode bits */ \
+ (WAIT_CYCLES << SSI_SPI_CTRLR0_WAIT_CYCLES_LSB) | /* Hi-Z dummy clocks following address + mode */ \
+ (SSI_SPI_CTRLR0_INST_L_VALUE_8B \
+ << SSI_SPI_CTRLR0_INST_L_LSB) | /* 8-bit instruction */ \
+ (SSI_SPI_CTRLR0_TRANS_TYPE_VALUE_1C2A /* Send Command in serial mode then address in Quad I/O mode */ \
+ << SSI_SPI_CTRLR0_TRANS_TYPE_LSB)
+
+ ldr r1, =(SPI_CTRLR0_ENTER_XIP)
+ ldr r0, =(XIP_SSI_BASE + SSI_SPI_CTRLR0_OFFSET) // SPI_CTRL0 Register
+ str r1, [r0]
+
+ movs r1, #1 // Re-enable SSI
+ str r1, [r3, #SSI_SSIENR_OFFSET]
+
+ movs r1, #CMD_READ
+ str r1, [r3, #SSI_DR0_OFFSET] // Push SPI command into TX FIFO
+ movs r1, #MODE_CONTINUOUS_READ // 32-bit: 24 address bits (we don't care, so 0) and M[7:4]=1010
+ str r1, [r3, #SSI_DR0_OFFSET] // Push Address into TX FIFO - this will trigger the transaction
+
+ // Poll for completion
+ bl wait_ssi_ready
+
+// The flash is in a state where we can blast addresses in parallel, and get
+// parallel data back. Now configure the SSI to translate XIP bus accesses
+// into QSPI transfers of this form.
+
+ movs r1, #0
+ str r1, [r3, #SSI_SSIENR_OFFSET] // Disable SSI (and clear FIFO) to allow further config
+
+// Note that the INST_L field is used to select what XIP data gets pushed into
+// the TX FIFO:
+// INST_L_0_BITS {ADDR[23:0],XIP_CMD[7:0]} Load "mode bits" into XIP_CMD
+// Anything else {XIP_CMD[7:0],ADDR[23:0]} Load SPI command into XIP_CMD
+configure_ssi:
+#define SPI_CTRLR0_XIP \
+ (MODE_CONTINUOUS_READ /* Mode bits to keep flash in continuous read mode */ \
+ << SSI_SPI_CTRLR0_XIP_CMD_LSB) | \
+ (ADDR_L << SSI_SPI_CTRLR0_ADDR_L_LSB) | /* Total number of address + mode bits */ \
+ (WAIT_CYCLES << SSI_SPI_CTRLR0_WAIT_CYCLES_LSB) | /* Hi-Z dummy clocks following address + mode */ \
+ (SSI_SPI_CTRLR0_INST_L_VALUE_NONE /* Do not send a command, instead send XIP_CMD as mode bits after address */ \
+ << SSI_SPI_CTRLR0_INST_L_LSB) | \
+ (SSI_SPI_CTRLR0_TRANS_TYPE_VALUE_2C2A /* Send Address in Quad I/O mode (and Command but that is zero bits long) */ \
+ << SSI_SPI_CTRLR0_TRANS_TYPE_LSB)
+
+ ldr r1, =(SPI_CTRLR0_XIP)
+ ldr r0, =(XIP_SSI_BASE + SSI_SPI_CTRLR0_OFFSET)
+ str r1, [r0]
+
+ movs r1, #1
+ str r1, [r3, #SSI_SSIENR_OFFSET] // Re-enable SSI
+
+// Bus accesses to the XIP window will now be transparently serviced by the
+// external flash on cache miss. We are ready to run code from flash.
+
+// Pull in standard exit routine
+#include "boot2_helpers/exit_from_boot2.S"
+
+// Common functions
+#include "boot2_helpers/wait_ssi_ready.S"
+#ifdef PROGRAM_STATUS_REG
+#include "boot2_helpers/read_flash_sreg.S"
+#endif
+
+.global literals
+literals:
+.ltorg
+
+.end
diff --git a/src/rp2_common/boot_stage2/boot2_generic_03h.S b/src/rp2_common/boot_stage2/boot2_generic_03h.S
new file mode 100644
index 0000000..a10e66a
--- /dev/null
+++ b/src/rp2_common/boot_stage2/boot2_generic_03h.S
@@ -0,0 +1,103 @@
+// ----------------------------------------------------------------------------
+// Second stage boot code
+// Copyright (c) 2019-2021 Raspberry Pi (Trading) Ltd.
+// SPDX-License-Identifier: BSD-3-Clause
+//
+// Device: Anything which responds to 03h serial read command
+//
+// Details: * Configure SSI to translate each APB read into a 03h command
+// * 8 command clocks, 24 address clocks and 32 data clocks
+// * This enables you to boot from almost anything: you can pretty
+// much solder a potato to your PCB, or a piece of cheese
+// * The tradeoff is performance around 3x worse than QSPI XIP
+//
+// Building: * This code must be position-independent, and use stack only
+// * The code will be padded to a size of 256 bytes, including a
+// 4-byte checksum. Therefore code size cannot exceed 252 bytes.
+// ----------------------------------------------------------------------------
+
+#include "pico/asm_helper.S"
+#include "hardware/regs/addressmap.h"
+#include "hardware/regs/ssi.h"
+
+// ----------------------------------------------------------------------------
+// Config section
+// ----------------------------------------------------------------------------
+// It should be possible to support most flash devices by modifying this section
+
+// The serial flash interface will run at clk_sys/PICO_FLASH_SPI_CLKDIV.
+// This must be a positive, even integer.
+// The bootrom is very conservative with SPI frequency, but here we should be
+// as aggressive as possible.
+#ifndef PICO_FLASH_SPI_CLKDIV
+#define PICO_FLASH_SPI_CLKDIV 4
+#endif
+
+#define CMD_READ 0x03
+
+// Value is number of address bits divided by 4
+#define ADDR_L 6
+
+#define CTRLR0_XIP \
+ (SSI_CTRLR0_SPI_FRF_VALUE_STD << SSI_CTRLR0_SPI_FRF_LSB) | /* Standard 1-bit SPI serial frames */ \
+ (31 << SSI_CTRLR0_DFS_32_LSB) | /* 32 clocks per data frame */ \
+ (SSI_CTRLR0_TMOD_VALUE_EEPROM_READ << SSI_CTRLR0_TMOD_LSB) /* Send instr + addr, receive data */
+
+#define SPI_CTRLR0_XIP \
+ (CMD_READ << SSI_SPI_CTRLR0_XIP_CMD_LSB) | /* Value of instruction prefix */ \
+ (ADDR_L << SSI_SPI_CTRLR0_ADDR_L_LSB) | /* Total number of address + mode bits */ \
+ (2 << SSI_SPI_CTRLR0_INST_L_LSB) | /* 8 bit command prefix (field value is bits divided by 4) */ \
+ (SSI_SPI_CTRLR0_TRANS_TYPE_VALUE_1C1A << SSI_SPI_CTRLR0_TRANS_TYPE_LSB) /* command and address both in serial format */
+
+// ----------------------------------------------------------------------------
+// Start of 2nd Stage Boot Code
+// ----------------------------------------------------------------------------
+
+.cpu cortex-m0
+.thumb
+
+.section .text
+
+.global _stage2_boot
+.type _stage2_boot,%function
+.thumb_func
+_stage2_boot:
+ push {lr}
+
+ ldr r3, =XIP_SSI_BASE // Use as base address where possible
+
+ // Disable SSI to allow further config
+ mov r1, #0
+ str r1, [r3, #SSI_SSIENR_OFFSET]
+
+ // Set baud rate
+ mov r1, #PICO_FLASH_SPI_CLKDIV
+ str r1, [r3, #SSI_BAUDR_OFFSET]
+
+ ldr r1, =(CTRLR0_XIP)
+ str r1, [r3, #SSI_CTRLR0_OFFSET]
+
+ ldr r1, =(SPI_CTRLR0_XIP)
+ ldr r0, =(XIP_SSI_BASE + SSI_SPI_CTRLR0_OFFSET)
+ str r1, [r0]
+
+ // NDF=0 (single 32b read)
+ mov r1, #0x0
+ str r1, [r3, #SSI_CTRLR1_OFFSET]
+
+ // Re-enable SSI
+ mov r1, #1
+ str r1, [r3, #SSI_SSIENR_OFFSET]
+
+// We are now in XIP mode. Any bus accesses to the XIP address window will be
+// translated by the SSI into 03h read commands to the external flash (if cache is missed),
+// and the data will be returned to the bus.
+
+// Pull in standard exit routine
+#include "boot2_helpers/exit_from_boot2.S"
+
+.global literals
+literals:
+.ltorg
+
+.end
diff --git a/src/rp2_common/boot_stage2/boot2_is25lp080.S b/src/rp2_common/boot_stage2/boot2_is25lp080.S
new file mode 100644
index 0000000..80bf9d1
--- /dev/null
+++ b/src/rp2_common/boot_stage2/boot2_is25lp080.S
@@ -0,0 +1,262 @@
+// ----------------------------------------------------------------------------
+// Copyright (c) 2019-2021 Raspberry Pi (Trading) Ltd.
+// SPDX-License-Identifier: BSD-3-Clause
+//
+// Device: ISSI IS25LP080D
+// Based on W25Q080 code: main difference is the QE bit being in
+// SR1 instead of SR2.
+//
+// Description: Configures IS25LP080D to run in Quad I/O continuous read XIP mode
+//
+// Details: * Check status register to determine if QSPI mode is enabled,
+// and perform an SR programming cycle if necessary.
+// * Use SSI to perform a dummy 0xEB read command, with the mode
+// continuation bits set, so that the flash will not require
+// 0xEB instruction prefix on subsequent reads.
+// * Configure SSI to write address, mode bits, but no instruction.
+// SSI + flash are now jointly in a state where continuous reads
+// can take place.
+// * Set VTOR = 0x10000100 (user vector table immediately after
+// this boot2 image).
+// * Read stack pointer (MSP) and reset vector from the flash
+// vector table; set SP and jump, as though the processor had
+// booted directly from flash.
+//
+// Building: * This code must be linked to run at 0x20027f00
+// * The code will be padded to a size of 256 bytes, including a
+// 4-byte checksum. Therefore code size cannot exceed 252 bytes.
+// ----------------------------------------------------------------------------
+
+#include "pico/asm_helper.S"
+#include "hardware/regs/addressmap.h"
+#include "hardware/regs/ssi.h"
+
+// ----------------------------------------------------------------------------
+// Config section
+// ----------------------------------------------------------------------------
+// It should be possible to support most flash devices by modifying this section
+
+// The serial flash interface will run at clk_sys/PICO_FLASH_SPI_CLKDIV.
+// This must be a positive, even integer.
+// The bootrom is very conservative with SPI frequency, but here we should be
+// as aggressive as possible.
+#ifndef PICO_FLASH_SPI_CLKDIV
+#define PICO_FLASH_SPI_CLKDIV 4
+#endif
+
+
+// Define interface width: single/dual/quad IO
+#define FRAME_FORMAT SSI_CTRLR0_SPI_FRF_VALUE_QUAD
+
+// For W25Q080 this is the "Read data fast quad IO" instruction:
+#define CMD_READ 0xeb
+
+// "Mode bits" are 8 special bits sent immediately after
+// the address bits in a "Read Data Fast Quad I/O" command sequence.
+// On W25Q080, the four LSBs are don't care, and if MSBs == 0xa, the
+// next read does not require the 0xeb instruction prefix.
+#define MODE_CONTINUOUS_READ 0xa0
+
+// The number of address + mode bits, divided by 4 (always 4, not function of
+// interface width).
+#define ADDR_L 8
+
+// How many clocks of Hi-Z following the mode bits. For W25Q080, 4 dummy cycles
+// are required.
+#define WAIT_CYCLES 4
+
+// If defined, we will read status reg, compare to SREG_DATA, and overwrite
+// with our value if the SR doesn't match.
+// This isn't great because it will remove block protections.
+// A better solution is to use a volatile SR write if your device supports it.
+#define PROGRAM_STATUS_REG
+
+#define CMD_WRITE_ENABLE 0x06
+#define CMD_READ_STATUS 0x05
+#define CMD_WRITE_STATUS 0x01
+#define SREG_DATA 0x40 // Enable quad-SPI mode
+
+// ----------------------------------------------------------------------------
+// Start of 2nd Stage Boot Code
+// ----------------------------------------------------------------------------
+
+.cpu cortex-m0
+.thumb
+
+.section .text
+
+.global _stage2_boot
+.type _stage2_boot,%function
+.thumb_func
+_stage2_boot:
+ push {lr}
+
+ ldr r3, =XIP_SSI_BASE // Use as base address where possible
+
+ // Disable SSI to allow further config
+ mov r1, #0
+ str r1, [r3, #SSI_SSIENR_OFFSET]
+
+ // Set baud rate
+ mov r1, #PICO_FLASH_SPI_CLKDIV
+ str r1, [r3, #SSI_BAUDR_OFFSET]
+
+// On QSPI parts we usually need a 01h SR-write command to enable QSPI mode
+// (i.e. turn WPn and HOLDn into IO2/IO3)
+#ifdef PROGRAM_STATUS_REG
+program_sregs:
+#define CTRL0_SPI_TXRX \
+ (7 << SSI_CTRLR0_DFS_32_LSB) | /* 8 bits per data frame */ \
+ (SSI_CTRLR0_TMOD_VALUE_TX_AND_RX << SSI_CTRLR0_TMOD_LSB)
+
+ ldr r1, =(CTRL0_SPI_TXRX)
+ str r1, [r3, #SSI_CTRLR0_OFFSET]
+
+ // Enable SSI and select slave 0
+ mov r1, #1
+ str r1, [r3, #SSI_SSIENR_OFFSET]
+
+ // Check whether SR needs updating
+ ldr r0, =CMD_READ_STATUS
+ bl read_flash_sreg
+ ldr r2, =SREG_DATA
+ cmp r0, r2
+ beq skip_sreg_programming
+
+ // Send write enable command
+ mov r1, #CMD_WRITE_ENABLE
+ str r1, [r3, #SSI_DR0_OFFSET]
+
+ // Poll for completion and discard RX
+ bl wait_ssi_ready
+ ldr r1, [r3, #SSI_DR0_OFFSET]
+
+ // Send status write command followed by data bytes
+ mov r1, #CMD_WRITE_STATUS
+ str r1, [r3, #SSI_DR0_OFFSET]
+ mov r0, #0
+ str r2, [r3, #SSI_DR0_OFFSET]
+
+ bl wait_ssi_ready
+ ldr r1, [r3, #SSI_DR0_OFFSET]
+ ldr r1, [r3, #SSI_DR0_OFFSET]
+
+ // Poll status register for write completion
+1:
+ ldr r0, =CMD_READ_STATUS
+ bl read_flash_sreg
+ mov r1, #1
+ tst r0, r1
+ bne 1b
+
+skip_sreg_programming:
+
+ // Send a 0xA3 high-performance-mode instruction
+// ldr r1, =0xa3
+// str r1, [r3, #SSI_DR0_OFFSET]
+// bl wait_ssi_ready
+
+ // Disable SSI again so that it can be reconfigured
+ mov r1, #0
+ str r1, [r3, #SSI_SSIENR_OFFSET]
+#endif
+
+
+// First we need to send the initial command to get us in to Fast Read Quad I/O
+// mode. As this transaction requires a command, we can't send it in XIP mode.
+// To enter Continuous Read mode as well we need to append 4'b0010 to the address
+// bits and then add a further 4 don't care bits. We will construct this by
+// specifying a 28-bit address, with the least significant bits being 4'b0010.
+// This is just a dummy transaction so we'll perform a read from address zero
+// and then discard what comes back. All we really care about is that at the
+// end of the transaction, the flash device is in Continuous Read mode
+// and from then on will only expect to receive addresses.
+dummy_read:
+#define CTRLR0_ENTER_XIP \
+ (FRAME_FORMAT /* Quad I/O mode */ \
+ << SSI_CTRLR0_SPI_FRF_LSB) | \
+ (31 << SSI_CTRLR0_DFS_32_LSB) | /* 32 data bits */ \
+ (SSI_CTRLR0_TMOD_VALUE_EEPROM_READ /* Send INST/ADDR, Receive Data */ \
+ << SSI_CTRLR0_TMOD_LSB)
+
+ ldr r1, =(CTRLR0_ENTER_XIP)
+ str r1, [r3, #SSI_CTRLR0_OFFSET]
+
+ mov r1, #0x0 // NDF=0 (single 32b read)
+ str r1, [r3, #SSI_CTRLR1_OFFSET]
+
+#define SPI_CTRLR0_ENTER_XIP \
+ (ADDR_L << SSI_SPI_CTRLR0_ADDR_L_LSB) | /* Address + mode bits */ \
+ (WAIT_CYCLES << SSI_SPI_CTRLR0_WAIT_CYCLES_LSB) | /* Hi-Z dummy clocks following address + mode */ \
+ (SSI_SPI_CTRLR0_INST_L_VALUE_8B \
+ << SSI_SPI_CTRLR0_INST_L_LSB) | /* 8-bit instruction */ \
+ (SSI_SPI_CTRLR0_TRANS_TYPE_VALUE_1C2A /* Send Command in serial mode then address in Quad I/O mode */ \
+ << SSI_SPI_CTRLR0_TRANS_TYPE_LSB)
+
+ ldr r1, =(SPI_CTRLR0_ENTER_XIP)
+ ldr r0, =(XIP_SSI_BASE + SSI_SPI_CTRLR0_OFFSET) // SPI_CTRL0 Register
+ str r1, [r0]
+
+ mov r1, #1 // Re-enable SSI
+ str r1, [r3, #SSI_SSIENR_OFFSET]
+
+ mov r1, #CMD_READ
+ str r1, [r3, #SSI_DR0_OFFSET] // Push SPI command into TX FIFO
+ mov r1, #MODE_CONTINUOUS_READ // 32-bit: 24 address bits (we don't care, so 0) and M[7:4]=1010
+ str r1, [r3, #SSI_DR0_OFFSET] // Push Address into TX FIFO - this will trigger the transaction
+
+ // Poll for completion
+ bl wait_ssi_ready
+
+// At this point CN# will be deasserted and the SPI clock will not be running.
+// The Winbond WX25X10CL device will be in continuous read, dual I/O mode and
+// only expecting address bits after the next CN# assertion. So long as we
+// send 4'b0010 (and 4 more dummy HiZ bits) after every subsequent 24b address
+// then the Winbond device will remain in continuous read mode. This is the
+// ideal mode for Execute-In-Place.
+// (If we want to exit continuous read mode then we will need to switch back
+// to APM mode and generate a 28-bit address phase with the extra nibble set
+// to 4'b0000).
+
+ mov r1, #0
+ str r1, [r3, #SSI_SSIENR_OFFSET] // Disable SSI (and clear FIFO) to allow further config
+
+// Note that the INST_L field is used to select what XIP data gets pushed into
+// the TX FIFO:
+// INST_L_0_BITS {ADDR[23:0],XIP_CMD[7:0]} Load "mode bits" into XIP_CMD
+// Anything else {XIP_CMD[7:0],ADDR[23:0]} Load SPI command into XIP_CMD
+configure_ssi:
+#define SPI_CTRLR0_XIP \
+ (MODE_CONTINUOUS_READ /* Mode bits to keep flash in continuous read mode */ \
+ << SSI_SPI_CTRLR0_XIP_CMD_LSB) | \
+ (ADDR_L << SSI_SPI_CTRLR0_ADDR_L_LSB) | /* Total number of address + mode bits */ \
+ (WAIT_CYCLES << SSI_SPI_CTRLR0_WAIT_CYCLES_LSB) | /* Hi-Z dummy clocks following address + mode */ \
+ (SSI_SPI_CTRLR0_INST_L_VALUE_NONE /* Do not send a command, instead send XIP_CMD as mode bits after address */ \
+ << SSI_SPI_CTRLR0_INST_L_LSB) | \
+ (SSI_SPI_CTRLR0_TRANS_TYPE_VALUE_2C2A /* Send Address in Quad I/O mode (and Command but that is zero bits long) */ \
+ << SSI_SPI_CTRLR0_TRANS_TYPE_LSB)
+
+ ldr r1, =(SPI_CTRLR0_XIP)
+ ldr r0, =(XIP_SSI_BASE + SSI_SPI_CTRLR0_OFFSET)
+ str r1, [r0]
+
+ mov r1, #1
+ str r1, [r3, #SSI_SSIENR_OFFSET] // Re-enable SSI
+
+// We are now in XIP mode, with all transactions using Dual I/O and only
+// needing to send 24-bit addresses (plus mode bits) for each read transaction.
+
+// Pull in standard exit routine
+#include "boot2_helpers/exit_from_boot2.S"
+
+// Common functions
+#include "boot2_helpers/wait_ssi_ready.S"
+#ifdef PROGRAM_STATUS_REG
+#include "boot2_helpers/read_flash_sreg.S"
+#endif
+
+.global literals
+literals:
+.ltorg
+
+.end
diff --git a/src/rp2_common/boot_stage2/boot2_usb_blinky.S b/src/rp2_common/boot_stage2/boot2_usb_blinky.S
new file mode 100644
index 0000000..74c47a3
--- /dev/null
+++ b/src/rp2_common/boot_stage2/boot2_usb_blinky.S
@@ -0,0 +1,53 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+// Stub second stage which calls into USB bootcode, with parameters.
+// USB boot takes two parameters:
+// - A GPIO mask for activity LED -- if mask is 0, don't touch GPIOs at all
+// - A mask of interfaces to disable. Bit 0 disables MSC, bit 1 disables PICOBoot
+// The bootrom passes 0 for both of these parameters, but user code (or this
+// second stage) can pass anything.
+
+#define USB_BOOT_MSD_AND_PICOBOOT 0x0
+#define USB_BOOT_MSD_ONLY 0x2
+#define USB_BOOT_PICOBOOT_ONLY 0x1
+
+// Config
+#define ACTIVITY_LED 0
+#define BOOT_MODE USB_BOOT_MSD_AND_PICOBOOT
+
+.cpu cortex-m0
+.thumb
+
+.section .text
+
+.global _stage2_boot
+.type _stage2_boot,%function
+
+.thumb_func
+_stage2_boot:
+ mov r7, #0x14 // Pointer to _well_known pointer table in ROM
+ ldrh r0, [r7, #0] // Offset 0 is 16 bit pointer to function table
+ ldrh r7, [r7, #4] // Offset 4 is 16 bit pointer to table lookup routine
+ ldr r1, =('U' | ('B' << 8)) // Symbol for USB Boot
+ blx r7
+ cmp r0, #0
+ beq dead
+
+ mov r7, r0
+ ldr r0, =(1u << ACTIVITY_LED) // Mask of which GPIO (or GPIOs) to use
+ mov r1, #BOOT_MODE
+ blx r7
+
+dead:
+ wfi
+ b dead
+
+.global literals
+literals:
+.ltorg
+
+.end
diff --git a/src/rp2_common/boot_stage2/boot2_w25q080.S b/src/rp2_common/boot_stage2/boot2_w25q080.S
new file mode 100644
index 0000000..ad3238e
--- /dev/null
+++ b/src/rp2_common/boot_stage2/boot2_w25q080.S
@@ -0,0 +1,287 @@
+// ----------------------------------------------------------------------------
+// Second stage boot code
+// Copyright (c) 2019-2021 Raspberry Pi (Trading) Ltd.
+// SPDX-License-Identifier: BSD-3-Clause
+//
+// Device: Winbond W25Q080
+// Also supports W25Q16JV (which has some different SR instructions)
+// Also supports AT25SF081
+// Also supports S25FL132K0
+//
+// Description: Configures W25Q080 to run in Quad I/O continuous read XIP mode
+//
+// Details: * Check status register 2 to determine if QSPI mode is enabled,
+// and perform an SR2 programming cycle if necessary.
+// * Use SSI to perform a dummy 0xEB read command, with the mode
+// continuation bits set, so that the flash will not require
+// 0xEB instruction prefix on subsequent reads.
+// * Configure SSI to write address, mode bits, but no instruction.
+// SSI + flash are now jointly in a state where continuous reads
+// can take place.
+// * Jump to exit pointer passed in via lr. Bootrom passes null,
+// in which case this code uses a default 256 byte flash offset
+//
+// Building: * This code must be position-independent, and use stack only
+// * The code will be padded to a size of 256 bytes, including a
+// 4-byte checksum. Therefore code size cannot exceed 252 bytes.
+// ----------------------------------------------------------------------------
+
+#include "pico/asm_helper.S"
+#include "hardware/regs/addressmap.h"
+#include "hardware/regs/ssi.h"
+#include "hardware/regs/pads_qspi.h"
+
+// ----------------------------------------------------------------------------
+// Config section
+// ----------------------------------------------------------------------------
+// It should be possible to support most flash devices by modifying this section
+
+// The serial flash interface will run at clk_sys/PICO_FLASH_SPI_CLKDIV.
+// This must be a positive, even integer.
+// The bootrom is very conservative with SPI frequency, but here we should be
+// as aggressive as possible.
+
+#ifndef PICO_FLASH_SPI_CLKDIV
+#define PICO_FLASH_SPI_CLKDIV 4
+#endif
+#if PICO_FLASH_SPI_CLKDIV & 1
+#error PICO_FLASH_SPI_CLKDIV must be even
+#endif
+
+// Define interface width: single/dual/quad IO
+#define FRAME_FORMAT SSI_CTRLR0_SPI_FRF_VALUE_QUAD
+
+// For W25Q080 this is the "Read data fast quad IO" instruction:
+#define CMD_READ 0xeb
+
+// "Mode bits" are 8 special bits sent immediately after
+// the address bits in a "Read Data Fast Quad I/O" command sequence.
+// On W25Q080, the four LSBs are don't care, and if MSBs == 0xa, the
+// next read does not require the 0xeb instruction prefix.
+#define MODE_CONTINUOUS_READ 0xa0
+
+// The number of address + mode bits, divided by 4 (always 4, not function of
+// interface width).
+#define ADDR_L 8
+
+// How many clocks of Hi-Z following the mode bits. For W25Q080, 4 dummy cycles
+// are required.
+#define WAIT_CYCLES 4
+
+// If defined, we will read status reg, compare to SREG_DATA, and overwrite
+// with our value if the SR doesn't match.
+// We do a two-byte write to SR1 (01h cmd) rather than a one-byte write to
+// SR2 (31h cmd) as the latter command isn't supported by WX25Q080.
+// This isn't great because it will remove block protections.
+// A better solution is to use a volatile SR write if your device supports it.
+#define PROGRAM_STATUS_REG
+
+#define CMD_WRITE_ENABLE 0x06
+#define CMD_READ_STATUS 0x05
+#define CMD_READ_STATUS2 0x35
+#define CMD_WRITE_STATUS 0x01
+#define SREG_DATA 0x02 // Enable quad-SPI mode
+
+// ----------------------------------------------------------------------------
+// Start of 2nd Stage Boot Code
+// ----------------------------------------------------------------------------
+
+.syntax unified
+.cpu cortex-m0plus
+.thumb
+
+.section .text
+
+// The exit point is passed in lr. If entered from bootrom, this will be the
+// flash address immediately following this second stage (0x10000100).
+// Otherwise it will be a return address -- second stage being called as a
+// function by user code, after copying out of XIP region. r3 holds SSI base,
+// r0...2 used as temporaries. Other GPRs not used.
+.global _stage2_boot
+.type _stage2_boot,%function
+.thumb_func
+_stage2_boot:
+ push {lr}
+
+ // Set pad configuration:
+ // - SCLK 8mA drive, no slew limiting
+ // - SDx disable input Schmitt to reduce delay
+
+ ldr r3, =PADS_QSPI_BASE
+ movs r0, #(2 << PADS_QSPI_GPIO_QSPI_SCLK_DRIVE_LSB | PADS_QSPI_GPIO_QSPI_SCLK_SLEWFAST_BITS)
+ str r0, [r3, #PADS_QSPI_GPIO_QSPI_SCLK_OFFSET]
+ ldr r0, [r3, #PADS_QSPI_GPIO_QSPI_SD0_OFFSET]
+ movs r1, #PADS_QSPI_GPIO_QSPI_SD0_SCHMITT_BITS
+ bics r0, r1
+ str r0, [r3, #PADS_QSPI_GPIO_QSPI_SD0_OFFSET]
+ str r0, [r3, #PADS_QSPI_GPIO_QSPI_SD1_OFFSET]
+ str r0, [r3, #PADS_QSPI_GPIO_QSPI_SD2_OFFSET]
+ str r0, [r3, #PADS_QSPI_GPIO_QSPI_SD3_OFFSET]
+
+ ldr r3, =XIP_SSI_BASE
+
+ // Disable SSI to allow further config
+ movs r1, #0
+ str r1, [r3, #SSI_SSIENR_OFFSET]
+
+ // Set baud rate
+ movs r1, #PICO_FLASH_SPI_CLKDIV
+ str r1, [r3, #SSI_BAUDR_OFFSET]
+
+ // Set 1-cycle sample delay. If PICO_FLASH_SPI_CLKDIV == 2 then this means,
+ // if the flash launches data on SCLK posedge, we capture it at the time that
+ // the next SCLK posedge is launched. This is shortly before that posedge
+ // arrives at the flash, so data hold time should be ok. For
+ // PICO_FLASH_SPI_CLKDIV > 2 this pretty much has no effect.
+
+ movs r1, #1
+ movs r2, #SSI_RX_SAMPLE_DLY_OFFSET // == 0xf0 so need 8 bits of offset significance
+ str r1, [r3, r2]
+
+
+// On QSPI parts we usually need a 01h SR-write command to enable QSPI mode
+// (i.e. turn WPn and HOLDn into IO2/IO3)
+#ifdef PROGRAM_STATUS_REG
+program_sregs:
+#define CTRL0_SPI_TXRX \
+ (7 << SSI_CTRLR0_DFS_32_LSB) | /* 8 bits per data frame */ \
+ (SSI_CTRLR0_TMOD_VALUE_TX_AND_RX << SSI_CTRLR0_TMOD_LSB)
+
+ ldr r1, =(CTRL0_SPI_TXRX)
+ str r1, [r3, #SSI_CTRLR0_OFFSET]
+
+ // Enable SSI and select slave 0
+ movs r1, #1
+ str r1, [r3, #SSI_SSIENR_OFFSET]
+
+ // Check whether SR needs updating
+ movs r0, #CMD_READ_STATUS2
+ bl read_flash_sreg
+ movs r2, #SREG_DATA
+ cmp r0, r2
+ beq skip_sreg_programming
+
+ // Send write enable command
+ movs r1, #CMD_WRITE_ENABLE
+ str r1, [r3, #SSI_DR0_OFFSET]
+
+ // Poll for completion and discard RX
+ bl wait_ssi_ready
+ ldr r1, [r3, #SSI_DR0_OFFSET]
+
+ // Send status write command followed by data bytes
+ movs r1, #CMD_WRITE_STATUS
+ str r1, [r3, #SSI_DR0_OFFSET]
+ movs r0, #0
+ str r0, [r3, #SSI_DR0_OFFSET]
+ str r2, [r3, #SSI_DR0_OFFSET]
+
+ bl wait_ssi_ready
+ ldr r1, [r3, #SSI_DR0_OFFSET]
+ ldr r1, [r3, #SSI_DR0_OFFSET]
+ ldr r1, [r3, #SSI_DR0_OFFSET]
+
+ // Poll status register for write completion
+1:
+ movs r0, #CMD_READ_STATUS
+ bl read_flash_sreg
+ movs r1, #1
+ tst r0, r1
+ bne 1b
+
+skip_sreg_programming:
+
+ // Disable SSI again so that it can be reconfigured
+ movs r1, #0
+ str r1, [r3, #SSI_SSIENR_OFFSET]
+#endif
+
+// Currently the flash expects an 8 bit serial command prefix on every
+// transfer, which is a waste of cycles. Perform a dummy Fast Read Quad I/O
+// command, with mode bits set such that the flash will not expect a serial
+// command prefix on *subsequent* transfers. We don't care about the results
+// of the read, the important part is the mode bits.
+
+dummy_read:
+#define CTRLR0_ENTER_XIP \
+ (FRAME_FORMAT /* Quad I/O mode */ \
+ << SSI_CTRLR0_SPI_FRF_LSB) | \
+ (31 << SSI_CTRLR0_DFS_32_LSB) | /* 32 data bits */ \
+ (SSI_CTRLR0_TMOD_VALUE_EEPROM_READ /* Send INST/ADDR, Receive Data */ \
+ << SSI_CTRLR0_TMOD_LSB)
+
+ ldr r1, =(CTRLR0_ENTER_XIP)
+ str r1, [r3, #SSI_CTRLR0_OFFSET]
+
+ movs r1, #0x0 // NDF=0 (single 32b read)
+ str r1, [r3, #SSI_CTRLR1_OFFSET]
+
+#define SPI_CTRLR0_ENTER_XIP \
+ (ADDR_L << SSI_SPI_CTRLR0_ADDR_L_LSB) | /* Address + mode bits */ \
+ (WAIT_CYCLES << SSI_SPI_CTRLR0_WAIT_CYCLES_LSB) | /* Hi-Z dummy clocks following address + mode */ \
+ (SSI_SPI_CTRLR0_INST_L_VALUE_8B \
+ << SSI_SPI_CTRLR0_INST_L_LSB) | /* 8-bit instruction */ \
+ (SSI_SPI_CTRLR0_TRANS_TYPE_VALUE_1C2A /* Send Command in serial mode then address in Quad I/O mode */ \
+ << SSI_SPI_CTRLR0_TRANS_TYPE_LSB)
+
+ ldr r1, =(SPI_CTRLR0_ENTER_XIP)
+ ldr r0, =(XIP_SSI_BASE + SSI_SPI_CTRLR0_OFFSET) // SPI_CTRL0 Register
+ str r1, [r0]
+
+ movs r1, #1 // Re-enable SSI
+ str r1, [r3, #SSI_SSIENR_OFFSET]
+
+ movs r1, #CMD_READ
+ str r1, [r3, #SSI_DR0_OFFSET] // Push SPI command into TX FIFO
+ movs r1, #MODE_CONTINUOUS_READ // 32-bit: 24 address bits (we don't care, so 0) and M[7:4]=1010
+ str r1, [r3, #SSI_DR0_OFFSET] // Push Address into TX FIFO - this will trigger the transaction
+
+ // Poll for completion
+ bl wait_ssi_ready
+
+// The flash is in a state where we can blast addresses in parallel, and get
+// parallel data back. Now configure the SSI to translate XIP bus accesses
+// into QSPI transfers of this form.
+
+ movs r1, #0
+ str r1, [r3, #SSI_SSIENR_OFFSET] // Disable SSI (and clear FIFO) to allow further config
+
+// Note that the INST_L field is used to select what XIP data gets pushed into
+// the TX FIFO:
+// INST_L_0_BITS {ADDR[23:0],XIP_CMD[7:0]} Load "mode bits" into XIP_CMD
+// Anything else {XIP_CMD[7:0],ADDR[23:0]} Load SPI command into XIP_CMD
+configure_ssi:
+#define SPI_CTRLR0_XIP \
+ (MODE_CONTINUOUS_READ /* Mode bits to keep flash in continuous read mode */ \
+ << SSI_SPI_CTRLR0_XIP_CMD_LSB) | \
+ (ADDR_L << SSI_SPI_CTRLR0_ADDR_L_LSB) | /* Total number of address + mode bits */ \
+ (WAIT_CYCLES << SSI_SPI_CTRLR0_WAIT_CYCLES_LSB) | /* Hi-Z dummy clocks following address + mode */ \
+ (SSI_SPI_CTRLR0_INST_L_VALUE_NONE /* Do not send a command, instead send XIP_CMD as mode bits after address */ \
+ << SSI_SPI_CTRLR0_INST_L_LSB) | \
+ (SSI_SPI_CTRLR0_TRANS_TYPE_VALUE_2C2A /* Send Address in Quad I/O mode (and Command but that is zero bits long) */ \
+ << SSI_SPI_CTRLR0_TRANS_TYPE_LSB)
+
+ ldr r1, =(SPI_CTRLR0_XIP)
+ ldr r0, =(XIP_SSI_BASE + SSI_SPI_CTRLR0_OFFSET)
+ str r1, [r0]
+
+ movs r1, #1
+ str r1, [r3, #SSI_SSIENR_OFFSET] // Re-enable SSI
+
+// Bus accesses to the XIP window will now be transparently serviced by the
+// external flash on cache miss. We are ready to run code from flash.
+
+// Pull in standard exit routine
+#include "boot2_helpers/exit_from_boot2.S"
+
+// Common functions
+#include "boot2_helpers/wait_ssi_ready.S"
+#ifdef PROGRAM_STATUS_REG
+#include "boot2_helpers/read_flash_sreg.S"
+#endif
+
+.global literals
+literals:
+.ltorg
+
+.end
diff --git a/src/rp2_common/boot_stage2/boot2_w25x10cl.S b/src/rp2_common/boot_stage2/boot2_w25x10cl.S
new file mode 100644
index 0000000..02628d4
--- /dev/null
+++ b/src/rp2_common/boot_stage2/boot2_w25x10cl.S
@@ -0,0 +1,196 @@
+// ----------------------------------------------------------------------------
+// Second stage boot code
+// Copyright (c) 2019-2021 Raspberry Pi (Trading) Ltd.
+// SPDX-License-Identifier: BSD-3-Clause
+//
+// Device: Winbond W25X10CL
+//
+// Description: Configures W25X10CL to run in Dual I/O continuous read XIP mode
+//
+// Details: * Disable SSI
+// * Configure SSI to generate 8b command + 28b address + 2 wait,
+// with address and data using dual SPI mode
+// * Enable SSI
+// * Generate dummy read with command = 0xBB, top 24b of address
+// of 0x000000 followed by M[7:0]=0010zzzz (with the HiZ being
+// generated by 2 wait cycles). This leaves the W25X10CL in
+// continuous read mode
+// * Disable SSI
+// * Configure SSI to generate 0b command + 28b address + 2 wait,
+// with the extra 4 bits of address LSB being 0x2 to keep the
+// W25X10CL in continuous read mode forever
+// * Enable SSI
+// * Set VTOR = 0x10000100
+// * Read MSP reset vector from 0x10000100 and write to MSP (this
+// will also enable XIP mode in the SSI wrapper)
+// * Read PC reset vector from 0x10000104 and jump to it
+//
+// Building: * This code must be linked to run at 0x20000000
+// * The code will be padded to a size of 256 bytes, including a
+// 4-byte checksum. Therefore code size cannot exceed 252 bytes.
+// ----------------------------------------------------------------------------
+
+#include "pico/asm_helper.S"
+#include "hardware/regs/addressmap.h"
+#include "hardware/regs/ssi.h"
+
+// The serial flash interface will run at clk_sys/PICO_FLASH_SPI_CLKDIV.
+// This must be an even number.
+#ifndef PICO_FLASH_SPI_CLKDIV
+#define PICO_FLASH_SPI_CLKDIV 4
+#endif
+
+// ----------------------------------------------------------------------------
+// The "System Control Block" is a set of internal Cortex-M0+ control registers
+// that are memory mapped and accessed like any other H/W register. They have
+// fixed addresses in the address map of every Cortex-M0+ system.
+// ----------------------------------------------------------------------------
+
+.equ SCB_VTOR, 0xE000ED08 // RW Vector Table Offset Register
+
+// ----------------------------------------------------------------------------
+// Winbond W25X10CL Supported Commands
+// Taken from "w25x10cl_reg_021714.pdf"
+// ----------------------------------------------------------------------------
+
+.equ W25X10CL_CMD_READ_DATA_FAST_DUAL_IO, 0xbb
+
+// ----------------------------------------------------------------------------
+// Winbond W25X10CL "Mode bits" are 8 special bits sent immediately after
+// the address bits in a "Read Data Fast Dual I/O" command sequence.
+// Of M[7:4], they say M[7:6] are reserved (set to zero), and bits M[3:0]
+// are don't care (we HiZ). Only M[5:4] are used, and they must be set
+// to M[5:4] = 2'b10 to enable continuous read mode.
+// ----------------------------------------------------------------------------
+
+.equ W25X10CL_MODE_CONTINUOUS_READ, 0x20
+
+// ----------------------------------------------------------------------------
+// Start of 2nd Stage Boot Code
+// ----------------------------------------------------------------------------
+
+.cpu cortex-m0
+.thumb
+
+.org 0
+
+.section .text
+
+// This code will get copied to 0x20000000 and then executed
+
+.global _stage2_boot
+.type _stage2_boot,%function
+.thumb_func
+_stage2_boot:
+ push {lr}
+ ldr r3, =XIP_SSI_BASE // Use as base address where possible
+
+// We are primarily interested in setting up Flash for DSPI XIP w/ continuous read
+
+ mov r1, #0
+ str r1, [r3, #SSI_SSIENR_OFFSET] // Disable SSI to allow further config
+
+// The Boot ROM sets a very conservative SPI clock frequency to be sure it can
+// read the initial 256 bytes from any device. Here we can be more aggressive.
+
+ mov r1, #PICO_FLASH_SPI_CLKDIV
+ str r1, [r3, #SSI_BAUDR_OFFSET] // Set SSI Clock
+
+// First we need to send the initial command to get us in to Fast Read Dual I/O
+// mode. As this transaction requires a command, we can't send it in XIP mode.
+// To enter Continuous Read mode as well we need to append 4'b0010 to the address
+// bits and then add a further 4 don't care bits. We will construct this by
+// specifying a 28-bit address, with the least significant bits being 4'b0010.
+// This is just a dummy transaction so we'll perform a read from address zero
+// and then discard what comes back. All we really care about is that at the
+// end of the transaction, the Winbond W25X10CL device is in Continuous Read mode
+// and from then on will only expect to receive addresses.
+
+#define CTRLR0_ENTER_XIP \
+ (SSI_CTRLR0_SPI_FRF_VALUE_DUAL /* Dual I/O mode */ \
+ << SSI_CTRLR0_SPI_FRF_LSB) | \
+ (31 << SSI_CTRLR0_DFS_32_LSB) | /* 32 data bits */ \
+ (SSI_CTRLR0_TMOD_VALUE_EEPROM_READ /* Send INST/ADDR, Receive Data */ \
+ << SSI_CTRLR0_TMOD_LSB)
+
+ ldr r1, =(CTRLR0_ENTER_XIP)
+ str r1, [r3, #SSI_CTRLR0_OFFSET]
+
+ mov r1, #0x0 // NDF=0 (single 32b read)
+ str r1, [r3, #SSI_CTRLR1_OFFSET]
+
+#define SPI_CTRLR0_ENTER_XIP \
+ (7 << SSI_SPI_CTRLR0_ADDR_L_LSB) | /* Send 28 bits (24 address + 4 mode) */ \
+ (2 << SSI_SPI_CTRLR0_WAIT_CYCLES_LSB) | /* Hi-Z the other 4 mode bits (2 cycles @ dual I/O = 4 bits) */ \
+ (SSI_SPI_CTRLR0_INST_L_VALUE_8B \
+ << SSI_SPI_CTRLR0_INST_L_LSB) | /* 8-bit instruction */ \
+ (SSI_SPI_CTRLR0_TRANS_TYPE_VALUE_1C2A /* Send Command in serial mode then address in Dual I/O mode */ \
+ << SSI_SPI_CTRLR0_TRANS_TYPE_LSB)
+
+ ldr r1, =(SPI_CTRLR0_ENTER_XIP)
+ ldr r0, =(XIP_SSI_BASE + SSI_SPI_CTRLR0_OFFSET) // SPI_CTRL0 Register
+ str r1, [r0]
+
+ mov r1, #1 // Re-enable SSI
+ str r1, [r3, #SSI_SSIENR_OFFSET]
+
+ mov r1, #W25X10CL_CMD_READ_DATA_FAST_DUAL_IO // 8b command = 0xBB
+ str r1, [r3, #SSI_DR0_OFFSET] // Push SPI command into TX FIFO
+ mov r1, #0x0000002 // 28-bit Address for dummy read = 0x000000 + 0x2 Mode bits to set M[5:4]=10
+ str r1, [r3, #SSI_DR0_OFFSET] // Push Address into TX FIFO - this will trigger the transaction
+
+// Now we wait for the read transaction to complete by monitoring the SSI
+// status register and checking for the "RX FIFO Not Empty" flag to assert.
+
+ mov r1, #SSI_SR_RFNE_BITS
+00:
+ ldr r0, [r3, #SSI_SR_OFFSET] // Read status register
+ tst r0, r1 // RFNE status flag set?
+ beq 00b // If not then wait
+
+// At this point CN# will be deasserted and the SPI clock will not be running.
+// The Winbond WX25X10CL device will be in continuous read, dual I/O mode and
+// only expecting address bits after the next CN# assertion. So long as we
+// send 4'b0010 (and 4 more dummy HiZ bits) after every subsequent 24b address
+// then the Winbond device will remain in continuous read mode. This is the
+// ideal mode for Execute-In-Place.
+// (If we want to exit continuous read mode then we will need to switch back
+// to APM mode and generate a 28-bit address phase with the extra nibble set
+// to 4'b0000).
+
+ mov r1, #0
+ str r1, [r3, #SSI_SSIENR_OFFSET] // Disable SSI (and clear FIFO) to allow further config
+
+// Note that the INST_L field is used to select what XIP data gets pushed into
+// the TX FIFO:
+// INST_L_0_BITS {ADDR[23:0],XIP_CMD[7:0]} Load "mode bits" into XIP_CMD
+// Anything else {XIP_CMD[7:0],ADDR[23:0]} Load SPI command into XIP_CMD
+
+#define SPI_CTRLR0_XIP \
+ (W25X10CL_MODE_CONTINUOUS_READ /* Mode bits to keep Winbond in continuous read mode */ \
+ << SSI_SPI_CTRLR0_XIP_CMD_LSB) | \
+ (7 << SSI_SPI_CTRLR0_ADDR_L_LSB) | /* Send 28 bits (24 address + 4 mode) */ \
+ (2 << SSI_SPI_CTRLR0_WAIT_CYCLES_LSB) | /* Hi-Z the other 4 mode bits (2 cycles @ dual I/O = 4 bits) */ \
+ (SSI_SPI_CTRLR0_INST_L_VALUE_NONE /* Do not send a command, instead send XIP_CMD as mode bits after address */ \
+ << SSI_SPI_CTRLR0_INST_L_LSB) | \
+ (SSI_SPI_CTRLR0_TRANS_TYPE_VALUE_2C2A /* Send Address in Dual I/O mode (and Command but that is zero bits long) */ \
+ << SSI_SPI_CTRLR0_TRANS_TYPE_LSB)
+
+ ldr r1, =(SPI_CTRLR0_XIP)
+ ldr r0, =(XIP_SSI_BASE + SSI_SPI_CTRLR0_OFFSET)
+ str r1, [r0]
+
+ mov r1, #1
+ str r1, [r3, #SSI_SSIENR_OFFSET] // Re-enable SSI
+
+// We are now in XIP mode, with all transactions using Dual I/O and only
+// needing to send 24-bit addresses (plus mode bits) for each read transaction.
+
+// Pull in standard exit routine
+#include "boot2_helpers/exit_from_boot2.S"
+
+.global literals
+literals:
+.ltorg
+
+.end
diff --git a/src/rp2_common/boot_stage2/boot_stage2.ld b/src/rp2_common/boot_stage2/boot_stage2.ld
new file mode 100644
index 0000000..f8669ab
--- /dev/null
+++ b/src/rp2_common/boot_stage2/boot_stage2.ld
@@ -0,0 +1,13 @@
+MEMORY {
+ /* We are loaded to the top 256 bytes of SRAM, which is above the bootrom
+ stack. Note 4 bytes occupied by checksum. */
+ SRAM(rx) : ORIGIN = 0x20041f00, LENGTH = 252
+}
+
+SECTIONS {
+ . = ORIGIN(SRAM);
+ .text : {
+ *(.entry)
+ *(.text)
+ } >SRAM
+}
diff --git a/src/rp2_common/boot_stage2/compile_time_choice.S b/src/rp2_common/boot_stage2/compile_time_choice.S
new file mode 100644
index 0000000..5aa2b96
--- /dev/null
+++ b/src/rp2_common/boot_stage2/compile_time_choice.S
@@ -0,0 +1,19 @@
+// ----------------------------------------------------------------------------
+// Second stage boot code
+// Copyright (c) 2019-2021 Raspberry Pi (Trading) Ltd.
+// SPDX-License-Identifier: BSD-3-Clause
+// ----------------------------------------------------------------------------
+//
+// This implementation uses the PICO_BOOT_STAGE2_CHOOSE_ preprocessor defines to pick
+// amongst a menu of known boot stage 2 implementations, allowing the board
+// configuration header to be able to specify the boot stage 2
+
+#include "boot_stage2/config.h"
+
+#ifdef PICO_BUILD_BOOT_STAGE2_NAME
+ // boot stage 2 is configured by cmake, so use the name specified there
+ #error PICO_BUILD_BOOT_STAGE2_NAME should not be defined for compile_time_choice builds
+#else
+ // boot stage 2 is selected by board config header, and PICO_BOOT_STAGE2_ASM is set in boot_stage2/config.h
+ #include PICO_BOOT_STAGE2_ASM
+#endif
diff --git a/src/rp2_common/boot_stage2/doc.h b/src/rp2_common/boot_stage2/doc.h
new file mode 100644
index 0000000..483dd68
--- /dev/null
+++ b/src/rp2_common/boot_stage2/doc.h
@@ -0,0 +1,4 @@
+/**
+ * \defgroup boot_stage2 boot_stage2
+ * \brief Second stage boot loaders responsible for setting up external flash
+ */
diff --git a/src/rp2_common/boot_stage2/include/boot_stage2/config.h b/src/rp2_common/boot_stage2/include/boot_stage2/config.h
new file mode 100644
index 0000000..5e57f95
--- /dev/null
+++ b/src/rp2_common/boot_stage2/include/boot_stage2/config.h
@@ -0,0 +1,94 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _BOOT_STAGE2_CONFIG_H_
+#define _BOOT_STAGE2_CONFIG_H_
+
+// NOTE THIS HEADER IS INCLUDED FROM ASSEMBLY
+
+#include "pico/config.h"
+
+// PICO_CONFIG: PICO_BUILD_BOOT_STAGE2_NAME, The name of the boot stage 2 if selected by the build, group=boot_stage2
+#ifdef PICO_BUILD_BOOT_STAGE2_NAME
+ #define _BOOT_STAGE2_SELECTED
+#else
+ // check that multiple boot stage 2 options haven't been set...
+
+// PICO_CONFIG: PICO_BOOT_STAGE2_CHOOSE_IS25LP080, Select boot2_is25lp080 as the boot stage 2 when no boot stage 2 selection is made by the CMake build, type=bool, default=0, group=boot_stage2
+#ifndef PICO_BOOT_STAGE2_CHOOSE_IS25LP080
+ #define PICO_BOOT_STAGE2_CHOOSE_IS25LP080 0
+#elif PICO_BOOT_STAGE2_CHOOSE_IS25LP080
+ #ifdef _BOOT_STAGE2_SELECTED
+ #error multiple boot stage 2 options chosen
+ #endif
+ #define _BOOT_STAGE2_SELECTED
+#endif
+// PICO_CONFIG: PICO_BOOT_STAGE2_CHOOSE_W25Q080, Select boot2_w25q080 as the boot stage 2 when no boot stage 2 selection is made by the CMake build, type=bool, default=0, group=boot_stage2
+#ifndef PICO_BOOT_STAGE2_CHOOSE_W25Q080
+ #define PICO_BOOT_STAGE2_CHOOSE_W25Q080 0
+#elif PICO_BOOT_STAGE2_CHOOSE_W25Q080
+ #ifdef _BOOT_STAGE2_SELECTED
+ #error multiple boot stage 2 options chosen
+ #endif
+ #define _BOOT_STAGE2_SELECTED
+#endif
+// PICO_CONFIG: PICO_BOOT_STAGE2_CHOOSE_W25X10CL, Select boot2_w25x10cl as the boot stage 2 when no boot stage 2 selection is made by the CMake build, type=bool, default=0, group=boot_stage2
+#ifndef PICO_BOOT_STAGE2_CHOOSE_W25X10CL
+ #define PICO_BOOT_STAGE2_CHOOSE_W25X10CL 0
+#elif PICO_BOOT_STAGE2_CHOOSE_W25X10CL
+ #ifdef _BOOT_STAGE2_SELECTED
+ #error multiple boot stage 2 options chosen
+ #endif
+ #define _BOOT_STAGE2_SELECTED
+#endif
+// PICO_CONFIG: PICO_BOOT_STAGE2_CHOOSE_AT25SF128A, Select boot2_at25sf128a as the boot stage 2 when no boot stage 2 selection is made by the CMake build, type=bool, default=0, group=boot_stage2
+#ifndef PICO_BOOT_STAGE2_CHOOSE_AT25SF128A
+ #define PICO_BOOT_STAGE2_CHOOSE_AT25SF128A 0
+#elif PICO_BOOT_STAGE2_CHOOSE_AT25SF128A
+ #ifdef _BOOT_STAGE2_SELECTED
+ #error multiple boot stage 2 options chosen
+ #endif
+ #define _BOOT_STAGE2_SELECTED
+#endif
+
+// PICO_CONFIG: PICO_BOOT_STAGE2_CHOOSE_GENERIC_03H, Select boot2_generic_03h as the boot stage 2 when no boot stage 2 selection is made by the CMake build, type=bool, default=1, group=boot_stage2
+#if defined(PICO_BOOT_STAGE2_CHOOSE_GENERIC_03H) && PICO_BOOT_STAGE2_CHOOSE_GENERIC_03H
+ #ifdef _BOOT_STAGE2_SELECTED
+ #error multiple boot stage 2 options chosen
+ #endif
+ #define _BOOT_STAGE2_SELECTED
+#endif
+
+#endif // PICO_BUILD_BOOT_STAGE2_NAME
+
+#ifdef PICO_BUILD_BOOT_STAGE2_NAME
+ // boot stage 2 is configured by cmake, so use the name specified there
+ #define PICO_BOOT_STAGE2_NAME PICO_BUILD_BOOT_STAGE2_NAME
+#else
+ // boot stage 2 is selected by board config header, so we have to do some work
+ #if PICO_BOOT_STAGE2_CHOOSE_IS25LP080
+ #define _BOOT_STAGE2 boot2_is25lp080
+ #elif PICO_BOOT_STAGE2_CHOOSE_W25Q080
+ #define _BOOT_STAGE2 boot2_w25q080
+ #elif PICO_BOOT_STAGE2_CHOOSE_W25X10CL
+ #define _BOOT_STAGE2 boot2_w25x10cl
+ #elif PICO_BOOT_STAGE2_CHOOSE_AT25SF128A
+ #define _BOOT_STAGE2 boot2_at25sf128a
+ #elif !defined(PICO_BOOT_STAGE2_CHOOSE_GENERIC_03H) || PICO_BOOT_STAGE2_CHOOSE_GENERIC_03H
+ #undef PICO_BOOT_STAGE2_CHOOSE_GENERIC_03H
+ #define PICO_BOOT_STAGE2_CHOOSE_GENERIC_03H 1
+ #define _BOOT_STAGE2 boot2_generic_03h
+ #else
+ #error no boot stage 2 is defined by PICO_BOOT_STAGE2_CHOOSE_ macro
+ #endif
+ // we can't include cdefs in assembly, so define our own, but avoid conflict with real ones for c inclusion
+ #define _PICO__STRING(x) #x
+ #define _PICO__XSTRING(x) _PICO__STRING(x)
+ #define _PICO__CONCAT1(x, y) x ## y
+ #define PICO_BOOT_STAGE2_NAME _PICO__XSTRING(_BOOT_STAGE2)
+ #define PICO_BOOT_STAGE2_ASM _PICO__XSTRING(_PICO__CONCAT1(_BOOT_STAGE2,.S))
+#endif
+#endif
diff --git a/src/rp2_common/boot_stage2/pad_checksum b/src/rp2_common/boot_stage2/pad_checksum
new file mode 100755
index 0000000..356227d
--- /dev/null
+++ b/src/rp2_common/boot_stage2/pad_checksum
@@ -0,0 +1,55 @@
+#!/usr/bin/env python3
+
+import argparse
+import binascii
+import struct
+import sys
+
+
+def any_int(x):
+ try:
+ return int(x, 0)
+ except:
+ raise argparse.ArgumentTypeError("expected an integer, not '{!r}'".format(x))
+
+
+def bitrev(x, width):
+ return int("{:0{w}b}".format(x, w=width)[::-1], 2)
+
+
+parser = argparse.ArgumentParser()
+parser.add_argument("ifile", help="Input file (binary)")
+parser.add_argument("ofile", help="Output file (assembly)")
+parser.add_argument("-p", "--pad", help="Padded size (bytes), including 4-byte checksum, default 256",
+ type=any_int, default=256)
+parser.add_argument("-s", "--seed", help="Checksum seed value, default 0",
+ type=any_int, default=0)
+args = parser.parse_args()
+
+try:
+ idata = open(args.ifile, "rb").read()
+except:
+ sys.exit("Could not open input file '{}'".format(args.ifile))
+
+if len(idata) >= args.pad - 4:
+ sys.exit("Input file size ({} bytes) too large for final size ({} bytes)".format(len(idata), args.pad))
+
+idata_padded = idata + bytes(args.pad - 4 - len(idata))
+
+# Our bootrom CRC32 is slightly bass-ackward but it's best to work around for now (FIXME)
+# 100% worth it to save two Thumb instructions
+checksum = bitrev(
+ (binascii.crc32(bytes(bitrev(b, 8) for b in idata_padded), args.seed ^ 0xffffffff) ^ 0xffffffff) & 0xffffffff, 32)
+odata = idata_padded + struct.pack("<L", checksum)
+
+try:
+ with open(args.ofile, "w") as ofile:
+ ofile.write("// Padded and checksummed version of: {}\n\n".format(args.ifile))
+ ofile.write(".cpu cortex-m0plus\n")
+ ofile.write(".thumb\n\n")
+ ofile.write(".section .boot2, \"ax\"\n\n")
+ for offs in range(0, len(odata), 16):
+ chunk = odata[offs:min(offs + 16, len(odata))]
+ ofile.write(".byte {}\n".format(", ".join("0x{:02x}".format(b) for b in chunk)))
+except:
+ sys.exit("Could not open output file '{}'".format(args.ofile))
diff --git a/src/rp2_common/cmsis/CMakeLists.txt b/src/rp2_common/cmsis/CMakeLists.txt
new file mode 100644
index 0000000..0d663e1
--- /dev/null
+++ b/src/rp2_common/cmsis/CMakeLists.txt
@@ -0,0 +1,66 @@
+# Deferring this config until we decide how to include other CMSIS libraries... it is likely that we always want to use the stub version of the core
+# at least if the vendor/device is RaspberryPi/RP2040...
+
+## PICO_CMAKE_CONFIG: PICO_CMSIS_PATH, directory to locate CMSIS installation, default="included stub CORE only impl", group=build
+#if (DEFINED PICO_CMSIS_PATH)
+# set(PICO_CMSIS_PATH "${PICO_CMSIS_PATH}" CACHE PATH "Path to the CMSIS tree to use with Raspberry Pi Pico SDK")
+# message("Using specified PICO_CMSIS_PATH for CMSIS ('${PICO_CMSIS_PATH}')")
+#elseif (DEFINED ENV{PICO_CMSIS_PATH})
+# set(PICO_CMSIS_PATH $ENV{PICO_CMSIS_PATH})
+# message("Using PICO_CMSIS_PATH from environment for CMSIS ('${PICO_CMSIS_PATH}')")
+#endif()
+#
+## PICO_CMAKE_CONFIG: PICO_CMSIS_VENDOR, vendor name for CMSIS, default="RaspberryPi", group=build
+#if (DEFINED PICO_CMSIS_VENDOR)
+# set(PICO_CMSIS_VENDOR "${PICO_CMSIS_VENDOR}" CACHE STRING "CMSIS vendor name to use")
+# message("Using specified PICO_CMSIS_VENDOR for CMSIS ('${PICO_CMSIS_VENDOR}')")
+#elseif (DEFINED ENV{PICO_CMSIS_VENDOR})
+# set(PICO_CMSIS_VENDOR $ENV{PICO_CMSIS_VENDOR})
+# message("Using PICO_CMSIS_VENDOR from environment for CMSIS ('${PICO_CMSIS_VENDOR}')")
+#else()
+# set(PICO_CMSIS_VENDOR RaspberryPi)
+#endif()
+#
+## PICO_CMAKE_CONFIG: PICO_CMSIS_DEVICE, device name for CMSIS, default="RP2040", group=build
+#if (DEFINED PICO_CMSIS_DEVICE)
+# set(PICO_CMSIS_DEVICE "${PICO_CMSIS_DEVICE}" CACHE STRING "CMSIS device name to use")
+# message("Using specified PICO_CMSIS_DEVICE for CMSIS ('${PICO_CMSIS_DEVICE}')")
+#elseif (DEFINED ENV{PICO_CMSIS_DEVICE})
+# set(PICO_CMSIS_DEVICE $ENV{PICO_CMSIS_DEVICE})
+# message("Using PICO_CMSIS_DEVICE from environment for CMSIS ('${PICO_CMSIS_DEVICE}')")
+#else()
+# set(PICO_CMSIS_DEVICE RP2040)
+#endif()
+#
+#if (PICO_CMSIS_PATH AND EXISTS ${PICO_CMSIS_PATH}/CMSIS/Core/${PICO_CMSIS_VENDOR}/${PICO_CMSIS_DEVICE})
+# message("CMSIS Core found for path ${PICO_CMSIS_PATH}, vendor ${PICO_CMSIS_VENDOR}, device ${PICO_CMSIS_DEVICE}")
+# set(PICO_CMSIS_CORE_PATH ${PICO_CMSIS_PATH}/CMSIS/Core)
+#elseif (PICO_CMSIS_VENDOR STREQUAL "RaspberryPi" AND PICO_CMSIS_DEVICE STREQUAL "RP2040")
+# set(PICO_CMSIS_CORE_PATH ${CMAKE_CURRENT_LIST_DIR}/stub)
+#elseif (PICO_CMSIS_PATH)
+# message(WARNING "CMSIS core not found in ${PICO_CMSIS_PATH} for vendor ${PICO_CMSIS_VENDOR}, device ${PICO_CMSIS_DEVICE}")
+# set(PICO_CMSIS_CORE_PATH)
+#else()
+# message(WARNING "Non-standard vendor ${PICO_CMSIS_VENDOR} amd device ${PICO_CMSIS_DEVICE} specified, but PICO_CMSIS_PATH was not set")
+#endif()
+
+# ... using these 3 lines instead
+set(PICO_CMSIS_CORE_PATH ${CMAKE_CURRENT_LIST_DIR}/stub)
+set(PICO_CMSIS_VENDOR RaspberryPi)
+set(PICO_CMSIS_DEVICE RP2040)
+
+if (PICO_CMSIS_CORE_PATH)
+ pico_add_impl_library(cmsis_core)
+ target_sources(cmsis_core INTERFACE
+ ${PICO_CMSIS_CORE_PATH}/CMSIS/Device/${PICO_CMSIS_VENDOR}/${PICO_CMSIS_DEVICE}/Source/system_${PICO_CMSIS_DEVICE}.c
+ )
+ target_include_directories(cmsis_core INTERFACE
+ ${PICO_CMSIS_CORE_PATH}/CMSIS/Core/Include
+ ${PICO_CMSIS_CORE_PATH}/CMSIS/Device/${PICO_CMSIS_VENDOR}/${PICO_CMSIS_DEVICE}/Include
+ )
+ target_link_libraries(cmsis_core INTERFACE hardware_clocks pico_platform)
+
+ list(APPEND PICO_RP2040_CONFIG_HEADER_FILES ${CMAKE_CURRENT_LIST_DIR}/include/cmsis/rename_exceptions.h)
+ pico_promote_common_scope_vars()
+endif()
+
diff --git a/src/rp2_common/cmsis/include/cmsis/rename_exceptions.h b/src/rp2_common/cmsis/include/cmsis/rename_exceptions.h
new file mode 100644
index 0000000..a523d27
--- /dev/null
+++ b/src/rp2_common/cmsis/include/cmsis/rename_exceptions.h
@@ -0,0 +1,53 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _CMSIS_RENAME_EXCEPTIONS_H
+#define _CMSIS_RENAME_EXCEPTIONS_H
+
+#if LIB_CMSIS_CORE
+// PICO_CONFIG: PICO_CMSIS_RENAME_EXCEPTIONS, Whether to rename SDK exceptions such as isr_nmi to their CMSIS equivalent i.e. NMI_Handler, type=bool, default=1, group=cmsis_core
+
+// Note that since this header is included at the config stage, if you wish to override this you should do so via build compiler define
+#ifndef PICO_CMSIS_RENAME_EXCEPTIONS
+#define PICO_CMSIS_RENAME_EXCEPTIONS 1
+#endif
+
+#if PICO_CMSIS_RENAME_EXCEPTIONS
+#define isr_nmi NMI_Handler
+#define isr_hardfault HardFault_Handler
+#define isr_svcall SVC_Handler
+#define isr_pendsv PendSV_Handler
+#define isr_systick SysTick_Handler
+#define isr_irq0 TIMER_IRQ_0_Handler
+#define isr_irq1 TIMER_IRQ_1_Handler
+#define isr_irq2 TIMER_IRQ_2_Handler
+#define isr_irq3 TIMER_IRQ_3_Handler
+#define isr_irq4 PWM_IRQ_WRAP_Handler
+#define isr_irq5 USBCTRL_IRQ_Handler
+#define isr_irq6 XIP_IRQ_Handler
+#define isr_irq7 PIO0_IRQ_0_Handler
+#define isr_irq8 PIO0_IRQ_1_Handler
+#define isr_irq9 PIO1_IRQ_0_Handler
+#define isr_irq10 PIO1_IRQ_1_Handler
+#define isr_irq11 DMA_IRQ_0_Handler
+#define isr_irq12 DMA_IRQ_1_Handler
+#define isr_irq13 IO_IRQ_BANK0_Handler
+#define isr_irq14 IO_IRQ_QSPI_Handler
+#define isr_irq15 SIO_IRQ_PROC0_Handler
+#define isr_irq16 SIO_IRQ_PROC1_Handler
+#define isr_irq17 CLOCKS_IRQ_Handler
+#define isr_irq18 SPI0_IRQ_Handler
+#define isr_irq19 SPI1_IRQ_Handler
+#define isr_irq20 UART0_IRQ_Handler
+#define isr_irq21 UART1_IRQ_Handler
+#define isr_irq22 ADC_IRQ_FIFO_Handler
+#define isr_irq23 I2C0_IRQ_Handler
+#define isr_irq24 I2C1_IRQ_Handler
+#define isr_irq25 RTC_IRQ_Handler
+#endif
+
+#endif
+#endif /* _CMSIS_RENAME_EXCEPTIONS_H */
diff --git a/src/rp2_common/cmsis/stub/CMSIS/Core/Include/cmsis_armcc.h b/src/rp2_common/cmsis/stub/CMSIS/Core/Include/cmsis_armcc.h
new file mode 100644
index 0000000..237ff6e
--- /dev/null
+++ b/src/rp2_common/cmsis/stub/CMSIS/Core/Include/cmsis_armcc.h
@@ -0,0 +1,885 @@
+/**************************************************************************//**
+ * @file cmsis_armcc.h
+ * @brief CMSIS compiler ARMCC (Arm Compiler 5) header file
+ * @version V5.2.1
+ * @date 26. March 2020
+ ******************************************************************************/
+/*
+ * Copyright (c) 2009-2020 Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef __CMSIS_ARMCC_H
+#define __CMSIS_ARMCC_H
+
+
+#if defined(__ARMCC_VERSION) && (__ARMCC_VERSION < 400677)
+ #error "Please use Arm Compiler Toolchain V4.0.677 or later!"
+#endif
+
+/* CMSIS compiler control architecture macros */
+#if ((defined (__TARGET_ARCH_6_M ) && (__TARGET_ARCH_6_M == 1)) || \
+ (defined (__TARGET_ARCH_6S_M ) && (__TARGET_ARCH_6S_M == 1)) )
+ #define __ARM_ARCH_6M__ 1
+#endif
+
+#if (defined (__TARGET_ARCH_7_M ) && (__TARGET_ARCH_7_M == 1))
+ #define __ARM_ARCH_7M__ 1
+#endif
+
+#if (defined (__TARGET_ARCH_7E_M) && (__TARGET_ARCH_7E_M == 1))
+ #define __ARM_ARCH_7EM__ 1
+#endif
+
+ /* __ARM_ARCH_8M_BASE__ not applicable */
+ /* __ARM_ARCH_8M_MAIN__ not applicable */
+ /* __ARM_ARCH_8_1M_MAIN__ not applicable */
+
+/* CMSIS compiler control DSP macros */
+#if ((defined (__ARM_ARCH_7EM__) && (__ARM_ARCH_7EM__ == 1)) )
+ #define __ARM_FEATURE_DSP 1
+#endif
+
+/* CMSIS compiler specific defines */
+#ifndef __ASM
+ #define __ASM __asm
+#endif
+#ifndef __INLINE
+ #define __INLINE __inline
+#endif
+#ifndef __STATIC_INLINE
+ #define __STATIC_INLINE static __inline
+#endif
+#ifndef __STATIC_FORCEINLINE
+ #define __STATIC_FORCEINLINE static __forceinline
+#endif
+#ifndef __NO_RETURN
+ #define __NO_RETURN __declspec(noreturn)
+#endif
+#ifndef __USED
+ #define __USED __attribute__((used))
+#endif
+#ifndef __WEAK
+ #define __WEAK __attribute__((weak))
+#endif
+#ifndef __PACKED
+ #define __PACKED __attribute__((packed))
+#endif
+#ifndef __PACKED_STRUCT
+ #define __PACKED_STRUCT __packed struct
+#endif
+#ifndef __PACKED_UNION
+ #define __PACKED_UNION __packed union
+#endif
+#ifndef __UNALIGNED_UINT32 /* deprecated */
+ #define __UNALIGNED_UINT32(x) (*((__packed uint32_t *)(x)))
+#endif
+#ifndef __UNALIGNED_UINT16_WRITE
+ #define __UNALIGNED_UINT16_WRITE(addr, val) ((*((__packed uint16_t *)(addr))) = (val))
+#endif
+#ifndef __UNALIGNED_UINT16_READ
+ #define __UNALIGNED_UINT16_READ(addr) (*((const __packed uint16_t *)(addr)))
+#endif
+#ifndef __UNALIGNED_UINT32_WRITE
+ #define __UNALIGNED_UINT32_WRITE(addr, val) ((*((__packed uint32_t *)(addr))) = (val))
+#endif
+#ifndef __UNALIGNED_UINT32_READ
+ #define __UNALIGNED_UINT32_READ(addr) (*((const __packed uint32_t *)(addr)))
+#endif
+#ifndef __ALIGNED
+ #define __ALIGNED(x) __attribute__((aligned(x)))
+#endif
+#ifndef __RESTRICT
+ #define __RESTRICT __restrict
+#endif
+#ifndef __COMPILER_BARRIER
+ #define __COMPILER_BARRIER() __memory_changed()
+#endif
+
+/* ######################### Startup and Lowlevel Init ######################## */
+
+#ifndef __PROGRAM_START
+#define __PROGRAM_START __main
+#endif
+
+#ifndef __INITIAL_SP
+#define __INITIAL_SP Image$$ARM_LIB_STACK$$ZI$$Limit
+#endif
+
+#ifndef __STACK_LIMIT
+#define __STACK_LIMIT Image$$ARM_LIB_STACK$$ZI$$Base
+#endif
+
+#ifndef __VECTOR_TABLE
+#define __VECTOR_TABLE __Vectors
+#endif
+
+#ifndef __VECTOR_TABLE_ATTRIBUTE
+#define __VECTOR_TABLE_ATTRIBUTE __attribute__((used, section("RESET")))
+#endif
+
+/* ########################### Core Function Access ########################### */
+/** \ingroup CMSIS_Core_FunctionInterface
+ \defgroup CMSIS_Core_RegAccFunctions CMSIS Core Register Access Functions
+ @{
+ */
+
+/**
+ \brief Enable IRQ Interrupts
+ \details Enables IRQ interrupts by clearing the I-bit in the CPSR.
+ Can only be executed in Privileged modes.
+ */
+/* intrinsic void __enable_irq(); */
+
+
+/**
+ \brief Disable IRQ Interrupts
+ \details Disables IRQ interrupts by setting the I-bit in the CPSR.
+ Can only be executed in Privileged modes.
+ */
+/* intrinsic void __disable_irq(); */
+
+/**
+ \brief Get Control Register
+ \details Returns the content of the Control Register.
+ \return Control Register value
+ */
+__STATIC_INLINE uint32_t __get_CONTROL(void)
+{
+ register uint32_t __regControl __ASM("control");
+ return(__regControl);
+}
+
+
+/**
+ \brief Set Control Register
+ \details Writes the given value to the Control Register.
+ \param [in] control Control Register value to set
+ */
+__STATIC_INLINE void __set_CONTROL(uint32_t control)
+{
+ register uint32_t __regControl __ASM("control");
+ __regControl = control;
+}
+
+
+/**
+ \brief Get IPSR Register
+ \details Returns the content of the IPSR Register.
+ \return IPSR Register value
+ */
+__STATIC_INLINE uint32_t __get_IPSR(void)
+{
+ register uint32_t __regIPSR __ASM("ipsr");
+ return(__regIPSR);
+}
+
+
+/**
+ \brief Get APSR Register
+ \details Returns the content of the APSR Register.
+ \return APSR Register value
+ */
+__STATIC_INLINE uint32_t __get_APSR(void)
+{
+ register uint32_t __regAPSR __ASM("apsr");
+ return(__regAPSR);
+}
+
+
+/**
+ \brief Get xPSR Register
+ \details Returns the content of the xPSR Register.
+ \return xPSR Register value
+ */
+__STATIC_INLINE uint32_t __get_xPSR(void)
+{
+ register uint32_t __regXPSR __ASM("xpsr");
+ return(__regXPSR);
+}
+
+
+/**
+ \brief Get Process Stack Pointer
+ \details Returns the current value of the Process Stack Pointer (PSP).
+ \return PSP Register value
+ */
+__STATIC_INLINE uint32_t __get_PSP(void)
+{
+ register uint32_t __regProcessStackPointer __ASM("psp");
+ return(__regProcessStackPointer);
+}
+
+
+/**
+ \brief Set Process Stack Pointer
+ \details Assigns the given value to the Process Stack Pointer (PSP).
+ \param [in] topOfProcStack Process Stack Pointer value to set
+ */
+__STATIC_INLINE void __set_PSP(uint32_t topOfProcStack)
+{
+ register uint32_t __regProcessStackPointer __ASM("psp");
+ __regProcessStackPointer = topOfProcStack;
+}
+
+
+/**
+ \brief Get Main Stack Pointer
+ \details Returns the current value of the Main Stack Pointer (MSP).
+ \return MSP Register value
+ */
+__STATIC_INLINE uint32_t __get_MSP(void)
+{
+ register uint32_t __regMainStackPointer __ASM("msp");
+ return(__regMainStackPointer);
+}
+
+
+/**
+ \brief Set Main Stack Pointer
+ \details Assigns the given value to the Main Stack Pointer (MSP).
+ \param [in] topOfMainStack Main Stack Pointer value to set
+ */
+__STATIC_INLINE void __set_MSP(uint32_t topOfMainStack)
+{
+ register uint32_t __regMainStackPointer __ASM("msp");
+ __regMainStackPointer = topOfMainStack;
+}
+
+
+/**
+ \brief Get Priority Mask
+ \details Returns the current state of the priority mask bit from the Priority Mask Register.
+ \return Priority Mask value
+ */
+__STATIC_INLINE uint32_t __get_PRIMASK(void)
+{
+ register uint32_t __regPriMask __ASM("primask");
+ return(__regPriMask);
+}
+
+
+/**
+ \brief Set Priority Mask
+ \details Assigns the given value to the Priority Mask Register.
+ \param [in] priMask Priority Mask
+ */
+__STATIC_INLINE void __set_PRIMASK(uint32_t priMask)
+{
+ register uint32_t __regPriMask __ASM("primask");
+ __regPriMask = (priMask);
+}
+
+
+#if ((defined (__ARM_ARCH_7M__ ) && (__ARM_ARCH_7M__ == 1)) || \
+ (defined (__ARM_ARCH_7EM__) && (__ARM_ARCH_7EM__ == 1)) )
+
+/**
+ \brief Enable FIQ
+ \details Enables FIQ interrupts by clearing the F-bit in the CPSR.
+ Can only be executed in Privileged modes.
+ */
+#define __enable_fault_irq __enable_fiq
+
+
+/**
+ \brief Disable FIQ
+ \details Disables FIQ interrupts by setting the F-bit in the CPSR.
+ Can only be executed in Privileged modes.
+ */
+#define __disable_fault_irq __disable_fiq
+
+
+/**
+ \brief Get Base Priority
+ \details Returns the current value of the Base Priority register.
+ \return Base Priority register value
+ */
+__STATIC_INLINE uint32_t __get_BASEPRI(void)
+{
+ register uint32_t __regBasePri __ASM("basepri");
+ return(__regBasePri);
+}
+
+
+/**
+ \brief Set Base Priority
+ \details Assigns the given value to the Base Priority register.
+ \param [in] basePri Base Priority value to set
+ */
+__STATIC_INLINE void __set_BASEPRI(uint32_t basePri)
+{
+ register uint32_t __regBasePri __ASM("basepri");
+ __regBasePri = (basePri & 0xFFU);
+}
+
+
+/**
+ \brief Set Base Priority with condition
+ \details Assigns the given value to the Base Priority register only if BASEPRI masking is disabled,
+ or the new value increases the BASEPRI priority level.
+ \param [in] basePri Base Priority value to set
+ */
+__STATIC_INLINE void __set_BASEPRI_MAX(uint32_t basePri)
+{
+ register uint32_t __regBasePriMax __ASM("basepri_max");
+ __regBasePriMax = (basePri & 0xFFU);
+}
+
+
+/**
+ \brief Get Fault Mask
+ \details Returns the current value of the Fault Mask register.
+ \return Fault Mask register value
+ */
+__STATIC_INLINE uint32_t __get_FAULTMASK(void)
+{
+ register uint32_t __regFaultMask __ASM("faultmask");
+ return(__regFaultMask);
+}
+
+
+/**
+ \brief Set Fault Mask
+ \details Assigns the given value to the Fault Mask register.
+ \param [in] faultMask Fault Mask value to set
+ */
+__STATIC_INLINE void __set_FAULTMASK(uint32_t faultMask)
+{
+ register uint32_t __regFaultMask __ASM("faultmask");
+ __regFaultMask = (faultMask & (uint32_t)1U);
+}
+
+#endif /* ((defined (__ARM_ARCH_7M__ ) && (__ARM_ARCH_7M__ == 1)) || \
+ (defined (__ARM_ARCH_7EM__) && (__ARM_ARCH_7EM__ == 1)) ) */
+
+
+/**
+ \brief Get FPSCR
+ \details Returns the current value of the Floating Point Status/Control register.
+ \return Floating Point Status/Control register value
+ */
+__STATIC_INLINE uint32_t __get_FPSCR(void)
+{
+#if ((defined (__FPU_PRESENT) && (__FPU_PRESENT == 1U)) && \
+ (defined (__FPU_USED ) && (__FPU_USED == 1U)) )
+ register uint32_t __regfpscr __ASM("fpscr");
+ return(__regfpscr);
+#else
+ return(0U);
+#endif
+}
+
+
+/**
+ \brief Set FPSCR
+ \details Assigns the given value to the Floating Point Status/Control register.
+ \param [in] fpscr Floating Point Status/Control value to set
+ */
+__STATIC_INLINE void __set_FPSCR(uint32_t fpscr)
+{
+#if ((defined (__FPU_PRESENT) && (__FPU_PRESENT == 1U)) && \
+ (defined (__FPU_USED ) && (__FPU_USED == 1U)) )
+ register uint32_t __regfpscr __ASM("fpscr");
+ __regfpscr = (fpscr);
+#else
+ (void)fpscr;
+#endif
+}
+
+
+/*@} end of CMSIS_Core_RegAccFunctions */
+
+
+/* ########################## Core Instruction Access ######################### */
+/** \defgroup CMSIS_Core_InstructionInterface CMSIS Core Instruction Interface
+ Access to dedicated instructions
+ @{
+*/
+
+/**
+ \brief No Operation
+ \details No Operation does nothing. This instruction can be used for code alignment purposes.
+ */
+#define __NOP __nop
+
+
+/**
+ \brief Wait For Interrupt
+ \details Wait For Interrupt is a hint instruction that suspends execution until one of a number of events occurs.
+ */
+#define __WFI __wfi
+
+
+/**
+ \brief Wait For Event
+ \details Wait For Event is a hint instruction that permits the processor to enter
+ a low-power state until one of a number of events occurs.
+ */
+#define __WFE __wfe
+
+
+/**
+ \brief Send Event
+ \details Send Event is a hint instruction. It causes an event to be signaled to the CPU.
+ */
+#define __SEV __sev
+
+
+/**
+ \brief Instruction Synchronization Barrier
+ \details Instruction Synchronization Barrier flushes the pipeline in the processor,
+ so that all instructions following the ISB are fetched from cache or memory,
+ after the instruction has been completed.
+ */
+#define __ISB() __isb(0xF)
+
+/**
+ \brief Data Synchronization Barrier
+ \details Acts as a special kind of Data Memory Barrier.
+ It completes when all explicit memory accesses before this instruction complete.
+ */
+#define __DSB() __dsb(0xF)
+
+/**
+ \brief Data Memory Barrier
+ \details Ensures the apparent order of the explicit memory operations before
+ and after the instruction, without ensuring their completion.
+ */
+#define __DMB() __dmb(0xF)
+
+
+/**
+ \brief Reverse byte order (32 bit)
+ \details Reverses the byte order in unsigned integer value. For example, 0x12345678 becomes 0x78563412.
+ \param [in] value Value to reverse
+ \return Reversed value
+ */
+#define __REV __rev
+
+
+/**
+ \brief Reverse byte order (16 bit)
+ \details Reverses the byte order within each halfword of a word. For example, 0x12345678 becomes 0x34127856.
+ \param [in] value Value to reverse
+ \return Reversed value
+ */
+#ifndef __NO_EMBEDDED_ASM
+__attribute__((section(".rev16_text"))) __STATIC_INLINE __ASM uint32_t __REV16(uint32_t value)
+{
+ rev16 r0, r0
+ bx lr
+}
+#endif
+
+
+/**
+ \brief Reverse byte order (16 bit)
+ \details Reverses the byte order in a 16-bit value and returns the signed 16-bit result. For example, 0x0080 becomes 0x8000.
+ \param [in] value Value to reverse
+ \return Reversed value
+ */
+#ifndef __NO_EMBEDDED_ASM
+__attribute__((section(".revsh_text"))) __STATIC_INLINE __ASM int16_t __REVSH(int16_t value)
+{
+ revsh r0, r0
+ bx lr
+}
+#endif
+
+
+/**
+ \brief Rotate Right in unsigned value (32 bit)
+ \details Rotate Right (immediate) provides the value of the contents of a register rotated by a variable number of bits.
+ \param [in] op1 Value to rotate
+ \param [in] op2 Number of Bits to rotate
+ \return Rotated value
+ */
+#define __ROR __ror
+
+
+/**
+ \brief Breakpoint
+ \details Causes the processor to enter Debug state.
+ Debug tools can use this to investigate system state when the instruction at a particular address is reached.
+ \param [in] value is ignored by the processor.
+ If required, a debugger can use it to store additional information about the breakpoint.
+ */
+#define __BKPT(value) __breakpoint(value)
+
+
+/**
+ \brief Reverse bit order of value
+ \details Reverses the bit order of the given value.
+ \param [in] value Value to reverse
+ \return Reversed value
+ */
+#if ((defined (__ARM_ARCH_7M__ ) && (__ARM_ARCH_7M__ == 1)) || \
+ (defined (__ARM_ARCH_7EM__) && (__ARM_ARCH_7EM__ == 1)) )
+ #define __RBIT __rbit
+#else
+__attribute__((always_inline)) __STATIC_INLINE uint32_t __RBIT(uint32_t value)
+{
+ uint32_t result;
+ uint32_t s = (4U /*sizeof(v)*/ * 8U) - 1U; /* extra shift needed at end */
+
+ result = value; /* r will be reversed bits of v; first get LSB of v */
+ for (value >>= 1U; value != 0U; value >>= 1U)
+ {
+ result <<= 1U;
+ result |= value & 1U;
+ s--;
+ }
+ result <<= s; /* shift when v's highest bits are zero */
+ return result;
+}
+#endif
+
+
+/**
+ \brief Count leading zeros
+ \details Counts the number of leading zeros of a data value.
+ \param [in] value Value to count the leading zeros
+ \return number of leading zeros in value
+ */
+#define __CLZ __clz
+
+
+#if ((defined (__ARM_ARCH_7M__ ) && (__ARM_ARCH_7M__ == 1)) || \
+ (defined (__ARM_ARCH_7EM__) && (__ARM_ARCH_7EM__ == 1)) )
+
+/**
+ \brief LDR Exclusive (8 bit)
+ \details Executes a exclusive LDR instruction for 8 bit value.
+ \param [in] ptr Pointer to data
+ \return value of type uint8_t at (*ptr)
+ */
+#if defined(__ARMCC_VERSION) && (__ARMCC_VERSION < 5060020)
+ #define __LDREXB(ptr) ((uint8_t ) __ldrex(ptr))
+#else
+ #define __LDREXB(ptr) _Pragma("push") _Pragma("diag_suppress 3731") ((uint8_t ) __ldrex(ptr)) _Pragma("pop")
+#endif
+
+
+/**
+ \brief LDR Exclusive (16 bit)
+ \details Executes a exclusive LDR instruction for 16 bit values.
+ \param [in] ptr Pointer to data
+ \return value of type uint16_t at (*ptr)
+ */
+#if defined(__ARMCC_VERSION) && (__ARMCC_VERSION < 5060020)
+ #define __LDREXH(ptr) ((uint16_t) __ldrex(ptr))
+#else
+ #define __LDREXH(ptr) _Pragma("push") _Pragma("diag_suppress 3731") ((uint16_t) __ldrex(ptr)) _Pragma("pop")
+#endif
+
+
+/**
+ \brief LDR Exclusive (32 bit)
+ \details Executes a exclusive LDR instruction for 32 bit values.
+ \param [in] ptr Pointer to data
+ \return value of type uint32_t at (*ptr)
+ */
+#if defined(__ARMCC_VERSION) && (__ARMCC_VERSION < 5060020)
+ #define __LDREXW(ptr) ((uint32_t ) __ldrex(ptr))
+#else
+ #define __LDREXW(ptr) _Pragma("push") _Pragma("diag_suppress 3731") ((uint32_t ) __ldrex(ptr)) _Pragma("pop")
+#endif
+
+
+/**
+ \brief STR Exclusive (8 bit)
+ \details Executes a exclusive STR instruction for 8 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ \return 0 Function succeeded
+ \return 1 Function failed
+ */
+#if defined(__ARMCC_VERSION) && (__ARMCC_VERSION < 5060020)
+ #define __STREXB(value, ptr) __strex(value, ptr)
+#else
+ #define __STREXB(value, ptr) _Pragma("push") _Pragma("diag_suppress 3731") __strex(value, ptr) _Pragma("pop")
+#endif
+
+
+/**
+ \brief STR Exclusive (16 bit)
+ \details Executes a exclusive STR instruction for 16 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ \return 0 Function succeeded
+ \return 1 Function failed
+ */
+#if defined(__ARMCC_VERSION) && (__ARMCC_VERSION < 5060020)
+ #define __STREXH(value, ptr) __strex(value, ptr)
+#else
+ #define __STREXH(value, ptr) _Pragma("push") _Pragma("diag_suppress 3731") __strex(value, ptr) _Pragma("pop")
+#endif
+
+
+/**
+ \brief STR Exclusive (32 bit)
+ \details Executes a exclusive STR instruction for 32 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ \return 0 Function succeeded
+ \return 1 Function failed
+ */
+#if defined(__ARMCC_VERSION) && (__ARMCC_VERSION < 5060020)
+ #define __STREXW(value, ptr) __strex(value, ptr)
+#else
+ #define __STREXW(value, ptr) _Pragma("push") _Pragma("diag_suppress 3731") __strex(value, ptr) _Pragma("pop")
+#endif
+
+
+/**
+ \brief Remove the exclusive lock
+ \details Removes the exclusive lock which is created by LDREX.
+ */
+#define __CLREX __clrex
+
+
+/**
+ \brief Signed Saturate
+ \details Saturates a signed value.
+ \param [in] value Value to be saturated
+ \param [in] sat Bit position to saturate to (1..32)
+ \return Saturated value
+ */
+#define __SSAT __ssat
+
+
+/**
+ \brief Unsigned Saturate
+ \details Saturates an unsigned value.
+ \param [in] value Value to be saturated
+ \param [in] sat Bit position to saturate to (0..31)
+ \return Saturated value
+ */
+#define __USAT __usat
+
+
+/**
+ \brief Rotate Right with Extend (32 bit)
+ \details Moves each bit of a bitstring right by one bit.
+ The carry input is shifted in at the left end of the bitstring.
+ \param [in] value Value to rotate
+ \return Rotated value
+ */
+#ifndef __NO_EMBEDDED_ASM
+__attribute__((section(".rrx_text"))) __STATIC_INLINE __ASM uint32_t __RRX(uint32_t value)
+{
+ rrx r0, r0
+ bx lr
+}
+#endif
+
+
+/**
+ \brief LDRT Unprivileged (8 bit)
+ \details Executes a Unprivileged LDRT instruction for 8 bit value.
+ \param [in] ptr Pointer to data
+ \return value of type uint8_t at (*ptr)
+ */
+#define __LDRBT(ptr) ((uint8_t ) __ldrt(ptr))
+
+
+/**
+ \brief LDRT Unprivileged (16 bit)
+ \details Executes a Unprivileged LDRT instruction for 16 bit values.
+ \param [in] ptr Pointer to data
+ \return value of type uint16_t at (*ptr)
+ */
+#define __LDRHT(ptr) ((uint16_t) __ldrt(ptr))
+
+
+/**
+ \brief LDRT Unprivileged (32 bit)
+ \details Executes a Unprivileged LDRT instruction for 32 bit values.
+ \param [in] ptr Pointer to data
+ \return value of type uint32_t at (*ptr)
+ */
+#define __LDRT(ptr) ((uint32_t ) __ldrt(ptr))
+
+
+/**
+ \brief STRT Unprivileged (8 bit)
+ \details Executes a Unprivileged STRT instruction for 8 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ */
+#define __STRBT(value, ptr) __strt(value, ptr)
+
+
+/**
+ \brief STRT Unprivileged (16 bit)
+ \details Executes a Unprivileged STRT instruction for 16 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ */
+#define __STRHT(value, ptr) __strt(value, ptr)
+
+
+/**
+ \brief STRT Unprivileged (32 bit)
+ \details Executes a Unprivileged STRT instruction for 32 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ */
+#define __STRT(value, ptr) __strt(value, ptr)
+
+#else /* ((defined (__ARM_ARCH_7M__ ) && (__ARM_ARCH_7M__ == 1)) || \
+ (defined (__ARM_ARCH_7EM__) && (__ARM_ARCH_7EM__ == 1)) ) */
+
+/**
+ \brief Signed Saturate
+ \details Saturates a signed value.
+ \param [in] value Value to be saturated
+ \param [in] sat Bit position to saturate to (1..32)
+ \return Saturated value
+ */
+__attribute__((always_inline)) __STATIC_INLINE int32_t __SSAT(int32_t val, uint32_t sat)
+{
+ if ((sat >= 1U) && (sat <= 32U))
+ {
+ const int32_t max = (int32_t)((1U << (sat - 1U)) - 1U);
+ const int32_t min = -1 - max ;
+ if (val > max)
+ {
+ return max;
+ }
+ else if (val < min)
+ {
+ return min;
+ }
+ }
+ return val;
+}
+
+/**
+ \brief Unsigned Saturate
+ \details Saturates an unsigned value.
+ \param [in] value Value to be saturated
+ \param [in] sat Bit position to saturate to (0..31)
+ \return Saturated value
+ */
+__attribute__((always_inline)) __STATIC_INLINE uint32_t __USAT(int32_t val, uint32_t sat)
+{
+ if (sat <= 31U)
+ {
+ const uint32_t max = ((1U << sat) - 1U);
+ if (val > (int32_t)max)
+ {
+ return max;
+ }
+ else if (val < 0)
+ {
+ return 0U;
+ }
+ }
+ return (uint32_t)val;
+}
+
+#endif /* ((defined (__ARM_ARCH_7M__ ) && (__ARM_ARCH_7M__ == 1)) || \
+ (defined (__ARM_ARCH_7EM__) && (__ARM_ARCH_7EM__ == 1)) ) */
+
+/*@}*/ /* end of group CMSIS_Core_InstructionInterface */
+
+
+/* ################### Compiler specific Intrinsics ########################### */
+/** \defgroup CMSIS_SIMD_intrinsics CMSIS SIMD Intrinsics
+ Access to dedicated SIMD instructions
+ @{
+*/
+
+#if ((defined (__ARM_ARCH_7EM__) && (__ARM_ARCH_7EM__ == 1)) )
+
+#define __SADD8 __sadd8
+#define __QADD8 __qadd8
+#define __SHADD8 __shadd8
+#define __UADD8 __uadd8
+#define __UQADD8 __uqadd8
+#define __UHADD8 __uhadd8
+#define __SSUB8 __ssub8
+#define __QSUB8 __qsub8
+#define __SHSUB8 __shsub8
+#define __USUB8 __usub8
+#define __UQSUB8 __uqsub8
+#define __UHSUB8 __uhsub8
+#define __SADD16 __sadd16
+#define __QADD16 __qadd16
+#define __SHADD16 __shadd16
+#define __UADD16 __uadd16
+#define __UQADD16 __uqadd16
+#define __UHADD16 __uhadd16
+#define __SSUB16 __ssub16
+#define __QSUB16 __qsub16
+#define __SHSUB16 __shsub16
+#define __USUB16 __usub16
+#define __UQSUB16 __uqsub16
+#define __UHSUB16 __uhsub16
+#define __SASX __sasx
+#define __QASX __qasx
+#define __SHASX __shasx
+#define __UASX __uasx
+#define __UQASX __uqasx
+#define __UHASX __uhasx
+#define __SSAX __ssax
+#define __QSAX __qsax
+#define __SHSAX __shsax
+#define __USAX __usax
+#define __UQSAX __uqsax
+#define __UHSAX __uhsax
+#define __USAD8 __usad8
+#define __USADA8 __usada8
+#define __SSAT16 __ssat16
+#define __USAT16 __usat16
+#define __UXTB16 __uxtb16
+#define __UXTAB16 __uxtab16
+#define __SXTB16 __sxtb16
+#define __SXTAB16 __sxtab16
+#define __SMUAD __smuad
+#define __SMUADX __smuadx
+#define __SMLAD __smlad
+#define __SMLADX __smladx
+#define __SMLALD __smlald
+#define __SMLALDX __smlaldx
+#define __SMUSD __smusd
+#define __SMUSDX __smusdx
+#define __SMLSD __smlsd
+#define __SMLSDX __smlsdx
+#define __SMLSLD __smlsld
+#define __SMLSLDX __smlsldx
+#define __SEL __sel
+#define __QADD __qadd
+#define __QSUB __qsub
+
+#define __PKHBT(ARG1,ARG2,ARG3) ( ((((uint32_t)(ARG1)) ) & 0x0000FFFFUL) | \
+ ((((uint32_t)(ARG2)) << (ARG3)) & 0xFFFF0000UL) )
+
+#define __PKHTB(ARG1,ARG2,ARG3) ( ((((uint32_t)(ARG1)) ) & 0xFFFF0000UL) | \
+ ((((uint32_t)(ARG2)) >> (ARG3)) & 0x0000FFFFUL) )
+
+#define __SMMLA(ARG1,ARG2,ARG3) ( (int32_t)((((int64_t)(ARG1) * (ARG2)) + \
+ ((int64_t)(ARG3) << 32U) ) >> 32U))
+
+#define __SXTB16_RORn(ARG1, ARG2) __SXTB16(__ROR(ARG1, ARG2))
+
+#endif /* ((defined (__ARM_ARCH_7EM__) && (__ARM_ARCH_7EM__ == 1)) ) */
+/*@} end of group CMSIS_SIMD_intrinsics */
+
+
+#endif /* __CMSIS_ARMCC_H */
diff --git a/src/rp2_common/cmsis/stub/CMSIS/Core/Include/cmsis_armclang.h b/src/rp2_common/cmsis/stub/CMSIS/Core/Include/cmsis_armclang.h
new file mode 100644
index 0000000..90de9db
--- /dev/null
+++ b/src/rp2_common/cmsis/stub/CMSIS/Core/Include/cmsis_armclang.h
@@ -0,0 +1,1467 @@
+/**************************************************************************//**
+ * @file cmsis_armclang.h
+ * @brief CMSIS compiler armclang (Arm Compiler 6) header file
+ * @version V5.3.1
+ * @date 26. March 2020
+ ******************************************************************************/
+/*
+ * Copyright (c) 2009-2020 Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/*lint -esym(9058, IRQn)*/ /* disable MISRA 2012 Rule 2.4 for IRQn */
+
+#ifndef __CMSIS_ARMCLANG_H
+#define __CMSIS_ARMCLANG_H
+
+#pragma clang system_header /* treat file as system include file */
+
+#ifndef __ARM_COMPAT_H
+#include <arm_compat.h> /* Compatibility header for Arm Compiler 5 intrinsics */
+#endif
+
+/* CMSIS compiler specific defines */
+#ifndef __ASM
+ #define __ASM __asm
+#endif
+#ifndef __INLINE
+ #define __INLINE __inline
+#endif
+#ifndef __STATIC_INLINE
+ #define __STATIC_INLINE static __inline
+#endif
+#ifndef __STATIC_FORCEINLINE
+ #define __STATIC_FORCEINLINE __attribute__((always_inline)) static __inline
+#endif
+#ifndef __NO_RETURN
+ #define __NO_RETURN __attribute__((__noreturn__))
+#endif
+#ifndef __USED
+ #define __USED __attribute__((used))
+#endif
+#ifndef __WEAK
+ #define __WEAK __attribute__((weak))
+#endif
+#ifndef __PACKED
+ #define __PACKED __attribute__((packed, aligned(1)))
+#endif
+#ifndef __PACKED_STRUCT
+ #define __PACKED_STRUCT struct __attribute__((packed, aligned(1)))
+#endif
+#ifndef __PACKED_UNION
+ #define __PACKED_UNION union __attribute__((packed, aligned(1)))
+#endif
+#ifndef __UNALIGNED_UINT32 /* deprecated */
+ #pragma clang diagnostic push
+ #pragma clang diagnostic ignored "-Wpacked"
+/*lint -esym(9058, T_UINT32)*/ /* disable MISRA 2012 Rule 2.4 for T_UINT32 */
+ struct __attribute__((packed)) T_UINT32 { uint32_t v; };
+ #pragma clang diagnostic pop
+ #define __UNALIGNED_UINT32(x) (((struct T_UINT32 *)(x))->v)
+#endif
+#ifndef __UNALIGNED_UINT16_WRITE
+ #pragma clang diagnostic push
+ #pragma clang diagnostic ignored "-Wpacked"
+/*lint -esym(9058, T_UINT16_WRITE)*/ /* disable MISRA 2012 Rule 2.4 for T_UINT16_WRITE */
+ __PACKED_STRUCT T_UINT16_WRITE { uint16_t v; };
+ #pragma clang diagnostic pop
+ #define __UNALIGNED_UINT16_WRITE(addr, val) (void)((((struct T_UINT16_WRITE *)(void *)(addr))->v) = (val))
+#endif
+#ifndef __UNALIGNED_UINT16_READ
+ #pragma clang diagnostic push
+ #pragma clang diagnostic ignored "-Wpacked"
+/*lint -esym(9058, T_UINT16_READ)*/ /* disable MISRA 2012 Rule 2.4 for T_UINT16_READ */
+ __PACKED_STRUCT T_UINT16_READ { uint16_t v; };
+ #pragma clang diagnostic pop
+ #define __UNALIGNED_UINT16_READ(addr) (((const struct T_UINT16_READ *)(const void *)(addr))->v)
+#endif
+#ifndef __UNALIGNED_UINT32_WRITE
+ #pragma clang diagnostic push
+ #pragma clang diagnostic ignored "-Wpacked"
+/*lint -esym(9058, T_UINT32_WRITE)*/ /* disable MISRA 2012 Rule 2.4 for T_UINT32_WRITE */
+ __PACKED_STRUCT T_UINT32_WRITE { uint32_t v; };
+ #pragma clang diagnostic pop
+ #define __UNALIGNED_UINT32_WRITE(addr, val) (void)((((struct T_UINT32_WRITE *)(void *)(addr))->v) = (val))
+#endif
+#ifndef __UNALIGNED_UINT32_READ
+ #pragma clang diagnostic push
+ #pragma clang diagnostic ignored "-Wpacked"
+/*lint -esym(9058, T_UINT32_READ)*/ /* disable MISRA 2012 Rule 2.4 for T_UINT32_READ */
+ __PACKED_STRUCT T_UINT32_READ { uint32_t v; };
+ #pragma clang diagnostic pop
+ #define __UNALIGNED_UINT32_READ(addr) (((const struct T_UINT32_READ *)(const void *)(addr))->v)
+#endif
+#ifndef __ALIGNED
+ #define __ALIGNED(x) __attribute__((aligned(x)))
+#endif
+#ifndef __RESTRICT
+ #define __RESTRICT __restrict
+#endif
+#ifndef __COMPILER_BARRIER
+ #define __COMPILER_BARRIER() __ASM volatile("":::"memory")
+#endif
+
+/* ######################### Startup and Lowlevel Init ######################## */
+
+#ifndef __PROGRAM_START
+#define __PROGRAM_START __main
+#endif
+
+#ifndef __INITIAL_SP
+#define __INITIAL_SP Image$$ARM_LIB_STACK$$ZI$$Limit
+#endif
+
+#ifndef __STACK_LIMIT
+#define __STACK_LIMIT Image$$ARM_LIB_STACK$$ZI$$Base
+#endif
+
+#ifndef __VECTOR_TABLE
+#define __VECTOR_TABLE __Vectors
+#endif
+
+#ifndef __VECTOR_TABLE_ATTRIBUTE
+#define __VECTOR_TABLE_ATTRIBUTE __attribute__((used, section("RESET")))
+#endif
+
+/* ########################### Core Function Access ########################### */
+/** \ingroup CMSIS_Core_FunctionInterface
+ \defgroup CMSIS_Core_RegAccFunctions CMSIS Core Register Access Functions
+ @{
+ */
+
+/**
+ \brief Enable IRQ Interrupts
+ \details Enables IRQ interrupts by clearing the I-bit in the CPSR.
+ Can only be executed in Privileged modes.
+ */
+/* intrinsic void __enable_irq(); see arm_compat.h */
+
+
+/**
+ \brief Disable IRQ Interrupts
+ \details Disables IRQ interrupts by setting the I-bit in the CPSR.
+ Can only be executed in Privileged modes.
+ */
+/* intrinsic void __disable_irq(); see arm_compat.h */
+
+
+/**
+ \brief Get Control Register
+ \details Returns the content of the Control Register.
+ \return Control Register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_CONTROL(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, control" : "=r" (result) );
+ return(result);
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Get Control Register (non-secure)
+ \details Returns the content of the non-secure Control Register when in secure mode.
+ \return non-secure Control Register value
+ */
+__STATIC_FORCEINLINE uint32_t __TZ_get_CONTROL_NS(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, control_ns" : "=r" (result) );
+ return(result);
+}
+#endif
+
+
+/**
+ \brief Set Control Register
+ \details Writes the given value to the Control Register.
+ \param [in] control Control Register value to set
+ */
+__STATIC_FORCEINLINE void __set_CONTROL(uint32_t control)
+{
+ __ASM volatile ("MSR control, %0" : : "r" (control) : "memory");
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Set Control Register (non-secure)
+ \details Writes the given value to the non-secure Control Register when in secure state.
+ \param [in] control Control Register value to set
+ */
+__STATIC_FORCEINLINE void __TZ_set_CONTROL_NS(uint32_t control)
+{
+ __ASM volatile ("MSR control_ns, %0" : : "r" (control) : "memory");
+}
+#endif
+
+
+/**
+ \brief Get IPSR Register
+ \details Returns the content of the IPSR Register.
+ \return IPSR Register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_IPSR(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, ipsr" : "=r" (result) );
+ return(result);
+}
+
+
+/**
+ \brief Get APSR Register
+ \details Returns the content of the APSR Register.
+ \return APSR Register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_APSR(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, apsr" : "=r" (result) );
+ return(result);
+}
+
+
+/**
+ \brief Get xPSR Register
+ \details Returns the content of the xPSR Register.
+ \return xPSR Register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_xPSR(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, xpsr" : "=r" (result) );
+ return(result);
+}
+
+
+/**
+ \brief Get Process Stack Pointer
+ \details Returns the current value of the Process Stack Pointer (PSP).
+ \return PSP Register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_PSP(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, psp" : "=r" (result) );
+ return(result);
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Get Process Stack Pointer (non-secure)
+ \details Returns the current value of the non-secure Process Stack Pointer (PSP) when in secure state.
+ \return PSP Register value
+ */
+__STATIC_FORCEINLINE uint32_t __TZ_get_PSP_NS(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, psp_ns" : "=r" (result) );
+ return(result);
+}
+#endif
+
+
+/**
+ \brief Set Process Stack Pointer
+ \details Assigns the given value to the Process Stack Pointer (PSP).
+ \param [in] topOfProcStack Process Stack Pointer value to set
+ */
+__STATIC_FORCEINLINE void __set_PSP(uint32_t topOfProcStack)
+{
+ __ASM volatile ("MSR psp, %0" : : "r" (topOfProcStack) : );
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Set Process Stack Pointer (non-secure)
+ \details Assigns the given value to the non-secure Process Stack Pointer (PSP) when in secure state.
+ \param [in] topOfProcStack Process Stack Pointer value to set
+ */
+__STATIC_FORCEINLINE void __TZ_set_PSP_NS(uint32_t topOfProcStack)
+{
+ __ASM volatile ("MSR psp_ns, %0" : : "r" (topOfProcStack) : );
+}
+#endif
+
+
+/**
+ \brief Get Main Stack Pointer
+ \details Returns the current value of the Main Stack Pointer (MSP).
+ \return MSP Register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_MSP(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, msp" : "=r" (result) );
+ return(result);
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Get Main Stack Pointer (non-secure)
+ \details Returns the current value of the non-secure Main Stack Pointer (MSP) when in secure state.
+ \return MSP Register value
+ */
+__STATIC_FORCEINLINE uint32_t __TZ_get_MSP_NS(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, msp_ns" : "=r" (result) );
+ return(result);
+}
+#endif
+
+
+/**
+ \brief Set Main Stack Pointer
+ \details Assigns the given value to the Main Stack Pointer (MSP).
+ \param [in] topOfMainStack Main Stack Pointer value to set
+ */
+__STATIC_FORCEINLINE void __set_MSP(uint32_t topOfMainStack)
+{
+ __ASM volatile ("MSR msp, %0" : : "r" (topOfMainStack) : );
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Set Main Stack Pointer (non-secure)
+ \details Assigns the given value to the non-secure Main Stack Pointer (MSP) when in secure state.
+ \param [in] topOfMainStack Main Stack Pointer value to set
+ */
+__STATIC_FORCEINLINE void __TZ_set_MSP_NS(uint32_t topOfMainStack)
+{
+ __ASM volatile ("MSR msp_ns, %0" : : "r" (topOfMainStack) : );
+}
+#endif
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Get Stack Pointer (non-secure)
+ \details Returns the current value of the non-secure Stack Pointer (SP) when in secure state.
+ \return SP Register value
+ */
+__STATIC_FORCEINLINE uint32_t __TZ_get_SP_NS(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, sp_ns" : "=r" (result) );
+ return(result);
+}
+
+
+/**
+ \brief Set Stack Pointer (non-secure)
+ \details Assigns the given value to the non-secure Stack Pointer (SP) when in secure state.
+ \param [in] topOfStack Stack Pointer value to set
+ */
+__STATIC_FORCEINLINE void __TZ_set_SP_NS(uint32_t topOfStack)
+{
+ __ASM volatile ("MSR sp_ns, %0" : : "r" (topOfStack) : );
+}
+#endif
+
+
+/**
+ \brief Get Priority Mask
+ \details Returns the current state of the priority mask bit from the Priority Mask Register.
+ \return Priority Mask value
+ */
+__STATIC_FORCEINLINE uint32_t __get_PRIMASK(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, primask" : "=r" (result) );
+ return(result);
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Get Priority Mask (non-secure)
+ \details Returns the current state of the non-secure priority mask bit from the Priority Mask Register when in secure state.
+ \return Priority Mask value
+ */
+__STATIC_FORCEINLINE uint32_t __TZ_get_PRIMASK_NS(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, primask_ns" : "=r" (result) );
+ return(result);
+}
+#endif
+
+
+/**
+ \brief Set Priority Mask
+ \details Assigns the given value to the Priority Mask Register.
+ \param [in] priMask Priority Mask
+ */
+__STATIC_FORCEINLINE void __set_PRIMASK(uint32_t priMask)
+{
+ __ASM volatile ("MSR primask, %0" : : "r" (priMask) : "memory");
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Set Priority Mask (non-secure)
+ \details Assigns the given value to the non-secure Priority Mask Register when in secure state.
+ \param [in] priMask Priority Mask
+ */
+__STATIC_FORCEINLINE void __TZ_set_PRIMASK_NS(uint32_t priMask)
+{
+ __ASM volatile ("MSR primask_ns, %0" : : "r" (priMask) : "memory");
+}
+#endif
+
+
+#if ((defined (__ARM_ARCH_7M__ ) && (__ARM_ARCH_7M__ == 1)) || \
+ (defined (__ARM_ARCH_7EM__ ) && (__ARM_ARCH_7EM__ == 1)) || \
+ (defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) || \
+ (defined (__ARM_ARCH_8_1M_MAIN__) && (__ARM_ARCH_8_1M_MAIN__ == 1)) )
+/**
+ \brief Enable FIQ
+ \details Enables FIQ interrupts by clearing the F-bit in the CPSR.
+ Can only be executed in Privileged modes.
+ */
+#define __enable_fault_irq __enable_fiq /* see arm_compat.h */
+
+
+/**
+ \brief Disable FIQ
+ \details Disables FIQ interrupts by setting the F-bit in the CPSR.
+ Can only be executed in Privileged modes.
+ */
+#define __disable_fault_irq __disable_fiq /* see arm_compat.h */
+
+
+/**
+ \brief Get Base Priority
+ \details Returns the current value of the Base Priority register.
+ \return Base Priority register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_BASEPRI(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, basepri" : "=r" (result) );
+ return(result);
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Get Base Priority (non-secure)
+ \details Returns the current value of the non-secure Base Priority register when in secure state.
+ \return Base Priority register value
+ */
+__STATIC_FORCEINLINE uint32_t __TZ_get_BASEPRI_NS(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, basepri_ns" : "=r" (result) );
+ return(result);
+}
+#endif
+
+
+/**
+ \brief Set Base Priority
+ \details Assigns the given value to the Base Priority register.
+ \param [in] basePri Base Priority value to set
+ */
+__STATIC_FORCEINLINE void __set_BASEPRI(uint32_t basePri)
+{
+ __ASM volatile ("MSR basepri, %0" : : "r" (basePri) : "memory");
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Set Base Priority (non-secure)
+ \details Assigns the given value to the non-secure Base Priority register when in secure state.
+ \param [in] basePri Base Priority value to set
+ */
+__STATIC_FORCEINLINE void __TZ_set_BASEPRI_NS(uint32_t basePri)
+{
+ __ASM volatile ("MSR basepri_ns, %0" : : "r" (basePri) : "memory");
+}
+#endif
+
+
+/**
+ \brief Set Base Priority with condition
+ \details Assigns the given value to the Base Priority register only if BASEPRI masking is disabled,
+ or the new value increases the BASEPRI priority level.
+ \param [in] basePri Base Priority value to set
+ */
+__STATIC_FORCEINLINE void __set_BASEPRI_MAX(uint32_t basePri)
+{
+ __ASM volatile ("MSR basepri_max, %0" : : "r" (basePri) : "memory");
+}
+
+
+/**
+ \brief Get Fault Mask
+ \details Returns the current value of the Fault Mask register.
+ \return Fault Mask register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_FAULTMASK(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, faultmask" : "=r" (result) );
+ return(result);
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Get Fault Mask (non-secure)
+ \details Returns the current value of the non-secure Fault Mask register when in secure state.
+ \return Fault Mask register value
+ */
+__STATIC_FORCEINLINE uint32_t __TZ_get_FAULTMASK_NS(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, faultmask_ns" : "=r" (result) );
+ return(result);
+}
+#endif
+
+
+/**
+ \brief Set Fault Mask
+ \details Assigns the given value to the Fault Mask register.
+ \param [in] faultMask Fault Mask value to set
+ */
+__STATIC_FORCEINLINE void __set_FAULTMASK(uint32_t faultMask)
+{
+ __ASM volatile ("MSR faultmask, %0" : : "r" (faultMask) : "memory");
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Set Fault Mask (non-secure)
+ \details Assigns the given value to the non-secure Fault Mask register when in secure state.
+ \param [in] faultMask Fault Mask value to set
+ */
+__STATIC_FORCEINLINE void __TZ_set_FAULTMASK_NS(uint32_t faultMask)
+{
+ __ASM volatile ("MSR faultmask_ns, %0" : : "r" (faultMask) : "memory");
+}
+#endif
+
+#endif /* ((defined (__ARM_ARCH_7M__ ) && (__ARM_ARCH_7M__ == 1)) || \
+ (defined (__ARM_ARCH_7EM__ ) && (__ARM_ARCH_7EM__ == 1)) || \
+ (defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) || \
+ (defined (__ARM_ARCH_8_1M_MAIN__) && (__ARM_ARCH_8_1M_MAIN__ == 1)) ) */
+
+
+#if ((defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) || \
+ (defined (__ARM_ARCH_8M_BASE__ ) && (__ARM_ARCH_8M_BASE__ == 1)) || \
+ (defined (__ARM_ARCH_8_1M_MAIN__) && (__ARM_ARCH_8_1M_MAIN__ == 1)) )
+
+/**
+ \brief Get Process Stack Pointer Limit
+ Devices without ARMv8-M Main Extensions (i.e. Cortex-M23) lack the non-secure
+ Stack Pointer Limit register hence zero is returned always in non-secure
+ mode.
+
+ \details Returns the current value of the Process Stack Pointer Limit (PSPLIM).
+ \return PSPLIM Register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_PSPLIM(void)
+{
+#if (!((defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) || \
+ (defined (__ARM_ARCH_8_1M_MAIN__ ) && (__ARM_ARCH_8_1M_MAIN__ == 1)) ) && \
+ (!defined (__ARM_FEATURE_CMSE) || (__ARM_FEATURE_CMSE < 3)))
+ // without main extensions, the non-secure PSPLIM is RAZ/WI
+ return 0U;
+#else
+ uint32_t result;
+ __ASM volatile ("MRS %0, psplim" : "=r" (result) );
+ return result;
+#endif
+}
+
+#if (defined (__ARM_FEATURE_CMSE) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Get Process Stack Pointer Limit (non-secure)
+ Devices without ARMv8-M Main Extensions (i.e. Cortex-M23) lack the non-secure
+ Stack Pointer Limit register hence zero is returned always in non-secure
+ mode.
+
+ \details Returns the current value of the non-secure Process Stack Pointer Limit (PSPLIM) when in secure state.
+ \return PSPLIM Register value
+ */
+__STATIC_FORCEINLINE uint32_t __TZ_get_PSPLIM_NS(void)
+{
+#if (!((defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) || \
+ (defined (__ARM_ARCH_8_1M_MAIN__ ) && (__ARM_ARCH_8_1M_MAIN__ == 1)) ) )
+ // without main extensions, the non-secure PSPLIM is RAZ/WI
+ return 0U;
+#else
+ uint32_t result;
+ __ASM volatile ("MRS %0, psplim_ns" : "=r" (result) );
+ return result;
+#endif
+}
+#endif
+
+
+/**
+ \brief Set Process Stack Pointer Limit
+ Devices without ARMv8-M Main Extensions (i.e. Cortex-M23) lack the non-secure
+ Stack Pointer Limit register hence the write is silently ignored in non-secure
+ mode.
+
+ \details Assigns the given value to the Process Stack Pointer Limit (PSPLIM).
+ \param [in] ProcStackPtrLimit Process Stack Pointer Limit value to set
+ */
+__STATIC_FORCEINLINE void __set_PSPLIM(uint32_t ProcStackPtrLimit)
+{
+#if (!((defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) || \
+ (defined (__ARM_ARCH_8_1M_MAIN__ ) && (__ARM_ARCH_8_1M_MAIN__ == 1)) ) && \
+ (!defined (__ARM_FEATURE_CMSE) || (__ARM_FEATURE_CMSE < 3)))
+ // without main extensions, the non-secure PSPLIM is RAZ/WI
+ (void)ProcStackPtrLimit;
+#else
+ __ASM volatile ("MSR psplim, %0" : : "r" (ProcStackPtrLimit));
+#endif
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Set Process Stack Pointer (non-secure)
+ Devices without ARMv8-M Main Extensions (i.e. Cortex-M23) lack the non-secure
+ Stack Pointer Limit register hence the write is silently ignored in non-secure
+ mode.
+
+ \details Assigns the given value to the non-secure Process Stack Pointer Limit (PSPLIM) when in secure state.
+ \param [in] ProcStackPtrLimit Process Stack Pointer Limit value to set
+ */
+__STATIC_FORCEINLINE void __TZ_set_PSPLIM_NS(uint32_t ProcStackPtrLimit)
+{
+#if (!((defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) || \
+ (defined (__ARM_ARCH_8_1M_MAIN__ ) && (__ARM_ARCH_8_1M_MAIN__ == 1)) ) )
+ // without main extensions, the non-secure PSPLIM is RAZ/WI
+ (void)ProcStackPtrLimit;
+#else
+ __ASM volatile ("MSR psplim_ns, %0\n" : : "r" (ProcStackPtrLimit));
+#endif
+}
+#endif
+
+
+/**
+ \brief Get Main Stack Pointer Limit
+ Devices without ARMv8-M Main Extensions (i.e. Cortex-M23) lack the non-secure
+ Stack Pointer Limit register hence zero is returned always.
+
+ \details Returns the current value of the Main Stack Pointer Limit (MSPLIM).
+ \return MSPLIM Register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_MSPLIM(void)
+{
+#if (!((defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) || \
+ (defined (__ARM_ARCH_8_1M_MAIN__ ) && (__ARM_ARCH_8_1M_MAIN__ == 1)) ) && \
+ (!defined (__ARM_FEATURE_CMSE) || (__ARM_FEATURE_CMSE < 3)))
+ // without main extensions, the non-secure MSPLIM is RAZ/WI
+ return 0U;
+#else
+ uint32_t result;
+ __ASM volatile ("MRS %0, msplim" : "=r" (result) );
+ return result;
+#endif
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Get Main Stack Pointer Limit (non-secure)
+ Devices without ARMv8-M Main Extensions (i.e. Cortex-M23) lack the non-secure
+ Stack Pointer Limit register hence zero is returned always.
+
+ \details Returns the current value of the non-secure Main Stack Pointer Limit(MSPLIM) when in secure state.
+ \return MSPLIM Register value
+ */
+__STATIC_FORCEINLINE uint32_t __TZ_get_MSPLIM_NS(void)
+{
+#if (!((defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) || \
+ (defined (__ARM_ARCH_8_1M_MAIN__ ) && (__ARM_ARCH_8_1M_MAIN__ == 1)) ) )
+ // without main extensions, the non-secure MSPLIM is RAZ/WI
+ return 0U;
+#else
+ uint32_t result;
+ __ASM volatile ("MRS %0, msplim_ns" : "=r" (result) );
+ return result;
+#endif
+}
+#endif
+
+
+/**
+ \brief Set Main Stack Pointer Limit
+ Devices without ARMv8-M Main Extensions (i.e. Cortex-M23) lack the non-secure
+ Stack Pointer Limit register hence the write is silently ignored.
+
+ \details Assigns the given value to the Main Stack Pointer Limit (MSPLIM).
+ \param [in] MainStackPtrLimit Main Stack Pointer Limit value to set
+ */
+__STATIC_FORCEINLINE void __set_MSPLIM(uint32_t MainStackPtrLimit)
+{
+#if (!((defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) || \
+ (defined (__ARM_ARCH_8_1M_MAIN__ ) && (__ARM_ARCH_8_1M_MAIN__ == 1)) ) && \
+ (!defined (__ARM_FEATURE_CMSE) || (__ARM_FEATURE_CMSE < 3)))
+ // without main extensions, the non-secure MSPLIM is RAZ/WI
+ (void)MainStackPtrLimit;
+#else
+ __ASM volatile ("MSR msplim, %0" : : "r" (MainStackPtrLimit));
+#endif
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Set Main Stack Pointer Limit (non-secure)
+ Devices without ARMv8-M Main Extensions (i.e. Cortex-M23) lack the non-secure
+ Stack Pointer Limit register hence the write is silently ignored.
+
+ \details Assigns the given value to the non-secure Main Stack Pointer Limit (MSPLIM) when in secure state.
+ \param [in] MainStackPtrLimit Main Stack Pointer value to set
+ */
+__STATIC_FORCEINLINE void __TZ_set_MSPLIM_NS(uint32_t MainStackPtrLimit)
+{
+#if (!((defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) || \
+ (defined (__ARM_ARCH_8_1M_MAIN__ ) && (__ARM_ARCH_8_1M_MAIN__ == 1)) ) )
+ // without main extensions, the non-secure MSPLIM is RAZ/WI
+ (void)MainStackPtrLimit;
+#else
+ __ASM volatile ("MSR msplim_ns, %0" : : "r" (MainStackPtrLimit));
+#endif
+}
+#endif
+
+#endif /* ((defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) || \
+ (defined (__ARM_ARCH_8M_BASE__ ) && (__ARM_ARCH_8M_BASE__ == 1)) || \
+ (defined (__ARM_ARCH_8_1M_MAIN__) && (__ARM_ARCH_8_1M_MAIN__ == 1)) ) */
+
+/**
+ \brief Get FPSCR
+ \details Returns the current value of the Floating Point Status/Control register.
+ \return Floating Point Status/Control register value
+ */
+#if ((defined (__FPU_PRESENT) && (__FPU_PRESENT == 1U)) && \
+ (defined (__FPU_USED ) && (__FPU_USED == 1U)) )
+#define __get_FPSCR (uint32_t)__builtin_arm_get_fpscr
+#else
+#define __get_FPSCR() ((uint32_t)0U)
+#endif
+
+/**
+ \brief Set FPSCR
+ \details Assigns the given value to the Floating Point Status/Control register.
+ \param [in] fpscr Floating Point Status/Control value to set
+ */
+#if ((defined (__FPU_PRESENT) && (__FPU_PRESENT == 1U)) && \
+ (defined (__FPU_USED ) && (__FPU_USED == 1U)) )
+#define __set_FPSCR __builtin_arm_set_fpscr
+#else
+#define __set_FPSCR(x) ((void)(x))
+#endif
+
+
+/*@} end of CMSIS_Core_RegAccFunctions */
+
+
+/* ########################## Core Instruction Access ######################### */
+/** \defgroup CMSIS_Core_InstructionInterface CMSIS Core Instruction Interface
+ Access to dedicated instructions
+ @{
+*/
+
+/* Define macros for porting to both thumb1 and thumb2.
+ * For thumb1, use low register (r0-r7), specified by constraint "l"
+ * Otherwise, use general registers, specified by constraint "r" */
+#if defined (__thumb__) && !defined (__thumb2__)
+#define __CMSIS_GCC_OUT_REG(r) "=l" (r)
+#define __CMSIS_GCC_RW_REG(r) "+l" (r)
+#define __CMSIS_GCC_USE_REG(r) "l" (r)
+#else
+#define __CMSIS_GCC_OUT_REG(r) "=r" (r)
+#define __CMSIS_GCC_RW_REG(r) "+r" (r)
+#define __CMSIS_GCC_USE_REG(r) "r" (r)
+#endif
+
+/**
+ \brief No Operation
+ \details No Operation does nothing. This instruction can be used for code alignment purposes.
+ */
+#define __NOP __builtin_arm_nop
+
+/**
+ \brief Wait For Interrupt
+ \details Wait For Interrupt is a hint instruction that suspends execution until one of a number of events occurs.
+ */
+#define __WFI __builtin_arm_wfi
+
+
+/**
+ \brief Wait For Event
+ \details Wait For Event is a hint instruction that permits the processor to enter
+ a low-power state until one of a number of events occurs.
+ */
+#define __WFE __builtin_arm_wfe
+
+
+/**
+ \brief Send Event
+ \details Send Event is a hint instruction. It causes an event to be signaled to the CPU.
+ */
+#define __SEV __builtin_arm_sev
+
+
+/**
+ \brief Instruction Synchronization Barrier
+ \details Instruction Synchronization Barrier flushes the pipeline in the processor,
+ so that all instructions following the ISB are fetched from cache or memory,
+ after the instruction has been completed.
+ */
+#define __ISB() __builtin_arm_isb(0xF)
+
+/**
+ \brief Data Synchronization Barrier
+ \details Acts as a special kind of Data Memory Barrier.
+ It completes when all explicit memory accesses before this instruction complete.
+ */
+#define __DSB() __builtin_arm_dsb(0xF)
+
+
+/**
+ \brief Data Memory Barrier
+ \details Ensures the apparent order of the explicit memory operations before
+ and after the instruction, without ensuring their completion.
+ */
+#define __DMB() __builtin_arm_dmb(0xF)
+
+
+/**
+ \brief Reverse byte order (32 bit)
+ \details Reverses the byte order in unsigned integer value. For example, 0x12345678 becomes 0x78563412.
+ \param [in] value Value to reverse
+ \return Reversed value
+ */
+#define __REV(value) __builtin_bswap32(value)
+
+
+/**
+ \brief Reverse byte order (16 bit)
+ \details Reverses the byte order within each halfword of a word. For example, 0x12345678 becomes 0x34127856.
+ \param [in] value Value to reverse
+ \return Reversed value
+ */
+#define __REV16(value) __ROR(__REV(value), 16)
+
+
+/**
+ \brief Reverse byte order (16 bit)
+ \details Reverses the byte order in a 16-bit value and returns the signed 16-bit result. For example, 0x0080 becomes 0x8000.
+ \param [in] value Value to reverse
+ \return Reversed value
+ */
+#define __REVSH(value) (int16_t)__builtin_bswap16(value)
+
+
+/**
+ \brief Rotate Right in unsigned value (32 bit)
+ \details Rotate Right (immediate) provides the value of the contents of a register rotated by a variable number of bits.
+ \param [in] op1 Value to rotate
+ \param [in] op2 Number of Bits to rotate
+ \return Rotated value
+ */
+__STATIC_FORCEINLINE uint32_t __ROR(uint32_t op1, uint32_t op2)
+{
+ op2 %= 32U;
+ if (op2 == 0U)
+ {
+ return op1;
+ }
+ return (op1 >> op2) | (op1 << (32U - op2));
+}
+
+
+/**
+ \brief Breakpoint
+ \details Causes the processor to enter Debug state.
+ Debug tools can use this to investigate system state when the instruction at a particular address is reached.
+ \param [in] value is ignored by the processor.
+ If required, a debugger can use it to store additional information about the breakpoint.
+ */
+#define __BKPT(value) __ASM volatile ("bkpt "#value)
+
+
+/**
+ \brief Reverse bit order of value
+ \details Reverses the bit order of the given value.
+ \param [in] value Value to reverse
+ \return Reversed value
+ */
+#define __RBIT __builtin_arm_rbit
+
+/**
+ \brief Count leading zeros
+ \details Counts the number of leading zeros of a data value.
+ \param [in] value Value to count the leading zeros
+ \return number of leading zeros in value
+ */
+__STATIC_FORCEINLINE uint8_t __CLZ(uint32_t value)
+{
+ /* Even though __builtin_clz produces a CLZ instruction on ARM, formally
+ __builtin_clz(0) is undefined behaviour, so handle this case specially.
+ This guarantees ARM-compatible results if happening to compile on a non-ARM
+ target, and ensures the compiler doesn't decide to activate any
+ optimisations using the logic "value was passed to __builtin_clz, so it
+ is non-zero".
+ ARM Compiler 6.10 and possibly earlier will optimise this test away, leaving a
+ single CLZ instruction.
+ */
+ if (value == 0U)
+ {
+ return 32U;
+ }
+ return __builtin_clz(value);
+}
+
+
+#if ((defined (__ARM_ARCH_7M__ ) && (__ARM_ARCH_7M__ == 1)) || \
+ (defined (__ARM_ARCH_7EM__ ) && (__ARM_ARCH_7EM__ == 1)) || \
+ (defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) || \
+ (defined (__ARM_ARCH_8M_BASE__ ) && (__ARM_ARCH_8M_BASE__ == 1)) || \
+ (defined (__ARM_ARCH_8_1M_MAIN__) && (__ARM_ARCH_8_1M_MAIN__ == 1)) )
+
+/**
+ \brief LDR Exclusive (8 bit)
+ \details Executes a exclusive LDR instruction for 8 bit value.
+ \param [in] ptr Pointer to data
+ \return value of type uint8_t at (*ptr)
+ */
+#define __LDREXB (uint8_t)__builtin_arm_ldrex
+
+
+/**
+ \brief LDR Exclusive (16 bit)
+ \details Executes a exclusive LDR instruction for 16 bit values.
+ \param [in] ptr Pointer to data
+ \return value of type uint16_t at (*ptr)
+ */
+#define __LDREXH (uint16_t)__builtin_arm_ldrex
+
+
+/**
+ \brief LDR Exclusive (32 bit)
+ \details Executes a exclusive LDR instruction for 32 bit values.
+ \param [in] ptr Pointer to data
+ \return value of type uint32_t at (*ptr)
+ */
+#define __LDREXW (uint32_t)__builtin_arm_ldrex
+
+
+/**
+ \brief STR Exclusive (8 bit)
+ \details Executes a exclusive STR instruction for 8 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ \return 0 Function succeeded
+ \return 1 Function failed
+ */
+#define __STREXB (uint32_t)__builtin_arm_strex
+
+
+/**
+ \brief STR Exclusive (16 bit)
+ \details Executes a exclusive STR instruction for 16 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ \return 0 Function succeeded
+ \return 1 Function failed
+ */
+#define __STREXH (uint32_t)__builtin_arm_strex
+
+
+/**
+ \brief STR Exclusive (32 bit)
+ \details Executes a exclusive STR instruction for 32 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ \return 0 Function succeeded
+ \return 1 Function failed
+ */
+#define __STREXW (uint32_t)__builtin_arm_strex
+
+
+/**
+ \brief Remove the exclusive lock
+ \details Removes the exclusive lock which is created by LDREX.
+ */
+#define __CLREX __builtin_arm_clrex
+
+#endif /* ((defined (__ARM_ARCH_7M__ ) && (__ARM_ARCH_7M__ == 1)) || \
+ (defined (__ARM_ARCH_7EM__ ) && (__ARM_ARCH_7EM__ == 1)) || \
+ (defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) || \
+ (defined (__ARM_ARCH_8M_BASE__ ) && (__ARM_ARCH_8M_BASE__ == 1)) || \
+ (defined (__ARM_ARCH_8_1M_MAIN__) && (__ARM_ARCH_8_1M_MAIN__ == 1)) ) */
+
+
+#if ((defined (__ARM_ARCH_7M__ ) && (__ARM_ARCH_7M__ == 1)) || \
+ (defined (__ARM_ARCH_7EM__ ) && (__ARM_ARCH_7EM__ == 1)) || \
+ (defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) || \
+ (defined (__ARM_ARCH_8_1M_MAIN__) && (__ARM_ARCH_8_1M_MAIN__ == 1)) )
+
+/**
+ \brief Signed Saturate
+ \details Saturates a signed value.
+ \param [in] value Value to be saturated
+ \param [in] sat Bit position to saturate to (1..32)
+ \return Saturated value
+ */
+#define __SSAT __builtin_arm_ssat
+
+
+/**
+ \brief Unsigned Saturate
+ \details Saturates an unsigned value.
+ \param [in] value Value to be saturated
+ \param [in] sat Bit position to saturate to (0..31)
+ \return Saturated value
+ */
+#define __USAT __builtin_arm_usat
+
+
+/**
+ \brief Rotate Right with Extend (32 bit)
+ \details Moves each bit of a bitstring right by one bit.
+ The carry input is shifted in at the left end of the bitstring.
+ \param [in] value Value to rotate
+ \return Rotated value
+ */
+__STATIC_FORCEINLINE uint32_t __RRX(uint32_t value)
+{
+ uint32_t result;
+
+ __ASM volatile ("rrx %0, %1" : __CMSIS_GCC_OUT_REG (result) : __CMSIS_GCC_USE_REG (value) );
+ return(result);
+}
+
+
+/**
+ \brief LDRT Unprivileged (8 bit)
+ \details Executes a Unprivileged LDRT instruction for 8 bit value.
+ \param [in] ptr Pointer to data
+ \return value of type uint8_t at (*ptr)
+ */
+__STATIC_FORCEINLINE uint8_t __LDRBT(volatile uint8_t *ptr)
+{
+ uint32_t result;
+
+ __ASM volatile ("ldrbt %0, %1" : "=r" (result) : "Q" (*ptr) );
+ return ((uint8_t) result); /* Add explicit type cast here */
+}
+
+
+/**
+ \brief LDRT Unprivileged (16 bit)
+ \details Executes a Unprivileged LDRT instruction for 16 bit values.
+ \param [in] ptr Pointer to data
+ \return value of type uint16_t at (*ptr)
+ */
+__STATIC_FORCEINLINE uint16_t __LDRHT(volatile uint16_t *ptr)
+{
+ uint32_t result;
+
+ __ASM volatile ("ldrht %0, %1" : "=r" (result) : "Q" (*ptr) );
+ return ((uint16_t) result); /* Add explicit type cast here */
+}
+
+
+/**
+ \brief LDRT Unprivileged (32 bit)
+ \details Executes a Unprivileged LDRT instruction for 32 bit values.
+ \param [in] ptr Pointer to data
+ \return value of type uint32_t at (*ptr)
+ */
+__STATIC_FORCEINLINE uint32_t __LDRT(volatile uint32_t *ptr)
+{
+ uint32_t result;
+
+ __ASM volatile ("ldrt %0, %1" : "=r" (result) : "Q" (*ptr) );
+ return(result);
+}
+
+
+/**
+ \brief STRT Unprivileged (8 bit)
+ \details Executes a Unprivileged STRT instruction for 8 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ */
+__STATIC_FORCEINLINE void __STRBT(uint8_t value, volatile uint8_t *ptr)
+{
+ __ASM volatile ("strbt %1, %0" : "=Q" (*ptr) : "r" ((uint32_t)value) );
+}
+
+
+/**
+ \brief STRT Unprivileged (16 bit)
+ \details Executes a Unprivileged STRT instruction for 16 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ */
+__STATIC_FORCEINLINE void __STRHT(uint16_t value, volatile uint16_t *ptr)
+{
+ __ASM volatile ("strht %1, %0" : "=Q" (*ptr) : "r" ((uint32_t)value) );
+}
+
+
+/**
+ \brief STRT Unprivileged (32 bit)
+ \details Executes a Unprivileged STRT instruction for 32 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ */
+__STATIC_FORCEINLINE void __STRT(uint32_t value, volatile uint32_t *ptr)
+{
+ __ASM volatile ("strt %1, %0" : "=Q" (*ptr) : "r" (value) );
+}
+
+#else /* ((defined (__ARM_ARCH_7M__ ) && (__ARM_ARCH_7M__ == 1)) || \
+ (defined (__ARM_ARCH_7EM__ ) && (__ARM_ARCH_7EM__ == 1)) || \
+ (defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) || \
+ (defined (__ARM_ARCH_8_1M_MAIN__) && (__ARM_ARCH_8_1M_MAIN__ == 1)) ) */
+
+/**
+ \brief Signed Saturate
+ \details Saturates a signed value.
+ \param [in] value Value to be saturated
+ \param [in] sat Bit position to saturate to (1..32)
+ \return Saturated value
+ */
+__STATIC_FORCEINLINE int32_t __SSAT(int32_t val, uint32_t sat)
+{
+ if ((sat >= 1U) && (sat <= 32U))
+ {
+ const int32_t max = (int32_t)((1U << (sat - 1U)) - 1U);
+ const int32_t min = -1 - max ;
+ if (val > max)
+ {
+ return max;
+ }
+ else if (val < min)
+ {
+ return min;
+ }
+ }
+ return val;
+}
+
+/**
+ \brief Unsigned Saturate
+ \details Saturates an unsigned value.
+ \param [in] value Value to be saturated
+ \param [in] sat Bit position to saturate to (0..31)
+ \return Saturated value
+ */
+__STATIC_FORCEINLINE uint32_t __USAT(int32_t val, uint32_t sat)
+{
+ if (sat <= 31U)
+ {
+ const uint32_t max = ((1U << sat) - 1U);
+ if (val > (int32_t)max)
+ {
+ return max;
+ }
+ else if (val < 0)
+ {
+ return 0U;
+ }
+ }
+ return (uint32_t)val;
+}
+
+#endif /* ((defined (__ARM_ARCH_7M__ ) && (__ARM_ARCH_7M__ == 1)) || \
+ (defined (__ARM_ARCH_7EM__ ) && (__ARM_ARCH_7EM__ == 1)) || \
+ (defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) || \
+ (defined (__ARM_ARCH_8_1M_MAIN__) && (__ARM_ARCH_8_1M_MAIN__ == 1)) ) */
+
+
+#if ((defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) || \
+ (defined (__ARM_ARCH_8M_BASE__ ) && (__ARM_ARCH_8M_BASE__ == 1)) || \
+ (defined (__ARM_ARCH_8_1M_MAIN__) && (__ARM_ARCH_8_1M_MAIN__ == 1)) )
+
+/**
+ \brief Load-Acquire (8 bit)
+ \details Executes a LDAB instruction for 8 bit value.
+ \param [in] ptr Pointer to data
+ \return value of type uint8_t at (*ptr)
+ */
+__STATIC_FORCEINLINE uint8_t __LDAB(volatile uint8_t *ptr)
+{
+ uint32_t result;
+
+ __ASM volatile ("ldab %0, %1" : "=r" (result) : "Q" (*ptr) : "memory" );
+ return ((uint8_t) result);
+}
+
+
+/**
+ \brief Load-Acquire (16 bit)
+ \details Executes a LDAH instruction for 16 bit values.
+ \param [in] ptr Pointer to data
+ \return value of type uint16_t at (*ptr)
+ */
+__STATIC_FORCEINLINE uint16_t __LDAH(volatile uint16_t *ptr)
+{
+ uint32_t result;
+
+ __ASM volatile ("ldah %0, %1" : "=r" (result) : "Q" (*ptr) : "memory" );
+ return ((uint16_t) result);
+}
+
+
+/**
+ \brief Load-Acquire (32 bit)
+ \details Executes a LDA instruction for 32 bit values.
+ \param [in] ptr Pointer to data
+ \return value of type uint32_t at (*ptr)
+ */
+__STATIC_FORCEINLINE uint32_t __LDA(volatile uint32_t *ptr)
+{
+ uint32_t result;
+
+ __ASM volatile ("lda %0, %1" : "=r" (result) : "Q" (*ptr) : "memory" );
+ return(result);
+}
+
+
+/**
+ \brief Store-Release (8 bit)
+ \details Executes a STLB instruction for 8 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ */
+__STATIC_FORCEINLINE void __STLB(uint8_t value, volatile uint8_t *ptr)
+{
+ __ASM volatile ("stlb %1, %0" : "=Q" (*ptr) : "r" ((uint32_t)value) : "memory" );
+}
+
+
+/**
+ \brief Store-Release (16 bit)
+ \details Executes a STLH instruction for 16 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ */
+__STATIC_FORCEINLINE void __STLH(uint16_t value, volatile uint16_t *ptr)
+{
+ __ASM volatile ("stlh %1, %0" : "=Q" (*ptr) : "r" ((uint32_t)value) : "memory" );
+}
+
+
+/**
+ \brief Store-Release (32 bit)
+ \details Executes a STL instruction for 32 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ */
+__STATIC_FORCEINLINE void __STL(uint32_t value, volatile uint32_t *ptr)
+{
+ __ASM volatile ("stl %1, %0" : "=Q" (*ptr) : "r" ((uint32_t)value) : "memory" );
+}
+
+
+/**
+ \brief Load-Acquire Exclusive (8 bit)
+ \details Executes a LDAB exclusive instruction for 8 bit value.
+ \param [in] ptr Pointer to data
+ \return value of type uint8_t at (*ptr)
+ */
+#define __LDAEXB (uint8_t)__builtin_arm_ldaex
+
+
+/**
+ \brief Load-Acquire Exclusive (16 bit)
+ \details Executes a LDAH exclusive instruction for 16 bit values.
+ \param [in] ptr Pointer to data
+ \return value of type uint16_t at (*ptr)
+ */
+#define __LDAEXH (uint16_t)__builtin_arm_ldaex
+
+
+/**
+ \brief Load-Acquire Exclusive (32 bit)
+ \details Executes a LDA exclusive instruction for 32 bit values.
+ \param [in] ptr Pointer to data
+ \return value of type uint32_t at (*ptr)
+ */
+#define __LDAEX (uint32_t)__builtin_arm_ldaex
+
+
+/**
+ \brief Store-Release Exclusive (8 bit)
+ \details Executes a STLB exclusive instruction for 8 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ \return 0 Function succeeded
+ \return 1 Function failed
+ */
+#define __STLEXB (uint32_t)__builtin_arm_stlex
+
+
+/**
+ \brief Store-Release Exclusive (16 bit)
+ \details Executes a STLH exclusive instruction for 16 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ \return 0 Function succeeded
+ \return 1 Function failed
+ */
+#define __STLEXH (uint32_t)__builtin_arm_stlex
+
+
+/**
+ \brief Store-Release Exclusive (32 bit)
+ \details Executes a STL exclusive instruction for 32 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ \return 0 Function succeeded
+ \return 1 Function failed
+ */
+#define __STLEX (uint32_t)__builtin_arm_stlex
+
+#endif /* ((defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) || \
+ (defined (__ARM_ARCH_8M_BASE__ ) && (__ARM_ARCH_8M_BASE__ == 1)) || \
+ (defined (__ARM_ARCH_8_1M_MAIN__) && (__ARM_ARCH_8_1M_MAIN__ == 1)) ) */
+
+/*@}*/ /* end of group CMSIS_Core_InstructionInterface */
+
+
+/* ################### Compiler specific Intrinsics ########################### */
+/** \defgroup CMSIS_SIMD_intrinsics CMSIS SIMD Intrinsics
+ Access to dedicated SIMD instructions
+ @{
+*/
+
+#if (defined (__ARM_FEATURE_DSP) && (__ARM_FEATURE_DSP == 1))
+
+#define __SADD8 __builtin_arm_sadd8
+#define __QADD8 __builtin_arm_qadd8
+#define __SHADD8 __builtin_arm_shadd8
+#define __UADD8 __builtin_arm_uadd8
+#define __UQADD8 __builtin_arm_uqadd8
+#define __UHADD8 __builtin_arm_uhadd8
+#define __SSUB8 __builtin_arm_ssub8
+#define __QSUB8 __builtin_arm_qsub8
+#define __SHSUB8 __builtin_arm_shsub8
+#define __USUB8 __builtin_arm_usub8
+#define __UQSUB8 __builtin_arm_uqsub8
+#define __UHSUB8 __builtin_arm_uhsub8
+#define __SADD16 __builtin_arm_sadd16
+#define __QADD16 __builtin_arm_qadd16
+#define __SHADD16 __builtin_arm_shadd16
+#define __UADD16 __builtin_arm_uadd16
+#define __UQADD16 __builtin_arm_uqadd16
+#define __UHADD16 __builtin_arm_uhadd16
+#define __SSUB16 __builtin_arm_ssub16
+#define __QSUB16 __builtin_arm_qsub16
+#define __SHSUB16 __builtin_arm_shsub16
+#define __USUB16 __builtin_arm_usub16
+#define __UQSUB16 __builtin_arm_uqsub16
+#define __UHSUB16 __builtin_arm_uhsub16
+#define __SASX __builtin_arm_sasx
+#define __QASX __builtin_arm_qasx
+#define __SHASX __builtin_arm_shasx
+#define __UASX __builtin_arm_uasx
+#define __UQASX __builtin_arm_uqasx
+#define __UHASX __builtin_arm_uhasx
+#define __SSAX __builtin_arm_ssax
+#define __QSAX __builtin_arm_qsax
+#define __SHSAX __builtin_arm_shsax
+#define __USAX __builtin_arm_usax
+#define __UQSAX __builtin_arm_uqsax
+#define __UHSAX __builtin_arm_uhsax
+#define __USAD8 __builtin_arm_usad8
+#define __USADA8 __builtin_arm_usada8
+#define __SSAT16 __builtin_arm_ssat16
+#define __USAT16 __builtin_arm_usat16
+#define __UXTB16 __builtin_arm_uxtb16
+#define __UXTAB16 __builtin_arm_uxtab16
+#define __SXTB16 __builtin_arm_sxtb16
+#define __SXTAB16 __builtin_arm_sxtab16
+#define __SMUAD __builtin_arm_smuad
+#define __SMUADX __builtin_arm_smuadx
+#define __SMLAD __builtin_arm_smlad
+#define __SMLADX __builtin_arm_smladx
+#define __SMLALD __builtin_arm_smlald
+#define __SMLALDX __builtin_arm_smlaldx
+#define __SMUSD __builtin_arm_smusd
+#define __SMUSDX __builtin_arm_smusdx
+#define __SMLSD __builtin_arm_smlsd
+#define __SMLSDX __builtin_arm_smlsdx
+#define __SMLSLD __builtin_arm_smlsld
+#define __SMLSLDX __builtin_arm_smlsldx
+#define __SEL __builtin_arm_sel
+#define __QADD __builtin_arm_qadd
+#define __QSUB __builtin_arm_qsub
+
+#define __PKHBT(ARG1,ARG2,ARG3) ( ((((uint32_t)(ARG1)) ) & 0x0000FFFFUL) | \
+ ((((uint32_t)(ARG2)) << (ARG3)) & 0xFFFF0000UL) )
+
+#define __PKHTB(ARG1,ARG2,ARG3) ( ((((uint32_t)(ARG1)) ) & 0xFFFF0000UL) | \
+ ((((uint32_t)(ARG2)) >> (ARG3)) & 0x0000FFFFUL) )
+
+#define __SXTB16_RORn(ARG1, ARG2) __SXTB16(__ROR(ARG1, ARG2))
+
+__STATIC_FORCEINLINE int32_t __SMMLA (int32_t op1, int32_t op2, int32_t op3)
+{
+ int32_t result;
+
+ __ASM volatile ("smmla %0, %1, %2, %3" : "=r" (result): "r" (op1), "r" (op2), "r" (op3) );
+ return(result);
+}
+
+#endif /* (__ARM_FEATURE_DSP == 1) */
+/*@} end of group CMSIS_SIMD_intrinsics */
+
+
+#endif /* __CMSIS_ARMCLANG_H */
diff --git a/src/rp2_common/cmsis/stub/CMSIS/Core/Include/cmsis_armclang_ltm.h b/src/rp2_common/cmsis/stub/CMSIS/Core/Include/cmsis_armclang_ltm.h
new file mode 100644
index 0000000..0e5c734
--- /dev/null
+++ b/src/rp2_common/cmsis/stub/CMSIS/Core/Include/cmsis_armclang_ltm.h
@@ -0,0 +1,1893 @@
+/**************************************************************************//**
+ * @file cmsis_armclang_ltm.h
+ * @brief CMSIS compiler armclang (Arm Compiler 6) header file
+ * @version V1.3.0
+ * @date 26. March 2020
+ ******************************************************************************/
+/*
+ * Copyright (c) 2018-2020 Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/*lint -esym(9058, IRQn)*/ /* disable MISRA 2012 Rule 2.4 for IRQn */
+
+#ifndef __CMSIS_ARMCLANG_H
+#define __CMSIS_ARMCLANG_H
+
+#pragma clang system_header /* treat file as system include file */
+
+#ifndef __ARM_COMPAT_H
+#include <arm_compat.h> /* Compatibility header for Arm Compiler 5 intrinsics */
+#endif
+
+/* CMSIS compiler specific defines */
+#ifndef __ASM
+ #define __ASM __asm
+#endif
+#ifndef __INLINE
+ #define __INLINE __inline
+#endif
+#ifndef __STATIC_INLINE
+ #define __STATIC_INLINE static __inline
+#endif
+#ifndef __STATIC_FORCEINLINE
+ #define __STATIC_FORCEINLINE __attribute__((always_inline)) static __inline
+#endif
+#ifndef __NO_RETURN
+ #define __NO_RETURN __attribute__((__noreturn__))
+#endif
+#ifndef __USED
+ #define __USED __attribute__((used))
+#endif
+#ifndef __WEAK
+ #define __WEAK __attribute__((weak))
+#endif
+#ifndef __PACKED
+ #define __PACKED __attribute__((packed, aligned(1)))
+#endif
+#ifndef __PACKED_STRUCT
+ #define __PACKED_STRUCT struct __attribute__((packed, aligned(1)))
+#endif
+#ifndef __PACKED_UNION
+ #define __PACKED_UNION union __attribute__((packed, aligned(1)))
+#endif
+#ifndef __UNALIGNED_UINT32 /* deprecated */
+ #pragma clang diagnostic push
+ #pragma clang diagnostic ignored "-Wpacked"
+/*lint -esym(9058, T_UINT32)*/ /* disable MISRA 2012 Rule 2.4 for T_UINT32 */
+ struct __attribute__((packed)) T_UINT32 { uint32_t v; };
+ #pragma clang diagnostic pop
+ #define __UNALIGNED_UINT32(x) (((struct T_UINT32 *)(x))->v)
+#endif
+#ifndef __UNALIGNED_UINT16_WRITE
+ #pragma clang diagnostic push
+ #pragma clang diagnostic ignored "-Wpacked"
+/*lint -esym(9058, T_UINT16_WRITE)*/ /* disable MISRA 2012 Rule 2.4 for T_UINT16_WRITE */
+ __PACKED_STRUCT T_UINT16_WRITE { uint16_t v; };
+ #pragma clang diagnostic pop
+ #define __UNALIGNED_UINT16_WRITE(addr, val) (void)((((struct T_UINT16_WRITE *)(void *)(addr))->v) = (val))
+#endif
+#ifndef __UNALIGNED_UINT16_READ
+ #pragma clang diagnostic push
+ #pragma clang diagnostic ignored "-Wpacked"
+/*lint -esym(9058, T_UINT16_READ)*/ /* disable MISRA 2012 Rule 2.4 for T_UINT16_READ */
+ __PACKED_STRUCT T_UINT16_READ { uint16_t v; };
+ #pragma clang diagnostic pop
+ #define __UNALIGNED_UINT16_READ(addr) (((const struct T_UINT16_READ *)(const void *)(addr))->v)
+#endif
+#ifndef __UNALIGNED_UINT32_WRITE
+ #pragma clang diagnostic push
+ #pragma clang diagnostic ignored "-Wpacked"
+/*lint -esym(9058, T_UINT32_WRITE)*/ /* disable MISRA 2012 Rule 2.4 for T_UINT32_WRITE */
+ __PACKED_STRUCT T_UINT32_WRITE { uint32_t v; };
+ #pragma clang diagnostic pop
+ #define __UNALIGNED_UINT32_WRITE(addr, val) (void)((((struct T_UINT32_WRITE *)(void *)(addr))->v) = (val))
+#endif
+#ifndef __UNALIGNED_UINT32_READ
+ #pragma clang diagnostic push
+ #pragma clang diagnostic ignored "-Wpacked"
+/*lint -esym(9058, T_UINT32_READ)*/ /* disable MISRA 2012 Rule 2.4 for T_UINT32_READ */
+ __PACKED_STRUCT T_UINT32_READ { uint32_t v; };
+ #pragma clang diagnostic pop
+ #define __UNALIGNED_UINT32_READ(addr) (((const struct T_UINT32_READ *)(const void *)(addr))->v)
+#endif
+#ifndef __ALIGNED
+ #define __ALIGNED(x) __attribute__((aligned(x)))
+#endif
+#ifndef __RESTRICT
+ #define __RESTRICT __restrict
+#endif
+#ifndef __COMPILER_BARRIER
+ #define __COMPILER_BARRIER() __ASM volatile("":::"memory")
+#endif
+
+/* ######################### Startup and Lowlevel Init ######################## */
+
+#ifndef __PROGRAM_START
+#define __PROGRAM_START __main
+#endif
+
+#ifndef __INITIAL_SP
+#define __INITIAL_SP Image$$ARM_LIB_STACK$$ZI$$Limit
+#endif
+
+#ifndef __STACK_LIMIT
+#define __STACK_LIMIT Image$$ARM_LIB_STACK$$ZI$$Base
+#endif
+
+#ifndef __VECTOR_TABLE
+#define __VECTOR_TABLE __Vectors
+#endif
+
+#ifndef __VECTOR_TABLE_ATTRIBUTE
+#define __VECTOR_TABLE_ATTRIBUTE __attribute__((used, section("RESET")))
+#endif
+
+
+/* ########################### Core Function Access ########################### */
+/** \ingroup CMSIS_Core_FunctionInterface
+ \defgroup CMSIS_Core_RegAccFunctions CMSIS Core Register Access Functions
+ @{
+ */
+
+/**
+ \brief Enable IRQ Interrupts
+ \details Enables IRQ interrupts by clearing the I-bit in the CPSR.
+ Can only be executed in Privileged modes.
+ */
+/* intrinsic void __enable_irq(); see arm_compat.h */
+
+
+/**
+ \brief Disable IRQ Interrupts
+ \details Disables IRQ interrupts by setting the I-bit in the CPSR.
+ Can only be executed in Privileged modes.
+ */
+/* intrinsic void __disable_irq(); see arm_compat.h */
+
+
+/**
+ \brief Get Control Register
+ \details Returns the content of the Control Register.
+ \return Control Register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_CONTROL(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, control" : "=r" (result) );
+ return(result);
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Get Control Register (non-secure)
+ \details Returns the content of the non-secure Control Register when in secure mode.
+ \return non-secure Control Register value
+ */
+__STATIC_FORCEINLINE uint32_t __TZ_get_CONTROL_NS(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, control_ns" : "=r" (result) );
+ return(result);
+}
+#endif
+
+
+/**
+ \brief Set Control Register
+ \details Writes the given value to the Control Register.
+ \param [in] control Control Register value to set
+ */
+__STATIC_FORCEINLINE void __set_CONTROL(uint32_t control)
+{
+ __ASM volatile ("MSR control, %0" : : "r" (control) : "memory");
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Set Control Register (non-secure)
+ \details Writes the given value to the non-secure Control Register when in secure state.
+ \param [in] control Control Register value to set
+ */
+__STATIC_FORCEINLINE void __TZ_set_CONTROL_NS(uint32_t control)
+{
+ __ASM volatile ("MSR control_ns, %0" : : "r" (control) : "memory");
+}
+#endif
+
+
+/**
+ \brief Get IPSR Register
+ \details Returns the content of the IPSR Register.
+ \return IPSR Register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_IPSR(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, ipsr" : "=r" (result) );
+ return(result);
+}
+
+
+/**
+ \brief Get APSR Register
+ \details Returns the content of the APSR Register.
+ \return APSR Register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_APSR(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, apsr" : "=r" (result) );
+ return(result);
+}
+
+
+/**
+ \brief Get xPSR Register
+ \details Returns the content of the xPSR Register.
+ \return xPSR Register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_xPSR(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, xpsr" : "=r" (result) );
+ return(result);
+}
+
+
+/**
+ \brief Get Process Stack Pointer
+ \details Returns the current value of the Process Stack Pointer (PSP).
+ \return PSP Register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_PSP(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, psp" : "=r" (result) );
+ return(result);
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Get Process Stack Pointer (non-secure)
+ \details Returns the current value of the non-secure Process Stack Pointer (PSP) when in secure state.
+ \return PSP Register value
+ */
+__STATIC_FORCEINLINE uint32_t __TZ_get_PSP_NS(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, psp_ns" : "=r" (result) );
+ return(result);
+}
+#endif
+
+
+/**
+ \brief Set Process Stack Pointer
+ \details Assigns the given value to the Process Stack Pointer (PSP).
+ \param [in] topOfProcStack Process Stack Pointer value to set
+ */
+__STATIC_FORCEINLINE void __set_PSP(uint32_t topOfProcStack)
+{
+ __ASM volatile ("MSR psp, %0" : : "r" (topOfProcStack) : );
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Set Process Stack Pointer (non-secure)
+ \details Assigns the given value to the non-secure Process Stack Pointer (PSP) when in secure state.
+ \param [in] topOfProcStack Process Stack Pointer value to set
+ */
+__STATIC_FORCEINLINE void __TZ_set_PSP_NS(uint32_t topOfProcStack)
+{
+ __ASM volatile ("MSR psp_ns, %0" : : "r" (topOfProcStack) : );
+}
+#endif
+
+
+/**
+ \brief Get Main Stack Pointer
+ \details Returns the current value of the Main Stack Pointer (MSP).
+ \return MSP Register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_MSP(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, msp" : "=r" (result) );
+ return(result);
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Get Main Stack Pointer (non-secure)
+ \details Returns the current value of the non-secure Main Stack Pointer (MSP) when in secure state.
+ \return MSP Register value
+ */
+__STATIC_FORCEINLINE uint32_t __TZ_get_MSP_NS(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, msp_ns" : "=r" (result) );
+ return(result);
+}
+#endif
+
+
+/**
+ \brief Set Main Stack Pointer
+ \details Assigns the given value to the Main Stack Pointer (MSP).
+ \param [in] topOfMainStack Main Stack Pointer value to set
+ */
+__STATIC_FORCEINLINE void __set_MSP(uint32_t topOfMainStack)
+{
+ __ASM volatile ("MSR msp, %0" : : "r" (topOfMainStack) : );
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Set Main Stack Pointer (non-secure)
+ \details Assigns the given value to the non-secure Main Stack Pointer (MSP) when in secure state.
+ \param [in] topOfMainStack Main Stack Pointer value to set
+ */
+__STATIC_FORCEINLINE void __TZ_set_MSP_NS(uint32_t topOfMainStack)
+{
+ __ASM volatile ("MSR msp_ns, %0" : : "r" (topOfMainStack) : );
+}
+#endif
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Get Stack Pointer (non-secure)
+ \details Returns the current value of the non-secure Stack Pointer (SP) when in secure state.
+ \return SP Register value
+ */
+__STATIC_FORCEINLINE uint32_t __TZ_get_SP_NS(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, sp_ns" : "=r" (result) );
+ return(result);
+}
+
+
+/**
+ \brief Set Stack Pointer (non-secure)
+ \details Assigns the given value to the non-secure Stack Pointer (SP) when in secure state.
+ \param [in] topOfStack Stack Pointer value to set
+ */
+__STATIC_FORCEINLINE void __TZ_set_SP_NS(uint32_t topOfStack)
+{
+ __ASM volatile ("MSR sp_ns, %0" : : "r" (topOfStack) : );
+}
+#endif
+
+
+/**
+ \brief Get Priority Mask
+ \details Returns the current state of the priority mask bit from the Priority Mask Register.
+ \return Priority Mask value
+ */
+__STATIC_FORCEINLINE uint32_t __get_PRIMASK(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, primask" : "=r" (result) );
+ return(result);
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Get Priority Mask (non-secure)
+ \details Returns the current state of the non-secure priority mask bit from the Priority Mask Register when in secure state.
+ \return Priority Mask value
+ */
+__STATIC_FORCEINLINE uint32_t __TZ_get_PRIMASK_NS(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, primask_ns" : "=r" (result) );
+ return(result);
+}
+#endif
+
+
+/**
+ \brief Set Priority Mask
+ \details Assigns the given value to the Priority Mask Register.
+ \param [in] priMask Priority Mask
+ */
+__STATIC_FORCEINLINE void __set_PRIMASK(uint32_t priMask)
+{
+ __ASM volatile ("MSR primask, %0" : : "r" (priMask) : "memory");
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Set Priority Mask (non-secure)
+ \details Assigns the given value to the non-secure Priority Mask Register when in secure state.
+ \param [in] priMask Priority Mask
+ */
+__STATIC_FORCEINLINE void __TZ_set_PRIMASK_NS(uint32_t priMask)
+{
+ __ASM volatile ("MSR primask_ns, %0" : : "r" (priMask) : "memory");
+}
+#endif
+
+
+#if ((defined (__ARM_ARCH_7M__ ) && (__ARM_ARCH_7M__ == 1)) || \
+ (defined (__ARM_ARCH_7EM__ ) && (__ARM_ARCH_7EM__ == 1)) || \
+ (defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) )
+/**
+ \brief Enable FIQ
+ \details Enables FIQ interrupts by clearing the F-bit in the CPSR.
+ Can only be executed in Privileged modes.
+ */
+#define __enable_fault_irq __enable_fiq /* see arm_compat.h */
+
+
+/**
+ \brief Disable FIQ
+ \details Disables FIQ interrupts by setting the F-bit in the CPSR.
+ Can only be executed in Privileged modes.
+ */
+#define __disable_fault_irq __disable_fiq /* see arm_compat.h */
+
+
+/**
+ \brief Get Base Priority
+ \details Returns the current value of the Base Priority register.
+ \return Base Priority register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_BASEPRI(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, basepri" : "=r" (result) );
+ return(result);
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Get Base Priority (non-secure)
+ \details Returns the current value of the non-secure Base Priority register when in secure state.
+ \return Base Priority register value
+ */
+__STATIC_FORCEINLINE uint32_t __TZ_get_BASEPRI_NS(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, basepri_ns" : "=r" (result) );
+ return(result);
+}
+#endif
+
+
+/**
+ \brief Set Base Priority
+ \details Assigns the given value to the Base Priority register.
+ \param [in] basePri Base Priority value to set
+ */
+__STATIC_FORCEINLINE void __set_BASEPRI(uint32_t basePri)
+{
+ __ASM volatile ("MSR basepri, %0" : : "r" (basePri) : "memory");
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Set Base Priority (non-secure)
+ \details Assigns the given value to the non-secure Base Priority register when in secure state.
+ \param [in] basePri Base Priority value to set
+ */
+__STATIC_FORCEINLINE void __TZ_set_BASEPRI_NS(uint32_t basePri)
+{
+ __ASM volatile ("MSR basepri_ns, %0" : : "r" (basePri) : "memory");
+}
+#endif
+
+
+/**
+ \brief Set Base Priority with condition
+ \details Assigns the given value to the Base Priority register only if BASEPRI masking is disabled,
+ or the new value increases the BASEPRI priority level.
+ \param [in] basePri Base Priority value to set
+ */
+__STATIC_FORCEINLINE void __set_BASEPRI_MAX(uint32_t basePri)
+{
+ __ASM volatile ("MSR basepri_max, %0" : : "r" (basePri) : "memory");
+}
+
+
+/**
+ \brief Get Fault Mask
+ \details Returns the current value of the Fault Mask register.
+ \return Fault Mask register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_FAULTMASK(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, faultmask" : "=r" (result) );
+ return(result);
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Get Fault Mask (non-secure)
+ \details Returns the current value of the non-secure Fault Mask register when in secure state.
+ \return Fault Mask register value
+ */
+__STATIC_FORCEINLINE uint32_t __TZ_get_FAULTMASK_NS(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, faultmask_ns" : "=r" (result) );
+ return(result);
+}
+#endif
+
+
+/**
+ \brief Set Fault Mask
+ \details Assigns the given value to the Fault Mask register.
+ \param [in] faultMask Fault Mask value to set
+ */
+__STATIC_FORCEINLINE void __set_FAULTMASK(uint32_t faultMask)
+{
+ __ASM volatile ("MSR faultmask, %0" : : "r" (faultMask) : "memory");
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Set Fault Mask (non-secure)
+ \details Assigns the given value to the non-secure Fault Mask register when in secure state.
+ \param [in] faultMask Fault Mask value to set
+ */
+__STATIC_FORCEINLINE void __TZ_set_FAULTMASK_NS(uint32_t faultMask)
+{
+ __ASM volatile ("MSR faultmask_ns, %0" : : "r" (faultMask) : "memory");
+}
+#endif
+
+#endif /* ((defined (__ARM_ARCH_7M__ ) && (__ARM_ARCH_7M__ == 1)) || \
+ (defined (__ARM_ARCH_7EM__ ) && (__ARM_ARCH_7EM__ == 1)) || \
+ (defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) ) */
+
+
+#if ((defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) || \
+ (defined (__ARM_ARCH_8M_BASE__ ) && (__ARM_ARCH_8M_BASE__ == 1)) )
+
+/**
+ \brief Get Process Stack Pointer Limit
+ Devices without ARMv8-M Main Extensions (i.e. Cortex-M23) lack the non-secure
+ Stack Pointer Limit register hence zero is returned always in non-secure
+ mode.
+
+ \details Returns the current value of the Process Stack Pointer Limit (PSPLIM).
+ \return PSPLIM Register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_PSPLIM(void)
+{
+#if (!(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) && \
+ (!defined (__ARM_FEATURE_CMSE) || (__ARM_FEATURE_CMSE < 3)))
+ // without main extensions, the non-secure PSPLIM is RAZ/WI
+ return 0U;
+#else
+ uint32_t result;
+ __ASM volatile ("MRS %0, psplim" : "=r" (result) );
+ return result;
+#endif
+}
+
+#if (defined (__ARM_FEATURE_CMSE) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Get Process Stack Pointer Limit (non-secure)
+ Devices without ARMv8-M Main Extensions (i.e. Cortex-M23) lack the non-secure
+ Stack Pointer Limit register hence zero is returned always in non-secure
+ mode.
+
+ \details Returns the current value of the non-secure Process Stack Pointer Limit (PSPLIM) when in secure state.
+ \return PSPLIM Register value
+ */
+__STATIC_FORCEINLINE uint32_t __TZ_get_PSPLIM_NS(void)
+{
+#if (!(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)))
+ // without main extensions, the non-secure PSPLIM is RAZ/WI
+ return 0U;
+#else
+ uint32_t result;
+ __ASM volatile ("MRS %0, psplim_ns" : "=r" (result) );
+ return result;
+#endif
+}
+#endif
+
+
+/**
+ \brief Set Process Stack Pointer Limit
+ Devices without ARMv8-M Main Extensions (i.e. Cortex-M23) lack the non-secure
+ Stack Pointer Limit register hence the write is silently ignored in non-secure
+ mode.
+
+ \details Assigns the given value to the Process Stack Pointer Limit (PSPLIM).
+ \param [in] ProcStackPtrLimit Process Stack Pointer Limit value to set
+ */
+__STATIC_FORCEINLINE void __set_PSPLIM(uint32_t ProcStackPtrLimit)
+{
+#if (!(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) && \
+ (!defined (__ARM_FEATURE_CMSE) || (__ARM_FEATURE_CMSE < 3)))
+ // without main extensions, the non-secure PSPLIM is RAZ/WI
+ (void)ProcStackPtrLimit;
+#else
+ __ASM volatile ("MSR psplim, %0" : : "r" (ProcStackPtrLimit));
+#endif
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Set Process Stack Pointer (non-secure)
+ Devices without ARMv8-M Main Extensions (i.e. Cortex-M23) lack the non-secure
+ Stack Pointer Limit register hence the write is silently ignored in non-secure
+ mode.
+
+ \details Assigns the given value to the non-secure Process Stack Pointer Limit (PSPLIM) when in secure state.
+ \param [in] ProcStackPtrLimit Process Stack Pointer Limit value to set
+ */
+__STATIC_FORCEINLINE void __TZ_set_PSPLIM_NS(uint32_t ProcStackPtrLimit)
+{
+#if (!(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)))
+ // without main extensions, the non-secure PSPLIM is RAZ/WI
+ (void)ProcStackPtrLimit;
+#else
+ __ASM volatile ("MSR psplim_ns, %0\n" : : "r" (ProcStackPtrLimit));
+#endif
+}
+#endif
+
+
+/**
+ \brief Get Main Stack Pointer Limit
+ Devices without ARMv8-M Main Extensions (i.e. Cortex-M23) lack the non-secure
+ Stack Pointer Limit register hence zero is returned always.
+
+ \details Returns the current value of the Main Stack Pointer Limit (MSPLIM).
+ \return MSPLIM Register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_MSPLIM(void)
+{
+#if (!(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) && \
+ (!defined (__ARM_FEATURE_CMSE) || (__ARM_FEATURE_CMSE < 3)))
+ // without main extensions, the non-secure MSPLIM is RAZ/WI
+ return 0U;
+#else
+ uint32_t result;
+ __ASM volatile ("MRS %0, msplim" : "=r" (result) );
+ return result;
+#endif
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Get Main Stack Pointer Limit (non-secure)
+ Devices without ARMv8-M Main Extensions (i.e. Cortex-M23) lack the non-secure
+ Stack Pointer Limit register hence zero is returned always.
+
+ \details Returns the current value of the non-secure Main Stack Pointer Limit(MSPLIM) when in secure state.
+ \return MSPLIM Register value
+ */
+__STATIC_FORCEINLINE uint32_t __TZ_get_MSPLIM_NS(void)
+{
+#if (!(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)))
+ // without main extensions, the non-secure MSPLIM is RAZ/WI
+ return 0U;
+#else
+ uint32_t result;
+ __ASM volatile ("MRS %0, msplim_ns" : "=r" (result) );
+ return result;
+#endif
+}
+#endif
+
+
+/**
+ \brief Set Main Stack Pointer Limit
+ Devices without ARMv8-M Main Extensions (i.e. Cortex-M23) lack the non-secure
+ Stack Pointer Limit register hence the write is silently ignored.
+
+ \details Assigns the given value to the Main Stack Pointer Limit (MSPLIM).
+ \param [in] MainStackPtrLimit Main Stack Pointer Limit value to set
+ */
+__STATIC_FORCEINLINE void __set_MSPLIM(uint32_t MainStackPtrLimit)
+{
+#if (!(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) && \
+ (!defined (__ARM_FEATURE_CMSE) || (__ARM_FEATURE_CMSE < 3)))
+ // without main extensions, the non-secure MSPLIM is RAZ/WI
+ (void)MainStackPtrLimit;
+#else
+ __ASM volatile ("MSR msplim, %0" : : "r" (MainStackPtrLimit));
+#endif
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Set Main Stack Pointer Limit (non-secure)
+ Devices without ARMv8-M Main Extensions (i.e. Cortex-M23) lack the non-secure
+ Stack Pointer Limit register hence the write is silently ignored.
+
+ \details Assigns the given value to the non-secure Main Stack Pointer Limit (MSPLIM) when in secure state.
+ \param [in] MainStackPtrLimit Main Stack Pointer value to set
+ */
+__STATIC_FORCEINLINE void __TZ_set_MSPLIM_NS(uint32_t MainStackPtrLimit)
+{
+#if (!(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)))
+ // without main extensions, the non-secure MSPLIM is RAZ/WI
+ (void)MainStackPtrLimit;
+#else
+ __ASM volatile ("MSR msplim_ns, %0" : : "r" (MainStackPtrLimit));
+#endif
+}
+#endif
+
+#endif /* ((defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) || \
+ (defined (__ARM_ARCH_8M_BASE__ ) && (__ARM_ARCH_8M_BASE__ == 1)) ) */
+
+/**
+ \brief Get FPSCR
+ \details Returns the current value of the Floating Point Status/Control register.
+ \return Floating Point Status/Control register value
+ */
+#if ((defined (__FPU_PRESENT) && (__FPU_PRESENT == 1U)) && \
+ (defined (__FPU_USED ) && (__FPU_USED == 1U)) )
+#define __get_FPSCR (uint32_t)__builtin_arm_get_fpscr
+#else
+#define __get_FPSCR() ((uint32_t)0U)
+#endif
+
+/**
+ \brief Set FPSCR
+ \details Assigns the given value to the Floating Point Status/Control register.
+ \param [in] fpscr Floating Point Status/Control value to set
+ */
+#if ((defined (__FPU_PRESENT) && (__FPU_PRESENT == 1U)) && \
+ (defined (__FPU_USED ) && (__FPU_USED == 1U)) )
+#define __set_FPSCR __builtin_arm_set_fpscr
+#else
+#define __set_FPSCR(x) ((void)(x))
+#endif
+
+
+/*@} end of CMSIS_Core_RegAccFunctions */
+
+
+/* ########################## Core Instruction Access ######################### */
+/** \defgroup CMSIS_Core_InstructionInterface CMSIS Core Instruction Interface
+ Access to dedicated instructions
+ @{
+*/
+
+/* Define macros for porting to both thumb1 and thumb2.
+ * For thumb1, use low register (r0-r7), specified by constraint "l"
+ * Otherwise, use general registers, specified by constraint "r" */
+#if defined (__thumb__) && !defined (__thumb2__)
+#define __CMSIS_GCC_OUT_REG(r) "=l" (r)
+#define __CMSIS_GCC_USE_REG(r) "l" (r)
+#else
+#define __CMSIS_GCC_OUT_REG(r) "=r" (r)
+#define __CMSIS_GCC_USE_REG(r) "r" (r)
+#endif
+
+/**
+ \brief No Operation
+ \details No Operation does nothing. This instruction can be used for code alignment purposes.
+ */
+#define __NOP __builtin_arm_nop
+
+/**
+ \brief Wait For Interrupt
+ \details Wait For Interrupt is a hint instruction that suspends execution until one of a number of events occurs.
+ */
+#define __WFI __builtin_arm_wfi
+
+
+/**
+ \brief Wait For Event
+ \details Wait For Event is a hint instruction that permits the processor to enter
+ a low-power state until one of a number of events occurs.
+ */
+#define __WFE __builtin_arm_wfe
+
+
+/**
+ \brief Send Event
+ \details Send Event is a hint instruction. It causes an event to be signaled to the CPU.
+ */
+#define __SEV __builtin_arm_sev
+
+
+/**
+ \brief Instruction Synchronization Barrier
+ \details Instruction Synchronization Barrier flushes the pipeline in the processor,
+ so that all instructions following the ISB are fetched from cache or memory,
+ after the instruction has been completed.
+ */
+#define __ISB() __builtin_arm_isb(0xF)
+
+/**
+ \brief Data Synchronization Barrier
+ \details Acts as a special kind of Data Memory Barrier.
+ It completes when all explicit memory accesses before this instruction complete.
+ */
+#define __DSB() __builtin_arm_dsb(0xF)
+
+
+/**
+ \brief Data Memory Barrier
+ \details Ensures the apparent order of the explicit memory operations before
+ and after the instruction, without ensuring their completion.
+ */
+#define __DMB() __builtin_arm_dmb(0xF)
+
+
+/**
+ \brief Reverse byte order (32 bit)
+ \details Reverses the byte order in unsigned integer value. For example, 0x12345678 becomes 0x78563412.
+ \param [in] value Value to reverse
+ \return Reversed value
+ */
+#define __REV(value) __builtin_bswap32(value)
+
+
+/**
+ \brief Reverse byte order (16 bit)
+ \details Reverses the byte order within each halfword of a word. For example, 0x12345678 becomes 0x34127856.
+ \param [in] value Value to reverse
+ \return Reversed value
+ */
+#define __REV16(value) __ROR(__REV(value), 16)
+
+
+/**
+ \brief Reverse byte order (16 bit)
+ \details Reverses the byte order in a 16-bit value and returns the signed 16-bit result. For example, 0x0080 becomes 0x8000.
+ \param [in] value Value to reverse
+ \return Reversed value
+ */
+#define __REVSH(value) (int16_t)__builtin_bswap16(value)
+
+
+/**
+ \brief Rotate Right in unsigned value (32 bit)
+ \details Rotate Right (immediate) provides the value of the contents of a register rotated by a variable number of bits.
+ \param [in] op1 Value to rotate
+ \param [in] op2 Number of Bits to rotate
+ \return Rotated value
+ */
+__STATIC_FORCEINLINE uint32_t __ROR(uint32_t op1, uint32_t op2)
+{
+ op2 %= 32U;
+ if (op2 == 0U)
+ {
+ return op1;
+ }
+ return (op1 >> op2) | (op1 << (32U - op2));
+}
+
+
+/**
+ \brief Breakpoint
+ \details Causes the processor to enter Debug state.
+ Debug tools can use this to investigate system state when the instruction at a particular address is reached.
+ \param [in] value is ignored by the processor.
+ If required, a debugger can use it to store additional information about the breakpoint.
+ */
+#define __BKPT(value) __ASM volatile ("bkpt "#value)
+
+
+/**
+ \brief Reverse bit order of value
+ \details Reverses the bit order of the given value.
+ \param [in] value Value to reverse
+ \return Reversed value
+ */
+#define __RBIT __builtin_arm_rbit
+
+/**
+ \brief Count leading zeros
+ \details Counts the number of leading zeros of a data value.
+ \param [in] value Value to count the leading zeros
+ \return number of leading zeros in value
+ */
+__STATIC_FORCEINLINE uint8_t __CLZ(uint32_t value)
+{
+ /* Even though __builtin_clz produces a CLZ instruction on ARM, formally
+ __builtin_clz(0) is undefined behaviour, so handle this case specially.
+ This guarantees ARM-compatible results if happening to compile on a non-ARM
+ target, and ensures the compiler doesn't decide to activate any
+ optimisations using the logic "value was passed to __builtin_clz, so it
+ is non-zero".
+ ARM Compiler 6.10 and possibly earlier will optimise this test away, leaving a
+ single CLZ instruction.
+ */
+ if (value == 0U)
+ {
+ return 32U;
+ }
+ return __builtin_clz(value);
+}
+
+
+#if ((defined (__ARM_ARCH_7M__ ) && (__ARM_ARCH_7M__ == 1)) || \
+ (defined (__ARM_ARCH_7EM__ ) && (__ARM_ARCH_7EM__ == 1)) || \
+ (defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) || \
+ (defined (__ARM_ARCH_8M_BASE__ ) && (__ARM_ARCH_8M_BASE__ == 1)) )
+/**
+ \brief LDR Exclusive (8 bit)
+ \details Executes a exclusive LDR instruction for 8 bit value.
+ \param [in] ptr Pointer to data
+ \return value of type uint8_t at (*ptr)
+ */
+#define __LDREXB (uint8_t)__builtin_arm_ldrex
+
+
+/**
+ \brief LDR Exclusive (16 bit)
+ \details Executes a exclusive LDR instruction for 16 bit values.
+ \param [in] ptr Pointer to data
+ \return value of type uint16_t at (*ptr)
+ */
+#define __LDREXH (uint16_t)__builtin_arm_ldrex
+
+
+/**
+ \brief LDR Exclusive (32 bit)
+ \details Executes a exclusive LDR instruction for 32 bit values.
+ \param [in] ptr Pointer to data
+ \return value of type uint32_t at (*ptr)
+ */
+#define __LDREXW (uint32_t)__builtin_arm_ldrex
+
+
+/**
+ \brief STR Exclusive (8 bit)
+ \details Executes a exclusive STR instruction for 8 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ \return 0 Function succeeded
+ \return 1 Function failed
+ */
+#define __STREXB (uint32_t)__builtin_arm_strex
+
+
+/**
+ \brief STR Exclusive (16 bit)
+ \details Executes a exclusive STR instruction for 16 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ \return 0 Function succeeded
+ \return 1 Function failed
+ */
+#define __STREXH (uint32_t)__builtin_arm_strex
+
+
+/**
+ \brief STR Exclusive (32 bit)
+ \details Executes a exclusive STR instruction for 32 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ \return 0 Function succeeded
+ \return 1 Function failed
+ */
+#define __STREXW (uint32_t)__builtin_arm_strex
+
+
+/**
+ \brief Remove the exclusive lock
+ \details Removes the exclusive lock which is created by LDREX.
+ */
+#define __CLREX __builtin_arm_clrex
+
+#endif /* ((defined (__ARM_ARCH_7M__ ) && (__ARM_ARCH_7M__ == 1)) || \
+ (defined (__ARM_ARCH_7EM__ ) && (__ARM_ARCH_7EM__ == 1)) || \
+ (defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) || \
+ (defined (__ARM_ARCH_8M_BASE__ ) && (__ARM_ARCH_8M_BASE__ == 1)) ) */
+
+
+#if ((defined (__ARM_ARCH_7M__ ) && (__ARM_ARCH_7M__ == 1)) || \
+ (defined (__ARM_ARCH_7EM__ ) && (__ARM_ARCH_7EM__ == 1)) || \
+ (defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) )
+
+/**
+ \brief Signed Saturate
+ \details Saturates a signed value.
+ \param [in] value Value to be saturated
+ \param [in] sat Bit position to saturate to (1..32)
+ \return Saturated value
+ */
+#define __SSAT __builtin_arm_ssat
+
+
+/**
+ \brief Unsigned Saturate
+ \details Saturates an unsigned value.
+ \param [in] value Value to be saturated
+ \param [in] sat Bit position to saturate to (0..31)
+ \return Saturated value
+ */
+#define __USAT __builtin_arm_usat
+
+
+/**
+ \brief Rotate Right with Extend (32 bit)
+ \details Moves each bit of a bitstring right by one bit.
+ The carry input is shifted in at the left end of the bitstring.
+ \param [in] value Value to rotate
+ \return Rotated value
+ */
+__STATIC_FORCEINLINE uint32_t __RRX(uint32_t value)
+{
+ uint32_t result;
+
+ __ASM volatile ("rrx %0, %1" : __CMSIS_GCC_OUT_REG (result) : __CMSIS_GCC_USE_REG (value) );
+ return(result);
+}
+
+
+/**
+ \brief LDRT Unprivileged (8 bit)
+ \details Executes a Unprivileged LDRT instruction for 8 bit value.
+ \param [in] ptr Pointer to data
+ \return value of type uint8_t at (*ptr)
+ */
+__STATIC_FORCEINLINE uint8_t __LDRBT(volatile uint8_t *ptr)
+{
+ uint32_t result;
+
+ __ASM volatile ("ldrbt %0, %1" : "=r" (result) : "Q" (*ptr) );
+ return ((uint8_t) result); /* Add explicit type cast here */
+}
+
+
+/**
+ \brief LDRT Unprivileged (16 bit)
+ \details Executes a Unprivileged LDRT instruction for 16 bit values.
+ \param [in] ptr Pointer to data
+ \return value of type uint16_t at (*ptr)
+ */
+__STATIC_FORCEINLINE uint16_t __LDRHT(volatile uint16_t *ptr)
+{
+ uint32_t result;
+
+ __ASM volatile ("ldrht %0, %1" : "=r" (result) : "Q" (*ptr) );
+ return ((uint16_t) result); /* Add explicit type cast here */
+}
+
+
+/**
+ \brief LDRT Unprivileged (32 bit)
+ \details Executes a Unprivileged LDRT instruction for 32 bit values.
+ \param [in] ptr Pointer to data
+ \return value of type uint32_t at (*ptr)
+ */
+__STATIC_FORCEINLINE uint32_t __LDRT(volatile uint32_t *ptr)
+{
+ uint32_t result;
+
+ __ASM volatile ("ldrt %0, %1" : "=r" (result) : "Q" (*ptr) );
+ return(result);
+}
+
+
+/**
+ \brief STRT Unprivileged (8 bit)
+ \details Executes a Unprivileged STRT instruction for 8 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ */
+__STATIC_FORCEINLINE void __STRBT(uint8_t value, volatile uint8_t *ptr)
+{
+ __ASM volatile ("strbt %1, %0" : "=Q" (*ptr) : "r" ((uint32_t)value) );
+}
+
+
+/**
+ \brief STRT Unprivileged (16 bit)
+ \details Executes a Unprivileged STRT instruction for 16 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ */
+__STATIC_FORCEINLINE void __STRHT(uint16_t value, volatile uint16_t *ptr)
+{
+ __ASM volatile ("strht %1, %0" : "=Q" (*ptr) : "r" ((uint32_t)value) );
+}
+
+
+/**
+ \brief STRT Unprivileged (32 bit)
+ \details Executes a Unprivileged STRT instruction for 32 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ */
+__STATIC_FORCEINLINE void __STRT(uint32_t value, volatile uint32_t *ptr)
+{
+ __ASM volatile ("strt %1, %0" : "=Q" (*ptr) : "r" (value) );
+}
+
+#else /* ((defined (__ARM_ARCH_7M__ ) && (__ARM_ARCH_7M__ == 1)) || \
+ (defined (__ARM_ARCH_7EM__ ) && (__ARM_ARCH_7EM__ == 1)) || \
+ (defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) ) */
+
+/**
+ \brief Signed Saturate
+ \details Saturates a signed value.
+ \param [in] value Value to be saturated
+ \param [in] sat Bit position to saturate to (1..32)
+ \return Saturated value
+ */
+__STATIC_FORCEINLINE int32_t __SSAT(int32_t val, uint32_t sat)
+{
+ if ((sat >= 1U) && (sat <= 32U))
+ {
+ const int32_t max = (int32_t)((1U << (sat - 1U)) - 1U);
+ const int32_t min = -1 - max ;
+ if (val > max)
+ {
+ return max;
+ }
+ else if (val < min)
+ {
+ return min;
+ }
+ }
+ return val;
+}
+
+/**
+ \brief Unsigned Saturate
+ \details Saturates an unsigned value.
+ \param [in] value Value to be saturated
+ \param [in] sat Bit position to saturate to (0..31)
+ \return Saturated value
+ */
+__STATIC_FORCEINLINE uint32_t __USAT(int32_t val, uint32_t sat)
+{
+ if (sat <= 31U)
+ {
+ const uint32_t max = ((1U << sat) - 1U);
+ if (val > (int32_t)max)
+ {
+ return max;
+ }
+ else if (val < 0)
+ {
+ return 0U;
+ }
+ }
+ return (uint32_t)val;
+}
+
+#endif /* ((defined (__ARM_ARCH_7M__ ) && (__ARM_ARCH_7M__ == 1)) || \
+ (defined (__ARM_ARCH_7EM__ ) && (__ARM_ARCH_7EM__ == 1)) || \
+ (defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) ) */
+
+
+#if ((defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) || \
+ (defined (__ARM_ARCH_8M_BASE__ ) && (__ARM_ARCH_8M_BASE__ == 1)) )
+/**
+ \brief Load-Acquire (8 bit)
+ \details Executes a LDAB instruction for 8 bit value.
+ \param [in] ptr Pointer to data
+ \return value of type uint8_t at (*ptr)
+ */
+__STATIC_FORCEINLINE uint8_t __LDAB(volatile uint8_t *ptr)
+{
+ uint32_t result;
+
+ __ASM volatile ("ldab %0, %1" : "=r" (result) : "Q" (*ptr) : "memory" );
+ return ((uint8_t) result);
+}
+
+
+/**
+ \brief Load-Acquire (16 bit)
+ \details Executes a LDAH instruction for 16 bit values.
+ \param [in] ptr Pointer to data
+ \return value of type uint16_t at (*ptr)
+ */
+__STATIC_FORCEINLINE uint16_t __LDAH(volatile uint16_t *ptr)
+{
+ uint32_t result;
+
+ __ASM volatile ("ldah %0, %1" : "=r" (result) : "Q" (*ptr) : "memory" );
+ return ((uint16_t) result);
+}
+
+
+/**
+ \brief Load-Acquire (32 bit)
+ \details Executes a LDA instruction for 32 bit values.
+ \param [in] ptr Pointer to data
+ \return value of type uint32_t at (*ptr)
+ */
+__STATIC_FORCEINLINE uint32_t __LDA(volatile uint32_t *ptr)
+{
+ uint32_t result;
+
+ __ASM volatile ("lda %0, %1" : "=r" (result) : "Q" (*ptr) : "memory" );
+ return(result);
+}
+
+
+/**
+ \brief Store-Release (8 bit)
+ \details Executes a STLB instruction for 8 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ */
+__STATIC_FORCEINLINE void __STLB(uint8_t value, volatile uint8_t *ptr)
+{
+ __ASM volatile ("stlb %1, %0" : "=Q" (*ptr) : "r" ((uint32_t)value) : "memory" );
+}
+
+
+/**
+ \brief Store-Release (16 bit)
+ \details Executes a STLH instruction for 16 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ */
+__STATIC_FORCEINLINE void __STLH(uint16_t value, volatile uint16_t *ptr)
+{
+ __ASM volatile ("stlh %1, %0" : "=Q" (*ptr) : "r" ((uint32_t)value) : "memory" );
+}
+
+
+/**
+ \brief Store-Release (32 bit)
+ \details Executes a STL instruction for 32 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ */
+__STATIC_FORCEINLINE void __STL(uint32_t value, volatile uint32_t *ptr)
+{
+ __ASM volatile ("stl %1, %0" : "=Q" (*ptr) : "r" ((uint32_t)value) : "memory" );
+}
+
+
+/**
+ \brief Load-Acquire Exclusive (8 bit)
+ \details Executes a LDAB exclusive instruction for 8 bit value.
+ \param [in] ptr Pointer to data
+ \return value of type uint8_t at (*ptr)
+ */
+#define __LDAEXB (uint8_t)__builtin_arm_ldaex
+
+
+/**
+ \brief Load-Acquire Exclusive (16 bit)
+ \details Executes a LDAH exclusive instruction for 16 bit values.
+ \param [in] ptr Pointer to data
+ \return value of type uint16_t at (*ptr)
+ */
+#define __LDAEXH (uint16_t)__builtin_arm_ldaex
+
+
+/**
+ \brief Load-Acquire Exclusive (32 bit)
+ \details Executes a LDA exclusive instruction for 32 bit values.
+ \param [in] ptr Pointer to data
+ \return value of type uint32_t at (*ptr)
+ */
+#define __LDAEX (uint32_t)__builtin_arm_ldaex
+
+
+/**
+ \brief Store-Release Exclusive (8 bit)
+ \details Executes a STLB exclusive instruction for 8 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ \return 0 Function succeeded
+ \return 1 Function failed
+ */
+#define __STLEXB (uint32_t)__builtin_arm_stlex
+
+
+/**
+ \brief Store-Release Exclusive (16 bit)
+ \details Executes a STLH exclusive instruction for 16 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ \return 0 Function succeeded
+ \return 1 Function failed
+ */
+#define __STLEXH (uint32_t)__builtin_arm_stlex
+
+
+/**
+ \brief Store-Release Exclusive (32 bit)
+ \details Executes a STL exclusive instruction for 32 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ \return 0 Function succeeded
+ \return 1 Function failed
+ */
+#define __STLEX (uint32_t)__builtin_arm_stlex
+
+#endif /* ((defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) || \
+ (defined (__ARM_ARCH_8M_BASE__ ) && (__ARM_ARCH_8M_BASE__ == 1)) ) */
+
+/*@}*/ /* end of group CMSIS_Core_InstructionInterface */
+
+
+/* ################### Compiler specific Intrinsics ########################### */
+/** \defgroup CMSIS_SIMD_intrinsics CMSIS SIMD Intrinsics
+ Access to dedicated SIMD instructions
+ @{
+*/
+
+#if (defined (__ARM_FEATURE_DSP) && (__ARM_FEATURE_DSP == 1))
+
+__STATIC_FORCEINLINE uint32_t __SADD8(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("sadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __QADD8(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("qadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __SHADD8(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("shadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __UADD8(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("uadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __UQADD8(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("uqadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __UHADD8(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("uhadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+
+__STATIC_FORCEINLINE uint32_t __SSUB8(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("ssub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __QSUB8(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("qsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __SHSUB8(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("shsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __USUB8(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("usub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __UQSUB8(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("uqsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __UHSUB8(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("uhsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+
+__STATIC_FORCEINLINE uint32_t __SADD16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("sadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __QADD16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("qadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __SHADD16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("shadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __UADD16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("uadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __UQADD16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("uqadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __UHADD16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("uhadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __SSUB16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("ssub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __QSUB16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("qsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __SHSUB16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("shsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __USUB16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("usub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __UQSUB16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("uqsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __UHSUB16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("uhsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __SASX(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("sasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __QASX(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("qasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __SHASX(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("shasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __UASX(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("uasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __UQASX(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("uqasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __UHASX(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("uhasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __SSAX(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("ssax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __QSAX(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("qsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __SHSAX(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("shsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __USAX(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("usax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __UQSAX(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("uqsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __UHSAX(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("uhsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __USAD8(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("usad8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __USADA8(uint32_t op1, uint32_t op2, uint32_t op3)
+{
+ uint32_t result;
+
+ __ASM volatile ("usada8 %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );
+ return(result);
+}
+
+#define __SSAT16(ARG1,ARG2) \
+({ \
+ int32_t __RES, __ARG1 = (ARG1); \
+ __ASM ("ssat16 %0, %1, %2" : "=r" (__RES) : "I" (ARG2), "r" (__ARG1) ); \
+ __RES; \
+ })
+
+#define __USAT16(ARG1,ARG2) \
+({ \
+ uint32_t __RES, __ARG1 = (ARG1); \
+ __ASM ("usat16 %0, %1, %2" : "=r" (__RES) : "I" (ARG2), "r" (__ARG1) ); \
+ __RES; \
+ })
+
+__STATIC_FORCEINLINE uint32_t __UXTB16(uint32_t op1)
+{
+ uint32_t result;
+
+ __ASM volatile ("uxtb16 %0, %1" : "=r" (result) : "r" (op1));
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __UXTAB16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("uxtab16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __SXTB16(uint32_t op1)
+{
+ uint32_t result;
+
+ __ASM volatile ("sxtb16 %0, %1" : "=r" (result) : "r" (op1));
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __SXTAB16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("sxtab16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __SMUAD (uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("smuad %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __SMUADX (uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("smuadx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __SMLAD (uint32_t op1, uint32_t op2, uint32_t op3)
+{
+ uint32_t result;
+
+ __ASM volatile ("smlad %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __SMLADX (uint32_t op1, uint32_t op2, uint32_t op3)
+{
+ uint32_t result;
+
+ __ASM volatile ("smladx %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint64_t __SMLALD (uint32_t op1, uint32_t op2, uint64_t acc)
+{
+ union llreg_u{
+ uint32_t w32[2];
+ uint64_t w64;
+ } llr;
+ llr.w64 = acc;
+
+#ifndef __ARMEB__ /* Little endian */
+ __ASM volatile ("smlald %0, %1, %2, %3" : "=r" (llr.w32[0]), "=r" (llr.w32[1]): "r" (op1), "r" (op2) , "0" (llr.w32[0]), "1" (llr.w32[1]) );
+#else /* Big endian */
+ __ASM volatile ("smlald %0, %1, %2, %3" : "=r" (llr.w32[1]), "=r" (llr.w32[0]): "r" (op1), "r" (op2) , "0" (llr.w32[1]), "1" (llr.w32[0]) );
+#endif
+
+ return(llr.w64);
+}
+
+__STATIC_FORCEINLINE uint64_t __SMLALDX (uint32_t op1, uint32_t op2, uint64_t acc)
+{
+ union llreg_u{
+ uint32_t w32[2];
+ uint64_t w64;
+ } llr;
+ llr.w64 = acc;
+
+#ifndef __ARMEB__ /* Little endian */
+ __ASM volatile ("smlaldx %0, %1, %2, %3" : "=r" (llr.w32[0]), "=r" (llr.w32[1]): "r" (op1), "r" (op2) , "0" (llr.w32[0]), "1" (llr.w32[1]) );
+#else /* Big endian */
+ __ASM volatile ("smlaldx %0, %1, %2, %3" : "=r" (llr.w32[1]), "=r" (llr.w32[0]): "r" (op1), "r" (op2) , "0" (llr.w32[1]), "1" (llr.w32[0]) );
+#endif
+
+ return(llr.w64);
+}
+
+__STATIC_FORCEINLINE uint32_t __SMUSD (uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("smusd %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __SMUSDX (uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("smusdx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __SMLSD (uint32_t op1, uint32_t op2, uint32_t op3)
+{
+ uint32_t result;
+
+ __ASM volatile ("smlsd %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __SMLSDX (uint32_t op1, uint32_t op2, uint32_t op3)
+{
+ uint32_t result;
+
+ __ASM volatile ("smlsdx %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint64_t __SMLSLD (uint32_t op1, uint32_t op2, uint64_t acc)
+{
+ union llreg_u{
+ uint32_t w32[2];
+ uint64_t w64;
+ } llr;
+ llr.w64 = acc;
+
+#ifndef __ARMEB__ /* Little endian */
+ __ASM volatile ("smlsld %0, %1, %2, %3" : "=r" (llr.w32[0]), "=r" (llr.w32[1]): "r" (op1), "r" (op2) , "0" (llr.w32[0]), "1" (llr.w32[1]) );
+#else /* Big endian */
+ __ASM volatile ("smlsld %0, %1, %2, %3" : "=r" (llr.w32[1]), "=r" (llr.w32[0]): "r" (op1), "r" (op2) , "0" (llr.w32[1]), "1" (llr.w32[0]) );
+#endif
+
+ return(llr.w64);
+}
+
+__STATIC_FORCEINLINE uint64_t __SMLSLDX (uint32_t op1, uint32_t op2, uint64_t acc)
+{
+ union llreg_u{
+ uint32_t w32[2];
+ uint64_t w64;
+ } llr;
+ llr.w64 = acc;
+
+#ifndef __ARMEB__ /* Little endian */
+ __ASM volatile ("smlsldx %0, %1, %2, %3" : "=r" (llr.w32[0]), "=r" (llr.w32[1]): "r" (op1), "r" (op2) , "0" (llr.w32[0]), "1" (llr.w32[1]) );
+#else /* Big endian */
+ __ASM volatile ("smlsldx %0, %1, %2, %3" : "=r" (llr.w32[1]), "=r" (llr.w32[0]): "r" (op1), "r" (op2) , "0" (llr.w32[1]), "1" (llr.w32[0]) );
+#endif
+
+ return(llr.w64);
+}
+
+__STATIC_FORCEINLINE uint32_t __SEL (uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("sel %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE int32_t __QADD( int32_t op1, int32_t op2)
+{
+ int32_t result;
+
+ __ASM volatile ("qadd %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE int32_t __QSUB( int32_t op1, int32_t op2)
+{
+ int32_t result;
+
+ __ASM volatile ("qsub %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+#define __PKHBT(ARG1,ARG2,ARG3) ( ((((uint32_t)(ARG1)) ) & 0x0000FFFFUL) | \
+ ((((uint32_t)(ARG2)) << (ARG3)) & 0xFFFF0000UL) )
+
+#define __PKHTB(ARG1,ARG2,ARG3) ( ((((uint32_t)(ARG1)) ) & 0xFFFF0000UL) | \
+ ((((uint32_t)(ARG2)) >> (ARG3)) & 0x0000FFFFUL) )
+
+#define __SXTB16_RORn(ARG1, ARG2) __SXTB16(__ROR(ARG1, ARG2))
+
+__STATIC_FORCEINLINE int32_t __SMMLA (int32_t op1, int32_t op2, int32_t op3)
+{
+ int32_t result;
+
+ __ASM volatile ("smmla %0, %1, %2, %3" : "=r" (result): "r" (op1), "r" (op2), "r" (op3) );
+ return(result);
+}
+
+#endif /* (__ARM_FEATURE_DSP == 1) */
+/*@} end of group CMSIS_SIMD_intrinsics */
+
+
+#endif /* __CMSIS_ARMCLANG_H */
diff --git a/src/rp2_common/cmsis/stub/CMSIS/Core/Include/cmsis_compiler.h b/src/rp2_common/cmsis/stub/CMSIS/Core/Include/cmsis_compiler.h
new file mode 100644
index 0000000..adbf296
--- /dev/null
+++ b/src/rp2_common/cmsis/stub/CMSIS/Core/Include/cmsis_compiler.h
@@ -0,0 +1,283 @@
+/**************************************************************************//**
+ * @file cmsis_compiler.h
+ * @brief CMSIS compiler generic header file
+ * @version V5.1.0
+ * @date 09. October 2018
+ ******************************************************************************/
+/*
+ * Copyright (c) 2009-2018 Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef __CMSIS_COMPILER_H
+#define __CMSIS_COMPILER_H
+
+#include <stdint.h>
+
+/*
+ * Arm Compiler 4/5
+ */
+#if defined ( __CC_ARM )
+ #include "cmsis_armcc.h"
+
+
+/*
+ * Arm Compiler 6.6 LTM (armclang)
+ */
+#elif defined (__ARMCC_VERSION) && (__ARMCC_VERSION >= 6010050) && (__ARMCC_VERSION < 6100100)
+ #include "cmsis_armclang_ltm.h"
+
+ /*
+ * Arm Compiler above 6.10.1 (armclang)
+ */
+#elif defined (__ARMCC_VERSION) && (__ARMCC_VERSION >= 6100100)
+ #include "cmsis_armclang.h"
+
+
+/*
+ * GNU Compiler
+ */
+#elif defined ( __GNUC__ )
+ #include "cmsis_gcc.h"
+
+
+/*
+ * IAR Compiler
+ */
+#elif defined ( __ICCARM__ )
+ #include <cmsis_iccarm.h>
+
+
+/*
+ * TI Arm Compiler
+ */
+#elif defined ( __TI_ARM__ )
+ #include <cmsis_ccs.h>
+
+ #ifndef __ASM
+ #define __ASM __asm
+ #endif
+ #ifndef __INLINE
+ #define __INLINE inline
+ #endif
+ #ifndef __STATIC_INLINE
+ #define __STATIC_INLINE static inline
+ #endif
+ #ifndef __STATIC_FORCEINLINE
+ #define __STATIC_FORCEINLINE __STATIC_INLINE
+ #endif
+ #ifndef __NO_RETURN
+ #define __NO_RETURN __attribute__((noreturn))
+ #endif
+ #ifndef __USED
+ #define __USED __attribute__((used))
+ #endif
+ #ifndef __WEAK
+ #define __WEAK __attribute__((weak))
+ #endif
+ #ifndef __PACKED
+ #define __PACKED __attribute__((packed))
+ #endif
+ #ifndef __PACKED_STRUCT
+ #define __PACKED_STRUCT struct __attribute__((packed))
+ #endif
+ #ifndef __PACKED_UNION
+ #define __PACKED_UNION union __attribute__((packed))
+ #endif
+ #ifndef __UNALIGNED_UINT32 /* deprecated */
+ struct __attribute__((packed)) T_UINT32 { uint32_t v; };
+ #define __UNALIGNED_UINT32(x) (((struct T_UINT32 *)(x))->v)
+ #endif
+ #ifndef __UNALIGNED_UINT16_WRITE
+ __PACKED_STRUCT T_UINT16_WRITE { uint16_t v; };
+ #define __UNALIGNED_UINT16_WRITE(addr, val) (void)((((struct T_UINT16_WRITE *)(void*)(addr))->v) = (val))
+ #endif
+ #ifndef __UNALIGNED_UINT16_READ
+ __PACKED_STRUCT T_UINT16_READ { uint16_t v; };
+ #define __UNALIGNED_UINT16_READ(addr) (((const struct T_UINT16_READ *)(const void *)(addr))->v)
+ #endif
+ #ifndef __UNALIGNED_UINT32_WRITE
+ __PACKED_STRUCT T_UINT32_WRITE { uint32_t v; };
+ #define __UNALIGNED_UINT32_WRITE(addr, val) (void)((((struct T_UINT32_WRITE *)(void *)(addr))->v) = (val))
+ #endif
+ #ifndef __UNALIGNED_UINT32_READ
+ __PACKED_STRUCT T_UINT32_READ { uint32_t v; };
+ #define __UNALIGNED_UINT32_READ(addr) (((const struct T_UINT32_READ *)(const void *)(addr))->v)
+ #endif
+ #ifndef __ALIGNED
+ #define __ALIGNED(x) __attribute__((aligned(x)))
+ #endif
+ #ifndef __RESTRICT
+ #define __RESTRICT __restrict
+ #endif
+ #ifndef __COMPILER_BARRIER
+ #warning No compiler specific solution for __COMPILER_BARRIER. __COMPILER_BARRIER is ignored.
+ #define __COMPILER_BARRIER() (void)0
+ #endif
+
+
+/*
+ * TASKING Compiler
+ */
+#elif defined ( __TASKING__ )
+ /*
+ * The CMSIS functions have been implemented as intrinsics in the compiler.
+ * Please use "carm -?i" to get an up to date list of all intrinsics,
+ * Including the CMSIS ones.
+ */
+
+ #ifndef __ASM
+ #define __ASM __asm
+ #endif
+ #ifndef __INLINE
+ #define __INLINE inline
+ #endif
+ #ifndef __STATIC_INLINE
+ #define __STATIC_INLINE static inline
+ #endif
+ #ifndef __STATIC_FORCEINLINE
+ #define __STATIC_FORCEINLINE __STATIC_INLINE
+ #endif
+ #ifndef __NO_RETURN
+ #define __NO_RETURN __attribute__((noreturn))
+ #endif
+ #ifndef __USED
+ #define __USED __attribute__((used))
+ #endif
+ #ifndef __WEAK
+ #define __WEAK __attribute__((weak))
+ #endif
+ #ifndef __PACKED
+ #define __PACKED __packed__
+ #endif
+ #ifndef __PACKED_STRUCT
+ #define __PACKED_STRUCT struct __packed__
+ #endif
+ #ifndef __PACKED_UNION
+ #define __PACKED_UNION union __packed__
+ #endif
+ #ifndef __UNALIGNED_UINT32 /* deprecated */
+ struct __packed__ T_UINT32 { uint32_t v; };
+ #define __UNALIGNED_UINT32(x) (((struct T_UINT32 *)(x))->v)
+ #endif
+ #ifndef __UNALIGNED_UINT16_WRITE
+ __PACKED_STRUCT T_UINT16_WRITE { uint16_t v; };
+ #define __UNALIGNED_UINT16_WRITE(addr, val) (void)((((struct T_UINT16_WRITE *)(void *)(addr))->v) = (val))
+ #endif
+ #ifndef __UNALIGNED_UINT16_READ
+ __PACKED_STRUCT T_UINT16_READ { uint16_t v; };
+ #define __UNALIGNED_UINT16_READ(addr) (((const struct T_UINT16_READ *)(const void *)(addr))->v)
+ #endif
+ #ifndef __UNALIGNED_UINT32_WRITE
+ __PACKED_STRUCT T_UINT32_WRITE { uint32_t v; };
+ #define __UNALIGNED_UINT32_WRITE(addr, val) (void)((((struct T_UINT32_WRITE *)(void *)(addr))->v) = (val))
+ #endif
+ #ifndef __UNALIGNED_UINT32_READ
+ __PACKED_STRUCT T_UINT32_READ { uint32_t v; };
+ #define __UNALIGNED_UINT32_READ(addr) (((const struct T_UINT32_READ *)(const void *)(addr))->v)
+ #endif
+ #ifndef __ALIGNED
+ #define __ALIGNED(x) __align(x)
+ #endif
+ #ifndef __RESTRICT
+ #warning No compiler specific solution for __RESTRICT. __RESTRICT is ignored.
+ #define __RESTRICT
+ #endif
+ #ifndef __COMPILER_BARRIER
+ #warning No compiler specific solution for __COMPILER_BARRIER. __COMPILER_BARRIER is ignored.
+ #define __COMPILER_BARRIER() (void)0
+ #endif
+
+
+/*
+ * COSMIC Compiler
+ */
+#elif defined ( __CSMC__ )
+ #include <cmsis_csm.h>
+
+ #ifndef __ASM
+ #define __ASM _asm
+ #endif
+ #ifndef __INLINE
+ #define __INLINE inline
+ #endif
+ #ifndef __STATIC_INLINE
+ #define __STATIC_INLINE static inline
+ #endif
+ #ifndef __STATIC_FORCEINLINE
+ #define __STATIC_FORCEINLINE __STATIC_INLINE
+ #endif
+ #ifndef __NO_RETURN
+ // NO RETURN is automatically detected hence no warning here
+ #define __NO_RETURN
+ #endif
+ #ifndef __USED
+ #warning No compiler specific solution for __USED. __USED is ignored.
+ #define __USED
+ #endif
+ #ifndef __WEAK
+ #define __WEAK __weak
+ #endif
+ #ifndef __PACKED
+ #define __PACKED @packed
+ #endif
+ #ifndef __PACKED_STRUCT
+ #define __PACKED_STRUCT @packed struct
+ #endif
+ #ifndef __PACKED_UNION
+ #define __PACKED_UNION @packed union
+ #endif
+ #ifndef __UNALIGNED_UINT32 /* deprecated */
+ @packed struct T_UINT32 { uint32_t v; };
+ #define __UNALIGNED_UINT32(x) (((struct T_UINT32 *)(x))->v)
+ #endif
+ #ifndef __UNALIGNED_UINT16_WRITE
+ __PACKED_STRUCT T_UINT16_WRITE { uint16_t v; };
+ #define __UNALIGNED_UINT16_WRITE(addr, val) (void)((((struct T_UINT16_WRITE *)(void *)(addr))->v) = (val))
+ #endif
+ #ifndef __UNALIGNED_UINT16_READ
+ __PACKED_STRUCT T_UINT16_READ { uint16_t v; };
+ #define __UNALIGNED_UINT16_READ(addr) (((const struct T_UINT16_READ *)(const void *)(addr))->v)
+ #endif
+ #ifndef __UNALIGNED_UINT32_WRITE
+ __PACKED_STRUCT T_UINT32_WRITE { uint32_t v; };
+ #define __UNALIGNED_UINT32_WRITE(addr, val) (void)((((struct T_UINT32_WRITE *)(void *)(addr))->v) = (val))
+ #endif
+ #ifndef __UNALIGNED_UINT32_READ
+ __PACKED_STRUCT T_UINT32_READ { uint32_t v; };
+ #define __UNALIGNED_UINT32_READ(addr) (((const struct T_UINT32_READ *)(const void *)(addr))->v)
+ #endif
+ #ifndef __ALIGNED
+ #warning No compiler specific solution for __ALIGNED. __ALIGNED is ignored.
+ #define __ALIGNED(x)
+ #endif
+ #ifndef __RESTRICT
+ #warning No compiler specific solution for __RESTRICT. __RESTRICT is ignored.
+ #define __RESTRICT
+ #endif
+ #ifndef __COMPILER_BARRIER
+ #warning No compiler specific solution for __COMPILER_BARRIER. __COMPILER_BARRIER is ignored.
+ #define __COMPILER_BARRIER() (void)0
+ #endif
+
+
+#else
+ #error Unknown compiler.
+#endif
+
+
+#endif /* __CMSIS_COMPILER_H */
+
diff --git a/src/rp2_common/cmsis/stub/CMSIS/Core/Include/cmsis_gcc.h b/src/rp2_common/cmsis/stub/CMSIS/Core/Include/cmsis_gcc.h
new file mode 100644
index 0000000..a2778f5
--- /dev/null
+++ b/src/rp2_common/cmsis/stub/CMSIS/Core/Include/cmsis_gcc.h
@@ -0,0 +1,2177 @@
+/**************************************************************************//**
+ * @file cmsis_gcc.h
+ * @brief CMSIS compiler GCC header file
+ * @version V5.3.0
+ * @date 26. March 2020
+ ******************************************************************************/
+/*
+ * Copyright (c) 2009-2020 Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef __CMSIS_GCC_H
+#define __CMSIS_GCC_H
+
+/* ignore some GCC warnings */
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wsign-conversion"
+#pragma GCC diagnostic ignored "-Wconversion"
+#pragma GCC diagnostic ignored "-Wunused-parameter"
+
+/* Fallback for __has_builtin */
+#ifndef __has_builtin
+ #define __has_builtin(x) (0)
+#endif
+
+/* CMSIS compiler specific defines */
+#ifndef __ASM
+ #define __ASM __asm
+#endif
+#ifndef __INLINE
+ #define __INLINE inline
+#endif
+#ifndef __STATIC_INLINE
+ #define __STATIC_INLINE static inline
+#endif
+#ifndef __STATIC_FORCEINLINE
+ #define __STATIC_FORCEINLINE __attribute__((always_inline)) static inline
+#endif
+#ifndef __NO_RETURN
+ #define __NO_RETURN __attribute__((__noreturn__))
+#endif
+#ifndef __USED
+ #define __USED __attribute__((used))
+#endif
+#ifndef __WEAK
+ #define __WEAK __attribute__((weak))
+#endif
+#ifndef __PACKED
+ #define __PACKED __attribute__((packed, aligned(1)))
+#endif
+#ifndef __PACKED_STRUCT
+ #define __PACKED_STRUCT struct __attribute__((packed, aligned(1)))
+#endif
+#ifndef __PACKED_UNION
+ #define __PACKED_UNION union __attribute__((packed, aligned(1)))
+#endif
+#ifndef __UNALIGNED_UINT32 /* deprecated */
+ #pragma GCC diagnostic push
+ #pragma GCC diagnostic ignored "-Wpacked"
+ #pragma GCC diagnostic ignored "-Wattributes"
+ struct __attribute__((packed)) T_UINT32 { uint32_t v; };
+ #pragma GCC diagnostic pop
+ #define __UNALIGNED_UINT32(x) (((struct T_UINT32 *)(x))->v)
+#endif
+#ifndef __UNALIGNED_UINT16_WRITE
+ #pragma GCC diagnostic push
+ #pragma GCC diagnostic ignored "-Wpacked"
+ #pragma GCC diagnostic ignored "-Wattributes"
+ __PACKED_STRUCT T_UINT16_WRITE { uint16_t v; };
+ #pragma GCC diagnostic pop
+ #define __UNALIGNED_UINT16_WRITE(addr, val) (void)((((struct T_UINT16_WRITE *)(void *)(addr))->v) = (val))
+#endif
+#ifndef __UNALIGNED_UINT16_READ
+ #pragma GCC diagnostic push
+ #pragma GCC diagnostic ignored "-Wpacked"
+ #pragma GCC diagnostic ignored "-Wattributes"
+ __PACKED_STRUCT T_UINT16_READ { uint16_t v; };
+ #pragma GCC diagnostic pop
+ #define __UNALIGNED_UINT16_READ(addr) (((const struct T_UINT16_READ *)(const void *)(addr))->v)
+#endif
+#ifndef __UNALIGNED_UINT32_WRITE
+ #pragma GCC diagnostic push
+ #pragma GCC diagnostic ignored "-Wpacked"
+ #pragma GCC diagnostic ignored "-Wattributes"
+ __PACKED_STRUCT T_UINT32_WRITE { uint32_t v; };
+ #pragma GCC diagnostic pop
+ #define __UNALIGNED_UINT32_WRITE(addr, val) (void)((((struct T_UINT32_WRITE *)(void *)(addr))->v) = (val))
+#endif
+#ifndef __UNALIGNED_UINT32_READ
+ #pragma GCC diagnostic push
+ #pragma GCC diagnostic ignored "-Wpacked"
+ #pragma GCC diagnostic ignored "-Wattributes"
+ __PACKED_STRUCT T_UINT32_READ { uint32_t v; };
+ #pragma GCC diagnostic pop
+ #define __UNALIGNED_UINT32_READ(addr) (((const struct T_UINT32_READ *)(const void *)(addr))->v)
+#endif
+#ifndef __ALIGNED
+ #define __ALIGNED(x) __attribute__((aligned(x)))
+#endif
+#ifndef __RESTRICT
+ #define __RESTRICT __restrict
+#endif
+#ifndef __COMPILER_BARRIER
+ #define __COMPILER_BARRIER() __ASM volatile("":::"memory")
+#endif
+
+/* ######################### Startup and Lowlevel Init ######################## */
+
+#ifndef __PROGRAM_START
+
+/**
+ \brief Initializes data and bss sections
+ \details This default implementations initialized all data and additional bss
+ sections relying on .copy.table and .zero.table specified properly
+ in the used linker script.
+
+ */
+__STATIC_FORCEINLINE __NO_RETURN void __cmsis_start(void)
+{
+ extern void _start(void) __NO_RETURN;
+
+ typedef struct {
+ uint32_t const* src;
+ uint32_t* dest;
+ uint32_t wlen;
+ } __copy_table_t;
+
+ typedef struct {
+ uint32_t* dest;
+ uint32_t wlen;
+ } __zero_table_t;
+
+ extern const __copy_table_t __copy_table_start__;
+ extern const __copy_table_t __copy_table_end__;
+ extern const __zero_table_t __zero_table_start__;
+ extern const __zero_table_t __zero_table_end__;
+
+ for (__copy_table_t const* pTable = &__copy_table_start__; pTable < &__copy_table_end__; ++pTable) {
+ for(uint32_t i=0u; i<pTable->wlen; ++i) {
+ pTable->dest[i] = pTable->src[i];
+ }
+ }
+
+ for (__zero_table_t const* pTable = &__zero_table_start__; pTable < &__zero_table_end__; ++pTable) {
+ for(uint32_t i=0u; i<pTable->wlen; ++i) {
+ pTable->dest[i] = 0u;
+ }
+ }
+
+ _start();
+}
+
+#define __PROGRAM_START __cmsis_start
+#endif
+
+#ifndef __INITIAL_SP
+#define __INITIAL_SP __StackTop
+#endif
+
+#ifndef __STACK_LIMIT
+#define __STACK_LIMIT __StackLimit
+#endif
+
+#ifndef __VECTOR_TABLE
+#define __VECTOR_TABLE __Vectors
+#endif
+
+#ifndef __VECTOR_TABLE_ATTRIBUTE
+#define __VECTOR_TABLE_ATTRIBUTE __attribute__((used, section(".vectors")))
+#endif
+
+/* ########################### Core Function Access ########################### */
+/** \ingroup CMSIS_Core_FunctionInterface
+ \defgroup CMSIS_Core_RegAccFunctions CMSIS Core Register Access Functions
+ @{
+ */
+
+/**
+ \brief Enable IRQ Interrupts
+ \details Enables IRQ interrupts by clearing the I-bit in the CPSR.
+ Can only be executed in Privileged modes.
+ */
+__STATIC_FORCEINLINE void __enable_irq(void)
+{
+ __ASM volatile ("cpsie i" : : : "memory");
+}
+
+
+/**
+ \brief Disable IRQ Interrupts
+ \details Disables IRQ interrupts by setting the I-bit in the CPSR.
+ Can only be executed in Privileged modes.
+ */
+__STATIC_FORCEINLINE void __disable_irq(void)
+{
+ __ASM volatile ("cpsid i" : : : "memory");
+}
+
+
+/**
+ \brief Get Control Register
+ \details Returns the content of the Control Register.
+ \return Control Register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_CONTROL(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, control" : "=r" (result) );
+ return(result);
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Get Control Register (non-secure)
+ \details Returns the content of the non-secure Control Register when in secure mode.
+ \return non-secure Control Register value
+ */
+__STATIC_FORCEINLINE uint32_t __TZ_get_CONTROL_NS(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, control_ns" : "=r" (result) );
+ return(result);
+}
+#endif
+
+
+/**
+ \brief Set Control Register
+ \details Writes the given value to the Control Register.
+ \param [in] control Control Register value to set
+ */
+__STATIC_FORCEINLINE void __set_CONTROL(uint32_t control)
+{
+ __ASM volatile ("MSR control, %0" : : "r" (control) : "memory");
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Set Control Register (non-secure)
+ \details Writes the given value to the non-secure Control Register when in secure state.
+ \param [in] control Control Register value to set
+ */
+__STATIC_FORCEINLINE void __TZ_set_CONTROL_NS(uint32_t control)
+{
+ __ASM volatile ("MSR control_ns, %0" : : "r" (control) : "memory");
+}
+#endif
+
+
+/**
+ \brief Get IPSR Register
+ \details Returns the content of the IPSR Register.
+ \return IPSR Register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_IPSR(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, ipsr" : "=r" (result) );
+ return(result);
+}
+
+
+/**
+ \brief Get APSR Register
+ \details Returns the content of the APSR Register.
+ \return APSR Register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_APSR(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, apsr" : "=r" (result) );
+ return(result);
+}
+
+
+/**
+ \brief Get xPSR Register
+ \details Returns the content of the xPSR Register.
+ \return xPSR Register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_xPSR(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, xpsr" : "=r" (result) );
+ return(result);
+}
+
+
+/**
+ \brief Get Process Stack Pointer
+ \details Returns the current value of the Process Stack Pointer (PSP).
+ \return PSP Register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_PSP(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, psp" : "=r" (result) );
+ return(result);
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Get Process Stack Pointer (non-secure)
+ \details Returns the current value of the non-secure Process Stack Pointer (PSP) when in secure state.
+ \return PSP Register value
+ */
+__STATIC_FORCEINLINE uint32_t __TZ_get_PSP_NS(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, psp_ns" : "=r" (result) );
+ return(result);
+}
+#endif
+
+
+/**
+ \brief Set Process Stack Pointer
+ \details Assigns the given value to the Process Stack Pointer (PSP).
+ \param [in] topOfProcStack Process Stack Pointer value to set
+ */
+__STATIC_FORCEINLINE void __set_PSP(uint32_t topOfProcStack)
+{
+ __ASM volatile ("MSR psp, %0" : : "r" (topOfProcStack) : );
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Set Process Stack Pointer (non-secure)
+ \details Assigns the given value to the non-secure Process Stack Pointer (PSP) when in secure state.
+ \param [in] topOfProcStack Process Stack Pointer value to set
+ */
+__STATIC_FORCEINLINE void __TZ_set_PSP_NS(uint32_t topOfProcStack)
+{
+ __ASM volatile ("MSR psp_ns, %0" : : "r" (topOfProcStack) : );
+}
+#endif
+
+
+/**
+ \brief Get Main Stack Pointer
+ \details Returns the current value of the Main Stack Pointer (MSP).
+ \return MSP Register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_MSP(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, msp" : "=r" (result) );
+ return(result);
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Get Main Stack Pointer (non-secure)
+ \details Returns the current value of the non-secure Main Stack Pointer (MSP) when in secure state.
+ \return MSP Register value
+ */
+__STATIC_FORCEINLINE uint32_t __TZ_get_MSP_NS(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, msp_ns" : "=r" (result) );
+ return(result);
+}
+#endif
+
+
+/**
+ \brief Set Main Stack Pointer
+ \details Assigns the given value to the Main Stack Pointer (MSP).
+ \param [in] topOfMainStack Main Stack Pointer value to set
+ */
+__STATIC_FORCEINLINE void __set_MSP(uint32_t topOfMainStack)
+{
+ __ASM volatile ("MSR msp, %0" : : "r" (topOfMainStack) : );
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Set Main Stack Pointer (non-secure)
+ \details Assigns the given value to the non-secure Main Stack Pointer (MSP) when in secure state.
+ \param [in] topOfMainStack Main Stack Pointer value to set
+ */
+__STATIC_FORCEINLINE void __TZ_set_MSP_NS(uint32_t topOfMainStack)
+{
+ __ASM volatile ("MSR msp_ns, %0" : : "r" (topOfMainStack) : );
+}
+#endif
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Get Stack Pointer (non-secure)
+ \details Returns the current value of the non-secure Stack Pointer (SP) when in secure state.
+ \return SP Register value
+ */
+__STATIC_FORCEINLINE uint32_t __TZ_get_SP_NS(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, sp_ns" : "=r" (result) );
+ return(result);
+}
+
+
+/**
+ \brief Set Stack Pointer (non-secure)
+ \details Assigns the given value to the non-secure Stack Pointer (SP) when in secure state.
+ \param [in] topOfStack Stack Pointer value to set
+ */
+__STATIC_FORCEINLINE void __TZ_set_SP_NS(uint32_t topOfStack)
+{
+ __ASM volatile ("MSR sp_ns, %0" : : "r" (topOfStack) : );
+}
+#endif
+
+
+/**
+ \brief Get Priority Mask
+ \details Returns the current state of the priority mask bit from the Priority Mask Register.
+ \return Priority Mask value
+ */
+__STATIC_FORCEINLINE uint32_t __get_PRIMASK(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, primask" : "=r" (result) );
+ return(result);
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Get Priority Mask (non-secure)
+ \details Returns the current state of the non-secure priority mask bit from the Priority Mask Register when in secure state.
+ \return Priority Mask value
+ */
+__STATIC_FORCEINLINE uint32_t __TZ_get_PRIMASK_NS(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, primask_ns" : "=r" (result) );
+ return(result);
+}
+#endif
+
+
+/**
+ \brief Set Priority Mask
+ \details Assigns the given value to the Priority Mask Register.
+ \param [in] priMask Priority Mask
+ */
+__STATIC_FORCEINLINE void __set_PRIMASK(uint32_t priMask)
+{
+ __ASM volatile ("MSR primask, %0" : : "r" (priMask) : "memory");
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Set Priority Mask (non-secure)
+ \details Assigns the given value to the non-secure Priority Mask Register when in secure state.
+ \param [in] priMask Priority Mask
+ */
+__STATIC_FORCEINLINE void __TZ_set_PRIMASK_NS(uint32_t priMask)
+{
+ __ASM volatile ("MSR primask_ns, %0" : : "r" (priMask) : "memory");
+}
+#endif
+
+
+#if ((defined (__ARM_ARCH_7M__ ) && (__ARM_ARCH_7M__ == 1)) || \
+ (defined (__ARM_ARCH_7EM__ ) && (__ARM_ARCH_7EM__ == 1)) || \
+ (defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) )
+/**
+ \brief Enable FIQ
+ \details Enables FIQ interrupts by clearing the F-bit in the CPSR.
+ Can only be executed in Privileged modes.
+ */
+__STATIC_FORCEINLINE void __enable_fault_irq(void)
+{
+ __ASM volatile ("cpsie f" : : : "memory");
+}
+
+
+/**
+ \brief Disable FIQ
+ \details Disables FIQ interrupts by setting the F-bit in the CPSR.
+ Can only be executed in Privileged modes.
+ */
+__STATIC_FORCEINLINE void __disable_fault_irq(void)
+{
+ __ASM volatile ("cpsid f" : : : "memory");
+}
+
+
+/**
+ \brief Get Base Priority
+ \details Returns the current value of the Base Priority register.
+ \return Base Priority register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_BASEPRI(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, basepri" : "=r" (result) );
+ return(result);
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Get Base Priority (non-secure)
+ \details Returns the current value of the non-secure Base Priority register when in secure state.
+ \return Base Priority register value
+ */
+__STATIC_FORCEINLINE uint32_t __TZ_get_BASEPRI_NS(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, basepri_ns" : "=r" (result) );
+ return(result);
+}
+#endif
+
+
+/**
+ \brief Set Base Priority
+ \details Assigns the given value to the Base Priority register.
+ \param [in] basePri Base Priority value to set
+ */
+__STATIC_FORCEINLINE void __set_BASEPRI(uint32_t basePri)
+{
+ __ASM volatile ("MSR basepri, %0" : : "r" (basePri) : "memory");
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Set Base Priority (non-secure)
+ \details Assigns the given value to the non-secure Base Priority register when in secure state.
+ \param [in] basePri Base Priority value to set
+ */
+__STATIC_FORCEINLINE void __TZ_set_BASEPRI_NS(uint32_t basePri)
+{
+ __ASM volatile ("MSR basepri_ns, %0" : : "r" (basePri) : "memory");
+}
+#endif
+
+
+/**
+ \brief Set Base Priority with condition
+ \details Assigns the given value to the Base Priority register only if BASEPRI masking is disabled,
+ or the new value increases the BASEPRI priority level.
+ \param [in] basePri Base Priority value to set
+ */
+__STATIC_FORCEINLINE void __set_BASEPRI_MAX(uint32_t basePri)
+{
+ __ASM volatile ("MSR basepri_max, %0" : : "r" (basePri) : "memory");
+}
+
+
+/**
+ \brief Get Fault Mask
+ \details Returns the current value of the Fault Mask register.
+ \return Fault Mask register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_FAULTMASK(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, faultmask" : "=r" (result) );
+ return(result);
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Get Fault Mask (non-secure)
+ \details Returns the current value of the non-secure Fault Mask register when in secure state.
+ \return Fault Mask register value
+ */
+__STATIC_FORCEINLINE uint32_t __TZ_get_FAULTMASK_NS(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, faultmask_ns" : "=r" (result) );
+ return(result);
+}
+#endif
+
+
+/**
+ \brief Set Fault Mask
+ \details Assigns the given value to the Fault Mask register.
+ \param [in] faultMask Fault Mask value to set
+ */
+__STATIC_FORCEINLINE void __set_FAULTMASK(uint32_t faultMask)
+{
+ __ASM volatile ("MSR faultmask, %0" : : "r" (faultMask) : "memory");
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Set Fault Mask (non-secure)
+ \details Assigns the given value to the non-secure Fault Mask register when in secure state.
+ \param [in] faultMask Fault Mask value to set
+ */
+__STATIC_FORCEINLINE void __TZ_set_FAULTMASK_NS(uint32_t faultMask)
+{
+ __ASM volatile ("MSR faultmask_ns, %0" : : "r" (faultMask) : "memory");
+}
+#endif
+
+#endif /* ((defined (__ARM_ARCH_7M__ ) && (__ARM_ARCH_7M__ == 1)) || \
+ (defined (__ARM_ARCH_7EM__ ) && (__ARM_ARCH_7EM__ == 1)) || \
+ (defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) ) */
+
+
+#if ((defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) || \
+ (defined (__ARM_ARCH_8M_BASE__ ) && (__ARM_ARCH_8M_BASE__ == 1)) )
+
+/**
+ \brief Get Process Stack Pointer Limit
+ Devices without ARMv8-M Main Extensions (i.e. Cortex-M23) lack the non-secure
+ Stack Pointer Limit register hence zero is returned always in non-secure
+ mode.
+
+ \details Returns the current value of the Process Stack Pointer Limit (PSPLIM).
+ \return PSPLIM Register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_PSPLIM(void)
+{
+#if (!(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) && \
+ (!defined (__ARM_FEATURE_CMSE) || (__ARM_FEATURE_CMSE < 3)))
+ // without main extensions, the non-secure PSPLIM is RAZ/WI
+ return 0U;
+#else
+ uint32_t result;
+ __ASM volatile ("MRS %0, psplim" : "=r" (result) );
+ return result;
+#endif
+}
+
+#if (defined (__ARM_FEATURE_CMSE) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Get Process Stack Pointer Limit (non-secure)
+ Devices without ARMv8-M Main Extensions (i.e. Cortex-M23) lack the non-secure
+ Stack Pointer Limit register hence zero is returned always.
+
+ \details Returns the current value of the non-secure Process Stack Pointer Limit (PSPLIM) when in secure state.
+ \return PSPLIM Register value
+ */
+__STATIC_FORCEINLINE uint32_t __TZ_get_PSPLIM_NS(void)
+{
+#if (!(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)))
+ // without main extensions, the non-secure PSPLIM is RAZ/WI
+ return 0U;
+#else
+ uint32_t result;
+ __ASM volatile ("MRS %0, psplim_ns" : "=r" (result) );
+ return result;
+#endif
+}
+#endif
+
+
+/**
+ \brief Set Process Stack Pointer Limit
+ Devices without ARMv8-M Main Extensions (i.e. Cortex-M23) lack the non-secure
+ Stack Pointer Limit register hence the write is silently ignored in non-secure
+ mode.
+
+ \details Assigns the given value to the Process Stack Pointer Limit (PSPLIM).
+ \param [in] ProcStackPtrLimit Process Stack Pointer Limit value to set
+ */
+__STATIC_FORCEINLINE void __set_PSPLIM(uint32_t ProcStackPtrLimit)
+{
+#if (!(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) && \
+ (!defined (__ARM_FEATURE_CMSE) || (__ARM_FEATURE_CMSE < 3)))
+ // without main extensions, the non-secure PSPLIM is RAZ/WI
+ (void)ProcStackPtrLimit;
+#else
+ __ASM volatile ("MSR psplim, %0" : : "r" (ProcStackPtrLimit));
+#endif
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Set Process Stack Pointer (non-secure)
+ Devices without ARMv8-M Main Extensions (i.e. Cortex-M23) lack the non-secure
+ Stack Pointer Limit register hence the write is silently ignored.
+
+ \details Assigns the given value to the non-secure Process Stack Pointer Limit (PSPLIM) when in secure state.
+ \param [in] ProcStackPtrLimit Process Stack Pointer Limit value to set
+ */
+__STATIC_FORCEINLINE void __TZ_set_PSPLIM_NS(uint32_t ProcStackPtrLimit)
+{
+#if (!(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)))
+ // without main extensions, the non-secure PSPLIM is RAZ/WI
+ (void)ProcStackPtrLimit;
+#else
+ __ASM volatile ("MSR psplim_ns, %0\n" : : "r" (ProcStackPtrLimit));
+#endif
+}
+#endif
+
+
+/**
+ \brief Get Main Stack Pointer Limit
+ Devices without ARMv8-M Main Extensions (i.e. Cortex-M23) lack the non-secure
+ Stack Pointer Limit register hence zero is returned always in non-secure
+ mode.
+
+ \details Returns the current value of the Main Stack Pointer Limit (MSPLIM).
+ \return MSPLIM Register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_MSPLIM(void)
+{
+#if (!(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) && \
+ (!defined (__ARM_FEATURE_CMSE) || (__ARM_FEATURE_CMSE < 3)))
+ // without main extensions, the non-secure MSPLIM is RAZ/WI
+ return 0U;
+#else
+ uint32_t result;
+ __ASM volatile ("MRS %0, msplim" : "=r" (result) );
+ return result;
+#endif
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Get Main Stack Pointer Limit (non-secure)
+ Devices without ARMv8-M Main Extensions (i.e. Cortex-M23) lack the non-secure
+ Stack Pointer Limit register hence zero is returned always.
+
+ \details Returns the current value of the non-secure Main Stack Pointer Limit(MSPLIM) when in secure state.
+ \return MSPLIM Register value
+ */
+__STATIC_FORCEINLINE uint32_t __TZ_get_MSPLIM_NS(void)
+{
+#if (!(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)))
+ // without main extensions, the non-secure MSPLIM is RAZ/WI
+ return 0U;
+#else
+ uint32_t result;
+ __ASM volatile ("MRS %0, msplim_ns" : "=r" (result) );
+ return result;
+#endif
+}
+#endif
+
+
+/**
+ \brief Set Main Stack Pointer Limit
+ Devices without ARMv8-M Main Extensions (i.e. Cortex-M23) lack the non-secure
+ Stack Pointer Limit register hence the write is silently ignored in non-secure
+ mode.
+
+ \details Assigns the given value to the Main Stack Pointer Limit (MSPLIM).
+ \param [in] MainStackPtrLimit Main Stack Pointer Limit value to set
+ */
+__STATIC_FORCEINLINE void __set_MSPLIM(uint32_t MainStackPtrLimit)
+{
+#if (!(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) && \
+ (!defined (__ARM_FEATURE_CMSE) || (__ARM_FEATURE_CMSE < 3)))
+ // without main extensions, the non-secure MSPLIM is RAZ/WI
+ (void)MainStackPtrLimit;
+#else
+ __ASM volatile ("MSR msplim, %0" : : "r" (MainStackPtrLimit));
+#endif
+}
+
+
+#if (defined (__ARM_FEATURE_CMSE ) && (__ARM_FEATURE_CMSE == 3))
+/**
+ \brief Set Main Stack Pointer Limit (non-secure)
+ Devices without ARMv8-M Main Extensions (i.e. Cortex-M23) lack the non-secure
+ Stack Pointer Limit register hence the write is silently ignored.
+
+ \details Assigns the given value to the non-secure Main Stack Pointer Limit (MSPLIM) when in secure state.
+ \param [in] MainStackPtrLimit Main Stack Pointer value to set
+ */
+__STATIC_FORCEINLINE void __TZ_set_MSPLIM_NS(uint32_t MainStackPtrLimit)
+{
+#if (!(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)))
+ // without main extensions, the non-secure MSPLIM is RAZ/WI
+ (void)MainStackPtrLimit;
+#else
+ __ASM volatile ("MSR msplim_ns, %0" : : "r" (MainStackPtrLimit));
+#endif
+}
+#endif
+
+#endif /* ((defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) || \
+ (defined (__ARM_ARCH_8M_BASE__ ) && (__ARM_ARCH_8M_BASE__ == 1)) ) */
+
+
+/**
+ \brief Get FPSCR
+ \details Returns the current value of the Floating Point Status/Control register.
+ \return Floating Point Status/Control register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_FPSCR(void)
+{
+#if ((defined (__FPU_PRESENT) && (__FPU_PRESENT == 1U)) && \
+ (defined (__FPU_USED ) && (__FPU_USED == 1U)) )
+#if __has_builtin(__builtin_arm_get_fpscr)
+// Re-enable using built-in when GCC has been fixed
+// || (__GNUC__ > 7) || (__GNUC__ == 7 && __GNUC_MINOR__ >= 2)
+ /* see https://gcc.gnu.org/ml/gcc-patches/2017-04/msg00443.html */
+ return __builtin_arm_get_fpscr();
+#else
+ uint32_t result;
+
+ __ASM volatile ("VMRS %0, fpscr" : "=r" (result) );
+ return(result);
+#endif
+#else
+ return(0U);
+#endif
+}
+
+
+/**
+ \brief Set FPSCR
+ \details Assigns the given value to the Floating Point Status/Control register.
+ \param [in] fpscr Floating Point Status/Control value to set
+ */
+__STATIC_FORCEINLINE void __set_FPSCR(uint32_t fpscr)
+{
+#if ((defined (__FPU_PRESENT) && (__FPU_PRESENT == 1U)) && \
+ (defined (__FPU_USED ) && (__FPU_USED == 1U)) )
+#if __has_builtin(__builtin_arm_set_fpscr)
+// Re-enable using built-in when GCC has been fixed
+// || (__GNUC__ > 7) || (__GNUC__ == 7 && __GNUC_MINOR__ >= 2)
+ /* see https://gcc.gnu.org/ml/gcc-patches/2017-04/msg00443.html */
+ __builtin_arm_set_fpscr(fpscr);
+#else
+ __ASM volatile ("VMSR fpscr, %0" : : "r" (fpscr) : "vfpcc", "memory");
+#endif
+#else
+ (void)fpscr;
+#endif
+}
+
+
+/*@} end of CMSIS_Core_RegAccFunctions */
+
+
+/* ########################## Core Instruction Access ######################### */
+/** \defgroup CMSIS_Core_InstructionInterface CMSIS Core Instruction Interface
+ Access to dedicated instructions
+ @{
+*/
+
+/* Define macros for porting to both thumb1 and thumb2.
+ * For thumb1, use low register (r0-r7), specified by constraint "l"
+ * Otherwise, use general registers, specified by constraint "r" */
+#if defined (__thumb__) && !defined (__thumb2__)
+#define __CMSIS_GCC_OUT_REG(r) "=l" (r)
+#define __CMSIS_GCC_RW_REG(r) "+l" (r)
+#define __CMSIS_GCC_USE_REG(r) "l" (r)
+#else
+#define __CMSIS_GCC_OUT_REG(r) "=r" (r)
+#define __CMSIS_GCC_RW_REG(r) "+r" (r)
+#define __CMSIS_GCC_USE_REG(r) "r" (r)
+#endif
+
+/**
+ \brief No Operation
+ \details No Operation does nothing. This instruction can be used for code alignment purposes.
+ */
+#define __NOP() __ASM volatile ("nop")
+
+/**
+ \brief Wait For Interrupt
+ \details Wait For Interrupt is a hint instruction that suspends execution until one of a number of events occurs.
+ */
+#define __WFI() __ASM volatile ("wfi":::"memory")
+
+
+/**
+ \brief Wait For Event
+ \details Wait For Event is a hint instruction that permits the processor to enter
+ a low-power state until one of a number of events occurs.
+ */
+#define __WFE() __ASM volatile ("wfe":::"memory")
+
+
+/**
+ \brief Send Event
+ \details Send Event is a hint instruction. It causes an event to be signaled to the CPU.
+ */
+#define __SEV() __ASM volatile ("sev")
+
+
+/**
+ \brief Instruction Synchronization Barrier
+ \details Instruction Synchronization Barrier flushes the pipeline in the processor,
+ so that all instructions following the ISB are fetched from cache or memory,
+ after the instruction has been completed.
+ */
+__STATIC_FORCEINLINE void __ISB(void)
+{
+ __ASM volatile ("isb 0xF":::"memory");
+}
+
+
+/**
+ \brief Data Synchronization Barrier
+ \details Acts as a special kind of Data Memory Barrier.
+ It completes when all explicit memory accesses before this instruction complete.
+ */
+__STATIC_FORCEINLINE void __DSB(void)
+{
+ __ASM volatile ("dsb 0xF":::"memory");
+}
+
+
+/**
+ \brief Data Memory Barrier
+ \details Ensures the apparent order of the explicit memory operations before
+ and after the instruction, without ensuring their completion.
+ */
+__STATIC_FORCEINLINE void __DMB(void)
+{
+ __ASM volatile ("dmb 0xF":::"memory");
+}
+
+
+/**
+ \brief Reverse byte order (32 bit)
+ \details Reverses the byte order in unsigned integer value. For example, 0x12345678 becomes 0x78563412.
+ \param [in] value Value to reverse
+ \return Reversed value
+ */
+__STATIC_FORCEINLINE uint32_t __REV(uint32_t value)
+{
+#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 5)
+ return __builtin_bswap32(value);
+#else
+ uint32_t result;
+
+ __ASM ("rev %0, %1" : __CMSIS_GCC_OUT_REG (result) : __CMSIS_GCC_USE_REG (value) );
+ return result;
+#endif
+}
+
+
+/**
+ \brief Reverse byte order (16 bit)
+ \details Reverses the byte order within each halfword of a word. For example, 0x12345678 becomes 0x34127856.
+ \param [in] value Value to reverse
+ \return Reversed value
+ */
+__STATIC_FORCEINLINE uint32_t __REV16(uint32_t value)
+{
+ uint32_t result;
+
+ __ASM ("rev16 %0, %1" : __CMSIS_GCC_OUT_REG (result) : __CMSIS_GCC_USE_REG (value) );
+ return result;
+}
+
+
+/**
+ \brief Reverse byte order (16 bit)
+ \details Reverses the byte order in a 16-bit value and returns the signed 16-bit result. For example, 0x0080 becomes 0x8000.
+ \param [in] value Value to reverse
+ \return Reversed value
+ */
+__STATIC_FORCEINLINE int16_t __REVSH(int16_t value)
+{
+#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 8)
+ return (int16_t)__builtin_bswap16(value);
+#else
+ int16_t result;
+
+ __ASM ("revsh %0, %1" : __CMSIS_GCC_OUT_REG (result) : __CMSIS_GCC_USE_REG (value) );
+ return result;
+#endif
+}
+
+
+/**
+ \brief Rotate Right in unsigned value (32 bit)
+ \details Rotate Right (immediate) provides the value of the contents of a register rotated by a variable number of bits.
+ \param [in] op1 Value to rotate
+ \param [in] op2 Number of Bits to rotate
+ \return Rotated value
+ */
+__STATIC_FORCEINLINE uint32_t __ROR(uint32_t op1, uint32_t op2)
+{
+ op2 %= 32U;
+ if (op2 == 0U)
+ {
+ return op1;
+ }
+ return (op1 >> op2) | (op1 << (32U - op2));
+}
+
+
+/**
+ \brief Breakpoint
+ \details Causes the processor to enter Debug state.
+ Debug tools can use this to investigate system state when the instruction at a particular address is reached.
+ \param [in] value is ignored by the processor.
+ If required, a debugger can use it to store additional information about the breakpoint.
+ */
+#define __BKPT(value) __ASM volatile ("bkpt "#value)
+
+
+/**
+ \brief Reverse bit order of value
+ \details Reverses the bit order of the given value.
+ \param [in] value Value to reverse
+ \return Reversed value
+ */
+__STATIC_FORCEINLINE uint32_t __RBIT(uint32_t value)
+{
+ uint32_t result;
+
+#if ((defined (__ARM_ARCH_7M__ ) && (__ARM_ARCH_7M__ == 1)) || \
+ (defined (__ARM_ARCH_7EM__ ) && (__ARM_ARCH_7EM__ == 1)) || \
+ (defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) )
+ __ASM ("rbit %0, %1" : "=r" (result) : "r" (value) );
+#else
+ uint32_t s = (4U /*sizeof(v)*/ * 8U) - 1U; /* extra shift needed at end */
+
+ result = value; /* r will be reversed bits of v; first get LSB of v */
+ for (value >>= 1U; value != 0U; value >>= 1U)
+ {
+ result <<= 1U;
+ result |= value & 1U;
+ s--;
+ }
+ result <<= s; /* shift when v's highest bits are zero */
+#endif
+ return result;
+}
+
+
+/**
+ \brief Count leading zeros
+ \details Counts the number of leading zeros of a data value.
+ \param [in] value Value to count the leading zeros
+ \return number of leading zeros in value
+ */
+__STATIC_FORCEINLINE uint8_t __CLZ(uint32_t value)
+{
+ /* Even though __builtin_clz produces a CLZ instruction on ARM, formally
+ __builtin_clz(0) is undefined behaviour, so handle this case specially.
+ This guarantees ARM-compatible results if happening to compile on a non-ARM
+ target, and ensures the compiler doesn't decide to activate any
+ optimisations using the logic "value was passed to __builtin_clz, so it
+ is non-zero".
+ ARM GCC 7.3 and possibly earlier will optimise this test away, leaving a
+ single CLZ instruction.
+ */
+ if (value == 0U)
+ {
+ return 32U;
+ }
+ return __builtin_clz(value);
+}
+
+
+#if ((defined (__ARM_ARCH_7M__ ) && (__ARM_ARCH_7M__ == 1)) || \
+ (defined (__ARM_ARCH_7EM__ ) && (__ARM_ARCH_7EM__ == 1)) || \
+ (defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) || \
+ (defined (__ARM_ARCH_8M_BASE__ ) && (__ARM_ARCH_8M_BASE__ == 1)) )
+/**
+ \brief LDR Exclusive (8 bit)
+ \details Executes a exclusive LDR instruction for 8 bit value.
+ \param [in] ptr Pointer to data
+ \return value of type uint8_t at (*ptr)
+ */
+__STATIC_FORCEINLINE uint8_t __LDREXB(volatile uint8_t *addr)
+{
+ uint32_t result;
+
+#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 8)
+ __ASM volatile ("ldrexb %0, %1" : "=r" (result) : "Q" (*addr) );
+#else
+ /* Prior to GCC 4.8, "Q" will be expanded to [rx, #0] which is not
+ accepted by assembler. So has to use following less efficient pattern.
+ */
+ __ASM volatile ("ldrexb %0, [%1]" : "=r" (result) : "r" (addr) : "memory" );
+#endif
+ return ((uint8_t) result); /* Add explicit type cast here */
+}
+
+
+/**
+ \brief LDR Exclusive (16 bit)
+ \details Executes a exclusive LDR instruction for 16 bit values.
+ \param [in] ptr Pointer to data
+ \return value of type uint16_t at (*ptr)
+ */
+__STATIC_FORCEINLINE uint16_t __LDREXH(volatile uint16_t *addr)
+{
+ uint32_t result;
+
+#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 8)
+ __ASM volatile ("ldrexh %0, %1" : "=r" (result) : "Q" (*addr) );
+#else
+ /* Prior to GCC 4.8, "Q" will be expanded to [rx, #0] which is not
+ accepted by assembler. So has to use following less efficient pattern.
+ */
+ __ASM volatile ("ldrexh %0, [%1]" : "=r" (result) : "r" (addr) : "memory" );
+#endif
+ return ((uint16_t) result); /* Add explicit type cast here */
+}
+
+
+/**
+ \brief LDR Exclusive (32 bit)
+ \details Executes a exclusive LDR instruction for 32 bit values.
+ \param [in] ptr Pointer to data
+ \return value of type uint32_t at (*ptr)
+ */
+__STATIC_FORCEINLINE uint32_t __LDREXW(volatile uint32_t *addr)
+{
+ uint32_t result;
+
+ __ASM volatile ("ldrex %0, %1" : "=r" (result) : "Q" (*addr) );
+ return(result);
+}
+
+
+/**
+ \brief STR Exclusive (8 bit)
+ \details Executes a exclusive STR instruction for 8 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ \return 0 Function succeeded
+ \return 1 Function failed
+ */
+__STATIC_FORCEINLINE uint32_t __STREXB(uint8_t value, volatile uint8_t *addr)
+{
+ uint32_t result;
+
+ __ASM volatile ("strexb %0, %2, %1" : "=&r" (result), "=Q" (*addr) : "r" ((uint32_t)value) );
+ return(result);
+}
+
+
+/**
+ \brief STR Exclusive (16 bit)
+ \details Executes a exclusive STR instruction for 16 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ \return 0 Function succeeded
+ \return 1 Function failed
+ */
+__STATIC_FORCEINLINE uint32_t __STREXH(uint16_t value, volatile uint16_t *addr)
+{
+ uint32_t result;
+
+ __ASM volatile ("strexh %0, %2, %1" : "=&r" (result), "=Q" (*addr) : "r" ((uint32_t)value) );
+ return(result);
+}
+
+
+/**
+ \brief STR Exclusive (32 bit)
+ \details Executes a exclusive STR instruction for 32 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ \return 0 Function succeeded
+ \return 1 Function failed
+ */
+__STATIC_FORCEINLINE uint32_t __STREXW(uint32_t value, volatile uint32_t *addr)
+{
+ uint32_t result;
+
+ __ASM volatile ("strex %0, %2, %1" : "=&r" (result), "=Q" (*addr) : "r" (value) );
+ return(result);
+}
+
+
+/**
+ \brief Remove the exclusive lock
+ \details Removes the exclusive lock which is created by LDREX.
+ */
+__STATIC_FORCEINLINE void __CLREX(void)
+{
+ __ASM volatile ("clrex" ::: "memory");
+}
+
+#endif /* ((defined (__ARM_ARCH_7M__ ) && (__ARM_ARCH_7M__ == 1)) || \
+ (defined (__ARM_ARCH_7EM__ ) && (__ARM_ARCH_7EM__ == 1)) || \
+ (defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) || \
+ (defined (__ARM_ARCH_8M_BASE__ ) && (__ARM_ARCH_8M_BASE__ == 1)) ) */
+
+
+#if ((defined (__ARM_ARCH_7M__ ) && (__ARM_ARCH_7M__ == 1)) || \
+ (defined (__ARM_ARCH_7EM__ ) && (__ARM_ARCH_7EM__ == 1)) || \
+ (defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) )
+/**
+ \brief Signed Saturate
+ \details Saturates a signed value.
+ \param [in] ARG1 Value to be saturated
+ \param [in] ARG2 Bit position to saturate to (1..32)
+ \return Saturated value
+ */
+#define __SSAT(ARG1, ARG2) \
+__extension__ \
+({ \
+ int32_t __RES, __ARG1 = (ARG1); \
+ __ASM volatile ("ssat %0, %1, %2" : "=r" (__RES) : "I" (ARG2), "r" (__ARG1) : "cc" ); \
+ __RES; \
+ })
+
+
+/**
+ \brief Unsigned Saturate
+ \details Saturates an unsigned value.
+ \param [in] ARG1 Value to be saturated
+ \param [in] ARG2 Bit position to saturate to (0..31)
+ \return Saturated value
+ */
+#define __USAT(ARG1, ARG2) \
+ __extension__ \
+({ \
+ uint32_t __RES, __ARG1 = (ARG1); \
+ __ASM volatile ("usat %0, %1, %2" : "=r" (__RES) : "I" (ARG2), "r" (__ARG1) : "cc" ); \
+ __RES; \
+ })
+
+
+/**
+ \brief Rotate Right with Extend (32 bit)
+ \details Moves each bit of a bitstring right by one bit.
+ The carry input is shifted in at the left end of the bitstring.
+ \param [in] value Value to rotate
+ \return Rotated value
+ */
+__STATIC_FORCEINLINE uint32_t __RRX(uint32_t value)
+{
+ uint32_t result;
+
+ __ASM volatile ("rrx %0, %1" : __CMSIS_GCC_OUT_REG (result) : __CMSIS_GCC_USE_REG (value) );
+ return(result);
+}
+
+
+/**
+ \brief LDRT Unprivileged (8 bit)
+ \details Executes a Unprivileged LDRT instruction for 8 bit value.
+ \param [in] ptr Pointer to data
+ \return value of type uint8_t at (*ptr)
+ */
+__STATIC_FORCEINLINE uint8_t __LDRBT(volatile uint8_t *ptr)
+{
+ uint32_t result;
+
+#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 8)
+ __ASM volatile ("ldrbt %0, %1" : "=r" (result) : "Q" (*ptr) );
+#else
+ /* Prior to GCC 4.8, "Q" will be expanded to [rx, #0] which is not
+ accepted by assembler. So has to use following less efficient pattern.
+ */
+ __ASM volatile ("ldrbt %0, [%1]" : "=r" (result) : "r" (ptr) : "memory" );
+#endif
+ return ((uint8_t) result); /* Add explicit type cast here */
+}
+
+
+/**
+ \brief LDRT Unprivileged (16 bit)
+ \details Executes a Unprivileged LDRT instruction for 16 bit values.
+ \param [in] ptr Pointer to data
+ \return value of type uint16_t at (*ptr)
+ */
+__STATIC_FORCEINLINE uint16_t __LDRHT(volatile uint16_t *ptr)
+{
+ uint32_t result;
+
+#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 8)
+ __ASM volatile ("ldrht %0, %1" : "=r" (result) : "Q" (*ptr) );
+#else
+ /* Prior to GCC 4.8, "Q" will be expanded to [rx, #0] which is not
+ accepted by assembler. So has to use following less efficient pattern.
+ */
+ __ASM volatile ("ldrht %0, [%1]" : "=r" (result) : "r" (ptr) : "memory" );
+#endif
+ return ((uint16_t) result); /* Add explicit type cast here */
+}
+
+
+/**
+ \brief LDRT Unprivileged (32 bit)
+ \details Executes a Unprivileged LDRT instruction for 32 bit values.
+ \param [in] ptr Pointer to data
+ \return value of type uint32_t at (*ptr)
+ */
+__STATIC_FORCEINLINE uint32_t __LDRT(volatile uint32_t *ptr)
+{
+ uint32_t result;
+
+ __ASM volatile ("ldrt %0, %1" : "=r" (result) : "Q" (*ptr) );
+ return(result);
+}
+
+
+/**
+ \brief STRT Unprivileged (8 bit)
+ \details Executes a Unprivileged STRT instruction for 8 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ */
+__STATIC_FORCEINLINE void __STRBT(uint8_t value, volatile uint8_t *ptr)
+{
+ __ASM volatile ("strbt %1, %0" : "=Q" (*ptr) : "r" ((uint32_t)value) );
+}
+
+
+/**
+ \brief STRT Unprivileged (16 bit)
+ \details Executes a Unprivileged STRT instruction for 16 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ */
+__STATIC_FORCEINLINE void __STRHT(uint16_t value, volatile uint16_t *ptr)
+{
+ __ASM volatile ("strht %1, %0" : "=Q" (*ptr) : "r" ((uint32_t)value) );
+}
+
+
+/**
+ \brief STRT Unprivileged (32 bit)
+ \details Executes a Unprivileged STRT instruction for 32 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ */
+__STATIC_FORCEINLINE void __STRT(uint32_t value, volatile uint32_t *ptr)
+{
+ __ASM volatile ("strt %1, %0" : "=Q" (*ptr) : "r" (value) );
+}
+
+#else /* ((defined (__ARM_ARCH_7M__ ) && (__ARM_ARCH_7M__ == 1)) || \
+ (defined (__ARM_ARCH_7EM__ ) && (__ARM_ARCH_7EM__ == 1)) || \
+ (defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) ) */
+
+/**
+ \brief Signed Saturate
+ \details Saturates a signed value.
+ \param [in] value Value to be saturated
+ \param [in] sat Bit position to saturate to (1..32)
+ \return Saturated value
+ */
+__STATIC_FORCEINLINE int32_t __SSAT(int32_t val, uint32_t sat)
+{
+ if ((sat >= 1U) && (sat <= 32U))
+ {
+ const int32_t max = (int32_t)((1U << (sat - 1U)) - 1U);
+ const int32_t min = -1 - max ;
+ if (val > max)
+ {
+ return max;
+ }
+ else if (val < min)
+ {
+ return min;
+ }
+ }
+ return val;
+}
+
+/**
+ \brief Unsigned Saturate
+ \details Saturates an unsigned value.
+ \param [in] value Value to be saturated
+ \param [in] sat Bit position to saturate to (0..31)
+ \return Saturated value
+ */
+__STATIC_FORCEINLINE uint32_t __USAT(int32_t val, uint32_t sat)
+{
+ if (sat <= 31U)
+ {
+ const uint32_t max = ((1U << sat) - 1U);
+ if (val > (int32_t)max)
+ {
+ return max;
+ }
+ else if (val < 0)
+ {
+ return 0U;
+ }
+ }
+ return (uint32_t)val;
+}
+
+#endif /* ((defined (__ARM_ARCH_7M__ ) && (__ARM_ARCH_7M__ == 1)) || \
+ (defined (__ARM_ARCH_7EM__ ) && (__ARM_ARCH_7EM__ == 1)) || \
+ (defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) ) */
+
+
+#if ((defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) || \
+ (defined (__ARM_ARCH_8M_BASE__ ) && (__ARM_ARCH_8M_BASE__ == 1)) )
+/**
+ \brief Load-Acquire (8 bit)
+ \details Executes a LDAB instruction for 8 bit value.
+ \param [in] ptr Pointer to data
+ \return value of type uint8_t at (*ptr)
+ */
+__STATIC_FORCEINLINE uint8_t __LDAB(volatile uint8_t *ptr)
+{
+ uint32_t result;
+
+ __ASM volatile ("ldab %0, %1" : "=r" (result) : "Q" (*ptr) : "memory" );
+ return ((uint8_t) result);
+}
+
+
+/**
+ \brief Load-Acquire (16 bit)
+ \details Executes a LDAH instruction for 16 bit values.
+ \param [in] ptr Pointer to data
+ \return value of type uint16_t at (*ptr)
+ */
+__STATIC_FORCEINLINE uint16_t __LDAH(volatile uint16_t *ptr)
+{
+ uint32_t result;
+
+ __ASM volatile ("ldah %0, %1" : "=r" (result) : "Q" (*ptr) : "memory" );
+ return ((uint16_t) result);
+}
+
+
+/**
+ \brief Load-Acquire (32 bit)
+ \details Executes a LDA instruction for 32 bit values.
+ \param [in] ptr Pointer to data
+ \return value of type uint32_t at (*ptr)
+ */
+__STATIC_FORCEINLINE uint32_t __LDA(volatile uint32_t *ptr)
+{
+ uint32_t result;
+
+ __ASM volatile ("lda %0, %1" : "=r" (result) : "Q" (*ptr) : "memory" );
+ return(result);
+}
+
+
+/**
+ \brief Store-Release (8 bit)
+ \details Executes a STLB instruction for 8 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ */
+__STATIC_FORCEINLINE void __STLB(uint8_t value, volatile uint8_t *ptr)
+{
+ __ASM volatile ("stlb %1, %0" : "=Q" (*ptr) : "r" ((uint32_t)value) : "memory" );
+}
+
+
+/**
+ \brief Store-Release (16 bit)
+ \details Executes a STLH instruction for 16 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ */
+__STATIC_FORCEINLINE void __STLH(uint16_t value, volatile uint16_t *ptr)
+{
+ __ASM volatile ("stlh %1, %0" : "=Q" (*ptr) : "r" ((uint32_t)value) : "memory" );
+}
+
+
+/**
+ \brief Store-Release (32 bit)
+ \details Executes a STL instruction for 32 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ */
+__STATIC_FORCEINLINE void __STL(uint32_t value, volatile uint32_t *ptr)
+{
+ __ASM volatile ("stl %1, %0" : "=Q" (*ptr) : "r" ((uint32_t)value) : "memory" );
+}
+
+
+/**
+ \brief Load-Acquire Exclusive (8 bit)
+ \details Executes a LDAB exclusive instruction for 8 bit value.
+ \param [in] ptr Pointer to data
+ \return value of type uint8_t at (*ptr)
+ */
+__STATIC_FORCEINLINE uint8_t __LDAEXB(volatile uint8_t *ptr)
+{
+ uint32_t result;
+
+ __ASM volatile ("ldaexb %0, %1" : "=r" (result) : "Q" (*ptr) : "memory" );
+ return ((uint8_t) result);
+}
+
+
+/**
+ \brief Load-Acquire Exclusive (16 bit)
+ \details Executes a LDAH exclusive instruction for 16 bit values.
+ \param [in] ptr Pointer to data
+ \return value of type uint16_t at (*ptr)
+ */
+__STATIC_FORCEINLINE uint16_t __LDAEXH(volatile uint16_t *ptr)
+{
+ uint32_t result;
+
+ __ASM volatile ("ldaexh %0, %1" : "=r" (result) : "Q" (*ptr) : "memory" );
+ return ((uint16_t) result);
+}
+
+
+/**
+ \brief Load-Acquire Exclusive (32 bit)
+ \details Executes a LDA exclusive instruction for 32 bit values.
+ \param [in] ptr Pointer to data
+ \return value of type uint32_t at (*ptr)
+ */
+__STATIC_FORCEINLINE uint32_t __LDAEX(volatile uint32_t *ptr)
+{
+ uint32_t result;
+
+ __ASM volatile ("ldaex %0, %1" : "=r" (result) : "Q" (*ptr) : "memory" );
+ return(result);
+}
+
+
+/**
+ \brief Store-Release Exclusive (8 bit)
+ \details Executes a STLB exclusive instruction for 8 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ \return 0 Function succeeded
+ \return 1 Function failed
+ */
+__STATIC_FORCEINLINE uint32_t __STLEXB(uint8_t value, volatile uint8_t *ptr)
+{
+ uint32_t result;
+
+ __ASM volatile ("stlexb %0, %2, %1" : "=&r" (result), "=Q" (*ptr) : "r" ((uint32_t)value) : "memory" );
+ return(result);
+}
+
+
+/**
+ \brief Store-Release Exclusive (16 bit)
+ \details Executes a STLH exclusive instruction for 16 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ \return 0 Function succeeded
+ \return 1 Function failed
+ */
+__STATIC_FORCEINLINE uint32_t __STLEXH(uint16_t value, volatile uint16_t *ptr)
+{
+ uint32_t result;
+
+ __ASM volatile ("stlexh %0, %2, %1" : "=&r" (result), "=Q" (*ptr) : "r" ((uint32_t)value) : "memory" );
+ return(result);
+}
+
+
+/**
+ \brief Store-Release Exclusive (32 bit)
+ \details Executes a STL exclusive instruction for 32 bit values.
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ \return 0 Function succeeded
+ \return 1 Function failed
+ */
+__STATIC_FORCEINLINE uint32_t __STLEX(uint32_t value, volatile uint32_t *ptr)
+{
+ uint32_t result;
+
+ __ASM volatile ("stlex %0, %2, %1" : "=&r" (result), "=Q" (*ptr) : "r" ((uint32_t)value) : "memory" );
+ return(result);
+}
+
+#endif /* ((defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) || \
+ (defined (__ARM_ARCH_8M_BASE__ ) && (__ARM_ARCH_8M_BASE__ == 1)) ) */
+
+/*@}*/ /* end of group CMSIS_Core_InstructionInterface */
+
+
+/* ################### Compiler specific Intrinsics ########################### */
+/** \defgroup CMSIS_SIMD_intrinsics CMSIS SIMD Intrinsics
+ Access to dedicated SIMD instructions
+ @{
+*/
+
+#if (defined (__ARM_FEATURE_DSP) && (__ARM_FEATURE_DSP == 1))
+
+__STATIC_FORCEINLINE uint32_t __SADD8(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("sadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __QADD8(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM ("qadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __SHADD8(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM ("shadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __UADD8(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("uadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __UQADD8(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM ("uqadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __UHADD8(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM ("uhadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+
+__STATIC_FORCEINLINE uint32_t __SSUB8(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("ssub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __QSUB8(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM ("qsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __SHSUB8(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM ("shsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __USUB8(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("usub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __UQSUB8(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM ("uqsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __UHSUB8(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM ("uhsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+
+__STATIC_FORCEINLINE uint32_t __SADD16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("sadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __QADD16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM ("qadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __SHADD16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM ("shadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __UADD16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("uadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __UQADD16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM ("uqadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __UHADD16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM ("uhadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __SSUB16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("ssub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __QSUB16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM ("qsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __SHSUB16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM ("shsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __USUB16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("usub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __UQSUB16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM ("uqsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __UHSUB16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM ("uhsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __SASX(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("sasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __QASX(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM ("qasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __SHASX(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM ("shasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __UASX(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("uasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __UQASX(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM ("uqasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __UHASX(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM ("uhasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __SSAX(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("ssax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __QSAX(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM ("qsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __SHSAX(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM ("shsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __USAX(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("usax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __UQSAX(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM ("uqsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __UHSAX(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM ("uhsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __USAD8(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM ("usad8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __USADA8(uint32_t op1, uint32_t op2, uint32_t op3)
+{
+ uint32_t result;
+
+ __ASM ("usada8 %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );
+ return(result);
+}
+
+#define __SSAT16(ARG1, ARG2) \
+({ \
+ int32_t __RES, __ARG1 = (ARG1); \
+ __ASM volatile ("ssat16 %0, %1, %2" : "=r" (__RES) : "I" (ARG2), "r" (__ARG1) : "cc" ); \
+ __RES; \
+ })
+
+#define __USAT16(ARG1, ARG2) \
+({ \
+ uint32_t __RES, __ARG1 = (ARG1); \
+ __ASM volatile ("usat16 %0, %1, %2" : "=r" (__RES) : "I" (ARG2), "r" (__ARG1) : "cc" ); \
+ __RES; \
+ })
+
+__STATIC_FORCEINLINE uint32_t __UXTB16(uint32_t op1)
+{
+ uint32_t result;
+
+ __ASM ("uxtb16 %0, %1" : "=r" (result) : "r" (op1));
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __UXTAB16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM ("uxtab16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __SXTB16(uint32_t op1)
+{
+ uint32_t result;
+
+ __ASM ("sxtb16 %0, %1" : "=r" (result) : "r" (op1));
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __SXTB16_RORn(uint32_t op1, uint32_t rotate)
+{
+ uint32_t result;
+
+ __ASM ("sxtb16 %0, %1, ROR %2" : "=r" (result) : "r" (op1), "i" (rotate) );
+
+ return result;
+}
+
+__STATIC_FORCEINLINE uint32_t __SXTAB16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM ("sxtab16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __SMUAD (uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("smuad %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __SMUADX (uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("smuadx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __SMLAD (uint32_t op1, uint32_t op2, uint32_t op3)
+{
+ uint32_t result;
+
+ __ASM volatile ("smlad %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __SMLADX (uint32_t op1, uint32_t op2, uint32_t op3)
+{
+ uint32_t result;
+
+ __ASM volatile ("smladx %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint64_t __SMLALD (uint32_t op1, uint32_t op2, uint64_t acc)
+{
+ union llreg_u{
+ uint32_t w32[2];
+ uint64_t w64;
+ } llr;
+ llr.w64 = acc;
+
+#ifndef __ARMEB__ /* Little endian */
+ __ASM volatile ("smlald %0, %1, %2, %3" : "=r" (llr.w32[0]), "=r" (llr.w32[1]): "r" (op1), "r" (op2) , "0" (llr.w32[0]), "1" (llr.w32[1]) );
+#else /* Big endian */
+ __ASM volatile ("smlald %0, %1, %2, %3" : "=r" (llr.w32[1]), "=r" (llr.w32[0]): "r" (op1), "r" (op2) , "0" (llr.w32[1]), "1" (llr.w32[0]) );
+#endif
+
+ return(llr.w64);
+}
+
+__STATIC_FORCEINLINE uint64_t __SMLALDX (uint32_t op1, uint32_t op2, uint64_t acc)
+{
+ union llreg_u{
+ uint32_t w32[2];
+ uint64_t w64;
+ } llr;
+ llr.w64 = acc;
+
+#ifndef __ARMEB__ /* Little endian */
+ __ASM volatile ("smlaldx %0, %1, %2, %3" : "=r" (llr.w32[0]), "=r" (llr.w32[1]): "r" (op1), "r" (op2) , "0" (llr.w32[0]), "1" (llr.w32[1]) );
+#else /* Big endian */
+ __ASM volatile ("smlaldx %0, %1, %2, %3" : "=r" (llr.w32[1]), "=r" (llr.w32[0]): "r" (op1), "r" (op2) , "0" (llr.w32[1]), "1" (llr.w32[0]) );
+#endif
+
+ return(llr.w64);
+}
+
+__STATIC_FORCEINLINE uint32_t __SMUSD (uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("smusd %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __SMUSDX (uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("smusdx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __SMLSD (uint32_t op1, uint32_t op2, uint32_t op3)
+{
+ uint32_t result;
+
+ __ASM volatile ("smlsd %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint32_t __SMLSDX (uint32_t op1, uint32_t op2, uint32_t op3)
+{
+ uint32_t result;
+
+ __ASM volatile ("smlsdx %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE uint64_t __SMLSLD (uint32_t op1, uint32_t op2, uint64_t acc)
+{
+ union llreg_u{
+ uint32_t w32[2];
+ uint64_t w64;
+ } llr;
+ llr.w64 = acc;
+
+#ifndef __ARMEB__ /* Little endian */
+ __ASM volatile ("smlsld %0, %1, %2, %3" : "=r" (llr.w32[0]), "=r" (llr.w32[1]): "r" (op1), "r" (op2) , "0" (llr.w32[0]), "1" (llr.w32[1]) );
+#else /* Big endian */
+ __ASM volatile ("smlsld %0, %1, %2, %3" : "=r" (llr.w32[1]), "=r" (llr.w32[0]): "r" (op1), "r" (op2) , "0" (llr.w32[1]), "1" (llr.w32[0]) );
+#endif
+
+ return(llr.w64);
+}
+
+__STATIC_FORCEINLINE uint64_t __SMLSLDX (uint32_t op1, uint32_t op2, uint64_t acc)
+{
+ union llreg_u{
+ uint32_t w32[2];
+ uint64_t w64;
+ } llr;
+ llr.w64 = acc;
+
+#ifndef __ARMEB__ /* Little endian */
+ __ASM volatile ("smlsldx %0, %1, %2, %3" : "=r" (llr.w32[0]), "=r" (llr.w32[1]): "r" (op1), "r" (op2) , "0" (llr.w32[0]), "1" (llr.w32[1]) );
+#else /* Big endian */
+ __ASM volatile ("smlsldx %0, %1, %2, %3" : "=r" (llr.w32[1]), "=r" (llr.w32[0]): "r" (op1), "r" (op2) , "0" (llr.w32[1]), "1" (llr.w32[0]) );
+#endif
+
+ return(llr.w64);
+}
+
+__STATIC_FORCEINLINE uint32_t __SEL (uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("sel %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE int32_t __QADD( int32_t op1, int32_t op2)
+{
+ int32_t result;
+
+ __ASM volatile ("qadd %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__STATIC_FORCEINLINE int32_t __QSUB( int32_t op1, int32_t op2)
+{
+ int32_t result;
+
+ __ASM volatile ("qsub %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+#if 0
+#define __PKHBT(ARG1,ARG2,ARG3) \
+({ \
+ uint32_t __RES, __ARG1 = (ARG1), __ARG2 = (ARG2); \
+ __ASM ("pkhbt %0, %1, %2, lsl %3" : "=r" (__RES) : "r" (__ARG1), "r" (__ARG2), "I" (ARG3) ); \
+ __RES; \
+ })
+
+#define __PKHTB(ARG1,ARG2,ARG3) \
+({ \
+ uint32_t __RES, __ARG1 = (ARG1), __ARG2 = (ARG2); \
+ if (ARG3 == 0) \
+ __ASM ("pkhtb %0, %1, %2" : "=r" (__RES) : "r" (__ARG1), "r" (__ARG2) ); \
+ else \
+ __ASM ("pkhtb %0, %1, %2, asr %3" : "=r" (__RES) : "r" (__ARG1), "r" (__ARG2), "I" (ARG3) ); \
+ __RES; \
+ })
+#endif
+
+#define __PKHBT(ARG1,ARG2,ARG3) ( ((((uint32_t)(ARG1)) ) & 0x0000FFFFUL) | \
+ ((((uint32_t)(ARG2)) << (ARG3)) & 0xFFFF0000UL) )
+
+#define __PKHTB(ARG1,ARG2,ARG3) ( ((((uint32_t)(ARG1)) ) & 0xFFFF0000UL) | \
+ ((((uint32_t)(ARG2)) >> (ARG3)) & 0x0000FFFFUL) )
+
+__STATIC_FORCEINLINE int32_t __SMMLA (int32_t op1, int32_t op2, int32_t op3)
+{
+ int32_t result;
+
+ __ASM ("smmla %0, %1, %2, %3" : "=r" (result): "r" (op1), "r" (op2), "r" (op3) );
+ return(result);
+}
+
+#endif /* (__ARM_FEATURE_DSP == 1) */
+/*@} end of group CMSIS_SIMD_intrinsics */
+
+
+#pragma GCC diagnostic pop
+
+#endif /* __CMSIS_GCC_H */
diff --git a/src/rp2_common/cmsis/stub/CMSIS/Core/Include/cmsis_iccarm.h b/src/rp2_common/cmsis/stub/CMSIS/Core/Include/cmsis_iccarm.h
new file mode 100644
index 0000000..7eeffca
--- /dev/null
+++ b/src/rp2_common/cmsis/stub/CMSIS/Core/Include/cmsis_iccarm.h
@@ -0,0 +1,968 @@
+/**************************************************************************//**
+ * @file cmsis_iccarm.h
+ * @brief CMSIS compiler ICCARM (IAR Compiler for Arm) header file
+ * @version V5.2.0
+ * @date 28. January 2020
+ ******************************************************************************/
+
+//------------------------------------------------------------------------------
+//
+// Copyright (c) 2017-2019 IAR Systems
+// Copyright (c) 2017-2019 Arm Limited. All rights reserved.
+//
+// SPDX-License-Identifier: Apache-2.0
+//
+// Licensed under the Apache License, Version 2.0 (the "License")
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+//------------------------------------------------------------------------------
+
+
+#ifndef __CMSIS_ICCARM_H__
+#define __CMSIS_ICCARM_H__
+
+#ifndef __ICCARM__
+ #error This file should only be compiled by ICCARM
+#endif
+
+#pragma system_include
+
+#define __IAR_FT _Pragma("inline=forced") __intrinsic
+
+#if (__VER__ >= 8000000)
+ #define __ICCARM_V8 1
+#else
+ #define __ICCARM_V8 0
+#endif
+
+#ifndef __ALIGNED
+ #if __ICCARM_V8
+ #define __ALIGNED(x) __attribute__((aligned(x)))
+ #elif (__VER__ >= 7080000)
+ /* Needs IAR language extensions */
+ #define __ALIGNED(x) __attribute__((aligned(x)))
+ #else
+ #warning No compiler specific solution for __ALIGNED.__ALIGNED is ignored.
+ #define __ALIGNED(x)
+ #endif
+#endif
+
+
+/* Define compiler macros for CPU architecture, used in CMSIS 5.
+ */
+#if __ARM_ARCH_6M__ || __ARM_ARCH_7M__ || __ARM_ARCH_7EM__ || __ARM_ARCH_8M_BASE__ || __ARM_ARCH_8M_MAIN__
+/* Macros already defined */
+#else
+ #if defined(__ARM8M_MAINLINE__) || defined(__ARM8EM_MAINLINE__)
+ #define __ARM_ARCH_8M_MAIN__ 1
+ #elif defined(__ARM8M_BASELINE__)
+ #define __ARM_ARCH_8M_BASE__ 1
+ #elif defined(__ARM_ARCH_PROFILE) && __ARM_ARCH_PROFILE == 'M'
+ #if __ARM_ARCH == 6
+ #define __ARM_ARCH_6M__ 1
+ #elif __ARM_ARCH == 7
+ #if __ARM_FEATURE_DSP
+ #define __ARM_ARCH_7EM__ 1
+ #else
+ #define __ARM_ARCH_7M__ 1
+ #endif
+ #endif /* __ARM_ARCH */
+ #endif /* __ARM_ARCH_PROFILE == 'M' */
+#endif
+
+/* Alternativ core deduction for older ICCARM's */
+#if !defined(__ARM_ARCH_6M__) && !defined(__ARM_ARCH_7M__) && !defined(__ARM_ARCH_7EM__) && \
+ !defined(__ARM_ARCH_8M_BASE__) && !defined(__ARM_ARCH_8M_MAIN__)
+ #if defined(__ARM6M__) && (__CORE__ == __ARM6M__)
+ #define __ARM_ARCH_6M__ 1
+ #elif defined(__ARM7M__) && (__CORE__ == __ARM7M__)
+ #define __ARM_ARCH_7M__ 1
+ #elif defined(__ARM7EM__) && (__CORE__ == __ARM7EM__)
+ #define __ARM_ARCH_7EM__ 1
+ #elif defined(__ARM8M_BASELINE__) && (__CORE == __ARM8M_BASELINE__)
+ #define __ARM_ARCH_8M_BASE__ 1
+ #elif defined(__ARM8M_MAINLINE__) && (__CORE == __ARM8M_MAINLINE__)
+ #define __ARM_ARCH_8M_MAIN__ 1
+ #elif defined(__ARM8EM_MAINLINE__) && (__CORE == __ARM8EM_MAINLINE__)
+ #define __ARM_ARCH_8M_MAIN__ 1
+ #else
+ #error "Unknown target."
+ #endif
+#endif
+
+
+
+#if defined(__ARM_ARCH_6M__) && __ARM_ARCH_6M__==1
+ #define __IAR_M0_FAMILY 1
+#elif defined(__ARM_ARCH_8M_BASE__) && __ARM_ARCH_8M_BASE__==1
+ #define __IAR_M0_FAMILY 1
+#else
+ #define __IAR_M0_FAMILY 0
+#endif
+
+
+#ifndef __ASM
+ #define __ASM __asm
+#endif
+
+#ifndef __COMPILER_BARRIER
+ #define __COMPILER_BARRIER() __ASM volatile("":::"memory")
+#endif
+
+#ifndef __INLINE
+ #define __INLINE inline
+#endif
+
+#ifndef __NO_RETURN
+ #if __ICCARM_V8
+ #define __NO_RETURN __attribute__((__noreturn__))
+ #else
+ #define __NO_RETURN _Pragma("object_attribute=__noreturn")
+ #endif
+#endif
+
+#ifndef __PACKED
+ #if __ICCARM_V8
+ #define __PACKED __attribute__((packed, aligned(1)))
+ #else
+ /* Needs IAR language extensions */
+ #define __PACKED __packed
+ #endif
+#endif
+
+#ifndef __PACKED_STRUCT
+ #if __ICCARM_V8
+ #define __PACKED_STRUCT struct __attribute__((packed, aligned(1)))
+ #else
+ /* Needs IAR language extensions */
+ #define __PACKED_STRUCT __packed struct
+ #endif
+#endif
+
+#ifndef __PACKED_UNION
+ #if __ICCARM_V8
+ #define __PACKED_UNION union __attribute__((packed, aligned(1)))
+ #else
+ /* Needs IAR language extensions */
+ #define __PACKED_UNION __packed union
+ #endif
+#endif
+
+#ifndef __RESTRICT
+ #if __ICCARM_V8
+ #define __RESTRICT __restrict
+ #else
+ /* Needs IAR language extensions */
+ #define __RESTRICT restrict
+ #endif
+#endif
+
+#ifndef __STATIC_INLINE
+ #define __STATIC_INLINE static inline
+#endif
+
+#ifndef __FORCEINLINE
+ #define __FORCEINLINE _Pragma("inline=forced")
+#endif
+
+#ifndef __STATIC_FORCEINLINE
+ #define __STATIC_FORCEINLINE __FORCEINLINE __STATIC_INLINE
+#endif
+
+#ifndef __UNALIGNED_UINT16_READ
+#pragma language=save
+#pragma language=extended
+__IAR_FT uint16_t __iar_uint16_read(void const *ptr)
+{
+ return *(__packed uint16_t*)(ptr);
+}
+#pragma language=restore
+#define __UNALIGNED_UINT16_READ(PTR) __iar_uint16_read(PTR)
+#endif
+
+
+#ifndef __UNALIGNED_UINT16_WRITE
+#pragma language=save
+#pragma language=extended
+__IAR_FT void __iar_uint16_write(void const *ptr, uint16_t val)
+{
+ *(__packed uint16_t*)(ptr) = val;;
+}
+#pragma language=restore
+#define __UNALIGNED_UINT16_WRITE(PTR,VAL) __iar_uint16_write(PTR,VAL)
+#endif
+
+#ifndef __UNALIGNED_UINT32_READ
+#pragma language=save
+#pragma language=extended
+__IAR_FT uint32_t __iar_uint32_read(void const *ptr)
+{
+ return *(__packed uint32_t*)(ptr);
+}
+#pragma language=restore
+#define __UNALIGNED_UINT32_READ(PTR) __iar_uint32_read(PTR)
+#endif
+
+#ifndef __UNALIGNED_UINT32_WRITE
+#pragma language=save
+#pragma language=extended
+__IAR_FT void __iar_uint32_write(void const *ptr, uint32_t val)
+{
+ *(__packed uint32_t*)(ptr) = val;;
+}
+#pragma language=restore
+#define __UNALIGNED_UINT32_WRITE(PTR,VAL) __iar_uint32_write(PTR,VAL)
+#endif
+
+#ifndef __UNALIGNED_UINT32 /* deprecated */
+#pragma language=save
+#pragma language=extended
+__packed struct __iar_u32 { uint32_t v; };
+#pragma language=restore
+#define __UNALIGNED_UINT32(PTR) (((struct __iar_u32 *)(PTR))->v)
+#endif
+
+#ifndef __USED
+ #if __ICCARM_V8
+ #define __USED __attribute__((used))
+ #else
+ #define __USED _Pragma("__root")
+ #endif
+#endif
+
+#ifndef __WEAK
+ #if __ICCARM_V8
+ #define __WEAK __attribute__((weak))
+ #else
+ #define __WEAK _Pragma("__weak")
+ #endif
+#endif
+
+#ifndef __PROGRAM_START
+#define __PROGRAM_START __iar_program_start
+#endif
+
+#ifndef __INITIAL_SP
+#define __INITIAL_SP CSTACK$$Limit
+#endif
+
+#ifndef __STACK_LIMIT
+#define __STACK_LIMIT CSTACK$$Base
+#endif
+
+#ifndef __VECTOR_TABLE
+#define __VECTOR_TABLE __vector_table
+#endif
+
+#ifndef __VECTOR_TABLE_ATTRIBUTE
+#define __VECTOR_TABLE_ATTRIBUTE @".intvec"
+#endif
+
+#ifndef __ICCARM_INTRINSICS_VERSION__
+ #define __ICCARM_INTRINSICS_VERSION__ 0
+#endif
+
+#if __ICCARM_INTRINSICS_VERSION__ == 2
+
+ #if defined(__CLZ)
+ #undef __CLZ
+ #endif
+ #if defined(__REVSH)
+ #undef __REVSH
+ #endif
+ #if defined(__RBIT)
+ #undef __RBIT
+ #endif
+ #if defined(__SSAT)
+ #undef __SSAT
+ #endif
+ #if defined(__USAT)
+ #undef __USAT
+ #endif
+
+ #include "iccarm_builtin.h"
+
+ #define __disable_fault_irq __iar_builtin_disable_fiq
+ #define __disable_irq __iar_builtin_disable_interrupt
+ #define __enable_fault_irq __iar_builtin_enable_fiq
+ #define __enable_irq __iar_builtin_enable_interrupt
+ #define __arm_rsr __iar_builtin_rsr
+ #define __arm_wsr __iar_builtin_wsr
+
+
+ #define __get_APSR() (__arm_rsr("APSR"))
+ #define __get_BASEPRI() (__arm_rsr("BASEPRI"))
+ #define __get_CONTROL() (__arm_rsr("CONTROL"))
+ #define __get_FAULTMASK() (__arm_rsr("FAULTMASK"))
+
+ #if ((defined (__FPU_PRESENT) && (__FPU_PRESENT == 1U)) && \
+ (defined (__FPU_USED ) && (__FPU_USED == 1U)) )
+ #define __get_FPSCR() (__arm_rsr("FPSCR"))
+ #define __set_FPSCR(VALUE) (__arm_wsr("FPSCR", (VALUE)))
+ #else
+ #define __get_FPSCR() ( 0 )
+ #define __set_FPSCR(VALUE) ((void)VALUE)
+ #endif
+
+ #define __get_IPSR() (__arm_rsr("IPSR"))
+ #define __get_MSP() (__arm_rsr("MSP"))
+ #if (!(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) && \
+ (!defined (__ARM_FEATURE_CMSE) || (__ARM_FEATURE_CMSE < 3)))
+ // without main extensions, the non-secure MSPLIM is RAZ/WI
+ #define __get_MSPLIM() (0U)
+ #else
+ #define __get_MSPLIM() (__arm_rsr("MSPLIM"))
+ #endif
+ #define __get_PRIMASK() (__arm_rsr("PRIMASK"))
+ #define __get_PSP() (__arm_rsr("PSP"))
+
+ #if (!(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) && \
+ (!defined (__ARM_FEATURE_CMSE) || (__ARM_FEATURE_CMSE < 3)))
+ // without main extensions, the non-secure PSPLIM is RAZ/WI
+ #define __get_PSPLIM() (0U)
+ #else
+ #define __get_PSPLIM() (__arm_rsr("PSPLIM"))
+ #endif
+
+ #define __get_xPSR() (__arm_rsr("xPSR"))
+
+ #define __set_BASEPRI(VALUE) (__arm_wsr("BASEPRI", (VALUE)))
+ #define __set_BASEPRI_MAX(VALUE) (__arm_wsr("BASEPRI_MAX", (VALUE)))
+ #define __set_CONTROL(VALUE) (__arm_wsr("CONTROL", (VALUE)))
+ #define __set_FAULTMASK(VALUE) (__arm_wsr("FAULTMASK", (VALUE)))
+ #define __set_MSP(VALUE) (__arm_wsr("MSP", (VALUE)))
+
+ #if (!(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) && \
+ (!defined (__ARM_FEATURE_CMSE) || (__ARM_FEATURE_CMSE < 3)))
+ // without main extensions, the non-secure MSPLIM is RAZ/WI
+ #define __set_MSPLIM(VALUE) ((void)(VALUE))
+ #else
+ #define __set_MSPLIM(VALUE) (__arm_wsr("MSPLIM", (VALUE)))
+ #endif
+ #define __set_PRIMASK(VALUE) (__arm_wsr("PRIMASK", (VALUE)))
+ #define __set_PSP(VALUE) (__arm_wsr("PSP", (VALUE)))
+ #if (!(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) && \
+ (!defined (__ARM_FEATURE_CMSE) || (__ARM_FEATURE_CMSE < 3)))
+ // without main extensions, the non-secure PSPLIM is RAZ/WI
+ #define __set_PSPLIM(VALUE) ((void)(VALUE))
+ #else
+ #define __set_PSPLIM(VALUE) (__arm_wsr("PSPLIM", (VALUE)))
+ #endif
+
+ #define __TZ_get_CONTROL_NS() (__arm_rsr("CONTROL_NS"))
+ #define __TZ_set_CONTROL_NS(VALUE) (__arm_wsr("CONTROL_NS", (VALUE)))
+ #define __TZ_get_PSP_NS() (__arm_rsr("PSP_NS"))
+ #define __TZ_set_PSP_NS(VALUE) (__arm_wsr("PSP_NS", (VALUE)))
+ #define __TZ_get_MSP_NS() (__arm_rsr("MSP_NS"))
+ #define __TZ_set_MSP_NS(VALUE) (__arm_wsr("MSP_NS", (VALUE)))
+ #define __TZ_get_SP_NS() (__arm_rsr("SP_NS"))
+ #define __TZ_set_SP_NS(VALUE) (__arm_wsr("SP_NS", (VALUE)))
+ #define __TZ_get_PRIMASK_NS() (__arm_rsr("PRIMASK_NS"))
+ #define __TZ_set_PRIMASK_NS(VALUE) (__arm_wsr("PRIMASK_NS", (VALUE)))
+ #define __TZ_get_BASEPRI_NS() (__arm_rsr("BASEPRI_NS"))
+ #define __TZ_set_BASEPRI_NS(VALUE) (__arm_wsr("BASEPRI_NS", (VALUE)))
+ #define __TZ_get_FAULTMASK_NS() (__arm_rsr("FAULTMASK_NS"))
+ #define __TZ_set_FAULTMASK_NS(VALUE)(__arm_wsr("FAULTMASK_NS", (VALUE)))
+
+ #if (!(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) && \
+ (!defined (__ARM_FEATURE_CMSE) || (__ARM_FEATURE_CMSE < 3)))
+ // without main extensions, the non-secure PSPLIM is RAZ/WI
+ #define __TZ_get_PSPLIM_NS() (0U)
+ #define __TZ_set_PSPLIM_NS(VALUE) ((void)(VALUE))
+ #else
+ #define __TZ_get_PSPLIM_NS() (__arm_rsr("PSPLIM_NS"))
+ #define __TZ_set_PSPLIM_NS(VALUE) (__arm_wsr("PSPLIM_NS", (VALUE)))
+ #endif
+
+ #define __TZ_get_MSPLIM_NS() (__arm_rsr("MSPLIM_NS"))
+ #define __TZ_set_MSPLIM_NS(VALUE) (__arm_wsr("MSPLIM_NS", (VALUE)))
+
+ #define __NOP __iar_builtin_no_operation
+
+ #define __CLZ __iar_builtin_CLZ
+ #define __CLREX __iar_builtin_CLREX
+
+ #define __DMB __iar_builtin_DMB
+ #define __DSB __iar_builtin_DSB
+ #define __ISB __iar_builtin_ISB
+
+ #define __LDREXB __iar_builtin_LDREXB
+ #define __LDREXH __iar_builtin_LDREXH
+ #define __LDREXW __iar_builtin_LDREX
+
+ #define __RBIT __iar_builtin_RBIT
+ #define __REV __iar_builtin_REV
+ #define __REV16 __iar_builtin_REV16
+
+ __IAR_FT int16_t __REVSH(int16_t val)
+ {
+ return (int16_t) __iar_builtin_REVSH(val);
+ }
+
+ #define __ROR __iar_builtin_ROR
+ #define __RRX __iar_builtin_RRX
+
+ #define __SEV __iar_builtin_SEV
+
+ #if !__IAR_M0_FAMILY
+ #define __SSAT __iar_builtin_SSAT
+ #endif
+
+ #define __STREXB __iar_builtin_STREXB
+ #define __STREXH __iar_builtin_STREXH
+ #define __STREXW __iar_builtin_STREX
+
+ #if !__IAR_M0_FAMILY
+ #define __USAT __iar_builtin_USAT
+ #endif
+
+ #define __WFE __iar_builtin_WFE
+ #define __WFI __iar_builtin_WFI
+
+ #if __ARM_MEDIA__
+ #define __SADD8 __iar_builtin_SADD8
+ #define __QADD8 __iar_builtin_QADD8
+ #define __SHADD8 __iar_builtin_SHADD8
+ #define __UADD8 __iar_builtin_UADD8
+ #define __UQADD8 __iar_builtin_UQADD8
+ #define __UHADD8 __iar_builtin_UHADD8
+ #define __SSUB8 __iar_builtin_SSUB8
+ #define __QSUB8 __iar_builtin_QSUB8
+ #define __SHSUB8 __iar_builtin_SHSUB8
+ #define __USUB8 __iar_builtin_USUB8
+ #define __UQSUB8 __iar_builtin_UQSUB8
+ #define __UHSUB8 __iar_builtin_UHSUB8
+ #define __SADD16 __iar_builtin_SADD16
+ #define __QADD16 __iar_builtin_QADD16
+ #define __SHADD16 __iar_builtin_SHADD16
+ #define __UADD16 __iar_builtin_UADD16
+ #define __UQADD16 __iar_builtin_UQADD16
+ #define __UHADD16 __iar_builtin_UHADD16
+ #define __SSUB16 __iar_builtin_SSUB16
+ #define __QSUB16 __iar_builtin_QSUB16
+ #define __SHSUB16 __iar_builtin_SHSUB16
+ #define __USUB16 __iar_builtin_USUB16
+ #define __UQSUB16 __iar_builtin_UQSUB16
+ #define __UHSUB16 __iar_builtin_UHSUB16
+ #define __SASX __iar_builtin_SASX
+ #define __QASX __iar_builtin_QASX
+ #define __SHASX __iar_builtin_SHASX
+ #define __UASX __iar_builtin_UASX
+ #define __UQASX __iar_builtin_UQASX
+ #define __UHASX __iar_builtin_UHASX
+ #define __SSAX __iar_builtin_SSAX
+ #define __QSAX __iar_builtin_QSAX
+ #define __SHSAX __iar_builtin_SHSAX
+ #define __USAX __iar_builtin_USAX
+ #define __UQSAX __iar_builtin_UQSAX
+ #define __UHSAX __iar_builtin_UHSAX
+ #define __USAD8 __iar_builtin_USAD8
+ #define __USADA8 __iar_builtin_USADA8
+ #define __SSAT16 __iar_builtin_SSAT16
+ #define __USAT16 __iar_builtin_USAT16
+ #define __UXTB16 __iar_builtin_UXTB16
+ #define __UXTAB16 __iar_builtin_UXTAB16
+ #define __SXTB16 __iar_builtin_SXTB16
+ #define __SXTAB16 __iar_builtin_SXTAB16
+ #define __SMUAD __iar_builtin_SMUAD
+ #define __SMUADX __iar_builtin_SMUADX
+ #define __SMMLA __iar_builtin_SMMLA
+ #define __SMLAD __iar_builtin_SMLAD
+ #define __SMLADX __iar_builtin_SMLADX
+ #define __SMLALD __iar_builtin_SMLALD
+ #define __SMLALDX __iar_builtin_SMLALDX
+ #define __SMUSD __iar_builtin_SMUSD
+ #define __SMUSDX __iar_builtin_SMUSDX
+ #define __SMLSD __iar_builtin_SMLSD
+ #define __SMLSDX __iar_builtin_SMLSDX
+ #define __SMLSLD __iar_builtin_SMLSLD
+ #define __SMLSLDX __iar_builtin_SMLSLDX
+ #define __SEL __iar_builtin_SEL
+ #define __QADD __iar_builtin_QADD
+ #define __QSUB __iar_builtin_QSUB
+ #define __PKHBT __iar_builtin_PKHBT
+ #define __PKHTB __iar_builtin_PKHTB
+ #endif
+
+#else /* __ICCARM_INTRINSICS_VERSION__ == 2 */
+
+ #if __IAR_M0_FAMILY
+ /* Avoid clash between intrinsics.h and arm_math.h when compiling for Cortex-M0. */
+ #define __CLZ __cmsis_iar_clz_not_active
+ #define __SSAT __cmsis_iar_ssat_not_active
+ #define __USAT __cmsis_iar_usat_not_active
+ #define __RBIT __cmsis_iar_rbit_not_active
+ #define __get_APSR __cmsis_iar_get_APSR_not_active
+ #endif
+
+
+ #if (!((defined (__FPU_PRESENT) && (__FPU_PRESENT == 1U)) && \
+ (defined (__FPU_USED ) && (__FPU_USED == 1U)) ))
+ #define __get_FPSCR __cmsis_iar_get_FPSR_not_active
+ #define __set_FPSCR __cmsis_iar_set_FPSR_not_active
+ #endif
+
+ #ifdef __INTRINSICS_INCLUDED
+ #error intrinsics.h is already included previously!
+ #endif
+
+ #include <intrinsics.h>
+
+ #if __IAR_M0_FAMILY
+ /* Avoid clash between intrinsics.h and arm_math.h when compiling for Cortex-M0. */
+ #undef __CLZ
+ #undef __SSAT
+ #undef __USAT
+ #undef __RBIT
+ #undef __get_APSR
+
+ __STATIC_INLINE uint8_t __CLZ(uint32_t data)
+ {
+ if (data == 0U) { return 32U; }
+
+ uint32_t count = 0U;
+ uint32_t mask = 0x80000000U;
+
+ while ((data & mask) == 0U)
+ {
+ count += 1U;
+ mask = mask >> 1U;
+ }
+ return count;
+ }
+
+ __STATIC_INLINE uint32_t __RBIT(uint32_t v)
+ {
+ uint8_t sc = 31U;
+ uint32_t r = v;
+ for (v >>= 1U; v; v >>= 1U)
+ {
+ r <<= 1U;
+ r |= v & 1U;
+ sc--;
+ }
+ return (r << sc);
+ }
+
+ __STATIC_INLINE uint32_t __get_APSR(void)
+ {
+ uint32_t res;
+ __asm("MRS %0,APSR" : "=r" (res));
+ return res;
+ }
+
+ #endif
+
+ #if (!((defined (__FPU_PRESENT) && (__FPU_PRESENT == 1U)) && \
+ (defined (__FPU_USED ) && (__FPU_USED == 1U)) ))
+ #undef __get_FPSCR
+ #undef __set_FPSCR
+ #define __get_FPSCR() (0)
+ #define __set_FPSCR(VALUE) ((void)VALUE)
+ #endif
+
+ #pragma diag_suppress=Pe940
+ #pragma diag_suppress=Pe177
+
+ #define __enable_irq __enable_interrupt
+ #define __disable_irq __disable_interrupt
+ #define __NOP __no_operation
+
+ #define __get_xPSR __get_PSR
+
+ #if (!defined(__ARM_ARCH_6M__) || __ARM_ARCH_6M__==0)
+
+ __IAR_FT uint32_t __LDREXW(uint32_t volatile *ptr)
+ {
+ return __LDREX((unsigned long *)ptr);
+ }
+
+ __IAR_FT uint32_t __STREXW(uint32_t value, uint32_t volatile *ptr)
+ {
+ return __STREX(value, (unsigned long *)ptr);
+ }
+ #endif
+
+
+ /* __CORTEX_M is defined in core_cm0.h, core_cm3.h and core_cm4.h. */
+ #if (__CORTEX_M >= 0x03)
+
+ __IAR_FT uint32_t __RRX(uint32_t value)
+ {
+ uint32_t result;
+ __ASM volatile("RRX %0, %1" : "=r"(result) : "r" (value));
+ return(result);
+ }
+
+ __IAR_FT void __set_BASEPRI_MAX(uint32_t value)
+ {
+ __asm volatile("MSR BASEPRI_MAX,%0"::"r" (value));
+ }
+
+
+ #define __enable_fault_irq __enable_fiq
+ #define __disable_fault_irq __disable_fiq
+
+
+ #endif /* (__CORTEX_M >= 0x03) */
+
+ __IAR_FT uint32_t __ROR(uint32_t op1, uint32_t op2)
+ {
+ return (op1 >> op2) | (op1 << ((sizeof(op1)*8)-op2));
+ }
+
+ #if ((defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) || \
+ (defined (__ARM_ARCH_8M_BASE__ ) && (__ARM_ARCH_8M_BASE__ == 1)) )
+
+ __IAR_FT uint32_t __get_MSPLIM(void)
+ {
+ uint32_t res;
+ #if (!(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) && \
+ (!defined (__ARM_FEATURE_CMSE ) || (__ARM_FEATURE_CMSE < 3)))
+ // without main extensions, the non-secure MSPLIM is RAZ/WI
+ res = 0U;
+ #else
+ __asm volatile("MRS %0,MSPLIM" : "=r" (res));
+ #endif
+ return res;
+ }
+
+ __IAR_FT void __set_MSPLIM(uint32_t value)
+ {
+ #if (!(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) && \
+ (!defined (__ARM_FEATURE_CMSE ) || (__ARM_FEATURE_CMSE < 3)))
+ // without main extensions, the non-secure MSPLIM is RAZ/WI
+ (void)value;
+ #else
+ __asm volatile("MSR MSPLIM,%0" :: "r" (value));
+ #endif
+ }
+
+ __IAR_FT uint32_t __get_PSPLIM(void)
+ {
+ uint32_t res;
+ #if (!(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) && \
+ (!defined (__ARM_FEATURE_CMSE ) || (__ARM_FEATURE_CMSE < 3)))
+ // without main extensions, the non-secure PSPLIM is RAZ/WI
+ res = 0U;
+ #else
+ __asm volatile("MRS %0,PSPLIM" : "=r" (res));
+ #endif
+ return res;
+ }
+
+ __IAR_FT void __set_PSPLIM(uint32_t value)
+ {
+ #if (!(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) && \
+ (!defined (__ARM_FEATURE_CMSE ) || (__ARM_FEATURE_CMSE < 3)))
+ // without main extensions, the non-secure PSPLIM is RAZ/WI
+ (void)value;
+ #else
+ __asm volatile("MSR PSPLIM,%0" :: "r" (value));
+ #endif
+ }
+
+ __IAR_FT uint32_t __TZ_get_CONTROL_NS(void)
+ {
+ uint32_t res;
+ __asm volatile("MRS %0,CONTROL_NS" : "=r" (res));
+ return res;
+ }
+
+ __IAR_FT void __TZ_set_CONTROL_NS(uint32_t value)
+ {
+ __asm volatile("MSR CONTROL_NS,%0" :: "r" (value));
+ }
+
+ __IAR_FT uint32_t __TZ_get_PSP_NS(void)
+ {
+ uint32_t res;
+ __asm volatile("MRS %0,PSP_NS" : "=r" (res));
+ return res;
+ }
+
+ __IAR_FT void __TZ_set_PSP_NS(uint32_t value)
+ {
+ __asm volatile("MSR PSP_NS,%0" :: "r" (value));
+ }
+
+ __IAR_FT uint32_t __TZ_get_MSP_NS(void)
+ {
+ uint32_t res;
+ __asm volatile("MRS %0,MSP_NS" : "=r" (res));
+ return res;
+ }
+
+ __IAR_FT void __TZ_set_MSP_NS(uint32_t value)
+ {
+ __asm volatile("MSR MSP_NS,%0" :: "r" (value));
+ }
+
+ __IAR_FT uint32_t __TZ_get_SP_NS(void)
+ {
+ uint32_t res;
+ __asm volatile("MRS %0,SP_NS" : "=r" (res));
+ return res;
+ }
+ __IAR_FT void __TZ_set_SP_NS(uint32_t value)
+ {
+ __asm volatile("MSR SP_NS,%0" :: "r" (value));
+ }
+
+ __IAR_FT uint32_t __TZ_get_PRIMASK_NS(void)
+ {
+ uint32_t res;
+ __asm volatile("MRS %0,PRIMASK_NS" : "=r" (res));
+ return res;
+ }
+
+ __IAR_FT void __TZ_set_PRIMASK_NS(uint32_t value)
+ {
+ __asm volatile("MSR PRIMASK_NS,%0" :: "r" (value));
+ }
+
+ __IAR_FT uint32_t __TZ_get_BASEPRI_NS(void)
+ {
+ uint32_t res;
+ __asm volatile("MRS %0,BASEPRI_NS" : "=r" (res));
+ return res;
+ }
+
+ __IAR_FT void __TZ_set_BASEPRI_NS(uint32_t value)
+ {
+ __asm volatile("MSR BASEPRI_NS,%0" :: "r" (value));
+ }
+
+ __IAR_FT uint32_t __TZ_get_FAULTMASK_NS(void)
+ {
+ uint32_t res;
+ __asm volatile("MRS %0,FAULTMASK_NS" : "=r" (res));
+ return res;
+ }
+
+ __IAR_FT void __TZ_set_FAULTMASK_NS(uint32_t value)
+ {
+ __asm volatile("MSR FAULTMASK_NS,%0" :: "r" (value));
+ }
+
+ __IAR_FT uint32_t __TZ_get_PSPLIM_NS(void)
+ {
+ uint32_t res;
+ #if (!(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) && \
+ (!defined (__ARM_FEATURE_CMSE ) || (__ARM_FEATURE_CMSE < 3)))
+ // without main extensions, the non-secure PSPLIM is RAZ/WI
+ res = 0U;
+ #else
+ __asm volatile("MRS %0,PSPLIM_NS" : "=r" (res));
+ #endif
+ return res;
+ }
+
+ __IAR_FT void __TZ_set_PSPLIM_NS(uint32_t value)
+ {
+ #if (!(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) && \
+ (!defined (__ARM_FEATURE_CMSE ) || (__ARM_FEATURE_CMSE < 3)))
+ // without main extensions, the non-secure PSPLIM is RAZ/WI
+ (void)value;
+ #else
+ __asm volatile("MSR PSPLIM_NS,%0" :: "r" (value));
+ #endif
+ }
+
+ __IAR_FT uint32_t __TZ_get_MSPLIM_NS(void)
+ {
+ uint32_t res;
+ __asm volatile("MRS %0,MSPLIM_NS" : "=r" (res));
+ return res;
+ }
+
+ __IAR_FT void __TZ_set_MSPLIM_NS(uint32_t value)
+ {
+ __asm volatile("MSR MSPLIM_NS,%0" :: "r" (value));
+ }
+
+ #endif /* __ARM_ARCH_8M_MAIN__ or __ARM_ARCH_8M_BASE__ */
+
+#endif /* __ICCARM_INTRINSICS_VERSION__ == 2 */
+
+#define __BKPT(value) __asm volatile ("BKPT %0" : : "i"(value))
+
+#if __IAR_M0_FAMILY
+ __STATIC_INLINE int32_t __SSAT(int32_t val, uint32_t sat)
+ {
+ if ((sat >= 1U) && (sat <= 32U))
+ {
+ const int32_t max = (int32_t)((1U << (sat - 1U)) - 1U);
+ const int32_t min = -1 - max ;
+ if (val > max)
+ {
+ return max;
+ }
+ else if (val < min)
+ {
+ return min;
+ }
+ }
+ return val;
+ }
+
+ __STATIC_INLINE uint32_t __USAT(int32_t val, uint32_t sat)
+ {
+ if (sat <= 31U)
+ {
+ const uint32_t max = ((1U << sat) - 1U);
+ if (val > (int32_t)max)
+ {
+ return max;
+ }
+ else if (val < 0)
+ {
+ return 0U;
+ }
+ }
+ return (uint32_t)val;
+ }
+#endif
+
+#if (__CORTEX_M >= 0x03) /* __CORTEX_M is defined in core_cm0.h, core_cm3.h and core_cm4.h. */
+
+ __IAR_FT uint8_t __LDRBT(volatile uint8_t *addr)
+ {
+ uint32_t res;
+ __ASM volatile ("LDRBT %0, [%1]" : "=r" (res) : "r" (addr) : "memory");
+ return ((uint8_t)res);
+ }
+
+ __IAR_FT uint16_t __LDRHT(volatile uint16_t *addr)
+ {
+ uint32_t res;
+ __ASM volatile ("LDRHT %0, [%1]" : "=r" (res) : "r" (addr) : "memory");
+ return ((uint16_t)res);
+ }
+
+ __IAR_FT uint32_t __LDRT(volatile uint32_t *addr)
+ {
+ uint32_t res;
+ __ASM volatile ("LDRT %0, [%1]" : "=r" (res) : "r" (addr) : "memory");
+ return res;
+ }
+
+ __IAR_FT void __STRBT(uint8_t value, volatile uint8_t *addr)
+ {
+ __ASM volatile ("STRBT %1, [%0]" : : "r" (addr), "r" ((uint32_t)value) : "memory");
+ }
+
+ __IAR_FT void __STRHT(uint16_t value, volatile uint16_t *addr)
+ {
+ __ASM volatile ("STRHT %1, [%0]" : : "r" (addr), "r" ((uint32_t)value) : "memory");
+ }
+
+ __IAR_FT void __STRT(uint32_t value, volatile uint32_t *addr)
+ {
+ __ASM volatile ("STRT %1, [%0]" : : "r" (addr), "r" (value) : "memory");
+ }
+
+#endif /* (__CORTEX_M >= 0x03) */
+
+#if ((defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) || \
+ (defined (__ARM_ARCH_8M_BASE__ ) && (__ARM_ARCH_8M_BASE__ == 1)) )
+
+
+ __IAR_FT uint8_t __LDAB(volatile uint8_t *ptr)
+ {
+ uint32_t res;
+ __ASM volatile ("LDAB %0, [%1]" : "=r" (res) : "r" (ptr) : "memory");
+ return ((uint8_t)res);
+ }
+
+ __IAR_FT uint16_t __LDAH(volatile uint16_t *ptr)
+ {
+ uint32_t res;
+ __ASM volatile ("LDAH %0, [%1]" : "=r" (res) : "r" (ptr) : "memory");
+ return ((uint16_t)res);
+ }
+
+ __IAR_FT uint32_t __LDA(volatile uint32_t *ptr)
+ {
+ uint32_t res;
+ __ASM volatile ("LDA %0, [%1]" : "=r" (res) : "r" (ptr) : "memory");
+ return res;
+ }
+
+ __IAR_FT void __STLB(uint8_t value, volatile uint8_t *ptr)
+ {
+ __ASM volatile ("STLB %1, [%0]" :: "r" (ptr), "r" (value) : "memory");
+ }
+
+ __IAR_FT void __STLH(uint16_t value, volatile uint16_t *ptr)
+ {
+ __ASM volatile ("STLH %1, [%0]" :: "r" (ptr), "r" (value) : "memory");
+ }
+
+ __IAR_FT void __STL(uint32_t value, volatile uint32_t *ptr)
+ {
+ __ASM volatile ("STL %1, [%0]" :: "r" (ptr), "r" (value) : "memory");
+ }
+
+ __IAR_FT uint8_t __LDAEXB(volatile uint8_t *ptr)
+ {
+ uint32_t res;
+ __ASM volatile ("LDAEXB %0, [%1]" : "=r" (res) : "r" (ptr) : "memory");
+ return ((uint8_t)res);
+ }
+
+ __IAR_FT uint16_t __LDAEXH(volatile uint16_t *ptr)
+ {
+ uint32_t res;
+ __ASM volatile ("LDAEXH %0, [%1]" : "=r" (res) : "r" (ptr) : "memory");
+ return ((uint16_t)res);
+ }
+
+ __IAR_FT uint32_t __LDAEX(volatile uint32_t *ptr)
+ {
+ uint32_t res;
+ __ASM volatile ("LDAEX %0, [%1]" : "=r" (res) : "r" (ptr) : "memory");
+ return res;
+ }
+
+ __IAR_FT uint32_t __STLEXB(uint8_t value, volatile uint8_t *ptr)
+ {
+ uint32_t res;
+ __ASM volatile ("STLEXB %0, %2, [%1]" : "=r" (res) : "r" (ptr), "r" (value) : "memory");
+ return res;
+ }
+
+ __IAR_FT uint32_t __STLEXH(uint16_t value, volatile uint16_t *ptr)
+ {
+ uint32_t res;
+ __ASM volatile ("STLEXH %0, %2, [%1]" : "=r" (res) : "r" (ptr), "r" (value) : "memory");
+ return res;
+ }
+
+ __IAR_FT uint32_t __STLEX(uint32_t value, volatile uint32_t *ptr)
+ {
+ uint32_t res;
+ __ASM volatile ("STLEX %0, %2, [%1]" : "=r" (res) : "r" (ptr), "r" (value) : "memory");
+ return res;
+ }
+
+#endif /* __ARM_ARCH_8M_MAIN__ or __ARM_ARCH_8M_BASE__ */
+
+#undef __IAR_FT
+#undef __IAR_M0_FAMILY
+#undef __ICCARM_V8
+
+#pragma diag_default=Pe940
+#pragma diag_default=Pe177
+
+#define __SXTB16_RORn(ARG1, ARG2) __SXTB16(__ROR(ARG1, ARG2))
+
+#endif /* __CMSIS_ICCARM_H__ */
diff --git a/src/rp2_common/cmsis/stub/CMSIS/Core/Include/cmsis_version.h b/src/rp2_common/cmsis/stub/CMSIS/Core/Include/cmsis_version.h
new file mode 100644
index 0000000..2f048e4
--- /dev/null
+++ b/src/rp2_common/cmsis/stub/CMSIS/Core/Include/cmsis_version.h
@@ -0,0 +1,39 @@
+/**************************************************************************//**
+ * @file cmsis_version.h
+ * @brief CMSIS Core(M) Version definitions
+ * @version V5.0.4
+ * @date 23. July 2019
+ ******************************************************************************/
+/*
+ * Copyright (c) 2009-2019 ARM Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#if defined ( __ICCARM__ )
+ #pragma system_include /* treat file as system include file for MISRA check */
+#elif defined (__clang__)
+ #pragma clang system_header /* treat file as system include file */
+#endif
+
+#ifndef __CMSIS_VERSION_H
+#define __CMSIS_VERSION_H
+
+/* CMSIS Version definitions */
+#define __CM_CMSIS_VERSION_MAIN ( 5U) /*!< [31:16] CMSIS Core(M) main version */
+#define __CM_CMSIS_VERSION_SUB ( 4U) /*!< [15:0] CMSIS Core(M) sub version */
+#define __CM_CMSIS_VERSION ((__CM_CMSIS_VERSION_MAIN << 16U) | \
+ __CM_CMSIS_VERSION_SUB ) /*!< CMSIS Core(M) version number */
+#endif
diff --git a/src/rp2_common/cmsis/stub/CMSIS/Core/Include/core_cm0plus.h b/src/rp2_common/cmsis/stub/CMSIS/Core/Include/core_cm0plus.h
new file mode 100644
index 0000000..4e7179a
--- /dev/null
+++ b/src/rp2_common/cmsis/stub/CMSIS/Core/Include/core_cm0plus.h
@@ -0,0 +1,1087 @@
+/**************************************************************************//**
+ * @file core_cm0plus.h
+ * @brief CMSIS Cortex-M0+ Core Peripheral Access Layer Header File
+ * @version V5.0.9
+ * @date 21. August 2019
+ ******************************************************************************/
+/*
+ * Copyright (c) 2009-2019 Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#if defined ( __ICCARM__ )
+ #pragma system_include /* treat file as system include file for MISRA check */
+#elif defined (__clang__)
+ #pragma clang system_header /* treat file as system include file */
+#endif
+
+#ifndef __CORE_CM0PLUS_H_GENERIC
+#define __CORE_CM0PLUS_H_GENERIC
+
+#include <stdint.h>
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+/**
+ \page CMSIS_MISRA_Exceptions MISRA-C:2004 Compliance Exceptions
+ CMSIS violates the following MISRA-C:2004 rules:
+
+ \li Required Rule 8.5, object/function definition in header file.<br>
+ Function definitions in header files are used to allow 'inlining'.
+
+ \li Required Rule 18.4, declaration of union type or object of union type: '{...}'.<br>
+ Unions are used for effective representation of core registers.
+
+ \li Advisory Rule 19.7, Function-like macro defined.<br>
+ Function-like macros are used to allow more efficient code.
+ */
+
+
+/*******************************************************************************
+ * CMSIS definitions
+ ******************************************************************************/
+/**
+ \ingroup Cortex-M0+
+ @{
+ */
+
+#include "cmsis_version.h"
+
+/* CMSIS CM0+ definitions */
+#define __CM0PLUS_CMSIS_VERSION_MAIN (__CM_CMSIS_VERSION_MAIN) /*!< \deprecated [31:16] CMSIS HAL main version */
+#define __CM0PLUS_CMSIS_VERSION_SUB (__CM_CMSIS_VERSION_SUB) /*!< \deprecated [15:0] CMSIS HAL sub version */
+#define __CM0PLUS_CMSIS_VERSION ((__CM0PLUS_CMSIS_VERSION_MAIN << 16U) | \
+ __CM0PLUS_CMSIS_VERSION_SUB ) /*!< \deprecated CMSIS HAL version number */
+
+#define __CORTEX_M (0U) /*!< Cortex-M Core */
+
+/** __FPU_USED indicates whether an FPU is used or not.
+ This core does not support an FPU at all
+*/
+#define __FPU_USED 0U
+
+#if defined ( __CC_ARM )
+ #if defined __TARGET_FPU_VFP
+ #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+ #endif
+
+#elif defined (__ARMCC_VERSION) && (__ARMCC_VERSION >= 6010050)
+ #if defined __ARM_FP
+ #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+ #endif
+
+#elif defined ( __GNUC__ )
+ #if defined (__VFP_FP__) && !defined(__SOFTFP__)
+ #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+ #endif
+
+#elif defined ( __ICCARM__ )
+ #if defined __ARMVFP__
+ #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+ #endif
+
+#elif defined ( __TI_ARM__ )
+ #if defined __TI_VFP_SUPPORT__
+ #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+ #endif
+
+#elif defined ( __TASKING__ )
+ #if defined __FPU_VFP__
+ #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+ #endif
+
+#elif defined ( __CSMC__ )
+ #if ( __CSMC__ & 0x400U)
+ #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+ #endif
+
+#endif
+
+#include "cmsis_compiler.h" /* CMSIS compiler specific defines */
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __CORE_CM0PLUS_H_GENERIC */
+
+#ifndef __CMSIS_GENERIC
+
+#ifndef __CORE_CM0PLUS_H_DEPENDANT
+#define __CORE_CM0PLUS_H_DEPENDANT
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+/* check device defines and use defaults */
+#if defined __CHECK_DEVICE_DEFINES
+ #ifndef __CM0PLUS_REV
+ #define __CM0PLUS_REV 0x0000U
+ #warning "__CM0PLUS_REV not defined in device header file; using default!"
+ #endif
+
+ #ifndef __MPU_PRESENT
+ #define __MPU_PRESENT 0U
+ #warning "__MPU_PRESENT not defined in device header file; using default!"
+ #endif
+
+ #ifndef __VTOR_PRESENT
+ #define __VTOR_PRESENT 0U
+ #warning "__VTOR_PRESENT not defined in device header file; using default!"
+ #endif
+
+ #ifndef __NVIC_PRIO_BITS
+ #define __NVIC_PRIO_BITS 2U
+ #warning "__NVIC_PRIO_BITS not defined in device header file; using default!"
+ #endif
+
+ #ifndef __Vendor_SysTickConfig
+ #define __Vendor_SysTickConfig 0U
+ #warning "__Vendor_SysTickConfig not defined in device header file; using default!"
+ #endif
+#endif
+
+/* IO definitions (access restrictions to peripheral registers) */
+/**
+ \defgroup CMSIS_glob_defs CMSIS Global Defines
+
+ <strong>IO Type Qualifiers</strong> are used
+ \li to specify the access to peripheral variables.
+ \li for automatic generation of peripheral register debug information.
+*/
+#ifdef __cplusplus
+ #define __I volatile /*!< Defines 'read only' permissions */
+#else
+ #define __I volatile const /*!< Defines 'read only' permissions */
+#endif
+#define __O volatile /*!< Defines 'write only' permissions */
+#define __IO volatile /*!< Defines 'read / write' permissions */
+
+/* following defines should be used for structure members */
+#define __IM volatile const /*! Defines 'read only' structure member permissions */
+#define __OM volatile /*! Defines 'write only' structure member permissions */
+#define __IOM volatile /*! Defines 'read / write' structure member permissions */
+
+/*@} end of group Cortex-M0+ */
+
+
+
+/*******************************************************************************
+ * Register Abstraction
+ Core Register contain:
+ - Core Register
+ - Core NVIC Register
+ - Core SCB Register
+ - Core SysTick Register
+ - Core MPU Register
+ ******************************************************************************/
+/**
+ \defgroup CMSIS_core_register Defines and Type Definitions
+ \brief Type definitions and defines for Cortex-M processor based devices.
+*/
+
+/**
+ \ingroup CMSIS_core_register
+ \defgroup CMSIS_CORE Status and Control Registers
+ \brief Core Register type definitions.
+ @{
+ */
+
+/**
+ \brief Union type to access the Application Program Status Register (APSR).
+ */
+typedef union
+{
+ struct
+ {
+ uint32_t _reserved0:28; /*!< bit: 0..27 Reserved */
+ uint32_t V:1; /*!< bit: 28 Overflow condition code flag */
+ uint32_t C:1; /*!< bit: 29 Carry condition code flag */
+ uint32_t Z:1; /*!< bit: 30 Zero condition code flag */
+ uint32_t N:1; /*!< bit: 31 Negative condition code flag */
+ } b; /*!< Structure used for bit access */
+ uint32_t w; /*!< Type used for word access */
+} APSR_Type;
+
+/* APSR Register Definitions */
+#define APSR_N_Pos 31U /*!< APSR: N Position */
+#define APSR_N_Msk (1UL << APSR_N_Pos) /*!< APSR: N Mask */
+
+#define APSR_Z_Pos 30U /*!< APSR: Z Position */
+#define APSR_Z_Msk (1UL << APSR_Z_Pos) /*!< APSR: Z Mask */
+
+#define APSR_C_Pos 29U /*!< APSR: C Position */
+#define APSR_C_Msk (1UL << APSR_C_Pos) /*!< APSR: C Mask */
+
+#define APSR_V_Pos 28U /*!< APSR: V Position */
+#define APSR_V_Msk (1UL << APSR_V_Pos) /*!< APSR: V Mask */
+
+
+/**
+ \brief Union type to access the Interrupt Program Status Register (IPSR).
+ */
+typedef union
+{
+ struct
+ {
+ uint32_t ISR:9; /*!< bit: 0.. 8 Exception number */
+ uint32_t _reserved0:23; /*!< bit: 9..31 Reserved */
+ } b; /*!< Structure used for bit access */
+ uint32_t w; /*!< Type used for word access */
+} IPSR_Type;
+
+/* IPSR Register Definitions */
+#define IPSR_ISR_Pos 0U /*!< IPSR: ISR Position */
+#define IPSR_ISR_Msk (0x1FFUL /*<< IPSR_ISR_Pos*/) /*!< IPSR: ISR Mask */
+
+
+/**
+ \brief Union type to access the Special-Purpose Program Status Registers (xPSR).
+ */
+typedef union
+{
+ struct
+ {
+ uint32_t ISR:9; /*!< bit: 0.. 8 Exception number */
+ uint32_t _reserved0:15; /*!< bit: 9..23 Reserved */
+ uint32_t T:1; /*!< bit: 24 Thumb bit (read 0) */
+ uint32_t _reserved1:3; /*!< bit: 25..27 Reserved */
+ uint32_t V:1; /*!< bit: 28 Overflow condition code flag */
+ uint32_t C:1; /*!< bit: 29 Carry condition code flag */
+ uint32_t Z:1; /*!< bit: 30 Zero condition code flag */
+ uint32_t N:1; /*!< bit: 31 Negative condition code flag */
+ } b; /*!< Structure used for bit access */
+ uint32_t w; /*!< Type used for word access */
+} xPSR_Type;
+
+/* xPSR Register Definitions */
+#define xPSR_N_Pos 31U /*!< xPSR: N Position */
+#define xPSR_N_Msk (1UL << xPSR_N_Pos) /*!< xPSR: N Mask */
+
+#define xPSR_Z_Pos 30U /*!< xPSR: Z Position */
+#define xPSR_Z_Msk (1UL << xPSR_Z_Pos) /*!< xPSR: Z Mask */
+
+#define xPSR_C_Pos 29U /*!< xPSR: C Position */
+#define xPSR_C_Msk (1UL << xPSR_C_Pos) /*!< xPSR: C Mask */
+
+#define xPSR_V_Pos 28U /*!< xPSR: V Position */
+#define xPSR_V_Msk (1UL << xPSR_V_Pos) /*!< xPSR: V Mask */
+
+#define xPSR_T_Pos 24U /*!< xPSR: T Position */
+#define xPSR_T_Msk (1UL << xPSR_T_Pos) /*!< xPSR: T Mask */
+
+#define xPSR_ISR_Pos 0U /*!< xPSR: ISR Position */
+#define xPSR_ISR_Msk (0x1FFUL /*<< xPSR_ISR_Pos*/) /*!< xPSR: ISR Mask */
+
+
+/**
+ \brief Union type to access the Control Registers (CONTROL).
+ */
+typedef union
+{
+ struct
+ {
+ uint32_t nPRIV:1; /*!< bit: 0 Execution privilege in Thread mode */
+ uint32_t SPSEL:1; /*!< bit: 1 Stack to be used */
+ uint32_t _reserved1:30; /*!< bit: 2..31 Reserved */
+ } b; /*!< Structure used for bit access */
+ uint32_t w; /*!< Type used for word access */
+} CONTROL_Type;
+
+/* CONTROL Register Definitions */
+#define CONTROL_SPSEL_Pos 1U /*!< CONTROL: SPSEL Position */
+#define CONTROL_SPSEL_Msk (1UL << CONTROL_SPSEL_Pos) /*!< CONTROL: SPSEL Mask */
+
+#define CONTROL_nPRIV_Pos 0U /*!< CONTROL: nPRIV Position */
+#define CONTROL_nPRIV_Msk (1UL /*<< CONTROL_nPRIV_Pos*/) /*!< CONTROL: nPRIV Mask */
+
+/*@} end of group CMSIS_CORE */
+
+
+/**
+ \ingroup CMSIS_core_register
+ \defgroup CMSIS_NVIC Nested Vectored Interrupt Controller (NVIC)
+ \brief Type definitions for the NVIC Registers
+ @{
+ */
+
+/**
+ \brief Structure type to access the Nested Vectored Interrupt Controller (NVIC).
+ */
+typedef struct
+{
+ __IOM uint32_t ISER[1U]; /*!< Offset: 0x000 (R/W) Interrupt Set Enable Register */
+ uint32_t RESERVED0[31U];
+ __IOM uint32_t ICER[1U]; /*!< Offset: 0x080 (R/W) Interrupt Clear Enable Register */
+ uint32_t RESERVED1[31U];
+ __IOM uint32_t ISPR[1U]; /*!< Offset: 0x100 (R/W) Interrupt Set Pending Register */
+ uint32_t RESERVED2[31U];
+ __IOM uint32_t ICPR[1U]; /*!< Offset: 0x180 (R/W) Interrupt Clear Pending Register */
+ uint32_t RESERVED3[31U];
+ uint32_t RESERVED4[64U];
+ __IOM uint32_t IP[8U]; /*!< Offset: 0x300 (R/W) Interrupt Priority Register */
+} NVIC_Type;
+
+/*@} end of group CMSIS_NVIC */
+
+
+/**
+ \ingroup CMSIS_core_register
+ \defgroup CMSIS_SCB System Control Block (SCB)
+ \brief Type definitions for the System Control Block Registers
+ @{
+ */
+
+/**
+ \brief Structure type to access the System Control Block (SCB).
+ */
+typedef struct
+{
+ __IM uint32_t CPUID; /*!< Offset: 0x000 (R/ ) CPUID Base Register */
+ __IOM uint32_t ICSR; /*!< Offset: 0x004 (R/W) Interrupt Control and State Register */
+#if defined (__VTOR_PRESENT) && (__VTOR_PRESENT == 1U)
+ __IOM uint32_t VTOR; /*!< Offset: 0x008 (R/W) Vector Table Offset Register */
+#else
+ uint32_t RESERVED0;
+#endif
+ __IOM uint32_t AIRCR; /*!< Offset: 0x00C (R/W) Application Interrupt and Reset Control Register */
+ __IOM uint32_t SCR; /*!< Offset: 0x010 (R/W) System Control Register */
+ __IOM uint32_t CCR; /*!< Offset: 0x014 (R/W) Configuration Control Register */
+ uint32_t RESERVED1;
+ __IOM uint32_t SHP[2U]; /*!< Offset: 0x01C (R/W) System Handlers Priority Registers. [0] is RESERVED */
+ __IOM uint32_t SHCSR; /*!< Offset: 0x024 (R/W) System Handler Control and State Register */
+} SCB_Type;
+
+/* SCB CPUID Register Definitions */
+#define SCB_CPUID_IMPLEMENTER_Pos 24U /*!< SCB CPUID: IMPLEMENTER Position */
+#define SCB_CPUID_IMPLEMENTER_Msk (0xFFUL << SCB_CPUID_IMPLEMENTER_Pos) /*!< SCB CPUID: IMPLEMENTER Mask */
+
+#define SCB_CPUID_VARIANT_Pos 20U /*!< SCB CPUID: VARIANT Position */
+#define SCB_CPUID_VARIANT_Msk (0xFUL << SCB_CPUID_VARIANT_Pos) /*!< SCB CPUID: VARIANT Mask */
+
+#define SCB_CPUID_ARCHITECTURE_Pos 16U /*!< SCB CPUID: ARCHITECTURE Position */
+#define SCB_CPUID_ARCHITECTURE_Msk (0xFUL << SCB_CPUID_ARCHITECTURE_Pos) /*!< SCB CPUID: ARCHITECTURE Mask */
+
+#define SCB_CPUID_PARTNO_Pos 4U /*!< SCB CPUID: PARTNO Position */
+#define SCB_CPUID_PARTNO_Msk (0xFFFUL << SCB_CPUID_PARTNO_Pos) /*!< SCB CPUID: PARTNO Mask */
+
+#define SCB_CPUID_REVISION_Pos 0U /*!< SCB CPUID: REVISION Position */
+#define SCB_CPUID_REVISION_Msk (0xFUL /*<< SCB_CPUID_REVISION_Pos*/) /*!< SCB CPUID: REVISION Mask */
+
+/* SCB Interrupt Control State Register Definitions */
+#define SCB_ICSR_NMIPENDSET_Pos 31U /*!< SCB ICSR: NMIPENDSET Position */
+#define SCB_ICSR_NMIPENDSET_Msk (1UL << SCB_ICSR_NMIPENDSET_Pos) /*!< SCB ICSR: NMIPENDSET Mask */
+
+#define SCB_ICSR_PENDSVSET_Pos 28U /*!< SCB ICSR: PENDSVSET Position */
+#define SCB_ICSR_PENDSVSET_Msk (1UL << SCB_ICSR_PENDSVSET_Pos) /*!< SCB ICSR: PENDSVSET Mask */
+
+#define SCB_ICSR_PENDSVCLR_Pos 27U /*!< SCB ICSR: PENDSVCLR Position */
+#define SCB_ICSR_PENDSVCLR_Msk (1UL << SCB_ICSR_PENDSVCLR_Pos) /*!< SCB ICSR: PENDSVCLR Mask */
+
+#define SCB_ICSR_PENDSTSET_Pos 26U /*!< SCB ICSR: PENDSTSET Position */
+#define SCB_ICSR_PENDSTSET_Msk (1UL << SCB_ICSR_PENDSTSET_Pos) /*!< SCB ICSR: PENDSTSET Mask */
+
+#define SCB_ICSR_PENDSTCLR_Pos 25U /*!< SCB ICSR: PENDSTCLR Position */
+#define SCB_ICSR_PENDSTCLR_Msk (1UL << SCB_ICSR_PENDSTCLR_Pos) /*!< SCB ICSR: PENDSTCLR Mask */
+
+#define SCB_ICSR_ISRPREEMPT_Pos 23U /*!< SCB ICSR: ISRPREEMPT Position */
+#define SCB_ICSR_ISRPREEMPT_Msk (1UL << SCB_ICSR_ISRPREEMPT_Pos) /*!< SCB ICSR: ISRPREEMPT Mask */
+
+#define SCB_ICSR_ISRPENDING_Pos 22U /*!< SCB ICSR: ISRPENDING Position */
+#define SCB_ICSR_ISRPENDING_Msk (1UL << SCB_ICSR_ISRPENDING_Pos) /*!< SCB ICSR: ISRPENDING Mask */
+
+#define SCB_ICSR_VECTPENDING_Pos 12U /*!< SCB ICSR: VECTPENDING Position */
+#define SCB_ICSR_VECTPENDING_Msk (0x1FFUL << SCB_ICSR_VECTPENDING_Pos) /*!< SCB ICSR: VECTPENDING Mask */
+
+#define SCB_ICSR_VECTACTIVE_Pos 0U /*!< SCB ICSR: VECTACTIVE Position */
+#define SCB_ICSR_VECTACTIVE_Msk (0x1FFUL /*<< SCB_ICSR_VECTACTIVE_Pos*/) /*!< SCB ICSR: VECTACTIVE Mask */
+
+#if defined (__VTOR_PRESENT) && (__VTOR_PRESENT == 1U)
+/* SCB Interrupt Control State Register Definitions */
+#define SCB_VTOR_TBLOFF_Pos 8U /*!< SCB VTOR: TBLOFF Position */
+#define SCB_VTOR_TBLOFF_Msk (0xFFFFFFUL << SCB_VTOR_TBLOFF_Pos) /*!< SCB VTOR: TBLOFF Mask */
+#endif
+
+/* SCB Application Interrupt and Reset Control Register Definitions */
+#define SCB_AIRCR_VECTKEY_Pos 16U /*!< SCB AIRCR: VECTKEY Position */
+#define SCB_AIRCR_VECTKEY_Msk (0xFFFFUL << SCB_AIRCR_VECTKEY_Pos) /*!< SCB AIRCR: VECTKEY Mask */
+
+#define SCB_AIRCR_VECTKEYSTAT_Pos 16U /*!< SCB AIRCR: VECTKEYSTAT Position */
+#define SCB_AIRCR_VECTKEYSTAT_Msk (0xFFFFUL << SCB_AIRCR_VECTKEYSTAT_Pos) /*!< SCB AIRCR: VECTKEYSTAT Mask */
+
+#define SCB_AIRCR_ENDIANESS_Pos 15U /*!< SCB AIRCR: ENDIANESS Position */
+#define SCB_AIRCR_ENDIANESS_Msk (1UL << SCB_AIRCR_ENDIANESS_Pos) /*!< SCB AIRCR: ENDIANESS Mask */
+
+#define SCB_AIRCR_SYSRESETREQ_Pos 2U /*!< SCB AIRCR: SYSRESETREQ Position */
+#define SCB_AIRCR_SYSRESETREQ_Msk (1UL << SCB_AIRCR_SYSRESETREQ_Pos) /*!< SCB AIRCR: SYSRESETREQ Mask */
+
+#define SCB_AIRCR_VECTCLRACTIVE_Pos 1U /*!< SCB AIRCR: VECTCLRACTIVE Position */
+#define SCB_AIRCR_VECTCLRACTIVE_Msk (1UL << SCB_AIRCR_VECTCLRACTIVE_Pos) /*!< SCB AIRCR: VECTCLRACTIVE Mask */
+
+/* SCB System Control Register Definitions */
+#define SCB_SCR_SEVONPEND_Pos 4U /*!< SCB SCR: SEVONPEND Position */
+#define SCB_SCR_SEVONPEND_Msk (1UL << SCB_SCR_SEVONPEND_Pos) /*!< SCB SCR: SEVONPEND Mask */
+
+#define SCB_SCR_SLEEPDEEP_Pos 2U /*!< SCB SCR: SLEEPDEEP Position */
+#define SCB_SCR_SLEEPDEEP_Msk (1UL << SCB_SCR_SLEEPDEEP_Pos) /*!< SCB SCR: SLEEPDEEP Mask */
+
+#define SCB_SCR_SLEEPONEXIT_Pos 1U /*!< SCB SCR: SLEEPONEXIT Position */
+#define SCB_SCR_SLEEPONEXIT_Msk (1UL << SCB_SCR_SLEEPONEXIT_Pos) /*!< SCB SCR: SLEEPONEXIT Mask */
+
+/* SCB Configuration Control Register Definitions */
+#define SCB_CCR_STKALIGN_Pos 9U /*!< SCB CCR: STKALIGN Position */
+#define SCB_CCR_STKALIGN_Msk (1UL << SCB_CCR_STKALIGN_Pos) /*!< SCB CCR: STKALIGN Mask */
+
+#define SCB_CCR_UNALIGN_TRP_Pos 3U /*!< SCB CCR: UNALIGN_TRP Position */
+#define SCB_CCR_UNALIGN_TRP_Msk (1UL << SCB_CCR_UNALIGN_TRP_Pos) /*!< SCB CCR: UNALIGN_TRP Mask */
+
+/* SCB System Handler Control and State Register Definitions */
+#define SCB_SHCSR_SVCALLPENDED_Pos 15U /*!< SCB SHCSR: SVCALLPENDED Position */
+#define SCB_SHCSR_SVCALLPENDED_Msk (1UL << SCB_SHCSR_SVCALLPENDED_Pos) /*!< SCB SHCSR: SVCALLPENDED Mask */
+
+/*@} end of group CMSIS_SCB */
+
+
+/**
+ \ingroup CMSIS_core_register
+ \defgroup CMSIS_SysTick System Tick Timer (SysTick)
+ \brief Type definitions for the System Timer Registers.
+ @{
+ */
+
+/**
+ \brief Structure type to access the System Timer (SysTick).
+ */
+typedef struct
+{
+ __IOM uint32_t CTRL; /*!< Offset: 0x000 (R/W) SysTick Control and Status Register */
+ __IOM uint32_t LOAD; /*!< Offset: 0x004 (R/W) SysTick Reload Value Register */
+ __IOM uint32_t VAL; /*!< Offset: 0x008 (R/W) SysTick Current Value Register */
+ __IM uint32_t CALIB; /*!< Offset: 0x00C (R/ ) SysTick Calibration Register */
+} SysTick_Type;
+
+/* SysTick Control / Status Register Definitions */
+#define SysTick_CTRL_COUNTFLAG_Pos 16U /*!< SysTick CTRL: COUNTFLAG Position */
+#define SysTick_CTRL_COUNTFLAG_Msk (1UL << SysTick_CTRL_COUNTFLAG_Pos) /*!< SysTick CTRL: COUNTFLAG Mask */
+
+#define SysTick_CTRL_CLKSOURCE_Pos 2U /*!< SysTick CTRL: CLKSOURCE Position */
+#define SysTick_CTRL_CLKSOURCE_Msk (1UL << SysTick_CTRL_CLKSOURCE_Pos) /*!< SysTick CTRL: CLKSOURCE Mask */
+
+#define SysTick_CTRL_TICKINT_Pos 1U /*!< SysTick CTRL: TICKINT Position */
+#define SysTick_CTRL_TICKINT_Msk (1UL << SysTick_CTRL_TICKINT_Pos) /*!< SysTick CTRL: TICKINT Mask */
+
+#define SysTick_CTRL_ENABLE_Pos 0U /*!< SysTick CTRL: ENABLE Position */
+#define SysTick_CTRL_ENABLE_Msk (1UL /*<< SysTick_CTRL_ENABLE_Pos*/) /*!< SysTick CTRL: ENABLE Mask */
+
+/* SysTick Reload Register Definitions */
+#define SysTick_LOAD_RELOAD_Pos 0U /*!< SysTick LOAD: RELOAD Position */
+#define SysTick_LOAD_RELOAD_Msk (0xFFFFFFUL /*<< SysTick_LOAD_RELOAD_Pos*/) /*!< SysTick LOAD: RELOAD Mask */
+
+/* SysTick Current Register Definitions */
+#define SysTick_VAL_CURRENT_Pos 0U /*!< SysTick VAL: CURRENT Position */
+#define SysTick_VAL_CURRENT_Msk (0xFFFFFFUL /*<< SysTick_VAL_CURRENT_Pos*/) /*!< SysTick VAL: CURRENT Mask */
+
+/* SysTick Calibration Register Definitions */
+#define SysTick_CALIB_NOREF_Pos 31U /*!< SysTick CALIB: NOREF Position */
+#define SysTick_CALIB_NOREF_Msk (1UL << SysTick_CALIB_NOREF_Pos) /*!< SysTick CALIB: NOREF Mask */
+
+#define SysTick_CALIB_SKEW_Pos 30U /*!< SysTick CALIB: SKEW Position */
+#define SysTick_CALIB_SKEW_Msk (1UL << SysTick_CALIB_SKEW_Pos) /*!< SysTick CALIB: SKEW Mask */
+
+#define SysTick_CALIB_TENMS_Pos 0U /*!< SysTick CALIB: TENMS Position */
+#define SysTick_CALIB_TENMS_Msk (0xFFFFFFUL /*<< SysTick_CALIB_TENMS_Pos*/) /*!< SysTick CALIB: TENMS Mask */
+
+/*@} end of group CMSIS_SysTick */
+
+#if defined (__MPU_PRESENT) && (__MPU_PRESENT == 1U)
+/**
+ \ingroup CMSIS_core_register
+ \defgroup CMSIS_MPU Memory Protection Unit (MPU)
+ \brief Type definitions for the Memory Protection Unit (MPU)
+ @{
+ */
+
+/**
+ \brief Structure type to access the Memory Protection Unit (MPU).
+ */
+typedef struct
+{
+ __IM uint32_t TYPE; /*!< Offset: 0x000 (R/ ) MPU Type Register */
+ __IOM uint32_t CTRL; /*!< Offset: 0x004 (R/W) MPU Control Register */
+ __IOM uint32_t RNR; /*!< Offset: 0x008 (R/W) MPU Region RNRber Register */
+ __IOM uint32_t RBAR; /*!< Offset: 0x00C (R/W) MPU Region Base Address Register */
+ __IOM uint32_t RASR; /*!< Offset: 0x010 (R/W) MPU Region Attribute and Size Register */
+} MPU_Type;
+
+#define MPU_TYPE_RALIASES 1U
+
+/* MPU Type Register Definitions */
+#define MPU_TYPE_IREGION_Pos 16U /*!< MPU TYPE: IREGION Position */
+#define MPU_TYPE_IREGION_Msk (0xFFUL << MPU_TYPE_IREGION_Pos) /*!< MPU TYPE: IREGION Mask */
+
+#define MPU_TYPE_DREGION_Pos 8U /*!< MPU TYPE: DREGION Position */
+#define MPU_TYPE_DREGION_Msk (0xFFUL << MPU_TYPE_DREGION_Pos) /*!< MPU TYPE: DREGION Mask */
+
+#define MPU_TYPE_SEPARATE_Pos 0U /*!< MPU TYPE: SEPARATE Position */
+#define MPU_TYPE_SEPARATE_Msk (1UL /*<< MPU_TYPE_SEPARATE_Pos*/) /*!< MPU TYPE: SEPARATE Mask */
+
+/* MPU Control Register Definitions */
+#define MPU_CTRL_PRIVDEFENA_Pos 2U /*!< MPU CTRL: PRIVDEFENA Position */
+#define MPU_CTRL_PRIVDEFENA_Msk (1UL << MPU_CTRL_PRIVDEFENA_Pos) /*!< MPU CTRL: PRIVDEFENA Mask */
+
+#define MPU_CTRL_HFNMIENA_Pos 1U /*!< MPU CTRL: HFNMIENA Position */
+#define MPU_CTRL_HFNMIENA_Msk (1UL << MPU_CTRL_HFNMIENA_Pos) /*!< MPU CTRL: HFNMIENA Mask */
+
+#define MPU_CTRL_ENABLE_Pos 0U /*!< MPU CTRL: ENABLE Position */
+#define MPU_CTRL_ENABLE_Msk (1UL /*<< MPU_CTRL_ENABLE_Pos*/) /*!< MPU CTRL: ENABLE Mask */
+
+/* MPU Region Number Register Definitions */
+#define MPU_RNR_REGION_Pos 0U /*!< MPU RNR: REGION Position */
+#define MPU_RNR_REGION_Msk (0xFFUL /*<< MPU_RNR_REGION_Pos*/) /*!< MPU RNR: REGION Mask */
+
+/* MPU Region Base Address Register Definitions */
+#define MPU_RBAR_ADDR_Pos 8U /*!< MPU RBAR: ADDR Position */
+#define MPU_RBAR_ADDR_Msk (0xFFFFFFUL << MPU_RBAR_ADDR_Pos) /*!< MPU RBAR: ADDR Mask */
+
+#define MPU_RBAR_VALID_Pos 4U /*!< MPU RBAR: VALID Position */
+#define MPU_RBAR_VALID_Msk (1UL << MPU_RBAR_VALID_Pos) /*!< MPU RBAR: VALID Mask */
+
+#define MPU_RBAR_REGION_Pos 0U /*!< MPU RBAR: REGION Position */
+#define MPU_RBAR_REGION_Msk (0xFUL /*<< MPU_RBAR_REGION_Pos*/) /*!< MPU RBAR: REGION Mask */
+
+/* MPU Region Attribute and Size Register Definitions */
+#define MPU_RASR_ATTRS_Pos 16U /*!< MPU RASR: MPU Region Attribute field Position */
+#define MPU_RASR_ATTRS_Msk (0xFFFFUL << MPU_RASR_ATTRS_Pos) /*!< MPU RASR: MPU Region Attribute field Mask */
+
+#define MPU_RASR_XN_Pos 28U /*!< MPU RASR: ATTRS.XN Position */
+#define MPU_RASR_XN_Msk (1UL << MPU_RASR_XN_Pos) /*!< MPU RASR: ATTRS.XN Mask */
+
+#define MPU_RASR_AP_Pos 24U /*!< MPU RASR: ATTRS.AP Position */
+#define MPU_RASR_AP_Msk (0x7UL << MPU_RASR_AP_Pos) /*!< MPU RASR: ATTRS.AP Mask */
+
+#define MPU_RASR_TEX_Pos 19U /*!< MPU RASR: ATTRS.TEX Position */
+#define MPU_RASR_TEX_Msk (0x7UL << MPU_RASR_TEX_Pos) /*!< MPU RASR: ATTRS.TEX Mask */
+
+#define MPU_RASR_S_Pos 18U /*!< MPU RASR: ATTRS.S Position */
+#define MPU_RASR_S_Msk (1UL << MPU_RASR_S_Pos) /*!< MPU RASR: ATTRS.S Mask */
+
+#define MPU_RASR_C_Pos 17U /*!< MPU RASR: ATTRS.C Position */
+#define MPU_RASR_C_Msk (1UL << MPU_RASR_C_Pos) /*!< MPU RASR: ATTRS.C Mask */
+
+#define MPU_RASR_B_Pos 16U /*!< MPU RASR: ATTRS.B Position */
+#define MPU_RASR_B_Msk (1UL << MPU_RASR_B_Pos) /*!< MPU RASR: ATTRS.B Mask */
+
+#define MPU_RASR_SRD_Pos 8U /*!< MPU RASR: Sub-Region Disable Position */
+#define MPU_RASR_SRD_Msk (0xFFUL << MPU_RASR_SRD_Pos) /*!< MPU RASR: Sub-Region Disable Mask */
+
+#define MPU_RASR_SIZE_Pos 1U /*!< MPU RASR: Region Size Field Position */
+#define MPU_RASR_SIZE_Msk (0x1FUL << MPU_RASR_SIZE_Pos) /*!< MPU RASR: Region Size Field Mask */
+
+#define MPU_RASR_ENABLE_Pos 0U /*!< MPU RASR: Region enable bit Position */
+#define MPU_RASR_ENABLE_Msk (1UL /*<< MPU_RASR_ENABLE_Pos*/) /*!< MPU RASR: Region enable bit Disable Mask */
+
+/*@} end of group CMSIS_MPU */
+#endif
+
+
+/**
+ \ingroup CMSIS_core_register
+ \defgroup CMSIS_CoreDebug Core Debug Registers (CoreDebug)
+ \brief Cortex-M0+ Core Debug Registers (DCB registers, SHCSR, and DFSR) are only accessible over DAP and not via processor.
+ Therefore they are not covered by the Cortex-M0+ header file.
+ @{
+ */
+/*@} end of group CMSIS_CoreDebug */
+
+
+/**
+ \ingroup CMSIS_core_register
+ \defgroup CMSIS_core_bitfield Core register bit field macros
+ \brief Macros for use with bit field definitions (xxx_Pos, xxx_Msk).
+ @{
+ */
+
+/**
+ \brief Mask and shift a bit field value for use in a register bit range.
+ \param[in] field Name of the register bit field.
+ \param[in] value Value of the bit field. This parameter is interpreted as an uint32_t type.
+ \return Masked and shifted value.
+*/
+#define _VAL2FLD(field, value) (((uint32_t)(value) << field ## _Pos) & field ## _Msk)
+
+/**
+ \brief Mask and shift a register value to extract a bit filed value.
+ \param[in] field Name of the register bit field.
+ \param[in] value Value of register. This parameter is interpreted as an uint32_t type.
+ \return Masked and shifted bit field value.
+*/
+#define _FLD2VAL(field, value) (((uint32_t)(value) & field ## _Msk) >> field ## _Pos)
+
+/*@} end of group CMSIS_core_bitfield */
+
+
+/**
+ \ingroup CMSIS_core_register
+ \defgroup CMSIS_core_base Core Definitions
+ \brief Definitions for base addresses, unions, and structures.
+ @{
+ */
+
+/* Memory mapping of Core Hardware */
+#define SCS_BASE (0xE000E000UL) /*!< System Control Space Base Address */
+#define SysTick_BASE (SCS_BASE + 0x0010UL) /*!< SysTick Base Address */
+#define NVIC_BASE (SCS_BASE + 0x0100UL) /*!< NVIC Base Address */
+#define SCB_BASE (SCS_BASE + 0x0D00UL) /*!< System Control Block Base Address */
+
+#define SCB ((SCB_Type *) SCB_BASE ) /*!< SCB configuration struct */
+#define SysTick ((SysTick_Type *) SysTick_BASE ) /*!< SysTick configuration struct */
+#define NVIC ((NVIC_Type *) NVIC_BASE ) /*!< NVIC configuration struct */
+
+#if defined (__MPU_PRESENT) && (__MPU_PRESENT == 1U)
+ #define MPU_BASE (SCS_BASE + 0x0D90UL) /*!< Memory Protection Unit */
+ #define MPU ((MPU_Type *) MPU_BASE ) /*!< Memory Protection Unit */
+#endif
+
+/*@} */
+
+
+
+/*******************************************************************************
+ * Hardware Abstraction Layer
+ Core Function Interface contains:
+ - Core NVIC Functions
+ - Core SysTick Functions
+ - Core Register Access Functions
+ ******************************************************************************/
+/**
+ \defgroup CMSIS_Core_FunctionInterface Functions and Instructions Reference
+*/
+
+
+
+/* ########################## NVIC functions #################################### */
+/**
+ \ingroup CMSIS_Core_FunctionInterface
+ \defgroup CMSIS_Core_NVICFunctions NVIC Functions
+ \brief Functions that manage interrupts and exceptions via the NVIC.
+ @{
+ */
+
+#ifdef CMSIS_NVIC_VIRTUAL
+ #ifndef CMSIS_NVIC_VIRTUAL_HEADER_FILE
+ #define CMSIS_NVIC_VIRTUAL_HEADER_FILE "cmsis_nvic_virtual.h"
+ #endif
+ #include CMSIS_NVIC_VIRTUAL_HEADER_FILE
+#else
+ #define NVIC_SetPriorityGrouping __NVIC_SetPriorityGrouping
+ #define NVIC_GetPriorityGrouping __NVIC_GetPriorityGrouping
+ #define NVIC_EnableIRQ __NVIC_EnableIRQ
+ #define NVIC_GetEnableIRQ __NVIC_GetEnableIRQ
+ #define NVIC_DisableIRQ __NVIC_DisableIRQ
+ #define NVIC_GetPendingIRQ __NVIC_GetPendingIRQ
+ #define NVIC_SetPendingIRQ __NVIC_SetPendingIRQ
+ #define NVIC_ClearPendingIRQ __NVIC_ClearPendingIRQ
+/*#define NVIC_GetActive __NVIC_GetActive not available for Cortex-M0+ */
+ #define NVIC_SetPriority __NVIC_SetPriority
+ #define NVIC_GetPriority __NVIC_GetPriority
+ #define NVIC_SystemReset __NVIC_SystemReset
+#endif /* CMSIS_NVIC_VIRTUAL */
+
+#ifdef CMSIS_VECTAB_VIRTUAL
+ #ifndef CMSIS_VECTAB_VIRTUAL_HEADER_FILE
+ #define CMSIS_VECTAB_VIRTUAL_HEADER_FILE "cmsis_vectab_virtual.h"
+ #endif
+ #include CMSIS_VECTAB_VIRTUAL_HEADER_FILE
+#else
+ #define NVIC_SetVector __NVIC_SetVector
+ #define NVIC_GetVector __NVIC_GetVector
+#endif /* (CMSIS_VECTAB_VIRTUAL) */
+
+#define NVIC_USER_IRQ_OFFSET 16
+
+
+/* The following EXC_RETURN values are saved the LR on exception entry */
+#define EXC_RETURN_HANDLER (0xFFFFFFF1UL) /* return to Handler mode, uses MSP after return */
+#define EXC_RETURN_THREAD_MSP (0xFFFFFFF9UL) /* return to Thread mode, uses MSP after return */
+#define EXC_RETURN_THREAD_PSP (0xFFFFFFFDUL) /* return to Thread mode, uses PSP after return */
+
+
+/* Interrupt Priorities are WORD accessible only under Armv6-M */
+/* The following MACROS handle generation of the register offset and byte masks */
+#define _BIT_SHIFT(IRQn) ( ((((uint32_t)(int32_t)(IRQn)) ) & 0x03UL) * 8UL)
+#define _SHP_IDX(IRQn) ( (((((uint32_t)(int32_t)(IRQn)) & 0x0FUL)-8UL) >> 2UL) )
+#define _IP_IDX(IRQn) ( (((uint32_t)(int32_t)(IRQn)) >> 2UL) )
+
+#define __NVIC_SetPriorityGrouping(X) (void)(X)
+#define __NVIC_GetPriorityGrouping() (0U)
+
+/**
+ \brief Enable Interrupt
+ \details Enables a device specific interrupt in the NVIC interrupt controller.
+ \param [in] IRQn Device specific interrupt number.
+ \note IRQn must not be negative.
+ */
+__STATIC_INLINE void __NVIC_EnableIRQ(IRQn_Type IRQn)
+{
+ if ((int32_t)(IRQn) >= 0)
+ {
+ __COMPILER_BARRIER();
+ NVIC->ISER[0U] = (uint32_t)(1UL << (((uint32_t)IRQn) & 0x1FUL));
+ __COMPILER_BARRIER();
+ }
+}
+
+
+/**
+ \brief Get Interrupt Enable status
+ \details Returns a device specific interrupt enable status from the NVIC interrupt controller.
+ \param [in] IRQn Device specific interrupt number.
+ \return 0 Interrupt is not enabled.
+ \return 1 Interrupt is enabled.
+ \note IRQn must not be negative.
+ */
+__STATIC_INLINE uint32_t __NVIC_GetEnableIRQ(IRQn_Type IRQn)
+{
+ if ((int32_t)(IRQn) >= 0)
+ {
+ return((uint32_t)(((NVIC->ISER[0U] & (1UL << (((uint32_t)IRQn) & 0x1FUL))) != 0UL) ? 1UL : 0UL));
+ }
+ else
+ {
+ return(0U);
+ }
+}
+
+
+/**
+ \brief Disable Interrupt
+ \details Disables a device specific interrupt in the NVIC interrupt controller.
+ \param [in] IRQn Device specific interrupt number.
+ \note IRQn must not be negative.
+ */
+__STATIC_INLINE void __NVIC_DisableIRQ(IRQn_Type IRQn)
+{
+ if ((int32_t)(IRQn) >= 0)
+ {
+ NVIC->ICER[0U] = (uint32_t)(1UL << (((uint32_t)IRQn) & 0x1FUL));
+ __DSB();
+ __ISB();
+ }
+}
+
+
+/**
+ \brief Get Pending Interrupt
+ \details Reads the NVIC pending register and returns the pending bit for the specified device specific interrupt.
+ \param [in] IRQn Device specific interrupt number.
+ \return 0 Interrupt status is not pending.
+ \return 1 Interrupt status is pending.
+ \note IRQn must not be negative.
+ */
+__STATIC_INLINE uint32_t __NVIC_GetPendingIRQ(IRQn_Type IRQn)
+{
+ if ((int32_t)(IRQn) >= 0)
+ {
+ return((uint32_t)(((NVIC->ISPR[0U] & (1UL << (((uint32_t)IRQn) & 0x1FUL))) != 0UL) ? 1UL : 0UL));
+ }
+ else
+ {
+ return(0U);
+ }
+}
+
+
+/**
+ \brief Set Pending Interrupt
+ \details Sets the pending bit of a device specific interrupt in the NVIC pending register.
+ \param [in] IRQn Device specific interrupt number.
+ \note IRQn must not be negative.
+ */
+__STATIC_INLINE void __NVIC_SetPendingIRQ(IRQn_Type IRQn)
+{
+ if ((int32_t)(IRQn) >= 0)
+ {
+ NVIC->ISPR[0U] = (uint32_t)(1UL << (((uint32_t)IRQn) & 0x1FUL));
+ }
+}
+
+
+/**
+ \brief Clear Pending Interrupt
+ \details Clears the pending bit of a device specific interrupt in the NVIC pending register.
+ \param [in] IRQn Device specific interrupt number.
+ \note IRQn must not be negative.
+ */
+__STATIC_INLINE void __NVIC_ClearPendingIRQ(IRQn_Type IRQn)
+{
+ if ((int32_t)(IRQn) >= 0)
+ {
+ NVIC->ICPR[0U] = (uint32_t)(1UL << (((uint32_t)IRQn) & 0x1FUL));
+ }
+}
+
+
+/**
+ \brief Set Interrupt Priority
+ \details Sets the priority of a device specific interrupt or a processor exception.
+ The interrupt number can be positive to specify a device specific interrupt,
+ or negative to specify a processor exception.
+ \param [in] IRQn Interrupt number.
+ \param [in] priority Priority to set.
+ \note The priority cannot be set for every processor exception.
+ */
+__STATIC_INLINE void __NVIC_SetPriority(IRQn_Type IRQn, uint32_t priority)
+{
+ if ((int32_t)(IRQn) >= 0)
+ {
+ NVIC->IP[_IP_IDX(IRQn)] = ((uint32_t)(NVIC->IP[_IP_IDX(IRQn)] & ~(0xFFUL << _BIT_SHIFT(IRQn))) |
+ (((priority << (8U - __NVIC_PRIO_BITS)) & (uint32_t)0xFFUL) << _BIT_SHIFT(IRQn)));
+ }
+ else
+ {
+ SCB->SHP[_SHP_IDX(IRQn)] = ((uint32_t)(SCB->SHP[_SHP_IDX(IRQn)] & ~(0xFFUL << _BIT_SHIFT(IRQn))) |
+ (((priority << (8U - __NVIC_PRIO_BITS)) & (uint32_t)0xFFUL) << _BIT_SHIFT(IRQn)));
+ }
+}
+
+
+/**
+ \brief Get Interrupt Priority
+ \details Reads the priority of a device specific interrupt or a processor exception.
+ The interrupt number can be positive to specify a device specific interrupt,
+ or negative to specify a processor exception.
+ \param [in] IRQn Interrupt number.
+ \return Interrupt Priority.
+ Value is aligned automatically to the implemented priority bits of the microcontroller.
+ */
+__STATIC_INLINE uint32_t __NVIC_GetPriority(IRQn_Type IRQn)
+{
+
+ if ((int32_t)(IRQn) >= 0)
+ {
+ return((uint32_t)(((NVIC->IP[ _IP_IDX(IRQn)] >> _BIT_SHIFT(IRQn) ) & (uint32_t)0xFFUL) >> (8U - __NVIC_PRIO_BITS)));
+ }
+ else
+ {
+ return((uint32_t)(((SCB->SHP[_SHP_IDX(IRQn)] >> _BIT_SHIFT(IRQn) ) & (uint32_t)0xFFUL) >> (8U - __NVIC_PRIO_BITS)));
+ }
+}
+
+
+/**
+ \brief Encode Priority
+ \details Encodes the priority for an interrupt with the given priority group,
+ preemptive priority value, and subpriority value.
+ In case of a conflict between priority grouping and available
+ priority bits (__NVIC_PRIO_BITS), the smallest possible priority group is set.
+ \param [in] PriorityGroup Used priority group.
+ \param [in] PreemptPriority Preemptive priority value (starting from 0).
+ \param [in] SubPriority Subpriority value (starting from 0).
+ \return Encoded priority. Value can be used in the function \ref NVIC_SetPriority().
+ */
+__STATIC_INLINE uint32_t NVIC_EncodePriority (uint32_t PriorityGroup, uint32_t PreemptPriority, uint32_t SubPriority)
+{
+ uint32_t PriorityGroupTmp = (PriorityGroup & (uint32_t)0x07UL); /* only values 0..7 are used */
+ uint32_t PreemptPriorityBits;
+ uint32_t SubPriorityBits;
+
+ PreemptPriorityBits = ((7UL - PriorityGroupTmp) > (uint32_t)(__NVIC_PRIO_BITS)) ? (uint32_t)(__NVIC_PRIO_BITS) : (uint32_t)(7UL - PriorityGroupTmp);
+ SubPriorityBits = ((PriorityGroupTmp + (uint32_t)(__NVIC_PRIO_BITS)) < (uint32_t)7UL) ? (uint32_t)0UL : (uint32_t)((PriorityGroupTmp - 7UL) + (uint32_t)(__NVIC_PRIO_BITS));
+
+ return (
+ ((PreemptPriority & (uint32_t)((1UL << (PreemptPriorityBits)) - 1UL)) << SubPriorityBits) |
+ ((SubPriority & (uint32_t)((1UL << (SubPriorityBits )) - 1UL)))
+ );
+}
+
+
+/**
+ \brief Decode Priority
+ \details Decodes an interrupt priority value with a given priority group to
+ preemptive priority value and subpriority value.
+ In case of a conflict between priority grouping and available
+ priority bits (__NVIC_PRIO_BITS) the smallest possible priority group is set.
+ \param [in] Priority Priority value, which can be retrieved with the function \ref NVIC_GetPriority().
+ \param [in] PriorityGroup Used priority group.
+ \param [out] pPreemptPriority Preemptive priority value (starting from 0).
+ \param [out] pSubPriority Subpriority value (starting from 0).
+ */
+__STATIC_INLINE void NVIC_DecodePriority (uint32_t Priority, uint32_t PriorityGroup, uint32_t* const pPreemptPriority, uint32_t* const pSubPriority)
+{
+ uint32_t PriorityGroupTmp = (PriorityGroup & (uint32_t)0x07UL); /* only values 0..7 are used */
+ uint32_t PreemptPriorityBits;
+ uint32_t SubPriorityBits;
+
+ PreemptPriorityBits = ((7UL - PriorityGroupTmp) > (uint32_t)(__NVIC_PRIO_BITS)) ? (uint32_t)(__NVIC_PRIO_BITS) : (uint32_t)(7UL - PriorityGroupTmp);
+ SubPriorityBits = ((PriorityGroupTmp + (uint32_t)(__NVIC_PRIO_BITS)) < (uint32_t)7UL) ? (uint32_t)0UL : (uint32_t)((PriorityGroupTmp - 7UL) + (uint32_t)(__NVIC_PRIO_BITS));
+
+ *pPreemptPriority = (Priority >> SubPriorityBits) & (uint32_t)((1UL << (PreemptPriorityBits)) - 1UL);
+ *pSubPriority = (Priority ) & (uint32_t)((1UL << (SubPriorityBits )) - 1UL);
+}
+
+
+/**
+ \brief Set Interrupt Vector
+ \details Sets an interrupt vector in SRAM based interrupt vector table.
+ The interrupt number can be positive to specify a device specific interrupt,
+ or negative to specify a processor exception.
+ VTOR must been relocated to SRAM before.
+ If VTOR is not present address 0 must be mapped to SRAM.
+ \param [in] IRQn Interrupt number
+ \param [in] vector Address of interrupt handler function
+ */
+__STATIC_INLINE void __NVIC_SetVector(IRQn_Type IRQn, uint32_t vector)
+{
+#if defined (__VTOR_PRESENT) && (__VTOR_PRESENT == 1U)
+ uint32_t *vectors = (uint32_t *)SCB->VTOR;
+ vectors[(int32_t)IRQn + NVIC_USER_IRQ_OFFSET] = vector;
+#else
+ uint32_t *vectors = (uint32_t *)(NVIC_USER_IRQ_OFFSET << 2); /* point to 1st user interrupt */
+ *(vectors + (int32_t)IRQn) = vector; /* use pointer arithmetic to access vector */
+#endif
+ /* ARM Application Note 321 states that the M0+ does not require the architectural barrier */
+}
+
+
+/**
+ \brief Get Interrupt Vector
+ \details Reads an interrupt vector from interrupt vector table.
+ The interrupt number can be positive to specify a device specific interrupt,
+ or negative to specify a processor exception.
+ \param [in] IRQn Interrupt number.
+ \return Address of interrupt handler function
+ */
+__STATIC_INLINE uint32_t __NVIC_GetVector(IRQn_Type IRQn)
+{
+#if defined (__VTOR_PRESENT) && (__VTOR_PRESENT == 1U)
+ uint32_t *vectors = (uint32_t *)SCB->VTOR;
+ return vectors[(int32_t)IRQn + NVIC_USER_IRQ_OFFSET];
+#else
+ uint32_t *vectors = (uint32_t *)(NVIC_USER_IRQ_OFFSET << 2); /* point to 1st user interrupt */
+ return *(vectors + (int32_t)IRQn); /* use pointer arithmetic to access vector */
+#endif
+}
+
+
+/**
+ \brief System Reset
+ \details Initiates a system reset request to reset the MCU.
+ */
+__NO_RETURN __STATIC_INLINE void __NVIC_SystemReset(void)
+{
+ __DSB(); /* Ensure all outstanding memory accesses included
+ buffered write are completed before reset */
+ SCB->AIRCR = ((0x5FAUL << SCB_AIRCR_VECTKEY_Pos) |
+ SCB_AIRCR_SYSRESETREQ_Msk);
+ __DSB(); /* Ensure completion of memory access */
+
+ for(;;) /* wait until reset */
+ {
+ __NOP();
+ }
+}
+
+/*@} end of CMSIS_Core_NVICFunctions */
+
+/* ########################## MPU functions #################################### */
+
+#if defined (__MPU_PRESENT) && (__MPU_PRESENT == 1U)
+
+#include "mpu_armv7.h"
+
+#endif
+
+/* ########################## FPU functions #################################### */
+/**
+ \ingroup CMSIS_Core_FunctionInterface
+ \defgroup CMSIS_Core_FpuFunctions FPU Functions
+ \brief Function that provides FPU type.
+ @{
+ */
+
+/**
+ \brief get FPU type
+ \details returns the FPU type
+ \returns
+ - \b 0: No FPU
+ - \b 1: Single precision FPU
+ - \b 2: Double + Single precision FPU
+ */
+__STATIC_INLINE uint32_t SCB_GetFPUType(void)
+{
+ return 0U; /* No FPU */
+}
+
+
+/*@} end of CMSIS_Core_FpuFunctions */
+
+
+
+/* ################################## SysTick function ############################################ */
+/**
+ \ingroup CMSIS_Core_FunctionInterface
+ \defgroup CMSIS_Core_SysTickFunctions SysTick Functions
+ \brief Functions that configure the System.
+ @{
+ */
+
+#if defined (__Vendor_SysTickConfig) && (__Vendor_SysTickConfig == 0U)
+
+/**
+ \brief System Tick Configuration
+ \details Initializes the System Timer and its interrupt, and starts the System Tick Timer.
+ Counter is in free running mode to generate periodic interrupts.
+ \param [in] ticks Number of ticks between two interrupts.
+ \return 0 Function succeeded.
+ \return 1 Function failed.
+ \note When the variable <b>__Vendor_SysTickConfig</b> is set to 1, then the
+ function <b>SysTick_Config</b> is not included. In this case, the file <b><i>device</i>.h</b>
+ must contain a vendor-specific implementation of this function.
+ */
+__STATIC_INLINE uint32_t SysTick_Config(uint32_t ticks)
+{
+ if ((ticks - 1UL) > SysTick_LOAD_RELOAD_Msk)
+ {
+ return (1UL); /* Reload value impossible */
+ }
+
+ SysTick->LOAD = (uint32_t)(ticks - 1UL); /* set reload register */
+ NVIC_SetPriority (SysTick_IRQn, (1UL << __NVIC_PRIO_BITS) - 1UL); /* set Priority for Systick Interrupt */
+ SysTick->VAL = 0UL; /* Load the SysTick Counter Value */
+ SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk |
+ SysTick_CTRL_TICKINT_Msk |
+ SysTick_CTRL_ENABLE_Msk; /* Enable SysTick IRQ and SysTick Timer */
+ return (0UL); /* Function successful */
+}
+
+#endif
+
+/*@} end of CMSIS_Core_SysTickFunctions */
+
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __CORE_CM0PLUS_H_DEPENDANT */
+
+#endif /* __CMSIS_GENERIC */
diff --git a/src/rp2_common/cmsis/stub/CMSIS/Core/Include/mpu_armv7.h b/src/rp2_common/cmsis/stub/CMSIS/Core/Include/mpu_armv7.h
new file mode 100644
index 0000000..791a8da
--- /dev/null
+++ b/src/rp2_common/cmsis/stub/CMSIS/Core/Include/mpu_armv7.h
@@ -0,0 +1,275 @@
+/******************************************************************************
+ * @file mpu_armv7.h
+ * @brief CMSIS MPU API for Armv7-M MPU
+ * @version V5.1.1
+ * @date 10. February 2020
+ ******************************************************************************/
+/*
+ * Copyright (c) 2017-2020 Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#if defined ( __ICCARM__ )
+ #pragma system_include /* treat file as system include file for MISRA check */
+#elif defined (__clang__)
+ #pragma clang system_header /* treat file as system include file */
+#endif
+
+#ifndef ARM_MPU_ARMV7_H
+#define ARM_MPU_ARMV7_H
+
+#define ARM_MPU_REGION_SIZE_32B ((uint8_t)0x04U) ///!< MPU Region Size 32 Bytes
+#define ARM_MPU_REGION_SIZE_64B ((uint8_t)0x05U) ///!< MPU Region Size 64 Bytes
+#define ARM_MPU_REGION_SIZE_128B ((uint8_t)0x06U) ///!< MPU Region Size 128 Bytes
+#define ARM_MPU_REGION_SIZE_256B ((uint8_t)0x07U) ///!< MPU Region Size 256 Bytes
+#define ARM_MPU_REGION_SIZE_512B ((uint8_t)0x08U) ///!< MPU Region Size 512 Bytes
+#define ARM_MPU_REGION_SIZE_1KB ((uint8_t)0x09U) ///!< MPU Region Size 1 KByte
+#define ARM_MPU_REGION_SIZE_2KB ((uint8_t)0x0AU) ///!< MPU Region Size 2 KBytes
+#define ARM_MPU_REGION_SIZE_4KB ((uint8_t)0x0BU) ///!< MPU Region Size 4 KBytes
+#define ARM_MPU_REGION_SIZE_8KB ((uint8_t)0x0CU) ///!< MPU Region Size 8 KBytes
+#define ARM_MPU_REGION_SIZE_16KB ((uint8_t)0x0DU) ///!< MPU Region Size 16 KBytes
+#define ARM_MPU_REGION_SIZE_32KB ((uint8_t)0x0EU) ///!< MPU Region Size 32 KBytes
+#define ARM_MPU_REGION_SIZE_64KB ((uint8_t)0x0FU) ///!< MPU Region Size 64 KBytes
+#define ARM_MPU_REGION_SIZE_128KB ((uint8_t)0x10U) ///!< MPU Region Size 128 KBytes
+#define ARM_MPU_REGION_SIZE_256KB ((uint8_t)0x11U) ///!< MPU Region Size 256 KBytes
+#define ARM_MPU_REGION_SIZE_512KB ((uint8_t)0x12U) ///!< MPU Region Size 512 KBytes
+#define ARM_MPU_REGION_SIZE_1MB ((uint8_t)0x13U) ///!< MPU Region Size 1 MByte
+#define ARM_MPU_REGION_SIZE_2MB ((uint8_t)0x14U) ///!< MPU Region Size 2 MBytes
+#define ARM_MPU_REGION_SIZE_4MB ((uint8_t)0x15U) ///!< MPU Region Size 4 MBytes
+#define ARM_MPU_REGION_SIZE_8MB ((uint8_t)0x16U) ///!< MPU Region Size 8 MBytes
+#define ARM_MPU_REGION_SIZE_16MB ((uint8_t)0x17U) ///!< MPU Region Size 16 MBytes
+#define ARM_MPU_REGION_SIZE_32MB ((uint8_t)0x18U) ///!< MPU Region Size 32 MBytes
+#define ARM_MPU_REGION_SIZE_64MB ((uint8_t)0x19U) ///!< MPU Region Size 64 MBytes
+#define ARM_MPU_REGION_SIZE_128MB ((uint8_t)0x1AU) ///!< MPU Region Size 128 MBytes
+#define ARM_MPU_REGION_SIZE_256MB ((uint8_t)0x1BU) ///!< MPU Region Size 256 MBytes
+#define ARM_MPU_REGION_SIZE_512MB ((uint8_t)0x1CU) ///!< MPU Region Size 512 MBytes
+#define ARM_MPU_REGION_SIZE_1GB ((uint8_t)0x1DU) ///!< MPU Region Size 1 GByte
+#define ARM_MPU_REGION_SIZE_2GB ((uint8_t)0x1EU) ///!< MPU Region Size 2 GBytes
+#define ARM_MPU_REGION_SIZE_4GB ((uint8_t)0x1FU) ///!< MPU Region Size 4 GBytes
+
+#define ARM_MPU_AP_NONE 0U ///!< MPU Access Permission no access
+#define ARM_MPU_AP_PRIV 1U ///!< MPU Access Permission privileged access only
+#define ARM_MPU_AP_URO 2U ///!< MPU Access Permission unprivileged access read-only
+#define ARM_MPU_AP_FULL 3U ///!< MPU Access Permission full access
+#define ARM_MPU_AP_PRO 5U ///!< MPU Access Permission privileged access read-only
+#define ARM_MPU_AP_RO 6U ///!< MPU Access Permission read-only access
+
+/** MPU Region Base Address Register Value
+*
+* \param Region The region to be configured, number 0 to 15.
+* \param BaseAddress The base address for the region.
+*/
+#define ARM_MPU_RBAR(Region, BaseAddress) \
+ (((BaseAddress) & MPU_RBAR_ADDR_Msk) | \
+ ((Region) & MPU_RBAR_REGION_Msk) | \
+ (MPU_RBAR_VALID_Msk))
+
+/**
+* MPU Memory Access Attributes
+*
+* \param TypeExtField Type extension field, allows you to configure memory access type, for example strongly ordered, peripheral.
+* \param IsShareable Region is shareable between multiple bus masters.
+* \param IsCacheable Region is cacheable, i.e. its value may be kept in cache.
+* \param IsBufferable Region is bufferable, i.e. using write-back caching. Cacheable but non-bufferable regions use write-through policy.
+*/
+#define ARM_MPU_ACCESS_(TypeExtField, IsShareable, IsCacheable, IsBufferable) \
+ ((((TypeExtField) << MPU_RASR_TEX_Pos) & MPU_RASR_TEX_Msk) | \
+ (((IsShareable) << MPU_RASR_S_Pos) & MPU_RASR_S_Msk) | \
+ (((IsCacheable) << MPU_RASR_C_Pos) & MPU_RASR_C_Msk) | \
+ (((IsBufferable) << MPU_RASR_B_Pos) & MPU_RASR_B_Msk))
+
+/**
+* MPU Region Attribute and Size Register Value
+*
+* \param DisableExec Instruction access disable bit, 1= disable instruction fetches.
+* \param AccessPermission Data access permissions, allows you to configure read/write access for User and Privileged mode.
+* \param AccessAttributes Memory access attribution, see \ref ARM_MPU_ACCESS_.
+* \param SubRegionDisable Sub-region disable field.
+* \param Size Region size of the region to be configured, for example 4K, 8K.
+*/
+#define ARM_MPU_RASR_EX(DisableExec, AccessPermission, AccessAttributes, SubRegionDisable, Size) \
+ ((((DisableExec) << MPU_RASR_XN_Pos) & MPU_RASR_XN_Msk) | \
+ (((AccessPermission) << MPU_RASR_AP_Pos) & MPU_RASR_AP_Msk) | \
+ (((AccessAttributes) & (MPU_RASR_TEX_Msk | MPU_RASR_S_Msk | MPU_RASR_C_Msk | MPU_RASR_B_Msk))) | \
+ (((SubRegionDisable) << MPU_RASR_SRD_Pos) & MPU_RASR_SRD_Msk) | \
+ (((Size) << MPU_RASR_SIZE_Pos) & MPU_RASR_SIZE_Msk) | \
+ (((MPU_RASR_ENABLE_Msk))))
+
+/**
+* MPU Region Attribute and Size Register Value
+*
+* \param DisableExec Instruction access disable bit, 1= disable instruction fetches.
+* \param AccessPermission Data access permissions, allows you to configure read/write access for User and Privileged mode.
+* \param TypeExtField Type extension field, allows you to configure memory access type, for example strongly ordered, peripheral.
+* \param IsShareable Region is shareable between multiple bus masters.
+* \param IsCacheable Region is cacheable, i.e. its value may be kept in cache.
+* \param IsBufferable Region is bufferable, i.e. using write-back caching. Cacheable but non-bufferable regions use write-through policy.
+* \param SubRegionDisable Sub-region disable field.
+* \param Size Region size of the region to be configured, for example 4K, 8K.
+*/
+#define ARM_MPU_RASR(DisableExec, AccessPermission, TypeExtField, IsShareable, IsCacheable, IsBufferable, SubRegionDisable, Size) \
+ ARM_MPU_RASR_EX(DisableExec, AccessPermission, ARM_MPU_ACCESS_(TypeExtField, IsShareable, IsCacheable, IsBufferable), SubRegionDisable, Size)
+
+/**
+* MPU Memory Access Attribute for strongly ordered memory.
+* - TEX: 000b
+* - Shareable
+* - Non-cacheable
+* - Non-bufferable
+*/
+#define ARM_MPU_ACCESS_ORDERED ARM_MPU_ACCESS_(0U, 1U, 0U, 0U)
+
+/**
+* MPU Memory Access Attribute for device memory.
+* - TEX: 000b (if shareable) or 010b (if non-shareable)
+* - Shareable or non-shareable
+* - Non-cacheable
+* - Bufferable (if shareable) or non-bufferable (if non-shareable)
+*
+* \param IsShareable Configures the device memory as shareable or non-shareable.
+*/
+#define ARM_MPU_ACCESS_DEVICE(IsShareable) ((IsShareable) ? ARM_MPU_ACCESS_(0U, 1U, 0U, 1U) : ARM_MPU_ACCESS_(2U, 0U, 0U, 0U))
+
+/**
+* MPU Memory Access Attribute for normal memory.
+* - TEX: 1BBb (reflecting outer cacheability rules)
+* - Shareable or non-shareable
+* - Cacheable or non-cacheable (reflecting inner cacheability rules)
+* - Bufferable or non-bufferable (reflecting inner cacheability rules)
+*
+* \param OuterCp Configures the outer cache policy.
+* \param InnerCp Configures the inner cache policy.
+* \param IsShareable Configures the memory as shareable or non-shareable.
+*/
+#define ARM_MPU_ACCESS_NORMAL(OuterCp, InnerCp, IsShareable) ARM_MPU_ACCESS_((4U | (OuterCp)), IsShareable, ((InnerCp) >> 1U), ((InnerCp) & 1U))
+
+/**
+* MPU Memory Access Attribute non-cacheable policy.
+*/
+#define ARM_MPU_CACHEP_NOCACHE 0U
+
+/**
+* MPU Memory Access Attribute write-back, write and read allocate policy.
+*/
+#define ARM_MPU_CACHEP_WB_WRA 1U
+
+/**
+* MPU Memory Access Attribute write-through, no write allocate policy.
+*/
+#define ARM_MPU_CACHEP_WT_NWA 2U
+
+/**
+* MPU Memory Access Attribute write-back, no write allocate policy.
+*/
+#define ARM_MPU_CACHEP_WB_NWA 3U
+
+
+/**
+* Struct for a single MPU Region
+*/
+typedef struct {
+ uint32_t RBAR; //!< The region base address register value (RBAR)
+ uint32_t RASR; //!< The region attribute and size register value (RASR) \ref MPU_RASR
+} ARM_MPU_Region_t;
+
+/** Enable the MPU.
+* \param MPU_Control Default access permissions for unconfigured regions.
+*/
+__STATIC_INLINE void ARM_MPU_Enable(uint32_t MPU_Control)
+{
+ __DMB();
+ MPU->CTRL = MPU_Control | MPU_CTRL_ENABLE_Msk;
+#ifdef SCB_SHCSR_MEMFAULTENA_Msk
+ SCB->SHCSR |= SCB_SHCSR_MEMFAULTENA_Msk;
+#endif
+ __DSB();
+ __ISB();
+}
+
+/** Disable the MPU.
+*/
+__STATIC_INLINE void ARM_MPU_Disable(void)
+{
+ __DMB();
+#ifdef SCB_SHCSR_MEMFAULTENA_Msk
+ SCB->SHCSR &= ~SCB_SHCSR_MEMFAULTENA_Msk;
+#endif
+ MPU->CTRL &= ~MPU_CTRL_ENABLE_Msk;
+ __DSB();
+ __ISB();
+}
+
+/** Clear and disable the given MPU region.
+* \param rnr Region number to be cleared.
+*/
+__STATIC_INLINE void ARM_MPU_ClrRegion(uint32_t rnr)
+{
+ MPU->RNR = rnr;
+ MPU->RASR = 0U;
+}
+
+/** Configure an MPU region.
+* \param rbar Value for RBAR register.
+* \param rsar Value for RSAR register.
+*/
+__STATIC_INLINE void ARM_MPU_SetRegion(uint32_t rbar, uint32_t rasr)
+{
+ MPU->RBAR = rbar;
+ MPU->RASR = rasr;
+}
+
+/** Configure the given MPU region.
+* \param rnr Region number to be configured.
+* \param rbar Value for RBAR register.
+* \param rsar Value for RSAR register.
+*/
+__STATIC_INLINE void ARM_MPU_SetRegionEx(uint32_t rnr, uint32_t rbar, uint32_t rasr)
+{
+ MPU->RNR = rnr;
+ MPU->RBAR = rbar;
+ MPU->RASR = rasr;
+}
+
+/** Memcopy with strictly ordered memory access, e.g. for register targets.
+* \param dst Destination data is copied to.
+* \param src Source data is copied from.
+* \param len Amount of data words to be copied.
+*/
+__STATIC_INLINE void ARM_MPU_OrderedMemcpy(volatile uint32_t* dst, const uint32_t* __RESTRICT src, uint32_t len)
+{
+ uint32_t i;
+ for (i = 0U; i < len; ++i)
+ {
+ dst[i] = src[i];
+ }
+}
+
+/** Load the given number of MPU regions from a table.
+* \param table Pointer to the MPU configuration table.
+* \param cnt Amount of regions to be configured.
+*/
+__STATIC_INLINE void ARM_MPU_Load(ARM_MPU_Region_t const* table, uint32_t cnt)
+{
+ const uint32_t rowWordSize = sizeof(ARM_MPU_Region_t)/4U;
+ while (cnt > MPU_TYPE_RALIASES) {
+ ARM_MPU_OrderedMemcpy(&(MPU->RBAR), &(table->RBAR), MPU_TYPE_RALIASES*rowWordSize);
+ table += MPU_TYPE_RALIASES;
+ cnt -= MPU_TYPE_RALIASES;
+ }
+ ARM_MPU_OrderedMemcpy(&(MPU->RBAR), &(table->RBAR), cnt*rowWordSize);
+}
+
+#endif
diff --git a/src/rp2_common/cmsis/stub/CMSIS/Device/RaspberryPi/RP2040/Include/RP2040.h b/src/rp2_common/cmsis/stub/CMSIS/Device/RaspberryPi/RP2040/Include/RP2040.h
new file mode 100644
index 0000000..a29b9e0
--- /dev/null
+++ b/src/rp2_common/cmsis/stub/CMSIS/Device/RaspberryPi/RP2040/Include/RP2040.h
@@ -0,0 +1,109 @@
+/*************************************************************************//**
+ * @file RP2040.h
+ * @brief CMSIS-Core(M) Device Peripheral Access Layer Header File for
+ * Device RP2040
+ * @version V1.0.0
+ * @date 5. May 2021
+ *****************************************************************************/
+/*
+ * Copyright (c) 2009-2021 Arm Limited. All rights reserved.
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _CMSIS_RP2040_H_
+#define _CMSIS_RP2040_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* =========================================================================================================================== */
+/* ================ Interrupt Number Definition ================ */
+/* =========================================================================================================================== */
+
+typedef enum
+{
+ /* ======================================= ARM Cortex-M0+ Specific Interrupt Numbers ======================================= */
+ Reset_IRQn = -15, /*!< -15 Reset Vector, invoked on Power up and warm reset */
+ NonMaskableInt_IRQn = -14, /*!< -14 Non maskable Interrupt, cannot be stopped or preempted */
+ HardFault_IRQn = -13, /*!< -13 Hard Fault, all classes of Fault */
+ SVCall_IRQn = -5, /*!< -5 System Service Call via SVC instruction */
+ PendSV_IRQn = -2, /*!< -2 Pendable request for system service */
+ SysTick_IRQn = -1, /*!< -1 System Tick Timer */
+ /* =========================================== RP2040 Specific Interrupt Numbers =========================================== */
+ TIMER_IRQ_0_IRQn = 0, /*!< 0 TIMER_IRQ_0 */
+ TIMER_IRQ_1_IRQn = 1, /*!< 1 TIMER_IRQ_1 */
+ TIMER_IRQ_2_IRQn = 2, /*!< 2 TIMER_IRQ_2 */
+ TIMER_IRQ_3_IRQn = 3, /*!< 3 TIMER_IRQ_3 */
+ PWM_IRQ_WRAP_IRQn = 4, /*!< 4 PWM_IRQ_WRAP */
+ USBCTRL_IRQ_IRQn = 5, /*!< 5 USBCTRL_IRQ */
+ XIP_IRQ_IRQn = 6, /*!< 6 XIP_IRQ */
+ PIO0_IRQ_0_IRQn = 7, /*!< 7 PIO0_IRQ_0 */
+ PIO0_IRQ_1_IRQn = 8, /*!< 8 PIO0_IRQ_1 */
+ PIO1_IRQ_0_IRQn = 9, /*!< 9 PIO1_IRQ_0 */
+ PIO1_IRQ_1_IRQn = 10, /*!< 10 PIO1_IRQ_1 */
+ DMA_IRQ_0_IRQn = 11, /*!< 11 DMA_IRQ_0 */
+ DMA_IRQ_1_IRQn = 12, /*!< 12 DMA_IRQ_1 */
+ IO_IRQ_BANK0_IRQn = 13, /*!< 13 IO_IRQ_BANK0 */
+ IO_IRQ_QSPI_IRQn = 14, /*!< 14 IO_IRQ_QSPI */
+ SIO_IRQ_PROC0_IRQn = 15, /*!< 15 SIO_IRQ_PROC0 */
+ SIO_IRQ_PROC1_IRQn = 16, /*!< 16 SIO_IRQ_PROC1 */
+ CLOCKS_IRQ_IRQn = 17, /*!< 17 CLOCKS_IRQ */
+ SPI0_IRQ_IRQn = 18, /*!< 18 SPI0_IRQ */
+ SPI1_IRQ_IRQn = 19, /*!< 19 SPI1_IRQ */
+ UART0_IRQ_IRQn = 20, /*!< 20 UART0_IRQ */
+ UART1_IRQ_IRQn = 21, /*!< 21 UART1_IRQ */
+ ADC_IRQ_FIFO_IRQn = 22, /*!< 22 ADC_IRQ_FIFO */
+ I2C0_IRQ_IRQn = 23, /*!< 23 I2C0_IRQ */
+ I2C1_IRQ_IRQn = 24, /*!< 24 I2C1_IRQ */
+ RTC_IRQ_IRQn = 25 /*!< 25 RTC_IRQ */
+} IRQn_Type;
+
+/* =========================================================================================================================== */
+/* ================ Processor and Core Peripheral Section ================ */
+/* =========================================================================================================================== */
+
+/* ========================== Configuration of the ARM Cortex-M0+ Processor and Core Peripherals =========================== */
+#define __CM0PLUS_REV 0x0001U /*!< CM0PLUS Core Revision */
+#define __NVIC_PRIO_BITS 2 /*!< Number of Bits used for Priority Levels */
+#define __Vendor_SysTickConfig 0 /*!< Set to 1 if different SysTick Config is used */
+#define __VTOR_PRESENT 1 /*!< Set to 1 if CPU supports Vector Table Offset Register */
+#define __MPU_PRESENT 1 /*!< MPU present */
+#define __FPU_PRESENT 0 /*!< FPU present */
+
+/** @} */ /* End of group Configuration_of_CMSIS */
+
+#include "core_cm0plus.h" /*!< ARM Cortex-M0+ processor and core peripherals */
+#include "system_RP2040.h" /*!< RP2040 System */
+
+#ifndef __IM /*!< Fallback for older CMSIS versions */
+#define __IM __I
+#endif
+#ifndef __OM /*!< Fallback for older CMSIS versions */
+#define __OM __O
+#endif
+#ifndef __IOM /*!< Fallback for older CMSIS versions */
+#define __IOM __IO
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _CMSIS_RP2040_H */
diff --git a/src/rp2_common/cmsis/stub/CMSIS/Device/RaspberryPi/RP2040/Include/system_RP2040.h b/src/rp2_common/cmsis/stub/CMSIS/Device/RaspberryPi/RP2040/Include/system_RP2040.h
new file mode 100644
index 0000000..30881cc
--- /dev/null
+++ b/src/rp2_common/cmsis/stub/CMSIS/Device/RaspberryPi/RP2040/Include/system_RP2040.h
@@ -0,0 +1,65 @@
+/*************************************************************************//**
+ * @file system_RP2040.h
+ * @brief CMSIS-Core(M) Device Peripheral Access Layer Header File for
+ * Device RP2040
+ * @version V1.0.0
+ * @date 5. May 2021
+ *****************************************************************************/
+/*
+ * Copyright (c) 2009-2021 Arm Limited. All rights reserved.
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _CMSIS_SYSTEM_RP2040_H
+#define _CMSIS_SYSTEM_RP2040_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ \brief Exception / Interrupt Handler Function Prototype
+*/
+typedef void(*VECTOR_TABLE_Type)(void);
+
+/**
+ \brief System Clock Frequency (Core Clock)
+*/
+extern uint32_t SystemCoreClock;
+
+/**
+ \brief Setup the microcontroller system.
+
+ Initialize the System and update the SystemCoreClock variable.
+ */
+extern void SystemInit (void);
+
+
+/**
+ \brief Update SystemCoreClock variable.
+
+ Updates the SystemCoreClock with current core Clock retrieved from cpu registers.
+ */
+extern void SystemCoreClockUpdate (void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _CMSIS_SYSTEM_RP2040_H */
diff --git a/src/rp2_common/cmsis/stub/CMSIS/Device/RaspberryPi/RP2040/Source/system_RP2040.c b/src/rp2_common/cmsis/stub/CMSIS/Device/RaspberryPi/RP2040/Source/system_RP2040.c
new file mode 100644
index 0000000..055a075
--- /dev/null
+++ b/src/rp2_common/cmsis/stub/CMSIS/Device/RaspberryPi/RP2040/Source/system_RP2040.c
@@ -0,0 +1,52 @@
+/*************************************************************************//**
+ * @file system_RP2040.c
+ * @brief CMSIS-Core(M) Device Peripheral Access Layer Header File for
+ * Device RP2040
+ * @version V1.0.0
+ * @date 5. May 2021
+ *****************************************************************************/
+/*
+ * Copyright (c) 2009-2021 Arm Limited. All rights reserved.
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+#include "RP2040.h"
+#include "hardware/clocks.h"
+
+/*---------------------------------------------------------------------------
+ System Core Clock Variable
+ *---------------------------------------------------------------------------*/
+uint32_t SystemCoreClock; /* System Clock Frequency (Core Clock)*/
+
+/*---------------------------------------------------------------------------
+ System Core Clock function
+ *---------------------------------------------------------------------------*/
+void SystemCoreClockUpdate (void)
+{
+ SystemCoreClock = clock_get_hz(clk_sys);
+}
+
+/*---------------------------------------------------------------------------
+ System initialization function
+ *---------------------------------------------------------------------------*/
+void __attribute__((constructor)) SystemInit (void)
+{
+ SystemCoreClockUpdate();
+}
\ No newline at end of file
diff --git a/src/rp2_common/cmsis/stub/CMSIS/LICENSE.txt b/src/rp2_common/cmsis/stub/CMSIS/LICENSE.txt
new file mode 100644
index 0000000..8dada3e
--- /dev/null
+++ b/src/rp2_common/cmsis/stub/CMSIS/LICENSE.txt
@@ -0,0 +1,201 @@
+ Apache License
+ Version 2.0, January 2004
+ http://www.apache.org/licenses/
+
+ TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+ 1. Definitions.
+
+ "License" shall mean the terms and conditions for use, reproduction,
+ and distribution as defined by Sections 1 through 9 of this document.
+
+ "Licensor" shall mean the copyright owner or entity authorized by
+ the copyright owner that is granting the License.
+
+ "Legal Entity" shall mean the union of the acting entity and all
+ other entities that control, are controlled by, or are under common
+ control with that entity. For the purposes of this definition,
+ "control" means (i) the power, direct or indirect, to cause the
+ direction or management of such entity, whether by contract or
+ otherwise, or (ii) ownership of fifty percent (50%) or more of the
+ outstanding shares, or (iii) beneficial ownership of such entity.
+
+ "You" (or "Your") shall mean an individual or Legal Entity
+ exercising permissions granted by this License.
+
+ "Source" form shall mean the preferred form for making modifications,
+ including but not limited to software source code, documentation
+ source, and configuration files.
+
+ "Object" form shall mean any form resulting from mechanical
+ transformation or translation of a Source form, including but
+ not limited to compiled object code, generated documentation,
+ and conversions to other media types.
+
+ "Work" shall mean the work of authorship, whether in Source or
+ Object form, made available under the License, as indicated by a
+ copyright notice that is included in or attached to the work
+ (an example is provided in the Appendix below).
+
+ "Derivative Works" shall mean any work, whether in Source or Object
+ form, that is based on (or derived from) the Work and for which the
+ editorial revisions, annotations, elaborations, or other modifications
+ represent, as a whole, an original work of authorship. For the purposes
+ of this License, Derivative Works shall not include works that remain
+ separable from, or merely link (or bind by name) to the interfaces of,
+ the Work and Derivative Works thereof.
+
+ "Contribution" shall mean any work of authorship, including
+ the original version of the Work and any modifications or additions
+ to that Work or Derivative Works thereof, that is intentionally
+ submitted to Licensor for inclusion in the Work by the copyright owner
+ or by an individual or Legal Entity authorized to submit on behalf of
+ the copyright owner. For the purposes of this definition, "submitted"
+ means any form of electronic, verbal, or written communication sent
+ to the Licensor or its representatives, including but not limited to
+ communication on electronic mailing lists, source code control systems,
+ and issue tracking systems that are managed by, or on behalf of, the
+ Licensor for the purpose of discussing and improving the Work, but
+ excluding communication that is conspicuously marked or otherwise
+ designated in writing by the copyright owner as "Not a Contribution."
+
+ "Contributor" shall mean Licensor and any individual or Legal Entity
+ on behalf of whom a Contribution has been received by Licensor and
+ subsequently incorporated within the Work.
+
+ 2. Grant of Copyright License. Subject to the terms and conditions of
+ this License, each Contributor hereby grants to You a perpetual,
+ worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+ copyright license to reproduce, prepare Derivative Works of,
+ publicly display, publicly perform, sublicense, and distribute the
+ Work and such Derivative Works in Source or Object form.
+
+ 3. Grant of Patent License. Subject to the terms and conditions of
+ this License, each Contributor hereby grants to You a perpetual,
+ worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+ (except as stated in this section) patent license to make, have made,
+ use, offer to sell, sell, import, and otherwise transfer the Work,
+ where such license applies only to those patent claims licensable
+ by such Contributor that are necessarily infringed by their
+ Contribution(s) alone or by combination of their Contribution(s)
+ with the Work to which such Contribution(s) was submitted. If You
+ institute patent litigation against any entity (including a
+ cross-claim or counterclaim in a lawsuit) alleging that the Work
+ or a Contribution incorporated within the Work constitutes direct
+ or contributory patent infringement, then any patent licenses
+ granted to You under this License for that Work shall terminate
+ as of the date such litigation is filed.
+
+ 4. Redistribution. You may reproduce and distribute copies of the
+ Work or Derivative Works thereof in any medium, with or without
+ modifications, and in Source or Object form, provided that You
+ meet the following conditions:
+
+ (a) You must give any other recipients of the Work or
+ Derivative Works a copy of this License; and
+
+ (b) You must cause any modified files to carry prominent notices
+ stating that You changed the files; and
+
+ (c) You must retain, in the Source form of any Derivative Works
+ that You distribute, all copyright, patent, trademark, and
+ attribution notices from the Source form of the Work,
+ excluding those notices that do not pertain to any part of
+ the Derivative Works; and
+
+ (d) If the Work includes a "NOTICE" text file as part of its
+ distribution, then any Derivative Works that You distribute must
+ include a readable copy of the attribution notices contained
+ within such NOTICE file, excluding those notices that do not
+ pertain to any part of the Derivative Works, in at least one
+ of the following places: within a NOTICE text file distributed
+ as part of the Derivative Works; within the Source form or
+ documentation, if provided along with the Derivative Works; or,
+ within a display generated by the Derivative Works, if and
+ wherever such third-party notices normally appear. The contents
+ of the NOTICE file are for informational purposes only and
+ do not modify the License. You may add Your own attribution
+ notices within Derivative Works that You distribute, alongside
+ or as an addendum to the NOTICE text from the Work, provided
+ that such additional attribution notices cannot be construed
+ as modifying the License.
+
+ You may add Your own copyright statement to Your modifications and
+ may provide additional or different license terms and conditions
+ for use, reproduction, or distribution of Your modifications, or
+ for any such Derivative Works as a whole, provided Your use,
+ reproduction, and distribution of the Work otherwise complies with
+ the conditions stated in this License.
+
+ 5. Submission of Contributions. Unless You explicitly state otherwise,
+ any Contribution intentionally submitted for inclusion in the Work
+ by You to the Licensor shall be under the terms and conditions of
+ this License, without any additional terms or conditions.
+ Notwithstanding the above, nothing herein shall supersede or modify
+ the terms of any separate license agreement you may have executed
+ with Licensor regarding such Contributions.
+
+ 6. Trademarks. This License does not grant permission to use the trade
+ names, trademarks, service marks, or product names of the Licensor,
+ except as required for reasonable and customary use in describing the
+ origin of the Work and reproducing the content of the NOTICE file.
+
+ 7. Disclaimer of Warranty. Unless required by applicable law or
+ agreed to in writing, Licensor provides the Work (and each
+ Contributor provides its Contributions) on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+ implied, including, without limitation, any warranties or conditions
+ of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+ PARTICULAR PURPOSE. You are solely responsible for determining the
+ appropriateness of using or redistributing the Work and assume any
+ risks associated with Your exercise of permissions under this License.
+
+ 8. Limitation of Liability. In no event and under no legal theory,
+ whether in tort (including negligence), contract, or otherwise,
+ unless required by applicable law (such as deliberate and grossly
+ negligent acts) or agreed to in writing, shall any Contributor be
+ liable to You for damages, including any direct, indirect, special,
+ incidental, or consequential damages of any character arising as a
+ result of this License or out of the use or inability to use the
+ Work (including but not limited to damages for loss of goodwill,
+ work stoppage, computer failure or malfunction, or any and all
+ other commercial damages or losses), even if such Contributor
+ has been advised of the possibility of such damages.
+
+ 9. Accepting Warranty or Additional Liability. While redistributing
+ the Work or Derivative Works thereof, You may choose to offer,
+ and charge a fee for, acceptance of support, warranty, indemnity,
+ or other liability obligations and/or rights consistent with this
+ License. However, in accepting such obligations, You may act only
+ on Your own behalf and on Your sole responsibility, not on behalf
+ of any other Contributor, and only if You agree to indemnify,
+ defend, and hold each Contributor harmless for any liability
+ incurred by, or claims asserted against, such Contributor by reason
+ of your accepting any such warranty or additional liability.
+
+ END OF TERMS AND CONDITIONS
+
+ APPENDIX: How to apply the Apache License to your work.
+
+ To apply the Apache License to your work, attach the following
+ boilerplate notice, with the fields enclosed by brackets "{}"
+ replaced with your own identifying information. (Don't include
+ the brackets!) The text should be enclosed in the appropriate
+ comment syntax for the file format. We also recommend that a
+ file or class name and description of purpose be included on the
+ same "printed page" as the copyright notice for easier
+ identification within third-party archives.
+
+ Copyright {yyyy} {name of copyright owner}
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
diff --git a/src/rp2_common/hardware_adc/CMakeLists.txt b/src/rp2_common/hardware_adc/CMakeLists.txt
new file mode 100644
index 0000000..291428a
--- /dev/null
+++ b/src/rp2_common/hardware_adc/CMakeLists.txt
@@ -0,0 +1,4 @@
+pico_simple_hardware_target(adc)
+
+# additional library
+target_link_libraries(hardware_adc INTERFACE hardware_resets)
\ No newline at end of file
diff --git a/src/rp2_common/hardware_adc/adc.c b/src/rp2_common/hardware_adc/adc.c
new file mode 100644
index 0000000..9058de0
--- /dev/null
+++ b/src/rp2_common/hardware_adc/adc.c
@@ -0,0 +1,23 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "pico.h"
+#include "hardware/adc.h"
+#include "hardware/resets.h"
+
+void adc_init(void) {
+ // ADC is in an unknown state. We should start by resetting it
+ reset_block(RESETS_RESET_ADC_BITS);
+ unreset_block_wait(RESETS_RESET_ADC_BITS);
+
+ // Now turn it back on. Staging of clock etc is handled internally
+ adc_hw->cs = ADC_CS_EN_BITS;
+
+ // Internal staging completes in a few cycles, but poll to be sure
+ while (!(adc_hw->cs & ADC_CS_READY_BITS)) {
+ tight_loop_contents();
+ }
+}
diff --git a/src/rp2_common/hardware_adc/include/hardware/adc.h b/src/rp2_common/hardware_adc/include/hardware/adc.h
new file mode 100644
index 0000000..be82025
--- /dev/null
+++ b/src/rp2_common/hardware_adc/include/hardware/adc.h
@@ -0,0 +1,257 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _HARDWARE_ADC_H_
+#define _HARDWARE_ADC_H_
+
+#include "pico.h"
+#include "hardware/structs/adc.h"
+#include "hardware/gpio.h"
+
+/** \file hardware/adc.h
+ * \defgroup hardware_adc hardware_adc
+ *
+ * Analog to Digital Converter (ADC) API
+ *
+ * The RP2040 has an internal analogue-digital converter (ADC) with the following features:
+ * - SAR ADC
+ * - 500 kS/s (Using an independent 48MHz clock)
+ * - 12 bit (8.7 ENOB)
+ * - 5 input mux:
+ * - 4 inputs that are available on package pins shared with GPIO[29:26]
+ * - 1 input is dedicated to the internal temperature sensor
+ * - 4 element receive sample FIFO
+ * - Interrupt generation
+ * - DMA interface
+ *
+ * Although there is only one ADC you can specify the input to it using the adc_select_input() function.
+ * In round robin mode (adc_set_round_robin()), the ADC will use that input and move to the next one after a read.
+ *
+ * User ADC inputs are on 0-3 (GPIO 26-29), the temperature sensor is on input 4.
+ *
+ * Temperature sensor values can be approximated in centigrade as:
+ *
+ * T = 27 - (ADC_Voltage - 0.706)/0.001721
+ *
+ * The FIFO, if used, can contain up to 4 entries.
+ *
+ * \subsection adc_example Example
+ * \addtogroup hardware_adc
+ *
+ * \include hello_adc.c
+ */
+
+// PICO_CONFIG: PARAM_ASSERTIONS_ENABLED_ADC, Enable/disable assertions in the ADC module, type=bool, default=0, group=hardware_adc
+#ifndef PARAM_ASSERTIONS_ENABLED_ADC
+#define PARAM_ASSERTIONS_ENABLED_ADC 0
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*! \brief Initialise the ADC HW
+ * \ingroup hardware_adc
+ *
+ */
+void adc_init(void);
+
+/*! \brief Initialise the gpio for use as an ADC pin
+ * \ingroup hardware_adc
+ *
+ * Prepare a GPIO for use with ADC by disabling all digital functions.
+ *
+ * \param gpio The GPIO number to use. Allowable GPIO numbers are 26 to 29 inclusive.
+ */
+static inline void adc_gpio_init(uint gpio) {
+ invalid_params_if(ADC, gpio < 26 || gpio > 29);
+ // Select NULL function to make output driver hi-Z
+ gpio_set_function(gpio, GPIO_FUNC_NULL);
+ // Also disable digital pulls and digital receiver
+ gpio_disable_pulls(gpio);
+ gpio_set_input_enabled(gpio, false);
+}
+
+/*! \brief ADC input select
+ * \ingroup hardware_adc
+ *
+ * Select an ADC input. 0...3 are GPIOs 26...29 respectively.
+ * Input 4 is the onboard temperature sensor.
+ *
+ * \param input Input to select.
+ */
+static inline void adc_select_input(uint input) {
+ valid_params_if(ADC, input < NUM_ADC_CHANNELS);
+ hw_write_masked(&adc_hw->cs, input << ADC_CS_AINSEL_LSB, ADC_CS_AINSEL_BITS);
+}
+
+/*! \brief Get the currently selected ADC input channel
+ * \ingroup hardware_adc
+ *
+ * \return The currently selected input channel. 0...3 are GPIOs 26...29 respectively. Input 4 is the onboard temperature sensor.
+ */
+static inline uint adc_get_selected_input(void) {
+ return (adc_hw->cs & ADC_CS_AINSEL_BITS) >> ADC_CS_AINSEL_LSB;
+}
+
+/*! \brief Round Robin sampling selector
+ * \ingroup hardware_adc
+ *
+ * This function sets which inputs are to be run through in round robin mode.
+ * Value between 0 and 0x1f (bit 0 to bit 4 for GPIO 26 to 29 and temperature sensor input respectively)
+ *
+ * \param input_mask A bit pattern indicating which of the 5 inputs are to be sampled. Write a value of 0 to disable round robin sampling.
+ */
+static inline void adc_set_round_robin(uint input_mask) {
+ invalid_params_if(ADC, input_mask & ~ADC_CS_RROBIN_BITS);
+ hw_write_masked(&adc_hw->cs, input_mask << ADC_CS_RROBIN_LSB, ADC_CS_RROBIN_BITS);
+}
+
+/*! \brief Enable the onboard temperature sensor
+ * \ingroup hardware_adc
+ *
+ * \param enable Set true to power on the onboard temperature sensor, false to power off.
+ *
+ */
+static inline void adc_set_temp_sensor_enabled(bool enable) {
+ if (enable)
+ hw_set_bits(&adc_hw->cs, ADC_CS_TS_EN_BITS);
+ else
+ hw_clear_bits(&adc_hw->cs, ADC_CS_TS_EN_BITS);
+}
+
+/*! \brief Perform a single conversion
+ * \ingroup hardware_adc
+ *
+ * Performs an ADC conversion, waits for the result, and then returns it.
+ *
+ * \return Result of the conversion.
+ */
+static inline uint16_t adc_read(void) {
+ hw_set_bits(&adc_hw->cs, ADC_CS_START_ONCE_BITS);
+
+ while (!(adc_hw->cs & ADC_CS_READY_BITS))
+ tight_loop_contents();
+
+ return (uint16_t) adc_hw->result;
+}
+
+/*! \brief Enable or disable free-running sampling mode
+ * \ingroup hardware_adc
+ *
+ * \param run false to disable, true to enable free running conversion mode.
+ */
+static inline void adc_run(bool run) {
+ if (run)
+ hw_set_bits(&adc_hw->cs, ADC_CS_START_MANY_BITS);
+ else
+ hw_clear_bits(&adc_hw->cs, ADC_CS_START_MANY_BITS);
+}
+
+/*! \brief Set the ADC Clock divisor
+ * \ingroup hardware_adc
+ *
+ * Period of samples will be (1 + div) cycles on average. Note it takes 96 cycles to perform a conversion,
+ * so any period less than that will be clamped to 96.
+ *
+ * \param clkdiv If non-zero, conversion will be started at intervals rather than back to back.
+ */
+static inline void adc_set_clkdiv(float clkdiv) {
+ invalid_params_if(ADC, clkdiv >= 1 << (ADC_DIV_INT_MSB - ADC_DIV_INT_LSB + 1));
+ adc_hw->div = (uint32_t)(clkdiv * (float) (1 << ADC_DIV_INT_LSB));
+}
+
+/*! \brief Setup the ADC FIFO
+ * \ingroup hardware_adc
+ *
+ * FIFO is 4 samples long, if a conversion is completed and the FIFO is full, the result is dropped.
+ *
+ * \param en Enables write each conversion result to the FIFO
+ * \param dreq_en Enable DMA requests when FIFO contains data
+ * \param dreq_thresh Threshold for DMA requests/FIFO IRQ if enabled.
+ * \param err_in_fifo If enabled, bit 15 of the FIFO contains error flag for each sample
+ * \param byte_shift Shift FIFO contents to be one byte in size (for byte DMA) - enables DMA to byte buffers.
+ */
+ static inline void adc_fifo_setup(bool en, bool dreq_en, uint16_t dreq_thresh, bool err_in_fifo, bool byte_shift) {
+ hw_write_masked(&adc_hw->fcs,
+ (bool_to_bit(en) << ADC_FCS_EN_LSB) |
+ (bool_to_bit(dreq_en) << ADC_FCS_DREQ_EN_LSB) |
+ (((uint)dreq_thresh) << ADC_FCS_THRESH_LSB) |
+ (bool_to_bit(err_in_fifo) << ADC_FCS_ERR_LSB) |
+ (bool_to_bit(byte_shift) << ADC_FCS_SHIFT_LSB),
+ ADC_FCS_EN_BITS |
+ ADC_FCS_DREQ_EN_BITS |
+ ADC_FCS_THRESH_BITS |
+ ADC_FCS_ERR_BITS |
+ ADC_FCS_SHIFT_BITS
+ );
+}
+
+/*! \brief Check FIFO empty state
+ * \ingroup hardware_adc
+ *
+ * \return Returns true if the FIFO is empty
+ */
+static inline bool adc_fifo_is_empty(void) {
+ return !!(adc_hw->fcs & ADC_FCS_EMPTY_BITS);
+}
+
+/*! \brief Get number of entries in the ADC FIFO
+ * \ingroup hardware_adc
+ *
+ * The ADC FIFO is 4 entries long. This function will return how many samples are currently present.
+ */
+static inline uint8_t adc_fifo_get_level(void) {
+ return (adc_hw->fcs & ADC_FCS_LEVEL_BITS) >> ADC_FCS_LEVEL_LSB;
+}
+
+/*! \brief Get ADC result from FIFO
+ * \ingroup hardware_adc
+ *
+ * Pops the latest result from the ADC FIFO.
+ */
+static inline uint16_t adc_fifo_get(void) {
+ return (uint16_t)adc_hw->fifo;
+}
+
+/*! \brief Wait for the ADC FIFO to have data.
+ * \ingroup hardware_adc
+ *
+ * Blocks until data is present in the FIFO
+ */
+static inline uint16_t adc_fifo_get_blocking(void) {
+ while (adc_fifo_is_empty())
+ tight_loop_contents();
+ return (uint16_t)adc_hw->fifo;
+}
+
+/*! \brief Drain the ADC FIFO
+ * \ingroup hardware_adc
+ *
+ * Will wait for any conversion to complete then drain the FIFO, discarding any results.
+ */
+static inline void adc_fifo_drain(void) {
+ // Potentially there is still a conversion in progress -- wait for this to complete before draining
+ while (!(adc_hw->cs & ADC_CS_READY_BITS))
+ tight_loop_contents();
+ while (!adc_fifo_is_empty())
+ (void) adc_fifo_get();
+}
+
+/*! \brief Enable/Disable ADC interrupts.
+ * \ingroup hardware_adc
+ *
+ * \param enabled Set to true to enable the ADC interrupts, false to disable
+ */
+static inline void adc_irq_set_enabled(bool enabled) {
+ adc_hw->inte = !!enabled;
+}
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/rp2_common/hardware_base/CMakeLists.txt b/src/rp2_common/hardware_base/CMakeLists.txt
new file mode 100644
index 0000000..e045618
--- /dev/null
+++ b/src/rp2_common/hardware_base/CMakeLists.txt
@@ -0,0 +1,3 @@
+add_library(hardware_base INTERFACE)
+target_include_directories(hardware_base INTERFACE ${CMAKE_CURRENT_LIST_DIR}/include)
+target_link_libraries(hardware_base INTERFACE pico_base_headers)
\ No newline at end of file
diff --git a/src/rp2_common/hardware_base/include/hardware/address_mapped.h b/src/rp2_common/hardware_base/include/hardware/address_mapped.h
new file mode 100644
index 0000000..a3b9584
--- /dev/null
+++ b/src/rp2_common/hardware_base/include/hardware/address_mapped.h
@@ -0,0 +1,165 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _HARDWARE_ADDRESS_MAPPED_H
+#define _HARDWARE_ADDRESS_MAPPED_H
+
+#include "pico.h"
+#include "hardware/regs/addressmap.h"
+
+/** \file address_mapped.h
+ * \defgroup hardware_base hardware_base
+ *
+ * Low-level types and (atomic) accessors for memory-mapped hardware registers
+ *
+ * `hardware_base` defines the low level types and access functions for memory mapped hardware registers. It is included
+ * by default by all other hardware libraries.
+ *
+ * The following register access typedefs codify the access type (read/write) and the bus size (8/16/32) of the hardware register.
+ * The register type names are formed by concatenating one from each of the 3 parts A, B, C
+
+ * A | B | C | Meaning
+ * ------|---|---|--------
+ * io_ | | | A Memory mapped IO register
+ * |ro_| | read-only access
+ * |rw_| | read-write access
+ * |wo_| | write-only access (can't actually be enforced via C API)
+ * | | 8| 8-bit wide access
+ * | | 16| 16-bit wide access
+ * | | 32| 32-bit wide access
+ *
+ * When dealing with these types, you will always use a pointer, i.e. `io_rw_32 *some_reg` is a pointer to a read/write
+ * 32 bit register that you can write with `*some_reg = value`, or read with `value = *some_reg`.
+ *
+ * RP2040 hardware is also aliased to provide atomic setting, clear or flipping of a subset of the bits within
+ * a hardware register so that concurrent access by two cores is always consistent with one atomic operation
+ * being performed first, followed by the second.
+ *
+ * See hw_set_bits(), hw_clear_bits() and hw_xor_bits() provide for atomic access via a pointer to a 32 bit register
+ *
+ * Additionally given a pointer to a structure representing a piece of hardware (e.g. `dma_hw_t *dma_hw` for the DMA controller), you can
+ * get an alias to the entire structure such that writing any member (register) within the structure is equivalent
+ * to an atomic operation via hw_set_alias(), hw_clear_alias() or hw_xor_alias()...
+ *
+ * For example `hw_set_alias(dma_hw)->inte1 = 0x80;` will set bit 7 of the INTE1 register of the DMA controller,
+ * leaving the other bits unchanged.
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define check_hw_layout(type, member, offset) static_assert(offsetof(type, member) == (offset), "hw offset mismatch")
+#define check_hw_size(type, size) static_assert(sizeof(type) == (size), "hw size mismatch")
+
+// PICO_CONFIG: PARAM_ASSERTIONS_ENABLED_ADDRESS_ALIAS, Enable/disable assertions in memory address aliasing macros, type=bool, default=0, group=hardware_base
+#ifndef PARAM_ASSERTIONS_ENABLED_ADDRESS_ALIAS
+#define PARAM_ASSERTIONS_ENABLED_ADDRESS_ALIAS 0
+#endif
+
+typedef volatile uint32_t io_rw_32;
+typedef const volatile uint32_t io_ro_32;
+typedef volatile uint32_t io_wo_32;
+typedef volatile uint16_t io_rw_16;
+typedef const volatile uint16_t io_ro_16;
+typedef volatile uint16_t io_wo_16;
+typedef volatile uint8_t io_rw_8;
+typedef const volatile uint8_t io_ro_8;
+typedef volatile uint8_t io_wo_8;
+
+typedef volatile uint8_t *const ioptr;
+typedef ioptr const const_ioptr;
+
+// A non-functional (empty) helper macro to help IDEs follow links from the autogenerated
+// hardware struct headers in hardware/structs/xxx.h to the raw register definitions
+// in hardware/regs/xxx.h. A preprocessor define such as TIMER_TIMEHW_OFFSET (a timer register offset)
+// is not generally clickable (in an IDE) if placed in a C comment, so _REG_(TIMER_TIMEHW_OFFSET) is
+// included outside of a comment instead
+#define _REG_(x)
+
+// Helper method used by hw_alias macros to optionally check input validity
+#define hw_alias_check_addr(addr) ((uintptr_t)(addr))
+// can't use the following impl as it breaks existing static declarations using hw_alias, so would be a backwards incompatibility
+//static __force_inline uint32_t hw_alias_check_addr(volatile void *addr) {
+// uint32_t rc = (uintptr_t)addr;
+// invalid_params_if(ADDRESS_ALIAS, rc < 0x40000000); // catch likely non HW pointer types
+// return rc;
+//}
+
+// Helper method used by xip_alias macros to optionally check input validity
+static __force_inline uint32_t xip_alias_check_addr(const void *addr) {
+ uint32_t rc = (uintptr_t)addr;
+ valid_params_if(ADDRESS_ALIAS, rc >= XIP_MAIN_BASE && rc < XIP_NOALLOC_BASE);
+ return rc;
+}
+
+// Untyped conversion alias pointer generation macros
+#define hw_set_alias_untyped(addr) ((void *)(REG_ALIAS_SET_BITS | hw_alias_check_addr(addr)))
+#define hw_clear_alias_untyped(addr) ((void *)(REG_ALIAS_CLR_BITS | hw_alias_check_addr(addr)))
+#define hw_xor_alias_untyped(addr) ((void *)(REG_ALIAS_XOR_BITS | hw_alias_check_addr(addr)))
+#define xip_noalloc_alias_untyped(addr) ((void *)(XIP_NOALLOC_BASE | xip_alias_check_addr(addr)))
+#define xip_nocache_alias_untyped(addr) ((void *)(XIP_NOCACHE_BASE | xip_alias_check_addr(addr)))
+#define xip_nocache_noalloc_alias_untyped(addr) ((void *)(XIP_NOCACHE_NOALLOC_BASE | xip_alias_check_addr(addr)))
+
+// Typed conversion alias pointer generation macros
+#define hw_set_alias(p) ((typeof(p))hw_set_alias_untyped(p))
+#define hw_clear_alias(p) ((typeof(p))hw_clear_alias_untyped(p))
+#define hw_xor_alias(p) ((typeof(p))hw_xor_alias_untyped(p))
+#define xip_noalloc_alias(p) ((typeof(p))xip_noalloc_alias_untyped(p))
+#define xip_nocache_alias(p) ((typeof(p))xip_nocache_alias_untyped(p))
+#define xip_nocache_noalloc_alias(p) ((typeof(p))xip_nocache_noalloc_alias_untyped(p))
+
+/*! \brief Atomically set the specified bits to 1 in a HW register
+ * \ingroup hardware_base
+ *
+ * \param addr Address of writable register
+ * \param mask Bit-mask specifying bits to set
+ */
+__force_inline static void hw_set_bits(io_rw_32 *addr, uint32_t mask) {
+ *(io_rw_32 *) hw_set_alias_untyped((volatile void *) addr) = mask;
+}
+
+/*! \brief Atomically clear the specified bits to 0 in a HW register
+ * \ingroup hardware_base
+ *
+ * \param addr Address of writable register
+ * \param mask Bit-mask specifying bits to clear
+ */
+__force_inline static void hw_clear_bits(io_rw_32 *addr, uint32_t mask) {
+ *(io_rw_32 *) hw_clear_alias_untyped((volatile void *) addr) = mask;
+}
+
+/*! \brief Atomically flip the specified bits in a HW register
+ * \ingroup hardware_base
+ *
+ * \param addr Address of writable register
+ * \param mask Bit-mask specifying bits to invert
+ */
+__force_inline static void hw_xor_bits(io_rw_32 *addr, uint32_t mask) {
+ *(io_rw_32 *) hw_xor_alias_untyped((volatile void *) addr) = mask;
+}
+
+/*! \brief Set new values for a sub-set of the bits in a HW register
+ * \ingroup hardware_base
+ *
+ * Sets destination bits to values specified in \p values, if and only if corresponding bit in \p write_mask is set
+ *
+ * Note: this method allows safe concurrent modification of *different* bits of
+ * a register, but multiple concurrent access to the same bits is still unsafe.
+ *
+ * \param addr Address of writable register
+ * \param values Bits values
+ * \param write_mask Mask of bits to change
+ */
+__force_inline static void hw_write_masked(io_rw_32 *addr, uint32_t values, uint32_t write_mask) {
+ hw_xor_bits(addr, (*addr ^ values) & write_mask);
+}
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/rp2_common/hardware_claim/CMakeLists.txt b/src/rp2_common/hardware_claim/CMakeLists.txt
new file mode 100644
index 0000000..63f4806
--- /dev/null
+++ b/src/rp2_common/hardware_claim/CMakeLists.txt
@@ -0,0 +1,2 @@
+pico_simple_hardware_target(claim)
+target_link_libraries(hardware_claim INTERFACE hardware_sync)
\ No newline at end of file
diff --git a/src/rp2_common/hardware_claim/claim.c b/src/rp2_common/hardware_claim/claim.c
new file mode 100644
index 0000000..c96764f
--- /dev/null
+++ b/src/rp2_common/hardware_claim/claim.c
@@ -0,0 +1,56 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "hardware/claim.h"
+
+uint32_t hw_claim_lock() {
+ return spin_lock_blocking(spin_lock_instance(PICO_SPINLOCK_ID_HARDWARE_CLAIM));
+}
+
+void hw_claim_unlock(uint32_t save) {
+ spin_unlock(spin_lock_instance(PICO_SPINLOCK_ID_HARDWARE_CLAIM), save);
+}
+
+inline bool hw_is_claimed(const uint8_t *bits, uint bit_index) {
+ return (bits[bit_index >> 3u] & (1u << (bit_index & 7u)));
+}
+
+void hw_claim_or_assert(uint8_t *bits, uint bit_index, const char *message) {
+ uint32_t save = hw_claim_lock();
+ if (hw_is_claimed(bits, bit_index)) {
+ panic(message, bit_index);
+ } else {
+ bits[bit_index >> 3u] |= (uint8_t)(1u << (bit_index & 7u));
+ }
+ hw_claim_unlock(save);
+}
+
+int hw_claim_unused_from_range(uint8_t *bits, bool required, uint bit_lsb, uint bit_msb, const char *message) {
+ // don't bother check lsb / msb order as if wrong, then it'll fail anyway
+ uint32_t save = hw_claim_lock();
+ int found_bit = -1;
+ for(uint bit=bit_lsb; bit <= bit_msb; bit++) {
+ if (!hw_is_claimed(bits, bit)) {
+ bits[bit >> 3u] |= (uint8_t)(1u << (bit & 7u));
+ found_bit = (int)bit;
+ break;
+ }
+ }
+ hw_claim_unlock(save);
+ if (found_bit < 0 && required) {
+ panic(message);
+ }
+ return found_bit;
+}
+
+void hw_claim_clear(uint8_t *bits, uint bit_index) {
+ uint32_t save = hw_claim_lock();
+ assert(hw_is_claimed(bits, bit_index));
+ bits[bit_index >> 3u] &= (uint8_t) ~(1u << (bit_index & 7u));
+ hw_claim_unlock(save);
+}
+
+
diff --git a/src/rp2_common/hardware_claim/include/hardware/claim.h b/src/rp2_common/hardware_claim/include/hardware/claim.h
new file mode 100644
index 0000000..5c93453
--- /dev/null
+++ b/src/rp2_common/hardware_claim/include/hardware/claim.h
@@ -0,0 +1,109 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _HARDWARE_CLAIM_H
+#define _HARDWARE_CLAIM_H
+
+#include "pico.h"
+#include "hardware/sync.h"
+
+/** \file claim.h
+ * \defgroup hardware_claim hardware_claim
+ *
+ * Lightweight hardware resource management
+ *
+ * `hardware_claim` provides a simple API for management of hardware resources at runtime.
+ *
+ * This API is usually called by other hardware specific _claiming_ APIs and provides simple
+ * multi-core safe methods to manipulate compact bit-sets representing hardware resources.
+ *
+ * This API allows any other library to cooperatively participate in a scheme by which
+ * both compile time and runtime allocation of resources can co-exist, and conflicts
+ * can be avoided or detected (depending on the use case) without the libraries having
+ * any other knowledge of each other.
+ *
+ * Facilities are providing for:
+ *
+ * 1. Claiming resources (and asserting if they are already claimed)
+ * 2. Freeing (unclaiming) resources
+ * 3. Finding unused resources
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*! \brief Atomically claim a resource, panicking if it is already in use
+ * \ingroup hardware_claim
+ *
+ * The resource ownership is indicated by the bit_index bit in an array of bits.
+ *
+ * \param bits pointer to an array of bits (8 bits per byte)
+ * \param bit_index resource to claim (bit index into array of bits)
+ * \param message string to display if the bit cannot be claimed; note this may have a single printf format "%d" for the bit
+ */
+void hw_claim_or_assert(uint8_t *bits, uint bit_index, const char *message);
+
+/*! \brief Atomically claim one resource out of a range of resources, optionally asserting if none are free
+ * \ingroup hardware_claim
+ *
+ * \param bits pointer to an array of bits (8 bits per byte)
+ * \param required true if this method should panic if the resource is not free
+ * \param bit_lsb the lower bound (inclusive) of the resource range to claim from
+ * \param bit_msb the upper bound (inclusive) of the resource range to claim from
+ * \param message string to display if the bit cannot be claimed
+ * \return the bit index representing the claimed or -1 if none are available in the range, and required = false
+ */
+int hw_claim_unused_from_range(uint8_t *bits, bool required, uint bit_lsb, uint bit_msb, const char *message);
+
+/*! \brief Determine if a resource is claimed at the time of the call
+ * \ingroup hardware_claim
+ *
+ * The resource ownership is indicated by the bit_index bit in an array of bits.
+ *
+ * \param bits pointer to an array of bits (8 bits per byte)
+ * \param bit_index resource to check (bit index into array of bits)
+ * \return true if the resource is claimed
+ */
+bool hw_is_claimed(const uint8_t *bits, uint bit_index);
+
+/*! \brief Atomically unclaim a resource
+ * \ingroup hardware_claim
+ *
+ * The resource ownership is indicated by the bit_index bit in an array of bits.
+ *
+ * \param bits pointer to an array of bits (8 bits per byte)
+ * \param bit_index resource to unclaim (bit index into array of bits)
+ */
+void hw_claim_clear(uint8_t *bits, uint bit_index);
+
+/*! \brief Acquire the runtime mutual exclusion lock provided by the `hardware_claim` library
+ * \ingroup hardware_claim
+ *
+ * This method is called automatically by the other `hw_claim_` methods, however it is provided as a convenience
+ * to code that might want to protect other hardware initialization code from concurrent use.
+ *
+ * \note hw_claim_lock() uses a spin lock internally, so disables interrupts on the calling core, and will deadlock
+ * if the calling core already owns the lock.
+ *
+ * \return a token to pass to hw_claim_unlock()
+ */
+uint32_t hw_claim_lock(void);
+
+/*! \brief Release the runtime mutual exclusion lock provided by the `hardware_claim` library
+ * \ingroup hardware_claim
+ *
+ * \note This method MUST be called from the same core that call hw_claim_lock()
+ *
+ * \param token the token returned by the corresponding call to hw_claim_lock()
+ */
+void hw_claim_unlock(uint32_t token);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/rp2_common/hardware_clocks/CMakeLists.txt b/src/rp2_common/hardware_clocks/CMakeLists.txt
new file mode 100644
index 0000000..ceb29e2
--- /dev/null
+++ b/src/rp2_common/hardware_clocks/CMakeLists.txt
@@ -0,0 +1,13 @@
+pico_simple_hardware_target(clocks)
+
+target_link_libraries(hardware_clocks INTERFACE
+ hardware_gpio
+ hardware_irq
+ hardware_resets
+ hardware_pll
+ # not currently used by clocks.c, but sensibly bundled here
+ # as changing frequencies may require upping voltage
+ hardware_vreg
+ hardware_watchdog
+ hardware_xosc
+)
\ No newline at end of file
diff --git a/src/rp2_common/hardware_clocks/clocks.c b/src/rp2_common/hardware_clocks/clocks.c
new file mode 100644
index 0000000..f51331a
--- /dev/null
+++ b/src/rp2_common/hardware_clocks/clocks.c
@@ -0,0 +1,390 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "pico.h"
+#include "hardware/regs/clocks.h"
+#include "hardware/platform_defs.h"
+#include "hardware/clocks.h"
+#include "hardware/watchdog.h"
+#include "hardware/pll.h"
+#include "hardware/xosc.h"
+#include "hardware/irq.h"
+#include "hardware/gpio.h"
+
+check_hw_layout(clocks_hw_t, clk[clk_adc].selected, CLOCKS_CLK_ADC_SELECTED_OFFSET);
+check_hw_layout(clocks_hw_t, fc0.result, CLOCKS_FC0_RESULT_OFFSET);
+check_hw_layout(clocks_hw_t, ints, CLOCKS_INTS_OFFSET);
+
+static uint32_t configured_freq[CLK_COUNT];
+
+static resus_callback_t _resus_callback;
+
+// Clock muxing consists of two components:
+// - A glitchless mux, which can be switched freely, but whose inputs must be
+// free-running
+// - An auxiliary (glitchy) mux, whose output glitches when switched, but has
+// no constraints on its inputs
+// Not all clocks have both types of mux.
+static inline bool has_glitchless_mux(enum clock_index clk_index) {
+ return clk_index == clk_sys || clk_index == clk_ref;
+}
+
+void clock_stop(enum clock_index clk_index) {
+ clock_hw_t *clock = &clocks_hw->clk[clk_index];
+ hw_clear_bits(&clock->ctrl, CLOCKS_CLK_USB_CTRL_ENABLE_BITS);
+ configured_freq[clk_index] = 0;
+}
+
+/// \tag::clock_configure[]
+bool clock_configure(enum clock_index clk_index, uint32_t src, uint32_t auxsrc, uint32_t src_freq, uint32_t freq) {
+ uint32_t div;
+
+ assert(src_freq >= freq);
+
+ if (freq > src_freq)
+ return false;
+
+ // Div register is 24.8 int.frac divider so multiply by 2^8 (left shift by 8)
+ div = (uint32_t) (((uint64_t) src_freq << 8) / freq);
+
+ clock_hw_t *clock = &clocks_hw->clk[clk_index];
+
+ // If increasing divisor, set divisor before source. Otherwise set source
+ // before divisor. This avoids a momentary overspeed when e.g. switching
+ // to a faster source and increasing divisor to compensate.
+ if (div > clock->div)
+ clock->div = div;
+
+ // If switching a glitchless slice (ref or sys) to an aux source, switch
+ // away from aux *first* to avoid passing glitches when changing aux mux.
+ // Assume (!!!) glitchless source 0 is no faster than the aux source.
+ if (has_glitchless_mux(clk_index) && src == CLOCKS_CLK_SYS_CTRL_SRC_VALUE_CLKSRC_CLK_SYS_AUX) {
+ hw_clear_bits(&clock->ctrl, CLOCKS_CLK_REF_CTRL_SRC_BITS);
+ while (!(clock->selected & 1u))
+ tight_loop_contents();
+ }
+ // If no glitchless mux, cleanly stop the clock to avoid glitches
+ // propagating when changing aux mux. Note it would be a really bad idea
+ // to do this on one of the glitchless clocks (clk_sys, clk_ref).
+ else {
+ // Disable clock. On clk_ref and clk_sys this does nothing,
+ // all other clocks have the ENABLE bit in the same position.
+ hw_clear_bits(&clock->ctrl, CLOCKS_CLK_GPOUT0_CTRL_ENABLE_BITS);
+ if (configured_freq[clk_index] > 0) {
+ // Delay for 3 cycles of the target clock, for ENABLE propagation.
+ // Note XOSC_COUNT is not helpful here because XOSC is not
+ // necessarily running, nor is timer... so, 3 cycles per loop:
+ uint delay_cyc = configured_freq[clk_sys] / configured_freq[clk_index] + 1;
+ asm volatile (
+ ".syntax unified \n\t"
+ "1: \n\t"
+ "subs %0, #1 \n\t"
+ "bne 1b"
+ : "+r" (delay_cyc)
+ );
+ }
+ }
+
+ // Set aux mux first, and then glitchless mux if this clock has one
+ hw_write_masked(&clock->ctrl,
+ (auxsrc << CLOCKS_CLK_SYS_CTRL_AUXSRC_LSB),
+ CLOCKS_CLK_SYS_CTRL_AUXSRC_BITS
+ );
+
+ if (has_glitchless_mux(clk_index)) {
+ hw_write_masked(&clock->ctrl,
+ src << CLOCKS_CLK_REF_CTRL_SRC_LSB,
+ CLOCKS_CLK_REF_CTRL_SRC_BITS
+ );
+ while (!(clock->selected & (1u << src)))
+ tight_loop_contents();
+ }
+
+ // Enable clock. On clk_ref and clk_sys this does nothing,
+ // all other clocks have the ENABLE bit in the same position.
+ hw_set_bits(&clock->ctrl, CLOCKS_CLK_GPOUT0_CTRL_ENABLE_BITS);
+
+ // Now that the source is configured, we can trust that the user-supplied
+ // divisor is a safe value.
+ clock->div = div;
+
+ // Store the configured frequency
+ configured_freq[clk_index] = (uint32_t)(((uint64_t) src_freq << 8) / div);
+
+ return true;
+}
+/// \end::clock_configure[]
+
+void clocks_init(void) {
+ // Start tick in watchdog
+ watchdog_start_tick(XOSC_MHZ);
+
+ // Everything is 48MHz on FPGA apart from RTC. Otherwise set to 0 and will be set in clock configure
+ if (running_on_fpga()) {
+ for (uint i = 0; i < CLK_COUNT; i++) {
+ configured_freq[i] = 48 * MHZ;
+ }
+ configured_freq[clk_rtc] = 46875;
+ return;
+ }
+
+ // Disable resus that may be enabled from previous software
+ clocks_hw->resus.ctrl = 0;
+
+ // Enable the xosc
+ xosc_init();
+
+ // Before we touch PLLs, switch sys and ref cleanly away from their aux sources.
+ hw_clear_bits(&clocks_hw->clk[clk_sys].ctrl, CLOCKS_CLK_SYS_CTRL_SRC_BITS);
+ while (clocks_hw->clk[clk_sys].selected != 0x1)
+ tight_loop_contents();
+ hw_clear_bits(&clocks_hw->clk[clk_ref].ctrl, CLOCKS_CLK_REF_CTRL_SRC_BITS);
+ while (clocks_hw->clk[clk_ref].selected != 0x1)
+ tight_loop_contents();
+
+ /// \tag::pll_settings[]
+ // Configure PLLs
+ // REF FBDIV VCO POSTDIV
+ // PLL SYS: 12 / 1 = 12MHz * 125 = 1500MHZ / 6 / 2 = 125MHz
+ // PLL USB: 12 / 1 = 12MHz * 40 = 480 MHz / 5 / 2 = 48MHz
+ /// \end::pll_settings[]
+
+ /// \tag::pll_init[]
+ pll_init(pll_sys, 1, 1500 * MHZ, 6, 2);
+ pll_init(pll_usb, 1, 480 * MHZ, 5, 2);
+ /// \end::pll_init[]
+
+ // Configure clocks
+ // CLK_REF = XOSC (12MHz) / 1 = 12MHz
+ clock_configure(clk_ref,
+ CLOCKS_CLK_REF_CTRL_SRC_VALUE_XOSC_CLKSRC,
+ 0, // No aux mux
+ 12 * MHZ,
+ 12 * MHZ);
+
+ /// \tag::configure_clk_sys[]
+ // CLK SYS = PLL SYS (125MHz) / 1 = 125MHz
+ clock_configure(clk_sys,
+ CLOCKS_CLK_SYS_CTRL_SRC_VALUE_CLKSRC_CLK_SYS_AUX,
+ CLOCKS_CLK_SYS_CTRL_AUXSRC_VALUE_CLKSRC_PLL_SYS,
+ 125 * MHZ,
+ 125 * MHZ);
+ /// \end::configure_clk_sys[]
+
+ // CLK USB = PLL USB (48MHz) / 1 = 48MHz
+ clock_configure(clk_usb,
+ 0, // No GLMUX
+ CLOCKS_CLK_USB_CTRL_AUXSRC_VALUE_CLKSRC_PLL_USB,
+ 48 * MHZ,
+ 48 * MHZ);
+
+ // CLK ADC = PLL USB (48MHZ) / 1 = 48MHz
+ clock_configure(clk_adc,
+ 0, // No GLMUX
+ CLOCKS_CLK_ADC_CTRL_AUXSRC_VALUE_CLKSRC_PLL_USB,
+ 48 * MHZ,
+ 48 * MHZ);
+
+ // CLK RTC = PLL USB (48MHz) / 1024 = 46875Hz
+ clock_configure(clk_rtc,
+ 0, // No GLMUX
+ CLOCKS_CLK_RTC_CTRL_AUXSRC_VALUE_CLKSRC_PLL_USB,
+ 48 * MHZ,
+ 46875);
+
+ // CLK PERI = clk_sys. Used as reference clock for Peripherals. No dividers so just select and enable
+ // Normally choose clk_sys or clk_usb
+ clock_configure(clk_peri,
+ 0,
+ CLOCKS_CLK_PERI_CTRL_AUXSRC_VALUE_CLK_SYS,
+ 125 * MHZ,
+ 125 * MHZ);
+}
+
+/// \tag::clock_get_hz[]
+uint32_t clock_get_hz(enum clock_index clk_index) {
+ return configured_freq[clk_index];
+}
+/// \end::clock_get_hz[]
+
+void clock_set_reported_hz(enum clock_index clk_index, uint hz) {
+ configured_freq[clk_index] = hz;
+}
+
+/// \tag::frequency_count_khz[]
+uint32_t frequency_count_khz(uint src) {
+ fc_hw_t *fc = &clocks_hw->fc0;
+
+ // If frequency counter is running need to wait for it. It runs even if the source is NULL
+ while(fc->status & CLOCKS_FC0_STATUS_RUNNING_BITS) {
+ tight_loop_contents();
+ }
+
+ // Set reference freq
+ fc->ref_khz = clock_get_hz(clk_ref) / 1000;
+
+ // FIXME: Don't pick random interval. Use best interval
+ fc->interval = 10;
+
+ // No min or max
+ fc->min_khz = 0;
+ fc->max_khz = 0xffffffff;
+
+ // Set SRC which automatically starts the measurement
+ fc->src = src;
+
+ while(!(fc->status & CLOCKS_FC0_STATUS_DONE_BITS)) {
+ tight_loop_contents();
+ }
+
+ // Return the result
+ return fc->result >> CLOCKS_FC0_RESULT_KHZ_LSB;
+}
+/// \end::frequency_count_khz[]
+
+static void clocks_handle_resus(void) {
+ // Set clk_sys back to the ref clock rather than it being forced to clk_ref
+ // by resus. Call the user's resus callback if they have set one
+
+ // CLK SYS = CLK_REF. Must be running for this code to be running
+ uint clk_ref_freq = clock_get_hz(clk_ref);
+ clock_configure(clk_sys,
+ CLOCKS_CLK_SYS_CTRL_SRC_VALUE_CLK_REF,
+ 0,
+ clk_ref_freq,
+ clk_ref_freq);
+
+ // Assert we have been resussed
+ assert(clocks_hw->resus.status & CLOCKS_CLK_SYS_RESUS_STATUS_RESUSSED_BITS);
+
+ // Now we have fixed clk_sys we can safely remove the resus
+ hw_set_bits(&clocks_hw->resus.ctrl, CLOCKS_CLK_SYS_RESUS_CTRL_CLEAR_BITS);
+ hw_clear_bits(&clocks_hw->resus.ctrl, CLOCKS_CLK_SYS_RESUS_CTRL_CLEAR_BITS);
+
+ // Now we should no longer be resussed
+ assert(!(clocks_hw->resus.status & CLOCKS_CLK_SYS_RESUS_STATUS_RESUSSED_BITS));
+
+ // Call the user's callback to notify them of the resus event
+ if (_resus_callback) {
+ _resus_callback();
+ }
+}
+
+static void clocks_irq_handler(void) {
+ // Clocks interrupt handler. Only resus but handle irq
+ // defensively just in case.
+ uint32_t ints = clocks_hw->ints;
+
+ if (ints & CLOCKS_INTE_CLK_SYS_RESUS_BITS) {
+ ints &= ~CLOCKS_INTE_CLK_SYS_RESUS_BITS;
+ clocks_handle_resus();
+ }
+
+#ifndef NDEBUG
+ if (ints) {
+ panic("Unexpected clocks irq\n");
+ }
+#endif
+}
+
+void clocks_enable_resus(resus_callback_t resus_callback) {
+ // Restart clk_sys if it is stopped by forcing it
+ // to the default source of clk_ref. If clk_ref stops running this will
+ // not work.
+
+ // Store user's resus callback
+ _resus_callback = resus_callback;
+
+ irq_set_exclusive_handler(CLOCKS_IRQ, clocks_irq_handler);
+
+ // Enable the resus interrupt in clocks
+ clocks_hw->inte = CLOCKS_INTE_CLK_SYS_RESUS_BITS;
+
+ // Enable the clocks irq
+ irq_set_enabled(CLOCKS_IRQ, true);
+
+ // 2 * clk_ref freq / clk_sys_min_freq;
+ // assume clk_ref is 3MHz and we want clk_sys to be no lower than 1MHz
+ uint timeout = 2 * 3 * 1;
+
+ // Enable resus with the maximum timeout
+ clocks_hw->resus.ctrl = CLOCKS_CLK_SYS_RESUS_CTRL_ENABLE_BITS | timeout;
+}
+
+void clock_gpio_init(uint gpio, uint src, uint div) {
+ // Bit messy but it's as much code to loop through a lookup
+ // table. The sources for each gpout generators are the same
+ // so just call with the sources from GP0
+ uint gpclk = 0;
+ if (gpio == 21) gpclk = clk_gpout0;
+ else if (gpio == 23) gpclk = clk_gpout1;
+ else if (gpio == 24) gpclk = clk_gpout2;
+ else if (gpio == 25) gpclk = clk_gpout3;
+ else {
+ invalid_params_if(CLOCKS, true);
+ }
+
+ // Set up the gpclk generator
+ clocks_hw->clk[gpclk].ctrl = (src << CLOCKS_CLK_GPOUT0_CTRL_AUXSRC_LSB) |
+ CLOCKS_CLK_GPOUT0_CTRL_ENABLE_BITS;
+ clocks_hw->clk[gpclk].div = div << CLOCKS_CLK_GPOUT0_DIV_INT_LSB;
+
+ // Set gpio pin to gpclock function
+ gpio_set_function(gpio, GPIO_FUNC_GPCK);
+}
+
+static const uint8_t gpin0_src[CLK_COUNT] = {
+ CLOCKS_CLK_GPOUT0_CTRL_AUXSRC_VALUE_CLKSRC_GPIN0, // CLK_GPOUT0
+ CLOCKS_CLK_GPOUT1_CTRL_AUXSRC_VALUE_CLKSRC_GPIN0, // CLK_GPOUT1
+ CLOCKS_CLK_GPOUT2_CTRL_AUXSRC_VALUE_CLKSRC_GPIN0, // CLK_GPOUT2
+ CLOCKS_CLK_GPOUT3_CTRL_AUXSRC_VALUE_CLKSRC_GPIN0, // CLK_GPOUT3
+ CLOCKS_CLK_REF_CTRL_AUXSRC_VALUE_CLKSRC_GPIN0, // CLK_REF
+ CLOCKS_CLK_SYS_CTRL_AUXSRC_VALUE_CLKSRC_GPIN0, // CLK_SYS
+ CLOCKS_CLK_PERI_CTRL_AUXSRC_VALUE_CLKSRC_GPIN0, // CLK_PERI
+ CLOCKS_CLK_USB_CTRL_AUXSRC_VALUE_CLKSRC_GPIN0, // CLK_USB
+ CLOCKS_CLK_ADC_CTRL_AUXSRC_VALUE_CLKSRC_GPIN0, // CLK_ADC
+ CLOCKS_CLK_RTC_CTRL_AUXSRC_VALUE_CLKSRC_GPIN0, // CLK_RTC
+};
+
+// Assert GPIN1 is GPIN0 + 1
+static_assert(CLOCKS_CLK_GPOUT0_CTRL_AUXSRC_VALUE_CLKSRC_GPIN1 == (CLOCKS_CLK_GPOUT0_CTRL_AUXSRC_VALUE_CLKSRC_GPIN0 + 1), "hw mismatch");
+static_assert(CLOCKS_CLK_GPOUT1_CTRL_AUXSRC_VALUE_CLKSRC_GPIN1 == (CLOCKS_CLK_GPOUT1_CTRL_AUXSRC_VALUE_CLKSRC_GPIN0 + 1), "hw mismatch");
+static_assert(CLOCKS_CLK_GPOUT2_CTRL_AUXSRC_VALUE_CLKSRC_GPIN1 == (CLOCKS_CLK_GPOUT2_CTRL_AUXSRC_VALUE_CLKSRC_GPIN0 + 1), "hw mismatch");
+static_assert(CLOCKS_CLK_GPOUT3_CTRL_AUXSRC_VALUE_CLKSRC_GPIN1 == (CLOCKS_CLK_GPOUT3_CTRL_AUXSRC_VALUE_CLKSRC_GPIN0 + 1), "hw mismatch");
+static_assert(CLOCKS_CLK_REF_CTRL_AUXSRC_VALUE_CLKSRC_GPIN1 == (CLOCKS_CLK_REF_CTRL_AUXSRC_VALUE_CLKSRC_GPIN0 + 1), "hw mismatch");
+static_assert(CLOCKS_CLK_SYS_CTRL_AUXSRC_VALUE_CLKSRC_GPIN1 == (CLOCKS_CLK_SYS_CTRL_AUXSRC_VALUE_CLKSRC_GPIN0 + 1), "hw mismatch");
+static_assert(CLOCKS_CLK_PERI_CTRL_AUXSRC_VALUE_CLKSRC_GPIN1 == (CLOCKS_CLK_PERI_CTRL_AUXSRC_VALUE_CLKSRC_GPIN0 + 1), "hw mismatch");
+static_assert(CLOCKS_CLK_USB_CTRL_AUXSRC_VALUE_CLKSRC_GPIN1 == (CLOCKS_CLK_USB_CTRL_AUXSRC_VALUE_CLKSRC_GPIN0 + 1), "hw mismatch");
+static_assert(CLOCKS_CLK_ADC_CTRL_AUXSRC_VALUE_CLKSRC_GPIN1 == (CLOCKS_CLK_ADC_CTRL_AUXSRC_VALUE_CLKSRC_GPIN0 + 1), "hw mismatch");
+static_assert(CLOCKS_CLK_RTC_CTRL_AUXSRC_VALUE_CLKSRC_GPIN1 == (CLOCKS_CLK_RTC_CTRL_AUXSRC_VALUE_CLKSRC_GPIN0 + 1), "hw mismatch");
+
+bool clock_configure_gpin(enum clock_index clk_index, uint gpio, uint32_t src_freq, uint32_t freq) {
+ // Configure a clock to run from a GPIO input
+ uint gpin = 0;
+ if (gpio == 20) gpin = 0;
+ else if (gpio == 22) gpin = 1;
+ else {
+ invalid_params_if(CLOCKS, true);
+ }
+
+ // Work out sources. GPIN is always an auxsrc
+ uint src = 0;
+
+ // GPIN1 == GPIN0 + 1
+ uint auxsrc = gpin0_src[clk_index] + gpin;
+
+ if (has_glitchless_mux(clk_index)) {
+ // AUX src is always 1
+ src = 1;
+ }
+
+ // Set the GPIO function
+ gpio_set_function(gpio, GPIO_FUNC_GPCK);
+
+ // Now we have the src, auxsrc, and configured the gpio input
+ // call clock configure to run the clock from a gpio
+ return clock_configure(clk_index, src, auxsrc, src_freq, freq);
+}
diff --git a/src/rp2_common/hardware_clocks/include/hardware/clocks.h b/src/rp2_common/hardware_clocks/include/hardware/clocks.h
new file mode 100644
index 0000000..04d373d
--- /dev/null
+++ b/src/rp2_common/hardware_clocks/include/hardware/clocks.h
@@ -0,0 +1,194 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _HARDWARE_CLOCKS_H_
+#define _HARDWARE_CLOCKS_H_
+
+#include "pico.h"
+#include "hardware/structs/clocks.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** \file hardware/clocks.h
+ * \defgroup hardware_clocks hardware_clocks
+ *
+ * Clock Management API
+ *
+ * This API provides a high level interface to the clock functions.
+ *
+ * The clocks block provides independent clocks to on-chip and external components. It takes inputs from a variety of clock
+ * sources allowing the user to trade off performance against cost, board area and power consumption. From these sources
+ * it uses multiple clock generators to provide the required clocks. This architecture allows the user flexibility to start and
+ * stop clocks independently and to vary some clock frequencies whilst maintaining others at their optimum frequencies
+ *
+ * Please refer to the datasheet for more details on the RP2040 clocks.
+ *
+ * The clock source depends on which clock you are attempting to configure. The first table below shows main clock sources. If
+ * you are not setting the Reference clock or the System clock, or you are specifying that one of those two will be using an auxiliary
+ * clock source, then you will need to use one of the entries from the subsequent tables.
+ *
+ * **Main Clock Sources**
+ *
+ * Source | Reference Clock | System Clock
+ * -------|-----------------|---------
+ * ROSC | CLOCKS_CLK_REF_CTRL_SRC_VALUE_ROSC_CLKSRC_PH | |
+ * Auxiliary | CLOCKS_CLK_REF_CTRL_SRC_VALUE_CLKSRC_CLK_REF_AUX | CLOCKS_CLK_SYS_CTRL_SRC_VALUE_CLKSRC_CLK_SYS_AUX
+ * XOSC | CLOCKS_CLK_REF_CTRL_SRC_VALUE_XOSC_CLKSRC | |
+ * Reference | | CLOCKS_CLK_SYS_CTRL_SRC_VALUE_CLK_REF
+ *
+ * **Auxiliary Clock Sources**
+ *
+ * The auxiliary clock sources available for use in the configure function depend on which clock is being configured. The following table
+ * describes the available values that can be used. Note that for clk_gpout[x], x can be 0-3.
+ *
+ *
+ * Aux Source | clk_gpout[x] | clk_ref | clk_sys
+ * -----------|------------|---------|--------
+ * System PLL | CLOCKS_CLK_GPOUTx_CTRL_AUXSRC_VALUE_CLKSRC_PLL_SYS | | CLOCKS_CLK_SYS_CTRL_AUXSRC_VALUE_CLKSRC_PLL_SYS
+ * GPIO in 0 | CLOCKS_CLK_GPOUTx_CTRL_AUXSRC_VALUE_CLKSRC_GPIN0 | CLOCKS_CLK_REF_CTRL_AUXSRC_VALUE_CLKSRC_GPIN0 | CLOCKS_CLK_SYS_CTRL_AUXSRC_VALUE_CLKSRC_GPIN0
+ * GPIO in 1 | CLOCKS_CLK_GPOUTx_CTRL_AUXSRC_VALUE_CLKSRC_GPIN1 | CLOCKS_CLK_REF_CTRL_AUXSRC_VALUE_CLKSRC_GPIN1 | CLOCKS_CLK_SYS_CTRL_AUXSRC_VALUE_CLKSRC_GPIN1
+ * USB PLL | CLOCKS_CLK_GPOUTx_CTRL_AUXSRC_VALUE_CLKSRC_PLL_USB | CLOCKS_CLK_REF_CTRL_AUXSRC_VALUE_CLKSRC_PLL_USB| CLOCKS_CLK_SYS_CTRL_AUXSRC_VALUE_CLKSRC_PLL_USB
+ * ROSC | CLOCKS_CLK_GPOUTx_CTRL_AUXSRC_VALUE_ROSC_CLKSRC | | CLOCKS_CLK_SYS_CTRL_AUXSRC_VALUE_ROSC_CLKSRC
+ * XOSC | CLOCKS_CLK_GPOUTx_CTRL_AUXSRC_VALUE_XOSC_CLKSRC | | CLOCKS_CLK_SYS_CTRL_AUXSRC_VALUE_ROSC_CLKSRC
+ * System clock | CLOCKS_CLK_GPOUTx_CTRL_AUXSRC_VALUE_CLK_SYS | | |
+ * USB Clock | CLOCKS_CLK_GPOUTx_CTRL_AUXSRC_VALUE_CLK_USB | | |
+ * ADC clock | CLOCKS_CLK_GPOUTx_CTRL_AUXSRC_VALUE_CLK_ADC | | |
+ * RTC Clock | CLOCKS_CLK_GPOUTx_CTRL_AUXSRC_VALUE_CLK_RTC | | |
+ * Ref clock | CLOCKS_CLK_GPOUTx_CTRL_AUXSRC_VALUE_CLK_REF | | |
+ *
+ * Aux Source | clk_peri | clk_usb | clk_adc
+ * -----------|-----------|---------|--------
+ * System PLL | CLOCKS_CLK_PERI_CTRL_AUXSRC_VALUE_CLKSRC_PLL_SYS | CLOCKS_CLK_USB_CTRL_AUXSRC_VALUE_CLKSRC_PLL_SYS | CLOCKS_CLK_ADC_CTRL_AUXSRC_VALUE_CLKSRC_PLL_SYS
+ * GPIO in 0 | CLOCKS_CLK_PERI_CTRL_AUXSRC_VALUE_CLKSRC_GPIN0 | CLOCKS_CLK_USB_CTRL_AUXSRC_VALUE_CLKSRC_GPIN0 | CLOCKS_CLK_ADC_CTRL_AUXSRC_VALUE_CLKSRC_GPIN0
+ * GPIO in 1 | CLOCKS_CLK_PERI_CTRL_AUXSRC_VALUE_CLKSRC_GPIN1 | CLOCKS_CLK_USB_CTRL_AUXSRC_VALUE_CLKSRC_GPIN1 | CLOCKS_CLK_ADC_CTRL_AUXSRC_VALUE_CLKSRC_GPIN1
+ * USB PLL | CLOCKS_CLK_PERI_CTRL_AUXSRC_VALUE_CLKSRC_PLL_USB | CLOCKS_CLK_USB_CTRL_AUXSRC_VALUE_CLKSRC_PLL_USB | CLOCKS_CLK_ADC_CTRL_AUXSRC_VALUE_CLKSRC_PLL_USB
+ * ROSC | CLOCKS_CLK_PERI_CTRL_AUXSRC_VALUE_ROSC_CLKSRC_PH | CLOCKS_CLK_USB_CTRL_AUXSRC_VALUE_ROSC_CLKSRC_PH | CLOCKS_CLK_ADC_CTRL_AUXSRC_VALUE_ROSC_CLKSRC_PH
+ * XOSC | CLOCKS_CLK_PERI_CTRL_AUXSRC_VALUE_XOSC_CLKSRC | CLOCKS_CLK_USB_CTRL_AUXSRC_VALUE_XOSC_CLKSRC | CLOCKS_CLK_ADC_CTRL_AUXSRC_VALUE_XOSC_CLKSRC
+ * System clock | CLOCKS_CLK_PERI_CTRL_AUXSRC_VALUE_CLK_SYS | | |
+ *
+ * Aux Source | clk_rtc
+ * -----------|----------
+ * System PLL | CLOCKS_CLK_RTC_CTRL_AUXSRC_VALUE_CLKSRC_PLL_SYS
+ * GPIO in 0 | CLOCKS_CLK_RTC_CTRL_AUXSRC_VALUE_CLKSRC_GPIN0
+ * GPIO in 1 | CLOCKS_CLK_RTC_CTRL_AUXSRC_VALUE_CLKSRC_GPIN1
+ * USB PLL | CLOCKS_CLK_RTC_CTRL_AUXSRC_VALUE_CLKSRC_PLL_USB
+ * ROSC | CLOCKS_CLK_RTC_CTRL_AUXSRC_VALUE_ROSC_CLKSRC_PH
+ * XOSC | CLOCKS_CLK_RTC_CTRL_AUXSRC_VALUE_XOSC_CLKSRC
+
+ *
+ * \section clock_example Example
+ * \addtogroup hardware_clocks
+ * \include hello_48MHz.c
+ */
+
+#define KHZ 1000
+#define MHZ 1000000
+
+// PICO_CONFIG: PARAM_ASSERTIONS_ENABLED_CLOCKS, Enable/disable assertions in the clocks module, type=bool, default=0, group=hardware_clocks
+#ifndef PARAM_ASSERTIONS_ENABLED_CLOCKS
+#define PARAM_ASSERTIONS_ENABLED_CLOCKS 0
+#endif
+
+/*! \brief Initialise the clock hardware
+ * \ingroup hardware_clocks
+ *
+ * Must be called before any other clock function.
+ */
+void clocks_init(void);
+
+/*! \brief Configure the specified clock
+ * \ingroup hardware_clocks
+ *
+ * See the tables in the description for details on the possible values for clock sources.
+ *
+ * \param clk_index The clock to configure
+ * \param src The main clock source, can be 0.
+ * \param auxsrc The auxiliary clock source, which depends on which clock is being set. Can be 0
+ * \param src_freq Frequency of the input clock source
+ * \param freq Requested frequency
+ */
+bool clock_configure(enum clock_index clk_index, uint32_t src, uint32_t auxsrc, uint32_t src_freq, uint32_t freq);
+
+/*! \brief Stop the specified clock
+ * \ingroup hardware_clocks
+ *
+ * \param clk_index The clock to stop
+ */
+void clock_stop(enum clock_index clk_index);
+
+/*! \brief Get the current frequency of the specified clock
+ * \ingroup hardware_clocks
+ *
+ * \param clk_index Clock
+ * \return Clock frequency in Hz
+ */
+uint32_t clock_get_hz(enum clock_index clk_index);
+
+/*! \brief Measure a clocks frequency using the Frequency counter.
+ * \ingroup hardware_clocks
+ *
+ * Uses the inbuilt frequency counter to measure the specified clocks frequency.
+ * Currently, this function is accurate to +-1KHz. See the datasheet for more details.
+ */
+uint32_t frequency_count_khz(uint src);
+
+/*! \brief Set the "current frequency" of the clock as reported by clock_get_hz without actually changing the clock
+ * \ingroup hardware_clocks
+ *
+ * \see clock_get_hz()
+ */
+void clock_set_reported_hz(enum clock_index clk_index, uint hz);
+
+/// \tag::frequency_count_mhz[]
+static inline float frequency_count_mhz(uint src) {
+ return ((float) (frequency_count_khz(src))) / KHZ;
+}
+/// \end::frequency_count_mhz[]
+
+/*! \brief Resus callback function type.
+ * \ingroup hardware_clocks
+ *
+ * User provided callback for a resus event (when clk_sys is stopped by the programmer and is restarted for them).
+ */
+typedef void (*resus_callback_t)(void);
+
+/*! \brief Enable the resus function. Restarts clk_sys if it is accidentally stopped.
+ * \ingroup hardware_clocks
+ *
+ * The resuscitate function will restart the system clock if it falls below a certain speed (or stops). This
+ * could happen if the clock source the system clock is running from stops. For example if a PLL is stopped.
+ *
+ * \param resus_callback a function pointer provided by the user to call if a resus event happens.
+ */
+void clocks_enable_resus(resus_callback_t resus_callback);
+
+/*! \brief Output an optionally divided clock to the specified gpio pin.
+ * \ingroup hardware_clocks
+ *
+ * \param gpio The GPIO pin to output the clock to. Valid GPIOs are: 21, 23, 24, 25. These GPIOs are connected to the GPOUT0-3 clock generators.
+ * \param src The source clock. See the register field CLOCKS_CLK_GPOUT0_CTRL_AUXSRC for a full list. The list is the same for each GPOUT clock generator.
+ * \param div The amount to divide the source clock by. This is useful to not overwhelm the GPIO pin with a fast clock.
+ */
+void clock_gpio_init(uint gpio, uint src, uint div);
+
+/*! \brief Configure a clock to come from a gpio input
+ * \ingroup hardware_clocks
+ *
+ * \param clk_index The clock to configure
+ * \param gpio The GPIO pin to run the clock from. Valid GPIOs are: 20 and 22.
+ * \param src_freq Frequency of the input clock source
+ * \param freq Requested frequency
+ */
+bool clock_configure_gpin(enum clock_index clk_index, uint gpio, uint32_t src_freq, uint32_t freq);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/rp2_common/hardware_clocks/scripts/vcocalc.py b/src/rp2_common/hardware_clocks/scripts/vcocalc.py
new file mode 100755
index 0000000..4d90146
--- /dev/null
+++ b/src/rp2_common/hardware_clocks/scripts/vcocalc.py
@@ -0,0 +1,37 @@
+#!/usr/bin/env python3
+
+import argparse
+
+parser = argparse.ArgumentParser(description="PLL parameter calculator")
+parser.add_argument("--input", "-i", default=12, help="Input (reference) frequency. Default 12 MHz", type=float)
+parser.add_argument("--vco-max", default=1600, help="Override maximum VCO frequency. Default 1600 MHz", type=float)
+parser.add_argument("--vco-min", default=400, help="Override minimum VCO frequency. Default 400 MHz", type=float)
+parser.add_argument("--low-vco", "-l", action="store_true", help="Use a lower VCO frequency when possible. This reduces power consumption, at the cost of increased jitter")
+parser.add_argument("output", help="Output frequency in MHz.", type=float)
+args = parser.parse_args()
+
+# Fixed hardware parameters
+fbdiv_range = range(16, 320 + 1)
+postdiv_range = range(1, 7 + 1)
+
+best = (0, 0, 0, 0)
+best_margin = args.output
+
+for fbdiv in (fbdiv_range if args.low_vco else reversed(fbdiv_range)):
+ vco = args.input * fbdiv
+ if vco < args.vco_min or vco > args.vco_max:
+ continue
+ # pd1 is inner loop so that we prefer higher ratios of pd1:pd2
+ for pd2 in postdiv_range:
+ for pd1 in postdiv_range:
+ out = vco / pd1 / pd2
+ margin = abs(out - args.output)
+ if margin < best_margin:
+ best = (out, fbdiv, pd1, pd2)
+ best_margin = margin
+
+print("Requested: {} MHz".format(args.output))
+print("Achieved: {} MHz".format(best[0]))
+print("FBDIV: {} (VCO = {} MHz)".format(best[1], args.input * best[1]))
+print("PD1: {}".format(best[2]))
+print("PD2: {}".format(best[3]))
diff --git a/src/rp2_common/hardware_divider/CMakeLists.txt b/src/rp2_common/hardware_divider/CMakeLists.txt
new file mode 100644
index 0000000..296a1ef
--- /dev/null
+++ b/src/rp2_common/hardware_divider/CMakeLists.txt
@@ -0,0 +1,3 @@
+pico_simple_hardware_headers_only_target(divider)
+target_sources(hardware_divider INTERFACE ${CMAKE_CURRENT_LIST_DIR}/divider.S)
+target_link_libraries(hardware_divider INTERFACE hardware_structs)
\ No newline at end of file
diff --git a/src/rp2_common/hardware_divider/divider.S b/src/rp2_common/hardware_divider/divider.S
new file mode 100644
index 0000000..b9389c5
--- /dev/null
+++ b/src/rp2_common/hardware_divider/divider.S
@@ -0,0 +1,76 @@
+#include "pico/asm_helper.S"
+#include "hardware/regs/addressmap.h"
+#include "hardware/regs/sio.h"
+
+.syntax unified
+.cpu cortex-m0plus
+.thumb
+
+// tag::hw_div_s32[]
+
+.macro __divider_delay
+ // delay 8 cycles
+ b 1f
+1: b 1f
+1: b 1f
+1: b 1f
+1:
+.endm
+
+.align 2
+
+regular_func_with_section hw_divider_divmod_s32
+ ldr r3, =(SIO_BASE)
+ str r0, [r3, #SIO_DIV_SDIVIDEND_OFFSET]
+ str r1, [r3, #SIO_DIV_SDIVISOR_OFFSET]
+ __divider_delay
+ // return 64 bit value so we can efficiently return both (note quotient must be read last)
+ ldr r1, [r3, #SIO_DIV_REMAINDER_OFFSET]
+ ldr r0, [r3, #SIO_DIV_QUOTIENT_OFFSET]
+ bx lr
+// end::hw_div_s32[]
+
+.align 2
+
+// tag::hw_div_u32[]
+regular_func_with_section hw_divider_divmod_u32
+ ldr r3, =(SIO_BASE)
+ str r0, [r3, #SIO_DIV_UDIVIDEND_OFFSET]
+ str r1, [r3, #SIO_DIV_UDIVISOR_OFFSET]
+ __divider_delay
+ // return 64 bit value so we can efficiently return both (note quotient must be read last)
+ ldr r1, [r3, #SIO_DIV_REMAINDER_OFFSET]
+ ldr r0, [r3, #SIO_DIV_QUOTIENT_OFFSET]
+ bx lr
+// end::hw_div_u32[]
+
+#if SIO_DIV_CSR_READY_LSB == 0
+.equ SIO_DIV_CSR_READY_SHIFT_FOR_CARRY, 1
+#else
+#error need to change SHIFT above
+#endif
+
+regular_func_with_section hw_divider_save_state
+ push {r4, r5, lr}
+ ldr r5, =SIO_BASE
+ ldr r4, [r5, #SIO_DIV_CSR_OFFSET]
+ # wait for results as we can't save signed-ness of operation
+1:
+ lsrs r4, #SIO_DIV_CSR_READY_SHIFT_FOR_CARRY
+ bcc 1b
+ ldr r1, [r5, #SIO_DIV_UDIVIDEND_OFFSET]
+ ldr r2, [r5, #SIO_DIV_UDIVISOR_OFFSET]
+ ldr r3, [r5, #SIO_DIV_REMAINDER_OFFSET]
+ ldr r4, [r5, #SIO_DIV_QUOTIENT_OFFSET]
+ stmia r0!, {r1-r4}
+ pop {r4, r5, pc}
+
+regular_func_with_section hw_divider_restore_state
+ push {r4, r5, lr}
+ ldr r5, =SIO_BASE
+ ldmia r0!, {r1-r4}
+ str r1, [r5, #SIO_DIV_UDIVIDEND_OFFSET]
+ str r2, [r5, #SIO_DIV_UDIVISOR_OFFSET]
+ str r3, [r5, #SIO_DIV_REMAINDER_OFFSET]
+ str r4, [r5, #SIO_DIV_QUOTIENT_OFFSET]
+ pop {r4, r5, pc}
diff --git a/src/rp2_common/hardware_divider/include/hardware/divider.h b/src/rp2_common/hardware_divider/include/hardware/divider.h
new file mode 100644
index 0000000..0be2ef8
--- /dev/null
+++ b/src/rp2_common/hardware_divider/include/hardware/divider.h
@@ -0,0 +1,403 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _HARDWARE_DIVIDER_H
+#define _HARDWARE_DIVIDER_H
+
+#include "pico.h"
+#include "hardware/structs/sio.h"
+
+/** \file hardware/divider.h
+ * \defgroup hardware_divider hardware_divider
+ *
+ * Low-level hardware-divider access
+ *
+ * The SIO contains an 8-cycle signed/unsigned divide/modulo circuit, per core. Calculation is started by writing a dividend
+ * and divisor to the two argument registers, DIVIDEND and DIVISOR. The divider calculates the quotient / and remainder % of
+ * this division over the next 8 cycles, and on the 9th cycle the results can be read from the two result registers
+ * DIV_QUOTIENT and DIV_REMAINDER. A 'ready' bit in register DIV_CSR can be polled to wait for the calculation to
+ * complete, or software can insert a fixed 8-cycle delay
+ *
+ * This header provides low level macros and inline functions for accessing the hardware dividers directly,
+ * and perhaps most usefully performing asynchronous divides. These functions however do not follow the regular
+ * SDK conventions for saving/restoring the divider state, so are not generally safe to call from interrupt handlers
+ *
+ * The pico_divider library provides a more user friendly set of APIs over the divider (and support for
+ * 64 bit divides), and of course by default regular C language integer divisions are redirected through that library, meaning
+ * you can just use C level `/` and `%` operators and gain the benefits of the fast hardware divider.
+ *
+ * @see pico_divider
+ *
+ * \subsection divider_example Example
+ * \addtogroup hardware_divider
+ * \include hello_divider.c
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef uint64_t divmod_result_t;
+
+/*! \brief Start a signed asynchronous divide
+ * \ingroup hardware_divider
+ *
+ * Start a divide of the specified signed parameters. You should wait for 8 cycles (__div_pause()) or wait for the ready bit to be set
+ * (hw_divider_wait_ready()) prior to reading the results.
+ *
+ * \param a The dividend
+ * \param b The divisor
+ */
+static inline void hw_divider_divmod_s32_start(int32_t a, int32_t b) {
+ check_hw_layout( sio_hw_t, div_sdividend, SIO_DIV_SDIVIDEND_OFFSET);
+ sio_hw->div_sdividend = (uint32_t)a;
+ sio_hw->div_sdivisor = (uint32_t)b;
+}
+
+/*! \brief Start an unsigned asynchronous divide
+ * \ingroup hardware_divider
+ *
+ * Start a divide of the specified unsigned parameters. You should wait for 8 cycles (__div_pause()) or wait for the ready bit to be set
+ * (hw_divider_wait_ready()) prior to reading the results.
+ *
+ * \param a The dividend
+ * \param b The divisor
+ */
+static inline void hw_divider_divmod_u32_start(uint32_t a, uint32_t b) {
+ check_hw_layout(
+ sio_hw_t, div_udividend, SIO_DIV_UDIVIDEND_OFFSET);
+ sio_hw->div_udividend = a;
+ sio_hw->div_udivisor = b;
+}
+
+/*! \brief Wait for a divide to complete
+ * \ingroup hardware_divider
+ *
+ * Wait for a divide to complete
+ */
+static inline void hw_divider_wait_ready(void) {
+ // this is #1 in lsr below
+ static_assert(SIO_DIV_CSR_READY_BITS == 1, "");
+
+ // we use one less register and instruction than gcc which uses a TST instruction
+
+ uint32_t tmp; // allow compiler to pick scratch register
+ asm volatile (
+ "hw_divider_result_loop_%=:"
+ "ldr %0, [%1, %2]\n\t"
+ "lsr %0, #1\n\t"
+ "bcc hw_divider_result_loop_%=\n\t"
+ : "=&l" (tmp)
+ : "l" (sio_hw), "I" (SIO_DIV_CSR_OFFSET)
+ :
+ );
+}
+
+/*! \brief Return result of HW divide, nowait
+ * \ingroup hardware_divider
+ *
+ * \note This is UNSAFE in that the calculation may not have been completed.
+ *
+ * \return Current result. Most significant 32 bits are the remainder, lower 32 bits are the quotient.
+ */
+static inline divmod_result_t hw_divider_result_nowait(void) {
+ // as ugly as this looks it is actually quite efficient
+ divmod_result_t rc = (((divmod_result_t) sio_hw->div_remainder) << 32u) | sio_hw->div_quotient;
+ return rc;
+}
+
+/*! \brief Return result of last asynchronous HW divide
+ * \ingroup hardware_divider
+ *
+ * This function waits for the result to be ready by calling hw_divider_wait_ready().
+ *
+ * \return Current result. Most significant 32 bits are the remainder, lower 32 bits are the quotient.
+ */
+static inline divmod_result_t hw_divider_result_wait(void) {
+ hw_divider_wait_ready();
+ return hw_divider_result_nowait();
+}
+
+/*! \brief Return result of last asynchronous HW divide, unsigned quotient only
+ * \ingroup hardware_divider
+ *
+ * This function waits for the result to be ready by calling hw_divider_wait_ready().
+ *
+ * \return Current unsigned quotient result.
+ */
+static inline uint32_t hw_divider_u32_quotient_wait(void) {
+ hw_divider_wait_ready();
+ return sio_hw->div_quotient;
+}
+
+/*! \brief Return result of last asynchronous HW divide, signed quotient only
+ * \ingroup hardware_divider
+ *
+ * This function waits for the result to be ready by calling hw_divider_wait_ready().
+ *
+ * \return Current signed quotient result.
+ */
+static inline int32_t hw_divider_s32_quotient_wait(void) {
+ hw_divider_wait_ready();
+ return (int32_t)sio_hw->div_quotient;
+}
+
+/*! \brief Return result of last asynchronous HW divide, unsigned remainder only
+ * \ingroup hardware_divider
+ *
+ * This function waits for the result to be ready by calling hw_divider_wait_ready().
+ *
+ * \return Current unsigned remainder result.
+ */
+static inline uint32_t hw_divider_u32_remainder_wait(void) {
+ hw_divider_wait_ready();
+ uint32_t rc = sio_hw->div_remainder;
+ sio_hw->div_quotient; // must read quotient to cooperate with other SDK code
+ return rc;
+}
+
+/*! \brief Return result of last asynchronous HW divide, signed remainder only
+ * \ingroup hardware_divider
+ *
+ * This function waits for the result to be ready by calling hw_divider_wait_ready().
+ *
+ * \return Current remainder results.
+ */
+static inline int32_t hw_divider_s32_remainder_wait(void) {
+ hw_divider_wait_ready();
+ int32_t rc = (int32_t)sio_hw->div_remainder;
+ sio_hw->div_quotient; // must read quotient to cooperate with other SDK code
+ return rc;
+}
+
+/*! \brief Do a signed HW divide and wait for result
+ * \ingroup hardware_divider
+ *
+ * Divide \p a by \p b, wait for calculation to complete, return result as a fixed point 32p32 value.
+ *
+ * \param a The dividend
+ * \param b The divisor
+ * \return Results of divide as a 32p32 fixed point value.
+ */
+divmod_result_t hw_divider_divmod_s32(int32_t a, int32_t b);
+
+/*! \brief Do an unsigned HW divide and wait for result
+ * \ingroup hardware_divider
+ *
+ * Divide \p a by \p b, wait for calculation to complete, return result as a fixed point 32p32 value.
+ *
+ * \param a The dividend
+ * \param b The divisor
+ * \return Results of divide as a 32p32 fixed point value.
+ */
+divmod_result_t hw_divider_divmod_u32(uint32_t a, uint32_t b);
+
+/*! \brief Efficient extraction of unsigned quotient from 32p32 fixed point
+ * \ingroup hardware_divider
+ *
+ * \param r 32p32 fixed point value.
+ * \return Unsigned quotient
+ */
+inline static uint32_t to_quotient_u32(divmod_result_t r) {
+ return (uint32_t) r;
+}
+
+/*! \brief Efficient extraction of signed quotient from 32p32 fixed point
+ * \ingroup hardware_divider
+ *
+ * \param r 32p32 fixed point value.
+ * \return Unsigned quotient
+ */
+inline static int32_t to_quotient_s32(divmod_result_t r) {
+ return (int32_t)(uint32_t)r;
+}
+
+/*! \brief Efficient extraction of unsigned remainder from 32p32 fixed point
+ * \ingroup hardware_divider
+ *
+ * \param r 32p32 fixed point value.
+ * \return Unsigned remainder
+ *
+ * \note On Arm this is just a 32 bit register move or a nop
+ */
+inline static uint32_t to_remainder_u32(divmod_result_t r) {
+ return (uint32_t)(r >> 32u);
+}
+
+/*! \brief Efficient extraction of signed remainder from 32p32 fixed point
+ * \ingroup hardware_divider
+ *
+ * \param r 32p32 fixed point value.
+ * \return Signed remainder
+ *
+ * \note On arm this is just a 32 bit register move or a nop
+ */
+inline static int32_t to_remainder_s32(divmod_result_t r) {
+ return (int32_t)(r >> 32u);
+}
+
+/*! \brief Do an unsigned HW divide, wait for result, return quotient
+ * \ingroup hardware_divider
+ *
+ * Divide \p a by \p b, wait for calculation to complete, return quotient.
+ *
+ * \param a The dividend
+ * \param b The divisor
+ * \return Quotient results of the divide
+ */
+static inline uint32_t hw_divider_u32_quotient(uint32_t a, uint32_t b) {
+ return to_quotient_u32(hw_divider_divmod_u32(a, b));
+}
+
+/*! \brief Do an unsigned HW divide, wait for result, return remainder
+ * \ingroup hardware_divider
+ *
+ * Divide \p a by \p b, wait for calculation to complete, return remainder.
+ *
+ * \param a The dividend
+ * \param b The divisor
+ * \return Remainder results of the divide
+ */
+static inline uint32_t hw_divider_u32_remainder(uint32_t a, uint32_t b) {
+ return to_remainder_u32(hw_divider_divmod_u32(a, b));
+}
+
+/*! \brief Do a signed HW divide, wait for result, return quotient
+ * \ingroup hardware_divider
+ *
+ * Divide \p a by \p b, wait for calculation to complete, return quotient.
+ *
+ * \param a The dividend
+ * \param b The divisor
+ * \return Quotient results of the divide
+ */
+static inline int32_t hw_divider_quotient_s32(int32_t a, int32_t b) {
+ return to_quotient_s32(hw_divider_divmod_s32(a, b));
+}
+
+/*! \brief Do a signed HW divide, wait for result, return remainder
+ * \ingroup hardware_divider
+ *
+ * Divide \p a by \p b, wait for calculation to complete, return remainder.
+ *
+ * \param a The dividend
+ * \param b The divisor
+ * \return Remainder results of the divide
+ */
+static inline int32_t hw_divider_remainder_s32(int32_t a, int32_t b) {
+ return to_remainder_s32(hw_divider_divmod_s32(a, b));
+}
+
+/*! \brief Pause for exact amount of time needed for a asynchronous divide to complete
+ * \ingroup hardware_divider
+ */
+static inline void hw_divider_pause(void) {
+ asm volatile (
+ "b _1_%=\n"
+ "_1_%=:\n"
+ "b _2_%=\n"
+ "_2_%=:\n"
+ "b _3_%=\n"
+ "_3_%=:\n"
+ "b _4_%=\n"
+ "_4_%=:\n"
+ :: : );
+}
+
+/*! \brief Do a hardware unsigned HW divide, wait for result, return quotient
+ * \ingroup hardware_divider
+ *
+ * Divide \p a by \p b, wait for calculation to complete, return quotient.
+ *
+ * \param a The dividend
+ * \param b The divisor
+ * \return Quotient result of the divide
+ */
+static inline uint32_t hw_divider_u32_quotient_inlined(uint32_t a, uint32_t b) {
+ hw_divider_divmod_u32_start(a, b);
+ hw_divider_pause();
+ return sio_hw->div_quotient;
+}
+
+/*! \brief Do a hardware unsigned HW divide, wait for result, return remainder
+ * \ingroup hardware_divider
+ *
+ * Divide \p a by \p b, wait for calculation to complete, return remainder.
+ *
+ * \param a The dividend
+ * \param b The divisor
+ * \return Remainder result of the divide
+ */
+static inline uint32_t hw_divider_u32_remainder_inlined(uint32_t a, uint32_t b) {
+ hw_divider_divmod_u32_start(a, b);
+ hw_divider_pause();
+ uint32_t rc = sio_hw->div_remainder;
+ sio_hw->div_quotient; // must read quotient to cooperate with other SDK code
+ return rc;
+}
+
+/*! \brief Do a hardware signed HW divide, wait for result, return quotient
+ * \ingroup hardware_divider
+ *
+ * Divide \p a by \p b, wait for calculation to complete, return quotient.
+ *
+ * \param a The dividend
+ * \param b The divisor
+ * \return Quotient result of the divide
+ */
+static inline int32_t hw_divider_s32_quotient_inlined(int32_t a, int32_t b) {
+ hw_divider_divmod_s32_start(a, b);
+ hw_divider_pause();
+ return (int32_t)sio_hw->div_quotient;
+}
+
+/*! \brief Do a hardware signed HW divide, wait for result, return remainder
+ * \ingroup hardware_divider
+ *
+ * Divide \p a by \p b, wait for calculation to complete, return remainder.
+ *
+ * \param a The dividend
+ * \param b The divisor
+ * \return Remainder result of the divide
+ */
+static inline int32_t hw_divider_s32_remainder_inlined(int32_t a, int32_t b) {
+ hw_divider_divmod_s32_start(a, b);
+ hw_divider_pause();
+ int32_t rc = (int32_t)sio_hw->div_remainder;
+ sio_hw->div_quotient; // must read quotient to cooperate with other SDK code
+ return rc;
+}
+
+typedef struct {
+ uint32_t values[4];
+} hw_divider_state_t;
+
+/*! \brief Save the calling cores hardware divider state
+ * \ingroup hardware_divider
+ *
+ * Copy the current core's hardware divider state into the provided structure. This method
+ * waits for the divider results to be stable, then copies them to memory.
+ * They can be restored via hw_divider_restore_state()
+ *
+ * \param dest the location to store the divider state
+ */
+void hw_divider_save_state(hw_divider_state_t *dest);
+
+/*! \brief Load a saved hardware divider state into the current core's hardware divider
+ * \ingroup hardware_divider
+ *
+ * Copy the passed hardware divider state into the hardware divider.
+ *
+ * \param src the location to load the divider state from
+ */
+
+void hw_divider_restore_state(hw_divider_state_t *src);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // _HARDWARE_DIVIDER_H
diff --git a/src/rp2_common/hardware_divider/include/hardware/divider_helper.S b/src/rp2_common/hardware_divider/include/hardware/divider_helper.S
new file mode 100644
index 0000000..062e12d
--- /dev/null
+++ b/src/rp2_common/hardware_divider/include/hardware/divider_helper.S
@@ -0,0 +1,68 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "hardware/regs/addressmap.h"
+#include "hardware/regs/sio.h"
+
+#if SIO_DIV_CSR_READY_LSB == 0
+.equ SIO_DIV_CSR_READY_SHIFT_FOR_CARRY, 1
+#else
+need to change SHIFT above
+#endif
+#if SIO_DIV_CSR_DIRTY_LSB == 1
+.equ SIO_DIV_CSR_DIRTY_SHIFT_FOR_CARRY, 2
+#else
+need to change SHIFT above
+#endif
+
+// SIO_BASE ptr in r2; pushes r4-r7, lr to stack
+// requires that division started at least 2 cycles prior to the start of the macro
+.macro save_div_state_and_lr
+// originally we did this, however a) it uses r3, and b) the push takes 6 cycles, b)
+// any IRQ which uses the divider will necessarily put the data back, which will
+// immediately make it ready
+//
+// // ldr r3, [r2, #SIO_DIV_CSR_OFFSET]
+// // // wait for results as we can't save signed-ness of operation
+// // 1:
+// // lsrs r3, #SIO_DIV_CSR_READY_SHIFT_FOR_CARRY
+// // bcc 1b
+
+// 6 cycles
+push {r4, r5, r6, r7, lr}
+// note we must read quotient last, and since it isn't the last reg, we'll not use ldmia!
+ldr r4, [r2, #SIO_DIV_UDIVIDEND_OFFSET]
+ldr r5, [r2, #SIO_DIV_UDIVISOR_OFFSET]
+ldr r7, [r2, #SIO_DIV_REMAINDER_OFFSET]
+ldr r6, [r2, #SIO_DIV_QUOTIENT_OFFSET]
+.endm
+
+// restores divider state from r4-r7, then pops them and pc
+.macro restore_div_state_and_return
+// writing sdividend (r4), sdivisor (r5), quotient (r6), remainder (r7) in that order
+//
+// it is worth considering what happens if we are interrupted
+//
+// after writing r4: we are DIRTY and !READY
+// ... interruptor using div will complete based on incorrect inputs, but dividend at least will be
+// saved/restored correctly and we'll restore the rest ourselves
+// after writing r4, r5: we are DIRTY and !READY
+// ... interruptor using div will complete based on possibly wrongly signed inputs, but dividend, divisor
+// at least will be saved/restored correctly and and we'll restore the rest ourselves
+// after writing r4, r5, r6: we are DIRTY and READY
+// ... interruptor using div will dividend, divisor, quotient registers as is (what we just restored ourselves),
+// and we'll restore the remainder after the fact
+
+// note we are not use STM not because it can be restarted due to interrupt which is harmless, more because this is 1 cycle IO space
+// and so 4 reads is cheaper (and we don't have to adjust r2)
+// note also, that we must restore via UDIVI* rather than SDIVI* to prevent the quotient/remainder being negated on read based
+// on the signs of the inputs
+str r4, [r2, #SIO_DIV_UDIVIDEND_OFFSET]
+str r5, [r2, #SIO_DIV_UDIVISOR_OFFSET]
+str r7, [r2, #SIO_DIV_REMAINDER_OFFSET]
+str r6, [r2, #SIO_DIV_QUOTIENT_OFFSET]
+pop {r4, r5, r6, r7, pc}
+.endm
\ No newline at end of file
diff --git a/src/rp2_common/hardware_dma/CMakeLists.txt b/src/rp2_common/hardware_dma/CMakeLists.txt
new file mode 100644
index 0000000..fe08541
--- /dev/null
+++ b/src/rp2_common/hardware_dma/CMakeLists.txt
@@ -0,0 +1,2 @@
+pico_simple_hardware_target(dma)
+target_link_libraries(hardware_dma INTERFACE hardware_claim)
\ No newline at end of file
diff --git a/src/rp2_common/hardware_dma/dma.c b/src/rp2_common/hardware_dma/dma.c
new file mode 100644
index 0000000..90fde06
--- /dev/null
+++ b/src/rp2_common/hardware_dma/dma.c
@@ -0,0 +1,94 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdio.h>
+#include "hardware/dma.h"
+#include "hardware/claim.h"
+
+#define DMA_CHAN_STRIDE (DMA_CH1_CTRL_TRIG_OFFSET - DMA_CH0_CTRL_TRIG_OFFSET)
+check_hw_size(dma_channel_hw_t, DMA_CHAN_STRIDE);
+check_hw_layout(dma_hw_t, abort, DMA_CHAN_ABORT_OFFSET);
+
+// sanity check
+static_assert(__builtin_offsetof(dma_hw_t, ch[0].ctrl_trig) == DMA_CH0_CTRL_TRIG_OFFSET, "hw mismatch");
+static_assert(__builtin_offsetof(dma_hw_t, ch[1].ctrl_trig) == DMA_CH1_CTRL_TRIG_OFFSET, "hw mismatch");
+
+static_assert(NUM_DMA_CHANNELS <= 16, "");
+static uint16_t _claimed;
+static uint8_t _timer_claimed;
+
+void dma_channel_claim(uint channel) {
+ check_dma_channel_param(channel);
+ hw_claim_or_assert((uint8_t *) &_claimed, channel, "DMA channel %d is already claimed");
+}
+
+void dma_claim_mask(uint32_t mask) {
+ for(uint i = 0; mask; i++, mask >>= 1u) {
+ if (mask & 1u) dma_channel_claim(i);
+ }
+}
+
+void dma_channel_unclaim(uint channel) {
+ check_dma_channel_param(channel);
+ hw_claim_clear((uint8_t *) &_claimed, channel);
+}
+
+int dma_claim_unused_channel(bool required) {
+ return hw_claim_unused_from_range((uint8_t*)&_claimed, required, 0, NUM_DMA_CHANNELS-1, "No DMA channels are available");
+}
+
+bool dma_channel_is_claimed(uint channel) {
+ check_dma_channel_param(channel);
+ return hw_is_claimed((uint8_t *) &_claimed, channel);
+}
+
+void dma_timer_claim(uint timer) {
+ check_dma_timer_param(timer);
+ hw_claim_or_assert(&_timer_claimed, timer, "DMA timer %d is already claimed");
+}
+
+void dma_timer_unclaim(uint timer) {
+ check_dma_timer_param(timer);
+ hw_claim_clear(&_timer_claimed, timer);
+}
+
+int dma_claim_unused_timer(bool required) {
+ return hw_claim_unused_from_range(&_timer_claimed, required, 0, NUM_DMA_TIMERS-1, "No DMA timers are available");
+}
+
+bool dma_timer_is_claimed(uint timer) {
+ check_dma_timer_param(timer);
+ return hw_is_claimed(&_timer_claimed, timer);
+}
+
+#ifndef NDEBUG
+
+void print_dma_ctrl(dma_channel_hw_t *channel) {
+ uint32_t ctrl = channel->ctrl_trig;
+ int rgsz = (ctrl & DMA_CH0_CTRL_TRIG_RING_SIZE_BITS) >> DMA_CH0_CTRL_TRIG_RING_SIZE_LSB;
+ printf("(%08x) ber %d rer %d wer %d busy %d trq %d cto %d rgsl %d rgsz %d inw %d inr %d sz %d hip %d en %d",
+ (uint) ctrl,
+ ctrl & DMA_CH0_CTRL_TRIG_AHB_ERROR_BITS ? 1 : 0,
+ ctrl & DMA_CH0_CTRL_TRIG_READ_ERROR_BITS ? 1 : 0,
+ ctrl & DMA_CH0_CTRL_TRIG_WRITE_ERROR_BITS ? 1 : 0,
+ ctrl & DMA_CH0_CTRL_TRIG_BUSY_BITS ? 1 : 0,
+ (int) ((ctrl & DMA_CH0_CTRL_TRIG_TREQ_SEL_BITS) >> DMA_CH0_CTRL_TRIG_TREQ_SEL_LSB),
+ (int) ((ctrl & DMA_CH0_CTRL_TRIG_CHAIN_TO_BITS) >> DMA_CH0_CTRL_TRIG_CHAIN_TO_LSB),
+ ctrl & DMA_CH0_CTRL_TRIG_RING_SEL_BITS ? 1 : 0,
+ rgsz ? (1 << rgsz) : 0,
+ ctrl & DMA_CH0_CTRL_TRIG_INCR_WRITE_BITS ? 1 : 0,
+ ctrl & DMA_CH0_CTRL_TRIG_INCR_READ_BITS ? 1 : 0,
+ 1 << ((ctrl & DMA_CH0_CTRL_TRIG_DATA_SIZE_BITS) >> DMA_CH0_CTRL_TRIG_DATA_SIZE_LSB),
+ ctrl & DMA_CH0_CTRL_TRIG_HIGH_PRIORITY_BITS ? 1 : 0,
+ ctrl & DMA_CH0_CTRL_TRIG_EN_BITS ? 1 : 0);
+}
+#endif
+
+#if PARAM_ASSERTIONS_ENABLED(DMA)
+void check_dma_channel_param_impl(uint __unused channel) {
+ valid_params_if(DMA, channel < NUM_DMA_CHANNELS);
+}
+#endif
diff --git a/src/rp2_common/hardware_dma/include/hardware/dma.h b/src/rp2_common/hardware_dma/include/hardware/dma.h
new file mode 100644
index 0000000..7c9406f
--- /dev/null
+++ b/src/rp2_common/hardware_dma/include/hardware/dma.h
@@ -0,0 +1,795 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _HARDWARE_DMA_H_
+#define _HARDWARE_DMA_H_
+
+#include "pico.h"
+#include "hardware/structs/dma.h"
+#include "hardware/regs/dreq.h"
+#include "pico/assert.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** \file hardware/dma.h
+ * \defgroup hardware_dma hardware_dma
+ *
+ * DMA Controller API
+ *
+ * The RP2040 Direct Memory Access (DMA) master performs bulk data transfers on a processor’s
+ * behalf. This leaves processors free to attend to other tasks, or enter low-power sleep states. The
+ * data throughput of the DMA is also significantly higher than one of RP2040’s processors.
+ *
+ * The DMA can perform one read access and one write access, up to 32 bits in size, every clock cycle.
+ * There are 12 independent channels, which each supervise a sequence of bus transfers, usually in
+ * one of the following scenarios:
+ *
+ * * Memory to peripheral
+ * * Peripheral to memory
+ * * Memory to memory
+ */
+
+// these are not defined in generated dreq.h
+#define DREQ_DMA_TIMER0 DMA_CH0_CTRL_TRIG_TREQ_SEL_VALUE_TIMER0
+#define DREQ_DMA_TIMER1 DMA_CH0_CTRL_TRIG_TREQ_SEL_VALUE_TIMER1
+#define DREQ_DMA_TIMER2 DMA_CH0_CTRL_TRIG_TREQ_SEL_VALUE_TIMER2
+#define DREQ_DMA_TIMER3 DMA_CH0_CTRL_TRIG_TREQ_SEL_VALUE_TIMER3
+#define DREQ_FORCE DMA_CH0_CTRL_TRIG_TREQ_SEL_VALUE_PERMANENT
+
+// PICO_CONFIG: PARAM_ASSERTIONS_ENABLED_DMA, Enable/disable DMA assertions, type=bool, default=0, group=hardware_dma
+#ifndef PARAM_ASSERTIONS_ENABLED_DMA
+#define PARAM_ASSERTIONS_ENABLED_DMA 0
+#endif
+
+static inline void check_dma_channel_param(__unused uint channel) {
+#if PARAM_ASSERTIONS_ENABLED(DMA)
+ // this method is used a lot by inline functions so avoid code bloat by deferring to function
+ extern void check_dma_channel_param_impl(uint channel);
+ check_dma_channel_param_impl(channel);
+#endif
+}
+
+static inline void check_dma_timer_param(__unused uint timer_num) {
+ valid_params_if(DMA, timer_num < NUM_DMA_TIMERS);
+}
+
+inline static dma_channel_hw_t *dma_channel_hw_addr(uint channel) {
+ check_dma_channel_param(channel);
+ return &dma_hw->ch[channel];
+}
+
+/*! \brief Mark a dma channel as used
+ * \ingroup hardware_dma
+ *
+ * Method for cooperative claiming of hardware. Will cause a panic if the channel
+ * is already claimed. Use of this method by libraries detects accidental
+ * configurations that would fail in unpredictable ways.
+ *
+ * \param channel the dma channel
+ */
+void dma_channel_claim(uint channel);
+
+/*! \brief Mark multiple dma channels as used
+ * \ingroup hardware_dma
+ *
+ * Method for cooperative claiming of hardware. Will cause a panic if any of the channels
+ * are already claimed. Use of this method by libraries detects accidental
+ * configurations that would fail in unpredictable ways.
+ *
+ * \param channel_mask Bitfield of all required channels to claim (bit 0 == channel 0, bit 1 == channel 1 etc)
+ */
+void dma_claim_mask(uint32_t channel_mask);
+
+/*! \brief Mark a dma channel as no longer used
+ * \ingroup hardware_dma
+ *
+ * Method for cooperative claiming of hardware.
+ *
+ * \param channel the dma channel to release
+ */
+void dma_channel_unclaim(uint channel);
+
+/*! \brief Claim a free dma channel
+ * \ingroup hardware_dma
+ *
+ * \param required if true the function will panic if none are available
+ * \return the dma channel number or -1 if required was false, and none were free
+ */
+int dma_claim_unused_channel(bool required);
+
+/*! \brief Determine if a dma channel is claimed
+ * \ingroup hardware_dma
+ *
+ * \param channel the dma channel
+ * \return true if the channel is claimed, false otherwise
+ * \see dma_channel_claim
+ * \see dma_channel_claim_mask
+ */
+bool dma_channel_is_claimed(uint channel);
+
+/** \brief DMA channel configuration
+ * \defgroup channel_config channel_config
+ * \ingroup hardware_dma
+ *
+ * A DMA channel needs to be configured, these functions provide handy helpers to set up configuration
+ * structures. See \ref dma_channel_config
+ *
+ */
+
+/*! \brief Enumeration of available DMA channel transfer sizes.
+ * \ingroup hardware_dma
+ *
+ * Names indicate the number of bits.
+ */
+enum dma_channel_transfer_size {
+ DMA_SIZE_8 = 0, ///< Byte transfer (8 bits)
+ DMA_SIZE_16 = 1, ///< Half word transfer (16 bits)
+ DMA_SIZE_32 = 2 ///< Word transfer (32 bits)
+};
+
+typedef struct {
+ uint32_t ctrl;
+} dma_channel_config;
+
+/*! \brief Set DMA channel read increment
+ * \ingroup channel_config
+ *
+ * \param c Pointer to channel configuration data
+ * \param incr True to enable read address increments, if false, each read will be from the same address
+ * Usually disabled for peripheral to memory transfers
+ */
+static inline void channel_config_set_read_increment(dma_channel_config *c, bool incr) {
+ c->ctrl = incr ? (c->ctrl | DMA_CH0_CTRL_TRIG_INCR_READ_BITS) : (c->ctrl & ~DMA_CH0_CTRL_TRIG_INCR_READ_BITS);
+}
+
+/*! \brief Set DMA channel write increment
+ * \ingroup channel_config
+ *
+ * \param c Pointer to channel configuration data
+ * \param incr True to enable write address increments, if false, each write will be to the same address
+ * Usually disabled for memory to peripheral transfers
+ * Usually disabled for memory to peripheral transfers
+ */
+static inline void channel_config_set_write_increment(dma_channel_config *c, bool incr) {
+ c->ctrl = incr ? (c->ctrl | DMA_CH0_CTRL_TRIG_INCR_WRITE_BITS) : (c->ctrl & ~DMA_CH0_CTRL_TRIG_INCR_WRITE_BITS);
+}
+
+/*! \brief Select a transfer request signal
+ * \ingroup channel_config
+ *
+ * The channel uses the transfer request signal to pace its data transfer rate.
+ * Sources for TREQ signals are internal (TIMERS) or external (DREQ, a Data Request from the system).
+ * 0x0 to 0x3a -> select DREQ n as TREQ
+ * 0x3b -> Select Timer 0 as TREQ
+ * 0x3c -> Select Timer 1 as TREQ
+ * 0x3d -> Select Timer 2 as TREQ (Optional)
+ * 0x3e -> Select Timer 3 as TREQ (Optional)
+ * 0x3f -> Permanent request, for unpaced transfers.
+ *
+ * \param c Pointer to channel configuration data
+ * \param dreq Source (see description)
+ */
+static inline void channel_config_set_dreq(dma_channel_config *c, uint dreq) {
+ assert(dreq <= DREQ_FORCE);
+ c->ctrl = (c->ctrl & ~DMA_CH0_CTRL_TRIG_TREQ_SEL_BITS) | (dreq << DMA_CH0_CTRL_TRIG_TREQ_SEL_LSB);
+}
+
+/*! \brief Set DMA channel completion channel
+ * \ingroup channel_config
+ *
+ * When this channel completes, it will trigger the channel indicated by chain_to. Disable by
+ * setting chain_to to itself (the same channel)
+ *
+ * \param c Pointer to channel configuration data
+ * \param chain_to Channel to trigger when this channel completes.
+ */
+static inline void channel_config_set_chain_to(dma_channel_config *c, uint chain_to) {
+ assert(chain_to <= NUM_DMA_CHANNELS);
+ c->ctrl = (c->ctrl & ~DMA_CH0_CTRL_TRIG_CHAIN_TO_BITS) | (chain_to << DMA_CH0_CTRL_TRIG_CHAIN_TO_LSB);
+}
+
+/*! \brief Set the size of each DMA bus transfer
+ * \ingroup channel_config
+ *
+ * Set the size of each bus transfer (byte/halfword/word). The read and write addresses
+ * advance by the specific amount (1/2/4 bytes) with each transfer.
+ *
+ * \param c Pointer to channel configuration data
+ * \param size See enum for possible values.
+ */
+static inline void channel_config_set_transfer_data_size(dma_channel_config *c, enum dma_channel_transfer_size size) {
+ assert(size == DMA_SIZE_8 || size == DMA_SIZE_16 || size == DMA_SIZE_32);
+ c->ctrl = (c->ctrl & ~DMA_CH0_CTRL_TRIG_DATA_SIZE_BITS) | (((uint)size) << DMA_CH0_CTRL_TRIG_DATA_SIZE_LSB);
+}
+
+/*! \brief Set address wrapping parameters
+ * \ingroup channel_config
+ *
+ * Size of address wrap region. If 0, don’t wrap. For values n > 0, only the lower n bits of the address
+ * will change. This wraps the address on a (1 << n) byte boundary, facilitating access to naturally-aligned
+ * ring buffers.
+ * Ring sizes between 2 and 32768 bytes are possible (size_bits from 1 - 15)
+ *
+ * 0x0 -> No wrapping.
+ *
+ * \param c Pointer to channel configuration data
+ * \param write True to apply to write addresses, false to apply to read addresses
+ * \param size_bits 0 to disable wrapping. Otherwise the size in bits of the changing part of the address.
+ * Effectively wraps the address on a (1 << size_bits) byte boundary.
+ */
+static inline void channel_config_set_ring(dma_channel_config *c, bool write, uint size_bits) {
+ assert(size_bits < 32);
+ c->ctrl = (c->ctrl & ~(DMA_CH0_CTRL_TRIG_RING_SIZE_BITS | DMA_CH0_CTRL_TRIG_RING_SEL_BITS)) |
+ (size_bits << DMA_CH0_CTRL_TRIG_RING_SIZE_LSB) |
+ (write ? DMA_CH0_CTRL_TRIG_RING_SEL_BITS : 0);
+}
+
+/*! \brief Set DMA byte swapping
+ * \ingroup channel_config
+ *
+ * No effect for byte data, for halfword data, the two bytes of each halfword are
+ * swapped. For word data, the four bytes of each word are swapped to reverse their order.
+ *
+ * \param c Pointer to channel configuration data
+ * \param bswap True to enable byte swapping
+ */
+static inline void channel_config_set_bswap(dma_channel_config *c, bool bswap) {
+ c->ctrl = bswap ? (c->ctrl | DMA_CH0_CTRL_TRIG_BSWAP_BITS) : (c->ctrl & ~DMA_CH0_CTRL_TRIG_BSWAP_BITS);
+}
+
+/*! \brief Set IRQ quiet mode
+ * \ingroup channel_config
+ *
+ * In QUIET mode, the channel does not generate IRQs at the end of every transfer block. Instead,
+ * an IRQ is raised when NULL is written to a trigger register, indicating the end of a control
+ * block chain.
+ *
+ * \param c Pointer to channel configuration data
+ * \param irq_quiet True to enable quiet mode, false to disable.
+ */
+static inline void channel_config_set_irq_quiet(dma_channel_config *c, bool irq_quiet) {
+ c->ctrl = irq_quiet ? (c->ctrl | DMA_CH0_CTRL_TRIG_IRQ_QUIET_BITS) : (c->ctrl & ~DMA_CH0_CTRL_TRIG_IRQ_QUIET_BITS);
+}
+
+/*!
+ * \brief Enable/Disable the DMA channel
+ * \ingroup channel_config
+ *
+ * When false, the channel will ignore triggers, stop issuing transfers, and pause the current transfer sequence (i.e. BUSY will
+ * remain high if already high)
+ *
+ * \param c Pointer to channel configuration data
+ * \param enable True to enable the DMA channel. When enabled, the channel will respond to triggering events, and start transferring data.
+ *
+ */
+static inline void channel_config_set_enable(dma_channel_config *c, bool enable) {
+ c->ctrl = enable ? (c->ctrl | DMA_CH0_CTRL_TRIG_EN_BITS) : (c->ctrl & ~DMA_CH0_CTRL_TRIG_EN_BITS);
+}
+
+/*! \brief Enable access to channel by sniff hardware.
+ * \ingroup channel_config
+ *
+ * Sniff HW must be enabled and have this channel selected.
+ *
+ * \param c Pointer to channel configuration data
+ * \param sniff_enable True to enable the Sniff HW access to this DMA channel.
+ */
+static inline void channel_config_set_sniff_enable(dma_channel_config *c, bool sniff_enable) {
+ c->ctrl = sniff_enable ? (c->ctrl | DMA_CH0_CTRL_TRIG_SNIFF_EN_BITS) : (c->ctrl &
+ ~DMA_CH0_CTRL_TRIG_SNIFF_EN_BITS);
+}
+
+/*! \brief Get the default channel configuration for a given channel
+ * \ingroup channel_config
+ *
+ * Setting | Default
+ * --------|--------
+ * Read Increment | true
+ * Write Increment | false
+ * DReq | DREQ_FORCE
+ * Chain to | self
+ * Data size | DMA_SIZE_32
+ * Ring | write=false, size=0 (i.e. off)
+ * Byte Swap | false
+ * Quiet IRQs | false
+ * Channel Enable | true
+ * Sniff Enable | false
+ *
+ * \param channel DMA channel
+ * \return the default configuration which can then be modified.
+ */
+static inline dma_channel_config dma_channel_get_default_config(uint channel) {
+ dma_channel_config c = {0};
+ channel_config_set_read_increment(&c, true);
+ channel_config_set_write_increment(&c, false);
+ channel_config_set_dreq(&c, DREQ_FORCE);
+ channel_config_set_chain_to(&c, channel);
+ channel_config_set_transfer_data_size(&c, DMA_SIZE_32);
+ channel_config_set_ring(&c, false, 0);
+ channel_config_set_bswap(&c, false);
+ channel_config_set_irq_quiet(&c, false);
+ channel_config_set_enable(&c, true);
+ channel_config_set_sniff_enable(&c, false);
+ return c;
+}
+
+/*! \brief Get the current configuration for the specified channel.
+ * \ingroup channel_config
+ *
+ * \param channel DMA channel
+ * \return The current configuration as read from the HW register (not cached)
+ */
+static inline dma_channel_config dma_get_channel_config(uint channel) {
+ dma_channel_config c;
+ c.ctrl = dma_channel_hw_addr(channel)->ctrl_trig;
+ return c;
+}
+
+/*! \brief Get the raw configuration register from a channel configuration
+ * \ingroup channel_config
+ *
+ * \param config Pointer to a config structure.
+ * \return Register content
+ */
+static inline uint32_t channel_config_get_ctrl_value(const dma_channel_config *config) {
+ return config->ctrl;
+}
+
+/*! \brief Set a channel configuration
+ * \ingroup hardware_dma
+ *
+ * \param channel DMA channel
+ * \param config Pointer to a config structure with required configuration
+ * \param trigger True to trigger the transfer immediately
+ */
+static inline void dma_channel_set_config(uint channel, const dma_channel_config *config, bool trigger) {
+ // Don't use CTRL_TRIG since we don't want to start a transfer
+ if (!trigger) {
+ dma_channel_hw_addr(channel)->al1_ctrl = channel_config_get_ctrl_value(config);
+ } else {
+ dma_channel_hw_addr(channel)->ctrl_trig = channel_config_get_ctrl_value(config);
+ }
+}
+
+/*! \brief Set the DMA initial read address.
+ * \ingroup hardware_dma
+ *
+ * \param channel DMA channel
+ * \param read_addr Initial read address of transfer.
+ * \param trigger True to start the transfer immediately
+ */
+static inline void dma_channel_set_read_addr(uint channel, const volatile void *read_addr, bool trigger) {
+ if (!trigger) {
+ dma_channel_hw_addr(channel)->read_addr = (uintptr_t) read_addr;
+ } else {
+ dma_channel_hw_addr(channel)->al3_read_addr_trig = (uintptr_t) read_addr;
+ }
+}
+
+/*! \brief Set the DMA initial write address
+ * \ingroup hardware_dma
+ *
+ * \param channel DMA channel
+ * \param write_addr Initial write address of transfer.
+ * \param trigger True to start the transfer immediately
+ */
+static inline void dma_channel_set_write_addr(uint channel, volatile void *write_addr, bool trigger) {
+ if (!trigger) {
+ dma_channel_hw_addr(channel)->write_addr = (uintptr_t) write_addr;
+ } else {
+ dma_channel_hw_addr(channel)->al2_write_addr_trig = (uintptr_t) write_addr;
+ }
+}
+
+/*! \brief Set the number of bus transfers the channel will do
+ * \ingroup hardware_dma
+ *
+ * \param channel DMA channel
+ * \param trans_count The number of transfers (not NOT bytes, see channel_config_set_transfer_data_size)
+ * \param trigger True to start the transfer immediately
+ */
+static inline void dma_channel_set_trans_count(uint channel, uint32_t trans_count, bool trigger) {
+ if (!trigger) {
+ dma_channel_hw_addr(channel)->transfer_count = trans_count;
+ } else {
+ dma_channel_hw_addr(channel)->al1_transfer_count_trig = trans_count;
+ }
+}
+
+/*! \brief Configure all DMA parameters and optionally start transfer
+ * \ingroup hardware_dma
+ *
+ * \param channel DMA channel
+ * \param config Pointer to DMA config structure
+ * \param write_addr Initial write address
+ * \param read_addr Initial read address
+ * \param transfer_count Number of transfers to perform
+ * \param trigger True to start the transfer immediately
+ */
+static inline void dma_channel_configure(uint channel, const dma_channel_config *config, volatile void *write_addr,
+ const volatile void *read_addr,
+ uint transfer_count, bool trigger) {
+ dma_channel_set_read_addr(channel, read_addr, false);
+ dma_channel_set_write_addr(channel, write_addr, false);
+ dma_channel_set_trans_count(channel, transfer_count, false);
+ dma_channel_set_config(channel, config, trigger);
+}
+
+/*! \brief Start a DMA transfer from a buffer immediately
+ * \ingroup hardware_dma
+ *
+ * \param channel DMA channel
+ * \param read_addr Sets the initial read address
+ * \param transfer_count Number of transfers to make. Not bytes, but the number of transfers of channel_config_set_transfer_data_size() to be sent.
+ */
+inline static void __attribute__((always_inline)) dma_channel_transfer_from_buffer_now(uint channel,
+ const volatile void *read_addr,
+ uint32_t transfer_count) {
+// check_dma_channel_param(channel);
+ dma_channel_hw_t *hw = dma_channel_hw_addr(channel);
+ hw->read_addr = (uintptr_t) read_addr;
+ hw->al1_transfer_count_trig = transfer_count;
+}
+
+/*! \brief Start a DMA transfer to a buffer immediately
+ * \ingroup hardware_dma
+ *
+ * \param channel DMA channel
+ * \param write_addr Sets the initial write address
+ * \param transfer_count Number of transfers to make. Not bytes, but the number of transfers of channel_config_set_transfer_data_size() to be sent.
+ */
+inline static void dma_channel_transfer_to_buffer_now(uint channel, volatile void *write_addr, uint32_t transfer_count) {
+ dma_channel_hw_t *hw = dma_channel_hw_addr(channel);
+ hw->write_addr = (uintptr_t) write_addr;
+ hw->al1_transfer_count_trig = transfer_count;
+}
+
+/*! \brief Start one or more channels simultaneously
+ * \ingroup hardware_dma
+ *
+ * \param chan_mask Bitmask of all the channels requiring starting. Channel 0 = bit 0, channel 1 = bit 1 etc.
+ */
+static inline void dma_start_channel_mask(uint32_t chan_mask) {
+ valid_params_if(DMA, chan_mask && chan_mask < (1u << NUM_DMA_CHANNELS));
+ dma_hw->multi_channel_trigger = chan_mask;
+}
+
+/*! \brief Start a single DMA channel
+ * \ingroup hardware_dma
+ *
+ * \param channel DMA channel
+ */
+static inline void dma_channel_start(uint channel) {
+ dma_start_channel_mask(1u << channel);
+}
+
+/*! \brief Stop a DMA transfer
+ * \ingroup hardware_dma
+ *
+ * Function will only return once the DMA has stopped.
+ *
+ * \param channel DMA channel
+ */
+static inline void dma_channel_abort(uint channel) {
+ check_dma_channel_param(channel);
+ dma_hw->abort = 1u << channel;
+ // Bit will go 0 once channel has reached safe state
+ // (i.e. any in-flight transfers have retired)
+ while (dma_hw->abort & (1ul << channel)) tight_loop_contents();
+}
+
+/*! \brief Enable single DMA channel's interrupt via DMA_IRQ_0
+ * \ingroup hardware_dma
+ *
+ * \param channel DMA channel
+ * \param enabled true to enable interrupt 0 on specified channel, false to disable.
+ */
+static inline void dma_channel_set_irq0_enabled(uint channel, bool enabled) {
+ check_dma_channel_param(channel);
+ check_hw_layout(dma_hw_t, inte0, DMA_INTE0_OFFSET);
+ if (enabled)
+ hw_set_bits(&dma_hw->inte0, 1u << channel);
+ else
+ hw_clear_bits(&dma_hw->inte0, 1u << channel);
+}
+
+/*! \brief Enable multiple DMA channels' interrupts via DMA_IRQ_0
+ * \ingroup hardware_dma
+ *
+ * \param channel_mask Bitmask of all the channels to enable/disable. Channel 0 = bit 0, channel 1 = bit 1 etc.
+ * \param enabled true to enable all the interrupts specified in the mask, false to disable all the interrupts specified in the mask.
+ */
+static inline void dma_set_irq0_channel_mask_enabled(uint32_t channel_mask, bool enabled) {
+ if (enabled) {
+ hw_set_bits(&dma_hw->inte0, channel_mask);
+ } else {
+ hw_clear_bits(&dma_hw->inte0, channel_mask);
+ }
+}
+
+/*! \brief Enable single DMA channel's interrupt via DMA_IRQ_1
+ * \ingroup hardware_dma
+ *
+ * \param channel DMA channel
+ * \param enabled true to enable interrupt 1 on specified channel, false to disable.
+ */
+static inline void dma_channel_set_irq1_enabled(uint channel, bool enabled) {
+ check_dma_channel_param(channel);
+ check_hw_layout(dma_hw_t, inte1, DMA_INTE1_OFFSET);
+ if (enabled)
+ hw_set_bits(&dma_hw->inte1, 1u << channel);
+ else
+ hw_clear_bits(&dma_hw->inte1, 1u << channel);
+}
+
+/*! \brief Enable multiple DMA channels' interrupts via DMA_IRQ_1
+ * \ingroup hardware_dma
+ *
+ * \param channel_mask Bitmask of all the channels to enable/disable. Channel 0 = bit 0, channel 1 = bit 1 etc.
+ * \param enabled true to enable all the interrupts specified in the mask, false to disable all the interrupts specified in the mask.
+ */
+static inline void dma_set_irq1_channel_mask_enabled(uint32_t channel_mask, bool enabled) {
+ if (enabled) {
+ hw_set_bits(&dma_hw->inte1, channel_mask);
+ } else {
+ hw_clear_bits(&dma_hw->inte1, channel_mask);
+ }
+}
+
+/*! \brief Enable single DMA channel interrupt on either DMA_IRQ_0 or DMA_IRQ_1
+ * \ingroup hardware_dma
+ *
+ * \param irq_index the IRQ index; either 0 or 1 for DMA_IRQ_0 or DMA_IRQ_1
+ * \param channel DMA channel
+ * \param enabled true to enable interrupt via irq_index for specified channel, false to disable.
+ */
+static inline void dma_irqn_set_channel_enabled(uint irq_index, uint channel, bool enabled) {
+ invalid_params_if(DMA, irq_index > 1);
+ if (irq_index) {
+ dma_channel_set_irq1_enabled(channel, enabled);
+ } else {
+ dma_channel_set_irq0_enabled(channel, enabled);
+ }
+}
+
+/*! \brief Enable multiple DMA channels' interrupt via either DMA_IRQ_0 or DMA_IRQ_1
+ * \ingroup hardware_dma
+ *
+ * \param irq_index the IRQ index; either 0 or 1 for DMA_IRQ_0 or DMA_IRQ_1
+ * \param channel_mask Bitmask of all the channels to enable/disable. Channel 0 = bit 0, channel 1 = bit 1 etc.
+ * \param enabled true to enable all the interrupts specified in the mask, false to disable all the interrupts specified in the mask.
+ */
+static inline void dma_irqn_set_channel_mask_enabled(uint irq_index, uint32_t channel_mask, bool enabled) {
+ invalid_params_if(DMA, irq_index > 1);
+ if (irq_index) {
+ dma_set_irq1_channel_mask_enabled(channel_mask, enabled);
+ } else {
+ dma_set_irq0_channel_mask_enabled(channel_mask, enabled);
+ }
+}
+
+/*! \brief Determine if a particular channel is a cause of DMA_IRQ_0
+ * \ingroup hardware_dma
+ *
+ * \param channel DMA channel
+ * \return true if the channel is a cause of DMA_IRQ_0, false otherwise
+ */
+static inline bool dma_channel_get_irq0_status(uint channel) {
+ check_dma_channel_param(channel);
+ return dma_hw->ints0 & (1u << channel);
+}
+
+/*! \brief Determine if a particular channel is a cause of DMA_IRQ_1
+ * \ingroup hardware_dma
+ *
+ * \param channel DMA channel
+ * \return true if the channel is a cause of DMA_IRQ_1, false otherwise
+ */
+static inline bool dma_channel_get_irq1_status(uint channel) {
+ check_dma_channel_param(channel);
+ return dma_hw->ints1 & (1u << channel);
+}
+
+/*! \brief Determine if a particular channel is a cause of DMA_IRQ_N
+ * \ingroup hardware_dma
+ *
+ * \param irq_index the IRQ index; either 0 or 1 for DMA_IRQ_0 or DMA_IRQ_1
+ * \param channel DMA channel
+ * \return true if the channel is a cause of the DMA_IRQ_N, false otherwise
+ */
+static inline bool dma_irqn_get_channel_status(uint irq_index, uint channel) {
+ invalid_params_if(DMA, irq_index > 1);
+ check_dma_channel_param(channel);
+ return (irq_index ? dma_hw->ints1 : dma_hw->ints0) & (1u << channel);
+}
+
+/*! \brief Acknowledge a channel IRQ, resetting it as the cause of DMA_IRQ_0
+ * \ingroup hardware_dma
+ *
+ * \param channel DMA channel
+ */
+static inline void dma_channel_acknowledge_irq0(uint channel) {
+ check_dma_channel_param(channel);
+ hw_set_bits(&dma_hw->ints0, (1u << channel));
+}
+
+/*! \brief Acknowledge a channel IRQ, resetting it as the cause of DMA_IRQ_1
+ * \ingroup hardware_dma
+ *
+ * \param channel DMA channel
+ */
+static inline void dma_channel_acknowledge_irq1(uint channel) {
+ check_dma_channel_param(channel);
+ hw_set_bits(&dma_hw->ints1, (1u << channel));
+}
+
+/*! \brief Acknowledge a channel IRQ, resetting it as the cause of DMA_IRQ_N
+ * \ingroup hardware_dma
+ *
+ * \param irq_index the IRQ index; either 0 or 1 for DMA_IRQ_0 or DMA_IRQ_1
+ * \param channel DMA channel
+ */
+static inline void dma_irqn_acknowledge_channel(uint irq_index, uint channel) {
+ invalid_params_if(DMA, irq_index > 1);
+ check_dma_channel_param(channel);
+ hw_set_bits(irq_index ? &dma_hw->ints1 : &dma_hw->ints0, (1u << channel));
+}
+
+/*! \brief Check if DMA channel is busy
+ * \ingroup hardware_dma
+ *
+ * \param channel DMA channel
+ * \return true if the channel is currently busy
+ */
+inline static bool dma_channel_is_busy(uint channel) {
+ check_dma_channel_param(channel);
+ return !!(dma_hw->ch[channel].al1_ctrl & DMA_CH0_CTRL_TRIG_BUSY_BITS);
+}
+
+/*! \brief Wait for a DMA channel transfer to complete
+ * \ingroup hardware_dma
+ *
+ * \param channel DMA channel
+ */
+inline static void dma_channel_wait_for_finish_blocking(uint channel) {
+ while (dma_channel_is_busy(channel)) tight_loop_contents();
+ // stop the compiler hoisting a non volatile buffer access above the DMA completion.
+ __compiler_memory_barrier();
+}
+
+/*! \brief Enable the DMA sniffing targeting the specified channel
+ * \ingroup hardware_dma
+ *
+ * The mode can be one of the following:
+ *
+ * Mode | Function
+ * -----|---------
+ * 0x0 | Calculate a CRC-32 (IEEE802.3 polynomial)
+ * 0x1 | Calculate a CRC-32 (IEEE802.3 polynomial) with bit reversed data
+ * 0x2 | Calculate a CRC-16-CCITT
+ * 0x3 | Calculate a CRC-16-CCITT with bit reversed data
+ * 0xe | XOR reduction over all data. == 1 if the total 1 population count is odd.
+ * 0xf | Calculate a simple 32-bit checksum (addition with a 32 bit accumulator)
+ *
+ * \param channel DMA channel
+ * \param mode See description
+ * \param force_channel_enable Set true to also turn on sniffing in the channel configuration (this
+ * is usually what you want, but sometimes you might have a chain DMA with only certain segments
+ * of the chain sniffed, in which case you might pass false).
+ */
+inline static void dma_sniffer_enable(uint channel, uint mode, bool force_channel_enable) {
+ check_dma_channel_param(channel);
+ check_hw_layout(dma_hw_t, sniff_ctrl, DMA_SNIFF_CTRL_OFFSET);
+ if (force_channel_enable) {
+ hw_set_bits(&dma_hw->ch[channel].al1_ctrl, DMA_CH0_CTRL_TRIG_SNIFF_EN_BITS);
+ }
+ dma_hw->sniff_ctrl = ((channel << DMA_SNIFF_CTRL_DMACH_LSB) & DMA_SNIFF_CTRL_DMACH_BITS) |
+ ((mode << DMA_SNIFF_CTRL_CALC_LSB) & DMA_SNIFF_CTRL_CALC_BITS) |
+ DMA_SNIFF_CTRL_EN_BITS;
+}
+
+/*! \brief Enable the Sniffer byte swap function
+ * \ingroup hardware_dma
+ *
+ * Locally perform a byte reverse on the sniffed data, before feeding into checksum.
+ *
+ * Note that the sniff hardware is downstream of the DMA channel byteswap performed in the
+ * read master: if channel_config_set_bswap() and dma_sniffer_set_byte_swap_enabled() are both enabled,
+ * their effects cancel from the sniffer’s point of view.
+ *
+ * \param swap Set true to enable byte swapping
+ */
+inline static void dma_sniffer_set_byte_swap_enabled(bool swap) {
+ if (swap)
+ hw_set_bits(&dma_hw->sniff_ctrl, DMA_SNIFF_CTRL_BSWAP_BITS);
+ else
+ hw_clear_bits(&dma_hw->sniff_ctrl, DMA_SNIFF_CTRL_BSWAP_BITS);
+}
+
+/*! \brief Disable the DMA sniffer
+ * \ingroup hardware_dma
+ *
+ */
+inline static void dma_sniffer_disable(void) {
+ dma_hw->sniff_ctrl = 0;
+}
+
+/*! \brief Mark a dma timer as used
+ * \ingroup hardware_dma
+ *
+ * Method for cooperative claiming of hardware. Will cause a panic if the timer
+ * is already claimed. Use of this method by libraries detects accidental
+ * configurations that would fail in unpredictable ways.
+ *
+ * \param timer the dma timer
+ */
+void dma_timer_claim(uint timer);
+
+/*! \brief Mark a dma timer as no longer used
+ * \ingroup hardware_dma
+ *
+ * Method for cooperative claiming of hardware.
+ *
+ * \param timer the dma timer to release
+ */
+void dma_timer_unclaim(uint timer);
+
+/*! \brief Claim a free dma timer
+ * \ingroup hardware_dma
+ *
+ * \param required if true the function will panic if none are available
+ * \return the dma timer number or -1 if required was false, and none were free
+ */
+int dma_claim_unused_timer(bool required);
+
+/*! \brief Determine if a dma timer is claimed
+ * \ingroup hardware_dma
+ *
+ * \param timer the dma timer
+ * \return true if the timer is claimed, false otherwise
+ * \see dma_timer_claim
+ */
+bool dma_timer_is_claimed(uint timer);
+
+/*! \brief Set the divider for the given DMA timer
+ * \ingroup hardware_dma
+ *
+ * The timer will run at the system_clock_freq * numerator / denominator, so this is the speed
+ * that data elements will be transferred at via a DMA channel using this timer as a DREQ
+ *
+ * \param timer the dma timer
+ * \param numerator the fraction's numerator
+ * \param denominator the fraction's denominator
+ */
+static inline void dma_timer_set_fraction(uint timer, uint16_t numerator, uint16_t denominator) {
+ check_dma_timer_param(timer);
+ dma_hw->timer[timer] = (((uint32_t)numerator) << DMA_TIMER0_X_LSB) | (((uint32_t)denominator) << DMA_TIMER0_Y_LSB);
+}
+
+/*! \brief Return the DREQ number for a given DMA timer
+ * \ingroup hardware_dma
+ *
+ * \param timer_num DMA timer number 0-3
+ */
+static inline uint dma_get_timer_dreq(uint timer_num) {
+ static_assert(DREQ_DMA_TIMER1 == DREQ_DMA_TIMER0 + 1, "");
+ static_assert(DREQ_DMA_TIMER2 == DREQ_DMA_TIMER0 + 2, "");
+ static_assert(DREQ_DMA_TIMER3 == DREQ_DMA_TIMER0 + 3, "");
+ check_dma_timer_param(timer_num);
+ return DREQ_DMA_TIMER0 + timer_num;
+}
+
+#ifndef NDEBUG
+void print_dma_ctrl(dma_channel_hw_t *channel);
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/rp2_common/hardware_exception/CMakeLists.txt b/src/rp2_common/hardware_exception/CMakeLists.txt
new file mode 100644
index 0000000..a994dc0
--- /dev/null
+++ b/src/rp2_common/hardware_exception/CMakeLists.txt
@@ -0,0 +1 @@
+pico_simple_hardware_target(exception)
\ No newline at end of file
diff --git a/src/rp2_common/hardware_exception/exception.c b/src/rp2_common/hardware_exception/exception.c
new file mode 100644
index 0000000..8a9a108
--- /dev/null
+++ b/src/rp2_common/hardware_exception/exception.c
@@ -0,0 +1,65 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "hardware/exception.h"
+#include "hardware/regs/m0plus.h"
+#include "hardware/platform_defs.h"
+#include "hardware/structs/scb.h"
+
+#include "pico/mutex.h"
+#include "pico/assert.h"
+
+#ifndef exception_is_compile_time_default
+static bool exception_is_compile_time_default(exception_handler_t handler) {
+ extern char __default_isrs_start;
+ extern char __default_isrs_end;
+ return ((uintptr_t)handler) >= (uintptr_t)&__default_isrs_start &&
+ ((uintptr_t)handler) < (uintptr_t)&__default_isrs_end;
+}
+#endif
+
+static inline exception_handler_t *get_vtable(void) {
+ return (exception_handler_t *) scb_hw->vtor;
+}
+
+static void set_raw_exception_handler_and_restore_interrupts(enum exception_number num, exception_handler_t handler, uint32_t save) {
+ // update vtable (vtable_handler may be same or updated depending on cases, but we do it anyway for compactness)
+ get_vtable()[16 + num] = handler;
+ __dmb();
+ restore_interrupts(save);
+}
+
+static inline void check_exception_param(__unused enum exception_number num) {
+ invalid_params_if(EXCEPTION, num < NMI_EXCEPTION || num >=0);
+}
+
+exception_handler_t exception_get_vtable_handler(enum exception_number num) {
+ check_exception_param(num);
+ return get_vtable()[16 + num];
+}
+
+exception_handler_t exception_set_exclusive_handler(enum exception_number num, exception_handler_t handler) {
+ check_exception_param(num);
+#if !PICO_NO_RAM_VECTOR_TABLE
+ uint32_t save = save_and_disable_interrupts();
+ exception_handler_t current = exception_get_vtable_handler(num);
+ hard_assert(handler == current || exception_is_compile_time_default(current));
+ set_raw_exception_handler_and_restore_interrupts(num, handler, save);
+#else
+ panic_unsupported();
+#endif
+ return current;
+}
+
+void exception_restore_handler(enum exception_number num, exception_handler_t original_handler) {
+ hard_assert(exception_is_compile_time_default(original_handler));
+#if !PICO_NO_RAM_VECTOR_TABLE
+ uint32_t save = save_and_disable_interrupts();
+ set_raw_exception_handler_and_restore_interrupts(num, original_handler, save);
+#else
+ panic_unsupported();
+#endif
+}
\ No newline at end of file
diff --git a/src/rp2_common/hardware_exception/include/hardware/exception.h b/src/rp2_common/hardware_exception/include/hardware/exception.h
new file mode 100644
index 0000000..005168b
--- /dev/null
+++ b/src/rp2_common/hardware_exception/include/hardware/exception.h
@@ -0,0 +1,106 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _HARDWARE_EXCEPTION_H_
+#define _HARDWARE_EXCEPTION_H_
+
+#include "pico.h"
+#include "hardware/address_mapped.h"
+#include "hardware/regs/m0plus.h"
+
+/** \file exception.h
+ * \defgroup hardware_exception hardware_exception
+ *
+ * Methods for setting processor exception handlers
+ *
+ * Exceptions are identified by a \ref exception_number which is a number from -15 to -1; these are the numbers relative to
+ * the index of the first IRQ vector in the vector table. (i.e. vector table index is exception_num plus 16)
+ *
+ * There is one set of exception handlers per core, so the exception handlers for each core as set by these methods are independent.
+ *
+ * \note That all exception APIs affect the executing core only (i.e. the core calling the function).
+ */
+
+// PICO_CONFIG: PARAM_ASSERTIONS_ENABLED_EXCEPTION, Enable/disable assertions in the exception module, type=bool, default=0, group=hardware_exception
+#ifndef PARAM_ASSERTIONS_ENABLED_EXCEPTION
+#define PARAM_ASSERTIONS_ENABLED_EXCEPTION 0
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*! \brief Exception number definitions
+ *
+ * Note for consistency with irq numbers, these numbers are defined to be negative. The VTABLE index is
+ * the number here plus 16.
+ *
+ * Name | Value | Exception
+ * ---------------------|-------|----------
+ * NMI_EXCEPTION | -14 | Non Maskable Interrupt
+ * HARDFAULT_EXCEPTION | -13 | HardFault
+ * SVCALL_EXCEPTION | -5 | SV Call
+ * PENDSV_EXCEPTION | -2 | Pend SV
+ * SYSTICK_EXCEPTION | -1 | System Tick
+ *
+ * \ingroup hardware_exception
+ */
+enum exception_number {
+ NMI_EXCEPTION = -14, /* Non Maskable Interrupt */
+ HARDFAULT_EXCEPTION = -13, /* HardFault Interrupt */
+ SVCALL_EXCEPTION = -5, /* SV Call Interrupt */
+ PENDSV_EXCEPTION = -2, /* Pend SV Interrupt */
+ SYSTICK_EXCEPTION = -1, /* System Tick Interrupt */
+};
+
+/*! \brief Exception handler function type
+ * \ingroup hardware_exception
+ *
+ * All exception handlers should be of this type, and follow normal ARM EABI register saving conventions
+ */
+typedef void (*exception_handler_t)(void);
+
+/*! \brief Set the exception handler for an exception on the executing core.
+ * \ingroup hardware_exception
+ *
+ * This method will assert if an exception handler has been set for this exception number on this core via
+ * this method, without an intervening restore via exception_restore_handler.
+ *
+ * \note this method may not be used to override an exception handler that was specified at link time by
+ * providing a strong replacement for the weakly defined stub exception handlers. It will assert in this case too.
+ *
+ * \param num Exception number
+ * \param handler The handler to set
+ * \see exception_number
+ */
+exception_handler_t exception_set_exclusive_handler(enum exception_number num, exception_handler_t handler);
+
+/*! \brief Restore the original exception handler for an exception on this core
+ * \ingroup hardware_exception
+ *
+ * This method may be used to restore the exception handler for an exception on this core to the state
+ * prior to the call to exception_set_exclusive_handler(), so that exception_set_exclusive_handler()
+ * may be called again in the future.
+ *
+ * \param num Exception number \ref exception_number
+ * \param original_handler The original handler returned from \ref exception_set_exclusive_handler
+ * \see exception_set_exclusive_handler()
+ */
+void exception_restore_handler(enum exception_number num, exception_handler_t original_handler);
+
+/*! \brief Get the current exception handler for the specified exception from the currently installed vector table
+ * of the execution core
+ * \ingroup hardware_exception
+ *
+ * \param num Exception number
+ * \return the address stored in the VTABLE for the given exception number
+ */
+exception_handler_t exception_get_vtable_handler(enum exception_number num);
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/rp2_common/hardware_flash/CMakeLists.txt b/src/rp2_common/hardware_flash/CMakeLists.txt
new file mode 100644
index 0000000..9682566
--- /dev/null
+++ b/src/rp2_common/hardware_flash/CMakeLists.txt
@@ -0,0 +1,2 @@
+pico_simple_hardware_target(flash)
+target_link_libraries(hardware_flash INTERFACE pico_bootrom)
diff --git a/src/rp2_common/hardware_flash/flash.c b/src/rp2_common/hardware_flash/flash.c
new file mode 100644
index 0000000..5699302
--- /dev/null
+++ b/src/rp2_common/hardware_flash/flash.c
@@ -0,0 +1,176 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "hardware/flash.h"
+#include "pico/bootrom.h"
+
+#include "hardware/structs/ssi.h"
+#include "hardware/structs/ioqspi.h"
+
+#define FLASH_BLOCK_ERASE_CMD 0xd8
+
+// Standard RUID instruction: 4Bh command prefix, 32 dummy bits, 64 data bits.
+#define FLASH_RUID_CMD 0x4b
+#define FLASH_RUID_DUMMY_BYTES 4
+#define FLASH_RUID_DATA_BYTES 8
+#define FLASH_RUID_TOTAL_BYTES (1 + FLASH_RUID_DUMMY_BYTES + FLASH_RUID_DATA_BYTES)
+
+//-----------------------------------------------------------------------------
+// Infrastructure for reentering XIP mode after exiting for programming (take
+// a copy of boot2 before XIP exit). Calling boot2 as a function works because
+// it accepts a return vector in LR (and doesn't trash r4-r7). Bootrom passes
+// NULL in LR, instructing boot2 to enter flash vector table's reset handler.
+
+#if !PICO_NO_FLASH
+
+#define BOOT2_SIZE_WORDS 64
+
+static uint32_t boot2_copyout[BOOT2_SIZE_WORDS];
+static bool boot2_copyout_valid = false;
+
+static void __no_inline_not_in_flash_func(flash_init_boot2_copyout)(void) {
+ if (boot2_copyout_valid)
+ return;
+ for (int i = 0; i < BOOT2_SIZE_WORDS; ++i)
+ boot2_copyout[i] = ((uint32_t *)XIP_BASE)[i];
+ __compiler_memory_barrier();
+ boot2_copyout_valid = true;
+}
+
+static void __no_inline_not_in_flash_func(flash_enable_xip_via_boot2)(void) {
+ ((void (*)(void))boot2_copyout+1)();
+}
+
+#else
+
+static void __no_inline_not_in_flash_func(flash_init_boot2_copyout)(void) {}
+
+static void __no_inline_not_in_flash_func(flash_enable_xip_via_boot2)(void) {
+ // Set up XIP for 03h read on bus access (slow but generic)
+ rom_flash_enter_cmd_xip_fn flash_enter_cmd_xip = (rom_flash_enter_cmd_xip_fn)rom_func_lookup_inline(ROM_FUNC_FLASH_ENTER_CMD_XIP);
+ assert(flash_enter_cmd_xip);
+ flash_enter_cmd_xip();
+}
+
+#endif
+
+//-----------------------------------------------------------------------------
+// Actual flash programming shims (work whether or not PICO_NO_FLASH==1)
+
+void __no_inline_not_in_flash_func(flash_range_erase)(uint32_t flash_offs, size_t count) {
+#ifdef PICO_FLASH_SIZE_BYTES
+ hard_assert(flash_offs + count <= PICO_FLASH_SIZE_BYTES);
+#endif
+ invalid_params_if(FLASH, flash_offs & (FLASH_SECTOR_SIZE - 1));
+ invalid_params_if(FLASH, count & (FLASH_SECTOR_SIZE - 1));
+ rom_connect_internal_flash_fn connect_internal_flash = (rom_connect_internal_flash_fn)rom_func_lookup_inline(ROM_FUNC_CONNECT_INTERNAL_FLASH);
+ rom_flash_exit_xip_fn flash_exit_xip = (rom_flash_exit_xip_fn)rom_func_lookup_inline(ROM_FUNC_FLASH_EXIT_XIP);
+ rom_flash_range_erase_fn flash_range_erase = (rom_flash_range_erase_fn)rom_func_lookup_inline(ROM_FUNC_FLASH_RANGE_ERASE);
+ rom_flash_flush_cache_fn flash_flush_cache = (rom_flash_flush_cache_fn)rom_func_lookup_inline(ROM_FUNC_FLASH_FLUSH_CACHE);
+ assert(connect_internal_flash && flash_exit_xip && flash_range_erase && flash_flush_cache);
+ flash_init_boot2_copyout();
+
+ // No flash accesses after this point
+ __compiler_memory_barrier();
+
+ connect_internal_flash();
+ flash_exit_xip();
+ flash_range_erase(flash_offs, count, FLASH_BLOCK_SIZE, FLASH_BLOCK_ERASE_CMD);
+ flash_flush_cache(); // Note this is needed to remove CSn IO force as well as cache flushing
+ flash_enable_xip_via_boot2();
+}
+
+void __no_inline_not_in_flash_func(flash_range_program)(uint32_t flash_offs, const uint8_t *data, size_t count) {
+#ifdef PICO_FLASH_SIZE_BYTES
+ hard_assert(flash_offs + count <= PICO_FLASH_SIZE_BYTES);
+#endif
+ invalid_params_if(FLASH, flash_offs & (FLASH_PAGE_SIZE - 1));
+ invalid_params_if(FLASH, count & (FLASH_PAGE_SIZE - 1));
+ rom_connect_internal_flash_fn connect_internal_flash = (rom_connect_internal_flash_fn)rom_func_lookup_inline(ROM_FUNC_CONNECT_INTERNAL_FLASH);
+ rom_flash_exit_xip_fn flash_exit_xip = (rom_flash_exit_xip_fn)rom_func_lookup_inline(ROM_FUNC_FLASH_EXIT_XIP);
+ rom_flash_range_program_fn flash_range_program = (rom_flash_range_program_fn)rom_func_lookup_inline(ROM_FUNC_FLASH_RANGE_PROGRAM);
+ rom_flash_flush_cache_fn flash_flush_cache = (rom_flash_flush_cache_fn)rom_func_lookup_inline(ROM_FUNC_FLASH_FLUSH_CACHE);
+ assert(connect_internal_flash && flash_exit_xip && flash_range_program && flash_flush_cache);
+ flash_init_boot2_copyout();
+
+ __compiler_memory_barrier();
+
+ connect_internal_flash();
+ flash_exit_xip();
+ flash_range_program(flash_offs, data, count);
+ flash_flush_cache(); // Note this is needed to remove CSn IO force as well as cache flushing
+ flash_enable_xip_via_boot2();
+}
+
+//-----------------------------------------------------------------------------
+// Lower-level flash access functions
+
+#if !PICO_NO_FLASH
+// Bitbanging the chip select using IO overrides, in case RAM-resident IRQs
+// are still running, and the FIFO bottoms out. (the bootrom does the same)
+static void __no_inline_not_in_flash_func(flash_cs_force)(bool high) {
+ uint32_t field_val = high ?
+ IO_QSPI_GPIO_QSPI_SS_CTRL_OUTOVER_VALUE_HIGH :
+ IO_QSPI_GPIO_QSPI_SS_CTRL_OUTOVER_VALUE_LOW;
+ hw_write_masked(&ioqspi_hw->io[1].ctrl,
+ field_val << IO_QSPI_GPIO_QSPI_SS_CTRL_OUTOVER_LSB,
+ IO_QSPI_GPIO_QSPI_SS_CTRL_OUTOVER_BITS
+ );
+}
+
+void __no_inline_not_in_flash_func(flash_do_cmd)(const uint8_t *txbuf, uint8_t *rxbuf, size_t count) {
+ rom_connect_internal_flash_fn connect_internal_flash = (rom_connect_internal_flash_fn)rom_func_lookup_inline(ROM_FUNC_CONNECT_INTERNAL_FLASH);
+ rom_flash_exit_xip_fn flash_exit_xip = (rom_flash_exit_xip_fn)rom_func_lookup_inline(ROM_FUNC_FLASH_EXIT_XIP);
+ rom_flash_flush_cache_fn flash_flush_cache = (rom_flash_flush_cache_fn)rom_func_lookup_inline(ROM_FUNC_FLASH_FLUSH_CACHE);
+ assert(connect_internal_flash && flash_exit_xip && flash_flush_cache);
+ flash_init_boot2_copyout();
+ __compiler_memory_barrier();
+ connect_internal_flash();
+ flash_exit_xip();
+
+ flash_cs_force(0);
+ size_t tx_remaining = count;
+ size_t rx_remaining = count;
+ // We may be interrupted -- don't want FIFO to overflow if we're distracted.
+ const size_t max_in_flight = 16 - 2;
+ while (tx_remaining || rx_remaining) {
+ uint32_t flags = ssi_hw->sr;
+ bool can_put = !!(flags & SSI_SR_TFNF_BITS);
+ bool can_get = !!(flags & SSI_SR_RFNE_BITS);
+ if (can_put && tx_remaining && rx_remaining - tx_remaining < max_in_flight) {
+ ssi_hw->dr0 = *txbuf++;
+ --tx_remaining;
+ }
+ if (can_get && rx_remaining) {
+ *rxbuf++ = (uint8_t)ssi_hw->dr0;
+ --rx_remaining;
+ }
+ }
+ flash_cs_force(1);
+
+ flash_flush_cache();
+ flash_enable_xip_via_boot2();
+}
+#endif
+
+// Use standard RUID command to get a unique identifier for the flash (and
+// hence the board)
+
+static_assert(FLASH_UNIQUE_ID_SIZE_BYTES == FLASH_RUID_DATA_BYTES, "");
+
+void flash_get_unique_id(uint8_t *id_out) {
+#if PICO_NO_FLASH
+ __unused uint8_t *ignore = id_out;
+ panic_unsupported();
+#else
+ uint8_t txbuf[FLASH_RUID_TOTAL_BYTES] = {0};
+ uint8_t rxbuf[FLASH_RUID_TOTAL_BYTES] = {0};
+ txbuf[0] = FLASH_RUID_CMD;
+ flash_do_cmd(txbuf, rxbuf, FLASH_RUID_TOTAL_BYTES);
+ for (int i = 0; i < FLASH_RUID_DATA_BYTES; i++)
+ id_out[i] = rxbuf[i + 1 + FLASH_RUID_DUMMY_BYTES];
+#endif
+}
diff --git a/src/rp2_common/hardware_flash/include/hardware/flash.h b/src/rp2_common/hardware_flash/include/hardware/flash.h
new file mode 100644
index 0000000..e6cd229
--- /dev/null
+++ b/src/rp2_common/hardware_flash/include/hardware/flash.h
@@ -0,0 +1,113 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _HARDWARE_FLASH_H
+#define _HARDWARE_FLASH_H
+
+#include "pico.h"
+
+/** \file flash.h
+ * \defgroup hardware_flash hardware_flash
+ *
+ * Low level flash programming and erase API
+ *
+ * Note these functions are *unsafe* if you have two cores concurrently
+ * executing from flash. In this case you must perform your own
+ * synchronisation to make sure no XIP accesses take place during flash
+ * programming.
+ *
+ * Likewise they are *unsafe* if you have interrupt handlers or an interrupt
+ * vector table in flash, so you must disable interrupts before calling in
+ * this case.
+ *
+ * If PICO_NO_FLASH=1 is not defined (i.e. if the program is built to run from
+ * flash) then these functions will make a static copy of the second stage
+ * bootloader in SRAM, and use this to reenter execute-in-place mode after
+ * programming or erasing flash, so that they can safely be called from
+ * flash-resident code.
+ *
+ * \subsection flash_example Example
+ * \include flash_program.c
+ */
+
+// PICO_CONFIG: PARAM_ASSERTIONS_ENABLED_FLASH, Enable/disable assertions in the flash module, type=bool, default=0, group=hardware_flash
+#ifndef PARAM_ASSERTIONS_ENABLED_FLASH
+#define PARAM_ASSERTIONS_ENABLED_FLASH 0
+#endif
+
+#define FLASH_PAGE_SIZE (1u << 8)
+#define FLASH_SECTOR_SIZE (1u << 12)
+#define FLASH_BLOCK_SIZE (1u << 16)
+
+#define FLASH_UNIQUE_ID_SIZE_BYTES 8
+
+// PICO_CONFIG: PICO_FLASH_SIZE_BYTES, size of primary flash in bytes, type=int, group=hardware_flash
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*! \brief Erase areas of flash
+ * \ingroup hardware_flash
+ *
+ * \param flash_offs Offset into flash, in bytes, to start the erase. Must be aligned to a 4096-byte flash sector.
+ * \param count Number of bytes to be erased. Must be a multiple of 4096 bytes (one sector).
+ */
+void flash_range_erase(uint32_t flash_offs, size_t count);
+
+/*! \brief Program flash
+ * \ingroup hardware_flash
+ *
+ * \param flash_offs Flash address of the first byte to be programmed. Must be aligned to a 256-byte flash page.
+ * \param data Pointer to the data to program into flash
+ * \param count Number of bytes to program. Must be a multiple of 256 bytes (one page).
+ */
+
+void flash_range_program(uint32_t flash_offs, const uint8_t *data, size_t count);
+
+/*! \brief Get flash unique 64 bit identifier
+ * \ingroup hardware_flash
+ *
+ * Use a standard 4Bh RUID instruction to retrieve the 64 bit unique
+ * identifier from a flash device attached to the QSPI interface. Since there
+ * is a 1:1 association between the MCU and this flash, this also serves as a
+ * unique identifier for the board.
+ *
+ * \param id_out Pointer to an 8-byte buffer to which the ID will be written
+ */
+void flash_get_unique_id(uint8_t *id_out);
+
+/*! \brief Execute bidirectional flash command
+ * \ingroup hardware_flash
+ *
+ * Low-level function to execute a serial command on a flash device attached
+ * to the QSPI interface. Bytes are simultaneously transmitted and received
+ * from txbuf and to rxbuf. Therefore, both buffers must be the same length,
+ * count, which is the length of the overall transaction. This is useful for
+ * reading metadata from the flash chip, such as device ID or SFDP
+ * parameters.
+ *
+ * The XIP cache is flushed following each command, in case flash state
+ * has been modified. Like other hardware_flash functions, the flash is not
+ * accessible for execute-in-place transfers whilst the command is in
+ * progress, so entering a flash-resident interrupt handler or executing flash
+ * code on the second core concurrently will be fatal. To avoid these pitfalls
+ * it is recommended that this function only be used to extract flash metadata
+ * during startup, before the main application begins to run: see the
+ * implementation of pico_get_unique_id() for an example of this.
+ *
+ * \param txbuf Pointer to a byte buffer which will be transmitted to the flash
+ * \param rxbuf Pointer to a byte buffer where data received from the flash will be written. txbuf and rxbuf may be the same buffer.
+ * \param count Length in bytes of txbuf and of rxbuf
+ */
+void flash_do_cmd(const uint8_t *txbuf, uint8_t *rxbuf, size_t count);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/rp2_common/hardware_gpio/CMakeLists.txt b/src/rp2_common/hardware_gpio/CMakeLists.txt
new file mode 100644
index 0000000..1bfb078
--- /dev/null
+++ b/src/rp2_common/hardware_gpio/CMakeLists.txt
@@ -0,0 +1 @@
+pico_simple_hardware_target(gpio)
\ No newline at end of file
diff --git a/src/rp2_common/hardware_gpio/gpio.c b/src/rp2_common/hardware_gpio/gpio.c
new file mode 100644
index 0000000..2816b97
--- /dev/null
+++ b/src/rp2_common/hardware_gpio/gpio.c
@@ -0,0 +1,228 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "hardware/gpio.h"
+#include "hardware/sync.h"
+
+#include "hardware/structs/iobank0.h"
+#include "hardware/irq.h"
+
+#if LIB_PICO_BINARY_INFO
+#include "pico/binary_info.h"
+#endif
+
+static gpio_irq_callback_t _callbacks[NUM_CORES];
+
+// Get the raw value from the pin, bypassing any muxing or overrides.
+int gpio_get_pad(uint gpio) {
+ invalid_params_if(GPIO, gpio >= NUM_BANK0_GPIOS);
+ hw_set_bits(&padsbank0_hw->io[gpio], PADS_BANK0_GPIO0_IE_BITS);
+ return (iobank0_hw->io[gpio].status & IO_BANK0_GPIO0_STATUS_INFROMPAD_BITS)
+ >> IO_BANK0_GPIO0_STATUS_INFROMPAD_LSB;
+}
+
+/// \tag::gpio_set_function[]
+// Select function for this GPIO, and ensure input/output are enabled at the pad.
+// This also clears the input/output/irq override bits.
+void gpio_set_function(uint gpio, enum gpio_function fn) {
+ invalid_params_if(GPIO, gpio >= NUM_BANK0_GPIOS);
+ invalid_params_if(GPIO, ((uint32_t)fn << IO_BANK0_GPIO0_CTRL_FUNCSEL_LSB) & ~IO_BANK0_GPIO0_CTRL_FUNCSEL_BITS);
+ // Set input enable on, output disable off
+ hw_write_masked(&padsbank0_hw->io[gpio],
+ PADS_BANK0_GPIO0_IE_BITS,
+ PADS_BANK0_GPIO0_IE_BITS | PADS_BANK0_GPIO0_OD_BITS
+ );
+ // Zero all fields apart from fsel; we want this IO to do what the peripheral tells it.
+ // This doesn't affect e.g. pullup/pulldown, as these are in pad controls.
+ iobank0_hw->io[gpio].ctrl = fn << IO_BANK0_GPIO0_CTRL_FUNCSEL_LSB;
+}
+/// \end::gpio_set_function[]
+
+enum gpio_function gpio_get_function(uint gpio) {
+ invalid_params_if(GPIO, gpio >= NUM_BANK0_GPIOS);
+ return (enum gpio_function) ((iobank0_hw->io[gpio].ctrl & IO_BANK0_GPIO0_CTRL_FUNCSEL_BITS) >> IO_BANK0_GPIO0_CTRL_FUNCSEL_LSB);
+}
+
+// Note that, on RP2040, setting both pulls enables a "bus keep" function,
+// i.e. weak pull to whatever is current high/low state of GPIO.
+void gpio_set_pulls(uint gpio, bool up, bool down) {
+ invalid_params_if(GPIO, gpio >= NUM_BANK0_GPIOS);
+ hw_write_masked(
+ &padsbank0_hw->io[gpio],
+ (bool_to_bit(up) << PADS_BANK0_GPIO0_PUE_LSB) | (bool_to_bit(down) << PADS_BANK0_GPIO0_PDE_LSB),
+ PADS_BANK0_GPIO0_PUE_BITS | PADS_BANK0_GPIO0_PDE_BITS
+ );
+}
+
+// Direct override for per-GPIO IRQ signal
+void gpio_set_irqover(uint gpio, uint value) {
+ invalid_params_if(GPIO, gpio >= NUM_BANK0_GPIOS);
+ hw_write_masked(&iobank0_hw->io[gpio].ctrl,
+ value << IO_BANK0_GPIO0_CTRL_IRQOVER_LSB,
+ IO_BANK0_GPIO0_CTRL_IRQOVER_BITS
+ );
+}
+
+// Direct overrides for pad controls
+void gpio_set_inover(uint gpio, uint value) {
+ invalid_params_if(GPIO, gpio >= NUM_BANK0_GPIOS);
+ hw_write_masked(&iobank0_hw->io[gpio].ctrl,
+ value << IO_BANK0_GPIO0_CTRL_INOVER_LSB,
+ IO_BANK0_GPIO0_CTRL_INOVER_BITS
+ );
+}
+
+void gpio_set_outover(uint gpio, uint value) {
+ invalid_params_if(GPIO, gpio >= NUM_BANK0_GPIOS);
+ hw_write_masked(&iobank0_hw->io[gpio].ctrl,
+ value << IO_BANK0_GPIO0_CTRL_OUTOVER_LSB,
+ IO_BANK0_GPIO0_CTRL_OUTOVER_BITS
+ );
+}
+
+void gpio_set_oeover(uint gpio, uint value) {
+ invalid_params_if(GPIO, gpio >= NUM_BANK0_GPIOS);
+ hw_write_masked(&iobank0_hw->io[gpio].ctrl,
+ value << IO_BANK0_GPIO0_CTRL_OEOVER_LSB,
+ IO_BANK0_GPIO0_CTRL_OEOVER_BITS
+ );
+}
+
+void gpio_set_input_hysteresis_enabled(uint gpio, bool enabled) {
+ invalid_params_if(GPIO, gpio >= NUM_BANK0_GPIOS);
+ if (enabled)
+ hw_set_bits(&padsbank0_hw->io[gpio], PADS_BANK0_GPIO0_SCHMITT_BITS);
+ else
+ hw_clear_bits(&padsbank0_hw->io[gpio], PADS_BANK0_GPIO0_SCHMITT_BITS);
+}
+
+
+bool gpio_is_input_hysteresis_enabled(uint gpio) {
+ invalid_params_if(GPIO, gpio >= NUM_BANK0_GPIOS);
+ return (padsbank0_hw->io[gpio] & PADS_BANK0_GPIO0_SCHMITT_BITS) != 0;
+}
+
+void gpio_set_slew_rate(uint gpio, enum gpio_slew_rate slew) {
+ invalid_params_if(GPIO, gpio >= NUM_BANK0_GPIOS);
+ hw_write_masked(&padsbank0_hw->io[gpio],
+ (uint)slew << PADS_BANK0_GPIO0_SLEWFAST_LSB,
+ PADS_BANK0_GPIO0_SLEWFAST_BITS
+ );
+}
+
+enum gpio_slew_rate gpio_get_slew_rate(uint gpio) {
+ invalid_params_if(GPIO, gpio >= NUM_BANK0_GPIOS);
+ return (enum gpio_slew_rate)((padsbank0_hw->io[gpio]
+ & PADS_BANK0_GPIO0_SLEWFAST_BITS)
+ >> PADS_BANK0_GPIO0_SLEWFAST_LSB);
+}
+
+
+// Enum encoding should match hardware encoding on RP2040
+static_assert(PADS_BANK0_GPIO0_DRIVE_VALUE_8MA == GPIO_DRIVE_STRENGTH_8MA, "");
+void gpio_set_drive_strength(uint gpio, enum gpio_drive_strength drive) {
+ invalid_params_if(GPIO, gpio >= NUM_BANK0_GPIOS);
+ hw_write_masked(&padsbank0_hw->io[gpio],
+ (uint)drive << PADS_BANK0_GPIO0_DRIVE_LSB,
+ PADS_BANK0_GPIO0_DRIVE_BITS
+ );
+}
+
+enum gpio_drive_strength gpio_get_drive_strength(uint gpio) {
+ invalid_params_if(GPIO, gpio >= NUM_BANK0_GPIOS);
+ return (enum gpio_drive_strength)((padsbank0_hw->io[gpio]
+ & PADS_BANK0_GPIO0_DRIVE_BITS)
+ >> PADS_BANK0_GPIO0_DRIVE_LSB);
+}
+
+static void gpio_irq_handler(void) {
+ io_irq_ctrl_hw_t *irq_ctrl_base = get_core_num() ?
+ &iobank0_hw->proc1_irq_ctrl : &iobank0_hw->proc0_irq_ctrl;
+ for (uint gpio = 0; gpio < NUM_BANK0_GPIOS; gpio++) {
+ io_ro_32 *status_reg = &irq_ctrl_base->ints[gpio / 8];
+ uint events = (*status_reg >> 4 * (gpio % 8)) & 0xf;
+ if (events) {
+ // TODO: If both cores care about this event then the second core won't get the irq?
+ gpio_acknowledge_irq(gpio, events);
+ gpio_irq_callback_t callback = _callbacks[get_core_num()];
+ if (callback) {
+ callback(gpio, events);
+ }
+ }
+ }
+}
+
+static void _gpio_set_irq_enabled(uint gpio, uint32_t events, bool enabled, io_irq_ctrl_hw_t *irq_ctrl_base) {
+ // Clear stale events which might cause immediate spurious handler entry
+ gpio_acknowledge_irq(gpio, events);
+
+ io_rw_32 *en_reg = &irq_ctrl_base->inte[gpio / 8];
+ events <<= 4 * (gpio % 8);
+
+ if (enabled)
+ hw_set_bits(en_reg, events);
+ else
+ hw_clear_bits(en_reg, events);
+}
+
+void gpio_set_irq_enabled(uint gpio, uint32_t events, bool enabled) {
+ // Separate mask/force/status per-core, so check which core called, and
+ // set the relevant IRQ controls.
+ io_irq_ctrl_hw_t *irq_ctrl_base = get_core_num() ?
+ &iobank0_hw->proc1_irq_ctrl : &iobank0_hw->proc0_irq_ctrl;
+ _gpio_set_irq_enabled(gpio, events, enabled, irq_ctrl_base);
+}
+
+void gpio_set_irq_enabled_with_callback(uint gpio, uint32_t events, bool enabled, gpio_irq_callback_t callback) {
+ gpio_set_irq_enabled(gpio, events, enabled);
+
+ // TODO: Do we want to support a callback per GPIO pin?
+ // Install IRQ handler
+ _callbacks[get_core_num()] = callback;
+ irq_set_exclusive_handler(IO_IRQ_BANK0, gpio_irq_handler);
+ irq_set_enabled(IO_IRQ_BANK0, true);
+}
+
+void gpio_set_dormant_irq_enabled(uint gpio, uint32_t events, bool enabled) {
+ io_irq_ctrl_hw_t *irq_ctrl_base = &iobank0_hw->dormant_wake_irq_ctrl;
+ _gpio_set_irq_enabled(gpio, events, enabled, irq_ctrl_base);
+}
+
+void gpio_acknowledge_irq(uint gpio, uint32_t events) {
+ iobank0_hw->intr[gpio / 8] = events << 4 * (gpio % 8);
+}
+
+#define DEBUG_PIN_MASK (((1u << PICO_DEBUG_PIN_COUNT)-1) << PICO_DEBUG_PIN_BASE)
+void gpio_debug_pins_init() {
+ gpio_init_mask(DEBUG_PIN_MASK);
+ gpio_set_dir_masked(DEBUG_PIN_MASK, DEBUG_PIN_MASK);
+#if LIB_PICO_BINARY_INFO
+ bi_decl_if_func_used(bi_pin_mask_with_names(DEBUG_PIN_MASK, "Debug"));
+#endif
+}
+
+void gpio_set_input_enabled(uint gpio, bool enabled) {
+ if (enabled)
+ hw_set_bits(&padsbank0_hw->io[gpio], PADS_BANK0_GPIO0_IE_BITS);
+ else
+ hw_clear_bits(&padsbank0_hw->io[gpio], PADS_BANK0_GPIO0_IE_BITS);
+}
+
+void gpio_init(uint gpio) {
+ sio_hw->gpio_oe_clr = 1ul << gpio;
+ sio_hw->gpio_clr = 1ul << gpio;
+ gpio_set_function(gpio, GPIO_FUNC_SIO);
+}
+
+void gpio_init_mask(uint gpio_mask) {
+ for(uint i=0;i<32;i++) {
+ if (gpio_mask & 1) {
+ gpio_init(i);
+ }
+ gpio_mask >>= 1;
+ }
+}
+
diff --git a/src/rp2_common/hardware_gpio/include/hardware/gpio.h b/src/rp2_common/hardware_gpio/include/hardware/gpio.h
new file mode 100644
index 0000000..7037e85
--- /dev/null
+++ b/src/rp2_common/hardware_gpio/include/hardware/gpio.h
@@ -0,0 +1,656 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _HARDWARE_GPIO_H_
+#define _HARDWARE_GPIO_H_
+
+#include "pico.h"
+#include "hardware/structs/sio.h"
+#include "hardware/structs/padsbank0.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// PICO_CONFIG: PARAM_ASSERTIONS_ENABLED_GPIO, Enable/disable assertions in the GPIO module, type=bool, default=0, group=hardware_gpio
+#ifndef PARAM_ASSERTIONS_ENABLED_GPIO
+#define PARAM_ASSERTIONS_ENABLED_GPIO 0
+#endif
+
+/** \file gpio.h
+ * \defgroup hardware_gpio hardware_gpio
+ *
+ * General Purpose Input/Output (GPIO) API
+ *
+ * RP2040 has 36 multi-functional General Purpose Input / Output (GPIO) pins, divided into two banks. In a typical use case,
+ * the pins in the QSPI bank (QSPI_SS, QSPI_SCLK and QSPI_SD0 to QSPI_SD3) are used to execute code from an external
+ * flash device, leaving the User bank (GPIO0 to GPIO29) for the programmer to use. All GPIOs support digital input and
+ * output, but GPIO26 to GPIO29 can also be used as inputs to the chip’s Analogue to Digital Converter (ADC). Each GPIO
+ * can be controlled directly by software running on the processors, or by a number of other functional blocks.
+ *
+ * The function allocated to each GPIO is selected by calling the \ref gpio_set_function function. \note Not all functions
+ * are available on all pins.
+ *
+ * Each GPIO can have one function selected at a time. Likewise, each peripheral input (e.g. UART0 RX) should only be selected on
+ * one _GPIO_ at a time. If the same peripheral input is connected to multiple GPIOs, the peripheral sees the logical OR of these
+ * GPIO inputs. Please refer to the datasheet for more information on GPIO function select.
+ *
+ * ### Function Select Table
+ *
+ * GPIO | F1 | F2 | F3 | F4 | F5 | F6 | F7 | F8 | F9
+ * -------|----------|-----------|----------|--------|-----|------|------|---------------|----
+ * 0 | SPI0 RX | UART0 TX | I2C0 SDA | PWM0 A | SIO | PIO0 | PIO1 | | USB OVCUR DET
+ * 1 | SPI0 CSn | UART0 RX | I2C0 SCL | PWM0 B | SIO | PIO0 | PIO1 | | USB VBUS DET
+ * 2 | SPI0 SCK | UART0 CTS | I2C1 SDA | PWM1 A | SIO | PIO0 | PIO1 | | USB VBUS EN
+ * 3 | SPI0 TX | UART0 RTS | I2C1 SCL | PWM1 B | SIO | PIO0 | PIO1 | | USB OVCUR DET
+ * 4 | SPI0 RX | UART1 TX | I2C0 SDA | PWM2 A | SIO | PIO0 | PIO1 | | USB VBUS DET
+ * 5 | SPI0 CSn | UART1 RX | I2C0 SCL | PWM2 B | SIO | PIO0 | PIO1 | | USB VBUS EN
+ * 6 | SPI0 SCK | UART1 CTS | I2C1 SDA | PWM3 A | SIO | PIO0 | PIO1 | | USB OVCUR DET
+ * 7 | SPI0 TX | UART1 RTS | I2C1 SCL | PWM3 B | SIO | PIO0 | PIO1 | | USB VBUS DET
+ * 8 | SPI1 RX | UART1 TX | I2C0 SDA | PWM4 A | SIO | PIO0 | PIO1 | | USB VBUS EN
+ * 9 | SPI1 CSn | UART1 RX | I2C0 SCL | PWM4 B | SIO | PIO0 | PIO1 | | USB OVCUR DET
+ * 10 | SPI1 SCK | UART1 CTS | I2C1 SDA | PWM5 A | SIO | PIO0 | PIO1 | | USB VBUS DET
+ * 11 | SPI1 TX | UART1 RTS | I2C1 SCL | PWM5 B | SIO | PIO0 | PIO1 | | USB VBUS EN
+ * 12 | SPI1 RX | UART0 TX | I2C0 SDA | PWM6 A | SIO | PIO0 | PIO1 | | USB OVCUR DET
+ * 13 | SPI1 CSn | UART0 RX | I2C0 SCL | PWM6 B | SIO | PIO0 | PIO1 | | USB VBUS DET
+ * 14 | SPI1 SCK | UART0 CTS | I2C1 SDA | PWM7 A | SIO | PIO0 | PIO1 | | USB VBUS EN
+ * 15 | SPI1 TX | UART0 RTS | I2C1 SCL | PWM7 B | SIO | PIO0 | PIO1 | | USB OVCUR DET
+ * 16 | SPI0 RX | UART0 TX | I2C0 SDA | PWM0 A | SIO | PIO0 | PIO1 | | USB VBUS DET
+ * 17 | SPI0 CSn | UART0 RX | I2C0 SCL | PWM0 B | SIO | PIO0 | PIO1 | | USB VBUS EN
+ * 18 | SPI0 SCK | UART0 CTS | I2C1 SDA | PWM1 A | SIO | PIO0 | PIO1 | | USB OVCUR DET
+ * 19 | SPI0 TX | UART0 RTS | I2C1 SCL | PWM1 B | SIO | PIO0 | PIO1 | | USB VBUS DET
+ * 20 | SPI0 RX | UART1 TX | I2C0 SDA | PWM2 A | SIO | PIO0 | PIO1 | CLOCK GPIN0 | USB VBUS EN
+ * 21 | SPI0 CSn | UART1 RX | I2C0 SCL | PWM2 B | SIO | PIO0 | PIO1 | CLOCK GPOUT0 | USB OVCUR DET
+ * 22 | SPI0 SCK | UART1 CTS | I2C1 SDA | PWM3 A | SIO | PIO0 | PIO1 | CLOCK GPIN1 | USB VBUS DET
+ * 23 | SPI0 TX | UART1 RTS | I2C1 SCL | PWM3 B | SIO | PIO0 | PIO1 | CLOCK GPOUT1 | USB VBUS EN
+ * 24 | SPI1 RX | UART1 TX | I2C0 SDA | PWM4 A | SIO | PIO0 | PIO1 | CLOCK GPOUT2 | USB OVCUR DET
+ * 25 | SPI1 CSn | UART1 RX | I2C0 SCL | PWM4 B | SIO | PIO0 | PIO1 | CLOCK GPOUT3 | USB VBUS DET
+ * 26 | SPI1 SCK | UART1 CTS | I2C1 SDA | PWM5 A | SIO | PIO0 | PIO1 | | USB VBUS EN
+ * 27 | SPI1 TX | UART1 RTS | I2C1 SCL | PWM5 B | SIO | PIO0 | PIO1 | | USB OVCUR DET
+ * 28 | SPI1 RX | UART0 TX | I2C0 SDA | PWM6 A | SIO | PIO0 | PIO1 | | USB VBUS DET
+ * 29 | SPI1 CSn | UART0 RX | I2C0 SCL | PWM6 B | SIO | PIO0 | PIO1 | | USB VBUS EN
+
+ */
+
+/*! \brief GPIO function definitions for use with function select
+ * \ingroup hardware_gpio
+ * \brief GPIO function selectors
+ *
+ * Each GPIO can have one function selected at a time. Likewise, each peripheral input (e.g. UART0 RX) should only be
+ * selected on one GPIO at a time. If the same peripheral input is connected to multiple GPIOs, the peripheral sees the logical
+ * OR of these GPIO inputs.
+ *
+ * Please refer to the datsheet for more information on GPIO function selection.
+ */
+enum gpio_function {
+ GPIO_FUNC_XIP = 0,
+ GPIO_FUNC_SPI = 1,
+ GPIO_FUNC_UART = 2,
+ GPIO_FUNC_I2C = 3,
+ GPIO_FUNC_PWM = 4,
+ GPIO_FUNC_SIO = 5,
+ GPIO_FUNC_PIO0 = 6,
+ GPIO_FUNC_PIO1 = 7,
+ GPIO_FUNC_GPCK = 8,
+ GPIO_FUNC_USB = 9,
+ GPIO_FUNC_NULL = 0x1f,
+};
+
+#define GPIO_OUT 1
+#define GPIO_IN 0
+
+/*! \brief GPIO Interrupt level definitions
+ * \ingroup hardware_gpio
+ * \brief GPIO Interrupt levels
+ *
+ * An interrupt can be generated for every GPIO pin in 4 scenarios:
+ *
+ * * Level High: the GPIO pin is a logical 1
+ * * Level Low: the GPIO pin is a logical 0
+ * * Edge High: the GPIO has transitioned from a logical 0 to a logical 1
+ * * Edge Low: the GPIO has transitioned from a logical 1 to a logical 0
+ *
+ * The level interrupts are not latched. This means that if the pin is a logical 1 and the level high interrupt is active, it will
+ * become inactive as soon as the pin changes to a logical 0. The edge interrupts are stored in the INTR register and can be
+ * cleared by writing to the INTR register.
+ */
+enum gpio_irq_level {
+ GPIO_IRQ_LEVEL_LOW = 0x1u,
+ GPIO_IRQ_LEVEL_HIGH = 0x2u,
+ GPIO_IRQ_EDGE_FALL = 0x4u,
+ GPIO_IRQ_EDGE_RISE = 0x8u,
+};
+
+/*! Callback function type for GPIO events
+ * \ingroup hardware_gpio
+ *
+ * \param gpio Which GPIO caused this interrupt
+ * \param events Which events caused this interrupt. See \ref gpio_set_irq_enabled for details.
+ * \sa gpio_set_irq_enabled_with_callback()
+ */
+typedef void (*gpio_irq_callback_t)(uint gpio, uint32_t events);
+
+enum gpio_override {
+ GPIO_OVERRIDE_NORMAL = 0, ///< peripheral signal selected via \ref gpio_set_function
+ GPIO_OVERRIDE_INVERT = 1, ///< invert peripheral signal selected via \ref gpio_set_function
+ GPIO_OVERRIDE_LOW = 2, ///< drive low/disable output
+ GPIO_OVERRIDE_HIGH = 3, ///< drive high/enable output
+};
+
+/*! \brief Slew rate limiting levels for GPIO outputs
+ * \ingroup hardware_gpio
+ *
+ * Slew rate limiting increases the minimum rise/fall time when a GPIO output
+ * is lightly loaded, which can help to reduce electromagnetic emissions.
+ * \sa gpio_set_slew_rate
+ */
+enum gpio_slew_rate {
+ GPIO_SLEW_RATE_SLOW = 0, ///< Slew rate limiting enabled
+ GPIO_SLEW_RATE_FAST = 1 ///< Slew rate limiting disabled
+};
+
+/*! \brief Drive strength levels for GPIO outputs
+ * \ingroup hardware_gpio
+ *
+ * Drive strength levels for GPIO outputs.
+ * \sa gpio_set_drive_strength
+ */
+enum gpio_drive_strength {
+ GPIO_DRIVE_STRENGTH_2MA = 0, ///< 2 mA nominal drive strength
+ GPIO_DRIVE_STRENGTH_4MA = 1, ///< 4 mA nominal drive strength
+ GPIO_DRIVE_STRENGTH_8MA = 2, ///< 8 mA nominal drive strength
+ GPIO_DRIVE_STRENGTH_12MA = 3 ///< 12 mA nominal drive strength
+};
+
+// ----------------------------------------------------------------------------
+// Pad Controls + IO Muxing
+// ----------------------------------------------------------------------------
+// Declarations for gpio.c
+
+/*! \brief Select GPIO function
+ * \ingroup hardware_gpio
+ *
+ * \param gpio GPIO number
+ * \param fn Which GPIO function select to use from list \ref gpio_function
+ */
+void gpio_set_function(uint gpio, enum gpio_function fn);
+
+/*! \brief Determine current GPIO function
+ * \ingroup hardware_gpio
+ *
+ * \param gpio GPIO number
+ * \return Which GPIO function is currently selected from list \ref gpio_function
+ */
+enum gpio_function gpio_get_function(uint gpio);
+
+/*! \brief Select up and down pulls on specific GPIO
+ * \ingroup hardware_gpio
+ *
+ * \param gpio GPIO number
+ * \param up If true set a pull up on the GPIO
+ * \param down If true set a pull down on the GPIO
+ *
+ * \note On the RP2040, setting both pulls enables a "bus keep" function,
+ * i.e. a weak pull to whatever is current high/low state of GPIO.
+ */
+void gpio_set_pulls(uint gpio, bool up, bool down);
+
+/*! \brief Set specified GPIO to be pulled up.
+ * \ingroup hardware_gpio
+ *
+ * \param gpio GPIO number
+ */
+static inline void gpio_pull_up(uint gpio) {
+ gpio_set_pulls(gpio, true, false);
+}
+
+/*! \brief Determine if the specified GPIO is pulled up.
+ * \ingroup hardware_gpio
+ *
+ * \param gpio GPIO number
+ * \return true if the GPIO is pulled up
+ */
+static inline bool gpio_is_pulled_up(uint gpio) {
+ return (padsbank0_hw->io[gpio] & PADS_BANK0_GPIO0_PUE_BITS) != 0;
+}
+
+/*! \brief Set specified GPIO to be pulled down.
+ * \ingroup hardware_gpio
+ *
+ * \param gpio GPIO number
+ */
+static inline void gpio_pull_down(uint gpio) {
+ gpio_set_pulls(gpio, false, true);
+}
+
+/*! \brief Determine if the specified GPIO is pulled down.
+ * \ingroup hardware_gpio
+ *
+ * \param gpio GPIO number
+ * \return true if the GPIO is pulled down
+ */
+static inline bool gpio_is_pulled_down(uint gpio) {
+ return (padsbank0_hw->io[gpio] & PADS_BANK0_GPIO0_PDE_BITS) != 0;
+}
+
+/*! \brief Disable pulls on specified GPIO
+ * \ingroup hardware_gpio
+ *
+ * \param gpio GPIO number
+ */
+static inline void gpio_disable_pulls(uint gpio) {
+ gpio_set_pulls(gpio, false, false);
+}
+
+/*! \brief Set GPIO IRQ override
+ * \ingroup hardware_gpio
+ *
+ * Optionally invert a GPIO IRQ signal, or drive it high or low
+ *
+ * \param gpio GPIO number
+ * \param value See \ref gpio_override
+ */
+void gpio_set_irqover(uint gpio, uint value);
+
+/*! \brief Set GPIO output override
+ * \ingroup hardware_gpio
+ *
+ * \param gpio GPIO number
+ * \param value See \ref gpio_override
+ */
+void gpio_set_outover(uint gpio, uint value);
+
+/*! \brief Select GPIO input override
+ * \ingroup hardware_gpio
+ *
+ * \param gpio GPIO number
+ * \param value See \ref gpio_override
+ */
+void gpio_set_inover(uint gpio, uint value);
+
+/*! \brief Select GPIO output enable override
+ * \ingroup hardware_gpio
+ *
+ * \param gpio GPIO number
+ * \param value See \ref gpio_override
+ */
+void gpio_set_oeover(uint gpio, uint value);
+
+/*! \brief Enable GPIO input
+ * \ingroup hardware_gpio
+ *
+ * \param gpio GPIO number
+ * \param enabled true to enable input on specified GPIO
+ */
+void gpio_set_input_enabled(uint gpio, bool enabled);
+
+/*! \brief Enable/disable GPIO input hysteresis (Schmitt trigger)
+ * \ingroup hardware_gpio
+ *
+ * Enable or disable the Schmitt trigger hysteresis on a given GPIO. This is
+ * enabled on all GPIOs by default. Disabling input hysteresis can lead to
+ * inconsistent readings when the input signal has very long rise or fall
+ * times, but slightly reduces the GPIO's input delay.
+ *
+ * \sa gpio_is_input_hysteresis_enabled
+ * \param gpio GPIO number
+ * \param enabled true to enable input hysteresis on specified GPIO
+ */
+void gpio_set_input_hysteresis_enabled(uint gpio, bool enabled);
+
+/*! \brief Determine whether input hysteresis is enabled on a specified GPIO
+ * \ingroup hardware_gpio
+ *
+ * \sa gpio_set_input_hysteresis_enabled
+ * \param gpio GPIO number
+ */
+bool gpio_is_input_hysteresis_enabled(uint gpio);
+
+
+/*! \brief Set slew rate for a specified GPIO
+ * \ingroup hardware_gpio
+ *
+ * \sa gpio_get_slew_rate
+ * \param gpio GPIO number
+ * \param slew GPIO output slew rate
+ */
+void gpio_set_slew_rate(uint gpio, enum gpio_slew_rate slew);
+
+/*! \brief Determine current slew rate for a specified GPIO
+ * \ingroup hardware_gpio
+ *
+ * \sa gpio_set_slew_rate
+ * \param gpio GPIO number
+ * \return Current slew rate of that GPIO
+ */
+enum gpio_slew_rate gpio_get_slew_rate(uint gpio);
+
+/*! \brief Set drive strength for a specified GPIO
+ * \ingroup hardware_gpio
+ *
+ * \sa gpio_get_drive_strength
+ * \param gpio GPIO number
+ * \param drive GPIO output drive strength
+ */
+void gpio_set_drive_strength(uint gpio, enum gpio_drive_strength drive);
+
+/*! \brief Determine current slew rate for a specified GPIO
+ * \ingroup hardware_gpio
+ *
+ * \sa gpio_set_drive_strength
+ * \param gpio GPIO number
+ * \return Current drive strength of that GPIO
+ */
+enum gpio_drive_strength gpio_get_drive_strength(uint gpio);
+
+/*! \brief Enable or disable interrupts for specified GPIO
+ * \ingroup hardware_gpio
+ *
+ * \note The IO IRQs are independent per-processor. This configures IRQs for
+ * the processor that calls the function.
+ *
+ * \param gpio GPIO number
+ * \param events Which events will cause an interrupt
+ * \param enabled Enable or disable flag
+ *
+ * Events is a bitmask of the following:
+ *
+ * bit | interrupt
+ * ----|----------
+ * 0 | Low level
+ * 1 | High level
+ * 2 | Edge low
+ * 3 | Edge high
+ */
+void gpio_set_irq_enabled(uint gpio, uint32_t events, bool enabled);
+
+/*! \brief Enable interrupts for specified GPIO
+ * \ingroup hardware_gpio
+ *
+ * \note The IO IRQs are independent per-processor. This configures IRQs for
+ * the processor that calls the function.
+ *
+ * \param gpio GPIO number
+ * \param events Which events will cause an interrupt. See \ref gpio_set_irq_enabled for details.
+ * \param enabled Enable or disable flag
+ * \param callback user function to call on GPIO irq. Note only one of these can be set per processor.
+ *
+ * \note Currently the GPIO parameter is ignored, and this callback will be called for any enabled GPIO IRQ on any pin.
+ *
+ */
+void gpio_set_irq_enabled_with_callback(uint gpio, uint32_t events, bool enabled, gpio_irq_callback_t callback);
+
+/*! \brief Enable dormant wake up interrupt for specified GPIO
+ * \ingroup hardware_gpio
+ *
+ * This configures IRQs to restart the XOSC or ROSC when they are
+ * disabled in dormant mode
+ *
+ * \param gpio GPIO number
+ * \param events Which events will cause an interrupt. See \ref gpio_set_irq_enabled for details.
+ * \param enabled Enable/disable flag
+ */
+void gpio_set_dormant_irq_enabled(uint gpio, uint32_t events, bool enabled);
+
+/*! \brief Acknowledge a GPIO interrupt
+ * \ingroup hardware_gpio
+ *
+ * \param gpio GPIO number
+ * \param events Bitmask of events to clear. See \ref gpio_set_irq_enabled for details.
+ *
+ */
+void gpio_acknowledge_irq(uint gpio, uint32_t events);
+
+/*! \brief Initialise a GPIO for (enabled I/O and set func to GPIO_FUNC_SIO)
+ * \ingroup hardware_gpio
+ *
+ * Clear the output enable (i.e. set to input).
+ * Clear any output value.
+ *
+ * \param gpio GPIO number
+ */
+void gpio_init(uint gpio);
+
+/*! \brief Initialise multiple GPIOs (enabled I/O and set func to GPIO_FUNC_SIO)
+ * \ingroup hardware_gpio
+ *
+ * Clear the output enable (i.e. set to input).
+ * Clear any output value.
+ *
+ * \param gpio_mask Mask with 1 bit per GPIO number to initialize
+ */
+void gpio_init_mask(uint gpio_mask);
+// ----------------------------------------------------------------------------
+// Input
+// ----------------------------------------------------------------------------
+
+/*! \brief Get state of a single specified GPIO
+ * \ingroup hardware_gpio
+ *
+ * \param gpio GPIO number
+ * \return Current state of the GPIO. 0 for low, non-zero for high
+ */
+static inline bool gpio_get(uint gpio) {
+ return !!((1ul << gpio) & sio_hw->gpio_in);
+}
+
+/*! \brief Get raw value of all GPIOs
+ * \ingroup hardware_gpio
+ *
+ * \return Bitmask of raw GPIO values, as bits 0-29
+ */
+static inline uint32_t gpio_get_all(void) {
+ return sio_hw->gpio_in;
+}
+
+// ----------------------------------------------------------------------------
+// Output
+// ----------------------------------------------------------------------------
+
+/*! \brief Drive high every GPIO appearing in mask
+ * \ingroup hardware_gpio
+ *
+ * \param mask Bitmask of GPIO values to set, as bits 0-29
+ */
+static inline void gpio_set_mask(uint32_t mask) {
+ sio_hw->gpio_set = mask;
+}
+
+/*! \brief Drive low every GPIO appearing in mask
+ * \ingroup hardware_gpio
+ *
+ * \param mask Bitmask of GPIO values to clear, as bits 0-29
+ */
+static inline void gpio_clr_mask(uint32_t mask) {
+ sio_hw->gpio_clr = mask;
+}
+
+/*! \brief Toggle every GPIO appearing in mask
+ * \ingroup hardware_gpio
+ *
+ * \param mask Bitmask of GPIO values to toggle, as bits 0-29
+ */
+static inline void gpio_xor_mask(uint32_t mask) {
+ sio_hw->gpio_togl = mask;
+}
+
+/*! \brief Drive GPIO high/low depending on parameters
+ * \ingroup hardware_gpio
+ *
+ * \param mask Bitmask of GPIO values to change, as bits 0-29
+ * \param value Value to set
+ *
+ * For each 1 bit in \p mask, drive that pin to the value given by
+ * corresponding bit in \p value, leaving other pins unchanged.
+ * Since this uses the TOGL alias, it is concurrency-safe with e.g. an IRQ
+ * bashing different pins from the same core.
+ */
+static inline void gpio_put_masked(uint32_t mask, uint32_t value) {
+ sio_hw->gpio_togl = (sio_hw->gpio_out ^ value) & mask;
+}
+
+/*! \brief Drive all pins simultaneously
+ * \ingroup hardware_gpio
+ *
+ * \param value Bitmask of GPIO values to change, as bits 0-29
+ */
+static inline void gpio_put_all(uint32_t value) {
+ sio_hw->gpio_out = value;
+}
+
+/*! \brief Drive a single GPIO high/low
+ * \ingroup hardware_gpio
+ *
+ * \param gpio GPIO number
+ * \param value If false clear the GPIO, otherwise set it.
+ */
+static inline void gpio_put(uint gpio, bool value) {
+ uint32_t mask = 1ul << gpio;
+ if (value)
+ gpio_set_mask(mask);
+ else
+ gpio_clr_mask(mask);
+}
+
+/*! \brief Determine whether a GPIO is currently driven high or low
+ * \ingroup hardware_gpio
+ *
+ * This function returns the high/low output level most recently assigned to a
+ * GPIO via gpio_put() or similar. This is the value that is presented outward
+ * to the IO muxing, *not* the input level back from the pad (which can be
+ * read using gpio_get()).
+ *
+ * To avoid races, this function must not be used for read-modify-write
+ * sequences when driving GPIOs -- instead functions like gpio_put() should be
+ * used to atomically update GPIOs. This accessor is intended for debug use
+ * only.
+ *
+ * \param gpio GPIO number
+ * \return true if the GPIO output level is high, false if low.
+ */
+static inline bool gpio_get_out_level(uint gpio) {
+ return !!(sio_hw->gpio_out & (1u << gpio));
+}
+
+// ----------------------------------------------------------------------------
+// Direction
+// ----------------------------------------------------------------------------
+
+/*! \brief Set a number of GPIOs to output
+ * \ingroup hardware_gpio
+ *
+ * Switch all GPIOs in "mask" to output
+ *
+ * \param mask Bitmask of GPIO to set to output, as bits 0-29
+ */
+static inline void gpio_set_dir_out_masked(uint32_t mask) {
+ sio_hw->gpio_oe_set = mask;
+}
+
+/*! \brief Set a number of GPIOs to input
+ * \ingroup hardware_gpio
+ *
+ * \param mask Bitmask of GPIO to set to input, as bits 0-29
+ */
+static inline void gpio_set_dir_in_masked(uint32_t mask) {
+ sio_hw->gpio_oe_clr = mask;
+}
+
+/*! \brief Set multiple GPIO directions
+ * \ingroup hardware_gpio
+ *
+ * \param mask Bitmask of GPIO to set to input, as bits 0-29
+ * \param value Values to set
+ *
+ * For each 1 bit in "mask", switch that pin to the direction given by
+ * corresponding bit in "value", leaving other pins unchanged.
+ * E.g. gpio_set_dir_masked(0x3, 0x2); -> set pin 0 to input, pin 1 to output,
+ * simultaneously.
+ */
+static inline void gpio_set_dir_masked(uint32_t mask, uint32_t value) {
+ sio_hw->gpio_oe_togl = (sio_hw->gpio_oe ^ value) & mask;
+}
+
+/*! \brief Set direction of all pins simultaneously.
+ * \ingroup hardware_gpio
+ *
+ * \param values individual settings for each gpio; for GPIO N, bit N is 1 for out, 0 for in
+ */
+static inline void gpio_set_dir_all_bits(uint32_t values) {
+ sio_hw->gpio_oe = values;
+}
+
+/*! \brief Set a single GPIO direction
+ * \ingroup hardware_gpio
+ *
+ * \param gpio GPIO number
+ * \param out true for out, false for in
+ */
+static inline void gpio_set_dir(uint gpio, bool out) {
+ uint32_t mask = 1ul << gpio;
+ if (out)
+ gpio_set_dir_out_masked(mask);
+ else
+ gpio_set_dir_in_masked(mask);
+}
+
+/*! \brief Check if a specific GPIO direction is OUT
+ * \ingroup hardware_gpio
+ *
+ * \param gpio GPIO number
+ * \return true if the direction for the pin is OUT
+ */
+static inline bool gpio_is_dir_out(uint gpio) {
+ return !!(sio_hw->gpio_oe & (1u << (gpio)));
+}
+
+/*! \brief Get a specific GPIO direction
+ * \ingroup hardware_gpio
+ *
+ * \param gpio GPIO number
+ * \return 1 for out, 0 for in
+ */
+static inline uint gpio_get_dir(uint gpio) {
+ return gpio_is_dir_out(gpio); // note GPIO_OUT is 1/true and GPIO_IN is 0/false anyway
+}
+
+extern void gpio_debug_pins_init(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+
+// PICO_CONFIG: PICO_DEBUG_PIN_BASE, First pin to use for debug output (if enabled), min=0, max=28, default=19, group=hardware_gpio
+#ifndef PICO_DEBUG_PIN_BASE
+#define PICO_DEBUG_PIN_BASE 19u
+#endif
+
+// PICO_CONFIG: PICO_DEBUG_PIN_COUNT, Number of pins to use for debug output (if enabled), min=1, max=28, default=3, group=hardware_gpio
+#ifndef PICO_DEBUG_PIN_COUNT
+#define PICO_DEBUG_PIN_COUNT 3u
+#endif
+
+#ifndef __cplusplus
+// note these two macros may only be used once per and only apply per compilation unit (hence the CU_)
+#define CU_REGISTER_DEBUG_PINS(...) enum __unused DEBUG_PIN_TYPE { _none = 0, __VA_ARGS__ }; static enum DEBUG_PIN_TYPE __selected_debug_pins;
+#define CU_SELECT_DEBUG_PINS(x) static enum DEBUG_PIN_TYPE __selected_debug_pins = (x);
+#define DEBUG_PINS_ENABLED(p) (__selected_debug_pins == (p))
+#else
+#define CU_REGISTER_DEBUG_PINS(p...) \
+ enum DEBUG_PIN_TYPE { _none = 0, p }; \
+ template <enum DEBUG_PIN_TYPE> class __debug_pin_settings { \
+ public: \
+ static inline bool enabled() { return false; } \
+ };
+#define CU_SELECT_DEBUG_PINS(x) template<> inline bool __debug_pin_settings<x>::enabled() { return true; };
+#define DEBUG_PINS_ENABLED(p) (__debug_pin_settings<p>::enabled())
+#endif
+#define DEBUG_PINS_SET(p, v) if (DEBUG_PINS_ENABLED(p)) gpio_set_mask((unsigned)(v)<<PICO_DEBUG_PIN_BASE)
+#define DEBUG_PINS_CLR(p, v) if (DEBUG_PINS_ENABLED(p)) gpio_clr_mask((unsigned)(v)<<PICO_DEBUG_PIN_BASE)
+#define DEBUG_PINS_XOR(p, v) if (DEBUG_PINS_ENABLED(p)) gpio_xor_mask((unsigned)(v)<<PICO_DEBUG_PIN_BASE)
+
+#endif // _GPIO_H_
diff --git a/src/rp2_common/hardware_i2c/CMakeLists.txt b/src/rp2_common/hardware_i2c/CMakeLists.txt
new file mode 100644
index 0000000..aba48dd
--- /dev/null
+++ b/src/rp2_common/hardware_i2c/CMakeLists.txt
@@ -0,0 +1 @@
+pico_simple_hardware_target(i2c)
diff --git a/src/rp2_common/hardware_i2c/i2c.c b/src/rp2_common/hardware_i2c/i2c.c
new file mode 100644
index 0000000..95bcfea
--- /dev/null
+++ b/src/rp2_common/hardware_i2c/i2c.c
@@ -0,0 +1,337 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "hardware/i2c.h"
+#include "hardware/resets.h"
+#include "hardware/clocks.h"
+#include "pico/timeout_helper.h"
+
+check_hw_layout(i2c_hw_t, enable, I2C_IC_ENABLE_OFFSET);
+check_hw_layout(i2c_hw_t, clr_restart_det, I2C_IC_CLR_RESTART_DET_OFFSET);
+
+i2c_inst_t i2c0_inst = {i2c0_hw, false};
+i2c_inst_t i2c1_inst = {i2c1_hw, false};
+
+static inline void i2c_reset(i2c_inst_t *i2c) {
+ invalid_params_if(I2C, i2c != i2c0 && i2c != i2c1);
+ reset_block(i2c == i2c0 ? RESETS_RESET_I2C0_BITS : RESETS_RESET_I2C1_BITS);
+}
+
+static inline void i2c_unreset(i2c_inst_t *i2c) {
+ invalid_params_if(I2C, i2c != i2c0 && i2c != i2c1);
+ unreset_block_wait(i2c == i2c0 ? RESETS_RESET_I2C0_BITS : RESETS_RESET_I2C1_BITS);
+}
+
+// Addresses of the form 000 0xxx or 111 1xxx are reserved. No slave should
+// have these addresses.
+static inline bool i2c_reserved_addr(uint8_t addr) {
+ return (addr & 0x78) == 0 || (addr & 0x78) == 0x78;
+}
+
+uint i2c_init(i2c_inst_t *i2c, uint baudrate) {
+ i2c_reset(i2c);
+ i2c_unreset(i2c);
+ i2c->restart_on_next = false;
+
+ i2c->hw->enable = 0;
+
+ // Configure as a fast-mode master with RepStart support, 7-bit addresses
+ i2c->hw->con =
+ I2C_IC_CON_SPEED_VALUE_FAST << I2C_IC_CON_SPEED_LSB |
+ I2C_IC_CON_MASTER_MODE_BITS |
+ I2C_IC_CON_IC_SLAVE_DISABLE_BITS |
+ I2C_IC_CON_IC_RESTART_EN_BITS |
+ I2C_IC_CON_TX_EMPTY_CTRL_BITS;
+
+ // Set FIFO watermarks to 1 to make things simpler. This is encoded by a register value of 0.
+ i2c->hw->tx_tl = 0;
+ i2c->hw->rx_tl = 0;
+
+ // Always enable the DREQ signalling -- harmless if DMA isn't listening
+ i2c->hw->dma_cr = I2C_IC_DMA_CR_TDMAE_BITS | I2C_IC_DMA_CR_RDMAE_BITS;
+
+ // Re-sets i2c->hw->enable upon returning:
+ return i2c_set_baudrate(i2c, baudrate);
+}
+
+void i2c_deinit(i2c_inst_t *i2c) {
+ i2c_reset(i2c);
+}
+
+uint i2c_set_baudrate(i2c_inst_t *i2c, uint baudrate) {
+ invalid_params_if(I2C, baudrate == 0);
+ // I2C is synchronous design that runs from clk_sys
+ uint freq_in = clock_get_hz(clk_sys);
+
+ // TODO there are some subtleties to I2C timing which we are completely ignoring here
+ uint period = (freq_in + baudrate / 2) / baudrate;
+ uint lcnt = period * 3 / 5; // oof this one hurts
+ uint hcnt = period - lcnt;
+ // Check for out-of-range divisors:
+ invalid_params_if(I2C, hcnt > I2C_IC_FS_SCL_HCNT_IC_FS_SCL_HCNT_BITS);
+ invalid_params_if(I2C, lcnt > I2C_IC_FS_SCL_LCNT_IC_FS_SCL_LCNT_BITS);
+ invalid_params_if(I2C, hcnt < 8);
+ invalid_params_if(I2C, lcnt < 8);
+
+ // Per I2C-bus specification a device in standard or fast mode must
+ // internally provide a hold time of at least 300ns for the SDA signal to
+ // bridge the undefined region of the falling edge of SCL. A smaller hold
+ // time of 120ns is used for fast mode plus.
+ uint sda_tx_hold_count;
+ if (baudrate < 1000000) {
+ // sda_tx_hold_count = freq_in [cycles/s] * 300ns * (1s / 1e9ns)
+ // Reduce 300/1e9 to 3/1e7 to avoid numbers that don't fit in uint.
+ // Add 1 to avoid division truncation.
+ sda_tx_hold_count = ((freq_in * 3) / 10000000) + 1;
+ } else {
+ // sda_tx_hold_count = freq_in [cycles/s] * 120ns * (1s / 1e9ns)
+ // Reduce 120/1e9 to 3/25e6 to avoid numbers that don't fit in uint.
+ // Add 1 to avoid division truncation.
+ sda_tx_hold_count = ((freq_in * 3) / 25000000) + 1;
+ }
+ assert(sda_tx_hold_count <= lcnt - 2);
+
+ i2c->hw->enable = 0;
+ // Always use "fast" mode (<= 400 kHz, works fine for standard mode too)
+ hw_write_masked(&i2c->hw->con,
+ I2C_IC_CON_SPEED_VALUE_FAST << I2C_IC_CON_SPEED_LSB,
+ I2C_IC_CON_SPEED_BITS
+ );
+ i2c->hw->fs_scl_hcnt = hcnt;
+ i2c->hw->fs_scl_lcnt = lcnt;
+ i2c->hw->fs_spklen = lcnt < 16 ? 1 : lcnt / 16;
+ hw_write_masked(&i2c->hw->sda_hold,
+ sda_tx_hold_count << I2C_IC_SDA_HOLD_IC_SDA_TX_HOLD_LSB,
+ I2C_IC_SDA_HOLD_IC_SDA_TX_HOLD_BITS);
+
+ i2c->hw->enable = 1;
+ return freq_in / period;
+}
+
+void i2c_set_slave_mode(i2c_inst_t *i2c, bool slave, uint8_t addr) {
+ invalid_params_if(I2C, addr >= 0x80); // 7-bit addresses
+ invalid_params_if(I2C, i2c_reserved_addr(addr));
+ i2c->hw->enable = 0;
+ uint32_t ctrl_set_if_master = I2C_IC_CON_MASTER_MODE_BITS | I2C_IC_CON_IC_SLAVE_DISABLE_BITS;
+ uint32_t ctrl_set_if_slave = I2C_IC_CON_RX_FIFO_FULL_HLD_CTRL_BITS;
+ if (slave) {
+ hw_write_masked(&i2c->hw->con,
+ ctrl_set_if_slave,
+ ctrl_set_if_master | ctrl_set_if_slave
+ );
+ i2c->hw->sar = addr;
+ } else {
+ hw_write_masked(&i2c->hw->con,
+ ctrl_set_if_master,
+ ctrl_set_if_master | ctrl_set_if_slave
+ );
+ }
+ i2c->hw->enable = 1;
+}
+
+static int i2c_write_blocking_internal(i2c_inst_t *i2c, uint8_t addr, const uint8_t *src, size_t len, bool nostop,
+ check_timeout_fn timeout_check, struct timeout_state *ts) {
+ invalid_params_if(I2C, addr >= 0x80); // 7-bit addresses
+ invalid_params_if(I2C, i2c_reserved_addr(addr));
+ // Synopsys hw accepts start/stop flags alongside data items in the same
+ // FIFO word, so no 0 byte transfers.
+ invalid_params_if(I2C, len == 0);
+ invalid_params_if(I2C, ((int)len) < 0);
+
+ i2c->hw->enable = 0;
+ i2c->hw->tar = addr;
+ i2c->hw->enable = 1;
+
+ bool abort = false;
+ bool timeout = false;
+
+ uint32_t abort_reason = 0;
+ int byte_ctr;
+
+ int ilen = (int)len;
+ for (byte_ctr = 0; byte_ctr < ilen; ++byte_ctr) {
+ bool first = byte_ctr == 0;
+ bool last = byte_ctr == ilen - 1;
+
+ i2c->hw->data_cmd =
+ bool_to_bit(first && i2c->restart_on_next) << I2C_IC_DATA_CMD_RESTART_LSB |
+ bool_to_bit(last && !nostop) << I2C_IC_DATA_CMD_STOP_LSB |
+ *src++;
+
+ // Wait until the transmission of the address/data from the internal
+ // shift register has completed. For this to function correctly, the
+ // TX_EMPTY_CTRL flag in IC_CON must be set. The TX_EMPTY_CTRL flag
+ // was set in i2c_init.
+ do {
+ if (timeout_check) {
+ timeout = timeout_check(ts);
+ abort |= timeout;
+ }
+ tight_loop_contents();
+ } while (!timeout && !(i2c->hw->raw_intr_stat & I2C_IC_RAW_INTR_STAT_TX_EMPTY_BITS));
+
+ // If there was a timeout, don't attempt to do anything else.
+ if (!timeout) {
+ abort_reason = i2c->hw->tx_abrt_source;
+ if (abort_reason) {
+ // Note clearing the abort flag also clears the reason, and
+ // this instance of flag is clear-on-read! Note also the
+ // IC_CLR_TX_ABRT register always reads as 0.
+ i2c->hw->clr_tx_abrt;
+ abort = true;
+ }
+
+ if (abort || (last && !nostop)) {
+ // If the transaction was aborted or if it completed
+ // successfully wait until the STOP condition has occured.
+
+ // TODO Could there be an abort while waiting for the STOP
+ // condition here? If so, additional code would be needed here
+ // to take care of the abort.
+ do {
+ if (timeout_check) {
+ timeout = timeout_check(ts);
+ abort |= timeout;
+ }
+ tight_loop_contents();
+ } while (!timeout && !(i2c->hw->raw_intr_stat & I2C_IC_RAW_INTR_STAT_STOP_DET_BITS));
+
+ // If there was a timeout, don't attempt to do anything else.
+ if (!timeout) {
+ i2c->hw->clr_stop_det;
+ }
+ }
+ }
+
+ // Note the hardware issues a STOP automatically on an abort condition.
+ // Note also the hardware clears RX FIFO as well as TX on abort,
+ // because we set hwparam IC_AVOID_RX_FIFO_FLUSH_ON_TX_ABRT to 0.
+ if (abort)
+ break;
+ }
+
+ int rval;
+
+ // A lot of things could have just happened due to the ingenious and
+ // creative design of I2C. Try to figure things out.
+ if (abort) {
+ if (timeout)
+ rval = PICO_ERROR_TIMEOUT;
+ else if (!abort_reason || abort_reason & I2C_IC_TX_ABRT_SOURCE_ABRT_7B_ADDR_NOACK_BITS) {
+ // No reported errors - seems to happen if there is nothing connected to the bus.
+ // Address byte not acknowledged
+ rval = PICO_ERROR_GENERIC;
+ } else if (abort_reason & I2C_IC_TX_ABRT_SOURCE_ABRT_TXDATA_NOACK_BITS) {
+ // Address acknowledged, some data not acknowledged
+ rval = byte_ctr;
+ } else {
+ //panic("Unknown abort from I2C instance @%08x: %08x\n", (uint32_t) i2c->hw, abort_reason);
+ rval = PICO_ERROR_GENERIC;
+ }
+ } else {
+ rval = byte_ctr;
+ }
+
+ // nostop means we are now at the end of a *message* but not the end of a *transfer*
+ i2c->restart_on_next = nostop;
+ return rval;
+}
+
+int i2c_write_blocking(i2c_inst_t *i2c, uint8_t addr, const uint8_t *src, size_t len, bool nostop) {
+ return i2c_write_blocking_internal(i2c, addr, src, len, nostop, NULL, NULL);
+}
+
+int i2c_write_blocking_until(i2c_inst_t *i2c, uint8_t addr, const uint8_t *src, size_t len, bool nostop,
+ absolute_time_t until) {
+ timeout_state_t ts;
+ return i2c_write_blocking_internal(i2c, addr, src, len, nostop, init_single_timeout_until(&ts, until), &ts);
+}
+
+int i2c_write_timeout_per_char_us(i2c_inst_t *i2c, uint8_t addr, const uint8_t *src, size_t len, bool nostop,
+ uint timeout_per_char_us) {
+ timeout_state_t ts;
+ return i2c_write_blocking_internal(i2c, addr, src, len, nostop,
+ init_per_iteration_timeout_us(&ts, timeout_per_char_us), &ts);
+}
+
+static int i2c_read_blocking_internal(i2c_inst_t *i2c, uint8_t addr, uint8_t *dst, size_t len, bool nostop,
+ check_timeout_fn timeout_check, timeout_state_t *ts) {
+ invalid_params_if(I2C, addr >= 0x80); // 7-bit addresses
+ invalid_params_if(I2C, i2c_reserved_addr(addr));
+ invalid_params_if(I2C, len == 0);
+ invalid_params_if(I2C, ((int)len) < 0);
+
+ i2c->hw->enable = 0;
+ i2c->hw->tar = addr;
+ i2c->hw->enable = 1;
+
+ bool abort = false;
+ bool timeout = false;
+ uint32_t abort_reason;
+ int byte_ctr;
+ int ilen = (int)len;
+ for (byte_ctr = 0; byte_ctr < ilen; ++byte_ctr) {
+ bool first = byte_ctr == 0;
+ bool last = byte_ctr == ilen - 1;
+ while (!i2c_get_write_available(i2c))
+ tight_loop_contents();
+
+ i2c->hw->data_cmd =
+ bool_to_bit(first && i2c->restart_on_next) << I2C_IC_DATA_CMD_RESTART_LSB |
+ bool_to_bit(last && !nostop) << I2C_IC_DATA_CMD_STOP_LSB |
+ I2C_IC_DATA_CMD_CMD_BITS; // -> 1 for read
+
+ do {
+ abort_reason = i2c->hw->tx_abrt_source;
+ abort = (bool) i2c->hw->clr_tx_abrt;
+ if (timeout_check) {
+ timeout = timeout_check(ts);
+ abort |= timeout;
+ }
+ } while (!abort && !i2c_get_read_available(i2c));
+
+ if (abort)
+ break;
+
+ *dst++ = (uint8_t) i2c->hw->data_cmd;
+ }
+
+ int rval;
+
+ if (abort) {
+ if (timeout)
+ rval = PICO_ERROR_TIMEOUT;
+ else if (!abort_reason || abort_reason & I2C_IC_TX_ABRT_SOURCE_ABRT_7B_ADDR_NOACK_BITS) {
+ // No reported errors - seems to happen if there is nothing connected to the bus.
+ // Address byte not acknowledged
+ rval = PICO_ERROR_GENERIC;
+ } else {
+// panic("Unknown abort from I2C instance @%08x: %08x\n", (uint32_t) i2c->hw, abort_reason);
+ rval = PICO_ERROR_GENERIC;
+ }
+ } else {
+ rval = byte_ctr;
+ }
+
+ i2c->restart_on_next = nostop;
+ return rval;
+}
+
+int i2c_read_blocking(i2c_inst_t *i2c, uint8_t addr, uint8_t *dst, size_t len, bool nostop) {
+ return i2c_read_blocking_internal(i2c, addr, dst, len, nostop, NULL, NULL);
+}
+
+int i2c_read_blocking_until(i2c_inst_t *i2c, uint8_t addr, uint8_t *dst, size_t len, bool nostop, absolute_time_t until) {
+ timeout_state_t ts;
+ return i2c_read_blocking_internal(i2c, addr, dst, len, nostop, init_single_timeout_until(&ts, until), &ts);
+}
+
+int i2c_read_timeout_per_char_us(i2c_inst_t *i2c, uint8_t addr, uint8_t *dst, size_t len, bool nostop,
+ uint timeout_per_char_us) {
+ timeout_state_t ts;
+ return i2c_read_blocking_internal(i2c, addr, dst, len, nostop,
+ init_per_iteration_timeout_us(&ts, timeout_per_char_us), &ts);
+}
diff --git a/src/rp2_common/hardware_i2c/include/hardware/i2c.h b/src/rp2_common/hardware_i2c/include/hardware/i2c.h
new file mode 100644
index 0000000..23ff8f1
--- /dev/null
+++ b/src/rp2_common/hardware_i2c/include/hardware/i2c.h
@@ -0,0 +1,332 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _HARDWARE_I2C_H
+#define _HARDWARE_I2C_H
+
+#include "pico.h"
+#include "pico/time.h"
+#include "hardware/structs/i2c.h"
+#include "hardware/regs/dreq.h"
+
+// PICO_CONFIG: PARAM_ASSERTIONS_ENABLED_I2C, Enable/disable assertions in the I2C module, type=bool, default=0, group=hardware_i2c
+#ifndef PARAM_ASSERTIONS_ENABLED_I2C
+#define PARAM_ASSERTIONS_ENABLED_I2C 0
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** \file hardware/i2c.h
+ * \defgroup hardware_i2c hardware_i2c
+ *
+ * I2C Controller API
+ *
+ * The I2C bus is a two-wire serial interface, consisting of a serial data line SDA and a serial clock SCL. These wires carry
+ * information between the devices connected to the bus. Each device is recognized by a unique 7-bit address and can operate as
+ * either a “transmitter” or “receiver”, depending on the function of the device. Devices can also be considered as masters or
+ * slaves when performing data transfers. A master is a device that initiates a data transfer on the bus and generates the
+ * clock signals to permit that transfer. The first byte in the data transfer always contains the 7-bit address and
+ * a read/write bit in the LSB position. This API takes care of toggling the read/write bit. After this, any device addressed
+ * is considered a slave.
+ *
+ * This API allows the controller to be set up as a master or a slave using the \ref i2c_set_slave_mode function.
+ *
+ * The external pins of each controller are connected to GPIO pins as defined in the GPIO muxing table in the datasheet. The muxing options
+ * give some IO flexibility, but each controller external pin should be connected to only one GPIO.
+ *
+ * Note that the controller does NOT support High speed mode or Ultra-fast speed mode, the fastest operation being fast mode plus
+ * at up to 1000Kb/s.
+ *
+ * See the datasheet for more information on the I2C controller and its usage.
+ *
+ * \subsection i2c_example Example
+ * \addtogroup hardware_i2c
+ * \include bus_scan.c
+ */
+
+typedef struct i2c_inst i2c_inst_t;
+
+// PICO_CONFIG: PICO_DEFAULT_I2C, Define the default I2C for a board, min=0, max=1, group=hardware_i2c
+// PICO_CONFIG: PICO_DEFAULT_I2C_SDA_PIN, Define the default I2C SDA pin, min=0, max=29, group=hardware_i2c
+// PICO_CONFIG: PICO_DEFAULT_I2C_SCL_PIN, Define the default I2C SCL pin, min=0, max=29, group=hardware_i2c
+
+/** The I2C identifiers for use in I2C functions.
+ *
+ * e.g. i2c_init(i2c0, 48000)
+ *
+ * \ingroup hardware_i2c
+ * @{
+ */
+extern i2c_inst_t i2c0_inst;
+extern i2c_inst_t i2c1_inst;
+
+#define i2c0 (&i2c0_inst) ///< Identifier for I2C HW Block 0
+#define i2c1 (&i2c1_inst) ///< Identifier for I2C HW Block 1
+
+#if !defined(PICO_DEFAULT_I2C_INSTANCE) && defined(PICO_DEFAULT_I2C)
+#define PICO_DEFAULT_I2C_INSTANCE (__CONCAT(i2c,PICO_DEFAULT_I2C))
+#endif
+
+#ifdef PICO_DEFAULT_I2C_INSTANCE
+#define i2c_default PICO_DEFAULT_I2C_INSTANCE
+#endif
+
+/** @} */
+
+// ----------------------------------------------------------------------------
+// Setup
+
+/*! \brief Initialise the I2C HW block
+ * \ingroup hardware_i2c
+ *
+ * Put the I2C hardware into a known state, and enable it. Must be called
+ * before other functions. By default, the I2C is configured to operate as a
+ * master.
+ *
+ * The I2C bus frequency is set as close as possible to requested, and
+ * the return actual rate set is returned
+ *
+ * \param i2c Either \ref i2c0 or \ref i2c1
+ * \param baudrate Baudrate in Hz (e.g. 100kHz is 100000)
+ * \return Actual set baudrate
+ */
+uint i2c_init(i2c_inst_t *i2c, uint baudrate);
+
+/*! \brief Disable the I2C HW block
+ * \ingroup hardware_i2c
+ *
+ * \param i2c Either \ref i2c0 or \ref i2c1
+ *
+ * Disable the I2C again if it is no longer used. Must be reinitialised before
+ * being used again.
+ */
+void i2c_deinit(i2c_inst_t *i2c);
+
+/*! \brief Set I2C baudrate
+ * \ingroup hardware_i2c
+ *
+ * Set I2C bus frequency as close as possible to requested, and return actual
+ * rate set.
+ * Baudrate may not be as exactly requested due to clocking limitations.
+ *
+ * \param i2c Either \ref i2c0 or \ref i2c1
+ * \param baudrate Baudrate in Hz (e.g. 100kHz is 100000)
+ * \return Actual set baudrate
+ */
+uint i2c_set_baudrate(i2c_inst_t *i2c, uint baudrate);
+
+/*! \brief Set I2C port to slave mode
+ * \ingroup hardware_i2c
+ *
+ * \param i2c Either \ref i2c0 or \ref i2c1
+ * \param slave true to use slave mode, false to use master mode
+ * \param addr If \p slave is true, set the slave address to this value
+ */
+void i2c_set_slave_mode(i2c_inst_t *i2c, bool slave, uint8_t addr);
+
+// ----------------------------------------------------------------------------
+// Generic input/output
+
+struct i2c_inst {
+ i2c_hw_t *hw;
+ bool restart_on_next;
+};
+
+/*! \brief Convert I2C instance to hardware instance number
+ * \ingroup hardware_i2c
+ *
+ * \param i2c I2C instance
+ * \return Number of I2C, 0 or 1.
+ */
+static inline uint i2c_hw_index(i2c_inst_t *i2c) {
+ invalid_params_if(I2C, i2c != i2c0 && i2c != i2c1);
+ return i2c == i2c1 ? 1 : 0;
+}
+
+static inline i2c_hw_t *i2c_get_hw(i2c_inst_t *i2c) {
+ i2c_hw_index(i2c); // check it is a hw i2c
+ return i2c->hw;
+}
+
+/*! \brief Attempt to write specified number of bytes to address, blocking until the specified absolute time is reached.
+ * \ingroup hardware_i2c
+ *
+ * \param i2c Either \ref i2c0 or \ref i2c1
+ * \param addr 7-bit address of device to write to
+ * \param src Pointer to data to send
+ * \param len Length of data in bytes to send
+ * \param nostop If true, master retains control of the bus at the end of the transfer (no Stop is issued),
+ * and the next transfer will begin with a Restart rather than a Start.
+ * \param until The absolute time that the block will wait until the entire transaction is complete. Note, an individual timeout of
+ * this value divided by the length of data is applied for each byte transfer, so if the first or subsequent
+ * bytes fails to transfer within that sub timeout, the function will return with an error.
+ *
+ * \return Number of bytes written, or PICO_ERROR_GENERIC if address not acknowledged, no device present, or PICO_ERROR_TIMEOUT if a timeout occurred.
+ */
+int i2c_write_blocking_until(i2c_inst_t *i2c, uint8_t addr, const uint8_t *src, size_t len, bool nostop, absolute_time_t until);
+
+/*! \brief Attempt to read specified number of bytes from address, blocking until the specified absolute time is reached.
+ * \ingroup hardware_i2c
+ *
+ * \param i2c Either \ref i2c0 or \ref i2c1
+ * \param addr 7-bit address of device to read from
+ * \param dst Pointer to buffer to receive data
+ * \param len Length of data in bytes to receive
+ * \param nostop If true, master retains control of the bus at the end of the transfer (no Stop is issued),
+ * and the next transfer will begin with a Restart rather than a Start.
+ * \param until The absolute time that the block will wait until the entire transaction is complete.
+ * \return Number of bytes read, or PICO_ERROR_GENERIC if address not acknowledged, no device present, or PICO_ERROR_TIMEOUT if a timeout occurred.
+ */
+int i2c_read_blocking_until(i2c_inst_t *i2c, uint8_t addr, uint8_t *dst, size_t len, bool nostop, absolute_time_t until);
+
+/*! \brief Attempt to write specified number of bytes to address, with timeout
+ * \ingroup hardware_i2c
+ *
+ * \param i2c Either \ref i2c0 or \ref i2c1
+ * \param addr 7-bit address of device to write to
+ * \param src Pointer to data to send
+ * \param len Length of data in bytes to send
+ * \param nostop If true, master retains control of the bus at the end of the transfer (no Stop is issued),
+ * and the next transfer will begin with a Restart rather than a Start.
+ * \param timeout_us The time that the function will wait for the entire transaction to complete. Note, an individual timeout of
+ * this value divided by the length of data is applied for each byte transfer, so if the first or subsequent
+ * bytes fails to transfer within that sub timeout, the function will return with an error.
+ *
+ * \return Number of bytes written, or PICO_ERROR_GENERIC if address not acknowledged, no device present, or PICO_ERROR_TIMEOUT if a timeout occurred.
+ */
+static inline int i2c_write_timeout_us(i2c_inst_t *i2c, uint8_t addr, const uint8_t *src, size_t len, bool nostop, uint timeout_us) {
+ absolute_time_t t = make_timeout_time_us(timeout_us);
+ return i2c_write_blocking_until(i2c, addr, src, len, nostop, t);
+}
+
+int i2c_write_timeout_per_char_us(i2c_inst_t *i2c, uint8_t addr, const uint8_t *src, size_t len, bool nostop, uint timeout_per_char_us);
+
+/*! \brief Attempt to read specified number of bytes from address, with timeout
+ * \ingroup hardware_i2c
+ *
+ * \param i2c Either \ref i2c0 or \ref i2c1
+ * \param addr 7-bit address of device to read from
+ * \param dst Pointer to buffer to receive data
+ * \param len Length of data in bytes to receive
+ * \param nostop If true, master retains control of the bus at the end of the transfer (no Stop is issued),
+ * and the next transfer will begin with a Restart rather than a Start.
+ * \param timeout_us The time that the function will wait for the entire transaction to complete
+ * \return Number of bytes read, or PICO_ERROR_GENERIC if address not acknowledged, no device present, or PICO_ERROR_TIMEOUT if a timeout occurred.
+ */
+static inline int i2c_read_timeout_us(i2c_inst_t *i2c, uint8_t addr, uint8_t *dst, size_t len, bool nostop, uint timeout_us) {
+ absolute_time_t t = make_timeout_time_us(timeout_us);
+ return i2c_read_blocking_until(i2c, addr, dst, len, nostop, t);
+}
+
+int i2c_read_timeout_per_char_us(i2c_inst_t *i2c, uint8_t addr, uint8_t *dst, size_t len, bool nostop, uint timeout_per_char_us);
+
+/*! \brief Attempt to write specified number of bytes to address, blocking
+ * \ingroup hardware_i2c
+ *
+ * \param i2c Either \ref i2c0 or \ref i2c1
+ * \param addr 7-bit address of device to write to
+ * \param src Pointer to data to send
+ * \param len Length of data in bytes to send
+ * \param nostop If true, master retains control of the bus at the end of the transfer (no Stop is issued),
+ * and the next transfer will begin with a Restart rather than a Start.
+ * \return Number of bytes written, or PICO_ERROR_GENERIC if address not acknowledged, no device present.
+ */
+int i2c_write_blocking(i2c_inst_t *i2c, uint8_t addr, const uint8_t *src, size_t len, bool nostop);
+
+/*! \brief Attempt to read specified number of bytes from address, blocking
+ * \ingroup hardware_i2c
+ *
+ * \param i2c Either \ref i2c0 or \ref i2c1
+ * \param addr 7-bit address of device to read from
+ * \param dst Pointer to buffer to receive data
+ * \param len Length of data in bytes to receive
+ * \param nostop If true, master retains control of the bus at the end of the transfer (no Stop is issued),
+ * and the next transfer will begin with a Restart rather than a Start.
+ * \return Number of bytes read, or PICO_ERROR_GENERIC if address not acknowledged or no device present.
+ */
+int i2c_read_blocking(i2c_inst_t *i2c, uint8_t addr, uint8_t *dst, size_t len, bool nostop);
+
+
+/*! \brief Determine non-blocking write space available
+ * \ingroup hardware_i2c
+ *
+ * \param i2c Either \ref i2c0 or \ref i2c1
+ * \return 0 if no space is available in the I2C to write more data. If return is nonzero, at
+ * least that many bytes can be written without blocking.
+ */
+static inline size_t i2c_get_write_available(i2c_inst_t *i2c) {
+ const size_t IC_TX_BUFFER_DEPTH = 16;
+ return IC_TX_BUFFER_DEPTH - i2c_get_hw(i2c)->txflr;
+}
+
+/*! \brief Determine number of bytes received
+ * \ingroup hardware_i2c
+ *
+ * \param i2c Either \ref i2c0 or \ref i2c1
+ * \return 0 if no data available, if return is nonzero at
+ * least that many bytes can be read without blocking.
+ */
+static inline size_t i2c_get_read_available(i2c_inst_t *i2c) {
+ return i2c_get_hw(i2c)->rxflr;
+}
+
+/*! \brief Write direct to TX FIFO
+ * \ingroup hardware_i2c
+ *
+ * \param i2c Either \ref i2c0 or \ref i2c1
+ * \param src Data to send
+ * \param len Number of bytes to send
+ *
+ * Writes directly to the I2C TX FIFO which is mainly useful for
+ * slave-mode operation.
+ */
+static inline void i2c_write_raw_blocking(i2c_inst_t *i2c, const uint8_t *src, size_t len) {
+ for (size_t i = 0; i < len; ++i) {
+ // TODO NACK or STOP on end?
+ while (!i2c_get_write_available(i2c))
+ tight_loop_contents();
+ i2c_get_hw(i2c)->data_cmd = *src++;
+ }
+}
+
+/*! \brief Read direct from RX FIFO
+ * \ingroup hardware_i2c
+ *
+ * \param i2c Either \ref i2c0 or \ref i2c1
+ * \param dst Buffer to accept data
+ * \param len Number of bytes to read
+ *
+ * Reads directly from the I2C RX FIFO which is mainly useful for
+ * slave-mode operation.
+ */
+static inline void i2c_read_raw_blocking(i2c_inst_t *i2c, uint8_t *dst, size_t len) {
+ for (size_t i = 0; i < len; ++i) {
+ while (!i2c_get_read_available(i2c))
+ tight_loop_contents();
+ *dst++ = (uint8_t)i2c_get_hw(i2c)->data_cmd;
+ }
+}
+
+/*! \brief Return the DREQ to use for pacing transfers to/from a particular I2C instance
+ * \ingroup hardware_i2c
+ *
+ * \param i2c Either \ref i2c0 or \ref i2c1
+ * \param is_tx true for sending data to the I2C instance, false for receiving data from the I2C instance
+ */
+static inline uint i2c_get_dreq(i2c_inst_t *i2c, bool is_tx) {
+ static_assert(DREQ_I2C0_RX == DREQ_I2C0_TX + 1, "");
+ static_assert(DREQ_I2C1_RX == DREQ_I2C1_TX + 1, "");
+ static_assert(DREQ_I2C1_TX == DREQ_I2C0_TX + 2, "");
+ return DREQ_I2C0_TX + i2c_hw_index(i2c) * 2 + !is_tx;
+}
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/rp2_common/hardware_interp/CMakeLists.txt b/src/rp2_common/hardware_interp/CMakeLists.txt
new file mode 100644
index 0000000..d6d693f
--- /dev/null
+++ b/src/rp2_common/hardware_interp/CMakeLists.txt
@@ -0,0 +1 @@
+pico_simple_hardware_target(interp)
\ No newline at end of file
diff --git a/src/rp2_common/hardware_interp/include/hardware/interp.h b/src/rp2_common/hardware_interp/include/hardware/interp.h
new file mode 100644
index 0000000..35372e9
--- /dev/null
+++ b/src/rp2_common/hardware_interp/include/hardware/interp.h
@@ -0,0 +1,458 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _HARDWARE_INTERP_H
+#define _HARDWARE_INTERP_H
+
+#include "pico.h"
+#include "hardware/structs/interp.h"
+#include "hardware/regs/sio.h"
+
+// PICO_CONFIG: PARAM_ASSERTIONS_ENABLED_INTERP, Enable/disable assertions in the interpolation module, type=bool, default=0, group=hardware_interp
+#ifndef PARAM_ASSERTIONS_ENABLED_INTERP
+#define PARAM_ASSERTIONS_ENABLED_INTERP 0
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** \file hardware/interp.h
+ * \defgroup hardware_interp hardware_interp
+ *
+ * Hardware Interpolator API
+ *
+ * Each core is equipped with two interpolators (INTERP0 and INTERP1) which can be used to accelerate
+ * tasks by combining certain pre-configured simple operations into a single processor cycle. Intended
+ * for cases where the pre-configured operation is repeated a large number of times, this results in
+ * code which uses both fewer CPU cycles and fewer CPU registers in the time critical sections of the
+ * code.
+ *
+ * The interpolators are used heavily to accelerate audio operations within the SDK, but their
+ * flexible configuration make it possible to optimise many other tasks such as quantization and
+ * dithering, table lookup address generation, affine texture mapping, decompression and linear feedback.
+ *
+ * Please refer to the RP2040 datasheet for more information on the HW interpolators and how they work.
+ */
+
+#define interp0 interp0_hw
+#define interp1 interp1_hw
+
+/** \brief Interpolator configuration
+ * \defgroup interp_config interp_config
+ * \ingroup hardware_interp
+ *
+ * Each interpolator needs to be configured, these functions provide handy helpers to set up configuration
+ * structures.
+ *
+ */
+
+typedef struct {
+ uint32_t ctrl;
+} interp_config;
+
+static inline uint interp_index(interp_hw_t *interp) {
+ valid_params_if(INTERP, interp == interp0 || interp == interp1);
+ return interp == interp1 ? 1 : 0;
+}
+
+/*! \brief Claim the interpolator lane specified
+ * \ingroup hardware_interp
+ *
+ * Use this function to claim exclusive access to the specified interpolator lane.
+ *
+ * This function will panic if the lane is already claimed.
+ *
+ * \param interp Interpolator on which to claim a lane. interp0 or interp1
+ * \param lane The lane number, 0 or 1.
+ */
+void interp_claim_lane(interp_hw_t *interp, uint lane);
+// The above really should be called this for consistency
+#define interp_lane_claim interp_claim_lane
+
+/*! \brief Claim the interpolator lanes specified in the mask
+ * \ingroup hardware_interp
+ *
+ * \param interp Interpolator on which to claim lanes. interp0 or interp1
+ * \param lane_mask Bit pattern of lanes to claim (only bits 0 and 1 are valid)
+ */
+void interp_claim_lane_mask(interp_hw_t *interp, uint lane_mask);
+
+/*! \brief Release a previously claimed interpolator lane
+ * \ingroup hardware_interp
+ *
+ * \param interp Interpolator on which to release a lane. interp0 or interp1
+ * \param lane The lane number, 0 or 1
+ */
+void interp_unclaim_lane(interp_hw_t *interp, uint lane);
+// The above really should be called this for consistency
+#define interp_lane_unclaim interp_unclaim_lane
+
+/*! \brief Determine if an interpolator lane is claimed
+ * \ingroup hardware_interp
+ *
+ * \param interp Interpolator whose lane to check
+ * \param lane The lane number, 0 or 1
+ * \return true if claimed, false otherwise
+ * \see interp_claim_lane
+ * \see interp_claim_lane_mask
+ */
+bool interp_lane_is_claimed(interp_hw_t *interp, uint lane);
+
+/*! \brief Release previously claimed interpolator lanes \see interp_claim_lane_mask
+ * \ingroup hardware_interp
+ *
+ * \param interp Interpolator on which to release lanes. interp0 or interp1
+ * \param lane_mask Bit pattern of lanes to unclaim (only bits 0 and 1 are valid)
+ */
+void interp_unclaim_lane_mask(interp_hw_t *interp, uint lane_mask);
+
+/*! \brief Set the interpolator shift value
+ * \ingroup interp_config
+ *
+ * Sets the number of bits the accumulator is shifted before masking, on each iteration.
+ *
+ * \param c Pointer to an interpolator config
+ * \param shift Number of bits
+ */
+static inline void interp_config_set_shift(interp_config *c, uint shift) {
+ valid_params_if(INTERP, shift < 32);
+ c->ctrl = (c->ctrl & ~SIO_INTERP0_CTRL_LANE0_SHIFT_BITS) |
+ ((shift << SIO_INTERP0_CTRL_LANE0_SHIFT_LSB) & SIO_INTERP0_CTRL_LANE0_SHIFT_BITS);
+}
+
+/*! \brief Set the interpolator mask range
+ * \ingroup interp_config
+ *
+ * Sets the range of bits (least to most) that are allowed to pass through the interpolator
+ *
+ * \param c Pointer to interpolation config
+ * \param mask_lsb The least significant bit allowed to pass
+ * \param mask_msb The most significant bit allowed to pass
+ */
+static inline void interp_config_set_mask(interp_config *c, uint mask_lsb, uint mask_msb) {
+ valid_params_if(INTERP, mask_msb < 32);
+ valid_params_if(INTERP, mask_lsb <= mask_msb);
+ c->ctrl = (c->ctrl & ~(SIO_INTERP0_CTRL_LANE0_MASK_LSB_BITS | SIO_INTERP0_CTRL_LANE0_MASK_MSB_BITS)) |
+ ((mask_lsb << SIO_INTERP0_CTRL_LANE0_MASK_LSB_LSB) & SIO_INTERP0_CTRL_LANE0_MASK_LSB_BITS) |
+ ((mask_msb << SIO_INTERP0_CTRL_LANE0_MASK_MSB_LSB) & SIO_INTERP0_CTRL_LANE0_MASK_MSB_BITS);
+}
+
+/*! \brief Enable cross input
+ * \ingroup interp_config
+ *
+ * Allows feeding of the accumulator content from the other lane back in to this lanes shift+mask hardware.
+ * This will take effect even if the interp_config_set_add_raw option is set as the cross input mux is before the
+ * shift+mask bypass
+ *
+ * \param c Pointer to interpolation config
+ * \param cross_input If true, enable the cross input.
+ */
+static inline void interp_config_set_cross_input(interp_config *c, bool cross_input) {
+ c->ctrl = (c->ctrl & ~SIO_INTERP0_CTRL_LANE0_CROSS_INPUT_BITS) |
+ (cross_input ? SIO_INTERP0_CTRL_LANE0_CROSS_INPUT_BITS : 0);
+}
+
+/*! \brief Enable cross results
+ * \ingroup interp_config
+ *
+ * Allows feeding of the other lane’s result into this lane’s accumulator on a POP operation.
+ *
+ * \param c Pointer to interpolation config
+ * \param cross_result If true, enables the cross result
+ */
+static inline void interp_config_set_cross_result(interp_config *c, bool cross_result) {
+ c->ctrl = (c->ctrl & ~SIO_INTERP0_CTRL_LANE0_CROSS_RESULT_BITS) |
+ (cross_result ? SIO_INTERP0_CTRL_LANE0_CROSS_RESULT_BITS : 0);
+}
+
+/*! \brief Set sign extension
+ * \ingroup interp_config
+ *
+ * Enables signed mode, where the shifted and masked accumulator value is sign-extended to 32 bits
+ * before adding to BASE1, and LANE1 PEEK/POP results appear extended to 32 bits when read by processor.
+ *
+ * \param c Pointer to interpolation config
+ * \param _signed If true, enables sign extension
+ */
+static inline void interp_config_set_signed(interp_config *c, bool _signed) {
+ c->ctrl = (c->ctrl & ~SIO_INTERP0_CTRL_LANE0_SIGNED_BITS) |
+ (_signed ? SIO_INTERP0_CTRL_LANE0_SIGNED_BITS : 0);
+}
+
+/*! \brief Set raw add option
+ * \ingroup interp_config
+ *
+ * When enabled, mask + shift is bypassed for LANE0 result. This does not affect the FULL result.
+ *
+ * \param c Pointer to interpolation config
+ * \param add_raw If true, enable raw add option.
+ */
+static inline void interp_config_set_add_raw(interp_config *c, bool add_raw) {
+ c->ctrl = (c->ctrl & ~SIO_INTERP0_CTRL_LANE0_ADD_RAW_BITS) |
+ (add_raw ? SIO_INTERP0_CTRL_LANE0_ADD_RAW_BITS : 0);
+}
+
+/*! \brief Set blend mode
+ * \ingroup interp_config
+ *
+ * If enabled, LANE1 result is a linear interpolation between BASE0 and BASE1, controlled
+ * by the 8 LSBs of lane 1 shift and mask value (a fractional number between 0 and 255/256ths)
+ *
+ * LANE0 result does not have BASE0 added (yields only the 8 LSBs of lane 1 shift+mask value)
+ *
+ * FULL result does not have lane 1 shift+mask value added (BASE2 + lane 0 shift+mask)
+ *
+ * LANE1 SIGNED flag controls whether the interpolation is signed or unsig
+ *
+ * \param c Pointer to interpolation config
+ * \param blend Set true to enable blend mode.
+*/
+static inline void interp_config_set_blend(interp_config *c, bool blend) {
+ c->ctrl = (c->ctrl & ~SIO_INTERP0_CTRL_LANE0_BLEND_BITS) |
+ (blend ? SIO_INTERP0_CTRL_LANE0_BLEND_BITS : 0);
+}
+
+/*! \brief Set interpolator clamp mode (Interpolator 1 only)
+ * \ingroup interp_config
+ *
+ * Only present on INTERP1 on each core. If CLAMP mode is enabled:
+ * - LANE0 result is a shifted and masked ACCUM0, clamped by a lower bound of BASE0 and an upper bound of BASE1.
+ * - Signedness of these comparisons is determined by LANE0_CTRL_SIGNED
+ *
+ * \param c Pointer to interpolation config
+ * \param clamp Set true to enable clamp mode
+ */
+static inline void interp_config_set_clamp(interp_config *c, bool clamp) {
+ c->ctrl = (c->ctrl & ~SIO_INTERP1_CTRL_LANE0_CLAMP_BITS) |
+ (clamp ? SIO_INTERP1_CTRL_LANE0_CLAMP_BITS : 0);
+}
+
+/*! \brief Set interpolator Force bits
+ * \ingroup interp_config
+ *
+ * ORed into bits 29:28 of the lane result presented to the processor on the bus.
+ *
+ * No effect on the internal 32-bit datapath. Handy for using a lane to generate sequence
+ * of pointers into flash or SRAM
+ *
+ * \param c Pointer to interpolation config
+ * \param bits Sets the force bits to that specified. Range 0-3 (two bits)
+ */
+static inline void interp_config_set_force_bits(interp_config *c, uint bits) {
+ invalid_params_if(INTERP, bits > 3);
+ // note cannot use hw_set_bits on SIO
+ c->ctrl = (c->ctrl & ~SIO_INTERP0_CTRL_LANE0_FORCE_MSB_BITS) |
+ (bits << SIO_INTERP0_CTRL_LANE0_FORCE_MSB_LSB);
+}
+
+/*! \brief Get a default configuration
+ * \ingroup interp_config
+ *
+ * \return A default interpolation configuration
+ */
+static inline interp_config interp_default_config(void) {
+ interp_config c = {0};
+ // Just pass through everything
+ interp_config_set_mask(&c, 0, 31);
+ return c;
+}
+
+/*! \brief Send configuration to a lane
+ * \ingroup interp_config
+ *
+ * If an invalid configuration is specified (ie a lane specific item is set on wrong lane),
+ * depending on setup this function can panic.
+ *
+ * \param interp Interpolator instance, interp0 or interp1.
+ * \param lane The lane to set
+ * \param config Pointer to interpolation config
+ */
+
+static inline void interp_set_config(interp_hw_t *interp, uint lane, interp_config *config) {
+ invalid_params_if(INTERP, lane > 1);
+ invalid_params_if(INTERP, config->ctrl & SIO_INTERP1_CTRL_LANE0_CLAMP_BITS &&
+ (!interp_index(interp) || lane)); // only interp1 lane 0 has clamp bit
+ invalid_params_if(INTERP, config->ctrl & SIO_INTERP0_CTRL_LANE0_BLEND_BITS &&
+ (interp_index(interp) || lane)); // only interp0 lane 0 has blend bit
+ interp->ctrl[lane] = config->ctrl;
+}
+
+/*! \brief Directly set the force bits on a specified lane
+ * \ingroup hardware_interp
+ *
+ * These bits are ORed into bits 29:28 of the lane result presented to the processor on the bus.
+ * There is no effect on the internal 32-bit datapath.
+ *
+ * Useful for using a lane to generate sequence of pointers into flash or SRAM, saving a subsequent
+ * OR or add operation.
+ *
+ * \param interp Interpolator instance, interp0 or interp1.
+ * \param lane The lane to set
+ * \param bits The bits to set (bits 0 and 1, value range 0-3)
+ */
+static inline void interp_set_force_bits(interp_hw_t *interp, uint lane, uint bits) {
+ // note cannot use hw_set_bits on SIO
+ interp->ctrl[lane] |= (bits << SIO_INTERP0_CTRL_LANE0_FORCE_MSB_LSB);
+}
+
+typedef struct {
+ uint32_t accum[2];
+ uint32_t base[3];
+ uint32_t ctrl[2];
+} interp_hw_save_t;
+
+/*! \brief Save the specified interpolator state
+ * \ingroup hardware_interp
+ *
+ * Can be used to save state if you need an interpolator for another purpose, state
+ * can then be recovered afterwards and continue from that point
+ *
+ * \param interp Interpolator instance, interp0 or interp1.
+ * \param saver Pointer to the save structure to fill in
+ */
+void interp_save(interp_hw_t *interp, interp_hw_save_t *saver);
+
+/*! \brief Restore an interpolator state
+ * \ingroup hardware_interp
+ *
+ * \param interp Interpolator instance, interp0 or interp1.
+ * \param saver Pointer to save structure to reapply to the specified interpolator
+ */
+void interp_restore(interp_hw_t *interp, interp_hw_save_t *saver);
+
+/*! \brief Sets the interpolator base register by lane
+ * \ingroup hardware_interp
+ *
+ * \param interp Interpolator instance, interp0 or interp1.
+ * \param lane The lane number, 0 or 1 or 2
+ * \param val The value to apply to the register
+ */
+static inline void interp_set_base(interp_hw_t *interp, uint lane, uint32_t val) {
+ interp->base[lane] = val;
+}
+
+/*! \brief Gets the content of interpolator base register by lane
+ * \ingroup hardware_interp
+ *
+ * \param interp Interpolator instance, interp0 or interp1.
+ * \param lane The lane number, 0 or 1 or 2
+ * \return The current content of the lane base register
+ */
+static inline uint32_t interp_get_base(interp_hw_t *interp, uint lane) {
+ return interp->base[lane];
+}
+
+/*! \brief Sets the interpolator base registers simultaneously
+ * \ingroup hardware_interp
+ *
+ * The lower 16 bits go to BASE0, upper bits to BASE1 simultaneously.
+ * Each half is sign-extended to 32 bits if that lane’s SIGNED flag is set.
+ *
+ * \param interp Interpolator instance, interp0 or interp1.
+ * \param val The value to apply to the register
+ */
+static inline void interp_set_base_both(interp_hw_t *interp, uint32_t val) {
+ interp->base01 = val;
+}
+
+
+/*! \brief Sets the interpolator accumulator register by lane
+ * \ingroup hardware_interp
+ *
+ * \param interp Interpolator instance, interp0 or interp1.
+ * \param lane The lane number, 0 or 1
+ * \param val The value to apply to the register
+ */
+static inline void interp_set_accumulator(interp_hw_t *interp, uint lane, uint32_t val) {
+ interp->accum[lane] = val;
+}
+
+/*! \brief Gets the content of the interpolator accumulator register by lane
+ * \ingroup hardware_interp
+ *
+ * \param interp Interpolator instance, interp0 or interp1.
+ * \param lane The lane number, 0 or 1
+ * \return The current content of the register
+ */
+static inline uint32_t interp_get_accumulator(interp_hw_t *interp, uint lane) {
+ return interp->accum[lane];
+}
+
+/*! \brief Read lane result, and write lane results to both accumulators to update the interpolator
+ * \ingroup hardware_interp
+ *
+ * \param interp Interpolator instance, interp0 or interp1.
+ * \param lane The lane number, 0 or 1
+ * \return The content of the lane result register
+ */
+static inline uint32_t interp_pop_lane_result(interp_hw_t *interp, uint lane) {
+ return interp->pop[lane];
+}
+
+/*! \brief Read lane result
+ * \ingroup hardware_interp
+ *
+ * \param interp Interpolator instance, interp0 or interp1.
+ * \param lane The lane number, 0 or 1
+ * \return The content of the lane result register
+ */
+static inline uint32_t interp_peek_lane_result(interp_hw_t *interp, uint lane) {
+ return interp->peek[lane];
+}
+
+/*! \brief Read lane result, and write lane results to both accumulators to update the interpolator
+ * \ingroup hardware_interp
+ *
+ * \param interp Interpolator instance, interp0 or interp1.
+ * \return The content of the FULL register
+ */
+static inline uint32_t interp_pop_full_result(interp_hw_t *interp) {
+ return interp->pop[2];
+}
+
+/*! \brief Read lane result
+ * \ingroup hardware_interp
+ *
+ * \param interp Interpolator instance, interp0 or interp1.
+ * \return The content of the FULL register
+ */
+static inline uint32_t interp_peek_full_result(interp_hw_t *interp) {
+ return interp->peek[2];
+}
+
+/*! \brief Add to accumulator
+ * \ingroup hardware_interp
+ *
+ * Atomically add the specified value to the accumulator on the specified lane
+ *
+ * \param interp Interpolator instance, interp0 or interp1.
+ * \param lane The lane number, 0 or 1
+ * \param val Value to add
+ * \return The content of the FULL register
+ */
+static inline void interp_add_accumulater(interp_hw_t *interp, uint lane, uint32_t val) {
+ interp->add_raw[lane] = val;
+}
+
+/*! \brief Get raw lane value
+ * \ingroup hardware_interp
+ *
+ * Returns the raw shift and mask value from the specified lane, BASE0 is NOT added
+ *
+ * \param interp Interpolator instance, interp0 or interp1.
+ * \param lane The lane number, 0 or 1
+ * \return The raw shift/mask value
+ */
+static inline uint32_t interp_get_raw(interp_hw_t *interp, uint lane) {
+ return interp->add_raw[lane];
+}
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/rp2_common/hardware_interp/interp.c b/src/rp2_common/hardware_interp/interp.c
new file mode 100644
index 0000000..37e8a91
--- /dev/null
+++ b/src/rp2_common/hardware_interp/interp.c
@@ -0,0 +1,68 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "hardware/interp.h"
+#include "hardware/structs/sio.h"
+#include "hardware/claim.h"
+
+check_hw_size(interp_hw_t, SIO_INTERP1_ACCUM0_OFFSET - SIO_INTERP0_ACCUM0_OFFSET);
+
+check_hw_layout(sio_hw_t, interp, SIO_INTERP0_ACCUM0_OFFSET);
+
+static_assert(NUM_DMA_CHANNELS <= 16, "");
+
+static uint8_t _claimed;
+
+static inline uint interp_lane_bit(interp_hw_t * interp, uint lane) {
+ return (interp_index(interp) << 1u) | lane;
+}
+
+void interp_claim_lane(interp_hw_t *interp, uint lane) {
+ valid_params_if(INTERP, lane < 2);
+ hw_claim_or_assert((uint8_t *) &_claimed, interp_lane_bit(interp, lane), "Lane is already claimed");
+}
+
+void interp_claim_lane_mask(interp_hw_t *interp, uint lane_mask) {
+ valid_params_if(INTERP, lane_mask && lane_mask <= 0x3);
+ if (lane_mask & 1u) interp_claim_lane(interp, 0);
+ if (lane_mask & 2u) interp_claim_lane(interp, 1);
+}
+
+void interp_unclaim_lane(interp_hw_t *interp, uint lane) {
+ valid_params_if(INTERP, lane < 2);
+ hw_claim_clear((uint8_t *) &_claimed, interp_lane_bit(interp, lane));
+}
+
+bool interp_lane_is_claimed(interp_hw_t *interp, uint lane) {
+ valid_params_if(INTERP, lane < 2);
+ return hw_is_claimed((uint8_t *) &_claimed, interp_lane_bit(interp, lane));
+}
+
+void interp_unclaim_lane_mask(interp_hw_t *interp, uint lane_mask) {
+ valid_params_if(INTERP, lane_mask <= 0x3);
+ if (lane_mask & 1u) interp_unclaim_lane(interp, 0);
+ if (lane_mask & 2u) interp_unclaim_lane(interp, 1);
+}
+
+void interp_save(interp_hw_t *interp, interp_hw_save_t *saver) {
+ saver->accum[0] = interp->accum[0];
+ saver->accum[1] = interp->accum[1];
+ saver->base[0] = interp->base[0];
+ saver->base[1] = interp->base[1];
+ saver->base[2] = interp->base[2];
+ saver->ctrl[0] = interp->ctrl[0];
+ saver->ctrl[1] = interp->ctrl[1];
+}
+
+void interp_restore(interp_hw_t *interp, interp_hw_save_t *saver) {
+ interp->accum[0] = saver->accum[0];
+ interp->accum[1] = saver->accum[1];
+ interp->base[0] = saver->base[0];
+ interp->base[1] = saver->base[1];
+ interp->base[2] = saver->base[2];
+ interp->ctrl[0] = saver->ctrl[0];
+ interp->ctrl[1] = saver->ctrl[1];
+}
diff --git a/src/rp2_common/hardware_irq/CMakeLists.txt b/src/rp2_common/hardware_irq/CMakeLists.txt
new file mode 100644
index 0000000..c218231
--- /dev/null
+++ b/src/rp2_common/hardware_irq/CMakeLists.txt
@@ -0,0 +1,6 @@
+pico_simple_hardware_target(irq)
+
+# additional sources/libraries
+
+target_sources(hardware_irq INTERFACE ${CMAKE_CURRENT_LIST_DIR}/irq_handler_chain.S)
+target_link_libraries(hardware_irq INTERFACE pico_sync)
\ No newline at end of file
diff --git a/src/rp2_common/hardware_irq/include/hardware/irq.h b/src/rp2_common/hardware_irq/include/hardware/irq.h
new file mode 100644
index 0000000..424a497
--- /dev/null
+++ b/src/rp2_common/hardware_irq/include/hardware/irq.h
@@ -0,0 +1,287 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _HARDWARE_IRQ_H_
+#define _HARDWARE_IRQ_H_
+
+// These two config items are also used by assembler, so keeping separate
+// PICO_CONFIG: PICO_MAX_SHARED_IRQ_HANDLERS, Maximum number of shared IRQ handlers, default=4, advanced=true, group=hardware_irq
+#ifndef PICO_MAX_SHARED_IRQ_HANDLERS
+#define PICO_MAX_SHARED_IRQ_HANDLERS 4u
+#endif
+
+// PICO_CONFIG: PICO_DISABLE_SHARED_IRQ_HANDLERS, Disable shared IRQ handlers, type=bool, default=0, group=hardware_irq
+#ifndef PICO_DISABLE_SHARED_IRQ_HANDLERS
+#define PICO_DISABLE_SHARED_IRQ_HANDLERS 0
+#endif
+
+#ifndef __ASSEMBLER__
+
+#include "pico.h"
+#include "hardware/address_mapped.h"
+#include "hardware/regs/intctrl.h"
+#include "hardware/regs/m0plus.h"
+
+/** \file irq.h
+ * \defgroup hardware_irq hardware_irq
+ *
+ * Hardware interrupt handling
+ *
+ * The RP2040 uses the standard ARM nested vectored interrupt controller (NVIC).
+ *
+ * Interrupts are identified by a number from 0 to 31.
+ *
+ * On the RP2040, only the lower 26 IRQ signals are connected on the NVIC; IRQs 26 to 31 are tied to zero (never firing).
+ *
+ * There is one NVIC per core, and each core's NVIC has the same hardware interrupt lines routed to it, with the exception of the IO interrupts
+ * where there is one IO interrupt per bank, per core. These are completely independent, so, for example, processor 0 can be
+ * interrupted by GPIO 0 in bank 0, and processor 1 by GPIO 1 in the same bank.
+ *
+ * \note That all IRQ APIs affect the executing core only (i.e. the core calling the function).
+ *
+ * \note You should not enable the same (shared) IRQ number on both cores, as this will lead to race conditions
+ * or starvation of one of the cores. Additionally, don't forget that disabling interrupts on one core does not disable interrupts
+ * on the other core.
+ *
+ * There are three different ways to set handlers for an IRQ:
+ * - Calling irq_add_shared_handler() at runtime to add a handler for a multiplexed interrupt (e.g. GPIO bank) on the current core. Each handler, should check and clear the relevant hardware interrupt source
+ * - Calling irq_set_exclusive_handler() at runtime to install a single handler for the interrupt on the current core
+ * - Defining the interrupt handler explicitly in your application (e.g. by defining void `isr_dma_0` will make that function the handler for the DMA_IRQ_0 on core 0, and
+ * you will not be able to change it using the above APIs at runtime). Using this method can cause link conflicts at runtime, and offers no runtime performance benefit (i.e, it should not generally be used).
+ *
+ * \note If an IRQ is enabled and fires with no handler installed, a breakpoint will be hit and the IRQ number will
+ * be in register r0.
+ *
+ * \section interrupt_nums Interrupt Numbers
+ *
+ * Interrupts are numbered as follows, a set of defines is available (intctrl.h) with these names to avoid using the numbers directly.
+ *
+ * IRQ | Interrupt Source
+ * ----|-----------------
+ * 0 | TIMER_IRQ_0
+ * 1 | TIMER_IRQ_1
+ * 2 | TIMER_IRQ_2
+ * 3 | TIMER_IRQ_3
+ * 4 | PWM_IRQ_WRAP
+ * 5 | USBCTRL_IRQ
+ * 6 | XIP_IRQ
+ * 7 | PIO0_IRQ_0
+ * 8 | PIO0_IRQ_1
+ * 9 | PIO1_IRQ_0
+ * 10 | PIO1_IRQ_1
+ * 11 | DMA_IRQ_0
+ * 12 | DMA_IRQ_1
+ * 13 | IO_IRQ_BANK0
+ * 14 | IO_IRQ_QSPI
+ * 15 | SIO_IRQ_PROC0
+ * 16 | SIO_IRQ_PROC1
+ * 17 | CLOCKS_IRQ
+ * 18 | SPI0_IRQ
+ * 19 | SPI1_IRQ
+ * 20 | UART0_IRQ
+ * 21 | UART1_IRQ
+ * 22 | ADC0_IRQ_FIFO
+ * 23 | I2C0_IRQ
+ * 24 | I2C1_IRQ
+ * 25 | RTC_IRQ
+ *
+ */
+
+// PICO_CONFIG: PICO_DEFAULT_IRQ_PRIORITY, Define the default IRQ priority, default=0x80, group=hardware_irq
+#ifndef PICO_DEFAULT_IRQ_PRIORITY
+#define PICO_DEFAULT_IRQ_PRIORITY 0x80
+#endif
+
+#define PICO_LOWEST_IRQ_PRIORITY 0xff
+#define PICO_HIGHEST_IRQ_PRIORITY 0x00
+
+// PICO_CONFIG: PICO_SHARED_IRQ_HANDLER_DEFAULT_ORDER_PRIORITY, Set default shared IRQ order priority, default=0x80, group=hardware_irq
+#ifndef PICO_SHARED_IRQ_HANDLER_DEFAULT_ORDER_PRIORITY
+#define PICO_SHARED_IRQ_HANDLER_DEFAULT_ORDER_PRIORITY 0x80
+#endif
+
+// PICO_CONFIG: PARAM_ASSERTIONS_ENABLED_IRQ, Enable/disable assertions in the IRQ module, type=bool, default=0, group=hardware_irq
+#ifndef PARAM_ASSERTIONS_ENABLED_IRQ
+#define PARAM_ASSERTIONS_ENABLED_IRQ 0
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*! \brief Interrupt handler function type
+ * \ingroup hardware_irq
+ *
+ * All interrupts handlers should be of this type, and follow normal ARM EABI register saving conventions
+ */
+typedef void (*irq_handler_t)(void);
+
+static inline void check_irq_param(__unused uint num) {
+ invalid_params_if(IRQ, num >= NUM_IRQS);
+}
+
+/*! \brief Set specified interrupt's priority
+ * \ingroup hardware_irq
+ *
+ * \param num Interrupt number
+ * \param hardware_priority Priority to set.
+ * Numerically-lower values indicate a higher priority. Hardware priorities
+ * range from 0 (highest priority) to 255 (lowest priority) though only the
+ * top 2 bits are significant on ARM Cortex-M0+. To make it easier to specify
+ * higher or lower priorities than the default, all IRQ priorities are
+ * initialized to PICO_DEFAULT_IRQ_PRIORITY by the SDK runtime at startup.
+ * PICO_DEFAULT_IRQ_PRIORITY defaults to 0x80
+ */
+void irq_set_priority(uint num, uint8_t hardware_priority);
+
+/*! \brief Get specified interrupt's priority
+ * \ingroup hardware_irq
+ *
+ * Numerically-lower values indicate a higher priority. Hardware priorities
+ * range from 0 (highest priority) to 255 (lowest priority) though only the
+ * top 2 bits are significant on ARM Cortex-M0+. To make it easier to specify
+ * higher or lower priorities than the default, all IRQ priorities are
+ * initialized to PICO_DEFAULT_IRQ_PRIORITY by the SDK runtime at startup.
+ * PICO_DEFAULT_IRQ_PRIORITY defaults to 0x80
+ *
+ * \param num Interrupt number
+ * \return the IRQ priority
+ */
+uint irq_get_priority(uint num);
+
+/*! \brief Enable or disable a specific interrupt on the executing core
+ * \ingroup hardware_irq
+ *
+ * \param num Interrupt number \ref interrupt_nums
+ * \param enabled true to enable the interrupt, false to disable
+ */
+void irq_set_enabled(uint num, bool enabled);
+
+/*! \brief Determine if a specific interrupt is enabled on the executing core
+ * \ingroup hardware_irq
+ *
+ * \param num Interrupt number \ref interrupt_nums
+ * \return true if the interrupt is enabled
+ */
+bool irq_is_enabled(uint num);
+
+/*! \brief Enable/disable multiple interrupts on the executing core
+ * \ingroup hardware_irq
+ *
+ * \param mask 32-bit mask with one bits set for the interrupts to enable/disable
+ * \param enabled true to enable the interrupts, false to disable them.
+ */
+void irq_set_mask_enabled(uint32_t mask, bool enabled);
+
+/*! \brief Set an exclusive interrupt handler for an interrupt on the executing core.
+ * \ingroup hardware_irq
+ *
+ * Use this method to set a handler for single IRQ source interrupts, or when
+ * your code, use case or performance requirements dictate that there should
+ * no other handlers for the interrupt.
+ *
+ * This method will assert if there is already any sort of interrupt handler installed
+ * for the specified irq number.
+ *
+ * \param num Interrupt number \ref interrupt_nums
+ * \param handler The handler to set. See \ref irq_handler_t
+ * \see irq_add_shared_handler()
+ */
+void irq_set_exclusive_handler(uint num, irq_handler_t handler);
+
+/*! \brief Get the exclusive interrupt handler for an interrupt on the executing core.
+ * \ingroup hardware_irq
+ *
+ * This method will return an exclusive IRQ handler set on this core
+ * by irq_set_exclusive_handler if there is one.
+ *
+ * \param num Interrupt number \ref interrupt_nums
+ * \see irq_set_exclusive_handler()
+ * \return handler The handler if an exclusive handler is set for the IRQ,
+ * NULL if no handler is set or shared/shareable handlers are installed
+ */
+irq_handler_t irq_get_exclusive_handler(uint num);
+
+/*! \brief Add a shared interrupt handler for an interrupt on the executing core
+ * \ingroup hardware_irq
+ *
+ * Use this method to add a handler on an irq number shared between multiple distinct hardware sources (e.g. GPIO, DMA or PIO IRQs).
+ * Handlers added by this method will all be called in sequence from highest order_priority to lowest. The
+ * irq_set_exclusive_handler() method should be used instead if you know there will or should only ever be one handler for the interrupt.
+ *
+ * This method will assert if there is an exclusive interrupt handler set for this irq number on this core, or if
+ * the (total across all IRQs on both cores) maximum (configurable via PICO_MAX_SHARED_IRQ_HANDLERS) number of shared handlers
+ * would be exceeded.
+ *
+ * \param num Interrupt number
+ * \param handler The handler to set. See \ref irq_handler_t
+ * \param order_priority The order priority controls the order that handlers for the same IRQ number on the core are called.
+ * The shared irq handlers for an interrupt are all called when an IRQ fires, however the order of the calls is based
+ * on the order_priority (higher priorities are called first, identical priorities are called in undefined order). A good
+ * rule of thumb is to use PICO_SHARED_IRQ_HANDLER_DEFAULT_ORDER_PRIORITY if you don't much care, as it is in the middle of
+ * the priority range by default.
+ *
+ * \see irq_set_exclusive_handler()
+ */
+void irq_add_shared_handler(uint num, irq_handler_t handler, uint8_t order_priority);
+
+/*! \brief Remove a specific interrupt handler for the given irq number on the executing core
+ * \ingroup hardware_irq
+ *
+ * This method may be used to remove an irq set via either irq_set_exclusive_handler() or
+ * irq_add_shared_handler(), and will assert if the handler is not currently installed for the given
+ * IRQ number
+ *
+ * \note This method may *only* be called from user (non IRQ code) or from within the handler
+ * itself (i.e. an IRQ handler may remove itself as part of handling the IRQ). Attempts to call
+ * from another IRQ will cause an assertion.
+ *
+ * \param num Interrupt number \ref interrupt_nums
+ * \param handler The handler to removed.
+ * \see irq_set_exclusive_handler()
+ * \see irq_add_shared_handler()
+ */
+void irq_remove_handler(uint num, irq_handler_t handler);
+
+/*! \brief Get the current IRQ handler for the specified IRQ from the currently installed hardware vector table (VTOR)
+ * of the execution core
+ * \ingroup hardware_irq
+ *
+ * \param num Interrupt number \ref interrupt_nums
+ * \return the address stored in the VTABLE for the given irq number
+ */
+irq_handler_t irq_get_vtable_handler(uint num);
+
+/*! \brief Clear a specific interrupt on the executing core
+ * \ingroup hardware_irq
+ *
+ * \param int_num Interrupt number \ref interrupt_nums
+ */
+static inline void irq_clear(uint int_num) {
+ *((volatile uint32_t *) (PPB_BASE + M0PLUS_NVIC_ICPR_OFFSET)) = (1u << ((uint32_t) (int_num & 0x1F)));
+}
+
+/*! \brief Force an interrupt to be pending on the executing core
+ * \ingroup hardware_irq
+ *
+ * This should generally not be used for IRQs connected to hardware.
+ *
+ * \param num Interrupt number \ref interrupt_nums
+ */
+void irq_set_pending(uint num);
+
+
+/*! \brief Perform IRQ priority initialization for the current core
+ *
+ * \note This is an internal method and user should generally not call it.
+ */
+void irq_init_priorities(void);
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+#endif
diff --git a/src/rp2_common/hardware_irq/irq.c b/src/rp2_common/hardware_irq/irq.c
new file mode 100644
index 0000000..211f6d0
--- /dev/null
+++ b/src/rp2_common/hardware_irq/irq.c
@@ -0,0 +1,413 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "hardware/irq.h"
+#include "hardware/regs/m0plus.h"
+#include "hardware/platform_defs.h"
+#include "hardware/structs/scb.h"
+
+#include "pico/mutex.h"
+#include "pico/assert.h"
+
+extern void __unhandled_user_irq(void);
+
+static inline irq_handler_t *get_vtable(void) {
+ return (irq_handler_t *) scb_hw->vtor;
+}
+
+static inline void *add_thumb_bit(void *addr) {
+ return (void *) (((uintptr_t) addr) | 0x1);
+}
+
+static inline void *remove_thumb_bit(void *addr) {
+ return (void *) (((uintptr_t) addr) & (uint)~0x1);
+}
+
+static void set_raw_irq_handler_and_unlock(uint num, irq_handler_t handler, uint32_t save) {
+ // update vtable (vtable_handler may be same or updated depending on cases, but we do it anyway for compactness)
+ get_vtable()[16 + num] = handler;
+ __dmb();
+ spin_unlock(spin_lock_instance(PICO_SPINLOCK_ID_IRQ), save);
+}
+
+void irq_set_enabled(uint num, bool enabled) {
+ check_irq_param(num);
+ irq_set_mask_enabled(1u << num, enabled);
+}
+
+bool irq_is_enabled(uint num) {
+ check_irq_param(num);
+ return 0 != ((1u << num) & *((io_rw_32 *) (PPB_BASE + M0PLUS_NVIC_ISER_OFFSET)));
+}
+
+void irq_set_mask_enabled(uint32_t mask, bool enabled) {
+ if (enabled) {
+ // Clear pending before enable
+ // (if IRQ is actually asserted, it will immediately re-pend)
+ *((io_rw_32 *) (PPB_BASE + M0PLUS_NVIC_ICPR_OFFSET)) = mask;
+ *((io_rw_32 *) (PPB_BASE + M0PLUS_NVIC_ISER_OFFSET)) = mask;
+ } else {
+ *((io_rw_32 *) (PPB_BASE + M0PLUS_NVIC_ICER_OFFSET)) = mask;
+ }
+}
+
+void irq_set_pending(uint num) {
+ check_irq_param(num);
+ *((io_rw_32 *) (PPB_BASE + M0PLUS_NVIC_ISPR_OFFSET)) = 1u << num;
+}
+
+#if !PICO_DISABLE_SHARED_IRQ_HANDLERS
+// limited by 8 bit relative links (and reality)
+static_assert(PICO_MAX_SHARED_IRQ_HANDLERS >= 1 && PICO_MAX_SHARED_IRQ_HANDLERS < 0x7f, "");
+
+// note these are not real functions, they are code fragments (i.e. don't call them)
+extern void irq_handler_chain_first_slot(void);
+extern void irq_handler_chain_remove_tail(void);
+
+extern struct irq_handler_chain_slot {
+ // first 3 half words are executable code (raw vtable handler points to one slot, and inst3 will jump to next
+ // in chain (or end of chain handler)
+ uint16_t inst1;
+ uint16_t inst2;
+ uint16_t inst3;
+ union {
+ // when a handler is removed while executing, it needs an extra instruction, which overwrites
+ // the link and the priority; this is ok because no one else is modifying the chain, as
+ // the chain is effectively core local, and the user code which might still need this link
+ // disable the IRQ in question before updating, which means we aren't executing!
+ struct {
+ int8_t link;
+ uint8_t priority;
+ };
+ uint16_t inst4;
+ };
+ irq_handler_t handler;
+} irq_handler_chain_slots[PICO_MAX_SHARED_IRQ_HANDLERS];
+
+static int8_t irq_hander_chain_free_slot_head;
+
+static inline bool is_shared_irq_raw_handler(irq_handler_t raw_handler) {
+ return (uintptr_t)raw_handler - (uintptr_t)irq_handler_chain_slots < sizeof(irq_handler_chain_slots);
+}
+#else
+#define is_shared_irq_raw_handler(h) false
+#endif
+
+irq_handler_t irq_get_vtable_handler(uint num) {
+ check_irq_param(num);
+ return get_vtable()[16 + num];
+}
+
+void irq_set_exclusive_handler(uint num, irq_handler_t handler) {
+ check_irq_param(num);
+#if !PICO_NO_RAM_VECTOR_TABLE
+ spin_lock_t *lock = spin_lock_instance(PICO_SPINLOCK_ID_IRQ);
+ uint32_t save = spin_lock_blocking(lock);
+ __unused irq_handler_t current = irq_get_vtable_handler(num);
+ hard_assert(current == __unhandled_user_irq || current == handler);
+ set_raw_irq_handler_and_unlock(num, handler, save);
+#else
+ panic_unsupported();
+#endif
+}
+
+irq_handler_t irq_get_exclusive_handler(uint num) {
+ check_irq_param(num);
+#if !PICO_NO_RAM_VECTOR_TABLE
+ spin_lock_t *lock = spin_lock_instance(PICO_SPINLOCK_ID_IRQ);
+ uint32_t save = spin_lock_blocking(lock);
+ irq_handler_t current = irq_get_vtable_handler(num);
+ spin_unlock(lock, save);
+ if (current == __unhandled_user_irq || is_shared_irq_raw_handler(current)) {
+ return NULL;
+ }
+ return current;
+#else
+ panic_unsupported();
+#endif
+}
+
+
+#if !PICO_DISABLE_SHARED_IRQ_HANDLERS
+static uint16_t make_branch(uint16_t *from, void *to) {
+ uint32_t ui_from = (uint32_t)from;
+ uint32_t ui_to = (uint32_t)to;
+ uint32_t delta = (ui_to - ui_from - 4) / 2;
+ assert(!(delta >> 11u));
+ return (uint16_t)(0xe000 | (delta & 0x7ff));
+}
+
+static void insert_branch_and_link(uint16_t *from, void *to) {
+ uint32_t ui_from = (uint32_t)from;
+ uint32_t ui_to = (uint32_t)to;
+ uint32_t delta = (ui_to - ui_from - 4) / 2;
+ assert(!(delta >> 11u));
+ from[0] = (uint16_t)(0xf000 | ((delta >> 11u) & 0x7ffu));
+ from[1] = (uint16_t)(0xf800 | (delta & 0x7ffu));
+}
+
+static inline void *resolve_branch(uint16_t *inst) {
+ assert(0x1c == (*inst)>>11u);
+ int32_t i_addr = (*inst) << 21u;
+ i_addr /= (int32_t)(1u<<21u);
+ return inst + 2 + i_addr;
+}
+
+// GCC produces horrible code for subtraction of pointers here, and it was bugging me
+static inline int8_t slot_diff(struct irq_handler_chain_slot *to, struct irq_handler_chain_slot *from) {
+ static_assert(sizeof(struct irq_handler_chain_slot) == 12, "");
+ int32_t result = 0xaaaa;
+ // return (to - from);
+ // note this implementation has limited range, but is fine for plenty more than -128->127 result
+ asm (".syntax unified\n"
+ "subs %1, %2\n"
+ "adcs %1, %1\n" // * 2 (and + 1 if negative for rounding)
+ "muls %0, %1\n"
+ "lsrs %0, 20\n"
+ : "+l" (result), "+l" (to)
+ : "l" (from)
+ :
+ );
+ return (int8_t)result;
+}
+
+static inline int8_t get_slot_index(struct irq_handler_chain_slot *slot) {
+ return slot_diff(slot, irq_handler_chain_slots);
+}
+#endif
+
+void irq_add_shared_handler(uint num, irq_handler_t handler, uint8_t order_priority) {
+ check_irq_param(num);
+#if PICO_NO_RAM_VECTOR_TABLE
+ panic_unsupported()
+#elif PICO_DISABLE_SHARED_IRQ_HANDLERS
+ irq_set_exclusive_handler(num, handler);
+#else
+ spin_lock_t *lock = spin_lock_instance(PICO_SPINLOCK_ID_IRQ);
+ uint32_t save = spin_lock_blocking(lock);
+ hard_assert(irq_hander_chain_free_slot_head >= 0); // we must have a slot
+ struct irq_handler_chain_slot *slot = &irq_handler_chain_slots[irq_hander_chain_free_slot_head];
+ int8_t slot_index = irq_hander_chain_free_slot_head;
+ irq_hander_chain_free_slot_head = slot->link;
+ irq_handler_t vtable_handler = get_vtable()[16 + num];
+ if (!is_shared_irq_raw_handler(vtable_handler)) {
+ // start new chain
+ hard_assert(vtable_handler == __unhandled_user_irq);
+ struct irq_handler_chain_slot slot_data = {
+ .inst1 = 0xa100, // add r1, pc, #0
+ .inst2 = make_branch(&slot->inst2, irq_handler_chain_first_slot), // b irq_handler_chain_first_slot
+ .inst3 = 0xbd00, // pop {pc}
+ .link = -1,
+ .priority = order_priority,
+ .handler = handler
+ };
+ *slot = slot_data;
+ vtable_handler = (irq_handler_t)add_thumb_bit(slot);
+ } else {
+ assert(!((((uintptr_t)vtable_handler) - ((uintptr_t)irq_handler_chain_slots) - 1)%sizeof(struct irq_handler_chain_slot)));
+ struct irq_handler_chain_slot *prev_slot = NULL;
+ struct irq_handler_chain_slot *existing_vtable_slot = remove_thumb_bit(vtable_handler);
+ struct irq_handler_chain_slot *cur_slot = existing_vtable_slot;
+ while (cur_slot->priority > order_priority) {
+ prev_slot = cur_slot;
+ if (cur_slot->link < 0) break;
+ cur_slot = &irq_handler_chain_slots[cur_slot->link];
+ }
+ if (prev_slot) {
+ // insert into chain
+ struct irq_handler_chain_slot slot_data = {
+ .inst1 = 0x4801, // ldr r0, [pc, #4]
+ .inst2 = 0x4780, // blx r0
+ .inst3 = prev_slot->link >= 0 ?
+ make_branch(&slot->inst3, resolve_branch(&prev_slot->inst3)) : // b next_slot
+ 0xbd00, // pop {pc}
+ .link = prev_slot->link,
+ .priority = order_priority,
+ .handler = handler
+ };
+ // update code and data links
+ prev_slot->inst3 = make_branch(&prev_slot->inst3, slot),
+ prev_slot->link = slot_index;
+ *slot = slot_data;
+ } else {
+ // update with new chain head
+ struct irq_handler_chain_slot slot_data = {
+ .inst1 = 0xa100, // add r1, pc, #0
+ .inst2 = make_branch(&slot->inst2, irq_handler_chain_first_slot), // b irq_handler_chain_first_slot
+ .inst3 = make_branch(&slot->inst3, existing_vtable_slot), // b existing_slot
+ .link = get_slot_index(existing_vtable_slot),
+ .priority = order_priority,
+ .handler = handler
+ };
+ *slot = slot_data;
+ // fixup previous head slot
+ existing_vtable_slot->inst1 = 0x4801; // ldr r0, [pc, #4]
+ existing_vtable_slot->inst2 = 0x4780; // blx r0
+ vtable_handler = (irq_handler_t)add_thumb_bit(slot);
+ }
+ }
+ set_raw_irq_handler_and_unlock(num, vtable_handler, save);
+#endif
+}
+
+void irq_remove_handler(uint num, irq_handler_t handler) {
+#if !PICO_NO_RAM_VECTOR_TABLE
+ spin_lock_t *lock = spin_lock_instance(PICO_SPINLOCK_ID_IRQ);
+ uint32_t save = spin_lock_blocking(lock);
+ irq_handler_t vtable_handler = get_vtable()[16 + num];
+ if (vtable_handler != __unhandled_user_irq && vtable_handler != handler) {
+#if !PICO_DISABLE_SHARED_IRQ_HANDLERS
+ if (is_shared_irq_raw_handler(vtable_handler)) {
+ // This is a bit tricky, as an executing IRQ handler doesn't take a lock.
+
+ // First thing to do is to disable the IRQ in question; that takes care of calls from user code.
+ // Note that a irq handler chain is local to our own core, so we don't need to worry about the other core
+ bool was_enabled = irq_is_enabled(num);
+ irq_set_enabled(num, false);
+ __dmb();
+
+ // It is possible we are being called while an IRQ for this chain is already in progress.
+ // The issue we have here is that we must not free a slot that is currently being executed, because
+ // inst3 is still to be executed, and inst3 might get overwritten if the slot is re-used.
+
+ // By disallowing other exceptions from removing an IRQ handler (which seems fair)
+ // we now only have to worry about removing a slot from a chain that is currently executing.
+
+ // Note we expect that the slot we are deleting is the one that is executing.
+ // In particular, bad things happen if the caller were to delete the handler in the chain
+ // before it. This is not an allowed use case though, and I can't imagine anyone wanting to in practice.
+ // Sadly this is not something we can detect.
+
+ uint exception = __get_current_exception();
+ hard_assert(!exception || exception == num + 16);
+
+ struct irq_handler_chain_slot *prev_slot = NULL;
+ struct irq_handler_chain_slot *existing_vtable_slot = remove_thumb_bit(vtable_handler);
+ struct irq_handler_chain_slot *to_free_slot = existing_vtable_slot;
+ int8_t to_free_slot_index = get_slot_index(to_free_slot);
+ while (to_free_slot->handler != handler) {
+ prev_slot = to_free_slot;
+ if (to_free_slot->link < 0) break;
+ to_free_slot = &irq_handler_chain_slots[to_free_slot->link];
+ }
+ if (to_free_slot->handler == handler) {
+ int8_t next_slot_index = to_free_slot->link;
+ if (next_slot_index >= 0) {
+ // There is another slot in the chain, so copy that over us, so that our inst3 points at something valid
+ // Note this only matters in the exception case anyway, and it that case, we will skip the next handler,
+ // however in that case it's IRQ cause should immediately cause re-entry of the IRQ and the only side
+ // effect will be that there was potentially brief out of priority order execution of the handlers
+ struct irq_handler_chain_slot *next_slot = &irq_handler_chain_slots[next_slot_index];
+ to_free_slot->handler = next_slot->handler;
+ to_free_slot->priority = next_slot->priority;
+ to_free_slot->link = next_slot->link;
+ to_free_slot->inst3 = next_slot->link >= 0 ?
+ make_branch(&to_free_slot->inst3, resolve_branch(&next_slot->inst3)) : // b mext_>slot->next_slot
+ 0xbd00; // pop {pc}
+
+ // add old next slot back to free list
+ next_slot->link = irq_hander_chain_free_slot_head;
+ irq_hander_chain_free_slot_head = next_slot_index;
+ } else {
+ // Slot being removed is at the end of the chain
+ if (!exception) {
+ // case when we're not in exception, we physically unlink now
+ if (prev_slot) {
+ // chain is not empty
+ prev_slot->link = -1;
+ prev_slot->inst3 = 0xbd00; // pop {pc}
+ } else {
+ // chain is not empty
+ vtable_handler = __unhandled_user_irq;
+ }
+ // add slot back to free list
+ to_free_slot->link = irq_hander_chain_free_slot_head;
+ irq_hander_chain_free_slot_head = to_free_slot_index;
+ } else {
+ // since we are the last slot we know that our inst3 hasn't executed yet, so we change
+ // it to bl to irq_handler_chain_remove_tail which will remove the slot.
+ // NOTE THAT THIS TRASHES PRIORITY AND LINK SINCE THIS IS A 4 BYTE INSTRUCTION
+ // BUT THEY ARE NOT NEEDED NOW
+ insert_branch_and_link(&to_free_slot->inst3, irq_handler_chain_remove_tail);
+ }
+ }
+ } else {
+ assert(false); // not found
+ }
+ irq_set_enabled(num, was_enabled);
+ }
+#else
+ assert(false); // not found
+#endif
+ } else {
+ vtable_handler = __unhandled_user_irq;
+ }
+ set_raw_irq_handler_and_unlock(num, vtable_handler, save);
+#else
+ panic_unsupported();
+#endif
+}
+
+void irq_set_priority(uint num, uint8_t hardware_priority) {
+ check_irq_param(num);
+
+ // note that only 32 bit writes are supported
+ io_rw_32 *p = (io_rw_32 *)((PPB_BASE + M0PLUS_NVIC_IPR0_OFFSET) + (num & ~3u));
+ *p = (*p & ~(0xffu << (8 * (num & 3u)))) | (((uint32_t) hardware_priority) << (8 * (num & 3u)));
+}
+
+uint irq_get_priority(uint num) {
+ check_irq_param(num);
+
+ // note that only 32 bit reads are supported
+ io_rw_32 *p = (io_rw_32 *)((PPB_BASE + M0PLUS_NVIC_IPR0_OFFSET) + (num & ~3u));
+ return (uint8_t)(*p >> (8 * (num & 3u)));
+}
+
+#if !PICO_DISABLE_SHARED_IRQ_HANDLERS
+// used by irq_handler_chain.S to remove the last link in a handler chain after it executes
+// note this must be called only with the last slot in a chain (and during the exception)
+void irq_add_tail_to_free_list(struct irq_handler_chain_slot *slot) {
+ irq_handler_t slot_handler = (irq_handler_t) add_thumb_bit(slot);
+ assert(is_shared_irq_raw_handler(slot_handler));
+
+ uint exception = __get_current_exception();
+ assert(exception);
+ spin_lock_t *lock = spin_lock_instance(PICO_SPINLOCK_ID_IRQ);
+ uint32_t save = spin_lock_blocking(lock);
+ int8_t slot_index = get_slot_index(slot);
+ if (slot_handler == get_vtable()[exception]) {
+ get_vtable()[exception] = __unhandled_user_irq;
+ } else {
+ bool __unused found = false;
+ // need to find who points at the slot and update it
+ for(uint i=0;i<count_of(irq_handler_chain_slots);i++) {
+ if (irq_handler_chain_slots[i].link == slot_index) {
+ irq_handler_chain_slots[i].link = -1;
+ irq_handler_chain_slots[i].inst3 = 0xbd00; // pop {pc}
+ found = true;
+ break;
+ }
+ }
+ assert(found);
+ }
+ // add slot to free list
+ slot->link = irq_hander_chain_free_slot_head;
+ irq_hander_chain_free_slot_head = slot_index;
+ spin_unlock(lock, save);
+}
+#endif
+
+void irq_init_priorities() {
+#if PICO_DEFAULT_IRQ_PRIORITY != 0
+ static_assert(!(NUM_IRQS & 3), "");
+ uint32_t prio4 = (PICO_DEFAULT_IRQ_PRIORITY & 0xff) * 0x1010101u;
+ io_rw_32 * p = (io_rw_32 *)(PPB_BASE + M0PLUS_NVIC_IPR0_OFFSET);
+ for (uint i = 0; i < NUM_IRQS / 4; i++) {
+ *p++ = prio4;
+ }
+#endif
+}
diff --git a/src/rp2_common/hardware_irq/irq_handler_chain.S b/src/rp2_common/hardware_irq/irq_handler_chain.S
new file mode 100644
index 0000000..7d7be7d
--- /dev/null
+++ b/src/rp2_common/hardware_irq/irq_handler_chain.S
@@ -0,0 +1,70 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "pico.h"
+#include "hardware/irq.h"
+
+#if !PICO_DISABLE_SHARED_IRQ_HANDLERS
+.syntax unified
+.cpu cortex-m0plus
+.thumb
+
+.data
+.align 2
+
+.global irq_handler_chain_slots
+
+.global irq_handler_chain_first_slot
+.global irq_handler_chain_remove_tail
+
+//
+// These Slots make up the code and structure of the handler chains; the only external information are the VTABLE entries
+// (obviously one set per core) and a free list head. Each individual handler chain starts with the VTABLE entry I
+// pointing at the address of slot S (with thumb bit set). Thus each slot which is part of a chain is executble.
+//
+// The execution jumps (via branch instruction) from one slot to the other, then jumps to the end of chain handler.
+// The entirety of the state needed to traverse the chain is contained within the slots of the chain, which is why
+// a VTABLE entry is all that is needed per chain (rather than requiring a separarte set of head pointers)
+//
+
+irq_handler_chain_slots:
+.set next_slot_number, 1
+.rept PICO_MAX_SHARED_IRQ_HANDLERS
+ // a slot is executable and is always 3 instructions long.
+ .hword 0 // inst1 (either: ldr r0, [pc, #4] or for the FIRST slot : add r1, pc, #0 )
+ .hword 0 // inst2 ( blx r0 b irq_handler_chain_first_slot )
+
+ .hword 0 // inst3 (either: b next_slot or for the LAST pop {pc} )
+
+ // next is a single byte index of next slot in chain (or -1 to end)
+.if next_slot_number == PICO_MAX_SHARED_IRQ_HANDLERS
+ .byte 0xff
+.else
+ .byte next_slot_number
+.endif
+ // next is the 8 bit unsigned priority
+ .byte 0x00
+1:
+ // and finally the handler function pointer
+ .word 0x00000000
+ .set next_slot_number, next_slot_number + 1
+.endr
+
+irq_handler_chain_first_slot:
+ push {lr}
+ ldr r0, [r1, #4]
+ adds r1, #1
+ mov lr, r1
+ bx r0
+irq_handler_chain_remove_tail:
+ mov r0, lr
+ subs r0, #9
+ ldr r1, =irq_add_tail_to_free_list
+ blx r1
+ pop {pc}
+
+
+#endif
diff --git a/src/rp2_common/hardware_pio/CMakeLists.txt b/src/rp2_common/hardware_pio/CMakeLists.txt
new file mode 100644
index 0000000..ad01869
--- /dev/null
+++ b/src/rp2_common/hardware_pio/CMakeLists.txt
@@ -0,0 +1,4 @@
+pico_simple_hardware_target(pio)
+
+# additional libraries
+target_link_libraries(hardware_pio INTERFACE hardware_gpio hardware_claim)
\ No newline at end of file
diff --git a/src/rp2_common/hardware_pio/include/hardware/pio.h b/src/rp2_common/hardware_pio/include/hardware/pio.h
new file mode 100644
index 0000000..d2377ac
--- /dev/null
+++ b/src/rp2_common/hardware_pio/include/hardware/pio.h
@@ -0,0 +1,1311 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _HARDWARE_PIO_H_
+#define _HARDWARE_PIO_H_
+
+#include "pico.h"
+#include "hardware/address_mapped.h"
+#include "hardware/structs/pio.h"
+#include "hardware/gpio.h"
+#include "hardware/regs/dreq.h"
+#include "hardware/pio_instructions.h"
+
+// PICO_CONFIG: PARAM_ASSERTIONS_ENABLED_PIO, Enable/disable assertions in the PIO module, type=bool, default=0, group=hardware_pio
+#ifndef PARAM_ASSERTIONS_ENABLED_PIO
+#define PARAM_ASSERTIONS_ENABLED_PIO 0
+#endif
+
+/** \file hardware/pio.h
+ * \defgroup hardware_pio hardware_pio
+ *
+ * Programmable I/O (PIO) API
+ *
+ * A programmable input/output block (PIO) is a versatile hardware interface which
+ * can support a number of different IO standards. There are two PIO blocks in the RP2040.
+ *
+ * Each PIO is programmable in the same sense as a processor: the four state machines independently
+ * execute short, sequential programs, to manipulate GPIOs and transfer data. Unlike a general
+ * purpose processor, PIO state machines are highly specialised for IO, with a focus on determinism,
+ * precise timing, and close integration with fixed-function hardware. Each state machine is equipped
+ * with:
+ * * Two 32-bit shift registers – either direction, any shift count
+ * * Two 32-bit scratch registers
+ * * 4×32 bit bus FIFO in each direction (TX/RX), reconfigurable as 8×32 in a single direction
+ * * Fractional clock divider (16 integer, 8 fractional bits)
+ * * Flexible GPIO mapping
+ * * DMA interface, sustained throughput up to 1 word per clock from system DMA
+ * * IRQ flag set/clear/status
+ *
+ * Full details of the PIO can be found in the RP2040 datasheet.
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+static_assert(PIO_SM0_SHIFTCTRL_FJOIN_RX_LSB == PIO_SM0_SHIFTCTRL_FJOIN_TX_LSB + 1, "");
+
+/** \brief FIFO join states
+ * \ingroup hardware_pio
+ */
+enum pio_fifo_join {
+ PIO_FIFO_JOIN_NONE = 0,
+ PIO_FIFO_JOIN_TX = 1,
+ PIO_FIFO_JOIN_RX = 2,
+};
+
+/** \brief MOV status types
+ * \ingroup hardware_pio
+ */
+enum pio_mov_status_type {
+ STATUS_TX_LESSTHAN = 0,
+ STATUS_RX_LESSTHAN = 1
+};
+
+typedef pio_hw_t *PIO;
+
+/** Identifier for the first (PIO 0) hardware PIO instance (for use in PIO functions).
+ *
+ * e.g. pio_gpio_init(pio0, 5)
+ *
+ * \ingroup hardware_pio
+ * @{
+ */
+#define pio0 pio0_hw
+/** @} */
+
+/** Identifier for the second (PIO 1) hardware PIO instance (for use in PIO functions).
+ *
+ * e.g. pio_gpio_init(pio1, 5)
+ *
+ * \ingroup hardware_pio
+ * @{
+ */
+#define pio1 pio1_hw
+/** @} */
+
+/** \brief PIO state machine configuration
+ * \defgroup sm_config sm_config
+ * \ingroup hardware_pio
+ *
+ * A PIO block needs to be configured, these functions provide helpers to set up configuration
+ * structures. See \ref pio_sm_set_config
+ *
+ */
+
+/** \brief PIO Configuration structure
+ * \ingroup sm_config
+ */
+typedef struct {
+ uint32_t clkdiv;
+ uint32_t execctrl;
+ uint32_t shiftctrl;
+ uint32_t pinctrl;
+} pio_sm_config;
+
+static inline void check_sm_param(__unused uint sm) {
+ valid_params_if(PIO, sm < NUM_PIO_STATE_MACHINES);
+}
+
+static inline void check_sm_mask(__unused uint mask) {
+ valid_params_if(PIO, mask < (1u << NUM_PIO_STATE_MACHINES));
+}
+
+
+static inline void check_pio_param(__unused PIO pio) {
+ valid_params_if(PIO, pio == pio0 || pio == pio1);
+}
+
+/*! \brief Set the 'out' pins in a state machine configuration
+ * \ingroup sm_config
+ *
+ * Can overlap with the 'in', 'set' and 'sideset' pins
+ *
+ * \param c Pointer to the configuration structure to modify
+ * \param out_base 0-31 First pin to set as output
+ * \param out_count 0-32 Number of pins to set.
+ */
+static inline void sm_config_set_out_pins(pio_sm_config *c, uint out_base, uint out_count) {
+ valid_params_if(PIO, out_base < 32);
+ valid_params_if(PIO, out_count <= 32);
+ c->pinctrl = (c->pinctrl & ~(PIO_SM0_PINCTRL_OUT_BASE_BITS | PIO_SM0_PINCTRL_OUT_COUNT_BITS)) |
+ (out_base << PIO_SM0_PINCTRL_OUT_BASE_LSB) |
+ (out_count << PIO_SM0_PINCTRL_OUT_COUNT_LSB);
+}
+
+/*! \brief Set the 'set' pins in a state machine configuration
+ * \ingroup sm_config
+ *
+ * Can overlap with the 'in', 'out' and 'sideset' pins
+ *
+ * \param c Pointer to the configuration structure to modify
+ * \param set_base 0-31 First pin to set as
+ * \param set_count 0-5 Number of pins to set.
+ */
+static inline void sm_config_set_set_pins(pio_sm_config *c, uint set_base, uint set_count) {
+ valid_params_if(PIO, set_base < 32);
+ valid_params_if(PIO, set_count <= 5);
+ c->pinctrl = (c->pinctrl & ~(PIO_SM0_PINCTRL_SET_BASE_BITS | PIO_SM0_PINCTRL_SET_COUNT_BITS)) |
+ (set_base << PIO_SM0_PINCTRL_SET_BASE_LSB) |
+ (set_count << PIO_SM0_PINCTRL_SET_COUNT_LSB);
+}
+
+/*! \brief Set the 'in' pins in a state machine configuration
+ * \ingroup sm_config
+ *
+ * Can overlap with the 'out', ''set' and 'sideset' pins
+ *
+ * \param c Pointer to the configuration structure to modify
+ * \param in_base 0-31 First pin to use as input
+ */
+static inline void sm_config_set_in_pins(pio_sm_config *c, uint in_base) {
+ valid_params_if(PIO, in_base < 32);
+ c->pinctrl = (c->pinctrl & ~PIO_SM0_PINCTRL_IN_BASE_BITS) |
+ (in_base << PIO_SM0_PINCTRL_IN_BASE_LSB);
+}
+
+/*! \brief Set the 'sideset' pins in a state machine configuration
+ * \ingroup sm_config
+ *
+ * Can overlap with the 'in', 'out' and 'set' pins
+ *
+ * \param c Pointer to the configuration structure to modify
+ * \param sideset_base 0-31 base pin for 'side set'
+ */
+static inline void sm_config_set_sideset_pins(pio_sm_config *c, uint sideset_base) {
+ valid_params_if(PIO, sideset_base < 32);
+ c->pinctrl = (c->pinctrl & ~PIO_SM0_PINCTRL_SIDESET_BASE_BITS) |
+ (sideset_base << PIO_SM0_PINCTRL_SIDESET_BASE_LSB);
+}
+
+/*! \brief Set the 'sideset' options in a state machine configuration
+ * \ingroup sm_config
+ *
+ * \param c Pointer to the configuration structure to modify
+ * \param bit_count Number of bits to steal from delay field in the instruction for use of side set (max 5)
+ * \param optional True if the topmost side set bit is used as a flag for whether to apply side set on that instruction
+ * \param pindirs True if the side set affects pin directions rather than values
+ */
+static inline void sm_config_set_sideset(pio_sm_config *c, uint bit_count, bool optional, bool pindirs) {
+ valid_params_if(PIO, bit_count <= 5);
+ valid_params_if(PIO, !optional || bit_count >= 1);
+ c->pinctrl = (c->pinctrl & ~PIO_SM0_PINCTRL_SIDESET_COUNT_BITS) |
+ (bit_count << PIO_SM0_PINCTRL_SIDESET_COUNT_LSB);
+
+ c->execctrl = (c->execctrl & ~(PIO_SM0_EXECCTRL_SIDE_EN_BITS | PIO_SM0_EXECCTRL_SIDE_PINDIR_BITS)) |
+ (bool_to_bit(optional) << PIO_SM0_EXECCTRL_SIDE_EN_LSB) |
+ (bool_to_bit(pindirs) << PIO_SM0_EXECCTRL_SIDE_PINDIR_LSB);
+}
+
+/*! \brief Set the state machine clock divider (from integer and fractional parts - 16:8) in a state machine configuration
+ * \ingroup sm_config
+ *
+ * The clock divider can slow the state machine's execution to some rate below
+ * the system clock frequency, by enabling the state machine on some cycles
+ * but not on others, in a regular pattern. This can be used to generate e.g.
+ * a given UART baud rate. See the datasheet for further detail.
+ *
+ * \param c Pointer to the configuration structure to modify
+ * \param div_int Integer part of the divisor
+ * \param div_frac Fractional part in 1/256ths
+ * \sa sm_config_set_clkdiv()
+ */
+static inline void sm_config_set_clkdiv_int_frac(pio_sm_config *c, uint16_t div_int, uint8_t div_frac) {
+ c->clkdiv =
+ (((uint)div_frac) << PIO_SM0_CLKDIV_FRAC_LSB) |
+ (((uint)div_int) << PIO_SM0_CLKDIV_INT_LSB);
+}
+
+static inline void pio_calculate_clkdiv_from_float(float div, uint16_t *div_int, uint8_t *div_frac) {
+ valid_params_if(PIO, div >= 1 && div <= 65536);
+ *div_int = (uint16_t)div;
+ if (*div_int == 0) {
+ *div_frac = 0;
+ } else {
+ *div_frac = (uint8_t)((div - (float)*div_int) * (1u << 8u));
+ }
+}
+
+/*! \brief Set the state machine clock divider (from a floating point value) in a state machine configuration
+ * \ingroup sm_config
+ *
+ * The clock divider slows the state machine's execution by masking the
+ * system clock on some cycles, in a repeating pattern, so that the state
+ * machine does not advance. Effectively this produces a slower clock for the
+ * state machine to run from, which can be used to generate e.g. a particular
+ * UART baud rate. See the datasheet for further detail.
+ *
+ * \param c Pointer to the configuration structure to modify
+ * \param div The fractional divisor to be set. 1 for full speed. An integer clock divisor of n
+ * will cause the state machine to run 1 cycle in every n.
+ * Note that for small n, the jitter introduced by a fractional divider (e.g. 2.5) may be unacceptable
+ * although it will depend on the use case.
+ */
+static inline void sm_config_set_clkdiv(pio_sm_config *c, float div) {
+ uint16_t div_int;
+ uint8_t div_frac;
+ pio_calculate_clkdiv_from_float(div, &div_int, &div_frac);
+ sm_config_set_clkdiv_int_frac(c, div_int, div_frac);
+}
+
+/*! \brief Set the wrap addresses in a state machine configuration
+ * \ingroup sm_config
+ *
+ * \param c Pointer to the configuration structure to modify
+ * \param wrap_target the instruction memory address to wrap to
+ * \param wrap the instruction memory address after which to set the program counter to wrap_target
+ * if the instruction does not itself update the program_counter
+ */
+static inline void sm_config_set_wrap(pio_sm_config *c, uint wrap_target, uint wrap) {
+ valid_params_if(PIO, wrap < PIO_INSTRUCTION_COUNT);
+ valid_params_if(PIO, wrap_target < PIO_INSTRUCTION_COUNT);
+ c->execctrl = (c->execctrl & ~(PIO_SM0_EXECCTRL_WRAP_TOP_BITS | PIO_SM0_EXECCTRL_WRAP_BOTTOM_BITS)) |
+ (wrap_target << PIO_SM0_EXECCTRL_WRAP_BOTTOM_LSB) |
+ (wrap << PIO_SM0_EXECCTRL_WRAP_TOP_LSB);
+}
+
+/*! \brief Set the 'jmp' pin in a state machine configuration
+ * \ingroup sm_config
+ *
+ * \param c Pointer to the configuration structure to modify
+ * \param pin The raw GPIO pin number to use as the source for a `jmp pin` instruction
+ */
+static inline void sm_config_set_jmp_pin(pio_sm_config *c, uint pin) {
+ valid_params_if(PIO, pin < 32);
+ c->execctrl = (c->execctrl & ~PIO_SM0_EXECCTRL_JMP_PIN_BITS) |
+ (pin << PIO_SM0_EXECCTRL_JMP_PIN_LSB);
+}
+
+/*! \brief Setup 'in' shifting parameters in a state machine configuration
+ * \ingroup sm_config
+ *
+ * \param c Pointer to the configuration structure to modify
+ * \param shift_right true to shift ISR to right, false to shift ISR to left
+ * \param autopush whether autopush is enabled
+ * \param push_threshold threshold in bits to shift in before auto/conditional re-pushing of the ISR
+ */
+static inline void sm_config_set_in_shift(pio_sm_config *c, bool shift_right, bool autopush, uint push_threshold) {
+ valid_params_if(PIO, push_threshold <= 32);
+ c->shiftctrl = (c->shiftctrl &
+ ~(PIO_SM0_SHIFTCTRL_IN_SHIFTDIR_BITS |
+ PIO_SM0_SHIFTCTRL_AUTOPUSH_BITS |
+ PIO_SM0_SHIFTCTRL_PUSH_THRESH_BITS)) |
+ (bool_to_bit(shift_right) << PIO_SM0_SHIFTCTRL_IN_SHIFTDIR_LSB) |
+ (bool_to_bit(autopush) << PIO_SM0_SHIFTCTRL_AUTOPUSH_LSB) |
+ ((push_threshold & 0x1fu) << PIO_SM0_SHIFTCTRL_PUSH_THRESH_LSB);
+}
+
+/*! \brief Setup 'out' shifting parameters in a state machine configuration
+ * \ingroup sm_config
+ *
+ * \param c Pointer to the configuration structure to modify
+ * \param shift_right true to shift OSR to right, false to shift OSR to left
+ * \param autopull whether autopull is enabled
+ * \param pull_threshold threshold in bits to shift out before auto/conditional re-pulling of the OSR
+ */
+static inline void sm_config_set_out_shift(pio_sm_config *c, bool shift_right, bool autopull, uint pull_threshold) {
+ valid_params_if(PIO, pull_threshold <= 32);
+ c->shiftctrl = (c->shiftctrl &
+ ~(PIO_SM0_SHIFTCTRL_OUT_SHIFTDIR_BITS |
+ PIO_SM0_SHIFTCTRL_AUTOPULL_BITS |
+ PIO_SM0_SHIFTCTRL_PULL_THRESH_BITS)) |
+ (bool_to_bit(shift_right) << PIO_SM0_SHIFTCTRL_OUT_SHIFTDIR_LSB) |
+ (bool_to_bit(autopull) << PIO_SM0_SHIFTCTRL_AUTOPULL_LSB) |
+ ((pull_threshold & 0x1fu) << PIO_SM0_SHIFTCTRL_PULL_THRESH_LSB);
+}
+
+/*! \brief Setup the FIFO joining in a state machine configuration
+ * \ingroup sm_config
+ *
+ * \param c Pointer to the configuration structure to modify
+ * \param join Specifies the join type. \see enum pio_fifo_join
+ */
+static inline void sm_config_set_fifo_join(pio_sm_config *c, enum pio_fifo_join join) {
+ valid_params_if(PIO, join == PIO_FIFO_JOIN_NONE || join == PIO_FIFO_JOIN_TX || join == PIO_FIFO_JOIN_RX);
+ c->shiftctrl = (c->shiftctrl & (uint)~(PIO_SM0_SHIFTCTRL_FJOIN_TX_BITS | PIO_SM0_SHIFTCTRL_FJOIN_RX_BITS)) |
+ (((uint)join) << PIO_SM0_SHIFTCTRL_FJOIN_TX_LSB);
+}
+
+/*! \brief Set special 'out' operations in a state machine configuration
+ * \ingroup sm_config
+ *
+ * \param c Pointer to the configuration structure to modify
+ * \param sticky to enable 'sticky' output (i.e. re-asserting most recent OUT/SET pin values on subsequent cycles)
+ * \param has_enable_pin true to enable auxiliary OUT enable pin
+ * \param enable_pin_index pin index for auxiliary OUT enable
+ */
+static inline void sm_config_set_out_special(pio_sm_config *c, bool sticky, bool has_enable_pin, uint enable_pin_index) {
+ c->execctrl = (c->execctrl &
+ (uint)~(PIO_SM0_EXECCTRL_OUT_STICKY_BITS | PIO_SM0_EXECCTRL_INLINE_OUT_EN_BITS |
+ PIO_SM0_EXECCTRL_OUT_EN_SEL_BITS)) |
+ (bool_to_bit(sticky) << PIO_SM0_EXECCTRL_OUT_STICKY_LSB) |
+ (bool_to_bit(has_enable_pin) << PIO_SM0_EXECCTRL_INLINE_OUT_EN_LSB) |
+ ((enable_pin_index << PIO_SM0_EXECCTRL_OUT_EN_SEL_LSB) & PIO_SM0_EXECCTRL_OUT_EN_SEL_BITS);
+}
+
+/*! \brief Set source for 'mov status' in a state machine configuration
+ * \ingroup sm_config
+ *
+ * \param c Pointer to the configuration structure to modify
+ * \param status_sel the status operation selector. \see enum pio_mov_status_type
+ * \param status_n parameter for the mov status operation (currently a bit count)
+ */
+static inline void sm_config_set_mov_status(pio_sm_config *c, enum pio_mov_status_type status_sel, uint status_n) {
+ valid_params_if(PIO, status_sel == STATUS_TX_LESSTHAN || status_sel == STATUS_RX_LESSTHAN);
+ c->execctrl = (c->execctrl
+ & ~(PIO_SM0_EXECCTRL_STATUS_SEL_BITS | PIO_SM0_EXECCTRL_STATUS_N_BITS))
+ | ((((uint)status_sel) << PIO_SM0_EXECCTRL_STATUS_SEL_LSB) & PIO_SM0_EXECCTRL_STATUS_SEL_BITS)
+ | ((status_n << PIO_SM0_EXECCTRL_STATUS_N_LSB) & PIO_SM0_EXECCTRL_STATUS_N_BITS);
+}
+
+
+/*! \brief Get the default state machine configuration
+ * \ingroup sm_config
+ *
+ * Setting | Default
+ * --------|--------
+ * Out Pins | 32 starting at 0
+ * Set Pins | 0 starting at 0
+ * In Pins (base) | 0
+ * Side Set Pins (base) | 0
+ * Side Set | disabled
+ * Wrap | wrap=31, wrap_to=0
+ * In Shift | shift_direction=right, autopush=false, push_thrshold=32
+ * Out Shift | shift_direction=right, autopull=false, pull_thrshold=32
+ * Jmp Pin | 0
+ * Out Special | sticky=false, has_enable_pin=false, enable_pin_index=0
+ * Mov Status | status_sel=STATUS_TX_LESSTHAN, n=0
+ *
+ * \return the default state machine configuration which can then be modified.
+ */
+static inline pio_sm_config pio_get_default_sm_config(void) {
+ pio_sm_config c = {0, 0, 0, 0};
+ sm_config_set_clkdiv_int_frac(&c, 1, 0);
+ sm_config_set_wrap(&c, 0, 31);
+ sm_config_set_in_shift(&c, true, false, 32);
+ sm_config_set_out_shift(&c, true, false, 32);
+ return c;
+}
+
+/*! \brief Apply a state machine configuration to a state machine
+ * \ingroup hardware_pio
+ *
+ * \param pio Handle to PIO instance; either \ref pio0 or \ref pio1
+ * \param sm State machine index (0..3)
+ * \param config the configuration to apply
+*/
+static inline void pio_sm_set_config(PIO pio, uint sm, const pio_sm_config *config) {
+ check_pio_param(pio);
+ check_sm_param(sm);
+ pio->sm[sm].clkdiv = config->clkdiv;
+ pio->sm[sm].execctrl = config->execctrl;
+ pio->sm[sm].shiftctrl = config->shiftctrl;
+ pio->sm[sm].pinctrl = config->pinctrl;
+}
+
+/*! \brief Return the instance number of a PIO instance
+ * \ingroup hardware_pio
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \return the PIO instance number (either 0 or 1)
+ */
+static inline uint pio_get_index(PIO pio) {
+ check_pio_param(pio);
+ return pio == pio1 ? 1 : 0;
+}
+
+/*! \brief Setup the function select for a GPIO to use output from the given PIO instance
+ * \ingroup hardware_pio
+ *
+ * PIO appears as an alternate function in the GPIO muxing, just like an SPI
+ * or UART. This function configures that multiplexing to connect a given PIO
+ * instance to a GPIO. Note that this is not necessary for a state machine to
+ * be able to read the *input* value from a GPIO, but only for it to set the
+ * output value or output enable.
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param pin the GPIO pin whose function select to set
+ */
+static inline void pio_gpio_init(PIO pio, uint pin) {
+ check_pio_param(pio);
+ valid_params_if(PIO, pin < 32);
+ gpio_set_function(pin, pio == pio0 ? GPIO_FUNC_PIO0 : GPIO_FUNC_PIO1);
+}
+
+/*! \brief Return the DREQ to use for pacing transfers to/from a particular state machine FIFO
+ * \ingroup hardware_pio
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param sm State machine index (0..3)
+ * \param is_tx true for sending data to the state machine, false for receiving data from the state machine
+ */
+static inline uint pio_get_dreq(PIO pio, uint sm, bool is_tx) {
+ static_assert(DREQ_PIO0_TX1 == DREQ_PIO0_TX0 + 1, "");
+ static_assert(DREQ_PIO0_TX2 == DREQ_PIO0_TX0 + 2, "");
+ static_assert(DREQ_PIO0_TX3 == DREQ_PIO0_TX0 + 3, "");
+ static_assert(DREQ_PIO0_RX0 == DREQ_PIO0_TX0 + NUM_PIO_STATE_MACHINES, "");
+ static_assert(DREQ_PIO1_RX0 == DREQ_PIO1_TX0 + NUM_PIO_STATE_MACHINES, "");
+ check_pio_param(pio);
+ check_sm_param(sm);
+ return sm + (is_tx ? 0 : NUM_PIO_STATE_MACHINES) + (pio == pio0 ? DREQ_PIO0_TX0 : DREQ_PIO1_TX0);
+}
+
+typedef struct pio_program {
+ const uint16_t *instructions;
+ uint8_t length;
+ int8_t origin; // required instruction memory origin or -1
+} __packed pio_program_t;
+
+/*! \brief Determine whether the given program can (at the time of the call) be loaded onto the PIO instance
+ * \ingroup hardware_pio
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param program the program definition
+ * \return true if the program can be loaded; false if there is not suitable space in the instruction memory
+ */
+bool pio_can_add_program(PIO pio, const pio_program_t *program);
+
+/*! \brief Determine whether the given program can (at the time of the call) be loaded onto the PIO instance starting at a particular location
+ * \ingroup hardware_pio
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param program the program definition
+ * \param offset the instruction memory offset wanted for the start of the program
+ * \return true if the program can be loaded at that location; false if there is not space in the instruction memory
+ */
+bool pio_can_add_program_at_offset(PIO pio, const pio_program_t *program, uint offset);
+
+/*! \brief Attempt to load the program, panicking if not possible
+ * \ingroup hardware_pio
+ *
+ * \see pio_can_add_program() if you need to check whether the program can be loaded
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param program the program definition
+ * \return the instruction memory offset the program is loaded at
+ */
+uint pio_add_program(PIO pio, const pio_program_t *program);
+
+/*! \brief Attempt to load the program at the specified instruction memory offset, panicking if not possible
+ * \ingroup hardware_pio
+ *
+ * \see pio_can_add_program_at_offset() if you need to check whether the program can be loaded
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param program the program definition
+ * \param offset the instruction memory offset wanted for the start of the program
+ */
+void pio_add_program_at_offset(PIO pio, const pio_program_t *program, uint offset);
+
+/*! \brief Remove a program from a PIO instance's instruction memory
+ * \ingroup hardware_pio
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param program the program definition
+ * \param loaded_offset the loaded offset returned when the program was added
+ */
+void pio_remove_program(PIO pio, const pio_program_t *program, uint loaded_offset);
+
+/*! \brief Clears all of a PIO instance's instruction memory
+ * \ingroup hardware_pio
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ */
+void pio_clear_instruction_memory(PIO pio);
+
+/*! \brief Resets the state machine to a consistent state, and configures it
+ * \ingroup hardware_pio
+ *
+ * This method:
+ * - Disables the state machine (if running)
+ * - Clears the FIFOs
+ * - Applies the configuration specified by 'config'
+ * - Resets any internal state e.g. shift counters
+ * - Jumps to the initial program location given by 'initial_pc'
+ *
+ * The state machine is left disabled on return from this call.
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param sm State machine index (0..3)
+ * \param initial_pc the initial program memory offset to run from
+ * \param config the configuration to apply (or NULL to apply defaults)
+ */
+void pio_sm_init(PIO pio, uint sm, uint initial_pc, const pio_sm_config *config);
+
+/*! \brief Enable or disable a PIO state machine
+ * \ingroup hardware_pio
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param sm State machine index (0..3)
+ * \param enabled true to enable the state machine; false to disable
+ */
+static inline void pio_sm_set_enabled(PIO pio, uint sm, bool enabled) {
+ check_pio_param(pio);
+ check_sm_param(sm);
+ pio->ctrl = (pio->ctrl & ~(1u << sm)) | (bool_to_bit(enabled) << sm);
+}
+
+/*! \brief Enable or disable multiple PIO state machines
+ * \ingroup hardware_pio
+ *
+ * Note that this method just sets the enabled state of the state machine;
+ * if now enabled they continue exactly from where they left off.
+ *
+ * \see pio_enable_sm_mask_in_sync() if you wish to enable multiple state machines
+ * and ensure their clock dividers are in sync.
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param mask bit mask of state machine indexes to modify the enabled state of
+ * \param enabled true to enable the state machines; false to disable
+ */
+static inline void pio_set_sm_mask_enabled(PIO pio, uint32_t mask, bool enabled) {
+ check_pio_param(pio);
+ check_sm_mask(mask);
+ pio->ctrl = (pio->ctrl & ~mask) | (enabled ? mask : 0u);
+}
+
+/*! \brief Restart a state machine with a known state
+ * \ingroup hardware_pio
+ *
+ * This method clears the ISR, shift counters, clock divider counter
+ * pin write flags, delay counter, latched EXEC instruction, and IRQ wait condition.
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param sm State machine index (0..3)
+ */
+static inline void pio_sm_restart(PIO pio, uint sm) {
+ check_pio_param(pio);
+ check_sm_param(sm);
+ pio->ctrl |= 1u << (PIO_CTRL_SM_RESTART_LSB + sm);
+}
+
+/*! \brief Restart multiple state machine with a known state
+ * \ingroup hardware_pio
+ *
+ * This method clears the ISR, shift counters, clock divider counter
+ * pin write flags, delay counter, latched EXEC instruction, and IRQ wait condition.
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param mask bit mask of state machine indexes to modify the enabled state of
+ */
+static inline void pio_restart_sm_mask(PIO pio, uint32_t mask) {
+ check_pio_param(pio);
+ check_sm_mask(mask);
+ pio->ctrl |= (mask << PIO_CTRL_SM_RESTART_LSB) & PIO_CTRL_SM_RESTART_BITS;
+}
+
+/*! \brief Restart a state machine's clock divider from a phase of 0
+ * \ingroup hardware_pio
+ *
+ * Each state machine's clock divider is a free-running piece of hardware,
+ * that generates a pattern of clock enable pulses for the state machine,
+ * based *only* on the configured integer/fractional divisor. The pattern of
+ * running/halted cycles slows the state machine's execution to some
+ * controlled rate.
+ *
+ * This function clears the divider's integer and fractional phase
+ * accumulators so that it restarts this pattern from the beginning. It is
+ * called automatically by pio_sm_init() but can also be called at a later
+ * time, when you enable the state machine, to ensure precisely consistent
+ * timing each time you load and run a given PIO program.
+ *
+ * More commonly this hardware mechanism is used to synchronise the execution
+ * clocks of multiple state machines -- see pio_clkdiv_restart_sm_mask().
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param sm State machine index (0..3)
+ */
+static inline void pio_sm_clkdiv_restart(PIO pio, uint sm) {
+ check_pio_param(pio);
+ check_sm_param(sm);
+ pio->ctrl |= 1u << (PIO_CTRL_CLKDIV_RESTART_LSB + sm);
+}
+
+/*! \brief Restart multiple state machines' clock dividers from a phase of 0.
+ * \ingroup hardware_pio
+ *
+ * Each state machine's clock divider is a free-running piece of hardware,
+ * that generates a pattern of clock enable pulses for the state machine,
+ * based *only* on the configured integer/fractional divisor. The pattern of
+ * running/halted cycles slows the state machine's execution to some
+ * controlled rate.
+ *
+ * This function simultaneously clears the integer and fractional phase
+ * accumulators of multiple state machines' clock dividers. If these state
+ * machines all have the same integer and fractional divisors configured,
+ * their clock dividers will run in precise deterministic lockstep from this
+ * point.
+ *
+ * With their execution clocks synchronised in this way, it is then safe to
+ * e.g. have multiple state machines performing a 'wait irq' on the same flag,
+ * and all clear it on the same cycle.
+ *
+ * Also note that this function can be called whilst state machines are
+ * running (e.g. if you have just changed the clock divisors of some state
+ * machines and wish to resynchronise them), and that disabling a state
+ * machine does not halt its clock divider: that is, if multiple state
+ * machines have their clocks synchronised, you can safely disable and
+ * reenable one of the state machines without losing synchronisation.
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param mask bit mask of state machine indexes to modify the enabled state of
+ */
+static inline void pio_clkdiv_restart_sm_mask(PIO pio, uint32_t mask) {
+ check_pio_param(pio);
+ check_sm_mask(mask);
+ pio->ctrl |= (mask << PIO_CTRL_CLKDIV_RESTART_LSB) & PIO_CTRL_CLKDIV_RESTART_BITS;
+}
+
+/*! \brief Enable multiple PIO state machines synchronizing their clock dividers
+ * \ingroup hardware_pio
+ *
+ * This is equivalent to calling both pio_set_sm_mask_enabled() and
+ * pio_clkdiv_restart_sm_mask() on the *same* clock cycle. All state machines
+ * specified by 'mask' are started simultaneously and, assuming they have the
+ * same clock divisors, their divided clocks will stay precisely synchronised.
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param mask bit mask of state machine indexes to modify the enabled state of
+ */
+static inline void pio_enable_sm_mask_in_sync(PIO pio, uint32_t mask) {
+ check_pio_param(pio);
+ check_sm_mask(mask);
+ pio->ctrl |= ((mask << PIO_CTRL_CLKDIV_RESTART_LSB) & PIO_CTRL_CLKDIV_RESTART_BITS) |
+ ((mask << PIO_CTRL_SM_ENABLE_LSB) & PIO_CTRL_SM_ENABLE_BITS);
+}
+
+/*! \brief PIO interrupt source numbers for pio related IRQs
+ * \ingroup hardware_pio
+ */
+enum pio_interrupt_source {
+ pis_interrupt0 = PIO_INTR_SM0_LSB,
+ pis_interrupt1 = PIO_INTR_SM1_LSB,
+ pis_interrupt2 = PIO_INTR_SM2_LSB,
+ pis_interrupt3 = PIO_INTR_SM3_LSB,
+ pis_sm0_tx_fifo_not_full = PIO_INTR_SM0_TXNFULL_LSB,
+ pis_sm1_tx_fifo_not_full = PIO_INTR_SM1_TXNFULL_LSB,
+ pis_sm2_tx_fifo_not_full = PIO_INTR_SM2_TXNFULL_LSB,
+ pis_sm3_tx_fifo_not_full = PIO_INTR_SM3_TXNFULL_LSB,
+ pis_sm0_rx_fifo_not_empty = PIO_INTR_SM0_RXNEMPTY_LSB,
+ pis_sm1_rx_fifo_not_empty = PIO_INTR_SM1_RXNEMPTY_LSB,
+ pis_sm2_rx_fifo_not_empty = PIO_INTR_SM2_RXNEMPTY_LSB,
+ pis_sm3_rx_fifo_not_empty = PIO_INTR_SM3_RXNEMPTY_LSB,
+};
+
+/*! \brief Enable/Disable a single source on a PIO's IRQ 0
+ * \ingroup hardware_pio
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param source the source number (see \ref pio_interrupt_source)
+ * \param enabled true to enable IRQ 0 for the source, false to disable.
+ */
+static inline void pio_set_irq0_source_enabled(PIO pio, enum pio_interrupt_source source, bool enabled) {
+ check_pio_param(pio);
+ invalid_params_if(PIO, source >= 12);
+ if (enabled)
+ hw_set_bits(&pio->inte0, 1u << source);
+ else
+ hw_clear_bits(&pio->inte0, 1u << source);
+}
+
+/*! \brief Enable/Disable a single source on a PIO's IRQ 1
+ * \ingroup hardware_pio
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param source the source number (see \ref pio_interrupt_source)
+ * \param enabled true to enable IRQ 0 for the source, false to disable.
+ */
+static inline void pio_set_irq1_source_enabled(PIO pio, enum pio_interrupt_source source, bool enabled) {
+ check_pio_param(pio);
+ invalid_params_if(PIO, source >= 12);
+ if (enabled)
+ hw_set_bits(&pio->inte1, 1u << source);
+ else
+ hw_clear_bits(&pio->inte1, 1u << source);
+}
+
+/*! \brief Enable/Disable multiple sources on a PIO's IRQ 0
+ * \ingroup hardware_pio
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param source_mask Mask of bits, one for each source number (see \ref pio_interrupt_source) to affect
+ * \param enabled true to enable all the sources specified in the mask on IRQ 0, false to disable all the sources specified in the mask on IRQ 0
+ */
+static inline void pio_set_irq0_source_mask_enabled(PIO pio, uint32_t source_mask, bool enabled) {
+ check_pio_param(pio);
+ invalid_params_if(PIO, source_mask > PIO_INTR_BITS);
+ if (enabled) {
+ hw_set_bits(&pio->inte0, source_mask);
+ } else {
+ hw_clear_bits(&pio->inte0, source_mask);
+ }
+}
+
+/*! \brief Enable/Disable multiple sources on a PIO's IRQ 1
+ * \ingroup hardware_pio
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param source_mask Mask of bits, one for each source number (see \ref pio_interrupt_source) to affect
+ * \param enabled true to enable all the sources specified in the mask on IRQ 1, false to disable all the source specified in the mask on IRQ 1
+ */
+static inline void pio_set_irq1_source_mask_enabled(PIO pio, uint32_t source_mask, bool enabled) {
+ check_pio_param(pio);
+ invalid_params_if(PIO, source_mask > PIO_INTR_BITS);
+ if (enabled) {
+ hw_set_bits(&pio->inte1, source_mask);
+ } else {
+ hw_clear_bits(&pio->inte1, source_mask);
+ }
+}
+
+/*! \brief Enable/Disable a single source on a PIO's specified (0/1) IRQ index
+ * \ingroup hardware_pio
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param irq_index the IRQ index; either 0 or 1
+ * \param source the source number (see \ref pio_interrupt_source)
+ * \param enabled true to enable the source on the specified IRQ, false to disable.
+ */
+static inline void pio_set_irqn_source_enabled(PIO pio, uint irq_index, enum pio_interrupt_source source, bool enabled) {
+ invalid_params_if(PIO, irq_index > 1);
+ if (irq_index) {
+ pio_set_irq1_source_enabled(pio, source, enabled);
+ } else {
+ pio_set_irq0_source_enabled(pio, source, enabled);
+ }
+}
+
+/*! \brief Enable/Disable multiple sources on a PIO's specified (0/1) IRQ index
+ * \ingroup hardware_pio
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param irq_index the IRQ index; either 0 or 1
+ * \param source_mask Mask of bits, one for each source number (see \ref pio_interrupt_source) to affect
+ * \param enabled true to enable all the sources specified in the mask on the specified IRQ, false to disable all the sources specified in the mask on the specified IRQ
+ */
+static inline void pio_set_irqn_source_mask_enabled(PIO pio, uint irq_index, uint32_t source_mask, bool enabled) {
+ invalid_params_if(PIO, irq_index > 1);
+ if (irq_index) {
+ pio_set_irq0_source_mask_enabled(pio, source_mask, enabled);
+ } else {
+ pio_set_irq1_source_mask_enabled(pio, source_mask, enabled);
+ }
+}
+
+/*! \brief Determine if a particular PIO interrupt is set
+ * \ingroup hardware_pio
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param pio_interrupt_num the PIO interrupt number 0-7
+ * \return true if corresponding PIO interrupt is currently set
+ */
+static inline bool pio_interrupt_get(PIO pio, uint pio_interrupt_num) {
+ check_pio_param(pio);
+ invalid_params_if(PIO, pio_interrupt_num >= 8);
+ return pio->irq & (1u << pio_interrupt_num);
+}
+
+/*! \brief Clear a particular PIO interrupt
+ * \ingroup hardware_pio
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param pio_interrupt_num the PIO interrupt number 0-7
+ */
+static inline void pio_interrupt_clear(PIO pio, uint pio_interrupt_num) {
+ check_pio_param(pio);
+ invalid_params_if(PIO, pio_interrupt_num >= 8);
+ hw_set_bits(&pio->irq, (1u << pio_interrupt_num));
+}
+
+/*! \brief Return the current program counter for a state machine
+ * \ingroup hardware_pio
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param sm State machine index (0..3)
+ * \return the program counter
+ */
+static inline uint8_t pio_sm_get_pc(PIO pio, uint sm) {
+ check_pio_param(pio);
+ check_sm_param(sm);
+ return (uint8_t) pio->sm[sm].addr;
+}
+
+/*! \brief Immediately execute an instruction on a state machine
+ * \ingroup hardware_pio
+ *
+ * This instruction is executed instead of the next instruction in the normal control flow on the state machine.
+ * Subsequent calls to this method replace the previous executed
+ * instruction if it is still running. \see pio_sm_is_exec_stalled() to see if an executed instruction
+ * is still running (i.e. it is stalled on some condition)
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param sm State machine index (0..3)
+ * \param instr the encoded PIO instruction
+ */
+inline static void pio_sm_exec(PIO pio, uint sm, uint instr) {
+ check_pio_param(pio);
+ check_sm_param(sm);
+ pio->sm[sm].instr = instr;
+}
+
+/*! \brief Determine if an instruction set by pio_sm_exec() is stalled executing
+ * \ingroup hardware_pio
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param sm State machine index (0..3)
+ * \return true if the executed instruction is still running (stalled)
+ */
+static inline bool pio_sm_is_exec_stalled(PIO pio, uint sm) {
+ check_pio_param(pio);
+ check_sm_param(sm);
+ return !!(pio->sm[sm].execctrl & PIO_SM0_EXECCTRL_EXEC_STALLED_BITS);
+}
+
+/*! \brief Immediately execute an instruction on a state machine and wait for it to complete
+ * \ingroup hardware_pio
+ *
+ * This instruction is executed instead of the next instruction in the normal control flow on the state machine.
+ * Subsequent calls to this method replace the previous executed
+ * instruction if it is still running. \see pio_sm_is_exec_stalled() to see if an executed instruction
+ * is still running (i.e. it is stalled on some condition)
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param sm State machine index (0..3)
+ * \param instr the encoded PIO instruction
+ */
+static inline void pio_sm_exec_wait_blocking(PIO pio, uint sm, uint instr) {
+ check_pio_param(pio);
+ check_sm_param(sm);
+ pio_sm_exec(pio, sm, instr);
+ while (pio_sm_is_exec_stalled(pio, sm)) tight_loop_contents();
+}
+
+/*! \brief Set the current wrap configuration for a state machine
+ * \ingroup hardware_pio
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param sm State machine index (0..3)
+ * \param wrap_target the instruction memory address to wrap to
+ * \param wrap the instruction memory address after which to set the program counter to wrap_target
+ * if the instruction does not itself update the program_counter
+ */
+static inline void pio_sm_set_wrap(PIO pio, uint sm, uint wrap_target, uint wrap) {
+ check_pio_param(pio);
+ check_sm_param(sm);
+ valid_params_if(PIO, wrap < PIO_INSTRUCTION_COUNT);
+ valid_params_if(PIO, wrap_target < PIO_INSTRUCTION_COUNT);
+ pio->sm[sm].execctrl =
+ (pio->sm[sm].execctrl & ~(PIO_SM0_EXECCTRL_WRAP_TOP_BITS | PIO_SM0_EXECCTRL_WRAP_BOTTOM_BITS)) |
+ (wrap_target << PIO_SM0_EXECCTRL_WRAP_BOTTOM_LSB) |
+ (wrap << PIO_SM0_EXECCTRL_WRAP_TOP_LSB);
+}
+
+/*! \brief Set the current 'out' pins for a state machine
+ * \ingroup sm_config
+ *
+ * Can overlap with the 'in', 'set' and 'sideset' pins
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param sm State machine index (0..3)
+ * \param out_base 0-31 First pin to set as output
+ * \param out_count 0-32 Number of pins to set.
+ */
+static inline void pio_sm_set_out_pins(PIO pio, uint sm, uint out_base, uint out_count) {
+ check_pio_param(pio);
+ check_sm_param(sm);
+ valid_params_if(PIO, out_base < 32);
+ valid_params_if(PIO, out_count <= 32);
+ pio->sm[sm].pinctrl = (pio->sm[sm].pinctrl & ~(PIO_SM0_PINCTRL_OUT_BASE_BITS | PIO_SM0_PINCTRL_OUT_COUNT_BITS)) |
+ (out_base << PIO_SM0_PINCTRL_OUT_BASE_LSB) |
+ (out_count << PIO_SM0_PINCTRL_OUT_COUNT_LSB);
+}
+
+
+/*! \brief Set the current 'set' pins for a state machine
+ * \ingroup sm_config
+ *
+ * Can overlap with the 'in', 'out' and 'sideset' pins
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param sm State machine index (0..3)
+ * \param set_base 0-31 First pin to set as
+ * \param set_count 0-5 Number of pins to set.
+ */
+static inline void pio_sm_set_set_pins(PIO pio, uint sm, uint set_base, uint set_count) {
+ check_pio_param(pio);
+ check_sm_param(sm);
+ valid_params_if(PIO, set_base < 32);
+ valid_params_if(PIO, set_count <= 5);
+ pio->sm[sm].pinctrl = (pio->sm[sm].pinctrl & ~(PIO_SM0_PINCTRL_SET_BASE_BITS | PIO_SM0_PINCTRL_SET_COUNT_BITS)) |
+ (set_base << PIO_SM0_PINCTRL_SET_BASE_LSB) |
+ (set_count << PIO_SM0_PINCTRL_SET_COUNT_LSB);
+}
+
+/*! \brief Set the current 'in' pins for a state machine
+ * \ingroup sm_config
+ *
+ * Can overlap with the 'out', ''set' and 'sideset' pins
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param sm State machine index (0..3)
+ * \param in_base 0-31 First pin to use as input
+ */
+static inline void pio_sm_set_in_pins(PIO pio, uint sm, uint in_base) {
+ check_pio_param(pio);
+ check_sm_param(sm);
+ valid_params_if(PIO, in_base < 32);
+ pio->sm[sm].pinctrl = (pio->sm[sm].pinctrl & ~PIO_SM0_PINCTRL_IN_BASE_BITS) |
+ (in_base << PIO_SM0_PINCTRL_IN_BASE_LSB);
+}
+
+/*! \brief Set the current 'sideset' pins for a state machine
+ * \ingroup sm_config
+ *
+ * Can overlap with the 'in', 'out' and 'set' pins
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param sm State machine index (0..3)
+ * \param sideset_base 0-31 base pin for 'side set'
+ */
+static inline void pio_sm_set_sideset_pins(PIO pio, uint sm, uint sideset_base) {
+ check_pio_param(pio);
+ check_sm_param(sm);
+ valid_params_if(PIO, sideset_base < 32);
+ pio->sm[sm].pinctrl = (pio->sm[sm].pinctrl & ~PIO_SM0_PINCTRL_SIDESET_BASE_BITS) |
+ (sideset_base << PIO_SM0_PINCTRL_SIDESET_BASE_LSB);
+}
+
+/*! \brief Write a word of data to a state machine's TX FIFO
+ * \ingroup hardware_pio
+ *
+ * This is a raw FIFO access that does not check for fullness. If the FIFO is
+ * full, the FIFO contents and state are not affected by the write attempt.
+ * Hardware sets the TXOVER sticky flag for this FIFO in FDEBUG, to indicate
+ * that the system attempted to write to a full FIFO.
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param sm State machine index (0..3)
+ * \param data the 32 bit data value
+ *
+ * \sa pio_sm_put_blocking()
+ */
+static inline void pio_sm_put(PIO pio, uint sm, uint32_t data) {
+ check_pio_param(pio);
+ check_sm_param(sm);
+ pio->txf[sm] = data;
+}
+
+/*! \brief Read a word of data from a state machine's RX FIFO
+ * \ingroup hardware_pio
+ *
+ * This is a raw FIFO access that does not check for emptiness. If the FIFO is
+ * empty, the hardware ignores the attempt to read from the FIFO (the FIFO
+ * remains in an empty state following the read) and the sticky RXUNDER flag
+ * for this FIFO is set in FDEBUG to indicate that the system tried to read
+ * from this FIFO when empty. The data returned by this function is undefined
+ * when the FIFO is empty.
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param sm State machine index (0..3)
+ *
+ * \sa pio_sm_get_blocking()
+ */
+static inline uint32_t pio_sm_get(PIO pio, uint sm) {
+ check_pio_param(pio);
+ check_sm_param(sm);
+ return pio->rxf[sm];
+}
+
+/*! \brief Determine if a state machine's RX FIFO is full
+ * \ingroup hardware_pio
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param sm State machine index (0..3)
+ * \return true if the RX FIFO is full
+ */
+static inline bool pio_sm_is_rx_fifo_full(PIO pio, uint sm) {
+ check_pio_param(pio);
+ check_sm_param(sm);
+ return (pio->fstat & (1u << (PIO_FSTAT_RXFULL_LSB + sm))) != 0;
+}
+
+/*! \brief Determine if a state machine's RX FIFO is empty
+ * \ingroup hardware_pio
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param sm State machine index (0..3)
+ * \return true if the RX FIFO is empty
+ */
+static inline bool pio_sm_is_rx_fifo_empty(PIO pio, uint sm) {
+ check_pio_param(pio);
+ check_sm_param(sm);
+ return (pio->fstat & (1u << (PIO_FSTAT_RXEMPTY_LSB + sm))) != 0;
+}
+
+/*! \brief Return the number of elements currently in a state machine's RX FIFO
+ * \ingroup hardware_pio
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param sm State machine index (0..3)
+ * \return the number of elements in the RX FIFO
+ */
+static inline uint pio_sm_get_rx_fifo_level(PIO pio, uint sm) {
+ check_pio_param(pio);
+ check_sm_param(sm);
+ uint bitoffs = PIO_FLEVEL_RX0_LSB + sm * (PIO_FLEVEL_RX1_LSB - PIO_FLEVEL_RX0_LSB);
+ const uint32_t mask = PIO_FLEVEL_RX0_BITS >> PIO_FLEVEL_RX0_LSB;
+ return (pio->flevel >> bitoffs) & mask;
+}
+
+/*! \brief Determine if a state machine's TX FIFO is full
+ * \ingroup hardware_pio
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param sm State machine index (0..3)
+ * \return true if the TX FIFO is full
+ */
+static inline bool pio_sm_is_tx_fifo_full(PIO pio, uint sm) {
+ check_pio_param(pio);
+ check_sm_param(sm);
+ return (pio->fstat & (1u << (PIO_FSTAT_TXFULL_LSB + sm))) != 0;
+}
+
+/*! \brief Determine if a state machine's TX FIFO is empty
+ * \ingroup hardware_pio
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param sm State machine index (0..3)
+ * \return true if the TX FIFO is empty
+ */
+static inline bool pio_sm_is_tx_fifo_empty(PIO pio, uint sm) {
+ check_pio_param(pio);
+ check_sm_param(sm);
+ return (pio->fstat & (1u << (PIO_FSTAT_TXEMPTY_LSB + sm))) != 0;
+}
+
+/*! \brief Return the number of elements currently in a state machine's TX FIFO
+ * \ingroup hardware_pio
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param sm State machine index (0..3)
+ * \return the number of elements in the TX FIFO
+ */
+static inline uint pio_sm_get_tx_fifo_level(PIO pio, uint sm) {
+ check_pio_param(pio);
+ check_sm_param(sm);
+ unsigned int bitoffs = PIO_FLEVEL_TX0_LSB + sm * (PIO_FLEVEL_TX1_LSB - PIO_FLEVEL_TX0_LSB);
+ const uint32_t mask = PIO_FLEVEL_TX0_BITS >> PIO_FLEVEL_TX0_LSB;
+ return (pio->flevel >> bitoffs) & mask;
+}
+
+/*! \brief Write a word of data to a state machine's TX FIFO, blocking if the FIFO is full
+ * \ingroup hardware_pio
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param sm State machine index (0..3)
+ * \param data the 32 bit data value
+ */
+static inline void pio_sm_put_blocking(PIO pio, uint sm, uint32_t data) {
+ check_pio_param(pio);
+ check_sm_param(sm);
+ while (pio_sm_is_tx_fifo_full(pio, sm)) tight_loop_contents();
+ pio_sm_put(pio, sm, data);
+}
+
+/*! \brief Read a word of data from a state machine's RX FIFO, blocking if the FIFO is empty
+ * \ingroup hardware_pio
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param sm State machine index (0..3)
+ */
+static inline uint32_t pio_sm_get_blocking(PIO pio, uint sm) {
+ check_pio_param(pio);
+ check_sm_param(sm);
+ while (pio_sm_is_rx_fifo_empty(pio, sm)) tight_loop_contents();
+ return pio_sm_get(pio, sm);
+}
+
+/*! \brief Empty out a state machine's TX FIFO
+ * \ingroup hardware_pio
+ *
+ * This method executes `pull` instructions on the state machine until the TX
+ * FIFO is empty. This disturbs the contents of the OSR, so see also
+ * pio_sm_clear_fifos() which clears both FIFOs but leaves the state machine's
+ * internal state undisturbed.
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param sm State machine index (0..3)
+ *
+ * \sa pio_sm_clear_fifos()
+ */
+void pio_sm_drain_tx_fifo(PIO pio, uint sm);
+
+/*! \brief set the current clock divider for a state machine using a 16:8 fraction
+ * \ingroup hardware_pio
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param sm State machine index (0..3)
+ * \param div_int the integer part of the clock divider
+ * \param div_frac the fractional part of the clock divider in 1/256s
+ */
+static inline void pio_sm_set_clkdiv_int_frac(PIO pio, uint sm, uint16_t div_int, uint8_t div_frac) {
+ check_pio_param(pio);
+ check_sm_param(sm);
+ pio->sm[sm].clkdiv =
+ (((uint)div_frac) << PIO_SM0_CLKDIV_FRAC_LSB) |
+ (((uint)div_int) << PIO_SM0_CLKDIV_INT_LSB);
+}
+
+/*! \brief set the current clock divider for a state machine
+ * \ingroup hardware_pio
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param sm State machine index (0..3)
+ * \param div the floating point clock divider
+ */
+static inline void pio_sm_set_clkdiv(PIO pio, uint sm, float div) {
+ check_pio_param(pio);
+ check_sm_param(sm);
+ uint16_t div_int;
+ uint8_t div_frac;
+ pio_calculate_clkdiv_from_float(div, &div_int, &div_frac);
+ pio_sm_set_clkdiv_int_frac(pio, sm, div_int, div_frac);
+}
+
+/*! \brief Clear a state machine's TX and RX FIFOs
+ * \ingroup hardware_pio
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param sm State machine index (0..3)
+ */
+static inline void pio_sm_clear_fifos(PIO pio, uint sm) {
+ // changing the FIFO join state clears the fifo
+ check_pio_param(pio);
+ check_sm_param(sm);
+ hw_xor_bits(&pio->sm[sm].shiftctrl, PIO_SM0_SHIFTCTRL_FJOIN_RX_BITS);
+ hw_xor_bits(&pio->sm[sm].shiftctrl, PIO_SM0_SHIFTCTRL_FJOIN_RX_BITS);
+}
+
+/*! \brief Use a state machine to set a value on all pins for the PIO instance
+ * \ingroup hardware_pio
+ *
+ * This method repeatedly reconfigures the target state machine's pin configuration and executes 'set' instructions to set values on all 32 pins,
+ * before restoring the state machine's pin configuration to what it was.
+ *
+ * This method is provided as a convenience to set initial pin states, and should not be used against a state machine that is enabled.
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param sm State machine index (0..3) to use
+ * \param pin_values the pin values to set
+ */
+void pio_sm_set_pins(PIO pio, uint sm, uint32_t pin_values);
+
+/*! \brief Use a state machine to set a value on multiple pins for the PIO instance
+ * \ingroup hardware_pio
+ *
+ * This method repeatedly reconfigures the target state machine's pin configuration and executes 'set' instructions to set values on up to 32 pins,
+ * before restoring the state machine's pin configuration to what it was.
+ *
+ * This method is provided as a convenience to set initial pin states, and should not be used against a state machine that is enabled.
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param sm State machine index (0..3) to use
+ * \param pin_values the pin values to set (if the corresponding bit in pin_mask is set)
+ * \param pin_mask a bit for each pin to indicate whether the corresponding pin_value for that pin should be applied.
+ */
+void pio_sm_set_pins_with_mask(PIO pio, uint sm, uint32_t pin_values, uint32_t pin_mask);
+
+/*! \brief Use a state machine to set the pin directions for multiple pins for the PIO instance
+ * \ingroup hardware_pio
+ *
+ * This method repeatedly reconfigures the target state machine's pin configuration and executes 'set' instructions to set pin directions on up to 32 pins,
+ * before restoring the state machine's pin configuration to what it was.
+ *
+ * This method is provided as a convenience to set initial pin directions, and should not be used against a state machine that is enabled.
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param sm State machine index (0..3) to use
+ * \param pin_dirs the pin directions to set - 1 = out, 0 = in (if the corresponding bit in pin_mask is set)
+ * \param pin_mask a bit for each pin to indicate whether the corresponding pin_value for that pin should be applied.
+ */
+void pio_sm_set_pindirs_with_mask(PIO pio, uint sm, uint32_t pin_dirs, uint32_t pin_mask);
+
+/*! \brief Use a state machine to set the same pin direction for multiple consecutive pins for the PIO instance
+ * \ingroup hardware_pio
+ *
+ * This method repeatedly reconfigures the target state machine's pin configuration and executes 'set' instructions to set the pin direction on consecutive pins,
+ * before restoring the state machine's pin configuration to what it was.
+ *
+ * This method is provided as a convenience to set initial pin directions, and should not be used against a state machine that is enabled.
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param sm State machine index (0..3) to use
+ * \param pin_base the first pin to set a direction for
+ * \param pin_count the count of consecutive pins to set the direction for
+ * \param is_out the direction to set; true = out, false = in
+ */
+void pio_sm_set_consecutive_pindirs(PIO pio, uint sm, uint pin_base, uint pin_count, bool is_out);
+
+/*! \brief Mark a state machine as used
+ * \ingroup hardware_pio
+ *
+ * Method for cooperative claiming of hardware. Will cause a panic if the state machine
+ * is already claimed. Use of this method by libraries detects accidental
+ * configurations that would fail in unpredictable ways.
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param sm State machine index (0..3)
+ */
+void pio_sm_claim(PIO pio, uint sm);
+
+/*! \brief Mark multiple state machines as used
+ * \ingroup hardware_pio
+ *
+ * Method for cooperative claiming of hardware. Will cause a panic if any of the state machines
+ * are already claimed. Use of this method by libraries detects accidental
+ * configurations that would fail in unpredictable ways.
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param sm_mask Mask of state machine indexes
+ */
+void pio_claim_sm_mask(PIO pio, uint sm_mask);
+
+/*! \brief Mark a state machine as no longer used
+ * \ingroup hardware_pio
+ *
+ * Method for cooperative claiming of hardware.
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param sm State machine index (0..3)
+ */
+void pio_sm_unclaim(PIO pio, uint sm);
+
+/*! \brief Claim a free state machine on a PIO instance
+ * \ingroup hardware_pio
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param required if true the function will panic if none are available
+ * \return the state machine index or -1 if required was false, and none were free
+ */
+int pio_claim_unused_sm(PIO pio, bool required);
+
+/*! \brief Determine if a PIO state machine is claimed
+ * \ingroup hardware_pio
+ *
+ * \param pio The PIO instance; either \ref pio0 or \ref pio1
+ * \param sm State machine index (0..3)
+ * \return true if claimed, false otherwise
+ * \see pio_sm_claim
+ * \see pio_claim_sm_mask
+ */
+bool pio_sm_is_claimed(PIO pio, uint sm);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // _PIO_H_
diff --git a/src/rp2_common/hardware_pio/include/hardware/pio_instructions.h b/src/rp2_common/hardware_pio/include/hardware/pio_instructions.h
new file mode 100644
index 0000000..3662135
--- /dev/null
+++ b/src/rp2_common/hardware_pio/include/hardware/pio_instructions.h
@@ -0,0 +1,484 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _HARDWARE_PIO_INSTRUCTIONS_H_
+#define _HARDWARE_PIO_INSTRUCTIONS_H_
+
+#include "pico.h"
+
+/** \brief PIO instruction encoding
+ * \defgroup pio_instructions pio_instructions
+ * \ingroup hardware_pio
+ *
+ * Functions for generating PIO instruction encodings programmatically. In debug builds
+ *`PARAM_ASSERTIONS_ENABLED_PIO_INSTRUCTIONS` can be set to 1 to enable validation of encoding function
+ * parameters.
+ *
+ * For fuller descriptions of the instructions in question see the "RP2040 Datasheet"
+ */
+
+// PICO_CONFIG: PARAM_ASSERTIONS_ENABLED_PIO_INSTRUCTIONS, Enable/disable assertions in the PIO instructions, type=bool, default=0, group=pio_instructions
+#ifndef PARAM_ASSERTIONS_ENABLED_PIO_INSTRUCTIONS
+#define PARAM_ASSERTIONS_ENABLED_PIO_INSTRUCTIONS 0
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+enum pio_instr_bits {
+ pio_instr_bits_jmp = 0x0000,
+ pio_instr_bits_wait = 0x2000,
+ pio_instr_bits_in = 0x4000,
+ pio_instr_bits_out = 0x6000,
+ pio_instr_bits_push = 0x8000,
+ pio_instr_bits_pull = 0x8080,
+ pio_instr_bits_mov = 0xa000,
+ pio_instr_bits_irq = 0xc000,
+ pio_instr_bits_set = 0xe000,
+};
+
+#ifndef NDEBUG
+#define _PIO_INVALID_IN_SRC 0x08u
+#define _PIO_INVALID_OUT_DEST 0x10u
+#define _PIO_INVALID_SET_DEST 0x20u
+#define _PIO_INVALID_MOV_SRC 0x40u
+#define _PIO_INVALID_MOV_DEST 0x80u
+#else
+#define _PIO_INVALID_IN_SRC 0u
+#define _PIO_INVALID_OUT_DEST 0u
+#define _PIO_INVALID_SET_DEST 0u
+#define _PIO_INVALID_MOV_SRC 0u
+#define _PIO_INVALID_MOV_DEST 0u
+#endif
+
+/*! \brief Enumeration of values to pass for source/destination args for instruction encoding functions
+ * \ingroup pio_instructions
+ *
+ * \note Not all values are suitable for all functions. Validity is only checked in debug mode when
+ * `PARAM_ASSERTIONS_ENABLED_PIO_INSTRUCTIONS` is 1
+ */
+enum pio_src_dest {
+ pio_pins = 0u,
+ pio_x = 1u,
+ pio_y = 2u,
+ pio_null = 3u | _PIO_INVALID_SET_DEST | _PIO_INVALID_MOV_DEST,
+ pio_pindirs = 4u | _PIO_INVALID_IN_SRC | _PIO_INVALID_MOV_SRC | _PIO_INVALID_MOV_DEST,
+ pio_exec_mov = 4u | _PIO_INVALID_IN_SRC | _PIO_INVALID_OUT_DEST | _PIO_INVALID_SET_DEST | _PIO_INVALID_MOV_SRC,
+ pio_status = 5u | _PIO_INVALID_IN_SRC | _PIO_INVALID_OUT_DEST | _PIO_INVALID_SET_DEST | _PIO_INVALID_MOV_DEST,
+ pio_pc = 5u | _PIO_INVALID_IN_SRC | _PIO_INVALID_SET_DEST | _PIO_INVALID_MOV_SRC,
+ pio_isr = 6u | _PIO_INVALID_SET_DEST,
+ pio_osr = 7u | _PIO_INVALID_OUT_DEST | _PIO_INVALID_SET_DEST,
+ pio_exec_out = 7u | _PIO_INVALID_IN_SRC | _PIO_INVALID_SET_DEST | _PIO_INVALID_MOV_SRC | _PIO_INVALID_MOV_DEST,
+};
+
+static inline uint _pio_major_instr_bits(uint instr) {
+ return instr & 0xe000u;
+}
+
+static inline uint _pio_encode_instr_and_args(enum pio_instr_bits instr_bits, uint arg1, uint arg2) {
+ valid_params_if(PIO_INSTRUCTIONS, arg1 <= 0x7);
+#if PARAM_ASSERTIONS_ENABLED(PIO_INSTRUCTIONS)
+ uint32_t major = _pio_major_instr_bits(instr_bits);
+ if (major == pio_instr_bits_in || major == pio_instr_bits_out) {
+ assert(arg2 && arg2 <= 32);
+ } else {
+ assert(arg2 <= 31);
+ }
+#endif
+ return instr_bits | (arg1 << 5u) | (arg2 & 0x1fu);
+}
+
+static inline uint _pio_encode_instr_and_src_dest(enum pio_instr_bits instr_bits, enum pio_src_dest dest, uint value) {
+ return _pio_encode_instr_and_args(instr_bits, dest & 7u, value);
+}
+
+/*! \brief Encode just the delay slot bits of an instruction
+ * \ingroup pio_instructions
+ *
+ * \note This function does not return a valid instruction encoding; instead it returns an encoding of the delay
+ * slot suitable for `OR`ing with the result of an encoding function for an actual instruction. Care should be taken when
+ * combining the results of this function with the results of \ref pio_encode_sideset and \ref pio_encode_sideset_opt
+ * as they share the same bits within the instruction encoding.
+ *
+ * \param cycles the number of cycles 0-31 (or less if side set is being used)
+ * \return the delay slot bits to be ORed with an instruction encoding
+ */
+static inline uint pio_encode_delay(uint cycles) {
+ // note that the maximum cycles will be smaller if sideset_bit_count > 0
+ valid_params_if(PIO_INSTRUCTIONS, cycles <= 0x1f);
+ return cycles << 8u;
+}
+
+/*! \brief Encode just the side set bits of an instruction (in non optional side set mode)
+ * \ingroup pio_instructions
+ *
+ * \note This function does not return a valid instruction encoding; instead it returns an encoding of the side set bits
+ * suitable for `OR`ing with the result of an encoding function for an actual instruction. Care should be taken when
+ * combining the results of this function with the results of \ref pio_encode_delay as they share the same bits
+ * within the instruction encoding.
+ *
+ * \param sideset_bit_count number of side set bits as would be specified via `.sideset` in pioasm
+ * \param value the value to sideset on the pins
+ * \return the side set bits to be ORed with an instruction encoding
+ */
+static inline uint pio_encode_sideset(uint sideset_bit_count, uint value) {
+ valid_params_if(PIO_INSTRUCTIONS, sideset_bit_count >= 1 && sideset_bit_count <= 5);
+ valid_params_if(PIO_INSTRUCTIONS, value <= ((1u << sideset_bit_count) - 1));
+ return value << (13u - sideset_bit_count);
+}
+
+/*! \brief Encode just the side set bits of an instruction (in optional -`opt` side set mode)
+ * \ingroup pio_instructions
+ *
+ * \note This function does not return a valid instruction encoding; instead it returns an encoding of the side set bits
+ * suitable for `OR`ing with the result of an encoding function for an actual instruction. Care should be taken when
+ * combining the results of this function with the results of \ref pio_encode_delay as they share the same bits
+ * within the instruction encoding.
+ *
+ * \param sideset_bit_count number of side set bits as would be specified via `.sideset <n> opt` in pioasm
+ * \param value the value to sideset on the pins
+ * \return the side set bits to be ORed with an instruction encoding
+ */
+static inline uint pio_encode_sideset_opt(uint sideset_bit_count, uint value) {
+ valid_params_if(PIO_INSTRUCTIONS, sideset_bit_count >= 1 && sideset_bit_count <= 4);
+ valid_params_if(PIO_INSTRUCTIONS, value <= ((1u << sideset_bit_count) - 1));
+ return 0x1000u | value << (12u - sideset_bit_count);
+}
+
+/*! \brief Encode an unconditional JMP instruction
+ * \ingroup pio_instructions
+ *
+ * This is the equivalent of `JMP <addr>`
+ *
+ * \param addr The target address 0-31 (an absolute address within the PIO instruction memory)
+ * \return The instruction encoding with 0 delay and no side set value
+ * \see pio_encode_delay, pio_encode_sideset, pio_encode_sideset_opt
+ */
+static inline uint pio_encode_jmp(uint addr) {
+ return _pio_encode_instr_and_args(pio_instr_bits_jmp, 0, addr);
+}
+
+/*! \brief Encode a conditional JMP if scratch X zero instruction
+ * \ingroup pio_instructions
+ *
+ * This is the equivalent of `JMP !X <addr>`
+ *
+ * \param addr The target address 0-31 (an absolute address within the PIO instruction memory)
+ * \return The instruction encoding with 0 delay and no side set value
+ * \see pio_encode_delay, pio_encode_sideset, pio_encode_sideset_opt
+ */
+static inline uint pio_encode_jmp_not_x(uint addr) {
+ return _pio_encode_instr_and_args(pio_instr_bits_jmp, 1, addr);
+}
+
+/*! \brief Encode a conditional JMP if scratch X non-zero (and post-decrement X) instruction
+ * \ingroup pio_instructions
+ *
+ * This is the equivalent of `JMP X-- <addr>`
+ *
+ * \param addr The target address 0-31 (an absolute address within the PIO instruction memory)
+ * \return The instruction encoding with 0 delay and no side set value
+ * \see pio_encode_delay, pio_encode_sideset, pio_encode_sideset_opt
+ */
+static inline uint pio_encode_jmp_x_dec(uint addr) {
+ return _pio_encode_instr_and_args(pio_instr_bits_jmp, 2, addr);
+}
+
+/*! \brief Encode a conditional JMP if scratch Y zero instruction
+ * \ingroup pio_instructions
+ *
+ * This is the equivalent of `JMP !Y <addr>`
+ *
+ * \param addr The target address 0-31 (an absolute address within the PIO instruction memory)
+ * \return The instruction encoding with 0 delay and no side set value
+ * \see pio_encode_delay, pio_encode_sideset, pio_encode_sideset_opt
+ */
+static inline uint pio_encode_jmp_not_y(uint addr) {
+ return _pio_encode_instr_and_args(pio_instr_bits_jmp, 3, addr);
+}
+
+/*! \brief Encode a conditional JMP if scratch Y non-zero (and post-decrement Y) instruction
+ * \ingroup pio_instructions
+ *
+ * This is the equivalent of `JMP Y-- <addr>`
+ *
+ * \param addr The target address 0-31 (an absolute address within the PIO instruction memory)
+ * \return The instruction encoding with 0 delay and no side set value
+ * \see pio_encode_delay, pio_encode_sideset, pio_encode_sideset_opt
+ */
+static inline uint pio_encode_jmp_y_dec(uint addr) {
+ return _pio_encode_instr_and_args(pio_instr_bits_jmp, 4, addr);
+}
+
+/*! \brief Encode a conditional JMP if scratch X not equal scratch Y instruction
+ * \ingroup pio_instructions
+ *
+ * This is the equivalent of `JMP X!=Y <addr>`
+ *
+ * \param addr The target address 0-31 (an absolute address within the PIO instruction memory)
+ * \return The instruction encoding with 0 delay and no side set value
+ * \see pio_encode_delay, pio_encode_sideset, pio_encode_sideset_opt
+ */
+static inline uint pio_encode_jmp_x_ne_y(uint addr) {
+ return _pio_encode_instr_and_args(pio_instr_bits_jmp, 5, addr);
+}
+
+/*! \brief Encode a conditional JMP if input pin high instruction
+ * \ingroup pio_instructions
+ *
+ * This is the equivalent of `JMP PIN <addr>`
+ *
+ * \param addr The target address 0-31 (an absolute address within the PIO instruction memory)
+ * \return The instruction encoding with 0 delay and no side set value
+ * \see pio_encode_delay, pio_encode_sideset, pio_encode_sideset_opt
+ */
+static inline uint pio_encode_jmp_pin(uint addr) {
+ return _pio_encode_instr_and_args(pio_instr_bits_jmp, 6, addr);
+}
+
+/*! \brief Encode a conditional JMP if output shift register not empty instruction
+ * \ingroup pio_instructions
+ *
+ * This is the equivalent of `JMP !OSRE <addr>`
+ *
+ * \param addr The target address 0-31 (an absolute address within the PIO instruction memory)
+ * \return The instruction encoding with 0 delay and no side set value
+ * \see pio_encode_delay, pio_encode_sideset, pio_encode_sideset_opt
+ */
+static inline uint pio_encode_jmp_not_osre(uint addr) {
+ return _pio_encode_instr_and_args(pio_instr_bits_jmp, 7, addr);
+}
+
+static inline uint _pio_encode_irq(bool relative, uint irq) {
+ valid_params_if(PIO_INSTRUCTIONS, irq <= 7);
+ return (relative ? 0x10u : 0x0u) | irq;
+}
+
+/*! \brief Encode a WAIT for GPIO pin instruction
+ * \ingroup pio_instructions
+ *
+ * This is the equivalent of `WAIT <polarity> GPIO <gpio>`
+ *
+ * \param polarity true for `WAIT 1`, false for `WAIT 0`
+ * \param gpio The real GPIO number 0-31
+ * \return The instruction encoding with 0 delay and no side set value
+ * \see pio_encode_delay, pio_encode_sideset, pio_encode_sideset_opt
+ */
+static inline uint pio_encode_wait_gpio(bool polarity, uint gpio) {
+ return _pio_encode_instr_and_args(pio_instr_bits_wait, 0u | (polarity ? 4u : 0u), gpio);
+}
+
+/*! \brief Encode a WAIT for pin instruction
+ * \ingroup pio_instructions
+ *
+ * This is the equivalent of `WAIT <polarity> PIN <pin>`
+ *
+ * \param polarity true for `WAIT 1`, false for `WAIT 0`
+ * \param pin The pin number 0-31 relative to the executing SM's input pin mapping
+ * \return The instruction encoding with 0 delay and no side set value
+ * \see pio_encode_delay, pio_encode_sideset, pio_encode_sideset_opt
+ */
+static inline uint pio_encode_wait_pin(bool polarity, uint pin) {
+ return _pio_encode_instr_and_args(pio_instr_bits_wait, 1u | (polarity ? 4u : 0u), pin);
+}
+
+/*! \brief Encode a WAIT for IRQ instruction
+ * \ingroup pio_instructions
+ *
+ * This is the equivalent of `WAIT <polarity> IRQ <irq> <relative>`
+ *
+ * \param polarity true for `WAIT 1`, false for `WAIT 0`
+ * \param relative true for a `WAIT IRQ <irq> REL`, false for regular `WAIT IRQ <irq>`
+ * \param irq the irq number 0-7
+ * \return The instruction encoding with 0 delay and no side set value
+ * \see pio_encode_delay, pio_encode_sideset, pio_encode_sideset_opt
+ */
+static inline uint pio_encode_wait_irq(bool polarity, bool relative, uint irq) {
+ valid_params_if(PIO_INSTRUCTIONS, irq <= 7);
+ return _pio_encode_instr_and_args(pio_instr_bits_wait, 2u | (polarity ? 4u : 0u), _pio_encode_irq(relative, irq));
+}
+
+/*! \brief Encode an IN instruction
+ * \ingroup pio_instructions
+ *
+ * This is the equivalent of `IN <src>, <count>`
+ *
+ * \param src The source to take data from
+ * \param count The number of bits 1-32
+ * \return The instruction encoding with 0 delay and no side set value
+ * \see pio_encode_delay, pio_encode_sideset, pio_encode_sideset_opt
+ */
+static inline uint pio_encode_in(enum pio_src_dest src, uint count) {
+ valid_params_if(PIO_INSTRUCTIONS, !(src & _PIO_INVALID_IN_SRC));
+ return _pio_encode_instr_and_src_dest(pio_instr_bits_in, src, count);
+}
+
+/*! \brief Encode an OUT instruction
+ * \ingroup pio_instructions
+ *
+ * This is the equivalent of `OUT <src>, <count>`
+ *
+ * \param dest The destination to write data to
+ * \param count The number of bits 1-32
+ * \return The instruction encoding with 0 delay and no side set value
+ * \see pio_encode_delay, pio_encode_sideset, pio_encode_sideset_opt
+ */
+static inline uint pio_encode_out(enum pio_src_dest dest, uint count) {
+ valid_params_if(PIO_INSTRUCTIONS, !(dest & _PIO_INVALID_OUT_DEST));
+ return _pio_encode_instr_and_src_dest(pio_instr_bits_out, dest, count);
+}
+
+/*! \brief Encode a PUSH instruction
+ * \ingroup pio_instructions
+ *
+ * This is the equivalent of `PUSH <if_full>, <block>`
+ *
+ * \param if_full true for `PUSH IF_FULL ...`, false for `PUSH ...`
+ * \param block true for `PUSH ... BLOCK`, false for `PUSH ...`
+ * \return The instruction encoding with 0 delay and no side set value
+ * \see pio_encode_delay, pio_encode_sideset, pio_encode_sideset_opt
+ */
+static inline uint pio_encode_push(bool if_full, bool block) {
+ return _pio_encode_instr_and_args(pio_instr_bits_push, (if_full ? 2u : 0u) | (block ? 1u : 0u), 0);
+}
+
+/*! \brief Encode a PULL instruction
+ * \ingroup pio_instructions
+ *
+ * This is the equivalent of `PULL <if_empty>, <block>`
+ *
+ * \param if_empty true for `PULL IF_EMPTY ...`, false for `PULL ...`
+ * \param block true for `PULL ... BLOCK`, false for `PULL ...`
+ * \return The instruction encoding with 0 delay and no side set value
+ * \see pio_encode_delay, pio_encode_sideset, pio_encode_sideset_opt
+ */
+static inline uint pio_encode_pull(bool if_empty, bool block) {
+ return _pio_encode_instr_and_args(pio_instr_bits_pull, (if_empty ? 2u : 0u) | (block ? 1u : 0u), 0);
+}
+
+/*! \brief Encode a MOV instruction
+ * \ingroup pio_instructions
+ *
+ * This is the equivalent of `MOV <dest>, <src>`
+ *
+ * \param dest The destination to write data to
+ * \param src The source to take data from
+ * \return The instruction encoding with 0 delay and no side set value
+ * \see pio_encode_delay, pio_encode_sideset, pio_encode_sideset_opt
+ */
+static inline uint pio_encode_mov(enum pio_src_dest dest, enum pio_src_dest src) {
+ valid_params_if(PIO_INSTRUCTIONS, !(dest & _PIO_INVALID_MOV_DEST));
+ valid_params_if(PIO_INSTRUCTIONS, !(src & _PIO_INVALID_MOV_SRC));
+ return _pio_encode_instr_and_src_dest(pio_instr_bits_mov, dest, src & 7u);
+}
+
+/*! \brief Encode a MOV instruction with bit invert
+ * \ingroup pio_instructions
+ *
+ * This is the equivalent of `MOV <dest>, ~<src>`
+ *
+ * \param dest The destination to write inverted data to
+ * \param src The source to take data from
+ * \return The instruction encoding with 0 delay and no side set value
+ * \see pio_encode_delay, pio_encode_sideset, pio_encode_sideset_opt
+ */
+static inline uint pio_encode_mov_not(enum pio_src_dest dest, enum pio_src_dest src) {
+ valid_params_if(PIO_INSTRUCTIONS, !(dest & _PIO_INVALID_MOV_DEST));
+ valid_params_if(PIO_INSTRUCTIONS, !(src & _PIO_INVALID_MOV_SRC));
+ return _pio_encode_instr_and_src_dest(pio_instr_bits_mov, dest, (1u << 3u) | (src & 7u));
+}
+
+/*! \brief Encode a MOV instruction with bit reverse
+ * \ingroup pio_instructions
+ *
+ * This is the equivalent of `MOV <dest>, ::<src>`
+ *
+ * \param dest The destination to write bit reversed data to
+ * \param src The source to take data from
+ * \return The instruction encoding with 0 delay and no side set value
+ * \see pio_encode_delay, pio_encode_sideset, pio_encode_sideset_opt
+ */
+static inline uint pio_encode_mov_reverse(enum pio_src_dest dest, enum pio_src_dest src) {
+ valid_params_if(PIO_INSTRUCTIONS, !(dest & _PIO_INVALID_MOV_DEST));
+ valid_params_if(PIO_INSTRUCTIONS, !(src & _PIO_INVALID_MOV_SRC));
+ return _pio_encode_instr_and_src_dest(pio_instr_bits_mov, dest, (2u << 3u) | (src & 7u));
+}
+
+/*! \brief Encode a IRQ SET instruction
+ * \ingroup pio_instructions
+ *
+ * This is the equivalent of `IRQ SET <irq> <relative>`
+ *
+ * \param relative true for a `IRQ SET <irq> REL`, false for regular `IRQ SET <irq>`
+ * \param irq the irq number 0-7
+ * \return The instruction encoding with 0 delay and no side set value
+ * \see pio_encode_delay, pio_encode_sideset, pio_encode_sideset_opt
+ */
+static inline uint pio_encode_irq_set(bool relative, uint irq) {
+ return _pio_encode_instr_and_args(pio_instr_bits_irq, 0, _pio_encode_irq(relative, irq));
+}
+
+/*! \brief Encode a IRQ WAIT instruction
+ * \ingroup pio_instructions
+ *
+ * This is the equivalent of `IRQ WAIT <irq> <relative>`
+ *
+ * \param relative true for a `IRQ WAIT <irq> REL`, false for regular `IRQ WAIT <irq>`
+ * \param irq the irq number 0-7
+ * \return The instruction encoding with 0 delay and no side set value
+ * \see pio_encode_delay, pio_encode_sideset, pio_encode_sideset_opt
+ */
+static inline uint pio_encode_irq_wait(bool relative, uint irq) {
+ return _pio_encode_instr_and_args(pio_instr_bits_irq, 1, _pio_encode_irq(relative, irq));
+}
+
+/*! \brief Encode a IRQ CLEAR instruction
+ * \ingroup pio_instructions
+ *
+ * This is the equivalent of `IRQ CLEAR <irq> <relative>`
+ *
+ * \param relative true for a `IRQ CLEAR <irq> REL`, false for regular `IRQ CLEAR <irq>`
+ * \param irq the irq number 0-7
+ * \return The instruction encoding with 0 delay and no side set value
+ * \see pio_encode_delay, pio_encode_sideset, pio_encode_sideset_opt
+ */
+static inline uint pio_encode_irq_clear(bool relative, uint irq) {
+ return _pio_encode_instr_and_args(pio_instr_bits_irq, 2, _pio_encode_irq(relative, irq));
+}
+
+/*! \brief Encode a SET instruction
+ * \ingroup pio_instructions
+ *
+ * This is the equivalent of `SET <dest>, <value>`
+ *
+ * \param dest The destination to apply the value to
+ * \param value The value 0-31
+ * \return The instruction encoding with 0 delay and no side set value
+ * \see pio_encode_delay, pio_encode_sideset, pio_encode_sideset_opt
+ */
+static inline uint pio_encode_set(enum pio_src_dest dest, uint value) {
+ valid_params_if(PIO_INSTRUCTIONS, !(dest & _PIO_INVALID_SET_DEST));
+ return _pio_encode_instr_and_src_dest(pio_instr_bits_set, dest, value);
+}
+
+/*! \brief Encode a NOP instruction
+ * \ingroup pio_instructions
+ *
+ * This is the equivalent of `NOP` which is itself encoded as `MOV y, y`
+ *
+ * \return The instruction encoding with 0 delay and no side set value
+ * \see pio_encode_delay, pio_encode_sideset, pio_encode_sideset_opt
+ */
+static inline uint pio_encode_nop(void) {
+ return pio_encode_mov(pio_y, pio_y);
+}
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/rp2_common/hardware_pio/pio.c b/src/rp2_common/hardware_pio/pio.c
new file mode 100644
index 0000000..0744110
--- /dev/null
+++ b/src/rp2_common/hardware_pio/pio.c
@@ -0,0 +1,256 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "hardware/claim.h"
+#include "hardware/pio.h"
+#include "hardware/pio_instructions.h"
+
+// sanity check
+check_hw_layout(pio_hw_t, sm[0].clkdiv, PIO_SM0_CLKDIV_OFFSET);
+check_hw_layout(pio_hw_t, sm[1].clkdiv, PIO_SM1_CLKDIV_OFFSET);
+check_hw_layout(pio_hw_t, instr_mem[0], PIO_INSTR_MEM0_OFFSET);
+check_hw_layout(pio_hw_t, inte0, PIO_IRQ0_INTE_OFFSET);
+check_hw_layout(pio_hw_t, txf[1], PIO_TXF1_OFFSET);
+check_hw_layout(pio_hw_t, rxf[3], PIO_RXF3_OFFSET);
+check_hw_layout(pio_hw_t, ints1, PIO_IRQ1_INTS_OFFSET);
+
+static_assert(NUM_PIO_STATE_MACHINES * NUM_PIOS <= 8, "");
+static uint8_t claimed;
+
+void pio_sm_claim(PIO pio, uint sm) {
+ check_sm_param(sm);
+ uint which = pio_get_index(pio);
+ if (which) {
+ hw_claim_or_assert(&claimed, NUM_PIO_STATE_MACHINES + sm, "PIO 1 SM (%d - 4) already claimed");
+ } else {
+ hw_claim_or_assert(&claimed, sm, "PIO 0 SM %d already claimed");
+ }
+}
+
+void pio_claim_sm_mask(PIO pio, uint sm_mask) {
+ for(uint i = 0; sm_mask; i++, sm_mask >>= 1u) {
+ if (sm_mask & 1u) pio_sm_claim(pio, i);
+ }
+}
+
+void pio_sm_unclaim(PIO pio, uint sm) {
+ check_sm_param(sm);
+ uint which = pio_get_index(pio);
+ hw_claim_clear(&claimed, which * NUM_PIO_STATE_MACHINES + sm);
+}
+
+int pio_claim_unused_sm(PIO pio, bool required) {
+ // PIO index is 0 or 1.
+ uint which = pio_get_index(pio);
+ uint base = which * NUM_PIO_STATE_MACHINES;
+ int index = hw_claim_unused_from_range((uint8_t*)&claimed, required, base,
+ base + NUM_PIO_STATE_MACHINES - 1, "No PIO state machines are available");
+ return index >= (int)base ? index - (int)base : -1;
+}
+
+bool pio_sm_is_claimed(PIO pio, uint sm) {
+ check_sm_param(sm);
+ uint which = pio_get_index(pio);
+ return hw_is_claimed(&claimed, which * NUM_PIO_STATE_MACHINES + sm);
+}
+
+static_assert(PIO_INSTRUCTION_COUNT <= 32, "");
+static uint32_t _used_instruction_space[2];
+
+static int _pio_find_offset_for_program(PIO pio, const pio_program_t *program) {
+ assert(program->length <= PIO_INSTRUCTION_COUNT);
+ uint32_t used_mask = _used_instruction_space[pio_get_index(pio)];
+ uint32_t program_mask = (1u << program->length) - 1;
+ if (program->origin >= 0) {
+ if (program->origin > 32 - program->length) return -1;
+ return used_mask & (program_mask << program->origin) ? -1 : program->origin;
+ } else {
+ // work down from the top always
+ for (int i = 32 - program->length; i >= 0; i--) {
+ if (!(used_mask & (program_mask << (uint) i))) {
+ return i;
+ }
+ }
+ return -1;
+ }
+}
+
+bool pio_can_add_program(PIO pio, const pio_program_t *program) {
+ uint32_t save = hw_claim_lock();
+ bool rc = -1 != _pio_find_offset_for_program(pio, program);
+ hw_claim_unlock(save);
+ return rc;
+}
+
+static bool _pio_can_add_program_at_offset(PIO pio, const pio_program_t *program, uint offset) {
+ valid_params_if(PIO, offset < PIO_INSTRUCTION_COUNT);
+ valid_params_if(PIO, offset + program->length <= PIO_INSTRUCTION_COUNT);
+ if (program->origin >= 0 && (uint)program->origin != offset) return false;
+ uint32_t used_mask = _used_instruction_space[pio_get_index(pio)];
+ uint32_t program_mask = (1u << program->length) - 1;
+ return !(used_mask & (program_mask << offset));
+}
+
+bool pio_can_add_program_at_offset(PIO pio, const pio_program_t *program, uint offset) {
+ uint32_t save = hw_claim_lock();
+ bool rc = _pio_can_add_program_at_offset(pio, program, offset);
+ hw_claim_unlock(save);
+ return rc;
+}
+
+static void _pio_add_program_at_offset(PIO pio, const pio_program_t *program, uint offset) {
+ if (!_pio_can_add_program_at_offset(pio, program, offset)) {
+ panic("No program space");
+ }
+ for (uint i = 0; i < program->length; ++i) {
+ uint16_t instr = program->instructions[i];
+ pio->instr_mem[offset + i] = pio_instr_bits_jmp != _pio_major_instr_bits(instr) ? instr : instr + offset;
+ }
+ uint32_t program_mask = (1u << program->length) - 1;
+ _used_instruction_space[pio_get_index(pio)] |= program_mask << offset;
+}
+
+// these assert if unable
+uint pio_add_program(PIO pio, const pio_program_t *program) {
+ uint32_t save = hw_claim_lock();
+ int offset = _pio_find_offset_for_program(pio, program);
+ if (offset < 0) {
+ panic("No program space");
+ }
+ _pio_add_program_at_offset(pio, program, (uint)offset);
+ hw_claim_unlock(save);
+ return (uint)offset;
+}
+
+void pio_add_program_at_offset(PIO pio, const pio_program_t *program, uint offset) {
+ uint32_t save = hw_claim_lock();
+ _pio_add_program_at_offset(pio, program, offset);
+ hw_claim_unlock(save);
+}
+
+void pio_remove_program(PIO pio, const pio_program_t *program, uint loaded_offset) {
+ uint32_t program_mask = (1u << program->length) - 1;
+ program_mask <<= loaded_offset;
+ uint32_t save = hw_claim_lock();
+ assert(program_mask == (_used_instruction_space[pio_get_index(pio)] & program_mask));
+ _used_instruction_space[pio_get_index(pio)] &= ~program_mask;
+ hw_claim_unlock(save);
+}
+
+void pio_clear_instruction_memory(PIO pio) {
+ uint32_t save = hw_claim_lock();
+ _used_instruction_space[pio_get_index(pio)] = 0;
+ for(uint i=0;i<PIO_INSTRUCTION_COUNT;i++) {
+ pio->instr_mem[i] = pio_encode_jmp(i);
+ }
+ hw_claim_unlock(save);
+}
+
+// Set the value of all PIO pins. This is done by forcibly executing
+// instructions on a "victim" state machine, sm. Ideally you should choose one
+// which is not currently running a program. This is intended for one-time
+// setup of initial pin states.
+void pio_sm_set_pins(PIO pio, uint sm, uint32_t pins) {
+ check_pio_param(pio);
+ check_sm_param(sm);
+ uint32_t pinctrl_saved = pio->sm[sm].pinctrl;
+ uint remaining = 32;
+ uint base = 0;
+ while (remaining) {
+ uint decrement = remaining > 5 ? 5 : remaining;
+ pio->sm[sm].pinctrl =
+ (decrement << PIO_SM0_PINCTRL_SET_COUNT_LSB) |
+ (base << PIO_SM0_PINCTRL_SET_BASE_LSB);
+ pio_sm_exec(pio, sm, pio_encode_set(pio_pins, pins & 0x1fu));
+ remaining -= decrement;
+ base += decrement;
+ pins >>= 5;
+ }
+ pio->sm[sm].pinctrl = pinctrl_saved;
+}
+
+void pio_sm_set_pins_with_mask(PIO pio, uint sm, uint32_t pinvals, uint32_t pin_mask) {
+ check_pio_param(pio);
+ check_sm_param(sm);
+ uint32_t pinctrl_saved = pio->sm[sm].pinctrl;
+ while (pin_mask) {
+ uint base = (uint)__builtin_ctz(pin_mask);
+ pio->sm[sm].pinctrl =
+ (1u << PIO_SM0_PINCTRL_SET_COUNT_LSB) |
+ (base << PIO_SM0_PINCTRL_SET_BASE_LSB);
+ pio_sm_exec(pio, sm, pio_encode_set(pio_pins, (pinvals >> base) & 0x1u));
+ pin_mask &= pin_mask - 1;
+ }
+ pio->sm[sm].pinctrl = pinctrl_saved;
+}
+
+void pio_sm_set_pindirs_with_mask(PIO pio, uint sm, uint32_t pindirs, uint32_t pin_mask) {
+ check_pio_param(pio);
+ check_sm_param(sm);
+ uint32_t pinctrl_saved = pio->sm[sm].pinctrl;
+ while (pin_mask) {
+ uint base = (uint)__builtin_ctz(pin_mask);
+ pio->sm[sm].pinctrl =
+ (1u << PIO_SM0_PINCTRL_SET_COUNT_LSB) |
+ (base << PIO_SM0_PINCTRL_SET_BASE_LSB);
+ pio_sm_exec(pio, sm, pio_encode_set(pio_pindirs, (pindirs >> base) & 0x1u));
+ pin_mask &= pin_mask - 1;
+ }
+ pio->sm[sm].pinctrl = pinctrl_saved;
+}
+
+void pio_sm_set_consecutive_pindirs(PIO pio, uint sm, uint pin, uint count, bool is_out) {
+ check_pio_param(pio);
+ check_sm_param(sm);
+ valid_params_if(PIO, pin < 32u);
+ uint32_t pinctrl_saved = pio->sm[sm].pinctrl;
+ uint pindir_val = is_out ? 0x1f : 0;
+ while (count > 5) {
+ pio->sm[sm].pinctrl = (5u << PIO_SM0_PINCTRL_SET_COUNT_LSB) | (pin << PIO_SM0_PINCTRL_SET_BASE_LSB);
+ pio_sm_exec(pio, sm, pio_encode_set(pio_pindirs, pindir_val));
+ count -= 5;
+ pin = (pin + 5) & 0x1f;
+ }
+ pio->sm[sm].pinctrl = (count << PIO_SM0_PINCTRL_SET_COUNT_LSB) | (pin << PIO_SM0_PINCTRL_SET_BASE_LSB);
+ pio_sm_exec(pio, sm, pio_encode_set(pio_pindirs, pindir_val));
+ pio->sm[sm].pinctrl = pinctrl_saved;
+}
+
+void pio_sm_init(PIO pio, uint sm, uint initial_pc, const pio_sm_config *config) {
+ valid_params_if(PIO, initial_pc < PIO_INSTRUCTION_COUNT);
+ // Halt the machine, set some sensible defaults
+ pio_sm_set_enabled(pio, sm, false);
+
+ if (config) {
+ pio_sm_set_config(pio, sm, config);
+ } else {
+ pio_sm_config c = pio_get_default_sm_config();
+ pio_sm_set_config(pio, sm, &c);
+ }
+
+ pio_sm_clear_fifos(pio, sm);
+
+ // Clear FIFO debug flags
+ const uint32_t fdebug_sm_mask =
+ (1u << PIO_FDEBUG_TXOVER_LSB) |
+ (1u << PIO_FDEBUG_RXUNDER_LSB) |
+ (1u << PIO_FDEBUG_TXSTALL_LSB) |
+ (1u << PIO_FDEBUG_RXSTALL_LSB);
+ pio->fdebug = fdebug_sm_mask << sm;
+
+ // Finally, clear some internal SM state
+ pio_sm_restart(pio, sm);
+ pio_sm_clkdiv_restart(pio, sm);
+ pio_sm_exec(pio, sm, pio_encode_jmp(initial_pc));
+}
+
+void pio_sm_drain_tx_fifo(PIO pio, uint sm) {
+ uint instr = (pio->sm[sm].shiftctrl & PIO_SM0_SHIFTCTRL_AUTOPULL_BITS) ? pio_encode_out(pio_null, 32) :
+ pio_encode_pull(false, false);
+ while (!pio_sm_is_tx_fifo_empty(pio, sm)) {
+ pio_sm_exec(pio, sm, instr);
+ }
+}
diff --git a/src/rp2_common/hardware_pll/CMakeLists.txt b/src/rp2_common/hardware_pll/CMakeLists.txt
new file mode 100644
index 0000000..37ff759
--- /dev/null
+++ b/src/rp2_common/hardware_pll/CMakeLists.txt
@@ -0,0 +1 @@
+pico_simple_hardware_target(pll)
\ No newline at end of file
diff --git a/src/rp2_common/hardware_pll/include/hardware/pll.h b/src/rp2_common/hardware_pll/include/hardware/pll.h
new file mode 100644
index 0000000..ee0c3ae
--- /dev/null
+++ b/src/rp2_common/hardware_pll/include/hardware/pll.h
@@ -0,0 +1,59 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _HARDWARE_PLL_H_
+#define _HARDWARE_PLL_H_
+
+#include "pico.h"
+#include "hardware/structs/pll.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** \file hardware/pll.h
+ * \defgroup hardware_pll hardware_pll
+ *
+ * Phase Locked Loop control APIs
+ *
+ * There are two PLLs in RP2040. They are:
+ * - pll_sys - Used to generate up to a 133MHz system clock
+ * - pll_usb - Used to generate a 48MHz USB reference clock
+ *
+ * For details on how the PLL's are calculated, please refer to the RP2040 datasheet.
+ */
+
+typedef pll_hw_t *PLL;
+
+#define pll_sys pll_sys_hw
+#define pll_usb pll_usb_hw
+
+/*! \brief Initialise specified PLL.
+ * \ingroup hardware_pll
+ * \param pll pll_sys or pll_usb
+ * \param ref_div Input clock divider.
+ * \param vco_freq Requested output from the VCO (voltage controlled oscillator)
+ * \param post_div1 Post Divider 1 - range 1-7. Must be >= post_div2
+ * \param post_div2 Post Divider 2 - range 1-7
+ */
+void pll_init(PLL pll, uint ref_div, uint vco_freq, uint post_div1, uint post_div2);
+
+/*! \brief Release/uninitialise specified PLL.
+ * \ingroup hardware_pll
+ *
+ * This will turn off the power to the specified PLL. Note this function does not currently check if
+ * the PLL is in use before powering it off so should be used with care.
+ *
+ * \param pll pll_sys or pll_usb
+ */
+void pll_deinit(PLL pll);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/rp2_common/hardware_pll/pll.c b/src/rp2_common/hardware_pll/pll.c
new file mode 100644
index 0000000..6cc6184
--- /dev/null
+++ b/src/rp2_common/hardware_pll/pll.c
@@ -0,0 +1,77 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+// For MHZ definitions etc
+#include "hardware/clocks.h"
+#include "hardware/pll.h"
+#include "hardware/resets.h"
+
+/// \tag::pll_init_calculations[]
+void pll_init(PLL pll, uint refdiv, uint vco_freq, uint post_div1, uint post_div2) {
+ uint32_t ref_mhz = XOSC_MHZ / refdiv;
+
+ // What are we multiplying the reference clock by to get the vco freq
+ // (The regs are called div, because you divide the vco output and compare it to the refclk)
+ uint32_t fbdiv = vco_freq / (ref_mhz * MHZ);
+/// \end::pll_init_calculations[]
+
+ // fbdiv
+ assert(fbdiv >= 16 && fbdiv <= 320);
+
+ // Check divider ranges
+ assert((post_div1 >= 1 && post_div1 <= 7) && (post_div2 >= 1 && post_div2 <= 7));
+
+ // post_div1 should be >= post_div2
+ // from appnote page 11
+ // postdiv1 is designed to operate with a higher input frequency
+ // than postdiv2
+ assert(post_div2 <= post_div1);
+
+ // Check that reference frequency is no greater than vco / 16
+ assert(ref_mhz <= (vco_freq / 16));
+
+ // div1 feeds into div2 so if div1 is 5 and div2 is 2 then you get a divide by 10
+ uint32_t pdiv = (post_div1 << PLL_PRIM_POSTDIV1_LSB) |
+ (post_div2 << PLL_PRIM_POSTDIV2_LSB);
+
+/// \tag::pll_init_finish[]
+ if ((pll->cs & PLL_CS_LOCK_BITS) &&
+ (refdiv == (pll->cs & PLL_CS_REFDIV_BITS)) &&
+ (fbdiv == (pll->fbdiv_int & PLL_FBDIV_INT_BITS)) &&
+ (pdiv == (pll->prim & (PLL_PRIM_POSTDIV1_BITS & PLL_PRIM_POSTDIV2_BITS)))) {
+ // do not disrupt PLL that is already correctly configured and operating
+ return;
+ }
+
+ uint32_t pll_reset = (pll_usb_hw == pll) ? RESETS_RESET_PLL_USB_BITS : RESETS_RESET_PLL_SYS_BITS;
+ reset_block(pll_reset);
+ unreset_block_wait(pll_reset);
+
+ // Load VCO-related dividers before starting VCO
+ pll->cs = refdiv;
+ pll->fbdiv_int = fbdiv;
+
+ // Turn on PLL
+ uint32_t power = PLL_PWR_PD_BITS | // Main power
+ PLL_PWR_VCOPD_BITS; // VCO Power
+
+ hw_clear_bits(&pll->pwr, power);
+
+ // Wait for PLL to lock
+ while (!(pll->cs & PLL_CS_LOCK_BITS)) tight_loop_contents();
+
+ // Set up post dividers
+ pll->prim = pdiv;
+
+ // Turn on post divider
+ hw_clear_bits(&pll->pwr, PLL_PWR_POSTDIVPD_BITS);
+/// \end::pll_init_finish[]
+}
+
+void pll_deinit(PLL pll) {
+ // todo: Make sure there are no sources running from this pll?
+ pll->pwr = PLL_PWR_BITS;
+}
diff --git a/src/rp2_common/hardware_pwm/CMakeLists.txt b/src/rp2_common/hardware_pwm/CMakeLists.txt
new file mode 100644
index 0000000..c8d3401
--- /dev/null
+++ b/src/rp2_common/hardware_pwm/CMakeLists.txt
@@ -0,0 +1 @@
+pico_simple_hardware_headers_only_target(pwm)
diff --git a/src/rp2_common/hardware_pwm/include/hardware/pwm.h b/src/rp2_common/hardware_pwm/include/hardware/pwm.h
new file mode 100644
index 0000000..634375e
--- /dev/null
+++ b/src/rp2_common/hardware_pwm/include/hardware/pwm.h
@@ -0,0 +1,559 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _HARDWARE_PWM_H
+#define _HARDWARE_PWM_H
+
+#include "pico.h"
+#include "hardware/structs/pwm.h"
+#include "hardware/regs/dreq.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// PICO_CONFIG: PARAM_ASSERTIONS_ENABLED_PWM, Enable/disable assertions in the PWM module, type=bool, default=0, group=hardware_pwm
+#ifndef PARAM_ASSERTIONS_ENABLED_PWM
+#define PARAM_ASSERTIONS_ENABLED_PWM 0
+#endif
+
+/** \file hardware/pwm.h
+ * \defgroup hardware_pwm hardware_pwm
+ *
+ * Hardware Pulse Width Modulation (PWM) API
+ *
+ * The RP2040 PWM block has 8 identical slices. Each slice can drive two PWM output signals, or
+ * measure the frequency or duty cycle of an input signal. This gives a total of up to 16 controllable
+ * PWM outputs. All 30 GPIOs can be driven by the PWM block.
+ *
+ * The PWM hardware functions by continuously comparing the input value to a free-running counter. This produces a
+ * toggling output where the amount of time spent at the high output level is proportional to the input value. The fraction of
+ * time spent at the high signal level is known as the duty cycle of the signal.
+ *
+ * The default behaviour of a PWM slice is to count upward until the wrap value (\ref pwm_config_set_wrap) is reached, and then
+ * immediately wrap to 0. PWM slices also offer a phase-correct mode, where the counter starts to count downward after
+ * reaching TOP, until it reaches 0 again.
+ *
+ * \subsection pwm_example Example
+ * \addtogroup hardware_pwm
+ * \include hello_pwm.c
+ */
+
+/** \brief PWM Divider mode settings
+ * \ingroup hardware_pwm
+ *
+ */
+enum pwm_clkdiv_mode
+{
+ PWM_DIV_FREE_RUNNING = 0, ///< Free-running counting at rate dictated by fractional divider
+ PWM_DIV_B_HIGH = 1, ///< Fractional divider is gated by the PWM B pin
+ PWM_DIV_B_RISING = 2, ///< Fractional divider advances with each rising edge of the PWM B pin
+ PWM_DIV_B_FALLING = 3 ///< Fractional divider advances with each falling edge of the PWM B pin
+};
+
+enum pwm_chan
+{
+ PWM_CHAN_A = 0,
+ PWM_CHAN_B = 1
+};
+
+typedef struct {
+ uint32_t csr;
+ uint32_t div;
+ uint32_t top;
+} pwm_config;
+
+static inline void check_slice_num_param(__unused uint slice_num) {
+ valid_params_if(PWM, slice_num < NUM_PWM_SLICES);
+}
+
+/** \brief Determine the PWM slice that is attached to the specified GPIO
+ * \ingroup hardware_pwm
+ *
+ * \return The PWM slice number that controls the specified GPIO.
+ */
+static inline uint pwm_gpio_to_slice_num(uint gpio) {
+ valid_params_if(PWM, gpio < NUM_BANK0_GPIOS);
+ return (gpio >> 1u) & 7u;
+}
+
+/** \brief Determine the PWM channel that is attached to the specified GPIO.
+ * \ingroup hardware_pwm
+ *
+ * Each slice 0 to 7 has two channels, A and B.
+ *
+ * \return The PWM channel that controls the specified GPIO.
+ */
+static inline uint pwm_gpio_to_channel(uint gpio) {
+ valid_params_if(PWM, gpio < NUM_BANK0_GPIOS);
+ return gpio & 1u;
+}
+
+/** \brief Set phase correction in a PWM configuration
+ * \ingroup hardware_pwm
+ *
+ * \param c PWM configuration struct to modify
+ * \param phase_correct true to set phase correct modulation, false to set trailing edge
+ *
+ * Setting phase control to true means that instead of wrapping back to zero when the wrap point is reached,
+ * the PWM starts counting back down. The output frequency is halved when phase-correct mode is enabled.
+ */
+static inline void pwm_config_set_phase_correct(pwm_config *c, bool phase_correct) {
+ c->csr = (c->csr & ~PWM_CH0_CSR_PH_CORRECT_BITS)
+ | (bool_to_bit(phase_correct) << PWM_CH0_CSR_PH_CORRECT_LSB);
+}
+
+/** \brief Set clock divider in a PWM configuration
+ * \ingroup hardware_pwm
+ *
+ * \param c PWM configuration struct to modify
+ * \param div Value to divide counting rate by. Must be greater than or equal to 1.
+ *
+ * If the divide mode is free-running, the PWM counter runs at clk_sys / div.
+ * Otherwise, the divider reduces the rate of events seen on the B pin input (level or edge)
+ * before passing them on to the PWM counter.
+ */
+static inline void pwm_config_set_clkdiv(pwm_config *c, float div) {
+ c->div = (uint32_t)(div * (float)(1u << PWM_CH0_DIV_INT_LSB));
+}
+
+/** \brief Set PWM clock divider in a PWM configuration
+ * \ingroup hardware_pwm
+ *
+ * \param c PWM configuration struct to modify
+ * \param div Integer value to reduce counting rate by. Must be greater than or equal to 1.
+ *
+ * If the divide mode is free-running, the PWM counter runs at clk_sys / div.
+ * Otherwise, the divider reduces the rate of events seen on the B pin input (level or edge)
+ * before passing them on to the PWM counter.
+ */
+static inline void pwm_config_set_clkdiv_int(pwm_config *c, uint div) {
+ c->div = div << PWM_CH0_DIV_INT_LSB;
+}
+
+/** \brief Set PWM counting mode in a PWM configuration
+ * \ingroup hardware_pwm
+ *
+ * \param c PWM configuration struct to modify
+ * \param mode PWM divide/count mode
+ *
+ * Configure which event gates the operation of the fractional divider.
+ * The default is always-on (free-running PWM). Can also be configured to count on
+ * high level, rising edge or falling edge of the B pin input.
+ */
+static inline void pwm_config_set_clkdiv_mode(pwm_config *c, enum pwm_clkdiv_mode mode) {
+ valid_params_if(PWM, mode == PWM_DIV_FREE_RUNNING ||
+ mode == PWM_DIV_B_RISING ||
+ mode == PWM_DIV_B_HIGH ||
+ mode == PWM_DIV_B_FALLING);
+ c->csr = (c->csr & ~PWM_CH0_CSR_DIVMODE_BITS)
+ | (((uint)mode) << PWM_CH0_CSR_DIVMODE_LSB);
+}
+
+/** \brief Set output polarity in a PWM configuration
+ * \ingroup hardware_pwm
+ *
+ * \param c PWM configuration struct to modify
+ * \param a true to invert output A
+ * \param b true to invert output B
+ */
+static inline void pwm_config_set_output_polarity(pwm_config *c, bool a, bool b) {
+ c->csr = (c->csr & ~(PWM_CH0_CSR_A_INV_BITS | PWM_CH0_CSR_B_INV_BITS))
+ | ((bool_to_bit(a) << PWM_CH0_CSR_A_INV_LSB) | (bool_to_bit(b) << PWM_CH0_CSR_B_INV_LSB));
+}
+
+/** \brief Set PWM counter wrap value in a PWM configuration
+ * \ingroup hardware_pwm
+ *
+ * Set the highest value the counter will reach before returning to 0. Also known as TOP.
+ *
+ * \param c PWM configuration struct to modify
+ * \param wrap Value to set wrap to
+ */
+static inline void pwm_config_set_wrap(pwm_config *c, uint16_t wrap) {
+ c->top = wrap;
+}
+
+/** \brief Initialise a PWM with settings from a configuration object
+ * \ingroup hardware_pwm
+ *
+ * Use the \ref pwm_get_default_config() function to initialise a config structure, make changes as
+ * needed using the pwm_config_* functions, then call this function to set up the PWM.
+ *
+ * \param slice_num PWM slice number
+ * \param c The configuration to use
+ * \param start If true the PWM will be started running once configured. If false you will need to start
+ * manually using \ref pwm_set_enabled() or \ref pwm_set_mask_enabled()
+ */
+static inline void pwm_init(uint slice_num, pwm_config *c, bool start) {
+ check_slice_num_param(slice_num);
+ pwm_hw->slice[slice_num].csr = 0;
+
+ pwm_hw->slice[slice_num].ctr = PWM_CH0_CTR_RESET;
+ pwm_hw->slice[slice_num].cc = PWM_CH0_CC_RESET;
+ pwm_hw->slice[slice_num].top = c->top;
+ pwm_hw->slice[slice_num].div = c->div;
+ pwm_hw->slice[slice_num].csr = c->csr | (bool_to_bit(start) << PWM_CH0_CSR_EN_LSB);
+}
+
+/** \brief Get a set of default values for PWM configuration
+ * \ingroup hardware_pwm
+ *
+ * PWM config is free-running at system clock speed, no phase correction, wrapping at 0xffff,
+ * with standard polarities for channels A and B.
+ *
+ * \return Set of default values.
+ */
+static inline pwm_config pwm_get_default_config(void) {
+ pwm_config c = {0, 0, 0};
+ pwm_config_set_phase_correct(&c, false);
+ pwm_config_set_clkdiv_int(&c, 1);
+ pwm_config_set_clkdiv_mode(&c, PWM_DIV_FREE_RUNNING);
+ pwm_config_set_output_polarity(&c, false, false);
+ pwm_config_set_wrap(&c, 0xffffu);
+ return c;
+}
+
+/** \brief Set the current PWM counter wrap value
+ * \ingroup hardware_pwm
+ *
+ * Set the highest value the counter will reach before returning to 0. Also
+ * known as TOP.
+ *
+ * The counter wrap value is double-buffered in hardware. This means that,
+ * when the PWM is running, a write to the counter wrap value does not take
+ * effect until after the next time the PWM slice wraps (or, in phase-correct
+ * mode, the next time the slice reaches 0). If the PWM is not running, the
+ * write is latched in immediately.
+ *
+ * \param slice_num PWM slice number
+ * \param wrap Value to set wrap to
+ */
+static inline void pwm_set_wrap(uint slice_num, uint16_t wrap) {
+ check_slice_num_param(slice_num);
+ pwm_hw->slice[slice_num].top = wrap;
+}
+
+/** \brief Set the current PWM counter compare value for one channel
+ * \ingroup hardware_pwm
+ *
+ * Set the value of the PWM counter compare value, for either channel A or channel B.
+ *
+ * The counter compare register is double-buffered in hardware. This means
+ * that, when the PWM is running, a write to the counter compare values does
+ * not take effect until the next time the PWM slice wraps (or, in
+ * phase-correct mode, the next time the slice reaches 0). If the PWM is not
+ * running, the write is latched in immediately.
+ *
+ * \param slice_num PWM slice number
+ * \param chan Which channel to update. 0 for A, 1 for B.
+ * \param level new level for the selected output
+ */
+static inline void pwm_set_chan_level(uint slice_num, uint chan, uint16_t level) {
+ check_slice_num_param(slice_num);
+ hw_write_masked(
+ &pwm_hw->slice[slice_num].cc,
+ ((uint)level) << (chan ? PWM_CH0_CC_B_LSB : PWM_CH0_CC_A_LSB),
+ chan ? PWM_CH0_CC_B_BITS : PWM_CH0_CC_A_BITS
+ );
+}
+
+/** \brief Set PWM counter compare values
+ * \ingroup hardware_pwm
+ *
+ * Set the value of the PWM counter compare values, A and B.
+ *
+ * The counter compare register is double-buffered in hardware. This means
+ * that, when the PWM is running, a write to the counter compare values does
+ * not take effect until the next time the PWM slice wraps (or, in
+ * phase-correct mode, the next time the slice reaches 0). If the PWM is not
+ * running, the write is latched in immediately.
+ *
+ * \param slice_num PWM slice number
+ * \param level_a Value to set compare A to. When the counter reaches this value the A output is deasserted
+ * \param level_b Value to set compare B to. When the counter reaches this value the B output is deasserted
+ */
+static inline void pwm_set_both_levels(uint slice_num, uint16_t level_a, uint16_t level_b) {
+ check_slice_num_param(slice_num);
+ pwm_hw->slice[slice_num].cc = (((uint)level_b) << PWM_CH0_CC_B_LSB) | (((uint)level_a) << PWM_CH0_CC_A_LSB);
+}
+
+/** \brief Helper function to set the PWM level for the slice and channel associated with a GPIO.
+ * \ingroup hardware_pwm
+ *
+ * Look up the correct slice (0 to 7) and channel (A or B) for a given GPIO, and update the corresponding
+ * counter compare field.
+ *
+ * This PWM slice should already have been configured and set running. Also be careful of multiple GPIOs
+ * mapping to the same slice and channel (if GPIOs have a difference of 16).
+ *
+ * The counter compare register is double-buffered in hardware. This means
+ * that, when the PWM is running, a write to the counter compare values does
+ * not take effect until the next time the PWM slice wraps (or, in
+ * phase-correct mode, the next time the slice reaches 0). If the PWM is not
+ * running, the write is latched in immediately.
+ *
+ * \param gpio GPIO to set level of
+ * \param level PWM level for this GPIO
+ */
+static inline void pwm_set_gpio_level(uint gpio, uint16_t level) {
+ valid_params_if(PWM, gpio < NUM_BANK0_GPIOS);
+ pwm_set_chan_level(pwm_gpio_to_slice_num(gpio), pwm_gpio_to_channel(gpio), level);
+}
+
+/** \brief Get PWM counter
+ * \ingroup hardware_pwm
+ *
+ * Get current value of PWM counter
+ *
+ * \param slice_num PWM slice number
+ * \return Current value of the PWM counter
+ */
+static inline uint16_t pwm_get_counter(uint slice_num) {
+ check_slice_num_param(slice_num);
+ return (uint16_t)(pwm_hw->slice[slice_num].ctr);
+}
+
+/** \brief Set PWM counter
+ * \ingroup hardware_pwm
+ *
+ * Set the value of the PWM counter
+ *
+ * \param slice_num PWM slice number
+ * \param c Value to set the PWM counter to
+ *
+ */
+static inline void pwm_set_counter(uint slice_num, uint16_t c) {
+ check_slice_num_param(slice_num);
+ pwm_hw->slice[slice_num].ctr = c;
+}
+
+/** \brief Advance PWM count
+ * \ingroup hardware_pwm
+ *
+ * Advance the phase of a running the counter by 1 count.
+ *
+ * This function will return once the increment is complete.
+ *
+ * \param slice_num PWM slice number
+ */
+static inline void pwm_advance_count(uint slice_num) {
+ check_slice_num_param(slice_num);
+ hw_set_bits(&pwm_hw->slice[slice_num].csr, PWM_CH0_CSR_PH_ADV_BITS);
+ while (pwm_hw->slice[slice_num].csr & PWM_CH0_CSR_PH_ADV_BITS) {
+ tight_loop_contents();
+ }
+}
+
+/** \brief Retard PWM count
+ * \ingroup hardware_pwm
+ *
+ * Retard the phase of a running counter by 1 count
+ *
+ * This function will return once the retardation is complete.
+ *
+ * \param slice_num PWM slice number
+ */
+static inline void pwm_retard_count(uint slice_num) {
+ check_slice_num_param(slice_num);
+ hw_set_bits(&pwm_hw->slice[slice_num].csr, PWM_CH0_CSR_PH_RET_BITS);
+ while (pwm_hw->slice[slice_num].csr & PWM_CH0_CSR_PH_RET_BITS) {
+ tight_loop_contents();
+ }
+}
+
+/** \brief Set PWM clock divider using an 8:4 fractional value
+ * \ingroup hardware_pwm
+ *
+ * Set the clock divider. Counter increment will be on sysclock divided by this value, taking into account the gating.
+ *
+ * \param slice_num PWM slice number
+ * \param integer 8 bit integer part of the clock divider
+ * \param fract 4 bit fractional part of the clock divider
+ */
+static inline void pwm_set_clkdiv_int_frac(uint slice_num, uint8_t integer, uint8_t fract) {
+ check_slice_num_param(slice_num);
+ valid_params_if(PWM, fract < 16);
+ pwm_hw->slice[slice_num].div = (((uint)integer) << PWM_CH0_DIV_INT_LSB) | (((uint)fract) << PWM_CH0_DIV_FRAC_LSB);
+}
+
+/** \brief Set PWM clock divider
+ * \ingroup hardware_pwm
+ *
+ * Set the clock divider. Counter increment will be on sysclock divided by this value, taking into account the gating.
+ *
+ * \param slice_num PWM slice number
+ * \param divider Floating point clock divider, 1.f <= value < 256.f
+ */
+static inline void pwm_set_clkdiv(uint slice_num, float divider) {
+ check_slice_num_param(slice_num);
+ valid_params_if(PWM, divider >= 1.f && divider < 256.f);
+ uint8_t i = (uint8_t)divider;
+ uint8_t f = (uint8_t)((divider - i) * (0x01 << 4));
+ pwm_set_clkdiv_int_frac(slice_num, i, f);
+}
+
+/** \brief Set PWM output polarity
+ * \ingroup hardware_pwm
+ *
+ * \param slice_num PWM slice number
+ * \param a true to invert output A
+ * \param b true to invert output B
+ */
+static inline void pwm_set_output_polarity(uint slice_num, bool a, bool b) {
+ check_slice_num_param(slice_num);
+ hw_write_masked(&pwm_hw->slice[slice_num].csr, bool_to_bit(a) << PWM_CH0_CSR_A_INV_LSB | bool_to_bit(b) << PWM_CH0_CSR_B_INV_LSB,
+ PWM_CH0_CSR_A_INV_BITS | PWM_CH0_CSR_B_INV_BITS);
+}
+
+
+/** \brief Set PWM divider mode
+ * \ingroup hardware_pwm
+ *
+ * \param slice_num PWM slice number
+ * \param mode Required divider mode
+ */
+static inline void pwm_set_clkdiv_mode(uint slice_num, enum pwm_clkdiv_mode mode) {
+ check_slice_num_param(slice_num);
+ valid_params_if(PWM, mode == PWM_DIV_FREE_RUNNING ||
+ mode == PWM_DIV_B_RISING ||
+ mode == PWM_DIV_B_HIGH ||
+ mode == PWM_DIV_B_FALLING);
+ hw_write_masked(&pwm_hw->slice[slice_num].csr, ((uint)mode) << PWM_CH0_CSR_DIVMODE_LSB, PWM_CH0_CSR_DIVMODE_BITS);
+}
+
+/** \brief Set PWM phase correct on/off
+ * \ingroup hardware_pwm
+ *
+ * \param slice_num PWM slice number
+ * \param phase_correct true to set phase correct modulation, false to set trailing edge
+ *
+ * Setting phase control to true means that instead of wrapping back to zero when the wrap point is reached,
+ * the PWM starts counting back down. The output frequency is halved when phase-correct mode is enabled.
+ */
+static inline void pwm_set_phase_correct(uint slice_num, bool phase_correct) {
+ check_slice_num_param(slice_num);
+ hw_write_masked(&pwm_hw->slice[slice_num].csr, bool_to_bit(phase_correct) << PWM_CH0_CSR_PH_CORRECT_LSB, PWM_CH0_CSR_PH_CORRECT_BITS);
+}
+
+/** \brief Enable/Disable PWM
+ * \ingroup hardware_pwm
+ *
+ * When a PWM is disabled, it halts its counter, and the output pins are left
+ * high or low depending on exactly when the counter is halted. When
+ * re-enabled the PWM resumes immediately from where it left off.
+ *
+ * If the PWM's output pins need to be low when halted:
+ *
+ * - The counter compare can be set to zero whilst the PWM is enabled, and
+ * then the PWM disabled once both pins are seen to be low
+ *
+ * - The GPIO output overrides can be used to force the actual pins low
+ *
+ * - The PWM can be run for one cycle (i.e. enabled then immediately disabled)
+ * with a TOP of 0, count of 0 and counter compare of 0, to force the pins
+ * low when the PWM has already been halted. The same method can be used
+ * with a counter compare value of 1 to force a pin high.
+ *
+ * Note that, when disabled, the PWM can still be advanced one count at a time
+ * by pulsing the PH_ADV bit in its CSR. The output pins transition as though
+ * the PWM were enabled.
+ *
+ * \param slice_num PWM slice number
+ * \param enabled true to enable the specified PWM, false to disable.
+ */
+static inline void pwm_set_enabled(uint slice_num, bool enabled) {
+ check_slice_num_param(slice_num);
+ hw_write_masked(&pwm_hw->slice[slice_num].csr, bool_to_bit(enabled) << PWM_CH0_CSR_EN_LSB, PWM_CH0_CSR_EN_BITS);
+}
+
+/** \brief Enable/Disable multiple PWM slices simultaneously
+ * \ingroup hardware_pwm
+ *
+ * \param mask Bitmap of PWMs to enable/disable. Bits 0 to 7 enable slices 0-7 respectively
+ */
+static inline void pwm_set_mask_enabled(uint32_t mask) {
+ pwm_hw->en = mask;
+}
+
+/*! \brief Enable PWM instance interrupt
+ * \ingroup hardware_pwm
+ *
+ * Used to enable a single PWM instance interrupt.
+ *
+ * \param slice_num PWM block to enable/disable
+ * \param enabled true to enable, false to disable
+ */
+static inline void pwm_set_irq_enabled(uint slice_num, bool enabled) {
+ check_slice_num_param(slice_num);
+ if (enabled) {
+ hw_set_bits(&pwm_hw->inte, 1u << slice_num);
+ } else {
+ hw_clear_bits(&pwm_hw->inte, 1u << slice_num);
+ }
+}
+
+/*! \brief Enable multiple PWM instance interrupts
+ * \ingroup hardware_pwm
+ *
+ * Use this to enable multiple PWM interrupts at once.
+ *
+ * \param slice_mask Bitmask of all the blocks to enable/disable. Channel 0 = bit 0, channel 1 = bit 1 etc.
+ * \param enabled true to enable, false to disable
+ */
+static inline void pwm_set_irq_mask_enabled(uint32_t slice_mask, bool enabled) {
+ valid_params_if(PWM, slice_mask < 256);
+ if (enabled) {
+ hw_set_bits(&pwm_hw->inte, slice_mask);
+ } else {
+ hw_clear_bits(&pwm_hw->inte, slice_mask);
+ }
+}
+
+/*! \brief Clear a single PWM channel interrupt
+ * \ingroup hardware_pwm
+ *
+ * \param slice_num PWM slice number
+ */
+static inline void pwm_clear_irq(uint slice_num) {
+ pwm_hw->intr = 1u << slice_num;
+}
+
+/*! \brief Get PWM interrupt status, raw
+ * \ingroup hardware_pwm
+ *
+ * \return Bitmask of all PWM interrupts currently set
+ */
+static inline uint32_t pwm_get_irq_status_mask(void) {
+ return pwm_hw->ints;
+}
+
+/*! \brief Force PWM interrupt
+ * \ingroup hardware_pwm
+ *
+ * \param slice_num PWM slice number
+ */
+static inline void pwm_force_irq(uint slice_num) {
+ pwm_hw->intf = 1u << slice_num;
+}
+
+/*! \brief Return the DREQ to use for pacing transfers to a particular PWM slice
+ * \ingroup hardware_pwm
+ *
+ * \param slice_num PWM slice number
+ */
+static inline uint pwm_get_dreq(uint slice_num) {
+ static_assert(DREQ_PWM_WRAP1 == DREQ_PWM_WRAP0 + 1, "");
+ static_assert(DREQ_PWM_WRAP7 == DREQ_PWM_WRAP0 + 7, "");
+ check_slice_num_param(slice_num);
+ return DREQ_PWM_WRAP0 + slice_num;
+}
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/rp2_common/hardware_resets/CMakeLists.txt b/src/rp2_common/hardware_resets/CMakeLists.txt
new file mode 100644
index 0000000..e0712e7
--- /dev/null
+++ b/src/rp2_common/hardware_resets/CMakeLists.txt
@@ -0,0 +1 @@
+pico_simple_hardware_headers_only_target(resets)
diff --git a/src/rp2_common/hardware_resets/include/hardware/resets.h b/src/rp2_common/hardware_resets/include/hardware/resets.h
new file mode 100644
index 0000000..a3f7014
--- /dev/null
+++ b/src/rp2_common/hardware_resets/include/hardware/resets.h
@@ -0,0 +1,99 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _HARDWARE_RESETS_H
+#define _HARDWARE_RESETS_H
+
+#include "pico.h"
+#include "hardware/structs/resets.h"
+
+/** \file hardware/resets.h
+ * \defgroup hardware_resets hardware_resets
+ *
+ * Hardware Reset API
+ *
+ * The reset controller allows software control of the resets to all of the peripherals that are not
+ * critical to boot the processor in the RP2040.
+ *
+ * \subsubsection reset_bitmask
+ * \addtogroup hardware_resets
+ *
+ * Multiple blocks are referred to using a bitmask as follows:
+ *
+ * Block to reset | Bit
+ * ---------------|----
+ * USB | 24
+ * UART 1 | 23
+ * UART 0 | 22
+ * Timer | 21
+ * TB Manager | 20
+ * SysInfo | 19
+ * System Config | 18
+ * SPI 1 | 17
+ * SPI 0 | 16
+ * RTC | 15
+ * PWM | 14
+ * PLL USB | 13
+ * PLL System | 12
+ * PIO 1 | 11
+ * PIO 0 | 10
+ * Pads - QSPI | 9
+ * Pads - bank 0 | 8
+ * JTAG | 7
+ * IO Bank 1 | 6
+ * IO Bank 0 | 5
+ * I2C 1 | 4
+ * I2C 0 | 3
+ * DMA | 2
+ * Bus Control | 1
+ * ADC 0 | 0
+ *
+ * \subsection reset_example Example
+ * \addtogroup hardware_resets
+ * \include hello_reset.c
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/// \tag::reset_funcs[]
+
+/*! \brief Reset the specified HW blocks
+ * \ingroup hardware_resets
+ *
+ * \param bits Bit pattern indicating blocks to reset. See \ref reset_bitmask
+ */
+static inline void reset_block(uint32_t bits) {
+ hw_set_bits(&resets_hw->reset, bits);
+}
+
+/*! \brief bring specified HW blocks out of reset
+ * \ingroup hardware_resets
+ *
+ * \param bits Bit pattern indicating blocks to unreset. See \ref reset_bitmask
+ */
+static inline void unreset_block(uint32_t bits) {
+ hw_clear_bits(&resets_hw->reset, bits);
+}
+
+/*! \brief Bring specified HW blocks out of reset and wait for completion
+ * \ingroup hardware_resets
+ *
+ * \param bits Bit pattern indicating blocks to unreset. See \ref reset_bitmask
+ */
+static inline void unreset_block_wait(uint32_t bits) {
+ hw_clear_bits(&resets_hw->reset, bits);
+ while (~resets_hw->reset_done & bits)
+ tight_loop_contents();
+}
+/// \end::reset_funcs[]
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/rp2_common/hardware_rtc/CMakeLists.txt b/src/rp2_common/hardware_rtc/CMakeLists.txt
new file mode 100644
index 0000000..dce6eff
--- /dev/null
+++ b/src/rp2_common/hardware_rtc/CMakeLists.txt
@@ -0,0 +1 @@
+pico_simple_hardware_target(rtc)
\ No newline at end of file
diff --git a/src/rp2_common/hardware_rtc/include/hardware/rtc.h b/src/rp2_common/hardware_rtc/include/hardware/rtc.h
new file mode 100644
index 0000000..8757e36
--- /dev/null
+++ b/src/rp2_common/hardware_rtc/include/hardware/rtc.h
@@ -0,0 +1,91 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _HARDWARE_RTC_H
+#define _HARDWARE_RTC_H
+
+#include "pico.h"
+#include "hardware/structs/rtc.h"
+
+/** \file hardware/rtc.h
+ * \defgroup hardware_rtc hardware_rtc
+ *
+ * Hardware Real Time Clock API
+ *
+ * The RTC keeps track of time in human readable format and generates events when the time is equal
+ * to a preset value. Think of a digital clock, not epoch time used by most computers. There are seven
+ * fields, one each for year (12 bit), month (4 bit), day (5 bit), day of the week (3 bit), hour (5 bit)
+ * minute (6 bit) and second (6 bit), storing the data in binary format.
+ *
+ * \sa datetime_t
+ *
+ * \subsection rtc_example Example
+ * \addtogroup hardware_rtc
+ *
+ * \include hello_rtc.c
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*! Callback function type for RTC alarms
+ * \ingroup hardware_rtc
+ *
+ * \sa rtc_set_alarm()
+ */
+typedef void (*rtc_callback_t)(void);
+
+/*! \brief Initialise the RTC system
+ * \ingroup hardware_rtc
+ */
+void rtc_init(void);
+
+/*! \brief Set the RTC to the specified time
+ * \ingroup hardware_rtc
+ *
+ * \param t Pointer to a \ref datetime_t structure contains time to set
+ * \return true if set, false if the passed in datetime was invalid.
+ */
+bool rtc_set_datetime(datetime_t *t);
+
+/*! \brief Get the current time from the RTC
+ * \ingroup hardware_rtc
+ *
+ * \param t Pointer to a \ref datetime_t structure to receive the current RTC time
+ * \return true if datetime is valid, false if the RTC is not running.
+ */
+bool rtc_get_datetime(datetime_t *t);
+
+/*! \brief Is the RTC running?
+ * \ingroup hardware_rtc
+ *
+ */
+bool rtc_running(void);
+
+/*! \brief Set a time in the future for the RTC to call a user provided callback
+ * \ingroup hardware_rtc
+ *
+ * \param t Pointer to a \ref datetime_t structure containing a time in the future to fire the alarm. Any values set to -1 will not be matched on.
+ * \param user_callback pointer to a \ref rtc_callback_t to call when the alarm fires
+ */
+void rtc_set_alarm(datetime_t *t, rtc_callback_t user_callback);
+
+/*! \brief Enable the RTC alarm (if inactive)
+ * \ingroup hardware_rtc
+ */
+void rtc_enable_alarm(void);
+
+/*! \brief Disable the RTC alarm (if active)
+ * \ingroup hardware_rtc
+ */
+void rtc_disable_alarm(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/rp2_common/hardware_rtc/rtc.c b/src/rp2_common/hardware_rtc/rtc.c
new file mode 100644
index 0000000..5429acd
--- /dev/null
+++ b/src/rp2_common/hardware_rtc/rtc.c
@@ -0,0 +1,191 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "pico.h"
+
+#include "hardware/irq.h"
+#include "hardware/rtc.h"
+#include "hardware/resets.h"
+#include "hardware/clocks.h"
+
+// Set this when setting an alarm
+static rtc_callback_t _callback = NULL;
+static bool _alarm_repeats = false;
+
+bool rtc_running(void) {
+ return (rtc_hw->ctrl & RTC_CTRL_RTC_ACTIVE_BITS);
+}
+
+void rtc_init(void) {
+ // Get clk_rtc freq and make sure it is running
+ uint rtc_freq = clock_get_hz(clk_rtc);
+ assert(rtc_freq != 0);
+
+ // Take rtc out of reset now that we know clk_rtc is running
+ reset_block(RESETS_RESET_RTC_BITS);
+ unreset_block_wait(RESETS_RESET_RTC_BITS);
+
+ // Set up the 1 second divider.
+ // If rtc_freq is 400 then clkdiv_m1 should be 399
+ rtc_freq -= 1;
+
+ // Check the freq is not too big to divide
+ assert(rtc_freq <= RTC_CLKDIV_M1_BITS);
+
+ // Write divide value
+ rtc_hw->clkdiv_m1 = rtc_freq;
+}
+
+static bool valid_datetime(datetime_t *t) {
+ // Valid ranges taken from RTC doc. Note when setting an RTC alarm
+ // these values are allowed to be -1 to say "don't match this value"
+ if (!(t->year >= 0 && t->year <= 4095)) return false;
+ if (!(t->month >= 1 && t->month <= 12)) return false;
+ if (!(t->day >= 1 && t->day <= 31)) return false;
+ if (!(t->dotw >= 0 && t->dotw <= 6)) return false;
+ if (!(t->hour >= 0 && t->hour <= 23)) return false;
+ if (!(t->min >= 0 && t->min <= 59)) return false;
+ if (!(t->sec >= 0 && t->sec <= 59)) return false;
+ return true;
+}
+
+bool rtc_set_datetime(datetime_t *t) {
+ if (!valid_datetime(t)) {
+ return false;
+ }
+
+ // Disable RTC
+ rtc_hw->ctrl = 0;
+ // Wait while it is still active
+ while (rtc_running()) {
+ tight_loop_contents();
+ }
+
+ // Write to setup registers
+ rtc_hw->setup_0 = (((uint)t->year) << RTC_SETUP_0_YEAR_LSB ) |
+ (((uint)t->month) << RTC_SETUP_0_MONTH_LSB) |
+ (((uint)t->day) << RTC_SETUP_0_DAY_LSB);
+ rtc_hw->setup_1 = (((uint)t->dotw) << RTC_SETUP_1_DOTW_LSB) |
+ (((uint)t->hour) << RTC_SETUP_1_HOUR_LSB) |
+ (((uint)t->min) << RTC_SETUP_1_MIN_LSB) |
+ (((uint)t->sec) << RTC_SETUP_1_SEC_LSB);
+
+ // Load setup values into rtc clock domain
+ rtc_hw->ctrl = RTC_CTRL_LOAD_BITS;
+
+ // Enable RTC and wait for it to be running
+ rtc_hw->ctrl = RTC_CTRL_RTC_ENABLE_BITS;
+ while (!rtc_running()) {
+ tight_loop_contents();
+ }
+
+ return true;
+}
+
+bool rtc_get_datetime(datetime_t *t) {
+ // Make sure RTC is running
+ if (!rtc_running()) {
+ return false;
+ }
+
+ // Note: RTC_0 should be read before RTC_1
+ uint32_t rtc_0 = rtc_hw->rtc_0;
+ uint32_t rtc_1 = rtc_hw->rtc_1;
+
+ t->dotw = (rtc_0 & RTC_RTC_0_DOTW_BITS ) >> RTC_RTC_0_DOTW_LSB;
+ t->hour = (rtc_0 & RTC_RTC_0_HOUR_BITS ) >> RTC_RTC_0_HOUR_LSB;
+ t->min = (rtc_0 & RTC_RTC_0_MIN_BITS ) >> RTC_RTC_0_MIN_LSB;
+ t->sec = (rtc_0 & RTC_RTC_0_SEC_BITS ) >> RTC_RTC_0_SEC_LSB;
+ t->year = (rtc_1 & RTC_RTC_1_YEAR_BITS ) >> RTC_RTC_1_YEAR_LSB;
+ t->month = (rtc_1 & RTC_RTC_1_MONTH_BITS) >> RTC_RTC_1_MONTH_LSB;
+ t->day = (rtc_1 & RTC_RTC_1_DAY_BITS ) >> RTC_RTC_1_DAY_LSB;
+
+ return true;
+}
+
+void rtc_enable_alarm(void) {
+ // Set matching and wait for it to be enabled
+ hw_set_bits(&rtc_hw->irq_setup_0, RTC_IRQ_SETUP_0_MATCH_ENA_BITS);
+ while(!(rtc_hw->irq_setup_0 & RTC_IRQ_SETUP_0_MATCH_ACTIVE_BITS)) {
+ tight_loop_contents();
+ }
+}
+
+static void rtc_irq_handler(void) {
+ // Always disable the alarm to clear the current IRQ.
+ // Even if it is a repeatable alarm, we don't want it to keep firing.
+ // If it matches on a second it can keep firing for that second.
+ rtc_disable_alarm();
+
+ if (_alarm_repeats) {
+ // If it is a repeatable alarm, re enable the alarm.
+ rtc_enable_alarm();
+ }
+
+ // Call user callback function
+ if (_callback) {
+ _callback();
+ }
+}
+
+static bool rtc_alarm_repeats(datetime_t *t) {
+ // If any value is set to -1 then we don't match on that value
+ // hence the alarm will eventually repeat
+ if (t->year < 0) return true;
+ if (t->month < 0) return true;
+ if (t->day < 0) return true;
+ if (t->dotw < 0) return true;
+ if (t->hour < 0) return true;
+ if (t->min < 0) return true;
+ if (t->sec < 0) return true;
+ return false;
+}
+
+void rtc_set_alarm(datetime_t *t, rtc_callback_t user_callback) {
+ rtc_disable_alarm();
+
+ // Only add to setup if it isn't -1
+ rtc_hw->irq_setup_0 = ((t->year < 0) ? 0 : (((uint)t->year) << RTC_IRQ_SETUP_0_YEAR_LSB )) |
+ ((t->month < 0) ? 0 : (((uint)t->month) << RTC_IRQ_SETUP_0_MONTH_LSB)) |
+ ((t->day < 0) ? 0 : (((uint)t->day) << RTC_IRQ_SETUP_0_DAY_LSB ));
+ rtc_hw->irq_setup_1 = ((t->dotw < 0) ? 0 : (((uint)t->dotw) << RTC_IRQ_SETUP_1_DOTW_LSB)) |
+ ((t->hour < 0) ? 0 : (((uint)t->hour) << RTC_IRQ_SETUP_1_HOUR_LSB)) |
+ ((t->min < 0) ? 0 : (((uint)t->min) << RTC_IRQ_SETUP_1_MIN_LSB )) |
+ ((t->sec < 0) ? 0 : (((uint)t->sec) << RTC_IRQ_SETUP_1_SEC_LSB ));
+
+ // Set the match enable bits for things we care about
+ if (t->year >= 0) hw_set_bits(&rtc_hw->irq_setup_0, RTC_IRQ_SETUP_0_YEAR_ENA_BITS);
+ if (t->month >= 0) hw_set_bits(&rtc_hw->irq_setup_0, RTC_IRQ_SETUP_0_MONTH_ENA_BITS);
+ if (t->day >= 0) hw_set_bits(&rtc_hw->irq_setup_0, RTC_IRQ_SETUP_0_DAY_ENA_BITS);
+ if (t->dotw >= 0) hw_set_bits(&rtc_hw->irq_setup_1, RTC_IRQ_SETUP_1_DOTW_ENA_BITS);
+ if (t->hour >= 0) hw_set_bits(&rtc_hw->irq_setup_1, RTC_IRQ_SETUP_1_HOUR_ENA_BITS);
+ if (t->min >= 0) hw_set_bits(&rtc_hw->irq_setup_1, RTC_IRQ_SETUP_1_MIN_ENA_BITS);
+ if (t->sec >= 0) hw_set_bits(&rtc_hw->irq_setup_1, RTC_IRQ_SETUP_1_SEC_ENA_BITS);
+
+ // Does it repeat? I.e. do we not match on any of the bits
+ _alarm_repeats = rtc_alarm_repeats(t);
+
+ // Store function pointer we can call later
+ _callback = user_callback;
+
+ irq_set_exclusive_handler(RTC_IRQ, rtc_irq_handler);
+
+ // Enable the IRQ at the peri
+ rtc_hw->inte = RTC_INTE_RTC_BITS;
+
+ // Enable the IRQ at the proc
+ irq_set_enabled(RTC_IRQ, true);
+
+ rtc_enable_alarm();
+}
+
+void rtc_disable_alarm(void) {
+ // Disable matching and wait for it to stop being active
+ hw_clear_bits(&rtc_hw->irq_setup_0, RTC_IRQ_SETUP_0_MATCH_ENA_BITS);
+ while(rtc_hw->irq_setup_0 & RTC_IRQ_SETUP_0_MATCH_ACTIVE_BITS) {
+ tight_loop_contents();
+ }
+}
diff --git a/src/rp2_common/hardware_spi/CMakeLists.txt b/src/rp2_common/hardware_spi/CMakeLists.txt
new file mode 100644
index 0000000..03e7f1f
--- /dev/null
+++ b/src/rp2_common/hardware_spi/CMakeLists.txt
@@ -0,0 +1 @@
+pico_simple_hardware_target(spi)
diff --git a/src/rp2_common/hardware_spi/include/hardware/spi.h b/src/rp2_common/hardware_spi/include/hardware/spi.h
new file mode 100644
index 0000000..e8dc952
--- /dev/null
+++ b/src/rp2_common/hardware_spi/include/hardware/spi.h
@@ -0,0 +1,358 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _HARDWARE_SPI_H
+#define _HARDWARE_SPI_H
+
+#include "pico.h"
+#include "pico/time.h"
+#include "hardware/structs/spi.h"
+#include "hardware/regs/dreq.h"
+
+// PICO_CONFIG: PARAM_ASSERTIONS_ENABLED_SPI, Enable/disable assertions in the SPI module, type=bool, default=0, group=hardware_spi
+#ifndef PARAM_ASSERTIONS_ENABLED_SPI
+#define PARAM_ASSERTIONS_ENABLED_SPI 0
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** \file hardware/spi.h
+ * \defgroup hardware_spi hardware_spi
+ *
+ * Hardware SPI API
+ *
+ * RP2040 has 2 identical instances of the Serial Peripheral Interface (SPI) controller.
+ *
+ * The PrimeCell SSP is a master or slave interface for synchronous serial communication with peripheral devices that have
+ * Motorola SPI, National Semiconductor Microwire, or Texas Instruments synchronous serial interfaces.
+ *
+ * Controller can be defined as master or slave using the \ref spi_set_slave function.
+ *
+ * Each controller can be connected to a number of GPIO pins, see the datasheet GPIO function selection table for more information.
+ */
+
+// PICO_CONFIG: PICO_DEFAULT_SPI, Define the default SPI for a board, min=0, max=1, group=hardware_spi
+// PICO_CONFIG: PICO_DEFAULT_SPI_SCK_PIN, Define the default SPI SCK pin, min=0, max=29, group=hardware_spi
+// PICO_CONFIG: PICO_DEFAULT_SPI_TX_PIN, Define the default SPI TX pin, min=0, max=29, group=hardware_spi
+// PICO_CONFIG: PICO_DEFAULT_SPI_RX_PIN, Define the default SPI RX pin, min=0, max=29, group=hardware_spi
+// PICO_CONFIG: PICO_DEFAULT_SPI_CSN_PIN, Define the default SPI CSN pin, min=0, max=29, group=hardware_spi
+
+/**
+ * Opaque type representing an SPI instance.
+ */
+typedef struct spi_inst spi_inst_t;
+
+/** Identifier for the first (SPI 0) hardware SPI instance (for use in SPI functions).
+ *
+ * e.g. spi_init(spi0, 48000)
+ *
+ * \ingroup hardware_spi
+ */
+#define spi0 ((spi_inst_t * const)spi0_hw)
+
+/** Identifier for the second (SPI 1) hardware SPI instance (for use in SPI functions).
+ *
+ * e.g. spi_init(spi1, 48000)
+ *
+ * \ingroup hardware_spi
+ */
+#define spi1 ((spi_inst_t * const)spi1_hw)
+
+#if !defined(PICO_DEFAULT_SPI_INSTANCE) && defined(PICO_DEFAULT_SPI)
+#define PICO_DEFAULT_SPI_INSTANCE (__CONCAT(spi,PICO_DEFAULT_SPI))
+#endif
+
+#ifdef PICO_DEFAULT_SPI_INSTANCE
+#define spi_default PICO_DEFAULT_SPI_INSTANCE
+#endif
+
+/** \brief Enumeration of SPI CPHA (clock phase) values.
+ * \ingroup hardware_spi
+ */
+typedef enum {
+ SPI_CPHA_0 = 0,
+ SPI_CPHA_1 = 1
+} spi_cpha_t;
+
+/** \brief Enumeration of SPI CPOL (clock polarity) values.
+ * \ingroup hardware_spi
+ */
+typedef enum {
+ SPI_CPOL_0 = 0,
+ SPI_CPOL_1 = 1
+} spi_cpol_t;
+
+/** \brief Enumeration of SPI bit-order values.
+ * \ingroup hardware_spi
+ */
+typedef enum {
+ SPI_LSB_FIRST = 0,
+ SPI_MSB_FIRST = 1
+} spi_order_t;
+
+// ----------------------------------------------------------------------------
+// Setup
+
+/*! \brief Initialise SPI instances
+ * \ingroup hardware_spi
+ * Puts the SPI into a known state, and enable it. Must be called before other
+ * functions.
+ *
+ * \note There is no guarantee that the baudrate requested can be achieved exactly; the nearest will be chosen
+ * and returned
+ *
+ * \param spi SPI instance specifier, either \ref spi0 or \ref spi1
+ * \param baudrate Baudrate requested in Hz
+ * \return the actual baud rate set
+ */
+uint spi_init(spi_inst_t *spi, uint baudrate);
+
+/*! \brief Deinitialise SPI instances
+ * \ingroup hardware_spi
+ * Puts the SPI into a disabled state. Init will need to be called to reenable the device
+ * functions.
+ *
+ * \param spi SPI instance specifier, either \ref spi0 or \ref spi1
+ */
+void spi_deinit(spi_inst_t *spi);
+
+/*! \brief Set SPI baudrate
+ * \ingroup hardware_spi
+ *
+ * Set SPI frequency as close as possible to baudrate, and return the actual
+ * achieved rate.
+ *
+ * \param spi SPI instance specifier, either \ref spi0 or \ref spi1
+ * \param baudrate Baudrate required in Hz, should be capable of a bitrate of at least 2Mbps, or higher, depending on system clock settings.
+ * \return The actual baudrate set
+ */
+uint spi_set_baudrate(spi_inst_t *spi, uint baudrate);
+
+/*! \brief Get SPI baudrate
+ * \ingroup hardware_spi
+ *
+ * Get SPI baudrate which was set by \see spi_set_baudrate
+ *
+ * \param spi SPI instance specifier, either \ref spi0 or \ref spi1
+ * \return The actual baudrate set
+ */
+uint spi_get_baudrate(const spi_inst_t *spi);
+
+/*! \brief Convert SPI instance to hardware instance number
+ * \ingroup hardware_spi
+ *
+ * \param spi SPI instance
+ * \return Number of SPI, 0 or 1.
+ */
+static inline uint spi_get_index(const spi_inst_t *spi) {
+ invalid_params_if(SPI, spi != spi0 && spi != spi1);
+ return spi == spi1 ? 1 : 0;
+}
+
+static inline spi_hw_t *spi_get_hw(spi_inst_t *spi) {
+ spi_get_index(spi); // check it is a hw spi
+ return (spi_hw_t *)spi;
+}
+
+static inline const spi_hw_t *spi_get_const_hw(const spi_inst_t *spi) {
+ spi_get_index(spi); // check it is a hw spi
+ return (const spi_hw_t *)spi;
+}
+
+/*! \brief Configure SPI
+ * \ingroup hardware_spi
+ *
+ * Configure how the SPI serialises and deserialises data on the wire
+ *
+ * \param spi SPI instance specifier, either \ref spi0 or \ref spi1
+ * \param data_bits Number of data bits per transfer. Valid values 4..16.
+ * \param cpol SSPCLKOUT polarity, applicable to Motorola SPI frame format only.
+ * \param cpha SSPCLKOUT phase, applicable to Motorola SPI frame format only
+ * \param order Must be SPI_MSB_FIRST, no other values supported on the PL022
+ */
+static inline void spi_set_format(spi_inst_t *spi, uint data_bits, spi_cpol_t cpol, spi_cpha_t cpha, __unused spi_order_t order) {
+ invalid_params_if(SPI, data_bits < 4 || data_bits > 16);
+ // LSB-first not supported on PL022:
+ invalid_params_if(SPI, order != SPI_MSB_FIRST);
+ invalid_params_if(SPI, cpol != SPI_CPOL_0 && cpol != SPI_CPOL_1);
+ invalid_params_if(SPI, cpha != SPI_CPHA_0 && cpha != SPI_CPHA_1);
+ hw_write_masked(&spi_get_hw(spi)->cr0,
+ ((uint)(data_bits - 1)) << SPI_SSPCR0_DSS_LSB |
+ ((uint)cpol) << SPI_SSPCR0_SPO_LSB |
+ ((uint)cpha) << SPI_SSPCR0_SPH_LSB,
+ SPI_SSPCR0_DSS_BITS |
+ SPI_SSPCR0_SPO_BITS |
+ SPI_SSPCR0_SPH_BITS);
+}
+
+/*! \brief Set SPI master/slave
+ * \ingroup hardware_spi
+ *
+ * Configure the SPI for master- or slave-mode operation. By default,
+ * spi_init() sets master-mode.
+ *
+ * \param spi SPI instance specifier, either \ref spi0 or \ref spi1
+ * \param slave true to set SPI device as a slave device, false for master.
+ */
+static inline void spi_set_slave(spi_inst_t *spi, bool slave) {
+ if (slave)
+ hw_set_bits(&spi_get_hw(spi)->cr1, SPI_SSPCR1_MS_BITS);
+ else
+ hw_clear_bits(&spi_get_hw(spi)->cr1, SPI_SSPCR1_MS_BITS);
+}
+
+// ----------------------------------------------------------------------------
+// Generic input/output
+
+/*! \brief Check whether a write can be done on SPI device
+ * \ingroup hardware_spi
+ *
+ * \param spi SPI instance specifier, either \ref spi0 or \ref spi1
+ * \return false if no space is available to write. True if a write is possible
+ */
+static inline bool spi_is_writable(const spi_inst_t *spi) {
+ return (spi_get_const_hw(spi)->sr & SPI_SSPSR_TNF_BITS);
+}
+
+/*! \brief Check whether a read can be done on SPI device
+ * \ingroup hardware_spi
+ *
+ * \param spi SPI instance specifier, either \ref spi0 or \ref spi1
+ * \return true if a read is possible i.e. data is present
+ */
+static inline bool spi_is_readable(const spi_inst_t *spi) {
+ return (spi_get_const_hw(spi)->sr & SPI_SSPSR_RNE_BITS);
+}
+
+/*! \brief Check whether SPI is busy
+ * \ingroup hardware_spi
+ *
+ * \param spi SPI instance specifier, either \ref spi0 or \ref spi1
+ * \return true if SPI is busy
+ */
+static inline bool spi_is_busy(const spi_inst_t *spi) {
+ return (spi_get_const_hw(spi)->sr & SPI_SSPSR_BSY_BITS);
+}
+
+/*! \brief Write/Read to/from an SPI device
+ * \ingroup hardware_spi
+ *
+ * Write \p len bytes from \p src to SPI. Simultaneously read \p len bytes from SPI to \p dst.
+ * Blocks until all data is transferred. No timeout, as SPI hardware always transfers at a known data rate.
+ *
+ * \param spi SPI instance specifier, either \ref spi0 or \ref spi1
+ * \param src Buffer of data to write
+ * \param dst Buffer for read data
+ * \param len Length of BOTH buffers
+ * \return Number of bytes written/read
+*/
+int spi_write_read_blocking(spi_inst_t *spi, const uint8_t *src, uint8_t *dst, size_t len);
+
+/*! \brief Write to an SPI device, blocking
+ * \ingroup hardware_spi
+ *
+ * Write \p len bytes from \p src to SPI, and discard any data received back
+ * Blocks until all data is transferred. No timeout, as SPI hardware always transfers at a known data rate.
+ *
+ * \param spi SPI instance specifier, either \ref spi0 or \ref spi1
+ * \param src Buffer of data to write
+ * \param len Length of \p src
+ * \return Number of bytes written/read
+ */
+int spi_write_blocking(spi_inst_t *spi, const uint8_t *src, size_t len);
+
+/*! \brief Read from an SPI device
+ * \ingroup hardware_spi
+ *
+ * Read \p len bytes from SPI to \p dst.
+ * Blocks until all data is transferred. No timeout, as SPI hardware always transfers at a known data rate.
+ * \p repeated_tx_data is output repeatedly on TX as data is read in from RX.
+ * Generally this can be 0, but some devices require a specific value here,
+ * e.g. SD cards expect 0xff
+ *
+ * \param spi SPI instance specifier, either \ref spi0 or \ref spi1
+ * \param repeated_tx_data Buffer of data to write
+ * \param dst Buffer for read data
+ * \param len Length of buffer \p dst
+ * \return Number of bytes written/read
+ */
+int spi_read_blocking(spi_inst_t *spi, uint8_t repeated_tx_data, uint8_t *dst, size_t len);
+
+// ----------------------------------------------------------------------------
+// SPI-specific operations and aliases
+
+// FIXME need some instance-private data for select() and deselect() if we are going that route
+
+/*! \brief Write/Read half words to/from an SPI device
+ * \ingroup hardware_spi
+ *
+ * Write \p len halfwords from \p src to SPI. Simultaneously read \p len halfwords from SPI to \p dst.
+ * Blocks until all data is transferred. No timeout, as SPI hardware always transfers at a known data rate.
+ *
+ * \note SPI should be initialised with 16 data_bits using \ref spi_set_format first, otherwise this function will only read/write 8 data_bits.
+ *
+ * \param spi SPI instance specifier, either \ref spi0 or \ref spi1
+ * \param src Buffer of data to write
+ * \param dst Buffer for read data
+ * \param len Length of BOTH buffers in halfwords
+ * \return Number of halfwords written/read
+*/
+int spi_write16_read16_blocking(spi_inst_t *spi, const uint16_t *src, uint16_t *dst, size_t len);
+
+/*! \brief Write to an SPI device
+ * \ingroup hardware_spi
+ *
+ * Write \p len halfwords from \p src to SPI. Discard any data received back.
+ * Blocks until all data is transferred. No timeout, as SPI hardware always transfers at a known data rate.
+ *
+ * \note SPI should be initialised with 16 data_bits using \ref spi_set_format first, otherwise this function will only write 8 data_bits.
+ *
+ * \param spi SPI instance specifier, either \ref spi0 or \ref spi1
+ * \param src Buffer of data to write
+ * \param len Length of buffers
+ * \return Number of halfwords written/read
+*/
+int spi_write16_blocking(spi_inst_t *spi, const uint16_t *src, size_t len);
+
+/*! \brief Read from an SPI device
+ * \ingroup hardware_spi
+ *
+ * Read \p len halfwords from SPI to \p dst.
+ * Blocks until all data is transferred. No timeout, as SPI hardware always transfers at a known data rate.
+ * \p repeated_tx_data is output repeatedly on TX as data is read in from RX.
+ * Generally this can be 0, but some devices require a specific value here,
+ * e.g. SD cards expect 0xff
+ *
+ * \note SPI should be initialised with 16 data_bits using \ref spi_set_format first, otherwise this function will only read 8 data_bits.
+ *
+ * \param spi SPI instance specifier, either \ref spi0 or \ref spi1
+ * \param repeated_tx_data Buffer of data to write
+ * \param dst Buffer for read data
+ * \param len Length of buffer \p dst in halfwords
+ * \return Number of halfwords written/read
+ */
+int spi_read16_blocking(spi_inst_t *spi, uint16_t repeated_tx_data, uint16_t *dst, size_t len);
+
+/*! \brief Return the DREQ to use for pacing transfers to/from a particular SPI instance
+ * \ingroup hardware_spi
+ *
+ * \param spi SPI instance specifier, either \ref spi0 or \ref spi1
+ * \param is_tx true for sending data to the SPI instance, false for receiving data from the SPI instance
+ */
+static inline uint spi_get_dreq(spi_inst_t *spi, bool is_tx) {
+ static_assert(DREQ_SPI0_RX == DREQ_SPI0_TX + 1, "");
+ static_assert(DREQ_SPI1_RX == DREQ_SPI1_TX + 1, "");
+ static_assert(DREQ_SPI1_TX == DREQ_SPI0_TX + 2, "");
+ return DREQ_SPI0_TX + spi_get_index(spi) * 2 + !is_tx;
+}
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/rp2_common/hardware_spi/spi.c b/src/rp2_common/hardware_spi/spi.c
new file mode 100644
index 0000000..880b534
--- /dev/null
+++ b/src/rp2_common/hardware_spi/spi.c
@@ -0,0 +1,212 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "hardware/resets.h"
+#include "hardware/clocks.h"
+#include "hardware/spi.h"
+
+static inline void spi_reset(spi_inst_t *spi) {
+ invalid_params_if(SPI, spi != spi0 && spi != spi1);
+ reset_block(spi == spi0 ? RESETS_RESET_SPI0_BITS : RESETS_RESET_SPI1_BITS);
+}
+
+static inline void spi_unreset(spi_inst_t *spi) {
+ invalid_params_if(SPI, spi != spi0 && spi != spi1);
+ unreset_block_wait(spi == spi0 ? RESETS_RESET_SPI0_BITS : RESETS_RESET_SPI1_BITS);
+}
+
+uint spi_init(spi_inst_t *spi, uint baudrate) {
+ spi_reset(spi);
+ spi_unreset(spi);
+
+ uint baud = spi_set_baudrate(spi, baudrate);
+ spi_set_format(spi, 8, SPI_CPOL_0, SPI_CPHA_0, SPI_MSB_FIRST);
+ // Always enable DREQ signals -- harmless if DMA is not listening
+ hw_set_bits(&spi_get_hw(spi)->dmacr, SPI_SSPDMACR_TXDMAE_BITS | SPI_SSPDMACR_RXDMAE_BITS);
+
+ // Finally enable the SPI
+ hw_set_bits(&spi_get_hw(spi)->cr1, SPI_SSPCR1_SSE_BITS);
+ return baud;
+}
+
+void spi_deinit(spi_inst_t *spi) {
+ hw_clear_bits(&spi_get_hw(spi)->cr1, SPI_SSPCR1_SSE_BITS);
+ hw_clear_bits(&spi_get_hw(spi)->dmacr, SPI_SSPDMACR_TXDMAE_BITS | SPI_SSPDMACR_RXDMAE_BITS);
+ spi_reset(spi);
+}
+
+uint spi_set_baudrate(spi_inst_t *spi, uint baudrate) {
+ uint freq_in = clock_get_hz(clk_peri);
+ uint prescale, postdiv;
+ invalid_params_if(SPI, baudrate > freq_in);
+
+ // Find smallest prescale value which puts output frequency in range of
+ // post-divide. Prescale is an even number from 2 to 254 inclusive.
+ for (prescale = 2; prescale <= 254; prescale += 2) {
+ if (freq_in < (prescale + 2) * 256 * (uint64_t) baudrate)
+ break;
+ }
+ invalid_params_if(SPI, prescale > 254); // Frequency too low
+
+ // Find largest post-divide which makes output <= baudrate. Post-divide is
+ // an integer in the range 1 to 256 inclusive.
+ for (postdiv = 256; postdiv > 1; --postdiv) {
+ if (freq_in / (prescale * (postdiv - 1)) > baudrate)
+ break;
+ }
+
+ spi_get_hw(spi)->cpsr = prescale;
+ hw_write_masked(&spi_get_hw(spi)->cr0, (postdiv - 1) << SPI_SSPCR0_SCR_LSB, SPI_SSPCR0_SCR_BITS);
+
+ // Return the frequency we were able to achieve
+ return freq_in / (prescale * postdiv);
+}
+
+uint spi_get_baudrate(const spi_inst_t *spi) {
+ uint prescale = spi_get_const_hw(spi)->cpsr;
+ uint postdiv = ((spi_get_const_hw(spi)->cr0 & SPI_SSPCR0_SCR_BITS) >> SPI_SSPCR0_SCR_LSB) + 1;
+ return clock_get_hz(clk_peri) / (prescale * postdiv);
+}
+
+// Write len bytes from src to SPI. Simultaneously read len bytes from SPI to dst.
+// Note this function is guaranteed to exit in a known amount of time (bits sent * time per bit)
+int __not_in_flash_func(spi_write_read_blocking)(spi_inst_t *spi, const uint8_t *src, uint8_t *dst, size_t len) {
+ invalid_params_if(SPI, 0 > (int)len);
+
+ // Never have more transfers in flight than will fit into the RX FIFO,
+ // else FIFO will overflow if this code is heavily interrupted.
+ const size_t fifo_depth = 8;
+ size_t rx_remaining = len, tx_remaining = len;
+
+ while (rx_remaining || tx_remaining) {
+ if (tx_remaining && spi_is_writable(spi) && rx_remaining < tx_remaining + fifo_depth) {
+ spi_get_hw(spi)->dr = (uint32_t) *src++;
+ --tx_remaining;
+ }
+ if (rx_remaining && spi_is_readable(spi)) {
+ *dst++ = (uint8_t) spi_get_hw(spi)->dr;
+ --rx_remaining;
+ }
+ }
+
+ return (int)len;
+}
+
+// Write len bytes directly from src to the SPI, and discard any data received back
+int __not_in_flash_func(spi_write_blocking)(spi_inst_t *spi, const uint8_t *src, size_t len) {
+ invalid_params_if(SPI, 0 > (int)len);
+ // Write to TX FIFO whilst ignoring RX, then clean up afterward. When RX
+ // is full, PL022 inhibits RX pushes, and sets a sticky flag on
+ // push-on-full, but continues shifting. Safe if SSPIMSC_RORIM is not set.
+ for (size_t i = 0; i < len; ++i) {
+ while (!spi_is_writable(spi))
+ tight_loop_contents();
+ spi_get_hw(spi)->dr = (uint32_t)src[i];
+ }
+ // Drain RX FIFO, then wait for shifting to finish (which may be *after*
+ // TX FIFO drains), then drain RX FIFO again
+ while (spi_is_readable(spi))
+ (void)spi_get_hw(spi)->dr;
+ while (spi_get_hw(spi)->sr & SPI_SSPSR_BSY_BITS)
+ tight_loop_contents();
+ while (spi_is_readable(spi))
+ (void)spi_get_hw(spi)->dr;
+
+ // Don't leave overrun flag set
+ spi_get_hw(spi)->icr = SPI_SSPICR_RORIC_BITS;
+
+ return (int)len;
+}
+
+// Read len bytes directly from the SPI to dst.
+// repeated_tx_data is output repeatedly on SO as data is read in from SI.
+// Generally this can be 0, but some devices require a specific value here,
+// e.g. SD cards expect 0xff
+int __not_in_flash_func(spi_read_blocking)(spi_inst_t *spi, uint8_t repeated_tx_data, uint8_t *dst, size_t len) {
+ invalid_params_if(SPI, 0 > (int)len);
+ const size_t fifo_depth = 8;
+ size_t rx_remaining = len, tx_remaining = len;
+
+ while (rx_remaining || tx_remaining) {
+ if (tx_remaining && spi_is_writable(spi) && rx_remaining < tx_remaining + fifo_depth) {
+ spi_get_hw(spi)->dr = (uint32_t) repeated_tx_data;
+ --tx_remaining;
+ }
+ if (rx_remaining && spi_is_readable(spi)) {
+ *dst++ = (uint8_t) spi_get_hw(spi)->dr;
+ --rx_remaining;
+ }
+ }
+
+ return (int)len;
+}
+
+// Write len halfwords from src to SPI. Simultaneously read len halfwords from SPI to dst.
+int __not_in_flash_func(spi_write16_read16_blocking)(spi_inst_t *spi, const uint16_t *src, uint16_t *dst, size_t len) {
+ invalid_params_if(SPI, 0 > (int)len);
+ // Never have more transfers in flight than will fit into the RX FIFO,
+ // else FIFO will overflow if this code is heavily interrupted.
+ const size_t fifo_depth = 8;
+ size_t rx_remaining = len, tx_remaining = len;
+
+ while (rx_remaining || tx_remaining) {
+ if (tx_remaining && spi_is_writable(spi) && rx_remaining < tx_remaining + fifo_depth) {
+ spi_get_hw(spi)->dr = (uint32_t) *src++;
+ --tx_remaining;
+ }
+ if (rx_remaining && spi_is_readable(spi)) {
+ *dst++ = (uint16_t) spi_get_hw(spi)->dr;
+ --rx_remaining;
+ }
+ }
+
+ return (int)len;
+}
+
+// Write len bytes directly from src to the SPI, and discard any data received back
+int __not_in_flash_func(spi_write16_blocking)(spi_inst_t *spi, const uint16_t *src, size_t len) {
+ invalid_params_if(SPI, 0 > (int)len);
+ // Deliberately overflow FIFO, then clean up afterward, to minimise amount
+ // of APB polling required per halfword
+ for (size_t i = 0; i < len; ++i) {
+ while (!spi_is_writable(spi))
+ tight_loop_contents();
+ spi_get_hw(spi)->dr = (uint32_t)src[i];
+ }
+
+ while (spi_is_readable(spi))
+ (void)spi_get_hw(spi)->dr;
+ while (spi_get_hw(spi)->sr & SPI_SSPSR_BSY_BITS)
+ tight_loop_contents();
+ while (spi_is_readable(spi))
+ (void)spi_get_hw(spi)->dr;
+
+ // Don't leave overrun flag set
+ spi_get_hw(spi)->icr = SPI_SSPICR_RORIC_BITS;
+
+ return (int)len;
+}
+
+// Read len halfwords directly from the SPI to dst.
+// repeated_tx_data is output repeatedly on SO as data is read in from SI.
+int __not_in_flash_func(spi_read16_blocking)(spi_inst_t *spi, uint16_t repeated_tx_data, uint16_t *dst, size_t len) {
+ invalid_params_if(SPI, 0 > (int)len);
+ const size_t fifo_depth = 8;
+ size_t rx_remaining = len, tx_remaining = len;
+
+ while (rx_remaining || tx_remaining) {
+ if (tx_remaining && spi_is_writable(spi) && rx_remaining < tx_remaining + fifo_depth) {
+ spi_get_hw(spi)->dr = (uint32_t) repeated_tx_data;
+ --tx_remaining;
+ }
+ if (rx_remaining && spi_is_readable(spi)) {
+ *dst++ = (uint16_t) spi_get_hw(spi)->dr;
+ --rx_remaining;
+ }
+ }
+
+ return (int)len;
+}
diff --git a/src/rp2_common/hardware_sync/CMakeLists.txt b/src/rp2_common/hardware_sync/CMakeLists.txt
new file mode 100644
index 0000000..1c64ed6
--- /dev/null
+++ b/src/rp2_common/hardware_sync/CMakeLists.txt
@@ -0,0 +1 @@
+pico_simple_hardware_target(sync)
diff --git a/src/rp2_common/hardware_sync/include/hardware/sync.h b/src/rp2_common/hardware_sync/include/hardware/sync.h
new file mode 100644
index 0000000..8f91d55
--- /dev/null
+++ b/src/rp2_common/hardware_sync/include/hardware/sync.h
@@ -0,0 +1,403 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _HARDWARE_SYNC_H
+#define _HARDWARE_SYNC_H
+
+#include "pico.h"
+#include "hardware/address_mapped.h"
+#include "hardware/regs/sio.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** \file hardware/sync.h
+ * \defgroup hardware_sync hardware_sync
+ *
+ * Low level hardware spin locks, barrier and processor event APIs
+ *
+ * Spin Locks
+ * ----------
+ *
+ * The RP2040 provides 32 hardware spin locks, which can be used to manage mutually-exclusive access to shared software
+ * and hardware resources.
+ *
+ * Generally each spin lock itself is a shared resource,
+ * i.e. the same hardware spin lock can be used by multiple higher level primitives (as long as the spin locks are neither held for long periods, nor
+ * held concurrently with other spin locks by the same core - which could lead to deadlock). A hardware spin lock that is exclusively owned can be used
+ * individually without more flexibility and without regard to other software. Note that no hardware spin lock may
+ * be acquired re-entrantly (i.e. hardware spin locks are not on their own safe for use by both thread code and IRQs) however the default spinlock related
+ * methods here (e.g. \ref spin_lock_blocking) always disable interrupts while the lock is held as use by IRQ handlers and user code is common/desirable,
+ * and spin locks are only expected to be held for brief periods.
+ *
+ * The SDK uses the following default spin lock assignments, classifying which spin locks are reserved for exclusive/special purposes
+ * vs those suitable for more general shared use:
+ *
+ * Number (ID) | Description
+ * :---------: | -----------
+ * 0-13 | Currently reserved for exclusive use by the SDK and other libraries. If you use these spin locks, you risk breaking SDK or other library functionality. Each reserved spin lock used individually has its own PICO_SPINLOCK_ID so you can search for those.
+ * 14,15 | (\ref PICO_SPINLOCK_ID_OS1 and \ref PICO_SPINLOCK_ID_OS2). Currently reserved for exclusive use by an operating system (or other system level software) co-existing with the SDK.
+ * 16-23 | (\ref PICO_SPINLOCK_ID_STRIPED_FIRST - \ref PICO_SPINLOCK_ID_STRIPED_LAST). Spin locks from this range are assigned in a round-robin fashion via \ref next_striped_spin_lock_num(). These spin locks are shared, but assigning numbers from a range reduces the probability that two higher level locking primitives using _striped_ spin locks will actually be using the same spin lock.
+ * 24-31 | (\ref PICO_SPINLOCK_ID_CLAIM_FREE_FIRST - \ref PICO_SPINLOCK_ID_CLAIM_FREE_LAST). These are reserved for exclusive use and are allocated on a first come first served basis at runtime via \ref spin_lock_claim_unused()
+ */
+
+// PICO_CONFIG: PARAM_ASSERTIONS_ENABLED_SYNC, Enable/disable assertions in the HW sync module, type=bool, default=0, group=hardware_sync
+#ifndef PARAM_ASSERTIONS_ENABLED_SYNC
+#define PARAM_ASSERTIONS_ENABLED_SYNC 0
+#endif
+
+/** \brief A spin lock identifier
+ * \ingroup hardware_sync
+ */
+typedef volatile uint32_t spin_lock_t;
+
+// PICO_CONFIG: PICO_SPINLOCK_ID_IRQ, Spinlock ID for IRQ protection, min=0, max=31, default=9, group=hardware_sync
+#ifndef PICO_SPINLOCK_ID_IRQ
+#define PICO_SPINLOCK_ID_IRQ 9
+#endif
+
+// PICO_CONFIG: PICO_SPINLOCK_ID_TIMER, Spinlock ID for Timer protection, min=0, max=31, default=10, group=hardware_sync
+#ifndef PICO_SPINLOCK_ID_TIMER
+#define PICO_SPINLOCK_ID_TIMER 10
+#endif
+
+// PICO_CONFIG: PICO_SPINLOCK_ID_HARDWARE_CLAIM, Spinlock ID for Hardware claim protection, min=0, max=31, default=11, group=hardware_sync
+#ifndef PICO_SPINLOCK_ID_HARDWARE_CLAIM
+#define PICO_SPINLOCK_ID_HARDWARE_CLAIM 11
+#endif
+
+// PICO_CONFIG: PICO_SPINLOCK_ID_OS1, First Spinlock ID reserved for use by low level OS style software, min=0, max=31, default=14, group=hardware_sync
+#ifndef PICO_SPINLOCK_ID_OS1
+#define PICO_SPINLOCK_ID_OS1 14
+#endif
+
+// PICO_CONFIG: PICO_SPINLOCK_ID_OS2, Second Spinlock ID reserved for use by low level OS style software, min=0, max=31, default=15, group=hardware_sync
+#ifndef PICO_SPINLOCK_ID_OS2
+#define PICO_SPINLOCK_ID_OS2 15
+#endif
+
+// PICO_CONFIG: PICO_SPINLOCK_ID_STRIPED_FIRST, Lowest Spinlock ID in the 'striped' range, min=0, max=31, default=16, group=hardware_sync
+#ifndef PICO_SPINLOCK_ID_STRIPED_FIRST
+#define PICO_SPINLOCK_ID_STRIPED_FIRST 16
+#endif
+
+// PICO_CONFIG: PICO_SPINLOCK_ID_STRIPED_LAST, Highest Spinlock ID in the 'striped' range, min=0, max=31, default=23, group=hardware_sync
+#ifndef PICO_SPINLOCK_ID_STRIPED_LAST
+#define PICO_SPINLOCK_ID_STRIPED_LAST 23
+#endif
+
+// PICO_CONFIG: PICO_SPINLOCK_ID_CLAIM_FREE_FIRST, Lowest Spinlock ID in the 'claim free' range, min=0, max=31, default=24, group=hardware_sync
+#ifndef PICO_SPINLOCK_ID_CLAIM_FREE_FIRST
+#define PICO_SPINLOCK_ID_CLAIM_FREE_FIRST 24
+#endif
+
+#ifdef PICO_SPINLOCK_ID_CLAIM_FREE_END
+#warning PICO_SPINLOCK_ID_CLAIM_FREE_END has been renamed to PICO_SPINLOCK_ID_CLAIM_FREE_LAST
+#endif
+
+// PICO_CONFIG: PICO_SPINLOCK_ID_CLAIM_FREE_LAST, Highest Spinlock ID in the 'claim free' range, min=0, max=31, default=31, group=hardware_sync
+#ifndef PICO_SPINLOCK_ID_CLAIM_FREE_LAST
+#define PICO_SPINLOCK_ID_CLAIM_FREE_LAST 31
+#endif
+
+/*! \brief Insert a SEV instruction in to the code path.
+ * \ingroup hardware_sync
+
+ * The SEV (send event) instruction sends an event to both cores.
+ */
+__force_inline static void __sev(void) {
+ __asm volatile ("sev");
+}
+
+/*! \brief Insert a WFE instruction in to the code path.
+ * \ingroup hardware_sync
+ *
+ * The WFE (wait for event) instruction waits until one of a number of
+ * events occurs, including events signalled by the SEV instruction on either core.
+ */
+__force_inline static void __wfe(void) {
+ __asm volatile ("wfe");
+}
+
+/*! \brief Insert a WFI instruction in to the code path.
+ * \ingroup hardware_sync
+*
+ * The WFI (wait for interrupt) instruction waits for a interrupt to wake up the core.
+ */
+__force_inline static void __wfi(void) {
+ __asm volatile ("wfi");
+}
+
+/*! \brief Insert a DMB instruction in to the code path.
+ * \ingroup hardware_sync
+ *
+ * The DMB (data memory barrier) acts as a memory barrier, all memory accesses prior to this
+ * instruction will be observed before any explicit access after the instruction.
+ */
+__force_inline static void __dmb(void) {
+ __asm volatile ("dmb" : : : "memory");
+}
+
+/*! \brief Insert a DSB instruction in to the code path.
+ * \ingroup hardware_sync
+ *
+ * The DSB (data synchronization barrier) acts as a special kind of data
+ * memory barrier (DMB). The DSB operation completes when all explicit memory
+ * accesses before this instruction complete.
+ */
+__force_inline static void __dsb(void) {
+ __asm volatile ("dsb" : : : "memory");
+}
+
+/*! \brief Insert a ISB instruction in to the code path.
+ * \ingroup hardware_sync
+ *
+ * ISB acts as an instruction synchronization barrier. It flushes the pipeline of the processor,
+ * so that all instructions following the ISB are fetched from cache or memory again, after
+ * the ISB instruction has been completed.
+ */
+__force_inline static void __isb(void) {
+ __asm volatile ("isb");
+}
+
+/*! \brief Acquire a memory fence
+ * \ingroup hardware_sync
+ */
+__force_inline static void __mem_fence_acquire(void) {
+ // the original code below makes it hard for us to be included from C++ via a header
+ // which itself is in an extern "C", so just use __dmb instead, which is what
+ // is required on Cortex M0+
+ __dmb();
+//#ifndef __cplusplus
+// atomic_thread_fence(memory_order_acquire);
+//#else
+// std::atomic_thread_fence(std::memory_order_acquire);
+//#endif
+}
+
+/*! \brief Release a memory fence
+ * \ingroup hardware_sync
+ *
+ */
+__force_inline static void __mem_fence_release(void) {
+ // the original code below makes it hard for us to be included from C++ via a header
+ // which itself is in an extern "C", so just use __dmb instead, which is what
+ // is required on Cortex M0+
+ __dmb();
+//#ifndef __cplusplus
+// atomic_thread_fence(memory_order_release);
+//#else
+// std::atomic_thread_fence(std::memory_order_release);
+//#endif
+}
+
+/*! \brief Save and disable interrupts
+ * \ingroup hardware_sync
+ *
+ * \return The prior interrupt enable status for restoration later via restore_interrupts()
+ */
+__force_inline static uint32_t save_and_disable_interrupts(void) {
+ uint32_t status;
+ __asm volatile ("mrs %0, PRIMASK" : "=r" (status)::);
+ __asm volatile ("cpsid i");
+ return status;
+}
+
+/*! \brief Restore interrupts to a specified state
+ * \ingroup hardware_sync
+ *
+ * \param status Previous interrupt status from save_and_disable_interrupts()
+ */
+__force_inline static void restore_interrupts(uint32_t status) {
+ __asm volatile ("msr PRIMASK,%0"::"r" (status) : );
+}
+
+/*! \brief Get HW Spinlock instance from number
+ * \ingroup hardware_sync
+ *
+ * \param lock_num Spinlock ID
+ * \return The spinlock instance
+ */
+__force_inline static spin_lock_t *spin_lock_instance(uint lock_num) {
+ invalid_params_if(SYNC, lock_num >= NUM_SPIN_LOCKS);
+ return (spin_lock_t *) (SIO_BASE + SIO_SPINLOCK0_OFFSET + lock_num * 4);
+}
+
+/*! \brief Get HW Spinlock number from instance
+ * \ingroup hardware_sync
+ *
+ * \param lock The Spinlock instance
+ * \return The Spinlock ID
+ */
+__force_inline static uint spin_lock_get_num(spin_lock_t *lock) {
+ invalid_params_if(SYNC, (uint) lock < SIO_BASE + SIO_SPINLOCK0_OFFSET ||
+ (uint) lock >= NUM_SPIN_LOCKS * sizeof(spin_lock_t) + SIO_BASE + SIO_SPINLOCK0_OFFSET ||
+ ((uint) lock - SIO_BASE + SIO_SPINLOCK0_OFFSET) % sizeof(spin_lock_t) != 0);
+ return (uint) (lock - (spin_lock_t *) (SIO_BASE + SIO_SPINLOCK0_OFFSET));
+}
+
+/*! \brief Acquire a spin lock without disabling interrupts (hence unsafe)
+ * \ingroup hardware_sync
+ *
+ * \param lock Spinlock instance
+ */
+__force_inline static void spin_lock_unsafe_blocking(spin_lock_t *lock) {
+ // Note we don't do a wfe or anything, because by convention these spin_locks are VERY SHORT LIVED and NEVER BLOCK and run
+ // with INTERRUPTS disabled (to ensure that)... therefore nothing on our core could be blocking us, so we just need to wait on another core
+ // anyway which should be finished soon
+ while (__builtin_expect(!*lock, 0));
+ __mem_fence_acquire();
+}
+
+/*! \brief Release a spin lock without re-enabling interrupts
+ * \ingroup hardware_sync
+ *
+ * \param lock Spinlock instance
+ */
+__force_inline static void spin_unlock_unsafe(spin_lock_t *lock) {
+ __mem_fence_release();
+ *lock = 0;
+}
+
+/*! \brief Acquire a spin lock safely
+ * \ingroup hardware_sync
+ *
+ * This function will disable interrupts prior to acquiring the spinlock
+ *
+ * \param lock Spinlock instance
+ * \return interrupt status to be used when unlocking, to restore to original state
+ */
+__force_inline static uint32_t spin_lock_blocking(spin_lock_t *lock) {
+ uint32_t save = save_and_disable_interrupts();
+ spin_lock_unsafe_blocking(lock);
+ return save;
+}
+
+/*! \brief Check to see if a spinlock is currently acquired elsewhere.
+ * \ingroup hardware_sync
+ *
+ * \param lock Spinlock instance
+ */
+inline static bool is_spin_locked(spin_lock_t *lock) {
+ check_hw_size(spin_lock_t, 4);
+ uint lock_num = spin_lock_get_num(lock);
+ return 0 != (*(io_ro_32 *) (SIO_BASE + SIO_SPINLOCK_ST_OFFSET) & (1u << lock_num));
+}
+
+/*! \brief Release a spin lock safely
+ * \ingroup hardware_sync
+ *
+ * This function will re-enable interrupts according to the parameters.
+ *
+ * \param lock Spinlock instance
+ * \param saved_irq Return value from the \ref spin_lock_blocking() function.
+ * \return interrupt status to be used when unlocking, to restore to original state
+ *
+ * \sa spin_lock_blocking()
+ */
+__force_inline static void spin_unlock(spin_lock_t *lock, uint32_t saved_irq) {
+ spin_unlock_unsafe(lock);
+ restore_interrupts(saved_irq);
+}
+
+/*! \brief Get the current core number
+ * \ingroup hardware_sync
+ *
+ * \return The core number the call was made from
+ */
+__force_inline static uint get_core_num(void) {
+ return (*(uint32_t *) (SIO_BASE + SIO_CPUID_OFFSET));
+}
+
+/*! \brief Initialise a spin lock
+ * \ingroup hardware_sync
+ *
+ * The spin lock is initially unlocked
+ *
+ * \param lock_num The spin lock number
+ * \return The spin lock instance
+ */
+spin_lock_t *spin_lock_init(uint lock_num);
+
+/*! \brief Release all spin locks
+ * \ingroup hardware_sync
+ */
+void spin_locks_reset(void);
+
+/*! \brief Return a spin lock number from the _striped_ range
+ * \ingroup hardware_sync
+ *
+ * Returns a spin lock number in the range PICO_SPINLOCK_ID_STRIPED_FIRST to PICO_SPINLOCK_ID_STRIPED_LAST
+ * in a round robin fashion. This does not grant the caller exclusive access to the spin lock, so the caller
+ * must:
+ *
+ * -# Abide (with other callers) by the contract of only holding this spin lock briefly (and with IRQs disabled - the default via \ref spin_lock_blocking()),
+ * and not whilst holding other spin locks.
+ * -# Be OK with any contention caused by the - brief due to the above requirement - contention with other possible users of the spin lock.
+ *
+ * \return lock_num a spin lock number the caller may use (non exclusively)
+ * \see PICO_SPINLOCK_ID_STRIPED_FIRST
+ * \see PICO_SPINLOCK_ID_STRIPED_LAST
+ */
+uint next_striped_spin_lock_num(void);
+
+/*! \brief Mark a spin lock as used
+ * \ingroup hardware_sync
+ *
+ * Method for cooperative claiming of hardware. Will cause a panic if the spin lock
+ * is already claimed. Use of this method by libraries detects accidental
+ * configurations that would fail in unpredictable ways.
+ *
+ * \param lock_num the spin lock number
+ */
+void spin_lock_claim(uint lock_num);
+
+/*! \brief Mark multiple spin locks as used
+ * \ingroup hardware_sync
+ *
+ * Method for cooperative claiming of hardware. Will cause a panic if any of the spin locks
+ * are already claimed. Use of this method by libraries detects accidental
+ * configurations that would fail in unpredictable ways.
+ *
+ * \param lock_num_mask Bitfield of all required spin locks to claim (bit 0 == spin lock 0, bit 1 == spin lock 1 etc)
+ */
+void spin_lock_claim_mask(uint32_t lock_num_mask);
+
+/*! \brief Mark a spin lock as no longer used
+ * \ingroup hardware_sync
+ *
+ * Method for cooperative claiming of hardware.
+ *
+ * \param lock_num the spin lock number to release
+ */
+void spin_lock_unclaim(uint lock_num);
+
+/*! \brief Claim a free spin lock
+ * \ingroup hardware_sync
+ *
+ * \param required if true the function will panic if none are available
+ * \return the spin lock number or -1 if required was false, and none were free
+ */
+int spin_lock_claim_unused(bool required);
+
+/*! \brief Determine if a spin lock is claimed
+ * \ingroup hardware_sync
+ *
+ * \param lock_num the spin lock number
+ * \return true if claimed, false otherwise
+ * \see spin_lock_claim
+ * \see spin_lock_claim_mask
+ */
+bool spin_lock_is_claimed(uint lock_num);
+
+#define remove_volatile_cast(t, x) ({__mem_fence_acquire(); (t)(x); })
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/rp2_common/hardware_sync/sync.c b/src/rp2_common/hardware_sync/sync.c
new file mode 100644
index 0000000..a15c8d6
--- /dev/null
+++ b/src/rp2_common/hardware_sync/sync.c
@@ -0,0 +1,64 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "hardware/sync.h"
+#include "hardware/claim.h"
+
+static_assert(PICO_SPINLOCK_ID_STRIPED_LAST >= PICO_SPINLOCK_ID_STRIPED_FIRST, "");
+static uint8_t striped_spin_lock_num = PICO_SPINLOCK_ID_STRIPED_FIRST;
+static uint32_t claimed;
+
+static void check_lock_num(uint __unused lock_num) {
+ invalid_params_if(SYNC, lock_num >= 32);
+}
+
+void spin_locks_reset(void) {
+ for (uint i = 0; i < NUM_SPIN_LOCKS; i++) {
+ spin_unlock_unsafe(spin_lock_instance(i));
+ }
+}
+
+spin_lock_t *spin_lock_init(uint lock_num) {
+ assert(lock_num < NUM_SPIN_LOCKS);
+ spin_lock_t *lock = spin_lock_instance(lock_num);
+ spin_unlock_unsafe(lock);
+ return lock;
+}
+
+uint next_striped_spin_lock_num() {
+ uint rc = striped_spin_lock_num++;
+ if (striped_spin_lock_num > PICO_SPINLOCK_ID_STRIPED_LAST) {
+ striped_spin_lock_num = PICO_SPINLOCK_ID_STRIPED_FIRST;
+ }
+ return rc;
+}
+
+void spin_lock_claim(uint lock_num) {
+ check_lock_num(lock_num);
+ hw_claim_or_assert((uint8_t *) &claimed, lock_num, "Spinlock %d is already claimed");
+}
+
+void spin_lock_claim_mask(uint32_t mask) {
+ for(uint i = 0; mask; i++, mask >>= 1u) {
+ if (mask & 1u) spin_lock_claim(i);
+ }
+}
+
+void spin_lock_unclaim(uint lock_num) {
+ check_lock_num(lock_num);
+ spin_unlock_unsafe(spin_lock_instance(lock_num));
+ hw_claim_clear((uint8_t *) &claimed, lock_num);
+}
+
+int spin_lock_claim_unused(bool required) {
+ return hw_claim_unused_from_range((uint8_t*)&claimed, required, PICO_SPINLOCK_ID_CLAIM_FREE_FIRST, PICO_SPINLOCK_ID_CLAIM_FREE_LAST, "No spinlocks are available");
+}
+
+bool spin_lock_is_claimed(uint lock_num) {
+ check_lock_num(lock_num);
+ return hw_is_claimed((uint8_t *) &claimed, lock_num);
+}
+
diff --git a/src/rp2_common/hardware_timer/CMakeLists.txt b/src/rp2_common/hardware_timer/CMakeLists.txt
new file mode 100644
index 0000000..358f74c
--- /dev/null
+++ b/src/rp2_common/hardware_timer/CMakeLists.txt
@@ -0,0 +1,2 @@
+pico_simple_hardware_target(timer)
+target_link_libraries(hardware_timer INTERFACE hardware_claim)
diff --git a/src/rp2_common/hardware_timer/include/hardware/timer.h b/src/rp2_common/hardware_timer/include/hardware/timer.h
new file mode 100644
index 0000000..1799cd0
--- /dev/null
+++ b/src/rp2_common/hardware_timer/include/hardware/timer.h
@@ -0,0 +1,198 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _HARDWARE_TIMER_H
+#define _HARDWARE_TIMER_H
+
+#include "pico.h"
+#include "hardware/structs/timer.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** \file hardware/timer.h
+ * \defgroup hardware_timer hardware_timer
+ *
+ * Low-level hardware timer API
+ *
+ * This API provides medium level access to the timer HW.
+ * See also \ref pico_time which provides higher levels functionality using the hardware timer.
+ *
+ * The timer peripheral on RP2040 supports the following features:
+ * - single 64-bit counter, incrementing once per microsecond
+ * - Latching two-stage read of counter, for race-free read over 32 bit bus
+ * - Four alarms: match on the lower 32 bits of counter, IRQ on match.
+ *
+ * By default the timer uses a one microsecond reference that is generated in the Watchdog (see Section 4.8.2) which is derived
+ * from the clk_ref.
+ *
+ * The timer has 4 alarms, and can output a separate interrupt for each alarm. The alarms match on the lower 32 bits of the 64
+ * bit counter which means they can be fired a maximum of 2^32 microseconds into the future. This is equivalent to:
+ * - 2^32 ÷ 10^6: ~4295 seconds
+ * - 4295 ÷ 60: ~72 minutes
+ *
+ * The timer is expected to be used for short sleeps, if you want a longer alarm see the \ref hardware_rtc functions.
+ *
+ * \subsection timer_example Example
+ * \addtogroup hardware_timer
+ *
+ * \include hello_timer.c
+ *
+ * \see pico_time
+ */
+
+// PICO_CONFIG: PARAM_ASSERTIONS_ENABLED_TIMER, Enable/disable assertions in the timer module, type=bool, default=0, group=hardware_timer
+#ifndef PARAM_ASSERTIONS_ENABLED_TIMER
+#define PARAM_ASSERTIONS_ENABLED_TIMER 0
+#endif
+
+static inline void check_hardware_alarm_num_param(__unused uint alarm_num) {
+ invalid_params_if(TIMER, alarm_num >= NUM_TIMERS);
+}
+
+/*! \brief Return a 32 bit timestamp value in microseconds
+* \ingroup hardware_timer
+*
+* Returns the low 32 bits of the hardware timer.
+* \note This value wraps roughly every 1 hour 11 minutes and 35 seconds.
+*
+* \return the 32 bit timestamp
+*/
+static inline uint32_t time_us_32(void) {
+ return timer_hw->timerawl;
+}
+
+/*! \brief Return the current 64 bit timestamp value in microseconds
+* \ingroup hardware_timer
+*
+* Returns the full 64 bits of the hardware timer. The \ref pico_time and other functions rely on the fact that this
+* value monotonically increases from power up. As such it is expected that this value counts upwards and never wraps
+* (we apologize for introducing a potential year 5851444 bug).
+*
+* \return the 64 bit timestamp
+*/
+uint64_t time_us_64(void);
+
+/*! \brief Busy wait wasting cycles for the given (32 bit) number of microseconds
+ * \ingroup hardware_timer
+ *
+ * \param delay_us delay amount in microseconds
+ */
+void busy_wait_us_32(uint32_t delay_us);
+
+/*! \brief Busy wait wasting cycles for the given (64 bit) number of microseconds
+ * \ingroup hardware_timer
+ *
+ * \param delay_us delay amount in microseconds
+ */
+void busy_wait_us(uint64_t delay_us);
+
+/*! \brief Busy wait wasting cycles for the given number of milliseconds
+ * \ingroup hardware_timer
+ *
+ * \param delay_ms delay amount in milliseconds
+ */
+void busy_wait_ms(uint32_t delay_ms);
+
+/*! \brief Busy wait wasting cycles until after the specified timestamp
+ * \ingroup hardware_timer
+ *
+ * \param t Absolute time to wait until
+ */
+void busy_wait_until(absolute_time_t t);
+
+/*! \brief Check if the specified timestamp has been reached
+ * \ingroup hardware_timer
+ *
+ * \param t Absolute time to compare against current time
+ * \return true if it is now after the specified timestamp
+ */
+static inline bool time_reached(absolute_time_t t) {
+ uint64_t target = to_us_since_boot(t);
+ uint32_t hi_target = (uint32_t)(target >> 32u);
+ uint32_t hi = timer_hw->timerawh;
+ return (hi >= hi_target && (timer_hw->timerawl >= (uint32_t) target || hi != hi_target));
+}
+
+/*! Callback function type for hardware alarms
+ * \ingroup hardware_timer
+ *
+ * \param alarm_num the hardware alarm number
+ * \sa hardware_alarm_set_callback()
+ */
+typedef void (*hardware_alarm_callback_t)(uint alarm_num);
+
+/*! \brief cooperatively claim the use of this hardware alarm_num
+ * \ingroup hardware_timer
+ *
+ * This method hard asserts if the hardware alarm is currently claimed.
+ *
+ * \param alarm_num the hardware alarm to claim
+ * \sa hardware_claiming
+ */
+void hardware_alarm_claim(uint alarm_num);
+
+/*! \brief cooperatively release the claim on use of this hardware alarm_num
+ * \ingroup hardware_timer
+ *
+ * \param alarm_num the hardware alarm to unclaim
+ * \sa hardware_claiming
+ */
+void hardware_alarm_unclaim(uint alarm_num);
+
+/*! \brief Determine if a hardware alarm has been claimed
+ * \ingroup hardware_timer
+ *
+ * \param alarm_num the hardware alarm number
+ * \return true if claimed, false otherwise
+ * \see hardware_alarm_claim
+ */
+bool hardware_alarm_is_claimed(uint alarm_num);
+
+/*! \brief Enable/Disable a callback for a hardware timer on this core
+ * \ingroup hardware_timer
+ *
+ * This method enables/disables the alarm IRQ for the specified hardware alarm on the
+ * calling core, and set the specified callback to be associated with that alarm.
+ *
+ * This callback will be used for the timeout set via hardware_alarm_set_target
+ *
+ * \note This will install the handler on the current core if the IRQ handler isn't already set.
+ * Therefore the user has the opportunity to call this up from the core of their choice
+ *
+ * \param alarm_num the hardware alarm number
+ * \param callback the callback to install, or NULL to unset
+ *
+ * \sa hardware_alarm_set_target()
+ */
+void hardware_alarm_set_callback(uint alarm_num, hardware_alarm_callback_t callback);
+
+/**
+ * \brief Set the current target for the specified hardware alarm
+ * \ingroup hardware_timer
+ *
+ * This will replace any existing target
+ *
+ * @param alarm_num the hardware alarm number
+ * @param t the target timestamp
+ * @return true if the target was "missed"; i.e. it was in the past, or occurred before a future hardware timeout could be set
+ */
+bool hardware_alarm_set_target(uint alarm_num, absolute_time_t t);
+
+/**
+ * \brief Cancel an existing target (if any) for a given hardware_alarm
+ * \ingroup hardware_timer
+ *
+ * @param alarm_num
+ */
+
+void hardware_alarm_cancel(uint alarm_num);
+
+#ifdef __cplusplus
+}
+#endif
+#endif
diff --git a/src/rp2_common/hardware_timer/timer.c b/src/rp2_common/hardware_timer/timer.c
new file mode 100644
index 0000000..f13d249
--- /dev/null
+++ b/src/rp2_common/hardware_timer/timer.c
@@ -0,0 +1,230 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "hardware/timer.h"
+#include "hardware/irq.h"
+#include "hardware/sync.h"
+#include "hardware/claim.h"
+
+check_hw_layout(timer_hw_t, ints, TIMER_INTS_OFFSET);
+
+static hardware_alarm_callback_t alarm_callbacks[NUM_TIMERS];
+static uint32_t target_hi[NUM_TIMERS];
+static uint8_t timer_callbacks_pending;
+
+static_assert(NUM_TIMERS <= 4, "");
+static uint8_t claimed;
+
+void hardware_alarm_claim(uint alarm_num) {
+ check_hardware_alarm_num_param(alarm_num);
+ hw_claim_or_assert(&claimed, alarm_num, "Hardware alarm %d already claimed");
+}
+
+void hardware_alarm_unclaim(uint alarm_num) {
+ check_hardware_alarm_num_param(alarm_num);
+ hw_claim_clear(&claimed, alarm_num);
+}
+
+bool hardware_alarm_is_claimed(uint alarm_num) {
+ check_hardware_alarm_num_param(alarm_num);
+ return hw_is_claimed(&claimed, alarm_num);
+}
+
+/// tag::time_us_64[]
+uint64_t time_us_64() {
+ // Need to make sure that the upper 32 bits of the timer
+ // don't change, so read that first
+ uint32_t hi = timer_hw->timerawh;
+ uint32_t lo;
+ do {
+ // Read the lower 32 bits
+ lo = timer_hw->timerawl;
+ // Now read the upper 32 bits again and
+ // check that it hasn't incremented. If it has loop around
+ // and read the lower 32 bits again to get an accurate value
+ uint32_t next_hi = timer_hw->timerawh;
+ if (hi == next_hi) break;
+ hi = next_hi;
+ } while (true);
+ return ((uint64_t) hi << 32u) | lo;
+}
+/// end::time_us_64[]
+
+/// \tag::busy_wait[]
+void busy_wait_us_32(uint32_t delay_us) {
+ if (0 <= (int32_t)delay_us) {
+ // we only allow 31 bits, otherwise we could have a race in the loop below with
+ // values very close to 2^32
+ uint32_t start = timer_hw->timerawl;
+ while (timer_hw->timerawl - start < delay_us) {
+ tight_loop_contents();
+ }
+ } else {
+ busy_wait_us(delay_us);
+ }
+}
+
+void busy_wait_us(uint64_t delay_us) {
+ uint64_t base = time_us_64();
+ uint64_t target = base + delay_us;
+ if (target < base) {
+ target = (uint64_t)-1;
+ }
+ absolute_time_t t;
+ update_us_since_boot(&t, target);
+ busy_wait_until(t);
+}
+
+void busy_wait_ms(uint32_t delay_ms)
+{
+ if (delay_ms <= 0x7fffffffu / 1000) {
+ busy_wait_us_32(delay_ms * 1000);
+ } else {
+ busy_wait_us(delay_ms * 1000ull);
+ }
+}
+
+void busy_wait_until(absolute_time_t t) {
+ uint64_t target = to_us_since_boot(t);
+ uint32_t hi_target = (uint32_t)(target >> 32u);
+ uint32_t hi = timer_hw->timerawh;
+ while (hi < hi_target) {
+ hi = timer_hw->timerawh;
+ tight_loop_contents();
+ }
+ while (hi == hi_target && timer_hw->timerawl < (uint32_t) target) {
+ hi = timer_hw->timerawh;
+ tight_loop_contents();
+ }
+}
+/// \end::busy_wait[]
+
+static inline uint harware_alarm_irq_number(uint alarm_num) {
+ return TIMER_IRQ_0 + alarm_num;
+}
+
+static void hardware_alarm_irq_handler(void) {
+ // Determine which timer this IRQ is for
+ uint32_t ipsr;
+ __asm volatile ("mrs %0, ipsr" : "=r" (ipsr)::);
+ uint alarm_num = (ipsr & 0x3fu) - 16 - TIMER_IRQ_0;
+ check_hardware_alarm_num_param(alarm_num);
+
+ hardware_alarm_callback_t callback = NULL;
+
+ spin_lock_t *lock = spin_lock_instance(PICO_SPINLOCK_ID_TIMER);
+ uint32_t save = spin_lock_blocking(lock);
+ // Clear the timer IRQ (inside lock, because we check whether we have handled the IRQ yet in alarm_set by looking at the interrupt status
+ timer_hw->intr = 1u << alarm_num;
+
+ // make sure the IRQ is still valid
+ if (timer_callbacks_pending & (1u << alarm_num)) {
+ // Now check whether we have a timer event to handle that isn't already obsolete (this could happen if we
+ // were already in the IRQ handler before someone else changed the timer setup
+ if (timer_hw->timerawh >= target_hi[alarm_num]) {
+ // we have reached the right high word as well as low word value
+ callback = alarm_callbacks[alarm_num];
+ timer_callbacks_pending &= (uint8_t)~(1u << alarm_num);
+ } else {
+ // try again in 2^32 us
+ timer_hw->alarm[alarm_num] = timer_hw->alarm[alarm_num]; // re-arm the timer
+ }
+ }
+
+ spin_unlock(lock, save);
+
+ if (callback) {
+ callback(alarm_num);
+ }
+}
+
+void hardware_alarm_set_callback(uint alarm_num, hardware_alarm_callback_t callback) {
+ // todo check current core owner
+ // note this should probably be subsumed by irq_set_exclusive_handler anyway, since that
+ // should disallow IRQ handlers on both cores
+ check_hardware_alarm_num_param(alarm_num);
+ uint irq_num = harware_alarm_irq_number(alarm_num);
+ spin_lock_t *lock = spin_lock_instance(PICO_SPINLOCK_ID_TIMER);
+ uint32_t save = spin_lock_blocking(lock);
+ if (callback) {
+ if (hardware_alarm_irq_handler != irq_get_vtable_handler(irq_num)) {
+ // note that set_exclusive will silently allow you to set the handler to the same thing
+ // since it is idempotent, which means we don't need to worry about locking ourselves
+ irq_set_exclusive_handler(irq_num, hardware_alarm_irq_handler);
+ irq_set_enabled(irq_num, true);
+ // Enable interrupt in block and at processor
+ hw_set_bits(&timer_hw->inte, 1u << alarm_num);
+ }
+ alarm_callbacks[alarm_num] = callback;
+ } else {
+ alarm_callbacks[alarm_num] = NULL;
+ timer_callbacks_pending &= (uint8_t)~(1u << alarm_num);
+ irq_remove_handler(irq_num, hardware_alarm_irq_handler);
+ irq_set_enabled(irq_num, false);
+ }
+ spin_unlock(lock, save);
+}
+
+bool hardware_alarm_set_target(uint alarm_num, absolute_time_t target) {
+ bool missed;
+ uint64_t now = time_us_64();
+ uint64_t t = to_us_since_boot(target);
+ if (now >= t) {
+ missed = true;
+ } else {
+ missed = false;
+
+ // 1) actually set the hardware timer
+ spin_lock_t *lock = spin_lock_instance(PICO_SPINLOCK_ID_TIMER);
+ uint32_t save = spin_lock_blocking(lock);
+ uint8_t old_timer_callbacks_pending = timer_callbacks_pending;
+ timer_callbacks_pending |= (uint8_t)(1u << alarm_num);
+ timer_hw->intr = 1u << alarm_num; // clear any IRQ
+ timer_hw->alarm[alarm_num] = (uint32_t) t;
+ // Set the alarm. Writing time should arm it
+ target_hi[alarm_num] = (uint32_t)(t >> 32u);
+
+ // 2) check for races
+ if (!(timer_hw->armed & 1u << alarm_num)) {
+ // not armed, so has already fired .. IRQ must be pending (we are still under lock)
+ assert(timer_hw->ints & 1u << alarm_num);
+ } else {
+ if (time_us_64() >= t) {
+ // we are already at or past the right time; there is no point in us racing against the IRQ
+ // we are about to generate. note however that, if there was already a timer pending before,
+ // then we still let the IRQ fire, as whatever it was, is not handled by our setting missed=true here
+ missed = true;
+ if (timer_callbacks_pending != old_timer_callbacks_pending) {
+ // disarm the timer
+ timer_hw->armed = 1u << alarm_num;
+ // clear the IRQ...
+ timer_hw->intr = 1u << alarm_num;
+ // ... including anything pending on the processor - perhaps unnecessary, but
+ // our timer flag says we aren't expecting anything.
+ irq_clear(harware_alarm_irq_number(alarm_num));
+ // and clear our flag so that if the IRQ handler is already active (because it is on
+ // the other core) it will also skip doing anything
+ timer_callbacks_pending = old_timer_callbacks_pending;
+ }
+ }
+ }
+ spin_unlock(lock, save);
+ // note at this point any pending timer IRQ can likely run
+ }
+ return missed;
+}
+
+void hardware_alarm_cancel(uint alarm_num) {
+ check_hardware_alarm_num_param(alarm_num);
+
+ spin_lock_t *lock = spin_lock_instance(PICO_SPINLOCK_ID_TIMER);
+ uint32_t save = spin_lock_blocking(lock);
+ timer_hw->armed = 1u << alarm_num;
+ timer_callbacks_pending &= (uint8_t)~(1u << alarm_num);
+ spin_unlock(lock, save);
+}
+
+
diff --git a/src/rp2_common/hardware_uart/CMakeLists.txt b/src/rp2_common/hardware_uart/CMakeLists.txt
new file mode 100644
index 0000000..9fe65d5
--- /dev/null
+++ b/src/rp2_common/hardware_uart/CMakeLists.txt
@@ -0,0 +1 @@
+pico_simple_hardware_target(uart)
\ No newline at end of file
diff --git a/src/rp2_common/hardware_uart/include/hardware/uart.h b/src/rp2_common/hardware_uart/include/hardware/uart.h
new file mode 100644
index 0000000..bce0d2f
--- /dev/null
+++ b/src/rp2_common/hardware_uart/include/hardware/uart.h
@@ -0,0 +1,453 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _HARDWARE_UART_H
+#define _HARDWARE_UART_H
+
+#include "pico.h"
+#include "hardware/structs/uart.h"
+#include "hardware/regs/dreq.h"
+
+// PICO_CONFIG: PARAM_ASSERTIONS_ENABLED_UART, Enable/disable assertions in the UART module, type=bool, default=0, group=hardware_uart
+#ifndef PARAM_ASSERTIONS_ENABLED_UART
+#define PARAM_ASSERTIONS_ENABLED_UART 0
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// PICO_CONFIG: PICO_UART_ENABLE_CRLF_SUPPORT, Enable/disable CR/LF translation support, type=bool, default=1, group=hardware_uart
+#ifndef PICO_UART_ENABLE_CRLF_SUPPORT
+#define PICO_UART_ENABLE_CRLF_SUPPORT 1
+#endif
+
+// PICO_CONFIG: PICO_UART_DEFAULT_CRLF, Enable/disable CR/LF translation on UART, type=bool, default=0, depends=PICO_UART_ENABLE_CRLF_SUPPORT, group=hardware_uart
+#ifndef PICO_UART_DEFAULT_CRLF
+#define PICO_UART_DEFAULT_CRLF 0
+#endif
+
+// PICO_CONFIG: PICO_DEFAULT_UART, Define the default UART used for printf etc, min=0, max=1, group=hardware_uart
+// PICO_CONFIG: PICO_DEFAULT_UART_TX_PIN, Define the default UART TX pin, min=0, max=29, group=hardware_uart
+// PICO_CONFIG: PICO_DEFAULT_UART_RX_PIN, Define the default UART RX pin, min=0, max=29, group=hardware_uart
+
+// PICO_CONFIG: PICO_DEFAULT_UART_BAUD_RATE, Define the default UART baudrate, max=921600, default=115200, group=hardware_uart
+#ifndef PICO_DEFAULT_UART_BAUD_RATE
+#define PICO_DEFAULT_UART_BAUD_RATE 115200 ///< Default baud rate
+#endif
+
+/** \file hardware/uart.h
+ * \defgroup hardware_uart hardware_uart
+ *
+ * Hardware UART API
+ *
+ * RP2040 has 2 identical instances of a UART peripheral, based on the ARM PL011. Each UART can be connected to a number
+ * of GPIO pins as defined in the GPIO muxing.
+ *
+ * Only the TX, RX, RTS, and CTS signals are
+ * connected, meaning that the modem mode and IrDA mode of the PL011 are not supported.
+ *
+ * \subsection uart_example Example
+ * \addtogroup hardware_uart
+ *
+ * \code
+ * int main() {
+ *
+ * // Initialise UART 0
+ * uart_init(uart0, 115200);
+ *
+ * // Set the GPIO pin mux to the UART - 0 is TX, 1 is RX
+ * gpio_set_function(0, GPIO_FUNC_UART);
+ * gpio_set_function(1, GPIO_FUNC_UART);
+ *
+ * uart_puts(uart0, "Hello world!");
+ * }
+ * \endcode
+ */
+
+// Currently always a pointer to hw but it might not be in the future
+typedef struct uart_inst uart_inst_t;
+
+/** The UART identifiers for use in UART functions.
+ *
+ * e.g. uart_init(uart1, 48000)
+ *
+ * \ingroup hardware_uart
+ * @{
+ */
+#define uart0 ((uart_inst_t * const)uart0_hw) ///< Identifier for UART instance 0
+#define uart1 ((uart_inst_t * const)uart1_hw) ///< Identifier for UART instance 1
+
+/** @} */
+
+#if !defined(PICO_DEFAULT_UART_INSTANCE) && defined(PICO_DEFAULT_UART)
+#define PICO_DEFAULT_UART_INSTANCE (__CONCAT(uart,PICO_DEFAULT_UART))
+#endif
+
+#ifdef PICO_DEFAULT_UART_INSTANCE
+#define uart_default PICO_DEFAULT_UART_INSTANCE
+#endif
+
+/*! \brief Convert UART instance to hardware instance number
+ * \ingroup hardware_uart
+ *
+ * \param uart UART instance
+ * \return Number of UART, 0 or 1.
+ */
+static inline uint uart_get_index(uart_inst_t *uart) {
+ invalid_params_if(UART, uart != uart0 && uart != uart1);
+ return uart == uart1 ? 1 : 0;
+}
+
+static inline uart_inst_t *uart_get_instance(uint instance) {
+ static_assert(NUM_UARTS == 2, "");
+ invalid_params_if(UART, instance >= NUM_UARTS);
+ return instance ? uart1 : uart0;
+}
+
+static inline uart_hw_t *uart_get_hw(uart_inst_t *uart) {
+ uart_get_index(uart); // check it is a hw uart
+ return (uart_hw_t *)uart;
+}
+
+/** \brief UART Parity enumeration
+ * \ingroup hardware_uart
+ */
+typedef enum {
+ UART_PARITY_NONE,
+ UART_PARITY_EVEN,
+ UART_PARITY_ODD
+} uart_parity_t;
+
+// ----------------------------------------------------------------------------
+// Setup
+
+/*! \brief Initialise a UART
+ * \ingroup hardware_uart
+ *
+ * Put the UART into a known state, and enable it. Must be called before other
+ * functions.
+ *
+ * \note There is no guarantee that the baudrate requested will be possible, the nearest will be chosen,
+ * and this function will return the configured baud rate.
+ *
+ * \param uart UART instance. \ref uart0 or \ref uart1
+ * \param baudrate Baudrate of UART in Hz
+ * \return Actual set baudrate
+ */
+uint uart_init(uart_inst_t *uart, uint baudrate);
+
+/*! \brief DeInitialise a UART
+ * \ingroup hardware_uart
+ *
+ * Disable the UART if it is no longer used. Must be reinitialised before
+ * being used again.
+ *
+ * \param uart UART instance. \ref uart0 or \ref uart1
+ */
+void uart_deinit(uart_inst_t *uart);
+
+/*! \brief Set UART baud rate
+ * \ingroup hardware_uart
+ *
+ * Set baud rate as close as possible to requested, and return actual rate selected.
+ *
+ * \param uart UART instance. \ref uart0 or \ref uart1
+ * \param baudrate Baudrate in Hz
+ * \return Actual set baudrate
+ */
+uint uart_set_baudrate(uart_inst_t *uart, uint baudrate);
+
+/*! \brief Set UART flow control CTS/RTS
+ * \ingroup hardware_uart
+ *
+ * \param uart UART instance. \ref uart0 or \ref uart1
+ * \param cts If true enable flow control of TX by clear-to-send input
+ * \param rts If true enable assertion of request-to-send output by RX flow control
+ */
+static inline void uart_set_hw_flow(uart_inst_t *uart, bool cts, bool rts) {
+ hw_write_masked(&uart_get_hw(uart)->cr,
+ (bool_to_bit(cts) << UART_UARTCR_CTSEN_LSB) | (bool_to_bit(rts) << UART_UARTCR_RTSEN_LSB),
+ UART_UARTCR_RTSEN_BITS | UART_UARTCR_CTSEN_BITS);
+}
+
+/*! \brief Set UART data format
+ * \ingroup hardware_uart
+ *
+ * Configure the data format (bits etc() for the UART
+ *
+ * \param uart UART instance. \ref uart0 or \ref uart1
+ * \param data_bits Number of bits of data. 5..8
+ * \param stop_bits Number of stop bits 1..2
+ * \param parity Parity option.
+ */
+static inline void uart_set_format(uart_inst_t *uart, uint data_bits, uint stop_bits, uart_parity_t parity) {
+ invalid_params_if(UART, data_bits < 5 || data_bits > 8);
+ invalid_params_if(UART, stop_bits != 1 && stop_bits != 2);
+ invalid_params_if(UART, parity != UART_PARITY_NONE && parity != UART_PARITY_EVEN && parity != UART_PARITY_ODD);
+ hw_write_masked(&uart_get_hw(uart)->lcr_h,
+ ((data_bits - 5u) << UART_UARTLCR_H_WLEN_LSB) |
+ ((stop_bits - 1u) << UART_UARTLCR_H_STP2_LSB) |
+ (bool_to_bit(parity != UART_PARITY_NONE) << UART_UARTLCR_H_PEN_LSB) |
+ (bool_to_bit(parity == UART_PARITY_EVEN) << UART_UARTLCR_H_EPS_LSB),
+ UART_UARTLCR_H_WLEN_BITS |
+ UART_UARTLCR_H_STP2_BITS |
+ UART_UARTLCR_H_PEN_BITS |
+ UART_UARTLCR_H_EPS_BITS);
+}
+
+/*! \brief Setup UART interrupts
+ * \ingroup hardware_uart
+ *
+ * Enable the UART's interrupt output. An interrupt handler will need to be installed prior to calling
+ * this function.
+ *
+ * \param uart UART instance. \ref uart0 or \ref uart1
+ * \param rx_has_data If true an interrupt will be fired when the RX FIFO contains data.
+ * \param tx_needs_data If true an interrupt will be fired when the TX FIFO needs data.
+ */
+static inline void uart_set_irq_enables(uart_inst_t *uart, bool rx_has_data, bool tx_needs_data) {
+ // Both UARTRXINTR (RX) and UARTRTINTR (RX timeout) interrupts are
+ // required for rx_has_data. RX asserts when >=4 characters are in the RX
+ // FIFO (for RXIFLSEL=0). RT asserts when there are >=1 characters and no
+ // more have been received for 32 bit periods.
+ uart_get_hw(uart)->imsc = (bool_to_bit(tx_needs_data) << UART_UARTIMSC_TXIM_LSB) |
+ (bool_to_bit(rx_has_data) << UART_UARTIMSC_RXIM_LSB) |
+ (bool_to_bit(rx_has_data) << UART_UARTIMSC_RTIM_LSB);
+ if (rx_has_data) {
+ // Set minimum threshold
+ hw_write_masked(&uart_get_hw(uart)->ifls, 0 << UART_UARTIFLS_RXIFLSEL_LSB,
+ UART_UARTIFLS_RXIFLSEL_BITS);
+ }
+ if (tx_needs_data) {
+ // Set maximum threshold
+ hw_write_masked(&uart_get_hw(uart)->ifls, 0 << UART_UARTIFLS_TXIFLSEL_LSB,
+ UART_UARTIFLS_TXIFLSEL_BITS);
+ }
+}
+
+/*! \brief Test if specific UART is enabled
+ * \ingroup hardware_uart
+ *
+ * \param uart UART instance. \ref uart0 or \ref uart1
+ * \return true if the UART is enabled
+ */
+static inline bool uart_is_enabled(uart_inst_t *uart) {
+ return !!(uart_get_hw(uart)->cr & UART_UARTCR_UARTEN_BITS);
+}
+
+/*! \brief Enable/Disable the FIFOs on specified UART
+ * \ingroup hardware_uart
+ *
+ * \param uart UART instance. \ref uart0 or \ref uart1
+ * \param enabled true to enable FIFO (default), false to disable
+ */
+static inline void uart_set_fifo_enabled(uart_inst_t *uart, bool enabled) {
+ hw_write_masked(&uart_get_hw(uart)->lcr_h,
+ (bool_to_bit(enabled) << UART_UARTLCR_H_FEN_LSB),
+ UART_UARTLCR_H_FEN_BITS);
+}
+
+
+// ----------------------------------------------------------------------------
+// Generic input/output
+
+/*! \brief Determine if space is available in the TX FIFO
+ * \ingroup hardware_uart
+ *
+ * \param uart UART instance. \ref uart0 or \ref uart1
+ * \return false if no space available, true otherwise
+ */
+static inline bool uart_is_writable(uart_inst_t *uart) {
+ return !(uart_get_hw(uart)->fr & UART_UARTFR_TXFF_BITS);
+}
+
+/*! \brief Wait for the UART TX fifo to be drained
+ * \ingroup hardware_uart
+ *
+ * \param uart UART instance. \ref uart0 or \ref uart1
+ */
+static inline void uart_tx_wait_blocking(uart_inst_t *uart) {
+ while (uart_get_hw(uart)->fr & UART_UARTFR_BUSY_BITS) tight_loop_contents();
+}
+
+/*! \brief Determine whether data is waiting in the RX FIFO
+ * \ingroup hardware_uart
+ *
+ * \param uart UART instance. \ref uart0 or \ref uart1
+ * \return 0 if no data available, otherwise the number of bytes, at least, that can be read
+ *
+ * \note HW limitations mean this function will return either 0 or 1.
+ */
+static inline bool uart_is_readable(uart_inst_t *uart) {
+ // PL011 doesn't expose levels directly, so return values are only 0 or 1
+ return !(uart_get_hw(uart)->fr & UART_UARTFR_RXFE_BITS);
+}
+
+/*! \brief Write to the UART for transmission.
+ * \ingroup hardware_uart
+ *
+ * This function will block until all the data has been sent to the UART
+ *
+ * \param uart UART instance. \ref uart0 or \ref uart1
+ * \param src The bytes to send
+ * \param len The number of bytes to send
+ */
+static inline void uart_write_blocking(uart_inst_t *uart, const uint8_t *src, size_t len) {
+ for (size_t i = 0; i < len; ++i) {
+ while (!uart_is_writable(uart))
+ tight_loop_contents();
+ uart_get_hw(uart)->dr = *src++;
+ }
+}
+
+/*! \brief Read from the UART
+ * \ingroup hardware_uart
+ *
+ * This function will block until all the data has been received from the UART
+ *
+ * \param uart UART instance. \ref uart0 or \ref uart1
+ * \param dst Buffer to accept received bytes
+ * \param len The number of bytes to receive.
+ */
+static inline void uart_read_blocking(uart_inst_t *uart, uint8_t *dst, size_t len) {
+ for (size_t i = 0; i < len; ++i) {
+ while (!uart_is_readable(uart))
+ tight_loop_contents();
+ *dst++ = (uint8_t) uart_get_hw(uart)->dr;
+ }
+}
+
+// ----------------------------------------------------------------------------
+// UART-specific operations and aliases
+
+/*! \brief Write single character to UART for transmission.
+ * \ingroup hardware_uart
+ *
+ * This function will block until the entire character has been sent
+ *
+ * \param uart UART instance. \ref uart0 or \ref uart1
+ * \param c The character to send
+ */
+static inline void uart_putc_raw(uart_inst_t *uart, char c) {
+ uart_write_blocking(uart, (const uint8_t *) &c, 1);
+}
+
+/*! \brief Write single character to UART for transmission, with optional CR/LF conversions
+ * \ingroup hardware_uart
+ *
+ * This function will block until the character has been sent
+ *
+ * \param uart UART instance. \ref uart0 or \ref uart1
+ * \param c The character to send
+ */
+static inline void uart_putc(uart_inst_t *uart, char c) {
+#if PICO_UART_ENABLE_CRLF_SUPPORT
+ extern short uart_char_to_line_feed[NUM_UARTS];
+ if (uart_char_to_line_feed[uart_get_index(uart)] == c)
+ uart_putc_raw(uart, '\r');
+#endif
+ uart_putc_raw(uart, c);
+}
+
+/*! \brief Write string to UART for transmission, doing any CR/LF conversions
+ * \ingroup hardware_uart
+ *
+ * This function will block until the entire string has been sent
+ *
+ * \param uart UART instance. \ref uart0 or \ref uart1
+ * \param s The null terminated string to send
+ */
+static inline void uart_puts(uart_inst_t *uart, const char *s) {
+#if PICO_UART_ENABLE_CRLF_SUPPORT
+ bool last_was_cr = false;
+ while (*s) {
+ // Don't add extra carriage returns if one is present
+ if (last_was_cr)
+ uart_putc_raw(uart, *s);
+ else
+ uart_putc(uart, *s);
+ last_was_cr = *s++ == '\r';
+ }
+#else
+ while (*s)
+ uart_putc(uart, *s++);
+#endif
+}
+
+/*! \brief Read a single character to UART
+ * \ingroup hardware_uart
+ *
+ * This function will block until the character has been read
+ *
+ * \param uart UART instance. \ref uart0 or \ref uart1
+ * \return The character read.
+ */
+static inline char uart_getc(uart_inst_t *uart) {
+ char c;
+ uart_read_blocking(uart, (uint8_t *) &c, 1);
+ return c;
+}
+
+/*! \brief Assert a break condition on the UART transmission.
+ * \ingroup hardware_uart
+ *
+ * \param uart UART instance. \ref uart0 or \ref uart1
+ * \param en Assert break condition (TX held low) if true. Clear break condition if false.
+ */
+static inline void uart_set_break(uart_inst_t *uart, bool en) {
+ if (en)
+ hw_set_bits(&uart_get_hw(uart)->lcr_h, UART_UARTLCR_H_BRK_BITS);
+ else
+ hw_clear_bits(&uart_get_hw(uart)->lcr_h, UART_UARTLCR_H_BRK_BITS);
+}
+
+/*! \brief Set CR/LF conversion on UART
+ * \ingroup hardware_uart
+ *
+ * \param uart UART instance. \ref uart0 or \ref uart1
+ * \param translate If true, convert line feeds to carriage return on transmissions
+ */
+void uart_set_translate_crlf(uart_inst_t *uart, bool translate);
+
+/*! \brief Wait for the default UART's TX FIFO to be drained
+ * \ingroup hardware_uart
+ */
+static inline void uart_default_tx_wait_blocking(void) {
+#ifdef uart_default
+ uart_tx_wait_blocking(uart_default);
+#else
+ assert(false);
+#endif
+}
+
+/*! \brief Wait for up to a certain number of microseconds for the RX FIFO to be non empty
+ * \ingroup hardware_uart
+ *
+ * \param uart UART instance. \ref uart0 or \ref uart1
+ * \param us the number of microseconds to wait at most (may be 0 for an instantaneous check)
+ * \return true if the RX FIFO became non empty before the timeout, false otherwise
+ */
+bool uart_is_readable_within_us(uart_inst_t *uart, uint32_t us);
+
+/*! \brief Return the DREQ to use for pacing transfers to/from a particular UART instance
+ * \ingroup hardware_uart
+ *
+ * \param uart UART instance. \ref uart0 or \ref uart1
+ * \param is_tx true for sending data to the UART instance, false for receiving data from the UART instance
+ */
+static inline uint uart_get_dreq(uart_inst_t *uart, bool is_tx) {
+ static_assert(DREQ_UART0_RX == DREQ_UART0_TX + 1, "");
+ static_assert(DREQ_UART1_RX == DREQ_UART1_TX + 1, "");
+ static_assert(DREQ_UART1_TX == DREQ_UART0_TX + 2, "");
+ return DREQ_UART0_TX + uart_get_index(uart) * 2 + !is_tx;
+}
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/rp2_common/hardware_uart/uart.c b/src/rp2_common/hardware_uart/uart.c
new file mode 100644
index 0000000..ba48e87
--- /dev/null
+++ b/src/rp2_common/hardware_uart/uart.c
@@ -0,0 +1,116 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "hardware/address_mapped.h"
+#include "hardware/platform_defs.h"
+#include "hardware/uart.h"
+
+#include "hardware/structs/uart.h"
+#include "hardware/resets.h"
+#include "hardware/clocks.h"
+#include "hardware/timer.h"
+
+#include "pico/assert.h"
+#include "pico.h"
+
+check_hw_layout(uart_hw_t, fr, UART_UARTFR_OFFSET);
+check_hw_layout(uart_hw_t, dmacr, UART_UARTDMACR_OFFSET);
+
+#if PICO_UART_ENABLE_CRLF_SUPPORT
+short uart_char_to_line_feed[NUM_UARTS];
+#endif
+
+/// \tag::uart_reset[]
+static inline void uart_reset(uart_inst_t *uart) {
+ invalid_params_if(UART, uart != uart0 && uart != uart1);
+ reset_block(uart_get_index(uart) ? RESETS_RESET_UART1_BITS : RESETS_RESET_UART0_BITS);
+}
+
+static inline void uart_unreset(uart_inst_t *uart) {
+ invalid_params_if(UART, uart != uart0 && uart != uart1);
+ unreset_block_wait(uart_get_index(uart) ? RESETS_RESET_UART1_BITS : RESETS_RESET_UART0_BITS);
+}
+/// \end::uart_reset[]
+
+/// \tag::uart_init[]
+uint uart_init(uart_inst_t *uart, uint baudrate) {
+ invalid_params_if(UART, uart != uart0 && uart != uart1);
+
+ if (clock_get_hz(clk_peri) == 0)
+ return 0;
+
+ uart_reset(uart);
+ uart_unreset(uart);
+
+#if PICO_UART_ENABLE_CRLF_SUPPORT
+ uart_set_translate_crlf(uart, PICO_UART_DEFAULT_CRLF);
+#endif
+
+ // Any LCR writes need to take place before enabling the UART
+ uint baud = uart_set_baudrate(uart, baudrate);
+ uart_set_format(uart, 8, 1, UART_PARITY_NONE);
+
+ // Enable the UART, both TX and RX
+ uart_get_hw(uart)->cr = UART_UARTCR_UARTEN_BITS | UART_UARTCR_TXE_BITS | UART_UARTCR_RXE_BITS;
+ // Enable FIFOs
+ hw_set_bits(&uart_get_hw(uart)->lcr_h, UART_UARTLCR_H_FEN_BITS);
+ // Always enable DREQ signals -- no harm in this if DMA is not listening
+ uart_get_hw(uart)->dmacr = UART_UARTDMACR_TXDMAE_BITS | UART_UARTDMACR_RXDMAE_BITS;
+
+ return baud;
+}
+/// \end::uart_init[]
+
+void uart_deinit(uart_inst_t *uart) {
+ invalid_params_if(UART, uart != uart0 && uart != uart1);
+ uart_reset(uart);
+}
+
+/// \tag::uart_set_baudrate[]
+uint uart_set_baudrate(uart_inst_t *uart, uint baudrate) {
+ invalid_params_if(UART, baudrate == 0);
+ uint32_t baud_rate_div = (8 * clock_get_hz(clk_peri) / baudrate);
+ uint32_t baud_ibrd = baud_rate_div >> 7;
+ uint32_t baud_fbrd;
+
+ if (baud_ibrd == 0) {
+ baud_ibrd = 1;
+ baud_fbrd = 0;
+ } else if (baud_ibrd >= 65535) {
+ baud_ibrd = 65535;
+ baud_fbrd = 0;
+ } else {
+ baud_fbrd = ((baud_rate_div & 0x7f) + 1) / 2;
+ }
+
+ // Load PL011's baud divisor registers
+ uart_get_hw(uart)->ibrd = baud_ibrd;
+ uart_get_hw(uart)->fbrd = baud_fbrd;
+
+ // PL011 needs a (dummy) line control register write to latch in the
+ // divisors. We don't want to actually change LCR contents here.
+ hw_set_bits(&uart_get_hw(uart)->lcr_h, 0);
+
+ // See datasheet
+ return (4 * clock_get_hz(clk_peri)) / (64 * baud_ibrd + baud_fbrd);
+}
+/// \end::uart_set_baudrate[]
+
+void uart_set_translate_crlf(uart_inst_t *uart, bool crlf) {
+#if PICO_UART_ENABLE_CRLF_SUPPORT
+ uart_char_to_line_feed[uart_get_index(uart)] = crlf ? '\n' : 0x100;
+#else
+ panic_unsupported();
+#endif
+}
+
+bool uart_is_readable_within_us(uart_inst_t *uart, uint32_t us) {
+ uint32_t t = time_us_32();
+ do {
+ if (uart_is_readable(uart)) return true;
+ } while ((time_us_32() - t) <= us);
+ return false;
+}
diff --git a/src/rp2_common/hardware_vreg/CMakeLists.txt b/src/rp2_common/hardware_vreg/CMakeLists.txt
new file mode 100644
index 0000000..9a5b150
--- /dev/null
+++ b/src/rp2_common/hardware_vreg/CMakeLists.txt
@@ -0,0 +1 @@
+pico_simple_hardware_target(vreg)
diff --git a/src/rp2_common/hardware_vreg/include/hardware/vreg.h b/src/rp2_common/hardware_vreg/include/hardware/vreg.h
new file mode 100644
index 0000000..7b4e259
--- /dev/null
+++ b/src/rp2_common/hardware_vreg/include/hardware/vreg.h
@@ -0,0 +1,55 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _HARDWARE_VREG_H_
+#define _HARDWARE_VREG_H_
+
+#include "pico.h"
+#include "hardware/structs/vreg_and_chip_reset.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** \file vreg.h
+ * \defgroup hardware_vreg hardware_vreg
+ *
+ * Voltage Regulation API
+ *
+ */
+
+/** Possible voltage values that can be applied to the regulator
+ */
+enum vreg_voltage {
+ VREG_VOLTAGE_0_85 = 0b0110, ///< 0.85v
+ VREG_VOLTAGE_0_90 = 0b0111, ///< 0.90v
+ VREG_VOLTAGE_0_95 = 0b1000, ///< 0.95v
+ VREG_VOLTAGE_1_00 = 0b1001, ///< 1.00v
+ VREG_VOLTAGE_1_05 = 0b1010, ///< 1.05v
+ VREG_VOLTAGE_1_10 = 0b1011, ///< 1.10v
+ VREG_VOLTAGE_1_15 = 0b1100, ///< 1.15v
+ VREG_VOLTAGE_1_20 = 0b1101, ///< 1.20v
+ VREG_VOLTAGE_1_25 = 0b1110, ///< 1.25v
+ VREG_VOLTAGE_1_30 = 0b1111, ///< 1.30v
+
+ VREG_VOLTAGE_MIN = VREG_VOLTAGE_0_85, ///< Always the minimum possible voltage
+ VREG_VOLTAGE_DEFAULT = VREG_VOLTAGE_1_10, ///< Default voltage on power up.
+ VREG_VOLTAGE_MAX = VREG_VOLTAGE_1_30, ///< Always the maximum possible voltage
+};
+
+
+/*! \brief Set voltage
+ * \ingroup hardware_vreg
+ *
+ * \param voltage The voltage (from enumeration \ref vreg_voltage) to apply to the voltage regulator
+ **/
+void vreg_set_voltage(enum vreg_voltage voltage);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
\ No newline at end of file
diff --git a/src/rp2_common/hardware_vreg/vreg.c b/src/rp2_common/hardware_vreg/vreg.c
new file mode 100644
index 0000000..501b0d4
--- /dev/null
+++ b/src/rp2_common/hardware_vreg/vreg.c
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "pico.h"
+#include "hardware/vreg.h"
+
+void vreg_set_voltage(enum vreg_voltage voltage) {
+ hw_write_masked(&vreg_and_chip_reset_hw->vreg, ((uint)voltage) << VREG_AND_CHIP_RESET_VREG_VSEL_LSB, VREG_AND_CHIP_RESET_VREG_VSEL_BITS);
+}
diff --git a/src/rp2_common/hardware_watchdog/CMakeLists.txt b/src/rp2_common/hardware_watchdog/CMakeLists.txt
new file mode 100644
index 0000000..43a401b
--- /dev/null
+++ b/src/rp2_common/hardware_watchdog/CMakeLists.txt
@@ -0,0 +1 @@
+pico_simple_hardware_target(watchdog)
\ No newline at end of file
diff --git a/src/rp2_common/hardware_watchdog/include/hardware/watchdog.h b/src/rp2_common/hardware_watchdog/include/hardware/watchdog.h
new file mode 100644
index 0000000..747838b
--- /dev/null
+++ b/src/rp2_common/hardware_watchdog/include/hardware/watchdog.h
@@ -0,0 +1,120 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _HARDWARE_WATCHDOG_H
+#define _HARDWARE_WATCHDOG_H
+
+#include "pico.h"
+#include "hardware/structs/watchdog.h"
+
+/** \file hardware/watchdog.h
+ * \defgroup hardware_watchdog hardware_watchdog
+ *
+ * Hardware Watchdog Timer API
+ *
+ * Supporting functions for the Pico hardware watchdog timer.
+ *
+ * The RP2040 has a built in HW watchdog Timer. This is a countdown timer that can restart parts of the chip if it reaches zero.
+ * For example, this can be used to restart the processor if the software running on it gets stuck in an infinite loop
+ * or similar. The programmer has to periodically write a value to the watchdog to stop it reaching zero.
+ *
+ * \subsection watchdog_example Example
+ * \addtogroup hardware_watchdog
+ * \include hello_watchdog.c
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*! \brief Define actions to perform at watchdog timeout
+ * \ingroup hardware_watchdog
+ *
+ * \note If \ref watchdog_start_tick value does not give a 1MHz clock to the watchdog system, then the \p delay_ms
+ * parameter will not be in microseconds. See the datasheet for more details.
+ *
+ * By default the SDK assumes a 12MHz XOSC and sets the \ref watchdog_start_tick appropriately.
+ *
+ * \param pc If Zero, a standard boot will be performed, if non-zero this is the program counter to jump to on reset.
+ * \param sp If \p pc is non-zero, this will be the stack pointer used.
+ * \param delay_ms Initial load value. Maximum value 0x7fffff, approximately 8.3s.
+ */
+void watchdog_reboot(uint32_t pc, uint32_t sp, uint32_t delay_ms);
+
+/*! \brief Start the watchdog tick
+ * \ingroup hardware_watchdog
+ *
+ * \param cycles This needs to be a divider that when applied to the XOSC input, produces a 1MHz clock. So if the XOSC is
+ * 12MHz, this will need to be 12.
+ */
+void watchdog_start_tick(uint cycles);
+
+/*! \brief Reload the watchdog counter with the amount of time set in watchdog_enable
+ * \ingroup hardware_watchdog
+ *
+ */
+void watchdog_update(void);
+
+/**
+ * \brief Enable the watchdog
+ * \ingroup hardware_watchdog
+ *
+ * \note If \ref watchdog_start_tick value does not give a 1MHz clock to the watchdog system, then the \p delay_ms
+ * parameter will not be in microseconds. See the datasheet for more details.
+ *
+ * By default the SDK assumes a 12MHz XOSC and sets the \ref watchdog_start_tick appropriately.
+ *
+ * This method sets a marker in the watchdog scratch register 4 that is checked by \ref watchdog_enable_caused_reboot.
+ * If the device is subsequently reset via a call to watchdog_reboot (including for example by dragging a UF2
+ * onto the RPI-RP2), then this value will be cleared, and so \ref watchdog_enable_caused_reboot will
+ * return false.
+ *
+ * \param delay_ms Number of milliseconds before watchdog will reboot without watchdog_update being called. Maximum of 0x7fffff, which is approximately 8.3 seconds
+ * \param pause_on_debug If the watchdog should be paused when the debugger is stepping through code
+ */
+void watchdog_enable(uint32_t delay_ms, bool pause_on_debug);
+
+/**
+ * \brief Did the watchdog cause the last reboot?
+ * \ingroup hardware_watchdog
+ *
+ * @return true If the watchdog timer or a watchdog force caused the last reboot
+ * @return false If there has been no watchdog reboot since the last power on reset. A power on reset is typically caused by a power cycle or the run pin (reset button) being toggled.
+ */
+bool watchdog_caused_reboot(void);
+
+/**
+ * \brief Did watchdog_enable cause the last reboot?
+ * \ingroup hardware_watchdog
+ *
+ * Perform additional checking along with \ref watchdog_caused_reboot to determine if a watchdog timeout initiated by
+ * \ref watchdog_enable caused the last reboot.
+ *
+ * This method checks for a special value in watchdog scratch register 4 placed there by \ref watchdog_enable.
+ * This would not be present if a watchdog reset is initiated by \ref watchdog_reboot or by the RP2040 bootrom
+ * (e.g. dragging a UF2 onto the RPI-RP2 drive).
+ *
+ * @return true If the watchdog timer or a watchdog force caused (see \reg watchdog_caused_reboot) the last reboot
+ * and the watchdog reboot happened after \ref watchdog_enable was called
+ * @return false If there has been no watchdog reboot since the last power on reset, or the watchdog reboot was not caused
+ * by a watchdog timeout after \ref watchdog_enable was called.
+ * A power on reset is typically caused by a power cycle or the run pin (reset button) being toggled.
+ */
+bool watchdog_enable_caused_reboot(void);
+
+/**
+ * @brief Returns the number of microseconds before the watchdog will reboot the chip.
+ * \ingroup hardware_watchdog
+ *
+ * @return The number of microseconds before the watchdog will reboot the chip.
+ */
+uint32_t watchdog_get_count(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/rp2_common/hardware_watchdog/watchdog.c b/src/rp2_common/hardware_watchdog/watchdog.c
new file mode 100644
index 0000000..9545205
--- /dev/null
+++ b/src/rp2_common/hardware_watchdog/watchdog.c
@@ -0,0 +1,106 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdio.h>
+
+#include "hardware/watchdog.h"
+#include "hardware/structs/watchdog.h"
+#include "hardware/structs/psm.h"
+
+/// \tag::watchdog_start_tick[]
+void watchdog_start_tick(uint cycles) {
+ // Important: This function also provides a tick reference to the timer
+ watchdog_hw->tick = cycles | WATCHDOG_TICK_ENABLE_BITS;
+}
+/// \end::watchdog_start_tick[]
+
+// Value to load when updating the watchdog
+
+// tag::watchdog_update[]
+static uint32_t load_value;
+
+void watchdog_update(void) {
+ watchdog_hw->load = load_value;
+}
+// end::watchdog_update[]
+
+uint32_t watchdog_get_count(void) {
+ return (watchdog_hw->ctrl & WATCHDOG_CTRL_TIME_BITS) / 2 ;
+}
+
+// tag::watchdog_enable[]
+// Helper function used by both watchdog_enable and watchdog_reboot
+void _watchdog_enable(uint32_t delay_ms, bool pause_on_debug) {
+ hw_clear_bits(&watchdog_hw->ctrl, WATCHDOG_CTRL_ENABLE_BITS);
+
+ // Reset everything apart from ROSC and XOSC
+ hw_set_bits(&psm_hw->wdsel, PSM_WDSEL_BITS & ~(PSM_WDSEL_ROSC_BITS | PSM_WDSEL_XOSC_BITS));
+
+ uint32_t dbg_bits = WATCHDOG_CTRL_PAUSE_DBG0_BITS |
+ WATCHDOG_CTRL_PAUSE_DBG1_BITS |
+ WATCHDOG_CTRL_PAUSE_JTAG_BITS;
+
+ if (pause_on_debug) {
+ hw_set_bits(&watchdog_hw->ctrl, dbg_bits);
+ } else {
+ hw_clear_bits(&watchdog_hw->ctrl, dbg_bits);
+ }
+
+ if (!delay_ms) {
+ hw_set_bits(&watchdog_hw->ctrl, WATCHDOG_CTRL_TRIGGER_BITS);
+ } else {
+ // Note, we have x2 here as the watchdog HW currently decrements twice per tick
+ load_value = delay_ms * 1000 * 2;
+
+ if (load_value > 0xffffffu)
+ load_value = 0xffffffu;
+
+ watchdog_update();
+
+ hw_set_bits(&watchdog_hw->ctrl, WATCHDOG_CTRL_ENABLE_BITS);
+ }
+}
+// end::watchdog_enable[]
+
+#define WATCHDOG_NON_REBOOT_MAGIC 0x6ab73121
+
+void watchdog_enable(uint32_t delay_ms, bool pause_on_debug) {
+ // update scratch[4] to distinguish from magic used for reboot to specific address, or 0 used to reboot
+ // into regular flash path
+ watchdog_hw->scratch[4] = WATCHDOG_NON_REBOOT_MAGIC;
+ _watchdog_enable(delay_ms, pause_on_debug);
+}
+
+void watchdog_reboot(uint32_t pc, uint32_t sp, uint32_t delay_ms) {
+ check_hw_layout(watchdog_hw_t, scratch[7], WATCHDOG_SCRATCH7_OFFSET);
+
+ // Clear enable before setting up scratch registers
+ hw_clear_bits(&watchdog_hw->ctrl, WATCHDOG_CTRL_ENABLE_BITS);
+
+ if (pc) {
+ pc |= 1u; // thumb mode
+ watchdog_hw->scratch[4] = 0xb007c0d3;
+ watchdog_hw->scratch[5] = pc ^ -0xb007c0d3;
+ watchdog_hw->scratch[6] = sp;
+ watchdog_hw->scratch[7] = pc;
+// printf("rebooting %08x/%08x in %dms...\n", (uint) pc, (uint) sp, (uint) delay_ms);
+ } else {
+ watchdog_hw->scratch[4] = 0;
+// printf("rebooting (regular)) in %dms...\n", (uint) delay_ms);
+ }
+
+ // Don't pause watchdog for debug
+ _watchdog_enable(delay_ms, 0);
+}
+
+bool watchdog_caused_reboot(void) {
+ // If any reason bits are set this is true
+ return watchdog_hw->reason;
+}
+
+bool watchdog_enable_caused_reboot(void) {
+ return watchdog_hw->reason && watchdog_hw->scratch[4] == WATCHDOG_NON_REBOOT_MAGIC;
+}
\ No newline at end of file
diff --git a/src/rp2_common/hardware_xosc/CMakeLists.txt b/src/rp2_common/hardware_xosc/CMakeLists.txt
new file mode 100644
index 0000000..50e86c2
--- /dev/null
+++ b/src/rp2_common/hardware_xosc/CMakeLists.txt
@@ -0,0 +1 @@
+pico_simple_hardware_target(xosc)
\ No newline at end of file
diff --git a/src/rp2_common/hardware_xosc/include/hardware/xosc.h b/src/rp2_common/hardware_xosc/include/hardware/xosc.h
new file mode 100644
index 0000000..a5e33b0
--- /dev/null
+++ b/src/rp2_common/hardware_xosc/include/hardware/xosc.h
@@ -0,0 +1,59 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _HARDWARE_XOSC_H_
+#define _HARDWARE_XOSC_H_
+
+#include "pico.h"
+#include "hardware/structs/xosc.h"
+
+
+// Allow lengthening startup delay to accommodate slow-starting oscillators
+
+// PICO_CONFIG: PICO_XOSC_STARTUP_DELAY_MULTIPLIER, Multiplier to lengthen xosc startup delay to accommodate slow-starting oscillators, type=int, min=1, default=1, group=hardware_xosc
+#ifndef PICO_XOSC_STARTUP_DELAY_MULTIPLIER
+#define PICO_XOSC_STARTUP_DELAY_MULTIPLIER 1
+#endif
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** \file hardware/xosc.h
+ * \defgroup hardware_xosc hardware_xosc
+ *
+ * Crystal Oscillator (XOSC) API
+ */
+
+/*! \brief Initialise the crystal oscillator system
+ * \ingroup hardware_xosc
+ *
+ * This function will block until the crystal oscillator has stabilised.
+ **/
+void xosc_init(void);
+
+/*! \brief Disable the Crystal oscillator
+ * \ingroup hardware_xosc
+ *
+ * Turns off the crystal oscillator source, and waits for it to become unstable
+ **/
+void xosc_disable(void);
+
+/*! \brief Set the crystal oscillator system to dormant
+ * \ingroup hardware_xosc
+ *
+ * Turns off the crystal oscillator until it is woken by an interrupt. This will block and hence
+ * the entire system will stop, until an interrupt wakes it up. This function will
+ * continue to block until the oscillator becomes stable after its wakeup.
+ **/
+void xosc_dormant(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/rp2_common/hardware_xosc/xosc.c b/src/rp2_common/hardware_xosc/xosc.c
new file mode 100644
index 0000000..7a7055c
--- /dev/null
+++ b/src/rp2_common/hardware_xosc/xosc.c
@@ -0,0 +1,55 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "pico.h"
+
+// For MHZ definitions etc
+#include "hardware/clocks.h"
+
+#include "hardware/platform_defs.h"
+#include "hardware/regs/xosc.h"
+#include "hardware/xosc.h"
+
+#if XOSC_MHZ < 1 || XOSC_MHZ > 50
+#error XOSC_MHZ must be in the range 1-50
+#endif
+
+#define STARTUP_DELAY (((((XOSC_MHZ * MHZ) / 1000) + 128) / 256) * PICO_XOSC_STARTUP_DELAY_MULTIPLIER)
+
+// The DELAY field in xosc_hw->startup is 14 bits wide.
+#if STARTUP_DELAY >= (1 << 13)
+#error PICO_XOSC_STARTUP_DELAY_MULTIPLIER is too large: XOSC STARTUP.DELAY must be < 8192
+#endif
+
+void xosc_init(void) {
+ // Assumes 1-15 MHz input, checked above.
+ xosc_hw->ctrl = XOSC_CTRL_FREQ_RANGE_VALUE_1_15MHZ;
+
+ // Set xosc startup delay
+ xosc_hw->startup = STARTUP_DELAY;
+
+ // Set the enable bit now that we have set freq range and startup delay
+ hw_set_bits(&xosc_hw->ctrl, XOSC_CTRL_ENABLE_VALUE_ENABLE << XOSC_CTRL_ENABLE_LSB);
+
+ // Wait for XOSC to be stable
+ while(!(xosc_hw->status & XOSC_STATUS_STABLE_BITS));
+}
+
+void xosc_disable(void) {
+ uint32_t tmp = xosc_hw->ctrl;
+ tmp &= (~XOSC_CTRL_ENABLE_BITS);
+ tmp |= (XOSC_CTRL_ENABLE_VALUE_DISABLE << XOSC_CTRL_ENABLE_LSB);
+ xosc_hw->ctrl = tmp;
+ // Wait for stable to go away
+ while(xosc_hw->status & XOSC_STATUS_STABLE_BITS);
+}
+
+void xosc_dormant(void) {
+ // WARNING: This stops the xosc until woken up by an irq
+ xosc_hw->dormant = XOSC_DORMANT_VALUE_DORMANT;
+ // Wait for it to become stable once woken up
+ while(!(xosc_hw->status & XOSC_STATUS_STABLE_BITS));
+}
diff --git a/src/rp2_common/pico_bit_ops/CMakeLists.txt b/src/rp2_common/pico_bit_ops/CMakeLists.txt
new file mode 100644
index 0000000..962c307
--- /dev/null
+++ b/src/rp2_common/pico_bit_ops/CMakeLists.txt
@@ -0,0 +1,48 @@
+if (NOT TARGET pico_bit_ops)
+ #shims for ROM functions for -lgcc functions (listed below)
+ pico_add_impl_library(pico_bit_ops)
+
+ # no custom implementation; falls thru to compiler
+ pico_add_impl_library(pico_bit_ops_compiler)
+
+ # add alias "default" which is just pico.
+ add_library(pico_bit_ops_default INTERFACE)
+ target_link_libraries(pico_bit_ops_default INTERFACE pico_bit_ops_pico)
+
+ set(PICO_DEFAULT_BIT_OPS_IMPL pico_bit_ops_default)
+
+ pico_add_impl_library(pico_bit_ops_pico)
+ target_link_libraries(pico_bit_ops INTERFACE
+ $<IF:$<BOOL:$<TARGET_PROPERTY:PICO_TARGET_BIT_OPS_IMPL>>,$<TARGET_PROPERTY:PICO_TARGET_BIT_OPS_IMPL>,${PICO_DEFAULT_BIT_OPS_IMPL}>)
+
+ target_sources(pico_bit_ops_pico INTERFACE
+ ${CMAKE_CURRENT_LIST_DIR}/bit_ops_aeabi.S
+ )
+
+ target_link_libraries(pico_bit_ops_pico INTERFACE pico_bootrom pico_bit_ops_headers)
+
+ # gcc
+ pico_wrap_function(pico_bit_ops_pico __clzsi2)
+ pico_wrap_function(pico_bit_ops_pico __clzsi2)
+ pico_wrap_function(pico_bit_ops_pico __clzdi2)
+ pico_wrap_function(pico_bit_ops_pico __ctzsi2)
+ pico_wrap_function(pico_bit_ops_pico __ctzdi2)
+ pico_wrap_function(pico_bit_ops_pico __popcountsi2)
+ pico_wrap_function(pico_bit_ops_pico __popcountdi2)
+
+ # armclang
+ pico_wrap_function(pico_bit_ops_pico __clz)
+ pico_wrap_function(pico_bit_ops_pico __clzl)
+ pico_wrap_function(pico_bit_ops_pico __clzsi2)
+ pico_wrap_function(pico_bit_ops_pico __clzll)
+
+ macro(pico_set_bit_ops_implementation TARGET IMPL)
+ get_target_property(target_type ${TARGET} TYPE)
+ if ("EXECUTABLE" STREQUAL "${target_type}")
+ set_target_properties(${TARGET} PROPERTIES PICO_TARGET_BIT_OPS_IMPL "pico_bit_ops_${IMPL}")
+ else()
+ message(FATAL_ERROR "bit_ops implementation must be set on executable not library")
+ endif()
+ endmacro()
+
+endif()
diff --git a/src/rp2_common/pico_bit_ops/bit_ops_aeabi.S b/src/rp2_common/pico_bit_ops/bit_ops_aeabi.S
new file mode 100644
index 0000000..eba839e
--- /dev/null
+++ b/src/rp2_common/pico_bit_ops/bit_ops_aeabi.S
@@ -0,0 +1,134 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+.syntax unified
+.cpu cortex-m0plus
+.thumb
+
+#include "pico/asm_helper.S"
+#include "pico/bootrom.h"
+
+__pre_init __aeabi_bits_init, 00010
+
+.macro bits_section name
+#if PICO_BITS_IN_RAM
+.section RAM_SECTION_NAME(\name), "ax"
+#else
+.section SECTION_NAME(\name), "ax"
+#endif
+.endm
+
+.section .data.aeabi_bits_funcs
+.global aeabi_bits_funcs, aeabi_bits_funcs_end
+.equ BITS_FUNC_COUNT, 4
+.align 4
+aeabi_bits_funcs:
+ .word ROM_FUNC_POPCOUNT32
+ .word ROM_FUNC_CLZ32
+ .word ROM_FUNC_CTZ32
+ .word ROM_FUNC_REVERSE32
+aeabi_bits_funcs_end:
+
+.section .text
+.thumb_func
+__aeabi_bits_init:
+ ldr r0, =aeabi_bits_funcs
+ movs r1, #BITS_FUNC_COUNT
+ ldr r3, =rom_funcs_lookup
+ bx r3
+
+.equ POPCOUNT32, 0
+.equ CLZ32, 4
+.equ CTZ32, 8
+.equ REVERSE32, 12
+
+bits_section clzsi
+wrapper_func __clz
+wrapper_func __clzl
+wrapper_func __clzsi2
+ ldr r3, =aeabi_bits_funcs
+ ldr r3, [r3, #CLZ32]
+ bx r3
+
+bits_section ctzsi
+wrapper_func __ctzsi2
+ ldr r3, =aeabi_bits_funcs
+ ldr r3, [r3, #CTZ32]
+ bx r3
+
+bits_section popcountsi
+wrapper_func __popcountsi2
+ ldr r3, =aeabi_bits_funcs
+ ldr r3, [r3, #POPCOUNT32]
+ bx r3
+
+bits_section clzdi
+wrapper_func __clzll
+wrapper_func __clzdi2
+ ldr r3, =aeabi_bits_funcs
+ ldr r3, [r3, #CLZ32]
+ cmp r1, #0
+ bne 1f
+ push {lr}
+ blx r3
+ adds r0, #32
+ pop {pc}
+1:
+ mov r0, r1
+ bx r3
+
+bits_section ctzdi
+wrapper_func __ctzdi2
+ ldr r3, =aeabi_bits_funcs
+ ldr r3, [r3, #CTZ32]
+ cmp r0, #0
+ bne 1f
+ bx r3
+1:
+ push {lr}
+ mov r0, r1
+ blx r3
+ adds r0, #32
+ pop {pc}
+
+bits_section popcountdi
+wrapper_func __popcountdi2
+ ldr r3, =aeabi_bits_funcs
+ ldr r3, [r3, #POPCOUNT32]
+ push {r1, r3, lr}
+ blx r3
+ mov ip, r0
+ pop {r0, r3}
+ blx r3
+ mov r1, ip
+ add r0, r1
+ pop {pc}
+
+bits_section reverse32
+regular_func reverse32
+ ldr r3, =aeabi_bits_funcs
+ ldr r3, [r3, #REVERSE32]
+ bx r3
+
+bits_section __rev
+regular_func __rev
+regular_func __revl
+ ldr r3, =aeabi_bits_funcs
+ ldr r3, [r3, #REVERSE32]
+ bx r3
+
+bits_section __revll
+regular_func __revll
+ push {lr}
+ ldr r3, =aeabi_bits_funcs
+ ldr r3, [r3, #REVERSE32]
+ push {r1, r3}
+ blx r3
+ mov ip, r0 // reverse32 preserves ip
+ pop {r0, r3}
+ blx r3
+ mov r1, ip
+ pop {pc}
diff --git a/src/rp2_common/pico_bootrom/CMakeLists.txt b/src/rp2_common/pico_bootrom/CMakeLists.txt
new file mode 100644
index 0000000..58ea575
--- /dev/null
+++ b/src/rp2_common/pico_bootrom/CMakeLists.txt
@@ -0,0 +1,8 @@
+add_library(pico_bootrom INTERFACE)
+
+target_sources(pico_bootrom INTERFACE
+ ${CMAKE_CURRENT_LIST_DIR}/bootrom.c
+ )
+
+target_include_directories(pico_bootrom INTERFACE ${CMAKE_CURRENT_LIST_DIR}/include)
+target_link_libraries(pico_bootrom INTERFACE pico_base_headers)
diff --git a/src/rp2_common/pico_bootrom/bootrom.c b/src/rp2_common/pico_bootrom/bootrom.c
new file mode 100644
index 0000000..af7d091
--- /dev/null
+++ b/src/rp2_common/pico_bootrom/bootrom.c
@@ -0,0 +1,36 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "pico/bootrom.h"
+
+/// \tag::table_lookup[]
+
+// Bootrom function: rom_table_lookup
+// Returns the 32 bit pointer into the ROM if found or NULL otherwise.
+typedef void *(*rom_table_lookup_fn)(uint16_t *table, uint32_t code);
+
+// Convert a 16 bit pointer stored at the given rom address into a 32 bit pointer
+#define rom_hword_as_ptr(rom_address) (void *)(uintptr_t)(*(uint16_t *)rom_address)
+
+void *rom_func_lookup(uint32_t code) {
+ return rom_func_lookup_inline(code);
+}
+
+void *rom_data_lookup(uint32_t code) {
+ rom_table_lookup_fn rom_table_lookup = (rom_table_lookup_fn) rom_hword_as_ptr(0x18);
+ uint16_t *data_table = (uint16_t *) rom_hword_as_ptr(0x16);
+ return rom_table_lookup(data_table, code);
+}
+/// \end::table_lookup[]
+
+bool rom_funcs_lookup(uint32_t *table, unsigned int count) {
+ bool ok = true;
+ for (unsigned int i = 0; i < count; i++) {
+ table[i] = (uintptr_t) rom_func_lookup(table[i]);
+ if (!table[i]) ok = false;
+ }
+ return ok;
+}
diff --git a/src/rp2_common/pico_bootrom/include/pico/bootrom.h b/src/rp2_common/pico_bootrom/include/pico/bootrom.h
new file mode 100644
index 0000000..e557893
--- /dev/null
+++ b/src/rp2_common/pico_bootrom/include/pico/bootrom.h
@@ -0,0 +1,162 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PLATFORM_BOOTROM_H
+#define _PLATFORM_BOOTROM_H
+
+#include "pico.h"
+
+/** \file bootrom.h
+ * \defgroup pico_bootrom pico_bootrom
+ * Access to functions and data in the RP2040 bootrom
+ *
+ * This header may be included by assembly code
+ */
+
+// ROM FUNCTIONS
+
+#define ROM_FUNC_POPCOUNT32 ROM_TABLE_CODE('P', '3')
+#define ROM_FUNC_REVERSE32 ROM_TABLE_CODE('R', '3')
+#define ROM_FUNC_CLZ32 ROM_TABLE_CODE('L', '3')
+#define ROM_FUNC_CTZ32 ROM_TABLE_CODE('T', '3')
+#define ROM_FUNC_MEMSET ROM_TABLE_CODE('M', 'S')
+#define ROM_FUNC_MEMSET4 ROM_TABLE_CODE('S', '4')
+#define ROM_FUNC_MEMCPY ROM_TABLE_CODE('M', 'C')
+#define ROM_FUNC_MEMCPY44 ROM_TABLE_CODE('C', '4')
+#define ROM_FUNC_RESET_USB_BOOT ROM_TABLE_CODE('U', 'B')
+#define ROM_FUNC_CONNECT_INTERNAL_FLASH ROM_TABLE_CODE('I', 'F')
+#define ROM_FUNC_FLASH_EXIT_XIP ROM_TABLE_CODE('E', 'X')
+#define ROM_FUNC_FLASH_RANGE_ERASE ROM_TABLE_CODE('R', 'E')
+#define ROM_FUNC_FLASH_RANGE_PROGRAM ROM_TABLE_CODE('R', 'P')
+#define ROM_FUNC_FLASH_FLUSH_CACHE ROM_TABLE_CODE('F', 'C')
+#define ROM_FUNC_FLASH_ENTER_CMD_XIP ROM_TABLE_CODE('C', 'X')
+
+/*! \brief Return a bootrom lookup code based on two ASCII characters
+ * \ingroup pico_bootrom
+ *
+ * These codes are uses to lookup data or function addresses in the bootrom
+ *
+ * \param c1 the first character
+ * \param c2 the second character
+ * \return the 'code' to use in rom_func_lookup() or rom_data_lookup()
+ */
+#define ROM_TABLE_CODE(c1, c2) ((c1) | ((c2) << 8))
+
+#ifndef __ASSEMBLER__
+
+// ROM FUNCTION SIGNATURES
+
+typedef uint32_t (*rom_popcount32_fn)(uint32_t);
+typedef uint32_t (*rom_reverse32_fn)(uint32_t);
+typedef uint32_t (*rom_clz32_fn)(uint32_t);
+typedef uint32_t (*rom_ctz32_fn)(uint32_t);
+typedef uint8_t *(*rom_memset_fn)(uint8_t *, uint8_t, uint32_t);
+typedef uint32_t *(*rom_memset4_fn)(uint32_t *, uint8_t, uint32_t);
+typedef uint32_t *(*rom_memcpy_fn)(uint8_t *, const uint8_t *, uint32_t);
+typedef uint32_t *(*rom_memcpy44_fn)(uint32_t *, const uint32_t *, uint32_t);
+typedef void __attribute__((noreturn)) (*rom_reset_usb_boot_fn)(uint32_t, uint32_t);
+typedef rom_reset_usb_boot_fn reset_usb_boot_fn; // kept for backwards compatibility
+typedef void (*rom_connect_internal_flash_fn)(void);
+typedef void (*rom_flash_exit_xip_fn)(void);
+typedef void (*rom_flash_range_erase_fn)(uint32_t, size_t, uint32_t, uint8_t);
+typedef void (*rom_flash_range_program_fn)(uint32_t, const uint8_t*, size_t);
+typedef void (*rom_flash_flush_cache_fn)(void);
+typedef void (*rom_flash_enter_cmd_xip_fn)(void);
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*! \brief Return a bootrom lookup code based on two ASCII characters
+ * \ingroup pico_bootrom
+ *
+ * These codes are uses to lookup data or function addresses in the bootrom
+ *
+ * \param c1 the first character
+ * \param c2 the second character
+ * \return the 'code' to use in rom_func_lookup() or rom_data_lookup()
+ */
+static inline uint32_t rom_table_code(uint8_t c1, uint8_t c2) {
+ return ROM_TABLE_CODE((uint32_t) c1, (uint32_t) c2);
+}
+
+/*!
+ * \brief Lookup a bootrom function by code
+ * \ingroup pico_bootrom
+ * \param code the code
+ * \return a pointer to the function, or NULL if the code does not match any bootrom function
+ */
+void *rom_func_lookup(uint32_t code);
+
+/*!
+ * \brief Lookup a bootrom address by code
+ * \ingroup pico_bootrom
+ * \param code the code
+ * \return a pointer to the data, or NULL if the code does not match any bootrom function
+ */
+void *rom_data_lookup(uint32_t code);
+
+/*!
+ * \brief Helper function to lookup the addresses of multiple bootrom functions
+ * \ingroup pico_bootrom
+ *
+ * This method looks up the 'codes' in the table, and convert each table entry to the looked up
+ * function pointer, if there is a function for that code in the bootrom.
+ *
+ * \param table an IN/OUT array, elements are codes on input, function pointers on success.
+ * \param count the number of elements in the table
+ * \return true if all the codes were found, and converted to function pointers, false otherwise
+ */
+bool rom_funcs_lookup(uint32_t *table, unsigned int count);
+
+// Bootrom function: rom_table_lookup
+// Returns the 32 bit pointer into the ROM if found or NULL otherwise.
+typedef void *(*rom_table_lookup_fn)(uint16_t *table, uint32_t code);
+
+// Convert a 16 bit pointer stored at the given rom address into a 32 bit pointer
+#define rom_hword_as_ptr(rom_address) (void *)(uintptr_t)(*(uint16_t *)rom_address)
+
+/*!
+ * \brief Lookup a bootrom function by code. This method is forceably inlined into the caller for FLASH/RAM sensitive code usage
+ * \ingroup pico_bootrom
+ * \param code the code
+ * \return a pointer to the function, or NULL if the code does not match any bootrom function
+ */
+static __force_inline void *rom_func_lookup_inline(uint32_t code) {
+ rom_table_lookup_fn rom_table_lookup = (rom_table_lookup_fn) rom_hword_as_ptr(0x18);
+ uint16_t *func_table = (uint16_t *) rom_hword_as_ptr(0x14);
+ return rom_table_lookup(func_table, code);
+}
+
+/*!
+ * \brief Reboot the device into BOOTSEL mode
+ * \ingroup pico_bootrom
+ *
+ * This function reboots the device into the BOOTSEL mode ('usb boot").
+ *
+ * Facilities are provided to enable an "activity light" via GPIO attached LED for the USB Mass Storage Device,
+ * and to limit the USB interfaces exposed.
+ *
+ * \param usb_activity_gpio_pin_mask 0 No pins are used as per a cold boot. Otherwise a single bit set indicating which
+ * GPIO pin should be set to output and raised whenever there is mass storage activity
+ * from the host.
+ * \param disable_interface_mask value to control exposed interfaces
+ * - 0 To enable both interfaces (as per a cold boot)
+ * - 1 To disable the USB Mass Storage Interface
+ * - 2 To disable the USB PICOBOOT Interface
+ */
+static inline void __attribute__((noreturn)) reset_usb_boot(uint32_t usb_activity_gpio_pin_mask,
+ uint32_t disable_interface_mask) {
+ rom_reset_usb_boot_fn func = (rom_reset_usb_boot_fn) rom_func_lookup(ROM_FUNC_RESET_USB_BOOT);
+ func(usb_activity_gpio_pin_mask, disable_interface_mask);
+}
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // !__ASSEMBLER__
+#endif
diff --git a/src/rp2_common/pico_bootrom/include/pico/bootrom/sf_table.h b/src/rp2_common/pico_bootrom/include/pico/bootrom/sf_table.h
new file mode 100644
index 0000000..4eb4b1c
--- /dev/null
+++ b/src/rp2_common/pico_bootrom/include/pico/bootrom/sf_table.h
@@ -0,0 +1,50 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PICO_SF_TABLE_H
+#define _PICO_SF_TABLE_H
+
+// NOTE THESE FUNCTION IMPLEMENTATIONS MATCH THE BEHAVIOR DESCRIBED IN THE BOOTROM SECTION OF THE RP2040 DATASHEET
+
+#define SF_TABLE_FADD 0x00
+#define SF_TABLE_FSUB 0x04
+#define SF_TABLE_FMUL 0x08
+#define SF_TABLE_FDIV 0x0c
+#define SF_TABLE_FCMP_FAST 0x10
+#define SF_TABLE_FCMP_FAST_FLAGS 0x14
+#define SF_TABLE_FSQRT 0x18
+#define SF_TABLE_FLOAT2INT 0x1c
+#define SF_TABLE_FLOAT2FIX 0x20
+#define SF_TABLE_FLOAT2UINT 0x24
+#define SF_TABLE_FLOAT2UFIX 0x28
+#define SF_TABLE_INT2FLOAT 0x2c
+#define SF_TABLE_FIX2FLOAT 0x30
+#define SF_TABLE_UINT2FLOAT 0x34
+#define SF_TABLE_UFIX2FLOAT 0x38
+#define SF_TABLE_FCOS 0x3c
+#define SF_TABLE_FSIN 0x40
+#define SF_TABLE_FTAN 0x44
+#define SF_TABLE_V3_FSINCOS 0x48
+#define SF_TABLE_FEXP 0x4c
+#define SF_TABLE_FLN 0x50
+
+#define SF_TABLE_V1_SIZE 0x54
+
+#define SF_TABLE_FCMP_BASIC 0x54
+#define SF_TABLE_FATAN2 0x58
+#define SF_TABLE_INT642FLOAT 0x5c
+#define SF_TABLE_FIX642FLOAT 0x60
+#define SF_TABLE_UINT642FLOAT 0x64
+#define SF_TABLE_UFIX642FLOAT 0x68
+#define SF_TABLE_FLOAT2INT64 0x6c
+#define SF_TABLE_FLOAT2FIX64 0x70
+#define SF_TABLE_FLOAT2UINT64 0x74
+#define SF_TABLE_FLOAT2UFIX64 0x78
+#define SF_TABLE_FLOAT2DOUBLE 0x7c
+
+#define SF_TABLE_V2_SIZE 0x80
+
+#endif
diff --git a/src/rp2_common/pico_bootsel_via_double_reset/CMakeLists.txt b/src/rp2_common/pico_bootsel_via_double_reset/CMakeLists.txt
new file mode 100644
index 0000000..17b4042
--- /dev/null
+++ b/src/rp2_common/pico_bootsel_via_double_reset/CMakeLists.txt
@@ -0,0 +1,10 @@
+pico_add_impl_library(pico_bootsel_via_double_reset)
+
+target_sources(pico_bootsel_via_double_reset INTERFACE
+ ${CMAKE_CURRENT_LIST_DIR}/pico_bootsel_via_double_reset.c
+ )
+
+target_link_libraries(pico_bootsel_via_double_reset INTERFACE
+ pico_bootrom
+ pico_time
+ )
diff --git a/src/rp2_common/pico_bootsel_via_double_reset/pico_bootsel_via_double_reset.c b/src/rp2_common/pico_bootsel_via_double_reset/pico_bootsel_via_double_reset.c
new file mode 100644
index 0000000..d333ab2
--- /dev/null
+++ b/src/rp2_common/pico_bootsel_via_double_reset/pico_bootsel_via_double_reset.c
@@ -0,0 +1,85 @@
+/*
+ * Copyright (c) 2021 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "pico.h"
+#include "pico/time.h"
+#include "pico/bootrom.h"
+#include "pico/binary_info.h"
+
+// PICO_CONFIG: PICO_BOOTSEL_VIA_DOUBLE_RESET_TIMEOUT_MS, Window of opportunity for a second press of a reset button to enter BOOTSEL mode (milliseconds), type=int, default=200, group=pico_bootsel_via_double_reset
+#ifndef PICO_BOOTSEL_VIA_DOUBLE_RESET_TIMEOUT_MS
+#define PICO_BOOTSEL_VIA_DOUBLE_RESET_TIMEOUT_MS 200
+#endif
+
+// PICO_CONFIG: PICO_BOOTSEL_VIA_DOUBLE_RESET_ACTIVITY_LED, Optionally define a pin to use as bootloader activity LED when BOOTSEL mode is entered via reset double tap, type=int, min=0, max=29, group=pico_bootsel_via_double_reset
+
+// PICO_CONFIG: PICO_BOOTSEL_VIA_DOUBLE_RESET_INTERFACE_DISABLE_MASK, Optionally disable either the mass storage interface (bit 0) or the PICOBOOT interface (bit 1) when entering BOOTSEL mode via double reset, type=int, min=0, max=3, default=0, group=pico_bootsel_via_double_reset
+#ifndef PICO_BOOTSEL_VIA_DOUBLE_RESET_INTERFACE_DISABLE_MASK
+#define PICO_BOOTSEL_VIA_DOUBLE_RESET_INTERFACE_DISABLE_MASK 0u
+#endif
+
+/** \defgroup pico_bootsel_via_double_reset pico_bootsel_via_double_reset
+ *
+ * When the 'pico_bootsel_via_double_reset' library is linked, a function is
+ * injected before main() which will detect when the system has been reset
+ * twice in quick succession, and enter the USB ROM bootloader (BOOTSEL mode)
+ * when this happens. This allows a double tap of a reset button on a
+ * development board to be used to enter the ROM bootloader, provided this
+ * library is always linked.
+ */
+
+#if !PICO_NO_BI_BOOTSEL_VIA_DOUBLE_RESET
+bi_decl(bi_program_feature("double reset -> BOOTSEL"));
+#endif
+
+// Doesn't make any sense for a RAM only binary
+#if !PICO_NO_FLASH
+static const uint32_t magic_token[] = {
+ 0xf01681de, 0xbd729b29, 0xd359be7a,
+};
+
+static uint32_t __uninitialized_ram(magic_location)[count_of(magic_token)];
+
+/* Check for double reset and enter BOOTSEL mode if detected
+ *
+ * This function is registered to run automatically before main(). The
+ * algorithm is:
+ *
+ * 1. Check for magic token in memory; enter BOOTSEL mode if found.
+ * 2. Initialise that memory with that magic token.
+ * 3. Do nothing for a short while (few hundred ms).
+ * 4. Clear the magic token.
+ * 5. Continue with normal boot.
+ *
+ * Resetting the device twice quickly will interrupt step 3, leaving the token
+ * in place so that the second boot will go to the bootloader.
+ */
+static void __attribute__((constructor)) boot_double_tap_check(void) {
+ for (uint i = 0; i < count_of(magic_token); i++) {
+ if (magic_location[i] != magic_token[i]) {
+ // Arm, wait, then disarm and continue booting
+ for (i = 0; i < count_of(magic_token); i++) {
+ magic_location[i] = magic_token[i];
+ }
+ busy_wait_us(PICO_BOOTSEL_VIA_DOUBLE_RESET_TIMEOUT_MS * 1000);
+ magic_location[0] = 0;
+ return;
+ }
+ }
+ // Detected a double reset, so enter USB bootloader
+ magic_location[0] = 0;
+#ifdef PICO_BOOTSEL_VIA_DOUBLE_RESET_ACTIVITY_LED
+ const uint32_t led_mask = 1u << PICO_BOOTSEL_VIA_DOUBLE_RESET_ACTIVITY_LED;
+#else
+ const uint32_t led_mask = 0u;
+#endif
+ reset_usb_boot(
+ led_mask,
+ PICO_BOOTSEL_VIA_DOUBLE_RESET_INTERFACE_DISABLE_MASK
+ );
+}
+
+#endif
diff --git a/src/rp2_common/pico_cxx_options/CMakeLists.txt b/src/rp2_common/pico_cxx_options/CMakeLists.txt
new file mode 100644
index 0000000..4b20e3a
--- /dev/null
+++ b/src/rp2_common/pico_cxx_options/CMakeLists.txt
@@ -0,0 +1,23 @@
+if (NOT TARGET pico_cxx_options)
+ add_library(pico_cxx_options INTERFACE)
+
+ # PICO_CMAKE_CONFIG: PICO_CXX_ENABLE_EXCEPTIONS, Enabled CXX exception handling, type=bool, default=0, group=pico_cxx_options
+ # PICO_BUILD_DEFINE: PICO_CXX_ENABLE_EXCEPTIONS, value of CMake var PICO_CXX_ENABLE_EXCEPTIONS, type=string, default=0, group=pico_cxx_options
+ if (NOT PICO_CXX_ENABLE_EXCEPTIONS)
+ target_compile_definitions( pico_cxx_options INTERFACE PICO_CXX_ENABLE_EXCEPTIONS=0)
+ target_compile_options( pico_cxx_options INTERFACE $<$<COMPILE_LANGUAGE:CXX>:-fno-exceptions>)
+ target_compile_options( pico_cxx_options INTERFACE $<$<COMPILE_LANGUAGE:CXX>:-fno-unwind-tables>)
+ else()
+ target_compile_definitions( pico_cxx_options INTERFACE PICO_CXX_ENABLE_EXCEPTIONS=1)
+ endif()
+
+ # PICO_CMAKE_CONFIG: PICO_CXX_ENABLE_RTTI, Enabled CXX rtti, type=bool, default=0, group=pico_cxx_options
+ if (NOT PICO_CXX_ENABLE_RTTI)
+ target_compile_options( pico_cxx_options INTERFACE $<$<COMPILE_LANGUAGE:CXX>:-fno-rtti>)
+ endif()
+
+ # PICO_CMAKE_CONFIG: PICO_CXX_ENABLE_CXA_ATEXIT, Enabled cxa-atexit, type=bool, default=0, group=pico_cxx_options
+ if (NOT PICO_CXX_ENABLE_CXA_ATEXIT)
+ target_compile_options( pico_cxx_options INTERFACE $<$<COMPILE_LANGUAGE:CXX>:-fno-use-cxa-atexit>)
+ endif()
+endif()
diff --git a/src/rp2_common/pico_cxx_options/doc.h b/src/rp2_common/pico_cxx_options/doc.h
new file mode 100644
index 0000000..5d84e22
--- /dev/null
+++ b/src/rp2_common/pico_cxx_options/doc.h
@@ -0,0 +1,4 @@
+/**
+ * \defgroup pico_cxx_options pico_cxx_options
+ * \brief non-code library controlling C++ related compile options
+ */
diff --git a/src/rp2_common/pico_divider/CMakeLists.txt b/src/rp2_common/pico_divider/CMakeLists.txt
new file mode 100644
index 0000000..ceed042
--- /dev/null
+++ b/src/rp2_common/pico_divider/CMakeLists.txt
@@ -0,0 +1,47 @@
+if (NOT TARGET pico_divider)
+ # library to be depended on - we make this depend on particular implementations using per target generator expressions
+ pico_add_impl_library(pico_divider)
+
+ # no custom implementation; falls thru to compiler
+ pico_add_impl_library(pico_divider_compiler)
+
+ # add alias "default" which is just hardware.
+ add_library(pico_divider_default INTERFACE)
+ target_link_libraries(pico_divider_default INTERFACE pico_divider_hardware)
+
+ set(PICO_DEFAULT_DIVIDER_IMPL pico_divider_default)
+
+ target_link_libraries(pico_divider INTERFACE
+ $<IF:$<BOOL:$<TARGET_PROPERTY:PICO_TARGET_DIVIDER_IMPL>>,$<TARGET_PROPERTY:PICO_TARGET_DIVIDER_IMPL>,${PICO_DEFAULT_DIVIDER_IMPL}>)
+
+ add_library(pico_divider_hardware_explicit INTERFACE)
+ target_sources(pico_divider_hardware_explicit INTERFACE
+ ${CMAKE_CURRENT_LIST_DIR}/divider.S
+ )
+
+ target_link_libraries(pico_divider_hardware_explicit INTERFACE
+ pico_divider_headers
+ hardware_divider
+ hardware_regs
+ )
+
+ pico_add_impl_library(pico_divider_hardware)
+
+ target_link_libraries(pico_divider_hardware INTERFACE pico_divider_hardware_explicit)
+
+ pico_wrap_function(pico_divider_hardware __aeabi_idiv)
+ pico_wrap_function(pico_divider_hardware __aeabi_idivmod)
+ pico_wrap_function(pico_divider_hardware __aeabi_ldivmod)
+ pico_wrap_function(pico_divider_hardware __aeabi_uidiv)
+ pico_wrap_function(pico_divider_hardware __aeabi_uidivmod)
+ pico_wrap_function(pico_divider_hardware __aeabi_uldivmod)
+
+ macro(pico_set_divider_implementation TARGET IMPL)
+ get_target_property(target_type ${TARGET} TYPE)
+ if ("EXECUTABLE" STREQUAL "${target_type}")
+ set_target_properties(${TARGET} PROPERTIES PICO_TARGET_DIVIDER_IMPL "pico_divider_${IMPL}")
+ else()
+ message(FATAL_ERROR "divider implementation must be set on executable not library")
+ endif()
+ endmacro()
+endif()
diff --git a/src/rp2_common/pico_divider/divider.S b/src/rp2_common/pico_divider/divider.S
new file mode 100644
index 0000000..234c1a4
--- /dev/null
+++ b/src/rp2_common/pico_divider/divider.S
@@ -0,0 +1,882 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "hardware/regs/addressmap.h"
+#include "hardware/divider_helper.S"
+
+.syntax unified
+.cpu cortex-m0plus
+.thumb
+
+// PICO_CONFIG: PICO_DIVIDER_DISABLE_INTERRUPTS, Disable interrupts around division such that divider state need not be saved/restored in exception handlers, default=0, group=pico_divider
+
+#include "pico/asm_helper.S"
+
+// PICO_CONFIG: PICO_DIVIDER_CALL_IDIV0, Whether 32 bit division by zero should call __aeabi_idiv0, default=1, group=pico_divider
+#ifndef PICO_DIVIDER_CALL_IDIV0
+#define PICO_DIVIDER_CALL_IDIV0 1
+#endif
+
+// PICO_CONFIG: PICO_DIVIDER_CALL_IDIV0, Whether 64 bit division by zero should call __aeabi_ldiv0, default=1, group=pico_divider
+#ifndef PICO_DIVIDER_CALL_LDIV0
+#define PICO_DIVIDER_CALL_LDIV0 1
+#endif
+
+// PICO_CONFIG: PICO_DIVIDER_IN_RAM, Whether divider functions should be placed in RAM, default=0, group=pico_divider
+.macro div_section name
+#if PICO_DIVIDER_IN_RAM
+.section RAM_SECTION_NAME(\name), "ax"
+#else
+.section SECTION_NAME(\name), "ax"
+#endif
+.endm
+
+@ wait 8-n cycles for the hardware divider
+.macro wait_div n
+.rept (8-\n) / 2
+ b 9f
+9:
+.endr
+.if (8-\n) % 2
+ nop
+.endif
+.endm
+
+#if (SIO_DIV_SDIVISOR_OFFSET != SIO_DIV_SDIVIDEND_OFFSET + 4) || (SIO_DIV_QUOTIENT_OFFSET != SIO_DIV_SDIVISOR_OFFSET + 4) || (SIO_DIV_REMAINDER_OFFSET != SIO_DIV_QUOTIENT_OFFSET + 4)
+#error register layout has changed - we rely on this order to make sure we save/restore in the right order
+#endif
+
+#if !PICO_DIVIDER_DISABLE_INTERRUPTS
+.macro save_div_state_and_lr_64
+ push {r4, r5, r6, r7, lr}
+ ldr r6, =SIO_BASE
+1:
+ ldr r5, [r6, #SIO_DIV_CSR_OFFSET]
+ // wait for results as we can't save signed-ness of operation
+ lsrs r5, #SIO_DIV_CSR_READY_SHIFT_FOR_CARRY
+ bcc 1b
+ // note we must read quotient last, and since it isn't the last reg, we'll not use ldmia!
+ ldr r4, [r6, #SIO_DIV_UDIVIDEND_OFFSET]
+ ldr r5, [r6, #SIO_DIV_UDIVISOR_OFFSET]
+ ldr r7, [r6, #SIO_DIV_REMAINDER_OFFSET]
+ ldr r6, [r6, #SIO_DIV_QUOTIENT_OFFSET]
+.endm
+
+.macro restore_div_state_and_return_64
+ // writing sdividend (r4), sdivisor (r5), quotient (r6), remainder (r7) in that order
+ //
+ // it is worth considering what happens if we are interrupted
+ //
+ // after writing r4: we are DIRTY and !READY
+ // ... interruptor using div will complete based on incorrect inputs, but dividend at least will be
+ // saved/restored correctly and we'll restore the rest ourselves
+ // after writing r4, r5: we are DIRTY and !READY
+ // ... interruptor using div will complete based on possibly wrongly signed inputs, but dividend, divisor
+ // at least will be saved/restored correctly and and we'll restore the rest ourselves
+ // after writing r4, r5, r6: we are DIRTY and READY
+ // ... interruptor using div will dividend, divisor, quotient registers as is (what we just restored ourselves),
+ // and we'll restore the remainder after the fact
+
+ mov ip, r2
+ ldr r2, =SIO_BASE
+ // note we are not use STM not because it can be restarted due to interrupt which is harmless, more because this is 1 cycle IO space
+ // and so 4 reads is cheaper (and we don't have to adjust r2)
+ str r4, [r2, #SIO_DIV_UDIVIDEND_OFFSET]
+ str r5, [r2, #SIO_DIV_UDIVISOR_OFFSET]
+ str r7, [r2, #SIO_DIV_REMAINDER_OFFSET]
+ str r6, [r2, #SIO_DIV_QUOTIENT_OFFSET]
+ mov r2, ip
+ pop {r4, r5, r6, r7, pc}
+.endm
+
+#endif /* !PICO_DIVIDER_DISABLE_INTERRUPTS */
+
+// since idiv and idivmod only differ by a cycle, we'll make them the same!
+div_section WRAPPER_FUNC_NAME(__aeabi_idiv)
+.align 2
+wrapper_func __aeabi_idiv
+wrapper_func __aeabi_idivmod
+regular_func div_s32s32
+regular_func divmod_s32s32
+#if !PICO_DIVIDER_DISABLE_INTERRUPTS
+ // to support IRQ usage (or context switch) we must save/restore divider state around call if state is dirty
+ ldr r2, =(SIO_BASE)
+ ldr r3, [r2, #SIO_DIV_CSR_OFFSET]
+ lsrs r3, #SIO_DIV_CSR_DIRTY_SHIFT_FOR_CARRY
+ bcs divmod_s32s32_savestate
+regular_func divmod_s32s32_unsafe
+#else
+// to avoid too much source code spaghetti with restoring interrupts, we make this the same as the other funcs
+// in the PICO_DIVIDER_DISABLE_INTERRUPTS case; i.e. it is not a faster function; this seems reasonable as there
+// are the hardware_divider functions that can be used instead anyway
+regular_func divmod_s32s32_unsafe
+ // to avoid worrying about IRQs (or context switches), simply disable interrupts around call
+ ldr r2, =(SIO_BASE)
+ mrs r3, PRIMASK
+ cpsid i
+#endif /* !PICO_DIVIDER_DISABLE_INTERRUPTS */
+ str r0, [r2, #SIO_DIV_SDIVIDEND_OFFSET]
+ str r1, [r2, #SIO_DIV_SDIVISOR_OFFSET]
+ cmp r1, #0
+ beq 1f
+ wait_div 2
+ // return 64 bit value so we can efficiently return both (note read order is important since QUOTIENT must be read last)
+ ldr r1, [r2, #SIO_DIV_REMAINDER_OFFSET]
+ ldr r0, [r2, #SIO_DIV_QUOTIENT_OFFSET]
+#if PICO_DIVIDER_DISABLE_INTERRUPTS
+ msr PRIMASK, r3
+#endif /* PICO_DIVIDER_DISABLE_INTERRUPTS */
+ bx lr
+1:
+#if PICO_DIVIDER_DISABLE_INTERRUPTS
+ msr PRIMASK, r3
+#endif /* PICO_DIVIDER_DISABLE_INTERRUPTS */
+ push {r2, lr}
+ movs r1, #0x80
+ lsls r1, #24
+ asrs r2, r0, #31
+ eors r1, r2
+ cmp r0, #0
+ beq 1f
+ mvns r0, r1
+1:
+#if PICO_DIVIDER_CALL_IDIV0
+ bl __aeabi_idiv0
+#endif
+ movs r1, #0 // remainder 0
+ // need to restore saved r2 as it hold SIO ptr
+ pop {r2, pc}
+#if !PICO_DIVIDER_DISABLE_INTERRUPTS
+.align 2
+regular_func divmod_s32s32_savestate
+ // note that we must be at least 2 cycles into division at this point,
+ // which we are because of the firty check before getting here (and of course the function call before that)
+ save_div_state_and_lr
+ bl divmod_s32s32_unsafe
+ restore_div_state_and_return
+#endif /* !PICO_DIVIDER_DISABLE_INTERRUPTS */
+
+// since uidiv and uidivmod only differ by a cycle, we'll make them the same!
+div_section WRAPPER_FUNC_NAME(__aeabi_uidiv)
+regular_func div_u32u32
+regular_func divmod_u32u32
+wrapper_func __aeabi_uidiv
+wrapper_func __aeabi_uidivmod
+#if !PICO_DIVIDER_DISABLE_INTERRUPTS
+ // to support IRQ usage (or context switch) we must save/restore divider state around call if state is dirty
+ ldr r2, =(SIO_BASE)
+ ldr r3, [r2, #SIO_DIV_CSR_OFFSET]
+ lsrs r3, #SIO_DIV_CSR_DIRTY_SHIFT_FOR_CARRY
+ bcs divmod_u32u32_savestate
+regular_func divmod_u32u32_unsafe
+#else
+// to avoid too much source code spaghetti with restoring interrupts, we make this the same as the other funcs
+// in the PICO_DIVIDER_DISABLE_INTERRUPTS case; i.e. it is not a faster function; this seems reasonable as there
+// are the hardware_divider functions that can be used instead anyway
+regular_func divmod_u32u32_unsafe
+ // to avoid worrying about IRQs (or context switches), simply disable interrupts around call
+ ldr r2, =(SIO_BASE)
+ mrs r3, PRIMASK
+ cpsid i
+#endif /* !PICO_DIVIDER_DISABLE_INTERRUPTS */
+ str r0, [r2, #SIO_DIV_UDIVIDEND_OFFSET]
+ str r1, [r2, #SIO_DIV_UDIVISOR_OFFSET]
+ cmp r1, #0
+ beq 1f
+ wait_div 2
+ // return 64 bit value so we can efficiently return both (note read order is important since QUOTIENT must be read last)
+ ldr r1, [r2, #SIO_DIV_REMAINDER_OFFSET]
+ ldr r0, [r2, #SIO_DIV_QUOTIENT_OFFSET]
+#if PICO_DIVIDER_DISABLE_INTERRUPTS
+ msr PRIMASK, r3
+#endif /* PICO_DIVIDER_DISABLE_INTERRUPTS */
+ bx lr
+1:
+#if PICO_DIVIDER_DISABLE_INTERRUPTS
+ msr PRIMASK, r3
+#endif /* PICO_DIVIDER_DISABLE_INTERRUPTS */
+ push {r2, lr}
+ cmp r0, #0
+ beq 1f
+ movs r0, #0
+ mvns r0, r0
+1:
+#if PICO_DIVIDER_CALL_IDIV0
+ bl __aeabi_idiv0
+#endif
+ movs r1, #0 // remainder 0
+ // need to restore saved r2 as it hold SIO ptr
+ pop {r2, pc}
+#if !PICO_DIVIDER_DISABLE_INTERRUPTS
+.align 2
+regular_func divmod_u32u32_savestate
+ save_div_state_and_lr
+ bl divmod_u32u32_unsafe
+ restore_div_state_and_return
+#endif /* !PICO_DIVIDER_DISABLE_INTERRUPTS */
+
+div_section WRAPPER_FUNC_NAME(__aeabi_ldiv)
+
+.align 2
+wrapper_func __aeabi_ldivmod
+regular_func div_s64s64
+regular_func divmod_s64s64
+#if !PICO_DIVIDER_DISABLE_INTERRUPTS
+ // to support IRQ usage (or context switch) we must save/restore divider state around call if state is dirty
+ mov ip, r2
+ ldr r2, =(SIO_BASE)
+ ldr r2, [r2, #SIO_DIV_CSR_OFFSET]
+ lsrs r2, #SIO_DIV_CSR_DIRTY_SHIFT_FOR_CARRY
+ mov r2, ip
+ bcs divmod_s64s64_savestate
+ b divmod_s64s64_unsafe
+.align 2
+divmod_s64s64_savestate:
+ save_div_state_and_lr_64
+ bl divmod_s64s64_unsafe
+ restore_div_state_and_return_64
+#else
+ // to avoid worrying about IRQs (or context switches), simply disable interrupts around call
+ push {r4, lr}
+ mrs r4, PRIMASK
+ cpsid i
+ bl divmod_s64s64_unsafe
+ msr PRIMASK, r4
+ pop {r4, pc}
+#endif /* !PICO_DIVIDER_DISABLE_INTERRUPTS */
+
+.align 2
+wrapper_func __aeabi_uldivmod
+regular_func div_u64u64
+regular_func divmod_u64u64
+#if !PICO_DIVIDER_DISABLE_INTERRUPTS
+ // to support IRQ usage (or context switch) we must save/restore divider state around call if state is dirty
+ mov ip, r2
+ ldr r2, =(SIO_BASE)
+ ldr r2, [r2, #SIO_DIV_CSR_OFFSET]
+ lsrs r2, #SIO_DIV_CSR_DIRTY_SHIFT_FOR_CARRY
+ mov r2, ip
+ bcs divmod_u64u64_savestate
+ b divmod_u64u64_unsafe
+.align 2
+regular_func divmod_u64u64_savestate
+ save_div_state_and_lr_64
+ bl divmod_u64u64_unsafe
+ restore_div_state_and_return_64
+#else
+ // to avoid worrying about IRQs (or context switches), simply disable interrupts around call
+ push {r4, lr}
+ mrs r4, PRIMASK
+ cpsid i
+ bl divmod_u64u64_unsafe
+ msr PRIMASK, r4
+ pop {r4, pc}
+#endif /* !PICO_DIVIDER_DISABLE_INTERRUPTS */
+
+.macro dneg lo,hi
+ mvns \hi,\hi
+ rsbs \lo,#0
+ bne l\@_1
+ adds \hi,#1
+l\@_1:
+.endm
+
+.align 2
+regular_func divmod_s64s64_unsafe
+ cmp r3,#0
+ blt 1f
+@ here x +ve
+ beq 2f @ could x be zero?
+3:
+ cmp r1,#0
+ bge divmod_u64u64_unsafe @ both positive
+@ y -ve, x +ve
+ push {r14}
+ dneg r0,r1
+ bl divmod_u64u64_unsafe
+ dneg r0,r1
+ dneg r2,r3
+ pop {r15}
+
+2:
+ cmp r2,#0
+ bne 3b @ back if x not zero
+
+ cmp r0,#0 @ y==0?
+ bne 4f
+ cmp r1,#0
+ beq 5f @ then pass 0 to __aeabi_ldiv0
+4:
+ movs r0,#0
+ lsrs r1,#31
+ lsls r1,#31 @ get sign bit
+ bne 5f @ y -ve? pass -2^63 to __aeabi_ldiv0
+ mvns r0,r0
+ lsrs r1,r0,#1 @ y +ve: pass 2^63-1 to __aeabi_ldiv0
+5:
+ push {r14}
+#if PICO_DIVIDER_CALL_LDIV0
+ bl __aeabi_ldiv0
+#endif
+ movs r2,#0 @ and return 0 for the remainder
+ movs r3,#0
+ pop {r15}
+
+1:
+@ here x -ve
+ push {r14}
+ cmp r1,#0
+ blt 1f
+@ y +ve, x -ve
+ dneg r2,r3
+ bl divmod_u64u64_unsafe
+ dneg r0,r1
+ pop {r15}
+
+1:
+@ y -ve, x -ve
+ dneg r0,r1
+ dneg r2,r3
+ bl divmod_u64u64_unsafe
+ dneg r2,r3
+ pop {r15}
+
+regular_func divmod_u64u64_unsafe
+ cmp r1,#0
+ bne y64 @ y fits in 32 bits?
+ cmp r3,#0 @ yes; and x?
+ bne 1f
+ cmp r2,#0
+ beq 2f @ x==0?
+ mov r12,r7
+ ldr r7,=#SIO_BASE
+ str r0,[r7,#SIO_DIV_UDIVIDEND_OFFSET]
+ str r2,[r7,#SIO_DIV_UDIVISOR_OFFSET]
+ movs r1,#0
+ movs r3,#0
+ wait_div 2
+ ldr r2,[r7,#SIO_DIV_REMAINDER_OFFSET]
+ ldr r0,[r7,#SIO_DIV_QUOTIENT_OFFSET]
+ mov r7,r12
+ bx r14
+
+2: @ divide by 0 with y<2^32
+ cmp r0,#0 @ y==0?
+ beq 3f @ then pass 0 to __aeabi_ldiv0
+udiv0:
+ ldr r0,=#0xffffffff
+ movs r1,r0 @ pass 2^64-1 to __aeabi_ldiv0
+3:
+ push {r14}
+#if PICO_DIVIDER_CALL_LDIV0
+ bl __aeabi_ldiv0
+#endif
+ movs r2,#0 @ and return 0 for the remainder
+ movs r3,#0
+ pop {r15}
+
+1:
+ movs r2,r0 @ x>y, so result is 0 remainder y
+ movs r3,r1
+ movs r0,#0
+ movs r1,#0
+ bx r14
+
+.ltorg
+
+@ here y occupies more than 32 bits
+@ split into cases acccording to the size of x
+y64:
+ cmp r3,#0
+ beq 1f
+ b y64_x48 @ if x does not fit in 32 bits, go to 48- and 64-bit cases
+1:
+ lsrs r3,r2,#16
+ bne y64_x32 @ jump if x is 17..32 bits
+
+@ here x is at most 16 bits
+
+ cmp r2,#0
+ beq udiv0 @ x==0? exit as with y!=0 case above
+ push {r7}
+ ldr r7,=#SIO_BASE
+ str r1,[r7,#SIO_DIV_UDIVIDEND_OFFSET]
+ str r2,[r7,#SIO_DIV_UDIVISOR_OFFSET]
+ wait_div 4
+ push {r4, r5}
+ lsrs r4,r0,#16
+ ldr r3,[r7,#SIO_DIV_REMAINDER_OFFSET] @ r0=y0-q0*x; 0<=r0<x
+ ldr r1,[r7,#SIO_DIV_QUOTIENT_OFFSET] @ q0=y0/x;
+ lsls r3,#16
+ orrs r3,r4
+ str r3,[r7,#SIO_DIV_UDIVIDEND_OFFSET] @ y1=(r0<<16)+(((ui32)y)>>16);
+ str r2,[r7,#SIO_DIV_UDIVISOR_OFFSET] @ must set divisor again, as we do not save/restore regs at all in IRQs if not dirty
+ wait_div 1
+ uxth r4,r0
+ ldr r3,[r7,#SIO_DIV_REMAINDER_OFFSET] @ r1=y1-q1*x; 0<=r1<x
+ ldr r5,[r7,#SIO_DIV_QUOTIENT_OFFSET] @ q1=y1/x;
+ lsls r3,#16
+ orrs r3,r4
+ str r3,[r7,#SIO_DIV_UDIVIDEND_OFFSET] @ y1=(r0<<16)+(((ui32)y)>>16);
+ str r2,[r7,#SIO_DIV_UDIVISOR_OFFSET] @ must set divisor again, as we do not save/restore regs at all in IRQs if not dirty
+ wait_div 3
+ movs r3,#0
+ lsls r4,r5,#16 @ quotient=(q0<<32)+(q1<<16)+q2
+ lsrs r5,#16
+ ldr r2,[r7,#SIO_DIV_REMAINDER_OFFSET] @ r2=y2-q2*x; 0<=r2<x
+ ldr r0,[r7,#SIO_DIV_QUOTIENT_OFFSET] @ q2=y2/x;
+ adds r0,r4
+ adcs r1,r5
+ pop {r4,r5,r7}
+ bx r14
+
+.ltorg
+
+y64_x32:
+@ here x is 17..32 bits
+ push {r4-r7,r14}
+ mov r12,r2 @ save x
+ movs r5,#0 @ xsh=0
+ lsrs r4,r2,#24
+ bne 1f
+ lsls r2,#8 @ if(x0<1U<<24) x0<<=8,xsh =8;
+ adds r5,#8
+1:
+ lsrs r4,r2,#28
+ bne 1f
+ lsls r2,#4 @ if(x0<1U<<28) x0<<=4,xsh+=4;
+ adds r5,#4
+1:
+ lsrs r4,r2,#30
+ bne 1f
+ lsls r2,#2 @ if(x0<1U<<30) x0<<=2,xsh+=2;
+ adds r5,#2
+1:
+ lsrs r4,r2,#31
+ bne 1f
+ lsls r2,#1 @ if(x0<1U<<31) x0<<=1,xsh+=1;
+ adds r5,#1
+1:
+@ now 2^31<=x0<2^32, 0<=xsh<16 (amount x is shifted in x0); number of quotient bits to be calculated qb=xsh+33 33<=qb<49
+ lsrs r4,r2,#15
+ adds r4,#1 @ x1=(x0>>15)+1; 2^16<x1<=2^17
+
+ ldr r7,=#SIO_BASE
+ str r4,[r7,#SIO_DIV_UDIVISOR_OFFSET]
+ ldr r4,=#0xffffffff
+ str r4,[r7,#SIO_DIV_UDIVIDEND_OFFSET]
+ lsrs r6,r1,#16
+ uxth r3,r2 @ x0l
+ wait_div 2
+ ldr r4,[r7,#SIO_DIV_QUOTIENT_OFFSET] @ r=0xffffffffU/x1; 2^15<=r<2^16 r is a normalised reciprocal of x, guaranteed not an overestimate
+
+@ here
+@ r0:r1 y
+@ r2 x0
+@ r4 r
+@ r5 xsh
+@ r12 x
+
+ muls r6,r4
+ lsrs r6,#16 @ q=((ui32)(y>>48)*r)>>16;
+ lsls r7,r6,#13
+ mov r14,r7 @ quh=q0<<13
+
+ muls r3,r6 @ x0l*q
+ lsrs r7,r3,#15
+ lsls r3,#17 @ r3:r7 is (x0l*q)<<17
+ subs r0,r3
+ sbcs r1,r7 @ y-=(x0l*q)<<17
+
+ lsrs r3,r2,#16 @ x0h
+ muls r3,r6 @ q*x0h
+ adds r3,r3
+ subs r1,r3 @ y-=(x0h*q)<<17
+
+ lsrs r6,r1,#3
+ muls r6,r4
+ lsrs r6,#16 @ q=((ui32)(y>>35)*r)>>16;
+ add r14,r6 @ quh+=q1
+
+ uxth r3,r2 @ x0l
+ muls r3,r6 @ x0l*q
+ lsrs r7,r3,#28
+ lsls r3,#4 @ r3:r7 is (x0l*q)<<4
+ subs r0,r3
+ sbcs r1,r7 @ y-=(x0l*q)<<4
+
+ lsrs r3,r2,#16 @ x0h
+ muls r3,r6 @ x0h*q
+ lsrs r7,r3,#12
+ lsls r3,#20 @ r3:r7 is (x0h*q)<<4
+ subs r0,r3
+ sbcs r1,r7 @ y-=(x0h*q)<<4
+
+ lsrs r6,r0,#22
+ lsls r7,r1,#10
+ orrs r6,r7 @ y>>22
+ muls r6,r4
+ lsrs r6,#16 @ q=((ui32)(y>>22)*r)>>16;
+
+ cmp r5,#9
+ blt last0 @ if(xsh<9) goto last0;
+
+@ on this path xsh>=9, which means x<2^23
+ lsrs r2,#9 @ x0>>9: this shift loses no bits
+@ the remainder y-x0*q is guaranteed less than a very small multiple of the remaining quotient
+@ bits (at most 6 bits) times x, and so fits in one word
+ muls r2,r6 @ x0*q
+ subs r0,r2 @ y-x0*q
+ lsls r7,r6,#13 @ qul=q<<13
+1:
+ lsrs r6,r0,#9
+ muls r6,r4
+ lsrs r6,#16 @ q=((ui32)(y>>9)*r)>>16;
+
+@ here
+@ r0 y
+@ r2 x0>>9
+@ r5 xsh
+@ r6 q
+@ r7 qul
+@ r12 x
+@ r14 quh
+
+ movs r3,#22
+ subs r3,r5 @ 22-xsh
+ lsrs r6,r3 @ q>>=22-xsh
+ lsrs r7,r3 @ qul>>=22-xsh
+ adds r7,r6 @ qul+=q
+ mov r4,r12
+ muls r6,r4 @ x*q
+ subs r2,r0,r6 @ y-=x*q
+ mov r0,r14 @ quh
+ adds r5,#4 @ xsh+4
+ adds r3,#6 @ 28-xsh
+ movs r1,r0
+ lsrs r1,r3
+ lsls r0,r5 @ r0:r1 is quh<<(4+xsh)
+ adds r0,r7
+ bcc 1f
+2:
+ adds r1,#1
+1: @ qu=((ui64)quh<<(4+xsh))+qul
+ cmp r2,r4
+ bhs 3f
+ movs r3,#0
+ pop {r4-r7,r15}
+
+.ltorg
+
+3:
+ subs r2,r4
+ adds r0,#1
+ bcc 1b
+ b 2b @ while(y>=x) y-=x,qu++;
+
+@ here:
+@ r0:r1 y
+@ r2 x0
+@ r4 r
+@ r5 xsh; xsh<9
+@ r6 q
+
+last0:
+ movs r7,#9
+ subs r7,r5 @ 9-xsh
+ lsrs r6,r7
+ mov r4,r12 @ x
+ uxth r2,r4
+ muls r2,r6 @ q*xlo
+ subs r0,r2
+ bcs 1f
+ subs r1,#1 @ y-=q*xlo
+1:
+ lsrs r2,r4,#16 @ xhi
+ muls r2,r6 @ q*xhi
+ lsrs r3,r2,#16
+ lsls r2,#16
+ subs r2,r0,r2
+ sbcs r1,r3 @ y-q*xhi
+ movs r3,r1 @ y now in r2:r3
+ mov r0,r14 @ quh
+ adds r5,#4 @ xsh+4
+ adds r7,#19 @ 28-xsh
+ movs r1,r0
+ lsrs r1,r7
+ lsls r0,r5 @ r0:r1 is quh<<(4+xsh)
+ adds r0,r6
+ bcc 1f
+ adds r1,#1 @ quh<<(xsh+4))+q
+1:
+ cmp r3,#0 @ y>=2^32?
+ bne 3f
+ cmp r2,r4 @ y>=x?
+ bhs 4f
+ pop {r4-r7,r15}
+
+3:
+ adds r0,#1 @ qu++
+ bcc 2f
+ adds r1,#1
+2:
+ subs r2,r4 @ y-=x
+ bcs 3b
+ subs r3,#1
+ bne 3b
+
+1:
+ cmp r2,r4
+ bhs 4f
+ pop {r4-r7,r15}
+
+4:
+ adds r0,#1 @ qu++
+ bcc 2f
+ adds r1,#1
+2:
+ subs r2,r4 @ y-=x
+ b 1b
+
+y64_x48:
+@ here x is 33..64 bits
+ push {r4-r7,r14} @ save a copy of x
+ lsrs r4,r3,#16
+ beq 1f
+ b y64_x64 @ jump if x is 49..64 bits
+1:
+ push {r2-r3} @ save a copy of x
+@ here x is 33..48 bits
+ movs r5,#0 @ xsh=0
+ lsrs r4,r3,#8
+ bne 1f
+ lsls r3,#8
+ lsrs r6,r2,#24
+ orrs r3,r6
+ lsls r2,#8 @ if(x0<1U<<40) x0<<=8,xsh =8;
+ adds r5,#8
+1:
+ lsrs r4,r3,#12
+ bne 1f
+ lsls r3,#4
+ lsrs r6,r2,#28
+ orrs r3,r6
+ lsls r2,#4 @ if(x0<1U<<44) x0<<=4,xsh+=4;
+ adds r5,#4
+1:
+ lsrs r4,r3,#14
+ bne 1f
+ lsls r3,#2
+ lsrs r6,r2,#30
+ orrs r3,r6
+ lsls r2,#2 @ if(x0<1U<<46) x0<<=2,xsh+=2;
+ adds r5,#2
+1:
+ lsrs r4,r3,#15
+ bne 1f
+ adds r2,r2
+ adcs r3,r3 @ if(x0<1U<<47) x0<<=1,xsh+=1;
+ adds r5,#1
+1:
+@ now 2^47<=x0<2^48, 0<=xsh<16 (amount x is shifted in x0); number of quotient bits to be calculated qb=xsh+17 17<=qb<33
+ movs r4,r3
+ adds r7,r2,r2
+ adcs r4,r4
+ adds r4,#1 @ x1=(ui32)(x0>>31)+1; // 2^16<x1<=2^17
+
+ ldr r7,=#SIO_BASE
+ str r4,[r7,#SIO_DIV_UDIVISOR_OFFSET]
+ ldr r4,=#0xffffffff
+ str r4,[r7,#SIO_DIV_UDIVIDEND_OFFSET]
+ lsrs r6,r1,#16
+ wait_div 1
+ ldr r4,[r7,#SIO_DIV_QUOTIENT_OFFSET] @ r=0xffffffffU/x1; 2^15<=r<2^16 r is a normalised reciprocal of x, guaranteed not an overestimate
+
+@ here
+@ r0:r1 y
+@ r2:r3 x0
+@ r4 r
+@ r5 xsh 0<=xsh<16
+
+ muls r6,r4
+ lsrs r6,#16 @ q=((ui32)(y>>48)*r)>>16;
+ lsls r7,r6,#13
+ mov r14,r7 @ save q<<13
+ uxth r7,r2 @ x0l
+ muls r7,r6
+ subs r0,r7
+ bcs 1f
+ subs r1,#1
+1:
+ subs r0,r7
+ bcs 1f
+ subs r1,#1
+1:
+ uxth r7,r3 @ x0h
+ muls r7,r6
+ subs r1,r7
+ subs r1,r7
+ lsrs r7,r2,#16 @ x0m
+ muls r7,r6
+ lsls r6,r7,#17
+ lsrs r7,#15
+ subs r0,r6
+ sbcs r1,r7 @ y-=((ui64)q*x0)<<1;
+
+ lsrs r6,r1,#3 @ y>>35
+ muls r6,r4
+ lsrs r6,#16 @ q=((ui32)(y>>35)*r)>>16;
+
+ cmp r5,#12
+ blt last1 @ if(xsh<12) goto last1;
+
+ add r14,r6 @ qu<<13+q
+ lsrs r2,#12
+ lsls r7,r3,#20
+ orrs r2,r7
+ lsrs r3,#12 @ x0>>12
+
+ uxth r7,r2 @ x0l
+ muls r7,r6
+ subs r0,r7
+ bcs 1f
+ subs r1,#1
+1:
+ uxth r7,r3 @ x0h
+ muls r7,r6
+ subs r1,r7
+ lsrs r7,r2,#16 @ x0m
+ muls r7,r6
+ lsls r6,r7,#16
+ lsrs r7,#16
+ subs r0,r6
+ sbcs r1,r7 @ y-=((ui64)q*x0)>>12
+
+ lsrs r6,r0,#22
+ lsls r7,r1,#10
+ orrs r6,r7 @ y>>22
+ muls r6,r4
+ movs r7,#41
+ subs r7,r5
+ lsrs r6,r7 @ q=((ui32)(y>>22)*r)>>(16+25-xsh)
+
+ subs r5,#12
+ mov r7,r14
+ lsls r7,r5
+2:
+ adds r7,r6 @ qu=(qu<<(xsh-12))+q
+ pop {r4,r5} @ recall x
+
+@ here
+@ r0:r1 y
+@ r4:r5 x
+@ r6 q
+@ r7 qu
+
+ uxth r2,r4
+ uxth r3,r5
+ muls r2,r6 @ xlo*q
+ muls r3,r6 @ xhi*q
+ subs r0,r2
+ sbcs r1,r3
+ lsrs r2,r4,#16
+ muls r2,r6
+ lsrs r3,r2,#16
+ lsls r2,#16 @ xm*q
+ subs r0,r2
+ sbcs r1,r3 @ y-=(ui64)q*x
+
+1:
+ movs r2,r0
+ movs r3,r1
+ adds r7,#1
+ subs r0,r4
+ sbcs r1,r5 @ while(y>=x) y-=x,qu++;
+ bhs 1b
+ subs r0,r7,#1 @ correction to qu
+ movs r1,#0
+ pop {r4-r7,r15}
+
+last1:
+@ r0:r1 y
+@ r2:r3 x0
+@ r5 xsh
+@ r6 q
+
+ movs r7,#12
+ subs r7,r5
+ lsrs r6,r7 @ q>>=12-xsh
+ mov r7,r14
+ lsrs r7,#13
+ lsls r7,r5
+ adds r7,r7 @ qu<<(xsh+1)
+ b 2b
+
+y64_x64:
+@ here x is 49..64 bits
+ movs r4,#0 @ q=0 if x>>32==0xffffffff
+ adds r5,r3,#1
+ beq 1f
+
+ ldr r7,=#SIO_BASE
+ str r5,[r7,#SIO_DIV_UDIVISOR_OFFSET]
+ str r1,[r7,#SIO_DIV_UDIVIDEND_OFFSET]
+ wait_div 0
+ ldr r4,[r7,#SIO_DIV_QUOTIENT_OFFSET] @ q=(ui32)(y>>32)/((x>>32)+1)
+1:
+ uxth r5,r2
+ uxth r6,r3
+ muls r5,r4
+ muls r6,r4
+ subs r0,r5
+ sbcs r1,r6
+ lsrs r5,r2,#16
+ lsrs r6,r3,#16
+ muls r5,r4
+ muls r6,r4
+ lsls r6,#16
+ lsrs r7,r5,#16
+ orrs r6,r7
+ lsls r5,#16
+ subs r0,r5
+ sbcs r1,r6 @ y-=(ui64)q*x
+
+ cmp r1,r3 @ while(y>=x) y-=x,q++
+ bhs 1f
+3:
+ movs r2,r0
+ movs r3,r1
+ movs r0,r4
+ movs r1,#0
+ pop {r4-r7,r15}
+
+1:
+ bne 2f
+ cmp r0,r2
+ blo 3b
+2:
+ subs r0,r2
+ sbcs r1,r3
+ adds r4,#1
+ cmp r1,r3
+ blo 3b
+ b 1b
+
+div_section divmod_s64s64_rem
+regular_func divmod_s64s64_rem
+ push {r4, lr}
+ bl divmod_s64s64
+ ldr r4, [sp, #8]
+ stmia r4!, {r2,r3}
+ pop {r4, pc}
+
+div_section divmod_u64u64_rem
+regular_func divmod_u64u64_rem
+ push {r4, lr}
+ bl divmod_u64u64
+ ldr r4, [sp, #8]
+ stmia r4!, {r2,r3}
+ pop {r4, pc}
diff --git a/src/rp2_common/pico_double/CMakeLists.txt b/src/rp2_common/pico_double/CMakeLists.txt
new file mode 100644
index 0000000..efe1b63
--- /dev/null
+++ b/src/rp2_common/pico_double/CMakeLists.txt
@@ -0,0 +1,117 @@
+if (NOT TARGET pico_double)
+ # library to be depended on - we make this depend on particular implementations using per target generator expressions
+ pico_add_impl_library(pico_double)
+
+ # no custom implementation; falls thru to compiler
+ pico_add_impl_library(pico_double_compiler)
+
+ add_library(pico_double_headers INTERFACE)
+ target_include_directories(pico_double_headers INTERFACE ${CMAKE_CURRENT_LIST_DIR}/include)
+
+ # add alias "default" which is just pico.
+ add_library(pico_double_default INTERFACE)
+ target_link_libraries(pico_double_default INTERFACE pico_double_pico)
+
+ set(PICO_DEFAULT_DOUBLE_IMPL pico_double_default)
+
+ target_link_libraries(pico_double INTERFACE
+ $<IF:$<BOOL:$<TARGET_PROPERTY:PICO_TARGET_DOUBLE_IMPL>>,$<TARGET_PROPERTY:PICO_TARGET_DOUBLE_IMPL>,${PICO_DEFAULT_DOUBLE_IMPL}>)
+
+ pico_add_impl_library(pico_double_pico)
+ target_sources(pico_double_pico INTERFACE
+ ${CMAKE_CURRENT_LIST_DIR}/double_aeabi.S
+ ${CMAKE_CURRENT_LIST_DIR}/double_init_rom.c
+ ${CMAKE_CURRENT_LIST_DIR}/double_math.c
+ ${CMAKE_CURRENT_LIST_DIR}/double_v1_rom_shim.S
+ )
+
+ target_link_libraries(pico_double_pico INTERFACE pico_bootrom pico_double_headers)
+
+ pico_add_impl_library(pico_double_none)
+ target_sources(pico_double_none INTERFACE
+ ${CMAKE_CURRENT_LIST_DIR}/double_none.S
+ )
+
+ target_link_libraries(pico_double_none INTERFACE pico_double_headers)
+
+ target_compile_definitions(pico_double_none INTERFACE
+ PICO_PRINTF_SUPPORT_FLOAT=0 # printing floats/doubles won't work, so we can save space by removing it
+ )
+
+ function(wrap_double_functions TARGET)
+ pico_wrap_function(${TARGET} __aeabi_dadd)
+ pico_wrap_function(${TARGET} __aeabi_ddiv)
+ pico_wrap_function(${TARGET} __aeabi_dmul)
+ pico_wrap_function(${TARGET} __aeabi_drsub)
+ pico_wrap_function(${TARGET} __aeabi_dsub)
+ pico_wrap_function(${TARGET} __aeabi_cdcmpeq)
+ pico_wrap_function(${TARGET} __aeabi_cdrcmple)
+ pico_wrap_function(${TARGET} __aeabi_cdcmple)
+ pico_wrap_function(${TARGET} __aeabi_dcmpeq)
+ pico_wrap_function(${TARGET} __aeabi_dcmplt)
+ pico_wrap_function(${TARGET} __aeabi_dcmple)
+ pico_wrap_function(${TARGET} __aeabi_dcmpge)
+ pico_wrap_function(${TARGET} __aeabi_dcmpgt)
+ pico_wrap_function(${TARGET} __aeabi_dcmpun)
+ pico_wrap_function(${TARGET} __aeabi_i2d)
+ pico_wrap_function(${TARGET} __aeabi_l2d)
+ pico_wrap_function(${TARGET} __aeabi_ui2d)
+ pico_wrap_function(${TARGET} __aeabi_ul2d)
+ pico_wrap_function(${TARGET} __aeabi_d2iz)
+ pico_wrap_function(${TARGET} __aeabi_d2lz)
+ pico_wrap_function(${TARGET} __aeabi_d2uiz)
+ pico_wrap_function(${TARGET} __aeabi_d2ulz)
+ pico_wrap_function(${TARGET} __aeabi_d2f)
+ pico_wrap_function(${TARGET} sqrt)
+ pico_wrap_function(${TARGET} cos)
+ pico_wrap_function(${TARGET} sin)
+ pico_wrap_function(${TARGET} tan)
+ pico_wrap_function(${TARGET} atan2)
+ pico_wrap_function(${TARGET} exp)
+ pico_wrap_function(${TARGET} log)
+
+ pico_wrap_function(${TARGET} ldexp)
+ pico_wrap_function(${TARGET} copysign)
+ pico_wrap_function(${TARGET} trunc)
+ pico_wrap_function(${TARGET} floor)
+ pico_wrap_function(${TARGET} ceil)
+ pico_wrap_function(${TARGET} round)
+ pico_wrap_function(${TARGET} sincos) # gnu
+ pico_wrap_function(${TARGET} asin)
+ pico_wrap_function(${TARGET} acos)
+ pico_wrap_function(${TARGET} atan)
+ pico_wrap_function(${TARGET} sinh)
+ pico_wrap_function(${TARGET} cosh)
+ pico_wrap_function(${TARGET} tanh)
+ pico_wrap_function(${TARGET} asinh)
+ pico_wrap_function(${TARGET} acosh)
+ pico_wrap_function(${TARGET} atanh)
+ pico_wrap_function(${TARGET} exp2)
+ pico_wrap_function(${TARGET} log2)
+ pico_wrap_function(${TARGET} exp10)
+ pico_wrap_function(${TARGET} log10)
+ pico_wrap_function(${TARGET} pow)
+ pico_wrap_function(${TARGET} powint) #gnu
+ pico_wrap_function(${TARGET} hypot)
+ pico_wrap_function(${TARGET} cbrt)
+ pico_wrap_function(${TARGET} fmod)
+ pico_wrap_function(${TARGET} drem)
+ pico_wrap_function(${TARGET} remainder)
+ pico_wrap_function(${TARGET} remquo)
+ pico_wrap_function(${TARGET} expm1)
+ pico_wrap_function(${TARGET} log1p)
+ pico_wrap_function(${TARGET} fma)
+ endfunction()
+
+ wrap_double_functions(pico_double_pico)
+ wrap_double_functions(pico_double_none)
+
+ macro(pico_set_double_implementation TARGET IMPL)
+ get_target_property(target_type ${TARGET} TYPE)
+ if ("EXECUTABLE" STREQUAL "${target_type}")
+ set_target_properties(${TARGET} PROPERTIES PICO_TARGET_DOUBLE_IMPL "pico_double_${IMPL}")
+ else()
+ message(FATAL_ERROR "double implementation must be set on executable not library")
+ endif()
+ endmacro()
+endif()
diff --git a/src/rp2_common/pico_double/double_aeabi.S b/src/rp2_common/pico_double/double_aeabi.S
new file mode 100644
index 0000000..0c59738
--- /dev/null
+++ b/src/rp2_common/pico_double/double_aeabi.S
@@ -0,0 +1,849 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "pico/asm_helper.S"
+#include "pico/bootrom/sf_table.h"
+#include "hardware/divider_helper.S"
+
+__pre_init __aeabi_double_init, 00020
+
+.syntax unified
+.cpu cortex-m0plus
+.thumb
+
+.macro double_section name
+#if PICO_DOUBLE_IN_RAM
+.section RAM_SECTION_NAME(\name), "ax"
+#else
+.section SECTION_NAME(\name), "ax"
+#endif
+.endm
+
+.macro _double_wrapper_func x
+ wrapper_func \x
+.endm
+
+.macro wrapper_func_d1 x
+ _double_wrapper_func \x
+#if PICO_DOUBLE_PROPAGATE_NANS
+ mov ip, lr
+ bl __check_nan_d1
+ mov lr, ip
+#endif
+.endm
+
+.macro wrapper_func_d2 x
+ _double_wrapper_func \x
+#if PICO_DOUBLE_PROPAGATE_NANS
+ mov ip, lr
+ bl __check_nan_d2
+ mov lr, ip
+#endif
+.endm
+
+.section .text
+
+#if PICO_DOUBLE_PROPAGATE_NANS
+.thumb_func
+__check_nan_d1:
+ movs r3, #1
+ lsls r3, #21
+ lsls r2, r1, #1
+ adds r2, r3
+ bhi 1f
+ bx lr
+1:
+ bx ip
+
+.thumb_func
+__check_nan_d2:
+ push {r0, r2}
+ movs r2, #1
+ lsls r2, #21
+ lsls r0, r1, #1
+ adds r0, r2
+ bhi 1f
+ lsls r0, r3, #1
+ adds r0, r2
+ bhi 2f
+ pop {r0, r2}
+ bx lr
+2:
+ pop {r0, r2}
+ mov r0, r2
+ mov r1, r3
+ bx ip
+1:
+ pop {r0, r2}
+ bx ip
+#endif
+
+.macro table_tail_call SF_TABLE_OFFSET
+ push {r3, r4}
+#if PICO_DOUBLE_SUPPORT_ROM_V1 && PICO_RP2040_B0_SUPPORTED
+#ifndef NDEBUG
+ movs r3, #0
+ mov ip, r3
+#endif
+#endif
+ ldr r3, =sd_table
+ ldr r3, [r3, #\SF_TABLE_OFFSET]
+ str r3, [sp, #4]
+ pop {r3, pc}
+.endm
+
+.macro shimmable_table_tail_call SF_TABLE_OFFSET shim
+ push {r3, r4}
+ ldr r3, =sd_table
+ ldr r3, [r3, #\SF_TABLE_OFFSET]
+#if PICO_DOUBLE_SUPPORT_ROM_V1 && PICO_RP2040_B0_SUPPORTED
+ mov ip, pc
+#endif
+ str r3, [sp, #4]
+ pop {r3, pc}
+#if PICO_DOUBLE_SUPPORT_ROM_V1 && PICO_RP2040_B0_SUPPORTED
+.byte \SF_TABLE_OFFSET, 0xdf
+.word \shim
+#endif
+.endm
+
+.macro double_wrapper_section func
+double_section WRAPPER_FUNC_NAME(\func)
+.endm
+
+double_section push_r8_r11
+regular_func push_r8_r11
+ mov r4,r8
+ mov r5,r9
+ mov r6,r10
+ mov r7,r11
+ push {r4-r7}
+ bx r14
+
+double_section pop_r8_r11
+regular_func pop_r8_r11
+ pop {r4-r7}
+ mov r8,r4
+ mov r9,r5
+ mov r10,r6
+ mov r11,r7
+ bx r14
+
+// note generally each function is in a separate section unless there is fall thru or branching between them
+// note fadd, fsub, fmul, fdiv are so tiny and just defer to rom so are lumped together so they can share constant pool
+
+// note functions are word aligned except where they are an odd number of linear instructions
+
+// double FUNC_NAME(__aeabi_dadd)(double, double) double-precision addition
+double_wrapper_section __aeabi_darithmetic
+// double FUNC_NAME(__aeabi_drsub)(double x, double y) double-precision reverse subtraction, y - x
+
+// frsub first because it is the only one that needs alignment
+.align 2
+wrapper_func __aeabi_drsub
+ eors r0, r1
+ eors r1, r0
+ eors r0, r1
+ // fall thru
+
+// double FUNC_NAME(__aeabi_dsub)(double x, double y) double-precision subtraction, x - y
+wrapper_func_d2 __aeabi_dsub
+#if PICO_DOUBLE_PROPAGATE_NANS
+ // we want to return nan for inf-inf or -inf - -inf, but without too much upfront cost
+ mov ip, r0
+ mov r0, r1
+ eors r0, r3
+ bmi 1f // different signs
+ mov r0, ip
+ push {r0-r3, lr}
+ bl 2f
+ b ddiv_dsub_nan_helper
+1:
+ mov r0, ip
+2:
+#endif
+ shimmable_table_tail_call SF_TABLE_FSUB dsub_shim
+
+wrapper_func_d2 __aeabi_dadd
+ shimmable_table_tail_call SF_TABLE_FADD dadd_shim
+
+// double FUNC_NAME(__aeabi_ddiv)(double n, double d) double-precision division, n / d
+wrapper_func_d2 __aeabi_ddiv
+#if PICO_DOUBLE_PROPAGATE_NANS
+ push {r0-r3, lr}
+ bl 1f
+ b ddiv_dsub_nan_helper
+1:
+#endif
+#if !PICO_DIVIDER_DISABLE_INTERRUPTS
+ // to support IRQ usage (or context switch) we must save/restore divider state around call if state is dirty
+ mov ip, r2
+ ldr r2, =(SIO_BASE)
+ ldr r2, [r2, #SIO_DIV_CSR_OFFSET]
+ lsrs r2, #SIO_DIV_CSR_DIRTY_SHIFT_FOR_CARRY
+ bcs ddiv_save_state
+ mov r2, ip
+#else
+ // to avoid worrying about IRQs (or context switches), simply disable interrupts around call
+ push {r4, lr}
+ mrs r4, PRIMASK
+ cpsid i
+ bl ddiv_shim_call
+ msr PRIMASK, r4
+ pop {r4, pc}
+#endif
+ddiv_shim_call:
+ shimmable_table_tail_call SF_TABLE_FDIV ddiv_shim
+
+#if !PICO_DIVIDER_DISABLE_INTERRUPTS
+ddiv_save_state:
+ ldr r2, =(SIO_BASE)
+ save_div_state_and_lr
+ mov r2, ip
+ bl ddiv_shim_call
+ ldr r2, =(SIO_BASE)
+ restore_div_state_and_return
+#endif
+
+ddiv_dsub_nan_helper:
+#if PICO_DOUBLE_PROPAGATE_NANS
+ // check for infinite op infinite (or rather check for infinite result with both
+ // operands being infinite)
+ lsls r2, r1, #1
+ asrs r2, r2, #21
+ adds r2, #1
+ beq 2f
+ add sp, #16
+ pop {pc}
+2:
+ ldr r2, [sp, #4]
+ ldr r3, [sp, #12]
+ lsls r2, #1
+ asrs r2, r2, #21
+ lsls r3, #1
+ asrs r3, r3, #24
+ ands r2, r3
+ adds r2, #1
+ bne 3f
+ // infinite to nan
+ movs r2, #1
+ lsls r2, #19
+ orrs r1, r2
+3:
+ add sp, #16
+ pop {pc}
+#endif
+
+// double FUNC_NAME(__aeabi_dmul)(double, double) double-precision multiplication
+wrapper_func_d2 __aeabi_dmul
+#if PICO_DOUBLE_PROPAGATE_NANS
+ push {r0-r3, lr}
+ bl 1f
+
+ // check for multiplication of infinite by zero (or rather check for infinite result with either
+ // operand 0)
+ lsls r3, r1, #1
+ asrs r3, r3, #21
+ adds r3, #1
+ beq 2f
+ add sp, #16
+ pop {pc}
+2:
+ ldr r2, [sp, #4]
+ ldr r3, [sp, #12]
+ ands r2, r3
+ bne 3f
+ // infinite to nan
+ movs r2, #1
+ lsls r2, #19
+ orrs r1, r2
+3:
+ add sp, #16
+ pop {pc}
+1:
+#endif
+ shimmable_table_tail_call SF_TABLE_FMUL dmul_shim
+
+// void FUNC_NAME(__aeabi_cdrcmple)(double, double) reversed 3-way (<, =, ?>) compare [1], result in PSR ZC flags
+double_wrapper_section __aeabi_cdcmple
+
+wrapper_func __aeabi_cdrcmple
+ push {r0-r7,r14}
+ eors r0, r2
+ eors r2, r0
+ eors r0, r2
+ eors r1, r3
+ eors r3, r1
+ eors r1, r3
+ b __aeabi_dfcmple_guts
+
+// NOTE these share an implementation as we have no excepting NaNs.
+// void FUNC_NAME(__aeabi_cdcmple)(double, double) 3-way (<, =, ?>) compare [1], result in PSR ZC flags
+// void FUNC_NAME(__aeabi_cdcmpeq)(double, double) non-excepting equality comparison [1], result in PSR ZC flags
+@ compare r0:r1 against r2:r3, returning -1/0/1 for <, =, >
+@ also set flags accordingly
+.align 2
+wrapper_func __aeabi_cdcmple
+wrapper_func __aeabi_cdcmpeq
+ push {r0-r7,r14}
+__aeabi_dfcmple_guts:
+ ldr r7,=#0x7ff @ flush NaNs and denormals
+ lsls r4,r1,#1
+ lsrs r4,#21
+ beq 1f
+ cmp r4,r7
+ bne 2f
+ lsls r4, r1, #12
+ bhi 7f
+1:
+ movs r0,#0
+ lsrs r1,#20
+ lsls r1,#20
+2:
+ lsls r4,r3,#1
+ lsrs r4,#21
+ beq 1f
+ cmp r4,r7
+ bne 2f
+ lsls r4, r3, #12
+ bhi 7f
+1:
+ movs r2,#0
+ lsrs r3,#20
+ lsls r3,#20
+2:
+ movs r6,#1
+ eors r3,r1
+ bmi 4f @ opposite signs? then can proceed on basis of sign of x
+ eors r3,r1 @ restore r3
+ bpl 2f
+ cmp r3,r1
+ bne 7f
+1:
+ cmp r2,r0
+7:
+ pop {r0-r7,r15}
+2:
+ cmp r1,r3
+ bne 7b
+1:
+ cmp r0,r2
+ pop {r0-r7,r15}
+4:
+ orrs r3,r1 @ make -0==+0
+ adds r3,r3
+ orrs r3,r0
+ orrs r3,r2
+ beq 7b
+ mvns r1, r1 @ carry inverse of r1 sign
+ adds r1, r1
+ pop {r0-r7,r15}
+
+
+// int FUNC_NAME(__aeabi_dcmpeq)(double, double) result (1, 0) denotes (=, ?<>) [2], use for C == and !=
+double_wrapper_section __aeabi_dcmpeq
+.align 2
+wrapper_func __aeabi_dcmpeq
+ push {lr}
+ bl __aeabi_cdcmpeq
+ beq 1f
+ movs r0, #0
+ pop {pc}
+1:
+ movs r0, #1
+ pop {pc}
+
+// int FUNC_NAME(__aeabi_dcmplt)(double, double) result (1, 0) denotes (<, ?>=) [2], use for C <
+double_wrapper_section __aeabi_dcmplt
+.align 2
+wrapper_func __aeabi_dcmplt
+ push {lr}
+ bl __aeabi_cdcmple
+ sbcs r0, r0
+ pop {pc}
+
+// int FUNC_NAME(__aeabi_dcmple)(double, double) result (1, 0) denotes (<=, ?>) [2], use for C <=
+double_wrapper_section __aeabi_dcmple
+.align 2
+wrapper_func __aeabi_dcmple
+ push {lr}
+ bl __aeabi_cdcmple
+ bls 1f
+ movs r0, #0
+ pop {pc}
+1:
+ movs r0, #1
+ pop {pc}
+
+// int FUNC_NAME(__aeabi_dcmpge)(double, double) result (1, 0) denotes (>=, ?<) [2], use for C >=
+double_wrapper_section __aeabi_dcmpge
+.align 2
+wrapper_func __aeabi_dcmpge
+ push {lr}
+ // because of NaNs it is better to reverse the args than the result
+ bl __aeabi_cdrcmple
+ bls 1f
+ movs r0, #0
+ pop {pc}
+1:
+ movs r0, #1
+ pop {pc}
+
+// int FUNC_NAME(__aeabi_dcmpgt)(double, double) result (1, 0) denotes (>, ?<=) [2], use for C >
+double_wrapper_section __aeabi_dcmpgt
+wrapper_func __aeabi_dcmpgt
+ push {lr}
+ // because of NaNs it is better to reverse the args than the result
+ bl __aeabi_cdrcmple
+ sbcs r0, r0
+ pop {pc}
+
+// int FUNC_NAME(__aeabi_dcmpun)(double, double) result (1, 0) denotes (?, <=>) [2], use for C99 isunordered()
+double_wrapper_section __aeabi_dcmpun
+wrapper_func __aeabi_dcmpun
+ movs r0, #1
+ lsls r0, #21
+ lsls r2, r1, #1
+ adds r2, r0
+ bhi 1f
+ lsls r2, r3, #1
+ adds r2, r0
+ bhi 1f
+ movs r0, #0
+ bx lr
+1:
+ movs r0, #1
+ bx lr
+
+ movs r0, #0
+ bx lr
+
+// double FUNC_NAME(__aeabi_ui2d)(unsigned) unsigned to double (double precision) conversion
+double_wrapper_section __aeabi_ui2d
+ shimmable_table_tail_call SF_TABLE_UINT2FLOAT uint2double_shim
+
+double_wrapper_section __aeabi_i2d
+
+wrapper_func __aeabi_ui2d
+ movs r1, #0
+ cmp r0, #0
+ bne 2f
+1:
+ bx lr
+// double FUNC_NAME(__aeabi_i2d)(int) integer to double (double precision) conversion
+wrapper_func __aeabi_i2d
+ asrs r1, r0, #31
+ eors r0, r1
+ subs r0, r1
+ beq 1b
+ lsls r1, #31
+2:
+ push {r0, r1, r4, lr}
+ ldr r3, =sf_clz_func
+ ldr r3, [r3]
+ blx r3
+ pop {r2, r3}
+ adds r4, r0, #1
+ lsls r2, r4
+ lsls r0, r2, #20
+ lsrs r2, #12
+ ldr r1,=#1055
+ subs r1, r4
+ lsls r1, #20
+ orrs r1, r3
+ orrs r1, r2
+ pop {r4, pc}
+
+// int FUNC_NAME(__aeabi_d2iz)(double) double (double precision) to integer C-style conversion [3]
+double_wrapper_section __aeabi_d2iz
+wrapper_func __aeabi_d2iz
+regular_func double2int_z
+ push {r4, lr}
+ lsls r4, r1, #1
+ lsrs r2, r4, #21
+ movs r3, #0x80
+ adds r2, r3
+ lsls r3, #3
+ subs r2, r3
+ lsls r3, #21
+ cmp r2, #126
+ ble 1f
+ subs r2, #158
+ bge 2f
+ asrs r4, r1, #31
+ lsls r1, #12
+ lsrs r1, #1
+ orrs r1, r3
+ negs r2, r2
+ lsrs r1, r2
+ lsls r4, #1
+ adds r4, #1
+ adds r2, #21
+ cmp r2, #32
+ bge 3f
+ lsrs r0, r2
+ orrs r0, r1
+ muls r0, r4
+ pop {r4, pc}
+1:
+ movs r0, #0
+ pop {r4, pc}
+3:
+ mov r0, r1
+ muls r0, r4
+ pop {r4, pc}
+2:
+ // overflow
+ lsrs r0, r1, #31
+ adds r0, r3
+ subs r0, #1
+ pop {r4, pc}
+
+double_section double2int
+regular_func double2int
+ shimmable_table_tail_call SF_TABLE_FLOAT2INT double2int_shim
+
+// unsigned FUNC_NAME(__aeabi_d2uiz)(double) double (double precision) to unsigned C-style conversion [3]
+double_wrapper_section __aeabi_d2uiz
+wrapper_func __aeabi_d2uiz
+regular_func double2uint
+ shimmable_table_tail_call SF_TABLE_FLOAT2UINT double2uint_shim
+
+double_section fix2double
+regular_func fix2double
+ shimmable_table_tail_call SF_TABLE_FIX2FLOAT fix2double_shim
+
+double_section ufix2double
+regular_func ufix2double
+ shimmable_table_tail_call SF_TABLE_UFIX2FLOAT ufix2double_shim
+
+double_section fix642double
+regular_func fix642double
+ shimmable_table_tail_call SF_TABLE_FIX642FLOAT fix642double_shim
+
+double_section ufix2double
+regular_func ufix642double
+ shimmable_table_tail_call SF_TABLE_UFIX642FLOAT ufix642double_shim
+
+// double FUNC_NAME(__aeabi_l2d)(long long) long long to double (double precision) conversion
+double_wrapper_section __aeabi_l2d
+wrapper_func __aeabi_l2d
+ shimmable_table_tail_call SF_TABLE_INT642FLOAT int642double_shim
+
+// double FUNC_NAME(__aeabi_l2f)(long long) long long to double (double precision) conversion
+double_wrapper_section __aeabi_ul2d
+wrapper_func __aeabi_ul2d
+ shimmable_table_tail_call SF_TABLE_UINT642FLOAT uint642double_shim
+
+// long long FUNC_NAME(__aeabi_d2lz)(double) double (double precision) to long long C-style conversion [3]
+double_wrapper_section __aeabi_d2lz
+wrapper_func __aeabi_d2lz
+regular_func double2int64_z
+ cmn r1, r1
+ bcc double2int64
+ push {lr}
+ lsls r1, #1
+ lsrs r1, #1
+ movs r2, #0
+ bl double2ufix64
+ cmp r1, #0
+ bmi 1f
+ movs r2, #0
+ rsbs r0, #0
+ sbcs r2, r1
+ mov r1, r2
+ pop {pc}
+1:
+ movs r1, #128
+ lsls r1, #24
+ movs r0, #0
+ pop {pc}
+
+double_section double2int64
+regular_func double2int64
+ shimmable_table_tail_call SF_TABLE_FLOAT2INT64 double2int64_shim
+
+// unsigned long long FUNC_NAME(__aeabi_d2ulz)(double) double to unsigned long long C-style conversion [3]
+double_wrapper_section __aeabi_d2ulz
+wrapper_func __aeabi_d2ulz
+ shimmable_table_tail_call SF_TABLE_FLOAT2UINT64 double2uint64_shim
+
+double_section double2fix64
+regular_func double2fix64
+ shimmable_table_tail_call SF_TABLE_FLOAT2FIX64 double2fix64_shim
+
+double_section double2ufix64
+regular_func double2ufix64
+ shimmable_table_tail_call SF_TABLE_FLOAT2UFIX64 double2ufix64_shim
+
+double_section double2fix
+regular_func double2fix
+ shimmable_table_tail_call SF_TABLE_FLOAT2FIX double2fix_shim
+
+double_section double2ufix
+regular_func double2ufix
+ shimmable_table_tail_call SF_TABLE_FLOAT2UFIX double2ufix_shim
+
+double_wrapper_section __aeabi_d2f
+1:
+#if PICO_DOUBLE_PROPAGATE_NANS
+ // copy sign bit and 23 NAN id bits into sign bit and significant id bits, also set high id bit
+
+ lsrs r0, #30
+ lsls r2, r1, #12
+ lsrs r2, #9
+ asrs r1, #22
+ lsls r1, #22
+ orrs r0, r1
+ orrs r0, r2
+ bx lr
+#endif
+wrapper_func __aeabi_d2f
+#if PICO_DOUBLE_PROPAGATE_NANS
+ movs r3, #1
+ lsls r3, #21
+ lsls r2, r1, #1
+ adds r2, r3
+ bhi 1b
+#endif
+ // note double->float in double table at same index as float->double in double table
+ shimmable_table_tail_call SF_TABLE_FLOAT2DOUBLE double2float_shim
+
+double_wrapper_section srqt
+wrapper_func_d1 sqrt
+ shimmable_table_tail_call SF_TABLE_FSQRT dsqrt_shim
+
+double_wrapper_section sincostan_remainder
+regular_func sincostan_remainder
+ ldr r2, =0x54442D18 // 2 * M_PI
+ ldr r3, =0x401921FB
+ push {lr}
+ // note remainder only uses the divider thru integer divider functions
+ // which save and restore themselves
+ bl remainder
+ pop {pc}
+
+double_wrapper_section cos
+#don't use _d1 as we're doing a range check anyway and infinites/nans are bigger than 1024
+wrapper_func cos
+ // rom version only works for -1024 < angle < 1024
+ lsls r2, r1, #2
+ bcc 1f
+ lsrs r2, #22
+ cmp r2, #9
+ bge 2f
+1:
+ shimmable_table_tail_call SF_TABLE_FCOS dcos_shim
+2:
+#if PICO_DOUBLE_PROPAGATE_NANS
+ lsls r2, r1, #1
+ asrs r2, #21
+ adds r2, #1
+ bne 3f
+ // infinite to nan
+ movs r2, #1
+ lsls r2, #19
+ orrs r1, r2
+ bx lr
+3:
+#endif
+ push {lr}
+ bl sincostan_remainder
+ pop {r2}
+ mov lr, r2
+ b 1b
+
+double_wrapper_section sin
+#don't use _d1 as we're doing a range check anyway and infinites/nans are bigger than 1024
+wrapper_func sin
+ // rom version only works for -1024 < angle < 1024
+ lsls r2, r1, #2
+ bcc 1f
+ lsrs r2, #22
+ cmp r2, #9
+ bge 2f
+1:
+ shimmable_table_tail_call SF_TABLE_FSIN dsin_shim
+2:
+#if PICO_DOUBLE_PROPAGATE_NANS
+ lsls r2, r1, #1
+ asrs r2, #21
+ adds r2, #1
+ bne 3f
+ // infinite to nan
+ movs r2, #1
+ lsls r2, #19
+ orrs r1, r2
+ bx lr
+3:
+#endif
+ push {lr}
+ bl sincostan_remainder
+ pop {r2}
+ mov lr, r2
+ b 1b
+
+double_wrapper_section sincos
+ // out of line remainder code for abs(angle)>=1024
+2:
+#if PICO_DOUBLE_PROPAGATE_NANS
+ lsls r2, r1, #1
+ asrs r2, #21
+ adds r2, #1
+ bne 3f
+ // infinite to nan
+ movs r2, #1
+ lsls r2, #19
+ orrs r1, r2
+ pop {r4-r5}
+ stmia r4!, {r0, r1}
+ stmia r5!, {r0, r1}
+ pop {r4, r5, pc}
+3:
+#endif
+ push {lr}
+ bl sincostan_remainder
+ pop {r2}
+ mov lr, r2
+ b 1f // continue with sincos
+
+wrapper_func sincos
+ push {r2-r5, lr}
+ // rom version only works for -1024 < angle < 1024
+ lsls r2, r1, #2
+ bcc 1f
+ lsrs r2, #22
+ cmp r2, #9
+ bge 2b
+1:
+ bl 2f // call the shim
+ pop {r4-r5}
+ stmia r4!, {r0, r1}
+ stmia r5!, {r2, r3}
+ pop {r4, r5, pc}
+
+2:
+ shimmable_table_tail_call SF_TABLE_V3_FSINCOS sincos_shim_bootstrap
+
+.thumb_func
+sincos_shim_bootstrap:
+ push {r2, r3, r4}
+ movs r3, #0x13
+ ldrb r3, [r3]
+#if PICO_DOUBLE_SUPPORT_ROM_V1 && PICO_RP2040_B0_SUPPORTED
+ cmp r3, #1
+ bne 1f
+ ldr r3, =dsincos_shim
+ b 2f
+#endif
+1:
+ ldr r3, =dsincos_shim_v2
+2:
+ ldr r2, =sd_table
+ str r3, [r2, #SF_TABLE_V3_FSINCOS]
+ str r3, [sp, #8]
+ pop {r2, r3, pc}
+.thumb_func
+dsincos_shim_v2:
+ push {r4-r7,r14}
+ bl push_r8_r11
+ bl v2_rom_dsincos_internal
+ mov r12,r0 @ save ε
+ bl v2_rom_dcos_finish
+ push {r0,r1}
+ mov r0,r12
+ bl v2_rom_dsin_finish
+ pop {r2,r3}
+ bl pop_r8_r11
+ pop {r4-r7,r15}
+.thumb_func
+v2_rom_dsincos_internal:
+ push {r0, lr}
+ ldr r0, =0x3855
+ str r0, [sp, #4]
+ pop {r0, pc}
+.thumb_func
+v2_rom_dcos_finish:
+ push {r0, r1}
+ ldr r0, =0x389d
+ str r0, [sp, #4]
+ pop {r0, pc}
+.thumb_func
+v2_rom_dsin_finish:
+ push {r0, r1}
+ ldr r0, =0x38d9
+ str r0, [sp, #4]
+ pop {r0, pc}
+
+double_wrapper_section tan
+#don't use _d1 as we're doing a range check anyway and infinites/nans are bigger than 1024
+wrapper_func tan
+ // rom version only works for -1024 < angle < 1024
+ lsls r2, r1, #2
+ bcc dtan_in_range
+ lsrs r2, #22
+ cmp r2, #9
+ bge dtan_angle_out_of_range
+dtan_in_range:
+#if !PICO_DIVIDER_DISABLE_INTERRUPTS
+ // to support IRQ usage (or context switch) we must save/restore divider state around call if state is dirty
+ mov ip, r2
+ ldr r2, =(SIO_BASE)
+ ldr r2, [r2, #SIO_DIV_CSR_OFFSET]
+ lsrs r2, #SIO_DIV_CSR_DIRTY_SHIFT_FOR_CARRY
+ bcs dtan_save_state
+ mov r2, ip
+#else
+ // to avoid worrying about IRQs (or context switches), simply disable interrupts around call
+ push {r4, lr}
+ mrs r4, PRIMASK
+ cpsid i
+ bl dtan_shim_call
+ msr PRIMASK, r4
+ pop {r4, pc}
+#endif
+dtan_shim_call:
+ shimmable_table_tail_call SF_TABLE_FTAN dtan_shim
+#if !PICO_DIVIDER_DISABLE_INTERRUPTS
+dtan_save_state:
+ ldr r2, =(SIO_BASE)
+ save_div_state_and_lr
+ mov r2, ip
+ bl dtan_shim_call
+ ldr r2, =(SIO_BASE)
+ restore_div_state_and_return
+#endif
+dtan_angle_out_of_range:
+#if PICO_DOUBLE_PROPAGATE_NANS
+ lsls r2, r1, #1
+ asrs r2, #21
+ adds r2, #1
+ bne 3f
+ // infinite to nan
+ movs r2, #1
+ lsls r2, #19
+ orrs r1, r2
+ bx lr
+3:
+#endif
+ push {lr}
+ bl sincostan_remainder
+ pop {r2}
+ mov lr, r2
+ b dtan_in_range
+
+double_wrapper_section atan2
+wrapper_func_d2 atan2
+ shimmable_table_tail_call SF_TABLE_FATAN2 datan2_shim
+
+double_wrapper_section exp
+wrapper_func_d1 exp
+ shimmable_table_tail_call SF_TABLE_FEXP dexp_shim
+
+double_wrapper_section log
+wrapper_func_d1 log
+ shimmable_table_tail_call SF_TABLE_FLN dln_shim
+
diff --git a/src/rp2_common/pico_double/double_init_rom.c b/src/rp2_common/pico_double/double_init_rom.c
new file mode 100644
index 0000000..af6f6a2
--- /dev/null
+++ b/src/rp2_common/pico_double/double_init_rom.c
@@ -0,0 +1,70 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <string.h>
+#include "pico/bootrom.h"
+#include "pico/bootrom/sf_table.h"
+
+// NOTE THIS FUNCTION TABLE IS NOT PUBLIC OR NECESSARILY COMPLETE...
+// IT IS ***NOT*** SAFE TO CALL THESE FUNCTION POINTERS FROM ARBITRARY CODE
+uint32_t sd_table[SF_TABLE_V2_SIZE / 2];
+
+#if !(PICO_DOUBLE_SUPPORT_ROM_V1 && PICO_RP2040_B0_SUPPORTED)
+static __attribute__((noreturn)) void missing_double_func_shim(void) {
+ panic("missing double function");
+}
+#endif
+extern void double_table_shim_on_use_helper(void);
+
+void __attribute__((weak)) *sf_clz_func;
+
+void __aeabi_double_init(void) {
+ int rom_version = rp2040_rom_version();
+#if PICO_DOUBLE_SUPPORT_ROM_V1 && PICO_RP2040_B0_SUPPORTED
+ if (rom_version == 1) {
+
+ // this is a little tricky.. we only want to pull in a shim if the corresponding function
+ // is called. to that end we include a SVC instruction with the table offset as the call number
+ // followed by the shim function pointer inside the actual wrapper function. that way if the wrapper
+ // function is garbage collected, so is the shim function.
+ //
+ // double_table_shim_on_use_helper expects this SVC instruction in the calling code soon after the address
+ // pointed to by IP and patches the double_table entry with the real shim the first time the function is called.
+ for(uint i=0; i<SF_TABLE_V2_SIZE/4; i++) {
+ sd_table[i] = (uintptr_t)double_table_shim_on_use_helper;
+ }
+ }
+#else
+ if (rom_version == 1) {
+ // opting for soft failure for now - you'll get a panic at runtime if you call any of the missing methods
+ for(uint i=0;i<SF_TABLE_V2_SIZE/4;i++) {
+ sd_table[i] = (uintptr_t)missing_double_func_shim;
+ }
+ }
+#endif
+ if (rom_version >= 2) {
+ void *rom_table = rom_data_lookup(rom_table_code('S', 'D'));
+ assert(*((uint8_t *)(((void *)rom_data_lookup(rom_table_code('S', 'F')))-2)) * 4 >= SF_TABLE_V2_SIZE);
+ memcpy(&sd_table, rom_table, SF_TABLE_V2_SIZE);
+ if (rom_version == 2) {
+#ifndef NDEBUG
+ if (*(uint16_t *)0x3854 != 0xb500 || // this is dsincos(_internal)
+
+ *(uint16_t *)0x38d8 != 0x4649 || // this is dsin_finish
+ *(uint16_t *)0x389c != 0x4659 // this is dcos_finish
+ ) {
+ panic(NULL);
+ }
+#endif
+ }
+ }
+ if (rom_version < 3) {
+ // we use the unused entry for SINCOS
+ sd_table[SF_TABLE_V3_FSINCOS / 4] = (uintptr_t) double_table_shim_on_use_helper;
+ }
+
+ sf_clz_func = rom_func_lookup(ROM_FUNC_CLZ32);
+}
diff --git a/src/rp2_common/pico_double/double_math.c b/src/rp2_common/pico_double/double_math.c
new file mode 100644
index 0000000..6c35ded
--- /dev/null
+++ b/src/rp2_common/pico_double/double_math.c
@@ -0,0 +1,626 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <math.h>
+#include "pico/types.h"
+#include "pico/double.h"
+#include "pico/platform.h"
+
+// opened a separate issue https://github.com/raspberrypi/pico-sdk/issues/166 to deal with these warnings if at all
+_Pragma("GCC diagnostic push")
+_Pragma("GCC diagnostic ignored \"-Wconversion\"")
+_Pragma("GCC diagnostic ignored \"-Wsign-conversion\"")
+
+typedef uint64_t ui64;
+typedef uint32_t ui32;
+typedef int64_t i64;
+
+#define PINF ( HUGE_VAL)
+#define MINF (-HUGE_VAL)
+#define PZERO (+0.0)
+#define MZERO (-0.0)
+
+
+#define PI 3.14159265358979323846
+#define LOG2 0.69314718055994530941
+// Unfortunately in double precision ln(10) is very close to half-way between to representable numbers
+#define LOG10 2.30258509299404568401
+#define LOG2E 1.44269504088896340737
+#define LOG10E 0.43429448190325182765
+#define ONETHIRD 0.33333333333333333333
+
+#define PIf 3.14159265358979323846f
+#define LOG2f 0.69314718055994530941f
+#define LOG2Ef 1.44269504088896340737f
+#define LOG10Ef 0.43429448190325182765f
+#define ONETHIRDf 0.33333333333333333333f
+
+#define DUNPACK(x,e,m) e=((x)>>52)&0x7ff,m=((x)&0x000fffffffffffffULL)|0x0010000000000000ULL
+#define DUNPACKS(x,s,e,m) s=((x)>>63),DUNPACK((x),(e),(m))
+
+typedef union {
+ double d;
+ ui64 ix;
+} double_ui64;
+
+static inline double ui642double(ui64 ix) {
+ double_ui64 tmp;
+ tmp.ix = ix;
+ return tmp.d;
+}
+
+static inline ui64 double2ui64(double d) {
+ double_ui64 tmp;
+ tmp.d = d;
+ return tmp.ix;
+}
+
+static inline bool disnan(double x) {
+ ui64 ix= double2ui64(x);
+ // checks the top bit of the low 32 bit of the NAN, but it I think that is ok
+ return ((uint32_t)(ix >> 31)) > 0xffe00000u;
+}
+
+#if PICO_DOUBLE_PROPAGATE_NANS
+#define check_nan_d1(x) if (disnan((x))) return (x)
+#define check_nan_d2(x,y) if (disnan((x))) return (x); else if (disnan((y))) return (y);
+#else
+#define check_nan_d1(x) ((void)0)
+#define check_nan_d2(x,y) ((void)0)
+#endif
+
+static inline int dgetsignexp(double x) {
+ ui64 ix=double2ui64(x);
+ return (ix>>52)&0xfff;
+}
+
+static inline int dgetexp(double x) {
+ ui64 ix=double2ui64(x);
+ return (ix>>52)&0x7ff;
+}
+
+static inline double dldexp(double x,int de) {
+ ui64 ix=double2ui64(x),iy;
+ int e;
+ e=dgetexp(x);
+ if(e==0||e==0x7ff) return x;
+ e+=de;
+ if(e<=0) iy=ix&0x8000000000000000ULL; // signed zero for underflow
+ else if(e>=0x7ff) iy=(ix&0x8000000000000000ULL)|0x7ff0000000000000ULL; // signed infinity on overflow
+ else iy=ix+((ui64)de<<52);
+ return ui642double(iy);
+}
+
+double WRAPPER_FUNC(ldexp)(double x, int de) {
+ check_nan_d1(x);
+ return dldexp(x, de);
+}
+
+
+static inline double dcopysign(double x,double y) {
+ ui64 ix=double2ui64(x),iy=double2ui64(y);
+ ix=((ix&0x7fffffffffffffffULL)|(iy&0x8000000000000000ULL));
+ return ui642double(ix);
+}
+
+double WRAPPER_FUNC(copysign)(double x, double y) {
+ check_nan_d2(x,y);
+ return dcopysign(x, y);
+}
+static inline int diszero(double x) { return dgetexp (x)==0; }
+static inline int dispzero(double x) { return dgetsignexp(x)==0; }
+static inline int dismzero(double x) { return dgetsignexp(x)==0x800; }
+static inline int disinf(double x) { return dgetexp (x)==0x7ff; }
+static inline int dispinf(double x) { return dgetsignexp(x)==0x7ff; }
+static inline int disminf(double x) { return dgetsignexp(x)==0xfff; }
+
+static inline int disint(double x) {
+ ui64 ix=double2ui64(x),m;
+ int e=dgetexp(x);
+ if(e==0) return 1; // 0 is an integer
+ e-=0x3ff; // remove exponent bias
+ if(e<0) return 0; // |x|<1
+ e=52-e; // bit position in mantissa with significance 1
+ if(e<=0) return 1; // |x| large, so must be an integer
+ m=(1ULL<<e)-1; // mask for bits of significance <1
+ if(ix&m) return 0; // not an integer
+ return 1;
+}
+
+static inline int disoddint(double x) {
+ ui64 ix=double2ui64(x),m;
+ int e=dgetexp(x);
+ e-=0x3ff; // remove exponent bias
+ if(e<0) return 0; // |x|<1; 0 is not odd
+ e=52-e; // bit position in mantissa with significance 1
+ if(e<0) return 0; // |x| large, so must be even
+ m=(1ULL<<e)-1; // mask for bits of significance <1 (if any)
+ if(ix&m) return 0; // not an integer
+ if(e==52) return 1; // value is exactly 1
+ return (ix>>e)&1;
+}
+
+static inline int disstrictneg(double x) {
+ ui64 ix=double2ui64(x);
+ if(diszero(x)) return 0;
+ return ix>>63;
+}
+
+static inline int disneg(double x) {
+ ui64 ix=double2ui64(x);
+ return ix>>63;
+}
+
+static inline double dneg(double x) {
+ ui64 ix=double2ui64(x);
+ ix^=0x8000000000000000ULL;
+ return ui642double(ix);
+}
+
+static inline int dispo2(double x) {
+ ui64 ix=double2ui64(x);
+ if(diszero(x)) return 0;
+ if(disinf(x)) return 0;
+ ix&=0x000fffffffffffffULL;
+ return ix==0;
+}
+
+static inline double dnan_or(double x) {
+#if PICO_DOUBLE_PROPAGATE_NANS
+ return NAN;
+#else
+ return x;
+#endif
+}
+
+double WRAPPER_FUNC(trunc)(double x) {
+ check_nan_d1(x);
+ ui64 ix=double2ui64(x),m;
+ int e=dgetexp(x);
+ e-=0x3ff; // remove exponent bias
+ if(e<0) { // |x|<1
+ ix&=0x8000000000000000ULL;
+ return ui642double(ix);
+ }
+ e=52-e; // bit position in mantissa with significance 1
+ if(e<=0) return x; // |x| large, so must be an integer
+ m=(1ULL<<e)-1; // mask for bits of significance <1
+ ix&=~m;
+ return ui642double(ix);
+}
+
+double WRAPPER_FUNC(round)(double x) {
+ check_nan_d1(x);
+ ui64 ix=double2ui64(x),m;
+ int e=dgetexp(x);
+ e-=0x3ff; // remove exponent bias
+ if(e<-1) { // |x|<0.5
+ ix&=0x8000000000000000ULL;
+ return ui642double(ix);
+ }
+ if(e==-1) { // 0.5<=|x|<1
+ ix&=0x8000000000000000ULL;
+ ix|=0x3ff0000000000000ULL; // ±1
+ return ui642double(ix);
+ }
+ e=52-e; // bit position in mantissa with significance 1, <=52
+ if(e<=0) return x; // |x| large, so must be an integer
+ m=1ULL<<(e-1); // mask for bit of significance 0.5
+ ix+=m;
+ m=m+m-1; // mask for bits of significance <1
+ ix&=~m;
+ return ui642double(ix);
+}
+
+double WRAPPER_FUNC(floor)(double x) {
+ check_nan_d1(x);
+ ui64 ix=double2ui64(x),m;
+ int e=dgetexp(x);
+ if(e==0) { // x==0
+ ix&=0x8000000000000000ULL;
+ return ui642double(ix);
+ }
+ e-=0x3ff; // remove exponent bias
+ if(e<0) { // |x|<1, not zero
+ if(disneg(x)) return -1;
+ return PZERO;
+ }
+ e=52-e; // bit position in mantissa with significance 1
+ if(e<=0) return x; // |x| large, so must be an integer
+ m=(1ULL<<e)-1; // mask for bit of significance <1
+ if(disneg(x)) ix+=m; // add 1-ε to magnitude if negative
+ ix&=~m; // truncate
+ return ui642double(ix);
+}
+
+double WRAPPER_FUNC(ceil)(double x) {
+ check_nan_d1(x);
+ ui64 ix=double2ui64(x),m;
+ int e=dgetexp(x);
+ if(e==0) { // x==0
+ ix&=0x8000000000000000ULL;
+ return ui642double(ix);
+ }
+ e-=0x3ff; // remove exponent bias
+ if(e<0) { // |x|<1, not zero
+ if(disneg(x)) return MZERO;
+ return 1;
+ }
+ e=52-e; // bit position in mantissa with significance 1
+ if(e<=0) return x; // |x| large, so must be an integer
+ m=(1ULL<<e)-1; // mask for bit of significance <1
+ if(!disneg(x)) ix+=m; // add 1-ε to magnitude if positive
+ ix&=~m; // truncate
+ return ui642double(ix);
+}
+
+double WRAPPER_FUNC(asin)(double x) {
+ check_nan_d1(x);
+ double u;
+ u=(1-x)*(1+x);
+ if(disstrictneg(u)) return dnan_or(PINF);
+ return atan2(x,sqrt(u));
+}
+
+double WRAPPER_FUNC(acos)(double x) {
+ check_nan_d1(x);
+ double u;
+ u=(1-x)*(1+x);
+ if(disstrictneg(u)) return dnan_or(PINF);
+ return atan2(sqrt(u),x);
+}
+
+double WRAPPER_FUNC(atan)(double x) {
+ check_nan_d1(x);
+ if(dispinf(x)) return PI/2;
+ if(disminf(x)) return -PI/2;
+ return atan2(x,1);
+}
+
+double WRAPPER_FUNC(sinh)(double x) {
+ check_nan_d1(x);
+ return dldexp((exp(x)-exp(dneg(x))),-1);
+}
+
+double WRAPPER_FUNC(cosh)(double x) {
+ check_nan_d1(x);
+ return dldexp((exp(x)+exp(dneg(x))),-1);
+}
+
+double WRAPPER_FUNC(tanh)(double x) {
+ check_nan_d1(x);
+ double u;
+ int e;
+ e=dgetexp(x);
+ if(e>=5+0x3ff) { // |x|>=32?
+ if(!disneg(x)) return 1; // 1 << exp 2x; avoid generating infinities later
+ else return -1; // 1 >> exp 2x
+ }
+ u=exp(dldexp(x,1));
+ return (u-1)/(u+1);
+}
+
+double WRAPPER_FUNC(asinh)(double x) {
+ check_nan_d1(x);
+ int e;
+ e=dgetexp(x);
+ if(e>=32+0x3ff) { // |x|>=2^32?
+ if(!disneg(x)) return log( x )+LOG2; // 1/x^2 << 1
+ else return dneg(log(dneg(x))+LOG2); // 1/x^2 << 1
+ }
+ if(x>0) return log(sqrt(x*x+1)+x);
+ else return dneg(log(sqrt(x*x+1)-x));
+}
+
+double WRAPPER_FUNC(acosh)(double x) {
+ check_nan_d1(x);
+ int e;
+ if(disneg(x)) x=dneg(x);
+ e=dgetexp(x);
+ if(e>=32+0x3ff) return log(x)+LOG2; // |x|>=2^32?
+ return log(sqrt((x-1)*(x+1))+x);
+}
+
+double WRAPPER_FUNC(atanh)(double x) {
+ check_nan_d1(x);
+ return dldexp(log((1+x)/(1-x)),-1);
+}
+
+double WRAPPER_FUNC(exp2)(double x) {
+ check_nan_d1(x);
+ int e;
+ // extra check for disminf as this catches -Nan, and x<=-4096 doesn't.
+ if (disminf(x) || x<=-4096) return 0; // easily underflows
+ else if (x>=4096) return PINF; // easily overflows
+ e=(int)round(x);
+ x-=e;
+ return dldexp(exp(x*LOG2),e);
+}
+double WRAPPER_FUNC(log2)(double x) { check_nan_d1(x); return log(x)*LOG2E; }
+double WRAPPER_FUNC(exp10)(double x) { check_nan_d1(x); return pow(10,x); }
+double WRAPPER_FUNC(log10)(double x) { check_nan_d1(x); return log(x)*LOG10E; }
+
+// todo these are marked as lofi
+double WRAPPER_FUNC(expm1(double x) { check_nan_d1(x); return exp)(x)-1; }
+double WRAPPER_FUNC(log1p(double x) { check_nan_d1(x); return log)(1+x); }
+double WRAPPER_FUNC(fma)(double x,double y,double z) { check_nan_d1(x); return x*y+z; }
+
+// general power, x>0, finite
+static double dpow_1(double x,double y) {
+ int a,b,c;
+ double t,rt,u,v,v0,v1,w,ry;
+ a=dgetexp(x)-0x3ff;
+ u=log2(dldexp(x,-a)); // now log_2 x = a+u
+ if(u>0.5) u-=1,a++; // |u|<=~0.5
+ if(a==0) return exp2(u*y);
+ // here |log_2 x| >~0.5
+ if(y>= 4096) { // then easily over/underflows
+ if(a<0) return 0;
+ return PINF;
+ }
+ if(y<=-4096) { // then easily over/underflows
+ if(a<0) return PINF;
+ return 0;
+ }
+ ry=round(y);
+ v=y-ry;
+ v0=dldexp(round(ldexp(v,26)),-26);
+ v1=v-v0;
+ b=(int)ry; // guaranteed to fit in an int; y=b+v0+v1
+ // now the result is exp2( (a+u) * (b+v0+v1) )
+ c=a*b; // integer
+ t=a*v0;
+ rt=round(t);
+ c+=(int)rt;
+ w=t-rt;
+ t=a*v1;
+ w+=t;
+ t=u*b;
+ rt=round(t);
+ c+=(int)rt;
+ w+=t-rt;
+ w+=u*v;
+ return dldexp(exp2(w),c);
+}
+
+static double dpow_int2(double x,int y) {
+ double u;
+ if(y==1) return x;
+ u=dpow_int2(x,y/2);
+ u*=u;
+ if(y&1) u*=x;
+ return u;
+}
+
+// for the case where x not zero or infinity, y small and not zero
+static inline double dpowint_1(double x,int y) {
+ if(y<0) x=1/x,y=-y;
+ return dpow_int2(x,y);
+}
+
+// for the case where x not zero or infinity
+static double dpowint_0(double x,int y) {
+ int e;
+ if(disneg(x)) {
+ if(disoddint(y)) return dneg(dpowint_0(dneg(x),y));
+ else return dpowint_0(dneg(x),y);
+ }
+ if(dispo2(x)) {
+ e=dgetexp(x)-0x3ff;
+ if(y>=2048) y= 2047; // avoid overflow
+ if(y<-2048) y=-2048;
+ y*=e;
+ return dldexp(1,y);
+ }
+ if(y==0) return 1;
+ if(y>=-32&&y<=32) return dpowint_1(x,y);
+ return dpow_1(x,y);
+}
+
+double WRAPPER_FUNC(powint)(double x,int y) {
+ _Pragma("GCC diagnostic push")
+ _Pragma("GCC diagnostic ignored \"-Wfloat-equal\"")
+ if(x==1.0||y==0) return 1;
+ _Pragma("GCC diagnostic pop")
+ check_nan_d1(x);
+ if(diszero(x)) {
+ if(y>0) {
+ if(y&1) return x;
+ else return 0;
+ }
+ if((y&1)) return dcopysign(PINF,x);
+ return PINF;
+ }
+ if(dispinf(x)) {
+ if(y<0) return 0;
+ else return PINF;
+ }
+ if(disminf(x)) {
+ if(y>0) {
+ if((y&1)) return MINF;
+ else return PINF;
+ }
+ if((y&1)) return MZERO;
+ else return PZERO;
+ }
+ return dpowint_0(x,y);
+}
+
+// for the case where y is guaranteed a finite integer, x not zero or infinity
+static double dpow_0(double x,double y) {
+ int e,p;
+ if(disneg(x)) {
+ if(disoddint(y)) return dneg(dpow_0(dneg(x),y));
+ else return dpow_0(dneg(x),y);
+ }
+ p=(int)y;
+ if(dispo2(x)) {
+ e=dgetexp(x)-0x3ff;
+ if(p>=2048) p= 2047; // avoid overflow
+ if(p<-2048) p=-2048;
+ p*=e;
+ return dldexp(1,p);
+ }
+ if(p==0) return 1;
+ if(p>=-32&&p<=32) return dpowint_1(x,p);
+ return dpow_1(x,y);
+}
+
+double WRAPPER_FUNC(pow)(double x,double y) {
+ _Pragma("GCC diagnostic push")
+ _Pragma("GCC diagnostic ignored \"-Wfloat-equal\"")
+
+ if(x==1.0||diszero(y)) return 1;
+ check_nan_d2(x, y);
+ if(x==-1.0&&disinf(y)) return 1;
+ _Pragma("GCC diagnostic pop")
+
+ if(diszero(x)) {
+ if(!disneg(y)) {
+ if(disoddint(y)) return x;
+ else return 0;
+ }
+ if(disoddint(y)) return dcopysign(PINF,x);
+ return PINF;
+ }
+ if(dispinf(x)) {
+ if(disneg(y)) return 0;
+ else return PINF;
+ }
+ if(disminf(x)) {
+ if(!disneg(y)) {
+ if(disoddint(y)) return MINF;
+ else return PINF;
+ }
+ if(disoddint(y)) return MZERO;
+ else return PZERO;
+ }
+ if(dispinf(y)) {
+ if(dgetexp(x)<0x3ff) return PZERO;
+ else return PINF;
+ }
+ if(disminf(y)) {
+ if(dgetexp(x)<0x3ff) return PINF;
+ else return PZERO;
+ }
+ if(disint(y)) return dpow_0(x,y);
+ if(disneg(x)) return PINF;
+ return dpow_1(x,y);
+}
+
+double WRAPPER_FUNC(hypot)(double x,double y) {
+ check_nan_d2(x, y);
+ int ex,ey;
+ ex=dgetexp(x); ey=dgetexp(y);
+ if(ex>=0x3ff+400||ey>=0x3ff+400) { // overflow, or nearly so
+ x=dldexp(x,-600),y=dldexp(y,-600);
+ return dldexp(sqrt(x*x+y*y), 600);
+ }
+ else if(ex<=0x3ff-400&&ey<=0x3ff-400) { // underflow, or nearly so
+ x=dldexp(x, 600),y=dldexp(y, 600);
+ return dldexp(sqrt(x*x+y*y),-600);
+ }
+ return sqrt(x*x+y*y);
+}
+
+double WRAPPER_FUNC(cbrt)(double x) {
+ check_nan_d1(x);
+ int e;
+ if(disneg(x)) return dneg(cbrt(dneg(x)));
+ if(diszero(x)) return dcopysign(PZERO,x);
+ e=dgetexp(x)-0x3ff;
+ e=(e*0x5555+0x8000)>>16; // ~e/3, rounded
+ x=dldexp(x,-e*3);
+ x=exp(log(x)*ONETHIRD);
+ return dldexp(x,e);
+}
+
+// reduces mx*2^e modulo my, returning bottom bits of quotient at *pquo
+// 2^52<=|mx|,my<2^53, e>=0; 0<=result<my
+static i64 drem_0(i64 mx,i64 my,int e,int*pquo) {
+ int quo=0,q,r=0,s;
+ if(e>0) {
+ r=0xffffffffU/(ui32)(my>>36); // reciprocal estimate Q16
+ }
+ while(e>0) {
+ s=e; if(s>12) s=12; // gain up to 12 bits on each iteration
+ q=(mx>>38)*r; // Q30
+ q=((q>>(29-s))+1)>>1; // Q(s), rounded
+ mx=(mx<<s)-my*q;
+ quo=(quo<<s)+q;
+ e-=s;
+ }
+ if(mx>=my) mx-=my,quo++; // when e==0 mx can be nearly as big as 2my
+ if(mx>=my) mx-=my,quo++;
+ if(mx<0) mx+=my,quo--;
+ if(mx<0) mx+=my,quo--;
+ if(pquo) *pquo=quo;
+ return mx;
+}
+
+double WRAPPER_FUNC(fmod)(double x,double y) {
+ check_nan_d2(x, y);
+ ui64 ix=double2ui64(x),iy=double2ui64(y);
+ int sx,ex,ey;
+ i64 mx,my;
+ DUNPACKS(ix,sx,ex,mx);
+ DUNPACK(iy,ey,my);
+ if(ex==0x7ff) return dnan_or(PINF);
+ if(ey==0) return PINF;
+ if(ex==0) {
+ if(!disneg(x)) return PZERO;
+ return MZERO;
+ }
+ if(ex<ey) return x; // |x|<|y|, including case x=±0
+ mx=drem_0(mx,my,ex-ey,0);
+ if(sx) mx=-mx;
+ return fix642double(mx,0x3ff-ey+52);
+}
+
+double WRAPPER_FUNC(remquo)(double x,double y,int*quo) {
+ check_nan_d2(x, y);
+ ui64 ix=double2ui64(x),iy=double2ui64(y);
+ int sx,sy,ex,ey,q;
+ i64 mx,my;
+ DUNPACKS(ix,sx,ex,mx);
+ DUNPACKS(iy,sy,ey,my);
+ if(quo) *quo=0;
+ if(ex==0x7ff) return PINF;
+ if(ey==0) return PINF;
+ if(ex==0) return PZERO;
+ if(ey==0x7ff) return x;
+ if(ex<ey-1) return x; // |x|<|y|/2
+ if(ex==ey-1) {
+ if(mx<=my) return x; // |x|<=|y|/2, even quotient
+ // here |y|/2<|x|<|y|
+ if(!sx) { // x>|y|/2
+ mx-=my+my;
+ ey--;
+ q=1;
+ } else { // x<-|y|/2
+ mx=my+my-mx;
+ ey--;
+ q=-1;
+ }
+ }
+ else {
+ if(sx) mx=-mx;
+ mx=drem_0(mx,my,ex-ey,&q);
+ if(mx+mx>my || (mx+mx==my&&(q&1)) ) { // |x|>|y|/2, or equality and an odd quotient?
+ mx-=my;
+ q++;
+ }
+ }
+ if(sy) q=-q;
+ if(quo) *quo=q;
+ return fix642double(mx,0x3ff-ey+52);
+}
+
+double WRAPPER_FUNC(drem)(double x,double y) { check_nan_d2(x, y); return remquo(x,y,0); }
+
+double WRAPPER_FUNC(remainder)(double x,double y) { check_nan_d2(x, y); return remquo(x,y,0); }
+
+_Pragma("GCC diagnostic pop") // conversion
\ No newline at end of file
diff --git a/src/rp2_common/pico_double/double_none.S b/src/rp2_common/pico_double/double_none.S
new file mode 100644
index 0000000..feded31
--- /dev/null
+++ b/src/rp2_common/pico_double/double_none.S
@@ -0,0 +1,82 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "pico/asm_helper.S"
+#include "pico/bootrom/sf_table.h"
+
+.syntax unified
+.cpu cortex-m0plus
+.thumb
+
+ wrapper_func __aeabi_dadd
+ wrapper_func __aeabi_ddiv
+ wrapper_func __aeabi_dmul
+ wrapper_func __aeabi_drsub
+ wrapper_func __aeabi_dsub
+ wrapper_func __aeabi_cdcmpeq
+ wrapper_func __aeabi_cdrcmple
+ wrapper_func __aeabi_cdcmple
+ wrapper_func __aeabi_dcmpeq
+ wrapper_func __aeabi_dcmplt
+ wrapper_func __aeabi_dcmple
+ wrapper_func __aeabi_dcmpge
+ wrapper_func __aeabi_dcmpgt
+ wrapper_func __aeabi_dcmpun
+ wrapper_func __aeabi_i2d
+ wrapper_func __aeabi_l2d
+ wrapper_func __aeabi_ui2d
+ wrapper_func __aeabi_ul2d
+ wrapper_func __aeabi_d2iz
+ wrapper_func __aeabi_d2lz
+ wrapper_func __aeabi_d2uiz
+ wrapper_func __aeabi_d2ulz
+ wrapper_func __aeabi_d2f
+ wrapper_func sqrt
+ wrapper_func cos
+ wrapper_func sin
+ wrapper_func tan
+ wrapper_func atan2
+ wrapper_func exp
+ wrapper_func log
+
+ wrapper_func ldexp
+ wrapper_func copysign
+ wrapper_func trunc
+ wrapper_func floor
+ wrapper_func ceil
+ wrapper_func round
+ wrapper_func sincos
+ wrapper_func asin
+ wrapper_func acos
+ wrapper_func atan
+ wrapper_func sinh
+ wrapper_func cosh
+ wrapper_func tanh
+ wrapper_func asinh
+ wrapper_func acosh
+ wrapper_func atanh
+ wrapper_func exp2
+ wrapper_func log2
+ wrapper_func exp10
+ wrapper_func log10
+ wrapper_func pow
+ wrapper_func powint
+ wrapper_func hypot
+ wrapper_func cbrt
+ wrapper_func fmod
+ wrapper_func drem
+ wrapper_func remainder
+ wrapper_func remquo
+ wrapper_func expm1
+ wrapper_func log1p
+ wrapper_func fma
+
+ push {lr} // keep stack trace sane
+ ldr r0, =str
+ bl panic
+
+str:
+ .asciz "double support is disabled"
\ No newline at end of file
diff --git a/src/rp2_common/pico_double/double_v1_rom_shim.S b/src/rp2_common/pico_double/double_v1_rom_shim.S
new file mode 100644
index 0000000..7a14ca4
--- /dev/null
+++ b/src/rp2_common/pico_double/double_v1_rom_shim.S
@@ -0,0 +1,2184 @@
+/**
+ * Copyright (c) 2020 Mark Owen https://www.quinapalus.com .
+ *
+ * Raspberry Pi (Trading) Ltd (Licensor) hereby grants to you a non-exclusive license to use the software solely on a
+ * Raspberry Pi Pico device. No other use is permitted under the terms of this license.
+ *
+ * This software is also available from the copyright owner under GPLv2 licence.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE LICENSOR AND COPYRIGHT OWNER "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 LICENSOR OR COPYRIGHT OWNER 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 "pico/asm_helper.S"
+
+.syntax unified
+.cpu cortex-m0plus
+.thumb
+
+.macro double_section name
+// todo separate flag for shims?
+#if PICO_DOUBLE_IN_RAM
+.section RAM_SECTION_NAME(\name), "ax"
+#else
+.section SECTION_NAME(\name), "ax"
+#endif
+.endm
+
+double_section double_table_shim_on_use_helper
+regular_func double_table_shim_on_use_helper
+ push {r0-r2, lr}
+ mov r0, ip
+#ifndef NDEBUG
+ // sanity check to make sure we weren't called by non (shimmable_) table_tail_call macro
+ cmp r0, #0
+ bne 1f
+ bkpt #0
+#endif
+1:
+ ldrh r1, [r0]
+ lsrs r2, r1, #8
+ adds r0, #2
+ cmp r2, #0xdf
+ bne 1b
+ uxtb r1, r1 // r1 holds table offset
+ lsrs r2, r0, #2
+ bcc 1f
+ // unaligned
+ ldrh r2, [r0, #0]
+ ldrh r0, [r0, #2]
+ lsls r0, #16
+ orrs r0, r2
+ b 2f
+1:
+ ldr r0, [r0]
+2:
+ ldr r2, =sd_table
+ str r0, [r2, r1]
+ str r0, [sp, #12]
+ pop {r0-r2, pc}
+
+#if PICO_DOUBLE_SUPPORT_ROM_V1 && PICO_RP2040_B0_SUPPORTED
+// Note that the V1 ROM has no double support, so this is basically the identical
+// library, and shim inter-function calls do not bother to redirect back thru the
+// wrapper functions
+
+.equ use_hw_div,1
+.equ IOPORT ,0xd0000000
+.equ DIV_UDIVIDEND,0x00000060
+.equ DIV_UDIVISOR ,0x00000064
+.equ DIV_QUOTIENT ,0x00000070
+.equ DIV_CSR ,0x00000078
+
+@ Notation:
+@ rx:ry means the concatenation of rx and ry with rx having the less significant bits
+
+.equ debug,0
+.macro mdump k
+.if debug
+ push {r0-r3}
+ push {r14}
+ push {r0-r3}
+ bl osp
+ movs r0,#\k
+ bl o1ch
+ pop {r0-r3}
+ bl dump
+ bl osp
+ bl osp
+ ldr r0,[r13]
+ bl o8hex @ r14
+ bl onl
+ pop {r0}
+ mov r14,r0
+ pop {r0-r3}
+.endif
+.endm
+
+
+@ IEEE double in ra:rb ->
+@ mantissa in ra:rb 12Q52 (53 significant bits) with implied 1 set
+@ exponent in re
+@ sign in rs
+@ trashes rt
+.macro mdunpack ra,rb,re,rs,rt
+ lsrs \re,\rb,#20 @ extract sign and exponent
+ subs \rs,\re,#1
+ lsls \rs,#20
+ subs \rb,\rs @ clear sign and exponent in mantissa; insert implied 1
+ lsrs \rs,\re,#11 @ sign
+ lsls \re,#21
+ lsrs \re,#21 @ exponent
+ beq l\@_1 @ zero exponent?
+ adds \rt,\re,#1
+ lsrs \rt,#11
+ beq l\@_2 @ exponent != 0x7ff? then done
+l\@_1:
+ movs \ra,#0
+ movs \rb,#1
+ lsls \rb,#20
+ subs \re,#128
+ lsls \re,#12
+l\@_2:
+.endm
+
+@ IEEE double in ra:rb ->
+@ signed mantissa in ra:rb 12Q52 (53 significant bits) with implied 1
+@ exponent in re
+@ trashes rt0 and rt1
+@ +zero, +denormal -> exponent=-0x80000
+@ -zero, -denormal -> exponent=-0x80000
+@ +Inf, +NaN -> exponent=+0x77f000
+@ -Inf, -NaN -> exponent=+0x77e000
+.macro mdunpacks ra,rb,re,rt0,rt1
+ lsrs \re,\rb,#20 @ extract sign and exponent
+ lsrs \rt1,\rb,#31 @ sign only
+ subs \rt0,\re,#1
+ lsls \rt0,#20
+ subs \rb,\rt0 @ clear sign and exponent in mantissa; insert implied 1
+ lsls \re,#21
+ bcc l\@_1 @ skip on positive
+ mvns \rb,\rb @ negate mantissa
+ rsbs \ra,#0
+ bcc l\@_1
+ adds \rb,#1
+l\@_1:
+ lsrs \re,#21
+ beq l\@_2 @ zero exponent?
+ adds \rt0,\re,#1
+ lsrs \rt0,#11
+ beq l\@_3 @ exponent != 0x7ff? then done
+ subs \re,\rt1
+l\@_2:
+ movs \ra,#0
+ lsls \rt1,#1 @ +ve: 0 -ve: 2
+ adds \rb,\rt1,#1 @ +ve: 1 -ve: 3
+ lsls \rb,#30 @ create +/-1 mantissa
+ asrs \rb,#10
+ subs \re,#128
+ lsls \re,#12
+l\@_3:
+.endm
+
+double_section WRAPPER_FUNC_NAME(__aeabi_dsub)
+
+# frsub first because it is the only one that needs alignment
+regular_func drsub_shim
+ push {r0-r3}
+ pop {r0-r1}
+ pop {r2-r3}
+ // fall thru
+
+regular_func dsub_shim
+ push {r4-r7,r14}
+ movs r4,#1
+ lsls r4,#31
+ eors r3,r4 @ flip sign on second argument
+ b da_entry @ continue in dadd
+
+.align 2
+double_section dadd_shim
+regular_func dadd_shim
+ push {r4-r7,r14}
+da_entry:
+ mdunpacks r0,r1,r4,r6,r7
+ mdunpacks r2,r3,r5,r6,r7
+ subs r7,r5,r4 @ ye-xe
+ subs r6,r4,r5 @ xe-ye
+ bmi da_ygtx
+@ here xe>=ye: need to shift y down r6 places
+ mov r12,r4 @ save exponent
+ cmp r6,#32
+ bge da_xrgty @ xe rather greater than ye?
+ adds r7,#32
+ movs r4,r2
+ lsls r4,r4,r7 @ rounding bit + sticky bits
+da_xgty0:
+ movs r5,r3
+ lsls r5,r5,r7
+ lsrs r2,r6
+ asrs r3,r6
+ orrs r2,r5
+da_add:
+ adds r0,r2
+ adcs r1,r3
+da_pack:
+@ here unnormalised signed result (possibly 0) is in r0:r1 with exponent r12, rounding + sticky bits in r4
+@ Note that if a large normalisation shift is required then the arguments were close in magnitude and so we
+@ cannot have not gone via the xrgty/yrgtx paths. There will therefore always be enough high bits in r4
+@ to provide a correct continuation of the exact result.
+@ now pack result back up
+ lsrs r3,r1,#31 @ get sign bit
+ beq 1f @ skip on positive
+ mvns r1,r1 @ negate mantissa
+ mvns r0,r0
+ movs r2,#0
+ rsbs r4,#0
+ adcs r0,r2
+ adcs r1,r2
+1:
+ mov r2,r12 @ get exponent
+ lsrs r5,r1,#21
+ bne da_0 @ shift down required?
+ lsrs r5,r1,#20
+ bne da_1 @ normalised?
+ cmp r0,#0
+ beq da_5 @ could mantissa be zero?
+da_2:
+ adds r4,r4
+ adcs r0,r0
+ adcs r1,r1
+ subs r2,#1 @ adjust exponent
+ lsrs r5,r1,#20
+ beq da_2
+da_1:
+ lsls r4,#1 @ check rounding bit
+ bcc da_3
+da_4:
+ adds r0,#1 @ round up
+ bcc 2f
+ adds r1,#1
+2:
+ cmp r4,#0 @ sticky bits zero?
+ bne da_3
+ lsrs r0,#1 @ round to even
+ lsls r0,#1
+da_3:
+ subs r2,#1
+ bmi da_6
+ adds r4,r2,#2 @ check if exponent is overflowing
+ lsrs r4,#11
+ bne da_7
+ lsls r2,#20 @ pack exponent and sign
+ add r1,r2
+ lsls r3,#31
+ add r1,r3
+ pop {r4-r7,r15}
+
+da_7:
+@ here exponent overflow: return signed infinity
+ lsls r1,r3,#31
+ ldr r3,=#0x7ff00000
+ orrs r1,r3
+ b 1f
+da_6:
+@ here exponent underflow: return signed zero
+ lsls r1,r3,#31
+1:
+ movs r0,#0
+ pop {r4-r7,r15}
+
+da_5:
+@ here mantissa could be zero
+ cmp r1,#0
+ bne da_2
+ cmp r4,#0
+ bne da_2
+@ inputs must have been of identical magnitude and opposite sign, so return +0
+ pop {r4-r7,r15}
+
+da_0:
+@ here a shift down by one place is required for normalisation
+ adds r2,#1 @ adjust exponent
+ lsls r6,r0,#31 @ save rounding bit
+ lsrs r0,#1
+ lsls r5,r1,#31
+ orrs r0,r5
+ lsrs r1,#1
+ cmp r6,#0
+ beq da_3
+ b da_4
+
+da_xrgty: @ xe>ye and shift>=32 places
+ cmp r6,#60
+ bge da_xmgty @ xe much greater than ye?
+ subs r6,#32
+ adds r7,#64
+
+ movs r4,r2
+ lsls r4,r4,r7 @ these would be shifted off the bottom of the sticky bits
+ beq 1f
+ movs r4,#1
+1:
+ lsrs r2,r2,r6
+ orrs r4,r2
+ movs r2,r3
+ lsls r3,r3,r7
+ orrs r4,r3
+ asrs r3,r2,#31 @ propagate sign bit
+ b da_xgty0
+
+da_ygtx:
+@ here ye>xe: need to shift x down r7 places
+ mov r12,r5 @ save exponent
+ cmp r7,#32
+ bge da_yrgtx @ ye rather greater than xe?
+ adds r6,#32
+ movs r4,r0
+ lsls r4,r4,r6 @ rounding bit + sticky bits
+da_ygtx0:
+ movs r5,r1
+ lsls r5,r5,r6
+ lsrs r0,r7
+ asrs r1,r7
+ orrs r0,r5
+ b da_add
+
+da_yrgtx:
+ cmp r7,#60
+ bge da_ymgtx @ ye much greater than xe?
+ subs r7,#32
+ adds r6,#64
+
+ movs r4,r0
+ lsls r4,r4,r6 @ these would be shifted off the bottom of the sticky bits
+ beq 1f
+ movs r4,#1
+1:
+ lsrs r0,r0,r7
+ orrs r4,r0
+ movs r0,r1
+ lsls r1,r1,r6
+ orrs r4,r1
+ asrs r1,r0,#31 @ propagate sign bit
+ b da_ygtx0
+
+da_ymgtx: @ result is just y
+ movs r0,r2
+ movs r1,r3
+da_xmgty: @ result is just x
+ movs r4,#0 @ clear sticky bits
+ b da_pack
+
+.ltorg
+
+@ equivalent of UMULL
+@ needs five temporary registers
+@ can have rt3==rx, in which case rx trashed
+@ can have rt4==ry, in which case ry trashed
+@ can have rzl==rx
+@ can have rzh==ry
+@ can have rzl,rzh==rt3,rt4
+.macro mul32_32_64 rx,ry,rzl,rzh,rt0,rt1,rt2,rt3,rt4
+ @ t0 t1 t2 t3 t4
+ @ (x) (y)
+ uxth \rt0,\rx @ xl
+ uxth \rt1,\ry @ yl
+ muls \rt0,\rt1 @ xlyl=L
+ lsrs \rt2,\rx,#16 @ xh
+ muls \rt1,\rt2 @ xhyl=M0
+ lsrs \rt4,\ry,#16 @ yh
+ muls \rt2,\rt4 @ xhyh=H
+ uxth \rt3,\rx @ xl
+ muls \rt3,\rt4 @ xlyh=M1
+ adds \rt1,\rt3 @ M0+M1=M
+ bcc l\@_1 @ addition of the two cross terms can overflow, so add carry into H
+ movs \rt3,#1 @ 1
+ lsls \rt3,#16 @ 0x10000
+ adds \rt2,\rt3 @ H'
+l\@_1:
+ @ t0 t1 t2 t3 t4
+ @ (zl) (zh)
+ lsls \rzl,\rt1,#16 @ ML
+ lsrs \rzh,\rt1,#16 @ MH
+ adds \rzl,\rt0 @ ZL
+ adcs \rzh,\rt2 @ ZH
+.endm
+
+@ SUMULL: x signed, y unsigned
+@ in table below ¯ means signed variable
+@ needs five temporary registers
+@ can have rt3==rx, in which case rx trashed
+@ can have rt4==ry, in which case ry trashed
+@ can have rzl==rx
+@ can have rzh==ry
+@ can have rzl,rzh==rt3,rt4
+.macro muls32_32_64 rx,ry,rzl,rzh,rt0,rt1,rt2,rt3,rt4
+ @ t0 t1 t2 t3 t4
+ @ ¯(x) (y)
+ uxth \rt0,\rx @ xl
+ uxth \rt1,\ry @ yl
+ muls \rt0,\rt1 @ xlyl=L
+ asrs \rt2,\rx,#16 @ ¯xh
+ muls \rt1,\rt2 @ ¯xhyl=M0
+ lsrs \rt4,\ry,#16 @ yh
+ muls \rt2,\rt4 @ ¯xhyh=H
+ uxth \rt3,\rx @ xl
+ muls \rt3,\rt4 @ xlyh=M1
+ asrs \rt4,\rt1,#31 @ M0sx (M1 sign extension is zero)
+ adds \rt1,\rt3 @ M0+M1=M
+ movs \rt3,#0 @ 0
+ adcs \rt4,\rt3 @ ¯Msx
+ lsls \rt4,#16 @ ¯Msx<<16
+ adds \rt2,\rt4 @ H'
+
+ @ t0 t1 t2 t3 t4
+ @ (zl) (zh)
+ lsls \rzl,\rt1,#16 @ M~
+ lsrs \rzh,\rt1,#16 @ M~
+ adds \rzl,\rt0 @ ZL
+ adcs \rzh,\rt2 @ ¯ZH
+.endm
+
+@ SSMULL: x signed, y signed
+@ in table below ¯ means signed variable
+@ needs five temporary registers
+@ can have rt3==rx, in which case rx trashed
+@ can have rt4==ry, in which case ry trashed
+@ can have rzl==rx
+@ can have rzh==ry
+@ can have rzl,rzh==rt3,rt4
+.macro muls32_s32_64 rx,ry,rzl,rzh,rt0,rt1,rt2,rt3,rt4
+ @ t0 t1 t2 t3 t4
+ @ ¯(x) (y)
+ uxth \rt0,\rx @ xl
+ uxth \rt1,\ry @ yl
+ muls \rt0,\rt1 @ xlyl=L
+ asrs \rt2,\rx,#16 @ ¯xh
+ muls \rt1,\rt2 @ ¯xhyl=M0
+ asrs \rt4,\ry,#16 @ ¯yh
+ muls \rt2,\rt4 @ ¯xhyh=H
+ uxth \rt3,\rx @ xl
+ muls \rt3,\rt4 @ ¯xlyh=M1
+ adds \rt1,\rt3 @ ¯M0+M1=M
+ asrs \rt3,\rt1,#31 @ Msx
+ bvc l\@_1 @
+ mvns \rt3,\rt3 @ ¯Msx flip sign extension bits if overflow
+l\@_1:
+ lsls \rt3,#16 @ ¯Msx<<16
+ adds \rt2,\rt3 @ H'
+
+ @ t0 t1 t2 t3 t4
+ @ (zl) (zh)
+ lsls \rzl,\rt1,#16 @ M~
+ lsrs \rzh,\rt1,#16 @ M~
+ adds \rzl,\rt0 @ ZL
+ adcs \rzh,\rt2 @ ¯ZH
+.endm
+
+@ can have rt2==rx, in which case rx trashed
+@ can have rzl==rx
+@ can have rzh==rt1
+.macro square32_64 rx,rzl,rzh,rt0,rt1,rt2
+ @ t0 t1 t2 zl zh
+ uxth \rt0,\rx @ xl
+ muls \rt0,\rt0 @ xlxl=L
+ uxth \rt1,\rx @ xl
+ lsrs \rt2,\rx,#16 @ xh
+ muls \rt1,\rt2 @ xlxh=M
+ muls \rt2,\rt2 @ xhxh=H
+ lsls \rzl,\rt1,#17 @ ML
+ lsrs \rzh,\rt1,#15 @ MH
+ adds \rzl,\rt0 @ ZL
+ adcs \rzh,\rt2 @ ZH
+.endm
+
+double_section dmul_shim
+ regular_func dmul_shim
+ push {r4-r7,r14}
+ mdunpack r0,r1,r4,r6,r5
+ mov r12,r4
+ mdunpack r2,r3,r4,r7,r5
+ eors r7,r6 @ sign of result
+ add r4,r12 @ exponent of result
+ push {r0-r2,r4,r7}
+
+@ accumulate full product in r12:r5:r6:r7
+ mul32_32_64 r0,r2, r0,r5, r4,r6,r7,r0,r5 @ XL*YL
+ mov r12,r0 @ save LL bits
+
+ mul32_32_64 r1,r3, r6,r7, r0,r2,r4,r6,r7 @ XH*YH
+
+ pop {r0} @ XL
+ mul32_32_64 r0,r3, r0,r3, r1,r2,r4,r0,r3 @ XL*YH
+ adds r5,r0
+ adcs r6,r3
+ movs r0,#0
+ adcs r7,r0
+
+ pop {r1,r2} @ XH,YL
+ mul32_32_64 r1,r2, r1,r2, r0,r3,r4, r1,r2 @ XH*YL
+ adds r5,r1
+ adcs r6,r2
+ movs r0,#0
+ adcs r7,r0
+
+@ here r5:r6:r7 holds the product [1..4) in Q(104-32)=Q72, with extra LSBs in r12
+ pop {r3,r4} @ exponent in r3, sign in r4
+ lsls r1,r7,#11
+ lsrs r2,r6,#21
+ orrs r1,r2
+ lsls r0,r6,#11
+ lsrs r2,r5,#21
+ orrs r0,r2
+ lsls r5,#11 @ now r5:r0:r1 Q83=Q(51+32), extra LSBs in r12
+ lsrs r2,r1,#20
+ bne 1f @ skip if in range [2..4)
+ adds r5,r5 @ shift up so always [2..4) Q83, i.e. [1..2) Q84=Q(52+32)
+ adcs r0,r0
+ adcs r1,r1
+ subs r3,#1 @ correct exponent
+1:
+ ldr r6,=#0x3ff
+ subs r3,r6 @ correct for exponent bias
+ lsls r6,#1 @ 0x7fe
+ cmp r3,r6
+ bhs dm_0 @ exponent over- or underflow
+ lsls r5,#1 @ rounding bit to carry
+ bcc 1f @ result is correctly rounded
+ adds r0,#1
+ movs r6,#0
+ adcs r1,r6 @ round up
+ mov r6,r12 @ remaining sticky bits
+ orrs r5,r6
+ bne 1f @ some sticky bits set?
+ lsrs r0,#1
+ lsls r0,#1 @ round to even
+1:
+ lsls r3,#20
+ adds r1,r3
+dm_2:
+ lsls r4,#31
+ add r1,r4
+ pop {r4-r7,r15}
+
+@ here for exponent over- or underflow
+dm_0:
+ bge dm_1 @ overflow?
+ adds r3,#1 @ would-be zero exponent?
+ bne 1f
+ adds r0,#1
+ bne 1f @ all-ones mantissa?
+ adds r1,#1
+ lsrs r7,r1,#21
+ beq 1f
+ lsrs r1,#1
+ b dm_2
+1:
+ lsls r1,r4,#31
+ movs r0,#0
+ pop {r4-r7,r15}
+
+@ here for exponent overflow
+dm_1:
+ adds r6,#1 @ 0x7ff
+ lsls r1,r6,#20
+ movs r0,#0
+ b dm_2
+
+.ltorg
+
+@ Approach to division y/x is as follows.
+@
+@ First generate u1, an approximation to 1/x to about 29 bits. Multiply this by the top
+@ 32 bits of y to generate a0, a first approximation to the result (good to 28 bits or so).
+@ Calculate the exact remainder r0=y-a0*x, which will be about 0. Calculate a correction
+@ d0=r0*u1, and then write a1=a0+d0. If near a rounding boundary, compute the exact
+@ remainder r1=y-a1*x (which can be done using r0 as a basis) to determine whether to
+@ round up or down.
+@
+@ The calculation of 1/x is as given in dreciptest.c. That code verifies exhaustively
+@ that | u1*x-1 | < 10*2^-32.
+@
+@ More precisely:
+@
+@ x0=(q16)x;
+@ x1=(q30)x;
+@ y0=(q31)y;
+@ u0=(q15~)"(0xffffffffU/(unsigned int)roundq(x/x_ulp))/powq(2,16)"(x0); // q15 approximation to 1/x; "~" denotes rounding rather than truncation
+@ v=(q30)(u0*x1-1);
+@ u1=(q30)u0-(q30~)(u0*v);
+@
+@ a0=(q30)(u1*y0);
+@ r0=(q82)y-a0*x;
+@ r0x=(q57)r0;
+@ d0=r0x*u1;
+@ a1=d0+a0;
+@
+@ Error analysis
+@
+@ Use Greek letters to represent the errors introduced by rounding and truncation.
+@
+@ r₀ = y - a₀x
+@ = y - [ u₁ ( y - α ) - β ] x where 0 ≤ α < 2^-31, 0 ≤ β < 2^-30
+@ = y ( 1 - u₁x ) + ( u₁α + β ) x
+@
+@ Hence
+@
+@ | r₀ / x | < 2 * 10*2^-32 + 2^-31 + 2^-30
+@ = 26*2^-32
+@
+@ r₁ = y - a₁x
+@ = y - a₀x - d₀x
+@ = r₀ - d₀x
+@ = r₀ - u₁ ( r₀ - γ ) x where 0 ≤ γ < 2^-57
+@ = r₀ ( 1 - u₁x ) + u₁γx
+@
+@ Hence
+@
+@ | r₁ / x | < 26*2^-32 * 10*2^-32 + 2^-57
+@ = (260+128)*2^-64
+@ < 2^-55
+@
+@ Empirically it seems to be nearly twice as good as this.
+@
+@ To determine correctly whether the exact remainder calculation can be skipped we need a result
+@ accurate to < 0.25ulp. In the case where x>y the quotient will be shifted up one place for normalisation
+@ and so 1ulp is 2^-53 and so the calculation above suffices.
+
+double_section ddiv_shim
+ regular_func ddiv_shim
+ push {r4-r7,r14}
+ddiv0: @ entry point from dtan
+ mdunpack r2,r3,r4,r7,r6 @ unpack divisor
+
+.if use_hw_div
+
+ movs r5,#IOPORT>>24
+ lsls r5,#24
+ movs r6,#0
+ mvns r6,r6
+ str r6,[r5,#DIV_UDIVIDEND]
+ lsrs r6,r3,#4 @ x0=(q16)x
+ str r6,[r5,#DIV_UDIVISOR]
+@ if there are not enough cycles from now to the read of the quotient for
+@ the divider to do its stuff we need a busy-wait here
+
+.endif
+
+@ unpack dividend by hand to save on register use
+ lsrs r6,r1,#31
+ adds r6,r7
+ mov r12,r6 @ result sign in r12b0; r12b1 trashed
+ lsls r1,#1
+ lsrs r7,r1,#21 @ exponent
+ beq 1f @ zero exponent?
+ adds r6,r7,#1
+ lsrs r6,#11
+ beq 2f @ exponent != 0x7ff? then done
+1:
+ movs r0,#0
+ movs r1,#0
+ subs r7,#64 @ less drastic fiddling of exponents to get 0/0, Inf/Inf correct
+ lsls r7,#12
+2:
+ subs r6,r7,r4
+ lsls r6,#2
+ add r12,r12,r6 @ (signed) exponent in r12[31..8]
+ subs r7,#1 @ implied 1
+ lsls r7,#21
+ subs r1,r7
+ lsrs r1,#1
+
+.if use_hw_div
+
+ ldr r6,[r5,#DIV_QUOTIENT]
+ adds r6,#1
+ lsrs r6,#1
+
+.else
+
+@ this is not beautiful; could be replaced by better code that uses knowledge of divisor range
+ push {r0-r3}
+ movs r0,#0
+ mvns r0,r0
+ lsrs r1,r3,#4 @ x0=(q16)x
+ bl __aeabi_uidiv @ !!! this could (but apparently does not) trash R12
+ adds r6,r0,#1
+ lsrs r6,#1
+ pop {r0-r3}
+
+.endif
+
+@ here
+@ r0:r1 y mantissa
+@ r2:r3 x mantissa
+@ r6 u0, first approximation to 1/x Q15
+@ r12: result sign, exponent
+
+ lsls r4,r3,#10
+ lsrs r5,r2,#22
+ orrs r5,r4 @ x1=(q30)x
+ muls r5,r6 @ u0*x1 Q45
+ asrs r5,#15 @ v=u0*x1-1 Q30
+ muls r5,r6 @ u0*v Q45
+ asrs r5,#14
+ adds r5,#1
+ asrs r5,#1 @ round u0*v to Q30
+ lsls r6,#15
+ subs r6,r5 @ u1 Q30
+
+@ here
+@ r0:r1 y mantissa
+@ r2:r3 x mantissa
+@ r6 u1, second approximation to 1/x Q30
+@ r12: result sign, exponent
+
+ push {r2,r3}
+ lsls r4,r1,#11
+ lsrs r5,r0,#21
+ orrs r4,r5 @ y0=(q31)y
+ mul32_32_64 r4,r6, r4,r5, r2,r3,r7,r4,r5 @ y0*u1 Q61
+ adds r4,r4
+ adcs r5,r5 @ a0=(q30)(y0*u1)
+
+@ here
+@ r0:r1 y mantissa
+@ r5 a0, first approximation to y/x Q30
+@ r6 u1, second approximation to 1/x Q30
+@ r12 result sign, exponent
+
+ ldr r2,[r13,#0] @ xL
+ mul32_32_64 r2,r5, r2,r3, r1,r4,r7,r2,r3 @ xL*a0
+ ldr r4,[r13,#4] @ xH
+ muls r4,r5 @ xH*a0
+ adds r3,r4 @ r2:r3 now x*a0 Q82
+ lsrs r2,#25
+ lsls r1,r3,#7
+ orrs r2,r1 @ r2 now x*a0 Q57; r7:r2 is x*a0 Q89
+ lsls r4,r0,#5 @ y Q57
+ subs r0,r4,r2 @ r0x=y-x*a0 Q57 (signed)
+
+@ here
+@ r0 r0x Q57
+@ r5 a0, first approximation to y/x Q30
+@ r4 yL Q57
+@ r6 u1 Q30
+@ r12 result sign, exponent
+
+ muls32_32_64 r0,r6, r7,r6, r1,r2,r3, r7,r6 @ r7:r6 r0x*u1 Q87
+ asrs r3,r6,#25
+ adds r5,r3
+ lsls r3,r6,#7 @ r3:r5 a1 Q62 (but bottom 7 bits are zero so 55 bits of precision after binary point)
+@ here we could recover another 7 bits of precision (but not accuracy) from the top of r7
+@ but these bits are thrown away in the rounding and conversion to Q52 below
+
+@ here
+@ r3:r5 a1 Q62 candidate quotient [0.5,2) or so
+@ r4 yL Q57
+@ r12 result sign, exponent
+
+ movs r6,#0
+ adds r3,#128 @ for initial rounding to Q53
+ adcs r5,r5,r6
+ lsrs r1,r5,#30
+ bne dd_0
+@ here candidate quotient a1 is in range [0.5,1)
+@ so 30 significant bits in r5
+
+ lsls r4,#1 @ y now Q58
+ lsrs r1,r5,#9 @ to Q52
+ lsls r0,r5,#23
+ lsrs r3,#9 @ 0.5ulp-significance bit in carry: if this is 1 we may need to correct result
+ orrs r0,r3
+ bcs dd_1
+ b dd_2
+dd_0:
+@ here candidate quotient a1 is in range [1,2)
+@ so 31 significant bits in r5
+
+ movs r2,#4
+ add r12,r12,r2 @ fix exponent; r3:r5 now effectively Q61
+ adds r3,#128 @ complete rounding to Q53
+ adcs r5,r5,r6
+ lsrs r1,r5,#10
+ lsls r0,r5,#22
+ lsrs r3,#10 @ 0.5ulp-significance bit in carry: if this is 1 we may need to correct result
+ orrs r0,r3
+ bcc dd_2
+dd_1:
+
+@ here
+@ r0:r1 rounded result Q53 [0.5,1) or Q52 [1,2), but may not be correctly rounded-to-nearest
+@ r4 yL Q58 or Q57
+@ r12 result sign, exponent
+@ carry set
+
+ adcs r0,r0,r0
+ adcs r1,r1,r1 @ z Q53 with 1 in LSB
+ lsls r4,#16 @ Q105-32=Q73
+ ldr r2,[r13,#0] @ xL Q52
+ ldr r3,[r13,#4] @ xH Q20
+
+ movs r5,r1 @ zH Q21
+ muls r5,r2 @ zH*xL Q73
+ subs r4,r5
+ muls r3,r0 @ zL*xH Q73
+ subs r4,r3
+ mul32_32_64 r2,r0, r2,r3, r5,r6,r7,r2,r3 @ xL*zL
+ rsbs r2,#0 @ borrow from low half?
+ sbcs r4,r3 @ y-xz Q73 (remainder bits 52..73)
+
+ cmp r4,#0
+
+ bmi 1f
+ movs r2,#0 @ round up
+ adds r0,#1
+ adcs r1,r2
+1:
+ lsrs r0,#1 @ shift back down to Q52
+ lsls r2,r1,#31
+ orrs r0,r2
+ lsrs r1,#1
+dd_2:
+ add r13,#8
+ mov r2,r12
+ lsls r7,r2,#31 @ result sign
+ asrs r2,#2 @ result exponent
+ ldr r3,=#0x3fd
+ adds r2,r3
+ ldr r3,=#0x7fe
+ cmp r2,r3
+ bhs dd_3 @ over- or underflow?
+ lsls r2,#20
+ adds r1,r2 @ pack exponent
+dd_5:
+ adds r1,r7 @ pack sign
+ pop {r4-r7,r15}
+
+dd_3:
+ movs r0,#0
+ cmp r2,#0
+ bgt dd_4 @ overflow?
+ movs r1,r7
+ pop {r4-r7,r15}
+
+dd_4:
+ adds r3,#1 @ 0x7ff
+ lsls r1,r3,#20
+ b dd_5
+
+.section SECTION_NAME(dsqrt_shim)
+/*
+Approach to square root x=sqrt(y) is as follows.
+
+First generate a3, an approximation to 1/sqrt(y) to about 30 bits. Multiply this by y
+to give a4~sqrt(y) to about 28 bits and a remainder r4=y-a4^2. Then, because
+d sqrt(y) / dy = 1 / (2 sqrt(y)) let d4=r4*a3/2 and then the value a5=a4+d4 is
+a better approximation to sqrt(y). If this is near a rounding boundary we
+compute an exact remainder y-a5*a5 to decide whether to round up or down.
+
+The calculation of a3 and a4 is as given in dsqrttest.c. That code verifies exhaustively
+that | 1 - a3a4 | < 10*2^-32, | r4 | < 40*2^-32 and | r4/y | < 20*2^-32.
+
+More precisely, with "y" representing y truncated to 30 binary places:
+
+u=(q3)y; // 24-entry table
+a0=(q8~)"1/sqrtq(x+x_ulp/2)"(u); // first approximation from table
+p0=(q16)(a0*a0) * (q16)y;
+r0=(q20)(p0-1);
+dy0=(q15)(r0*a0); // Newton-Raphson correction term
+a1=(q16)a0-dy0/2; // good to ~9 bits
+
+p1=(q19)(a1*a1)*(q19)y;
+r1=(q23)(p1-1);
+dy1=(q15~)(r1*a1); // second Newton-Raphson correction
+a2x=(q16)a1-dy1/2; // good to ~16 bits
+a2=a2x-a2x/1t16; // prevent overflow of a2*a2 in 32 bits
+
+p2=(a2*a2)*(q30)y; // Q62
+r2=(q36)(p2-1+1t-31);
+dy2=(q30)(r2*a2); // Q52->Q30
+a3=(q31)a2-dy2/2; // good to about 30 bits
+a4=(q30)(a3*(q30)y+1t-31); // good to about 28 bits
+
+Error analysis
+
+ r₄ = y - a₄²
+ d₄ = 1/2 a₃r₄
+ a₅ = a₄ + d₄
+ r₅ = y - a₅²
+ = y - ( a₄ + d₄ )²
+ = y - a₄² - a₃a₄r₄ - 1/4 a₃²r₄²
+ = r₄ - a₃a₄r₄ - 1/4 a₃²r₄²
+
+ | r₅ | < | r₄ | | 1 - a₃a₄ | + 1/4 r₄²
+
+ a₅ = √y √( 1 - r₅/y )
+ = √y ( 1 - 1/2 r₅/y + ... )
+
+So to first order (second order being very tiny)
+
+ √y - a₅ = 1/2 r₅/y
+
+and
+
+ | √y - a₅ | < 1/2 ( | r₄/y | | 1 - a₃a₄ | + 1/4 r₄²/y )
+
+From dsqrttest.c (conservatively):
+
+ < 1/2 ( 20*2^-32 * 10*2^-32 + 1/4 * 40*2^-32*20*2^-32 )
+ = 1/2 ( 200 + 200 ) * 2^-64
+ < 2^-56
+
+Empirically we see about 1ulp worst-case error including rounding at Q57.
+
+To determine correctly whether the exact remainder calculation can be skipped we need a result
+accurate to < 0.25ulp at Q52, or 2^-54.
+*/
+
+dq_2:
+ bge dq_3 @ +Inf?
+ movs r1,#0
+ b dq_4
+
+dq_0:
+ lsrs r1,#31
+ lsls r1,#31 @ preserve sign bit
+ lsrs r2,#21 @ extract exponent
+ beq dq_4 @ -0? return it
+ asrs r1,#11 @ make -Inf
+ b dq_4
+
+dq_3:
+ ldr r1,=#0x7ff
+ lsls r1,#20 @ return +Inf
+dq_4:
+ movs r0,#0
+dq_1:
+ bx r14
+
+.align 2
+regular_func dsqrt_shim
+ lsls r2,r1,#1
+ bcs dq_0 @ negative?
+ lsrs r2,#21 @ extract exponent
+ subs r2,#1
+ ldr r3,=#0x7fe
+ cmp r2,r3
+ bhs dq_2 @ catches 0 and +Inf
+ push {r4-r7,r14}
+ lsls r4,r2,#20
+ subs r1,r4 @ insert implied 1
+ lsrs r2,#1
+ bcc 1f @ even exponent? skip
+ adds r0,r0,r0 @ odd exponent: shift up mantissa
+ adcs r1,r1,r1
+1:
+ lsrs r3,#2
+ adds r2,r3
+ lsls r2,#20
+ mov r12,r2 @ save result exponent
+
+@ here
+@ r0:r1 y mantissa Q52 [1,4)
+@ r12 result exponent
+
+ adr r4,drsqrtapp-8 @ first eight table entries are never accessed because of the mantissa's leading 1
+ lsrs r2,r1,#17 @ y Q3
+ ldrb r2,[r4,r2] @ initial approximation to reciprocal square root a0 Q8
+ lsrs r3,r1,#4 @ first Newton-Raphson iteration
+ muls r3,r2
+ muls r3,r2 @ i32 p0=a0*a0*(y>>14); // Q32
+ asrs r3,r3,#12 @ i32 r0=p0>>12; // Q20
+ muls r3,r2
+ asrs r3,#13 @ i32 dy0=(r0*a0)>>13; // Q15
+ lsls r2,#8
+ subs r2,r3 @ i32 a1=(a0<<8)-dy0; // Q16
+
+ movs r3,r2
+ muls r3,r3
+ lsrs r3,#13
+ lsrs r4,r1,#1
+ muls r3,r4 @ i32 p1=((a1*a1)>>11)*(y>>11); // Q19*Q19=Q38
+ asrs r3,#15 @ i32 r1=p1>>15; // Q23
+ muls r3,r2
+ asrs r3,#23
+ adds r3,#1
+ asrs r3,#1 @ i32 dy1=(r1*a1+(1<<23))>>24; // Q23*Q16=Q39; Q15
+ subs r2,r3 @ i32 a2=a1-dy1; // Q16
+ lsrs r3,r2,#16
+ subs r2,r3 @ if(a2>=0x10000) a2=0xffff; to prevent overflow of a2*a2
+
+@ here
+@ r0:r1 y mantissa
+@ r2 a2 ~ 1/sqrt(y) Q16
+@ r12 result exponent
+
+ movs r3,r2
+ muls r3,r3
+ lsls r1,#10
+ lsrs r4,r0,#22
+ orrs r1,r4 @ y Q30
+ mul32_32_64 r1,r3, r4,r3, r5,r6,r7,r4,r3 @ i64 p2=(ui64)(a2*a2)*(ui64)y; // Q62 r4:r3
+ lsls r5,r3,#6
+ lsrs r4,#26
+ orrs r4,r5
+ adds r4,#0x20 @ i32 r2=(p2>>26)+0x20; // Q36 r4
+ uxth r5,r4
+ muls r5,r2
+ asrs r4,#16
+ muls r4,r2
+ lsrs r5,#16
+ adds r4,r5
+ asrs r4,#6 @ i32 dy2=((i64)r2*(i64)a2)>>22; // Q36*Q16=Q52; Q30
+ lsls r2,#15
+ subs r2,r4
+
+@ here
+@ r0 y low bits
+@ r1 y Q30
+@ r2 a3 ~ 1/sqrt(y) Q31
+@ r12 result exponent
+
+ mul32_32_64 r2,r1, r3,r4, r5,r6,r7,r3,r4
+ adds r3,r3,r3
+ adcs r4,r4,r4
+ adds r3,r3,r3
+ movs r3,#0
+ adcs r3,r4 @ ui32 a4=((ui64)a3*(ui64)y+(1U<<31))>>31; // Q30
+
+@ here
+@ r0 y low bits
+@ r1 y Q30
+@ r2 a3 Q31 ~ 1/sqrt(y)
+@ r3 a4 Q30 ~ sqrt(y)
+@ r12 result exponent
+
+ square32_64 r3, r4,r5, r6,r5,r7
+ lsls r6,r0,#8
+ lsrs r7,r1,#2
+ subs r6,r4
+ sbcs r7,r5 @ r4=(q60)y-a4*a4
+
+@ by exhaustive testing, r4 = fffffffc0e134fdc .. 00000003c2bf539c Q60
+
+ lsls r5,r7,#29
+ lsrs r6,#3
+ adcs r6,r5 @ r4 Q57 with rounding
+ muls32_32_64 r6,r2, r6,r2, r4,r5,r7,r6,r2 @ d4=a3*r4/2 Q89
+@ r4+d4 is correct to 1ULP at Q57, tested on ~9bn cases including all extreme values of r4 for each possible y Q30
+
+ adds r2,#8
+ asrs r2,#5 @ d4 Q52, rounded to Q53 with spare bit in carry
+
+@ here
+@ r0 y low bits
+@ r1 y Q30
+@ r2 d4 Q52, rounded to Q53
+@ C flag contains d4_b53
+@ r3 a4 Q30
+
+ bcs dq_5
+
+ lsrs r5,r3,#10 @ a4 Q52
+ lsls r4,r3,#22
+
+ asrs r1,r2,#31
+ adds r0,r2,r4
+ adcs r1,r5 @ a4+d4
+
+ add r1,r12 @ pack exponent
+ pop {r4-r7,r15}
+
+.ltorg
+
+
+@ round(sqrt(2^22./[68:8:252]))
+drsqrtapp:
+.byte 0xf8,0xeb,0xdf,0xd6,0xcd,0xc5,0xbe,0xb8
+.byte 0xb2,0xad,0xa8,0xa4,0xa0,0x9c,0x99,0x95
+.byte 0x92,0x8f,0x8d,0x8a,0x88,0x85,0x83,0x81
+
+dq_5:
+@ here we are near a rounding boundary, C is set
+ adcs r2,r2,r2 @ d4 Q53+1ulp
+ lsrs r5,r3,#9
+ lsls r4,r3,#23 @ r4:r5 a4 Q53
+ asrs r1,r2,#31
+ adds r4,r2,r4
+ adcs r5,r1 @ r4:r5 a5=a4+d4 Q53+1ulp
+ movs r3,r5
+ muls r3,r4
+ square32_64 r4,r1,r2,r6,r2,r7
+ adds r2,r3
+ adds r2,r3 @ r1:r2 a5^2 Q106
+ lsls r0,#22 @ y Q84
+
+ rsbs r1,#0
+ sbcs r0,r2 @ remainder y-a5^2
+ bmi 1f @ y<a5^2: no need to increment a5
+ movs r3,#0
+ adds r4,#1
+ adcs r5,r3 @ bump a5 if over rounding boundary
+1:
+ lsrs r0,r4,#1
+ lsrs r1,r5,#1
+ lsls r5,#31
+ orrs r0,r5
+ add r1,r12
+ pop {r4-r7,r15}
+
+@ "scientific" functions start here
+
+@ double-length CORDIC rotation step
+
+@ r0:r1 ω
+@ r6 32-i (complementary shift)
+@ r7 i (shift)
+@ r8:r9 x
+@ r10:r11 y
+@ r12 coefficient pointer
+
+@ an option in rotation mode would be to compute the sequence of σ values
+@ in one pass, rotate the initial vector by the residual ω and then run a
+@ second pass to compute the final x and y. This would relieve pressure
+@ on registers and hence possibly be faster. The same trick does not work
+@ in vectoring mode (but perhaps one could work to single precision in
+@ a first pass and then double precision in a second pass?).
+
+double_section dcordic_vec_step
+ regular_func dcordic_vec_step
+ mov r2,r12
+ ldmia r2!,{r3,r4}
+ mov r12,r2
+ mov r2,r11
+ cmp r2,#0
+ blt 1f
+ b 2f
+
+double_section dcordic_rot_step
+ regular_func dcordic_rot_step
+ mov r2,r12
+ ldmia r2!,{r3,r4}
+ mov r12,r2
+ cmp r1,#0
+ bge 1f
+2:
+@ ω<0 / y>=0
+@ ω+=dω
+@ x+=y>>i, y-=x>>i
+ adds r0,r3
+ adcs r1,r4
+
+ mov r3,r11
+ asrs r3,r7
+ mov r4,r11
+ lsls r4,r6
+ mov r2,r10
+ lsrs r2,r7
+ orrs r2,r4 @ r2:r3 y>>i, rounding in carry
+ mov r4,r8
+ mov r5,r9 @ r4:r5 x
+ adcs r2,r4
+ adcs r3,r5 @ r2:r3 x+(y>>i)
+ mov r8,r2
+ mov r9,r3
+
+ mov r3,r5
+ lsls r3,r6
+ asrs r5,r7
+ lsrs r4,r7
+ orrs r4,r3 @ r4:r5 x>>i, rounding in carry
+ mov r2,r10
+ mov r3,r11
+ sbcs r2,r4
+ sbcs r3,r5 @ r2:r3 y-(x>>i)
+ mov r10,r2
+ mov r11,r3
+ bx r14
+
+
+@ ω>0 / y<0
+@ ω-=dω
+@ x-=y>>i, y+=x>>i
+1:
+ subs r0,r3
+ sbcs r1,r4
+
+ mov r3,r9
+ asrs r3,r7
+ mov r4,r9
+ lsls r4,r6
+ mov r2,r8
+ lsrs r2,r7
+ orrs r2,r4 @ r2:r3 x>>i, rounding in carry
+ mov r4,r10
+ mov r5,r11 @ r4:r5 y
+ adcs r2,r4
+ adcs r3,r5 @ r2:r3 y+(x>>i)
+ mov r10,r2
+ mov r11,r3
+
+ mov r3,r5
+ lsls r3,r6
+ asrs r5,r7
+ lsrs r4,r7
+ orrs r4,r3 @ r4:r5 y>>i, rounding in carry
+ mov r2,r8
+ mov r3,r9
+ sbcs r2,r4
+ sbcs r3,r5 @ r2:r3 x-(y>>i)
+ mov r8,r2
+ mov r9,r3
+ bx r14
+
+@ convert packed double in r0:r1 to signed/unsigned 32/64-bit integer/fixed-point value in r0:r1 [with r2 places after point], with rounding towards -Inf
+@ fixed-point versions only work with reasonable values in r2 because of the way dunpacks works
+
+double_section double2int_shim
+ regular_func double2int_shim
+ movs r2,#0 @ and fall through
+regular_func double2fix_shim
+ push {r14}
+ adds r2,#32
+ bl double2fix64_shim
+ movs r0,r1
+ pop {r15}
+
+double_section double2uint_shim
+ regular_func double2uint_shim
+ movs r2,#0 @ and fall through
+regular_func double2ufix_shim
+ push {r14}
+ adds r2,#32
+ bl double2ufix64_shim
+ movs r0,r1
+ pop {r15}
+
+double_section double2int64_shim
+ regular_func double2int64_shim
+ movs r2,#0 @ and fall through
+regular_func double2fix64_shim
+ push {r14}
+ bl d2fix
+
+ asrs r2,r1,#31
+ cmp r2,r3
+ bne 1f @ sign extension bits fail to match sign of result?
+ pop {r15}
+1:
+ mvns r0,r3
+ movs r1,#1
+ lsls r1,#31
+ eors r1,r1,r0 @ generate extreme fixed-point values
+ pop {r15}
+
+double_section double2uint64_shim
+ regular_func double2uint64_shim
+ movs r2,#0 @ and fall through
+regular_func double2ufix64_shim
+ asrs r3,r1,#20 @ negative? return 0
+ bmi ret_dzero
+@ and fall through
+
+@ convert double in r0:r1 to signed fixed point in r0:r1:r3, r2 places after point, rounding towards -Inf
+@ result clamped so that r3 can only be 0 or -1
+@ trashes r12
+.thumb_func
+d2fix:
+ push {r4,r14}
+ mov r12,r2
+ bl dunpacks
+ asrs r4,r2,#16
+ adds r4,#1
+ bge 1f
+ movs r1,#0 @ -0 -> +0
+1:
+ asrs r3,r1,#31
+ ldr r4, =d2fix_a
+ bx r4
+
+ret_dzero:
+ movs r0,#0
+ movs r1,#0
+ bx r14
+
+.weak d2fix_a // weak because it exists in float code too
+regular_func d2fix_a
+@ here
+@ r0:r1 two's complement mantissa
+@ r2 unbaised exponent
+@ r3 mantissa sign extension bits
+ add r2,r12 @ exponent plus offset for required binary point position
+ subs r2,#52 @ required shift
+ bmi 1f @ shift down?
+@ here a shift up by r2 places
+ cmp r2,#12 @ will clamp?
+ bge 2f
+ movs r4,r0
+ lsls r1,r2
+ lsls r0,r2
+ rsbs r2,#0
+ adds r2,#32 @ complementary shift
+ lsrs r4,r2
+ orrs r1,r4
+ pop {r4,r15}
+2:
+ mvns r0,r3
+ mvns r1,r3 @ overflow: clamp to extreme fixed-point values
+ pop {r4,r15}
+1:
+@ here a shift down by -r2 places
+ adds r2,#32
+ bmi 1f @ long shift?
+ mov r4,r1
+ lsls r4,r2
+ rsbs r2,#0
+ adds r2,#32 @ complementary shift
+ asrs r1,r2
+ lsrs r0,r2
+ orrs r0,r4
+ pop {r4,r15}
+1:
+@ here a long shift down
+ movs r0,r1
+ asrs r1,#31 @ shift down 32 places
+ adds r2,#32
+ bmi 1f @ very long shift?
+ rsbs r2,#0
+ adds r2,#32
+ asrs r0,r2
+ pop {r4,r15}
+1:
+ movs r0,r3 @ result very near zero: use sign extension bits
+ movs r1,r3
+ pop {r4,r15}
+
+double_section double2float_shim
+ regular_func double2float_shim
+ lsls r2,r1,#1
+ lsrs r2,#21 @ exponent
+ ldr r3,=#0x3ff-0x7f
+ subs r2,r3 @ fix exponent bias
+ ble 1f @ underflow or zero
+ cmp r2,#0xff
+ bge 2f @ overflow or infinity
+ lsls r2,#23 @ position exponent of result
+ lsrs r3,r1,#31
+ lsls r3,#31
+ orrs r2,r3 @ insert sign
+ lsls r3,r0,#3 @ rounding bits
+ lsrs r0,#29
+ lsls r1,#12
+ lsrs r1,#9
+ orrs r0,r1 @ assemble mantissa
+ orrs r0,r2 @ insert exponent and sign
+ lsls r3,#1
+ bcc 3f @ no rounding
+ beq 4f @ all sticky bits 0?
+5:
+ adds r0,#1
+3:
+ bx r14
+4:
+ lsrs r3,r0,#1 @ odd? then round up
+ bcs 5b
+ bx r14
+1:
+ beq 6f @ check case where value is just less than smallest normal
+7:
+ lsrs r0,r1,#31
+ lsls r0,#31
+ bx r14
+6:
+ lsls r2,r1,#12 @ 20 1:s at top of mantissa?
+ asrs r2,#12
+ adds r2,#1
+ bne 7b
+ lsrs r2,r0,#29 @ and 3 more 1:s?
+ cmp r2,#7
+ bne 7b
+ movs r2,#1 @ return smallest normal with correct sign
+ b 8f
+2:
+ movs r2,#0xff
+8:
+ lsrs r0,r1,#31 @ return signed infinity
+ lsls r0,#8
+ adds r0,r2
+ lsls r0,#23
+ bx r14
+
+double_section x2double_shims
+@ convert signed/unsigned 32/64-bit integer/fixed-point value in r0:r1 [with r2 places after point] to packed double in r0:r1, with rounding
+
+.align 2
+regular_func uint2double_shim
+ movs r1,#0 @ and fall through
+regular_func ufix2double_shim
+ movs r2,r1
+ movs r1,#0
+ b ufix642double_shim
+
+.align 2
+regular_func int2double_shim
+ movs r1,#0 @ and fall through
+regular_func fix2double_shim
+ movs r2,r1
+ asrs r1,r0,#31 @ sign extend
+ b fix642double_shim
+
+.align 2
+regular_func uint642double_shim
+ movs r2,#0 @ and fall through
+regular_func ufix642double_shim
+ movs r3,#0
+ b uf2d
+
+.align 2
+regular_func int642double_shim
+ movs r2,#0 @ and fall through
+regular_func fix642double_shim
+ asrs r3,r1,#31 @ sign bit across all bits
+ eors r0,r3
+ eors r1,r3
+ subs r0,r3
+ sbcs r1,r3
+uf2d:
+ push {r4,r5,r14}
+ ldr r4,=#0x432
+ subs r2,r4,r2 @ form biased exponent
+@ here
+@ r0:r1 unnormalised mantissa
+@ r2 -Q (will become exponent)
+@ r3 sign across all bits
+ cmp r1,#0
+ bne 1f @ short normalising shift?
+ movs r1,r0
+ beq 2f @ zero? return it
+ movs r0,#0
+ subs r2,#32 @ fix exponent
+1:
+ asrs r4,r1,#21
+ bne 3f @ will need shift down (and rounding?)
+ bcs 4f @ normalised already?
+5:
+ subs r2,#1
+ adds r0,r0 @ shift up
+ adcs r1,r1
+ lsrs r4,r1,#21
+ bcc 5b
+4:
+ ldr r4,=#0x7fe
+ cmp r2,r4
+ bhs 6f @ over/underflow? return signed zero/infinity
+7:
+ lsls r2,#20 @ pack and return
+ adds r1,r2
+ lsls r3,#31
+ adds r1,r3
+2:
+ pop {r4,r5,r15}
+6: @ return signed zero/infinity according to unclamped exponent in r2
+ mvns r2,r2
+ lsrs r2,#21
+ movs r0,#0
+ movs r1,#0
+ b 7b
+
+3:
+@ here we need to shift down to normalise and possibly round
+ bmi 1f @ already normalised to Q63?
+2:
+ subs r2,#1
+ adds r0,r0 @ shift up
+ adcs r1,r1
+ bpl 2b
+1:
+@ here we have a 1 in b63 of r0:r1
+ adds r2,#11 @ correct exponent for subsequent shift down
+ lsls r4,r0,#21 @ save bits for rounding
+ lsrs r0,#11
+ lsls r5,r1,#21
+ orrs r0,r5
+ lsrs r1,#11
+ lsls r4,#1
+ beq 1f @ sticky bits are zero?
+8:
+ movs r4,#0
+ adcs r0,r4
+ adcs r1,r4
+ b 4b
+1:
+ bcc 4b @ sticky bits are zero but not on rounding boundary
+ lsrs r4,r0,#1 @ increment if odd (force round to even)
+ b 8b
+
+
+.ltorg
+
+double_section dunpacks
+ regular_func dunpacks
+ mdunpacks r0,r1,r2,r3,r4
+ ldr r3,=#0x3ff
+ subs r2,r3 @ exponent without offset
+ bx r14
+
+@ r0:r1 signed mantissa Q52
+@ r2 unbiased exponent < 10 (i.e., |x|<2^10)
+@ r4 pointer to:
+@ - divisor reciprocal approximation r=1/d Q15
+@ - divisor d Q62 0..20
+@ - divisor d Q62 21..41
+@ - divisor d Q62 42..62
+@ returns:
+@ r0:r1 reduced result y Q62, -0.6 d < y < 0.6 d (better in practice)
+@ r2 quotient q (number of reductions)
+@ if exponent >=10, returns r0:r1=0, r2=1024*mantissa sign
+@ designed to work for 0.5<d<2, in particular d=ln2 (~0.7) and d=π/2 (~1.6)
+double_section dreduce
+ regular_func dreduce
+ adds r2,#2 @ e+2
+ bmi 1f @ |x|<0.25, too small to need adjustment
+ cmp r2,#12
+ bge 4f
+2:
+ movs r5,#17
+ subs r5,r2 @ 15-e
+ movs r3,r1 @ Q20
+ asrs r3,r5 @ x Q5
+ adds r2,#8 @ e+10
+ adds r5,#7 @ 22-e = 32-(e+10)
+ movs r6,r0
+ lsrs r6,r5
+ lsls r0,r2
+ lsls r1,r2
+ orrs r1,r6 @ r0:r1 x Q62
+ ldmia r4,{r4-r7}
+ muls r3,r4 @ rx Q20
+ asrs r2,r3,#20
+ movs r3,#0
+ adcs r2,r3 @ rx Q0 rounded = q; for e.g. r=1.5 |q|<1.5*2^10
+ muls r5,r2 @ qd in pieces: L Q62
+ muls r6,r2 @ M Q41
+ muls r7,r2 @ H Q20
+ lsls r7,#10
+ asrs r4,r6,#11
+ lsls r6,#21
+ adds r6,r5
+ adcs r7,r4
+ asrs r5,#31
+ adds r7,r5 @ r6:r7 qd Q62
+ subs r0,r6
+ sbcs r1,r7 @ remainder Q62
+ bx r14
+4:
+ movs r2,#12 @ overflow: clamp to +/-1024
+ movs r0,#0
+ asrs r1,#31
+ lsls r1,#1
+ adds r1,#1
+ lsls r1,#20
+ b 2b
+
+1:
+ lsls r1,#8
+ lsrs r3,r0,#24
+ orrs r1,r3
+ lsls r0,#8 @ r0:r1 Q60, to be shifted down -r2 places
+ rsbs r3,r2,#0
+ adds r2,#32 @ shift down in r3, complementary shift in r2
+ bmi 1f @ long shift?
+2:
+ movs r4,r1
+ asrs r1,r3
+ lsls r4,r2
+ lsrs r0,r3
+ orrs r0,r4
+ movs r2,#0 @ rounding
+ adcs r0,r2
+ adcs r1,r2
+ bx r14
+
+1:
+ movs r0,r1 @ down 32 places
+ asrs r1,#31
+ subs r3,#32
+ adds r2,#32
+ bpl 2b
+ movs r0,#0 @ very long shift? return 0
+ movs r1,#0
+ movs r2,#0
+ bx r14
+
+double_section dtan_shim
+ regular_func dtan_shim
+ push {r4-r7,r14}
+ bl push_r8_r11
+ bl dsincos_internal
+ mov r12,r0 @ save ε
+ bl dcos_finish
+ push {r0,r1}
+ mov r0,r12
+ bl dsin_finish
+ pop {r2,r3}
+ bl pop_r8_r11
+ b ddiv0 @ compute sin θ/cos θ
+
+double_section dcos_shim
+ regular_func dcos_shim
+ push {r4-r7,r14}
+ bl push_r8_r11
+ bl dsincos_internal
+ bl dcos_finish
+ b 1f
+
+double_section dsin_shim
+ regular_func dsin_shim
+ push {r4-r7,r14}
+ bl push_r8_r11
+ bl dsincos_internal
+ bl dsin_finish
+1:
+ bl pop_r8_r11
+ pop {r4-r7,r15}
+
+double_section dsincos_shim
+
+ @ Note that this function returns in r0-r3
+ regular_func dsincos_shim
+
+ push {r4-r7,r14}
+ bl push_r8_r11
+ bl dsincos_internal
+ mov r12,r0 @ save ε
+ bl dcos_finish
+ push {r0,r1}
+ mov r0,r12
+ bl dsin_finish
+ pop {r2,r3}
+ bl pop_r8_r11
+ pop {r4-r7,r15}
+
+double_section dtrig_guts
+
+@ unpack double θ in r0:r1, range reduce and calculate ε, cos α and sin α such that
+@ θ=α+ε and |ε|≤2^-32
+@ on return:
+@ r0:r1 ε (residual ω, where θ=α+ε) Q62, |ε|≤2^-32 (so fits in r0)
+@ r8:r9 cos α Q62
+@ r10:r11 sin α Q62
+.align 2
+.thumb_func
+dsincos_internal:
+ push {r14}
+ bl dunpacks
+ adr r4,dreddata0
+ bl dreduce
+
+ movs r4,#0
+ ldr r5,=#0x9df04dbb @ this value compensates for the non-unity scaling of the CORDIC rotations
+ ldr r6,=#0x36f656c5
+ lsls r2,#31
+ bcc 1f
+@ quadrant 2 or 3
+ mvns r6,r6
+ rsbs r5,r5,#0
+ adcs r6,r4
+1:
+ lsls r2,#1
+ bcs 1f
+@ even quadrant
+ mov r10,r4
+ mov r11,r4
+ mov r8,r5
+ mov r9,r6
+ b 2f
+1:
+@ odd quadrant
+ mov r8,r4
+ mov r9,r4
+ mov r10,r5
+ mov r11,r6
+2:
+ adr r4,dtab_cc
+ mov r12,r4
+ movs r7,#1
+ movs r6,#31
+1:
+ bl dcordic_rot_step
+ adds r7,#1
+ subs r6,#1
+ cmp r7,#33
+ bne 1b
+ pop {r15}
+
+dcos_finish:
+@ here
+@ r0:r1 ε (residual ω, where θ=α+ε) Q62, |ε|≤2^-32 (so fits in r0)
+@ r8:r9 cos α Q62
+@ r10:r11 sin α Q62
+@ and we wish to calculate cos θ=cos(α+ε)~cos α - ε sin α
+ mov r1,r11
+@ mov r2,r10
+@ lsrs r2,#31
+@ adds r1,r2 @ rounding improves accuracy very slightly
+ muls32_s32_64 r0,r1, r2,r3, r4,r5,r6,r2,r3
+@ r2:r3 ε sin α Q(62+62-32)=Q92
+ mov r0,r8
+ mov r1,r9
+ lsls r5,r3,#2
+ asrs r3,r3,#30
+ lsrs r2,r2,#30
+ orrs r2,r5
+ sbcs r0,r2 @ include rounding
+ sbcs r1,r3
+ movs r2,#62
+ b fix642double_shim
+
+dsin_finish:
+@ here
+@ r0:r1 ε (residual ω, where θ=α+ε) Q62, |ε|≤2^-32 (so fits in r0)
+@ r8:r9 cos α Q62
+@ r10:r11 sin α Q62
+@ and we wish to calculate sin θ=sin(α+ε)~sin α + ε cos α
+ mov r1,r9
+ muls32_s32_64 r0,r1, r2,r3, r4,r5,r6,r2,r3
+@ r2:r3 ε cos α Q(62+62-32)=Q92
+ mov r0,r10
+ mov r1,r11
+ lsls r5,r3,#2
+ asrs r3,r3,#30
+ lsrs r2,r2,#30
+ orrs r2,r5
+ adcs r0,r2 @ include rounding
+ adcs r1,r3
+ movs r2,#62
+ b fix642double_shim
+
+.ltorg
+.align 2
+dreddata0:
+.word 0x0000517d @ 2/π Q15
+.word 0x0014611A @ π/2 Q62=6487ED5110B4611A split into 21-bit pieces
+.word 0x000A8885
+.word 0x001921FB
+
+
+.align 2
+regular_func datan2_shim
+@ r0:r1 y
+@ r2:r3 x
+ push {r4-r7,r14}
+ bl push_r8_r11
+ ldr r5,=#0x7ff00000
+ movs r4,r1
+ ands r4,r5 @ y==0?
+ beq 1f
+ cmp r4,r5 @ or Inf/NaN?
+ bne 2f
+1:
+ lsrs r1,#20 @ flush
+ lsls r1,#20
+ movs r0,#0
+2:
+ movs r4,r3
+ ands r4,r5 @ x==0?
+ beq 1f
+ cmp r4,r5 @ or Inf/NaN?
+ bne 2f
+1:
+ lsrs r3,#20 @ flush
+ lsls r3,#20
+ movs r2,#0
+2:
+ movs r6,#0 @ quadrant offset
+ lsls r5,#11 @ constant 0x80000000
+ cmp r3,#0
+ bpl 1f @ skip if x positive
+ movs r6,#2
+ eors r3,r5
+ eors r1,r5
+ bmi 1f @ quadrant offset=+2 if y was positive
+ rsbs r6,#0 @ quadrant offset=-2 if y was negative
+1:
+@ now in quadrant 0 or 3
+ adds r7,r1,r5 @ r7=-r1
+ bpl 1f
+@ y>=0: in quadrant 0
+ cmp r1,r3
+ ble 2f @ y<~x so 0≤θ<~π/4: skip
+ adds r6,#1
+ eors r1,r5 @ negate x
+ b 3f @ and exchange x and y = rotate by -π/2
+1:
+ cmp r3,r7
+ bge 2f @ -y<~x so -π/4<~θ≤0: skip
+ subs r6,#1
+ eors r3,r5 @ negate y and ...
+3:
+ movs r7,r0 @ exchange x and y
+ movs r0,r2
+ movs r2,r7
+ movs r7,r1
+ movs r1,r3
+ movs r3,r7
+2:
+@ here -π/4<~θ<~π/4
+@ r6 has quadrant offset
+ push {r6}
+ cmp r2,#0
+ bne 1f
+ cmp r3,#0
+ beq 10f @ x==0 going into division?
+ lsls r4,r3,#1
+ asrs r4,#21
+ adds r4,#1
+ bne 1f @ x==Inf going into division?
+ lsls r4,r1,#1
+ asrs r4,#21
+ adds r4,#1 @ y also ±Inf?
+ bne 10f
+ subs r1,#1 @ make them both just finite
+ subs r3,#1
+ b 1f
+
+10:
+ movs r0,#0
+ movs r1,#0
+ b 12f
+
+1:
+ bl ddiv_shim
+ movs r2,#62
+ bl double2fix64_shim
+@ r0:r1 y/x
+ mov r10,r0
+ mov r11,r1
+ movs r0,#0 @ ω=0
+ movs r1,#0
+ mov r8,r0
+ movs r2,#1
+ lsls r2,#30
+ mov r9,r2 @ x=1
+
+ adr r4,dtab_cc
+ mov r12,r4
+ movs r7,#1
+ movs r6,#31
+1:
+ bl dcordic_vec_step
+ adds r7,#1
+ subs r6,#1
+ cmp r7,#33
+ bne 1b
+@ r0:r1 atan(y/x) Q62
+@ r8:r9 x residual Q62
+@ r10:r11 y residual Q62
+ mov r2,r9
+ mov r3,r10
+ subs r2,#12 @ this makes atan(0)==0
+@ the following is basically a division residual y/x ~ atan(residual y/x)
+ movs r4,#1
+ lsls r4,#29
+ movs r7,#0
+2:
+ lsrs r2,#1
+ movs r3,r3 @ preserve carry
+ bmi 1f
+ sbcs r3,r2
+ adds r0,r4
+ adcs r1,r7
+ lsrs r4,#1
+ bne 2b
+ b 3f
+1:
+ adcs r3,r2
+ subs r0,r4
+ sbcs r1,r7
+ lsrs r4,#1
+ bne 2b
+3:
+ lsls r6,r1,#31
+ asrs r1,#1
+ lsrs r0,#1
+ orrs r0,r6 @ Q61
+
+12:
+ pop {r6}
+
+ cmp r6,#0
+ beq 1f
+ ldr r4,=#0x885A308D @ π/2 Q61
+ ldr r5,=#0x3243F6A8
+ bpl 2f
+ mvns r4,r4 @ negative quadrant offset
+ mvns r5,r5
+2:
+ lsls r6,#31
+ bne 2f @ skip if quadrant offset is ±1
+ adds r0,r4
+ adcs r1,r5
+2:
+ adds r0,r4
+ adcs r1,r5
+1:
+ movs r2,#61
+ bl fix642double_shim
+
+ bl pop_r8_r11
+ pop {r4-r7,r15}
+
+.ltorg
+
+dtab_cc:
+.word 0x61bb4f69, 0x1dac6705 @ atan 2^-1 Q62
+.word 0x96406eb1, 0x0fadbafc @ atan 2^-2 Q62
+.word 0xab0bdb72, 0x07f56ea6 @ atan 2^-3 Q62
+.word 0xe59fbd39, 0x03feab76 @ atan 2^-4 Q62
+.word 0xba97624b, 0x01ffd55b @ atan 2^-5 Q62
+.word 0xdddb94d6, 0x00fffaaa @ atan 2^-6 Q62
+.word 0x56eeea5d, 0x007fff55 @ atan 2^-7 Q62
+.word 0xaab7776e, 0x003fffea @ atan 2^-8 Q62
+.word 0x5555bbbc, 0x001ffffd @ atan 2^-9 Q62
+.word 0xaaaaadde, 0x000fffff @ atan 2^-10 Q62
+.word 0xf555556f, 0x0007ffff @ atan 2^-11 Q62
+.word 0xfeaaaaab, 0x0003ffff @ atan 2^-12 Q62
+.word 0xffd55555, 0x0001ffff @ atan 2^-13 Q62
+.word 0xfffaaaab, 0x0000ffff @ atan 2^-14 Q62
+.word 0xffff5555, 0x00007fff @ atan 2^-15 Q62
+.word 0xffffeaab, 0x00003fff @ atan 2^-16 Q62
+.word 0xfffffd55, 0x00001fff @ atan 2^-17 Q62
+.word 0xffffffab, 0x00000fff @ atan 2^-18 Q62
+.word 0xfffffff5, 0x000007ff @ atan 2^-19 Q62
+.word 0xffffffff, 0x000003ff @ atan 2^-20 Q62
+.word 0x00000000, 0x00000200 @ atan 2^-21 Q62 @ consider optimising these
+.word 0x00000000, 0x00000100 @ atan 2^-22 Q62
+.word 0x00000000, 0x00000080 @ atan 2^-23 Q62
+.word 0x00000000, 0x00000040 @ atan 2^-24 Q62
+.word 0x00000000, 0x00000020 @ atan 2^-25 Q62
+.word 0x00000000, 0x00000010 @ atan 2^-26 Q62
+.word 0x00000000, 0x00000008 @ atan 2^-27 Q62
+.word 0x00000000, 0x00000004 @ atan 2^-28 Q62
+.word 0x00000000, 0x00000002 @ atan 2^-29 Q62
+.word 0x00000000, 0x00000001 @ atan 2^-30 Q62
+.word 0x80000000, 0x00000000 @ atan 2^-31 Q62
+.word 0x40000000, 0x00000000 @ atan 2^-32 Q62
+
+double_section dexp_guts
+regular_func dexp_shim
+ push {r4-r7,r14}
+ bl dunpacks
+ adr r4,dreddata1
+ bl dreduce
+ cmp r1,#0
+ bge 1f
+ ldr r4,=#0xF473DE6B
+ ldr r5,=#0x2C5C85FD @ ln2 Q62
+ adds r0,r4
+ adcs r1,r5
+ subs r2,#1
+1:
+ push {r2}
+ movs r7,#1 @ shift
+ adr r6,dtab_exp
+ movs r2,#0
+ movs r3,#1
+ lsls r3,#30 @ x=1 Q62
+
+3:
+ ldmia r6!,{r4,r5}
+ mov r12,r6
+ subs r0,r4
+ sbcs r1,r5
+ bmi 1f
+
+ rsbs r6,r7,#0
+ adds r6,#32 @ complementary shift
+ movs r5,r3
+ asrs r5,r7
+ movs r4,r3
+ lsls r4,r6
+ movs r6,r2
+ lsrs r6,r7 @ rounding bit in carry
+ orrs r4,r6
+ adcs r2,r4
+ adcs r3,r5 @ x+=x>>i
+ b 2f
+
+1:
+ adds r0,r4 @ restore argument
+ adcs r1,r5
+2:
+ mov r6,r12
+ adds r7,#1
+ cmp r7,#33
+ bne 3b
+
+@ here
+@ r0:r1 ε (residual x, where x=a+ε) Q62, |ε|≤2^-32 (so fits in r0)
+@ r2:r3 exp a Q62
+@ and we wish to calculate exp x=exp a exp ε~(exp a)(1+ε)
+ muls32_32_64 r0,r3, r4,r1, r5,r6,r7,r4,r1
+@ r4:r1 ε exp a Q(62+62-32)=Q92
+ lsrs r4,#30
+ lsls r0,r1,#2
+ orrs r0,r4
+ asrs r1,#30
+ adds r0,r2
+ adcs r1,r3
+
+ pop {r2}
+ rsbs r2,#0
+ adds r2,#62
+ bl fix642double_shim @ in principle we can pack faster than this because we know the exponent
+ pop {r4-r7,r15}
+
+.ltorg
+
+.align 2
+regular_func dln_shim
+ push {r4-r7,r14}
+ lsls r7,r1,#1
+ bcs 5f @ <0 ...
+ asrs r7,#21
+ beq 5f @ ... or =0? return -Inf
+ adds r7,#1
+ beq 6f @ Inf/NaN? return +Inf
+ bl dunpacks
+ push {r2}
+ lsls r1,#9
+ lsrs r2,r0,#23
+ orrs r1,r2
+ lsls r0,#9
+@ r0:r1 m Q61 = m/2 Q62 0.5≤m/2<1
+
+ movs r7,#1 @ shift
+ adr r6,dtab_exp
+ mov r12,r6
+ movs r2,#0
+ movs r3,#0 @ y=0 Q62
+
+3:
+ rsbs r6,r7,#0
+ adds r6,#32 @ complementary shift
+ movs r5,r1
+ asrs r5,r7
+ movs r4,r1
+ lsls r4,r6
+ movs r6,r0
+ lsrs r6,r7
+ orrs r4,r6 @ x>>i, rounding bit in carry
+ adcs r4,r0
+ adcs r5,r1 @ x+(x>>i)
+
+ lsrs r6,r5,#30
+ bne 1f @ x+(x>>i)>1?
+ movs r0,r4
+ movs r1,r5 @ x+=x>>i
+ mov r6,r12
+ ldmia r6!,{r4,r5}
+ subs r2,r4
+ sbcs r3,r5
+
+1:
+ movs r4,#8
+ add r12,r4
+ adds r7,#1
+ cmp r7,#33
+ bne 3b
+@ here:
+@ r0:r1 residual x, nearly 1 Q62
+@ r2:r3 y ~ ln m/2 = ln m - ln2 Q62
+@ result is y + ln2 + ln x ~ y + ln2 + (x-1)
+ lsls r1,#2
+ asrs r1,#2 @ x-1
+ adds r2,r0
+ adcs r3,r1
+
+ pop {r7}
+@ here:
+@ r2:r3 ln m/2 = ln m - ln2 Q62
+@ r7 unbiased exponent
+
+ adr r4,dreddata1+4
+ ldmia r4,{r0,r1,r4}
+ adds r7,#1
+ muls r0,r7 @ Q62
+ muls r1,r7 @ Q41
+ muls r4,r7 @ Q20
+ lsls r7,r1,#21
+ asrs r1,#11
+ asrs r5,r1,#31
+ adds r0,r7
+ adcs r1,r5
+ lsls r7,r4,#10
+ asrs r4,#22
+ asrs r5,r1,#31
+ adds r1,r7
+ adcs r4,r5
+@ r0:r1:r4 exponent*ln2 Q62
+ asrs r5,r3,#31
+ adds r0,r2
+ adcs r1,r3
+ adcs r4,r5
+@ r0:r1:r4 result Q62
+ movs r2,#62
+1:
+ asrs r5,r1,#31
+ cmp r4,r5
+ beq 2f @ r4 a sign extension of r1?
+ lsrs r0,#4 @ no: shift down 4 places and try again
+ lsls r6,r1,#28
+ orrs r0,r6
+ lsrs r1,#4
+ lsls r6,r4,#28
+ orrs r1,r6
+ asrs r4,#4
+ subs r2,#4
+ b 1b
+2:
+ bl fix642double_shim
+ pop {r4-r7,r15}
+
+5:
+ ldr r1,=#0xfff00000
+ movs r0,#0
+ pop {r4-r7,r15}
+
+6:
+ ldr r1,=#0x7ff00000
+ movs r0,#0
+ pop {r4-r7,r15}
+
+.ltorg
+
+.align 2
+dreddata1:
+.word 0x0000B8AA @ 1/ln2 Q15
+.word 0x0013DE6B @ ln2 Q62 Q62=2C5C85FDF473DE6B split into 21-bit pieces
+.word 0x000FEFA3
+.word 0x000B1721
+
+dtab_exp:
+.word 0xbf984bf3, 0x19f323ec @ log 1+2^-1 Q62
+.word 0xcd4d10d6, 0x0e47fbe3 @ log 1+2^-2 Q62
+.word 0x8abcb97a, 0x0789c1db @ log 1+2^-3 Q62
+.word 0x022c54cc, 0x03e14618 @ log 1+2^-4 Q62
+.word 0xe7833005, 0x01f829b0 @ log 1+2^-5 Q62
+.word 0x87e01f1e, 0x00fe0545 @ log 1+2^-6 Q62
+.word 0xac419e24, 0x007f80a9 @ log 1+2^-7 Q62
+.word 0x45621781, 0x003fe015 @ log 1+2^-8 Q62
+.word 0xa9ab10e6, 0x001ff802 @ log 1+2^-9 Q62
+.word 0x55455888, 0x000ffe00 @ log 1+2^-10 Q62
+.word 0x0aa9aac4, 0x0007ff80 @ log 1+2^-11 Q62
+.word 0x01554556, 0x0003ffe0 @ log 1+2^-12 Q62
+.word 0x002aa9ab, 0x0001fff8 @ log 1+2^-13 Q62
+.word 0x00055545, 0x0000fffe @ log 1+2^-14 Q62
+.word 0x8000aaaa, 0x00007fff @ log 1+2^-15 Q62
+.word 0xe0001555, 0x00003fff @ log 1+2^-16 Q62
+.word 0xf80002ab, 0x00001fff @ log 1+2^-17 Q62
+.word 0xfe000055, 0x00000fff @ log 1+2^-18 Q62
+.word 0xff80000b, 0x000007ff @ log 1+2^-19 Q62
+.word 0xffe00001, 0x000003ff @ log 1+2^-20 Q62
+.word 0xfff80000, 0x000001ff @ log 1+2^-21 Q62
+.word 0xfffe0000, 0x000000ff @ log 1+2^-22 Q62
+.word 0xffff8000, 0x0000007f @ log 1+2^-23 Q62
+.word 0xffffe000, 0x0000003f @ log 1+2^-24 Q62
+.word 0xfffff800, 0x0000001f @ log 1+2^-25 Q62
+.word 0xfffffe00, 0x0000000f @ log 1+2^-26 Q62
+.word 0xffffff80, 0x00000007 @ log 1+2^-27 Q62
+.word 0xffffffe0, 0x00000003 @ log 1+2^-28 Q62
+.word 0xfffffff8, 0x00000001 @ log 1+2^-29 Q62
+.word 0xfffffffe, 0x00000000 @ log 1+2^-30 Q62
+.word 0x80000000, 0x00000000 @ log 1+2^-31 Q62
+.word 0x40000000, 0x00000000 @ log 1+2^-32 Q62
+
+
+#endif
diff --git a/src/rp2_common/pico_double/include/pico/double.h b/src/rp2_common/pico_double/include/pico/double.h
new file mode 100644
index 0000000..0893233
--- /dev/null
+++ b/src/rp2_common/pico_double/include/pico/double.h
@@ -0,0 +1,60 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PICO_DOUBLE_H
+#define _PICO_DOUBLE_H
+
+#include <math.h>
+#include "pico/types.h"
+#include "pico/bootrom/sf_table.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** \file double.h
+* \defgroup pico_double pico_double
+*
+* Optimized double-precision floating point functions
+*
+* (Replacement) optimized implementations are provided of the following compiler built-ins
+* and math library functions:
+*
+* - __aeabi_dadd, __aeabi_ddiv, __aeabi_dmul, __aeabi_drsub, __aeabi_dsub, __aeabi_cdcmpeq, __aeabi_cdrcmple, __aeabi_cdcmple, __aeabi_dcmpeq, __aeabi_dcmplt, __aeabi_dcmple, __aeabi_dcmpge, __aeabi_dcmpgt, __aeabi_dcmpun, __aeabi_i2d, __aeabi_l2d, __aeabi_ui2d, __aeabi_ul2d, __aeabi_d2iz, __aeabi_d2lz, __aeabi_d2uiz, __aeabi_d2ulz, __aeabi_d2f
+* - sqrt, cos, sin, tan, atan2, exp, log, ldexp, copysign, trunc, floor, ceil, round, asin, acos, atan, sinh, cosh, tanh, asinh, acosh, atanh, exp2, log2, exp10, log10, pow,, hypot, cbrt, fmod, drem, remainder, remquo, expm1, log1p, fma
+* - powint, sincos (GNU extensions)
+*
+* The following additional optimized functions are also provided:
+*
+* - fix2double, ufix2double, fix642double, ufix642double, double2fix, double2ufix, double2fix64, double2ufix64, double2int, double2int64, double2int_z, double2int64_z
+*/
+
+double fix2double(int32_t m, int e);
+double ufix2double(uint32_t m, int e);
+double fix642double(int64_t m, int e);
+double ufix642double(uint64_t m, int e);
+
+// These methods round towards -Infinity.
+int32_t double2fix(double f, int e);
+uint32_t double2ufix(double f, int e);
+int64_t double2fix64(double f, int e);
+uint64_t double2ufix64(double f, int e);
+int32_t double2int(double f);
+int64_t double2int64(double f);
+
+// These methods round towards 0.
+int32_t double2int_z(double f);
+int64_t double2int64_z(double f);
+
+double exp10(double x);
+void sincos(double x, double *sinx, double *cosx);
+double powint(double x, int y);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
\ No newline at end of file
diff --git a/src/rp2_common/pico_fix/CMakeLists.txt b/src/rp2_common/pico_fix/CMakeLists.txt
new file mode 100644
index 0000000..81a9eaa
--- /dev/null
+++ b/src/rp2_common/pico_fix/CMakeLists.txt
@@ -0,0 +1 @@
+pico_add_subdirectory(rp2040_usb_device_enumeration)
\ No newline at end of file
diff --git a/src/rp2_common/pico_fix/rp2040_usb_device_enumeration/CMakeLists.txt b/src/rp2_common/pico_fix/rp2040_usb_device_enumeration/CMakeLists.txt
new file mode 100644
index 0000000..18fdc28
--- /dev/null
+++ b/src/rp2_common/pico_fix/rp2040_usb_device_enumeration/CMakeLists.txt
@@ -0,0 +1,9 @@
+pico_add_impl_library(pico_fix_rp2040_usb_device_enumeration)
+
+target_sources(pico_fix_rp2040_usb_device_enumeration INTERFACE
+ ${CMAKE_CURRENT_LIST_DIR}/rp2040_usb_device_enumeration.c
+ )
+
+target_include_directories(pico_fix_rp2040_usb_device_enumeration INTERFACE ${CMAKE_CURRENT_LIST_DIR}/include)
+
+target_link_libraries(pico_fix_rp2040_usb_device_enumeration INTERFACE hardware_structs pico_time)
\ No newline at end of file
diff --git a/src/rp2_common/pico_fix/rp2040_usb_device_enumeration/include/pico/fix/rp2040_usb_device_enumeration.h b/src/rp2_common/pico_fix/rp2040_usb_device_enumeration/include/pico/fix/rp2040_usb_device_enumeration.h
new file mode 100644
index 0000000..49f115e
--- /dev/null
+++ b/src/rp2_common/pico_fix/rp2040_usb_device_enumeration/include/pico/fix/rp2040_usb_device_enumeration.h
@@ -0,0 +1,17 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PICO_FIX_RP2040_USB_DEVICE_ENUMERATION_H
+#define _PICO_FIX_RP2040_USB_DEVICE_ENUMERATION_H
+
+/*! \brief Perform a brute force workaround for USB device enumeration issue
+ * \ingroup pico_fix
+ *
+ * This method should be called during the IRQ handler for a bus reset
+ */
+void rp2040_usb_device_enumeration_fix(void);
+
+#endif
\ No newline at end of file
diff --git a/src/rp2_common/pico_fix/rp2040_usb_device_enumeration/rp2040_usb_device_enumeration.c b/src/rp2_common/pico_fix/rp2040_usb_device_enumeration/rp2040_usb_device_enumeration.c
new file mode 100644
index 0000000..e1073ca
--- /dev/null
+++ b/src/rp2_common/pico_fix/rp2040_usb_device_enumeration/rp2040_usb_device_enumeration.c
@@ -0,0 +1,150 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "pico.h"
+#include "pico/time.h"
+#include "hardware/structs/usb.h"
+#include "hardware/gpio.h"
+#include "hardware/structs/iobank0.h"
+#include "hardware/structs/padsbank0.h"
+#include "pico/fix/rp2040_usb_device_enumeration.h"
+
+#define LS_SE0 0b00
+#define LS_J 0b01
+#define LS_K 0b10
+#define LS_SE1 0b11
+
+static void hw_enumeration_fix_wait_se0(void);
+static void hw_enumeration_fix_force_ls_j(void);
+static void hw_enumeration_fix_finish(void);
+
+void rp2040_usb_device_enumeration_fix(void) {
+#if PICO_RP2040_B0_SUPPORTED || PICO_RP2040_B1_SUPPORTED
+ // Actually check for B0/B1 h/w
+ if (rp2040_chip_version() == 1) {
+ // After coming out of reset, the hardware expects 800us of LS_J (linestate J) time
+ // before it will move to the connected state. However on a hub that broadcasts packets
+ // for other devices this isn't the case. The plan here is to wait for the end of the bus
+ // reset, force an LS_J for 1ms and then switch control back to the USB phy. Unfortunately
+ // this requires us to use GPIO15 as there is no other way to force the input path.
+ // We only need to force DP as DM can be left at zero. It will be gated off by GPIO
+ // logic if it isn't func selected.
+
+ // Wait SE0 phase will call force ls_j phase which will call finish phase
+ hw_enumeration_fix_wait_se0();
+ }
+#endif
+}
+
+static inline uint8_t hw_line_state(void) {
+ return (usb_hw->sie_status & USB_SIE_STATUS_LINE_STATE_BITS) >> USB_SIE_STATUS_LINE_STATE_LSB;
+}
+
+int64_t hw_enumeration_fix_wait_se0_callback(__unused alarm_id_t id, __unused void *user_data) {
+ if (hw_line_state() == LS_SE0) {
+ // Come back in 1ms and check again
+ return 1000;
+ } else {
+ // Now force LS_J (next stage of fix)
+ hw_enumeration_fix_force_ls_j();
+ // No more repeats
+ return 0;
+ }
+}
+
+static inline void hw_enumeration_fix_busy_wait_se0(void) {
+ while (hw_line_state() == LS_SE0) tight_loop_contents();
+ // Now force LS_J (next stage of fix)
+ hw_enumeration_fix_force_ls_j();
+}
+
+static void hw_enumeration_fix_wait_se0(void) {
+ // Wait for SE0 to end (i.e. the host to stop resetting). This reset can last quite long.
+ // 10-15ms so we are going to set a timer callback.
+
+#if !PICO_TIME_DEFAULT_ALARM_POOL_DISABLED
+ if (add_alarm_in_ms(1, hw_enumeration_fix_wait_se0_callback, NULL, true) >= 0) {
+ // hw_enumeration_fix_wait_se0_callback will be called in 1ms to check if se0 has finished
+ // (and will poll every 1ms from there)
+ return;
+ }
+#endif
+ // if timer pool disabled, or no timer available, have to busy wait.
+ hw_enumeration_fix_busy_wait_se0();
+}
+
+int64_t hw_enumeration_fix_force_ls_j_done(__unused alarm_id_t id, __unused void *user_data) {
+ hw_enumeration_fix_finish();
+ return 0;
+}
+
+static uint32_t gpio_ctrl_prev = 0;
+static uint32_t pad_ctrl_prev = 0;
+static const uint dp = 15;
+static const uint dm = 16;
+
+static void hw_enumeration_fix_force_ls_j(void) {
+ // DM must be 0 for this to work. This is true if it is selected
+ // to any other function. fn 8 on this pin is only for debug so shouldn't
+ // be selected
+ if (gpio_get_function(dm) == 8) {
+ panic("Not expecting DM to be function 8");
+ }
+
+ // Before changing any pin state, take a copy of the current gpio control register
+ gpio_ctrl_prev = iobank0_hw->io[dp].ctrl;
+ // Also take a copy of the pads register
+ pad_ctrl_prev = padsbank0_hw->io[dp];
+
+ // Enable bus keep and force pin to tristate, so USB DP muxing doesn't affect
+ // pin state
+ gpio_set_pulls(dp, true, true);
+ gpio_set_oeover(dp, GPIO_OVERRIDE_LOW);
+ // Select function 8 (USB debug muxing) without disturbing other controls
+ hw_write_masked(&iobank0_hw->io[dp].ctrl,
+ 8 << IO_BANK0_GPIO15_CTRL_FUNCSEL_LSB, IO_BANK0_GPIO15_CTRL_FUNCSEL_BITS);
+
+ // J state is a differential 1 for a full speed device so
+ // DP = 1 and DM = 0. Don't actually need to set DM low as it
+ // is already gated assuming it isn't funcseld.
+ gpio_set_inover(dp, GPIO_OVERRIDE_HIGH);
+
+ // Force PHY pull up to stay before switching away from the phy
+ hw_set_alias(usb_hw)->phy_direct = USB_USBPHY_DIRECT_DP_PULLUP_EN_BITS;
+ hw_set_alias(usb_hw)->phy_direct_override = USB_USBPHY_DIRECT_OVERRIDE_DP_PULLUP_EN_OVERRIDE_EN_BITS;
+
+ // Switch to GPIO phy with LS_J forced
+ usb_hw->muxing = USB_USB_MUXING_TO_DIGITAL_PAD_BITS | USB_USB_MUXING_SOFTCON_BITS;
+
+ // LS_J is now forced but while loop here just to check
+ hard_assert(hw_line_state() == LS_J); // "LS_J not forced!"
+
+#if !PICO_TIME_DEFAULT_ALARM_POOL_DISABLED
+ if (add_alarm_in_ms(1, hw_enumeration_fix_force_ls_j_done, NULL, true) >= 0) {
+ // hw_enumeration_fix_force_ls_j_done will be called in 1ms
+ return;
+ }
+#endif
+ // if timer pool disabled, or no timer available, have to busy wait.
+ busy_wait_us(1000);
+ hw_enumeration_fix_finish();
+}
+
+static void hw_enumeration_fix_finish(void) {
+ // Should think we are connected now
+ while (!(usb_hw->sie_status & USB_SIE_STATUS_CONNECTED_BITS)) tight_loop_contents();
+
+ // Switch back to USB phy
+ usb_hw->muxing = USB_USB_MUXING_TO_PHY_BITS | USB_USB_MUXING_SOFTCON_BITS;
+
+ // Get rid of DP pullup override
+ hw_clear_alias(usb_hw)->phy_direct_override = USB_USBPHY_DIRECT_OVERRIDE_DP_PULLUP_EN_OVERRIDE_EN_BITS;
+
+ // Finally, restore the gpio ctrl value back to GPIO15
+ iobank0_hw->io[dp].ctrl = gpio_ctrl_prev;
+ // Restore the pad ctrl value
+ padsbank0_hw->io[dp] = pad_ctrl_prev;
+}
diff --git a/src/rp2_common/pico_float/CMakeLists.txt b/src/rp2_common/pico_float/CMakeLists.txt
new file mode 100644
index 0000000..3543b03
--- /dev/null
+++ b/src/rp2_common/pico_float/CMakeLists.txt
@@ -0,0 +1,113 @@
+if (NOT TARGET pico_float)
+ # library to be depended on - we make this depend on particular implementations using per target generator expressions
+ pico_add_impl_library(pico_float)
+
+ # no custom implementation; falls thru to compiler
+ pico_add_impl_library(pico_float_compiler)
+
+ add_library(pico_float_headers INTERFACE)
+ target_include_directories(pico_float_headers INTERFACE ${CMAKE_CURRENT_LIST_DIR}/include)
+
+ # add alias "default" which is just rom.
+ add_library(pico_float_default INTERFACE)
+ target_link_libraries(pico_float_default INTERFACE pico_float_pico)
+
+ set(PICO_DEFAULT_FLOAT_IMPL pico_float_default)
+
+ target_link_libraries(pico_float INTERFACE
+ $<IF:$<BOOL:$<TARGET_PROPERTY:PICO_TARGET_FLOAT_IMPL>>,$<TARGET_PROPERTY:PICO_TARGET_FLOAT_IMPL>,${PICO_DEFAULT_FLOAT_IMPL}>)
+
+ pico_add_impl_library(pico_float_pico)
+ target_sources(pico_float_pico INTERFACE
+ ${CMAKE_CURRENT_LIST_DIR}/float_aeabi.S
+ ${CMAKE_CURRENT_LIST_DIR}/float_init_rom.c
+ ${CMAKE_CURRENT_LIST_DIR}/float_math.c
+ ${CMAKE_CURRENT_LIST_DIR}/float_v1_rom_shim.S
+ )
+
+ target_link_libraries(pico_float_pico INTERFACE pico_bootrom pico_float_headers)
+
+ pico_add_impl_library(pico_float_none)
+ target_sources(pico_float_none INTERFACE
+ ${CMAKE_CURRENT_LIST_DIR}/float_none.S
+ )
+
+ target_link_libraries(pico_float_none INTERFACE pico_float_headers)
+
+ function(wrap_float_functions TARGET)
+ pico_wrap_function(${TARGET} __aeabi_fadd)
+ pico_wrap_function(${TARGET} __aeabi_fdiv)
+ pico_wrap_function(${TARGET} __aeabi_fmul)
+ pico_wrap_function(${TARGET} __aeabi_frsub)
+ pico_wrap_function(${TARGET} __aeabi_fsub)
+ pico_wrap_function(${TARGET} __aeabi_cfcmpeq)
+ pico_wrap_function(${TARGET} __aeabi_cfrcmple)
+ pico_wrap_function(${TARGET} __aeabi_cfcmple)
+ pico_wrap_function(${TARGET} __aeabi_fcmpeq)
+ pico_wrap_function(${TARGET} __aeabi_fcmplt)
+ pico_wrap_function(${TARGET} __aeabi_fcmple)
+ pico_wrap_function(${TARGET} __aeabi_fcmpge)
+ pico_wrap_function(${TARGET} __aeabi_fcmpgt)
+ pico_wrap_function(${TARGET} __aeabi_fcmpun)
+ pico_wrap_function(${TARGET} __aeabi_i2f)
+ pico_wrap_function(${TARGET} __aeabi_l2f)
+ pico_wrap_function(${TARGET} __aeabi_ui2f)
+ pico_wrap_function(${TARGET} __aeabi_ul2f)
+ pico_wrap_function(${TARGET} __aeabi_f2iz)
+ pico_wrap_function(${TARGET} __aeabi_f2lz)
+ pico_wrap_function(${TARGET} __aeabi_f2uiz)
+ pico_wrap_function(${TARGET} __aeabi_f2ulz)
+ pico_wrap_function(${TARGET} __aeabi_f2d)
+ pico_wrap_function(${TARGET} sqrtf)
+ pico_wrap_function(${TARGET} cosf)
+ pico_wrap_function(${TARGET} sinf)
+ pico_wrap_function(${TARGET} tanf)
+ pico_wrap_function(${TARGET} atan2f)
+ pico_wrap_function(${TARGET} expf)
+ pico_wrap_function(${TARGET} logf)
+
+ pico_wrap_function(${TARGET} ldexpf)
+ pico_wrap_function(${TARGET} copysignf)
+ pico_wrap_function(${TARGET} truncf)
+ pico_wrap_function(${TARGET} floorf)
+ pico_wrap_function(${TARGET} ceilf)
+ pico_wrap_function(${TARGET} roundf)
+ pico_wrap_function(${TARGET} sincosf) # gnu
+ pico_wrap_function(${TARGET} asinf)
+ pico_wrap_function(${TARGET} acosf)
+ pico_wrap_function(${TARGET} atanf)
+ pico_wrap_function(${TARGET} sinhf)
+ pico_wrap_function(${TARGET} coshf)
+ pico_wrap_function(${TARGET} tanhf)
+ pico_wrap_function(${TARGET} asinhf)
+ pico_wrap_function(${TARGET} acoshf)
+ pico_wrap_function(${TARGET} atanhf)
+ pico_wrap_function(${TARGET} exp2f)
+ pico_wrap_function(${TARGET} log2f)
+ pico_wrap_function(${TARGET} exp10f)
+ pico_wrap_function(${TARGET} log10f)
+ pico_wrap_function(${TARGET} powf)
+ pico_wrap_function(${TARGET} powintf) #gnu
+ pico_wrap_function(${TARGET} hypotf)
+ pico_wrap_function(${TARGET} cbrtf)
+ pico_wrap_function(${TARGET} fmodf)
+ pico_wrap_function(${TARGET} dremf)
+ pico_wrap_function(${TARGET} remainderf)
+ pico_wrap_function(${TARGET} remquof)
+ pico_wrap_function(${TARGET} expm1f)
+ pico_wrap_function(${TARGET} log1pf)
+ pico_wrap_function(${TARGET} fmaf)
+ endfunction()
+
+ wrap_float_functions(pico_float_pico)
+ wrap_float_functions(pico_float_none)
+
+ macro(pico_set_float_implementation TARGET IMPL)
+ get_target_property(target_type ${TARGET} TYPE)
+ if ("EXECUTABLE" STREQUAL "${target_type}")
+ set_target_properties(${TARGET} PROPERTIES PICO_TARGET_FLOAT_IMPL "pico_float_${IMPL}")
+ else()
+ message(FATAL_ERROR "float implementation must be set on executable not library")
+ endif()
+ endmacro()
+endif()
diff --git a/src/rp2_common/pico_float/float_aeabi.S b/src/rp2_common/pico_float/float_aeabi.S
new file mode 100644
index 0000000..db393df
--- /dev/null
+++ b/src/rp2_common/pico_float/float_aeabi.S
@@ -0,0 +1,771 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "pico/asm_helper.S"
+#include "pico/bootrom/sf_table.h"
+#include "hardware/divider_helper.S"
+
+__pre_init __aeabi_float_init, 00020
+
+.syntax unified
+.cpu cortex-m0plus
+.thumb
+
+.macro float_section name
+#if PICO_FLOAT_IN_RAM
+.section RAM_SECTION_NAME(\name), "ax"
+#else
+.section SECTION_NAME(\name), "ax"
+#endif
+.endm
+
+.macro float_wrapper_section func
+float_section WRAPPER_FUNC_NAME(\func)
+.endm
+
+.macro _float_wrapper_func x
+ wrapper_func \x
+.endm
+
+.macro wrapper_func_f1 x
+ _float_wrapper_func \x
+#if PICO_FLOAT_PROPAGATE_NANS
+ mov ip, lr
+ bl __check_nan_f1
+ mov lr, ip
+#endif
+.endm
+
+.macro wrapper_func_f2 x
+ _float_wrapper_func \x
+#if PICO_FLOAT_PROPAGATE_NANS
+ mov ip, lr
+ bl __check_nan_f2
+ mov lr, ip
+#endif
+.endm
+
+.section .text
+
+#if PICO_FLOAT_PROPAGATE_NANS
+.thumb_func
+__check_nan_f1:
+ movs r3, #1
+ lsls r3, #24
+ lsls r2, r0, #1
+ adds r2, r3
+ bhi 1f
+ bx lr
+1:
+ bx ip
+
+.thumb_func
+__check_nan_f2:
+ movs r3, #1
+ lsls r3, #24
+ lsls r2, r0, #1
+ adds r2, r3
+ bhi 1f
+ lsls r2, r1, #1
+ adds r2, r3
+ bhi 2f
+ bx lr
+2:
+ mov r0, r1
+1:
+ bx ip
+#endif
+
+.macro table_tail_call SF_TABLE_OFFSET
+#if PICO_FLOAT_SUPPORT_ROM_V1 && PICO_RP2040_B0_SUPPORTED
+#ifndef NDEBUG
+ movs r3, #0
+ mov ip, r3
+#endif
+#endif
+ ldr r3, =sf_table
+ ldr r3, [r3, #\SF_TABLE_OFFSET]
+ bx r3
+.endm
+
+.macro shimmable_table_tail_call SF_TABLE_OFFSET shim
+ ldr r3, =sf_table
+ ldr r3, [r3, #\SF_TABLE_OFFSET]
+#if PICO_FLOAT_SUPPORT_ROM_V1 && PICO_RP2040_B0_SUPPORTED
+ mov ip, pc
+#endif
+ bx r3
+#if PICO_FLOAT_SUPPORT_ROM_V1 && PICO_RP2040_B0_SUPPORTED
+.byte \SF_TABLE_OFFSET, 0xdf
+.word \shim
+#endif
+.endm
+
+
+// note generally each function is in a separate section unless there is fall thru or branching between them
+// note fadd, fsub, fmul, fdiv are so tiny and just defer to rom so are lumped together so they can share constant pool
+
+// note functions are word aligned except where they are an odd number of linear instructions
+
+// float FUNC_NAME(__aeabi_fadd)(float, float) single-precision addition
+float_wrapper_section __aeabi_farithmetic
+// float FUNC_NAME(__aeabi_frsub)(float x, float y) single-precision reverse subtraction, y - x
+
+// frsub first because it is the only one that needs alignment
+.align 2
+wrapper_func __aeabi_frsub
+ eors r0, r1
+ eors r1, r0
+ eors r0, r1
+ // fall thru
+
+// float FUNC_NAME(__aeabi_fsub)(float x, float y) single-precision subtraction, x - y
+wrapper_func_f2 __aeabi_fsub
+#if PICO_FLOAT_PROPAGATE_NANS
+ // we want to return nan for inf-inf or -inf - -inf, but without too much upfront cost
+ mov r2, r0
+ eors r2, r1
+ bmi 1f // different signs
+ push {r0, r1, lr}
+ bl 1f
+ b fdiv_fsub_nan_helper
+1:
+#endif
+ table_tail_call SF_TABLE_FSUB
+
+wrapper_func_f2 __aeabi_fadd
+ table_tail_call SF_TABLE_FADD
+
+// float FUNC_NAME(__aeabi_fdiv)(float n, float d) single-precision division, n / d
+wrapper_func_f2 __aeabi_fdiv
+#if PICO_FLOAT_PROPAGATE_NANS
+ push {r0, r1, lr}
+ bl 1f
+ b fdiv_fsub_nan_helper
+1:
+#endif
+#if !PICO_DIVIDER_DISABLE_INTERRUPTS
+ // to support IRQ usage (or context switch) we must save/restore divider state around call if state is dirty
+ ldr r2, =(SIO_BASE)
+ ldr r3, [r2, #SIO_DIV_CSR_OFFSET]
+ lsrs r3, #SIO_DIV_CSR_DIRTY_SHIFT_FOR_CARRY
+ bcs fdiv_save_state
+#else
+ // to avoid worrying about IRQs (or context switches), simply disable interrupts around call
+ push {r4, lr}
+ mrs r4, PRIMASK
+ cpsid i
+ bl fdiv_shim_call
+ msr PRIMASK, r4
+ pop {r4, pc}
+#endif
+fdiv_shim_call:
+ table_tail_call SF_TABLE_FDIV
+#if !PICO_DIVIDER_DISABLE_INTERRUPTS
+fdiv_save_state:
+ save_div_state_and_lr
+ bl fdiv_shim_call
+ ldr r2, =(SIO_BASE)
+ restore_div_state_and_return
+#endif
+
+fdiv_fsub_nan_helper:
+#if PICO_FLOAT_PROPAGATE_NANS
+ pop {r1, r2}
+
+ // check for infinite op infinite (or rather check for infinite result with both
+ // operands being infinite)
+ lsls r3, r0, #1
+ asrs r3, r3, #24
+ adds r3, #1
+ beq 2f
+ pop {pc}
+2:
+ lsls r1, #1
+ asrs r1, r1, #24
+ lsls r2, #1
+ asrs r2, r2, #24
+ ands r1, r2
+ adds r1, #1
+ bne 3f
+ // infinite to nan
+ movs r1, #1
+ lsls r1, #22
+ orrs r0, r1
+3:
+ pop {pc}
+#endif
+
+// float FUNC_NAME(__aeabi_fmul)(float, float) single-precision multiplication
+wrapper_func_f2 __aeabi_fmul
+#if PICO_FLOAT_PROPAGATE_NANS
+ push {r0, r1, lr}
+ bl 1f
+ pop {r1, r2}
+
+ // check for multiplication of infinite by zero (or rather check for infinite result with either
+ // operand 0)
+ lsls r3, r0, #1
+ asrs r3, r3, #24
+ adds r3, #1
+ beq 2f
+ pop {pc}
+2:
+ ands r1, r2
+ bne 3f
+ // infinite to nan
+ movs r1, #1
+ lsls r1, #22
+ orrs r0, r1
+3:
+ pop {pc}
+1:
+#endif
+ table_tail_call SF_TABLE_FMUL
+
+// void FUNC_NAME(__aeabi_cfrcmple)(float, float) reversed 3-way (<, =, ?>) compare [1], result in PSR ZC flags
+float_wrapper_section __aeabi_cfcmple
+.align 2
+wrapper_func __aeabi_cfrcmple
+ push {r0-r2, lr}
+ eors r0, r1
+ eors r1, r0
+ eors r0, r1
+ b __aeabi_cfcmple_guts
+
+// NOTE these share an implementation as we have no excepting NaNs.
+// void FUNC_NAME(__aeabi_cfcmple)(float, float) 3-way (<, =, ?>) compare [1], result in PSR ZC flags
+// void FUNC_NAME(__aeabi_cfcmpeq)(float, float) non-excepting equality comparison [1], result in PSR ZC flags
+.align 2
+wrapper_func __aeabi_cfcmple
+wrapper_func __aeabi_cfcmpeq
+ push {r0-r2, lr}
+
+__aeabi_cfcmple_guts:
+ lsls r2,r0,#1
+ lsrs r2,#24
+ beq 1f
+ cmp r2,#0xff
+ bne 2f
+ lsls r2, r0, #9
+ bhi 3f
+1:
+ lsrs r0,#23 @ clear mantissa if denormal or infinite
+ lsls r0,#23
+2:
+ lsls r2,r1,#1
+ lsrs r2,#24
+ beq 1f
+ cmp r2,#0xff
+ bne 2f
+ lsls r2, r1, #9
+ bhi 3f
+1:
+ lsrs r1,#23 @ clear mantissa if denormal or infinite
+ lsls r1,#23
+2:
+ movs r2,#1 @ initialise result
+ eors r1,r0
+ bmi 2f @ opposite signs? then can proceed on basis of sign of x
+ eors r1,r0 @ restore y
+ bpl 1f
+ cmp r1,r0
+ pop {r0-r2, pc}
+1:
+ cmp r0,r1
+ pop {r0-r2, pc}
+2:
+ orrs r1, r0 @ handle 0/-0
+ adds r1, r1 @ note this always sets C
+ beq 3f
+ mvns r0, r0 @ carry inverse of r0 sign
+ adds r0, r0
+3:
+ pop {r0-r2, pc}
+
+
+// int FUNC_NAME(__aeabi_fcmpeq)(float, float) result (1, 0) denotes (=, ?<>) [2], use for C == and !=
+float_wrapper_section __aeabi_fcmpeq
+.align 2
+wrapper_func __aeabi_fcmpeq
+ push {lr}
+ bl __aeabi_cfcmpeq
+ beq 1f
+ movs r0, #0
+ pop {pc}
+1:
+ movs r0, #1
+ pop {pc}
+
+// int FUNC_NAME(__aeabi_fcmplt)(float, float) result (1, 0) denotes (<, ?>=) [2], use for C <
+float_wrapper_section __aeabi_fcmplt
+.align 2
+wrapper_func __aeabi_fcmplt
+ push {lr}
+ bl __aeabi_cfcmple
+ sbcs r0, r0
+ pop {pc}
+
+// int FUNC_NAME(__aeabi_fcmple)(float, float) result (1, 0) denotes (<=, ?>) [2], use for C <=
+float_wrapper_section __aeabi_fcmple
+.align 2
+wrapper_func __aeabi_fcmple
+ push {lr}
+ bl __aeabi_cfcmple
+ bls 1f
+ movs r0, #0
+ pop {pc}
+1:
+ movs r0, #1
+ pop {pc}
+
+// int FUNC_NAME(__aeabi_fcmpge)(float, float) result (1, 0) denotes (>=, ?<) [2], use for C >=
+float_wrapper_section __aeabi_fcmpge
+.align 2
+wrapper_func __aeabi_fcmpge
+ push {lr}
+ // because of NaNs it is better to reverse the args than the result
+ bl __aeabi_cfrcmple
+ bls 1f
+ movs r0, #0
+ pop {pc}
+1:
+ movs r0, #1
+ pop {pc}
+
+// int FUNC_NAME(__aeabi_fcmpgt)(float, float) result (1, 0) denotes (>, ?<=) [2], use for C >
+float_wrapper_section __aeabi_fcmpgt
+wrapper_func __aeabi_fcmpgt
+ push {lr}
+ // because of NaNs it is better to reverse the args than the result
+ bl __aeabi_cfrcmple
+ sbcs r0, r0
+ pop {pc}
+
+// int FUNC_NAME(__aeabi_fcmpun)(float, float) result (1, 0) denotes (?, <=>) [2], use for C99 isunordered()
+float_wrapper_section __aeabi_fcmpun
+wrapper_func __aeabi_fcmpun
+ movs r3, #1
+ lsls r3, #24
+ lsls r2, r0, #1
+ adds r2, r3
+ bhi 1f
+ lsls r2, r1, #1
+ adds r2, r3
+ bhi 1f
+ movs r0, #0
+ bx lr
+1:
+ movs r0, #1
+ bx lr
+
+
+// float FUNC_NAME(__aeabi_ui2f)(unsigned) unsigned to float (single precision) conversion
+float_wrapper_section __aeabi_ui2f
+wrapper_func __aeabi_ui2f
+ subs r1, r1
+ cmp r0, #0
+ bne __aeabi_i2f_main
+ mov r0, r1
+ bx lr
+
+float_wrapper_section __aeabi_i2f
+// float FUNC_NAME(__aeabi_i2f)(int) integer to float (single precision) conversion
+wrapper_func __aeabi_i2f
+ lsrs r1, r0, #31
+ lsls r1, #31
+ bpl 1f
+ rsbs r0, #0
+1:
+ cmp r0, #0
+ beq 7f
+__aeabi_i2f_main:
+
+ mov ip, lr
+ push {r0, r1}
+ ldr r3, =sf_clz_func
+ ldr r3, [r3]
+ blx r3
+ pop {r1, r2}
+ lsls r1, r0
+ subs r0, #158
+ rsbs r0, #0
+
+ adds r1,#0x80 @ rounding
+ bcs 5f @ tripped carry? then have leading 1 in C as required (and result is even so can ignore sticky bits)
+
+ lsls r3,r1,#24 @ check bottom 8 bits of r1
+ beq 6f @ in rounding-tie case?
+ lsls r1,#1 @ remove leading 1
+3:
+ lsrs r1,#9 @ align mantissa
+ lsls r0,#23 @ align exponent
+ orrs r0,r2 @ assemble exponent and mantissa
+4:
+ orrs r0,r1 @ apply sign
+1:
+ bx ip
+5:
+ adds r0,#1 @ correct exponent offset
+ b 3b
+6:
+ lsrs r1,#9 @ ensure even result
+ lsls r1,#10
+ b 3b
+7:
+ bx lr
+
+
+// int FUNC_NAME(__aeabi_f2iz)(float) float (single precision) to integer C-style conversion [3]
+float_wrapper_section __aeabi_f2iz
+wrapper_func __aeabi_f2iz
+regular_func float2int_z
+ lsls r1, r0, #1
+ lsrs r2, r1, #24
+ movs r3, #0x80
+ lsls r3, #24
+ cmp r2, #126
+ ble 1f
+ subs r2, #158
+ bge 2f
+ asrs r1, r0, #31
+ lsls r0, #9
+ lsrs r0, #1
+ orrs r0, r3
+ negs r2, r2
+ lsrs r0, r2
+ lsls r1, #1
+ adds r1, #1
+ muls r0, r1
+ bx lr
+1:
+ movs r0, #0
+ bx lr
+2:
+ lsrs r0, #31
+ adds r0, r3
+ subs r0, #1
+ bx lr
+
+ cmn r0, r0
+ bcc float2int
+ push {lr}
+ lsls r0, #1
+ lsrs r0, #1
+ movs r1, #0
+ bl __aeabi_f2uiz
+ cmp r0, #0
+ bmi 1f
+ rsbs r0, #0
+ pop {pc}
+1:
+ movs r0, #128
+ lsls r0, #24
+ pop {pc}
+
+float_section float2int
+regular_func float2int
+ shimmable_table_tail_call SF_TABLE_FLOAT2INT float2int_shim
+
+float_section float2fix
+regular_func float2fix
+ shimmable_table_tail_call SF_TABLE_FLOAT2FIX float2fix_shim
+
+float_section float2ufix
+regular_func float2ufix
+ table_tail_call SF_TABLE_FLOAT2UFIX
+
+// unsigned FUNC_NAME(__aeabi_f2uiz)(float) float (single precision) to unsigned C-style conversion [3]
+float_wrapper_section __aeabi_f2uiz
+wrapper_func __aeabi_f2uiz
+ table_tail_call SF_TABLE_FLOAT2UINT
+
+float_section fix2float
+regular_func fix2float
+ table_tail_call SF_TABLE_FIX2FLOAT
+
+float_section ufix2float
+regular_func ufix2float
+ table_tail_call SF_TABLE_UFIX2FLOAT
+
+float_section fix642float
+regular_func fix642float
+ shimmable_table_tail_call SF_TABLE_FIX642FLOAT fix642float_shim
+
+float_section ufix642float
+regular_func ufix642float
+ shimmable_table_tail_call SF_TABLE_UFIX642FLOAT ufix642float_shim
+
+// float FUNC_NAME(__aeabi_l2f)(long long) long long to float (single precision) conversion
+float_wrapper_section __aeabi_l2f
+1:
+ ldr r2, =__aeabi_i2f
+ bx r2
+wrapper_func __aeabi_l2f
+ asrs r2, r0, #31
+ cmp r1, r2
+ beq 1b
+ shimmable_table_tail_call SF_TABLE_INT642FLOAT int642float_shim
+
+// float FUNC_NAME(__aeabi_l2f)(long long) long long to float (single precision) conversion
+float_wrapper_section __aeabi_ul2f
+1:
+ ldr r2, =__aeabi_ui2f
+ bx r2
+wrapper_func __aeabi_ul2f
+ cmp r1, #0
+ beq 1b
+ shimmable_table_tail_call SF_TABLE_UINT642FLOAT uint642float_shim
+
+// long long FUNC_NAME(__aeabi_f2lz)(float) float (single precision) to long long C-style conversion [3]
+float_wrapper_section __aeabi_f2lz
+wrapper_func __aeabi_f2lz
+regular_func float2int64_z
+ cmn r0, r0
+ bcc float2int64
+ push {lr}
+ lsls r0, #1
+ lsrs r0, #1
+ movs r1, #0
+ bl float2ufix64
+ cmp r1, #0
+ bmi 1f
+ movs r2, #0
+ rsbs r0, #0
+ sbcs r2, r1
+ mov r1, r2
+ pop {pc}
+1:
+ movs r1, #128
+ lsls r1, #24
+ movs r0, #0
+ pop {pc}
+
+float_section float2int64
+regular_func float2int64
+ shimmable_table_tail_call SF_TABLE_FLOAT2INT64 float2int64_shim
+
+float_section float2fix64
+regular_func float2fix64
+ shimmable_table_tail_call SF_TABLE_FLOAT2FIX64 float2fix64_shim
+
+// unsigned long long FUNC_NAME(__aeabi_f2ulz)(float) float to unsigned long long C-style conversion [3]
+float_wrapper_section __aeabi_f2ulz
+wrapper_func __aeabi_f2ulz
+ shimmable_table_tail_call SF_TABLE_FLOAT2UINT64 float2uint64_shim
+
+float_section float2ufix64
+regular_func float2ufix64
+ shimmable_table_tail_call SF_TABLE_FLOAT2UFIX64 float2ufix64_shim
+
+float_wrapper_section __aeabi_f2d
+1:
+#if PICO_FLOAT_PROPAGATE_NANS
+ // copy sign bit and 25 NAN id bits into sign bit and significant ID bits, also setting the high id bit
+ asrs r1, r0, #3
+ movs r2, #0xf
+ lsls r2, #27
+ orrs r1, r2
+ lsls r0, #25
+ bx lr
+#endif
+wrapper_func __aeabi_f2d
+#if PICO_FLOAT_PROPAGATE_NANS
+ movs r3, #1
+ lsls r3, #24
+ lsls r2, r0, #1
+ adds r2, r3
+ bhi 1b
+#endif
+ shimmable_table_tail_call SF_TABLE_FLOAT2DOUBLE float2double_shim
+
+float_wrapper_section srqtf
+wrapper_func_f1 sqrtf
+#if PICO_FLOAT_SUPPORT_ROM_V1 && PICO_RP2040_B0_SUPPORTED
+ // check for negative
+ asrs r1, r0, #23
+ bmi 1f
+#endif
+ table_tail_call SF_TABLE_FSQRT
+#if PICO_FLOAT_SUPPORT_ROM_V1 && PICO_RP2040_B0_SUPPORTED
+1:
+ mvns r0, r1
+ cmp r0, #255
+ bne 2f
+ // -0 or -Denormal return -0 (0x80000000)
+ lsls r0, #31
+ bx lr
+2:
+ // return -Inf (0xff800000)
+ asrs r0, r1, #31
+ lsls r0, #23
+ bx lr
+#endif
+
+float_wrapper_section cosf
+// note we don't use _f1 since we do an infinity/nan check for outside of range
+wrapper_func cosf
+ // rom version only works for -128 < angle < 128
+ lsls r1, r0, #1
+ lsrs r1, #24
+ cmp r1, #127 + 7
+ bge 1f
+2:
+ table_tail_call SF_TABLE_FCOS
+1:
+#if PICO_FLOAT_PROPAGATE_NANS
+ // also check for infinites
+ cmp r1, #255
+ bne 3f
+ // infinite to nan
+ movs r1, #1
+ lsls r1, #22
+ orrs r0, r1
+ bx lr
+3:
+#endif
+ ldr r1, =0x40c90fdb // 2 * M_PI
+ push {lr}
+ bl remainderf
+ pop {r1}
+ mov lr, r1
+ b 2b
+
+float_wrapper_section sinf
+// note we don't use _f1 since we do an infinity/nan check for outside of range
+wrapper_func sinf
+ // rom version only works for -128 < angle < 128
+ lsls r1, r0, #1
+ lsrs r1, #24
+ cmp r1, #127 + 7
+ bge 1f
+2:
+ table_tail_call SF_TABLE_FSIN
+1:
+#if PICO_FLOAT_PROPAGATE_NANS
+ // also check for infinites
+ cmp r1, #255
+ bne 3f
+ // infinite to nan
+ movs r1, #1
+ lsls r1, #22
+ orrs r0, r1
+ bx lr
+3:
+#endif
+ ldr r1, =0x40c90fdb // 2 * M_PI
+ push {lr}
+ bl remainderf
+ pop {r1}
+ mov lr, r1
+ b 2b
+
+float_wrapper_section sincosf
+// note we don't use _f1 since we do an infinity/nan check for outside of range
+wrapper_func sincosf
+ push {r1, r2, lr}
+ // rom version only works for -128 < angle < 128
+ lsls r3, r0, #1
+ lsrs r3, #24
+ cmp r3, #127 + 7
+ bge 3f
+2:
+ ldr r3, =sf_table
+ ldr r3, [r3, #SF_TABLE_FSIN]
+ blx r3
+ pop {r2, r3}
+ str r0, [r2]
+ str r1, [r3]
+ pop {pc}
+#if PICO_FLOAT_PROPAGATE_NANS
+.align 2
+ pop {pc}
+#endif
+3:
+#if PICO_FLOAT_PROPAGATE_NANS
+ // also check for infinites
+ cmp r3, #255
+ bne 4f
+ // infinite to nan
+ movs r3, #1
+ lsls r3, #22
+ orrs r0, r3
+ str r0, [r1]
+ str r0, [r2]
+ add sp, #12
+ bx lr
+4:
+#endif
+ ldr r1, =0x40c90fdb // 2 * M_PI
+ push {lr}
+ bl remainderf
+ pop {r1}
+ mov lr, r1
+ b 2b
+
+float_wrapper_section tanf
+// note we don't use _f1 since we do an infinity/nan check for outside of range
+wrapper_func tanf
+ // rom version only works for -128 < angle < 128
+ lsls r1, r0, #1
+ lsrs r1, #24
+ cmp r1, #127 + 7
+ bge ftan_out_of_range
+ftan_in_range:
+#if !PICO_DIVIDER_DISABLE_INTERRUPTS
+ // to support IRQ usage (or context switch) we must save/restore divider state around call if state is dirty
+ ldr r2, =(SIO_BASE)
+ ldr r3, [r2, #SIO_DIV_CSR_OFFSET]
+ lsrs r3, #SIO_DIV_CSR_DIRTY_SHIFT_FOR_CARRY
+ bcs ftan_save_state
+#else
+ // to avoid worrying about IRQs (or context switches), simply disable interrupts around call
+ push {r4, lr}
+ mrs r4, PRIMASK
+ cpsid i
+ bl ftan_shim_call
+ msr PRIMASK, r4
+ pop {r4, pc}
+#endif
+ftan_shim_call:
+ table_tail_call SF_TABLE_FTAN
+#if !PICO_DIVIDER_DISABLE_INTERRUPTS
+ftan_save_state:
+ save_div_state_and_lr
+ bl ftan_shim_call
+ ldr r2, =(SIO_BASE)
+ restore_div_state_and_return
+#endif
+ftan_out_of_range:
+#if PICO_FLOAT_PROPAGATE_NANS
+ // also check for infinites
+ cmp r1, #255
+ bne 3f
+ // infinite to nan
+ movs r1, #1
+ lsls r1, #22
+ orrs r0, r1
+ bx lr
+3:
+#endif
+ ldr r1, =0x40c90fdb // 2 * M_PI
+ push {lr}
+ bl remainderf
+ pop {r1}
+ mov lr, r1
+ b ftan_in_range
+
+float_wrapper_section atan2f
+wrapper_func_f2 atan2f
+ shimmable_table_tail_call SF_TABLE_FATAN2 fatan2_shim
+
+float_wrapper_section expf
+wrapper_func_f1 expf
+ table_tail_call SF_TABLE_FEXP
+
+float_wrapper_section logf
+wrapper_func_f1 logf
+ table_tail_call SF_TABLE_FLN
diff --git a/src/rp2_common/pico_float/float_init_rom.c b/src/rp2_common/pico_float/float_init_rom.c
new file mode 100644
index 0000000..646c0e9
--- /dev/null
+++ b/src/rp2_common/pico_float/float_init_rom.c
@@ -0,0 +1,70 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <string.h>
+#include "pico/bootrom.h"
+#include "pico/bootrom/sf_table.h"
+
+// NOTE THIS FUNCTION TABLE IS NOT PUBLIC OR NECESSARILY COMPLETE...
+// IT IS ***NOT*** SAFE TO CALL THESE FUNCTION POINTERS FROM ARBITRARY CODE
+uint32_t sf_table[SF_TABLE_V2_SIZE / 2];
+void __attribute__((weak)) *sf_clz_func;
+
+#if !(PICO_FLOAT_SUPPORT_ROM_V1 && PICO_RP2040_B0_SUPPORTED)
+static __attribute__((noreturn)) void missing_float_func_shim(void) {
+ panic("");
+}
+#endif
+
+void __aeabi_float_init(void) {
+ int rom_version = rp2040_rom_version();
+ void *rom_table = rom_data_lookup(rom_table_code('S', 'F'));
+#if PICO_FLOAT_SUPPORT_ROM_V1 && PICO_RP2040_B0_SUPPORTED
+ if (rom_version == 1) {
+ memcpy(&sf_table, rom_table, SF_TABLE_V1_SIZE);
+ extern void float_table_shim_on_use_helper(void);
+ // todo replace NDEBUG with a more exclusive assertion guard
+#ifndef NDEBUG
+ if (*(uint16_t *)0x29ee != 0x0fc4 || // this is packx
+ *(uint16_t *)0x29c0 != 0x0dc2 || // this is upackx
+ *(uint16_t *)0x2b96 != 0xb5c0 || // this is cordic_vec
+ *(uint16_t *)0x2b18 != 0x2500 || // this is packretns
+ *(uint16_t *)0x2acc != 0xb510 || // this is float2fix
+ *(uint32_t *)0x2cfc != 0x6487ed51 // pi_q29
+ ) {
+ panic("");
+ }
+#endif
+
+ // this is a little tricky.. we only want to pull in a shim if the corresponding function
+ // is called. to that end we include a SVC instruction with the table offset as the call number
+ // followed by the shim function pointer inside the actual wrapper function. that way if the wrapper
+ // function is garbage collected, so is the shim function.
+ //
+ // float_table_shim_on_use_helper expects this SVC instruction in the calling code soon after the address
+ // pointed to by IP and patches the float_table entry with the real shim the first time the function is called.
+
+ for(uint i=SF_TABLE_V1_SIZE/4; i<SF_TABLE_V2_SIZE/4; i++) {
+ sf_table[i] = (uintptr_t)float_table_shim_on_use_helper;
+ }
+ // we shim these for -0 and -denormal handling
+ sf_table[SF_TABLE_FLOAT2INT/4] = sf_table[SF_TABLE_FLOAT2FIX/4] = (uintptr_t)float_table_shim_on_use_helper;
+ }
+#else
+ if (rom_version == 1) {
+ memcpy(&sf_table, rom_table, SF_TABLE_V1_SIZE);
+ // opting for soft failure for now - you'll get a panic at runtime if you call any of the missing methods
+ for(uint i=0;i<SF_TABLE_V2_SIZE/4;i++) {
+ if (!sf_table[i]) sf_table[i] = (uintptr_t)missing_float_func_shim;
+ }
+ }
+#endif
+ if (rom_version >= 2) {
+ assert(*((uint8_t *)(rom_table-2)) * 4 >= SF_TABLE_V2_SIZE);
+ memcpy(&sf_table, rom_table, SF_TABLE_V2_SIZE);
+ }
+ sf_clz_func = rom_func_lookup(ROM_FUNC_CLZ32);
+}
diff --git a/src/rp2_common/pico_float/float_math.c b/src/rp2_common/pico_float/float_math.c
new file mode 100644
index 0000000..a48a2d7
--- /dev/null
+++ b/src/rp2_common/pico_float/float_math.c
@@ -0,0 +1,584 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "pico/types.h"
+#include "pico/float.h"
+#include "pico/platform.h"
+
+// opened a separate issue https://github.com/raspberrypi/pico-sdk/issues/166 to deal with these warnings if at all
+_Pragma("GCC diagnostic push")
+_Pragma("GCC diagnostic ignored \"-Wconversion\"")
+_Pragma("GCC diagnostic ignored \"-Wsign-conversion\"")
+
+typedef uint32_t ui32;
+typedef int32_t i32;
+
+#define FPINF ( HUGE_VALF)
+#define FMINF (-HUGE_VALF)
+#define NANF ((float)NAN)
+#define PZERO (+0.0)
+#define MZERO (-0.0)
+
+#define PI 3.14159265358979323846
+#define LOG2 0.69314718055994530941
+// Unfortunately in double precision ln(10) is very close to half-way between to representable numbers
+#define LOG10 2.30258509299404568401
+#define LOG2E 1.44269504088896340737
+#define LOG10E 0.43429448190325182765
+#define ONETHIRD 0.33333333333333333333
+
+#define PIf 3.14159265358979323846f
+#define LOG2f 0.69314718055994530941f
+#define LOG2Ef 1.44269504088896340737f
+#define LOG10Ef 0.43429448190325182765f
+#define ONETHIRDf 0.33333333333333333333f
+
+#define FUNPACK(x,e,m) e=((x)>>23)&0xff,m=((x)&0x007fffff)|0x00800000
+#define FUNPACKS(x,s,e,m) s=((x)>>31),FUNPACK((x),(e),(m))
+
+typedef union {
+ float f;
+ ui32 ix;
+} float_ui32;
+
+static inline float ui322float(ui32 ix) {
+ float_ui32 tmp;
+ tmp.ix = ix;
+ return tmp.f;
+}
+
+static inline ui32 float2ui32(float f) {
+ float_ui32 tmp;
+ tmp.f = f;
+ return tmp.ix;
+}
+
+static inline bool fisnan(float x) {
+ ui32 ix=float2ui32(x);
+ return ix * 2 > 0xff000000u;
+}
+
+#if PICO_FLOAT_PROPAGATE_NANS
+#define check_nan_f1(x) if (fisnan((x))) return (x)
+#define check_nan_f2(x,y) if (fisnan((x))) return (x); else if (fisnan((y))) return (y);
+#else
+#define check_nan_f1(x) ((void)0)
+#define check_nan_f2(x,y) ((void)0)
+#endif
+
+static inline int fgetsignexp(float x) {
+ ui32 ix=float2ui32(x);
+ return (ix>>23)&0x1ff;
+}
+
+static inline int fgetexp(float x) {
+ ui32 ix=float2ui32(x);
+ return (ix>>23)&0xff;
+}
+
+static inline float fldexp(float x,int de) {
+ ui32 ix=float2ui32(x),iy;
+ int e;
+ e=fgetexp(x);
+ if(e==0||e==0xff) return x;
+ e+=de;
+ if(e<=0) iy=ix&0x80000000; // signed zero for underflow
+ else if(e>=0xff) iy=(ix&0x80000000)|0x7f800000ULL; // signed infinity on overflow
+ else iy=ix+((ui32)de<<23);
+ return ui322float(iy);
+}
+
+float WRAPPER_FUNC(ldexpf)(float x, int de) {
+ check_nan_f1(x);
+ return fldexp(x, de);
+}
+
+static inline float fcopysign(float x,float y) {
+ ui32 ix=float2ui32(x),iy=float2ui32(y);
+ ix=((ix&0x7fffffff)|(iy&0x80000000));
+ return ui322float(ix);
+}
+
+float WRAPPER_FUNC(copysignf)(float x, float y) {
+ check_nan_f2(x,y);
+ return fcopysign(x, y);
+}
+
+static inline int fiszero(float x) { return fgetexp (x)==0; }
+static inline int fispzero(float x) { return fgetsignexp(x)==0; }
+static inline int fismzero(float x) { return fgetsignexp(x)==0x100; }
+static inline int fisinf(float x) { return fgetexp (x)==0xff; }
+static inline int fispinf(float x) { return fgetsignexp(x)==0xff; }
+static inline int fisminf(float x) { return fgetsignexp(x)==0x1ff; }
+
+static inline int fisint(float x) {
+ ui32 ix=float2ui32(x),m;
+ int e=fgetexp(x);
+ if(e==0) return 1; // 0 is an integer
+ e-=0x7f; // remove exponent bias
+ if(e<0) return 0; // |x|<1
+ e=23-e; // bit position in mantissa with significance 1
+ if(e<=0) return 1; // |x| large, so must be an integer
+ m=(1<<e)-1; // mask for bits of significance <1
+ if(ix&m) return 0; // not an integer
+ return 1;
+}
+
+static inline int fisoddint(float x) {
+ ui32 ix=float2ui32(x),m;
+ int e=fgetexp(x);
+ e-=0x7f; // remove exponent bias
+ if(e<0) return 0; // |x|<1; 0 is not odd
+ e=23-e; // bit position in mantissa with significance 1
+ if(e<0) return 0; // |x| large, so must be even
+ m=(1<<e)-1; // mask for bits of significance <1 (if any)
+ if(ix&m) return 0; // not an integer
+ if(e==23) return 1; // value is exactly 1
+ return (ix>>e)&1;
+}
+
+static inline int fisstrictneg(float x) {
+ ui32 ix=float2ui32(x);
+ if(fiszero(x)) return 0;
+ return ix>>31;
+}
+
+static inline int fisneg(float x) {
+ ui32 ix=float2ui32(x);
+ return ix>>31;
+}
+
+static inline float fneg(float x) {
+ ui32 ix=float2ui32(x);
+ ix^=0x80000000;
+ return ui322float(ix);
+}
+
+static inline int fispo2(float x) {
+ ui32 ix=float2ui32(x);
+ if(fiszero(x)) return 0;
+ if(fisinf(x)) return 0;
+ ix&=0x007fffff;
+ return ix==0;
+}
+
+static inline float fnan_or(float x) {
+#if PICO_FLOAT_PROPAGATE_NANS
+ return NANF;
+#else
+ return x;
+#endif
+}
+
+float WRAPPER_FUNC(truncf)(float x) {
+ check_nan_f1(x);
+ ui32 ix=float2ui32(x),m;
+ int e=fgetexp(x);
+ e-=0x7f; // remove exponent bias
+ if(e<0) { // |x|<1
+ ix&=0x80000000;
+ return ui322float(ix);
+ }
+ e=23-e; // bit position in mantissa with significance 1
+ if(e<=0) return x; // |x| large, so must be an integer
+ m=(1<<e)-1; // mask for bits of significance <1
+ ix&=~m;
+ return ui322float(ix);
+}
+
+float WRAPPER_FUNC(roundf)(float x) {
+ check_nan_f1(x);
+ ui32 ix=float2ui32(x),m;
+ int e=fgetexp(x);
+ e-=0x7f; // remove exponent bias
+ if(e<-1) { // |x|<0.5
+ ix&=0x80000000;
+ return ui322float(ix);
+ }
+ if(e==-1) { // 0.5<=|x|<1
+ ix&=0x80000000;
+ ix|=0x3f800000; // ±1
+ return ui322float(ix);
+ }
+ e=23-e; // bit position in mantissa with significance 1, <=23
+ if(e<=0) return x; // |x| large, so must be an integer
+ m=1<<(e-1); // mask for bit of significance 0.5
+ ix+=m;
+ m=m+m-1; // mask for bits of significance <1
+ ix&=~m;
+ return ui322float(ix);
+}
+
+float WRAPPER_FUNC(floorf)(float x) {
+ check_nan_f1(x);
+ ui32 ix=float2ui32(x),m;
+ int e=fgetexp(x);
+ if(e==0) { // x==0
+ ix&=0x80000000;
+ return ui322float(ix);
+ }
+ e-=0x7f; // remove exponent bias
+ if(e<0) { // |x|<1, not zero
+ if(fisneg(x)) return -1;
+ return PZERO;
+ }
+ e=23-e; // bit position in mantissa with significance 1
+ if(e<=0) return x; // |x| large, so must be an integer
+ m=(1<<e)-1; // mask for bit of significance <1
+ if(fisneg(x)) ix+=m; // add 1-ε to magnitude if negative
+ ix&=~m; // truncate
+ return ui322float(ix);
+}
+
+float WRAPPER_FUNC(ceilf)(float x) {
+ check_nan_f1(x);
+ ui32 ix=float2ui32(x),m;
+ int e=fgetexp(x);
+ if(e==0) { // x==0
+ ix&=0x80000000;
+ return ui322float(ix);
+ }
+ e-=0x7f; // remove exponent bias
+ if(e<0) { // |x|<1, not zero
+ if(fisneg(x)) return MZERO;
+ return 1;
+ }
+ e=23-e; // bit position in mantissa with significance 1
+ if(e<=0) return x; // |x| large, so must be an integer
+ m=(1<<e)-1; // mask for bit of significance <1
+ if(!fisneg(x)) ix+=m; // add 1-ε to magnitude if positive
+ ix&=~m; // truncate
+ return ui322float(ix);
+}
+
+float WRAPPER_FUNC(asinf)(float x) {
+ check_nan_f1(x);
+ float u;
+ u=(1.0f-x)*(1.0f+x);
+ if(fisstrictneg(u)) return fnan_or(FPINF);
+ return atan2f(x,sqrtf(u));
+}
+
+float WRAPPER_FUNC(acosf)(float x) {
+ check_nan_f1(x);
+ float u;
+ u=(1.0f-x)*(1.0f+x);
+ if(fisstrictneg(u)) return fnan_or(FPINF);
+ return atan2f(sqrtf(u),x);
+}
+
+float WRAPPER_FUNC(atanf)(float x) {
+ check_nan_f1(x);
+ if(fispinf(x)) return (float)( PIf/2);
+ if(fisminf(x)) return (float)(-PIf/2);
+ return atan2f(x,1.0f);
+}
+
+float WRAPPER_FUNC(sinhf)(float x) {
+ check_nan_f1(x);
+ return fldexp((expf(x)-expf(fneg(x))),-1);
+}
+
+float WRAPPER_FUNC(coshf)(float x) {
+ check_nan_f1(x);
+ return fldexp((expf(x)+expf(fneg(x))),-1);
+}
+
+float WRAPPER_FUNC(tanhf)(float x) {
+ check_nan_f1(x);
+ float u;
+ int e;
+ e=fgetexp(x);
+ if(e>=4+0x7f) { // |x|>=16?
+ if(!fisneg(x)) return 1; // 1 << exp 2x; avoid generating infinities later
+ else return -1; // 1 >> exp 2x
+ }
+ u=expf(fldexp(x,1));
+ return (u-1.0f)/(u+1.0f);
+}
+
+float WRAPPER_FUNC(asinhf)(float x) {
+ check_nan_f1(x);
+ int e;
+ e=fgetexp(x);
+ if(e>=16+0x7f) { // |x|>=2^16?
+ if(!fisneg(x)) return logf( x )+LOG2f; // 1/x^2 << 1
+ else return fneg(logf(fneg(x))+LOG2f); // 1/x^2 << 1
+ }
+ if(x>0) return (float)log(sqrt((double)x*(double)x+1.0)+(double)x);
+ else return fneg((float)log(sqrt((double)x*(double)x+1.0)-(double)x));
+}
+
+float WRAPPER_FUNC(acoshf)(float x) {
+ check_nan_f1(x);
+ int e;
+ if(fisneg(x)) x=fneg(x);
+ e=fgetexp(x);
+ if(e>=16+0x7f) return logf(x)+LOG2f; // |x|>=2^16?
+ return (float)log(sqrt(((double)x+1.0)*((double)x-1.0))+(double)x);
+}
+
+float WRAPPER_FUNC(atanhf)(float x) {
+ check_nan_f1(x);
+ return fldexp(logf((1.0f+x)/(1.0f-x)),-1);
+}
+
+float WRAPPER_FUNC(exp2f)(float x) { check_nan_f1(x); return (float)exp((double)x*LOG2); }
+float WRAPPER_FUNC(log2f)(float x) { check_nan_f1(x); return logf(x)*LOG2Ef; }
+float WRAPPER_FUNC(exp10f)(float x) { check_nan_f1(x); return (float)exp((double)x*LOG10); }
+float WRAPPER_FUNC(log10f)(float x) { check_nan_f1(x); return logf(x)*LOG10Ef; }
+
+float WRAPPER_FUNC(expm1f)(float x) { check_nan_f1(x); return (float)(exp((double)x)-1); }
+float WRAPPER_FUNC(log1pf)(float x) { check_nan_f1(x); return (float)(log(1+(double)x)); }
+float WRAPPER_FUNC(fmaf)(float x,float y,float z) {
+ check_nan_f2(x,y);
+ check_nan_f1(z);
+ return (float)((double)x*(double)y+(double)z);
+} // has double rounding so not exact
+
+// general power, x>0
+static inline float fpow_1(float x,float y) {
+ return (float)exp(log((double)x)*(double)y); // using double-precision intermediates for better accuracy
+}
+
+static float fpow_int2(float x,int y) {
+ float u;
+ if(y==1) return x;
+ u=fpow_int2(x,y/2);
+ u*=u;
+ if(y&1) u*=x;
+ return u;
+}
+
+// for the case where x not zero or infinity, y small and not zero
+static inline float fpowint_1(float x,int y) {
+ if(y<0) x=1.0f/x,y=-y;
+ return fpow_int2(x,y);
+}
+
+// for the case where x not zero or infinity
+static float fpowint_0(float x,int y) {
+ int e;
+ if(fisneg(x)) {
+ if(fisoddint(y)) return fneg(fpowint_0(fneg(x),y));
+ else return fpowint_0(fneg(x),y);
+ }
+ if(fispo2(x)) {
+ e=fgetexp(x)-0x7f;
+ if(y>=256) y= 255; // avoid overflow
+ if(y<-256) y=-256;
+ y*=e;
+ return fldexp(1,y);
+ }
+ if(y==0) return 1;
+ if(y>=-32&&y<=32) return fpowint_1(x,y);
+ return fpow_1(x,y);
+}
+
+float WRAPPER_FUNC(powintf)(float x,int y) {
+ _Pragma("GCC diagnostic push")
+ _Pragma("GCC diagnostic ignored \"-Wfloat-equal\"")
+ if(x==1.0f||y==0) return 1;
+ if(x==0.0f) {
+ if(y>0) {
+ if(y&1) return x;
+ else return 0;
+ }
+ if((y&1)) return fcopysign(FPINF,x);
+ return FPINF;
+ }
+ _Pragma("GCC diagnostic pop")
+ check_nan_f1(x);
+ if(fispinf(x)) {
+ if(y<0) return 0;
+ else return FPINF;
+ }
+ if(fisminf(x)) {
+ if(y>0) {
+ if((y&1)) return FMINF;
+ else return FPINF;
+ }
+ if((y&1)) return MZERO;
+ else return PZERO;
+ }
+ return fpowint_0(x,y);
+}
+
+// for the case where y is guaranteed a finite integer, x not zero or infinity
+static float fpow_0(float x,float y) {
+ int e,p;
+ if(fisneg(x)) {
+ if(fisoddint(y)) return fneg(fpow_0(fneg(x),y));
+ else return fpow_0(fneg(x),y);
+ }
+ p=(int)y;
+ if(fispo2(x)) {
+ e=fgetexp(x)-0x7f;
+ if(p>=256) p= 255; // avoid overflow
+ if(p<-256) p=-256;
+ p*=e;
+ return fldexp(1,p);
+ }
+ if(p==0) return 1;
+ if(p>=-32&&p<=32) return fpowint_1(x,p);
+ return fpow_1(x,y);
+}
+
+float WRAPPER_FUNC(powf)(float x,float y) {
+ _Pragma("GCC diagnostic push")
+ _Pragma("GCC diagnostic ignored \"-Wfloat-equal\"")
+ if(x==1.0f||fiszero(y)) return 1;
+ check_nan_f2(x,y);
+ if(x==-1.0f&&fisinf(y)) return 1;
+ _Pragma("GCC diagnostic pop")
+ if(fiszero(x)) {
+ if(!fisneg(y)) {
+ if(fisoddint(y)) return x;
+ else return 0;
+ }
+ if(fisoddint(y)) return fcopysign(FPINF,x);
+ return FPINF;
+ }
+ if(fispinf(x)) {
+ if(fisneg(y)) return 0;
+ else return FPINF;
+ }
+ if(fisminf(x)) {
+ if(!fisneg(y)) {
+ if(fisoddint(y)) return FMINF;
+ else return FPINF;
+ }
+ if(fisoddint(y)) return MZERO;
+ else return PZERO;
+ }
+ if(fispinf(y)) {
+ if(fgetexp(x)<0x7f) return PZERO;
+ else return FPINF;
+ }
+ if(fisminf(y)) {
+ if(fgetexp(x)<0x7f) return FPINF;
+ else return PZERO;
+ }
+ if(fisint(y)) return fpow_0(x,y);
+ if(fisneg(x)) return FPINF;
+ return fpow_1(x,y);
+}
+
+float WRAPPER_FUNC(hypotf)(float x,float y) {
+ check_nan_f2(x,y);
+ int ex,ey;
+ ex=fgetexp(x); ey=fgetexp(y);
+ if(ex>=0x7f+50||ey>=0x7f+50) { // overflow, or nearly so
+ x=fldexp(x,-70),y=fldexp(y,-70);
+ return fldexp(sqrtf(x*x+y*y), 70);
+ }
+ else if(ex<=0x7f-50&&ey<=0x7f-50) { // underflow, or nearly so
+ x=fldexp(x, 70),y=fldexp(y, 70);
+ return fldexp(sqrtf(x*x+y*y),-70);
+ }
+ return sqrtf(x*x+y*y);
+}
+
+float WRAPPER_FUNC(cbrtf)(float x) {
+ check_nan_f1(x);
+ int e;
+ if(fisneg(x)) return fneg(cbrtf(fneg(x)));
+ if(fiszero(x)) return fcopysign(PZERO,x);
+ e=fgetexp(x)-0x7f;
+ e=(e*0x5555+0x8000)>>16; // ~e/3, rounded
+ x=fldexp(x,-e*3);
+ x=expf(logf(x)*ONETHIRDf);
+ return fldexp(x,e);
+}
+
+// reduces mx*2^e modulo my, returning bottom bits of quotient at *pquo
+// 2^23<=|mx|,my<2^24, e>=0; 0<=result<my
+static i32 frem_0(i32 mx,i32 my,int e,int*pquo) {
+ int quo=0,q,r=0,s;
+ if(e>0) {
+ r=0xffffffffU/(ui32)(my>>7); // reciprocal estimate Q16
+ }
+ while(e>0) {
+ s=e; if(s>12) s=12; // gain up to 12 bits on each iteration
+ q=(mx>>9)*r; // Q30
+ q=((q>>(29-s))+1)>>1; // Q(s), rounded
+ mx=(mx<<s)-my*q;
+ quo=(quo<<s)+q;
+ e-=s;
+ }
+ if(mx>=my) mx-=my,quo++; // when e==0 mx can be nearly as big as 2my
+ if(mx>=my) mx-=my,quo++;
+ if(mx<0) mx+=my,quo--;
+ if(mx<0) mx+=my,quo--;
+ if(pquo) *pquo=quo;
+ return mx;
+}
+
+float WRAPPER_FUNC(fmodf)(float x,float y) {
+ check_nan_f2(x,y);
+ ui32 ix=float2ui32(x),iy=float2ui32(y);
+ int sx,ex,ey;
+ i32 mx,my;
+ FUNPACKS(ix,sx,ex,mx);
+ FUNPACK(iy,ey,my);
+ if(ex==0xff) {
+ return fnan_or(FPINF);
+ }
+ if(ey==0) return FPINF;
+ if(ex==0) {
+ if(!fisneg(x)) return PZERO;
+ return MZERO;
+ }
+ if(ex<ey) return x; // |x|<|y|, including case x=±0
+ mx=frem_0(mx,my,ex-ey,0);
+ if(sx) mx=-mx;
+ return fix2float(mx,0x7f-ey+23);
+}
+
+float WRAPPER_FUNC(remquof)(float x,float y,int*quo) {
+ check_nan_f2(x,y);
+ ui32 ix=float2ui32(x),iy=float2ui32(y);
+ int sx,sy,ex,ey,q;
+ i32 mx,my;
+ FUNPACKS(ix,sx,ex,mx);
+ FUNPACKS(iy,sy,ey,my);
+ if(quo) *quo=0;
+ if(ex==0xff) return FPINF;
+ if(ey==0) return FPINF;
+ if(ex==0) return PZERO;
+ if(ey==0xff) return x;
+ if(ex<ey-1) return x; // |x|<|y|/2
+ if(ex==ey-1) {
+ if(mx<=my) return x; // |x|<=|y|/2, even quotient
+ // here |y|/2<|x|<|y|
+ if(!sx) { // x>|y|/2
+ mx-=my+my;
+ ey--;
+ q=1;
+ } else { // x<-|y|/2
+ mx=my+my-mx;
+ ey--;
+ q=-1;
+ }
+ }
+ else {
+ if(sx) mx=-mx;
+ mx=frem_0(mx,my,ex-ey,&q);
+ if(mx+mx>my || (mx+mx==my&&(q&1)) ) { // |x|>|y|/2, or equality and an odd quotient?
+ mx-=my;
+ q++;
+ }
+ }
+ if(sy) q=-q;
+ if(quo) *quo=q;
+ return fix2float(mx,0x7f-ey+23);
+}
+
+float WRAPPER_FUNC(dremf)(float x,float y) { check_nan_f2(x,y); return remquof(x,y,0); }
+
+float WRAPPER_FUNC(remainderf)(float x,float y) { check_nan_f2(x,y); return remquof(x,y,0); }
+
+_Pragma("GCC diagnostic pop") // conversion
\ No newline at end of file
diff --git a/src/rp2_common/pico_float/float_none.S b/src/rp2_common/pico_float/float_none.S
new file mode 100644
index 0000000..55baf29
--- /dev/null
+++ b/src/rp2_common/pico_float/float_none.S
@@ -0,0 +1,81 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "pico/asm_helper.S"
+#include "pico/bootrom/sf_table.h"
+
+.syntax unified
+.cpu cortex-m0plus
+.thumb
+
+wrapper_func __aeabi_fadd
+wrapper_func __aeabi_fdiv
+wrapper_func __aeabi_fmul
+wrapper_func __aeabi_frsub
+wrapper_func __aeabi_fsub
+wrapper_func __aeabi_cfcmpeq
+wrapper_func __aeabi_cfrcmple
+wrapper_func __aeabi_cfcmple
+wrapper_func __aeabi_fcmpeq
+wrapper_func __aeabi_fcmplt
+wrapper_func __aeabi_fcmple
+wrapper_func __aeabi_fcmpge
+wrapper_func __aeabi_fcmpgt
+wrapper_func __aeabi_fcmpun
+wrapper_func __aeabi_i2f
+wrapper_func __aeabi_l2f
+wrapper_func __aeabi_ui2f
+wrapper_func __aeabi_ul2f
+wrapper_func __aeabi_i2f
+wrapper_func __aeabi_f2iz
+wrapper_func __aeabi_f2lz
+wrapper_func __aeabi_f2uiz
+wrapper_func __aeabi_f2ulz
+wrapper_func __aeabi_f2d
+wrapper_func sqrtf
+wrapper_func cosf
+wrapper_func sinf
+wrapper_func tanf
+wrapper_func atan2f
+wrapper_func expf
+wrapper_func logf
+wrapper_func ldexpf
+wrapper_func copysignf
+wrapper_func truncf
+wrapper_func floorf
+wrapper_func ceilf
+wrapper_func roundf
+wrapper_func sincosf
+wrapper_func asinf
+wrapper_func acosf
+wrapper_func atanf
+wrapper_func sinhf
+wrapper_func coshf
+wrapper_func tanhf
+wrapper_func asinhf
+wrapper_func acoshf
+wrapper_func atanhf
+wrapper_func exp2f
+wrapper_func log2f
+wrapper_func exp10f
+wrapper_func log10f
+wrapper_func powf
+wrapper_func powintf
+wrapper_func hypotf
+wrapper_func cbrtf
+wrapper_func fmodf
+wrapper_func dremf
+wrapper_func remainderf
+wrapper_func remquof
+wrapper_func expm1f
+wrapper_func log1pf
+wrapper_func fmaf
+ push {lr} // keep stack trace sane
+ ldr r0, =str
+ bl panic
+
+str:
+ .asciz "float support is disabled"
\ No newline at end of file
diff --git a/src/rp2_common/pico_float/float_v1_rom_shim.S b/src/rp2_common/pico_float/float_v1_rom_shim.S
new file mode 100644
index 0000000..d7541a4
--- /dev/null
+++ b/src/rp2_common/pico_float/float_v1_rom_shim.S
@@ -0,0 +1,347 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "pico/asm_helper.S"
+
+#if PICO_FLOAT_SUPPORT_ROM_V1 && PICO_RP2040_B0_SUPPORTED
+.syntax unified
+.cpu cortex-m0plus
+.thumb
+
+#ifndef PICO_FLOAT_IN_RAM
+#define PICO_FLOAT_IN_RAM 0
+#endif
+
+.macro float_section name
+// todo separate flag for shims?
+#if PICO_FLOAT_IN_RAM
+.section RAM_SECTION_NAME(\name), "ax"
+#else
+.section SECTION_NAME(\name), "ax"
+#endif
+.endm
+
+float_section float_table_shim_on_use_helper
+regular_func float_table_shim_on_use_helper
+ push {r0-r2, lr}
+ mov r0, ip
+#ifndef NDEBUG
+ // sanity check to make sure we weren't called by non (shimmable_) table_tail_call macro
+ cmp r0, #0
+ bne 1f
+ bkpt #0
+#endif
+1:
+ ldrh r1, [r0]
+ lsrs r2, r1, #8
+ adds r0, #2
+ cmp r2, #0xdf
+ bne 1b
+ uxtb r1, r1 // r1 holds table offset
+ lsrs r2, r0, #2
+ bcc 1f
+ // unaligned
+ ldrh r2, [r0, #0]
+ ldrh r0, [r0, #2]
+ lsls r0, #16
+ orrs r0, r2
+ b 2f
+1:
+ ldr r0, [r0]
+2:
+ ldr r2, =sf_table
+ str r0, [r2, r1]
+ str r0, [sp, #12]
+ pop {r0-r2, pc}
+
+float_section 642float_shims
+
+@ convert uint64 to float, rounding
+regular_func uint642float_shim
+ movs r2,#0 @ fall through
+
+@ convert unsigned 64-bit fix to float, rounding; number of r0:r1 bits after point in r2
+regular_func ufix642float_shim
+ push {r4,r5,r14}
+ cmp r1,#0
+ bpl 3f @ positive? we can use signed code
+ lsls r5,r1,#31 @ contribution to sticky bits
+ orrs r5,r0
+ lsrs r0,r1,#1
+ subs r2,#1
+ b 4f
+
+@ convert int64 to float, rounding
+regular_func int642float_shim
+ movs r2,#0 @ fall through
+
+@ convert signed 64-bit fix to float, rounding; number of r0:r1 bits after point in r2
+regular_func fix642float_shim
+ push {r4,r5,r14}
+3:
+ movs r5,r0
+ orrs r5,r1
+ beq ret_pop45 @ zero? return +0
+ asrs r5,r1,#31 @ sign bits
+2:
+ asrs r4,r1,#24 @ try shifting 7 bits at a time
+ cmp r4,r5
+ bne 1f @ next shift will overflow?
+ lsls r1,#7
+ lsrs r4,r0,#25
+ orrs r1,r4
+ lsls r0,#7
+ adds r2,#7
+ b 2b
+1:
+ movs r5,r0
+ movs r0,r1
+4:
+ rsbs r2,#0
+ adds r2,#32+29
+
+ // bl packx
+ ldr r1, =0x29ef // packx
+ blx r1
+ret_pop45:
+ pop {r4,r5,r15}
+
+float_section fatan2_shim
+regular_func fatan2_shim
+ push {r4,r5,r14}
+
+ ldr r4, =0x29c1 // unpackx
+ mov ip, r4
+@ unpack arguments and shift one down to have common exponent
+ blx ip
+ mov r4,r0
+ mov r0,r1
+ mov r1,r4
+ mov r4,r2
+ mov r2,r3
+ mov r3,r4
+ blx ip
+ lsls r0,r0,#5 @ Q28
+ lsls r1,r1,#5 @ Q28
+ adds r4,r2,r3 @ this is -760 if both arguments are 0 and at least -380-126=-506 otherwise
+ asrs r4,#9
+ adds r4,#1
+ bmi 2f @ force y to 0 proper, so result will be zero
+ subs r4,r2,r3 @ calculate shift
+ bge 1f @ ex>=ey?
+ rsbs r4,#0 @ make shift positive
+ asrs r0,r4
+ cmp r4,#28
+ blo 3f
+ asrs r0,#31
+ b 3f
+1:
+ asrs r1,r4
+ cmp r4,#28
+ blo 3f
+2:
+@ here |x|>>|y| or both x and y are ±0
+ cmp r0,#0
+ bge 4f @ x positive, return signed 0
+ ldr r3, =0x2cfc @ &pi_q29, circular coefficients
+ ldr r0,[r3] @ x negative, return +/- pi
+ asrs r1,#31
+ eors r0,r1
+ b 7f
+4:
+ asrs r0,r1,#31
+ b 7f
+3:
+ movs r2,#0 @ initial angle
+ ldr r3, =0x2cfc @ &pi_q29, circular coefficients
+ cmp r0,#0 @ x negative
+ bge 5f
+ rsbs r0,#0 @ rotate to 1st/4th quadrants
+ rsbs r1,#0
+ ldr r2,[r3] @ pi Q29
+5:
+ movs r4,#1 @ m=1
+ ldr r5, =0x2b97 @ cordic_vec
+ blx r5 @ also produces magnitude (with scaling factor 1.646760119), which is discarded
+ mov r0,r2 @ result here is -pi/2..3pi/2 Q29
+@ asrs r2,#29
+@ subs r0,r2
+ ldr r3, =0x2cfc @ &pi_q29, circular coefficients
+ ldr r2,[r3] @ pi Q29
+ adds r4,r0,r2 @ attempt to fix -3pi/2..-pi case
+ bcs 6f @ -pi/2..0? leave result as is
+ subs r4,r0,r2 @ <pi? leave as is
+ bmi 6f
+ subs r0,r4,r2 @ >pi: take off 2pi
+6:
+ subs r0,#1 @ fiddle factor so atan2(0,1)==0
+7:
+ movs r2,#0 @ exponent for pack
+ ldr r3, =0x2b19
+ bx r3
+
+float_section float232_shims
+
+regular_func float2int_shim
+ movs r1,#0 @ fall through
+regular_func float2fix_shim
+ // check for -0 or -denormal upfront
+ asrs r2, r0, #23
+ adds r2, #128
+ adds r2, #128
+ beq 1f
+ // call original
+ ldr r2, =0x2acd
+ bx r2
+ 1:
+ movs r0, #0
+ bx lr
+
+float_section float264_shims
+
+regular_func float2int64_shim
+ movs r1,#0 @ and fall through
+regular_func float2fix64_shim
+ push {r14}
+ bl f2fix
+ b d2f64_a
+
+regular_func float2uint64_shim
+ movs r1,#0 @ and fall through
+regular_func float2ufix64_shim
+ asrs r3,r0,#23 @ negative? return 0
+ bmi ret_dzero
+@ and fall through
+
+@ convert float in r0 to signed fixed point in r0:r1:r3, r1 places after point, rounding towards -Inf
+@ result clamped so that r3 can only be 0 or -1
+@ trashes r12
+.thumb_func
+f2fix:
+ push {r4,r14}
+ mov r12,r1
+ asrs r3,r0,#31
+ lsls r0,#1
+ lsrs r2,r0,#24
+ beq 1f @ zero?
+ cmp r2,#0xff @ Inf?
+ beq 2f
+ subs r1,r2,#1
+ subs r2,#0x7f @ remove exponent bias
+ lsls r1,#24
+ subs r0,r1 @ insert implied 1
+ eors r0,r3
+ subs r0,r3 @ top two's complement
+ asrs r1,r0,#4 @ convert to double format
+ lsls r0,#28
+ ldr r4, =d2fix_a
+ bx r4
+1:
+ movs r0,#0
+ movs r1,r0
+ movs r3,r0
+ pop {r4,r15}
+2:
+ mvns r0,r3 @ return max/min value
+ mvns r1,r3
+ pop {r4,r15}
+
+ret_dzero:
+ movs r0,#0
+ movs r1,#0
+ bx r14
+
+float_section d2fix_a_float
+
+.weak d2fix_a // weak because it exists in float shims too
+.thumb_func
+d2fix_a:
+@ here
+@ r0:r1 two's complement mantissa
+@ r2 unbaised exponent
+@ r3 mantissa sign extension bits
+ add r2,r12 @ exponent plus offset for required binary point position
+ subs r2,#52 @ required shift
+ bmi 1f @ shift down?
+@ here a shift up by r2 places
+ cmp r2,#12 @ will clamp?
+ bge 2f
+ movs r4,r0
+ lsls r1,r2
+ lsls r0,r2
+ rsbs r2,#0
+ adds r2,#32 @ complementary shift
+ lsrs r4,r2
+ orrs r1,r4
+ pop {r4,r15}
+2:
+ mvns r0,r3
+ mvns r1,r3 @ overflow: clamp to extreme fixed-point values
+ pop {r4,r15}
+1:
+@ here a shift down by -r2 places
+ adds r2,#32
+ bmi 1f @ long shift?
+ mov r4,r1
+ lsls r4,r2
+ rsbs r2,#0
+ adds r2,#32 @ complementary shift
+ asrs r1,r2
+ lsrs r0,r2
+ orrs r0,r4
+ pop {r4,r15}
+1:
+@ here a long shift down
+ movs r0,r1
+ asrs r1,#31 @ shift down 32 places
+ adds r2,#32
+ bmi 1f @ very long shift?
+ rsbs r2,#0
+ adds r2,#32
+ asrs r0,r2
+ pop {r4,r15}
+1:
+ movs r0,r3 @ result very near zero: use sign extension bits
+ movs r1,r3
+ pop {r4,r15}
+d2f64_a:
+ asrs r2,r1,#31
+ cmp r2,r3
+ bne 1f @ sign extension bits fail to match sign of result?
+ pop {r15}
+1:
+ mvns r0,r3
+ movs r1,#1
+ lsls r1,#31
+ eors r1,r1,r0 @ generate extreme fixed-point values
+ pop {r15}
+
+float_section float2double_shim
+regular_func float2double_shim
+ lsrs r3,r0,#31 @ sign bit
+ lsls r3,#31
+ lsls r1,r0,#1
+ lsrs r2,r1,#24 @ exponent
+ beq 1f @ zero?
+ cmp r2,#0xff @ Inf?
+ beq 2f
+ lsrs r1,#4 @ exponent and top 20 bits of mantissa
+ ldr r2,=#(0x3ff-0x7f)<<20 @ difference in exponent offsets
+ adds r1,r2
+ orrs r1,r3
+ lsls r0,#29 @ bottom 3 bits of mantissa
+ bx r14
+1:
+ movs r1,r3 @ return signed zero
+3:
+ movs r0,#0
+ bx r14
+2:
+ ldr r1,=#0x7ff00000 @ return signed infinity
+ adds r1,r3
+ b 3b
+
+#endif
\ No newline at end of file
diff --git a/src/rp2_common/pico_float/include/pico/float.h b/src/rp2_common/pico_float/include/pico/float.h
new file mode 100644
index 0000000..8b06ea8
--- /dev/null
+++ b/src/rp2_common/pico_float/include/pico/float.h
@@ -0,0 +1,61 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PICO_FLOAT_H
+#define _PICO_FLOAT_H
+
+#include <math.h>
+#include <float.h>
+#include "pico/types.h"
+#include "pico/bootrom/sf_table.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** \file float.h
+* \defgroup pico_float pico_float
+*
+* Optimized single-precision floating point functions
+*
+* (Replacement) optimized implementations are provided of the following compiler built-ins
+* and math library functions:
+*
+* - __aeabi_fadd, __aeabi_fdiv, __aeabi_fmul, __aeabi_frsub, __aeabi_fsub, __aeabi_cfcmpeq, __aeabi_cfrcmple, __aeabi_cfcmple, __aeabi_fcmpeq, __aeabi_fcmplt, __aeabi_fcmple, __aeabi_fcmpge, __aeabi_fcmpgt, __aeabi_fcmpun, __aeabi_i2f, __aeabi_l2f, __aeabi_ui2f, __aeabi_ul2f, __aeabi_f2iz, __aeabi_f2lz, __aeabi_f2uiz, __aeabi_f2ulz, __aeabi_f2d, sqrtf, cosf, sinf, tanf, atan2f, expf, logf
+* - ldexpf, copysignf, truncf, floorf, ceilf, roundf, asinf, acosf, atanf, sinhf, coshf, tanhf, asinhf, acoshf, atanhf, exp2f, log2f, exp10f, log10f, powf, hypotf, cbrtf, fmodf, dremf, remainderf, remquof, expm1f, log1pf, fmaf
+* - powintf, sincosf (GNU extensions)
+*
+* The following additional optimized functions are also provided:
+*
+* - fix2float, ufix2float, fix642float, ufix642float, float2fix, float2ufix, float2fix64, float2ufix64, float2int, float2int64, float2int_z, float2int64_z
+*/
+
+float fix2float(int32_t m, int e);
+float ufix2float(uint32_t m, int e);
+float fix642float(int64_t m, int e);
+float ufix642float(uint64_t m, int e);
+
+// These methods round towards -Infinity.
+int32_t float2fix(float f, int e);
+uint32_t float2ufix(float f, int e);
+int64_t float2fix64(float f, int e);
+uint64_t float2ufix64(float f, int e);
+int32_t float2int(float f);
+int64_t float2int64(float f);
+
+// These methods round towards 0.
+int32_t float2int_z(float f);
+int64_t float2int64_z(float f);
+
+float exp10f(float x);
+void sincosf(float x, float *sinx, float *cosx);
+float powintf(float x, int y);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
\ No newline at end of file
diff --git a/src/rp2_common/pico_int64_ops/CMakeLists.txt b/src/rp2_common/pico_int64_ops/CMakeLists.txt
new file mode 100644
index 0000000..73e7d17
--- /dev/null
+++ b/src/rp2_common/pico_int64_ops/CMakeLists.txt
@@ -0,0 +1,35 @@
+if (NOT TARGET pico_int64_ops)
+
+ #shims for ROM functions for -lgcc functions (listed below)
+ pico_add_impl_library(pico_int64_ops)
+
+ # no custom implementation; falls thru to compiler
+ pico_add_impl_library(pico_int64_ops_compiler)
+
+ # add alias "default" which is just pico.
+ add_library(pico_int64_ops_default INTERFACE)
+ target_link_libraries(pico_int64_ops_default INTERFACE pico_int64_ops_pico)
+
+ set(PICO_DEFAULT_INT64_OPS_IMPL pico_int64_ops_default)
+
+ target_link_libraries(pico_int64_ops INTERFACE
+ $<IF:$<BOOL:$<TARGET_PROPERTY:PICO_TARGET_INT64_OPS_IMPL>>,$<TARGET_PROPERTY:PICO_TARGET_INT64_OPS_IMPL>,${PICO_DEFAULT_INT64_OPS_IMPL}>)
+
+ pico_add_impl_library(pico_int64_ops_pico)
+ target_include_directories(pico_int64_ops_pico INTERFACE ${CMAKE_CURRENT_LIST_DIR}/include)
+
+ target_sources(pico_int64_ops_pico INTERFACE
+ ${CMAKE_CURRENT_LIST_DIR}/pico_int64_ops_aeabi.S
+ )
+
+ pico_wrap_function(pico_int64_ops_pico __aeabi_lmul)
+
+ macro(pico_set_int64_ops_implementation TARGET IMPL)
+ get_target_property(target_type ${TARGET} TYPE)
+ if ("EXECUTABLE" STREQUAL "${target_type}")
+ set_target_properties(${TARGET} PROPERTIES PICO_TARGET_INT64_OPS_IMPL "pico_int64_ops_${IMPL}")
+ else()
+ message(FATAL_ERROR "int64_ops implementation must be set on executable not library")
+ endif()
+ endmacro()
+endif()
\ No newline at end of file
diff --git a/src/rp2_common/pico_int64_ops/include/pico/int64_ops.h b/src/rp2_common/pico_int64_ops/include/pico/int64_ops.h
new file mode 100644
index 0000000..db3213e
--- /dev/null
+++ b/src/rp2_common/pico_int64_ops/include/pico/int64_ops.h
@@ -0,0 +1,20 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PICO_INT64_H
+#define _PICO_INT64_H
+
+#include "pico/types.h"
+
+/** \file int64_ops.h
+ * \defgroup pico_int64_ops pico_int64_ops
+ *
+ * Optimized replacement implementations of the compiler built-in 64 bit multiplication
+ *
+ * This library does not provide any additional functions
+*/
+
+#endif
\ No newline at end of file
diff --git a/src/rp2_common/pico_int64_ops/pico_int64_ops_aeabi.S b/src/rp2_common/pico_int64_ops/pico_int64_ops_aeabi.S
new file mode 100644
index 0000000..0dbc67c
--- /dev/null
+++ b/src/rp2_common/pico_int64_ops/pico_int64_ops_aeabi.S
@@ -0,0 +1,44 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+.syntax unified
+.cpu cortex-m0plus
+.thumb
+
+#include "pico/asm_helper.S"
+
+#if PICO_INT64_OPS_IN_RAM
+.section RAM_SECTION_NAME(__aeabi_lmul)
+#else
+.section SECTION_NAME(__aeabi_lmul)
+#endif
+wrapper_func __aeabi_lmul
+ muls r1, r2
+ muls r3, r0
+ adds r1, r3
+ mov r12, r1
+ lsrs r1, r2, #16
+ uxth r3, r0
+ muls r3, r1
+ push {r4}
+ lsrs r4, r0, #16
+ muls r1, r4
+ uxth r2, r2
+ uxth r0, r0
+ muls r0, r2
+ muls r2, r4
+ lsls r4, r3, #16
+ lsrs r3, #16
+ adds r0, r4
+ pop {r4}
+ adcs r1, r3
+ lsls r3, r2, #16
+ lsrs r2, #16
+ adds r0, r3
+ adcs r1, r2
+ add r1, r12
+ bx lr
+
diff --git a/src/rp2_common/pico_malloc/CMakeLists.txt b/src/rp2_common/pico_malloc/CMakeLists.txt
new file mode 100644
index 0000000..deeb30f
--- /dev/null
+++ b/src/rp2_common/pico_malloc/CMakeLists.txt
@@ -0,0 +1,16 @@
+if (NOT TARGET pico_malloc)
+ #shims for ROM functions for -lgcc functions (listed below)
+ pico_add_impl_library(pico_malloc)
+
+ target_sources(pico_malloc INTERFACE
+ ${CMAKE_CURRENT_LIST_DIR}/pico_malloc.c
+ )
+
+ target_include_directories(pico_malloc INTERFACE ${CMAKE_CURRENT_LIST_DIR}/include)
+
+ pico_wrap_function(pico_malloc malloc)
+ pico_wrap_function(pico_malloc calloc)
+ pico_wrap_function(pico_malloc free)
+
+ target_link_libraries(pico_malloc INTERFACE pico_sync)
+endif()
diff --git a/src/rp2_common/pico_malloc/include/pico/malloc.h b/src/rp2_common/pico_malloc/include/pico/malloc.h
new file mode 100644
index 0000000..b4ae2b6
--- /dev/null
+++ b/src/rp2_common/pico_malloc/include/pico/malloc.h
@@ -0,0 +1,38 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PICO_MALLOC_H
+#define _PICO_MALLOC_H
+
+/** \file malloc.h
+* \defgroup pico_malloc pico_malloc
+*
+* Multi-core safety for malloc, calloc and free
+*
+* This library does not provide any additional functions
+*/
+
+// PICO_CONFIG: PICO_USE_MALLOC_MUTEX, Whether to protect malloc etc with a mutex, type=bool, default=1 with pico_multicore, 0 otherwise, group=pico_malloc
+#if LIB_PICO_MULTICORE && !defined(PICO_USE_MALLOC_MUTEX)
+#define PICO_USE_MALLOC_MUTEX 1
+#endif
+
+// PICO_CONFIG: PICO_MALLOC_PANIC, Enable/disable panic when an allocation failure occurs, type=bool, default=1, group=pico_malloc
+#ifndef PICO_MALLOC_PANIC
+#define PICO_MALLOC_PANIC 1
+#endif
+
+// PICO_CONFIG: PICO_DEBUG_MALLOC, Enable/disable debug printf from malloc, type=bool, default=0, group=pico_malloc
+#ifndef PICO_DEBUG_MALLOC
+#define PICO_DEBUG_MALLOC 0
+#endif
+
+// PICO_CONFIG: PICO_DEBUG_MALLOC_LOW_WATER, Define the lower bound for allocation addresses to be printed by PICO_DEBUG_MALLOC, min=0, default=0, group=pico_malloc
+#ifndef PICO_DEBUG_MALLOC_LOW_WATER
+#define PICO_DEBUG_MALLOC_LOW_WATER 0
+#endif
+
+#endif
\ No newline at end of file
diff --git a/src/rp2_common/pico_malloc/pico_malloc.c b/src/rp2_common/pico_malloc/pico_malloc.c
new file mode 100644
index 0000000..4928a8e
--- /dev/null
+++ b/src/rp2_common/pico_malloc/pico_malloc.c
@@ -0,0 +1,73 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdlib.h>
+#include <stdio.h>
+#include "pico.h"
+#include "pico/malloc.h"
+
+#if PICO_USE_MALLOC_MUTEX
+#include "pico/mutex.h"
+auto_init_mutex(malloc_mutex);
+#endif
+
+extern void *__real_malloc(size_t size);
+extern void *__real_calloc(size_t count, size_t size);
+extern void __real_free(void *mem);
+
+extern char __StackLimit; /* Set by linker. */
+
+static inline void check_alloc(__unused void *mem, __unused uint size) {
+#if PICO_MALLOC_PANIC
+ if (!mem || (((char *)mem) + size) > &__StackLimit) {
+ panic("Out of memory");
+ }
+#endif
+}
+
+void *__wrap_malloc(size_t size) {
+#if PICO_USE_MALLOC_MUTEX
+ mutex_enter_blocking(&malloc_mutex);
+#endif
+ void *rc = __real_malloc(size);
+#if PICO_USE_MALLOC_MUTEX
+ mutex_exit(&malloc_mutex);
+#endif
+#if PICO_DEBUG_MALLOC
+ if (!rc || ((uint8_t *)rc) + size > (uint8_t*)PICO_DEBUG_MALLOC_LOW_WATER) {
+ printf("malloc %d %p->%p\n", (uint) size, rc, ((uint8_t *) rc) + size);
+ }
+#endif
+ check_alloc(rc, size);
+ return rc;
+}
+
+void *__wrap_calloc(size_t count, size_t size) {
+#if PICO_USE_MALLOC_MUTEX
+ mutex_enter_blocking(&malloc_mutex);
+#endif
+ void *rc = __real_calloc(count, size);
+#if PICO_USE_MALLOC_MUTEX
+ mutex_exit(&malloc_mutex);
+#endif
+#if PICO_DEBUG_MALLOC
+ if (!rc || ((uint8_t *)rc) + size > (uint8_t*)PICO_DEBUG_MALLOC_LOW_WATER) {
+ printf("calloc %d %p->%p\n", (uint) (count * size), rc, ((uint8_t *) rc) + size);
+ }
+#endif
+ check_alloc(rc, size);
+ return rc;
+}
+
+void __wrap_free(void *mem) {
+#if PICO_USE_MALLOC_MUTEX
+ mutex_enter_blocking(&malloc_mutex);
+#endif
+ __real_free(mem);
+#if PICO_USE_MALLOC_MUTEX
+ mutex_exit(&malloc_mutex);
+#endif
+}
diff --git a/src/rp2_common/pico_mem_ops/CMakeLists.txt b/src/rp2_common/pico_mem_ops/CMakeLists.txt
new file mode 100644
index 0000000..997bb20
--- /dev/null
+++ b/src/rp2_common/pico_mem_ops/CMakeLists.txt
@@ -0,0 +1,42 @@
+if (NOT TARGET pico_mem_ops)
+ #shims for ROM functions for -lgcc functions (listed below)
+ pico_add_impl_library(pico_mem_ops)
+
+ # no custom implementation; falls thru to compiler
+ pico_add_impl_library(pico_mem_ops_compiler)
+
+ # add alias "default" which is just pico.
+ add_library(pico_mem_ops_default INTERFACE)
+ target_link_libraries(pico_mem_ops_default INTERFACE pico_mem_ops_pico)
+
+ set(PICO_DEFAULT_MEM_OPS_IMPL pico_mem_ops_default)
+
+ pico_add_impl_library(pico_mem_ops_pico)
+ target_link_libraries(pico_mem_ops INTERFACE
+ $<IF:$<BOOL:$<TARGET_PROPERTY:PICO_TARGET_MEM_OPS_IMPL>>,$<TARGET_PROPERTY:PICO_TARGET_MEM_OPS_IMPL>,${PICO_DEFAULT_MEM_OPS_IMPL}>)
+
+ target_sources(pico_mem_ops_pico INTERFACE
+ ${CMAKE_CURRENT_LIST_DIR}/mem_ops_aeabi.S
+ )
+
+
+ target_link_libraries(pico_mem_ops INTERFACE pico_bootrom)
+
+ pico_wrap_function(pico_mem_ops_pico memcpy)
+ pico_wrap_function(pico_mem_ops_pico memset)
+ pico_wrap_function(pico_mem_ops_pico __aeabi_memcpy)
+ pico_wrap_function(pico_mem_ops_pico __aeabi_memset)
+ pico_wrap_function(pico_mem_ops_pico __aeabi_memcpy4)
+ pico_wrap_function(pico_mem_ops_pico __aeabi_memset4)
+ pico_wrap_function(pico_mem_ops_pico __aeabi_memcpy8)
+ pico_wrap_function(pico_mem_ops_pico __aeabi_memset8)
+
+ macro(pico_set_mem_ops_implementation TARGET IMPL)
+ get_target_property(target_type ${TARGET} TYPE)
+ if ("EXECUTABLE" STREQUAL "${target_type}")
+ set_target_properties(${TARGET} PROPERTIES PICO_TARGET_MEM_OPS_IMPL "pico_mem_ops_${IMPL}")
+ else()
+ message(FATAL_ERROR "mem_ops implementation must be set on executable not library")
+ endif()
+ endmacro()
+endif()
diff --git a/src/rp2_common/pico_mem_ops/include/pico/mem_ops.h b/src/rp2_common/pico_mem_ops/include/pico/mem_ops.h
new file mode 100644
index 0000000..0c224fb
--- /dev/null
+++ b/src/rp2_common/pico_mem_ops/include/pico/mem_ops.h
@@ -0,0 +1,22 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PICO_MEMORY_H
+#define _PICO_MEMORY_H
+
+#include "pico/types.h"
+
+/** \file mem_ops.h
+ * \defgroup pico_mem_ops pico_mem_ops
+ *
+ * Provides optimized replacement implementations of the compiler built-in memcpy, memset and related functions:
+ *
+ * - memset, memcpy
+ * - __aeabi_memset, __aeabi_memset4, __aeabi_memset8, __aeabi_memcpy, __aeabi_memcpy4, __aeabi_memcpy8
+ *
+ * This library does not provide any additional functions
+ */
+#endif
\ No newline at end of file
diff --git a/src/rp2_common/pico_mem_ops/mem_ops.c b/src/rp2_common/pico_mem_ops/mem_ops.c
new file mode 100644
index 0000000..4047d23
--- /dev/null
+++ b/src/rp2_common/pico_mem_ops/mem_ops.c
@@ -0,0 +1,7 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "pico/mem_ops.h"
diff --git a/src/rp2_common/pico_mem_ops/mem_ops_aeabi.S b/src/rp2_common/pico_mem_ops/mem_ops_aeabi.S
new file mode 100644
index 0000000..8540905
--- /dev/null
+++ b/src/rp2_common/pico_mem_ops/mem_ops_aeabi.S
@@ -0,0 +1,99 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+.syntax unified
+.cpu cortex-m0plus
+.thumb
+
+#include "pico/asm_helper.S"
+#include "pico/bootrom.h"
+
+__pre_init __aeabi_mem_init, 00001
+
+.macro mem_section name
+#if PICO_MEM_IN_RAM
+.section RAM_SECTION_NAME(\name), "ax"
+#else
+.section SECTION_NAME(\name), "ax"
+#endif
+.endm
+
+.equ MEMSET, 0
+.equ MEMCPY, 4
+.equ MEMSET4, 8
+.equ MEMCPY4, 12
+.equ MEM_FUNC_COUNT, 4
+
+# NOTE: All code sections are placed in RAM (at the expense of some veneer cost for calls from flash) because
+# otherwise code using basic c division operators will require XIP flash access.
+
+.section .data.aeabi_mem_funcs
+.global aeabi_mem_funcs, aeabi_mem_funcs_end
+
+.align 2
+aeabi_mem_funcs:
+ .word ROM_FUNC_MEMSET
+ .word ROM_FUNC_MEMCPY
+ .word ROM_FUNC_MEMSET4
+ .word ROM_FUNC_MEMCPY44
+aeabi_mem_funcs_end:
+
+.section .text
+regular_func __aeabi_mem_init
+ ldr r0, =aeabi_mem_funcs
+ movs r1, #MEM_FUNC_COUNT
+ ldr r3, =rom_funcs_lookup
+ bx r3
+
+# lump them both together because likely both to be used, in which case doing so saves 1 word
+# and it only costs 1 word if not
+
+// Note from Run-time ABI for the ARM architecture 4.3.4:
+// If there is an attached device with efficient memory copying or clearing operations
+// (such as a DMA engine), its device supplement specifies whether it may be used in
+// implementations of these functions and what effect such use has on the device’s state.
+
+mem_section aeabi_memset_memcpy
+
+wrapper_func __aeabi_memset
+ // 2nd/3rd args are reversed
+ eors r2, r1
+ eors r1, r2
+ eors r2, r1
+ ldr r3, =aeabi_mem_funcs
+ ldr r3, [r3, #MEMSET]
+ bx r3
+
+wrapper_func __aeabi_memset4
+wrapper_func __aeabi_memset8
+ // 2nd/3rd args are reversed
+ eors r2, r1
+ eors r1, r2
+ eors r2, r1
+ ldr r3, =aeabi_mem_funcs
+ ldr r3, [r3, #MEMSET4]
+ bx r3
+
+wrapper_func __aeabi_memcpy4
+wrapper_func __aeabi_memcpy8
+ ldr r3, =aeabi_mem_funcs
+ ldr r3, [r3, #MEMCPY4]
+ bx r3
+
+mem_section memset
+
+wrapper_func memset
+ ldr r3, =aeabi_mem_funcs
+ ldr r3, [r3, #MEMSET]
+ bx r3
+
+mem_section memcpy
+wrapper_func __aeabi_memcpy
+wrapper_func memcpy
+ ldr r3, =aeabi_mem_funcs
+ ldr r3, [r3, #MEMCPY]
+ bx r3
+
diff --git a/src/rp2_common/pico_multicore/CMakeLists.txt b/src/rp2_common/pico_multicore/CMakeLists.txt
new file mode 100644
index 0000000..2401061
--- /dev/null
+++ b/src/rp2_common/pico_multicore/CMakeLists.txt
@@ -0,0 +1,13 @@
+if (NOT TARGET pico_multicore)
+ pico_add_impl_library(pico_multicore)
+
+ target_sources(pico_multicore INTERFACE
+ ${CMAKE_CURRENT_LIST_DIR}/multicore.c)
+
+ target_include_directories(pico_multicore INTERFACE ${CMAKE_CURRENT_LIST_DIR}/include)
+
+ target_link_libraries(pico_multicore INTERFACE pico_sync hardware_irq)
+endif()
+
+
+
diff --git a/src/rp2_common/pico_multicore/include/pico/multicore.h b/src/rp2_common/pico_multicore/include/pico/multicore.h
new file mode 100644
index 0000000..6f12f41
--- /dev/null
+++ b/src/rp2_common/pico_multicore/include/pico/multicore.h
@@ -0,0 +1,310 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PICO_MULTICORE_H
+#define _PICO_MULTICORE_H
+
+#include "pico/types.h"
+#include "pico/sync.h"
+#include "hardware/structs/sio.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** \file multicore.h
+ * \defgroup pico_multicore pico_multicore
+ * Adds support for running code on the second processor core (core 1)
+ *
+ * \subsection multicore_example Example
+ * \addtogroup pico_multicore
+ * \include multicore.c
+*/
+
+// PICO_CONFIG: PICO_CORE1_STACK_SIZE, Stack size for core 1, min=0x100, max=0x10000, default=PICO_STACK_SIZE (0x800), group=pico_multicore
+#ifndef PICO_CORE1_STACK_SIZE
+#ifdef PICO_STACK_SIZE
+#define PICO_CORE1_STACK_SIZE PICO_STACK_SIZE
+#else
+#define PICO_CORE1_STACK_SIZE 0x800
+#endif
+#endif
+
+/*! \brief Reset core 1
+ * \ingroup pico_multicore
+ *
+ * This function can be used to reset core 1 into its initial state (ready for launching code against via \ref multicore_launch_core1 and similar methods)
+ *
+ * \note this function should only be called from core 0
+ */
+void multicore_reset_core1(void);
+
+/*! \brief Run code on core 1
+ * \ingroup pico_multicore
+ *
+ * Wake up (a previously reset) core 1 and enter the given function on core 1 using the default core 1 stack (below core 0 stack).
+ *
+ * core 1 must previously have been reset either as a result of a system reset or by calling \ref multicore_reset_core1
+ *
+ * core 1 will use the same vector table as core 0
+ *
+ * \param entry Function entry point
+ * \see multicore_reset_core1
+ */
+void multicore_launch_core1(void (*entry)(void));
+
+/*! \brief Launch code on core 1 with stack
+ * \ingroup pico_multicore
+ *
+ * Wake up (a previously reset) core 1 and enter the given function on core 1 using the passed stack for core 1
+ *
+ * core 1 must previously have been reset either as a result of a system reset or by calling \ref multicore_reset_core1
+ *
+ * core 1 will use the same vector table as core 0
+ *
+ * \param entry Function entry point
+ * \param stack_bottom The bottom (lowest address) of the stack
+ * \param stack_size_bytes The size of the stack in bytes (must be a multiple of 4)
+ * \see multicore_reset_core1
+ */
+void multicore_launch_core1_with_stack(void (*entry)(void), uint32_t *stack_bottom, size_t stack_size_bytes);
+
+/*! \brief Launch code on core 1 with no stack protection
+ * \ingroup pico_multicore
+ *
+ * Wake up (a previously reset) core 1 and start it executing with a specific entry point, stack pointer
+ * and vector table.
+ *
+ * This is a low level function that does not provide a stack guard even if USE_STACK_GUARDS is defined
+ *
+ * core 1 must previously have been reset either as a result of a system reset or by calling \ref multicore_reset_core1
+ *
+ * \param entry Function entry point
+ * \param sp Pointer to the top of the core 1 stack
+ * \param vector_table address of the vector table to use for core 1
+ * \see multicore_reset_core1
+ */
+void multicore_launch_core1_raw(void (*entry)(void), uint32_t *sp, uint32_t vector_table);
+
+/*!
+ * \defgroup multicore_fifo fifo
+ * \ingroup pico_multicore
+ * \brief Functions for the inter-core FIFOs
+ *
+ * The RP2040 contains two FIFOs for passing data, messages or ordered events between the two cores. Each FIFO is 32 bits
+ * wide, and 8 entries deep. One of the FIFOs can only be written by core 0, and read by core 1. The other can only be written
+ * by core 1, and read by core 0.
+ *
+ * \note The inter-core FIFOs are a very precious resource and are frequently used for SDK functionality (e.g. during
+ * core 1 launch or by the \ref multicore_lockout functions). Additionally they are often required for the exclusive use
+ * of an RTOS (e.g. FreeRTOS SMP). For these reasons it is suggested that you do not use the FIFO for your own purposes
+ * unless none of the above concerns apply; the majority of cases for transferring data between cores can be eqaully
+ * well handled by using a \ref queue
+ */
+
+/*! \brief Check the read FIFO to see if there is data available (sent by the other core)
+ * \ingroup multicore_fifo
+ *
+ * See the note in the \ref multicore_fifo section for considerations regarding use of the inter-core FIFOs
+ *
+ * \return true if the FIFO has data in it, false otherwise
+ */
+static inline bool multicore_fifo_rvalid(void) {
+ return !!(sio_hw->fifo_st & SIO_FIFO_ST_VLD_BITS);
+}
+
+/*! \brief Check the write FIFO to see if it has space for more data
+ * \ingroup multicore_fifo
+ *
+ * See the note in the \ref multicore_fifo section for considerations regarding use of the inter-core FIFOs
+ *
+ * @return true if the FIFO has room for more data, false otherwise
+ */
+static inline bool multicore_fifo_wready(void) {
+ return !!(sio_hw->fifo_st & SIO_FIFO_ST_RDY_BITS);
+}
+
+/*! \brief Push data on to the write FIFO (data to the other core).
+ * \ingroup multicore_fifo
+ *
+ * This function will block until there is space for the data to be sent.
+ * Use multicore_fifo_wready() to check if it is possible to write to the
+ * FIFO if you don't want to block.
+ *
+ * See the note in the \ref multicore_fifo section for considerations regarding use of the inter-core FIFOs
+ *
+ * \param data A 32 bit value to push on to the FIFO
+ */
+void multicore_fifo_push_blocking(uint32_t data);
+
+/*! \brief Push data on to the write FIFO (data to the other core) with timeout.
+ * \ingroup multicore_fifo
+ *
+ * This function will block until there is space for the data to be sent
+ * or the timeout is reached
+ *
+ * \param data A 32 bit value to push on to the FIFO
+ * \param timeout_us the timeout in microseconds
+ * \return true if the data was pushed, false if the timeout occurred before data could be pushed
+ */
+bool multicore_fifo_push_timeout_us(uint32_t data, uint64_t timeout_us);
+
+/*! \brief Pop data from the read FIFO (data from the other core).
+ * \ingroup multicore_fifo
+ *
+ * This function will block until there is data ready to be read
+ * Use multicore_fifo_rvalid() to check if data is ready to be read if you don't
+ * want to block.
+ *
+ * See the note in the \ref multicore_fifo section for considerations regarding use of the inter-core FIFOs
+ *
+ * \return 32 bit data from the read FIFO.
+ */
+uint32_t multicore_fifo_pop_blocking(void);
+
+/*! \brief Pop data from the read FIFO (data from the other core) with timeout.
+ * \ingroup multicore_fifo
+ *
+ * This function will block until there is data ready to be read or the timeout is reached
+ *
+ * See the note in the \ref multicore_fifo section for considerations regarding use of the inter-core FIFOs
+ *
+ * \param timeout_us the timeout in microseconds
+ * \param out the location to store the popped data if available
+ * \return true if the data was popped and a value copied into `out`, false if the timeout occurred before data could be popped
+ */
+bool multicore_fifo_pop_timeout_us(uint64_t timeout_us, uint32_t *out);
+
+/*! \brief Discard any data in the read FIFO
+ * \ingroup multicore_fifo
+ *
+ * See the note in the \ref multicore_fifo section for considerations regarding use of the inter-core FIFOs
+ */
+static inline void multicore_fifo_drain(void) {
+ while (multicore_fifo_rvalid())
+ (void) sio_hw->fifo_rd;
+}
+
+/*! \brief Clear FIFO interrupt
+ * \ingroup multicore_fifo
+ *
+ * Note that this only clears an interrupt that was caused by the ROE or WOF flags.
+ * To clear the VLD flag you need to use one of the 'pop' or 'drain' functions.
+ *
+ * See the note in the \ref multicore_fifo section for considerations regarding use of the inter-core FIFOs
+ *
+ * \see multicore_fifo_get_status
+*/
+static inline void multicore_fifo_clear_irq(void) {
+ // Write any value to clear the error flags
+ sio_hw->fifo_st = 0xff;
+}
+
+/*! \brief Get FIFO statuses
+ * \ingroup multicore_fifo
+ *
+ * \return The statuses as a bitfield
+ *
+ * Bit | Description
+ * ----|------------
+ * 3 | Sticky flag indicating the RX FIFO was read when empty (ROE). This read was ignored by the FIFO.
+ * 2 | Sticky flag indicating the TX FIFO was written when full (WOF). This write was ignored by the FIFO.
+ * 1 | Value is 1 if this core’s TX FIFO is not full (i.e. if FIFO_WR is ready for more data)
+ * 0 | Value is 1 if this core’s RX FIFO is not empty (i.e. if FIFO_RD is valid)
+ *
+ * See the note in the \ref multicore_fifo section for considerations regarding use of the inter-core FIFOs
+ *
+*/
+static inline uint32_t multicore_fifo_get_status(void) {
+ return sio_hw->fifo_st;
+}
+
+/*!
+ * \defgroup multicore_lockout lockout
+ * \ingroup pico_multicore
+ * \brief Functions to enable one core to force the other core to pause execution in a known state.
+ *
+ * Sometimes it is useful to enter a critical section on both cores at once. On a single
+ * core system a critical section can trivially be entered by disabling interrupts, however on a multi-core
+ * system that is not sufficient, and unless the other core is polling in some way, then it will need to be interrupted
+ * in order to cooperatively enter a blocked state.
+ *
+ * These "lockout" functions use the inter core FIFOs to cause an interrupt on one core from the other, and manage
+ * waiting for the other core to enter the "locked out" state.
+ *
+ * The usage is that the "victim" core ... i.e the core that can be "locked out" by the other core calls
+ * \ref multicore_lockout_victim_init to hook the FIFO interrupt. Note that either or both cores may do this.
+ *
+ * \note When "locked out" the victim core is paused (it is actually executing a tight loop with code in RAM) and has interrupts disabled.
+ * This makes the lockout functions suitable for use by code that wants to write to flash (at which point no code may be executing
+ * from flash)
+ *
+ * The core which wishes to lockout the other core calls \ref multicore_lockout_start_blocking or
+ * \ref multicore_lockout_start_timeout_us to interrupt the other "victim" core and wait for it to be in a
+ * "locked out" state. Once the lockout is no longer needed it calls \ref multicore_lockout_end_blocking or
+ * \ref multicore_lockout_end_timeout_us to release the lockout and wait for confirmation.
+ *
+ * \note Because multicore lockout uses the intercore FIFOs, the FIFOs <b>cannot</b> be used for any other purpose
+ */
+
+/*! \brief Initialize the current core such that it can be a "victim" of lockout (i.e. forced to pause in a known state by the other core)
+ * \ingroup multicore_lockout
+ *
+ * This code hooks the intercore FIFO IRQ, and the FIFO may not be used for any other purpose after this.
+ */
+void multicore_lockout_victim_init(void);
+
+/*! \brief Request the other core to pause in a known state and wait for it to do so
+ * \ingroup multicore_lockout
+ *
+ * The other (victim) core must have previously executed \ref multicore_lockout_victim_init()
+ *
+ * \note multicore_lockout_start_ functions are not nestable, and must be paired with a call to a corresponding
+ * \ref multicore_lockout_end_blocking
+ */
+void multicore_lockout_start_blocking(void);
+
+/*! \brief Request the other core to pause in a known state and wait up to a time limit for it to do so
+ * \ingroup multicore_lockout
+ *
+ * The other core must have previously executed \ref multicore_lockout_victim_init()
+ *
+ * \note multicore_lockout_start_ functions are not nestable, and must be paired with a call to a corresponding
+ * \ref multicore_lockout_end_blocking
+ *
+ * \param timeout_us the timeout in microseconds
+ * \return true if the other core entered the locked out state within the timeout, false otherwise
+ */
+bool multicore_lockout_start_timeout_us(uint64_t timeout_us);
+
+/*! \brief Release the other core from a locked out state amd wait for it to acknowledge
+ * \ingroup multicore_lockout
+ *
+ * \note The other core must previously have been "locked out" by calling a `multicore_lockout_start_` function
+ * from this core
+ */
+void multicore_lockout_end_blocking(void);
+
+/*! \brief Release the other core from a locked out state amd wait up to a time limit for it to acknowledge
+ * \ingroup multicore_lockout
+ *
+ * The other core must previously have been "locked out" by calling a `multicore_lockout_start_` function
+ * from this core
+ *
+ * \note be very careful using small timeout values, as a timeout here will leave the "lockout" functionality
+ * in a bad state. It is probably preferable to use \ref multicore_lockout_end_blocking anyway as if you have
+ * already waited for the victim core to enter the lockout state, then the victim core will be ready to exit
+ * the lockout state very quickly.
+ *
+ * \param timeout_us the timeout in microseconds
+ * \return true if the other core successfully exited locked out state within the timeout, false otherwise
+ */
+bool multicore_lockout_end_timeout_us(uint64_t timeout_us);
+
+#ifdef __cplusplus
+}
+#endif
+#endif
diff --git a/src/rp2_common/pico_multicore/multicore.c b/src/rp2_common/pico_multicore/multicore.c
new file mode 100644
index 0000000..121b4fb
--- /dev/null
+++ b/src/rp2_common/pico_multicore/multicore.c
@@ -0,0 +1,269 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "pico/multicore.h"
+#include "hardware/sync.h"
+#include "hardware/irq.h"
+#include "hardware/structs/scb.h"
+#include "hardware/structs/sio.h"
+#include "hardware/regs/psm.h"
+#include "hardware/claim.h"
+#if PICO_USE_STACK_GUARDS
+#include "pico/runtime.h"
+#endif
+
+static inline void multicore_fifo_push_blocking_inline(uint32_t data) {
+ // We wait for the fifo to have some space
+ while (!multicore_fifo_wready())
+ tight_loop_contents();
+
+ sio_hw->fifo_wr = data;
+
+ // Fire off an event to the other core
+ __sev();
+}
+
+void multicore_fifo_push_blocking(uint32_t data) {
+ multicore_fifo_push_blocking_inline(data);
+}
+
+bool multicore_fifo_push_timeout_us(uint32_t data, uint64_t timeout_us) {
+ absolute_time_t end_time = make_timeout_time_us(timeout_us);
+ // We wait for the fifo to have some space
+ while (!multicore_fifo_wready()) {
+ tight_loop_contents();
+ if (time_reached(end_time)) return false;
+ }
+
+ sio_hw->fifo_wr = data;
+
+ // Fire off an event to the other core
+ __sev();
+ return true;
+}
+
+static inline uint32_t multicore_fifo_pop_blocking_inline(void) {
+ // If nothing there yet, we wait for an event first,
+ // to try and avoid too much busy waiting
+ while (!multicore_fifo_rvalid())
+ __wfe();
+
+ return sio_hw->fifo_rd;
+}
+
+uint32_t multicore_fifo_pop_blocking() {
+ return multicore_fifo_pop_blocking_inline();
+}
+
+bool multicore_fifo_pop_timeout_us(uint64_t timeout_us, uint32_t *out) {
+ absolute_time_t end_time = make_timeout_time_us(timeout_us);
+ // If nothing there yet, we wait for an event first,
+ // to try and avoid too much busy waiting
+ while (!multicore_fifo_rvalid()) {
+ __wfe();
+ if (time_reached(end_time)) return false;
+ }
+
+ *out = sio_hw->fifo_rd;
+ return true;
+}
+
+// Default stack for core1 ... if multicore_launch_core1 is not included then .stack1 section will be garbage collected
+static uint32_t __attribute__((section(".stack1"))) core1_stack[PICO_CORE1_STACK_SIZE / sizeof(uint32_t)];
+
+static void __attribute__ ((naked)) core1_trampoline(void) {
+ __asm("pop {r0, r1, pc}");
+}
+
+int core1_wrapper(int (*entry)(void), void *stack_base) {
+#if PICO_USE_STACK_GUARDS
+ // install core1 stack guard
+ runtime_install_stack_guard(stack_base);
+#else
+ __unused void *ignore = stack_base;
+#endif
+ irq_init_priorities();
+ return (*entry)();
+}
+
+void multicore_reset_core1() {
+ // Use atomic aliases just in case core 1 is also manipulating some PSM state
+ io_rw_32 *power_off = (io_rw_32 *) (PSM_BASE + PSM_FRCE_OFF_OFFSET);
+ io_rw_32 *power_off_set = hw_set_alias(power_off);
+ io_rw_32 *power_off_clr = hw_clear_alias(power_off);
+
+ // Hard-reset core 1.
+ // Reading back confirms the core 1 reset is in the correct state, but also
+ // forces APB IO bridges to fence on any internal store buffering
+ *power_off_set = PSM_FRCE_OFF_PROC1_BITS;
+ while (!(*power_off & PSM_FRCE_OFF_PROC1_BITS)) tight_loop_contents();
+
+ // Bring core 1 back out of reset. It will drain its own mailbox FIFO, then push
+ // a 0 to our mailbox to tell us it has done this.
+ *power_off_clr = PSM_FRCE_OFF_PROC1_BITS;
+}
+
+void multicore_launch_core1_with_stack(void (*entry)(void), uint32_t *stack_bottom, size_t stack_size_bytes) {
+ assert(!(stack_size_bytes & 3u));
+ uint32_t *stack_ptr = stack_bottom + stack_size_bytes / sizeof(uint32_t);
+ // push 2 values onto top of stack for core1_trampoline
+ stack_ptr -= 3;
+ stack_ptr[0] = (uintptr_t) entry;
+ stack_ptr[1] = (uintptr_t) stack_bottom;
+ stack_ptr[2] = (uintptr_t) core1_wrapper;
+ multicore_launch_core1_raw(core1_trampoline, stack_ptr, scb_hw->vtor);
+}
+
+void multicore_launch_core1(void (*entry)(void)) {
+ extern uint32_t __StackOneBottom;
+ uint32_t *stack_limit = (uint32_t *) &__StackOneBottom;
+ // hack to reference core1_stack although that pointer is wrong.... core1_stack should always be <= stack_limit, if not boom!
+ uint32_t *stack = core1_stack <= stack_limit ? stack_limit : (uint32_t *) -1;
+ multicore_launch_core1_with_stack(entry, stack, sizeof(core1_stack));
+}
+
+void multicore_launch_core1_raw(void (*entry)(void), uint32_t *sp, uint32_t vector_table) {
+ // Allow for the fact that the caller may have already enabled the FIFO IRQ for their
+ // own purposes (expecting FIFO content after core 1 is launched). We must disable
+ // the IRQ during the handshake, then restore afterwards.
+ bool enabled = irq_is_enabled(SIO_IRQ_PROC0);
+ irq_set_enabled(SIO_IRQ_PROC0, false);
+
+ // Values to be sent in order over the FIFO from core 0 to core 1
+ //
+ // vector_table is value for VTOR register
+ // sp is initial stack pointer (SP)
+ // entry is the initial program counter (PC) (don't forget to set the thumb bit!)
+ const uint32_t cmd_sequence[] =
+ {0, 0, 1, (uintptr_t) vector_table, (uintptr_t) sp, (uintptr_t) entry};
+
+ uint seq = 0;
+ do {
+ uint cmd = cmd_sequence[seq];
+ // Always drain the READ FIFO (from core 1) before sending a 0
+ if (!cmd) {
+ multicore_fifo_drain();
+ // Execute a SEV as core 1 may be waiting for FIFO space via WFE
+ __sev();
+ }
+ multicore_fifo_push_blocking(cmd);
+ uint32_t response = multicore_fifo_pop_blocking();
+ // Move to next state on correct response (echo-d value) otherwise start over
+ seq = cmd == response ? seq + 1 : 0;
+ } while (seq < count_of(cmd_sequence));
+
+ irq_set_enabled(SIO_IRQ_PROC0, enabled);
+}
+
+#define LOCKOUT_MAGIC_START 0x73a8831eu
+#define LOCKOUT_MAGIC_END (~LOCKOUT_MAGIC_START)
+
+static_assert(SIO_IRQ_PROC1 == SIO_IRQ_PROC0 + 1, "");
+
+static mutex_t lockout_mutex;
+static bool lockout_in_progress;
+
+// note this method is in RAM because lockout is used when writing to flash
+// it only makes inline calls
+static void __isr __not_in_flash_func(multicore_lockout_handler)(void) {
+ multicore_fifo_clear_irq();
+ while (multicore_fifo_rvalid()) {
+ if (sio_hw->fifo_rd == LOCKOUT_MAGIC_START) {
+ uint32_t save = save_and_disable_interrupts();
+ multicore_fifo_push_blocking_inline(LOCKOUT_MAGIC_START);
+ while (multicore_fifo_pop_blocking_inline() != LOCKOUT_MAGIC_END) {
+ tight_loop_contents(); // not tight but endless potentially
+ }
+ restore_interrupts(save);
+ multicore_fifo_push_blocking_inline(LOCKOUT_MAGIC_END);
+ }
+ }
+}
+
+static void check_lockout_mutex_init(void) {
+ // use known available lock - we only need it briefly
+ uint32_t save = hw_claim_lock();
+ if (!mutex_is_initialized(&lockout_mutex)) {
+ mutex_init(&lockout_mutex);
+ }
+ hw_claim_unlock(save);
+}
+
+void multicore_lockout_victim_init() {
+ check_lockout_mutex_init();
+ uint core_num = get_core_num();
+ irq_set_exclusive_handler(SIO_IRQ_PROC0 + core_num, multicore_lockout_handler);
+ irq_set_enabled(SIO_IRQ_PROC0 + core_num, true);
+}
+
+static bool multicore_lockout_handshake(uint32_t magic, absolute_time_t until) {
+ uint irq_num = SIO_IRQ_PROC0 + get_core_num();
+ bool enabled = irq_is_enabled(irq_num);
+ if (enabled) irq_set_enabled(irq_num, false);
+ bool rc = false;
+ do {
+ int64_t next_timeout_us = absolute_time_diff_us(get_absolute_time(), until);
+ if (next_timeout_us < 0) {
+ break;
+ }
+ multicore_fifo_push_timeout_us(magic, (uint64_t)next_timeout_us);
+ next_timeout_us = absolute_time_diff_us(get_absolute_time(), until);
+ if (next_timeout_us < 0) {
+ break;
+ }
+ uint32_t word = 0;
+ if (!multicore_fifo_pop_timeout_us((uint64_t)next_timeout_us, &word)) {
+ break;
+ }
+ if (word == magic) {
+ rc = true;
+ }
+ } while (!rc);
+ if (enabled) irq_set_enabled(irq_num, true);
+ return rc;
+}
+
+static bool multicore_lockout_start_block_until(absolute_time_t until) {
+ check_lockout_mutex_init();
+ if (!mutex_enter_block_until(&lockout_mutex, until)) {
+ return false;
+ }
+ hard_assert(!lockout_in_progress);
+ bool rc = multicore_lockout_handshake(LOCKOUT_MAGIC_START, until);
+ lockout_in_progress = rc;
+ mutex_exit(&lockout_mutex);
+ return rc;
+}
+
+bool multicore_lockout_start_timeout_us(uint64_t timeout_us) {
+ return multicore_lockout_start_block_until(make_timeout_time_us(timeout_us));
+}
+
+void multicore_lockout_start_blocking() {
+ multicore_lockout_start_block_until(at_the_end_of_time);
+}
+
+static bool multicore_lockout_end_block_until(absolute_time_t until) {
+ assert(mutex_is_initialized(&lockout_mutex));
+ if (!mutex_enter_block_until(&lockout_mutex, until)) {
+ return false;
+ }
+ assert(lockout_in_progress);
+ bool rc = multicore_lockout_handshake(LOCKOUT_MAGIC_END, until);
+ if (rc) {
+ lockout_in_progress = false;
+ }
+ mutex_exit(&lockout_mutex);
+ return rc;
+}
+
+bool multicore_lockout_end_timeout_us(uint64_t timeout_us) {
+ return multicore_lockout_end_block_until(make_timeout_time_us(timeout_us));
+}
+
+void multicore_lockout_end_blocking() {
+ multicore_lockout_end_block_until(at_the_end_of_time);
+}
diff --git a/src/rp2_common/pico_platform/CMakeLists.txt b/src/rp2_common/pico_platform/CMakeLists.txt
new file mode 100644
index 0000000..8a6f1d2
--- /dev/null
+++ b/src/rp2_common/pico_platform/CMakeLists.txt
@@ -0,0 +1,25 @@
+if (NOT TARGET pico_platform_headers)
+ add_library(pico_platform_headers INTERFACE)
+
+ target_compile_definitions(pico_platform_headers INTERFACE
+ PICO_NO_HARDWARE=0
+ PICO_ON_DEVICE=1
+ PICO_BUILD=1
+ )
+
+ target_include_directories(pico_platform_headers INTERFACE ${CMAKE_CURRENT_LIST_DIR}/include)
+
+ target_link_libraries(pico_platform_headers INTERFACE hardware_regs)
+endif()
+
+if (NOT TARGET pico_platform)
+ pico_add_impl_library(pico_platform)
+ target_sources(pico_platform INTERFACE
+ ${CMAKE_CURRENT_LIST_DIR}/platform.c)
+
+ target_link_libraries(pico_platform INTERFACE pico_platform_headers)
+endif()
+
+function(pico_add_platform_library TARGET)
+ target_link_libraries(pico_platform INTERFACE ${TARGET})
+endfunction()
\ No newline at end of file
diff --git a/src/rp2_common/pico_platform/include/pico/asm_helper.S b/src/rp2_common/pico_platform/include/pico/asm_helper.S
new file mode 100644
index 0000000..3e01c66
--- /dev/null
+++ b/src/rp2_common/pico_platform/include/pico/asm_helper.S
@@ -0,0 +1,32 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "pico.h"
+
+// do not put align in here as it is used mid function sometimes
+.macro regular_func x
+.global \x
+.type \x,%function
+.thumb_func
+\x:
+.endm
+
+.macro regular_func_with_section x
+.section .text.\x
+regular_func \x
+.endm
+
+// do not put align in here as it is used mid function sometimes
+.macro wrapper_func x
+regular_func WRAPPER_FUNC_NAME(\x)
+.endm
+
+.macro __pre_init func, priority_string
+.section .preinit_array.\priority_string
+.align 2
+.word \func
+.endm
+
diff --git a/src/rp2_common/pico_platform/include/pico/platform.h b/src/rp2_common/pico_platform/include/pico/platform.h
new file mode 100644
index 0000000..ee1d360
--- /dev/null
+++ b/src/rp2_common/pico_platform/include/pico/platform.h
@@ -0,0 +1,411 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PICO_PLATFORM_H_
+#define _PICO_PLATFORM_H_
+
+/** \file platform.h
+ * \defgroup pico_platform pico_platform
+ *
+ * Macros and definitions (and functions when included by non assembly code) for the RP2 family device / architecture
+ * to provide a common abstraction over low level compiler / platform specifics.
+ *
+ * This header may be included by assembly code
+ */
+
+#include "hardware/platform_defs.h"
+
+// Marker for builds targeting the RP2040
+#define PICO_RP2040 1
+
+// PICO_CONFIG: PICO_STACK_SIZE, Stack Size, min=0x100, default=0x800, advanced=true, group=pico_platform
+#ifndef PICO_STACK_SIZE
+#define PICO_STACK_SIZE _u(0x800)
+#endif
+
+// PICO_CONFIG: PICO_HEAP_SIZE, Heap size to reserve, min=0x100, default=0x800, advanced=true, group=pico_platform
+#ifndef PICO_HEAP_SIZE
+#define PICO_HEAP_SIZE _u(0x800)
+#endif
+
+// PICO_CONFIG: PICO_NO_RAM_VECTOR_TABLE, Enable/disable the RAM vector table, type=bool, default=0, advanced=true, group=pico_platform
+#ifndef PICO_NO_RAM_VECTOR_TABLE
+#define PICO_NO_RAM_VECTOR_TABLE 0
+#endif
+
+// PICO_CONFIG: PICO_RP2040_B0_SUPPORTED, Whether to include any specific software support for RP2040 B0 revision, type=bool, default=1, advanced=true, group=pico_platform
+#ifndef PICO_RP2040_B0_SUPPORTED
+#define PICO_RP2040_B0_SUPPORTED 1
+#endif
+
+// PICO_CONFIG: PICO_FLOAT_SUPPORT_ROM_V1, Include float support code for RP2040 B0 when that chip revision is supported , type=bool, default=1, advanced=true, group=pico_platform
+#ifndef PICO_FLOAT_SUPPORT_ROM_V1
+#define PICO_FLOAT_SUPPORT_ROM_V1 1
+#endif
+
+// PICO_CONFIG: PICO_DOUBLE_SUPPORT_ROM_V1, Include double support code for RP2040 B0 when that chip revision is supported , type=bool, default=1, advanced=true, group=pico_platform
+#ifndef PICO_DOUBLE_SUPPORT_ROM_V1
+#define PICO_DOUBLE_SUPPORT_ROM_V1 1
+#endif
+
+
+// PICO_CONFIG: PICO_RP2040_B1_SUPPORTED, Whether to include any specific software support for RP2040 B1 revision, type=bool, default=1, advanced=true, group=pico_platform
+#ifndef PICO_RP2040_B1_SUPPORTED
+#define PICO_RP2040_B1_SUPPORTED 1
+#endif
+
+// PICO_CONFIG: PICO_RP2040_B2_SUPPORTED, Whether to include any specific software support for RP2040 B2 revision, type=bool, default=1, advanced=true, group=pico_platform
+#ifndef PICO_RP2040_B2_SUPPORTED
+#define PICO_RP2040_B2_SUPPORTED 1
+#endif
+
+// --- remainder of file is not included by assembly code ---
+
+#ifndef __ASSEMBLER__
+
+#include <sys/cdefs.h>
+#include "pico/types.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*! \brief Marker for an interrupt handler
+ * \ingroup pico_platform
+ * For example an IRQ handler function called my_interrupt_handler:
+ *
+ * void __isr my_interrupt_handler(void) {
+ */
+#define __isr
+
+/*! \brief Section attribute macro for placement in RAM after the `.data` section
+ * \ingroup pico_platform
+ *
+ * For example a 400 element `uint32_t` array placed after the .data section
+ *
+ * uint32_t __after_data("my_group_name") a_big_array[400];
+ *
+ * The section attribute is `.after_data.<group>`
+ *
+ * \param group a string suffix to use in the section name to distinguish groups that can be linker
+ * garbage-collected independently
+ */
+#define __after_data(group) __attribute__((section(".after_data." group)))
+
+/*! \brief Section attribute macro for placement not in flash (i.e in RAM)
+ * \ingroup pico_platform
+ *
+ * For example a 3 element `uint32_t` array placed in RAM (even though it is `static const`)
+ *
+ * static const uint32_t __not_in_flash("my_group_name") an_array[3];
+ *
+ * The section attribute is `.time_critical.<group>`
+ *
+ * \param group a string suffix to use in the section name to distinguish groups that can be linker
+ * garbage-collected independently
+ */
+#define __not_in_flash(group) __attribute__((section(".time_critical." group)))
+
+/*! \brief Section attribute macro for placement in the SRAM bank 4 (known as "scratch X")
+ * \ingroup pico_platform
+ *
+ * Scratch X is commonly used for critical data and functions accessed only by one core (when only
+ * one core is accessing the RAM bank, there is no opportunity for stalls)
+ *
+ * For example a `uint32_t` variable placed in "scratch X"
+ *
+ * uint32_t __scratch_x("my_group_name") foo = 23;
+ *
+ * The section attribute is `.scratch_x.<group>`
+ *
+ * \param group a string suffix to use in the section name to distinguish groups that can be linker
+ * garbage-collected independently
+ */
+#define __scratch_x(group) __attribute__((section(".scratch_x." group)))
+
+/*! \brief Section attribute macro for placement in the SRAM bank 5 (known as "scratch Y")
+ * \ingroup pico_platform
+ *
+ * Scratch Y is commonly used for critical data and functions accessed only by one core (when only
+ * one core is accessing the RAM bank, there is no opportunity for stalls)
+ *
+ * For example a `uint32_t` variable placed in "scratch Y"
+ *
+ * uint32_t __scratch_y("my_group_name") foo = 23;
+ *
+ * The section attribute is `.scratch_y.<group>`
+ *
+ * \param group a string suffix to use in the section name to distinguish groups that can be linker
+ * garbage-collected independently
+ */
+#define __scratch_y(group) __attribute__((section(".scratch_y." group)))
+
+/*! \brief Section attribute macro for data that is to be left uninitialized
+ * \ingroup pico_platform
+ *
+ * Data marked this way will retain its value across a reset (normally uninitialized data - in the .bss
+ * section) is initialized to zero during runtime initialization
+ *
+ * For example a `uint32_t` foo that will retain its value if the program is restarted by reset.
+ *
+ * uint32_t __uninitialized_ram("my_group_name") foo;
+ *
+ * The section attribute is `.uninitialized_ram.<group>`
+ *
+ * \param group a string suffix to use in the section name to distinguish groups that can be linker
+ * garbage-collected independently
+ */
+#define __uninitialized_ram(group) __attribute__((section(".uninitialized_ram." #group))) group
+
+/*! \brief Section attribute macro for placement in flash even in a COPY_TO_RAM binary
+ * \ingroup pico_platform
+ *
+ * For example a `uint32_t` variable explicitly placed in flash (it will hard fault if you attempt to write it!)
+ *
+ * uint32_t __in_flash("my_group_name") foo = 23;
+ *
+ * The section attribute is `.flashdata.<group>`
+ *
+ * \param group a string suffix to use in the section name to distinguish groups that can be linker
+ * garbage-collected independently
+ */
+#define __in_flash(group) __attribute__((section(".flashdata" group)))
+
+/*! \brief Indicates a function should not be stored in flash
+ * \ingroup pico_platform
+ *
+ * Decorates a function name, such that the function will execute from RAM (assuming it is not inlined
+ * into a flash function by the compiler)
+ *
+ * For example a function called my_func taking an int parameter:
+ *
+ * void __not_in_flash_func(my_func)(int some_arg) {
+ *
+ * The function is placed in the `.time_critical.<func_name>` linker section
+ *
+ * \see __no_inline_not_in_flash_func
+ */
+#define __not_in_flash_func(func_name) __not_in_flash(__STRING(func_name)) func_name
+
+/*! \brief Indicates a function is time/latency critical and should not run from flash
+ * \ingroup pico_platform
+ *
+ * Decorates a function name, such that the function will execute from RAM (assuming it is not inlined
+ * into a flash function by the compiler) to avoid possible flash latency. Currently this macro is identical
+ * in implementation to `__not_in_flash_func`, however the semantics are distinct and a `__time_critical_func`
+ * may in the future be treated more specially to reduce the overhead when calling such function from a flash
+ * function.
+ *
+ * For example a function called my_func taking an int parameter:
+ *
+ * void __time_critical(my_func)(int some_arg) {
+ *
+ * The function is placed in the `.time_critical.<func_name>` linker section
+ *
+ * \see __not_in_flash_func
+ */
+#define __time_critical_func(func_name) __not_in_flash_func(func_name)
+
+/*! \brief Indicate a function should not be stored in flash and should not be inlined
+ * \ingroup pico_platform
+ *
+ * Decorates a function name, such that the function will execute from RAM, explicitly marking it as
+ * noinline to prevent it being inlined into a flash function by the compiler
+ *
+ * For example a function called my_func taking an int parameter:
+ *
+ * void __no_inline_not_in_flash_func(my_func)(int some_arg) {
+ *
+ * The function is placed in the `.time_critical.<func_name>` linker section
+ */
+#define __no_inline_not_in_flash_func(func_name) __noinline __not_in_flash_func(func_name)
+
+#define __packed_aligned __packed __aligned(4)
+
+/*! \brief Attribute to force inlining of a function regardless of optimization level
+ * \ingroup pico_platform
+ *
+ * For example my_function here will always be inlined:
+ *
+ * int __force_inline my_function(int x) {
+ *
+ */
+#if defined(__GNUC__) && __GNUC__ <= 7
+#define __force_inline inline __always_inline
+#else
+#define __force_inline __always_inline
+#endif
+
+/*! \brief Macro to determine the number of elements in an array
+ * \ingroup pico_platform
+ */
+#ifndef count_of
+#define count_of(a) (sizeof(a)/sizeof((a)[0]))
+#endif
+
+/*! \brief Macro to return the maximum of two comparable values
+ * \ingroup pico_platform
+ */
+#ifndef MAX
+#define MAX(a, b) ((a)>(b)?(a):(b))
+#endif
+
+/*! \brief Macro to return the minimum of two comparable values
+ * \ingroup pico_platform
+ */
+#ifndef MIN
+#define MIN(a, b) ((b)>(a)?(a):(b))
+#endif
+
+/*! \brief Execute a breakpoint instruction
+ * \ingroup pico_platform
+ */
+static inline void __breakpoint(void) {
+ __asm__("bkpt #0");
+}
+
+/*! \brief Ensure that the compiler does not move memory access across this method call
+ * \ingroup pico_platform
+ *
+ * For example in the following code:
+ *
+ * *some_memory_location = var_a;
+ * __compiler_memory_barrier();
+ * uint32_t var_b = *some_other_memory_location
+ *
+ * The compiler will not move the load from `some_other_memory_location` above the memory barrier (which it otherwise
+ * might - even above the memory store!)
+ */
+__force_inline static void __compiler_memory_barrier(void) {
+ __asm__ volatile ("" : : : "memory");
+}
+
+/*! \brief Macro for converting memory addresses to 32 bit addresses suitable for DMA
+ * \ingroup pico_platform
+ *
+ * This is just a cast to `uintptr_t` on the RP2040, however you may want to use this when developing code
+ * that also runs in "host" mode. If the host mode is 64 bit and you are embedding data pointers
+ * in other data (e.g. DMA chaining), then there is a need in "host" mode to convert a 64 bit native
+ * pointer to a 32 bit value for storage, which can be done using this macro.
+ */
+#define host_safe_hw_ptr(x) ((uintptr_t)(x))
+#define native_safe_hw_ptr(x) host_safe_hw_ptr(x)
+
+
+/*! \brief Panics with the message "Unsupported"
+ * \ingroup pico_platform
+ * \see panic
+ */
+void __attribute__((noreturn)) panic_unsupported(void);
+
+/*! \brief Displays a panic message and halts execution
+ * \ingroup pico_platform
+ *
+ * An attempt is made to output the message to all registered STDOUT drivers
+ * after which this method executes a BKPT instruction.
+ *
+ * @param fmt format string (printf-like)
+ * @param ... printf-like arguments
+ */
+void __attribute__((noreturn)) panic(const char *fmt, ...);
+
+// PICO_CONFIG: PICO_NO_FPGA_CHECK, Remove the FPGA platform check for small code size reduction, type=bool, default=0, advanced=true, group=pico_runtime
+#ifndef PICO_NO_FPGA_CHECK
+#define PICO_NO_FPGA_CHECK 0
+#endif
+
+#if PICO_NO_FPGA_CHECK
+static inline bool running_on_fpga(void) {return false;}
+#else
+bool running_on_fpga(void);
+#endif
+
+/*! \brief Returns the RP2040 chip revision number
+ * \ingroup pico_platform
+ * @return the RP2040 chip revision number (1 for B0/B1, 2 for B2)
+ */
+uint8_t rp2040_chip_version(void);
+
+/*! \brief Returns the RP2040 rom version number
+ * \ingroup pico_platform
+ * @return the RP2040 rom version number (1 for RP2040-B0, 2 for RP2040-B1, 3 for RP2040-B2)
+ */
+static inline uint8_t rp2040_rom_version(void) {
+ return *(uint8_t*)0x13;
+}
+
+/*! \brief No-op function for the body of tight loops
+ * \ingroup pico_platform
+ *
+ * Np-op function intended to be called by any tight hardware polling loop. Using this ubiquitously
+ * makes it much easier to find tight loops, but also in the future \#ifdef-ed support for lockup
+ * debugging might be added
+ */
+static __force_inline void tight_loop_contents(void) {}
+
+/*! \brief Multiply two integers using an assembly `MUL` instruction
+ * \ingroup pico_platform
+ *
+ * This multiplies a by b using multiply instruction using the ARM mul instruction regardless of values (the compiler
+ * might otherwise choose to perform shifts/adds), i.e. this is a 1 cycle operation.
+ *
+ * \param a the first operand
+ * \param b the second operand
+ * \return a * b
+ */
+__force_inline static int32_t __mul_instruction(int32_t a, int32_t b) {
+ asm ("mul %0, %1" : "+l" (a) : "l" (b) : );
+ return a;
+}
+
+/*! \brief multiply two integer values using the fastest method possible
+ * \ingroup pico_platform
+ *
+ * Efficiently multiplies value a by possibly constant value b.
+ *
+ * If b is known to be constant and not zero or a power of 2, then a mul instruction is used rather than gcc's default
+ * which is often a slow combination of shifts and adds. If b is a power of 2 then a single shift is of course preferable
+ * and will be used
+ *
+ * \param a the first operand
+ * \param b the second operand
+ * \return a * b
+ */
+#define __fast_mul(a, b) __builtin_choose_expr(__builtin_constant_p(b) && !__builtin_constant_p(a), \
+(__builtin_popcount(b) >= 2 ? __mul_instruction(a,b) : (a)*(b)), \
+(a)*(b))
+
+/*! \brief Utility macro to assert two types are equivalent.
+ * \ingroup pico_platform
+ *
+ * This macro can be useful in other macros along with `typeof` to assert that two parameters are of equivalent type
+ * (or that a single parameter is of an expected type)
+ */
+#define __check_type_compatible(type_a, type_b) static_assert(__builtin_types_compatible_p(type_a, type_b), __STRING(type_a) " is not compatible with " __STRING(type_b));
+
+/*! \brief Get the current exception level on this core
+ * \ingroup pico_platform
+ *
+ * \return the exception number if the CPU is handling an exception, or 0 otherwise
+ */
+uint __get_current_exception(void);
+
+#define WRAPPER_FUNC(x) __wrap_ ## x
+#define REAL_FUNC(x) __real_ ## x
+
+#ifdef __cplusplus
+}
+#endif
+
+#else // __ASSEMBLER__
+
+#define WRAPPER_FUNC_NAME(x) __wrap_##x
+#define SECTION_NAME(x) .text.##x
+#define RAM_SECTION_NAME(x) .time_critical.##x
+
+#endif // !__ASSEMBLER__
+
+#endif
diff --git a/src/rp2_common/pico_platform/platform.c b/src/rp2_common/pico_platform/platform.c
new file mode 100644
index 0000000..9bbcf26
--- /dev/null
+++ b/src/rp2_common/pico_platform/platform.c
@@ -0,0 +1,39 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "pico.h"
+#include "hardware/address_mapped.h"
+#include "hardware/regs/tbman.h"
+#include "hardware/regs/sysinfo.h"
+
+// Note we leave the FPGA check in by default so that we can run bug repro
+// binaries coming in from the wild on the FPGA platform. It takes up around
+// 48 bytes if you include all the calls, so you can pass PICO_NO_FPGA_CHECK=1
+// to remove it. The FPGA check is used to skip initialisation of hardware
+// (mainly clock generators and oscillators) that aren't present on FPGA.
+
+#if !PICO_NO_FPGA_CHECK
+// Inline stub provided in header if this code is unused (so folding can be
+// done in each TU instead of relying on LTO)
+bool running_on_fpga() {
+ return !!((*(io_ro_32 *)TBMAN_BASE) & TBMAN_PLATFORM_FPGA_BITS);
+}
+#endif
+
+#define MANUFACTURER_RPI 0x927
+#define PART_RP2 0x2
+
+uint8_t rp2040_chip_version() {
+ // First register of sysinfo is chip id
+ uint32_t chip_id = *((io_ro_32*)(SYSINFO_BASE + SYSINFO_CHIP_ID_OFFSET));
+ uint32_t __unused manufacturer = chip_id & SYSINFO_CHIP_ID_MANUFACTURER_BITS;
+ uint32_t __unused part = (chip_id & SYSINFO_CHIP_ID_PART_BITS) >> SYSINFO_CHIP_ID_PART_LSB;
+ assert(manufacturer == MANUFACTURER_RPI);
+ assert(part == PART_RP2);
+ // Version 1 == B0/B1
+ uint version = (chip_id & SYSINFO_CHIP_ID_REVISION_BITS) >> SYSINFO_CHIP_ID_REVISION_LSB;
+ return (uint8_t)version;
+}
\ No newline at end of file
diff --git a/src/rp2_common/pico_printf/CMakeLists.txt b/src/rp2_common/pico_printf/CMakeLists.txt
new file mode 100644
index 0000000..989dcd1
--- /dev/null
+++ b/src/rp2_common/pico_printf/CMakeLists.txt
@@ -0,0 +1,52 @@
+if (NOT TARGET pico_printf)
+ # library to be depended on - we make this depend on particular implementations using per target generator expressions
+ pico_add_impl_library(pico_printf)
+
+ # no custom implementation; falls thru to compiler
+ pico_add_impl_library(pico_printf_compiler)
+
+ add_library(pico_printf_headers INTERFACE)
+ target_include_directories(pico_printf_headers INTERFACE ${CMAKE_CURRENT_LIST_DIR}/include)
+
+ # add alias "default" which is just pico.
+ add_library(pico_printf_default INTERFACE)
+ target_link_libraries(pico_printf_default INTERFACE pico_printf_pico)
+
+ set(PICO_DEFAULT_PRINTF_IMPL pico_printf_default)
+
+ target_link_libraries(pico_printf INTERFACE
+ $<IF:$<BOOL:$<TARGET_PROPERTY:PICO_TARGET_PRINTF_IMPL>>,$<TARGET_PROPERTY:PICO_TARGET_PRINTF_IMPL>,${PICO_DEFAULT_PRINTF_IMPL}>)
+
+ pico_add_impl_library(pico_printf_pico)
+ target_sources(pico_printf_pico INTERFACE
+ ${CMAKE_CURRENT_LIST_DIR}/printf.c
+ )
+
+ target_link_libraries(pico_printf_pico INTERFACE pico_printf_headers)
+
+ pico_add_impl_library(pico_printf_none)
+ target_sources(pico_printf_none INTERFACE
+ ${CMAKE_CURRENT_LIST_DIR}/printf_none.S
+ )
+
+ target_link_libraries(pico_printf_none INTERFACE pico_printf_headers)
+
+ function(wrap_printf_functions TARGET)
+ # note that printf and vprintf are in pico_stdio so we can provide thread safety
+ pico_wrap_function(${TARGET} sprintf)
+ pico_wrap_function(${TARGET} snprintf)
+ pico_wrap_function(${TARGET} vsnprintf)
+ endfunction()
+
+ wrap_printf_functions(pico_printf_pico)
+ wrap_printf_functions(pico_printf_none)
+
+ macro(pico_set_printf_implementation TARGET IMPL)
+ get_target_property(target_type ${TARGET} TYPE)
+ if ("EXECUTABLE" STREQUAL "${target_type}")
+ set_target_properties(${TARGET} PROPERTIES PICO_TARGET_PRINTF_IMPL "pico_printf_${IMPL}")
+ else()
+ message(FATAL_ERROR "printf implementation must be set on executable not library")
+ endif()
+ endmacro()
+endif()
diff --git a/src/rp2_common/pico_printf/include/pico/printf.h b/src/rp2_common/pico_printf/include/pico/printf.h
new file mode 100644
index 0000000..25cea48
--- /dev/null
+++ b/src/rp2_common/pico_printf/include/pico/printf.h
@@ -0,0 +1,93 @@
+///////////////////////////////////////////////////////////////////////////////
+// \author (c) Marco Paland (info@paland.com)
+// 2014-2019, PALANDesign Hannover, Germany
+//
+// \license The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+// THE SOFTWARE.
+//
+// \brief Tiny printf, sprintf and snprintf implementation, optimized for speed on
+// embedded systems with a very limited resources.
+// Use this instead of bloated standard/newlib printf.
+// These routines are thread safe and reentrant.
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef PICO_PRINTF_H_
+#define PICO_PRINTF_H_
+
+/** \file printf.h
+ * \defgroup pico_printf pico_printf
+ *
+ * Compact replacement for printf by Marco Paland (info@paland.com)
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "pico.h"
+#include <stdio.h>
+#include <stdarg.h>
+
+// PICO_CONFIG: PICO_PRINTF_ALWAYS_INCLUDED, Whether to always include printf code even if only called weakly (by panic), type=bool, default=1 in debug build 0 otherwise, group=pico_printf
+#ifndef PICO_PRINTF_ALWAYS_INCLUDED
+#ifndef NDEBUG
+#define PICO_PRINTF_ALWAYS_INCLUDED 1
+#else
+#define PICO_PRINTF_ALWAYS_INCLUDED 0
+#endif
+#endif
+
+#if LIB_PICO_PRINTF_PICO
+// weak raw printf may be a puts if printf has not been called,
+// so that we can support gc of printf when it isn't called
+//
+// it is called raw to distinguish it from the regular printf which
+// is in stdio.c and does mutex protection
+#if !PICO_PRINTF_ALWAYS_INCLUDED
+bool __printflike(1, 0) weak_raw_printf(const char *fmt, ...);
+bool weak_raw_vprintf(const char *fmt, va_list args);
+#else
+#define weak_raw_printf(...) ({printf(__VA_ARGS__); true;})
+#define weak_raw_vprintf(fmt,va) ({vprintf(fmt,va); true;})
+#endif
+
+/**
+ * printf with output function
+ * You may use this as dynamic alternative to printf() with its fixed _putchar() output
+ * \param out An output function which takes one character and an argument pointer
+ * \param arg An argument pointer for user data passed to output function
+ * \param format A string that specifies the format of the output
+ * \return The number of characters that are sent to the output function, not counting the terminating null character
+ */
+int vfctprintf(void (*out)(char character, void *arg), void *arg, const char *format, va_list va);
+
+#else
+
+#define weak_raw_printf(...) ({printf(__VA_ARGS__); true;})
+#define weak_raw_vprintf(fmt,va) ({vprintf(fmt,va); true;})
+
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // _PRINTF_H_
diff --git a/src/rp2_common/pico_printf/printf.c b/src/rp2_common/pico_printf/printf.c
new file mode 100644
index 0000000..2594c57
--- /dev/null
+++ b/src/rp2_common/pico_printf/printf.c
@@ -0,0 +1,942 @@
+///////////////////////////////////////////////////////////////////////////////
+// \author (c) Marco Paland (info@paland.com)
+// 2014-2019, PALANDesign Hannover, Germany
+//
+// \license The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+// THE SOFTWARE.
+//
+// \brief Tiny printf, sprintf and (v)snprintf implementation, optimized for speed on
+// embedded systems with a very limited resources. These routines are thread
+// safe and reentrant!
+// Use this instead of the bloated standard/newlib printf cause these use
+// malloc for printf (and may not be thread safe).
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+
+#include "pico/platform.h"
+#include "pico/printf.h"
+
+// PICO_CONFIG: PICO_PRINTF_NTOA_BUFFER_SIZE, Define printf ntoa buffer size, min=0, max=128, default=32, group=pico_printf
+// 'ntoa' conversion buffer size, this must be big enough to hold one converted
+// numeric number including padded zeros (dynamically created on stack)
+#ifndef PICO_PRINTF_NTOA_BUFFER_SIZE
+#define PICO_PRINTF_NTOA_BUFFER_SIZE 32U
+#endif
+
+// PICO_CONFIG: PICO_PRINTF_FTOA_BUFFER_SIZE, Define printf ftoa buffer size, min=0, max=128, default=32, group=pico_printf
+// 'ftoa' conversion buffer size, this must be big enough to hold one converted
+// float number including padded zeros (dynamically created on stack)
+#ifndef PICO_PRINTF_FTOA_BUFFER_SIZE
+#define PICO_PRINTF_FTOA_BUFFER_SIZE 32U
+#endif
+
+// PICO_CONFIG: PICO_PRINTF_SUPPORT_FLOAT, Enable floating point printing, type=bool, default=1, group=pico_printf
+// support for the floating point type (%f)
+#ifndef PICO_PRINTF_SUPPORT_FLOAT
+#define PICO_PRINTF_SUPPORT_FLOAT 1
+#endif
+
+// PICO_CONFIG: PICO_PRINTF_SUPPORT_EXPONENTIAL, Enable exponential floating point printing, type=bool, default=1, group=pico_printf
+// support for exponential floating point notation (%e/%g)
+#ifndef PICO_PRINTF_SUPPORT_EXPONENTIAL
+#define PICO_PRINTF_SUPPORT_EXPONENTIAL 1
+#endif
+
+// PICO_CONFIG: PICO_PRINTF_DEFAULT_FLOAT_PRECISION, Define default floating point precision, min=1, max=16, default=6, group=pico_printf
+#ifndef PICO_PRINTF_DEFAULT_FLOAT_PRECISION
+#define PICO_PRINTF_DEFAULT_FLOAT_PRECISION 6U
+#endif
+
+// PICO_CONFIG: PICO_PRINTF_MAX_FLOAT, Define the largest float suitable to print with %f, min=1, max=1e9, default=1e9, group=pico_printf
+#ifndef PICO_PRINTF_MAX_FLOAT
+#define PICO_PRINTF_MAX_FLOAT 1e9
+#endif
+
+// PICO_CONFIG: PICO_PRINTF_SUPPORT_LONG_LONG, Enable support for long long types (%llu or %p), type=bool, default=1, group=pico_printf
+#ifndef PICO_PRINTF_SUPPORT_LONG_LONG
+#define PICO_PRINTF_SUPPORT_LONG_LONG 1
+#endif
+
+// PICO_CONFIG: PICO_PRINTF_SUPPORT_PTRDIFF_T, Enable support for the ptrdiff_t type (%t), type=bool, default=1, group=pico_printf
+// ptrdiff_t is normally defined in <stddef.h> as long or long long type
+#ifndef PICO_PRINTF_SUPPORT_PTRDIFF_T
+#define PICO_PRINTF_SUPPORT_PTRDIFF_T 1
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+
+// internal flag definitions
+#define FLAGS_ZEROPAD (1U << 0U)
+#define FLAGS_LEFT (1U << 1U)
+#define FLAGS_PLUS (1U << 2U)
+#define FLAGS_SPACE (1U << 3U)
+#define FLAGS_HASH (1U << 4U)
+#define FLAGS_UPPERCASE (1U << 5U)
+#define FLAGS_CHAR (1U << 6U)
+#define FLAGS_SHORT (1U << 7U)
+#define FLAGS_LONG (1U << 8U)
+#define FLAGS_LONG_LONG (1U << 9U)
+#define FLAGS_PRECISION (1U << 10U)
+#define FLAGS_ADAPT_EXP (1U << 11U)
+
+// import float.h for DBL_MAX
+#if PICO_PRINTF_SUPPORT_FLOAT
+
+#include <float.h>
+
+#endif
+
+/**
+ * Output a character to a custom device like UART, used by the printf() function
+ * This function is declared here only. You have to write your custom implementation somewhere
+ * \param character Character to output
+ */
+static void _putchar(char character) {
+ putchar(character);
+}
+
+// output function type
+typedef void (*out_fct_type)(char character, void *buffer, size_t idx, size_t maxlen);
+
+#if !PICO_PRINTF_ALWAYS_INCLUDED
+// we don't have a way to specify a truly weak symbol reference (the linker will always include targets in a single link step,
+// so we make a function pointer that is initialized on the first printf called... if printf is not included in the binary
+// (or has never been called - we can't tell) then this will be null. the assumption is that if you are using printf
+// you are likely to have printed something.
+static int (*lazy_vsnprintf)(out_fct_type out, char *buffer, const size_t maxlen, const char *format, va_list va);
+#endif
+
+// wrapper (used as buffer) for output function type
+typedef struct {
+ void (*fct)(char character, void *arg);
+ void *arg;
+} out_fct_wrap_type;
+
+// internal buffer output
+static inline void _out_buffer(char character, void *buffer, size_t idx, size_t maxlen) {
+ if (idx < maxlen) {
+ ((char *) buffer)[idx] = character;
+ }
+}
+
+// internal null output
+static inline void _out_null(char character, void *buffer, size_t idx, size_t maxlen) {
+ (void) character;
+ (void) buffer;
+ (void) idx;
+ (void) maxlen;
+}
+
+// internal _putchar wrapper
+static inline void _out_char(char character, void *buffer, size_t idx, size_t maxlen) {
+ (void) buffer;
+ (void) idx;
+ (void) maxlen;
+ if (character) {
+ _putchar(character);
+ }
+}
+
+
+// internal output function wrapper
+static inline void _out_fct(char character, void *buffer, size_t idx, size_t maxlen) {
+ (void) idx;
+ (void) maxlen;
+ if (character) {
+ // buffer is the output fct pointer
+ ((out_fct_wrap_type *) buffer)->fct(character, ((out_fct_wrap_type *) buffer)->arg);
+ }
+}
+
+
+// internal secure strlen
+// \return The length of the string (excluding the terminating 0) limited by 'maxsize'
+static inline unsigned int _strnlen_s(const char *str, size_t maxsize) {
+ const char *s;
+ for (s = str; *s && maxsize--; ++s);
+ return (unsigned int) (s - str);
+}
+
+
+// internal test if char is a digit (0-9)
+// \return true if char is a digit
+static inline bool _is_digit(char ch) {
+ return (ch >= '0') && (ch <= '9');
+}
+
+
+// internal ASCII string to unsigned int conversion
+static unsigned int _atoi(const char **str) {
+ unsigned int i = 0U;
+ while (_is_digit(**str)) {
+ i = i * 10U + (unsigned int) (*((*str)++) - '0');
+ }
+ return i;
+}
+
+
+// output the specified string in reverse, taking care of any zero-padding
+static size_t _out_rev(out_fct_type out, char *buffer, size_t idx, size_t maxlen, const char *buf, size_t len,
+ unsigned int width, unsigned int flags) {
+ const size_t start_idx = idx;
+
+ // pad spaces up to given width
+ if (!(flags & FLAGS_LEFT) && !(flags & FLAGS_ZEROPAD)) {
+ for (size_t i = len; i < width; i++) {
+ out(' ', buffer, idx++, maxlen);
+ }
+ }
+
+ // reverse string
+ while (len) {
+ out(buf[--len], buffer, idx++, maxlen);
+ }
+
+ // append pad spaces up to given width
+ if (flags & FLAGS_LEFT) {
+ while (idx - start_idx < width) {
+ out(' ', buffer, idx++, maxlen);
+ }
+ }
+
+ return idx;
+}
+
+
+// internal itoa format
+static size_t _ntoa_format(out_fct_type out, char *buffer, size_t idx, size_t maxlen, char *buf, size_t len,
+ bool negative, unsigned int base, unsigned int prec, unsigned int width,
+ unsigned int flags) {
+ // pad leading zeros
+ if (!(flags & FLAGS_LEFT)) {
+ if (width && (flags & FLAGS_ZEROPAD) && (negative || (flags & (FLAGS_PLUS | FLAGS_SPACE)))) {
+ width--;
+ }
+ while ((len < prec) && (len < PICO_PRINTF_NTOA_BUFFER_SIZE)) {
+ buf[len++] = '0';
+ }
+ while ((flags & FLAGS_ZEROPAD) && (len < width) && (len < PICO_PRINTF_NTOA_BUFFER_SIZE)) {
+ buf[len++] = '0';
+ }
+ }
+
+ // handle hash
+ if (flags & FLAGS_HASH) {
+ if (!(flags & FLAGS_PRECISION) && len && ((len == prec) || (len == width))) {
+ len--;
+ if (len && (base == 16U)) {
+ len--;
+ }
+ }
+ if ((base == 16U) && !(flags & FLAGS_UPPERCASE) && (len < PICO_PRINTF_NTOA_BUFFER_SIZE)) {
+ buf[len++] = 'x';
+ } else if ((base == 16U) && (flags & FLAGS_UPPERCASE) && (len < PICO_PRINTF_NTOA_BUFFER_SIZE)) {
+ buf[len++] = 'X';
+ } else if ((base == 2U) && (len < PICO_PRINTF_NTOA_BUFFER_SIZE)) {
+ buf[len++] = 'b';
+ }
+ if (len < PICO_PRINTF_NTOA_BUFFER_SIZE) {
+ buf[len++] = '0';
+ }
+ }
+
+ if (len < PICO_PRINTF_NTOA_BUFFER_SIZE) {
+ if (negative) {
+ buf[len++] = '-';
+ } else if (flags & FLAGS_PLUS) {
+ buf[len++] = '+'; // ignore the space if the '+' exists
+ } else if (flags & FLAGS_SPACE) {
+ buf[len++] = ' ';
+ }
+ }
+
+ return _out_rev(out, buffer, idx, maxlen, buf, len, width, flags);
+}
+
+
+// internal itoa for 'long' type
+static size_t _ntoa_long(out_fct_type out, char *buffer, size_t idx, size_t maxlen, unsigned long value, bool negative,
+ unsigned long base, unsigned int prec, unsigned int width, unsigned int flags) {
+ char buf[PICO_PRINTF_NTOA_BUFFER_SIZE];
+ size_t len = 0U;
+
+ // no hash for 0 values
+ if (!value) {
+ flags &= ~FLAGS_HASH;
+ }
+
+ // write if precision != 0 and value is != 0
+ if (!(flags & FLAGS_PRECISION) || value) {
+ do {
+ const char digit = (char) (value % base);
+ buf[len++] = (char)(digit < 10 ? '0' + digit : (flags & FLAGS_UPPERCASE ? 'A' : 'a') + digit - 10);
+ value /= base;
+ } while (value && (len < PICO_PRINTF_NTOA_BUFFER_SIZE));
+ }
+
+ return _ntoa_format(out, buffer, idx, maxlen, buf, len, negative, (unsigned int) base, prec, width, flags);
+}
+
+
+// internal itoa for 'long long' type
+#if PICO_PRINTF_SUPPORT_LONG_LONG
+
+static size_t _ntoa_long_long(out_fct_type out, char *buffer, size_t idx, size_t maxlen, unsigned long long value,
+ bool negative, unsigned long long base, unsigned int prec, unsigned int width,
+ unsigned int flags) {
+ char buf[PICO_PRINTF_NTOA_BUFFER_SIZE];
+ size_t len = 0U;
+
+ // no hash for 0 values
+ if (!value) {
+ flags &= ~FLAGS_HASH;
+ }
+
+ // write if precision != 0 and value is != 0
+ if (!(flags & FLAGS_PRECISION) || value) {
+ do {
+ const char digit = (char) (value % base);
+ buf[len++] = (char)(digit < 10 ? '0' + digit : (flags & FLAGS_UPPERCASE ? 'A' : 'a') + digit - 10);
+ value /= base;
+ } while (value && (len < PICO_PRINTF_NTOA_BUFFER_SIZE));
+ }
+
+ return _ntoa_format(out, buffer, idx, maxlen, buf, len, negative, (unsigned int) base, prec, width, flags);
+}
+
+#endif // PICO_PRINTF_SUPPORT_LONG_LONG
+
+
+#if PICO_PRINTF_SUPPORT_FLOAT
+
+#if PICO_PRINTF_SUPPORT_EXPONENTIAL
+// forward declaration so that _ftoa can switch to exp notation for values > PICO_PRINTF_MAX_FLOAT
+static size_t _etoa(out_fct_type out, char *buffer, size_t idx, size_t maxlen, double value, unsigned int prec,
+ unsigned int width, unsigned int flags);
+#endif
+
+#define is_nan __builtin_isnan
+
+// internal ftoa for fixed decimal floating point
+static size_t _ftoa(out_fct_type out, char *buffer, size_t idx, size_t maxlen, double value, unsigned int prec,
+ unsigned int width, unsigned int flags) {
+ char buf[PICO_PRINTF_FTOA_BUFFER_SIZE];
+ size_t len = 0U;
+ double diff = 0.0;
+
+ // powers of 10
+ static const double pow10[] = {1, 10, 100, 1000, 10000, 100000, 1000000, 10000000, 100000000, 1000000000};
+
+ // test for special values
+ if (is_nan(value))
+ return _out_rev(out, buffer, idx, maxlen, "nan", 3, width, flags);
+ if (value < -DBL_MAX)
+ return _out_rev(out, buffer, idx, maxlen, "fni-", 4, width, flags);
+ if (value > DBL_MAX)
+ return _out_rev(out, buffer, idx, maxlen, (flags & FLAGS_PLUS) ? "fni+" : "fni", (flags & FLAGS_PLUS) ? 4U : 3U,
+ width, flags);
+
+ // test for very large values
+ // standard printf behavior is to print EVERY whole number digit -- which could be 100s of characters overflowing your buffers == bad
+ if ((value > PICO_PRINTF_MAX_FLOAT) || (value < -PICO_PRINTF_MAX_FLOAT)) {
+#if PICO_PRINTF_SUPPORT_EXPONENTIAL
+ return _etoa(out, buffer, idx, maxlen, value, prec, width, flags);
+#else
+ return 0U;
+#endif
+ }
+
+ // test for negative
+ bool negative = false;
+ if (value < 0) {
+ negative = true;
+ value = 0 - value;
+ }
+
+ // set default precision, if not set explicitly
+ if (!(flags & FLAGS_PRECISION)) {
+ prec = PICO_PRINTF_DEFAULT_FLOAT_PRECISION;
+ }
+ // limit precision to 9, cause a prec >= 10 can lead to overflow errors
+ while ((len < PICO_PRINTF_FTOA_BUFFER_SIZE) && (prec > 9U)) {
+ buf[len++] = '0';
+ prec--;
+ }
+
+ int whole = (int) value;
+ double tmp = (value - whole) * pow10[prec];
+ unsigned long frac = (unsigned long) tmp;
+ diff = tmp - frac;
+
+ if (diff > 0.5) {
+ ++frac;
+ // handle rollover, e.g. case 0.99 with prec 1 is 1.0
+ if (frac >= pow10[prec]) {
+ frac = 0;
+ ++whole;
+ }
+ } else if (diff < 0.5) {
+ } else if ((frac == 0U) || (frac & 1U)) {
+ // if halfway, round up if odd OR if last digit is 0
+ ++frac;
+ }
+
+ if (prec == 0U) {
+ diff = value - (double) whole;
+ if (!((diff < 0.5) || (diff > 0.5)) && (whole & 1)) {
+ // exactly 0.5 and ODD, then round up
+ // 1.5 -> 2, but 2.5 -> 2
+ ++whole;
+ }
+ } else {
+ unsigned int count = prec;
+ // now do fractional part, as an unsigned number
+ while (len < PICO_PRINTF_FTOA_BUFFER_SIZE) {
+ --count;
+ buf[len++] = (char) (48U + (frac % 10U));
+ if (!(frac /= 10U)) {
+ break;
+ }
+ }
+ // add extra 0s
+ while ((len < PICO_PRINTF_FTOA_BUFFER_SIZE) && (count-- > 0U)) {
+ buf[len++] = '0';
+ }
+ if (len < PICO_PRINTF_FTOA_BUFFER_SIZE) {
+ // add decimal
+ buf[len++] = '.';
+ }
+ }
+
+ // do whole part, number is reversed
+ while (len < PICO_PRINTF_FTOA_BUFFER_SIZE) {
+ buf[len++] = (char) (48 + (whole % 10));
+ if (!(whole /= 10)) {
+ break;
+ }
+ }
+
+ // pad leading zeros
+ if (!(flags & FLAGS_LEFT) && (flags & FLAGS_ZEROPAD)) {
+ if (width && (negative || (flags & (FLAGS_PLUS | FLAGS_SPACE)))) {
+ width--;
+ }
+ while ((len < width) && (len < PICO_PRINTF_FTOA_BUFFER_SIZE)) {
+ buf[len++] = '0';
+ }
+ }
+
+ if (len < PICO_PRINTF_FTOA_BUFFER_SIZE) {
+ if (negative) {
+ buf[len++] = '-';
+ } else if (flags & FLAGS_PLUS) {
+ buf[len++] = '+'; // ignore the space if the '+' exists
+ } else if (flags & FLAGS_SPACE) {
+ buf[len++] = ' ';
+ }
+ }
+
+ return _out_rev(out, buffer, idx, maxlen, buf, len, width, flags);
+}
+
+
+#if PICO_PRINTF_SUPPORT_EXPONENTIAL
+
+// internal ftoa variant for exponential floating-point type, contributed by Martijn Jasperse <m.jasperse@gmail.com>
+static size_t _etoa(out_fct_type out, char *buffer, size_t idx, size_t maxlen, double value, unsigned int prec,
+ unsigned int width, unsigned int flags) {
+ // check for NaN and special values
+ if (is_nan(value) || (value > DBL_MAX) || (value < -DBL_MAX)) {
+ return _ftoa(out, buffer, idx, maxlen, value, prec, width, flags);
+ }
+
+ // determine the sign
+ const bool negative = value < 0;
+ if (negative) {
+ value = -value;
+ }
+
+ // default precision
+ if (!(flags & FLAGS_PRECISION)) {
+ prec = PICO_PRINTF_DEFAULT_FLOAT_PRECISION;
+ }
+
+ // determine the decimal exponent
+ // based on the algorithm by David Gay (https://www.ampl.com/netlib/fp/dtoa.c)
+ union {
+ uint64_t U;
+ double F;
+ } conv;
+
+ conv.F = value;
+ int expval;
+ if (conv.U) {
+ int exp2 = (int) ((conv.U >> 52U) & 0x07FFU) - 1023; // effectively log2
+ conv.U = (conv.U & ((1ULL << 52U) - 1U)) | (1023ULL << 52U); // drop the exponent so conv.F is now in [1,2)
+ // now approximate log10 from the log2 integer part and an expansion of ln around 1.5
+ expval = (int) (0.1760912590558 + exp2 * 0.301029995663981 + (conv.F - 1.5) * 0.289529654602168);
+ // now we want to compute 10^expval but we want to be sure it won't overflow
+ exp2 = (int) (expval * 3.321928094887362 + 0.5);
+ const double z = expval * 2.302585092994046 - exp2 * 0.6931471805599453;
+ const double z2 = z * z;
+ conv.U = (uint64_t) (exp2 + 1023) << 52U;
+ // compute exp(z) using continued fractions, see https://en.wikipedia.org/wiki/Exponential_function#Continued_fractions_for_ex
+ conv.F *= 1 + 2 * z / (2 - z + (z2 / (6 + (z2 / (10 + z2 / 14)))));
+ // correct for rounding errors
+ if (value < conv.F) {
+ expval--;
+ conv.F /= 10;
+ }
+ } else {
+ expval = 0;
+ }
+
+ // the exponent format is "%+03d" and largest value is "307", so set aside 4-5 characters
+ unsigned int minwidth = ((expval < 100) && (expval > -100)) ? 4U : 5U;
+
+ // in "%g" mode, "prec" is the number of *significant figures* not decimals
+ if (flags & FLAGS_ADAPT_EXP) {
+ // do we want to fall-back to "%f" mode?
+ if ((conv.U == 0) || ((value >= 1e-4) && (value < 1e6))) {
+ if ((int) prec > expval) {
+ prec = (unsigned) ((int) prec - expval - 1);
+ } else {
+ prec = 0;
+ }
+ flags |= FLAGS_PRECISION; // make sure _ftoa respects precision
+ // no characters in exponent
+ minwidth = 0U;
+ expval = 0;
+ } else {
+ // we use one sigfig for the whole part
+ if ((prec > 0) && (flags & FLAGS_PRECISION)) {
+ --prec;
+ }
+ }
+ }
+
+ // will everything fit?
+ unsigned int fwidth = width;
+ if (width > minwidth) {
+ // we didn't fall-back so subtract the characters required for the exponent
+ fwidth -= minwidth;
+ } else {
+ // not enough characters, so go back to default sizing
+ fwidth = 0U;
+ }
+ if ((flags & FLAGS_LEFT) && minwidth) {
+ // if we're padding on the right, DON'T pad the floating part
+ fwidth = 0U;
+ }
+
+ // rescale the float value
+ if (expval) {
+ value /= conv.F;
+ }
+
+ // output the floating part
+ const size_t start_idx = idx;
+ idx = _ftoa(out, buffer, idx, maxlen, negative ? -value : value, prec, fwidth, flags & ~FLAGS_ADAPT_EXP);
+
+ // output the exponent part
+ if (minwidth) {
+ // output the exponential symbol
+ out((flags & FLAGS_UPPERCASE) ? 'E' : 'e', buffer, idx++, maxlen);
+ // output the exponent value
+ idx = _ntoa_long(out, buffer, idx, maxlen, (uint)((expval < 0) ? -expval : expval), expval < 0, 10, 0, minwidth - 1,
+ FLAGS_ZEROPAD | FLAGS_PLUS);
+ // might need to right-pad spaces
+ if (flags & FLAGS_LEFT) {
+ while (idx - start_idx < width) out(' ', buffer, idx++, maxlen);
+ }
+ }
+ return idx;
+}
+
+#endif // PICO_PRINTF_SUPPORT_EXPONENTIAL
+#endif // PICO_PRINTF_SUPPORT_FLOAT
+
+// internal vsnprintf
+static int _vsnprintf(out_fct_type out, char *buffer, const size_t maxlen, const char *format, va_list va) {
+#if !PICO_PRINTF_ALWAYS_INCLUDED
+ lazy_vsnprintf = _vsnprintf;
+#endif
+ unsigned int flags, width, precision, n;
+ size_t idx = 0U;
+
+ if (!buffer) {
+ // use null output function
+ out = _out_null;
+ }
+
+ while (*format) {
+ // format specifier? %[flags][width][.precision][length]
+ if (*format != '%') {
+ // no
+ out(*format, buffer, idx++, maxlen);
+ format++;
+ continue;
+ } else {
+ // yes, evaluate it
+ format++;
+ }
+
+ // evaluate flags
+ flags = 0U;
+ do {
+ switch (*format) {
+ case '0':
+ flags |= FLAGS_ZEROPAD;
+ format++;
+ n = 1U;
+ break;
+ case '-':
+ flags |= FLAGS_LEFT;
+ format++;
+ n = 1U;
+ break;
+ case '+':
+ flags |= FLAGS_PLUS;
+ format++;
+ n = 1U;
+ break;
+ case ' ':
+ flags |= FLAGS_SPACE;
+ format++;
+ n = 1U;
+ break;
+ case '#':
+ flags |= FLAGS_HASH;
+ format++;
+ n = 1U;
+ break;
+ default :
+ n = 0U;
+ break;
+ }
+ } while (n);
+
+ // evaluate width field
+ width = 0U;
+ if (_is_digit(*format)) {
+ width = _atoi(&format);
+ } else if (*format == '*') {
+ const int w = va_arg(va, int);
+ if (w < 0) {
+ flags |= FLAGS_LEFT; // reverse padding
+ width = (unsigned int) -w;
+ } else {
+ width = (unsigned int) w;
+ }
+ format++;
+ }
+
+ // evaluate precision field
+ precision = 0U;
+ if (*format == '.') {
+ flags |= FLAGS_PRECISION;
+ format++;
+ if (_is_digit(*format)) {
+ precision = _atoi(&format);
+ } else if (*format == '*') {
+ const int prec = (int) va_arg(va, int);
+ precision = prec > 0 ? (unsigned int) prec : 0U;
+ format++;
+ }
+ }
+
+ // evaluate length field
+ switch (*format) {
+ case 'l' :
+ flags |= FLAGS_LONG;
+ format++;
+ if (*format == 'l') {
+ flags |= FLAGS_LONG_LONG;
+ format++;
+ }
+ break;
+ case 'h' :
+ flags |= FLAGS_SHORT;
+ format++;
+ if (*format == 'h') {
+ flags |= FLAGS_CHAR;
+ format++;
+ }
+ break;
+#if PICO_PRINTF_SUPPORT_PTRDIFF_T
+ case 't' :
+ flags |= (sizeof(ptrdiff_t) == sizeof(long) ? FLAGS_LONG : FLAGS_LONG_LONG);
+ format++;
+ break;
+#endif
+ case 'j' :
+ flags |= (sizeof(intmax_t) == sizeof(long) ? FLAGS_LONG : FLAGS_LONG_LONG);
+ format++;
+ break;
+ case 'z' :
+ flags |= (sizeof(size_t) == sizeof(long) ? FLAGS_LONG : FLAGS_LONG_LONG);
+ format++;
+ break;
+ default :
+ break;
+ }
+
+ // evaluate specifier
+ switch (*format) {
+ case 'd' :
+ case 'i' :
+ case 'u' :
+ case 'x' :
+ case 'X' :
+ case 'o' :
+ case 'b' : {
+ // set the base
+ unsigned int base;
+ if (*format == 'x' || *format == 'X') {
+ base = 16U;
+ } else if (*format == 'o') {
+ base = 8U;
+ } else if (*format == 'b') {
+ base = 2U;
+ } else {
+ base = 10U;
+ flags &= ~FLAGS_HASH; // no hash for dec format
+ }
+ // uppercase
+ if (*format == 'X') {
+ flags |= FLAGS_UPPERCASE;
+ }
+
+ // no plus or space flag for u, x, X, o, b
+ if ((*format != 'i') && (*format != 'd')) {
+ flags &= ~(FLAGS_PLUS | FLAGS_SPACE);
+ }
+
+ // ignore '0' flag when precision is given
+ if (flags & FLAGS_PRECISION) {
+ flags &= ~FLAGS_ZEROPAD;
+ }
+
+ // convert the integer
+ if ((*format == 'i') || (*format == 'd')) {
+ // signed
+ if (flags & FLAGS_LONG_LONG) {
+#if PICO_PRINTF_SUPPORT_LONG_LONG
+ const long long value = va_arg(va, long long);
+ idx = _ntoa_long_long(out, buffer, idx, maxlen,
+ (unsigned long long) (value > 0 ? value : 0 - value), value < 0, base,
+ precision, width, flags);
+#endif
+ } else if (flags & FLAGS_LONG) {
+ const long value = va_arg(va, long);
+ idx = _ntoa_long(out, buffer, idx, maxlen, (unsigned long) (value > 0 ? value : 0 - value),
+ value < 0, base, precision, width, flags);
+ } else {
+ const int value = (flags & FLAGS_CHAR) ? (char) va_arg(va, int) : (flags & FLAGS_SHORT)
+ ? (short int) va_arg(va, int)
+ : va_arg(va, int);
+ idx = _ntoa_long(out, buffer, idx, maxlen, (unsigned int) (value > 0 ? value : 0 - value),
+ value < 0, base, precision, width, flags);
+ }
+ } else {
+ // unsigned
+ if (flags & FLAGS_LONG_LONG) {
+#if PICO_PRINTF_SUPPORT_LONG_LONG
+ idx = _ntoa_long_long(out, buffer, idx, maxlen, va_arg(va, unsigned long long), false, base,
+ precision, width, flags);
+#endif
+ } else if (flags & FLAGS_LONG) {
+ idx = _ntoa_long(out, buffer, idx, maxlen, va_arg(va, unsigned long), false, base, precision,
+ width, flags);
+ } else {
+ const unsigned int value = (flags & FLAGS_CHAR) ? (unsigned char) va_arg(va, unsigned int)
+ : (flags & FLAGS_SHORT)
+ ? (unsigned short int) va_arg(va,
+ unsigned int)
+ : va_arg(va, unsigned int);
+ idx = _ntoa_long(out, buffer, idx, maxlen, value, false, base, precision, width, flags);
+ }
+ }
+ format++;
+ break;
+ }
+ case 'f' :
+ case 'F' :
+#if PICO_PRINTF_SUPPORT_FLOAT
+ if (*format == 'F') flags |= FLAGS_UPPERCASE;
+ idx = _ftoa(out, buffer, idx, maxlen, va_arg(va, double), precision, width, flags);
+#else
+ for(int i=0;i<2;i++) out('?', buffer, idx++, maxlen);
+ va_arg(va, double);
+#endif
+ format++;
+ break;
+ case 'e':
+ case 'E':
+ case 'g':
+ case 'G':
+#if PICO_PRINTF_SUPPORT_FLOAT && PICO_PRINTF_SUPPORT_EXPONENTIAL
+ if ((*format == 'g') || (*format == 'G')) flags |= FLAGS_ADAPT_EXP;
+ if ((*format == 'E') || (*format == 'G')) flags |= FLAGS_UPPERCASE;
+ idx = _etoa(out, buffer, idx, maxlen, va_arg(va, double), precision, width, flags);
+#else
+ for(int i=0;i<2;i++) out('?', buffer, idx++, maxlen);
+ va_arg(va, double);
+#endif
+ format++;
+ break;
+ case 'c' : {
+ unsigned int l = 1U;
+ // pre padding
+ if (!(flags & FLAGS_LEFT)) {
+ while (l++ < width) {
+ out(' ', buffer, idx++, maxlen);
+ }
+ }
+ // char output
+ out((char) va_arg(va, int), buffer, idx++, maxlen);
+ // post padding
+ if (flags & FLAGS_LEFT) {
+ while (l++ < width) {
+ out(' ', buffer, idx++, maxlen);
+ }
+ }
+ format++;
+ break;
+ }
+
+ case 's' : {
+ const char *p = va_arg(va, char*);
+ unsigned int l = _strnlen_s(p, precision ? precision : (size_t) -1);
+ // pre padding
+ if (flags & FLAGS_PRECISION) {
+ l = (l < precision ? l : precision);
+ }
+ if (!(flags & FLAGS_LEFT)) {
+ while (l++ < width) {
+ out(' ', buffer, idx++, maxlen);
+ }
+ }
+ // string output
+ while ((*p != 0) && (!(flags & FLAGS_PRECISION) || precision--)) {
+ out(*(p++), buffer, idx++, maxlen);
+ }
+ // post padding
+ if (flags & FLAGS_LEFT) {
+ while (l++ < width) {
+ out(' ', buffer, idx++, maxlen);
+ }
+ }
+ format++;
+ break;
+ }
+
+ case 'p' : {
+ width = sizeof(void *) * 2U;
+ flags |= FLAGS_ZEROPAD | FLAGS_UPPERCASE;
+#if PICO_PRINTF_SUPPORT_LONG_LONG
+ const bool is_ll = sizeof(uintptr_t) == sizeof(long long);
+ if (is_ll) {
+ idx = _ntoa_long_long(out, buffer, idx, maxlen, (uintptr_t) va_arg(va, void*), false, 16U,
+ precision, width, flags);
+ } else {
+#endif
+ idx = _ntoa_long(out, buffer, idx, maxlen, (unsigned long) ((uintptr_t) va_arg(va, void*)), false,
+ 16U, precision, width, flags);
+#if PICO_PRINTF_SUPPORT_LONG_LONG
+ }
+#endif
+ format++;
+ break;
+ }
+
+ case '%' :
+ out('%', buffer, idx++, maxlen);
+ format++;
+ break;
+
+ default :
+ out(*format, buffer, idx++, maxlen);
+ format++;
+ break;
+ }
+ }
+
+ // termination
+ out((char) 0, buffer, idx < maxlen ? idx : maxlen - 1U, maxlen);
+
+ // return written chars without terminating \0
+ return (int) idx;
+}
+
+
+///////////////////////////////////////////////////////////////////////////////
+
+int WRAPPER_FUNC(sprintf)(char *buffer, const char *format, ...) {
+ va_list va;
+ va_start(va, format);
+ const int ret = _vsnprintf(_out_buffer, buffer, (size_t) -1, format, va);
+ va_end(va);
+ return ret;
+}
+
+int WRAPPER_FUNC(snprintf)(char *buffer, size_t count, const char *format, ...) {
+ va_list va;
+ va_start(va, format);
+ const int ret = _vsnprintf(_out_buffer, buffer, count, format, va);
+ va_end(va);
+ return ret;
+}
+
+int WRAPPER_FUNC(vsnprintf)(char *buffer, size_t count, const char *format, va_list va) {
+ return _vsnprintf(_out_buffer, buffer, count, format, va);
+}
+
+int vfctprintf(void (*out)(char character, void *arg), void *arg, const char *format, va_list va) {
+ const out_fct_wrap_type out_fct_wrap = {out, arg};
+ return _vsnprintf(_out_fct, (char *) (uintptr_t) &out_fct_wrap, (size_t) -1, format, va);
+}
+
+#if LIB_PICO_PRINTF_PICO
+#if !PICO_PRINTF_ALWAYS_INCLUDED
+bool weak_raw_printf(const char *fmt, ...) {
+ va_list va;
+ va_start(va, fmt);
+ bool rc = weak_raw_vprintf(fmt, va);
+ va_end(va);
+ return rc;
+}
+
+bool weak_raw_vprintf(const char *fmt, va_list args) {
+ if (lazy_vsnprintf) {
+ char buffer[1];
+ lazy_vsnprintf(_out_char, buffer, (size_t) -1, fmt, args);
+ return true;
+ } else {
+ puts(fmt);
+ return false;
+ }
+}
+#endif
+#endif
diff --git a/src/rp2_common/pico_printf/printf_none.S b/src/rp2_common/pico_printf/printf_none.S
new file mode 100644
index 0000000..adc00ee
--- /dev/null
+++ b/src/rp2_common/pico_printf/printf_none.S
@@ -0,0 +1,23 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "pico/asm_helper.S"
+#include "pico/bootrom/sf_table.h"
+
+.syntax unified
+.cpu cortex-m0plus
+.thumb
+
+wrapper_func sprintf
+wrapper_func snprintf
+wrapper_func vsnprintf
+regular_func printf_none_assert
+ push {lr} // keep stack trace sane
+ ldr r0, =str
+ bl panic
+
+str:
+ .asciz "printf support is disabled"
\ No newline at end of file
diff --git a/src/rp2_common/pico_runtime/CMakeLists.txt b/src/rp2_common/pico_runtime/CMakeLists.txt
new file mode 100644
index 0000000..3b6cc18
--- /dev/null
+++ b/src/rp2_common/pico_runtime/CMakeLists.txt
@@ -0,0 +1,44 @@
+pico_add_impl_library(pico_runtime)
+
+target_sources(pico_runtime INTERFACE
+ ${CMAKE_CURRENT_LIST_DIR}/runtime.c
+)
+
+target_include_directories(pico_runtime INTERFACE ${CMAKE_CURRENT_LIST_DIR}/include)
+
+target_link_libraries(pico_runtime INTERFACE
+ hardware_uart
+ hardware_clocks
+ hardware_irq
+ pico_printf
+ pico_sync
+ )
+
+if (TARGET pico_bit_ops)
+ target_link_libraries(pico_runtime INTERFACE pico_bit_ops)
+endif()
+if (TARGET pico_divider)
+ target_link_libraries(pico_runtime INTERFACE pico_divider)
+endif()
+if (TARGET pico_double)
+ target_link_libraries(pico_runtime INTERFACE pico_double)
+endif()
+if (TARGET pico_int64_ops)
+ target_link_libraries(pico_runtime INTERFACE pico_int64_ops)
+endif()
+if (TARGET pico_float)
+ target_link_libraries(pico_runtime INTERFACE pico_float)
+endif()
+if (TARGET pico_malloc)
+ target_link_libraries(pico_runtime INTERFACE pico_malloc)
+endif()
+if (TARGET pico_mem_ops)
+ target_link_libraries(pico_runtime INTERFACE pico_mem_ops)
+endif()
+if (TARGET pico_standard_link)
+ target_link_libraries(pico_runtime INTERFACE pico_standard_link)
+endif()
+
+# todo is this correct/needed?
+target_link_options(pico_runtime INTERFACE "--specs=nosys.specs")
+
diff --git a/src/rp2_common/pico_runtime/include/pico/runtime.h b/src/rp2_common/pico_runtime/include/pico/runtime.h
new file mode 100644
index 0000000..752ec6c
--- /dev/null
+++ b/src/rp2_common/pico_runtime/include/pico/runtime.h
@@ -0,0 +1,26 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PICO_RUNTIME_H
+#define _PICO_RUNTIME_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** \file runtime.h
+* \defgroup pico_runtime pico_runtime
+* Aggregate runtime support including @ref pico_bit_ops, @ref pico_divider, @ref pico_double, @ref pico_int64_ops, @ref pico_float, @ref pico_malloc, @ref pico_mem_ops and @ref pico_standard_link
+*/
+
+
+void runtime_install_stack_guard(void *stack_bottom);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
\ No newline at end of file
diff --git a/src/rp2_common/pico_runtime/runtime.c b/src/rp2_common/pico_runtime/runtime.c
new file mode 100644
index 0000000..575e23e
--- /dev/null
+++ b/src/rp2_common/pico_runtime/runtime.c
@@ -0,0 +1,289 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdio.h>
+#include <stdarg.h>
+#include "pico.h"
+
+#include "hardware/regs/m0plus.h"
+#include "hardware/regs/resets.h"
+#include "hardware/structs/mpu.h"
+#include "hardware/structs/scb.h"
+#include "hardware/structs/padsbank0.h"
+
+#include "hardware/clocks.h"
+#include "hardware/irq.h"
+#include "hardware/resets.h"
+
+#include "pico/mutex.h"
+#include "pico/time.h"
+
+#if LIB_PICO_PRINTF_PICO
+#include "pico/printf.h"
+#else
+#define weak_raw_printf printf
+#define weak_raw_vprintf vprintf
+#endif
+
+#if PICO_ENTER_USB_BOOT_ON_EXIT
+#include "pico/bootrom.h"
+#endif
+
+extern char __StackLimit; /* Set by linker. */
+
+uint32_t __attribute__((section(".ram_vector_table"))) ram_vector_table[48];
+
+// this is called for each thread since they have their own MPU
+void runtime_install_stack_guard(void *stack_bottom) {
+ // this is called b4 runtime_init is complete, so beware printf or assert
+
+ // make sure no one is using the MPU yet
+ if (mpu_hw->ctrl) {
+ // Note that it would be tempting to change this to a panic, but it happens so early, printing is not a good idea
+ __breakpoint();
+ }
+
+ uintptr_t addr = (uintptr_t) stack_bottom;
+ // the minimum we can protect is 32 bytes on a 32 byte boundary, so round up which will
+ // just shorten the valid stack range a tad
+ addr = (addr + 31u) & ~31u;
+
+ // mask is 1 bit per 32 bytes of the 256 byte range... clear the bit for the segment we want
+ uint32_t subregion_select = 0xffu ^ (1u << ((addr >> 5u) & 7u));
+ mpu_hw->ctrl = 5; // enable mpu with background default map
+ mpu_hw->rbar = (addr & (uint)~0xff) | 0x8 | 0;
+ mpu_hw->rasr = 1 // enable region
+ | (0x7 << 1) // size 2^(7 + 1) = 256
+ | (subregion_select << 8)
+ | 0x10000000; // XN = disable instruction fetch; no other bits means no permissions
+}
+
+void runtime_init(void) {
+ // Reset all peripherals to put system into a known state,
+ // - except for QSPI pads and the XIP IO bank, as this is fatal if running from flash
+ // - and the PLLs, as this is fatal if clock muxing has not been reset on this boot
+ // - and USB, syscfg, as this disturbs USB-to-SWD on core 1
+ reset_block(~(
+ RESETS_RESET_IO_QSPI_BITS |
+ RESETS_RESET_PADS_QSPI_BITS |
+ RESETS_RESET_PLL_USB_BITS |
+ RESETS_RESET_USBCTRL_BITS |
+ RESETS_RESET_SYSCFG_BITS |
+ RESETS_RESET_PLL_SYS_BITS
+ ));
+
+ // Remove reset from peripherals which are clocked only by clk_sys and
+ // clk_ref. Other peripherals stay in reset until we've configured clocks.
+ unreset_block_wait(RESETS_RESET_BITS & ~(
+ RESETS_RESET_ADC_BITS |
+ RESETS_RESET_RTC_BITS |
+ RESETS_RESET_SPI0_BITS |
+ RESETS_RESET_SPI1_BITS |
+ RESETS_RESET_UART0_BITS |
+ RESETS_RESET_UART1_BITS |
+ RESETS_RESET_USBCTRL_BITS
+ ));
+
+ // pre-init runs really early since we need it even for memcpy and divide!
+ // (basically anything in aeabi that uses bootrom)
+
+ // Start and end points of the constructor list,
+ // defined by the linker script.
+ extern void (*__preinit_array_start)(void);
+ extern void (*__preinit_array_end)(void);
+
+ // Call each function in the list.
+ // We have to take the address of the symbols, as __preinit_array_start *is*
+ // the first function pointer, not the address of it.
+ for (void (**p)(void) = &__preinit_array_start; p < &__preinit_array_end; ++p) {
+ (*p)();
+ }
+
+ // After calling preinit we have enough runtime to do the exciting maths
+ // in clocks_init
+ clocks_init();
+
+ // Peripheral clocks should now all be running
+ unreset_block_wait(RESETS_RESET_BITS);
+
+#if !PICO_IE_26_29_UNCHANGED_ON_RESET
+ // after resetting BANK0 we should disable IE on 26-29
+ hw_clear_alias(padsbank0_hw)->io[26] = hw_clear_alias(padsbank0_hw)->io[27] =
+ hw_clear_alias(padsbank0_hw)->io[28] = hw_clear_alias(padsbank0_hw)->io[29] = PADS_BANK0_GPIO0_IE_BITS;
+#endif
+
+ // this is an array of either mutex_t or recursive_mutex_t (i.e. not necessarily the same size)
+ // however each starts with a lock_core_t, and the spin_lock is initialized to address 1 for a recursive
+ // spinlock and 0 for a regular one.
+
+ static_assert(!(sizeof(mutex_t)&3), "");
+ static_assert(!(sizeof(recursive_mutex_t)&3), "");
+ static_assert(!offsetof(mutex_t, core), "");
+ static_assert(!offsetof(recursive_mutex_t, core), "");
+ extern lock_core_t __mutex_array_start;
+ extern lock_core_t __mutex_array_end;
+
+ for (lock_core_t *l = &__mutex_array_start; l < &__mutex_array_end; ) {
+ if (l->spin_lock) {
+ assert(1 == (uintptr_t)l->spin_lock); // indicator for a recursive mutex
+ recursive_mutex_t *rm = (recursive_mutex_t *)l;
+ recursive_mutex_init(rm);
+ l = &rm[1].core; // next
+ } else {
+ mutex_t *m = (mutex_t *)l;
+ mutex_init(m);
+ l = &m[1].core; // next
+ }
+ }
+
+#if !(PICO_NO_RAM_VECTOR_TABLE || PICO_NO_FLASH)
+ __builtin_memcpy(ram_vector_table, (uint32_t *) scb_hw->vtor, sizeof(ram_vector_table));
+ scb_hw->vtor = (uintptr_t) ram_vector_table;
+#endif
+
+#ifndef NDEBUG
+ if (__get_current_exception()) {
+ // crap; started in exception handler
+ __asm ("bkpt #0");
+ }
+#endif
+
+#if PICO_USE_STACK_GUARDS
+ // install core0 stack guard
+ extern char __StackBottom;
+ runtime_install_stack_guard(&__StackBottom);
+#endif
+
+ spin_locks_reset();
+ irq_init_priorities();
+ alarm_pool_init_default();
+
+ // Start and end points of the constructor list,
+ // defined by the linker script.
+ extern void (*__init_array_start)(void);
+ extern void (*__init_array_end)(void);
+
+ // Call each function in the list.
+ // We have to take the address of the symbols, as __init_array_start *is*
+ // the first function pointer, not the address of it.
+ for (void (**p)(void) = &__init_array_start; p < &__init_array_end; ++p) {
+ (*p)();
+ }
+
+}
+
+void _exit(__unused int status) {
+#if PICO_ENTER_USB_BOOT_ON_EXIT
+ reset_usb_boot(0,0);
+#else
+ while (1) {
+ __breakpoint();
+ }
+#endif
+}
+
+void *_sbrk(int incr) {
+ extern char end; /* Set by linker. */
+ static char *heap_end;
+ char *prev_heap_end;
+
+ if (heap_end == 0)
+ heap_end = &end;
+
+ prev_heap_end = heap_end;
+ char *next_heap_end = heap_end + incr;
+
+ if (__builtin_expect(next_heap_end > (&__StackLimit), false)) {
+#if PICO_USE_OPTIMISTIC_SBRK
+ if (heap_end == &__StackLimit) {
+// errno = ENOMEM;
+ return (char *) -1;
+ }
+ next_heap_end = &__StackLimit;
+#else
+ return (char *) -1;
+#endif
+ }
+
+ heap_end = next_heap_end;
+ return (void *) prev_heap_end;
+}
+
+// exit is not useful... no desire to pull in __call_exitprocs
+void exit(int status) {
+ _exit(status);
+}
+
+// incorrect warning from GCC 6
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wsuggest-attribute=format"
+void __assert_func(const char *file, int line, const char *func, const char *failedexpr) {
+ weak_raw_printf("assertion \"%s\" failed: file \"%s\", line %d%s%s\n",
+ failedexpr, file, line, func ? ", function: " : "",
+ func ? func : "");
+
+ _exit(1);
+}
+
+#pragma GCC diagnostic pop
+
+void __attribute__((noreturn)) panic_unsupported() {
+ panic("not supported");
+}
+
+// PICO_CONFIG: PICO_PANIC_FUNCTION, Name of a function to use in place of the stock panic function or empty string to simply breakpoint on panic, group=pico_runtime
+// note the default is not "panic" it is undefined
+#ifdef PICO_PANIC_FUNCTION
+#define PICO_PANIC_FUNCTION_EMPTY (__CONCAT(PICO_PANIC_FUNCTION, 1) == 1)
+#if !PICO_PANIC_FUNCTION_EMPTY
+extern void __attribute__((noreturn)) __printflike(1, 0) PICO_PANIC_FUNCTION(__unused const char *fmt, ...);
+#endif
+// Use a forwarding method here as it is a little simpler than renaming the symbol as it is used from assembler
+void __attribute__((naked, noreturn)) __printflike(1, 0) panic(__unused const char *fmt, ...) {
+ // if you get an undefined reference here, you didn't define your PICO_PANIC_FUNCTION!
+ __asm (
+ "push {lr}\n"
+#if !PICO_PANIC_FUNCTION_EMPTY
+ "bl " __XSTRING(PICO_PANIC_FUNCTION) "\n"
+#endif
+ "bkpt #0\n"
+ "1: b 1b\n" // loop for ever as we are no return
+ :
+ :
+ :
+ );
+}
+#else
+// todo consider making this try harder to output if we panic early
+// right now, print mutex may be uninitialised (in which case it deadlocks - although after printing "PANIC")
+// more importantly there may be no stdout/UART initialized yet
+// todo we may want to think about where we print panic messages to; writing to USB appears to work
+// though it doesn't seem like we can expect it to... fine for now
+void __attribute__((noreturn)) __printflike(1, 0) panic(const char *fmt, ...) {
+ puts("\n*** PANIC ***\n");
+ if (fmt) {
+#if LIB_PICO_PRINTF_NONE
+ puts(fmt);
+#else
+ va_list args;
+ va_start(args, fmt);
+#if PICO_PRINTF_ALWAYS_INCLUDED
+ vprintf(fmt, args);
+#else
+ weak_raw_vprintf(fmt, args);
+#endif
+ va_end(args);
+ puts("\n");
+#endif
+ }
+
+ _exit(1);
+}
+#endif
+
+void hard_assertion_failure(void) {
+ panic("Hard assert");
+}
diff --git a/src/rp2_common/pico_standard_link/CMakeLists.txt b/src/rp2_common/pico_standard_link/CMakeLists.txt
new file mode 100644
index 0000000..a730540
--- /dev/null
+++ b/src/rp2_common/pico_standard_link/CMakeLists.txt
@@ -0,0 +1,97 @@
+if (NOT TARGET pico_standard_link)
+ pico_add_impl_library(pico_standard_link)
+
+ target_sources(pico_standard_link INTERFACE
+ ${CMAKE_CURRENT_LIST_DIR}/crt0.S
+ ${CMAKE_CURRENT_LIST_DIR}/new_delete.cpp
+ ${CMAKE_CURRENT_LIST_DIR}/binary_info.c
+ )
+
+ pico_add_map_output(pico_standard_link)
+
+ # todo revisit when we do Clang
+ if (CMAKE_C_COMPILER_ID STREQUAL "Clang")
+ target_link_options(pico_standard_link INTERFACE "LINKER:-nostdlib")
+ endif ()
+
+ target_link_libraries(pico_standard_link INTERFACE hardware_regs boot_stage2_headers pico_bootrom pico_binary_info pico_cxx_options)
+
+ function(pico_add_link_depend TARGET dependency)
+ get_target_property(target_type ${TARGET} TYPE)
+ if (${target_type} STREQUAL "INTERFACE_LIBRARY")
+ set(PROP "INTERFACE_LINK_DEPENDS")
+ else()
+ set(PROP "LINK_DEPENDS")
+ endif()
+ get_target_property(_LINK_DEPENDS ${TARGET} ${PROP})
+ if (NOT _LINK_DEPENDS)
+ set(_LINK_DEPENDS ${dependency})
+ else()
+ list(APPEND _LINK_DEPENDS ${dependency})
+ endif()
+ set_target_properties(${TARGET} PROPERTIES ${PROP} "${_LINK_DEPENDS}")
+ endfunction()
+
+ # need this because cmake does not appear to have a way to override an INTERFACE variable
+ function(pico_set_linker_script TARGET LDSCRIPT)
+ set_target_properties(${TARGET} PROPERTIES PICO_TARGET_LINKER_SCRIPT ${LDSCRIPT})
+ pico_add_link_depend(${TARGET} ${LDSCRIPT})
+ endfunction()
+
+ function(pico_set_binary_type TARGET TYPE)
+ set_target_properties(${TARGET} PROPERTIES PICO_TARGET_BINARY_TYPE ${TYPE})
+ endfunction()
+
+ if (PICO_NO_FLASH)
+ set(PICO_DEFAULT_BINARY_TYPE no_flash)
+ elseif (PICO_USE_BLOCKED_RAM)
+ set(PICO_DEFAULT_BINARY_TYPE blocked_ram)
+ elseif (PICO_COPY_TO_RAM)
+ set(PICO_DEFAULT_BINARY_TYPE copy_to_ram)
+ else()
+ set(PICO_DEFAULT_BINARY_TYPE default)
+ endif()
+
+ # LINKER script will be PICO_TARGET_LINKER_SCRIPT if set on target, or ${CMAKE_CURRENT_LIST_DIR}/memmap_foo.ld
+ # if PICO_TARGET_BINARY_TYPE is set to foo on the target, otherwise ${CMAKE_CURRENT_LIST_DIR}/memmap_${PICO_DEFAULT_BINARY_TYPE).ld
+ target_link_options(pico_standard_link INTERFACE
+ "LINKER:--script=$<IF:$<BOOL:$<TARGET_PROPERTY:PICO_TARGET_LINKER_SCRIPT>>,$<TARGET_PROPERTY:PICO_TARGET_LINKER_SCRIPT>,${CMAKE_CURRENT_LIST_DIR}/memmap_$<IF:$<STREQUAL:$<TARGET_PROPERTY:PICO_TARGET_BINARY_TYPE>,>,${PICO_DEFAULT_BINARY_TYPE},$<TARGET_PROPERTY:PICO_TARGET_BINARY_TYPE>>.ld>"
+ )
+
+ # PICO_NO_FLASH will be set based on PICO_TARGET_BUILD_TYPE target property being equal to no_flash if set, otherwise to the value of the PICO_NO_FLASH cmake variable unless PICO_TARGET_TYPE is set to something else
+ # PICO_BUILD_DEFINE: PICO_NO_FLASH, whether this is a 'no_flash' build, type=bool, default=0, but dependent on CMake options, group=pico_standard_link
+ target_compile_definitions(pico_standard_link INTERFACE PICO_NO_FLASH=$<IF:$<STREQUAL:$<TARGET_PROPERTY:PICO_TARGET_BINARY_TYPE>,no_flash>,1,$<AND:$<BOOL:${PICO_NO_FLASH}>,$<STREQUAL:,$<TARGET_PROPERTY:PICO_TARGET_BINARY_TYPE>>>>)
+ # PICO_USE_BLOCKED_RAM will be set based on PICO_TARGET_BUILD_TYPE target property being equal to use_blocked_ram if set, otherwise to the value of the PICO_USE_BLOCKED_RAM cmake variable unless PICO_TARGET_TYPE is set to something else
+ # PICO_BUILD_DEFINE: PICO_USE_BLOCKS_RAM, whether this is a 'blocked_ram' build, type=bool, default=0, but dependent on CMake options, group=pico_standard_link
+ target_compile_definitions(pico_standard_link INTERFACE PICO_USE_BLOCKED_RAM=$<IF:$<STREQUAL:$<TARGET_PROPERTY:PICO_TARGET_BINARY_TYPE>,use_blocked_ram>,1,$<AND:$<BOOL:${PICO_USE_BLOCKED_RAM}>,$<STREQUAL:,$<TARGET_PROPERTY:PICO_TARGET_BINARY_TYPE>>>>)
+ # PICO_COPY_TO_RAM will be set based on PICO_TARGET_BUILD_TYPE target property being equal to copy_to_ram if set, otherwise to the value of the PICO_COPY_TO_RAM cmake variable unless PICO_TARGET_TYPE is set to something else
+ # PICO_BUILD_DEFINE: PICO_COPY_TO_RAM, whether this is a 'copy_to_ram' build, type=bool, default=0, but dependent on CMake options, group=pico_standard_link
+ target_compile_definitions(pico_standard_link INTERFACE PICO_COPY_TO_RAM=$<IF:$<STREQUAL:$<TARGET_PROPERTY:PICO_TARGET_BINARY_TYPE>,copy_to_ram>,1,$<AND:$<BOOL:${PICO_COPY_TO_RAM}>,$<STREQUAL:,$<TARGET_PROPERTY:PICO_TARGET_BINARY_TYPE>>>>)
+
+ target_compile_definitions(pico_standard_link INTERFACE PICO_CMAKE_BUILD_TYPE="${CMAKE_BUILD_TYPE}")
+ if (PICO_DEOPTIMIZED_DEBUG AND "${CMAKE_BUILD_TYPE}" STREQUAL "Debug")
+ target_compile_definitions(pico_standard_link INTERFACE PICO_DEOPTIMIZED_DEBUG=1)
+ endif()
+
+ # todo revisit/recall reasoning for why not -nostartfiles always?
+ # -nostartfiles will be added if PICO_NO_FLASH would be defined to 1
+ target_link_options(pico_standard_link INTERFACE $<$<IF:$<STREQUAL:$<TARGET_PROPERTY:PICO_TARGET_BINARY_TYPE>,no_flash>,1,$<AND:$<BOOL:${PICO_NO_FLASH}>,$<STREQUAL:,$<TARGET_PROPERTY:PICO_TARGET_BINARY_TYPE>>>>:-nostartfiles>)
+ # boot_stage2 will be linked if PICO_NO_FLASH would be defined to 0
+ target_link_libraries(pico_standard_link INTERFACE $<$<NOT:$<IF:$<STREQUAL:$<TARGET_PROPERTY:PICO_TARGET_BINARY_TYPE>,no_flash>,1,$<AND:$<BOOL:${PICO_NO_FLASH}>,$<STREQUAL:,$<TARGET_PROPERTY:PICO_TARGET_BINARY_TYPE>>>>>:$<IF:$<BOOL:$<TARGET_PROPERTY:PICO_TARGET_BOOT_STAGE2>>,$<TARGET_PROPERTY:PICO_TARGET_BOOT_STAGE2>,bs2_default>_library>)
+
+ # PICO_CMAKE_CONFIG: PICO_USE_DEFAULT_MAX_PAGE_SIZE, Don't shrink linker max page to 4096, type=bool, default=0, advanced=true, group=pico_standard_link
+ if (NOT PICO_USE_DEFAULT_MAX_PAGE_SIZE)
+ target_link_options(pico_standard_link INTERFACE "LINKER:-z,max-page-size=4096")
+ endif()
+ # done in compiler now
+ #target_link_options(pico_standard_link INTERFACE "LINKER:--build-id=none")
+
+ # this line occasionally useful for debugging ... todo maybe make a PICO_ var
+ # target_compile_options(pico_standard_link INTERFACE --save-temps) #debugging only
+
+ # PICO_CMAKE_CONFIG: PICO_NO_GC_SECTIONS, Disable -ffunction-sections -fdata-sections, and --gc-sections, type=bool, default=0, advanced=true, group=pico_standard_link
+ if (NOT PICO_NO_GC_SECTIONS)
+ target_compile_options(pico_standard_link INTERFACE -ffunction-sections -fdata-sections)
+ target_link_options(pico_standard_link INTERFACE "LINKER:--gc-sections")
+ endif()
+endif()
diff --git a/src/rp2_common/pico_standard_link/binary_info.c b/src/rp2_common/pico_standard_link/binary_info.c
new file mode 100644
index 0000000..aa67ac4
--- /dev/null
+++ b/src/rp2_common/pico_standard_link/binary_info.c
@@ -0,0 +1,95 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#if !PICO_NO_BINARY_INFO && !PICO_NO_PROGRAM_INFO
+#include "pico/binary_info.h"
+
+#if !PICO_NO_FLASH
+#include "boot_stage2/config.h"
+#endif
+
+// Note we put at most 4 pieces of binary info in the reset section because that's how much spare space we had
+// (picked the most common ones)... if there is a link failure because of .reset section overflow then move
+// more out.
+#define reset_section_attr __attribute__((section(".reset")))
+
+#if !PICO_NO_FLASH
+#ifndef PICO_NO_BI_BINARY_SIZE
+extern char __flash_binary_end;
+bi_decl_with_attr(bi_binary_end((intptr_t)&__flash_binary_end), reset_section_attr)
+#endif
+#endif
+
+#if !PICO_NO_BI_PROGRAM_BUILD_DATE
+#ifndef PICO_PROGRAM_BUILD_DATE
+#define PICO_PROGRAM_BUILD_DATE __DATE__
+#endif
+bi_decl_with_attr(bi_program_build_date_string(PICO_PROGRAM_BUILD_DATE), reset_section_attr);
+#endif
+
+#if !PICO_NO_BI_PROGRAM_NAME
+#if !defined(PICO_PROGRAM_NAME) && defined(PICO_TARGET_NAME)
+#define PICO_PROGRAM_NAME PICO_TARGET_NAME
+#endif
+#ifdef PICO_PROGRAM_NAME
+bi_decl_with_attr(bi_program_name(PICO_PROGRAM_NAME), reset_section_attr)
+#endif
+#endif
+
+#if !PICO_NO_BI_PICO_BOARD
+#ifdef PICO_BOARD
+bi_decl(bi_string(BINARY_INFO_TAG_RASPBERRY_PI, BINARY_INFO_ID_RP_PICO_BOARD, PICO_BOARD))
+#endif
+#endif
+
+#if !PICO_NO_BI_SDK_VERSION
+#ifdef PICO_SDK_VERSION_STRING
+bi_decl_with_attr(bi_string(BINARY_INFO_TAG_RASPBERRY_PI, BINARY_INFO_ID_RP_SDK_VERSION, PICO_SDK_VERSION_STRING),reset_section_attr)
+#endif
+#endif
+
+#if !PICO_NO_BI_PROGRAM_VERSION_STRING
+#ifdef PICO_PROGRAM_VERSION_STRING
+bi_decl(bi_program_version_string(PICO_PROGRAM_VERSION_STRING))
+#endif
+#endif
+
+
+#if !PICO_NO_BI_PROGRAM_DESCRIPTION
+#ifdef PICO_PROGRAM_DESCRIPTION
+bi_decl(bi_program_description(PICO_PROGRAM_DESCRIPTION))
+#endif
+#endif
+
+#if !PICO_NO_BI_PROGRAM_URL
+#ifdef PICO_PROGRAM_URL
+bi_decl(bi_program_url(PICO_PROGRAM_URL))
+#endif
+#endif
+
+#if !PICO_NO_BI_BOOT_STAGE2_NAME
+#ifdef PICO_BOOT_STAGE2_NAME
+bi_decl(bi_string(BINARY_INFO_TAG_RASPBERRY_PI, BINARY_INFO_ID_RP_BOOT2_NAME, PICO_BOOT_STAGE2_NAME))
+#endif
+#endif
+
+#if !PICO_NO_BI_BUILD_TYPE
+#ifdef PICO_CMAKE_BUILD_TYPE
+bi_decl(bi_program_build_attribute(PICO_CMAKE_BUILD_TYPE))
+#else
+#ifndef NDEBUG
+bi_decl(bi_program_build_attribute("Debug"))
+#else
+bi_decl(bi_program_build_attribute("Release"))
+#endif
+#endif
+
+#if PICO_DEOPTIMIZED_DEBUG
+bi_decl(bi_program_build_attribute("All optimization disabled"))
+#endif
+#endif
+
+#endif
diff --git a/src/rp2_common/pico_standard_link/crt0.S b/src/rp2_common/pico_standard_link/crt0.S
new file mode 100644
index 0000000..f57ddfd
--- /dev/null
+++ b/src/rp2_common/pico_standard_link/crt0.S
@@ -0,0 +1,338 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "pico.h"
+#include "hardware/regs/m0plus.h"
+#include "hardware/regs/addressmap.h"
+#include "hardware/regs/sio.h"
+#include "pico/binary_info/defs.h"
+
+#ifdef NDEBUG
+#ifndef COLLAPSE_IRQS
+#define COLLAPSE_IRQS
+#endif
+#endif
+
+.syntax unified
+.cpu cortex-m0plus
+.thumb
+
+.section .vectors, "ax"
+.align 2
+
+.global __vectors, __VECTOR_TABLE
+__VECTOR_TABLE:
+__vectors:
+.word __StackTop
+.word _reset_handler
+.word isr_nmi
+.word isr_hardfault
+.word isr_invalid // Reserved, should never fire
+.word isr_invalid // Reserved, should never fire
+.word isr_invalid // Reserved, should never fire
+.word isr_invalid // Reserved, should never fire
+.word isr_invalid // Reserved, should never fire
+.word isr_invalid // Reserved, should never fire
+.word isr_invalid // Reserved, should never fire
+.word isr_svcall
+.word isr_invalid // Reserved, should never fire
+.word isr_invalid // Reserved, should never fire
+.word isr_pendsv
+.word isr_systick
+.word isr_irq0
+.word isr_irq1
+.word isr_irq2
+.word isr_irq3
+.word isr_irq4
+.word isr_irq5
+.word isr_irq6
+.word isr_irq7
+.word isr_irq8
+.word isr_irq9
+.word isr_irq10
+.word isr_irq11
+.word isr_irq12
+.word isr_irq13
+.word isr_irq14
+.word isr_irq15
+.word isr_irq16
+.word isr_irq17
+.word isr_irq18
+.word isr_irq19
+.word isr_irq20
+.word isr_irq21
+.word isr_irq22
+.word isr_irq23
+.word isr_irq24
+.word isr_irq25
+.word isr_irq26
+.word isr_irq27
+.word isr_irq28
+.word isr_irq29
+.word isr_irq30
+.word isr_irq31
+
+// all default exception handlers do nothing, and we can check for them being set to our
+// default values by seeing if they point to somewhere between __defaults_isrs_start and __default_isrs_end
+.global __default_isrs_start
+__default_isrs_start:
+
+// Declare a weak symbol for each ISR.
+// By default, they will fall through to the undefined IRQ handler below (breakpoint),
+// but can be overridden by C functions with correct name.
+
+.macro decl_isr_bkpt name
+.weak \name
+.type \name,%function
+.thumb_func
+\name:
+ bkpt #0
+.endm
+
+// these are separated out for clarity
+decl_isr_bkpt isr_invalid
+decl_isr_bkpt isr_nmi
+decl_isr_bkpt isr_hardfault
+decl_isr_bkpt isr_svcall
+decl_isr_bkpt isr_pendsv
+decl_isr_bkpt isr_systick
+
+.global __default_isrs_end
+__default_isrs_end:
+
+.macro decl_isr name
+.weak \name
+.type \name,%function
+.thumb_func
+\name:
+.endm
+
+decl_isr isr_irq0
+decl_isr isr_irq1
+decl_isr isr_irq2
+decl_isr isr_irq3
+decl_isr isr_irq4
+decl_isr isr_irq5
+decl_isr isr_irq6
+decl_isr isr_irq7
+decl_isr isr_irq8
+decl_isr isr_irq9
+decl_isr isr_irq10
+decl_isr isr_irq11
+decl_isr isr_irq12
+decl_isr isr_irq13
+decl_isr isr_irq14
+decl_isr isr_irq15
+decl_isr isr_irq16
+decl_isr isr_irq17
+decl_isr isr_irq18
+decl_isr isr_irq19
+decl_isr isr_irq20
+decl_isr isr_irq21
+decl_isr isr_irq22
+decl_isr isr_irq23
+decl_isr isr_irq24
+decl_isr isr_irq25
+decl_isr isr_irq26
+decl_isr isr_irq27
+decl_isr isr_irq28
+decl_isr isr_irq29
+decl_isr isr_irq30
+decl_isr isr_irq31
+
+// All unhandled USER IRQs fall through to here
+.global __unhandled_user_irq
+.thumb_func
+__unhandled_user_irq:
+ bl __get_current_exception
+ subs r0, #16
+.global unhandled_user_irq_num_in_r0
+unhandled_user_irq_num_in_r0:
+ bkpt #0
+
+// ----------------------------------------------------------------------------
+
+.section .binary_info_header, "a"
+
+// Header must be in first 256 bytes of main image (i.e. excluding flash boot2).
+// For flash builds we put it immediately after vector table; for NO_FLASH the
+// vectors are at a +0x100 offset because the bootrom enters RAM images directly
+// at their lowest address, so we put the header in the VTOR alignment hole.
+
+#if !PICO_NO_BINARY_INFO
+binary_info_header:
+.word BINARY_INFO_MARKER_START
+.word __binary_info_start
+.word __binary_info_end
+.word data_cpy_table // we may need to decode pointers that are in RAM at runtime.
+.word BINARY_INFO_MARKER_END
+#endif
+
+// ----------------------------------------------------------------------------
+
+.section .reset, "ax"
+
+// On flash builds, the vector table comes first in the image (conventional).
+// On NO_FLASH builds, the reset handler section comes first, as the entry
+// point is at offset 0 (fixed due to bootrom), and VTOR is highly-aligned.
+// Image is entered in various ways:
+//
+// - NO_FLASH builds are entered from beginning by UF2 bootloader
+//
+// - Flash builds vector through the table into _reset_handler from boot2
+//
+// - Either type can be entered via _entry_point by the debugger, and flash builds
+// must then be sent back round the boot sequence to properly initialise flash
+
+// ELF entry point:
+.type _entry_point,%function
+.thumb_func
+.global _entry_point
+_entry_point:
+
+#if PICO_NO_FLASH
+ // Vector through our own table (SP, VTOR will not have been set up at
+ // this point). Same path for debugger entry and bootloader entry.
+ ldr r0, =__vectors
+#else
+ // Debugger tried to run code after loading, so SSI is in 03h-only mode.
+ // Go back through bootrom + boot2 to properly initialise flash.
+ movs r0, #0
+#endif
+ ldr r1, =(PPB_BASE + M0PLUS_VTOR_OFFSET)
+ str r0, [r1]
+ ldmia r0!, {r1, r2}
+ msr msp, r1
+ bx r2
+
+// Reset handler:
+// - initialises .data
+// - clears .bss
+// - calls runtime_init
+// - calls main
+// - calls exit (which should eventually hang the processor via _exit)
+
+.type _reset_handler,%function
+.thumb_func
+_reset_handler:
+ // Only core 0 should run the C runtime startup code; core 1 is normally
+ // sleeping in the bootrom at this point but check to be sure
+ ldr r0, =(SIO_BASE + SIO_CPUID_OFFSET)
+ ldr r0, [r0]
+ cmp r0, #0
+ bne hold_non_core0_in_bootrom
+
+ adr r4, data_cpy_table
+
+ // assume there is at least one entry
+1:
+ ldmia r4!, {r1-r3}
+ cmp r1, #0
+ beq 2f
+ bl data_cpy
+ b 1b
+2:
+
+ // Zero out the BSS
+ ldr r1, =__bss_start__
+ ldr r2, =__bss_end__
+ movs r0, #0
+ b bss_fill_test
+bss_fill_loop:
+ stm r1!, {r0}
+bss_fill_test:
+ cmp r1, r2
+ bne bss_fill_loop
+
+platform_entry: // symbol for stack traces
+ // Use 32-bit jumps, in case these symbols are moved out of branch range
+ // (e.g. if main is in SRAM and crt0 in flash)
+ ldr r1, =runtime_init
+ blx r1
+ ldr r1, =main
+ blx r1
+ ldr r1, =exit
+ blx r1
+ // exit should not return. If it does, hang the core.
+ // (fall thru into our hang _exit impl
+.weak _exit
+.type _exit,%function
+.thumb_func
+_exit:
+1: // separate label because _exit can be moved out of branch range
+ bkpt #0
+ b 1b
+
+data_cpy_loop:
+ ldm r1!, {r0}
+ stm r2!, {r0}
+data_cpy:
+ cmp r2, r3
+ blo data_cpy_loop
+ bx lr
+
+.align 2
+data_cpy_table:
+#if PICO_COPY_TO_RAM
+.word __ram_text_source__
+.word __ram_text_start__
+.word __ram_text_end__
+#endif
+.word __etext
+.word __data_start__
+.word __data_end__
+
+.word __scratch_x_source__
+.word __scratch_x_start__
+.word __scratch_x_end__
+
+.word __scratch_y_source__
+.word __scratch_y_start__
+.word __scratch_y_end__
+
+.word 0 // null terminator
+
+// ----------------------------------------------------------------------------
+// Provide safe defaults for _exit and runtime_init
+// Full implementations usually provided by platform.c
+
+.weak runtime_init
+.type runtime_init,%function
+.thumb_func
+runtime_init:
+ bx lr
+
+// ----------------------------------------------------------------------------
+// If core 1 somehow gets into crt0 due to a spectacular VTOR mishap, we need to
+// catch it and send back to the sleep-and-launch code in the bootrom. Shouldn't
+// happen (it should sleep in the ROM until given an entry point via the
+// cross-core FIFOs) but it's good to be defensive.
+
+hold_non_core0_in_bootrom:
+ ldr r0, = 'W' | ('V' << 8)
+ bl rom_func_lookup
+ bx r0
+
+.global __get_current_exception
+.thumb_func
+__get_current_exception:
+ mrs r0, ipsr
+ uxtb r0, r0
+ bx lr
+
+// ----------------------------------------------------------------------------
+// Stack/heap dummies to set size
+
+.section .stack
+// align to allow for memory protection (although this alignment is pretty much ignored by linker script)
+.align 5
+ .equ StackSize, PICO_STACK_SIZE
+.space StackSize
+
+.section .heap
+.align 2
+ .equ HeapSize, PICO_HEAP_SIZE
+.space HeapSize
diff --git a/src/rp2_common/pico_standard_link/doc.h b/src/rp2_common/pico_standard_link/doc.h
new file mode 100644
index 0000000..d8ce3d4
--- /dev/null
+++ b/src/rp2_common/pico_standard_link/doc.h
@@ -0,0 +1,10 @@
+/**
+ * \defgroup pico_standard_link pico_standard_link
+ * \brief Standard link step providing the basics for creating a runnable binary
+ *
+ * This includes
+ * - C runtime initialization
+ * - Linker scripts for 'default', 'no_flash', 'blocked_ram' and 'copy_to_ram' binaries
+ * - 'Binary Information' support
+ * - Linker option control
+ */
diff --git a/src/rp2_common/pico_standard_link/memmap_blocked_ram.ld b/src/rp2_common/pico_standard_link/memmap_blocked_ram.ld
new file mode 100644
index 0000000..5b0afe6
--- /dev/null
+++ b/src/rp2_common/pico_standard_link/memmap_blocked_ram.ld
@@ -0,0 +1,252 @@
+/* Based on GCC ARM embedded samples.
+ Defines the following symbols for use by code:
+ __exidx_start
+ __exidx_end
+ __etext
+ __data_start__
+ __preinit_array_start
+ __preinit_array_end
+ __init_array_start
+ __init_array_end
+ __fini_array_start
+ __fini_array_end
+ __data_end__
+ __bss_start__
+ __bss_end__
+ __end__
+ end
+ __HeapLimit
+ __StackLimit
+ __StackTop
+ __stack (== StackTop)
+*/
+
+MEMORY
+{
+ FLASH(rx) : ORIGIN = 0x10000000, LENGTH = 2048k
+ RAM(rwx) : ORIGIN = 0x21000000, LENGTH = 256k
+ SCRATCH_X(rwx) : ORIGIN = 0x20040000, LENGTH = 4k
+ SCRATCH_Y(rwx) : ORIGIN = 0x20041000, LENGTH = 4k
+}
+
+ENTRY(_entry_point)
+
+SECTIONS
+{
+ /* Second stage bootloader is prepended to the image. It must be 256 bytes big
+ and checksummed. It is usually built by the boot_stage2 target
+ in the Raspberry Pi Pico SDK
+ */
+
+ .flash_begin : {
+ __flash_binary_start = .;
+ } > FLASH
+
+ .boot2 : {
+ __boot2_start__ = .;
+ KEEP (*(.boot2))
+ __boot2_end__ = .;
+ } > FLASH
+
+ ASSERT(__boot2_end__ - __boot2_start__ == 256,
+ "ERROR: Pico second stage bootloader must be 256 bytes in size")
+
+ /* The second stage will always enter the image at the start of .text.
+ The debugger will use the ELF entry point, which is the _entry_point
+ symbol if present, otherwise defaults to start of .text.
+ This can be used to transfer control back to the bootrom on debugger
+ launches only, to perform proper flash setup.
+ */
+
+ .text : {
+ __logical_binary_start = .;
+ KEEP (*(.vectors))
+ KEEP (*(.binary_info_header))
+ __binary_info_header_end = .;
+ KEEP (*(.reset))
+ /* TODO revisit this now memset/memcpy/float in ROM */
+ /* bit of a hack right now to exclude all floating point and time critical (e.g. memset, memcpy) code from
+ * FLASH ... we will include any thing excluded here in .data below by default */
+ *(.init)
+ *(EXCLUDE_FILE(*libgcc.a: *libc.a:*lib_a-mem*.o *libm.a:) .text*)
+ *(.fini)
+ /* Pull all c'tors into .text */
+ *crtbegin.o(.ctors)
+ *crtbegin?.o(.ctors)
+ *(EXCLUDE_FILE(*crtend?.o *crtend.o) .ctors)
+ *(SORT(.ctors.*))
+ *(.ctors)
+ /* Followed by destructors */
+ *crtbegin.o(.dtors)
+ *crtbegin?.o(.dtors)
+ *(EXCLUDE_FILE(*crtend?.o *crtend.o) .dtors)
+ *(SORT(.dtors.*))
+ *(.dtors)
+
+ *(.eh_frame*)
+ . = ALIGN(4);
+ } > FLASH
+
+ .rodata : {
+ *(EXCLUDE_FILE(*libgcc.a: *libc.a:*lib_a-mem*.o *libm.a:) .rodata*)
+ . = ALIGN(4);
+ *(SORT_BY_ALIGNMENT(SORT_BY_NAME(.flashdata*)))
+ . = ALIGN(4);
+ } > FLASH
+
+ .ARM.extab :
+ {
+ *(.ARM.extab* .gnu.linkonce.armextab.*)
+ } > FLASH
+
+ __exidx_start = .;
+ .ARM.exidx :
+ {
+ *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+ } > FLASH
+ __exidx_end = .;
+
+ /* Machine inspectable binary information */
+ . = ALIGN(4);
+ __binary_info_start = .;
+ .binary_info :
+ {
+ KEEP(*(.binary_info.keep.*))
+ *(.binary_info.*)
+ } > FLASH
+ __binary_info_end = .;
+ . = ALIGN(4);
+
+ /* End of .text-like segments */
+ __etext = .;
+
+ .ram_vector_table (COPY): {
+ *(.ram_vector_table)
+ } > RAM
+
+ .data : {
+ __data_start__ = .;
+ *(vtable)
+
+ *(.time_critical*)
+
+ /* remaining .text and .rodata; i.e. stuff we exclude above because we want it in RAM */
+ *(.text*)
+ . = ALIGN(4);
+ *(.rodata*)
+ . = ALIGN(4);
+
+ *(.data*)
+
+ . = ALIGN(4);
+ *(.after_data.*)
+ . = ALIGN(4);
+ /* preinit data */
+ PROVIDE_HIDDEN (__mutex_array_start = .);
+ KEEP(*(SORT(.mutex_array.*)))
+ KEEP(*(.mutex_array))
+ PROVIDE_HIDDEN (__mutex_array_end = .);
+
+ . = ALIGN(4);
+ /* preinit data */
+ PROVIDE_HIDDEN (__preinit_array_start = .);
+ KEEP(*(SORT(.preinit_array.*)))
+ KEEP(*(.preinit_array))
+ PROVIDE_HIDDEN (__preinit_array_end = .);
+
+ . = ALIGN(4);
+ /* init data */
+ PROVIDE_HIDDEN (__init_array_start = .);
+ KEEP(*(SORT(.init_array.*)))
+ KEEP(*(.init_array))
+ PROVIDE_HIDDEN (__init_array_end = .);
+
+ . = ALIGN(4);
+ /* finit data */
+ PROVIDE_HIDDEN (__fini_array_start = .);
+ *(SORT(.fini_array.*))
+ *(.fini_array)
+ PROVIDE_HIDDEN (__fini_array_end = .);
+
+ *(.jcr)
+ . = ALIGN(4);
+ /* All data end */
+ __data_end__ = .;
+ } > RAM AT> FLASH
+
+ .uninitialized_data (COPY): {
+ . = ALIGN(4);
+ *(.uninitialized_data*)
+ } > RAM
+
+ /* Start and end symbols must be word-aligned */
+ .scratch_x : {
+ __scratch_x_start__ = .;
+ *(.scratch_x.*)
+ . = ALIGN(4);
+ __scratch_x_end__ = .;
+ } > SCRATCH_X AT > FLASH
+ __scratch_x_source__ = LOADADDR(.scratch_x);
+
+ .scratch_y : {
+ __scratch_y_start__ = .;
+ *(.scratch_y.*)
+ . = ALIGN(4);
+ __scratch_y_end__ = .;
+ } > SCRATCH_Y AT > FLASH
+ __scratch_y_source__ = LOADADDR(.scratch_y);
+
+ .bss : {
+ . = ALIGN(4);
+ __bss_start__ = .;
+ *(SORT_BY_ALIGNMENT(SORT_BY_NAME(.bss*)))
+ *(COMMON)
+ . = ALIGN(4);
+ __bss_end__ = .;
+ } > RAM
+
+ .heap (COPY):
+ {
+ __end__ = .;
+ end = __end__;
+ *(.heap*)
+ __HeapLimit = .;
+ } > RAM
+
+ /* .stack*_dummy section doesn't contains any symbols. It is only
+ * used for linker to calculate size of stack sections, and assign
+ * values to stack symbols later
+ *
+ * stack1 section may be empty/missing if platform_launch_core1 is not used */
+
+ /* by default we put core 0 stack at the end of scratch Y, so that if core 1
+ * stack is not used then all of SCRATCH_X is free.
+ */
+ .stack1_dummy (COPY):
+ {
+ *(.stack1*)
+ } > SCRATCH_X
+ .stack_dummy (COPY):
+ {
+ *(.stack*)
+ } > SCRATCH_Y
+
+ .flash_end : {
+ __flash_binary_end = .;
+ } > FLASH
+
+ /* stack limit is poorly named, but historically is maximum heap ptr */
+ __StackLimit = ORIGIN(RAM) + LENGTH(RAM);
+ __StackOneTop = ORIGIN(SCRATCH_X) + LENGTH(SCRATCH_X);
+ __StackTop = ORIGIN(SCRATCH_Y) + LENGTH(SCRATCH_Y);
+ __StackOneBottom = __StackOneTop - SIZEOF(.stack1_dummy);
+ __StackBottom = __StackTop - SIZEOF(.stack_dummy);
+ PROVIDE(__stack = __StackTop);
+
+ /* Check if data + heap + stack exceeds RAM limit */
+ ASSERT(__StackLimit >= __HeapLimit, "region RAM overflowed")
+
+ ASSERT( __binary_info_header_end - __logical_binary_start <= 256, "Binary info must be in first 256 bytes of the binary")
+ /* todo assert on extra code */
+}
+
diff --git a/src/rp2_common/pico_standard_link/memmap_copy_to_ram.ld b/src/rp2_common/pico_standard_link/memmap_copy_to_ram.ld
new file mode 100644
index 0000000..90975b5
--- /dev/null
+++ b/src/rp2_common/pico_standard_link/memmap_copy_to_ram.ld
@@ -0,0 +1,253 @@
+/* Based on GCC ARM embedded samples.
+ Defines the following symbols for use by code:
+ __exidx_start
+ __exidx_end
+ __etext
+ __data_start__
+ __preinit_array_start
+ __preinit_array_end
+ __init_array_start
+ __init_array_end
+ __fini_array_start
+ __fini_array_end
+ __data_end__
+ __bss_start__
+ __bss_end__
+ __end__
+ end
+ __HeapLimit
+ __StackLimit
+ __StackTop
+ __stack (== StackTop)
+*/
+
+MEMORY
+{
+ FLASH(rx) : ORIGIN = 0x10000000, LENGTH = 2048k
+ RAM(rwx) : ORIGIN = 0x20000000, LENGTH = 256k
+ SCRATCH_X(rwx) : ORIGIN = 0x20040000, LENGTH = 4k
+ SCRATCH_Y(rwx) : ORIGIN = 0x20041000, LENGTH = 4k
+}
+
+ENTRY(_entry_point)
+
+SECTIONS
+{
+ /* Second stage bootloader is prepended to the image. It must be 256 bytes big
+ and checksummed. It is usually built by the boot_stage2 target
+ in the Raspberry Pi Pico SDK
+ */
+
+ .flash_begin : {
+ __flash_binary_start = .;
+ } > FLASH
+
+ .boot2 : {
+ __boot2_start__ = .;
+ KEEP (*(.boot2))
+ __boot2_end__ = .;
+ } > FLASH
+
+ ASSERT(__boot2_end__ - __boot2_start__ == 256,
+ "ERROR: Pico second stage bootloader must be 256 bytes in size")
+
+ /* The second stage will always enter the image at the start of .text.
+ The debugger will use the ELF entry point, which is the _entry_point
+ symbol if present, otherwise defaults to start of .text.
+ This can be used to transfer control back to the bootrom on debugger
+ launches only, to perform proper flash setup.
+ */
+
+ .flashtext : {
+ __logical_binary_start = .;
+ KEEP (*(.vectors))
+ KEEP (*(.binary_info_header))
+ __binary_info_header_end = .;
+ KEEP (*(.reset))
+ }
+
+ .rodata : {
+ /* segments not marked as .flashdata are instead pulled into .data (in RAM) to avoid accidental flash accesses */
+ *(SORT_BY_ALIGNMENT(SORT_BY_NAME(.flashdata*)))
+ . = ALIGN(4);
+ } > FLASH
+
+ .ARM.extab :
+ {
+ *(.ARM.extab* .gnu.linkonce.armextab.*)
+ } > FLASH
+
+ __exidx_start = .;
+ .ARM.exidx :
+ {
+ *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+ } > FLASH
+ __exidx_end = .;
+
+ /* Machine inspectable binary information */
+ . = ALIGN(4);
+ __binary_info_start = .;
+ .binary_info :
+ {
+ KEEP(*(.binary_info.keep.*))
+ *(.binary_info.*)
+ } > FLASH
+ __binary_info_end = .;
+ . = ALIGN(4);
+
+ /* Vector table goes first in RAM, to avoid large alignment hole */
+ .ram_vector_table (COPY): {
+ *(.ram_vector_table)
+ } > RAM
+
+ .text : {
+ __ram_text_start__ = .;
+ *(.init)
+ *(.text*)
+ *(.fini)
+ /* Pull all c'tors into .text */
+ *crtbegin.o(.ctors)
+ *crtbegin?.o(.ctors)
+ *(EXCLUDE_FILE(*crtend?.o *crtend.o) .ctors)
+ *(SORT(.ctors.*))
+ *(.ctors)
+ /* Followed by destructors */
+ *crtbegin.o(.dtors)
+ *crtbegin?.o(.dtors)
+ *(EXCLUDE_FILE(*crtend?.o *crtend.o) .dtors)
+ *(SORT(.dtors.*))
+ *(.dtors)
+
+ *(.eh_frame*)
+ . = ALIGN(4);
+ __ram_text_end__ = .;
+ } > RAM AT> FLASH
+ __ram_text_source__ = LOADADDR(.text);
+
+
+ .data : {
+ __data_start__ = .;
+ *(vtable)
+
+ *(.time_critical*)
+
+ . = ALIGN(4);
+ *(.rodata*)
+ . = ALIGN(4);
+
+ *(.data*)
+
+ . = ALIGN(4);
+ *(.after_data.*)
+ . = ALIGN(4);
+ /* preinit data */
+ PROVIDE_HIDDEN (__mutex_array_start = .);
+ KEEP(*(SORT(.mutex_array.*)))
+ KEEP(*(.mutex_array))
+ PROVIDE_HIDDEN (__mutex_array_end = .);
+
+ . = ALIGN(4);
+ /* preinit data */
+ PROVIDE_HIDDEN (__preinit_array_start = .);
+ KEEP(*(SORT(.preinit_array.*)))
+ KEEP(*(.preinit_array))
+ PROVIDE_HIDDEN (__preinit_array_end = .);
+
+ . = ALIGN(4);
+ /* init data */
+ PROVIDE_HIDDEN (__init_array_start = .);
+ KEEP(*(SORT(.init_array.*)))
+ KEEP(*(.init_array))
+ PROVIDE_HIDDEN (__init_array_end = .);
+
+ . = ALIGN(4);
+ /* finit data */
+ PROVIDE_HIDDEN (__fini_array_start = .);
+ *(SORT(.fini_array.*))
+ *(.fini_array)
+ PROVIDE_HIDDEN (__fini_array_end = .);
+
+ *(.jcr)
+ . = ALIGN(4);
+ /* All data end */
+ __data_end__ = .;
+ } > RAM AT> FLASH
+ /* __etext is the name of the .data init source pointer (...) */
+ __etext = LOADADDR(.data);
+
+ .uninitialized_data (COPY): {
+ . = ALIGN(4);
+ *(.uninitialized_data*)
+ } > RAM
+
+ /* Start and end symbols must be word-aligned */
+ .scratch_x : {
+ __scratch_x_start__ = .;
+ *(.scratch_x.*)
+ . = ALIGN(4);
+ __scratch_x_end__ = .;
+ } > SCRATCH_X AT > FLASH
+ __scratch_x_source__ = LOADADDR(.scratch_x);
+
+ .scratch_y : {
+ __scratch_y_start__ = .;
+ *(.scratch_y.*)
+ . = ALIGN(4);
+ __scratch_y_end__ = .;
+ } > SCRATCH_Y AT > FLASH
+ __scratch_y_source__ = LOADADDR(.scratch_y);
+
+ .bss : {
+ . = ALIGN(4);
+ __bss_start__ = .;
+ *(SORT_BY_ALIGNMENT(SORT_BY_NAME(.bss*)))
+ *(COMMON)
+ . = ALIGN(4);
+ __bss_end__ = .;
+ } > RAM
+
+ .heap (COPY):
+ {
+ __end__ = .;
+ end = __end__;
+ *(.heap*)
+ __HeapLimit = .;
+ } > RAM
+
+ /* .stack*_dummy section doesn't contains any symbols. It is only
+ * used for linker to calculate size of stack sections, and assign
+ * values to stack symbols later
+ *
+ * stack1 section may be empty/missing if platform_launch_core1 is not used */
+
+ /* by default we put core 0 stack at the end of scratch Y, so that if core 1
+ * stack is not used then all of SCRATCH_X is free.
+ */
+ .stack1_dummy (COPY):
+ {
+ *(.stack1*)
+ } > SCRATCH_X
+ .stack_dummy (COPY):
+ {
+ *(.stack*)
+ } > SCRATCH_Y
+
+ .flash_end : {
+ __flash_binary_end = .;
+ } > FLASH
+
+ /* stack limit is poorly named, but historically is maximum heap ptr */
+ __StackLimit = ORIGIN(RAM) + LENGTH(RAM);
+ __StackOneTop = ORIGIN(SCRATCH_X) + LENGTH(SCRATCH_X);
+ __StackTop = ORIGIN(SCRATCH_Y) + LENGTH(SCRATCH_Y);
+ __StackOneBottom = __StackOneTop - SIZEOF(.stack1_dummy);
+ __StackBottom = __StackTop - SIZEOF(.stack_dummy);
+ PROVIDE(__stack = __StackTop);
+
+ /* Check if data + heap + stack exceeds RAM limit */
+ ASSERT(__StackLimit >= __HeapLimit, "region RAM overflowed")
+
+ ASSERT( __binary_info_header_end - __logical_binary_start <= 256, "Binary info must be in first 256 bytes of the binary")
+ /* todo assert on extra code */
+}
+
diff --git a/src/rp2_common/pico_standard_link/memmap_default.ld b/src/rp2_common/pico_standard_link/memmap_default.ld
new file mode 100644
index 0000000..07d5812
--- /dev/null
+++ b/src/rp2_common/pico_standard_link/memmap_default.ld
@@ -0,0 +1,252 @@
+/* Based on GCC ARM embedded samples.
+ Defines the following symbols for use by code:
+ __exidx_start
+ __exidx_end
+ __etext
+ __data_start__
+ __preinit_array_start
+ __preinit_array_end
+ __init_array_start
+ __init_array_end
+ __fini_array_start
+ __fini_array_end
+ __data_end__
+ __bss_start__
+ __bss_end__
+ __end__
+ end
+ __HeapLimit
+ __StackLimit
+ __StackTop
+ __stack (== StackTop)
+*/
+
+MEMORY
+{
+ FLASH(rx) : ORIGIN = 0x10000000, LENGTH = 2048k
+ RAM(rwx) : ORIGIN = 0x20000000, LENGTH = 256k
+ SCRATCH_X(rwx) : ORIGIN = 0x20040000, LENGTH = 4k
+ SCRATCH_Y(rwx) : ORIGIN = 0x20041000, LENGTH = 4k
+}
+
+ENTRY(_entry_point)
+
+SECTIONS
+{
+ /* Second stage bootloader is prepended to the image. It must be 256 bytes big
+ and checksummed. It is usually built by the boot_stage2 target
+ in the Raspberry Pi Pico SDK
+ */
+
+ .flash_begin : {
+ __flash_binary_start = .;
+ } > FLASH
+
+ .boot2 : {
+ __boot2_start__ = .;
+ KEEP (*(.boot2))
+ __boot2_end__ = .;
+ } > FLASH
+
+ ASSERT(__boot2_end__ - __boot2_start__ == 256,
+ "ERROR: Pico second stage bootloader must be 256 bytes in size")
+
+ /* The second stage will always enter the image at the start of .text.
+ The debugger will use the ELF entry point, which is the _entry_point
+ symbol if present, otherwise defaults to start of .text.
+ This can be used to transfer control back to the bootrom on debugger
+ launches only, to perform proper flash setup.
+ */
+
+ .text : {
+ __logical_binary_start = .;
+ KEEP (*(.vectors))
+ KEEP (*(.binary_info_header))
+ __binary_info_header_end = .;
+ KEEP (*(.reset))
+ /* TODO revisit this now memset/memcpy/float in ROM */
+ /* bit of a hack right now to exclude all floating point and time critical (e.g. memset, memcpy) code from
+ * FLASH ... we will include any thing excluded here in .data below by default */
+ *(.init)
+ *(EXCLUDE_FILE(*libgcc.a: *libc.a:*lib_a-mem*.o *libm.a:) .text*)
+ *(.fini)
+ /* Pull all c'tors into .text */
+ *crtbegin.o(.ctors)
+ *crtbegin?.o(.ctors)
+ *(EXCLUDE_FILE(*crtend?.o *crtend.o) .ctors)
+ *(SORT(.ctors.*))
+ *(.ctors)
+ /* Followed by destructors */
+ *crtbegin.o(.dtors)
+ *crtbegin?.o(.dtors)
+ *(EXCLUDE_FILE(*crtend?.o *crtend.o) .dtors)
+ *(SORT(.dtors.*))
+ *(.dtors)
+
+ *(.eh_frame*)
+ . = ALIGN(4);
+ } > FLASH
+
+ .rodata : {
+ *(EXCLUDE_FILE(*libgcc.a: *libc.a:*lib_a-mem*.o *libm.a:) .rodata*)
+ . = ALIGN(4);
+ *(SORT_BY_ALIGNMENT(SORT_BY_NAME(.flashdata*)))
+ . = ALIGN(4);
+ } > FLASH
+
+ .ARM.extab :
+ {
+ *(.ARM.extab* .gnu.linkonce.armextab.*)
+ } > FLASH
+
+ __exidx_start = .;
+ .ARM.exidx :
+ {
+ *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+ } > FLASH
+ __exidx_end = .;
+
+ /* Machine inspectable binary information */
+ . = ALIGN(4);
+ __binary_info_start = .;
+ .binary_info :
+ {
+ KEEP(*(.binary_info.keep.*))
+ *(.binary_info.*)
+ } > FLASH
+ __binary_info_end = .;
+ . = ALIGN(4);
+
+ /* End of .text-like segments */
+ __etext = .;
+
+ .ram_vector_table (COPY): {
+ *(.ram_vector_table)
+ } > RAM
+
+ .data : {
+ __data_start__ = .;
+ *(vtable)
+
+ *(.time_critical*)
+
+ /* remaining .text and .rodata; i.e. stuff we exclude above because we want it in RAM */
+ *(.text*)
+ . = ALIGN(4);
+ *(.rodata*)
+ . = ALIGN(4);
+
+ *(.data*)
+
+ . = ALIGN(4);
+ *(.after_data.*)
+ . = ALIGN(4);
+ /* preinit data */
+ PROVIDE_HIDDEN (__mutex_array_start = .);
+ KEEP(*(SORT(.mutex_array.*)))
+ KEEP(*(.mutex_array))
+ PROVIDE_HIDDEN (__mutex_array_end = .);
+
+ . = ALIGN(4);
+ /* preinit data */
+ PROVIDE_HIDDEN (__preinit_array_start = .);
+ KEEP(*(SORT(.preinit_array.*)))
+ KEEP(*(.preinit_array))
+ PROVIDE_HIDDEN (__preinit_array_end = .);
+
+ . = ALIGN(4);
+ /* init data */
+ PROVIDE_HIDDEN (__init_array_start = .);
+ KEEP(*(SORT(.init_array.*)))
+ KEEP(*(.init_array))
+ PROVIDE_HIDDEN (__init_array_end = .);
+
+ . = ALIGN(4);
+ /* finit data */
+ PROVIDE_HIDDEN (__fini_array_start = .);
+ *(SORT(.fini_array.*))
+ *(.fini_array)
+ PROVIDE_HIDDEN (__fini_array_end = .);
+
+ *(.jcr)
+ . = ALIGN(4);
+ /* All data end */
+ __data_end__ = .;
+ } > RAM AT> FLASH
+
+ .uninitialized_data (COPY): {
+ . = ALIGN(4);
+ *(.uninitialized_data*)
+ } > RAM
+
+ /* Start and end symbols must be word-aligned */
+ .scratch_x : {
+ __scratch_x_start__ = .;
+ *(.scratch_x.*)
+ . = ALIGN(4);
+ __scratch_x_end__ = .;
+ } > SCRATCH_X AT > FLASH
+ __scratch_x_source__ = LOADADDR(.scratch_x);
+
+ .scratch_y : {
+ __scratch_y_start__ = .;
+ *(.scratch_y.*)
+ . = ALIGN(4);
+ __scratch_y_end__ = .;
+ } > SCRATCH_Y AT > FLASH
+ __scratch_y_source__ = LOADADDR(.scratch_y);
+
+ .bss : {
+ . = ALIGN(4);
+ __bss_start__ = .;
+ *(SORT_BY_ALIGNMENT(SORT_BY_NAME(.bss*)))
+ *(COMMON)
+ . = ALIGN(4);
+ __bss_end__ = .;
+ } > RAM
+
+ .heap (COPY):
+ {
+ __end__ = .;
+ end = __end__;
+ *(.heap*)
+ __HeapLimit = .;
+ } > RAM
+
+ /* .stack*_dummy section doesn't contains any symbols. It is only
+ * used for linker to calculate size of stack sections, and assign
+ * values to stack symbols later
+ *
+ * stack1 section may be empty/missing if platform_launch_core1 is not used */
+
+ /* by default we put core 0 stack at the end of scratch Y, so that if core 1
+ * stack is not used then all of SCRATCH_X is free.
+ */
+ .stack1_dummy (COPY):
+ {
+ *(.stack1*)
+ } > SCRATCH_X
+ .stack_dummy (COPY):
+ {
+ *(.stack*)
+ } > SCRATCH_Y
+
+ .flash_end : {
+ __flash_binary_end = .;
+ } > FLASH
+
+ /* stack limit is poorly named, but historically is maximum heap ptr */
+ __StackLimit = ORIGIN(RAM) + LENGTH(RAM);
+ __StackOneTop = ORIGIN(SCRATCH_X) + LENGTH(SCRATCH_X);
+ __StackTop = ORIGIN(SCRATCH_Y) + LENGTH(SCRATCH_Y);
+ __StackOneBottom = __StackOneTop - SIZEOF(.stack1_dummy);
+ __StackBottom = __StackTop - SIZEOF(.stack_dummy);
+ PROVIDE(__stack = __StackTop);
+
+ /* Check if data + heap + stack exceeds RAM limit */
+ ASSERT(__StackLimit >= __HeapLimit, "region RAM overflowed")
+
+ ASSERT( __binary_info_header_end - __logical_binary_start <= 256, "Binary info must be in first 256 bytes of the binary")
+ /* todo assert on extra code */
+}
+
diff --git a/src/rp2_common/pico_standard_link/memmap_no_flash.ld b/src/rp2_common/pico_standard_link/memmap_no_flash.ld
new file mode 100644
index 0000000..7a5977f
--- /dev/null
+++ b/src/rp2_common/pico_standard_link/memmap_no_flash.ld
@@ -0,0 +1,217 @@
+/* Based on GCC ARM embedded samples.
+ Defines the following symbols for use by code:
+ __exidx_start
+ __exidx_end
+ __etext
+ __data_start__
+ __preinit_array_start
+ __preinit_array_end
+ __init_array_start
+ __init_array_end
+ __fini_array_start
+ __fini_array_end
+ __data_end__
+ __bss_start__
+ __bss_end__
+ __end__
+ end
+ __HeapLimit
+ __StackLimit
+ __StackTop
+ __stack (== StackTop)
+*/
+
+MEMORY
+{
+ RAM(rwx) : ORIGIN = 0x20000000, LENGTH = 256k
+ SCRATCH_X(rwx) : ORIGIN = 0x20040000, LENGTH = 4k
+ SCRATCH_Y(rwx) : ORIGIN = 0x20041000, LENGTH = 4k
+}
+
+ENTRY(_entry_point)
+
+SECTIONS
+{
+ /* Note in NO_FLASH builds the entry point for both the bootrom, and debugger
+ entry (ELF entry point), are *first* in the image, and the vector table
+ follows immediately afterward. This is because the bootrom enters RAM
+ binaries directly at their lowest address (preferring main RAM over XIP
+ cache-as-SRAM if both are used).
+ */
+
+ .text : {
+ __logical_binary_start = .;
+ __reset_start = .;
+ KEEP (*(.reset))
+ __reset_end = .;
+ KEEP (*(.binary_info_header))
+ __binary_info_header_end = .;
+ . = ALIGN(256);
+ KEEP (*(.vectors))
+ *(.time_critical*)
+ *(.text*)
+ . = ALIGN(4);
+ *(.init)
+ *(.fini)
+ /* Pull all c'tors into .text */
+ *crtbegin.o(.ctors)
+ *crtbegin?.o(.ctors)
+ *(EXCLUDE_FILE(*crtend?.o *crtend.o) .ctors)
+ *(SORT(.ctors.*))
+ *(.ctors)
+ /* Followed by destructors */
+ *crtbegin.o(.dtors)
+ *crtbegin?.o(.dtors)
+ *(EXCLUDE_FILE(*crtend?.o *crtend.o) .dtors)
+ *(SORT(.dtors.*))
+ *(.dtors)
+
+ *(.eh_frame*)
+ } > RAM
+
+ .rodata : {
+ *(.rodata*)
+ . = ALIGN(4);
+ *(SORT_BY_ALIGNMENT(SORT_BY_NAME(.flashdata*)))
+ . = ALIGN(4);
+ } > RAM
+
+ .ARM.extab :
+ {
+ *(.ARM.extab* .gnu.linkonce.armextab.*)
+ } > RAM
+
+ __exidx_start = .;
+ .ARM.exidx :
+ {
+ *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+ } > RAM
+ __exidx_end = .;
+
+ /* Machine inspectable binary information */
+ . = ALIGN(4);
+ __binary_info_start = .;
+ .binary_info :
+ {
+ KEEP(*(.binary_info.keep.*))
+ *(.binary_info.*)
+ } > RAM
+ __binary_info_end = .;
+ . = ALIGN(4);
+
+ .data : {
+ /* End of .text-like segments */
+ __etext = .;
+ __data_start__ = .;
+ *(vtable)
+ *(.data*)
+
+ . = ALIGN(4);
+ *(.after_data.*)
+
+ . = ALIGN(4);
+ /* preinit data */
+ PROVIDE_HIDDEN (__mutex_array_start = .);
+ KEEP(*(SORT(.mutex_array.*)))
+ KEEP(*(.mutex_array))
+ PROVIDE_HIDDEN (__mutex_array_end = .);
+
+ . = ALIGN(4);
+ /* preinit data */
+ PROVIDE_HIDDEN (__preinit_array_start = .);
+ KEEP(*(SORT(.preinit_array.*)))
+ KEEP(*(.preinit_array))
+ PROVIDE_HIDDEN (__preinit_array_end = .);
+
+ . = ALIGN(4);
+ /* init data */
+ PROVIDE_HIDDEN (__init_array_start = .);
+ KEEP(*(SORT(.init_array.*)))
+ KEEP(*(.init_array))
+ PROVIDE_HIDDEN (__init_array_end = .);
+
+ . = ALIGN(4);
+ /* finit data */
+ PROVIDE_HIDDEN (__fini_array_start = .);
+ *(SORT(.fini_array.*))
+ *(.fini_array)
+ PROVIDE_HIDDEN (__fini_array_end = .);
+
+ *(.jcr)
+ . = ALIGN(4);
+ /* All data end */
+ __data_end__ = .;
+ } > RAM
+
+ .uninitialized_data (COPY): {
+ . = ALIGN(4);
+ *(.uninitialized_data*)
+ } > RAM
+
+ /* Start and end symbols must be word-aligned */
+ .scratch_x : {
+ __scratch_x_start__ = .;
+ *(.scratch_x.*)
+ . = ALIGN(4);
+ __scratch_x_end__ = .;
+ } > SCRATCH_X
+ __scratch_x_source__ = LOADADDR(.scratch_x);
+
+ .scratch_y : {
+ __scratch_y_start__ = .;
+ *(.scratch_y.*)
+ . = ALIGN(4);
+ __scratch_y_end__ = .;
+ } > SCRATCH_Y
+ __scratch_y_source__ = LOADADDR(.scratch_y);
+
+ .bss : {
+ . = ALIGN(4);
+ __bss_start__ = .;
+ *(SORT_BY_ALIGNMENT(SORT_BY_NAME(.bss*)))
+ *(COMMON)
+ . = ALIGN(4);
+ __bss_end__ = .;
+ } > RAM
+
+ .heap (COPY):
+ {
+ __end__ = .;
+ end = __end__;
+ *(.heap*)
+ __HeapLimit = .;
+ } > RAM
+
+ /* .stack*_dummy section doesn't contains any symbols. It is only
+ * used for linker to calculate size of stack sections, and assign
+ * values to stack symbols later
+ *
+ * stack1 section may be empty/missing if platform_launch_core1 is not used */
+
+ /* by default we put core 0 stack at the end of scratch Y, so that if core 1
+ * stack is not used then all of SCRATCH_X is free.
+ */
+ .stack1_dummy (COPY):
+ {
+ *(.stack1*)
+ } > SCRATCH_X
+ .stack_dummy (COPY):
+ {
+ *(.stack*)
+ } > SCRATCH_Y
+
+ /* stack limit is poorly named, but historically is maximum heap ptr */
+ __StackLimit = ORIGIN(RAM) + LENGTH(RAM);
+ __StackOneTop = ORIGIN(SCRATCH_X) + LENGTH(SCRATCH_X);
+ __StackTop = ORIGIN(SCRATCH_Y) + LENGTH(SCRATCH_Y);
+ __StackOneBottom = __StackOneTop - SIZEOF(.stack1_dummy);
+ __StackBottom = __StackTop - SIZEOF(.stack_dummy);
+ PROVIDE(__stack = __StackTop);
+
+ /* Check if data + heap + stack exceeds RAM limit */
+ ASSERT(__StackLimit >= __HeapLimit, "region RAM overflowed")
+
+ ASSERT( __binary_info_header_end - __logical_binary_start <= 256, "Binary info must be in first 256 bytes of the binary")
+ /* todo assert on extra code */
+}
+
diff --git a/src/rp2_common/pico_standard_link/new_delete.cpp b/src/rp2_common/pico_standard_link/new_delete.cpp
new file mode 100644
index 0000000..c122dc0
--- /dev/null
+++ b/src/rp2_common/pico_standard_link/new_delete.cpp
@@ -0,0 +1,32 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#if !PICO_CXX_ENABLE_EXCEPTIONS
+// Override the standard allocators to use regular malloc/free
+
+#include <cstdlib>
+
+void *operator new(std::size_t n) {
+ return std::malloc(n);
+}
+
+void *operator new[](std::size_t n) {
+ return std::malloc(n);
+}
+
+void operator delete(void *p) { std::free(p); }
+
+void operator delete[](void *p) noexcept { std::free(p); }
+
+#if __cpp_sized_deallocation
+
+void operator delete(void *p, __unused std::size_t n) noexcept { std::free(p); }
+
+void operator delete[](void *p, __unused std::size_t n) noexcept { std::free(p); }
+
+#endif
+
+#endif
diff --git a/src/rp2_common/pico_stdio/CMakeLists.txt b/src/rp2_common/pico_stdio/CMakeLists.txt
new file mode 100644
index 0000000..cdc9c3b
--- /dev/null
+++ b/src/rp2_common/pico_stdio/CMakeLists.txt
@@ -0,0 +1,19 @@
+if (NOT TARGET pico_stdio)
+ pico_add_impl_library(pico_stdio)
+
+ target_include_directories(pico_stdio INTERFACE ${CMAKE_CURRENT_LIST_DIR}/include)
+
+ target_sources(pico_stdio INTERFACE
+ ${CMAKE_CURRENT_LIST_DIR}/stdio.c
+ )
+
+ pico_wrap_function(pico_stdio printf)
+ pico_wrap_function(pico_stdio vprintf)
+ pico_wrap_function(pico_stdio puts)
+ pico_wrap_function(pico_stdio putchar)
+ pico_wrap_function(pico_stdio getchar)
+
+ if (TARGET pico_printf)
+ target_link_libraries(pico_stdio INTERFACE pico_printf)
+ endif()
+endif()
\ No newline at end of file
diff --git a/src/rp2_common/pico_stdio/LICENSE b/src/rp2_common/pico_stdio/LICENSE
new file mode 100644
index 0000000..8f7ebd0
--- /dev/null
+++ b/src/rp2_common/pico_stdio/LICENSE
@@ -0,0 +1,22 @@
+The MIT License (MIT)
+
+Copyright (c) 2014 Marco Paland
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+
diff --git a/src/rp2_common/pico_stdio/include/pico/stdio.h b/src/rp2_common/pico_stdio/include/pico/stdio.h
new file mode 100644
index 0000000..e44c01d
--- /dev/null
+++ b/src/rp2_common/pico_stdio/include/pico/stdio.h
@@ -0,0 +1,120 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PICO_STDIO_H
+#define _PICO_STDIO_H
+
+/** \file stdio.h
+* \defgroup pico_stdio pico_stdio
+* Customized stdio support allowing for input and output from UART, USB, semi-hosting etc.
+*
+* Note the API for adding additional input output devices is not yet considered stable
+*/
+
+#include "pico.h"
+
+// PICO_CONFIG: PICO_STDOUT_MUTEX, Enable/disable mutex around stdout, type=bool, default=1, group=pico_stdio
+#ifndef PICO_STDOUT_MUTEX
+#define PICO_STDOUT_MUTEX 1
+#endif
+
+// PICO_CONFIG: PICO_STDIO_ENABLE_CRLF_SUPPORT, Enable/disable CR/LF output conversion support, type=bool, default=1, group=pico_stdio
+#ifndef PICO_STDIO_ENABLE_CRLF_SUPPORT
+#define PICO_STDIO_ENABLE_CRLF_SUPPORT 1
+#endif
+
+// PICO_CONFIG: PICO_STDIO_DEFAULT_CRLF, Default for CR/LF conversion enabled on all stdio outputs, type=bool, default=1, depends=PICO_STDIO_ENABLE_CRLF_SUPPORT, group=pico_stdio
+#ifndef PICO_STDIO_DEFAULT_CRLF
+#define PICO_STDIO_DEFAULT_CRLF 1
+#endif
+
+// PICO_CONFIG: PICO_STDIO_STACK_BUFFER_SIZE, Define printf buffer size (on stack)... this is just a working buffer not a max output size, min=0, max=512, default=128, group=pico_stdio
+#ifndef PICO_STDIO_STACK_BUFFER_SIZE
+#define PICO_STDIO_STACK_BUFFER_SIZE 128
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+typedef struct stdio_driver stdio_driver_t;
+
+/*! \brief Initialize all of the present standard stdio types that are linked into the binary.
+ * \ingroup pico_stdio
+ *
+ * Call this method once you have set up your clocks to enable the stdio support for UART, USB
+ * and semihosting based on the presence of the respective libraries in the binary.
+ *
+ * When stdio_usb is configured, this method can be optionally made to block, waiting for a connection
+ * via the variables specified in \ref stdio_usb_init (i.e. \ref PICO_STDIO_USB_CONNECT_WAIT_TIMEOUT_MS)
+ *
+ * \see stdio_uart, stdio_usb, stdio_semihosting
+ */
+void stdio_init_all(void);
+
+/*! \brief Initialize all of the present standard stdio types that are linked into the binary.
+ * \ingroup pico_stdio
+ *
+ * Call this method once you have set up your clocks to enable the stdio support for UART, USB
+ * and semihosting based on the presence of the respective libraries in the binary.
+ *
+ * \see stdio_uart, stdio_usb, stdio_semihosting
+ */
+void stdio_flush(void);
+
+/*! \brief Return a character from stdin if there is one available within a timeout
+ * \ingroup pico_stdio
+ *
+ * \param timeout_us the timeout in microseconds, or 0 to not wait for a character if none available.
+ * \return the character from 0-255 or PICO_ERROR_TIMEOUT if timeout occurs
+ */
+int getchar_timeout_us(uint32_t timeout_us);
+
+/*! \brief Adds or removes a driver from the list of active drivers used for input/output
+ * \ingroup pico_stdio
+ *
+ * \note this method should always be called on an initialized driver and is not re-entrant
+ * \param driver the driver
+ * \param enabled true to add, false to remove
+ */
+void stdio_set_driver_enabled(stdio_driver_t *driver, bool enabled);
+
+/*! \brief Control limiting of output to a single driver
+ * \ingroup pico_stdio
+ *
+ * \note this method should always be called on an initialized driver
+ *
+ * \param driver if non-null then output only that driver will be used for input/output (assuming it is in the list of enabled drivers).
+ * if NULL then all enabled drivers will be used
+ */
+void stdio_filter_driver(stdio_driver_t *driver);
+
+/*! \brief control conversion of line feeds to carriage return on transmissions
+ * \ingroup pico_stdio
+ *
+ * \note this method should always be called on an initialized driver
+ *
+ * \param driver the driver
+ * \param translate If true, convert line feeds to carriage return on transmissions
+ */
+void stdio_set_translate_crlf(stdio_driver_t *driver, bool translate);
+
+/*! \brief putchar variant that skips any CR/LF conversion if enabled
+ * \ingroup pico_stdio
+ */
+int putchar_raw(int c);
+
+/*! \brief puts variant that skips any CR/LF conversion if enabled
+ * \ingroup pico_stdio
+ */
+int puts_raw(const char *s);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/rp2_common/pico_stdio/include/pico/stdio/driver.h b/src/rp2_common/pico_stdio/include/pico/stdio/driver.h
new file mode 100644
index 0000000..1c8c27c
--- /dev/null
+++ b/src/rp2_common/pico_stdio/include/pico/stdio/driver.h
@@ -0,0 +1,24 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PICO_STDIO_DRIVER_H
+#define _PICO_STDIO_DRIVER_H
+
+#include "pico/stdio.h"
+#include "pico/platform.h"
+
+struct stdio_driver {
+ void (*out_chars)(const char *buf, int len);
+ void (*out_flush)(void);
+ int (*in_chars)(char *buf, int len);
+ stdio_driver_t *next;
+#if PICO_STDIO_ENABLE_CRLF_SUPPORT
+ bool last_ended_with_cr;
+ bool crlf_enabled;
+#endif
+};
+
+#endif
diff --git a/src/rp2_common/pico_stdio/stdio.c b/src/rp2_common/pico_stdio/stdio.c
new file mode 100644
index 0000000..b457b8a
--- /dev/null
+++ b/src/rp2_common/pico_stdio/stdio.c
@@ -0,0 +1,310 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <string.h>
+#include <stdio.h>
+#include <stdarg.h>
+
+#include "pico.h"
+#include "pico/mutex.h"
+#if LIB_PICO_PRINTF_PICO
+#include "pico/printf.h"
+#endif
+#include "pico/stdio.h"
+#include "pico/stdio/driver.h"
+#include "pico/time.h"
+
+#if LIB_PICO_STDIO_UART
+#include "pico/stdio_uart.h"
+#endif
+
+#if LIB_PICO_STDIO_USB
+#include "pico/stdio_usb.h"
+#endif
+
+#if LIB_PICO_STDIO_SEMIHOSTING
+#include "pico/stdio_semihosting.h"
+#endif
+
+static stdio_driver_t *drivers;
+static stdio_driver_t *filter;
+
+#if PICO_STDOUT_MUTEX
+auto_init_mutex(print_mutex);
+
+bool stdout_serialize_begin(void) {
+ lock_owner_id_t caller = lock_get_caller_owner_id();
+ // not using lock_owner_id_t to avoid backwards incompatibility change to mutex_try_enter API
+ static_assert(sizeof(lock_owner_id_t) <= 4, "");
+ uint32_t owner;
+ if (!mutex_try_enter(&print_mutex, &owner)) {
+ if (owner == (uint32_t)caller) {
+ return false;
+ }
+ // we are not a nested call, so lets wait
+ mutex_enter_blocking(&print_mutex);
+ }
+ return true;
+}
+
+void stdout_serialize_end(void) {
+ mutex_exit(&print_mutex);
+}
+
+#else
+static bool stdout_serialize_begin(void) {
+ return true;
+}
+static void stdout_serialize_end(void) {
+}
+#endif
+static void stdio_out_chars_no_crlf(stdio_driver_t *driver, const char *s, int len) {
+ driver->out_chars(s, len);
+}
+
+static void stdio_out_chars_crlf(stdio_driver_t *driver, const char *s, int len) {
+#if PICO_STDIO_ENABLE_CRLF_SUPPORT
+ if (!driver->crlf_enabled) {
+ driver->out_chars(s, len);
+ return;
+ }
+ int first_of_chunk = 0;
+ static const char crlf_str[] = {'\r', '\n'};
+ for (int i = 0; i < len; i++) {
+ bool prev_char_was_cr = i > 0 ? s[i - 1] == '\r' : driver->last_ended_with_cr;
+ if (s[i] == '\n' && !prev_char_was_cr) {
+ if (i > first_of_chunk) {
+ driver->out_chars(&s[first_of_chunk], i - first_of_chunk);
+ }
+ driver->out_chars(crlf_str, 2);
+ first_of_chunk = i + 1;
+ }
+ }
+ if (first_of_chunk < len) {
+ driver->out_chars(&s[first_of_chunk], len - first_of_chunk);
+ }
+ if (len > 0) {
+ driver->last_ended_with_cr = s[len - 1] == '\r';
+ }
+#else
+ driver->out_chars(s, len);
+#endif
+}
+
+static bool stdio_put_string(const char *s, int len, bool newline, bool no_cr) {
+ bool serialized = stdout_serialize_begin();
+ if (!serialized) {
+#if PICO_STDIO_IGNORE_NESTED_STDOUT
+ return false;
+#endif
+ }
+ if (len == -1) len = (int)strlen(s);
+ void (*out_func)(stdio_driver_t *, const char *, int) = no_cr ? stdio_out_chars_no_crlf : stdio_out_chars_crlf;
+ for (stdio_driver_t *driver = drivers; driver; driver = driver->next) {
+ if (!driver->out_chars) continue;
+ if (filter && filter != driver) continue;
+ out_func(driver, s, len);
+ if (newline) {
+ const char c = '\n';
+ out_func(driver, &c, 1);
+ }
+ }
+ if (serialized) {
+ stdout_serialize_end();
+ }
+ return len;
+}
+
+static int stdio_get_until(char *buf, int len, absolute_time_t until) {
+ do {
+ // todo round robin might be nice on each call, but then again hopefully
+ // no source will starve the others
+ for (stdio_driver_t *driver = drivers; driver; driver = driver->next) {
+ if (filter && filter != driver) continue;
+ if (driver->in_chars) {
+ int read = driver->in_chars(buf, len);
+ if (read > 0) {
+ return read;
+ }
+ }
+ }
+ // we sleep here in case the in_chars methods acquire mutexes or disable IRQs and
+ // potentially starve out what they are waiting on (have seen this with USB)
+ busy_wait_us(1);
+ } while (!time_reached(until));
+ return PICO_ERROR_TIMEOUT;
+}
+
+int WRAPPER_FUNC(putchar)(int c) {
+ char cc = (char)c;
+ stdio_put_string(&cc, 1, false, false);
+ return c;
+}
+
+int WRAPPER_FUNC(puts)(const char *s) {
+ int len = (int)strlen(s);
+ stdio_put_string(s, len, true, false);
+ stdio_flush();
+ return len;
+}
+
+int putchar_raw(int c) {
+ char cc = (char)c;
+ stdio_put_string(&cc, 1, false, true);
+ return c;
+}
+
+int puts_raw(const char *s) {
+ int len = (int)strlen(s);
+ stdio_put_string(s, len, true, true);
+ stdio_flush();
+ return len;
+}
+
+int _read(int handle, char *buffer, int length) {
+ if (handle == 0) {
+ return stdio_get_until(buffer, length, at_the_end_of_time);
+ }
+ return -1;
+}
+
+int _write(int handle, char *buffer, int length) {
+ if (handle == 1) {
+ stdio_put_string(buffer, length, false, false);
+ return length;
+ }
+ return -1;
+}
+
+void stdio_set_driver_enabled(stdio_driver_t *driver, bool enable) {
+ stdio_driver_t *prev = drivers;
+ for (stdio_driver_t *d = drivers; d; d = d->next) {
+ if (d == driver) {
+ if (!enable) {
+ prev->next = d->next;
+ driver->next = NULL;
+ }
+ return;
+ }
+ prev = d;
+ }
+ if (enable) {
+ if (prev) prev->next = driver;
+ else drivers = driver;
+ }
+}
+
+void stdio_flush() {
+ for (stdio_driver_t *d = drivers; d; d = d->next) {
+ if (d->out_flush) d->out_flush();
+ }
+}
+
+typedef struct stdio_stack_buffer {
+ int used;
+ char buf[PICO_STDIO_STACK_BUFFER_SIZE];
+} stdio_stack_buffer_t;
+
+static void stdio_stack_buffer_flush(stdio_stack_buffer_t *buffer) {
+ if (buffer->used) {
+ for (stdio_driver_t *d = drivers; d; d = d->next) {
+ if (!d->out_chars) continue;
+ if (filter && filter != d) continue;
+ stdio_out_chars_crlf(d, buffer->buf, buffer->used);
+ }
+ buffer->used = 0;
+ }
+}
+
+static void stdio_buffered_printer(char c, void *arg) {
+ stdio_stack_buffer_t *buffer = (stdio_stack_buffer_t *)arg;
+ if (buffer->used == PICO_STDIO_STACK_BUFFER_SIZE) {
+ stdio_stack_buffer_flush(buffer);
+ }
+ buffer->buf[buffer->used++] = c;
+}
+
+int WRAPPER_FUNC(vprintf)(const char *format, va_list va) {
+ bool serialzed = stdout_serialize_begin();
+ if (!serialzed) {
+#if PICO_STDIO_IGNORE_NESTED_STDOUT
+ return 0;
+#endif
+ }
+ int ret;
+#if LIB_PICO_PRINTF_PICO
+ struct stdio_stack_buffer buffer = {.used = 0};
+ ret = vfctprintf(stdio_buffered_printer, &buffer, format, va);
+ stdio_stack_buffer_flush(&buffer);
+ stdio_flush();
+#elif LIB_PICO_PRINTF_NONE
+ extern void printf_none_assert();
+ printf_none_assert();
+#else
+ extern int REAL_FUNC(vprintf)(const char *format, va_list va);
+ ret = REAL_FUNC(vprintf)(format, va);
+#endif
+ if (serialzed) {
+ stdout_serialize_end();
+ }
+ return ret;
+}
+
+int __printflike(1, 0) WRAPPER_FUNC(printf)(const char* format, ...)
+{
+ va_list va;
+ va_start(va, format);
+ int ret = vprintf(format, va);
+ va_end(va);
+ return ret;
+}
+
+void stdio_init_all(void) {
+ // todo add explicit custom, or registered although you can call stdio_enable_driver explicitly anyway
+ // These are well known ones
+#if LIB_PICO_STDIO_UART
+ stdio_uart_init();
+#endif
+
+#if LIB_PICO_STDIO_SEMIHOSTING
+ stdio_semihosting_init();
+#endif
+
+#if LIB_PICO_STDIO_USB
+ stdio_usb_init();
+#endif
+}
+
+int WRAPPER_FUNC(getchar)(void) {
+ char buf[1];
+ int len = stdio_get_until(buf, 1, at_the_end_of_time);
+ if (len < 0) return len;
+ assert(len == 1);
+ return (uint8_t)buf[0];
+}
+
+int getchar_timeout_us(uint32_t timeout_us) {
+ char buf[1];
+ int rc = stdio_get_until(buf, sizeof(buf), make_timeout_time_us(timeout_us));
+ if (rc < 0) return rc;
+ assert(rc);
+ return (uint8_t)buf[0];
+}
+
+void stdio_filter_driver(stdio_driver_t *driver) {
+ filter = driver;
+}
+
+void stdio_set_translate_crlf(stdio_driver_t *driver, bool enabled) {
+#if PICO_STDIO_ENABLE_CRLF_SUPPORT
+ if (enabled && !driver->crlf_enabled) {
+ driver->last_ended_with_cr = false;
+ }
+ driver->crlf_enabled = enabled;
+#else
+ panic_unsupported();
+#endif
+}
diff --git a/src/rp2_common/pico_stdio_semihosting/CMakeLists.txt b/src/rp2_common/pico_stdio_semihosting/CMakeLists.txt
new file mode 100644
index 0000000..699170e
--- /dev/null
+++ b/src/rp2_common/pico_stdio_semihosting/CMakeLists.txt
@@ -0,0 +1,9 @@
+pico_add_impl_library(pico_stdio_semihosting)
+
+target_sources(pico_stdio_semihosting INTERFACE
+ ${CMAKE_CURRENT_LIST_DIR}/stdio_semihosting.c
+)
+
+target_include_directories(pico_stdio_semihosting INTERFACE ${CMAKE_CURRENT_LIST_DIR}/include)
+
+target_link_libraries(pico_stdio_semihosting INTERFACE pico_stdio)
\ No newline at end of file
diff --git a/src/rp2_common/pico_stdio_semihosting/include/pico/stdio_semihosting.h b/src/rp2_common/pico_stdio_semihosting/include/pico/stdio_semihosting.h
new file mode 100644
index 0000000..3304368
--- /dev/null
+++ b/src/rp2_common/pico_stdio_semihosting/include/pico/stdio_semihosting.h
@@ -0,0 +1,42 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PICO_STDIO_SEMIHOSTING_H
+#define _PICO_STDIO_SEMIHOSTING_H
+
+#include "pico/stdio.h"
+
+/** \brief Experimental support for stdout using RAM semihosting
+ * \defgroup pico_stdio_semihosting pico_stdio_semihosting
+ * \ingroup pico_stdio
+ *
+ * Linking this library or calling `pico_enable_stdio_semihosting(TARGET)` in the CMake (which
+ * achieves the same thing) will add semihosting to the drivers used for standard output
+ */
+
+// PICO_CONFIG: PICO_STDIO_SEMIHOSTING_DEFAULT_CRLF, Default state of CR/LF translation for semihosting output, type=bool, default=PICO_STDIO_DEFAULT_CRLF, group=pico_stdio_semihosting
+#ifndef PICO_STDIO_SEMIHOSTING_DEFAULT_CRLF
+#define PICO_STDIO_SEMIHOSTING_DEFAULT_CRLF PICO_STDIO_DEFAULT_CRLF
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+extern stdio_driver_t stdio_semihosting;
+
+/*! \brief Explicitly initialize stdout over semihosting and add it to the current set of stdout targets
+ * \ingroup pico_stdio_semihosting
+ *
+ * \note this method is automatically called by \ref stdio_init_all() if `pico_stdio_semihosting` is included in the build
+ */
+void stdio_semihosting_init(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/rp2_common/pico_stdio_semihosting/stdio_semihosting.c b/src/rp2_common/pico_stdio_semihosting/stdio_semihosting.c
new file mode 100644
index 0000000..4eb673f
--- /dev/null
+++ b/src/rp2_common/pico_stdio_semihosting/stdio_semihosting.c
@@ -0,0 +1,51 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "pico/stdio/driver.h"
+#include "pico/stdio_semihosting.h"
+#include "pico/binary_info.h"
+
+//static void __attribute__((naked)) semihosting_puts(const char *s) {
+// __asm (
+//
+// "mov r1, r0\n"
+// "mov r0, #4\n"
+// "bkpt 0xab\n"
+// "bx lr\n"
+// );
+//}
+
+static void __attribute__((naked)) semihosting_putc(__unused const char *c) {
+ __asm (
+
+ "mov r1, r0\n"
+ "mov r0, #3\n"
+ "bkpt 0xab\n"
+ "bx lr\n"
+ );
+}
+
+
+static void stdio_semihosting_out_chars(const char *buf, int length) {
+ for (int i = 0; i < length; i++) {
+ semihosting_putc(&buf[i]);
+ }
+}
+
+stdio_driver_t stdio_semihosting = {
+ .out_chars = stdio_semihosting_out_chars,
+#if PICO_STDIO_ENABLE_CRLF_SUPPORT
+ .crlf_enabled = PICO_STDIO_SEMIHOSTING_DEFAULT_CRLF
+#endif
+};
+
+void stdio_semihosting_init() {
+#if !PICO_NO_BI_STDIO_SEMIHOSTING
+ bi_decl_if_func_used(bi_program_feature("semihosting stdout"));
+#endif
+ stdio_set_driver_enabled(&stdio_semihosting, true);
+}
+
diff --git a/src/rp2_common/pico_stdio_uart/CMakeLists.txt b/src/rp2_common/pico_stdio_uart/CMakeLists.txt
new file mode 100644
index 0000000..d10507b
--- /dev/null
+++ b/src/rp2_common/pico_stdio_uart/CMakeLists.txt
@@ -0,0 +1,9 @@
+pico_add_impl_library(pico_stdio_uart)
+
+target_sources(pico_stdio_uart INTERFACE
+ ${CMAKE_CURRENT_LIST_DIR}/stdio_uart.c
+)
+
+target_include_directories(pico_stdio_uart INTERFACE ${CMAKE_CURRENT_LIST_DIR}/include)
+
+target_link_libraries(pico_stdio_uart INTERFACE pico_stdio)
\ No newline at end of file
diff --git a/src/rp2_common/pico_stdio_uart/include/pico/stdio_uart.h b/src/rp2_common/pico_stdio_uart/include/pico/stdio_uart.h
new file mode 100644
index 0000000..5afe910
--- /dev/null
+++ b/src/rp2_common/pico_stdio_uart/include/pico/stdio_uart.h
@@ -0,0 +1,70 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PICO_STDIO_UART_H
+#define _PICO_STDIO_UART_H
+
+#include "pico/stdio.h"
+#include "hardware/uart.h"
+
+/** \brief Support for stdin/stdout using UART
+ * \defgroup pico_stdio_uart pico_stdio_uart
+ * \ingroup pico_stdio
+ *
+ * Linking this library or calling `pico_enable_stdio_uart(TARGET)` in the CMake (which
+ * achieves the same thing) will add UART to the drivers used for standard output
+ */
+
+// PICO_CONFIG: PICO_STDIO_UART_DEFAULT_CRLF, Default state of CR/LF translation for UART output, type=bool, default=PICO_STDIO_DEFAULT_CRLF, group=pico_stdio_uart
+#ifndef PICO_STDIO_UART_DEFAULT_CRLF
+#define PICO_STDIO_UART_DEFAULT_CRLF PICO_STDIO_DEFAULT_CRLF
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+extern stdio_driver_t stdio_uart;
+
+/*! \brief Explicitly initialize stdin/stdout over UART and add it to the current set of stdin/stdout drivers
+ * \ingroup pico_stdio_uart
+ *
+ * This method sets up PICO_DEFAULT_UART_TX_PIN for UART output (if defined), PICO_DEFAULT_UART_RX_PIN for input (if defined)
+ * and configures the baud rate as PICO_DEFAULT_UART_BAUD_RATE.
+ *
+ * \note this method is automatically called by \ref stdio_init_all() if `pico_stdio_uart` is included in the build
+ */
+void stdio_uart_init(void);
+
+/*! \brief Explicitly initialize stdout only (no stdin) over UART and add it to the current set of stdout drivers
+ * \ingroup pico_stdio_uart
+ *
+ * This method sets up PICO_DEFAULT_UART_TX_PIN for UART output (if defined) , and configures the baud rate as PICO_DEFAULT_UART_BAUD_RATE
+ */
+void stdout_uart_init(void);
+
+/*! \brief Explicitly initialize stdin only (no stdout) over UART and add it to the current set of stdin drivers
+ * \ingroup pico_stdio_uart
+ *
+ * This method sets up PICO_DEFAULT_UART_RX_PIN for UART input (if defined) , and configures the baud rate as PICO_DEFAULT_UART_BAUD_RATE
+ */
+void stdin_uart_init(void);
+
+/*! \brief Perform custom initialization initialize stdin/stdout over UART and add it to the current set of stdin/stdout drivers
+ * \ingroup pico_stdio_uart
+ *
+ * \param uart the uart instance to use, \ref uart0 or \ref uart1
+ * \param baud_rate the baud rate in Hz
+ * \param tx_pin the UART pin to use for stdout (or -1 for no stdout)
+ * \param rx_pin the UART pin to use for stdin (or -1 for no stdin)
+ */
+void stdio_uart_init_full(uart_inst_t *uart, uint baud_rate, int tx_pin, int rx_pin);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/rp2_common/pico_stdio_uart/stdio_uart.c b/src/rp2_common/pico_stdio_uart/stdio_uart.c
new file mode 100644
index 0000000..b8ff761
--- /dev/null
+++ b/src/rp2_common/pico_stdio_uart/stdio_uart.c
@@ -0,0 +1,99 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "pico/stdio/driver.h"
+#include "pico/stdio_uart.h"
+#include "pico/binary_info.h"
+#include "hardware/gpio.h"
+
+static uart_inst_t *uart_instance;
+
+#if PICO_NO_BI_STDIO_UART
+#define stdio_bi_decl_if_func_used(x)
+#else
+#define stdio_bi_decl_if_func_used bi_decl_if_func_used
+#endif
+
+void stdio_uart_init() {
+#ifdef uart_default
+ int tx_pin = -1;
+ int rx_pin = -1;
+#ifdef PICO_DEFAULT_UART_TX_PIN
+ tx_pin = PICO_DEFAULT_UART_TX_PIN;
+#ifdef PICO_DEFAULT_UART_RX_PIN
+ rx_pin = PICO_DEFAULT_UART_RX_PIN;
+ stdio_bi_decl_if_func_used(bi_program_feature("UART stdin / stdout"));
+ bi_decl_if_func_used(bi_2pins_with_func(PICO_DEFAULT_UART_RX_PIN, PICO_DEFAULT_UART_TX_PIN, GPIO_FUNC_UART));
+#else
+ stdio_bi_decl_if_func_used(bi_program_feature("UART stdout"));
+ bi_decl_if_func_used(bi_1pin_with_func(PICO_DEFAULT_UART_TX_PIN, GPIO_FUNC_UART));
+#endif
+#elif defined(PICO_DEFAULT_UART_RX_PIN)
+ rx_pin = PICO_DEFAULT_UART_RX_PIN;
+ stdio_bi_decl_if_func_used(bi_program_feature("UART stdin"));
+ bi_decl_if_func_used(bi_1pin_with_func(PICO_DEFAULT_UART_RX_PIN, GPIO_FUNC_UART));
+#endif
+#if !defined(PICO_DEFAULT_UART_BAUD_RATE)
+ panic("UART baud rate undefined");
+#else
+ stdio_uart_init_full(uart_default, PICO_DEFAULT_UART_BAUD_RATE, tx_pin, rx_pin);
+#endif
+#endif
+}
+
+void stdout_uart_init() {
+#if defined(uart_default) && defined(PICO_DEFAULT_UART_TX_PIN)
+ bi_decl_if_func_used(bi_1pin_with_func(PICO_DEFAULT_UART_TX_PIN, GPIO_FUNC_UART));
+#if !defined(PICO_DEFAULT_UART_BAUD_RATE)
+ panic("UART baud rate undefined");
+#else
+ stdio_bi_decl_if_func_used(bi_program_feature("UART stdout"));
+ stdio_uart_init_full(uart_default, PICO_DEFAULT_UART_BAUD_RATE, PICO_DEFAULT_UART_TX_PIN, -1);
+#endif
+#endif
+}
+
+void stdin_uart_init() {
+#if defined(uart_default) && defined(PICO_DEFAULT_UART_RX_PIN)
+ bi_decl_if_func_used(bi_1pin_with_func(PICO_DEFAULT_UART_RX_PIN, GPIO_FUNC_UART));
+#if !defined(PICO_DEFAULT_UART_BAUD_RATE)
+ panic("UART baud rate undefined");
+#else
+ stdio_bi_decl_if_func_used(bi_program_feature("UART stdin"));
+ stdio_uart_init_full(uart_default, PICO_DEFAULT_UART_BAUD_RATE, -1, PICO_DEFAULT_UART_RX_PIN);
+#endif
+#endif
+}
+
+void stdio_uart_init_full(struct uart_inst *uart, uint baud_rate, int tx_pin, int rx_pin) {
+ uart_instance = uart;
+ uart_init(uart_instance, baud_rate);
+ if (tx_pin >= 0) gpio_set_function((uint)tx_pin, GPIO_FUNC_UART);
+ if (rx_pin >= 0) gpio_set_function((uint)rx_pin, GPIO_FUNC_UART);
+ stdio_set_driver_enabled(&stdio_uart, true);
+}
+
+static void stdio_uart_out_chars(const char *buf, int length) {
+ for (int i = 0; i <length; i++) {
+ uart_putc(uart_instance, buf[i]);
+ }
+}
+
+int stdio_uart_in_chars(char *buf, int length) {
+ int i=0;
+ while (i<length && uart_is_readable(uart_instance)) {
+ buf[i++] = uart_getc(uart_instance);
+ }
+ return i ? i : PICO_ERROR_NO_DATA;
+}
+
+stdio_driver_t stdio_uart = {
+ .out_chars = stdio_uart_out_chars,
+ .in_chars = stdio_uart_in_chars,
+#if PICO_STDIO_ENABLE_CRLF_SUPPORT
+ .crlf_enabled = PICO_STDIO_UART_DEFAULT_CRLF
+#endif
+};
diff --git a/src/rp2_common/pico_stdio_usb/CMakeLists.txt b/src/rp2_common/pico_stdio_usb/CMakeLists.txt
new file mode 100644
index 0000000..b6abadd
--- /dev/null
+++ b/src/rp2_common/pico_stdio_usb/CMakeLists.txt
@@ -0,0 +1,19 @@
+if (TARGET tinyusb_device_unmarked)
+ pico_add_impl_library(pico_stdio_usb)
+
+ target_include_directories(pico_stdio_usb INTERFACE ${CMAKE_CURRENT_LIST_DIR}/include)
+
+ target_sources(pico_stdio_usb INTERFACE
+ ${CMAKE_CURRENT_LIST_DIR}/reset_interface.c
+ ${CMAKE_CURRENT_LIST_DIR}/stdio_usb.c
+ ${CMAKE_CURRENT_LIST_DIR}/stdio_usb_descriptors.c
+ )
+
+ target_link_libraries(pico_stdio_usb INTERFACE
+ tinyusb_device_unmarked
+ pico_stdio
+ pico_time
+ pico_unique_id
+ pico_usb_reset_interface_headers
+ )
+endif()
diff --git a/src/rp2_common/pico_stdio_usb/include/pico/stdio_usb.h b/src/rp2_common/pico_stdio_usb/include/pico/stdio_usb.h
new file mode 100644
index 0000000..2aeb647
--- /dev/null
+++ b/src/rp2_common/pico_stdio_usb/include/pico/stdio_usb.h
@@ -0,0 +1,127 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PICO_STDIO_USB_H
+#define _PICO_STDIO_USB_H
+
+#include "pico/stdio.h"
+
+/** \brief Support for stdin/stdout over USB serial (CDC)
+ * \defgroup pico_stdio_usb pico_stdio_usb
+ * \ingroup pico_stdio
+ *
+ * Linking this library or calling `pico_enable_stdio_usb(TARGET)` in the CMake (which
+ * achieves the same thing) will add USB CDC to the drivers used for standard output
+ *
+ * Note this library is a developer convenience. It is not applicable in all cases; for one it takes full control of the USB device precluding your
+ * use of the USB in device or host mode. For this reason, this library will automatically disengage if you try to using it alongside \ref tinyusb_device or
+ * \ref tinyusb_host. It also takes control of a lower level IRQ and sets up a periodic background task.
+ *
+ * This library also includes (by default) functionality to enable the RP2040 to be reset over the USB interface.
+ */
+
+// PICO_CONFIG: PICO_STDIO_USB_DEFAULT_CRLF, Default state of CR/LF translation for USB output, type=bool, default=PICO_STDIO_DEFAULT_CRLF, group=pico_stdio_usb
+#ifndef PICO_STDIO_USB_DEFAULT_CRLF
+#define PICO_STDIO_USB_DEFAULT_CRLF PICO_STDIO_DEFAULT_CRLF
+#endif
+
+// PICO_CONFIG: PICO_STDIO_USB_STDOUT_TIMEOUT_US, Number of microseconds to be blocked trying to write USB output before assuming the host has disappeared and discarding data, default=500000, group=pico_stdio_usb
+#ifndef PICO_STDIO_USB_STDOUT_TIMEOUT_US
+#define PICO_STDIO_USB_STDOUT_TIMEOUT_US 500000
+#endif
+
+// todo perhaps unnecessarily frequent?
+// PICO_CONFIG: PICO_STDIO_USB_TASK_INTERVAL_US, Period of microseconds between calling tud_task in the background, default=1000, advanced=true, group=pico_stdio_usb
+#ifndef PICO_STDIO_USB_TASK_INTERVAL_US
+#define PICO_STDIO_USB_TASK_INTERVAL_US 1000
+#endif
+
+// PICO_CONFIG: PICO_STDIO_USB_LOW_PRIORITY_IRQ, low priority (non hardware) IRQ number to claim for tud_task() background execution, default=31, advanced=true, group=pico_stdio_usb
+#ifndef PICO_STDIO_USB_LOW_PRIORITY_IRQ
+#define PICO_STDIO_USB_LOW_PRIORITY_IRQ 31
+#endif
+
+// PICO_CONFIG: PICO_STDIO_USB_ENABLE_RESET_VIA_BAUD_RATE, Enable/disable resetting into BOOTSEL mode if the host sets the baud rate to a magic value (PICO_STDIO_USB_RESET_MAGIC_BAUD_RATE), type=bool, default=1, group=pico_stdio_usb
+#ifndef PICO_STDIO_USB_ENABLE_RESET_VIA_BAUD_RATE
+#define PICO_STDIO_USB_ENABLE_RESET_VIA_BAUD_RATE 1
+#endif
+
+// PICO_CONFIG: PICO_STDIO_USB_RESET_MAGIC_BAUD_RATE, baud rate that if selected causes a reset into BOOTSEL mode (if PICO_STDIO_USB_ENABLE_RESET_VIA_BAUD_RATE is set), default=1200, group=pico_stdio_usb
+#ifndef PICO_STDIO_USB_RESET_MAGIC_BAUD_RATE
+#define PICO_STDIO_USB_RESET_MAGIC_BAUD_RATE 1200
+#endif
+
+// PICO_CONFIG: PICO_STDIO_USB_CONNECT_WAIT_TIMEOUT_MS, Maximum number of milliseconds to wait during initialization for a CDC connection from the host (negative means indefinite) during initialization, default=0, group=pico_stdio_usb
+#ifndef PICO_STDIO_USB_CONNECT_WAIT_TIMEOUT_MS
+#define PICO_STDIO_USB_CONNECT_WAIT_TIMEOUT_MS 0
+#endif
+
+// PICO_CONFIG: PICO_STDIO_USB_POST_CONNECT_WAIT_DELAY_MS, Number of extra milliseconds to wait when using PICO_STDIO_USB_CONNECT_WAIT_TIMEOUT_MS after a host CDC connection is detected (some host terminals seem to sometimes lose transmissions sent right after connection), default=50, group=pico_stdio_usb
+#ifndef PICO_STDIO_USB_POST_CONNECT_WAIT_DELAY_MS
+#define PICO_STDIO_USB_POST_CONNECT_WAIT_DELAY_MS 50
+#endif
+
+// PICO_CONFIG: PICO_STDIO_USB_RESET_BOOTSEL_ACTIVITY_LED, Optionally define a pin to use as bootloader activity LED when BOOTSEL mode is entered via USB (either VIA_BAUD_RATE or VIA_VENDOR_INTERFACE), type=int, min=0, max=29, group=pico_stdio_usb
+
+// PICO_CONFIG: PICO_STDIO_USB_RESET_BOOTSEL_FIXED_ACTIVITY_LED, Whether the pin specified by PICO_STDIO_USB_RESET_BOOTSEL_ACTIVITY_LED is fixed or can be modified by picotool over the VENDOR USB interface, type=bool, default=0, group=pico_stdio_usb
+#ifndef PICO_STDIO_USB_RESET_BOOTSEL_FIXED_ACTIVITY_LED
+#define PICO_STDIO_USB_RESET_BOOTSEL_FIXED_ACTIVITY_LED 0
+#endif
+
+// Any modes disabled here can't be re-enabled by picotool via VENDOR_INTERFACE.
+// PICO_CONFIG: PICO_STDIO_USB_RESET_BOOTSEL_INTERFACE_DISABLE_MASK, Optionally disable either the mass storage interface (bit 0) or the PICOBOOT interface (bit 1) when entering BOOTSEL mode via USB (either VIA_BAUD_RATE or VIA_VENDOR_INTERFACE), type=int, min=0, max=3, default=0, group=pico_stdio_usb
+#ifndef PICO_STDIO_USB_RESET_BOOTSEL_INTERFACE_DISABLE_MASK
+#define PICO_STDIO_USB_RESET_BOOTSEL_INTERFACE_DISABLE_MASK 0u
+#endif
+
+// PICO_CONFIG: PICO_STDIO_USB_ENABLE_RESET_VIA_VENDOR_INTERFACE, Enable/disable resetting into BOOTSEL mode via an additional VENDOR USB interface - enables picotool based reset, type=bool, default=1, group=pico_stdio_usb
+#ifndef PICO_STDIO_USB_ENABLE_RESET_VIA_VENDOR_INTERFACE
+#define PICO_STDIO_USB_ENABLE_RESET_VIA_VENDOR_INTERFACE 1
+#endif
+
+// PICO_CONFIG: PICO_STDIO_USB_RESET_INTERFACE_SUPPORT_RESET_TO_BOOTSEL, If vendor reset interface is included allow rebooting to BOOTSEL mode, type=bool, default=1, group=pico_stdio_usb
+#ifndef PICO_STDIO_USB_RESET_INTERFACE_SUPPORT_RESET_TO_BOOTSEL
+#define PICO_STDIO_USB_RESET_INTERFACE_SUPPORT_RESET_TO_BOOTSEL 1
+#endif
+
+// PICO_CONFIG: PICO_STDIO_USB_RESET_INTERFACE_SUPPORT_RESET_TO_FLASH_BOOT, If vendor reset interface is included allow rebooting with regular flash boot, type=bool, default=1, group=pico_stdio_usb
+#ifndef PICO_STDIO_USB_RESET_INTERFACE_SUPPORT_RESET_TO_FLASH_BOOT
+#define PICO_STDIO_USB_RESET_INTERFACE_SUPPORT_RESET_TO_FLASH_BOOT 1
+#endif
+
+// PICO_CONFIG: PICO_STDIO_USB_RESET_RESET_TO_FLASH_DELAY_MS, delays in ms before rebooting via regular flash boot, default=100, group=pico_stdio_usb
+#ifndef PICO_STDIO_USB_RESET_RESET_TO_FLASH_DELAY_MS
+#define PICO_STDIO_USB_RESET_RESET_TO_FLASH_DELAY_MS 100
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+extern stdio_driver_t stdio_usb;
+
+/*! \brief Explicitly initialize USB stdio and add it to the current set of stdin drivers
+ * \ingroup pico_stdio_usb
+ *
+ * \ref PICO_STDIO_USB_CONNECT_WAIT_TIMEOUT_MS can be set to cause this method to wait for a CDC connection
+ * from the host before returning, which is useful if you don't want any initial stdout output to be discarded
+ * before the connection is established.
+ *
+ * \return true if the USB CDC was initialized, false if an error occurred
+ */
+bool stdio_usb_init(void);
+
+/*! \brief Check if there is an active stdio CDC connection to a host
+ * \ingroup pico_stdio_usb
+ *
+ * \return true if stdio is connected over CDC
+ */
+bool stdio_usb_connected(void);
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/rp2_common/pico_stdio_usb/include/pico/stdio_usb/reset_interface.h b/src/rp2_common/pico_stdio_usb/include/pico/stdio_usb/reset_interface.h
new file mode 100644
index 0000000..e0cca10
--- /dev/null
+++ b/src/rp2_common/pico_stdio_usb/include/pico/stdio_usb/reset_interface.h
@@ -0,0 +1,13 @@
+/*
+ * Copyright (c) 2021 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PICO_STDIO_USB_RESET_INTERFACE_H
+#define _PICO_STDIO_USB_RESET_INTERFACE_H
+
+// definitions have been moved here
+#include "pico/usb_reset_interface.h"
+
+#endif
\ No newline at end of file
diff --git a/src/rp2_common/pico_stdio_usb/include/tusb_config.h b/src/rp2_common/pico_stdio_usb/include/tusb_config.h
new file mode 100644
index 0000000..c0a2843
--- /dev/null
+++ b/src/rp2_common/pico_stdio_usb/include/tusb_config.h
@@ -0,0 +1,40 @@
+/*
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ * Copyright (c) 2020 Damien P. George
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ */
+
+#ifndef _PICO_STDIO_USB_TUSB_CONFIG_H
+#define _PICO_STDIO_USB_TUSB_CONFIG_H
+
+#include "pico/stdio_usb.h"
+
+#define CFG_TUSB_RHPORT0_MODE (OPT_MODE_DEVICE)
+
+#define CFG_TUD_CDC (1)
+#define CFG_TUD_CDC_RX_BUFSIZE (256)
+#define CFG_TUD_CDC_TX_BUFSIZE (256)
+
+// We use a vendor specific interface but with our own driver
+#define CFG_TUD_VENDOR (0)
+#endif
diff --git a/src/rp2_common/pico_stdio_usb/reset_interface.c b/src/rp2_common/pico_stdio_usb/reset_interface.c
new file mode 100644
index 0000000..06dce86
--- /dev/null
+++ b/src/rp2_common/pico_stdio_usb/reset_interface.c
@@ -0,0 +1,112 @@
+/**
+ * Copyright (c) 2021 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#include "tusb.h"
+
+#include "pico/bootrom.h"
+
+#if PICO_STDIO_USB_ENABLE_RESET_VIA_VENDOR_INTERFACE && !(PICO_STDIO_USB_RESET_INTERFACE_SUPPORT_RESET_TO_BOOTSEL || PICO_STDIO_USB_RESET_INTERFACE_SUPPORT_RESET_TO_FLASH_BOOT)
+#warning PICO_STDIO_USB_ENABLE_RESET_VIA_VENDOR_INTERFACE has been selected but neither PICO_STDIO_USB_RESET_INTERFACE_SUPPORT_RESET_TO_BOOTSEL nor PICO_STDIO_USB_RESET_INTERFACE_SUPPORT_RESET_TO_FLASH_BOOT have been selected.
+#endif
+
+#if PICO_STDIO_USB_ENABLE_RESET_VIA_VENDOR_INTERFACE
+#include "pico/stdio_usb/reset_interface.h"
+#include "hardware/watchdog.h"
+#include "device/usbd_pvt.h"
+
+static uint8_t itf_num;
+
+static void resetd_init(void) {
+}
+
+static void resetd_reset(uint8_t __unused rhport) {
+ itf_num = 0;
+}
+
+static uint16_t resetd_open(uint8_t __unused rhport, tusb_desc_interface_t const *itf_desc, uint16_t max_len) {
+ TU_VERIFY(TUSB_CLASS_VENDOR_SPECIFIC == itf_desc->bInterfaceClass &&
+ RESET_INTERFACE_SUBCLASS == itf_desc->bInterfaceSubClass &&
+ RESET_INTERFACE_PROTOCOL == itf_desc->bInterfaceProtocol, 0);
+
+ uint16_t const drv_len = sizeof(tusb_desc_interface_t);
+ TU_VERIFY(max_len >= drv_len, 0);
+
+ itf_num = itf_desc->bInterfaceNumber;
+ return drv_len;
+}
+
+// Support for parameterized reset via vendor interface control request
+static bool resetd_control_xfer_cb(uint8_t __unused rhport, uint8_t stage, tusb_control_request_t const * request) {
+ // nothing to do with DATA & ACK stage
+ if (stage != CONTROL_STAGE_SETUP) return true;
+
+ if (request->wIndex == itf_num) {
+
+#if PICO_STDIO_USB_RESET_INTERFACE_SUPPORT_RESET_TO_BOOTSEL
+ if (request->bRequest == RESET_REQUEST_BOOTSEL) {
+#ifdef PICO_STDIO_USB_RESET_BOOTSEL_ACTIVITY_LED
+ uint gpio_mask = 1u << PICO_STDIO_USB_RESET_BOOTSEL_ACTIVITY_LED;
+#else
+ uint gpio_mask = 0u;
+#endif
+#if !PICO_STDIO_USB_RESET_BOOTSEL_FIXED_ACTIVITY_LED
+ if (request->wValue & 0x100) {
+ gpio_mask = 1u << (request->wValue >> 9u);
+ }
+#endif
+ reset_usb_boot(gpio_mask, (request->wValue & 0x7f) | PICO_STDIO_USB_RESET_BOOTSEL_INTERFACE_DISABLE_MASK);
+ // does not return, otherwise we'd return true
+ }
+#endif
+
+#if PICO_STDIO_USB_RESET_INTERFACE_SUPPORT_RESET_TO_FLASH_BOOT
+ if (request->bRequest == RESET_REQUEST_FLASH) {
+ watchdog_reboot(0, 0, PICO_STDIO_USB_RESET_RESET_TO_FLASH_DELAY_MS);
+ return true;
+ }
+#endif
+
+ }
+ return false;
+}
+
+static bool resetd_xfer_cb(uint8_t __unused rhport, uint8_t __unused ep_addr, xfer_result_t __unused result, uint32_t __unused xferred_bytes) {
+ return true;
+}
+
+static usbd_class_driver_t const _resetd_driver =
+{
+#if CFG_TUSB_DEBUG >= 2
+ .name = "RESET",
+#endif
+ .init = resetd_init,
+ .reset = resetd_reset,
+ .open = resetd_open,
+ .control_xfer_cb = resetd_control_xfer_cb,
+ .xfer_cb = resetd_xfer_cb,
+ .sof = NULL
+};
+
+// Implement callback to add our custom driver
+usbd_class_driver_t const *usbd_app_driver_get_cb(uint8_t *driver_count) {
+ *driver_count = 1;
+ return &_resetd_driver;
+}
+#endif
+
+#if PICO_STDIO_USB_ENABLE_RESET_VIA_BAUD_RATE
+// Support for default BOOTSEL reset by changing baud rate
+void tud_cdc_line_coding_cb(__unused uint8_t itf, cdc_line_coding_t const* p_line_coding) {
+ if (p_line_coding->bit_rate == PICO_STDIO_USB_RESET_MAGIC_BAUD_RATE) {
+#ifdef PICO_STDIO_USB_RESET_BOOTSEL_ACTIVITY_LED
+ const uint gpio_mask = 1u << PICO_STDIO_USB_RESET_BOOTSEL_ACTIVITY_LED;
+#else
+ const uint gpio_mask = 0u;
+#endif
+ reset_usb_boot(gpio_mask, PICO_STDIO_USB_RESET_BOOTSEL_INTERFACE_DISABLE_MASK);
+ }
+}
+#endif
+
diff --git a/src/rp2_common/pico_stdio_usb/stdio_usb.c b/src/rp2_common/pico_stdio_usb/stdio_usb.c
new file mode 100644
index 0000000..913d606
--- /dev/null
+++ b/src/rp2_common/pico_stdio_usb/stdio_usb.c
@@ -0,0 +1,135 @@
+/**
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#if !defined(LIB_TINYUSB_HOST) && !defined(LIB_TINYUSB_DEVICE)
+#include "tusb.h"
+
+#include "pico/time.h"
+#include "pico/stdio/driver.h"
+#include "pico/binary_info.h"
+#include "pico/mutex.h"
+#include "hardware/irq.h"
+
+static_assert(PICO_STDIO_USB_LOW_PRIORITY_IRQ > RTC_IRQ, ""); // note RTC_IRQ is currently the last one
+static mutex_t stdio_usb_mutex;
+
+static void low_priority_worker_irq(void) {
+ // if the mutex is already owned, then we are in user code
+ // in this file which will do a tud_task itself, so we'll just do nothing
+ // until the next tick; we won't starve
+ if (mutex_try_enter(&stdio_usb_mutex, NULL)) {
+ tud_task();
+ mutex_exit(&stdio_usb_mutex);
+ }
+}
+
+static int64_t timer_task(__unused alarm_id_t id, __unused void *user_data) {
+ irq_set_pending(PICO_STDIO_USB_LOW_PRIORITY_IRQ);
+ return PICO_STDIO_USB_TASK_INTERVAL_US;
+}
+
+static void stdio_usb_out_chars(const char *buf, int length) {
+ static uint64_t last_avail_time;
+ uint32_t owner;
+ if (!mutex_try_enter(&stdio_usb_mutex, &owner)) {
+ if (owner == get_core_num()) return; // would deadlock otherwise
+ mutex_enter_blocking(&stdio_usb_mutex);
+ }
+ if (tud_cdc_connected()) {
+ for (int i = 0; i < length;) {
+ int n = length - i;
+ int avail = (int) tud_cdc_write_available();
+ if (n > avail) n = avail;
+ if (n) {
+ int n2 = (int) tud_cdc_write(buf + i, (uint32_t)n);
+ tud_task();
+ tud_cdc_write_flush();
+ i += n2;
+ last_avail_time = time_us_64();
+ } else {
+ tud_task();
+ tud_cdc_write_flush();
+ if (!tud_cdc_connected() ||
+ (!tud_cdc_write_available() && time_us_64() > last_avail_time + PICO_STDIO_USB_STDOUT_TIMEOUT_US)) {
+ break;
+ }
+ }
+ }
+ } else {
+ // reset our timeout
+ last_avail_time = 0;
+ }
+ mutex_exit(&stdio_usb_mutex);
+}
+
+int stdio_usb_in_chars(char *buf, int length) {
+ uint32_t owner;
+ if (!mutex_try_enter(&stdio_usb_mutex, &owner)) {
+ if (owner == get_core_num()) return PICO_ERROR_NO_DATA; // would deadlock otherwise
+ mutex_enter_blocking(&stdio_usb_mutex);
+ }
+ int rc = PICO_ERROR_NO_DATA;
+ if (tud_cdc_connected() && tud_cdc_available()) {
+ int count = (int) tud_cdc_read(buf, (uint32_t) length);
+ rc = count ? count : PICO_ERROR_NO_DATA;
+ }
+ mutex_exit(&stdio_usb_mutex);
+ return rc;
+}
+
+stdio_driver_t stdio_usb = {
+ .out_chars = stdio_usb_out_chars,
+ .in_chars = stdio_usb_in_chars,
+#if PICO_STDIO_ENABLE_CRLF_SUPPORT
+ .crlf_enabled = PICO_STDIO_USB_DEFAULT_CRLF
+#endif
+};
+
+bool stdio_usb_init(void) {
+#if !PICO_NO_BI_STDIO_USB
+ bi_decl_if_func_used(bi_program_feature("USB stdin / stdout"));
+#endif
+
+ // initialize TinyUSB
+ tusb_init();
+
+ irq_set_exclusive_handler(PICO_STDIO_USB_LOW_PRIORITY_IRQ, low_priority_worker_irq);
+ irq_set_enabled(PICO_STDIO_USB_LOW_PRIORITY_IRQ, true);
+
+ mutex_init(&stdio_usb_mutex);
+ bool rc = add_alarm_in_us(PICO_STDIO_USB_TASK_INTERVAL_US, timer_task, NULL, true);
+ if (rc) {
+ stdio_set_driver_enabled(&stdio_usb, true);
+#if PICO_STDIO_USB_CONNECT_WAIT_TIMEOUT_MS
+#if PICO_STDIO_USB_CONNECT_WAIT_TIMEOUT_MS > 0
+ absolute_time_t until = make_timeout_time_ms(PICO_STDIO_USB_CONNECT_WAIT_TIMEOUT_MS);
+#else
+ absolute_time_t until = at_the_end_of_time;
+#endif
+ do {
+ if (stdio_usb_connected()) {
+#if PICO_STDIO_USB_POST_CONNECT_WAIT_DELAY_MS != 0
+ sleep_ms(PICO_STDIO_USB_POST_CONNECT_WAIT_DELAY_MS);
+#endif
+ break;
+ }
+ sleep_ms(10);
+ } while (!time_reached(until));
+#endif
+ }
+ return rc;
+}
+
+bool stdio_usb_connected(void) {
+ return tud_cdc_connected();
+}
+#else
+#include "pico/stdio_usb.h"
+#warning stdio USB was configured, but is being disabled as TinyUSB is explicitly linked
+bool stdio_usb_init(void) {
+ return false;
+}
+#endif
diff --git a/src/rp2_common/pico_stdio_usb/stdio_usb_descriptors.c b/src/rp2_common/pico_stdio_usb/stdio_usb_descriptors.c
new file mode 100644
index 0000000..b3cfed0
--- /dev/null
+++ b/src/rp2_common/pico_stdio_usb/stdio_usb_descriptors.c
@@ -0,0 +1,151 @@
+/*
+ * This file is based on a file originally part of the
+ * MicroPython project, http://micropython.org/
+ *
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ * Copyright (c) 2019 Damien P. George
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#if !defined(LIB_TINYUSB_HOST) && !defined(LIB_TINYUSB_DEVICE)
+
+#include "tusb.h"
+#include "pico/stdio_usb/reset_interface.h"
+#include "pico/unique_id.h"
+
+#define USBD_VID (0x2E8A) // Raspberry Pi
+#define USBD_PID (0x000a) // Raspberry Pi Pico SDK CDC
+
+#define TUD_RPI_RESET_DESC_LEN 9
+#if !PICO_STDIO_USB_ENABLE_RESET_VIA_VENDOR_INTERFACE
+#define USBD_DESC_LEN (TUD_CONFIG_DESC_LEN + TUD_CDC_DESC_LEN)
+#else
+#define USBD_DESC_LEN (TUD_CONFIG_DESC_LEN + TUD_CDC_DESC_LEN + TUD_RPI_RESET_DESC_LEN)
+#endif
+#define USBD_MAX_POWER_MA (250)
+
+#define USBD_ITF_CDC (0) // needs 2 interfaces
+#if !PICO_STDIO_USB_ENABLE_RESET_VIA_VENDOR_INTERFACE
+#define USBD_ITF_MAX (2)
+#else
+#define USBD_ITF_RPI_RESET (2)
+#define USBD_ITF_MAX (3)
+#endif
+
+#define USBD_CDC_EP_CMD (0x81)
+#define USBD_CDC_EP_OUT (0x02)
+#define USBD_CDC_EP_IN (0x82)
+#define USBD_CDC_CMD_MAX_SIZE (8)
+#define USBD_CDC_IN_OUT_MAX_SIZE (64)
+
+#define USBD_STR_0 (0x00)
+#define USBD_STR_MANUF (0x01)
+#define USBD_STR_PRODUCT (0x02)
+#define USBD_STR_SERIAL (0x03)
+#define USBD_STR_CDC (0x04)
+#define USBD_STR_RPI_RESET (0x05)
+
+// Note: descriptors returned from callbacks must exist long enough for transfer to complete
+
+static const tusb_desc_device_t usbd_desc_device = {
+ .bLength = sizeof(tusb_desc_device_t),
+ .bDescriptorType = TUSB_DESC_DEVICE,
+ .bcdUSB = 0x0200,
+ .bDeviceClass = TUSB_CLASS_MISC,
+ .bDeviceSubClass = MISC_SUBCLASS_COMMON,
+ .bDeviceProtocol = MISC_PROTOCOL_IAD,
+ .bMaxPacketSize0 = CFG_TUD_ENDPOINT0_SIZE,
+ .idVendor = USBD_VID,
+ .idProduct = USBD_PID,
+ .bcdDevice = 0x0100,
+ .iManufacturer = USBD_STR_MANUF,
+ .iProduct = USBD_STR_PRODUCT,
+ .iSerialNumber = USBD_STR_SERIAL,
+ .bNumConfigurations = 1,
+};
+
+#define TUD_RPI_RESET_DESCRIPTOR(_itfnum, _stridx) \
+ /* Interface */\
+ 9, TUSB_DESC_INTERFACE, _itfnum, 0, 0, TUSB_CLASS_VENDOR_SPECIFIC, RESET_INTERFACE_SUBCLASS, RESET_INTERFACE_PROTOCOL, _stridx,
+
+static const uint8_t usbd_desc_cfg[USBD_DESC_LEN] = {
+ TUD_CONFIG_DESCRIPTOR(1, USBD_ITF_MAX, USBD_STR_0, USBD_DESC_LEN,
+ 0, USBD_MAX_POWER_MA),
+
+ TUD_CDC_DESCRIPTOR(USBD_ITF_CDC, USBD_STR_CDC, USBD_CDC_EP_CMD,
+ USBD_CDC_CMD_MAX_SIZE, USBD_CDC_EP_OUT, USBD_CDC_EP_IN, USBD_CDC_IN_OUT_MAX_SIZE),
+
+#if PICO_STDIO_USB_ENABLE_RESET_VIA_VENDOR_INTERFACE
+ TUD_RPI_RESET_DESCRIPTOR(USBD_ITF_RPI_RESET, USBD_STR_RPI_RESET)
+#endif
+};
+
+static char usbd_serial_str[PICO_UNIQUE_BOARD_ID_SIZE_BYTES * 2 + 1];
+
+static const char *const usbd_desc_str[] = {
+ [USBD_STR_MANUF] = "Raspberry Pi",
+ [USBD_STR_PRODUCT] = "Pico",
+ [USBD_STR_SERIAL] = usbd_serial_str,
+ [USBD_STR_CDC] = "Board CDC",
+#if PICO_STDIO_USB_ENABLE_RESET_VIA_VENDOR_INTERFACE
+ [USBD_STR_RPI_RESET] = "Reset",
+#endif
+};
+
+const uint8_t *tud_descriptor_device_cb(void) {
+ return (const uint8_t *)&usbd_desc_device;
+}
+
+const uint8_t *tud_descriptor_configuration_cb(__unused uint8_t index) {
+ return usbd_desc_cfg;
+}
+
+const uint16_t *tud_descriptor_string_cb(uint8_t index, __unused uint16_t langid) {
+ #define DESC_STR_MAX (20)
+ static uint16_t desc_str[DESC_STR_MAX];
+
+ // Assign the SN using the unique flash id
+ if (!usbd_serial_str[0]) {
+ pico_get_unique_board_id_string(usbd_serial_str, sizeof(usbd_serial_str));
+ }
+
+ uint8_t len;
+ if (index == 0) {
+ desc_str[1] = 0x0409; // supported language is English
+ len = 1;
+ } else {
+ if (index >= sizeof(usbd_desc_str) / sizeof(usbd_desc_str[0])) {
+ return NULL;
+ }
+ const char *str = usbd_desc_str[index];
+ for (len = 0; len < DESC_STR_MAX - 1 && str[len]; ++len) {
+ desc_str[1 + len] = str[len];
+ }
+ }
+
+ // first byte is length (including header), second byte is string type
+ desc_str[0] = (uint16_t) ((TUSB_DESC_STRING << 8) | (2 * len + 2));
+
+ return desc_str;
+}
+
+#endif
diff --git a/src/rp2_common/pico_stdlib/CMakeLists.txt b/src/rp2_common/pico_stdlib/CMakeLists.txt
new file mode 100644
index 0000000..b54639b
--- /dev/null
+++ b/src/rp2_common/pico_stdlib/CMakeLists.txt
@@ -0,0 +1,45 @@
+# PICO_CMAKE_CONFIG: PICO_STDIO_UART, OPTION: Globally enable stdio UART, default=1, group=pico_stdlib
+option(PICO_STDIO_UART "Globablly enable stdio UART" 1)
+# PICO_CMAKE_CONFIG: PICO_STDIO_USB, OPTION: Globally enable stdio USB, default=0, group=pico_stdlib
+option(PICO_STDIO_USB "Globablly enable stdio USB" 0)
+# PICO_CMAKE_CONFIG: PICO_STDIO_USB, OPTIONS: Globally enable stdio semihosting, default=0, group=pico_stdlib
+option(PICO_STDIO_USB "Globablly enable stdio semihosting " 0)
+
+if (NOT TARGET pico_stdlib)
+ pico_add_impl_library(pico_stdlib)
+ target_sources(pico_stdlib INTERFACE
+ ${CMAKE_CURRENT_LIST_DIR}/stdlib.c
+ )
+ target_link_libraries(pico_stdlib INTERFACE
+ pico_stdlib_headers
+ pico_platform
+ pico_runtime
+ pico_stdio
+ pico_time
+ )
+
+ function(pico_enable_stdio_uart TARGET ENABLED)
+ set_target_properties(${TARGET} PROPERTIES PICO_TARGET_STDIO_UART ${ENABLED})
+ endfunction()
+
+ function(pico_enable_stdio_usb TARGET ENABLED)
+ set_target_properties(${TARGET} PROPERTIES PICO_TARGET_STDIO_USB ${ENABLED})
+ endfunction()
+
+ function(pico_enable_stdio_semihosting TARGET ENABLED)
+ set_target_properties(${TARGET} PROPERTIES PICO_TARGET_STDIO_SEMIHOSTING ${ENABLED})
+ endfunction()
+
+ if (TARGET pico_stdio_uart)
+ target_link_libraries(pico_stdlib INTERFACE $<IF:$<BOOL:$<IF:$<STREQUAL:$<TARGET_PROPERTY:PICO_TARGET_STDIO_UART>,>,${PICO_STDIO_UART},$<TARGET_PROPERTY:PICO_TARGET_STDIO_UART>>>,pico_stdio_uart,>)
+ endif()
+
+ if (TARGET pico_stdio_usb)
+ target_link_libraries(pico_stdlib INTERFACE $<IF:$<BOOL:$<IF:$<STREQUAL:$<TARGET_PROPERTY:PICO_TARGET_STDIO_USB>,>,${PICO_STDIO_USB},$<TARGET_PROPERTY:PICO_TARGET_STDIO_USB>>>,pico_stdio_usb,>)
+ endif()
+
+ if (TARGET pico_stdio_semihosting)
+ target_link_libraries(pico_stdlib INTERFACE $<IF:$<BOOL:$<IF:$<STREQUAL:$<TARGET_PROPERTY:PICO_TARGET_STDIO_SEMIHOSTING>,>,${PICO_STDIO_SEMIHOSTING},$<TARGET_PROPERTY:PICO_TARGET_STDIO_SEMIHOSTING>>>,pico_stdio_semihosting,>)
+ endif()
+
+endif()
diff --git a/src/rp2_common/pico_stdlib/stdlib.c b/src/rp2_common/pico_stdlib/stdlib.c
new file mode 100644
index 0000000..7c9854e
--- /dev/null
+++ b/src/rp2_common/pico_stdlib/stdlib.c
@@ -0,0 +1,104 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "pico/stdlib.h"
+#include "hardware/pll.h"
+#include "hardware/clocks.h"
+#if LIB_PICO_STDIO_UART
+#include "pico/stdio_uart.h"
+#else
+#include "pico/binary_info.h"
+#endif
+
+// everything running off the USB oscillator
+void set_sys_clock_48mhz() {
+ if (!running_on_fpga()) {
+ // Change clk_sys to be 48MHz. The simplest way is to take this from PLL_USB
+ // which has a source frequency of 48MHz
+ clock_configure(clk_sys,
+ CLOCKS_CLK_SYS_CTRL_SRC_VALUE_CLKSRC_CLK_SYS_AUX,
+ CLOCKS_CLK_SYS_CTRL_AUXSRC_VALUE_CLKSRC_PLL_USB,
+ 48 * MHZ,
+ 48 * MHZ);
+
+ // Turn off PLL sys for good measure
+ pll_deinit(pll_sys);
+
+ // CLK peri is clocked from clk_sys so need to change clk_peri's freq
+ clock_configure(clk_peri,
+ 0,
+ CLOCKS_CLK_PERI_CTRL_AUXSRC_VALUE_CLK_SYS,
+ 48 * MHZ,
+ 48 * MHZ);
+ }
+}
+
+void set_sys_clock_pll(uint32_t vco_freq, uint post_div1, uint post_div2) {
+ if (!running_on_fpga()) {
+ clock_configure(clk_sys,
+ CLOCKS_CLK_SYS_CTRL_SRC_VALUE_CLKSRC_CLK_SYS_AUX,
+ CLOCKS_CLK_SYS_CTRL_AUXSRC_VALUE_CLKSRC_PLL_USB,
+ 48 * MHZ,
+ 48 * MHZ);
+
+ pll_init(pll_sys, 1, vco_freq, post_div1, post_div2);
+ uint32_t freq = vco_freq / (post_div1 * post_div2);
+
+ // Configure clocks
+ // CLK_REF = XOSC (12MHz) / 1 = 12MHz
+ clock_configure(clk_ref,
+ CLOCKS_CLK_REF_CTRL_SRC_VALUE_XOSC_CLKSRC,
+ 0, // No aux mux
+ 12 * MHZ,
+ 12 * MHZ);
+
+ // CLK SYS = PLL SYS (125MHz) / 1 = 125MHz
+ clock_configure(clk_sys,
+ CLOCKS_CLK_SYS_CTRL_SRC_VALUE_CLKSRC_CLK_SYS_AUX,
+ CLOCKS_CLK_SYS_CTRL_AUXSRC_VALUE_CLKSRC_PLL_SYS,
+ freq, freq);
+
+ clock_configure(clk_peri,
+ 0, // Only AUX mux on ADC
+ CLOCKS_CLK_PERI_CTRL_AUXSRC_VALUE_CLKSRC_PLL_USB,
+ 48 * MHZ,
+ 48 * MHZ);
+ }
+}
+
+bool check_sys_clock_khz(uint32_t freq_khz, uint *vco_out, uint *postdiv1_out, uint *postdiv_out) {
+ uint crystal_freq_khz = clock_get_hz(clk_ref) / 1000;
+ for (uint fbdiv = 320; fbdiv >= 16; fbdiv--) {
+ uint vco = fbdiv * crystal_freq_khz;
+ if (vco < 400000 || vco > 1600000) continue;
+ for (uint postdiv1 = 7; postdiv1 >= 1; postdiv1--) {
+ for (uint postdiv2 = postdiv1; postdiv2 >= 1; postdiv2--) {
+ uint out = vco / (postdiv1 * postdiv2);
+ if (out == freq_khz && !(vco % (postdiv1 * postdiv2))) {
+ *vco_out = vco * 1000;
+ *postdiv1_out = postdiv1;
+ *postdiv_out = postdiv2;
+ return true;
+ }
+ }
+ }
+ }
+ return false;
+}
+
+void setup_default_uart() {
+#if LIB_PICO_STDIO_UART
+ stdio_uart_init();
+#elif defined(PICO_DEFAULT_UART_BAUD_RATE) && defined(PICO_DEFAULT_UART_TX_PIN) && defined(PICO_DEFAULT_UART_RX_PIN)
+ // this is mostly for backwards compatibility - stdio_uart_init is a bit more nuanced, and usually likely to be present
+ uart_init(uart_default, PICO_DEFAULT_UART_BAUD_RATE);
+ if (PICO_DEFAULT_UART_TX_PIN >= 0)
+ gpio_set_function(PICO_DEFAULT_UART_TX_PIN, GPIO_FUNC_UART);
+ if (PICO_DEFAULT_UART_RX_PIN >= 0)
+ gpio_set_function(PICO_DEFAULT_UART_RX_PIN, GPIO_FUNC_UART);
+ bi_decl_if_func_used(bi_2pins_with_func(PICO_DEFAULT_UART_RX_PIN, PICO_DEFAULT_UART_TX_PIN, GPIO_FUNC_UART));
+#endif
+}
diff --git a/src/rp2_common/pico_unique_id/CMakeLists.txt b/src/rp2_common/pico_unique_id/CMakeLists.txt
new file mode 100644
index 0000000..4c69074
--- /dev/null
+++ b/src/rp2_common/pico_unique_id/CMakeLists.txt
@@ -0,0 +1,9 @@
+pico_add_impl_library(pico_unique_id)
+
+target_sources(pico_unique_id INTERFACE
+ ${CMAKE_CURRENT_LIST_DIR}/unique_id.c
+)
+
+target_include_directories(pico_unique_id INTERFACE ${CMAKE_CURRENT_LIST_DIR}/include)
+
+target_link_libraries(pico_unique_id INTERFACE hardware_flash)
diff --git a/src/rp2_common/pico_unique_id/include/pico/unique_id.h b/src/rp2_common/pico_unique_id/include/pico/unique_id.h
new file mode 100644
index 0000000..4132a60
--- /dev/null
+++ b/src/rp2_common/pico_unique_id/include/pico/unique_id.h
@@ -0,0 +1,81 @@
+/*
+ * Copyright (c) 2021 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PICO_UNIQUE_ID_H_
+#define _PICO_UNIQUE_ID_H_
+
+#include "pico.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** \file pico/unique_id.h
+ * \defgroup pico_unique_id pico_unique_id
+ *
+ * Unique device ID access API
+ *
+ * RP2040 does not have an on-board unique identifier (all instances of RP2040
+ * silicon are identical and have no persistent state). However, RP2040 boots
+ * from serial NOR flash devices which have a 64-bit unique ID as a standard
+ * feature, and there is a 1:1 association between RP2040 and flash, so this
+ * is suitable for use as a unique identifier for an RP2040-based board.
+ *
+ * This library injects a call to the flash_get_unique_id function from the
+ * hardware_flash library, to run before main, and stores the result in a
+ * static location which can safely be accessed at any time via
+ * pico_get_unique_id().
+ *
+ * This avoids some pitfalls of the hardware_flash API, which requires any
+ * flash-resident interrupt routines to be disabled when called into.
+ */
+
+#define PICO_UNIQUE_BOARD_ID_SIZE_BYTES 8
+
+/**
+ * \brief Unique board identifier
+ * \ingroup pico_unique_id
+ *
+ * This struct is suitable for holding the unique identifier of a NOR flash
+ * device on an RP2040-based board. It contains an array of
+ * PICO_UNIQUE_BOARD_ID_SIZE_BYTES identifier bytes.
+ */
+typedef struct {
+ uint8_t id[PICO_UNIQUE_BOARD_ID_SIZE_BYTES];
+} pico_unique_board_id_t;
+
+/*! \brief Get unique ID
+ * \ingroup pico_unique_id
+ *
+ * Get the unique 64-bit device identifier which was retrieved from the
+ * external NOR flash device at boot.
+ *
+ * On PICO_NO_FLASH builds the unique identifier is set to all 0xEE.
+ *
+ * \param id_out a pointer to a pico_unique_board_id_t struct, to which the identifier will be written
+ */
+void pico_get_unique_board_id(pico_unique_board_id_t *id_out);
+
+/*! \brief Get unique ID in string format
+ * \ingroup pico_unique_id
+ *
+ * Get the unique 64-bit device identifier which was retrieved from the
+ * external NOR flash device at boot, formatted as an ASCII hex string.
+ * Will always 0-terminate.
+ *
+ * On PICO_NO_FLASH builds the unique identifier is set to all 0xEE.
+ *
+ * \param id_out a pointer to a char buffer of size len, to which the identifier will be written
+ * \param len the size of id_out. For full serial, len >= 2 * PICO_UNIQUE_BOARD_ID_SIZE_BYTES + 1
+ */
+void pico_get_unique_board_id_string(char *id_out, uint len);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/rp2_common/pico_unique_id/unique_id.c b/src/rp2_common/pico_unique_id/unique_id.c
new file mode 100644
index 0000000..2e652c8
--- /dev/null
+++ b/src/rp2_common/pico_unique_id/unique_id.c
@@ -0,0 +1,39 @@
+/*
+ * Copyright (c) 2021 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "hardware/flash.h"
+#include "pico/unique_id.h"
+
+static_assert(PICO_UNIQUE_BOARD_ID_SIZE_BYTES == FLASH_UNIQUE_ID_SIZE_BYTES, "Board ID size must match flash ID size");
+
+static pico_unique_board_id_t retrieved_id;
+
+static void __attribute__((constructor)) _retrieve_unique_id_on_boot(void) {
+#if PICO_NO_FLASH
+ // The hardware_flash call will panic() if called directly on a NO_FLASH
+ // build. Since this constructor is pre-main it would be annoying to
+ // debug, so just produce something well-defined and obviously wrong.
+ for (int i = 0; i < PICO_UNIQUE_BOARD_ID_SIZE_BYTES; i++)
+ retrieved_id.id[i] = 0xee;
+#else
+ flash_get_unique_id(retrieved_id.id);
+#endif
+}
+
+void pico_get_unique_board_id(pico_unique_board_id_t *id_out) {
+ *id_out = retrieved_id;
+}
+
+void pico_get_unique_board_id_string(char *id_out, uint len) {
+ assert(len > 0);
+ size_t i;
+ // Generate hex one nibble at a time
+ for (i = 0; (i < len - 1) && (i < PICO_UNIQUE_BOARD_ID_SIZE_BYTES * 2); i++) {
+ int nibble = (retrieved_id.id[i/2] >> (4 - 4 * (i&1))) & 0xf;
+ id_out[i] = (char)(nibble < 10 ? nibble + '0' : nibble + 'A' - 10);
+ }
+ id_out[i] = 0;
+}
diff --git a/src/rp2_common/tinyusb/CMakeLists.txt b/src/rp2_common/tinyusb/CMakeLists.txt
new file mode 100644
index 0000000..80a6e78
--- /dev/null
+++ b/src/rp2_common/tinyusb/CMakeLists.txt
@@ -0,0 +1,49 @@
+if (DEFINED ENV{PICO_TINYUSB_PATH} AND (NOT PICO_TINYUSB_PATH))
+ set(PICO_TINYUSB_PATH $ENV{PICO_TINYUSB_PATH})
+ message("Using PICO_TINYUSB_PATH from environment ('${PICO_TINYUSB_PATH}')")
+endif ()
+
+set(TINYUSB_TEST_PATH "src/portable/raspberrypi/rp2040")
+if (NOT PICO_TINYUSB_PATH)
+ set(PICO_TINYUSB_PATH ${PROJECT_SOURCE_DIR}/lib/tinyusb)
+ if (NOT EXISTS ${PICO_TINYUSB_PATH}/${TINYUSB_TEST_PATH})
+ message(WARNING "TinyUSB submodule has not been initialized; USB support will be unavailable
+hint: try 'git submodule update --init' from your SDK directory (${PICO_SDK_PATH}).")
+ endif()
+elseif (NOT EXISTS ${PICO_TINYUSB_PATH}/${TINYUSB_TEST_PATH})
+ message(WARNING "PICO_TINYUSB_PATH specified but content not present.")
+endif()
+
+if (EXISTS ${PICO_TINYUSB_PATH}/${TINYUSB_TEST_PATH})
+ message("TinyUSB available at ${PICO_TINYUSB_PATH}/${TINYUSB_TEST_PATH}; enabling build support for USB.")
+
+ pico_register_common_scope_var(PICO_TINYUSB_PATH)
+
+ set(BOARD pico_sdk)
+ set(FAMILY rp2040)
+ include(${PICO_TINYUSB_PATH}/hw/bsp/family_support.cmake)
+
+ add_library(tinyusb_common INTERFACE)
+ target_link_libraries(tinyusb_common INTERFACE tinyusb_common_base)
+
+ add_library(tinyusb_device_unmarked INTERFACE)
+ target_link_libraries(tinyusb_device_unmarked INTERFACE tinyusb_device_base)
+ target_compile_definitions(tinyusb_device_unmarked INTERFACE
+ # off by default note TUD_OPT_RP2040_USB_DEVICE_ENUMERATION_FIX defaults from PICO_RP2040_USB_DEVICE_ENUMERATION_FIX
+# TUD_OPT_RP2040_USB_DEVICE_ENUMERATION_FIX=1
+ )
+
+ # unmarked version used by stdio USB
+ target_link_libraries(tinyusb_device_unmarked INTERFACE tinyusb_common pico_fix_rp2040_usb_device_enumeration tinyusb_device_base)
+
+ pico_add_impl_library(tinyusb_device)
+ target_link_libraries(tinyusb_device INTERFACE tinyusb_device_unmarked)
+
+ pico_add_impl_library(tinyusb_host)
+ target_link_libraries(tinyusb_host INTERFACE tinyusb_host_base tinyusb_common)
+
+ pico_add_impl_library(tinyusb_board)
+ target_link_libraries(tinyusb_board INTERFACE tinyusb_bsp)
+
+ pico_promote_common_scope_vars()
+endif()
diff --git a/src/rp2_common/tinyusb/doc.h b/src/rp2_common/tinyusb/doc.h
new file mode 100644
index 0000000..6c361e0
--- /dev/null
+++ b/src/rp2_common/tinyusb/doc.h
@@ -0,0 +1,7 @@
+/**
+ * \defgroup tinyusb_device tinyusb_device
+ * \brief <a href="https://github.com/hathach/tinyusb">TinyUSB</a> Device-mode support for the RP2040
+ *
+ * \defgroup tinyusb_host tinyusb_host
+ * \brief <a href="https://github.com/hathach/tinyusb">TinyUSB</a> Host-mode support for the RP2040
+ */