Add a RISC-V RTOS, which natievly supports multiple harts

This is a work in progress, but it's at the point where you can actually
debug multi-hart programs now.

11 files changed, 1398 insertions, 963 deletions

diff --git a/src/rtos/Makefile.am b/src/rtos/Makefile.am
index fdca394..a7dab00 100644
--- a/src/rtos/Makefile.am
+++ b/src/rtos/Makefile.am
@@ -20,8 +20,8 @@ include $(top_srcdir)/common.mk
 
 METASOURCES = AUTO
 noinst_LTLIBRARIES = librtos.la
-noinst_HEADERS = rtos.h rtos_standard_stackings.h rtos_ecos_stackings.h linux_header.h rtos_chibios_stackings.h rtos_embkernel_stackings.h rtos_mqx_stackings.h
-librtos_la_SOURCES = rtos.c rtos_standard_stackings.c rtos_ecos_stackings.c  rtos_chibios_stackings.c rtos_embkernel_stackings.c rtos_mqx_stackings.c FreeRTOS.c ThreadX.c eCos.c linux.c ChibiOS.c embKernel.c mqx.c
+noinst_HEADERS = rtos.h rtos_standard_stackings.h rtos_ecos_stackings.h linux_header.h rtos_chibios_stackings.h rtos_embkernel_stackings.h rtos_mqx_stackings.h riscv_debug.h
+librtos_la_SOURCES = rtos.c rtos_standard_stackings.c rtos_ecos_stackings.c  rtos_chibios_stackings.c rtos_embkernel_stackings.c rtos_mqx_stackings.c FreeRTOS.c ThreadX.c eCos.c linux.c ChibiOS.c embKernel.c mqx.c riscv_debug.c
 
 librtos_la_CFLAGS =
 if IS_MINGW
diff --git a/src/rtos/riscv_debug.c b/src/rtos/riscv_debug.c
new file mode 100644
index 0000000..181d471
--- /dev/null
+++ b/src/rtos/riscv_debug.c
@@ -0,0 +1,280 @@
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "riscv_debug.h"
+#include "target/target.h"
+#include "target/riscv/riscv.h"
+#include "rtos.h"
+#include "server/gdb_server.h"
+
+static int riscv_update_threads(struct rtos *rtos);
+static int riscv_gdb_thread_packet(struct connection *connection, const char *packet, int packet_size);
+
+static int riscv_detect_rtos(struct target *target)
+{
+	LOG_ERROR("riscv_detect_rtos() unimplemented");
+	return -1;
+}
+
+static int riscv_create_rtos(struct target *target)
+{
+	LOG_DEBUG("RISC-V Debug 'RTOS' created: this doesn't meat you're running an RTOS, just that you have multi-hart support on RISC-V");
+
+	struct riscv_rtos *r = calloc(1, sizeof(*r));
+	target->rtos->rtos_specific_params = r;
+#if 0
+	r->target_hartid = 0;
+	r->target_any_hart = true;
+	r->target_every_hart = true;
+#endif
+
+	target->rtos->current_threadid = 1;
+	target->rtos->current_thread = 1;
+	riscv_update_threads(target->rtos);
+
+	target->rtos->gdb_thread_packet = riscv_gdb_thread_packet;
+
+	return JIM_OK;
+}
+
+static int riscv_update_threads(struct rtos *rtos)
+{
+	LOG_DEBUG("Updating the RISC-V Hart List");
+
+	/* Figures out how many harts there are on the system. */
+	int hart_count = riscv_count_harts(rtos->target);
+	if (rtos->thread_count != hart_count) {
+		rtos_free_threadlist(rtos);
+		rtos->thread_count = hart_count;
+		rtos->thread_details = calloc(rtos->thread_count, sizeof(*rtos->thread_details));
+		for (int i = 0; i < rtos->thread_count; ++i) {
+			LOG_DEBUG("  Setting up Hart %d", i);
+			rtos->thread_details[i].threadid = i + 1;
+			rtos->thread_details[i].exists = true;
+			if (asprintf(&rtos->thread_details[i].thread_name_str, "Hart %d", i) < 0)
+				LOG_ERROR("riscv_update_threads() failed asprintf");
+			if (asprintf(&rtos->thread_details[i].extra_info_str, "RV64") < 0)
+				LOG_ERROR("riscv_update_threads() failed asprintf");
+		}
+	}
+	return JIM_OK;
+}
+
+static int riscv_gdb_thread_packet(struct connection *connection, const char *packet, int packet_size)
+{
+	struct target *target = get_target_from_connection(connection);
+	struct rtos *rtos = target->rtos;
+	struct riscv_rtos *r = (struct riscv_rtos *)(target->rtos->rtos_specific_params);
+
+		char *packet_stttrr = malloc(packet_size + 1);
+		memset(packet_stttrr, '\0', packet_size + 1);
+		memcpy(packet_stttrr, packet, packet_size);
+		LOG_DEBUG("riscv_gdb_thread_packet(%s)", packet_stttrr);
+
+	switch (packet[0]) {
+	case 'q':
+		if (strncmp(packet, "qfThreadInfo", 12) == 0) {
+			riscv_update_threads(target->rtos);
+			r->qs_thread_info_offset = 1;
+			
+			char m[16];
+			snprintf(m, 16, "m%08x", (int)rtos->thread_details[0].threadid);
+			gdb_put_packet(connection, m, strlen(m));
+			return ERROR_OK;
+		}
+
+		if (strncmp(packet, "qsThreadInfo", 12) == 0) {
+			if (r->qs_thread_info_offset >= rtos->thread_count) {
+				gdb_put_packet(connection, "l", 1);
+				return ERROR_OK;
+			}
+
+			int tid = r->qs_thread_info_offset++;
+			char m[16];
+			snprintf(m, 16, "m%08x", (int)rtos->thread_details[tid].threadid);
+			gdb_put_packet(connection, m, strlen(m));
+			return ERROR_OK;
+		}
+
+		if (strncmp(packet, "qAttached", 9) == 0) {
+			gdb_put_packet(connection, "1", 1);
+			return ERROR_OK;
+		}
+
+		if (strncmp(packet, "qThreadExtraInfo", 16) == 0) {
+			char tid_str[32];
+			memcpy(tid_str, packet + 17, packet_size - 17);
+			tid_str[packet_size - 17] = '\0';
+			char *end;
+			int tid = strtol(tid_str, &end, 16);
+			if (*end != '\0') {
+				LOG_ERROR("Got qThreadExtraInfo with non-numeric TID: '%s'", tid_str);
+				gdb_put_packet(connection, NULL, 0);
+				return ERROR_FAIL;
+			}
+
+			char m[16];
+			snprintf(m, 16, "hart %d", tid);
+			char h[33];
+			h[0] = '\0';
+			for (size_t i = 0; i < strlen(m); ++i) {
+				char byte[3];
+				snprintf(byte, 3, "%02x", m[i]);
+				strncat(h, byte, 32);
+			}
+			gdb_put_packet(connection, h, strlen(h));
+			return ERROR_OK;
+		}
+
+		return GDB_THREAD_PACKET_NOT_CONSUMED;
+
+	case 'Q':
+		return GDB_THREAD_PACKET_NOT_CONSUMED;
+
+	case 'H':
+		/* ‘H op thread-id’
+		 *
+		 * Set thread for subsequent operations (‘m’, ‘M’, ‘g’, ‘G’,
+		 * et.al.). Depending on the operation to be performed, op
+		 * should be ‘c’ for step and continue operations (note that
+		 * this is deprecated, supporting the ‘vCont’ command is a
+		 * better option), and ‘g’ for other operations. The thread
+		 * designator thread-id has the format and interpretation
+		 * described in thread-id syntax.
+		 *
+		 * Reply:
+		 * ‘OK’ for success
+		 * ‘E NN’ for an error
+		 */
+	{
+		char tid_str[32];
+		memcpy(tid_str, packet + 2, packet_size - 2);
+		tid_str[packet_size - 2] = '\0';
+		char *entptr;
+		int tid = strtol(tid_str, &entptr, 16);
+		if (*entptr != '\0') {
+			LOG_ERROR("Got H packet, but without integer: %s", tid_str);
+			return GDB_THREAD_PACKET_NOT_CONSUMED;
+		}
+
+		riscv_enable_rtos(target);
+		switch (tid) {
+		case 0:
+		case -1:
+			riscv_set_all_rtos_harts(target);
+			break;
+		default:
+			riscv_set_rtos_hartid(target, tid - 1);
+			break;
+		}
+
+		switch (packet[1]) {
+		case 'g':
+		case 'c':
+			gdb_put_packet(connection, "OK", 2);
+			return ERROR_OK;
+		default:
+			LOG_ERROR("Unknown H packet subtype %2x\n", packet[1]);
+			gdb_put_packet(connection, NULL, 0);
+			return ERROR_FAIL;
+		}
+	}
+
+	case 'T':
+	{
+		char tid_str[32];
+		memcpy(tid_str, packet + 1, packet_size - 1);
+		tid_str[packet_size - 1] = '\0';
+		char *end;
+		int tid = strtol(tid_str, &end, 16);
+		if (*end != '\0') {
+			LOG_ERROR("T packet with non-numeric tid %s", tid_str);
+			gdb_put_packet(connection, NULL, 0);
+			return ERROR_FAIL;
+		}
+
+		riscv_enable_rtos(target);
+		riscv_update_threads(target->rtos);
+		if (tid <= target->rtos->thread_count) {
+			gdb_put_packet(connection, "OK", 2);
+			return ERROR_OK;
+		} else {
+			gdb_put_packet(connection, "E00", 3);
+			return ERROR_OK;
+		}
+	}
+
+
+	case 'c':
+	case 's':
+		target->state = TARGET_HALTED;
+		return JIM_OK;
+
+	case 'R':
+	{
+		char *packet_str = malloc(packet_size + 1);
+		memset(packet_str, '\0', packet_size + 1);
+		memcpy(packet_str, packet, packet_size);
+		LOG_WARNING("riscv_gdb_thread_packet(%s): unimplemented", packet_str);
+		gdb_put_packet(connection, NULL, 0);
+		return JIM_OK;
+	}
+	default:
+		LOG_ERROR("Unknown packet of type 0x%2.2x", packet[0]);
+		gdb_put_packet(connection, NULL, 0);
+		return JIM_OK;
+	}
+}
+
+static int riscv_get_thread_reg_list(struct rtos *rtos, int64_t thread_id, char **hex_reg_list)
+{
+	LOG_DEBUG("Updating RISC-V regiser list for hart %d", (int)(thread_id - 1));
+
+#if 0
+	LOG_ERROR("  Not actually updating");
+	*hex_reg_list = 0;
+	return JIM_OK;
+#endif
+
+	size_t n_regs = 32;
+	size_t xlen = 64;
+	size_t reg_chars = xlen / 8 * 2;
+
+	ssize_t hex_reg_list_length = n_regs * reg_chars + 2;
+	*hex_reg_list = malloc(hex_reg_list_length);
+	*hex_reg_list[0] = '\0';
+	for (size_t i = 0; i < n_regs; ++i) {
+		if (riscv_has_register(rtos->target, thread_id, i)) {
+			uint64_t reg_value = riscv_get_register(rtos->target, thread_id - 1, i);
+			for (size_t byte = 0; byte < xlen / 8; ++byte) {
+				uint8_t reg_byte = reg_value >> (byte * 8);
+				char hex[3];
+				snprintf(hex, 3, "%02x", reg_byte);
+				strncat(*hex_reg_list, hex, hex_reg_list_length);
+			}
+		} else {
+			for (size_t byte = 0; byte < xlen / 8; ++byte)
+				strncat(*hex_reg_list, "xx", hex_reg_list_length);
+		}
+	}
+	return JIM_OK;
+}
+
+static int riscv_get_symbol_list_to_lookup(symbol_table_elem_t *symbol_list[])
+{
+	*symbol_list = calloc(1, sizeof(symbol_table_elem_t));
+	(*symbol_list)[0].symbol_name = NULL;
+	(*symbol_list)[0].optional = false;
+	return JIM_OK;
+}
+
+const struct rtos_type riscv_rtos =
+{
+	.name = "riscv",
+	.detect_rtos = riscv_detect_rtos,
+	.create = riscv_create_rtos,
+	.update_threads = riscv_update_threads,
+	.get_thread_reg_list = riscv_get_thread_reg_list,
+	.get_symbol_list_to_lookup = riscv_get_symbol_list_to_lookup,
+};
diff --git a/src/rtos/riscv_debug.h b/src/rtos/riscv_debug.h
new file mode 100644
index 0000000..bcc7411
--- /dev/null
+++ b/src/rtos/riscv_debug.h
@@ -0,0 +1,9 @@
+#ifndef RTOS__RISCV_H
+#define RTOS__RISCV_H
+
+struct riscv_rtos {
+	/* The index into the thread list used to handle */
+	int qs_thread_info_offset;
+};
+
+#endif
diff --git a/src/rtos/rtos.c b/src/rtos/rtos.c
index 4c99ad2..76443b6 100644
--- a/src/rtos/rtos.c
+++ b/src/rtos/rtos.c
@@ -34,6 +34,7 @@ extern struct rtos_type Linux_os;
 extern struct rtos_type ChibiOS_rtos;
 extern struct rtos_type embKernel_rtos;
 extern struct rtos_type mqx_rtos;
