Allow all harts to be reset

3 files changed, 112 insertions, 39 deletions

diff --git a/src/target/riscv/riscv-013.c b/src/target/riscv/riscv-013.c
index 1d526df..5c1bd32 100644
--- a/src/target/riscv/riscv-013.c
+++ b/src/target/riscv/riscv-013.c
@@ -59,6 +59,7 @@ static void riscv013_fill_dmi_write_u64(struct target *target, char *buf, int a,
 static void riscv013_fill_dmi_read_u64(struct target *target, char *buf, int a);
 static int riscv013_dmi_write_u64_bits(struct target *target);
 static void riscv013_fill_dmi_nop_u64(struct target *target, char *buf);
+static void riscv013_reset_current_hart(struct target *target);
 
 /**
  * Since almost everything can be accomplish by scanning the dbus register, all
@@ -674,6 +675,7 @@ static int init_target(struct command_context *cmd_ctx,
 	generic_info->fill_dmi_read_u64 = &riscv013_fill_dmi_read_u64;
 	generic_info->fill_dmi_nop_u64 = &riscv013_fill_dmi_nop_u64;
 	generic_info->dmi_write_u64_bits = &riscv013_dmi_write_u64_bits;
+	generic_info->reset_current_hart = &riscv013_reset_current_hart;
 
 	generic_info->version_specific = calloc(1, sizeof(riscv013_info_t));
 	if (!generic_info->version_specific)
@@ -1151,7 +1153,6 @@ static int examine(struct target *target)
 static int assert_reset(struct target *target)
 {
   /*FIXME -- this only works for single-hart.*/
-  assert(!riscv_rtos_enabled(target));
   RISCV_INFO(r);
   assert(r->current_hartid == 0);
 
@@ -1181,7 +1182,6 @@ static int deassert_reset(struct target *target)
   select_dmi(target);
 
   /*FIXME -- this only works for Single Hart*/
-  assert(!riscv_rtos_enabled(target));
   assert(r->current_hartid == 0);
 
   /*FIXME -- is there bookkeeping we need to do here*/
@@ -1851,6 +1851,28 @@ int riscv013_dmi_write_u64_bits(struct target *target)
 	return info->abits + DTM_DMI_DATA_LENGTH + DTM_DMI_OP_LENGTH;
 }
 
+void riscv013_reset_current_hart(struct target *target)
+{
+  RISCV_INFO(r);
+  RISCV013_INFO(info);
+
+  select_dmi(target);
+  uint32_t control = dmi_read(target, DMI_DMCONTROL);
+  control = set_field(control, DMI_DMCONTROL_NDMRESET, 1);
+  control = set_field(control, DMI_DMCONTROL_HALTREQ, 1);
+  dmi_write(target, DMI_DMCONTROL, control);
+
+  control = set_field(control, DMI_DMCONTROL_NDMRESET, 0);
+  dmi_write(target, DMI_DMCONTROL, control);
+
+  while (get_field(dmi_read(target, DMI_DMSTATUS), DMI_DMSTATUS_ALLHALTED) == 0);
+
+  control = set_field(control, DMI_DMCONTROL_HALTREQ, 0);
+  dmi_write(target, DMI_DMCONTROL, control);
+  return ERROR_OK;
+
+}
+
 /* Helper Functions. */
 static void riscv013_on_step_or_resume(struct target *target, bool step)
 {
diff --git a/src/target/riscv/riscv.c b/src/target/riscv/riscv.c
index d754cd6..bbf042e 100644
--- a/src/target/riscv/riscv.c
+++ b/src/target/riscv/riscv.c
@@ -592,43 +592,6 @@ int riscv_blank_check_memory(struct target * target,
   return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 }
 
-struct target_type riscv_target =
-{
-	.name = "riscv",
-
-	.init_target = riscv_init_target,
-	.deinit_target = riscv_deinit_target,
-	.examine = riscv_examine,
-
-	/* poll current target status */
-	.poll = oldriscv_poll,
-
-	.halt = riscv_halt,
-	.resume = riscv_resume,
-	.step = riscv_step,
-
-	.assert_reset = riscv_assert_reset,
-	.deassert_reset = riscv_deassert_reset,
-
-	.read_memory = riscv_read_memory,
-	.write_memory = riscv_write_memory,
-
-	.blank_check_memory = riscv_blank_check_memory,
-	.checksum_memory = riscv_checksum_memory,
-
-	.get_gdb_reg_list = riscv_get_gdb_reg_list,
-
-	.add_breakpoint = riscv_add_breakpoint,
-	.remove_breakpoint = riscv_remove_breakpoint,
-
-	.add_watchpoint = riscv_add_watchpoint,
-	.remove_watchpoint = riscv_remove_watchpoint,
-
-	.arch_state = riscv_arch_state,
-
-	.run_algorithm = riscv_run_algorithm,
-};
-
 /*** OpenOCD Helper Functions ***/
 
 /* 0 means nothing happened, 1 means the hart's state changed (and thus the
@@ -784,6 +747,65 @@ int riscv_openocd_step(
 	return out;
 }
 
+int riscv_openocd_assert_reset(struct target *target)
+{
+	LOG_DEBUG("asserting reset for all harts");
+	int out = riscv_reset_all_harts(target);
+	if (out != ERROR_OK) {
+		LOG_ERROR("unable to reset all harts");
+		return out;
+	}
+
+	return out;
+}
+
+int riscv_openocd_deassert_reset(struct target *target)
+{
+	LOG_DEBUG("deasserting reset for all harts");
+	if (target->reset_halt)
+		riscv_halt_all_harts(target);
+	else
+		riscv_resume_all_harts(target);
+	return ERROR_OK;
+}
+
+struct target_type riscv_target =
+{
+	.name = "riscv",
+
+	.init_target = riscv_init_target,
+	.deinit_target = riscv_deinit_target,
+	.examine = riscv_examine,
+
+	/* poll current target status */
+	.poll = old_or_new_riscv_poll,
+
+	.halt = riscv_openocd_halt,
+	.resume = riscv_openocd_resume,
+	.step = riscv_openocd_step,
+
+	.assert_reset = riscv_openocd_assert_reset,
+	.deassert_reset = riscv_openocd_deassert_reset,
+
+	.read_memory = riscv_read_memory,
+	.write_memory = riscv_write_memory,
+
+	.blank_check_memory = riscv_blank_check_memory,
+	.checksum_memory = riscv_checksum_memory,
+
+	.get_gdb_reg_list = riscv_get_gdb_reg_list,
+
+	.add_breakpoint = riscv_add_breakpoint,
+	.remove_breakpoint = riscv_remove_breakpoint,
+
+	.add_watchpoint = riscv_add_watchpoint,
+	.remove_watchpoint = riscv_remove_watchpoint,
+
+	.arch_state = riscv_arch_state,
+
+	.run_algorithm = riscv_run_algorithm,
+};
+
 /*** RISC-V Interface ***/
 
 void riscv_info_init(riscv_info_t *r)
