X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Ftarget%2Fadi_v5_swd.c;h=aea730d4d138fd7d568c3790f6bac6ace31af81b;hb=5f14140953ebf94dafa1150f8c7d3aeeff769bb7;hp=6e403e8b22ea2eeb9a9a8a568eed753a8fadeac6;hpb=81afe6e3fdb1253659d7d5a1be9f4f444c2c18d5;p=fw%2Fopenocd

diff --git a/src/target/adi_v5_swd.c b/src/target/adi_v5_swd.c
index 6e403e8b2..aea730d4d 100644
--- a/src/target/adi_v5_swd.c
+++ b/src/target/adi_v5_swd.c
@@ -1,19 +1,8 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+
 /***************************************************************************
  *
  *   Copyright (C) 2010 by David Brownell
- *
- *   This program is free software; you can redistribute it and/or modify
- *   it under the terms of the GNU General Public License as published by
- *   the Free Software Foundation; either version 2 of the License, or
- *   (at your option) any later version.
- *
- *   This program is distributed in the hope that it will be useful,
- *   but WITHOUT ANY WARRANTY; without even the implied warranty of
- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *   GNU General Public License for more details.
- *
- *   You should have received a copy of the GNU General Public License
- *   along with this program.  If not, see <http://www.gnu.org/licenses/>.
  ***************************************************************************/
 
 /**
@@ -29,6 +18,7 @@
  * for details, see "ARM IHI 0031A"
  * ARM Debug Interface v5 Architecture Specification
  * especially section 5.3 for SWD protocol
+ * and "ARM IHI 0074C" ARM Debug Interface Architecture Specification ADIv6.0
  *
  * On many chips (most current Cortex-M3 parts) SWD is a run-time alternative
  * to JTAG.  Boards may support one or both.  There are also SWD-only chips,
@@ -56,8 +46,9 @@
 /* for debug, set do_sync to true to force synchronous transfers */
 static bool do_sync;
 
+static struct adiv5_dap *swd_multidrop_selected_dap;
+
 
-static int swd_run(struct adiv5_dap *dap);
 static int swd_queue_dp_write_inner(struct adiv5_dap *dap, unsigned int reg,
 		uint32_t data);
 
