fix reset to bootloader issue with busy SoftDevice

Author: hathach

cores/nRF5/common_func.h

@@ -179,13 +179,13 @@ const char* dbg_err_str(int32_t err_id); // TODO move to other place
 #define PRINT_LOCATION()      PRINTF("%s: %d:\r\n", __PRETTY_FUNCTION__, __LINE__)
 #define PRINT_MESS(x)         PRINTF("%s: %d: %s \r\n"   , __FUNCTION__, __LINE__, (char*)(x))
 #define PRINT_HEAP()          if (CFG_DEBUG >= 3) PRINTF("\n%s: %d: Heap free: %d\r\n", __FUNCTION__, __LINE__, util_heap_get_free_size())
-#define PRINT_STR(x)          PRINTF("%s: %d: " #x " = %s\r\n"   , __FUNCTION__, __LINE__, (char*)(x) )
-#define PRINT_INT(x)          PRINTF("%s: %d: " #x " = %ld\r\n"  , __FUNCTION__, __LINE__, (uint32_t) (x) )
-#define PRINT_FLOAT(x)        PRINTF("%s: %d: " #x " = %f\r\n"  , __FUNCTION__, __LINE__, (float) (x) )
+#define PRINT_STR(x)          PRINTF(#x " = %s\r\n" , (char*)(x) )
+#define PRINT_INT(x)          PRINTF(#x " = %ld\r\n", (uint32_t) (x) )
+#define PRINT_FLOAT(x)        PRINTF(#x " = %f\r\n" , (float) (x) )
 
 #define PRINT_HEX(x) \
   do {\
-    PRINTF("%s: %d: " #x " = 0x", __PRETTY_FUNCTION__, __LINE__);\
+    PRINTF(#x " = 0x");\
     char fmt[] = "%00X\r\n";\
     fmt[2] += 2*sizeof(x); /* Hex with correct size */\
     PRINTF(fmt, (x) );\

cores/nRF5/wiring.c

@@ -74,22 +74,41 @@ uint32_t readResetReason(void)
   return _reset_reason;
 }
 
-void enterUf2Dfu(void)
+static void reset_mcu(uint32_t gpregret)
 {
-  NRF_POWER->GPREGRET = DFU_MAGIC_UF2_RESET;
+  // disable SD
+  sd_softdevice_disable();
+
+  // Disable all interrupts
+  NVIC->ICER[0]=0xFFFFFFFF;
+  NVIC->ICPR[0]=0xFFFFFFFF;
+#if defined(__NRF_NVIC_ISER_COUNT) && __NRF_NVIC_ISER_COUNT == 2
+  NVIC->ICER[1]=0xFFFFFFFF;
+  NVIC->ICPR[1]=0xFFFFFFFF;
+#endif
+
+  NRF_POWER->GPREGRET = gpregret;
+
   NVIC_SystemReset();
+
+  // maybe yield ?
+  while(1) {}
+}
+
+void enterUf2Dfu(void)
+{
+  reset_mcu(DFU_MAGIC_UF2_RESET);
 }
 
 void enterSerialDfu(void)
 {
-  NRF_POWER->GPREGRET = DFU_MAGIC_SERIAL_ONLY_RESET;
-  NVIC_SystemReset();
+  // shut down everything
+  reset_mcu(DFU_MAGIC_SERIAL_ONLY_RESET);
 }
 
 void enterOTADfu(void)
 {
-  NRF_POWER->GPREGRET = DFU_MAGIC_OTA_RESET;
-  NVIC_SystemReset();
+  reset_mcu(DFU_MAGIC_OTA_RESET);
 }
 
 void waitForEvent(void)