From 6a98fb7076f8b49561d4250d4c0ebce1e3473be5 Mon Sep 17 00:00:00 2001
From: Tim Newsome <tim@sifive.com>
Date: Fri, 26 Jan 2018 16:39:58 -0800
Subject: [PATCH] Detect hartsellen, limiting which harts we probe

Tested with doctored spike with hartsellens of 0, 1, 3, and 10.

Change-Id: I97f57c7d03b076792d5ecd66545d9b9e853ed515
---
 src/target/riscv/debug_defines.h | 127 +++++++++++++++++--------------
 src/target/riscv/riscv-013.c     |  80 ++++++++++---------
 2 files changed, 116 insertions(+), 91 deletions(-)

diff --git a/src/target/riscv/debug_defines.h b/src/target/riscv/debug_defines.h
index e970293bf..04500e572 100644
--- a/src/target/riscv/debug_defines.h
+++ b/src/target/riscv/debug_defines.h
@@ -441,12 +441,6 @@
 #define CSR_MCONTROL_M_LENGTH               1
 #define CSR_MCONTROL_M                      (0x1ULL << CSR_MCONTROL_M_OFFSET)
 /*
-* When set, enable this trigger in H mode.
- */
-#define CSR_MCONTROL_H_OFFSET               5
-#define CSR_MCONTROL_H_LENGTH               1
-#define CSR_MCONTROL_H                      (0x1ULL << CSR_MCONTROL_H_OFFSET)
-/*
 * When set, enable this trigger in S mode.
  */
 #define CSR_MCONTROL_S_OFFSET               4
@@ -487,7 +481,7 @@
 /*
 * When count is decremented to 0, the trigger fires. Instead of
 * changing \Fcount from 1 to 0, it is also acceptable for hardware to
-* clear \Fm, \Fh, \Fs, and \Fu. This allows \Fcount to be hard-wired
+* clear \Fm, \Fs, and \Fu. This allows \Fcount to be hard-wired
 * to 1 if this register just exists for single step.
  */
 #define CSR_ICOUNT_COUNT_OFFSET             10
@@ -501,13 +495,6 @@
 #define CSR_ICOUNT_M_LENGTH                 1
 #define CSR_ICOUNT_M                        (0x1ULL << CSR_ICOUNT_M_OFFSET)
 /*
-* When set, every instruction completed or exception taken in in H mode decrements \Fcount
-* by 1.
- */
-#define CSR_ICOUNT_H_OFFSET                 8
-#define CSR_ICOUNT_H_LENGTH                 1
-#define CSR_ICOUNT_H                        (0x1ULL << CSR_ICOUNT_H_OFFSET)
-/*
 * When set, every instruction completed or exception taken in S mode decrements \Fcount
 * by 1.
  */
@@ -544,19 +531,6 @@
 #define CSR_ICOUNT_ACTION                   (0x3fULL << CSR_ICOUNT_ACTION_OFFSET)
 #define DMI_DMSTATUS                        0x11
 /*
-* Gets set if the Debug Module was accessed incorrectly.
-*
-* 0 (none): No error.
-*
-* 1 (badaddr): There was an access to an unimplemented Debug Module
-* address.
-*
-* 7 (other): An access failed for another reason.
- */
-#define DMI_DMSTATUS_DMERR_OFFSET           24
-#define DMI_DMSTATUS_DMERR_LENGTH           3
-#define DMI_DMSTATUS_DMERR                  (0x7U << DMI_DMSTATUS_DMERR_OFFSET)
-/*
 * If 1, then there is an implicit {\tt ebreak} instruction at the
 * non-existent word immediately after the Program Buffer. This saves
 * the debugger from having to write the {\tt ebreak} itself, and
@@ -700,7 +674,7 @@
  */
 #define DMI_DMCONTROL_HALTREQ_OFFSET        31
 #define DMI_DMCONTROL_HALTREQ_LENGTH        1
-#define DMI_DMCONTROL_HALTREQ               (0x1U << DMI_DMCONTROL_HALTREQ_OFFSET)
+#define DMI_DMCONTROL_HALTREQ               (0x1ULL << DMI_DMCONTROL_HALTREQ_OFFSET)
 /*
 * Writes the resume request bit for all currently selected harts.
 * When set to 1, each selected hart will resume if it is currently
@@ -713,7 +687,7 @@
  */
 #define DMI_DMCONTROL_RESUMEREQ_OFFSET      30
 #define DMI_DMCONTROL_RESUMEREQ_LENGTH      1
