[p2] Only restore handler if one was backed up.

Return error if overriding handler multiple times.

Author: scott-brust

hal/src/rtl872x/interrupts_hal.cpp

@@ -258,36 +258,39 @@ int hal_interrupt_set_direct_handler(IRQn_Type irqn, hal_interrupt_direct_handle
         return 1;
     }
 
+    int error = 0;
     int32_t state = HAL_disable_irq();
 
     if (handler == nullptr && (flags & HAL_INTERRUPT_DIRECT_FLAG_RESTORE)) {
-        // Restore
+        // Restore old handler only if one was backed up
         uint32_t old_handler = hal_interrupts_handler_backup[IRQN_TO_IDX(irqn)];
-        __NVIC_SetVector(irqn, (uint32_t)old_handler);
-
-        hal_interrupts_handler_backup[IRQN_TO_IDX(irqn)] = 0;
-    } else {
-
-        // If there is currently a backup: Return error
-        if(hal_interrupts_handler_backup[IRQN_TO_IDX(irqn)]){
-            return 1;
+        if (old_handler) {
+            __NVIC_SetVector(irqn, (uint32_t)old_handler);
+            hal_interrupts_handler_backup[IRQN_TO_IDX(irqn)] = 0;
         }
-
-        // If there is a current handler, back it up
-        uint32_t current_handler = __NVIC_GetVector(irqn);
-        if (current_handler) {
-            hal_interrupts_handler_backup[IRQN_TO_IDX(irqn)] = current_handler;    
+    } else {
+        // If there is currently a handler backup: Return error
+        if (hal_interrupts_handler_backup[IRQN_TO_IDX(irqn)]) {
+            error = SYSTEM_ERROR_NOT_SUPPORTED;
+        } else {
+            // If there is a current handler, back it up
+            uint32_t current_handler = __NVIC_GetVector(irqn);
+            if (current_handler) {
+                hal_interrupts_handler_backup[IRQN_TO_IDX(irqn)] = current_handler;    
+            }    
         }
     }
 
-    if (flags & HAL_INTERRUPT_DIRECT_FLAG_DISABLE) {
-        // Disable
-        //__NVIC_DisableIRQ(irqn); // actually disable the interrupt (ie like systick!?)
-    } else if (flags & HAL_INTERRUPT_DIRECT_FLAG_ENABLE) {
-        __NVIC_SetVector(irqn, (uint32_t)handler);
+    if (!error) {
+        if (flags & HAL_INTERRUPT_DIRECT_FLAG_DISABLE) {
+            // Disable
+            __NVIC_DisableIRQ(irqn);
+        } else if (flags & HAL_INTERRUPT_DIRECT_FLAG_ENABLE) {
+            __NVIC_SetVector(irqn, (uint32_t)handler);
+        }
     }
 
     HAL_enable_irq(state);
 
-    return 0;
+    return error;
 }

user/tests/wiring/no_fixture/interrupts.cpp

@@ -47,4 +47,11 @@ test(INTERRUPTS_01_isisr_willpreempt_servicedirqn)
 	assertFalse(hal_interrupt_will_preempt(SysTick_IRQn, SysTick_IRQn));
 	assertTrue(hal_interrupt_will_preempt(NonMaskableInt_IRQn, SysTick_IRQn));
 	assertFalse(hal_interrupt_will_preempt(SysTick_IRQn, NonMaskableInt_IRQn));
+
+#if HAL_PLATFORM_RTL872X
+	// Using arbitrary interrupt
+	assertTrue(attachInterruptDirect(I2C0_IRQ, []() {return;}));
+	assertFalse(attachInterruptDirect(I2C0_IRQ, []() {return;}));
+	assertTrue(detachInterruptDirect(I2C0_IRQ));
+#endif
 }