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.

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,6 +830,14 @@ static int add_trigger(struct target *target, struct trigger *trigger)
 			continue;
 		}
 
+		uint64_t 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,
@@ -839,7 +860,6 @@ static int add_trigger(struct target *target, struct trigger *trigger)
 
 			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);
 
@@ -848,10 +868,20 @@ static int add_trigger(struct target *target, struct trigger *trigger)
 						PRIx64 " to tdata1 it contains 0x%" PRIx64,
 						i, tdata1, tdata1_rb);
 				register_write_direct(target, GDB_REGNO_TDATA1, 0);
-			continue;
+				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) {
+			continue;
+		}
 
 		LOG_DEBUG("Using resource %d for bp %d", 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);
+	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;