-#define DMI_DMCONTROL_RESUMEREQ             (0x1U << DMI_DMCONTROL_RESUMEREQ_OFFSET)
+#define DMI_DMCONTROL_RESUMEREQ             (0x1ULL << DMI_DMCONTROL_RESUMEREQ_OFFSET)
 /*
 * This optional field writes the reset bit for all the currently
 * selected harts.  To perform a reset the debugger writes 1, and then
@@ -727,7 +701,7 @@
  */
 #define DMI_DMCONTROL_HARTRESET_OFFSET      29
 #define DMI_DMCONTROL_HARTRESET_LENGTH      1
-#define DMI_DMCONTROL_HARTRESET             (0x1U << DMI_DMCONTROL_HARTRESET_OFFSET)
+#define DMI_DMCONTROL_HARTRESET             (0x1ULL << DMI_DMCONTROL_HARTRESET_OFFSET)
 /*
 * Writing 1 to this bit clears the {\tt havereset} bits for
 * any selected harts.
@@ -736,7 +710,7 @@
  */
 #define DMI_DMCONTROL_ACKHAVERESET_OFFSET   28
 #define DMI_DMCONTROL_ACKHAVERESET_LENGTH   1
-#define DMI_DMCONTROL_ACKHAVERESET          (0x1U << DMI_DMCONTROL_ACKHAVERESET_OFFSET)
+#define DMI_DMCONTROL_ACKHAVERESET          (0x1ULL << DMI_DMCONTROL_ACKHAVERESET_OFFSET)
 /*
 * Selects the  definition of currently selected harts.
 *
@@ -752,14 +726,14 @@
  */
 #define DMI_DMCONTROL_HASEL_OFFSET          26
 #define DMI_DMCONTROL_HASEL_LENGTH          1
-#define DMI_DMCONTROL_HASEL                 (0x1U << DMI_DMCONTROL_HASEL_OFFSET)
+#define DMI_DMCONTROL_HASEL                 (0x1ULL << DMI_DMCONTROL_HASEL_OFFSET)
 /*
 * The DM-specific index of the hart to select. This hart is always part of the
 * currently selected harts.
  */
 #define DMI_DMCONTROL_HARTSEL_OFFSET        16
-#define DMI_DMCONTROL_HARTSEL_LENGTH        10
-#define DMI_DMCONTROL_HARTSEL               (0x3ffU << DMI_DMCONTROL_HARTSEL_OFFSET)
+#define DMI_DMCONTROL_HARTSEL_LENGTH        HARTSELLEN
+#define DMI_DMCONTROL_HARTSEL               (((1L<<HARTSELLEN)-1) << DMI_DMCONTROL_HARTSEL_OFFSET)
 /*
 * This bit controls the reset signal from the DM to the rest of the
 * system. The signal should reset every part of the system, including
@@ -771,7 +745,7 @@
  */
 #define DMI_DMCONTROL_NDMRESET_OFFSET       1
 #define DMI_DMCONTROL_NDMRESET_LENGTH       1
-#define DMI_DMCONTROL_NDMRESET              (0x1U << DMI_DMCONTROL_NDMRESET_OFFSET)
+#define DMI_DMCONTROL_NDMRESET              (0x1ULL << DMI_DMCONTROL_NDMRESET_OFFSET)
 /*
 * This bit serves as a reset signal for the Debug Module itself.
 *
@@ -794,7 +768,7 @@
  */
 #define DMI_DMCONTROL_DMACTIVE_OFFSET       0
 #define DMI_DMCONTROL_DMACTIVE_LENGTH       1
