From 159d55215057a627713748192897d21a5da731e2 Mon Sep 17 00:00:00 2001
From: Graham Sanderson <graham.sanderson@raspberrypi.com>
Date: Mon, 16 May 2022 13:44:15 -0500
Subject: [PATCH] Fix bug in irq_remove_shared_handler and add test #823 (#825)

* Fix bug in irq_remove_shared_handler and add test #823

* Add comments to irq_handler_chain.S

Co-authored-by: Luke Wren <wren6991@gmail.com>
---
 .../hardware_irq/include/hardware/irq.h       |   6 +
 src/rp2_common/hardware_irq/irq.c             |   3 +-
 .../hardware_irq/irq_handler_chain.S          |  17 +-
 test/CMakeLists.txt                           |   1 +
 test/hardware_irq_test/CMakeLists.txt         |   4 +
 test/hardware_irq_test/hardware_irq_test.c    | 282 ++++++++++++++++++
 test/kitchen_sink/kitchen_sink.c              |  43 ---
 7 files changed, 303 insertions(+), 53 deletions(-)
 create mode 100644 test/hardware_irq_test/CMakeLists.txt
 create mode 100644 test/hardware_irq_test/hardware_irq_test.c

diff --git a/src/rp2_common/hardware_irq/include/hardware/irq.h b/src/rp2_common/hardware_irq/include/hardware/irq.h
index 1c37c33..f4451b8 100644
--- a/src/rp2_common/hardware_irq/include/hardware/irq.h
+++ b/src/rp2_common/hardware_irq/include/hardware/irq.h
@@ -103,6 +103,9 @@
 #define PICO_SHARED_IRQ_HANDLER_DEFAULT_ORDER_PRIORITY 0x80
 #endif
 
+#define PICO_SHARED_IRQ_HANDLER_HIGHEST_ORDER_PRIORITY 0xff
+#define PICO_SHARED_IRQ_HANDLER_LOWEST_ORDER_PRIORITY 0x00
+
 // 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
@@ -224,6 +227,9 @@ irq_handler_t irq_get_exclusive_handler(uint num);
  * 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.
  *
+ * \note The order_priority uses \em higher values for higher priorities which is the \em opposite of the CPU interrupt priorities passed
+ * to irq_set_priority() which use lower values for higher priorities.
+ *
  * \see irq_set_exclusive_handler()
  */
 void irq_add_shared_handler(uint num, irq_handler_t handler, uint8_t order_priority);
diff --git a/src/rp2_common/hardware_irq/irq.c b/src/rp2_common/hardware_irq/irq.c
index 770ed49..def098b 100644
--- a/src/rp2_common/hardware_irq/irq.c
+++ b/src/rp2_common/hardware_irq/irq.c
@@ -287,7 +287,6 @@ void irq_remove_handler(uint num, irq_handler_t handler) {
             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;
@@ -325,7 +324,7 @@ void irq_remove_handler(uint num, irq_handler_t handler) {
                         }
                         // 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;
+                        irq_hander_chain_free_slot_head = get_slot_index(to_free_slot);
                     } 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.
diff --git a/src/rp2_common/hardware_irq/irq_handler_chain.S b/src/rp2_common/hardware_irq/irq_handler_chain.S
index 7d7be7d..ca15292 100644
--- a/src/rp2_common/hardware_irq/irq_handler_chain.S
+++ b/src/rp2_common/hardware_irq/irq_handler_chain.S
@@ -54,17 +54,18 @@ irq_handler_chain_slots:
 .endr
 
 irq_handler_chain_first_slot:
