From 788908fcf03e1fb449f206bf267ccad2290f24bc Mon Sep 17 00:00:00 2001
From: Palmer Dabbelt <palmer@dabbelt.com>
Date: Tue, 20 Jun 2017 17:14:18 -0700
Subject: [PATCH] Factor out checking if harts should be used

Rather than having a bunch of "if rtos" stuff, I now just check "if
hart_enabled".  This makes some code paths cleaner, all of which were
buggy in the non-RTOS multi-hart mode.
---
 src/target/riscv/riscv-013.c |  6 +++++
 src/target/riscv/riscv.c     | 44 +++++++++++++++++++++---------------
 src/target/riscv/riscv.h     |  3 +++
 3 files changed, 35 insertions(+), 18 deletions(-)

diff --git a/src/target/riscv/riscv-013.c b/src/target/riscv/riscv-013.c
index 2fab9a1ff..352f737d5 100644
--- a/src/target/riscv/riscv-013.c
+++ b/src/target/riscv/riscv-013.c
@@ -1143,6 +1143,9 @@ static int examine(struct target *target)
 	/* Find the address of the program buffer, which must be done without
 	 * knowing anything about the target. */
 	for (int i = 0; i < riscv_count_harts(target); ++i) {
+		if (!riscv_hart_enabled(target, i))
+			continue;
+
 		riscv_set_current_hartid(target, i);
 
 		/* Without knowing anything else we can at least mess with the
@@ -1213,6 +1216,9 @@ static int examine(struct target *target)
 
 	/* Then we check the number of triggers availiable to each hart. */
 	for (int i = 0; i < riscv_count_harts(target); ++i) {
+		if (!riscv_hart_enabled(target, i))
+			continue;
+
 		for (uint32_t t = 0; t < RISCV_MAX_TRIGGERS; ++t) {
 			riscv_set_current_hartid(target, i);
 
diff --git a/src/target/riscv/riscv.c b/src/target/riscv/riscv.c
index ad5eddbef..785938393 100644
--- a/src/target/riscv/riscv.c
+++ b/src/target/riscv/riscv.c
@@ -890,11 +890,11 @@ void riscv_info_init(struct target *target, riscv_info_t *r)
 
 int riscv_halt_all_harts(struct target *target)
 {
-	if (riscv_rtos_enabled(target)) {
-		for (int i = 0; i < riscv_count_harts(target); ++i)
-			riscv_halt_one_hart(target, i);
-	} else {
-		riscv_halt_one_hart(target, riscv_current_hartid(target));
+	for (int i = 0; i < riscv_count_harts(target); ++i) {
+		if (!riscv_hart_enabled(target, i))
+			continue;
+
+		riscv_halt_one_hart(target, i);
 	}
 
 	return ERROR_OK;
@@ -916,11 +916,11 @@ int riscv_halt_one_hart(struct target *target, int hartid)
 
 int riscv_resume_all_harts(struct target *target)
 {
-	if (riscv_rtos_enabled(target)) {
-		for (int i = 0; i < riscv_count_harts(target); ++i)
-			riscv_resume_one_hart(target, i);
-	} else {
-		riscv_resume_one_hart(target, riscv_current_hartid(target));
+	for (int i = 0; i < riscv_count_harts(target); ++i) {
+		if (!riscv_hart_enabled(target, i))
+			continue;
+
+		riscv_resume_one_hart(target, i);
 	}
 
 	riscv_invalidate_register_cache(target);
@@ -944,11 +944,11 @@ int riscv_resume_one_hart(struct target *target, int hartid)
 
 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));
+	for (int i = 0; i < riscv_count_harts(target); ++i) {
+		if (!riscv_hart_enabled(target, i))
+			continue;
+
+		riscv_reset_one_hart(target, i);
 	}
 
 	riscv_invalidate_register_cache(target);
@@ -1018,10 +1018,9 @@ void riscv_set_current_hartid(struct target *target, int hartid)
 
 	int previous_hartid = riscv_current_hartid(target);
 	r->current_hartid = hartid;
-	assert(riscv_rtos_enabled(target) || target->coreid == hartid);
+	assert(riscv_hart_enabled(target, hartid));
 	LOG_DEBUG("setting hartid to %d, was %d", hartid, previous_hartid);
-	if (riscv_rtos_enabled(target))
-		r->select_current_hart(target);
+	r->select_current_hart(target);
 
 	/* This might get called during init, in which case we shouldn't be
 	 * setting up the register cache. */
@@ -1234,3 +1233,12 @@ int riscv_dmi_write_u64_bits(struct target *target)
 	RISCV_INFO(r);
 	return r->dmi_write_u64_bits(target);
 }
+
+bool riscv_hart_enabled(struct target *target, int hartid)
+{
+	/* FIXME: Add a hart mask to the RTOS. */
+	if (riscv_rtos_enabled(target))
+		return hartid < riscv_count_harts(target);
+
+	return hartid == target->coreid;
+}
diff --git a/src/target/riscv/riscv.h b/src/target/riscv/riscv.h
index 53fb08673..94a6080f2 100644
--- a/src/target/riscv/riscv.h
+++ b/src/target/riscv/riscv.h
@@ -215,4 +215,7 @@ int riscv_dmi_write_u64_bits(struct target *target);
 /* Invalidates the register cache. */
 void riscv_invalidate_register_cache(struct target *target);
 
+/* Returns TRUE when a hart is enabled in this target. */
+bool riscv_hart_enabled(struct target *target, int hartid);
+
 #endif