Old fixes from June (#311)

* Changed logging level

* Added logging statement

* Removed halt event when attaching to target

* Extended some packet handling

* Extended handling of rtos_hart_id and clearing of register cache

* Extended execute_fence to handle all harts

* Removing logging statement again

* Updated according to review comments

* Forgot to re-add the return statement

* Was removing too much for the if statement to work

* This needs to >= 3 now to handle both a fence and a fence.i

4 files changed, 63 insertions, 36 deletions

diff --git a/src/rtos/riscv_debug.c b/src/rtos/riscv_debug.c
index 6703008..3650021 100644
--- a/src/rtos/riscv_debug.c
+++ b/src/rtos/riscv_debug.c
@@ -124,6 +124,11 @@ static int riscv_gdb_thread_packet(struct connection *connection, const char *pa
 			return ERROR_OK;
 		}
 
+		if (strcmp(packet, "qTStatus") == 0) {
+			gdb_put_packet(connection, "T0", 2);
+			return ERROR_OK;
+		}
+
 		if (strcmp(packet, "qC") == 0) {
 			char rep_str[32];
 			snprintf(rep_str, 32, "QC%" PRIx64, rtos->current_threadid);
@@ -250,6 +255,7 @@ static int riscv_gdb_v_packet(struct connection *connection, const char *packet,
 	if (strcmp(packet_stttrr, "vCont;c") == 0) {
 		target_call_event_callbacks(target, TARGET_EVENT_GDB_START);
 		target_call_event_callbacks(target, TARGET_EVENT_RESUME_START);
+		riscv_set_all_rtos_harts(target);
 		riscv_openocd_resume(target, 1, 0, 0, 0);
 		target->state = TARGET_RUNNING;
 		gdb_set_frontend_state_running(connection);
diff --git a/src/target/riscv/batch.c b/src/target/riscv/batch.c
index 9327cb3..e6d37e6 100644
--- a/src/target/riscv/batch.c
+++ b/src/target/riscv/batch.c
@@ -51,7 +51,6 @@ int riscv_batch_run(struct riscv_batch *batch)
 
 	keep_alive();
 
-	LOG_DEBUG("running a batch of %ld scans", (long)batch->used_scans);
 	riscv_batch_add_nop(batch);
 
 	for (size_t i = 0; i < batch->used_scans; ++i) {
@@ -60,7 +59,6 @@ int riscv_batch_run(struct riscv_batch *batch)
 			jtag_add_runtest(batch->idle_count, TAP_IDLE);
 	}
 
-	LOG_DEBUG("executing queue");
 	if (jtag_execute_queue() != ERROR_OK) {
 		LOG_ERROR("Unable to execute JTAG queue");
 		return ERROR_FAIL;
@@ -102,9 +100,6 @@ size_t riscv_batch_add_dmi_read(struct riscv_batch *batch, unsigned address)
 	riscv_batch_add_nop(batch);
 
 	batch->read_keys[batch->read_keys_used] = batch->used_scans - 1;
-	LOG_DEBUG("read key %u for batch 0x%p is %u (0x%p)",
-			(unsigned) batch->read_keys_used, batch, (unsigned) (batch->used_scans - 1),
-			batch->data_in + sizeof(uint64_t) * (batch->used_scans + 1));
 	return batch->read_keys_used++;
 }
 
@@ -135,7 +130,6 @@ void riscv_batch_add_nop(struct riscv_batch *batch)
 	riscv_fill_dmi_nop_u64(batch->target, (char *)field->in_value);
 	batch->last_scan = RISCV_SCAN_TYPE_NOP;
 	batch->used_scans++;
-	LOG_DEBUG("  added NOP with in_value=0x%p", field->in_value);
 }
 
 void dump_field(const struct scan_field *field)
diff --git a/src/target/riscv/riscv-013.c b/src/target/riscv/riscv-013.c
index 8919d4a..bc9085e 100644
--- a/src/target/riscv/riscv-013.c
+++ b/src/target/riscv/riscv-013.c
@@ -1771,13 +1771,38 @@ static void write_to_buf(uint8_t *buffer, uint64_t value, unsigned size)
 
 static int execute_fence(struct target *target)
 {
-	struct riscv_program program;
-	riscv_program_init(&program, target);
-	riscv_program_fence(&program);
-	int result = riscv_program_exec(&program, target);
-	if (result != ERROR_OK)
-		LOG_ERROR("Unable to execute fence");
-	return result;
+	int old_hartid = riscv_current_hartid(target);
+
+	/* FIXME: For non-coherent systems we need to flush the caches right
+	 * here, but there's no ISA-defined way of doing that. */
+	{
+		struct riscv_program program;
+		riscv_program_init(&program, target);
+		riscv_program_fence_i(&program);
+		riscv_program_fence(&program);
+		int result = riscv_program_exec(&program, target);
+		if (result != ERROR_OK)
+			LOG_DEBUG("Unable to execute pre-fence");
+	}
+
+	for (int i = 0; i < riscv_count_harts(target); ++i) {
+		if (!riscv_hart_enabled(target, i))
+			continue;
+
+		riscv_set_current_hartid(target, i);
+
+		struct riscv_program program;
+		riscv_program_init(&program, target);
+		riscv_program_fence_i(&program);
+		riscv_program_fence(&program);
+		int result = riscv_program_exec(&program, target);
+		if (result != ERROR_OK)
+			LOG_DEBUG("Unable to execute fence on hart %d", i);
+	}
+
+	riscv_set_current_hartid(target, old_hartid);
+
+	return ERROR_OK;
 }
 
 static void log_memory_access(target_addr_t address, uint64_t value,
@@ -3344,15 +3369,8 @@ static int maybe_execute_fence_i(struct target *target)
 {
 	RISCV013_INFO(info);
 	RISCV_INFO(r);
-	if (info->progbufsize + r->impebreak >= 2) {
-		struct riscv_program program;
-		riscv_program_init(&program, target);
-		if (riscv_program_fence_i(&program) != ERROR_OK)
-			return ERROR_FAIL;
-		if (riscv_program_exec(&program, target) != ERROR_OK) {
-			LOG_ERROR("Failed to execute fence.i");
-			return ERROR_FAIL;
-		}
+	if (info->progbufsize + r->impebreak >= 3) {
+		return execute_fence(target);
 	}
 	return ERROR_OK;
 }
diff --git a/src/target/riscv/riscv.c b/src/target/riscv/riscv.c
index 65bb199..074949d 100644
--- a/src/target/riscv/riscv.c
+++ b/src/target/riscv/riscv.c
@@ -266,6 +266,8 @@ static int riscv_init_target(struct command_context *cmd_ctx,
 
 	riscv_semihosting_init(target);
 
+	target->debug_reason = DBG_REASON_DBGRQ;
+
 	return ERROR_OK;
 }
 
@@ -856,9 +858,11 @@ static int old_or_new_riscv_resume(
 static int riscv_select_current_hart(struct target *target)
 {
 	RISCV_INFO(r);
-	if (r->rtos_hartid != -1 && riscv_rtos_enabled(target))
+	if (riscv_rtos_enabled(target)) {
+		if (r->rtos_hartid == -1)
+			r->rtos_hartid = target->rtos->current_threadid - 1;
 		return riscv_set_current_hartid(target, r->rtos_hartid);
-	else
+	} else
 		return riscv_set_current_hartid(target, target->coreid);
 }
 
@@ -1196,8 +1200,12 @@ int riscv_openocd_halt(struct target *target)
 
 	register_cache_invalidate(target->reg_cache);
 	if (riscv_rtos_enabled(target)) {
-		target->rtos->current_threadid = r->rtos_hartid + 1;
-		target->rtos->current_thread = r->rtos_hartid + 1;
+		if (r->rtos_hartid != -1) {
+			LOG_DEBUG("halt requested on RTOS hartid %d", r->rtos_hartid);
+			target->rtos->current_threadid = r->rtos_hartid + 1;
+			target->rtos->current_thread = r->rtos_hartid + 1;
+		} else
+			LOG_DEBUG("halt requested, but no known RTOS hartid");
 	}
 
 	target->state = TARGET_HALTED;
@@ -1817,6 +1825,8 @@ int riscv_halt_all_harts(struct target *target)
 		riscv_halt_one_hart(target, i);
 	}
 
+	riscv_invalidate_register_cache(target);
+
 	return ERROR_OK;
 }
 
@@ -1869,7 +1879,7 @@ int riscv_step_rtos_hart(struct target *target)
 	if (riscv_rtos_enabled(target)) {
 		hartid = r->rtos_hartid;
 		if (hartid == -1) {
-			LOG_USER("GDB has asked me to step \"any\" thread, so I'm stepping hart 0.");
+			LOG_DEBUG("GDB has asked me to step \"any\" thread, so I'm stepping hart 0.");
 			hartid = 0;
 		}
 	}
@@ -1942,15 +1952,6 @@ int riscv_set_current_hartid(struct target *target, int hartid)
 	if (!target_was_examined(target))
 		return ERROR_OK;
 
-	/* Avoid invalidating the register cache all the time. */
-	if (r->registers_initialized
-			&& (!riscv_rtos_enabled(target) || (previous_hartid == hartid))
-			&& target->reg_cache->reg_list[GDB_REGNO_ZERO].size == (unsigned)riscv_xlen(target)
-			&& (!riscv_rtos_enabled(target) || (r->rtos_hartid != -1))) {
-		return ERROR_OK;
-	} else
-		LOG_DEBUG("Initializing registers: xlen=%d", riscv_xlen(target));
-
 	riscv_invalidate_register_cache(target);
 	return ERROR_OK;
 }
@@ -2031,7 +2032,15 @@ int riscv_get_register_on_hart(struct target *target, riscv_reg_t *value,
 		int hartid, enum gdb_regno regid)
 {
 	RISCV_INFO(r);
+
+	if (hartid != riscv_current_hartid(target))
+		riscv_invalidate_register_cache(target);
+
 	int result = r->get_register(target, value, hartid, regid);
+
+	if (hartid != riscv_current_hartid(target))
+		riscv_invalidate_register_cache(target);
+
 	LOG_DEBUG("[%d] %s: %" PRIx64, hartid, gdb_regno_name(regid), *value);
 	return result;
 }