Merge pull request #347 from riscv/hwthread

`-rtos hwthread` support

12 files changed, 787 insertions, 143 deletions

diff --git a/doc/openocd.texi b/doc/openocd.texi
index 7914515..168e660 100644
--- a/doc/openocd.texi
+++ b/doc/openocd.texi
@@ -9869,55 +9869,6 @@ and GDB would require stopping the target to get the prompt back.
 Do not use this mode under an IDE like Eclipse as it caches values of
 previously shown varibles.
 
-@anchor{usingopenocdsmpwithgdb}
-@section Using OpenOCD SMP with GDB
-@cindex SMP
-For SMP support following GDB serial protocol packet have been defined :
-@itemize @bullet
-@item j - smp status request
-@item J - smp set request
-@end itemize
-
-OpenOCD implements :
-@itemize @bullet
-@item @option{jc} packet for reading core id displayed by
-GDB connection. Reply is @option{XXXXXXXX} (8 hex digits giving core id) or
- @option{E01} for target not smp.
-@item @option{JcXXXXXXXX} (8 hex digits) packet for setting core id displayed at next GDB continue
-(core id -1 is reserved for returning to normal resume mode). Reply @option{E01}
-for target not smp or @option{OK} on success.
-@end itemize
-
-Handling of this packet within GDB can be done :
-@itemize @bullet
-@item by the creation of an internal variable (i.e @option{_core}) by mean
-of function allocate_computed_value allowing following GDB command.
-@example
-set $_core 1
-#Jc01 packet is sent
-print $_core
-#jc packet is sent and result is affected in $
-@end example
-
-@item by the usage of GDB maintenance command as described in following example (2 cpus in SMP with
-core id 0 and 1 @pxref{definecputargetsworkinginsmp,,Define CPU targets working in SMP}).
-
-@example
-# toggle0 : force display of coreid 0
-define toggle0
-maint packet Jc0
-continue
-main packet Jc-1
-end
-# toggle1 : force display of coreid 1
-define toggle1
-maint packet Jc1
-continue
-main packet Jc-1
-end
-@end example
-@end itemize
-
 @section RTOS Support
 @cindex RTOS Support
 @anchor{gdbrtossupport}
@@ -9948,12 +9899,11 @@ Currently supported rtos's include:
 @item @option{mqx}
 @item @option{uCOS-III}
 @item @option{nuttx}
+@item @option{hwthread} (This is not an actual RTOS. @xref{usingopenocdsmpwithgdb,,Using OpenOCD SMP with GDB}.)
 @end itemize
 
-@quotation Note
 Before an RTOS can be detected, it must export certain symbols; otherwise, it cannot
 be used by OpenOCD. Below is a list of the required symbols for each supported RTOS.
-@end quotation
 
 @table @code
 @item eCos symbols
@@ -10000,6 +9950,72 @@ contrib/rtos-helpers/FreeRTOS-openocd.c
 contrib/rtos-helpers/uCOS-III-openocd.c
 @end table
 
+@anchor{usingopenocdsmpwithgdb}
+@section Using OpenOCD SMP with GDB
+@cindex SMP
+@cindex RTOS
+@cindex hwthread
+OpenOCD includes a pseudo RTOS called @emph{hwthread} that presents CPU cores
+("hardware threads") in an SMP system as threads to GDB. With this extension,
+GDB can be used to inspect the state of an SMP system in a natural way.
+After halting the system, using the GDB command @command{info threads} will
+list the context of each active CPU core in the system. GDB's @command{thread}
+command can be used to switch the view to a different CPU core.
+The @command{step} and @command{stepi} commands can be used to step a specific core
+while other cores are free-running or remain halted, depending on the
+scheduler-locking mode configured in GDB.
+
+@section Legacy SMP core switching support
+@quotation Note
+This method is deprecated in favor of the @emph{hwthread} pseudo RTOS.
+@end quotation
+
+For SMP support following GDB serial protocol packet have been defined :
+@itemize @bullet
+@item j - smp status request
+@item J - smp set request
+@end itemize
+
+OpenOCD implements :
+@itemize @bullet
+@item @option{jc} packet for reading core id displayed by
+GDB connection. Reply is @option{XXXXXXXX} (8 hex digits giving core id) or
+ @option{E01} for target not smp.
+@item @option{JcXXXXXXXX} (8 hex digits) packet for setting core id displayed at next GDB continue
+(core id -1 is reserved for returning to normal resume mode). Reply @option{E01}
+for target not smp or @option{OK} on success.
+@end itemize
+
+Handling of this packet within GDB can be done :
+@itemize @bullet
+@item by the creation of an internal variable (i.e @option{_core}) by mean
+of function allocate_computed_value allowing following GDB command.
+@example
+set $_core 1
+#Jc01 packet is sent
+print $_core
+#jc packet is sent and result is affected in $
+@end example
+
+@item by the usage of GDB maintenance command as described in following example (2 cpus in SMP with
+core id 0 and 1 @pxref{definecputargetsworkinginsmp,,Define CPU targets working in SMP}).
+
+@example
+# toggle0 : force display of coreid 0
+define toggle0
+maint packet Jc0
+continue
+main packet Jc-1
+end
+# toggle1 : force display of coreid 1
+define toggle1
+maint packet Jc1
+continue
+main packet Jc-1
+end
+@end example
+@end itemize
+
 @node Tcl Scripting API
 @chapter Tcl Scripting API
 @cindex Tcl Scripting API
diff --git a/src/rtos/Makefile.am b/src/rtos/Makefile.am
index 6f14b42..5e18854 100644
--- a/src/rtos/Makefile.am
+++ b/src/rtos/Makefile.am
@@ -17,6 +17,7 @@ noinst_LTLIBRARIES += %D%/librtos.la
 	%D%/riscv_debug.c \
 	%D%/uCOS-III.c \
 	%D%/nuttx.c \
+	%D%/hwthread.c \
 	%D%/rtos.h \
 	%D%/rtos_standard_stackings.h \
 	%D%/rtos_ecos_stackings.h \