-#define DMI_DMCONTROL_DMACTIVE              (0x1U << DMI_DMCONTROL_DMACTIVE_OFFSET)
+#define DMI_DMCONTROL_DMACTIVE              (0x1ULL << DMI_DMCONTROL_DMACTIVE_OFFSET)
 #define DMI_HARTINFO                        0x12
 /*
 * Number of {\tt dscratch} registers available for the debugger
@@ -822,6 +796,9 @@
 *
 * If \Fdataaccess is 1: Number of 32-bit words in the memory map
 * dedicated to shadowing the {\tt data} registers.
+*
+* Since there are at most 12 {\tt data} registers, the value in this
+* register must be 12 or smaller.
  */
 #define DMI_HARTINFO_DATASIZE_OFFSET        12
 #define DMI_HARTINFO_DATASIZE_LENGTH        4
@@ -988,8 +965,8 @@
 * abstract command interface. Valid sizes are 0 - 12.
  */
 #define DMI_ABSTRACTCS_DATACOUNT_OFFSET     0
-#define DMI_ABSTRACTCS_DATACOUNT_LENGTH     5
-#define DMI_ABSTRACTCS_DATACOUNT            (0x1fU << DMI_ABSTRACTCS_DATACOUNT_OFFSET)
+#define DMI_ABSTRACTCS_DATACOUNT_LENGTH     4
+#define DMI_ABSTRACTCS_DATACOUNT            (0xfU << DMI_ABSTRACTCS_DATACOUNT_OFFSET)
 #define DMI_COMMAND                         0x17
 /*
 * The type determines the overall functionality of this
@@ -1007,14 +984,14 @@
 #define DMI_COMMAND_CONTROL                 (0xffffffU << DMI_COMMAND_CONTROL_OFFSET)
 #define DMI_ABSTRACTAUTO                    0x18
 /*
-* When a bit in this field is 1, read or write accesses the corresponding {\tt progbuf} word
+* When a bit in this field is 1, read or write accesses to the corresponding {\tt progbuf} word
 * cause the command in \Rcommand to be executed again.
  */
 #define DMI_ABSTRACTAUTO_AUTOEXECPROGBUF_OFFSET 16
 #define DMI_ABSTRACTAUTO_AUTOEXECPROGBUF_LENGTH 16
 #define DMI_ABSTRACTAUTO_AUTOEXECPROGBUF    (0xffffU << DMI_ABSTRACTAUTO_AUTOEXECPROGBUF_OFFSET)
 /*
-* When a bit in this field is 1, read or write accesses the corresponding {\tt data} word
+* When a bit in this field is 1, read or write accesses to the corresponding {\tt data} word
 * cause the command in \Rcommand to be executed again.
  */
 #define DMI_ABSTRACTAUTO_AUTOEXECDATA_OFFSET 0
@@ -1041,17 +1018,59 @@
 #define DMI_AUTHDATA_DATA_OFFSET            0
 #define DMI_AUTHDATA_DATA_LENGTH            32
 #define DMI_AUTHDATA_DATA                   (0xffffffffU << DMI_AUTHDATA_DATA_OFFSET)
+#define DMI_SBADDRESS3                      0x37
+/*
+* Accesses bits 127:96 of the physical address in {\tt sbaddress} (if
+* the system address bus is that wide).
+ */
+#define DMI_SBADDRESS3_ADDRESS_OFFSET       0
+#define DMI_SBADDRESS3_ADDRESS_LENGTH       32
+#define DMI_SBADDRESS3_ADDRESS              (0xffffffffU << DMI_SBADDRESS3_ADDRESS_OFFSET)
 #define DMI_SBCS                            0x38
 /*
-* When a 1 is written here, triggers a read at the address in {\tt
-* sbaddress} using the access size set by \Fsbaccess.
+* 0: The System Bus interface conforms to mainline drafts of this
+* spec older than 1 January, 2018.
+*
+* 1: The System Bus interface conforms to this version of the spec.
+*
+* Other values are reserved for future versions.
  */