+extern struct rtos_type riscv_rtos;
 
 static struct rtos_type *rtos_types[] = {
 	&ThreadX_rtos,
@@ -43,6 +44,7 @@ static struct rtos_type *rtos_types[] = {
 	&ChibiOS_rtos,
 	&embKernel_rtos,
 	&mqx_rtos,
+	&riscv_rtos,
 	NULL
 };
 
@@ -418,7 +420,7 @@ int rtos_get_gdb_reg_list(struct connection *connection)
 			(target->smp))) {	/* in smp several current thread are possible */
 		char *hex_reg_list;
 
-		LOG_DEBUG("RTOS: getting register list for thread 0x%" PRIx64
+		LOG_INFO("RTOS: getting register list for thread 0x%" PRIx64
 				  ", target->rtos->current_thread=0x%" PRIx64 "\r\n",
 										current_threadid,
 										target->rtos->current_thread);
diff --git a/src/target/riscv/asm.h b/src/target/riscv/asm.h
new file mode 100644
index 0000000..051e0f9
--- /dev/null
+++ b/src/target/riscv/asm.h
@@ -0,0 +1,36 @@
+#ifndef TARGET__RISCV__ASM_H
+#define TARGET__RISCV__ASM_H
+
+#include "riscv.h"
+
+/*** Version-independent functions that we don't want in the main address space. ***/
+
+static uint32_t load(const struct target *target, unsigned int rd,
+                unsigned int base, uint16_t offset) __attribute__ ((unused));
+static uint32_t load(const struct target *target, unsigned int rd,
+                unsigned int base, uint16_t offset)
+{
+        switch (riscv_xlen(target)) {
+                case 32:
+                        return lw(rd, base, offset);
+                case 64:
+                        return ld(rd, base, offset);
+        }
+        assert(0);
+}
+
+static uint32_t store(const struct target *target, unsigned int src,
+                unsigned int base, uint16_t offset) __attribute__ ((unused));
+static uint32_t store(const struct target *target, unsigned int src,
+                unsigned int base, uint16_t offset)
+{
+        switch (riscv_xlen(target)) {
+                case 32:
+                        return sw(src, base, offset);
+                case 64:
+                        return sd(src, base, offset);
+        }
+        assert(0);
+}
+
+#endif
diff --git a/src/target/riscv/gdb_regs.h b/src/target/riscv/gdb_regs.h
new file mode 100644
index 0000000..a12c36f
--- /dev/null
+++ b/src/target/riscv/gdb_regs.h
@@ -0,0 +1,25 @@
+#ifndef TARGET__RISCV__GDB_REGS_H
+#define TARGET__RISCV__GDB_REGS_H
+
+enum gdb_regno {
+        GDB_REGNO_XPR0 = 0,
+	GDB_REGNO_S0 = GDB_REGNO_XPR0 + 8,
+	GDB_REGNO_S1 = GDB_REGNO_XPR0 + 9,
+        GDB_REGNO_XPR31 = GDB_REGNO_XPR0 + 31,
+        GDB_REGNO_PC = 32,
+        GDB_REGNO_FPR0 = 33,
+        GDB_REGNO_FPR31 = GDB_REGNO_FPR0 + 31,
+        GDB_REGNO_CSR0 = 65,
+        GDB_REGNO_TSELECT = CSR_TSELECT + GDB_REGNO_CSR0,
+        GDB_REGNO_TDATA1 = CSR_TDATA1 + GDB_REGNO_CSR0,
+        GDB_REGNO_TDATA2 = CSR_TDATA2 + GDB_REGNO_CSR0,
+        GDB_REGNO_MISA = CSR_MISA + GDB_REGNO_CSR0,
+        GDB_REGNO_DPC = CSR_DPC + GDB_REGNO_CSR0,
+        GDB_REGNO_DCSR = CSR_DCSR + GDB_REGNO_CSR0,
+        GDB_REGNO_MSTATUS = CSR_MSTATUS + GDB_REGNO_CSR0,
+        GDB_REGNO_CSR4095 = GDB_REGNO_CSR0 + 4095,
+        GDB_REGNO_PRIV = 4161,
+        GDB_REGNO_COUNT
+};
+
+#endif
diff --git a/src/target/riscv/opcodes.h b/src/target/riscv/opcodes.h
index 5a47e08..f5fac02 100644
--- a/src/target/riscv/opcodes.h
+++ b/src/target/riscv/opcodes.h
@@ -271,3 +271,9 @@ static uint32_t srli(unsigned int dest, unsigned int src, uint8_t shamt)
 		(dest << 7) |
 		MATCH_SRLI;
 }
+
+static uint32_t fence(void) __attribute__((unused));
+static uint32_t fence(void)
+{
+	return MATCH_FENCE;
+}
diff --git a/src/target/riscv/riscv-011.c b/src/target/riscv/riscv-011.c
index e940d20..ed2a057 100644
--- a/src/target/riscv/riscv-011.c
+++ b/src/target/riscv/riscv-011.c
@@ -20,6 +20,7 @@
 #include "breakpoints.h"
 #include "helper/time_support.h"
 #include "riscv.h"
+#include "asm.h"
 
 /**
  * Since almost everything can be accomplish by scanning the dbus register, all
@@ -260,7 +261,7 @@ static riscv011_info_t *get_info(const struct target *target)
 static unsigned int slot_offset(const struct target *target, slot_t slot)
 {
 	riscv011_info_t *info = get_info(target);
-	switch (xlen(target)) {
+	switch (riscv_xlen(target)) {
 		case 32:
 			switch (slot) {
 				case SLOT0: return 4;
@@ -275,7 +276,7 @@ static unsigned int slot_offset(const struct target *target, slot_t slot)
 			}
 	}
 	LOG_ERROR("slot_offset called with xlen=%d, slot=%d",
-			xlen(target), slot);
+			riscv_xlen(target), slot);
 	assert(0);
 }
 
@@ -537,8 +538,8 @@ static scans_t *scans_new(struct target *target, unsigned int scan_count)
 	scans_t *scans = malloc(sizeof(scans_t));
 	scans->scan_count = scan_count;
 	// This code also gets called before xlen is detected.
-	if (xlen(target))
-		scans->scan_size = 2 + xlen(target) / 8;
+	if (riscv_xlen(target))
+		scans->scan_size = 2 + riscv_xlen(target) / 8;
 	else
 		scans->scan_size = 2 + 128 / 8;
 	scans->next_scan = 0;
@@ -641,7 +642,7 @@ static void scans_add_read32(scans_t *scans, uint16_t address, bool set_interrup
 static void scans_add_read(scans_t *scans, slot_t slot, bool set_interrupt)
 {
 	const struct target *target = scans->target;
-	switch (xlen(target)) {
+	switch (riscv_xlen(target)) {
 		case 32:
 			scans_add_read32(scans, slot_offset(target, slot), set_interrupt);
 			break;
@@ -776,7 +777,7 @@ static void cache_set(struct target *target, slot_t slot, uint64_t data)
 {
 	unsigned int offset = slot_offset(target, slot);
 	cache_set32(target, offset, data);
-	if (xlen(target) > 32) {
+	if (riscv_xlen(target) > 32) {
 		cache_set32(target, offset + 1, data >> 32);
 	}
 }
@@ -996,7 +997,7 @@ static uint64_t cache_get(struct target *target, slot_t slot)
 {
 	unsigned int offset = slot_offset(target, slot);
 	uint64_t value = cache_get32(target, offset);
-	if (xlen(target) > 32) {
+	if (riscv_xlen(target) > 32) {
 		value |= ((uint64_t) cache_get32(target, offset + 1)) << 32;
 	}
 	return value;
@@ -1117,7 +1118,7 @@ static int execute_resume(struct target *target, bool step)
 
 	struct reg *mstatus_reg = &target->reg_cache->reg_list[REG_MSTATUS];
 	if (mstatus_reg->valid) {
-		uint64_t mstatus_user = buf_get_u64(mstatus_reg->value, 0, xlen(target));
+		uint64_t mstatus_user = buf_get_u64(mstatus_reg->value, 0, riscv_xlen(target));
 		if (mstatus_user != info->mstatus_actual) {
 			cache_set_load(target, 0, S0, SLOT0);
 			cache_set32(target, 1, csrw(S0, CSR_MSTATUS));
@@ -1198,18 +1199,18 @@ static void update_reg_list(struct target *target)
 	if (info->reg_values) {
 		free(info->reg_values);
 	}
-	info->reg_values = malloc(REG_COUNT * xlen(target) / 4);
+	info->reg_values = malloc(REG_COUNT * riscv_xlen(target) / 4);
 
 	for (unsigned int i = 0; i < REG_COUNT; i++) {
 		struct reg *r = &target->reg_cache->reg_list[i];
-		r->value = info->reg_values + i * xlen(target) / 4;
+		r->value = info->reg_values + i * riscv_xlen(target) / 4;
 		if (r->dirty) {
 			LOG_ERROR("Register %d was dirty. Its value is lost.", i);
 		}
 		if (i == REG_PRIV) {
 			r->size = 8;
 		} else {
-			r->size = xlen(target);
+			r->size = riscv_xlen(target);
 		}
 		r->valid = false;
 	}
@@ -1259,7 +1260,7 @@ static int register_get(struct reg *reg)
 	maybe_write_tselect(target);
 
 	if (reg->number <= REG_XPR31) {
-		buf_set_u64(reg->value, 0, xlen(target), reg_cache_get(target, reg->number));
+		buf_set_u64(reg->value, 0, riscv_xlen(target), reg_cache_get(target, reg->number));
 		LOG_DEBUG("%s=0x%" PRIx64, reg->name, reg_cache_get(target, reg->number));
 		return ERROR_OK;
 	} else if (reg->number == REG_PC) {
@@ -1280,7 +1281,7 @@ static int register_get(struct reg *reg)
 			cache_set(target, SLOT1, info->mstatus_actual);
 		}
 
-		if (xlen(target) == 32) {
+		if (riscv_xlen(target) == 32) {
 			cache_set32(target, i++, fsw(reg->number - REG_FPR0, 0, DEBUG_RAM_START + 16));
 		} else {
 			cache_set32(target, i++, fsd(reg->number - REG_FPR0, 0, DEBUG_RAM_START + 16));
@@ -1308,13 +1309,13 @@ static int register_get(struct reg *reg)
 	if (exception) {
 		LOG_ERROR("Got exception 0x%x when reading register %d", exception,
 				reg->number);
-		buf_set_u64(reg->value, 0, xlen(target), ~0);
+		buf_set_u64(reg->value, 0, riscv_xlen(target), ~0);
 		return ERROR_FAIL;
 	}
 
 	uint64_t value = cache_get(target, SLOT0);
 	LOG_DEBUG("%s=0x%" PRIx64, reg->name, value);
-	buf_set_u64(reg->value, 0, xlen(target), value);
+	buf_set_u64(reg->value, 0, riscv_xlen(target), value);
 
 	if (reg->number == REG_MSTATUS) {
 		info->mstatus_actual = value;
@@ -1358,7 +1359,7 @@ static int register_write(struct target *target, unsigned int number,
 			cache_set(target, SLOT1, info->mstatus_actual);
 		}
 
-		if (xlen(target) == 32) {
+		if (riscv_xlen(target) == 32) {
 			cache_set32(target, i++, flw(number - REG_FPR0, 0, DEBUG_RAM_START + 16));
 		} else {
 			cache_set32(target, i++, fld(number - REG_FPR0, 0, DEBUG_RAM_START + 16));
@@ -1399,7 +1400,7 @@ static int register_set(struct reg *reg, uint8_t *buf)
 {
 	struct target *target = (struct target *) reg->arch_info;
 
-	uint64_t value = buf_get_u64(buf, 0, xlen(target));
+	uint64_t value = buf_get_u64(buf, 0, riscv_xlen(target));
 
 	LOG_DEBUG("write 0x%" PRIx64 " to %s", value, reg->name);
 	struct reg *r = &target->reg_cache->reg_list[reg->number];
@@ -1437,6 +1438,7 @@ static int init_target(struct command_context *cmd_ctx,
 {
 	LOG_DEBUG("init");
 	riscv_info_t *generic_info = (riscv_info_t *) target->arch_info;
+	generic_info->get_register = NULL;
 	generic_info->version_specific = calloc(1, sizeof(riscv011_info_t));
 	if (!generic_info->version_specific)
 		return ERROR_FAIL;
@@ -1510,7 +1512,7 @@ static int add_trigger(struct target *target, struct trigger *trigger)
 
 		uint64_t tdata1;
 		read_csr(target, &tdata1, CSR_TDATA1);
-		int type = get_field(tdata1, MCONTROL_TYPE(xlen(target)));
+		int type = get_field(tdata1, MCONTROL_TYPE(riscv_xlen(target)));
 
 		if (type != 2) {
 			continue;
@@ -1522,7 +1524,7 @@ static int add_trigger(struct target *target, struct trigger *trigger)
 		}
 
 		// address/data match trigger
-		tdata1 |= MCONTROL_DMODE(xlen(target));
+		tdata1 |= MCONTROL_DMODE(riscv_xlen(target));
 		tdata1 = set_field(tdata1, MCONTROL_ACTION,
 				MCONTROL_ACTION_DEBUG_MODE);
 		tdata1 = set_field(tdata1, MCONTROL_MATCH, MCONTROL_MATCH_EQUAL);
@@ -1769,9 +1771,9 @@ static int step(struct target *target, int current, uint32_t address,
 	jtag_add_ir_scan(target->tap, &select_dbus, TAP_IDLE);
 
 	if (!current) {
-		if (xlen(target) > 32) {
+		if (riscv_xlen(target) > 32) {
 			LOG_WARNING("Asked to resume at 32-bit PC on %d-bit target.",
-					xlen(target));
+					riscv_xlen(target));
 		}
 		int result = register_write(target, REG_PC, address);
 		if (result != ERROR_OK)
@@ -1875,11 +1877,11 @@ static int examine(struct target *target)
 	uint32_t word1 = cache_get32(target, 1);
 	riscv_info_t *generic_info = (riscv_info_t *) target->arch_info;
 	if (word0 == 1 && word1 == 0) {
-		generic_info->xlen = 32;
+		generic_info->xlen[0] = 32;
 	} else if (word0 == 0xffffffff && word1 == 3) {
-		generic_info->xlen = 64;
+		generic_info->xlen[0] = 64;
 	} else if (word0 == 0xffffffff && word1 == 0xffffffff) {
-		generic_info->xlen = 128;
+		generic_info->xlen[0] = 128;
 	} else {
 		uint32_t exception = cache_get32(target, info->dramsize-1);
 		LOG_ERROR("Failed to discover xlen; word0=0x%x, word1=0x%x, exception=0x%x",
@@ -1887,7 +1889,7 @@ static int examine(struct target *target)
 		dump_debug_ram(target);
 		return ERROR_FAIL;
 	}
-	LOG_DEBUG("Discovered XLEN is %d", xlen(target));
+	LOG_DEBUG("Discovered XLEN is %d", riscv_xlen(target));
 
 	// Update register list to match discovered XLEN.
 	update_reg_list(target);
@@ -1905,7 +1907,7 @@ static int examine(struct target *target)
 	}
 
 	target_set_examined(target);
-	LOG_INFO("Examined RISCV core; XLEN=%d, misa=0x%" PRIx64, xlen(target), info->misa);
+	LOG_INFO("Examined RISCV core; XLEN=%d, misa=0x%" PRIx64, riscv_xlen(target), info->misa);
 
 	return ERROR_OK;
 }
@@ -2030,10 +2032,10 @@ static riscv_error_t handle_halt_routine(struct target *target)
 				default:
 						 assert(0);
 			}
-			if (xlen(target) == 32) {
+			if (riscv_xlen(target) == 32) {
 				reg_cache_set(target, reg, data & 0xffffffff);
 				result++;
-			} else if (xlen(target) == 64) {
+			} else if (riscv_xlen(target) == 64) {
 				if (address == 4) {
 					value = data & 0xffffffff;
 				} else if (address == 5) {
@@ -2125,7 +2127,7 @@ static int handle_halt(struct target *target, bool announce)
 				break;
 			uint64_t tdata1;
 			read_csr(target, &tdata1, CSR_TDATA1);
-			if ((tdata1 & MCONTROL_DMODE(xlen(target))) &&
+			if ((tdata1 & MCONTROL_DMODE(riscv_xlen(target))) &&
 					(tdata1 & (MCONTROL_EXECUTE | MCONTROL_STORE | MCONTROL_LOAD))) {
 				write_csr(target, CSR_TDATA1, 0);
 			}
@@ -2196,9 +2198,9 @@ static int riscv011_resume(struct target *target, int current, uint32_t address,
 	jtag_add_ir_scan(target->tap, &select_dbus, TAP_IDLE);
 
 	if (!current) {
-		if (xlen(target) > 32) {
+		if (riscv_xlen(target) > 32) {
 			LOG_WARNING("Asked to resume at 32-bit PC on %d-bit target.",
-					xlen(target));
+					riscv_xlen(target));
 		}
 		int result = register_write(target, REG_PC, address);
 		if (result != ERROR_OK)
diff --git a/src/target/riscv/riscv-013.c b/src/target/riscv/riscv-013.c
index e82cb3d..2eb28bf 100644
--- a/src/target/riscv/riscv-013.c
+++ b/src/target/riscv/riscv-013.c
@@ -21,6 +21,23 @@
 #include "helper/time_support.h"
 #include "riscv.h"
 #include "debug_defines.h"
+#include "rtos/rtos.h"
+
+static void riscv013_on_step_or_resume(struct target *target, bool step);
+static void riscv013_step_or_resume_current_hart(struct target *target, bool step);
+
+/* Implementations of the functions in riscv_info_t. */
+static riscv_reg_t riscv013_get_register(struct target *target, int hartid, int regid);
+static void riscv013_set_register(struct target *target, int hartid, int regid, uint64_t value);
+static void riscv013_select_current_hart(struct target *target);
+static void riscv013_halt_current_hart(struct target *target);
+static void riscv013_resume_current_hart(struct target *target);
+static void riscv013_step_current_hart(struct target *target);
+static void riscv013_on_halt(struct target *target);
+static void riscv013_on_step(struct target *target);
+static void riscv013_on_resume(struct target *target);
+static bool riscv013_is_halted(struct target *target);
+static enum riscv_halt_reason riscv013_halt_reason(struct target *target);
 
 /**
  * Since almost everything can be accomplish by scanning the dbus register, all
@@ -78,27 +95,6 @@ typedef enum slot {
 
 #define WALL_CLOCK_TIMEOUT		2
 
-// gdb's register list is defined in riscv_gdb_reg_names gdb/riscv-tdep.c in
-// its source tree. We must interpret the numbers the same here.
-enum {
-	REG_XPR0 = 0,
-	REG_XPR31 = 31,
-	REG_PC = 32,
-	REG_FPR0 = 33,
-	REG_FPR31 = 64,
-	REG_CSR0 = 65,
-	REG_TSELECT = CSR_TSELECT + REG_CSR0,
-	REG_TDATA1 = CSR_TDATA1 + REG_CSR0,
-	REG_TDATA2 = CSR_TDATA2 + REG_CSR0,
-	REG_MISA = CSR_MISA + REG_CSR0,
-	REG_DPC = CSR_DPC + REG_CSR0,
-	REG_DCSR = CSR_DCSR + REG_CSR0,
-	REG_MSTATUS = CSR_MSTATUS + REG_CSR0,
-	REG_CSR4095 = 4160,
-	REG_PRIV = 4161,
-	REG_COUNT
-};
-
 #define MAX_HWBPS			16
 
 struct trigger {
@@ -125,8 +121,6 @@ typedef struct {
 	unsigned progsize;
 	/* Number of Program Buffer registers. */
 	/* Number of words in Debug RAM. */
-	uint64_t dcsr;
-	uint64_t dpc;
 	uint64_t misa;
 	uint64_t tselect;
 	bool tselect_dirty;
@@ -164,14 +158,8 @@ typedef struct {
 	unsigned int ac_busy_delay;
 
 	bool need_strict_step;
-	bool never_halted;
 } riscv013_info_t;
 
-typedef struct {
-	bool haltnot;
-	bool interrupt;
-} bits_t;
-
 static void dump_field(const struct scan_field *field)
 {
 	static const char *op_string[] = {"-", "r", "w", "?"};
@@ -210,106 +198,8 @@ static riscv013_info_t *get_info(const struct target *target)
 	return (riscv013_info_t *) info->version_specific;
 }
 
-/*** scans "class" ***/
-
-typedef struct {
-	// Number of scans that space is reserved for.
-	unsigned int scan_count;
-	// Size reserved in memory for each scan, in bytes.
-	unsigned int scan_size;
-	unsigned int next_scan;
-	uint8_t *in;
-	uint8_t *out;
-	struct scan_field *field;
-	const struct target *target;
-} scans_t;
-
-static scans_t *scans_new(struct target *target, unsigned int scan_count)
-{
-	scans_t *scans = malloc(sizeof(scans_t));
-	scans->scan_count = scan_count;
-	// This code also gets called before xlen is detected.
-	if (xlen(target))
-		scans->scan_size = 2 + xlen(target) / 8;
-	else
-		scans->scan_size = 2 + 128 / 8;
-	scans->next_scan = 0;
-	scans->in = calloc(scans->scan_size, scans->scan_count);
-	scans->out = calloc(scans->scan_size, scans->scan_count);
-	scans->field = calloc(scans->scan_count, sizeof(struct scan_field));
-	scans->target = target;
-	return scans;
-}
-
-static scans_t *scans_delete(scans_t *scans)
-{
-	assert(scans);
-	free(scans->field);
-	free(scans->out);
-	free(scans->in);
-	free(scans);
-	return NULL;
-}
-
-static void scans_dump(scans_t *scans)
-{
-	for (unsigned int i = 0; i < scans->next_scan; i++) {
-		dump_field(&scans->field[i]);
-	}
-}
-
-static int scans_execute(scans_t *scans)
-{
-	int retval = jtag_execute_queue();
-	if (retval != ERROR_OK) {
-		LOG_ERROR("failed jtag scan: %d", retval);
-		return retval;
-	}
-
-	scans_dump(scans);
-
-	return ERROR_OK;
-}
-
-static void scans_add_dmi_write(scans_t *scans, unsigned address,
-		uint32_t value, bool exec)
-{
-	riscv013_info_t *info = get_info(scans->target);
-	assert(scans->next_scan < scans->scan_count);
-	const unsigned int i = scans->next_scan;
-	int data_offset = scans->scan_size * i;
-	struct scan_field *field = scans->field + i;
-
-	uint8_t *out = scans->out + data_offset;
-	field->num_bits = info->abits + DTM_DMI_OP_LENGTH + DTM_DMI_DATA_LENGTH;
-	// We gain a lot of speed in remote bitbang by not looking at the return
-	// value.
-	field->in_value = NULL;
-	field->out_value = out;
-
-	buf_set_u64(out, DTM_DMI_OP_OFFSET, DTM_DMI_OP_LENGTH, DMI_OP_WRITE);
-	buf_set_u64(out, DTM_DMI_DATA_OFFSET, DTM_DMI_DATA_LENGTH, value);
-	buf_set_u64(out, DTM_DMI_ADDRESS_OFFSET, info->abits, address);
-
-	/* Assume dbus is already selected. */
-	jtag_add_dr_scan(scans->target->tap, 1, field, TAP_IDLE);
-
-	int idle_count = info->dtmcontrol_idle + info->dmi_busy_delay;
-	if (exec)
-		idle_count += info->ac_busy_delay;
-
-	if (idle_count) {
-		jtag_add_runtest(idle_count, TAP_IDLE);
-	}
-
-	scans->next_scan++;
-}
-
-/*** end of scans class ***/
 /*** Necessary prototypes. ***/
 
-static int poll_target(struct target *target, bool announce);
-static int riscv013_poll(struct target *target);
 static int register_get(struct reg *reg);
 
 /*** Utility functions. ***/
@@ -372,15 +262,6 @@ static uint32_t dtmcontrol_scan(struct target *target, uint32_t out)
 	return in;
 }
 
