[p2] fixes INTERRUPTS_01_isisr_willpreempt_servicedirqn test

Author: scott-brust

hal/inc/interrupts_hal.h

@@ -111,12 +111,6 @@ int hal_interrupt_set_direct_handler(IRQn_Type irqn, hal_interrupt_direct_handle
 
 #ifdef USE_STDPERIPH_DRIVER
 
-    #if defined(STM32F10X_MD) || defined(STM32F10X_HD)
-        #include "stm32f10x.h"
-    #elif defined(STM32F2XX)
-        #include "stm32f2xx.h"
-    #endif // defined(STM32F10X_MD) || defined(STM32F10X_HD)
-
     #ifdef nRF52840
         #include <nrf52840.h>
     #endif /* nRF52840 */

hal/src/rtl872x/interrupts_hal.cpp

@@ -36,6 +36,7 @@ using namespace particle;
 #endif
 
 extern uintptr_t link_ram_interrupt_vectors_location[];
+static uint32_t hal_interrupts_handler_backup[MAX_VECTOR_TABLE_NUM] = {};
 
 namespace {
 
@@ -258,20 +259,32 @@ int hal_interrupt_set_direct_handler(IRQn_Type irqn, hal_interrupt_direct_handle
     }
 
     int32_t state = HAL_disable_irq();
-    volatile uint32_t* isrs = (volatile uint32_t*)&link_ram_interrupt_vectors_location;
 
     if (handler == nullptr && (flags & HAL_INTERRUPT_DIRECT_FLAG_RESTORE)) {
         // Restore
-        // HAL_Core_Restore_Interrupt(irqn);
+        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 {
-        isrs[IRQN_TO_IDX(irqn)] = (uint32_t)handler;
+
+        // If there is currently a backup: Return error
+        if(hal_interrupts_handler_backup[IRQN_TO_IDX(irqn)]){
+            return 1;
+        }
+
+        // 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
-        // sd_nvic_DisableIRQ(irqn);
+        //__NVIC_DisableIRQ(irqn); // actually disable the interrupt (ie like systick!?)
     } else if (flags & HAL_INTERRUPT_DIRECT_FLAG_ENABLE) {
-        // sd_nvic_EnableIRQ(irqn);
+        __NVIC_SetVector(irqn, (uint32_t)handler);
     }
 
     HAL_enable_irq(state);