Set hardware triggers on all harts.

Right now we're using "threads" to represent harts. gdb/OpenOCD assume
there's only one set of hardware breakpoints among all threads. Make it
so.

Author: timsifive

src/target/riscv/riscv-013.c

@@ -579,6 +579,10 @@ static int register_write_direct(struct target *target, unsigned number,
 		uint64_t value)
 {
 	struct riscv_program program;
+
+	LOG_DEBUG("[%d] reg[0x%x] <- 0x%" PRIx64, riscv_current_hartid(target),
+			number, value);
+
 	riscv_program_init(&program, target);
 
 	riscv_addr_t input = riscv_program_alloc_d(&program);
@@ -641,7 +645,8 @@ static int register_read_direct(struct target *target, uint64_t *value, uint32_t
 	*value = 0;
 	*value |= ((uint64_t)(riscv_program_read_ram(&program, output + 4))) << 32;
 	*value |= riscv_program_read_ram(&program, output);
-	LOG_DEBUG("register 0x%x = 0x%" PRIx64, number, *value);
+	LOG_DEBUG("[%d] reg[0x%x] = 0x%" PRIx64, riscv_current_hartid(target),
+			number, *value);
 	return ERROR_OK;
 }
 
@@ -794,6 +799,14 @@ static void deinit_target(struct target *target)
 static int add_trigger(struct target *target, struct trigger *trigger)
 {
 	riscv013_info_t *info = get_info(target);
+
+	// While we're using threads to fake harts, both gdb and OpenOCD assume
+	// that hardware breakpoints are shared among threads. Make this true by
+	// setting the same breakpoints on all harts.
+
+	// Assume that all triggers are configured the same on all harts.
+	riscv_set_current_hartid(target, 0);
+
 	maybe_read_tselect(target);
 
 	int i;
@@ -817,42 +830,59 @@ static int add_trigger(struct target *target, struct trigger *trigger)
 			continue;
 		}
 
-		// address/data match trigger
-		tdata1 |= MCONTROL_DMODE(riscv_xlen(target));
-		tdata1 = set_field(tdata1, MCONTROL_ACTION,
-				MCONTROL_ACTION_DEBUG_MODE);
-		tdata1 = set_field(tdata1, MCONTROL_MATCH, MCONTROL_MATCH_EQUAL);
-		tdata1 |= MCONTROL_M;
-		if (info->misa & (1 << ('H' - 'A')))
-			tdata1 |= MCONTROL_H;
-		if (info->misa & (1 << ('S' - 'A')))
-			tdata1 |= MCONTROL_S;
-		if (info->misa & (1 << ('U' - 'A')))
-			tdata1 |= MCONTROL_U;
-
-		if (trigger->execute)
-			tdata1 |= MCONTROL_EXECUTE;
-		if (trigger->read)
-			tdata1 |= MCONTROL_LOAD;
-		if (trigger->write)
-			tdata1 |= MCONTROL_STORE;
-
-		register_write_direct(target, GDB_REGNO_TDATA1, tdata1);
-
 		uint64_t tdata1_rb;
-		register_read_direct(target, &tdata1_rb, GDB_REGNO_TDATA1);
-		LOG_DEBUG("tdata1=0x%" PRIx64, tdata1_rb);
+		for (int hartid = 0; hartid < riscv_count_harts(target); ++hartid) {
+			riscv_set_current_hartid(target, hartid);
+
+			if (hartid > 0) {
+				register_write_direct(target, GDB_REGNO_TSELECT, i);
+			}
+
+			// address/data match trigger
+			tdata1 |= MCONTROL_DMODE(riscv_xlen(target));
+			tdata1 = set_field(tdata1, MCONTROL_ACTION,
+					MCONTROL_ACTION_DEBUG_MODE);
+			tdata1 = set_field(tdata1, MCONTROL_MATCH, MCONTROL_MATCH_EQUAL);
+			tdata1 |= MCONTROL_M;
+			if (info->misa & (1 << ('H' - 'A')))
+				tdata1 |= MCONTROL_H;
+			if (info->misa & (1 << ('S' - 'A')))
+				tdata1 |= MCONTROL_S;
+			if (info->misa & (1 << ('U' - 'A')))
+				tdata1 |= MCONTROL_U;
+
+			if (trigger->execute)
+				tdata1 |= MCONTROL_EXECUTE;
+			if (trigger->read)
+				tdata1 |= MCONTROL_LOAD;
+			if (trigger->write)
+				tdata1 |= MCONTROL_STORE;
+
+			register_write_direct(target, GDB_REGNO_TDATA1, tdata1);
+
+			register_read_direct(target, &tdata1_rb, GDB_REGNO_TDATA1);
+			LOG_DEBUG("tdata1=0x%" PRIx64, tdata1_rb);
+
+			if (tdata1 != tdata1_rb) {
+				LOG_DEBUG("Trigger %d doesn't support what we need; After writing 0x%"
+						PRIx64 " to tdata1 it contains 0x%" PRIx64,
+						i, tdata1, tdata1_rb);
+				register_write_direct(target, GDB_REGNO_TDATA1, 0);
+				if (hartid > 0) {
+					LOG_ERROR("Setting hardware breakpoints requires "
+							"homogeneous harts.");
+					return ERROR_FAIL;
+				}
+				break;
+			}
+
+			register_write_direct(target, GDB_REGNO_TDATA2, trigger->address);
+		}
 
 		if (tdata1 != tdata1_rb) {
-			LOG_DEBUG("Trigger %d doesn't support what we need; After writing 0x%"
-					PRIx64 " to tdata1 it contains 0x%" PRIx64,
-					i, tdata1, tdata1_rb);
-			register_write_direct(target, GDB_REGNO_TDATA1, 0);
 			continue;
 		}
 
-		register_write_direct(target, GDB_REGNO_TDATA2, trigger->address);
-
 		LOG_DEBUG("Using resource %d for bp %d", i,
 				trigger->unique_id);
 		info->trigger_unique_id[i] = trigger->unique_id;
@@ -870,6 +900,9 @@ static int remove_trigger(struct target *target, struct trigger *trigger)
 {
 	riscv013_info_t *info = get_info(target);
 
+	// Assume that all triggers are configured the same on all harts.
+	riscv_set_current_hartid(target, 0);
+
 	maybe_read_tselect(target);
 
 	int i;
@@ -884,8 +917,11 @@ static int remove_trigger(struct target *target, struct trigger *trigger)
 		return ERROR_FAIL;
 	}
 	LOG_DEBUG("Stop using resource %d for bp %d", i, trigger->unique_id);
-	register_write_direct(target, GDB_REGNO_TSELECT, i);
-	register_write_direct(target, GDB_REGNO_TDATA1, 0);
+	for (int hartid = 0; hartid < riscv_count_harts(target); ++hartid) {
+		riscv_set_current_hartid(target, hartid);
+		register_write_direct(target, GDB_REGNO_TSELECT, i);
+		register_write_direct(target, GDB_REGNO_TDATA1, 0);
+	}
 	info->trigger_unique_id[i] = -1;
 
 	return ERROR_OK;