-    push {lr}
-    ldr  r0, [r1, #4]
-    adds r1, #1
-    mov  lr, r1
-    bx   r0
+    push {lr}         // Save EXC_RETURN token, so `pop {pc}` will return from interrupt
+    ldr  r0, [r1, #4] // Get `handler` field of irq_handler_chain_slot
+    adds r1, #1       // r1 points to `inst3` field of slot struct. Set Thumb bit on r1,
+    mov  lr, r1       // and copy to lr, so `inst3` is executed on return from handler
+    bx   r0           // Enter handler
+
 irq_handler_chain_remove_tail:
-    mov  r0, lr
-    subs r0, #9
+    mov  r0, lr       // Get start of struct. This function was called by a bl at offset +4,
+    subs r0, #9       // so lr points to offset +8. Note also lr has its Thumb bit set!
     ldr  r1, =irq_add_tail_to_free_list
     blx  r1
-    pop  {pc}
+    pop  {pc}         // Top of stack is EXC_RETURN
 
 
 #endif
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 7ed2a00..7396605 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -6,6 +6,7 @@ add_subdirectory(pico_divider_test)
 if (PICO_ON_DEVICE)
     add_subdirectory(pico_float_test)
     add_subdirectory(kitchen_sink)
+    add_subdirectory(hardware_irq_test)
     add_subdirectory(hardware_pwm_test)
     add_subdirectory(cmsis_test)
 endif()
\ No newline at end of file
diff --git a/test/hardware_irq_test/CMakeLists.txt b/test/hardware_irq_test/CMakeLists.txt
new file mode 100644
index 0000000..90ef50a
--- /dev/null
+++ b/test/hardware_irq_test/CMakeLists.txt
@@ -0,0 +1,4 @@
+add_executable(hardware_irq_test hardware_irq_test.c)
+
+target_link_libraries(hardware_irq_test PRIVATE pico_test hardware_irq hardware_dma)
+pico_add_extra_outputs(hardware_irq_test)
\ No newline at end of file
diff --git a/test/hardware_irq_test/hardware_irq_test.c b/test/hardware_irq_test/hardware_irq_test.c
new file mode 100644
index 0000000..ed46a32
--- /dev/null
+++ b/test/hardware_irq_test/hardware_irq_test.c
@@ -0,0 +1,282 @@
+/**
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdio.h>
+#include <stdarg.h>
+#include "pico/stdlib.h"
+#include "pico/test.h"
+#include "pico/time.h"
+#include "hardware/irq.h"
+#include "hardware/dma.h"
+#include "hardware/structs/scb.h"
+
+PICOTEST_MODULE_NAME("IRQ", "IRQ Handler test");
+
+extern void __unhandled_user_irq(void);
+
+static bool remove_handler1_in_dma;
+static bool remove_handler2_in_dma;
+static bool remove_handler3_in_dma;
+static dma_channel_config config;
+
+#define MAX_FIRE_COUNT 4
+static int fire_count;
+static int fired[MAX_FIRE_COUNT];
+
+static uint32_t dma_to = 0;
+static uint32_t dma_from = 0xaaaa5555;
+
+int record_fire(int which) {
+    PICOTEST_CHECK(fire_count < MAX_FIRE_COUNT, "too many firings");
+    fired[fire_count++] = which;
+    return 0;
+}
+
+void __isr handler1(void) {
+    record_fire(1);
+    dma_channel_acknowledge_irq0(0);
+    if (remove_handler1_in_dma) {
+        irq_remove_handler(DMA_IRQ_0, handler1);
+        remove_handler1_in_dma = false;
+    }
+}
+
+void __isr handler2(void) {
+    record_fire(2);
+    dma_channel_acknowledge_irq0(0);
+    if (remove_handler2_in_dma) {
+        irq_remove_handler(DMA_IRQ_0, handler2);
+        remove_handler2_in_dma = false;
+    }
+}
+
+void __isr handler3(void) {
+    record_fire(3);
+    dma_channel_acknowledge_irq0(0);
+    if (remove_handler3_in_dma) {
+        irq_remove_handler(DMA_IRQ_0, handler3);
+        remove_handler3_in_dma = false;
+    }
+}
+
+static inline irq_handler_t *get_vtable(void) {
+    return (irq_handler_t *) scb_hw->vtor;
+}
+
+int dma_check(int expected, ...) {
+    if (expected == 0) {
+        // doing the DMA if there are no IRQ handlers will cause a hard fault, so we just check we are pointing at the handler which does this.
+        PICOTEST_CHECK_AND_ABORT(get_vtable()[16 + DMA_IRQ_0] == __unhandled_user_irq, "Expected there to be no IRQ handlers");
+        return 0;
+    }
+    fire_count = 0;
+    dma_channel_configure(0, &config, &dma_to, &dma_from, 1, true);
+    sleep_ms(100);
+    va_list args;
+    va_start(args, expected);
+    bool ok = expected == fire_count;
+    for(int i=0;ok && i<expected;i++) {
+        if (fired[i] != va_arg(args, int)) {
+            ok = false;
+            break;
+        }
+    }
+    va_end(args);
+    if (!ok) {
+        PICOTEST_CHECK(ok, "DMA handlers were not called in the order expected");
+        printf("  EXPECTED handlers: ");
+        va_start(args, expected);
+        for(int i=0;i<expected;i++) {
+            if (i) printf(", ");
+            printf("%d", va_arg(args, int));
+        }
+        printf("\n");
+        va_end(args);
+        printf("  CALLED handlers:   ");
+        for(int i=0;i<fire_count;i++) {
+            if (i) printf(", ");
+            printf("%d", fired[i]);
+        }
+        printf("\n");
+        return -1;
+    }
+    return 0;
+}
+
+int main() {
+    stdio_init_all();
+
+    PICOTEST_START();
+    config = dma_channel_get_default_config(0);
+    dma_channel_set_irq0_enabled(0, true);
+    irq_set_enabled(DMA_IRQ_0, true);
+
+    // part I, just add/remove two handlers
+    PICOTEST_START_SECTION("I: Add handler1 default priority");
+        irq_add_shared_handler(DMA_IRQ_0, handler1, PICO_SHARED_IRQ_HANDLER_DEFAULT_ORDER_PRIORITY);
+        dma_check(1, 1);
+    PICOTEST_END_SECTION();
+
+    PICOTEST_START_SECTION("I: Add handler2 default priority");
+        irq_add_shared_handler(DMA_IRQ_0, handler2, PICO_SHARED_IRQ_HANDLER_DEFAULT_ORDER_PRIORITY);
+        dma_check(2, 2, 1);
+    PICOTEST_END_SECTION();
+
+    PICOTEST_START_SECTION("I: Remove handler1");
+        irq_remove_handler(DMA_IRQ_0, handler1);
+        dma_check(1, 2);
+    PICOTEST_END_SECTION();
+
+    PICOTEST_START_SECTION("I: Remove handler2");
+        irq_remove_handler(DMA_IRQ_0, handler2);
+        dma_check(0);
+    PICOTEST_END_SECTION();
+
+    // part II, add/remove three handlers including one in the middle
+
+    PICOTEST_START_SECTION("II: Add handler3 default priority");
+        irq_add_shared_handler(DMA_IRQ_0, handler3, PICO_SHARED_IRQ_HANDLER_DEFAULT_ORDER_PRIORITY);
+        dma_check(1, 3);
+    PICOTEST_END_SECTION();
+
+    PICOTEST_START_SECTION("II: Add handler2 default priority");
+        irq_add_shared_handler(DMA_IRQ_0, handler2, PICO_SHARED_IRQ_HANDLER_DEFAULT_ORDER_PRIORITY);
+        dma_check(2, 2, 3);
+    PICOTEST_END_SECTION();
+
+    PICOTEST_START_SECTION("II: Add handler1 default priority");
+        irq_add_shared_handler(DMA_IRQ_0, handler1, PICO_SHARED_IRQ_HANDLER_DEFAULT_ORDER_PRIORITY);
+        dma_check(3, 1, 2, 3);
+    PICOTEST_END_SECTION();
+
+    PICOTEST_START_SECTION("II: Remove handler2");
+        irq_remove_handler(DMA_IRQ_0, handler2);
+        dma_check(2, 1, 3);
+    PICOTEST_END_SECTION();
+
+    PICOTEST_START_SECTION("II: Remove handler3");
+        irq_remove_handler(DMA_IRQ_0, handler3);
+        dma_check(1, 1);
+    PICOTEST_END_SECTION();
+
+    PICOTEST_START_SECTION("II: Remove handler1");
+        irq_remove_handler(DMA_IRQ_0, handler1);
+        dma_check(0);
+    PICOTEST_END_SECTION();
+
+    // part III, the same as part II, but removing the handlers during the IRQ
+
+    PICOTEST_START_SECTION("III: Add handler3 default priority");
+        irq_add_shared_handler(DMA_IRQ_0, handler3, PICO_SHARED_IRQ_HANDLER_DEFAULT_ORDER_PRIORITY);
+        dma_check(1, 3);
+    PICOTEST_END_SECTION();
+
+    PICOTEST_START_SECTION("III: Add handler2 default priority");
+        irq_add_shared_handler(DMA_IRQ_0, handler2, PICO_SHARED_IRQ_HANDLER_DEFAULT_ORDER_PRIORITY);
+        dma_check(2, 2, 3);
+    PICOTEST_END_SECTION();
+
+    PICOTEST_START_SECTION("III: Add handler1 default priority");
+        irq_add_shared_handler(DMA_IRQ_0, handler1, PICO_SHARED_IRQ_HANDLER_DEFAULT_ORDER_PRIORITY);
+        dma_check(3, 1, 2, 3);
+    PICOTEST_END_SECTION();
+
+    PICOTEST_START_SECTION("III: Remove handler2");
+        remove_handler2_in_dma = true;
+        // note that this is the defined behavior, that any handlers after the removed handler are not called. the
+        // reasoning is that the same IRQ will immediately be re-entered, as we have missed clearing an interrupt (in whichever
+        // handlers were not called), and we will call any remaining handlers then. Because each IRQ handler is clearing the same
+        // interrupt handler in our case, this does not happen.
+        dma_check(2, 1, 2);
+        // but we call again to check
+        dma_check(2, 1, 3);
+    PICOTEST_END_SECTION();
+
+    PICOTEST_START_SECTION("III: Remove handler3");
+        remove_handler3_in_dma = true;
+        // note that this is the defined behavior, that any handlers after the removed handler are not called. the
+        // reasoning is that the same IRQ will immediately be re-entered, as we have missed clearing an interrupt (in whichever
+        // handlers were not called), and we will call any remaining handlers then. Because each IRQ handler is clearing the same
+        // interrupt handler in our case, this does not happen.
+        dma_check(2, 1, 3);
+        // but we call again to check
+        dma_check(1, 1);
+    PICOTEST_END_SECTION();
+
+    PICOTEST_START_SECTION("III: Remove handler3");
+        remove_handler1_in_dma = true;
+        // note that this is the defined behavior, that any handlers after the removed handler are not called. the
+        // reasoning is that the same IRQ will immediately be re-entered, as we have missed clearing an interrupt (in whichever
+        // handlers were not called), and we will call any remaining handlers then. Because each IRQ handler is clearing the same
+        // interrupt handler in our case, this does not happen.
+        dma_check(1, 1);
+        // but we call again to check
+        dma_check(0);
+    PICOTEST_END_SECTION();
+
+    // part IV, checking priorities
+    PICOTEST_START_SECTION("IV: Add handler1 high priority");
+        irq_add_shared_handler(DMA_IRQ_0, handler1, PICO_SHARED_IRQ_HANDLER_HIGHEST_ORDER_PRIORITY);
+        dma_check(1, 1);
+    PICOTEST_END_SECTION();
+
+    PICOTEST_START_SECTION("IV: Add handler2 normal priority");
+        irq_add_shared_handler(DMA_IRQ_0, handler2, PICO_SHARED_IRQ_HANDLER_DEFAULT_ORDER_PRIORITY);
+        dma_check(2, 1, 2);
+    PICOTEST_END_SECTION();
+
+    PICOTEST_START_SECTION("IV: Add handler3 lowest priority");
+        irq_add_shared_handler(DMA_IRQ_0, handler3, PICO_SHARED_IRQ_HANDLER_LOWEST_ORDER_PRIORITY);
+        dma_check(3, 1, 2, 3);
+    PICOTEST_END_SECTION();
+
+    PICOTEST_START_SECTION("IV: Remove handler3");
+        irq_remove_handler(DMA_IRQ_0, handler3);
+        dma_check(2, 1, 2);
+    PICOTEST_END_SECTION();
+
+    PICOTEST_START_SECTION("IV: Add handler3 normal priority");
+        irq_add_shared_handler(DMA_IRQ_0, handler3, PICO_SHARED_IRQ_HANDLER_DEFAULT_ORDER_PRIORITY);
+        dma_check(3, 1, 3, 2);
+    PICOTEST_END_SECTION();
+
+    PICOTEST_START_SECTION("IV: Remove handler2");
+        irq_remove_handler(DMA_IRQ_0, handler2);
+        dma_check(2, 1, 3);
+    PICOTEST_END_SECTION();
+
+    PICOTEST_START_SECTION("IV: Add handler2 normal priority - 2");
+        irq_add_shared_handler(DMA_IRQ_0, handler2, PICO_SHARED_IRQ_HANDLER_DEFAULT_ORDER_PRIORITY - 2);
+        dma_check(3, 1, 3, 2);
+    PICOTEST_END_SECTION();
+
+    PICOTEST_START_SECTION("IV: Remove handler1");
+        irq_remove_handler(DMA_IRQ_0, handler1);
+        dma_check(2, 3, 2);
+    PICOTEST_END_SECTION();
+
+    PICOTEST_START_SECTION("IV: Add handler1 normal priority - 1");
+        irq_add_shared_handler(DMA_IRQ_0, handler1, PICO_SHARED_IRQ_HANDLER_DEFAULT_ORDER_PRIORITY - 1);
+        dma_check(3, 3, 1, 2);
+    PICOTEST_END_SECTION();
+
+    PICOTEST_START_SECTION("IV: Remove handler2 again");
+        irq_remove_handler(DMA_IRQ_0, handler2);
+        dma_check(2, 3, 1);
+
+    PICOTEST_END_SECTION();
+    PICOTEST_START_SECTION("IV: Remove handler3 again");
+        irq_remove_handler(DMA_IRQ_0, handler3);
+        dma_check(1, 1);
+    PICOTEST_END_SECTION();
+
+    PICOTEST_START_SECTION("IV: Remove handler1 again");
+        irq_remove_handler(DMA_IRQ_0, handler1);
+        dma_check(0);
+    PICOTEST_END_SECTION();
+    PICOTEST_END_TEST();
+}
+
diff --git a/test/kitchen_sink/kitchen_sink.c b/test/kitchen_sink/kitchen_sink.c
index 4c28701..f61ece3 100644
--- a/test/kitchen_sink/kitchen_sink.c
+++ b/test/kitchen_sink/kitchen_sink.c
@@ -110,26 +110,6 @@ __force_inline int something_inlined(int x) {
     return x * 2;
 }
 
-void __isr dma_handler_a(void) {
-    printf("HELLO A\n");
-    if (dma_hw->ints1 & 1) {
-        dma_hw->ints1 = 1;
-        printf("A WINS DMA_TO %08x\n", (uint) dma_to);
-        irq_remove_handler(DMA_IRQ_1, dma_handler_a);
-    }
-}
-
-void __isr dma_handler_b(void) {
-    printf("HELLO B\n");
-    if (dma_hw->ints1 & 1) {
-        dma_hw->ints1 = 1;
-        printf("B WINS DMA_TO %08x\n", (uint) dma_to);
-//        irq_remove_handler(DMA_IRQ_1, dma_handler_b);
-    }
-}
-
-//#pragma GCC pop_options
-
 int main(void) {
     spiggle();
 
@@ -138,29 +118,6 @@ int main(void) {
     printf("HI %d\n", something_inlined((int)time_us_32()));
     puts("Hello Everything!");
     puts("Hello Everything2!");
-
-    irq_add_shared_handler(DMA_IRQ_1, dma_handler_a, 0x80);
-    irq_add_shared_handler(DMA_IRQ_1, dma_handler_b, 0xC0);
-
-    dma_channel_config config = dma_channel_get_default_config(0);
-//    set_exclusive_irq_handler(DMA_IRQ_1, dma_handler_a);
-    dma_channel_set_irq1_enabled(0, true);
-    irq_set_enabled(DMA_IRQ_1, true);
-    dma_channel_configure(0, &config, &dma_to, &dma_from, 1, true);
-    dma_channel_set_config(0, &config, false);
-
-    // note this loop expects to cause a breakpoint!!
-    for (int i = 0; i < 20; i++) {
-        puts("sleepy");
-        sleep_ms(1000);
-        dma_channel_configure(0, &config, &dma_to, &dma_from, 1, true);
-        if (i==3) {
-            irq_remove_handler(DMA_IRQ_1, dma_handler_a);
-        }
-        if (i==2) {
-            irq_remove_handler(DMA_IRQ_1, dma_handler_b);
-        }
-    }
     // this should compile as we are Cortex M0+
     __asm volatile("SVC #3");
 }