@@ -111,20 +102,20 @@ static inline int check_sync(struct adiv5_dap *dap)
 /** Select the DP register bank matching bits 7:4 of reg. */
 static int swd_queue_dp_bankselect(struct adiv5_dap *dap, unsigned int reg)
 {
-	/* Only register address 4 is banked. */
-	if ((reg & 0xf) != 4)
+	/* Only register address 0 and 4 are banked. */
+	if ((reg & 0xf) > 4)
 		return ERROR_OK;
 
-	uint32_t select_dp_bank = (reg & 0x000000F0) >> 4;
-	uint32_t sel = select_dp_bank
-			| (dap->select & (DP_SELECT_APSEL | DP_SELECT_APBANK));
+	uint64_t sel = (reg & 0x000000F0) >> 4;
+	if (dap->select != DP_SELECT_INVALID)
+		sel |= dap->select & ~0xfULL;
 
 	if (sel == dap->select)
 		return ERROR_OK;
 
 	dap->select = sel;
 
-	int retval = swd_queue_dp_write_inner(dap, DP_SELECT, sel);
+	int retval = swd_queue_dp_write_inner(dap, DP_SELECT, (uint32_t)sel);
 	if (retval != ERROR_OK)
 		dap->select = DP_SELECT_INVALID;
 
@@ -176,59 +167,207 @@ static int swd_queue_dp_write_inner(struct adiv5_dap *dap, unsigned int reg,
 	return check_sync(dap);
 }
 
-static int swd_connect(struct adiv5_dap *dap)
+
+static int swd_multidrop_select_inner(struct adiv5_dap *dap, uint32_t *dpidr_ptr,
+		uint32_t *dlpidr_ptr, bool clear_sticky)
 {
-	const struct swd_driver *swd = adiv5_dap_swd_driver(dap);
-	uint32_t dpidr = 0xdeadbeef;
-	int status;
+	int retval;
+	uint32_t dpidr, dlpidr;
 
-	/* FIXME validate transport config ... is the
-	 * configured DAP present (check IDCODE)?
-	 * Is *only* one DAP configured?
+	assert(dap_is_multidrop(dap));
+
+	swd_send_sequence(dap, LINE_RESET);
+	/* From ARM IHI 0074C ADIv6.0, chapter B4.3.3 "Connection and line reset
+	 * sequence":
+	 * - line reset sets DP_SELECT_DPBANK to zero;
+	 * - read of DP_DPIDR takes the connection out of reset;
+	 * - write of DP_TARGETSEL keeps the connection in reset;
+	 * - other accesses return protocol error (SWDIO not driven by target).
 	 *
-	 * MUST READ DPIDR
+	 * Read DP_DPIDR to get out of reset. Initialize dap->select to zero to
+	 * skip the write to DP_SELECT, avoiding the protocol error. Set again
+	 * dap->select to DP_SELECT_INVALID because the rest of the register is
+	 * unknown after line reset.
 	 */
+	dap->select = 0;
 
-	/* Check if we should reset srst already when connecting, but not if reconnecting. */
-	if (!dap->do_reconnect) {
-		enum reset_types jtag_reset_config = jtag_get_reset_config();
+	retval = swd_queue_dp_write_inner(dap, DP_TARGETSEL, dap->multidrop_targetsel);
+	if (retval != ERROR_OK)
+		return retval;
 
-		if (jtag_reset_config & RESET_CNCT_UNDER_SRST) {
-			if (jtag_reset_config & RESET_SRST_NO_GATING)
-				adapter_assert_reset();
-			else
-				LOG_WARNING("\'srst_nogate\' reset_config option is required");
+	retval = swd_queue_dp_read_inner(dap, DP_DPIDR, &dpidr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (clear_sticky) {
+		/* Clear all sticky errors (including ORUN) */
+		swd_clear_sticky_errors(dap);
+	} else {
+		/* Ideally just clear ORUN flag which is set by reset */
+		retval = swd_queue_dp_write_inner(dap, DP_ABORT, ORUNERRCLR);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	dap->select = DP_SELECT_INVALID;
+
+	retval = swd_queue_dp_read_inner(dap, DP_DLPIDR, &dlpidr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = swd_run_inner(dap);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if ((dpidr & DP_DPIDR_VERSION_MASK) < (2UL << DP_DPIDR_VERSION_SHIFT)) {
+		LOG_INFO("Read DPIDR 0x%08" PRIx32
+				 " has version < 2. A non multidrop capable device connected?",
+				 dpidr);
+		return ERROR_FAIL;
+	}
+
+	/* TODO: check TARGETID if DLIPDR is same for more than one DP */
+	uint32_t expected_dlpidr = DP_DLPIDR_PROTVSN |
+			(dap->multidrop_targetsel & DP_TARGETSEL_INSTANCEID_MASK);
+	if (dlpidr != expected_dlpidr) {
+		LOG_INFO("Read incorrect DLPIDR 0x%08" PRIx32
+				 " (possibly CTRL/STAT value)",
+				 dlpidr);
+		return ERROR_FAIL;
+	}
+
+	LOG_DEBUG_IO("Selected DP_TARGETSEL 0x%08" PRIx32, dap->multidrop_targetsel);
+	swd_multidrop_selected_dap = dap;
+
+	if (dpidr_ptr)
+		*dpidr_ptr = dpidr;
+
+	if (dlpidr_ptr)
+		*dlpidr_ptr = dlpidr;
+
+	return retval;
+}
+
+static int swd_multidrop_select(struct adiv5_dap *dap)
+{
+	if (!dap_is_multidrop(dap))
+		return ERROR_OK;
+
+	if (swd_multidrop_selected_dap == dap)
+		return ERROR_OK;
+
+	int retval = ERROR_OK;
+	for (unsigned int retry = 0; ; retry++) {
+		bool clear_sticky = retry > 0;
+
+		retval = swd_multidrop_select_inner(dap, NULL, NULL, clear_sticky);
+		if (retval == ERROR_OK)
+			break;
+
+		swd_multidrop_selected_dap = NULL;
+		if (retry > 3) {
+			LOG_ERROR("Failed to select multidrop %s", adiv5_dap_name(dap));
+			return retval;
 		}
+
+		LOG_DEBUG("Failed to select multidrop %s, retrying...",
+				  adiv5_dap_name(dap));
+		/* we going to retry localy, do not ask for full reconnect */
+		dap->do_reconnect = false;
 	}
 
+	return retval;
+}
 
+static int swd_connect_multidrop(struct adiv5_dap *dap)
+{
+	int retval;
+	uint32_t dpidr = 0xdeadbeef;
+	uint32_t dlpidr = 0xdeadbeef;
 	int64_t timeout = timeval_ms() + 500;
 
 	do {
-		/* Note, debugport_init() does setup too */
-		swd->switch_seq(JTAG_TO_SWD);
+		swd_send_sequence(dap, JTAG_TO_DORMANT);
+		swd_send_sequence(dap, DORMANT_TO_SWD);
 
 		/* Clear link state, including the SELECT cache. */
 		dap->do_reconnect = false;
 		dap_invalidate_cache(dap);
+		swd_multidrop_selected_dap = NULL;
 
-		status = swd_queue_dp_read_inner(dap, DP_DPIDR, &dpidr);
-		if (status == ERROR_OK) {
-			status = swd_run_inner(dap);
-			if (status == ERROR_OK)
+		retval = swd_multidrop_select_inner(dap, &dpidr, &dlpidr, true);
+		if (retval == ERROR_OK)
+			break;
+
+		alive_sleep(1);
+
+	} while (timeval_ms() < timeout);
+
+	if (retval != ERROR_OK) {
+		swd_multidrop_selected_dap = NULL;
+		LOG_ERROR("Failed to connect multidrop %s", adiv5_dap_name(dap));
+		return retval;
+	}
+
+	LOG_INFO("SWD DPIDR 0x%08" PRIx32 ", DLPIDR 0x%08" PRIx32,
+			  dpidr, dlpidr);
+
+	return retval;
+}
+
+static int swd_connect_single(struct adiv5_dap *dap)
+{
+	int retval;
+	uint32_t dpidr = 0xdeadbeef;
+	int64_t timeout = timeval_ms() + 500;
+
+	do {
+		if (dap->switch_through_dormant) {
+			swd_send_sequence(dap, JTAG_TO_DORMANT);
+			swd_send_sequence(dap, DORMANT_TO_SWD);
+		} else {
+			swd_send_sequence(dap, JTAG_TO_SWD);
+		}
+
+		/* Clear link state, including the SELECT cache. */
+		dap->do_reconnect = false;
+		dap_invalidate_cache(dap);
+
+		/* The sequences to enter in SWD (JTAG_TO_SWD and DORMANT_TO_SWD) end
+		 * with a SWD line reset sequence (50 clk with SWDIO high).
+		 * From ARM IHI 0074C ADIv6.0, chapter B4.3.3 "Connection and line reset
+		 * sequence":
+		 * - line reset sets DP_SELECT_DPBANK to zero;
+		 * - read of DP_DPIDR takes the connection out of reset;
+		 * - write of DP_TARGETSEL keeps the connection in reset;
+		 * - other accesses return protocol error (SWDIO not driven by target).
+		 *
+		 * Read DP_DPIDR to get out of reset. Initialize dap->select to zero to
+		 * skip the write to DP_SELECT, avoiding the protocol error. Set again
+		 * dap->select to DP_SELECT_INVALID because the rest of the register is
+		 * unknown after line reset.
+		 */
+		dap->select = 0;
+		retval = swd_queue_dp_read_inner(dap, DP_DPIDR, &dpidr);
+		if (retval == ERROR_OK) {
+			retval = swd_run_inner(dap);
+			if (retval == ERROR_OK)
 				break;
 		}
 
 		alive_sleep(1);
 
+		dap->switch_through_dormant = !dap->switch_through_dormant;
 	} while (timeval_ms() < timeout);
 
-	if (status != ERROR_OK) {
+	dap->select = DP_SELECT_INVALID;
+
+	if (retval != ERROR_OK) {
 		LOG_ERROR("Error connecting DP: cannot read IDR");
-		return status;
+		return retval;
 	}
 
-	LOG_INFO("SWD DPIDR %#8.8" PRIx32, dpidr);
+	LOG_INFO("SWD DPIDR 0x%08" PRIx32, dpidr);
 
 	do {
 		dap->do_reconnect = false;
@@ -236,14 +375,42 @@ static int swd_connect(struct adiv5_dap *dap)
 		/* force clear all sticky faults */
 		swd_clear_sticky_errors(dap);
 
-		status = swd_run_inner(dap);
-		if (status != ERROR_WAIT)
+		retval = swd_run_inner(dap);
+		if (retval != ERROR_WAIT)
 			break;
 
 		alive_sleep(10);
 
 	} while (timeval_ms() < timeout);
 
+	return retval;
+}
+
+static int swd_connect(struct adiv5_dap *dap)
+{
+	int status;
+
+	/* FIXME validate transport config ... is the
+	 * configured DAP present (check IDCODE)?
+	 */
+
+	/* Check if we should reset srst already when connecting, but not if reconnecting. */
+	if (!dap->do_reconnect) {
+		enum reset_types jtag_reset_config = jtag_get_reset_config();
+
+		if (jtag_reset_config & RESET_CNCT_UNDER_SRST) {
+			if (jtag_reset_config & RESET_SRST_NO_GATING)
+				adapter_assert_reset();
+			else
+				LOG_WARNING("\'srst_nogate\' reset_config option is required");
+		}
+	}
+
+	if (dap_is_multidrop(dap))
+		status = swd_connect_multidrop(dap);
+	else
+		status = swd_connect_single(dap);
+
 	/* IHI 0031E B4.3.2:
 	 * "A WAIT response must not be issued to the ...
 	 * ... writes to the ABORT register"
@@ -257,9 +424,11 @@ static int swd_connect(struct adiv5_dap *dap)
 
 		dap->do_reconnect = false;
 
-		swd->write_reg(swd_cmd(false, false, DP_ABORT),
-			DAPABORT | STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR, 0);
-		status = swd_run_inner(dap);
+		status = swd_queue_dp_write_inner(dap, DP_ABORT,
+			DAPABORT | STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR);
+
+		if (status == ERROR_OK)
+			status = swd_run_inner(dap);
 	}
 
 	if (status == ERROR_OK)
@@ -281,6 +450,14 @@ static int swd_queue_ap_abort(struct adiv5_dap *dap, uint8_t *ack)
 	const struct swd_driver *swd = adiv5_dap_swd_driver(dap);
 	assert(swd);
 
+	/* TODO: Send DAPABORT in swd_multidrop_select_inner()
+	 * in the case the multidrop dap is not selected?
+	 * swd_queue_ap_abort() is not currently used anyway...
+	 */
+	int retval = swd_multidrop_select(dap);
+	if (retval != ERROR_OK)
+		return retval;
+
 	swd->write_reg(swd_cmd(false, false, DP_ABORT),
 		DAPABORT | STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR, 0);
 	return check_sync(dap);
@@ -293,6 +470,10 @@ static int swd_queue_dp_read(struct adiv5_dap *dap, unsigned reg,
 	if (retval != ERROR_OK)
 		return retval;
 
+	retval = swd_multidrop_select(dap);
+	if (retval != ERROR_OK)
+		return retval;
+
 	return swd_queue_dp_read_inner(dap, reg, data);
 }
 
@@ -306,23 +487,52 @@ static int swd_queue_dp_write(struct adiv5_dap *dap, unsigned reg,
 	if (retval != ERROR_OK)
 		return retval;
 
+	retval = swd_multidrop_select(dap);
+	if (retval != ERROR_OK)
+		return retval;
+
 	return swd_queue_dp_write_inner(dap, reg, data);
 }
 
 /** Select the AP register bank matching bits 7:4 of reg. */
 static int swd_queue_ap_bankselect(struct adiv5_ap *ap, unsigned reg)
 {
+	int retval;
 	struct adiv5_dap *dap = ap->dap;
-	uint32_t sel = ((uint32_t)ap->ap_num << 24)
-			| (reg & 0x000000F0)
-			| (dap->select & DP_SELECT_DPBANK);
+	uint64_t sel;
+
+	if (is_adiv6(dap)) {
+		sel = ap->ap_num | (reg & 0x00000FF0);
+		if (sel == (dap->select & ~0xfULL))
+			return ERROR_OK;
+
+		if (dap->select != DP_SELECT_INVALID)
+			sel |= dap->select & 0xf;
+		dap->select = sel;
+		LOG_DEBUG("AP BANKSEL: %" PRIx64, sel);
+
+		retval = swd_queue_dp_write(dap, DP_SELECT, (uint32_t)sel);
+
+		if (retval == ERROR_OK && dap->asize > 32)
+			retval = swd_queue_dp_write(dap, DP_SELECT1, (uint32_t)(sel >> 32));
+
+		if (retval != ERROR_OK)
+			dap->select = DP_SELECT_INVALID;
+
+		return retval;
+	}
+
+	/* ADIv5 */
+	sel = (ap->ap_num << 24) | (reg & 0x000000F0);
+	if (dap->select != DP_SELECT_INVALID)
+		sel |= dap->select & DP_SELECT_DPBANK;
 
 	if (sel == dap->select)
 		return ERROR_OK;
 
 	dap->select = sel;
 
-	int retval = swd_queue_dp_write_inner(dap, DP_SELECT, sel);
+	retval = swd_queue_dp_write_inner(dap, DP_SELECT, sel);
 	if (retval != ERROR_OK)
 		dap->select = DP_SELECT_INVALID;
 
@@ -340,6 +550,10 @@ static int swd_queue_ap_read(struct adiv5_ap *ap, unsigned reg,
 	if (retval != ERROR_OK)
 		return retval;
 
+	retval = swd_multidrop_select(dap);
+	if (retval != ERROR_OK)
+		return retval;
+
 	retval = swd_queue_ap_bankselect(ap, reg);
 	if (retval != ERROR_OK)
 		return retval;
@@ -361,7 +575,12 @@ static int swd_queue_ap_write(struct adiv5_ap *ap, unsigned reg,
 	if (retval != ERROR_OK)
 		return retval;
 
+	retval = swd_multidrop_select(dap);
+	if (retval != ERROR_OK)
+		return retval;
+
 	swd_finish_read(dap);
+
 	retval = swd_queue_ap_bankselect(ap, reg);
 	if (retval != ERROR_OK)
 		return retval;
@@ -374,7 +593,12 @@ static int swd_queue_ap_write(struct adiv5_ap *ap, unsigned reg,
 /** Executes all queued DAP operations. */
 static int swd_run(struct adiv5_dap *dap)
 {
+	int retval = swd_multidrop_select(dap);
+	if (retval != ERROR_OK)
+		return retval;
+
 	swd_finish_read(dap);
+
 	return swd_run_inner(dap);
 }
 
@@ -382,9 +606,32 @@ static int swd_run(struct adiv5_dap *dap)
 static void swd_quit(struct adiv5_dap *dap)
 {
 	const struct swd_driver *swd = adiv5_dap_swd_driver(dap);
+	static bool done;
+
+	/* There is no difference if the sequence is sent at the last
+	 * or the first swd_quit() call, send it just once */
+	if (done)
+		return;
+
+	done = true;
+	if (dap_is_multidrop(dap)) {
+		swd->switch_seq(SWD_TO_DORMANT);
+		/* Revisit!
+		 * Leaving DPs in dormant state was tested and offers some safety
+		 * against DPs mismatch in case of unintentional use of non-multidrop SWD.
+		 * To put SWJ-DPs to power-on state issue
+		 * swd->switch_seq(DORMANT_TO_JTAG);
+		 */
+	} else {
+		if (dap->switch_through_dormant) {
+			swd->switch_seq(SWD_TO_DORMANT);
+			swd->switch_seq(DORMANT_TO_JTAG);
+		} else {
+			swd->switch_seq(SWD_TO_JTAG);
+		}
+	}
 
-	swd->switch_seq(SWD_TO_JTAG);
-	/* flush the queue before exit */
+	/* flush the queue to shift out the sequence before exit */
 	swd->run();
 }