diff --git a/src/rtos/hwthread.c b/src/rtos/hwthread.c
new file mode 100644
index 0000000..8a271fb
--- /dev/null
+++ b/src/rtos/hwthread.c
@@ -0,0 +1,382 @@
+/***************************************************************************
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <helper/time_support.h>
+#include <jtag/jtag.h>
+#include "target/target.h"
+#include "target/target_type.h"
+#include "target/register.h"
+#include "rtos.h"
+#include "helper/log.h"
+#include "helper/types.h"
+#include "server/gdb_server.h"
+
+static bool hwthread_detect_rtos(struct target *target);
+static int hwthread_create(struct target *target);
+static int hwthread_update_threads(struct rtos *rtos);
+static int hwthread_get_thread_reg(struct rtos *rtos, int64_t thread_id,
+		uint32_t reg_num, struct rtos_reg *rtos_reg);
+static int hwthread_get_thread_reg_list(struct rtos *rtos, int64_t thread_id,
+		struct rtos_reg **reg_list, int *num_regs);
+static int hwthread_get_symbol_list_to_lookup(symbol_table_elem_t *symbol_list[]);
+static int hwthread_smp_init(struct target *target);
+int hwthread_set_reg(struct rtos *rtos, uint32_t reg_num, uint8_t *reg_value);
+
+#define HW_THREAD_NAME_STR_SIZE (32)
+
+extern int rtos_thread_packet(struct connection *connection, const char *packet, int packet_size);
+
+static inline threadid_t threadid_from_target(const struct target *target)
+{
+	return target->coreid + 1;
+}
+
+const struct rtos_type hwthread_rtos = {
+	.name = "hwthread",
+	.detect_rtos = hwthread_detect_rtos,
+	.create = hwthread_create,
+	.update_threads = hwthread_update_threads,
+	.get_thread_reg_list = hwthread_get_thread_reg_list,
+	.get_thread_reg = hwthread_get_thread_reg,
+	.get_symbol_list_to_lookup = hwthread_get_symbol_list_to_lookup,
+	.smp_init = hwthread_smp_init,
+	.set_reg = hwthread_set_reg,
+};
+
+struct hwthread_params {
+	int dummy_param;
+};
+
+static int hwthread_fill_thread(struct rtos *rtos, struct target *curr, int thread_num)
+{
+	char tmp_str[HW_THREAD_NAME_STR_SIZE];
+	threadid_t tid = threadid_from_target(curr);
+
+	memset(tmp_str, 0, HW_THREAD_NAME_STR_SIZE);
+
+	/* thread-id is the core-id of this core inside the SMP group plus 1 */
+	rtos->thread_details[thread_num].threadid = tid;
+	/* create the thread name */
+	rtos->thread_details[thread_num].exists = true;
+	rtos->thread_details[thread_num].thread_name_str = strdup(target_name(curr));
+	snprintf(tmp_str, HW_THREAD_NAME_STR_SIZE-1, "state: %s", debug_reason_name(curr));
+	rtos->thread_details[thread_num].extra_info_str = strdup(tmp_str);
+
+	return ERROR_OK;
+}
+
+static int hwthread_update_threads(struct rtos *rtos)
+{
+	int threads_found = 0;
+	int thread_list_size = 0;
+	struct target_list *head;
+	struct target *target;
+	int64_t current_thread = 0;
+	enum target_debug_reason current_reason = DBG_REASON_UNDEFINED;
+
+	if (rtos == NULL)
+		return ERROR_FAIL;
+
+	target = rtos->target;
+
+	/* determine the number of "threads" */
+	if (target->smp) {
+		for (head = target->head; head != NULL; head = head->next) {
+			struct target *curr = head->target;
+
+			if (!target_was_examined(curr))
+				continue;
+
+			++thread_list_size;
+		}
+	} else
+		thread_list_size = 1;
+
+	/* Wipe out previous thread details if any, but preserve threadid. */
+	int64_t current_threadid = rtos->current_threadid;
+	rtos_free_threadlist(rtos);
+	rtos->current_threadid = current_threadid;
+
+	/* create space for new thread details */
+	rtos->thread_details = malloc(sizeof(struct thread_detail) * thread_list_size);
+
+	if (target->smp) {
+		/* loop over all threads */
+		for (head = target->head; head != NULL; head = head->next) {
+			struct target *curr = head->target;
+
+			if (!target_was_examined(curr))
+				continue;
+
+			threadid_t tid = threadid_from_target(curr);
+
+			hwthread_fill_thread(rtos, curr, threads_found);
+
+			/* find an interesting thread to set as current */
+			switch (current_reason) {
+			case DBG_REASON_UNDEFINED:
+				current_reason = curr->debug_reason;
+				current_thread = tid;
+				break;
+			case DBG_REASON_SINGLESTEP:
+				/* single-step can only be overridden by itself */
+				if (curr->debug_reason == DBG_REASON_SINGLESTEP) {
+					if (tid == rtos->current_threadid)
+						current_thread = tid;
+				}
+				break;
+			case DBG_REASON_BREAKPOINT:
+				/* single-step overrides breakpoint */
+				if (curr->debug_reason == DBG_REASON_SINGLESTEP) {
+					current_reason = curr->debug_reason;
+					current_thread = tid;
+				} else
+				/* multiple breakpoints, prefer gdbs' threadid */
+				if (curr->debug_reason == DBG_REASON_BREAKPOINT) {
+					if (tid == rtos->current_threadid)
+						current_thread = tid;
+				}
+				break;
+			case DBG_REASON_WATCHPOINT:
+				/* breakpoint and single-step override watchpoint */
+				if (curr->debug_reason == DBG_REASON_SINGLESTEP ||
+						curr->debug_reason == DBG_REASON_BREAKPOINT) {
+					current_reason = curr->debug_reason;
+					current_thread = tid;
+				}
+				break;
+			case DBG_REASON_DBGRQ:
+				/* all other reasons override debug-request */
+				if (curr->debug_reason == DBG_REASON_SINGLESTEP ||
+						curr->debug_reason == DBG_REASON_WATCHPOINT ||
+						curr->debug_reason == DBG_REASON_BREAKPOINT) {
+					current_reason = curr->debug_reason;
+					current_thread = tid;
+				} else
+				if (curr->debug_reason == DBG_REASON_DBGRQ) {
+					if (tid == rtos->current_threadid)
+						current_thread = tid;
+				}
+
+				break;
+
+			default:
+				break;
+			}
+
+			threads_found++;
+		}
+	} else {
+		hwthread_fill_thread(rtos, target, threads_found);
+		current_thread = threadid_from_target(target);
+		threads_found++;
+	}
+
+	rtos->thread_count = threads_found;
+
+	/* we found an interesting thread, set it as current */
+	if (current_thread != 0)
+		rtos->current_thread = current_thread;
+	else if (rtos->current_threadid != 0)
+		rtos->current_thread = rtos->current_threadid;
+	else
+		rtos->current_thread = threadid_from_target(target);
+
+	return 0;
+}
+
+static int hwthread_smp_init(struct target *target)
+{
+	return hwthread_update_threads(target->rtos);
+}
+
+static struct target *find_thread(struct target *target, int64_t thread_id)
+{
+	/* Find the thread with that thread_id */
+	if (target == NULL)
+		return NULL;
+	if (target->smp) {
+		for (struct target_list *head = target->head; head != NULL; head = head->next) {
+			if (thread_id == threadid_from_target(head->target))
+				return head->target;
+		}
+	} else if (thread_id == threadid_from_target(target)) {
+		return target;
+	}
+	return NULL;
+}
+
+static int hwthread_get_thread_reg_list(struct rtos *rtos, int64_t thread_id,
+		struct rtos_reg **rtos_reg_list, int *rtos_reg_list_size)
+{
+	if (rtos == NULL)
+		return ERROR_FAIL;
+
+	struct target *target = rtos->target;
+
+	struct target *curr = find_thread(target, thread_id);
+	if (curr == NULL)
+		return ERROR_FAIL;
+
+	if (!target_was_examined(curr))
+		return ERROR_FAIL;
+
+	struct reg **reg_list;
+	int retval = target_get_gdb_reg_list(curr, &reg_list, rtos_reg_list_size,
+			REG_CLASS_GENERAL);
+	if (retval != ERROR_OK)
+		return retval;
+
+	*rtos_reg_list = calloc(*rtos_reg_list_size, sizeof(struct rtos_reg));
+	if (*rtos_reg_list == NULL) {
+		free(reg_list);
+		return ERROR_FAIL;
+	}
+
+	for (int i = 0; i < *rtos_reg_list_size; i++) {
+		(*rtos_reg_list)[i].number = (*reg_list)[i].number;
+		(*rtos_reg_list)[i].size = (*reg_list)[i].size;
+		memcpy((*rtos_reg_list)[i].value, (*reg_list)[i].value,
+				((*reg_list)[i].size + 7) / 8);
+	}
+	free(reg_list);
+
+	return ERROR_OK;
+}
+
+static int hwthread_get_thread_reg(struct rtos *rtos, int64_t thread_id,
+		uint32_t reg_num, struct rtos_reg *rtos_reg)
+{
+	if (rtos == NULL)
+		return ERROR_FAIL;
+
+	struct target *target = rtos->target;
+
+	struct target *curr = find_thread(target, thread_id);
+	if (curr == NULL)
+		return ERROR_FAIL;
+
+	if (!target_was_examined(curr))
+		return ERROR_FAIL;
+
+	struct reg *reg = register_get_by_number(curr->reg_cache, reg_num, true);
+	if (!reg)
+		return ERROR_FAIL;
+
+	if (reg->type->get(reg) != ERROR_OK)
+		return ERROR_FAIL;
+
+	rtos_reg->number = reg->number;
+	rtos_reg->size = reg->size;
+	unsigned bytes = (reg->size + 7) / 8;
+	assert(bytes <= sizeof(rtos_reg->value));
+	memcpy(rtos_reg->value, reg->value, bytes);
+
+	return ERROR_OK;
+}
+
+int hwthread_set_reg(struct rtos *rtos, uint32_t reg_num, uint8_t *reg_value)
+{
+	if (rtos == NULL)
+		return ERROR_FAIL;
+
+	struct target *target = rtos->target;
+
+	struct target *curr = find_thread(target, rtos->current_thread);
+	if (curr == NULL)
+		return ERROR_FAIL;
+
+	struct reg *reg = register_get_by_number(curr->reg_cache, reg_num, true);
+	if (!reg)
+		return ERROR_FAIL;
+
+	return reg->type->set(reg, reg_value);
+}
+
+static int hwthread_get_symbol_list_to_lookup(symbol_table_elem_t *symbol_list[])
+{
+	/* return an empty list, we don't have any symbols to look up */
+	*symbol_list = calloc(1, sizeof(symbol_table_elem_t));
+	(*symbol_list)[0].symbol_name = NULL;
+	return 0;
+}
+
+static int hwthread_target_for_threadid(struct connection *connection, int64_t thread_id, struct target **p_target)
+{
+	struct target *target = get_target_from_connection(connection);
+
+	struct target *curr = find_thread(target, thread_id);
+	if (curr == NULL)
+		return ERROR_FAIL;
+
+	*p_target = curr;
+
+	return ERROR_OK;
+}
+
+static bool hwthread_detect_rtos(struct target *target)
+{
+	/* always return 0, avoid auto-detection */
+	return false;
+}
+
+static int hwthread_thread_packet(struct connection *connection, const char *packet, int packet_size)
+{
+	struct target *target = get_target_from_connection(connection);
+
+	struct target *curr = NULL;
+	int64_t current_threadid;
+
+	if (packet[0] == 'H' && packet[1] == 'g') {
+		/* Never reached, because this case is handled by rtos_thread_packet(). */
+		sscanf(packet, "Hg%16" SCNx64, &current_threadid);
+
+		if (current_threadid > 0) {
+			if (hwthread_target_for_threadid(connection, current_threadid, &curr) != ERROR_OK) {
+				LOG_ERROR("hwthread: cannot find thread id %"PRId64, current_threadid);
+				gdb_put_packet(connection, "E01", 3);
+				return ERROR_FAIL;
+			}
+			target->rtos->current_thread = current_threadid;
+		} else
+		if (current_threadid == 0 || current_threadid == -1)
+			target->rtos->current_thread = threadid_from_target(target);
+
+		target->rtos->current_threadid = current_threadid;
+		LOG_DEBUG("current_threadid=%" PRId64, current_threadid);
+
+		gdb_put_packet(connection, "OK", 2);
+		return ERROR_OK;
+	}
+
+	return rtos_thread_packet(connection, packet, packet_size);
+}
+
+static int hwthread_create(struct target *target)
+{
+	LOG_INFO("Hardware thread awareness created");
+
+	target->rtos->rtos_specific_params = NULL;
+	target->rtos->current_thread = 0;
+	target->rtos->thread_details = NULL;
+	target->rtos->gdb_target_for_threadid = hwthread_target_for_threadid;
+	target->rtos->gdb_thread_packet = hwthread_thread_packet;
+	return 0;
+}
diff --git a/src/rtos/rtos.c b/src/rtos/rtos.c
index 9b05ab5..b055f91 100644
--- a/src/rtos/rtos.c
+++ b/src/rtos/rtos.c
@@ -36,6 +36,7 @@ extern struct rtos_type embKernel_rtos;
 extern struct rtos_type mqx_rtos;
 extern struct rtos_type uCOS_III_rtos;
 extern struct rtos_type nuttx_rtos;