-#define DMI_SBCS_SBSINGLEREAD_OFFSET        20
-#define DMI_SBCS_SBSINGLEREAD_LENGTH        1
-#define DMI_SBCS_SBSINGLEREAD               (0x1U << DMI_SBCS_SBSINGLEREAD_OFFSET)
+#define DMI_SBCS_SBVERSION_OFFSET           29
+#define DMI_SBCS_SBVERSION_LENGTH           3
+#define DMI_SBCS_SBVERSION                  (0x7U << DMI_SBCS_SBVERSION_OFFSET)
 /*
-* Select the access size to use for system bus accesses triggered by
-* writes to the {\tt sbaddress} registers or \Rsbdatazero.
+* Set when the debugger attempts to read data while a read is in
+* progress, or when the debugger initiates a new access while one is
+* already in progress (while \Fsbbusy is set). It remains set until
+* it's explicitly cleared by the debugger.
+*
+* While this field is non-zero, no more system bus accesses can be
+* initiated by the debug module.
+ */
+#define DMI_SBCS_SBBUSYERROR_OFFSET         22
+#define DMI_SBCS_SBBUSYERROR_LENGTH         1
+#define DMI_SBCS_SBBUSYERROR                (0x1U << DMI_SBCS_SBBUSYERROR_OFFSET)
+/*
+* When 1, indicates the system bus master is busy. (Whether the
+* system bus itself is busy is related, but not the same thing.) This
+* bit goes high immediately when a read or write is requested for any
+* reason, and does not go low until the access is fully completed.
+*
+* To avoid race conditions, debuggers must not try to clear \Fsberror
+* until they read \Fsbbusy as 0.
+ */
+#define DMI_SBCS_SBBUSY_OFFSET              21
+#define DMI_SBCS_SBBUSY_LENGTH              1
+#define DMI_SBCS_SBBUSY                     (0x1U << DMI_SBCS_SBBUSY_OFFSET)
+/*
+* When 1, every write to \Rsbaddresszero automatically triggers a
+* system bus read at the new address.
+ */
+#define DMI_SBCS_SBREADONADDR_OFFSET        20
+#define DMI_SBCS_SBREADONADDR_LENGTH        1
+#define DMI_SBCS_SBREADONADDR               (0x1U << DMI_SBCS_SBREADONADDR_OFFSET)
+/*
+* Select the access size to use for system bus accesses.
 *
 * 0: 8-bit
 *
@@ -1063,8 +1082,8 @@
 *
 * 4: 128-bit
 *
-* If an unsupported system bus access size is written here, the DM
-* does not perform the access and sberror is set to 3.
+* If \Fsbaccess has an unsupported value when the DM starts a bus
+* access, the access is not performed and \Fsberror is set to 3.
  */
 #define DMI_SBCS_SBACCESS_OFFSET            17
 #define DMI_SBCS_SBACCESS_LENGTH            3
@@ -1080,9 +1099,9 @@
 * When 1, every read from \Rsbdatazero automatically triggers a
 * system bus read at the (possibly auto-incremented) address.
  */
-#define DMI_SBCS_SBAUTOREAD_OFFSET          15
-#define DMI_SBCS_SBAUTOREAD_LENGTH          1
-#define DMI_SBCS_SBAUTOREAD                 (0x1U << DMI_SBCS_SBAUTOREAD_OFFSET)
+#define DMI_SBCS_SBREADONDATA_OFFSET        15
+#define DMI_SBCS_SBREADONDATA_LENGTH        1
+#define DMI_SBCS_SBREADONDATA               (0x1U << DMI_SBCS_SBREADONDATA_OFFSET)
 /*
 * When the debug module's system bus
 * master causes a bus error, this field gets set. The bits in this
@@ -1097,10 +1116,6 @@
 * 2: A bad address was accessed.
 *
 * 3: There was some other error (eg. alignment).
-*
-* 4: The system bus master was busy when one of the
-* {\tt sbaddress} or {\tt sbdata} registers was written,
-* or \Rsbdatazero was read when it had stale data.
  */
 #define DMI_SBCS_SBERROR_OFFSET             12
 #define DMI_SBCS_SBERROR_LENGTH             3
diff --git a/src/target/riscv/riscv-013.c b/src/target/riscv/riscv-013.c
index 48390a6f0..730d76f5b 100644
--- a/src/target/riscv/riscv-013.c
+++ b/src/target/riscv/riscv-013.c
@@ -179,8 +179,23 @@ typedef struct {
 	uint8_t datasize;
 	uint8_t dataaccess;
 	int16_t dataaddr;
+
+	/* The width of the hartsel field. */
+	unsigned hartsellen;
 } riscv013_info_t;
 
