cortex_m: Do additional initialization during reset

SAM4L requires additional steps to be taken right after SYSRESETREQ is
issued in order to function robustly:

       - CMSIS-DAP DAP driver needs to explicitly check for sticky bit
         errors since it is possible for adapter to perform successful
         write opration, report no errors and then, under the hood, do
         some other things that will result in sticky bit being set.

       - Debugger needs to wait for security system to finish
         intialization and assert CDBGPWRUPACK before proceeding

This change is related to commit http://openocd.zylin.com/#/c/1995/

Change-Id: I741c95a809bfd60d930cec9482239e4796a62326
Signed-off-by: Andrey Smirnov <andrew.smirnov@gmail.com>
Reviewed-on: http://openocd.zylin.com/2088
Tested-by: jenkins
Reviewed-by: Andreas Fritiofson <andreas.fritiofson@gmail.com>
Reviewed-by: Andrey Yurovsky <yurovsky@gmail.com>

2 files changed, 43 insertions, 2 deletions

diff --git a/src/target/adi_v5_cmsis_dap.c b/src/target/adi_v5_cmsis_dap.c
index e830089f..9b591464 100644
--- a/src/target/adi_v5_cmsis_dap.c
+++ b/src/target/adi_v5_cmsis_dap.c
@@ -167,10 +167,38 @@ static int (cmsis_dap_queue_ap_write)(struct adiv5_dap *dap, unsigned reg, uint3
 /** Executes all queued DAP operations. */
 static int cmsis_dap_run(struct adiv5_dap *dap)
 {
-	LOG_DEBUG("CMSIS-ADI: cmsis_dap_run");
+	LOG_DEBUG(" ");
 	/* FIXME: for now the CMSIS-DAP interface hard-wires a zero-size queue. */
+	int ret;
+	uint32_t ctrlstat;
 
-	return ERROR_OK;
+	/*
+	  Some debug dongles do more than asked for(e.g. EDBG from
+	  Atmel) behind the scene and issuing an AP write
+	  may result in more than just APACC SWD transaction, which in
+	  turn can possibly set sticky error bit in CTRL/STAT register
+	  of the DP(an example would be writing SYSRESETREQ to AIRCR).
+	  Such adapters may interpret CMSIS-DAP secification
+	  differently and not guarantee to be report those failures
+	  via status byte of the return USB packet from CMSIS-DAP, so
+	  we need to check CTRL/STAT and if that happens to clear it.
+	*/
+	ret = cmsis_dap_queue_dp_read(dap, DP_CTRL_STAT, &ctrlstat);
+	if (ret != ERROR_OK) {
+		LOG_ERROR("Failed to read CTRL/STAT register");
+		return ret;
+	}
+
+	if (ctrlstat & SSTICKYERR) {
+		LOG_WARNING("SSTICKYERR was set, clearing it");
+		ret = cmsis_dap_clear_sticky_errors(dap);
+		if (ret != ERROR_OK) {
+			LOG_ERROR("Failed to clear sticky errors");
+			return ret;
+		}
+	}
+
+	return ret;
 }
 
 const struct dap_ops cmsis_dap_ops = {
diff --git a/src/target/cortex_m.c b/src/target/cortex_m.c
index dc8d3444..103ce502 100644
--- a/src/target/cortex_m.c
+++ b/src/target/cortex_m.c
@@ -1069,6 +1069,19 @@ static int cortex_m_assert_reset(struct target *target)
 				"handler to reset any peripherals or configure hardware srst support.");
 		}
 
+		/*
+		  SAM4L needs to execute security initalization
+		  startup sequence before AP access would be enabled.
+		  During the intialization CDBGPWRUPACK is pulled low and we
+		  need to wait for it to be set to 1 again.
+		*/
+		retval = dap_dp_poll_register(swjdp, DP_CTRL_STAT,
+					      CDBGPWRUPACK, CDBGPWRUPACK, 100);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("Failed waitnig for CDBGPWRUPACK");
+			return ERROR_FAIL;
+		}
+
 		{
 			/* I do not know why this is necessary, but it
 			 * fixes strange effects (step/resume cause NMI