-static void increase_ac_busy_delay(struct target *target)
-{
-	riscv013_info_t *info = get_info(target);
-	info->ac_busy_delay += info->ac_busy_delay / 10 + 1;
-	LOG_INFO("dtmcontrol_idle=%d, dmi_busy_delay=%d, ac_busy_delay=%d",
-			info->dtmcontrol_idle, info->dmi_busy_delay,
-			info->ac_busy_delay);
-}
-
 static void increase_dmi_busy_delay(struct target *target)
 {
 	riscv013_info_t *info = get_info(target);
@@ -450,6 +331,8 @@ static dmi_status_t dmi_scan(struct target *target, uint16_t *address_in,
 
 static uint64_t dmi_read(struct target *target, uint16_t address)
 {
+	select_dmi(target);
+
 	uint64_t value;
 	dmi_status_t status;
 	uint16_t address_in;
@@ -478,6 +361,7 @@ static uint64_t dmi_read(struct target *target, uint16_t address)
 
 static void dmi_write(struct target *target, uint16_t address, uint64_t value)
 {
+	select_dmi(target);
 	dmi_status_t status = DMI_STATUS_BUSY;
 	unsigned i = 0;
 	while (status == DMI_STATUS_BUSY && i++ < 256) {
@@ -493,24 +377,16 @@ static void dmi_write(struct target *target, uint16_t address, uint64_t value)
 	}
 }
 
-/** Selects the core id that cooresponds to the given hart */
-static void select_hart(struct target *target)
-{
-	uint64_t dmcontrol = dmi_read(target, DMI_DMCONTROL);
-	dmcontrol = set_field(dmcontrol, DMI_DMCONTROL, target->coreid);
-	dmi_write(target, DMI_DMCONTROL, dmcontrol);
-}
-
 /** Convert register number (internal OpenOCD number) to the number expected by
  * the abstract command interface. */
 static unsigned reg_number_to_no(unsigned reg_num)
 {
-	if (reg_num <= REG_XPR31) {
-		return reg_num + 0x1000 - REG_XPR0;
-	} else if (reg_num >= REG_CSR0 && reg_num <= REG_CSR4095) {
-		return reg_num - REG_CSR0;
-	} else if (reg_num >= REG_FPR0 && reg_num <= REG_FPR31) {
-		return reg_num + 0x1020 - REG_FPR0;
+	if (reg_num <= GDB_REGNO_XPR31) {
+		return reg_num + 0x1000 - GDB_REGNO_XPR0;
+	} else if (reg_num >= GDB_REGNO_CSR0 && reg_num <= GDB_REGNO_CSR4095) {
+		return reg_num - GDB_REGNO_CSR0;
+	} else if (reg_num >= GDB_REGNO_FPR0 && reg_num <= GDB_REGNO_FPR31) {
+		return reg_num + 0x1020 - GDB_REGNO_FPR0;
 	} else {
 		return ~0;
 	}
@@ -538,10 +414,28 @@ static int wait_for_idle(struct target *target, uint32_t *abstractcs)
 	time_t start = time(NULL);
 	while (1) {
 		*abstractcs = dmi_read(target, DMI_ABSTRACTCS);
+
 		if (get_field(*abstractcs, DMI_ABSTRACTCS_BUSY) == 0) {
 			return ERROR_OK;
 		}
+
 		if (time(NULL) - start > WALL_CLOCK_TIMEOUT) {
+			if (get_field(*abstractcs, DMI_ABSTRACTCS_CMDERR) != CMDERR_NONE) {
+				const char *errors[8] = {
+					"none",
+					"busy",
+					"not supported",
+					"exception",
+					"halt/resume",
+					"reserved",
+					"reserved",
+					"other" };
+
+				LOG_ERROR("Abstract command ended in error '%s' (abstractcs=0x%x)",
+						errors[get_field(*abstractcs, DMI_ABSTRACTCS_CMDERR)],
+						*abstractcs);
+			}
+
 			LOG_ERROR("Timed out waiting for busy to go low. (abstractcs=0x%x)",
 					*abstractcs);
 			return ERROR_FAIL;
@@ -591,6 +485,8 @@ typedef struct {
 	uint64_t write_value;
 } program_t;
 
+static void program_add32(program_t *program, uint32_t instruction);
+
 static program_t *program_new(void)
 {
 	program_t *program = malloc(sizeof(program_t));
@@ -600,6 +496,7 @@ static program_t *program_new(void)
 		program->write = false;
 		program->regno = 0x1000;
 	}
+	program_add32(program, fence_i());
 	return program;
 }
 
@@ -661,7 +558,7 @@ static int execute_program(struct target *target, const program_t *program)
 	} else {
 		command |= AC_ACCESS_REGISTER_PREEXEC;
 	}
-	command |= abstract_register_size(xlen(target));
+	command |= abstract_register_size(riscv_xlen(target));
 	command |= program->regno;
 
 	return execute_abstract_command(target, command);
@@ -727,7 +624,7 @@ static int abstract_write_register(struct target *target,
 
 static int update_mstatus_actual(struct target *target)
 {
-	struct reg *mstatus_reg = &target->reg_cache->reg_list[REG_MSTATUS];
+	struct reg *mstatus_reg = &target->reg_cache->reg_list[GDB_REGNO_MSTATUS];
 	if (mstatus_reg->valid) {
 		// We previously made it valid.
 		return ERROR_OK;
@@ -737,7 +634,7 @@ static int update_mstatus_actual(struct target *target)
 
 	// Force reading the register. In that process mstatus_actual will be
 	// updated.
-	return register_get(&target->reg_cache->reg_list[REG_MSTATUS]);
+	return register_get(&target->reg_cache->reg_list[GDB_REGNO_MSTATUS]);
 }
 
 static int register_write_direct(struct target *target, unsigned number,
@@ -746,22 +643,22 @@ static int register_write_direct(struct target *target, unsigned number,
 	riscv013_info_t *info = get_info(target);
 	LOG_DEBUG("register 0x%x <- 0x%" PRIx64, number, value);
 
-	if (number == REG_MSTATUS) {
+	if (number == GDB_REGNO_MSTATUS) {
 		info->mstatus_actual = value;
 	}
 
-	int result = abstract_write_register(target, number, xlen(target), value);
+	int result = abstract_write_register(target, number, riscv_xlen(target), value);
 	if (result == ERROR_OK)
 		return result;
 
 	// Fall back to program buffer.
-	if (number >= REG_FPR0 && number <= REG_FPR31) {
+	if (number >= GDB_REGNO_FPR0 && number <= GDB_REGNO_FPR31) {
 		result = update_mstatus_actual(target);
 		if (result != ERROR_OK) {
 			return result;
 		}
 		if ((info->mstatus_actual & MSTATUS_FS) == 0) {
-			result = register_write_direct(target, REG_MSTATUS, 
+			result = register_write_direct(target, GDB_REGNO_MSTATUS, 
 					set_field(info->mstatus_actual, MSTATUS_FS, 1));
 			if (result != ERROR_OK)
 				return result;
@@ -769,18 +666,18 @@ static int register_write_direct(struct target *target, unsigned number,
 
 		program_t *program = program_new();
 		// TODO: Fully support D extension on RV32.
-		if (supports_extension(target, 'D') && xlen(target) >= 64) {
-			program_add32(program, fmv_d_x(number - REG_FPR0, S0));
+		if (supports_extension(target, 'D') && riscv_xlen(target) >= 64) {
+			program_add32(program, fmv_d_x(number - GDB_REGNO_FPR0, S0));
 		} else {
-			program_add32(program, fmv_s_x(number - REG_FPR0, S0));
+			program_add32(program, fmv_s_x(number - GDB_REGNO_FPR0, S0));
 		}
 		program_add32(program, ebreak());
 		program_set_write(program, S0, value);
 		result = execute_program(target, program);
 		program_delete(program);
-	} else if (number >= REG_CSR0 && number <= REG_CSR4095) {
+	} else if (number >= GDB_REGNO_CSR0 && number <= GDB_REGNO_CSR4095) {
 		program_t *program = program_new();
-		program_add32(program, csrw(S0, number - REG_CSR0));
+		program_add32(program, csrw(S0, number - GDB_REGNO_CSR0));
 		program_add32(program, ebreak());
 		program_set_write(program, S0, value);
 		result = execute_program(target, program);
@@ -796,18 +693,18 @@ static int register_write_direct(struct target *target, unsigned number,
 static int register_read_direct(struct target *target, uint64_t *value, uint32_t number)
 {
 	riscv013_info_t *info = get_info(target);
-	int result = abstract_read_register(target, value, number, xlen(target));
+	int result = abstract_read_register(target, value, number, riscv_xlen(target));
 	if (result == ERROR_OK)
 		return result;
 
 	// Fall back to program buffer.
-	if (number >= REG_FPR0 && number <= REG_FPR31) {
+	if (number >= GDB_REGNO_FPR0 && number <= GDB_REGNO_FPR31) {
 		result = update_mstatus_actual(target);
 		if (result != ERROR_OK) {
 			return result;
 		}
 		if ((info->mstatus_actual & MSTATUS_FS) == 0) {
-			result = register_write_direct(target, REG_MSTATUS,
+			result = register_write_direct(target, GDB_REGNO_MSTATUS,
 					set_field(info->mstatus_actual, MSTATUS_FS, 1));
 			if (result != ERROR_OK)
 				return result;
@@ -815,18 +712,18 @@ static int register_read_direct(struct target *target, uint64_t *value, uint32_t
 		LOG_DEBUG("mstatus_actual=0x%lx", info->mstatus_actual);
 
 		program_t *program = program_new();
-		if (supports_extension(target, 'D') && xlen(target) >= 64) {
-			program_add32(program, fmv_x_d(S0, number - REG_FPR0));
+		if (supports_extension(target, 'D') && riscv_xlen(target) >= 64) {
+			program_add32(program, fmv_x_d(S0, number - GDB_REGNO_FPR0));
 		} else {
-			program_add32(program, fmv_x_s(S0, number - REG_FPR0));
+			program_add32(program, fmv_x_s(S0, number - GDB_REGNO_FPR0));
 		}
 		program_add32(program, ebreak());
 		program_set_read(program, S0);
 		result = execute_program(target, program);
 		program_delete(program);
-	} else if (number >= REG_CSR0 && number <= REG_CSR4095) {
+	} else if (number >= GDB_REGNO_CSR0 && number <= GDB_REGNO_CSR4095) {
 		program_t *program = program_new();
-		program_add32(program, csrr(S0, number - REG_CSR0));
+		program_add32(program, csrr(S0, number - GDB_REGNO_CSR0));
 		program_add32(program, ebreak());
 		program_set_read(program, S0);
 		result = execute_program(target, program);
@@ -852,7 +749,7 @@ static int maybe_read_tselect(struct target *target)
 	riscv013_info_t *info = get_info(target);
 
 	if (info->tselect_dirty) {
-		int result = register_read_direct(target, &info->tselect, REG_TSELECT);
+		int result = register_read_direct(target, &info->tselect, GDB_REGNO_TSELECT);
 		if (result != ERROR_OK)
 			return result;
 		info->tselect_dirty = false;
@@ -866,7 +763,7 @@ static int maybe_write_tselect(struct target *target)
 	riscv013_info_t *info = get_info(target);
 
 	if (!info->tselect_dirty) {
-		int result = register_write_direct(target, REG_TSELECT, info->tselect);
+		int result = register_write_direct(target, GDB_REGNO_TSELECT, info->tselect);
 		if (result != ERROR_OK)
 			return result;
 		info->tselect_dirty = true;
@@ -875,154 +772,6 @@ static int maybe_write_tselect(struct target *target)
 	return ERROR_OK;
 }
 
-static void reg_cache_set(struct target *target, unsigned int number,
-		uint64_t value)
-{
-	struct reg *r = &target->reg_cache->reg_list[number];
-	LOG_DEBUG("%s <= 0x%" PRIx64, r->name, value);
-	r->valid = true;
-	buf_set_u64(r->value, 0, r->size, value);
-}
-
-static uint64_t reg_cache_get(struct target *target, unsigned int number)
-{
-	struct reg *r = &target->reg_cache->reg_list[number];
-	if (!r->valid) {
-		LOG_ERROR("Register cache entry for %d is invalid!", number);
-		assert(r->valid);
-	}
-	uint64_t value = buf_get_u64(r->value, 0, r->size);
-	LOG_DEBUG("%s = 0x%" PRIx64, r->name, value);
-	return value;
-}
-
-static int execute_resume(struct target *target, bool step)
-{
-	riscv013_info_t *info = get_info(target);
-
-	LOG_DEBUG("step=%d", step);
-
-	maybe_write_tselect(target);
-
-	// TODO: check if dpc is dirty (which also is true if an exception was hit
-	// at any time)
-	if (register_write_direct(target, REG_DPC, info->dpc) != ERROR_OK) {
-		return ERROR_FAIL;
-	}
-
-	struct reg *mstatus_reg = &target->reg_cache->reg_list[REG_MSTATUS];
-	if (mstatus_reg->valid) {
-		uint64_t mstatus_user = buf_get_u64(mstatus_reg->value, 0, xlen(target));
-		if (mstatus_user != info->mstatus_actual) {
-			if (register_write_direct(target, REG_MSTATUS, mstatus_user) != ERROR_OK) {
-				return ERROR_FAIL;
-			}
-		}
-	}
-
-	info->dcsr |= CSR_DCSR_EBREAKM | CSR_DCSR_EBREAKH | CSR_DCSR_EBREAKS |
-		CSR_DCSR_EBREAKU;
-
-	if (step) {
-		info->dcsr |= CSR_DCSR_STEP;
-	} else {
-		info->dcsr &= ~CSR_DCSR_STEP;
-	}
-
-	if (register_write_direct(target, REG_DCSR, info->dcsr) != ERROR_OK) {
-		return ERROR_FAIL;
-	}
-
-	// Restore GPRs
-	if (register_write_direct(target, S0, reg_cache_get(target, S0)) != ERROR_OK) {
-		return ERROR_FAIL;
-	}
-	if (register_write_direct(target, S1, reg_cache_get(target, S1)) != ERROR_OK) {
-		return ERROR_FAIL;
-	}
-
-	program_t *program = program_new();
-	program_add32(program, fence_i());
-	program_add32(program, ebreak());
-	if (execute_program(target, program) != ERROR_OK) {
-		return ERROR_FAIL;
-	}
-	program_delete(program);
-
-	dmi_write(
-		target,
-		DMI_DMCONTROL,
-		DMI_DMCONTROL_DMACTIVE
-		| DMI_DMCONTROL_RESUMEREQ
-		| (target->coreid << DMI_DMCONTROL_HARTSEL_OFFSET)
-	);
-
-	target->state = TARGET_RUNNING;
-	register_cache_invalidate(target->reg_cache);
-	reg_cache_set(target, ZERO, 0);
-
-	return ERROR_OK;
-}
-
-// Execute a step, and wait for reentry into Debug Mode.
-static int full_step(struct target *target, bool announce)
-{
-	int result = execute_resume(target, true);
-	if (result != ERROR_OK)
-		return result;
-	time_t start = time(NULL);
-	while (1) {
-		result = poll_target(target, announce);
-		if (result != ERROR_OK)
-			return result;
-		if (target->state != TARGET_DEBUG_RUNNING)
-			break;
-		if (time(NULL) - start > WALL_CLOCK_TIMEOUT) {
-			LOG_ERROR("Timed out waiting for step to complete.");
-			return ERROR_FAIL;
-		}
-	}
-	return ERROR_OK;
-}
-
-static int resume(struct target *target, int debug_execution, bool step)
-{
-	if (debug_execution) {
-		LOG_ERROR("TODO: debug_execution is true");
-		return ERROR_FAIL;
-	}
-
-	return execute_resume(target, step);
-}
-
-/** Update register sizes based on xlen. */
-static void update_reg_list(struct target *target)
-{
-	riscv013_info_t *info = get_info(target);
-	if (info->reg_values) {
-		free(info->reg_values);
-	}
-	info->reg_values = malloc(REG_COUNT * xlen(target) / 4);
-
-	for (unsigned int i = 0; i < REG_COUNT; i++) {
-		struct reg *r = &target->reg_cache->reg_list[i];
-		r->value = info->reg_values + i * xlen(target) / 4;
-		if (r->dirty) {
-			LOG_ERROR("Register %d was dirty. Its value is lost.", i);
-		}
-		if (i == REG_PRIV) {
-			r->size = 8;
-		} else {
-			r->size = xlen(target);
-		}
-		if (i == ZERO) {
-			r->valid = true;
-		} else {
-			r->valid = false;
-		}
-	}
-}
-
 /*** OpenOCD target functions. ***/
 
 static int register_get(struct reg *reg)
@@ -1032,19 +781,17 @@ static int register_get(struct reg *reg)
 
 	maybe_write_tselect(target);
 
-	if (reg->number <= REG_XPR31) {
-		buf_set_u64(reg->value, 0, xlen(target), reg_cache_get(target, reg->number));
-		LOG_DEBUG("%s=0x%" PRIx64, reg->name, reg_cache_get(target, reg->number));
+	if (reg->number <= GDB_REGNO_XPR31) {
+		register_read_direct(target, reg->value, reg->number);
 		return ERROR_OK;
-	} else if (reg->number == REG_PC) {
-		buf_set_u32(reg->value, 0, 32, info->dpc);
+	} else if (reg->number == GDB_REGNO_PC) {
+		buf_set_u32(reg->value, 0, 32, riscv_peek_register(target, GDB_REGNO_DPC));
 		reg->valid = true;
-		LOG_DEBUG("%s=0x%" PRIx64 " (cached)", reg->name, info->dpc);
 		return ERROR_OK;
-	} else if (reg->number == REG_PRIV) {
-		buf_set_u64(reg->value, 0, 8, get_field(info->dcsr, CSR_DCSR_PRV));
-		LOG_DEBUG("%s=%d (cached)", reg->name,
-				(int) get_field(info->dcsr, CSR_DCSR_PRV));
+	} else if (reg->number == GDB_REGNO_PRIV) {
+		uint64_t dcsr = riscv_peek_register(target, CSR_DCSR);
+		buf_set_u64(reg->value, 0, 8, get_field(dcsr, CSR_DCSR_PRV));
+		riscv_overwrite_register(target, CSR_DCSR, dcsr);
 		return ERROR_OK;
 	} else {
 		uint64_t value;
@@ -1053,9 +800,9 @@ static int register_get(struct reg *reg)
 			return result;
 		}
 		LOG_DEBUG("%s=0x%" PRIx64, reg->name, value);
-		buf_set_u64(reg->value, 0, xlen(target), value);
+		buf_set_u64(reg->value, 0, riscv_xlen(target), value);
 
-		if (reg->number == REG_MSTATUS) {
+		if (reg->number == GDB_REGNO_MSTATUS) {
 			info->mstatus_actual = value;
 			reg->valid = true;
 		}
@@ -1067,14 +814,14 @@ static int register_get(struct reg *reg)
 static int register_write(struct target *target, unsigned int number,
 		uint64_t value)
 {
-	riscv013_info_t *info = get_info(target);
-
 	maybe_write_tselect(target);
 
-	if (number == REG_PC) {
-		info->dpc = value;
-	} else if (number == REG_PRIV) {
-		info->dcsr = set_field(info->dcsr, CSR_DCSR_PRV, value);
+	if (number == GDB_REGNO_PC) {
+		riscv_overwrite_register(target, GDB_REGNO_DPC, value);
+	} else if (number == GDB_REGNO_PRIV) {
+		uint64_t dcsr = riscv_peek_register(target, CSR_DCSR);
+		dcsr = set_field(dcsr, CSR_DCSR_PRV, value);
+		riscv_overwrite_register(target, GDB_REGNO_DCSR, dcsr);
 	} else {
 		return register_write_direct(target, number, value);
 	}
@@ -1086,7 +833,7 @@ static int register_set(struct reg *reg, uint8_t *buf)
 {
 	struct target *target = (struct target *) reg->arch_info;
 
-	uint64_t value = buf_get_u64(buf, 0, xlen(target));
+	uint64_t value = buf_get_u64(buf, 0, riscv_xlen(target));
 
 	LOG_DEBUG("write 0x%" PRIx64 " to %s", value, reg->name);
 	struct reg *r = &target->reg_cache->reg_list[reg->number];
@@ -1101,27 +848,25 @@ static struct reg_arch_type riscv_reg_arch_type = {
 	.set = register_set
 };
 
-static int halt(struct target *target)
-{
-	LOG_DEBUG("riscv_halt()");
-	select_dmi(target);
-
-	dmi_write(
-		target,
-		DMI_DMCONTROL,
-		DMI_DMCONTROL_HALTREQ
-		| DMI_DMCONTROL_DMACTIVE
-		| (target->coreid << DMI_DMCONTROL_HARTSEL_OFFSET)
-	);
-
-	return ERROR_OK;
-}
-
 static int init_target(struct command_context *cmd_ctx,
 		struct target *target)
 {
 	LOG_DEBUG("init");
 	riscv_info_t *generic_info = (riscv_info_t *) target->arch_info;
+
+	riscv_info_init(generic_info);
+	generic_info->get_register = &riscv013_get_register;
+	generic_info->set_register = &riscv013_set_register;
+	generic_info->select_current_hart = &riscv013_select_current_hart;
+	generic_info->is_halted = &riscv013_is_halted;
+	generic_info->halt_current_hart = &riscv013_halt_current_hart;
+	generic_info->resume_current_hart = &riscv013_resume_current_hart;
+	generic_info->step_current_hart = &riscv013_step_current_hart;
+	generic_info->on_halt = &riscv013_on_halt;
+	generic_info->on_resume = &riscv013_on_resume;
+	generic_info->on_step = &riscv013_on_step;
+	generic_info->halt_reason = &riscv013_halt_reason;
+
 	generic_info->version_specific = calloc(1, sizeof(riscv013_info_t));
 	if (!generic_info->version_specific)
 		return ERROR_FAIL;
@@ -1129,16 +874,16 @@ static int init_target(struct command_context *cmd_ctx,
 
 	target->reg_cache = calloc(1, sizeof(*target->reg_cache));
 	target->reg_cache->name = "RISC-V registers";
-	target->reg_cache->num_regs = REG_COUNT;
+	target->reg_cache->num_regs = GDB_REGNO_COUNT;
 
-	target->reg_cache->reg_list = calloc(REG_COUNT, sizeof(struct reg));
+	target->reg_cache->reg_list = calloc(GDB_REGNO_COUNT, sizeof(struct reg));
 
 	const unsigned int max_reg_name_len = 12;
-	info->reg_names = calloc(1, REG_COUNT * max_reg_name_len);
+	info->reg_names = calloc(1, GDB_REGNO_COUNT * max_reg_name_len);
 	char *reg_name = info->reg_names;
 	info->reg_values = NULL;
 
-	for (unsigned int i = 0; i < REG_COUNT; i++) {
+	for (unsigned int i = 0; i < GDB_REGNO_COUNT; i++) {
 		struct reg *r = &target->reg_cache->reg_list[i];
 		r->number = i;
 		r->caller_save = true;
@@ -1147,24 +892,26 @@ static int init_target(struct command_context *cmd_ctx,
 		r->exist = true;
 		r->type = &riscv_reg_arch_type;
 		r->arch_info = target;
-		if (i <= REG_XPR31) {
+		if (i <= GDB_REGNO_XPR31) {
 			sprintf(reg_name, "x%d", i);
-		} else if (i == REG_PC) {
+		} else if (i == GDB_REGNO_PC) {
 			sprintf(reg_name, "pc");
-		} else if (i >= REG_FPR0 && i <= REG_FPR31) {
-			sprintf(reg_name, "f%d", i - REG_FPR0);
-		} else if (i >= REG_CSR0 && i <= REG_CSR4095) {
-			sprintf(reg_name, "csr%d", i - REG_CSR0);
-		} else if (i == REG_PRIV) {
+		} else if (i >= GDB_REGNO_FPR0 && i <= GDB_REGNO_FPR31) {
+			sprintf(reg_name, "f%d", i - GDB_REGNO_FPR0);
+		} else if (i >= GDB_REGNO_CSR0 && i <= GDB_REGNO_CSR4095) {
+			sprintf(reg_name, "csr%d", i - GDB_REGNO_CSR0);
+		} else if (i == GDB_REGNO_PRIV) {
 			sprintf(reg_name, "priv");
 		}
 		if (reg_name[0]) {
 			r->name = reg_name;
 		}
 		reg_name += strlen(reg_name) + 1;
-		assert(reg_name < info->reg_names + REG_COUNT * max_reg_name_len);
+		assert(reg_name < info->reg_names + GDB_REGNO_COUNT * max_reg_name_len);
 	}
+#if 0
 	update_reg_list(target);
+#endif
 
 	memset(info->trigger_unique_id, 0xff, sizeof(info->trigger_unique_id));
 
@@ -1191,11 +938,11 @@ static int add_trigger(struct target *target, struct trigger *trigger)
 			continue;
 		}
 
-		register_write_direct(target, REG_TSELECT, i);
+		register_write_direct(target, GDB_REGNO_TSELECT, i);
 
 		uint64_t tdata1;
-		register_read_direct(target, &tdata1, REG_TDATA1);
-		int type = get_field(tdata1, MCONTROL_TYPE(xlen(target)));
+		register_read_direct(target, &tdata1, GDB_REGNO_TDATA1);
+		int type = get_field(tdata1, MCONTROL_TYPE(riscv_xlen(target)));
 
 		if (type != 2) {
 			continue;
@@ -1207,7 +954,7 @@ static int add_trigger(struct target *target, struct trigger *trigger)
 		}
 
 		// address/data match trigger
-		tdata1 |= MCONTROL_DMODE(xlen(target));
+		tdata1 |= MCONTROL_DMODE(riscv_xlen(target));
 		tdata1 = set_field(tdata1, MCONTROL_ACTION,
 				MCONTROL_ACTION_DEBUG_MODE);
 		tdata1 = set_field(tdata1, MCONTROL_MATCH, MCONTROL_MATCH_EQUAL);
@@ -1226,21 +973,21 @@ static int add_trigger(struct target *target, struct trigger *trigger)
 		if (trigger->write)
 			tdata1 |= MCONTROL_STORE;
 
-		register_write_direct(target, REG_TDATA1, tdata1);
+		register_write_direct(target, GDB_REGNO_TDATA1, tdata1);
 
 		uint64_t tdata1_rb;
-		register_read_direct(target, &tdata1_rb, REG_TDATA1);
+		register_read_direct(target, &tdata1_rb, GDB_REGNO_TDATA1);
 		LOG_DEBUG("tdata1=0x%" PRIx64, tdata1_rb);
 
 		if (tdata1 != tdata1_rb) {
 			LOG_DEBUG("Trigger %d doesn't support what we need; After writing 0x%"
 					PRIx64 " to tdata1 it contains 0x%" PRIx64,
 					i, tdata1, tdata1_rb);
-			register_write_direct(target, REG_TDATA1, 0);
+			register_write_direct(target, GDB_REGNO_TDATA1, 0);
 			continue;
 		}
 
-		register_write_direct(target, REG_TDATA2, trigger->address);
+		register_write_direct(target, GDB_REGNO_TDATA2, trigger->address);
 
 		LOG_DEBUG("Using resource %d for bp %d", i,
 				trigger->unique_id);
@@ -1273,8 +1020,8 @@ 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);
-	register_write_direct(target, REG_TSELECT, i);
-	register_write_direct(target, REG_TDATA1, 0);
+	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;
@@ -1337,7 +1084,6 @@ static int add_breakpoint(struct target *target,
 		if (result != ERROR_OK) {
 			return result;
 		}
-
 	} else {
 		LOG_INFO("OpenOCD only supports hardware and software breakpoints.");
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
@@ -1366,7 +1112,6 @@ static int remove_breakpoint(struct target *target,
 		if (result != ERROR_OK) {
 			return result;
 		}
-
 	} else {
 		LOG_INFO("OpenOCD only supports hardware and software breakpoints.");
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
@@ -1407,73 +1152,6 @@ static int remove_watchpoint(struct target *target,
 	return ERROR_OK;
 }
 
-static int strict_step(struct target *target, bool announce)
-{
-	riscv013_info_t *info = get_info(target);
-
-	LOG_DEBUG("enter");
-
-	struct breakpoint *breakpoint = target->breakpoints;
-	while (breakpoint) {
-		remove_breakpoint(target, breakpoint);
-		breakpoint = breakpoint->next;
-	}
-
-	struct watchpoint *watchpoint = target->watchpoints;
-	while (watchpoint) {
-		remove_watchpoint(target, watchpoint);
-		watchpoint = watchpoint->next;
-	}
-
-	int result = full_step(target, announce);
-	if (result != ERROR_OK)
-		return result;
-
-	breakpoint = target->breakpoints;
-	while (breakpoint) {
-		add_breakpoint(target, breakpoint);
-		breakpoint = breakpoint->next;
-	}
-
-	watchpoint = target->watchpoints;
-	while (watchpoint) {
-		add_watchpoint(target, watchpoint);
-		watchpoint = watchpoint->next;
-	}
-
-	info->need_strict_step = false;
-
-	return ERROR_OK;
-}
-
-static int step(struct target *target, int current, uint32_t address,
-		int handle_breakpoints)
-{
-	riscv013_info_t *info = get_info(target);
-
-	select_dmi(target);
-
-	if (!current) {
-		if (xlen(target) > 32) {
-			LOG_WARNING("Asked to resume at 32-bit PC on %d-bit target.",
-					xlen(target));
-		}
-		int result = register_write(target, REG_PC, address);
-		if (result != ERROR_OK)
-			return result;
-	}
-
-	if (info->need_strict_step || handle_breakpoints) {
-		int result = strict_step(target, true);
-		if (result != ERROR_OK)
-			return result;
-	} else {
-		return resume(target, 0, true);
-	}
-
-	return ERROR_OK;
-}
-
 static int examine(struct target *target)
 {
 	// Don't need to select dbus, since the first thing we do is read dtmcontrol.
@@ -1508,9 +1186,8 @@ static int examine(struct target *target)
 	}
 
 	// Reset the Debug Module.
-	//This means the DM isn't reset, but reseting the DM nukes other harts.
-	//dmi_write(target, DMI_DMCONTROL, 0);
-	dmi_write(target, DMI_DMCONTROL, DMI_DMCONTROL_DMACTIVE | (target->coreid << DMI_DMCONTROL_HARTSEL_OFFSET));
+	dmi_write(target, DMI_DMCONTROL, 0);
+	dmi_write(target, DMI_DMCONTROL, DMI_DMCONTROL_DMACTIVE);
 	dmcontrol = dmi_read(target, DMI_DMCONTROL);
 
 	LOG_DEBUG("dmcontrol: 0x%08x", dmcontrol);
@@ -1540,308 +1217,38 @@ static int examine(struct target *target)
 
 	// Check that abstract data registers are accessible.
 	uint32_t abstractcs = dmi_read(target, DMI_ABSTRACTCS);
-	LOG_DEBUG("abstractcs=0x%x", abstractcs);
 	info->datacount = get_field(abstractcs, DMI_ABSTRACTCS_DATACOUNT);
 	info->progsize = get_field(abstractcs, DMI_ABSTRACTCS_PROGSIZE);
 
-	uint32_t value = 0x53467665;
-	for (unsigned i = 0; i < info->datacount; i++) {
-		dmi_write(target, DMI_DATA0 + i, value);
-		value += 0x52534335;
-	}
+	/* Halt every hart so we can probe them. */
+	riscv_halt_all_harts(target);
 
-	for (unsigned i = 0; i < info->progsize; i++) {
-		dmi_write(target, DMI_PROGBUF0 + i, value);
-		value += 0x52534335;
-	}
+	/* Examines every hart, first checking XLEN. */
+	for (int i = 0; i < riscv_count_harts(target); ++i) {
+		RISCV_INFO(r);
+		riscv_set_current_hartid(target, i);
 
-	value = 0x53467665;
-	for (unsigned i = 0; i < info->datacount; i++) {
-		uint32_t check = dmi_read(target, DMI_DATA0 + i);
-		if (check != value) {
-			LOG_ERROR("Wrote 0x%x to dbus address 0x%x but got back 0x%x",
-					value, DMI_DATA0 + i, check);
-			return ERROR_FAIL;
-		}
-		value += 0x52534335;
-	}
-	for (unsigned i = 0; i < info->progsize; i++) {
-		uint32_t check = dmi_read(target, DMI_PROGBUF0 + i);
-		if (check != value) {
-			LOG_ERROR("Wrote 0x%x to dbus address 0x%x but got back 0x%x",
-					value, DMI_PROGBUF0 + i, check);
-			return ERROR_FAIL;
-		}
-		value += 0x52534335;
-	}
-
-	bool should_attempt_resume = false;
-	if (get_field(dmstatus, DMI_DMSTATUS_ANYRUNNING)) {
-		should_attempt_resume = true;
-		LOG_DEBUG("Hart currently running. Requesting halt.\n");
-		dmi_write(target, DMI_DMCONTROL, DMI_DMCONTROL_HALTREQ |
-				DMI_DMCONTROL_DMACTIVE | (target->coreid << DMI_DMCONTROL_HARTSEL_OFFSET));
-		for (unsigned i = 0; i < 256; i++) {
-			dmcontrol = dmi_read(target, DMI_DMCONTROL);
-			dmstatus = dmi_read(target, DMI_DMSTATUS);
-			if (get_field(dmstatus, DMI_DMSTATUS_ALLHALTED))
-				break;
-		}
-		if (!get_field(dmstatus, DMI_DMSTATUS_ALLHALTED)) {
-			LOG_ERROR("hart didn't halt; dmstatus=0x%x", dmstatus);
-			return ERROR_FAIL;
-		}
-	}
-
-	// TODO: do this using Quick Access, if supported.
-
-	riscv_info_t *generic_info = (riscv_info_t *) target->arch_info;
-	if (abstract_read_register(target, NULL, S0, 128) == ERROR_OK) {
-		generic_info->xlen = 128;
-	} else if (abstract_read_register(target, NULL, S0, 64) == ERROR_OK) {
-		generic_info->xlen = 64;
-	} else if (abstract_read_register(target, NULL, S0, 32) == ERROR_OK) {
-		generic_info->xlen = 32;
-	} else {
-		LOG_ERROR("Failed to discover size using abstract register reads.");
-		return ERROR_FAIL;
-	}
-
-	LOG_DEBUG("Discovered XLEN is %d", xlen(target));
-
-	// Update register list to match discovered XLEN.
-	update_reg_list(target);
-
-	if (register_read_direct(target, &info->misa, REG_MISA) != ERROR_OK) {
-		LOG_ERROR("Failed to read misa.");
-		return ERROR_FAIL;
-	}
-
-	if (should_attempt_resume) {
-		LOG_DEBUG("Resuming hart.\n");
-		// Resume if the hart had been running.
-		dmi_write(target, DMI_DMCONTROL, DMI_DMCONTROL_DMACTIVE |
-				DMI_DMCONTROL_RESUMEREQ | (target->coreid << DMI_DMCONTROL_HARTSEL_OFFSET));
-		for (unsigned i = 0; i < 256; i++) {
-			dmcontrol = dmi_read(target, DMI_DMCONTROL);
-			dmstatus = dmi_read(target, DMI_DMSTATUS);
-			if (get_field(dmstatus, DMI_DMSTATUS_ALLRUNNING))
-				break;
-		}
-		if (!get_field(dmstatus, DMI_DMSTATUS_ALLRUNNING)) {
-			LOG_ERROR("hart didn't resume; dmstatus=0x%x", dmstatus);
+		if (abstract_read_register(target, NULL, S0, 128) == ERROR_OK) {
+			r->xlen[i] = 128;
+		} else if (abstract_read_register(target, NULL, S0, 64) == ERROR_OK) {
+			r->xlen[i] = 64;
+		} else if (abstract_read_register(target, NULL, S0, 32) == ERROR_OK) {
+			r->xlen[i] = 32;
+		} else {
+			LOG_ERROR("Failed to discover size using abstract register reads.");
 			return ERROR_FAIL;
 		}
 	}
 
-	info->never_halted = true;
-
-	int result = riscv013_poll(target);
-	if (result != ERROR_OK) {
-		return result;
-	}
+	/* FIXME: Are there 2 triggers? */
+	info->trigger_count = 2;
 
+	/* Resumes all the harts, so the debugger can later pause them. */
+	riscv_resume_all_harts(target);
 	target_set_examined(target);
-	LOG_INFO("Examined RISCV core; XLEN=%d, misa=0x%" PRIx64, xlen(target), info->misa);
-
-	return ERROR_OK;
-}
-
-static riscv_error_t handle_halt_routine(struct target *target)
-{
-	riscv013_info_t *info = get_info(target);
-	select_hart(target);
-
-	// Read all GPRs as fast as we can, because gdb is going to ask for them
-	// anyway. Reading them one at a time is much slower.
-
-	for (int reg = 1; reg < 32; reg++) {
-		uint64_t value;
-		int result = abstract_read_register(target, &value, reg, xlen(target));
-		if (result != ERROR_OK)
-			return result;
-		reg_cache_set(target, reg, value);
-	}
-
-	unsigned int csr[] = {REG_DPC, REG_DCSR};
-	for (unsigned int i = 0; i < DIM(csr); i++) {
-		uint64_t value;
-		int reg = csr[i];
-		int result = register_read_direct(target, &value, reg);
-		if (result != ERROR_OK)
-			return result;
-		reg_cache_set(target, reg, value);
-	}
-
-	// TODO: get rid of those 2 variables and talk to the cache directly.
-	info->dpc = reg_cache_get(target, REG_DPC);
-	info->dcsr = reg_cache_get(target, REG_DCSR);
-
-	return RE_OK;
-}
-
-static int handle_halt(struct target *target, bool announce)
-{
-	riscv013_info_t *info = get_info(target);
-	target->state = TARGET_HALTED;
-
-	riscv_error_t re;
-	do {
-		re = handle_halt_routine(target);
-	} while (re == RE_AGAIN);
-	if (re != RE_OK) {
-		LOG_ERROR("handle_halt_routine failed");
-		return ERROR_FAIL;
-	}
-
-	int cause = get_field(info->dcsr, CSR_DCSR_CAUSE);
-	switch (cause) {
-		case CSR_DCSR_CAUSE_SWBP:
-			target->debug_reason = DBG_REASON_BREAKPOINT;
-			break;
-		case CSR_DCSR_CAUSE_TRIGGER:
-			target->debug_reason = DBG_REASON_WPTANDBKPT;
-			// If we halted because of a data trigger, gdb doesn't know to do
-			// the disable-breakpoints-step-enable-breakpoints dance.
-			info->need_strict_step = true;
-			break;
-		case CSR_DCSR_CAUSE_DEBUGINT:
-			target->debug_reason = DBG_REASON_DBGRQ;
-			break;
-		case CSR_DCSR_CAUSE_STEP:
-			target->debug_reason = DBG_REASON_SINGLESTEP;
-			break;
-		case CSR_DCSR_CAUSE_HALT:
-			target->debug_reason = DBG_REASON_DBGRQ;
-			break;
-		default:
-			LOG_ERROR("Invalid halt cause %d in CSR_DCSR (0x%" PRIx64 ")",
-					cause, info->dcsr);
-	}
-
-	if (info->never_halted) {
-		info->never_halted = false;
-
-		// Disable any hardware triggers that have dmode set. We can't have set
-		// them ourselves. Maybe they're left over from some killed debug
-		// session.
-		// Count the number of triggers while we're at it.
-
-		int result = maybe_read_tselect(target);
-		if (result != ERROR_OK)
-			return result;
-		for (info->trigger_count = 0; info->trigger_count < MAX_HWBPS; info->trigger_count++) {
-			register_write_direct(target, REG_TSELECT, info->trigger_count);
-			uint64_t tselect_rb;
-			register_read_direct(target, &tselect_rb, REG_TSELECT);
-			if (info->trigger_count != tselect_rb)
-				break;
-			uint64_t tdata1;
-			register_read_direct(target, &tdata1, REG_TDATA1);
-			if ((tdata1 & MCONTROL_DMODE(xlen(target))) &&
-					(tdata1 & (MCONTROL_EXECUTE | MCONTROL_STORE | MCONTROL_LOAD))) {
-				register_write_direct(target, REG_TDATA1, 0);
-			}
-		}
-	}
-
-	if (announce) {
-		target_call_event_callbacks(target, TARGET_EVENT_HALTED);
-	}
-
-	const char *cause_string[] = {
-		"none",
-		"software breakpoint",
-		"hardware trigger",
-		"debug interrupt",
-		"step",
-		"halt"
-	};
-	// This is logged to the user so that gdb will show it when a user types
-	// 'monitor reset init'. At that time gdb appears to have the pc cached
-	// still so if a user manually inspects the pc it will still have the old
-	// value.
-	LOG_USER("halted at 0x%" PRIx64 " due to %s", info->dpc, cause_string[cause]);
-
 	return ERROR_OK;
 }
 
-static int poll_target(struct target *target, bool announce)
-{
-	select_dmi(target);
-	select_hart(target);
-
-	if (target->smp && target->gdb_service && target->gdb_service->core[1] == target->coreid) {
-		target->gdb_service->target = target;
-		target->gdb_service->core[0] = target->coreid;
-	}
-
-	// Inhibit debug logging during poll(), which isn't usually interesting and
-	// just fills up the screen/logs with clutter.
-	int old_debug_level = debug_level;
-	if (debug_level >= LOG_LVL_DEBUG) {
-		debug_level = LOG_LVL_INFO;
-	}
-	debug_level = old_debug_level;
-
-	uint32_t dmstatus = dmi_read(target, DMI_DMSTATUS);
-	if (get_field(dmstatus, DMI_DMSTATUS_ALLHALTED)) {
-		if (target->state != TARGET_HALTED) {
-			return handle_halt(target, announce);
-		}
-		return ERROR_OK;
-	}
-
-	if (get_field(dmstatus, DMI_DMSTATUS_ALLRUNNING)) {
-		target->state = TARGET_RUNNING;
-		return ERROR_OK;
-	}
-
-	if (get_field(dmstatus, DMI_DMSTATUS_ANYUNAVAIL)) {
-		target->state = TARGET_RESET;
-		return ERROR_OK;
-	}
-
-	if (get_field(dmstatus, DMI_DMSTATUS_ANYNONEXISTENT)) {
-		LOG_ERROR("Hart disappeared!");
-		return ERROR_FAIL;
-	}
-
-	return ERROR_OK;
-}
-
-static int riscv013_poll(struct target *target)
-{
-	return poll_target(target, true);
-}
-
-static int riscv013_resume(struct target *target, int current, uint32_t address,
-		int handle_breakpoints, int debug_execution)
-{
-	riscv013_info_t *info = get_info(target);
-
-	select_dmi(target);
-	select_hart(target);
-
-	if (!current) {
-		if (xlen(target) > 32) {
-			LOG_WARNING("Asked to resume at 32-bit PC on %d-bit target.",
-					xlen(target));
-		}
-		int result = register_write(target, REG_PC, address);
-		if (result != ERROR_OK)
-			return result;
-	}
-
-	if (info->need_strict_step || handle_breakpoints) {
-		int result = strict_step(target, false);
-		if (result != ERROR_OK)
-			return result;
-	}
-
-	return resume(target, debug_execution, false);
-}
-
 static int assert_reset(struct target *target)
 {
 	return ERROR_FAIL;
@@ -1852,90 +1259,6 @@ static int deassert_reset(struct target *target)
 	return ERROR_FAIL;
 }
 
-static int read_memory(struct target *target, uint32_t address,
-		uint32_t size, uint32_t count, uint8_t *buffer)
-{
-	select_dmi(target);
-	select_hart(target);
-
-        while (1) {
-          abstract_write_register(target, S0, xlen(target), address);
-
-          program_t *program = program_new();
-          switch (size) {
-          case 1:
-            program_add32(program, lb(S1, S0, 0));
-            break;
-          case 2:
-            program_add32(program, lh(S1, S0, 0));
-            break;
-          case 4:
-            program_add32(program, lw(S1, S0, 0));
-            break;
-          default:
-            LOG_ERROR("Unsupported size: %d", size);
-            return ERROR_FAIL;
-          }
-          program_add32(program, addi(S0, S0, size));
-          program_add32(program, ebreak());
-          write_program(target, program);
-          program_delete(program);
-
-          if (execute_abstract_command(target,
-                                       AC_ACCESS_REGISTER_PREEXEC |
-                                       abstract_register_size(xlen(target)) | reg_number_to_no(S1)) != ERROR_OK) {
-            return ERROR_FAIL;
-          }
-
-          uint32_t abstractcs;
-          for (uint32_t i = 0; i < count; i++) {
-            uint32_t value = dmi_read(target, DMI_DATA0);
-	    if (i == 0)
-              dmi_write(target, DMI_ABSTRACTAUTO, 0x1 << DMI_ABSTRACTAUTO_AUTOEXECDATA_OFFSET);
-            switch (size) {
-            case 1:
-              buffer[i] = value;
-              break;
-            case 2:
-              buffer[2*i] = value;
-              buffer[2*i+1] = value >> 8;
-              break;
-            case 4:
-              buffer[4*i] = value;
-              buffer[4*i+1] = value >> 8;
-              buffer[4*i+2] = value >> 16;
-              buffer[4*i+3] = value >> 24;
-              break;
-            default:
-              return ERROR_FAIL;
-            }
-            // The above dmi_read started an abstract command. If we just
-            // immediately read here, we'll probably get a busy error. Wait for idle first,
-            // or otherwise take ac_command_busy into account (this defeats the purpose
-            // of autoexec, this whole code needs optimization).
-            if (wait_for_idle(target, &abstractcs) != ERROR_OK) {
-              return ERROR_FAIL;
-            }
-          }
-          dmi_write(target, DMI_ABSTRACTAUTO, 0);
-	  abstractcs = dmi_read(target, DMI_ABSTRACTCS);
-          unsigned cmderr = get_field(abstractcs, DMI_ABSTRACTCS_CMDERR);
-          if (cmderr == CMDERR_BUSY) {
-	    // Clear the error and wait longer.
-            dmi_write(target, DMI_ABSTRACTCS, DMI_ABSTRACTCS_CMDERR);
-            increase_ac_busy_delay(target);
-          } else if (cmderr) {
-            LOG_ERROR("read_memory(): cmderr=%d", get_field(abstractcs, DMI_ABSTRACTCS_CMDERR));
-            return ERROR_FAIL;
-          } else {
-            return ERROR_OK;
-          }
-        }
-        // Should not get here.
-        assert(0);
-        return ERROR_OK;
-}
-
 /**
  * If there was a DMI error, clear that error and return 1.
  * Otherwise return 0.
@@ -1953,15 +1276,93 @@ static int check_dmi_error(struct target *target)
 	return 0;
 }
 
+static int read_memory(struct target *target, uint32_t address,
+		uint32_t size, uint32_t count, uint8_t *buffer)
+{
+	select_dmi(target);
+	riscv_set_current_hartid(target, 0);
+
+	for (uint32_t i = 0; i < count; ++i) {
+		uint32_t offset = i*size;
+		uint32_t t_addr = address + offset;
+		uint8_t *t_buffer = buffer + offset;
+
+		abstract_write_register(target, S0, riscv_xlen(target), t_addr);
+
+		program_t *program = program_new();
+		switch (size) {
+		case 1:
+			program_add32(program, lb(S1, S0, 0));
+			break;
+		case 2:
+			program_add32(program, lh(S1, S0, 0));
+			break;
+		case 4:
+			program_add32(program, lw(S1, S0, 0));
+			break;
+		default:
+			LOG_ERROR("Unsupported size: %d", size);
+			return ERROR_FAIL;
+		}
+		program_add32(program, fence());
+		program_add32(program, ebreak());
+		program_set_read(program, S1);
+		write_program(target, program);
+		execute_program(target, program);
+		uint32_t abstractcs;
+		wait_for_idle(target, &abstractcs);
+		program_delete(program);
+
+		uint32_t value = dmi_read(target, DMI_DATA0);
+		switch (size) {
+		case 1:
+			t_buffer[0] = value;
+			break;
+		case 2:
+		  t_buffer[0] = value;
+		  t_buffer[1] = value >> 8;
+		  break;
+		case 4:
+		  t_buffer[0] = value;
+		  t_buffer[1] = value >> 8;
+		  t_buffer[2] = value >> 16;
+		  t_buffer[3] = value >> 24;
+		  break;
+		default:
+		  return ERROR_FAIL;
+		}
+
+		LOG_INFO("read 0x%08x from 0x%08x", value, t_addr);
+
+		if (check_dmi_error(target)) {
+			LOG_ERROR("DMI error");
+			return ERROR_FAIL;
+		}
+	}
+
+	program_t *program = program_new();
+	program_add32(program, ebreak());
+	program_add32(program, ebreak());
+	program_add32(program, ebreak());
+	program_add32(program, ebreak());
+	write_program(target, program);
+	program_delete(program);
+
+	return ERROR_OK;
+}
+
 static int write_memory(struct target *target, uint32_t address,
 		uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	select_dmi(target);
-	select_hart(target);
+	riscv_set_current_hartid(target, 0);
 
-	while (1) {
-		abstract_write_register(target, S0, xlen(target), address);
+	for (uint32_t i = 0; i < count; ++i) {
+		uint32_t offset = size*i;
+		uint32_t t_addr = address + offset;
+		const uint8_t *t_buffer = buffer + offset;
 
+		abstract_write_register(target, S0, riscv_xlen(target), t_addr);
 		program_t *program = program_new();
 		switch (size) {
 			case 1:
@@ -1977,65 +1378,50 @@ static int write_memory(struct target *target, uint32_t address,
 				LOG_ERROR("Unsupported size: %d", size);
 				return ERROR_FAIL;
 		}
-		program_add32(program, addi(S0, S0, size));
+		program_add32(program, fence());
 		program_add32(program, ebreak());
-		write_program(target, program);
-		program_delete(program);
 
-		scans_t *scans = scans_new(target, count + 10);
-		for (uint32_t i = 0; i < count; i++) {
-			uint32_t value;
-			switch (size) {
-				case 1:
-					value = buffer[i];
-					break;
-				case 2:
-					value = buffer[2*i] | ((uint32_t) buffer[2*i+1] << 8);
-					break;
-				case 4:
-					value = buffer[4*i] |
-						((uint32_t) buffer[4*i+1] << 8) |
-						((uint32_t) buffer[4*i+2] << 16) |
-						((uint32_t) buffer[4*i+3] << 24);
-					break;
-				default:
-					return ERROR_FAIL;
-			}
-			scans_add_dmi_write(scans, DMI_DATA0, value, true);
-
-			if (i == 0) {
-				scans_add_dmi_write(scans, DMI_COMMAND,
-						AC_ACCESS_REGISTER_WRITE | AC_ACCESS_REGISTER_POSTEXEC
-						| abstract_register_size(xlen(target)) |
-						reg_number_to_no(S1), true);
-				scans_add_dmi_write(scans, DMI_ABSTRACTAUTO,
-						0x1 << DMI_ABSTRACTAUTO_AUTOEXECDATA_OFFSET,
-						false);
-			}
+		uint32_t value;
+		switch (size) {
+			case 1:
+				value = t_buffer[0];
+				break;
+			case 2:
+				value = t_buffer[0] | ((uint32_t) t_buffer[1] << 8);
+				break;
+			case 4:
+				value = t_buffer[0] |
+					((uint32_t) t_buffer[1] << 8) |
+					((uint32_t) t_buffer[2] << 16) |
+					((uint32_t) t_buffer[3] << 24);
+				break;
+			default:
+				return ERROR_FAIL;
 		}
-		int result = scans_execute(scans);
-		scans_delete(scans);
-		scans = NULL;
-		if (result != ERROR_OK)
-			return result;
+		abstract_write_register(target, S1, riscv_xlen(target), value);
+		program_set_write(program, S1, value);
+
+		LOG_INFO("writing 0x%08x to 0x%08x", value, t_addr);
+
+		write_program(target, program);
+		execute_program(target, program);
+		uint32_t abstractcs;
+		wait_for_idle(target, &abstractcs);
+		program_delete(program);
 
-		int dmi_error = check_dmi_error(target);
-
-		// Clear autoexec.
-		dmi_write(target, DMI_ABSTRACTAUTO, 0);
-		uint32_t abstractcs = dmi_read(target, DMI_ABSTRACTCS);
-		unsigned cmderr = get_field(abstractcs, DMI_ABSTRACTCS_CMDERR);
-		if (cmderr == CMDERR_BUSY) {
-			dmi_write(target, DMI_ABSTRACTCS, DMI_ABSTRACTCS_CMDERR);
-			increase_ac_busy_delay(target);
-		} else if (cmderr) {
-			LOG_ERROR("write_memory: cmderr=%d", get_field(abstractcs, DMI_ABSTRACTCS_CMDERR));
+		if (check_dmi_error(target)) {
+			LOG_ERROR("DMI error");
 			return ERROR_FAIL;
-		} else if (!dmi_error) {
-			break;
 		}
 	}
 
+	program_t *program = program_new();
+	program_add32(program, ebreak());
+	program_add32(program, ebreak());
+	program_add32(program, ebreak());
+	program_add32(program, ebreak());
+	write_program(target, program);
+	program_delete(program);
 	return ERROR_OK;
 }
 
@@ -2052,12 +1438,10 @@ struct target_type riscv013_target =
 	.deinit_target = deinit_target,
 	.examine = examine,
 
-	/* poll current target status */
-	.poll = riscv013_poll,
-
-	.halt = halt,
-	.resume = riscv013_resume,
-	.step = step,
+	.poll = &riscv_openocd_poll,
+	.halt = &riscv_openocd_halt,
+	.resume = &riscv_openocd_resume,
+	.step = &riscv_openocd_step,
 
 	.assert_reset = assert_reset,
 	.deassert_reset = deassert_reset,
@@ -2073,3 +1457,178 @@ struct target_type riscv013_target =
 
 	.arch_state = arch_state,
 };
+
+/*** 0.13-specific implementations of various RISC-V hepler functions. ***/
+static riscv_reg_t riscv013_get_register(struct target *target, int hid, int rid)
+{
+	riscv_set_current_hartid(target, hid);
+
+	uint64_t out;
+	register_read_direct(target, &out, rid);
+	return out;
+}
+
+static void riscv013_set_register(struct target *target, int hid, int rid, uint64_t value)
+{
+	riscv_set_current_hartid(target, hid);
+
+	register_write_direct(target, rid, value);
+}
+
+static void riscv013_select_current_hart(struct target *target)
+{
+	RISCV_INFO(r);
+
+	uint64_t dmcontrol = dmi_read(target, DMI_DMCONTROL);
+	dmcontrol = set_field(dmcontrol, DMI_DMCONTROL_HARTSEL, r->current_hartid);
+	dmi_write(target, DMI_DMCONTROL, dmcontrol);
+}
+
+static void riscv013_halt_current_hart(struct target *target)
+{
+	RISCV_INFO(r);
+	LOG_DEBUG("halting hart %d", r->current_hartid);
+	assert(!riscv_is_halted(target));
+
+	/* Issue the halt command, and then wait for the current hart to halt. */
+	uint32_t dmcontrol = dmi_read(target, DMI_DMCONTROL);
+	dmcontrol = set_field(dmcontrol, DMI_DMCONTROL_HALTREQ, 1);
+	dmi_write(target, DMI_DMCONTROL, dmcontrol);
+	for (size_t i = 0; i < 256; ++i)
+		if (riscv_is_halted(target))
+			break;
+
+	if (!riscv_is_halted(target)) {
+		uint32_t dmstatus = dmi_read(target, DMI_DMSTATUS);
+		dmcontrol = dmi_read(target, DMI_DMCONTROL);
+
+		LOG_ERROR("unable to halt hart %d", r->current_hartid);
+		LOG_ERROR("  dmcontrol=0x%08x", dmcontrol);
+		LOG_ERROR("  dmstatus =0x%08x", dmstatus);
+		abort();
+	}
+
+	dmcontrol = set_field(dmcontrol, DMI_DMCONTROL_HALTREQ, 0);
+	dmi_write(target, DMI_DMCONTROL, dmcontrol);
+}
+
+static void riscv013_resume_current_hart(struct target *target)
+{
+	return riscv013_step_or_resume_current_hart(target, false);
+}
+
+static void riscv013_step_current_hart(struct target *target)
+{
+	return riscv013_step_or_resume_current_hart(target, true);
+}
+
+static void riscv013_on_resume(struct target *target)
+{
+	return riscv013_on_step_or_resume(target, false);
+}
+
+static void riscv013_on_step(struct target *target)
+{
+	return riscv013_on_step_or_resume(target, true);
+}
+
+static void riscv013_on_halt(struct target *target)
+{
+	RISCV_INFO(r);
+	LOG_DEBUG("saving register state for hart %d", r->current_hartid);
+	riscv_save_register(target, GDB_REGNO_S0);
+	riscv_save_register(target, GDB_REGNO_S1);
+	riscv_save_register(target, GDB_REGNO_DPC);
+	riscv_save_register(target, GDB_REGNO_DCSR);
+}
+
+static bool riscv013_is_halted(struct target *target)
+{
+	uint32_t dmstatus = dmi_read(target, DMI_DMSTATUS);
+	return get_field(dmstatus, DMI_DMSTATUS_ALLHALTED);
+}
+
+static enum riscv_halt_reason riscv013_halt_reason(struct target *target)
+{
+	uint64_t dcsr = riscv_peek_register(target, GDB_REGNO_DCSR);
+	switch (get_field(dcsr, CSR_DCSR_CAUSE)) {
+	case CSR_DCSR_CAUSE_SWBP:
+	case CSR_DCSR_CAUSE_TRIGGER:
+		return RISCV_HALT_BREAKPOINT;
+	case CSR_DCSR_CAUSE_STEP:
+		return RISCV_HALT_SINGLESTEP;
+	case CSR_DCSR_CAUSE_DEBUGINT:
+	case CSR_DCSR_CAUSE_HALT:
+		return RISCV_HALT_INTERRUPT;
+	}
+
+	LOG_ERROR("Unknown DCSR cause field: %x", (int)get_field(dcsr, CSR_DCSR_CAUSE));
+	abort();
+}
+
+/* Helper Functions. */
+static void riscv013_on_step_or_resume(struct target *target, bool step)
+{
+	RISCV_INFO(r);
+	LOG_DEBUG("restoring register state for hart %d", r->current_hartid);
+
+	program_t *program = program_new();
+	program_add32(program, fence_i());
+	program_add32(program, ebreak());
+	write_program(target, program);
+	if (execute_program(target, program) != ERROR_OK) {
+		LOG_ERROR("Unable to execute fence.i");
+	}
+	program_delete(program);
+
+	/* We want to twiddle some bits in the debug CSR so debugging works. */
+	uint64_t dcsr = riscv_peek_register(target, GDB_REGNO_DCSR);
+	dcsr = set_field(dcsr, CSR_DCSR_STEP, step);
+	dcsr = set_field(dcsr, CSR_DCSR_EBREAKM, 1);
+	dcsr = set_field(dcsr, CSR_DCSR_EBREAKH, 1);
+	dcsr = set_field(dcsr, CSR_DCSR_EBREAKS, 1);
+	dcsr = set_field(dcsr, CSR_DCSR_EBREAKU, 1);
+	riscv_overwrite_register(target, GDB_REGNO_DCSR, dcsr);
+
+	riscv_restore_register(target, GDB_REGNO_DCSR);
+	riscv_restore_register(target, GDB_REGNO_DPC);
+	riscv_restore_register(target, GDB_REGNO_S1);
+	riscv_restore_register(target, GDB_REGNO_S0);
+}
+
+static void riscv013_step_or_resume_current_hart(struct target *target, bool step)
+{
+	RISCV_INFO(r);
+	LOG_DEBUG("resuming hart %d", r->current_hartid);
+	assert(riscv_is_halted(target));
+
+	/* Issue the halt command, and then wait for the current hart to halt. */
+	uint32_t dmcontrol = dmi_read(target, DMI_DMCONTROL);
+	dmcontrol = set_field(dmcontrol, DMI_DMCONTROL_RESUMEREQ, 1);
+	dmi_write(target, DMI_DMCONTROL, dmcontrol);
+
+#if 1
+	/* FIXME: ... well, after a short time. */
+	usleep(100);
+#else
+	/* FIXME: there's a race condition in stepping now, so just return
+	 * right away... */
+	for (size_t i = 0; i < 256; ++i) {
+		if (!riscv_is_halted(target))
+			break;
+	}
+
+	if (riscv_is_halted(target)) {
+		uint32_t dmstatus = dmi_read(target, DMI_DMSTATUS);
+		dmcontrol = dmi_read(target, DMI_DMCONTROL);
+
+		LOG_ERROR("unable to resume hart %d", r->current_hartid);
+		LOG_ERROR("  dmcontrol=0x%08x", dmcontrol);
+		LOG_ERROR("  dmstatus =0x%08x", dmstatus);
+		abort();
+	}
+#endif
+
+	dmcontrol = set_field(dmcontrol, DMI_DMCONTROL_RESUMEREQ, 0);
+	dmi_write(target, DMI_DMCONTROL, dmcontrol);
+}
diff --git a/src/target/riscv/riscv.c b/src/target/riscv/riscv.c
index 87c808e..13410bd 100644
--- a/src/target/riscv/riscv.c
+++ b/src/target/riscv/riscv.c
@@ -15,6 +15,8 @@
 #include "breakpoints.h"
 #include "helper/time_support.h"
 #include "riscv.h"
+#include "gdb_regs.h"
+#include "rtos/rtos.h"
 
 /**
  * Since almost everything can be accomplish by scanning the dbus register, all
@@ -332,7 +334,7 @@ static int riscv_examine(struct target *target)
 	return tt->examine(target);
 }
 
-static int riscv_poll(struct target *target)
+static int oldriscv_poll(struct target *target)
 {
 	struct target_type *tt = get_target_type(target);
 	return tt->poll(target);
@@ -376,7 +378,13 @@ static int riscv_get_gdb_reg_list(struct target *target,
 		struct reg **reg_list[], int *reg_list_size,
 		enum target_register_class reg_class)
 {
+	RISCV_INFO(r);
 	LOG_DEBUG("reg_class=%d", reg_class);
+	LOG_DEBUG("riscv_get_gdb_reg_list: rtos_hartid=%d current_hartid=%d", r->rtos_hartid, r->current_hartid);
+	if (r->rtos_hartid != -1)
+		riscv_set_current_hartid(target, r->rtos_hartid);
+	else
+		riscv_set_current_hartid(target, 0);
 
 	switch (reg_class) {
 		case REG_CLASS_GENERAL:
@@ -395,6 +403,7 @@ static int riscv_get_gdb_reg_list(struct target *target,
 		return ERROR_FAIL;
 	}
 	for (int i = 0; i < *reg_list_size; i++) {
+		assert(target->reg_cache->reg_list[i].size > 0);
 		(*reg_list)[i] = &target->reg_cache->reg_list[i];
 	}
 
@@ -474,7 +483,7 @@ static int riscv_run_algorithm(struct target *target, int num_mem_params,
 	reg_mstatus->type->get(reg_mstatus);
 	current_mstatus = buf_get_u64(reg_mstatus->value, 0, reg_mstatus->size);
 	uint64_t ie_mask = MSTATUS_MIE | MSTATUS_HIE | MSTATUS_SIE | MSTATUS_UIE;
-	buf_set_u64(mstatus_bytes, 0, info->xlen, set_field(current_mstatus,
+	buf_set_u64(mstatus_bytes, 0, info->xlen[0], set_field(current_mstatus,
 				ie_mask, 0));
 
 	reg_mstatus->type->set(reg_mstatus, mstatus_bytes);
@@ -492,11 +501,11 @@ static int riscv_run_algorithm(struct target *target, int num_mem_params,
 		if (now - start > timeout_ms) {
 			LOG_ERROR("Algorithm timed out after %d ms.", timeout_ms);
 			riscv_halt(target);
-			riscv_poll(target);
+			oldriscv_poll(target);
 			return ERROR_TARGET_TIMEOUT;
 		}
 
-		int result = riscv_poll(target);
+		int result = oldriscv_poll(target);
 		if (result != ERROR_OK) {
 			return result;
 		}
@@ -514,12 +523,12 @@ static int riscv_run_algorithm(struct target *target, int num_mem_params,
 
 	// Restore Interrupts
 	LOG_DEBUG("Restoring Interrupts");
-	buf_set_u64(mstatus_bytes, 0, info->xlen, current_mstatus);
+	buf_set_u64(mstatus_bytes, 0, info->xlen[0], current_mstatus);
 	reg_mstatus->type->set(reg_mstatus, mstatus_bytes);
 
 	/// Restore registers
 	uint8_t buf[8];
-	buf_set_u64(buf, 0, info->xlen, saved_pc);
+	buf_set_u64(buf, 0, info->xlen[0], saved_pc);
 	if (reg_pc->type->set(reg_pc, buf) != ERROR_OK) {
 		return ERROR_FAIL;
 	}
@@ -527,7 +536,7 @@ static int riscv_run_algorithm(struct target *target, int num_mem_params,
 	for (int i = 0; i < num_reg_params; i++) {
 		LOG_DEBUG("restore %s", reg_params[i].reg_name);
 		struct reg *r = register_get_by_name(target->reg_cache, reg_params[i].reg_name, 0);
-		buf_set_u64(buf, 0, info->xlen, saved_regs[r->number]);
+		buf_set_u64(buf, 0, info->xlen[0], saved_regs[r->number]);
 		if (r->type->set(r, buf) != ERROR_OK) {
 			return ERROR_FAIL;
 		}
@@ -572,7 +581,7 @@ struct target_type riscv_target =
 	.examine = riscv_examine,
 
 	/* poll current target status */
-	.poll = riscv_poll,
+	.poll = oldriscv_poll,
 
 	.halt = riscv_halt,
 	.resume = riscv_resume,
@@ -599,3 +608,400 @@ struct target_type riscv_target =
 
 	.run_algorithm = riscv_run_algorithm,
 };
+
+/*** OpenOCD Helper Functions ***/
+
+/* 0 means nothing happened, 1 means the hart's state changed (and thus the
+ * poll should terminate), and -1 means there was an error. */
+static int riscv_poll_hart(struct target *target, int hartid)
+{
+	RISCV_INFO(r);
+	LOG_DEBUG("polling hart %d", hartid);
+
+	/* If there's no new event then there's nothing to do. */
+	riscv_set_current_hartid(target, hartid);
+	assert((riscv_was_halted(target) && riscv_is_halted(target)) || !riscv_was_halted(target));
+	if (riscv_was_halted(target) || !riscv_is_halted(target))
+		return 0;
+
+	/* If we got here then this must be the first poll during which this
+	 * hart halted.  We need to synchronize the hart's state with the
+	 * debugger, and inform the outer polling loop that there's something
+	 * to do. */
+	r->hart_state[hartid] = RISCV_HART_HALTED;
+	r->on_halt(target);
+	return 1;
+}
+
+/*** OpenOCD Interface ***/
+int riscv_openocd_poll(struct target *target)
+{
+	LOG_DEBUG("polling all harts");
+	if (riscv_rtos_enabled(target)) {
+		/* Check every hart for an event. */
+		int triggered_hart = -1;
+		for (int i = 0; i < riscv_count_harts(target); ++i) {
+			int out = riscv_poll_hart(target, i);
+			switch (out) {
+			case 0:
+				continue;
+			case 1:
+				triggered_hart = i;
+				break;
+			case -1:
+				return ERROR_FAIL;
+			}
+		}
+		if (triggered_hart == -1) {
+			LOG_DEBUG("  no harts halted");
+			return ERROR_OK;
+		}
+		LOG_DEBUG("  hart %d halted", triggered_hart);
+
+		/* If we're here then at least one hart triggered.  That means
+		 * we want to go and halt _every_ hart in the system, as that's
+		 * the invariant we hold here.  Some harts might have already
+		 * halted (as we're either in single-step mode or they also
+		 * triggered a breakpoint), so don't attempt to halt those
+		 * harts. */
+		for (int i = 0; i < riscv_count_harts(target); ++i)
+			riscv_halt_one_hart(target, i);
+
+		target->state = TARGET_HALTED;
+		switch (riscv_halt_reason(target, triggered_hart)) {
+		case RISCV_HALT_BREAKPOINT:
+			target->debug_reason = DBG_REASON_BREAKPOINT;
+			break;
+		case RISCV_HALT_INTERRUPT:
+			target->debug_reason = DBG_REASON_DBGRQ;
+			break;
+		case RISCV_HALT_SINGLESTEP:
+			target->debug_reason = DBG_REASON_SINGLESTEP;
+			break;
+		}
+		
+		target->rtos->current_threadid = triggered_hart + 1;
+		target->rtos->current_thread = triggered_hart + 1;
+
+		target_call_event_callbacks(target, TARGET_EVENT_HALTED);
+		return ERROR_OK;
+	} else {
+		return riscv_poll_hart(target, riscv_current_hartid(target));
+	}
+}
+
+int riscv_openocd_halt(struct target *target)
+{
+	int out = riscv_halt_all_harts(target);
+	if (out != ERROR_OK)
+		return out;
+
+	target->state = TARGET_HALTED;
+	return out;
+}
+
+int riscv_openocd_resume(
+        struct target *target,
+        int current,
+        uint32_t address,
+        int handle_breakpoints,
+        int debug_execution
+) {
+	if (!current) {
+		LOG_ERROR("resume-at-pc unimplemented");
+		return ERROR_FAIL;
+	}
+
+	int out = riscv_resume_all_harts(target);
+	if (out != ERROR_OK)
+		return out;
+
+	target->state = TARGET_RUNNING;
+	return out;
+}
+
+int riscv_openocd_step(
+        struct target *target,
+        int current,
+        uint32_t address,
+        int handle_breakpoints
+) {
+	if (!current) {
+		LOG_ERROR("step-at-pc unimplemented");
+		return ERROR_FAIL;
+	}
+
+	int out = riscv_step_rtos_hart(target);
+	if (out != ERROR_OK)
+		return out;
+
+	target->state = TARGET_RUNNING;
+	return out;
+}
+
+/*** RISC-V Interface ***/
+
+void riscv_info_init(riscv_info_t *r)
+{
+	memset(r, 0, sizeof(*r));
+	r->dtm_version = 1;
+
+	/* FIXME: The RTOS gets enabled before the target gets initialized. */
+	r->rtos_enabled = true;
+
+	for (size_t h = 0; h < RISCV_MAX_HARTS; ++h) {
+		/* FIXME: I need to rip out Tim's probing sequence, as it
+		 * disrupts the running code.  For now, I'm just hard-coding
+		 * XLEN to 64 for all cores at reset. */
+		r->xlen[h] = 64;
+		r->hart_state[h] = RISCV_HART_UNKNOWN;
+
+		for (size_t e = 0; e < RISCV_MAX_REGISTERS; ++e)
+			r->valid_saved_registers[h][e] = false;
+	}
+}
+
+void riscv_save_register(struct target *target, int regno)
+{
+	RISCV_INFO(r);
+	int hartno = r->current_hartid;
+	LOG_DEBUG("riscv_save_register(%d, %d)", hartno, regno);
+	assert(r->valid_saved_registers[hartno][regno] == false);
+	r->valid_saved_registers[hartno][regno] = true;
+	r->saved_registers[hartno][regno] = riscv_get_register(target, hartno, regno);
+}
+
+uint64_t riscv_peek_register(struct target *target, int regno)
+{
+	RISCV_INFO(r);
+	int hartno = r->current_hartid;
+	LOG_DEBUG("riscv_peek_register(%d, %d)", hartno, regno);
+	assert(r->valid_saved_registers[hartno][regno] == true);
+	return r->saved_registers[hartno][regno];
+}
+
+void riscv_overwrite_register(struct target *target, int regno, uint64_t newval)
+{
+	RISCV_INFO(r);
+	int hartno = r->current_hartid;
+	LOG_DEBUG("riscv_overwrite_register(%d, %d)", hartno, regno);
+	assert(r->valid_saved_registers[hartno][regno] == true);
+	r->saved_registers[hartno][regno] = newval;
+}
+
+void riscv_restore_register(struct target *target, int regno)
+{
+	RISCV_INFO(r);
+	int hartno = r->current_hartid;
+	LOG_DEBUG("riscv_restore_register(%d, %d)", hartno, regno);
+	assert(r->valid_saved_registers[hartno][regno] == true);
+	r->valid_saved_registers[hartno][regno] = false;
+	riscv_set_register(target, hartno, regno, r->saved_registers[hartno][regno]);
+}
+
+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));
+	}
+
+	return ERROR_OK;
+}
+
+int riscv_halt_one_hart(struct target *target, int hartid)
+{
+	RISCV_INFO(r);
+	LOG_DEBUG("halting hart %d", hartid);
+	riscv_set_current_hartid(target, hartid);
+
+	if (r->hart_state[hartid] == RISCV_HART_UNKNOWN) {
+		r->hart_state[hartid] = riscv_is_halted(target) ? RISCV_HART_HALTED : RISCV_HART_RUNNING;
+		if (riscv_was_halted(target)) {
+			LOG_WARNING("Connected to hart %d, which was halted.  s0, s1, and pc were overwritten by your previous debugger session and cannot be restored.", hartid);
+			r->on_halt(target);
+		}
+	}
+
+	if (riscv_was_halted(target)) {
+		LOG_DEBUG("  hart %d requested halt, but was already halted", hartid);
+		return ERROR_OK;
+	}
+
+	r->halt_current_hart(target);
+	return ERROR_OK;
+}
+
+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));
+	}
+
+	return ERROR_OK;
+}
+
+int riscv_resume_one_hart(struct target *target, int hartid)
+{
+	RISCV_INFO(r);
+	LOG_DEBUG("resuming hart %d", hartid);
+	riscv_set_current_hartid(target, hartid);
+
+	if (r->hart_state[hartid] == RISCV_HART_UNKNOWN) {
+		r->hart_state[hartid] = riscv_is_halted(target) ? RISCV_HART_HALTED : RISCV_HART_RUNNING;
+		if (!riscv_was_halted(target)) {
+			LOG_ERROR("Asked to resume hart %d, which was in an unknown state", hartid);
+			r->on_resume(target);
+		}
+	}
+
+	if (!riscv_was_halted(target)) {
+		LOG_DEBUG("  hart %d requested resume, but was already resumed", hartid);
+		return ERROR_OK;
+	}
+
+	r->on_resume(target);
+	r->resume_current_hart(target);
+	r->hart_state[hartid] = RISCV_HART_RUNNING;
+	return ERROR_OK;
+}
+
+int riscv_step_rtos_hart(struct target *target)
+{
+	RISCV_INFO(r);
+	int hartid = r->current_hartid;
+	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.");
+			hartid = 0;
+		}
+	}
+	riscv_set_current_hartid(target, hartid);
+	LOG_DEBUG("stepping hart %d", hartid);
+
+	assert(r->hart_state[hartid] == RISCV_HART_HALTED);
+	r->on_step(target);
+	r->step_current_hart(target);
+	/* FIXME: There's a race condition with step. */
+	r->hart_state[hartid] = RISCV_HART_RUNNING;
+	return ERROR_OK;
+}
+
+int riscv_xlen(const struct target *target)
+{
+	return riscv_xlen_of_hart(target, riscv_current_hartid(target));
+}
+
+int riscv_xlen_of_hart(const struct target *target, int hartid)
+{
+	RISCV_INFO(r);
+	assert(r->xlen[hartid] != -1);
+	return r->xlen[hartid];
+}
+
+void riscv_enable_rtos(struct target *target)
+{
+	RISCV_INFO(r);
+	r->rtos_enabled = true;
+}
+
+bool riscv_rtos_enabled(const struct target *target)
+{
+	RISCV_INFO(r);
+	return r->rtos_enabled;
+}
+
+void riscv_set_current_hartid(struct target *target, int hartid)
+{
+	RISCV_INFO(r);
+	register_cache_invalidate(target->reg_cache);
+	r->current_hartid = hartid;
+	r->select_current_hart(target);
+
+	/* Update the register list's widths. */
+	for (size_t i = 0; i < GDB_REGNO_COUNT; ++i) {
+		struct reg *reg = &target->reg_cache->reg_list[i];
+
+		reg->value = &r->reg_cache_values[i];
+		reg->valid = false;
+
+		switch (i) {
+		case GDB_REGNO_PRIV:
+			reg->size = 8;
+			break;
+		default:
+			reg->size = riscv_xlen(target);
+			break;
+		}
+	}
+}
+
+int riscv_current_hartid(const struct target *target)
+{
+	RISCV_INFO(r);
+	assert(riscv_rtos_enabled(target) || target->coreid == r->current_hartid);
+	return r->current_hartid;
+}
+
+void riscv_set_all_rtos_harts(struct target *target)
+{
+	RISCV_INFO(r);
+	r->rtos_hartid = -1;
+}
+
+void riscv_set_rtos_hartid(struct target *target, int hartid)
+{
+	RISCV_INFO(r);
+	r->rtos_hartid = hartid;
+}
+
+int riscv_count_harts(struct target *target)
+{
+	return 3;
+}
+
+bool riscv_has_register(struct target *target, int hartid, int regid)
+{
+	return 1;
+}
+
+void riscv_set_register(struct target *target, int hartid, enum gdb_regno regid, uint64_t value)
+{
+	RISCV_INFO(r);
+	LOG_DEBUG("writing register %d on hart %d", regid, hartid);
+	return r->set_register(target, hartid, regid, value);
+}
+
+uint64_t riscv_get_register(struct target *target, int hartid, enum gdb_regno regid)
+{
+	RISCV_INFO(r);
+	LOG_DEBUG("reading register %d on hart %d", regid, hartid);
+	return r->get_register(target, hartid, regid);
+}
+
+bool riscv_is_halted(struct target *target)
+{
+	RISCV_INFO(r);
+	return r->is_halted(target);
+}
+
+bool riscv_was_halted(struct target *target)
+{
+	RISCV_INFO(r);
+	assert(r->hart_state[r->current_hartid] != RISCV_HART_UNKNOWN);
+	return r->hart_state[r->current_hartid] == RISCV_HART_HALTED;
+}
+
+enum riscv_halt_reason riscv_halt_reason(struct target *target, int hartid)
+{
+	RISCV_INFO(r);
+	riscv_set_current_hartid(target, hartid);
+	assert(riscv_is_halted(target));
+	return r->halt_reason(target);
+}
diff --git a/src/target/riscv/riscv.h b/src/target/riscv/riscv.h
index 2589b64..7ce2622 100644
--- a/src/target/riscv/riscv.h
+++ b/src/target/riscv/riscv.h
@@ -2,6 +2,11 @@
 #define RISCV_H
 
 #include "opcodes.h"
+#include "gdb_regs.h"
+
+/* The register cache is staticly allocated. */
+#define RISCV_MAX_HARTS 32
+#define RISCV_MAX_REGISTERS 5000
 
 extern struct target_type riscv011_target;
 extern struct target_type riscv013_target;
@@ -9,15 +14,75 @@ extern struct target_type riscv013_target;
 /*
  * Definitions shared by code supporting all RISC-V versions.
  */
+typedef uint64_t riscv_reg_t;
+
+enum riscv_hart_state {
+	RISCV_HART_UNKNOWN,
+	RISCV_HART_HALTED,
+	RISCV_HART_RUNNING,
+};
+
+enum riscv_halt_reason {
+	RISCV_HALT_INTERRUPT,
+	RISCV_HALT_BREAKPOINT,
+	RISCV_HALT_SINGLESTEP,
+};
 
 typedef struct {
 	unsigned dtm_version;
 	struct command_context *cmd_ctx;
 	void *version_specific;
-	/* Width of a GPR (and many other things) in bits. */
-	uint8_t xlen;
+
+	/* When the RTOS is enabled this target is expected to handle all the
+	 * harts in the system.  When it's disabled this just uses the regular
+	 * multi-target mode.  */
+	bool rtos_enabled;
+
+	/* The hart that the RTOS thinks is currently being debugged. */
+	int rtos_hartid;
+
+	/* The hart that is currently being debugged.  Note that this is
+	 * different than the hartid that the RTOS is expected to use.  This
+	 * one will change all the time, it's more of a global argument to
+	 * every function than an actual */
+	int current_hartid;
+
+	/* Enough space to store all the registers we might need to save. */
+	/* FIXME: This should probably be a bunch of register caches. */
+	uint64_t saved_registers[RISCV_MAX_HARTS][RISCV_MAX_REGISTERS];
+	bool valid_saved_registers[RISCV_MAX_HARTS][RISCV_MAX_REGISTERS];
+
+	/* The register cache points into here. */
+	uint64_t reg_cache_values[RISCV_MAX_REGISTERS];
+	
+	/* It's possible that each core has a different supported ISA set. */
+	int xlen[RISCV_MAX_HARTS];
+
+	/* The state of every hart. */
+	enum riscv_hart_state hart_state[RISCV_MAX_HARTS];
+
+	/* Helper functions that target the various RISC-V debug spec
+	 * implementations. */
+	riscv_reg_t (*get_register)(struct target *, int, int);
+	void (*set_register)(struct target *, int, int, uint64_t);
+	void (*select_current_hart)(struct target *);
+	bool (*is_halted)(struct target *target);
+	void (*halt_current_hart)(struct target *);
+	void (*resume_current_hart)(struct target *target);
+	void (*step_current_hart)(struct target *target);
+	void (*on_halt)(struct target *target);
+	void (*on_resume)(struct target *target);
+	void (*on_step)(struct target *target);
+	enum riscv_halt_reason (*halt_reason)(struct target *target);
 } riscv_info_t;
 
+/* Everything needs the RISC-V specific info structure, so here's a nice macro
+ * that provides that. */
+static inline riscv_info_t *riscv_info(const struct target *target) __attribute__((unused));
+static inline riscv_info_t *riscv_info(const struct target *target)
+{ return target->arch_info; }
+#define RISCV_INFO(R) riscv_info_t *R = riscv_info(target);
+
 extern uint8_t ir_dtmcontrol[1];
 extern struct scan_field select_dtmcontrol;
 extern uint8_t ir_dbus[1];
@@ -25,43 +90,88 @@ extern struct scan_field select_dbus;
 extern uint8_t ir_idcode[1];
 extern struct scan_field select_idcode;
 
-/*** Version-independent functions that we don't want in the main address space. ***/
-
-static uint32_t load(const struct target *target, unsigned int rd,
-		unsigned int base, uint16_t offset) __attribute__ ((unused));
-static uint32_t load(const struct target *target, unsigned int rd,
-		unsigned int base, uint16_t offset)
-{
-	riscv_info_t *info = (riscv_info_t *) target->arch_info;
-	switch (info->xlen) {
-		case 32:
-			return lw(rd, base, offset);
-		case 64:
-			return ld(rd, base, offset);
-	}
-	assert(0);
-}
-
-static uint32_t store(const struct target *target, unsigned int src,
-		unsigned int base, uint16_t offset) __attribute__ ((unused));
-static uint32_t store(const struct target *target, unsigned int src,
-		unsigned int base, uint16_t offset)
-{
-	riscv_info_t *info = (riscv_info_t *) target->arch_info;
-	switch (info->xlen) {
-		case 32:
-			return sw(src, base, offset);
-		case 64:
-			return sd(src, base, offset);
-	}
-	assert(0);
-}
-
-static unsigned xlen(const struct target *target) __attribute__ ((unused));
-static unsigned xlen(const struct target *target)
-{
-	riscv_info_t *info = (riscv_info_t *) target->arch_info;
-	return info->xlen;
-}
+/*** OpenOCD Interface */
+int riscv_openocd_poll(struct target *target);
+
+int riscv_openocd_halt(struct target *target);
+
+int riscv_openocd_resume(
+	struct target *target,
+	int current,
+	uint32_t address,
+	int handle_breakpoints, 
+	int debug_execution
+);
+
+int riscv_openocd_step(
+	struct target *target,
+	int current,
+	uint32_t address,
+	int handle_breakpoints
+);
+
+/*** RISC-V Interface ***/
+
+/* Initializes the shared RISC-V structure. */
+void riscv_info_init(riscv_info_t *r);
+
+/* Functions that save and restore registers.  */
+void riscv_save_register(struct target *target, int regno);
+uint64_t riscv_peek_register(struct target *target, int regno);
+void riscv_overwrite_register(struct target *target, int regno, uint64_t newval);
+void riscv_restore_register(struct target *target, int regno);
+
+/* Run control, possibly for multiple harts.  The _all_harts versions resume
+ * all the enabled harts, which when running in RTOS mode is all the harts on
+ * the system. */
+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);
+
+/* Steps the hart that's currently selected in the RTOS, or if there is no RTOS
+ * then the only hart. */
+int riscv_step_rtos_hart(struct target *target);
+
+/* Returns XLEN for the given (or current) hart. */
+int riscv_xlen(const struct target *target);
+int riscv_xlen_of_hart(const struct target *target, int hartid);
+
+/* Enables RISC-V RTOS support for this target.  This causes the coreid field
+ * of the generic target struct to be ignored, and instead for operations to
+ * apply to all the harts in the system. */
+void riscv_enable_rtos(struct target *target);
+bool riscv_rtos_enabled(const struct target *target);
+
+/* Sets the current hart, which is the hart that will actually be used when
+ * issuing debug commands. */
+void riscv_set_current_hartid(struct target *target, int hartid);
+int riscv_current_hartid(const struct target *target);
+
+/*** Support functions for the RISC-V 'RTOS', which provides multihart support
+ * without requiring multiple targets.  */
+
+/* When using the RTOS to debug, this selects the hart that is currently being
+ * debugged.  This doesn't propogate to the hardware. */
+void riscv_set_all_rtos_harts(struct target *target);
+void riscv_set_rtos_hartid(struct target *target, int hartid);
+
+/* Lists the number of harts in the system, which are assumed to be
+ * concecutive and start with mhartid=0. */
+int riscv_count_harts(struct target *target);
+
+/* Returns TRUE if the target has the given register on the given hart.  */
+bool riscv_has_register(struct target *target, int hartid, int regid);
+
+/* Returns the value of the given register on the given hart.  32-bit registers
+ * are zero extended to 64 bits.  */
+void riscv_set_register(struct target *target, int hid, enum gdb_regno rid, uint64_t v);
+riscv_reg_t riscv_get_register(struct target *target, int hid, enum gdb_regno rid);
+
+/* Checks the state of the current hart -- "is_halted" checks the actual
+ * on-device register, while "was_halted" checks the machine's state. */
+bool riscv_is_halted(struct target *target);
+bool riscv_was_halted(struct target *target);
+enum riscv_halt_reason riscv_halt_reason(struct target *target, int hartid);
 
 #endif