+static riscv013_info_t *get_info(const struct target *target)
+{
+	riscv_info_t *info = (riscv_info_t *) target->arch_info;
+	return (riscv013_info_t *) info->version_specific;
+}
+
+static uint32_t hartsel_mask(const struct target *target)
+{
+	RISCV013_INFO(info);
+	return ((1L<<info->hartsellen)-1) << DMI_DMCONTROL_HARTSEL_OFFSET;
+}
+
 static void decode_dmi(char *text, unsigned address, unsigned data)
 {
 	static const struct {
@@ -192,7 +207,7 @@ static void decode_dmi(char *text, unsigned address, unsigned data)
 		{ DMI_DMCONTROL, DMI_DMCONTROL_RESUMEREQ, "resumereq" },
 		{ DMI_DMCONTROL, DMI_DMCONTROL_HARTRESET, "hartreset" },
 		{ DMI_DMCONTROL, DMI_DMCONTROL_HASEL, "hasel" },
-		{ DMI_DMCONTROL, DMI_DMCONTROL_HARTSEL, "hartsel" },
+		{ DMI_DMCONTROL, ((1L<<10)-1) << DMI_DMCONTROL_HARTSEL_OFFSET, "hartsel" },
 		{ DMI_DMCONTROL, DMI_DMCONTROL_NDMRESET, "ndmreset" },
 		{ DMI_DMCONTROL, DMI_DMCONTROL_DMACTIVE, "dmactive" },
 
@@ -275,12 +290,6 @@ static void dump_field(const struct scan_field *field)
 	}
 }
 
-static riscv013_info_t *get_info(const struct target *target)
-{
-	riscv_info_t *info = (riscv_info_t *) target->arch_info;
-	return (riscv013_info_t *) info->version_specific;
-}
-
 /*** Utility functions. ***/
 
 static void select_dmi(struct target *target)
@@ -1155,54 +1164,50 @@ static int examine(struct target *target)
 	}
 
 	/* Reset the Debug Module. */
-	dmi_write(target, DMI_DMCONTROL, 0);
-	dmi_write(target, DMI_DMCONTROL, DMI_DMCONTROL_DMACTIVE);
+	uint32_t max_hartsel_mask = ((1L<<10)-1) << DMI_DMCONTROL_HARTSEL_OFFSET;
+	dmi_write(target, DMI_DMCONTROL, max_hartsel_mask);
+	dmi_write(target, DMI_DMCONTROL, max_hartsel_mask | DMI_DMCONTROL_DMACTIVE);
 	uint32_t dmcontrol = dmi_read(target, DMI_DMCONTROL);
 
-	uint32_t hartinfo = dmi_read(target, DMI_HARTINFO);
-
-	LOG_DEBUG("dmcontrol: 0x%08x", dmcontrol);
-	LOG_DEBUG("dmstatus:  0x%08x", dmstatus);
-	LOG_DEBUG("hartinfo:  0x%08x", hartinfo);
-
-	info->datasize = get_field(hartinfo, DMI_HARTINFO_DATASIZE);
-	info->dataaccess = get_field(hartinfo, DMI_HARTINFO_DATAACCESS);
-	info->dataaddr = get_field(hartinfo, DMI_HARTINFO_DATAADDR);
-
 	if (!get_field(dmcontrol, DMI_DMCONTROL_DMACTIVE)) {
 		LOG_ERROR("Debug Module did not become active. dmcontrol=0x%x",
 				dmcontrol);
 		return ERROR_FAIL;
 	}
 
+	uint32_t hartsel = get_field(dmcontrol, max_hartsel_mask);
+	info->hartsellen = 0;
+	while (hartsel & 1) {
+		info->hartsellen++;
+		hartsel >>= 1;
+	}
+	LOG_DEBUG("hartsellen=%d", info->hartsellen);
+
+	uint32_t hartinfo = dmi_read(target, DMI_HARTINFO);
+
+	info->datasize = get_field(hartinfo, DMI_HARTINFO_DATASIZE);
+	info->dataaccess = get_field(hartinfo, DMI_HARTINFO_DATAACCESS);
+	info->dataaddr = get_field(hartinfo, DMI_HARTINFO_DATAADDR);
+
 	if (!get_field(dmstatus, DMI_DMSTATUS_AUTHENTICATED)) {
 		LOG_ERROR("Authentication required by RISC-V core but not "
 				"supported by OpenOCD. dmcontrol=0x%x", dmcontrol);
 		return ERROR_FAIL;
 	}
 
