From 6cd2e576a3fe099aad3973d34d964ad619d97e84 Mon Sep 17 00:00:00 2001
From: Michal Jakabovic <michal.jakabovic@nxp.com>
Date: Fri, 11 Nov 2016 19:03:33 +0100
Subject: [PATCH] Powering down the DP when disabling DAPLink SWD control.

---
 source/daplink/interface/swd_host.c | 52 +++++++++++++++++++++++++++--
 1 file changed, 49 insertions(+), 3 deletions(-)

diff --git a/source/daplink/interface/swd_host.c b/source/daplink/interface/swd_host.c
index 6a62b8bbdd..fa38ef56a5 100644
--- a/source/daplink/interface/swd_host.c
+++ b/source/daplink/interface/swd_host.c
@@ -73,6 +73,7 @@ typedef struct {
     uint32_t xpsr;
 } DEBUG_STATE;
 
+static bool swd_enabled = false;
 static DAP_STATE dap_state;
 
 static uint8_t swd_read_core_register(uint32_t n, uint32_t *val);
@@ -111,12 +112,38 @@ uint8_t swd_init(void)
     //       and fixed.
     DAP_Setup();
     PORT_SWD_SETUP();
+    swd_enabled = true;
     return 1;
 }
 
 uint8_t swd_off(void)
 {
+    // Clear CSYSPWRUPREQ and CDBGPWRUPREQ if SWD had been enabled. This is to ensure we don't
+    // leave the target's debug logic powered up.
+    if (swd_enabled)  {
+
+        // Ensure CTRL/STAT register selected in DPBANKSEL
+        if (!swd_write_dp(DP_SELECT, 0)) {
+            return 0;
+        }
+        if (!swd_write_dp(DP_ABORT, 0x04)) {
+            return 0;
+        }
+
+        // Power down.
+        if (!swd_write_dp(DP_CTRL_STAT, CDBGRSTREQ)) {
+            return 0;
+        }
+
+        //The debugger requires reading from it before it succesfully resets itself
+        uint32_t temp = 0;
+        if (!swd_read_dp(DP_CTRL_STAT, &temp)) {
+            return 0;
+        }
+    }
+
     PORT_OFF();
+    swd_enabled = false;
     return 1;
 }
 
@@ -835,17 +862,17 @@ uint8_t swd_uninit_debug(void)
     // Assume debug is powered up already
     // Clear (CSYSPWRUPREQ | CDBGPWRUPREQ) in DP_CTRL_STAT register
     uint32_t ctrl_stat_read;
-    
+
     if (!swd_write_dp(DP_CTRL_STAT, 0x0)) {
         return 0;
     }
-    
+
     do {
         if (!swd_read_dp(DP_CTRL_STAT, &ctrl_stat_read)) {
             return 0;
         }
     } while ((ctrl_stat_read & (CSYSPWRUPREQ | CDBGPWRUPREQ)));
-    
+
     return 1;
 }
 
@@ -916,6 +943,16 @@ uint8_t swd_set_target_state_hw(TARGET_RESET_STATE state)
                 return 0;
             }
 
+            // Ensure CTRL/STAT register selected in DPBANKSEL
+            if (!swd_write_dp(DP_SELECT, 0)) {
+                return 0;
+            }
+
+            // Power down, reset the debugger
+            if (!swd_write_dp(DP_CTRL_STAT, CDBGRSTREQ)) {
+                return 0;
+            }
+
             break;
 
         case DEBUG:
@@ -1015,6 +1052,15 @@ uint8_t swd_set_target_state_sw(TARGET_RESET_STATE state)
             if (!swd_write_word(DBG_HCSR, DBGKEY)) {
                 return 0;
             }
+            // Ensure CTRL/STAT register selected in DPBANKSEL
+            if (!swd_write_dp(DP_SELECT, 0)) {
+                return 0;
+            }
+
+            // Power down, reset the debugger
+            if (!swd_write_dp(DP_CTRL_STAT, CDBGRSTREQ)) {
+                return 0;
+            }
 
             break;