+extern struct rtos_type hwthread_rtos;
 extern struct rtos_type riscv_rtos;
 
 static struct rtos_type *rtos_types[] = {
@@ -48,6 +49,7 @@ static struct rtos_type *rtos_types[] = {
 	&mqx_rtos,
 	&uCOS_III_rtos,
 	&nuttx_rtos,
+	&hwthread_rtos,
 	&riscv_rtos,
 	NULL
 };
@@ -461,6 +463,7 @@ static int rtos_put_gdb_reg_list(struct connection *connection,
 	return ERROR_OK;
 }
 
+/** Look through all registers to find this register. */
 int rtos_get_gdb_reg(struct connection *connection, int reg_num)
 {
 	struct target *target = get_target_from_connection(connection);
@@ -478,10 +481,18 @@ int rtos_get_gdb_reg(struct connection *connection, int reg_num)
 										current_threadid,
 										target->rtos->current_thread);
 
-		int retval = target->rtos->type->get_thread_reg_list(target->rtos,
-				current_threadid,
-				&reg_list,
-				&num_regs);
+		int retval;
+		if (target->rtos->type->get_thread_reg) {
+			reg_list = calloc(1, sizeof(*reg_list));
+			num_regs = 1;
+			retval = target->rtos->type->get_thread_reg(target->rtos,
+					current_threadid, reg_num, &reg_list[0]);
+		} else {
+			retval = target->rtos->type->get_thread_reg_list(target->rtos,
+					current_threadid,
+					&reg_list,
+					&num_regs);
+		}
 		if (retval != ERROR_OK) {
 			LOG_ERROR("RTOS: failed to get register list");
 			return retval;
@@ -500,6 +511,7 @@ int rtos_get_gdb_reg(struct connection *connection, int reg_num)
 	return ERROR_FAIL;
 }
 
+/** Return a list of general registers. */
 int rtos_get_gdb_reg_list(struct connection *connection)
 {
 	struct target *target = get_target_from_connection(connection);
@@ -533,6 +545,20 @@ int rtos_get_gdb_reg_list(struct connection *connection)
 	return ERROR_FAIL;
 }
 
+int rtos_set_reg(struct connection *connection, int reg_num,
+		uint8_t *reg_value)
+{
+	struct target *target = get_target_from_connection(connection);
+	int64_t current_threadid = target->rtos->current_threadid;
+	if ((target->rtos != NULL) &&
+			(target->rtos->type->set_reg != NULL) &&
+			(current_threadid != -1) &&
+			(current_threadid != 0)) {
+		return target->rtos->type->set_reg(target->rtos, reg_num, reg_value);
+	}
+	return ERROR_FAIL;
+}
+
 int rtos_generic_stack_read(struct target *target,
 	const struct rtos_register_stacking *stacking,
 	int64_t stack_ptr,
diff --git a/src/rtos/rtos.h b/src/rtos/rtos.h
index 22de33f..81829b1 100644
--- a/src/rtos/rtos.h
+++ b/src/rtos/rtos.h
@@ -20,6 +20,7 @@
 #define OPENOCD_RTOS_RTOS_H
 
 #include "server/server.h"
+#include "target/target.h"
 #include <jim-nvp.h>
 
 typedef int64_t threadid_t;
@@ -49,7 +50,9 @@ struct rtos {
 	symbol_table_elem_t *symbols;
 	struct target *target;
 	/*  add a context variable instead of global variable */
+	/* The thread currently selected by gdb. */
 	int64_t current_threadid;
+	/* The currently selected thread according to the target. */
 	threadid_t current_thread;
 	struct thread_detail *thread_details;
 	int thread_count;
@@ -71,11 +74,15 @@ struct rtos_type {
 	int (*create)(struct target *target);
 	int (*smp_init)(struct target *target);
 	int (*update_threads)(struct rtos *rtos);
+	/** Return a list of general registers, with their values filled out. */
 	int (*get_thread_reg_list)(struct rtos *rtos, int64_t thread_id,
 			struct rtos_reg **reg_list, int *num_regs);
+	int (*get_thread_reg)(struct rtos *rtos, int64_t thread_id,
+			uint32_t reg_num, struct rtos_reg *reg);
 	int (*get_symbol_list_to_lookup)(symbol_table_elem_t *symbol_list[]);
 	int (*clean)(struct target *target);
 	char * (*ps_command)(struct target *target);
+	int (*set_reg)(struct rtos *rtos, uint32_t reg_num, uint8_t *reg_value);
 };
 
 struct stack_register_offset {
@@ -105,6 +112,8 @@ struct rtos_register_stacking {
 #define GDB_THREAD_PACKET_NOT_CONSUMED (-40)
 
 int rtos_create(Jim_GetOptInfo *goi, struct target *target);
+int rtos_set_reg(struct connection *connection, int reg_num,
+		uint8_t *reg_value);
 int rtos_generic_stack_read(struct target *target,
 		const struct rtos_register_stacking *stacking,
 		int64_t stack_ptr,
diff --git a/src/server/gdb_server.c b/src/server/gdb_server.c
index 435195e..b630001 100644
--- a/src/server/gdb_server.c
+++ b/src/server/gdb_server.c
@@ -733,17 +733,26 @@ static void gdb_signal_reply(struct target *target, struct connection *connectio
 	if (target->debug_reason == DBG_REASON_EXIT) {
 		sig_reply_len = snprintf(sig_reply, sizeof(sig_reply), "W00");
 	} else {
+		struct target *ct;
+		if (target->rtos != NULL) {
+			target->rtos->current_threadid = target->rtos->current_thread;
+			LOG_DEBUG("current_threadid=%" PRId64, target->rtos->current_threadid);
+			target->rtos->gdb_target_for_threadid(connection, target->rtos->current_threadid, &ct);
+		} else {
+			ct = target;
+		}
+
 		if (gdb_connection->ctrl_c) {
 			signal_var = 0x2;
 		} else
-			signal_var = gdb_last_signal(target);
+			signal_var = gdb_last_signal(ct);
 
 		stop_reason[0] = '\0';
-		if (target->debug_reason == DBG_REASON_WATCHPOINT) {
+		if (ct->debug_reason == DBG_REASON_WATCHPOINT) {
 			enum watchpoint_rw hit_wp_type;
 			target_addr_t hit_wp_address;
 
-			if (watchpoint_hit(target, &hit_wp_type, &hit_wp_address) == ERROR_OK) {
+			if (watchpoint_hit(ct, &hit_wp_type, &hit_wp_address) == ERROR_OK) {
 
 				switch (hit_wp_type) {
 					case WPT_WRITE:
@@ -765,15 +774,9 @@ static void gdb_signal_reply(struct target *target, struct connection *connectio
 		}
 
 		current_thread[0] = '\0';
-		if (target->rtos != NULL) {
-			struct target *ct;
+		if (target->rtos != NULL)
 			snprintf(current_thread, sizeof(current_thread), "thread:%" PRIx64 ";",
 					target->rtos->current_thread);
-			target->rtos->current_threadid = target->rtos->current_thread;
-			target->rtos->gdb_target_for_threadid(connection, target->rtos->current_threadid, &ct);
-			if (!gdb_connection->ctrl_c)
-				signal_var = gdb_last_signal(ct);
-		}
 
 		sig_reply_len = snprintf(sig_reply, sizeof(sig_reply), "T%2.2x%s%s",
 				signal_var, stop_reason, current_thread);
@@ -1340,37 +1343,49 @@ static int gdb_set_register_packet(struct connection *connection,
 {
 	struct target *target = get_target_from_connection(connection);
 	char *separator;
-	uint8_t *bin_buf;
 	int reg_num = strtoul(packet + 1, &separator, 16);
 	struct reg **reg_list;
 	int reg_list_size;
 	int retval;
 
+#ifdef _DEBUG_GDB_IO_
 	LOG_DEBUG("-");
+#endif
+
+	if (*separator != '=') {
+		LOG_ERROR("GDB 'set register packet', but no '=' following the register number");
+		return ERROR_SERVER_REMOTE_CLOSED;
+	}
+	size_t chars = strlen(separator + 1);
+	uint8_t *bin_buf = malloc(chars / 2);
+	gdb_target_to_reg(target, separator + 1, chars, bin_buf);
+
+	if ((target->rtos != NULL) &&
+			(ERROR_OK == rtos_set_reg(connection, reg_num, bin_buf))) {
+		free(bin_buf);
+		gdb_put_packet(connection, "OK", 2);
+		return ERROR_OK;
+	}
 
 	retval = target_get_gdb_reg_list(target, &reg_list, &reg_list_size,
 			REG_CLASS_ALL);
-	if (retval != ERROR_OK)
+	if (retval != ERROR_OK) {
+		free(bin_buf);
 		return gdb_error(connection, retval);
+	}
 
 	if (reg_list_size <= reg_num) {
 		LOG_ERROR("gdb requested a non-existing register");
+		free(bin_buf);
+		free(reg_list);
 		return ERROR_SERVER_REMOTE_CLOSED;
 	}
 
-	if (*separator != '=') {
-		LOG_ERROR("GDB 'set register packet', but no '=' following the register number");
-		return ERROR_SERVER_REMOTE_CLOSED;
-	}
-
-	/* convert from GDB-string (target-endian) to hex-string (big-endian) */
-	bin_buf = malloc(DIV_ROUND_UP(reg_list[reg_num]->size, 8));
-	int chars = (DIV_ROUND_UP(reg_list[reg_num]->size, 8) * 2);
-
-	if ((unsigned int)chars != strlen(separator + 1)) {
-		LOG_ERROR("gdb sent %zu bits for a %d-bit register (%s)",
-				strlen(separator + 1) * 4, chars * 4, reg_list[reg_num]->name);
+	if (chars != (DIV_ROUND_UP(reg_list[reg_num]->size, 8) * 2)) {
+		LOG_ERROR("gdb sent %d bits for a %d-bit register (%s)",
+				(int) chars * 4, reg_list[reg_num]->size, reg_list[reg_num]->name);
 		free(bin_buf);
+		free(reg_list);
 		return ERROR_SERVER_REMOTE_CLOSED;
 	}
 
@@ -1640,7 +1655,7 @@ static int gdb_breakpoint_watchpoint_packet(struct connection *connection,
 	char *separator;
 	int retval;
 
-	LOG_DEBUG("-");
+	LOG_DEBUG("[%d]", target->coreid);
 
 	type = strtoul(packet + 1, &separator, 16);
 
@@ -2398,6 +2413,7 @@ static int gdb_target_description_supported(struct target *target, int *supporte
 			&reg_list_size, REG_CLASS_ALL);
 	if (retval != ERROR_OK) {
 		LOG_ERROR("get register list failed");
+		reg_list = NULL;
 		goto error;
 	}
 
diff --git a/src/target/breakpoints.c b/src/target/breakpoints.c
index 58bcc86..58e4f61 100644
--- a/src/target/breakpoints.c
+++ b/src/target/breakpoints.c
@@ -98,7 +98,9 @@ fail:
 			return retval;
 	}
 
-	LOG_DEBUG("added %s breakpoint at " TARGET_ADDR_FMT " of length 0x%8.8x, (BPID: %" PRIu32 ")",
+	LOG_DEBUG("[%d] added %s breakpoint at " TARGET_ADDR_FMT
+			" of length 0x%8.8x, (BPID: %" PRIu32 ")",
+		target->coreid,
 		breakpoint_type_strings[(*breakpoint_p)->type],
 		(*breakpoint_p)->address, (*breakpoint_p)->length,
 		(*breakpoint_p)->unique_id);
@@ -385,8 +387,8 @@ struct breakpoint *breakpoint_find(struct target *target, target_addr_t address)
 	return NULL;
 }
 
-int watchpoint_add(struct target *target, target_addr_t address, uint32_t length,
-	enum watchpoint_rw rw, uint32_t value, uint32_t mask)
+int watchpoint_add_internal(struct target *target, target_addr_t address,
+		uint32_t length, enum watchpoint_rw rw, uint32_t value, uint32_t mask)
 {
 	struct watchpoint *watchpoint = target->watchpoints;
 	struct watchpoint **watchpoint_p = &target->watchpoints;
@@ -451,6 +453,29 @@ bye:
 	return ERROR_OK;
 }
 
+int watchpoint_add(struct target *target, target_addr_t address,
+		uint32_t length, enum watchpoint_rw rw, uint32_t value, uint32_t mask)
+{
+	int retval = ERROR_OK;
+	if (target->smp) {
+		struct target_list *head;
+		struct target *curr;
+		head = target->head;
+
+		while (head != (struct target_list *)NULL) {
+			curr = head->target;
+			retval = watchpoint_add_internal(curr, address, length, rw, value,
+					mask);
+			if (retval != ERROR_OK)
+				return retval;
+			head = head->next;
+		}
+		return retval;
+	} else
+		return watchpoint_add_internal(target, address, length, rw, value,
+				mask);
+}
+
 static void watchpoint_free(struct target *target, struct watchpoint *watchpoint_to_remove)
 {
 	struct watchpoint *watchpoint = target->watchpoints;
@@ -472,7 +497,7 @@ static void watchpoint_free(struct target *target, struct watchpoint *watchpoint
 	free(watchpoint);
 }
 
-void watchpoint_remove(struct target *target, target_addr_t address)
+int watchpoint_remove_internal(struct target *target, target_addr_t address)
 {
 	struct watchpoint *watchpoint = target->watchpoints;
 
@@ -482,10 +507,32 @@ void watchpoint_remove(struct target *target, target_addr_t address)
 		watchpoint = watchpoint->next;
 	}
 
-	if (watchpoint)
+	if (watchpoint) {
 		watchpoint_free(target, watchpoint);
-	else
-		LOG_ERROR("no watchpoint at address " TARGET_ADDR_FMT " found", address);
+		return 1;
+	} else {
+		if (!target->smp)
+			LOG_ERROR("no watchpoint at address " TARGET_ADDR_FMT " found", address);
+		return 0;
+	}
+}
+
+void watchpoint_remove(struct target *target, target_addr_t address)
+{
+	int found = 0;
+	if (target->smp) {
+		struct target_list *head;
+		struct target *curr;
+		head = target->head;
+		while (head != (struct target_list *)NULL) {
+			curr = head->target;
+			found += watchpoint_remove_internal(curr, address);
+			head = head->next;
+		}
+		if (found == 0)
+			LOG_ERROR("no watchpoint at address " TARGET_ADDR_FMT " found", address);
+	} else
+		watchpoint_remove_internal(target, address);
 }
 
 void watchpoint_clear_target(struct target *target)
diff --git a/src/target/register.c b/src/target/register.c
index 8506414..dee25b2 100644
--- a/src/target/register.c
+++ b/src/target/register.c
@@ -36,6 +36,29 @@
  * may be separate registers associated with debug or trace modules.
  */
 
+struct reg *register_get_by_number(struct reg_cache *first,
+		uint32_t reg_num, bool search_all)
+{
+	unsigned i;
+	struct reg_cache *cache = first;
+
+	while (cache) {
+		for (i = 0; i < cache->num_regs; i++) {
+			if (cache->reg_list[i].exist == false)
+				continue;
+			if (cache->reg_list[i].number == reg_num)
+				return &(cache->reg_list[i]);
+		}
+
+		if (search_all)
+			cache = cache->next;
+		else
+			break;
+	}
+
+	return NULL;
+}
+
 struct reg *register_get_by_name(struct reg_cache *first,
 		const char *name, bool search_all)
 {
diff --git a/src/target/register.h b/src/target/register.h
index 32c1f39..7c53d6e 100644
--- a/src/target/register.h
+++ b/src/target/register.h
@@ -159,6 +159,8 @@ struct reg_arch_type {
 	int (*set)(struct reg *reg, uint8_t *buf);
 };
 
+struct reg *register_get_by_number(struct reg_cache *first,
+		uint32_t reg_num, bool search_all);
 struct reg *register_get_by_name(struct reg_cache *first,
 		const char *name, bool search_all);
 struct reg_cache **register_get_last_cache_p(struct reg_cache **first);
diff --git a/src/target/riscv/riscv-013.c b/src/target/riscv/riscv-013.c
index 3611d48..fe1d4cb 100644
--- a/src/target/riscv/riscv-013.c
+++ b/src/target/riscv/riscv-013.c
@@ -1146,7 +1146,6 @@ static int register_write_direct(struct target *target, unsigned number,
 	if (result == ERROR_OK && target->reg_cache) {
 		struct reg *reg = &target->reg_cache->reg_list[number];
 		buf_set_u64(reg->value, 0, reg->size, value);
-		reg->valid = true;
 	}
 	if (result == ERROR_OK || info->progbufsize + r->impebreak < 2 ||
 			!riscv_is_halted(target))
@@ -1205,7 +1204,6 @@ static int register_write_direct(struct target *target, unsigned number,
 	if (exec_out == ERROR_OK && target->reg_cache) {
 		struct reg *reg = &target->reg_cache->reg_list[number];
 		buf_set_u64(reg->value, 0, reg->size, value);
-		reg->valid = true;
 	}
 
 	if (use_scratch)
@@ -1225,24 +1223,12 @@ static int register_read(struct target *target, uint64_t *value, uint32_t number
 		*value = 0;
 		return ERROR_OK;
 	}
-	if (target->reg_cache &&
-			(number <= GDB_REGNO_XPR31 ||
-			 (number >= GDB_REGNO_FPR0 && number <= GDB_REGNO_FPR31))) {
-		/* Only check the cache for registers that we know won't spontaneously
-		 * change. */
-		struct reg *reg = &target->reg_cache->reg_list[number];
-		if (reg && reg->valid) {
-			*value = buf_get_u64(reg->value, 0, reg->size);
-			return ERROR_OK;
-		}
-	}
 	int result = register_read_direct(target, value, number);
 	if (result != ERROR_OK)
 		return ERROR_FAIL;
 	if (target->reg_cache) {
 		struct reg *reg = &target->reg_cache->reg_list[number];
 		buf_set_u64(reg->value, 0, reg->size, *value);
-		reg->valid = true;
 	}
 	return ERROR_OK;
 }
@@ -2834,7 +2820,7 @@ static int riscv013_get_register(struct target *target,
 	int result = ERROR_OK;
 	if (rid == GDB_REGNO_PC) {
 		result = register_read(target, value, GDB_REGNO_DPC);
-		LOG_DEBUG("read PC from DPC: 0x%016" PRIx64, *value);
+		LOG_DEBUG("read PC from DPC: 0x%" PRIx64, *value);
 	} else if (rid == GDB_REGNO_PRIV) {
 		uint64_t dcsr;
 		result = register_read(target, &dcsr, GDB_REGNO_DCSR);
@@ -2858,7 +2844,7 @@ static int riscv013_set_register(struct target *target, int hid, int rid, uint64
 	if (rid <= GDB_REGNO_XPR31) {
 		return register_write_direct(target, rid, value);
 	} else if (rid == GDB_REGNO_PC) {
-		LOG_DEBUG("writing PC to DPC: 0x%016" PRIx64, value);
+		LOG_DEBUG("writing PC to DPC: 0x%" PRIx64, value);
 		register_write_direct(target, GDB_REGNO_DPC, value);
 		uint64_t actual_value;
 		register_read_direct(target, &actual_value, GDB_REGNO_DPC);
@@ -3002,6 +2988,7 @@ static enum riscv_halt_reason riscv013_halt_reason(struct target *target)
 		 * already set when we connected. Force enumeration now, which has the
 		 * side effect of clearing any triggers we did not set. */
 		riscv_enumerate_triggers(target);
+		LOG_DEBUG("[%d] halted because of trigger", target->coreid);
 		return RISCV_HALT_TRIGGER;
 	case CSR_DCSR_CAUSE_STEP:
 		return RISCV_HALT_SINGLESTEP;
diff --git a/src/target/riscv/riscv.c b/src/target/riscv/riscv.c
index 6e56218..01b84bf 100644
--- a/src/target/riscv/riscv.c
+++ b/src/target/riscv/riscv.c
@@ -489,8 +489,8 @@ static int add_trigger(struct target *target, struct trigger *trigger)
 		if (result != ERROR_OK)
 			continue;
 
-		LOG_DEBUG("Using trigger %d (type %d) for bp %d", i, type,
-				trigger->unique_id);
+		LOG_DEBUG("[%d] Using trigger %d (type %d) for bp %d", target->coreid,
+				i, type, trigger->unique_id);
 		r->trigger_unique_id[i] = trigger->unique_id;
 		break;
 	}
@@ -512,6 +512,7 @@ static int add_trigger(struct target *target, struct trigger *trigger)
 
 int riscv_add_breakpoint(struct target *target, struct breakpoint *breakpoint)
 {
+	LOG_DEBUG("[%d] @0x%" TARGET_PRIxADDR, target->coreid, breakpoint->address);
 	assert(breakpoint);
 	if (breakpoint->type == BKPT_SOFT) {
 		/** @todo check RVC for size/alignment */
@@ -585,7 +586,8 @@ static int remove_trigger(struct target *target, struct trigger *trigger)
 				"trigger.");
 		return ERROR_FAIL;
 	}
-	LOG_DEBUG("Stop using resource %d for bp %d", i, trigger->unique_id);
+	LOG_DEBUG("[%d] Stop using resource %d for bp %d", target->coreid, i,
+			trigger->unique_id);
 	for (int hartid = first_hart; hartid < riscv_count_harts(target); ++hartid) {
 		if (!riscv_hart_enabled(target, hartid))
 			continue;
@@ -660,6 +662,8 @@ int riscv_add_watchpoint(struct target *target, struct watchpoint *watchpoint)
 int riscv_remove_watchpoint(struct target *target,
 		struct watchpoint *watchpoint)
 {
+	LOG_DEBUG("[%d] @0x%" TARGET_PRIxADDR, target->coreid, watchpoint->address);
+
 	struct trigger trigger;
 	trigger_from_watchpoint(&trigger, watchpoint);
 
@@ -829,13 +833,15 @@ static int old_or_new_riscv_halt(struct target *target)
 
 static int riscv_assert_reset(struct target *target)
 {
+	LOG_DEBUG("[%d]", target->coreid);
 	struct target_type *tt = get_target_type(target);
+	riscv_invalidate_register_cache(target);
 	return tt->assert_reset(target);
 }
 
 static int riscv_deassert_reset(struct target *target)
 {
-	LOG_DEBUG("RISCV DEASSERT RESET");
+	LOG_DEBUG("[%d]", target->coreid);
 	struct target_type *tt = get_target_type(target);
 	return tt->deassert_reset(target);
 }
@@ -856,8 +862,28 @@ static int old_or_new_riscv_resume(
 		int handle_breakpoints,
 		int debug_execution
 ){
-	RISCV_INFO(r);
 	LOG_DEBUG("handle_breakpoints=%d", handle_breakpoints);
+	if (target->smp) {
+		struct target_list *targets = target->head;
+		int result = ERROR_OK;
+		while (targets) {
+			struct target *t = targets->target;
+			riscv_info_t *r = riscv_info(t);
+			if (r->is_halted == NULL) {
+				if (oldriscv_resume(t, current, address, handle_breakpoints,
+							debug_execution) != ERROR_OK)
+					result = ERROR_FAIL;
+			} else {
+				if (riscv_openocd_resume(t, current, address,
+							handle_breakpoints, debug_execution) != ERROR_OK)
+					result = ERROR_FAIL;
+			}
+			targets = targets->next;
+		}
+		return result;
+	}
+
+	RISCV_INFO(r);
 	if (r->is_halted == NULL)
 		return oldriscv_resume(target, current, address, handle_breakpoints, debug_execution);
 	else
@@ -911,7 +937,7 @@ static int riscv_get_gdb_reg_list(struct target *target,
 
 	switch (reg_class) {
 		case REG_CLASS_GENERAL:
-			*reg_list_size = 32;
+			*reg_list_size = 33;
 			break;
 		case REG_CLASS_ALL:
 			*reg_list_size = target->reg_cache->num_regs;
@@ -925,10 +951,19 @@ static int riscv_get_gdb_reg_list(struct target *target,
 	if (!*reg_list)
 		return ERROR_FAIL;
 
+	bool read = true;
 	for (int i = 0; i < *reg_list_size; i++) {
 		assert(!target->reg_cache->reg_list[i].valid ||
 				target->reg_cache->reg_list[i].size > 0);
 		(*reg_list)[i] = &target->reg_cache->reg_list[i];
+		if (read && !target->reg_cache->reg_list[i].valid) {
+			/* This function gets called from
+			 * gdb_target_description_supported(), and we end up failing in
+			 * that case. Allow failures for now. */
+			if (target->reg_cache->reg_list[i].type->get(
+						&target->reg_cache->reg_list[i]) != ERROR_OK)
+				read = false;
+		}
 	}
 
 	return ERROR_OK;
@@ -1110,6 +1145,30 @@ static enum riscv_poll_hart riscv_poll_hart(struct target *target, int hartid)
 	return RPH_NO_CHANGE;
 }
 
+int set_debug_reason(struct target *target, int hartid)
+{
+	switch (riscv_halt_reason(target, hartid)) {
+		case RISCV_HALT_BREAKPOINT:
+			target->debug_reason = DBG_REASON_BREAKPOINT;
+			break;
+		case RISCV_HALT_TRIGGER:
+			target->debug_reason = DBG_REASON_WATCHPOINT;
+			break;
+		case RISCV_HALT_INTERRUPT:
+			target->debug_reason = DBG_REASON_DBGRQ;
+			break;
+		case RISCV_HALT_SINGLESTEP:
+			target->debug_reason = DBG_REASON_SINGLESTEP;
+			break;
+		case RISCV_HALT_UNKNOWN:
+			target->debug_reason = DBG_REASON_UNDEFINED;
+			break;
+		case RISCV_HALT_ERROR:
+			return ERROR_FAIL;
+	}
+	return ERROR_OK;
+}
+
 /*** OpenOCD Interface ***/
 int riscv_openocd_poll(struct target *target)
 {
@@ -1144,6 +1203,64 @@ int riscv_openocd_poll(struct target *target)
 		 * harts. */
 		for (int i = 0; i < riscv_count_harts(target); ++i)
 			riscv_halt_one_hart(target, i);
+
+	} else if (target->smp) {
+		bool halt_discovered = false;
+		bool newly_halted[128] = {0};
+		unsigned i = 0;
+		for (struct target_list *list = target->head; list != NULL;
+				list = list->next, i++) {
+			struct target *t = list->target;
+			riscv_info_t *r = riscv_info(t);
+			assert(i < DIM(newly_halted));
+			enum riscv_poll_hart out = riscv_poll_hart(t, r->current_hartid);
+			switch (out) {
+				case RPH_NO_CHANGE:
+					break;
+				case RPH_DISCOVERED_RUNNING:
+					t->state = TARGET_RUNNING;
+					break;
+				case RPH_DISCOVERED_HALTED:
+					halt_discovered = true;
+					newly_halted[i] = true;
+					t->state = TARGET_HALTED;
+					if (set_debug_reason(t, r->current_hartid) != ERROR_OK)
+						return ERROR_FAIL;
+					break;
+				case RPH_ERROR:
+					return ERROR_FAIL;
+			}
+		}
+
+		if (halt_discovered) {
+			LOG_DEBUG("Halt other targets in this SMP group.");
+			i = 0;
+			for (struct target_list *list = target->head; list != NULL;
+					list = list->next, i++) {
+				struct target *t = list->target;
+				riscv_info_t *r = riscv_info(t);
+				if (t->state != TARGET_HALTED) {
+					if (riscv_halt_one_hart(t, r->current_hartid) != ERROR_OK)
+						return ERROR_FAIL;
+					t->state = TARGET_HALTED;
+					if (set_debug_reason(t, r->current_hartid) != ERROR_OK)
+						return ERROR_FAIL;
+					newly_halted[i] = true;
+				}
+			}
+
+			/* Now that we have all our ducks in a row, tell the higher layers
+			 * what just happened. */
+			i = 0;
+			for (struct target_list *list = target->head; list != NULL;
+					list = list->next, i++) {
+				struct target *t = list->target;
+				if (newly_halted[i])
+					target_call_event_callbacks(t, TARGET_EVENT_HALTED);
+			}
+		}
+		return ERROR_OK;
+
 	} else {
 		enum riscv_poll_hart out = riscv_poll_hart(target,
 				riscv_current_hartid(target));
@@ -1157,25 +1274,8 @@ int riscv_openocd_poll(struct target *target)
 	}
 
 	target->state = TARGET_HALTED;
-	switch (riscv_halt_reason(target, halted_hart)) {
-	case RISCV_HALT_BREAKPOINT:
-		target->debug_reason = DBG_REASON_BREAKPOINT;
-		break;
-	case RISCV_HALT_TRIGGER:
-		target->debug_reason = DBG_REASON_WATCHPOINT;
-		break;
-	case RISCV_HALT_INTERRUPT:
-		target->debug_reason = DBG_REASON_DBGRQ;
-		break;
-	case RISCV_HALT_SINGLESTEP:
-		target->debug_reason = DBG_REASON_SINGLESTEP;
-		break;
-	case RISCV_HALT_UNKNOWN:
-		target->debug_reason = DBG_REASON_UNDEFINED;
-		break;
-	case RISCV_HALT_ERROR:
+	if (set_debug_reason(target, halted_hart) != ERROR_OK)
 		return ERROR_FAIL;
-	}
 
 	if (riscv_rtos_enabled(target)) {
 		target->rtos->current_threadid = halted_hart + 1;
@@ -1198,16 +1298,26 @@ int riscv_openocd_poll(struct target *target)
 int riscv_openocd_halt(struct target *target)
 {
 	RISCV_INFO(r);
+	int result;
 
-	LOG_DEBUG("halting all harts");
+	LOG_DEBUG("[%d] halting all harts", target->coreid);
 
-	int out = riscv_halt_all_harts(target);
-	if (out != ERROR_OK) {
-		LOG_ERROR("Unable to halt all harts");
-		return out;
+	if (target->smp) {
+		LOG_DEBUG("Halt other targets in this SMP group.");
+		struct target_list *targets = target->head;
+		result = ERROR_OK;
+		while (targets) {
+			struct target *t = targets->target;
+			targets = targets->next;
+			if (t->state != TARGET_HALTED) {
+				if (riscv_halt_all_harts(t) != ERROR_OK)
+					result = ERROR_FAIL;
+			}
+		}
+	} else {
+		result = riscv_halt_all_harts(target);
 	}
 
-	register_cache_invalidate(target->reg_cache);
 	if (riscv_rtos_enabled(target)) {
 		if (r->rtos_hartid != -1) {
 			LOG_DEBUG("halt requested on RTOS hartid %d", r->rtos_hartid);
@@ -1220,7 +1330,7 @@ int riscv_openocd_halt(struct target *target)
 	target->state = TARGET_HALTED;
 	target->debug_reason = DBG_REASON_DBGRQ;
 	target_call_event_callbacks(target, TARGET_EVENT_HALTED);
-	return out;
+	return result;
 }
 
 int riscv_openocd_resume(
@@ -1909,7 +2019,9 @@ int riscv_halt_one_hart(struct target *target, int hartid)
 		return ERROR_OK;
 	}
 
-	return r->halt_current_hart(target);
+	int result = r->halt_current_hart(target);
+	register_cache_invalidate(target->reg_cache);
+	return result;
 }
 
 int riscv_resume_all_harts(struct target *target)
@@ -1997,9 +2109,10 @@ int riscv_xlen_of_hart(const struct target *target, int hartid)
 	return r->xlen[hartid];
 }
 
+extern struct rtos_type riscv_rtos;
 bool riscv_rtos_enabled(const struct target *target)
 {
-	return target->rtos != NULL;
+	return target->rtos && target->rtos->type == &riscv_rtos;
 }
 
 int riscv_set_current_hartid(struct target *target, int hartid)
@@ -2017,10 +2130,9 @@ int riscv_set_current_hartid(struct target *target, int hartid)
 
 	/* This might get called during init, in which case we shouldn't be
 	 * setting up the register cache. */
-	if (!target_was_examined(target))
-		return ERROR_OK;
+	if (target_was_examined(target) && riscv_rtos_enabled(target))
+		riscv_invalidate_register_cache(target);
 
-	riscv_invalidate_register_cache(target);
 	return ERROR_OK;
 }
 
@@ -2104,6 +2216,12 @@ int riscv_get_register_on_hart(struct target *target, riscv_reg_t *value,
 	if (hartid != riscv_current_hartid(target))
 		riscv_invalidate_register_cache(target);
 
+	struct reg *reg = &target->reg_cache->reg_list[regid];
+	if (reg && reg->valid) {
+		*value = buf_get_u64(reg->value, 0, reg->size);
+		return ERROR_OK;
+	}
+
 	int result = r->get_register(target, value, hartid, regid);
 
 	if (hartid != riscv_current_hartid(target))
@@ -2317,6 +2435,15 @@ static int register_get(struct reg *reg)
 	if (result != ERROR_OK)
 		return result;
 	buf_set_u64(reg->value, 0, reg->size, value);
+	/* CSRs (and possibly other extension) registers may change value at any
+	 * time. */
+	if (reg->number <= GDB_REGNO_XPR31 ||
+			(reg->number >= GDB_REGNO_FPR0 && reg->number <= GDB_REGNO_FPR31) ||
+			reg->number == GDB_REGNO_PC)
+		reg->valid = true;
+	LOG_DEBUG("[%d,%d] read 0x%" PRIx64 " from %s (valid=%d)",
+			target->coreid, riscv_current_hartid(target), value, reg->name,
+			reg->valid);
 	return ERROR_OK;
 }
 
@@ -2327,9 +2454,16 @@ static int register_set(struct reg *reg, uint8_t *buf)
 
 	uint64_t value = buf_get_u64(buf, 0, reg->size);
 
-	LOG_DEBUG("write 0x%" PRIx64 " to %s", value, reg->name);
+	LOG_DEBUG("[%d,%d] write 0x%" PRIx64 " to %s (valid=%d)",
+			target->coreid, riscv_current_hartid(target), value, reg->name,
+			reg->valid);
 	struct reg *r = &target->reg_cache->reg_list[reg->number];
-	r->valid = true;
+	/* CSRs (and possibly other extension) registers may change value at any
+	 * time. */
+	if (reg->number <= GDB_REGNO_XPR31 ||
+			(reg->number >= GDB_REGNO_FPR0 && reg->number <= GDB_REGNO_FPR31) ||
+			reg->number == GDB_REGNO_PC)
+		r->valid = true;
 	memcpy(r->value, buf, (r->size + 7) / 8);
 
 	riscv_set_register(target, reg->number, value);
diff --git a/src/target/target.c b/src/target/target.c
index 7de3e76..ffd82fb 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -1575,8 +1575,9 @@ int target_call_event_callbacks(struct target *target, enum target_event event)
 		target_call_event_callbacks(target, TARGET_EVENT_GDB_HALT);
 	}
 
-	LOG_DEBUG("target event %i (%s)", event,
-			Jim_Nvp_value2name_simple(nvp_target_event, event)->name);
+	LOG_DEBUG("target event %i (%s) for core %d", event,
+			Jim_Nvp_value2name_simple(nvp_target_event, event)->name,
+			target->coreid);
 
 	target_handle_event(target, event);