-	if (get_field(dmstatus, DMI_DMSTATUS_ANYUNAVAIL)) {
-		LOG_ERROR("The hart is unavailable.");
-		return ERROR_FAIL;
-	}
-
-	if (get_field(dmstatus, DMI_DMSTATUS_ANYNONEXISTENT)) {
-		LOG_ERROR("The hart doesn't exist.");
-		return ERROR_FAIL;
-	}
-
 	/* Check that abstract data registers are accessible. */
 	uint32_t abstractcs = dmi_read(target, DMI_ABSTRACTCS);
 	info->datacount = get_field(abstractcs, DMI_ABSTRACTCS_DATACOUNT);
 	info->progbufsize = get_field(abstractcs, DMI_ABSTRACTCS_PROGBUFSIZE);
 
-	/* Before doing anything else we must first enumerate the harts. */
 	RISCV_INFO(r);
 	r->impebreak = get_field(dmstatus, DMI_DMSTATUS_IMPEBREAK);
 
+	/* Before doing anything else we must first enumerate the harts. */
+
 	/* Don't call any riscv_* functions until after we've counted the number of
 	 * cores and initialized registers. */
-	for (int i = 0; i < RISCV_MAX_HARTS; ++i) {
+	for (int i = 0; i < MIN(RISCV_MAX_HARTS, 1 << info->hartsellen); ++i) {
 		if (!riscv_rtos_enabled(target) && i != target->coreid)
 			continue;
 
@@ -1248,6 +1253,11 @@ static int examine(struct target *target)
 
 	LOG_DEBUG("Enumerated %d harts", r->hart_count);
 
+	if (r->hart_count == 0) {
+		LOG_ERROR("No harts found!");
+		return ERROR_FAIL;
+	}
+
 	/* Then we check the number of triggers availiable to each hart. */
 	riscv_enumerate_triggers(target);
 
@@ -1297,7 +1307,7 @@ static int assert_reset(struct target *target)
 			if (!riscv_hart_enabled(target, i))
 				continue;
 
-			control = set_field(control_base, DMI_DMCONTROL_HARTSEL, i);
+			control = set_field(control_base, hartsel_mask(target), i);
 			control = set_field(control, DMI_DMCONTROL_HALTREQ,
 					target->reset_halt ? 1 : 0);
 			dmi_write(target, DMI_DMCONTROL, control);
@@ -1308,7 +1318,7 @@ static int assert_reset(struct target *target)
 
 	} else {
 		/* Reset just this hart. */
-		uint32_t control = set_field(control_base, DMI_DMCONTROL_HARTSEL,
+		uint32_t control = set_field(control_base, hartsel_mask(target),
 				r->current_hartid);
 		control = set_field(control, DMI_DMCONTROL_HALTREQ,
 				target->reset_halt ? 1 : 0);
@@ -1342,7 +1352,7 @@ static int deassert_reset(struct target *target)
 	/* Clear the reset, but make sure haltreq is still set */
 	uint32_t control = 0;
 	control = set_field(control, DMI_DMCONTROL_HALTREQ, target->reset_halt ? 1 : 0);
-	control = set_field(control, DMI_DMCONTROL_HARTSEL, r->current_hartid);
+	control = set_field(control, hartsel_mask(target), r->current_hartid);
 	control = set_field(control, DMI_DMCONTROL_DMACTIVE, 1);
 	dmi_write(target, DMI_DMCONTROL, control);
 
@@ -1948,7 +1958,7 @@ static void riscv013_select_current_hart(struct target *target)
 	RISCV_INFO(r);
 
 	uint64_t dmcontrol = dmi_read(target, DMI_DMCONTROL);
-	dmcontrol = set_field(dmcontrol, DMI_DMCONTROL_HARTSEL, r->current_hartid);
+	dmcontrol = set_field(dmcontrol, hartsel_mask(target), r->current_hartid);
 	dmi_write(target, DMI_DMCONTROL, dmcontrol);
 }