@@ -854,6 +876,32 @@ int riscv_resume_one_hart(struct target *target, int hartid)
 	return ERROR_OK;
 }
 
+int riscv_reset_all_harts(struct target *target)
+{
+	if (riscv_rtos_enabled(target)) {
+		for (int i = 0; i < riscv_count_harts(target); ++i)
+			riscv_reset_one_hart(target, i);
+	} else {
+		riscv_reset_one_hart(target, riscv_current_hartid(target));
+	}
+
+	return ERROR_OK;
+}
+
+int riscv_reset_one_hart(struct target *target, int hartid)
+{
+	RISCV_INFO(r);
+	LOG_DEBUG("resetting hart %d", hartid);
+	riscv_halt_one_hart(target, hartid);
+	riscv_set_current_hartid(target, hartid);
+	r->reset_current_hart(target);
+	/* At this point the hart must be halted.  On platforms that support
+	 * "reset halt" exactly we expect the hart to have been halted before
+	 * executing any instructions, while on older cores it'll just have
+	 * halted quickly. */
+	return ERROR_OK;
+}
+
 int riscv_step_rtos_hart(struct target *target)
 {
 	RISCV_INFO(r);
diff --git a/src/target/riscv/riscv.h b/src/target/riscv/riscv.h
index a38d04e..877cc3e 100644
--- a/src/target/riscv/riscv.h
+++ b/src/target/riscv/riscv.h
@@ -90,6 +90,7 @@ typedef struct {
 	void (*fill_dmi_write_u64)(struct target *target, char *buf, int a, uint64_t d);
 	void (*fill_dmi_read_u64)(struct target *target, char *buf, int a);
 	void (*fill_dmi_nop_u64)(struct target *target, char *buf);
+	void (*reset_current_hart)(struct target *target);
 } riscv_info_t;
 
 /* Everything needs the RISC-V specific info structure, so here's a nice macro
@@ -138,6 +139,8 @@ int riscv_halt_all_harts(struct target *target);
 int riscv_halt_one_hart(struct target *target, int hartid);
 int riscv_resume_all_harts(struct target *target);
 int riscv_resume_one_hart(struct target *target, int hartid);
+int riscv_reset_all_harts(struct target *target);
+int riscv_reset_one_hart(struct target *target, int hartid);
 
 /* Steps the hart that's currently selected in the RTOS, or if there is no RTOS
  * then the only hart. */