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/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