Add 64-bit and multihart support

This is a major rewrite of the RISC-V v0.13 OpenOCD port.  This
shouldn't have any meaningful effect on the v0.11 support, but it does
add generic versions of many functions that will allow me to later
refactor the v0.11 support so it's easier to maintain both ports.  This
started as an emergency feature branch and went on for a long time, so
it's all been squashed down into one commit so there isn't a big set of
broken commits lying around.  The changes are:

 * You can pass "-rtos riscv" to the target in OpenOCD's configuration
   file, which enables multi-hart mode.  This uses OpenOCD's RTOS
   support to control all the harts from the debug module using commands
   like "info threads" in GDB.  This support is still expermental.

 * There is support for RV64I, but due to OpenOCD limitations we only
   support 32-bit physical addresses.  I hope to remedy this by rebasing
   onto the latest OpenOCD release, which I've heard should fix this.

 * This matches the latest draft version of the RISC-V debug spec, as of
   April 26th.  This version fixes a number of spec bugs and should be
   close to the final debug spec.

22 files changed, 3210 insertions, 1451 deletions

diff --git a/src/flash/nor/fespi.c b/src/flash/nor/fespi.c
index 2437bf3..1320043 100644
--- a/src/flash/nor/fespi.c
+++ b/src/flash/nor/fespi.c
@@ -42,6 +42,7 @@
 #include <jtag/jtag.h>
 #include <helper/time_support.h>
 #include <target/algorithm.h>
+#include "target/riscv/riscv.h"
 
 /* Register offsets */
 
@@ -121,24 +122,28 @@
 
 
 #define FESPI_READ_REG(a) (_FESPI_READ_REG(a))
-#define _FESPI_READ_REG(a)			\
-{									\
+#define _FESPI_READ_REG(a)					\
+{								\
 	int __a;						\
-	uint32_t __v;					\
-									\
-	__a = target_read_u32(target, ctrl_base + (a), &__v); \
-	if (__a != ERROR_OK)			\
+	uint32_t __v;						\
+								\
+	__a = target_read_u32(target, ctrl_base + (a), &__v); 	\
+	if (__a != ERROR_OK) {					\
+		LOG_ERROR("FESPI_READ_REG error");		\
 		return __a;					\
+	}							\
 	__v;							\
 }
 
-#define FESPI_WRITE_REG(a, v)			\
-{									\
+#define FESPI_WRITE_REG(a, v)					\
+{								\
 	int __r;						\
-									\
-	__r = target_write_u32(target, ctrl_base + (a), (v)); \
-	if (__r != ERROR_OK)			\
+								\
+	__r = target_write_u32(target, ctrl_base + (a), (v)); 	\
+	if (__r != ERROR_OK) {					\
+		LOG_ERROR("FESPI_WRITE_REG error");		\
 		return __r;					\
+	}							\
 }
 
 #define FESPI_DISABLE_HW_MODE()	FESPI_WRITE_REG(FESPI_REG_FCTRL, \
@@ -779,12 +784,13 @@ static int steps_execute(struct algorithm_steps *as,
 	struct fespi_flash_bank *fespi_info = bank->driver_priv;
 	uint32_t ctrl_base = fespi_info->ctrl_base;
 	uint8_t *data_buf = malloc(data_wa->size);
+	int xlen = riscv_xlen(target);
 
 	struct reg_param reg_params[2];
-	init_reg_param(&reg_params[0], "x10", 32, PARAM_OUT);
-	init_reg_param(&reg_params[1], "x11", 32, PARAM_OUT);
-	buf_set_u32(reg_params[0].value, 0, 32, ctrl_base);
-	buf_set_u32(reg_params[1].value, 0, 32, data_wa->address);
+	init_reg_param(&reg_params[0], "x10", xlen, PARAM_OUT);
+	init_reg_param(&reg_params[1], "x11", xlen, PARAM_OUT);
+	buf_set_u64(reg_params[0].value, 0, xlen, ctrl_base);
+	buf_set_u64(reg_params[1].value, 0, xlen, data_wa->address);
 	while (!as_empty(as)) {
 		keep_alive();
 		unsigned bytes = as_compile(as, data_buf, data_wa->size);
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..0be869e
--- /dev/null
+++ b/src/rtos/riscv_debug.c
@@ -0,0 +1,315 @@
+#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_gdb_v_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 mean 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;
+	target->rtos->gdb_v_packet = riscv_gdb_v_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("handling 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;
+		}
+
+		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_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':
+		gdb_put_packet(connection, "E00", 3);
+		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_gdb_v_packet(struct connection *connection, const char *packet, int packet_size)
+{
+	char *packet_stttrr = malloc(packet_size + 1);
+	memset(packet_stttrr, '\0', packet_size + 1);
+	memcpy(packet_stttrr, packet, packet_size);
+	LOG_DEBUG("handling packet '%s'", packet_stttrr);
+
+	struct target *target = get_target_from_connection(connection);
+
+	if (strcmp(packet_stttrr, "vCont?") == 0) {
+		static const char *message = "OK";
+		gdb_put_packet(connection, (char *)message, strlen(message));
+		return JIM_OK;
+	}
+
+	int threadid;
+	if (sscanf(packet_stttrr, "vCont;s:%d;c", &threadid) == 1) {
+		riscv_set_rtos_hartid(target, threadid - 1);
+		riscv_step_rtos_hart(target);
+
+		gdb_put_packet(connection, "S02", 3);
+		return JIM_OK;
+	}
+
+	if (strcmp(packet_stttrr, "vCont;c") == 0) {
+		target->state = TARGET_RUNNING;
+		gdb_set_frontend_state_running(connection);
+		target_call_event_callbacks(target, TARGET_EVENT_GDB_START);
+		target_call_event_callbacks(target, TARGET_EVENT_RESUME_START);
+		riscv_resume_all_harts(target);
+		target_call_event_callbacks(target, TARGET_EVENT_RESUME_END);
+		return JIM_OK;
+	}
+
+	if (strncmp(packet_stttrr, "vCont", 5) == 0)
+		LOG_ERROR("Got unknown vCont-type packet");
+
+	return GDB_THREAD_PACKET_NOT_CONSUMED;
+}
+
+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_on_hart(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] = {'x', 'x', 'x'};
+				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);
+		}
+	}
+	LOG_DEBUG("%s", *hex_reg_list);
+	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..9bf218e 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
 };
 
@@ -70,6 +72,7 @@ static int os_alloc(struct target *target, struct rtos_type *ostype)
 
 	/* RTOS drivers can override the packet handler in _create(). */
 	os->gdb_thread_packet = rtos_thread_packet;
+	os->gdb_v_packet = NULL;
 
 	return JIM_OK;
 }
@@ -418,7 +421,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/rtos/rtos.h b/src/rtos/rtos.h
index 70c1193..70cec87 100644
--- a/src/rtos/rtos.h
+++ b/src/rtos/rtos.h
@@ -54,6 +54,7 @@ struct rtos {
 	struct thread_detail *thread_details;
 	int thread_count;
 	int (*gdb_thread_packet)(struct connection *connection, char const *packet, int packet_size);
+	int (*gdb_v_packet)(struct connection *connection, char const *packet, int packet_size);
 	void *rtos_specific_params;
 };
 
diff --git a/src/server/gdb_server.c b/src/server/gdb_server.c
index 2387496..4828c21 100644
--- a/src/server/gdb_server.c
+++ b/src/server/gdb_server.c
@@ -2445,6 +2445,13 @@ static int gdb_v_packet(struct connection *connection,
 	struct gdb_service *gdb_service = connection->service->priv;
 	int result;
 
+	struct target *target = get_target_from_connection(connection);
+	if (target->rtos != NULL && target->rtos->gdb_v_packet != NULL) {
+		int out = target->rtos->gdb_v_packet(connection, packet, packet_size);
+		if (out != GDB_THREAD_PACKET_NOT_CONSUMED)
+			return out;
+	}
+
 	/* if flash programming disabled - send a empty reply */
 
 	if (gdb_flash_program == 0) {
@@ -2643,7 +2650,7 @@ static void gdb_log_callback(void *priv, const char *file, unsigned line,
 	gdb_output_con(connection, string);
 }
 
-static void gdb_sig_halted(struct connection *connection)
+void gdb_sig_halted(struct connection *connection)
 {
 	char sig_reply[4];
 	snprintf(sig_reply, 4, "T%2.2x", 2);
@@ -3195,3 +3202,9 @@ int gdb_register_commands(struct command_context *cmd_ctx)
 	gdb_port_next = strdup("3333");
 	return register_commands(cmd_ctx, NULL, gdb_command_handlers);
 }
+
+void gdb_set_frontend_state_running(struct connection *connection)
+{
+	struct gdb_connection *gdb_con = connection->priv;
+	gdb_con->frontend_state = TARGET_RUNNING;
+}
diff --git a/src/server/gdb_server.h b/src/server/gdb_server.h
index 2b4ac4e..1dc30e0 100644
--- a/src/server/gdb_server.h
+++ b/src/server/gdb_server.h
@@ -45,6 +45,9 @@ static inline struct target *get_target_from_connection(struct connection *conne
 	return gdb_service->target;
 }
 
+void gdb_set_frontend_state_running(struct connection *connection);
+void gdb_sig_halted(struct connection *connection);
+
 #define ERROR_GDB_BUFFER_TOO_SMALL (-800)
 #define ERROR_GDB_TIMEOUT (-801)
 
diff --git a/src/target/Makefile.am b/src/target/Makefile.am
index 44299fc..ba15300 100644
--- a/src/target/Makefile.am
+++ b/src/target/Makefile.am
@@ -138,7 +138,9 @@ INTEL_IA32_SRC = \
 RISCV_SRC = \
 	riscv/riscv-011.c \
 	riscv/riscv-013.c \
-	riscv/riscv.c
+	riscv/riscv.c \
+	riscv/program.c \
+	riscv/batch.c
 
 noinst_HEADERS = \
 	algorithm.h \
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/batch.c b/src/target/riscv/batch.c
new file mode 100644
index 0000000..d48b34a
--- /dev/null
+++ b/src/target/riscv/batch.c
@@ -0,0 +1,156 @@
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "batch.h"
+#include "debug_defines.h"
+#include "riscv.h"
+
+#define get_field(reg, mask) (((reg) & (mask)) / ((mask) & ~((mask) << 1)))
+#define set_field(reg, mask, val) (((reg) & ~(mask)) | (((val) * ((mask) & ~((mask) << 1))) & (mask)))
+
+static void dump_field(const struct scan_field *field);
+
+struct riscv_batch *riscv_batch_alloc(struct target *target, size_t scans, size_t idle)
+{
+	scans += 4;
+	struct riscv_batch *out = malloc(sizeof(*out));
+	memset(out, 0, sizeof(*out));
+	out->target = target;
+	out->allocated_scans = scans;
+	out->used_scans = 0;
+	out->idle_count = idle;
+	out->data_out = malloc(sizeof(*out->data_out) * (scans) * sizeof(uint64_t));
+	out->data_in  = malloc(sizeof(*out->data_in)  * (scans) * sizeof(uint64_t));
+	out->fields = malloc(sizeof(*out->fields) * (scans));
+	out->last_scan = RISCV_SCAN_TYPE_INVALID;
+	out->read_keys = malloc(sizeof(*out->read_keys) * (scans));
+	out->read_keys_used = 0;
+	return out;
+}
+
+void riscv_batch_free(struct riscv_batch *batch)
+{
+	free(batch->data_in);
+	free(batch->data_out);
+	free(batch->fields);
+	free(batch);
+}
+
+bool riscv_batch_full(struct riscv_batch *batch)
+{
+	return batch->used_scans > (batch->allocated_scans - 4);
+}
+
+void riscv_batch_run(struct riscv_batch *batch)
+{
+	LOG_DEBUG("running a batch of %ld scans", (long)batch->used_scans);
+	riscv_batch_add_nop(batch);
+
+	for (size_t i = 0; i < batch->used_scans; ++i) {
+		dump_field(batch->fields + i);
+		jtag_add_dr_scan(batch->target->tap, 1, batch->fields + i, TAP_IDLE);
+		if (batch->idle_count > 0)
+			jtag_add_runtest(batch->idle_count, TAP_IDLE);
+	}
+
+	LOG_DEBUG("executing queue");
+	if (jtag_execute_queue() != ERROR_OK) {
+		LOG_ERROR("Unable to execute JTAG queue");
+		abort();
+	}
+
+	for (size_t i = 0; i < batch->used_scans; ++i)
+		dump_field(batch->fields + i);
+}
+
+void riscv_batch_add_dmi_write(struct riscv_batch *batch, unsigned address, uint64_t data)
+{
+	assert(batch->used_scans < batch->allocated_scans);
+	struct scan_field *field = batch->fields + batch->used_scans;
+	field->num_bits = riscv_dmi_write_u64_bits(batch->target);
+	field->out_value = (void *)(batch->data_out + batch->used_scans * sizeof(uint64_t));
+	field->in_value  = (void *)(batch->data_in  + batch->used_scans * sizeof(uint64_t));
+	riscv_fill_dmi_write_u64(batch->target, (char *)field->out_value, address, data);
+	riscv_fill_dmi_nop_u64(batch->target, (char *)field->in_value);
+	batch->last_scan = RISCV_SCAN_TYPE_WRITE;
+	batch->used_scans++;
+}
+
+size_t riscv_batch_add_dmi_read(struct riscv_batch *batch, unsigned address)
+{
+	assert(batch->used_scans < batch->allocated_scans);
+	struct scan_field *field = batch->fields + batch->used_scans;
+	field->num_bits = riscv_dmi_write_u64_bits(batch->target);
+	field->out_value = (void *)(batch->data_out + batch->used_scans * sizeof(uint64_t));
+	field->in_value  = (void *)(batch->data_in  + batch->used_scans * sizeof(uint64_t));
+	riscv_fill_dmi_read_u64(batch->target, (char *)field->out_value, address);
+	riscv_fill_dmi_nop_u64(batch->target, (char *)field->in_value);
+	batch->last_scan = RISCV_SCAN_TYPE_READ;
+	batch->used_scans++;
+
+	/* FIXME We get the read response back on the next scan.  For now I'm
+	 * just sticking a NOP in there, but this should be coelesced away. */
+	riscv_batch_add_nop(batch);
+
+	batch->read_keys[batch->read_keys_used] = batch->used_scans - 1;
+	LOG_DEBUG("read key %ld for batch 0x%p is %ld (0x%p)", batch->read_keys_used, batch, batch->used_scans - 1, (uint64_t*)batch->data_in + (batch->used_scans + 1));
+	return batch->read_keys_used++;
+}
+
+uint64_t riscv_batch_get_dmi_read(struct riscv_batch *batch, size_t key)
+{
+	assert(key < batch->read_keys_used);
+	size_t index = batch->read_keys[key];
+	assert(index <= batch->used_scans);
+	uint64_t *addr = ((uint64_t *)(batch->data_in) + index);
+	return *addr;
+}
+
+void riscv_batch_add_nop(struct riscv_batch *batch)
+{
+	assert(batch->used_scans < batch->allocated_scans);
+	struct scan_field *field = batch->fields + batch->used_scans;
+	field->num_bits = riscv_dmi_write_u64_bits(batch->target);
+	field->out_value = (void *)(batch->data_out + batch->used_scans * sizeof(uint64_t));
+	field->in_value  = (void *)(batch->data_in  + batch->used_scans * sizeof(uint64_t));
+	riscv_fill_dmi_nop_u64(batch->target, (char *)field->out_value);
+	riscv_fill_dmi_nop_u64(batch->target, (char *)field->in_value);
+	batch->last_scan = RISCV_SCAN_TYPE_NOP;
+	batch->used_scans++;
+	LOG_DEBUG("  added NOP with in_value=0x%p", field->in_value);
+}
+
+void dump_field(const struct scan_field *field)
+{
+        static const char *op_string[] = {"-", "r", "w", "?"};
+        static const char *status_string[] = {"+", "?", "F", "b"};
+
+        if (debug_level < LOG_LVL_DEBUG)
+                return;
+
+	assert(field->out_value != NULL);
+        uint64_t out = buf_get_u64(field->out_value, 0, field->num_bits);
+        unsigned int out_op = get_field(out, DTM_DMI_OP);
+        unsigned int out_data = get_field(out, DTM_DMI_DATA);
+        unsigned int out_address = out >> DTM_DMI_ADDRESS_OFFSET;
+
+        if (field->in_value) {
+                uint64_t in = buf_get_u64(field->in_value, 0, field->num_bits);
+                unsigned int in_op = get_field(in, DTM_DMI_OP);
+                unsigned int in_data = get_field(in, DTM_DMI_DATA);
+                unsigned int in_address = in >> DTM_DMI_ADDRESS_OFFSET;
+
+                log_printf_lf(LOG_LVL_DEBUG,
+                                __FILE__, __LINE__, __PRETTY_FUNCTION__,
+                                "%db %s %08x @%02x -> %s %08x @%02x [0x%p -> 0x%p]",
+                                field->num_bits,
+                                op_string[out_op], out_data, out_address,
+                                status_string[in_op], in_data, in_address,
+				field->out_value, field->in_value);
+        } else {
+                log_printf_lf(LOG_LVL_DEBUG,
+                                __FILE__, __LINE__, __PRETTY_FUNCTION__, "%db %s %08x @%02x -> ?",
+                                field->num_bits, op_string[out_op], out_data, out_address);
+        }
+}
diff --git a/src/target/riscv/batch.h b/src/target/riscv/batch.h
new file mode 100644
index 0000000..776d93a
--- /dev/null
+++ b/src/target/riscv/batch.h
@@ -0,0 +1,64 @@
+#ifndef TARGET__RISCV__SCANS_H
+#define TARGET__RISCV__SCANS_H
+
+#include "target/target.h"
+#include "jtag/jtag.h"
+
+enum riscv_scan_type {
+	RISCV_SCAN_TYPE_INVALID,
+	RISCV_SCAN_TYPE_NOP,
+	RISCV_SCAN_TYPE_READ,
+	RISCV_SCAN_TYPE_WRITE,
+};
+
+/* A batch of multiple JTAG scans, which are grouped together to avoid the
+ * overhead of some JTAG adapters when sending single commands.  This is
+ * designed to support block copies, as that's what we actually need to go
+ * fast. */
+struct riscv_batch {
+	struct target *target;
+
+	size_t allocated_scans;
+	size_t used_scans;
+
+	size_t idle_count;
+
+	char *data_out;
+	char *data_in;
+	struct scan_field *fields;
+
+	/* In JTAG we scan out the previous value's output when performing a
+	 * scan.  This is a pain for users, so we just provide them the
+	 * illusion of not having to do this by eliding all but the last NOP.
+	 * */
+	enum riscv_scan_type last_scan;
+
+	/* The read keys. */
+	size_t *read_keys;
+	size_t read_keys_used;
+};
+
+/* Allocates (or frees) a new scan set.  "scans" is the maximum number of JTAG
+ * scans that can be issued to this object, and idle is the number of JTAG idle
+ * cycles between every real scan. */
+struct riscv_batch *riscv_batch_alloc(struct target *target, size_t scans, size_t idle);
+void riscv_batch_free(struct riscv_batch *batch);
+
+/* Checks to see if this batch is full. */
+bool riscv_batch_full(struct riscv_batch *batch);
+
+/* Executes this scan batch. */
+void riscv_batch_run(struct riscv_batch *batch);
+
+/* Adds a DMI write to this batch. */
+void riscv_batch_add_dmi_write(struct riscv_batch *batch, unsigned address, uint64_t data);
+
+/* DMI reads must be handled in two parts: the first one schedules a read and
+ * provides a key, the second one actually obtains the value of that read .*/
+size_t riscv_batch_add_dmi_read(struct riscv_batch *batch, unsigned address);
+uint64_t riscv_batch_get_dmi_read(struct riscv_batch *batch, size_t key);
+
+/* Scans in a NOP. */
+void riscv_batch_add_nop(struct riscv_batch *batch);
+
+#endif
diff --git a/src/target/riscv/debug_defines.h b/src/target/riscv/debug_defines.h
index ec535db..2fb541b 100644
--- a/src/target/riscv/debug_defines.h
+++ b/src/target/riscv/debug_defines.h
@@ -26,15 +26,28 @@
 #define DTM_IDCODE_1                        (0x1 << DTM_IDCODE_1_OFFSET)
 #define DTM_DTMCS                           0x10
 /*
-* Writing 1 to this bit resets the DMI controller, clearing any
-* sticky error state.
+* Writing 1 to this bit does a hard reset of the DTM,
+* causing the DTM to forget about any outstanding DMI transactions.
+* In general this should only be used when the Debugger has
+* reason to expect that the outstanding DMI transaction will never
+* complete (e.g. a reset condition caused an inflight DMI transaction to
+* be cancelled).
+ */
+#define DTM_DTMCS_DMIHARDRESET_OFFSET       17
+#define DTM_DTMCS_DMIHARDRESET_LENGTH       1
+#define DTM_DTMCS_DMIHARDRESET              (0x1 << DTM_DTMCS_DMIHARDRESET_OFFSET)
+/*
+* Writing 1 to this bit clears the sticky error state
+* and allows the DTM to retry or complete the previous
+* transaction.
  */
 #define DTM_DTMCS_DMIRESET_OFFSET           16
 #define DTM_DTMCS_DMIRESET_LENGTH           1
 #define DTM_DTMCS_DMIRESET                  (0x1 << DTM_DTMCS_DMIRESET_OFFSET)
 /*
-* This is the minimum number of cycles a debugger should spend in
-* Run-Test/Idle after every DMI scan to avoid a 'busy'
+* This is a hint to the debugger of the minimum number of
+* cycles a debugger should spend in
+* Run-Test/Idle after every DMI scan to avoid a `busy'
 * return code (\Fdmistat of 3). A debugger must still
 * check \Fdmistat when necessary.
 *
@@ -146,26 +159,26 @@
 #define CSR_DCSR_XDEBUGVER_LENGTH           2
 #define CSR_DCSR_XDEBUGVER                  (0x3 << CSR_DCSR_XDEBUGVER_OFFSET)
 /*
-* When 1, {\tt ebreak} instructions in Machine Mode enter Halt Mode.
+* When 1, {\tt ebreak} instructions in Machine Mode enter Debug Mode.
  */
 #define CSR_DCSR_EBREAKM_OFFSET             15
 #define CSR_DCSR_EBREAKM_LENGTH             1
 #define CSR_DCSR_EBREAKM                    (0x1 << CSR_DCSR_EBREAKM_OFFSET)
 /*
-* When 1, {\tt ebreak} instructions in Hypervisor Mode enter Halt Mode.
+* When 1, {\tt ebreak} instructions in Hypervisor Mode enter Debug Mode.
  */
 #define CSR_DCSR_EBREAKH_OFFSET             14
 #define CSR_DCSR_EBREAKH_LENGTH             1
 #define CSR_DCSR_EBREAKH                    (0x1 << CSR_DCSR_EBREAKH_OFFSET)
 /*
-* When 1, {\tt ebreak} instructions in Supervisor Mode enter Halt Mode.
+* When 1, {\tt ebreak} instructions in Supervisor Mode enter Debug Mode.
  */
 #define CSR_DCSR_EBREAKS_OFFSET             13
 #define CSR_DCSR_EBREAKS_LENGTH             1
 #define CSR_DCSR_EBREAKS                    (0x1 << CSR_DCSR_EBREAKS_OFFSET)
 /*
 * When 1, {\tt ebreak} instructions in User/Application Mode enter
-* Halt Mode.
+* Debug Mode.
  */
 #define CSR_DCSR_EBREAKU_OFFSET             12
 #define CSR_DCSR_EBREAKU_LENGTH             1
@@ -173,7 +186,7 @@
 /*
 * 0: Increment counters as usual.
 *
-* 1: Don't increment any counters while in Halt Mode.  This includes
+* 1: Don't increment any counters while in Debug Mode.  This includes
 * the {\tt cycle} and {\tt instret} CSRs. This is preferred for most
 * debugging scenarios.
 *
@@ -187,7 +200,7 @@
 /*
 * 0: Increment timers as usual.
 *
-* 1: Don't increment any hart-local timers while in Halt Mode.
+* 1: Don't increment any hart-local timers while in Debug Mode.
 *
 * An implementation may choose not to support writing to this bit.
 * The debugger must read back the value it writes to check whether
@@ -197,29 +210,27 @@
 #define CSR_DCSR_STOPTIME_LENGTH            1
 #define CSR_DCSR_STOPTIME                   (0x1 << CSR_DCSR_STOPTIME_OFFSET)
 /*
-* Explains why Halt Mode was entered.
+* Explains why Debug Mode was entered.
 *
-* When there are multiple reasons to enter Halt Mode in a single
+* When there are multiple reasons to enter Debug Mode in a single
 * cycle, the cause with the highest priority is the one written.
 *
-* 1: A software breakpoint was hit. (priority 3)
+* 1: An {\tt ebreak} instruction was executed. (priority 3)
 *
 * 2: The Trigger Module caused a halt. (priority 4)
 *
-* 3: The debug interrupt was asserted by the Debug Module. (priority 2)
+* 3: \Fhaltreq was set. (priority 2)
 *
 * 4: The hart single stepped because \Fstep was set. (priority 1)
 *
-* 5: \Fhaltreq was set. (priority 0)
-*
 * Other values are reserved for future use.
  */
 #define CSR_DCSR_CAUSE_OFFSET               6
 #define CSR_DCSR_CAUSE_LENGTH               3
 #define CSR_DCSR_CAUSE                      (0x7 << CSR_DCSR_CAUSE_OFFSET)
 /*
-* When set and not in Halt Mode, the hart will only execute a single
-* instruction, and then enter Halt Mode. Interrupts are disabled
+* When set and not in Debug Mode, the hart will only execute a single
+* instruction, and then enter Debug Mode. Interrupts are disabled
 * when this bit is set.
  */
 #define CSR_DCSR_STEP_OFFSET                2
@@ -227,9 +238,9 @@
 #define CSR_DCSR_STEP                       (0x1 << CSR_DCSR_STEP_OFFSET)
 /*
 * Contains the privilege level the hart was operating in when Debug
-* Mode was entered. The encoding is describe in Table
+* Mode was entered. The encoding is described in Table
 * \ref{tab:privlevel}.  A debugger can change this value to change
-* the hart's privilege level when exiting Halt Mode.
+* the hart's privilege level when exiting Debug Mode.
 *
 * Not all privilege levels are supported on all harts. If the
 * encoding written is not supported or the debugger is not allowed to
@@ -247,9 +258,9 @@
 #define CSR_PRIV                            virtual
 /*
 * Contains the privilege level the hart was operating in when Debug
-* Mode was entered. The encoding is describe in Table
+* Mode was entered. The encoding is described in Table
 * \ref{tab:privlevel}. A user can write this value to change the
-* hart's privilege level when exiting Halt Mode.
+* hart's privilege level when exiting Debug Mode.
  */
 #define CSR_PRIV_PRV_OFFSET                 0
 #define CSR_PRIV_PRV_LENGTH                 2
@@ -283,10 +294,10 @@
 * 0: Both Debug and M Mode can write the {\tt tdata} registers at the
 * selected \Rtselect.
 *
-* 1: Only Halt Mode can write the {\tt tdata} registers at the
+* 1: Only Debug Mode can write the {\tt tdata} registers at the
 * selected \Rtselect.  Writes from other modes are ignored.
 *
-* This bit is only writable from Halt Mode.
+* This bit is only writable from Debug Mode.
  */
 #define CSR_TDATA1_HMODE_OFFSET             XLEN-5
 #define CSR_TDATA1_HMODE_LENGTH             1
@@ -366,7 +377,7 @@
 * 0: Raise a breakpoint exception. (Used when software wants to use
 * the trigger module without an external debugger attached.)
 *
-* 1: Enter Halt Mode. (Only supported when \Fhmode is 1.)
+* 1: Enter Debug Mode. (Only supported when \Fhmode is 1.)
 *
 * 2: Start tracing.
 *
@@ -504,10 +515,10 @@
 /*
 * Determines what happens when this trigger matches.
 *
-* 0: Raise a debug exception. (Used when software wants to use the
+* 0: Raise a breakpoint exception. (Used when software wants to use the
 * trigger module without an external debugger attached.)
 *
-* 1: Enter Halt Mode. (Only supported when \Fhmode is 1.)
+* 1: Enter Debug Mode. (Only supported when \Fhmode is 1.)
 *
 * 2: Start tracing.
 *
@@ -524,6 +535,18 @@
 #define CSR_ICOUNT_ACTION                   (0x3fL << CSR_ICOUNT_ACTION_OFFSET)
 #define DMI_DMSTATUS                        0x11
 /*
+* This field is 1 when all currently selected harts have acknowledged the previous \Fresumereq.
+ */
+#define DMI_DMSTATUS_ALLRESUMEACK_OFFSET    17
+#define DMI_DMSTATUS_ALLRESUMEACK_LENGTH    1
+#define DMI_DMSTATUS_ALLRESUMEACK           (0x1 << DMI_DMSTATUS_ALLRESUMEACK_OFFSET)
+/*
+* This field is 1 when any currently selected hart has acknowledged the previous \Fresumereq.
+ */
+#define DMI_DMSTATUS_ANYRESUMEACK_OFFSET    16
+#define DMI_DMSTATUS_ANYRESUMEACK_LENGTH    1
+#define DMI_DMSTATUS_ANYRESUMEACK           (0x1 << DMI_DMSTATUS_ANYRESUMEACK_OFFSET)
+/*
 * This field is 1 when all currently selected harts do not exist in this system.
  */
 #define DMI_DMSTATUS_ALLNONEXISTENT_OFFSET  15
@@ -617,7 +640,7 @@
 #define DMI_DMSTATUS_VERSIONLO              (0x3 << DMI_DMSTATUS_VERSIONLO_OFFSET)
 #define DMI_DMCONTROL                       0x10
 /*
-* Halt request signal for all currently selected harts. When 1, the
+* Halt request signal for all currently selected harts. When set to 1, the
 * hart will halt if it is not currently halted.
 * Setting both \Fhaltreq and \Fresumereq leads to undefined behavior.
 *
@@ -627,7 +650,7 @@
 #define DMI_DMCONTROL_HALTREQ_LENGTH        1
 #define DMI_DMCONTROL_HALTREQ               (0x1 << DMI_DMCONTROL_HALTREQ_OFFSET)
 /*
-* Resume request signal for all currently selected harts. When 1,
+* Resume request signal for all currently selected harts. When set to 1,
 * the hart will resume if it is currently halted.
 * Setting both \Fhaltreq and \Fresumereq leads to undefined behavior.
 *
@@ -942,31 +965,12 @@
 #define DMI_DATA0_DATA_OFFSET               0
 #define DMI_DATA0_DATA_LENGTH               32
 #define DMI_DATA0_DATA                      (0xffffffff << DMI_DATA0_DATA_OFFSET)
-#define DMI_DATA1                           0x05
-#define DMI_DATA2                           0x06
-#define DMI_DATA3                           0x07
-#define DMI_DATA4                           0x08
-#define DMI_DATA5                           0x09
-#define DMI_DATA6                           0x0a
-#define DMI_DATA7                           0x0b
-#define DMI_DATA8                           0x0c
-#define DMI_DATA9                           0x0d
-#define DMI_DATA10                          0x0e
 #define DMI_DATA11                          0x0f
 #define DMI_PROGBUF0                        0x20
 #define DMI_PROGBUF0_DATA_OFFSET            0
 #define DMI_PROGBUF0_DATA_LENGTH            32
 #define DMI_PROGBUF0_DATA                   (0xffffffff << DMI_PROGBUF0_DATA_OFFSET)
-#define DMI_PROGBUF1                        0x21
-#define DMI_PROGBUF2                        0x22
-#define DMI_PROGBUF3                        0x23
-#define DMI_PROGBUF4                        0x24
-#define DMI_PROGBUF5                        0x25
-#define DMI_PROGBUF6                        0x26
-#define DMI_PROGBUF7                        0x27
-#define DMI_PROGBUF8                        0x28
-#define DMI_PROGBUF9                        0x29
-#define DMI_PROGBUF10                       0x2a
+#define DMI_PROGBUF15                       0x2f
 #define DMI_AUTHDATA                        0x30
 #define DMI_AUTHDATA_DATA_OFFSET            0
 #define DMI_AUTHDATA_DATA_LENGTH            32
@@ -1233,93 +1237,6 @@
 #define DMI_SBDATA3_DATA_OFFSET             0
 #define DMI_SBDATA3_DATA_LENGTH             32
 #define DMI_SBDATA3_DATA                    (0xffffffff << DMI_SBDATA3_DATA_OFFSET)
-#define SERINFO                             0x280
-/*
-* Like \Fserialzero.
- */
-#define SERINFO_SERIAL7_OFFSET              7
-#define SERINFO_SERIAL7_LENGTH              1
-#define SERINFO_SERIAL7                     (0x1 << SERINFO_SERIAL7_OFFSET)
-/*
-* Like \Fserialzero.
- */
-#define SERINFO_SERIAL6_OFFSET              6
-#define SERINFO_SERIAL6_LENGTH              1
-#define SERINFO_SERIAL6                     (0x1 << SERINFO_SERIAL6_OFFSET)
-/*
-* Like \Fserialzero.
- */
-#define SERINFO_SERIAL5_OFFSET              5
-#define SERINFO_SERIAL5_LENGTH              1
-#define SERINFO_SERIAL5                     (0x1 << SERINFO_SERIAL5_OFFSET)
-/*
-* Like \Fserialzero.
- */
-#define SERINFO_SERIAL4_OFFSET              4
-#define SERINFO_SERIAL4_LENGTH              1
-#define SERINFO_SERIAL4                     (0x1 << SERINFO_SERIAL4_OFFSET)
-/*
-* Like \Fserialzero.
- */
-#define SERINFO_SERIAL3_OFFSET              3
-#define SERINFO_SERIAL3_LENGTH              1
-#define SERINFO_SERIAL3                     (0x1 << SERINFO_SERIAL3_OFFSET)
-/*
-* Like \Fserialzero.
- */
-#define SERINFO_SERIAL2_OFFSET              2
-#define SERINFO_SERIAL2_LENGTH              1
-#define SERINFO_SERIAL2                     (0x1 << SERINFO_SERIAL2_OFFSET)
-/*
-* Like \Fserialzero.
- */
-#define SERINFO_SERIAL1_OFFSET              1
-#define SERINFO_SERIAL1_LENGTH              1
-#define SERINFO_SERIAL1                     (0x1 << SERINFO_SERIAL1_OFFSET)
-/*
-* 1 means serial interface 0 is supported.
- */
-#define SERINFO_SERIAL0_OFFSET              0
-#define SERINFO_SERIAL0_LENGTH              1
-#define SERINFO_SERIAL0                     (0x1 << SERINFO_SERIAL0_OFFSET)
-#define SERSEND0                            0x200
-#define SERRECV0                            0x204
-#define SERSTAT0                            0x208
-/*
-* Send ready. 1 when the core-to-debugger queue is not full. 0
-* otherwise.
- */
-#define SERSTAT0_SENDR_OFFSET               1
-#define SERSTAT0_SENDR_LENGTH               1
-#define SERSTAT0_SENDR                      (0x1 << SERSTAT0_SENDR_OFFSET)
-/*
-* Receive ready. 1 when the debugger-to-core queue is not empty. 0
-* otherwise.
- */
-#define SERSTAT0_RECVR_OFFSET               0
-#define SERSTAT0_RECVR_LENGTH               1
-#define SERSTAT0_RECVR                      (0x1 << SERSTAT0_RECVR_OFFSET)
-#define SERSEND1                            0x210
-#define SERRECV1                            0x214
-#define SERSTAT1                            0x218
-#define SERSEND2                            0x220
-#define SERRECV2                            0x224
-#define SERSTAT2                            0x228
-#define SERSEND3                            0x230
-#define SERRECV3                            0x234
-#define SERSTAT3                            0x238
-#define SERSEND4                            0x240
-#define SERRECV4                            0x244
-#define SERSTAT4                            0x248
-#define SERSEND5                            0x250
-#define SERRECV5                            0x254
-#define SERSTAT5                            0x258
-#define SERSEND6                            0x260
-#define SERRECV6                            0x264
-#define SERSTAT6                            0x268
-#define SERSEND7                            0x274
-#define SERRECV7                            0x278
-#define SERSTAT7                            0x27c
 #define TRACE                               0x728
 /*
 * 1 if the trace buffer has wrapped since the last time \Fdiscard was
@@ -1454,14 +1371,6 @@
 #define AC_ACCESS_REGISTER_SIZE             (0x7 << AC_ACCESS_REGISTER_SIZE_OFFSET)
 /*
 * When 1, execute the program in the Program Buffer exactly once
-* before performing the transfer.
-* \textbf{WARNING: preexec is considered for removal.}
- */
-#define AC_ACCESS_REGISTER_PREEXEC_OFFSET   19
-#define AC_ACCESS_REGISTER_PREEXEC_LENGTH   1
-#define AC_ACCESS_REGISTER_PREEXEC          (0x1 << AC_ACCESS_REGISTER_PREEXEC_OFFSET)
-/*
-* When 1, execute the program in the Program Buffer exactly once
 * after performing the transfer, if any.
  */
 #define AC_ACCESS_REGISTER_POSTEXEC_OFFSET  18
diff --git a/src/target/riscv/gdb_regs.h b/src/target/riscv/gdb_regs.h
new file mode 100644
index 0000000..e366f7d
--- /dev/null
+++ b/src/target/riscv/gdb_regs.h
@@ -0,0 +1,28 @@
+#ifndef TARGET__RISCV__GDB_REGS_H
+#define TARGET__RISCV__GDB_REGS_H
+
+enum gdb_regno {
+        GDB_REGNO_XPR0 = 0,
+	GDB_REGNO_X0 = GDB_REGNO_XPR0 + 0,
+	GDB_REGNO_ZERO = 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_DSCRATCH = CSR_DSCRATCH + 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..3e1f8a2 100644
--- a/src/target/riscv/opcodes.h
+++ b/src/target/riscv/opcodes.h
@@ -125,6 +125,16 @@ static uint32_t csrr(unsigned int rd, unsigned int csr) {
   return (csr << 20) | (rd << 7) | MATCH_CSRRS;
 }
 
+static uint32_t csrrs(unsigned int rd, unsigned int rs, unsigned int csr) __attribute__ ((unused));
+static uint32_t csrrs(unsigned int rd, unsigned int rs, unsigned int csr) {
+  return (csr << 20) | (rs << 15) | (rd << 7) | MATCH_CSRRS;
+}
+
+static uint32_t csrrw(unsigned int rd, unsigned int rs, unsigned int csr) __attribute__ ((unused));
+static uint32_t csrrw(unsigned int rd, unsigned int rs, unsigned int csr) {
+  return (csr << 20) | (rs << 15) | (rd << 7) | MATCH_CSRRW;
+}
+
 static uint32_t fsw(unsigned int src, unsigned int base, uint16_t offset) __attribute__ ((unused));
 static uint32_t fsw(unsigned int src, unsigned int base, uint16_t offset)
 {
@@ -206,7 +216,6 @@ static uint32_t fence_i(void)
   return MATCH_FENCE_I;
 }
 
-/*
 static uint32_t lui(unsigned int dest, uint32_t imm) __attribute__ ((unused));
 static uint32_t lui(unsigned int dest, uint32_t imm)
 {
@@ -215,6 +224,7 @@ static uint32_t lui(unsigned int dest, uint32_t imm)
     MATCH_LUI;
 }
 
+/*
 static uint32_t csrci(unsigned int csr, uint16_t imm) __attribute__ ((unused));
 static uint32_t csrci(unsigned int csr, uint16_t imm) {
   return (csr << 20) |
@@ -271,3 +281,15 @@ 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;
+}
+
+static uint32_t auipc(unsigned int dest) __attribute__((unused));
+static uint32_t auipc(unsigned int dest)
+{
+	return MATCH_AUIPC | (dest << 7);
+}
diff --git a/src/target/riscv/program.c b/src/target/riscv/program.c
new file mode 100644
index 0000000..1487316
--- /dev/null
+++ b/src/target/riscv/program.c
@@ -0,0 +1,491 @@
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "target/target.h"
+#include "riscv.h"
+#include "program.h"
+#include "helper/log.h"
+
+#include "asm.h"
+#include "encoding.h"
+
+riscv_addr_t riscv_program_gal(struct riscv_program *p, riscv_addr_t addr);
+int riscv_program_lah(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr);
+int riscv_program_lal(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr);
+
+/* Program interface. */
+int riscv_program_init(struct riscv_program *p, struct target *target)
+{
+	LOG_DEBUG("riscv_program_init: p=0x%p", p);
+
+	memset(p, 0, sizeof(*p));
+	p->target = target;
+	p->instruction_count = 0;
+	p->data_count = 0;
+	p->writes_memory = 0;
+	p->target_xlen = riscv_xlen(target);
+	for (size_t i = 0; i < RISCV_REGISTER_COUNT; ++i) {
+		p->writes_xreg[i] = 0;
+		p->in_use[i] = 0;
+	}
+
+	for(size_t i = 0; i < RISCV_MAX_DEBUG_BUFFER_SIZE; ++i)
+		p->debug_buffer[i] = -1;
+
+	if (riscv_debug_buffer_enter(target, p) != ERROR_OK) {
+		LOG_ERROR("unable to write progam buffer enter code");
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
+int riscv_program_exec(struct riscv_program *p, struct target *t)
+{
+	if (riscv_debug_buffer_leave(t, p) != ERROR_OK) {
+		LOG_ERROR("unable to write program buffer exit code");
+		return ERROR_FAIL;
+	}
+
+	riscv_reg_t saved_registers[GDB_REGNO_XPR31 + 1];
+	for (size_t i = GDB_REGNO_XPR0 + 1; i <= GDB_REGNO_XPR31; ++i) {
+		if (p->writes_xreg[i]) {
+			LOG_DEBUG("Saving register %d as used by program", (int)i);
+			saved_registers[i] = riscv_get_register(t, i);
+		}
+	}
+
+	if (p->writes_memory && (riscv_program_fence(p) != ERROR_OK)) {
+		LOG_ERROR("Unable to write fence");
+		for(size_t i = 0; i < riscv_debug_buffer_size(p->target); ++i)
+			LOG_ERROR("ram[%02x]: DASM(0x%08lx) [0x%08lx]", (int)i, (long)p->debug_buffer[i], (long)p->debug_buffer[i]);
+		abort();
+		return ERROR_FAIL;
+	}
+
+	if (riscv_program_ebreak(p) != ERROR_OK) {
+		LOG_ERROR("Unable to write ebreak");
+		for(size_t i = 0; i < riscv_debug_buffer_size(p->target); ++i)
+			LOG_ERROR("ram[%02x]: DASM(0x%08lx) [0x%08lx]", (int)i, (long)p->debug_buffer[i], (long)p->debug_buffer[i]);
+		abort();
+		return ERROR_FAIL;
+	}
+
+	for (size_t i = 0; i < riscv_debug_buffer_size(p->target); ++i) {
+		LOG_DEBUG("Executing program 0x%p: debug_buffer[%02x] = DASM(0x%08lx)", p, (int)i, (long)p->debug_buffer[i]);
+		if (i <= p->instruction_count || i >= riscv_debug_buffer_size(p->target) - p->data_count)
+			riscv_write_debug_buffer(t, i, p->debug_buffer[i]);
+	}
+
+	if (riscv_execute_debug_buffer(t) != ERROR_OK) {
+		LOG_DEBUG("Unable to execute program 0x%p", p);
+		return ERROR_FAIL;
+	}
+
+	for (size_t i = 0; i < riscv_debug_buffer_size(p->target); ++i)
+		if (i >= riscv_debug_buffer_size(p->target) - p->data_count)
+			p->debug_buffer[i] = riscv_read_debug_buffer(t, i);
+
+	for (size_t i = GDB_REGNO_XPR0; i <= GDB_REGNO_XPR31; ++i)
+		if (p->writes_xreg[i])
+			riscv_set_register(t, i, saved_registers[i]);
+
+	return ERROR_OK;
+}
+
+riscv_addr_t riscv_program_alloc_data(struct riscv_program *p, size_t bytes)
+{
+	LOG_DEBUG("allocating %d bytes of data", (int)bytes);
+
+	riscv_addr_t addr = 
+		riscv_debug_buffer_addr(p->target) 
+		+ riscv_debug_buffer_size(p->target) * sizeof(p->debug_buffer[0])
+		- p->data_count * sizeof(p->debug_buffer[0])
+		- bytes;
+	while (addr % bytes != 0) addr--;
+
+	riscv_addr_t ptop =
+		riscv_debug_buffer_addr(p->target)
+		+ p->instruction_count * sizeof(p->debug_buffer[0]);
+
+	if (addr <= ptop) {
+		LOG_DEBUG("unable to allocate %d bytes", (int)bytes);
+		return RISCV_PROGRAM_ALLOC_FAIL;
+	}
+
+	LOG_DEBUG("allocated %d bytes at 0x%08lx", (int)bytes, (long)addr);
+	p->data_count = 
+		+ riscv_debug_buffer_size(p->target)
+		- (addr - riscv_debug_buffer_addr(p->target)) / sizeof(p->debug_buffer[0]);
+	return addr;
+}
+
+riscv_addr_t riscv_program_alloc_x(struct riscv_program *p)
+{
+	return riscv_program_alloc_data(p, p->target_xlen / 8);
+}
+
+riscv_addr_t riscv_program_alloc_d(struct riscv_program *p)
+{
+	return riscv_program_alloc_data(p, 8);
+}
+
+riscv_addr_t riscv_program_alloc_w(struct riscv_program *p)
+{
+	return riscv_program_alloc_data(p, 4);
+}
+
+riscv_addr_t riscv_program_alloc_h(struct riscv_program *p)
+{
+	return riscv_program_alloc_data(p, 2);
+}
+
+riscv_addr_t riscv_program_alloc_b(struct riscv_program *p)
+{
+	return riscv_program_alloc_data(p, 1);
+}
+
+riscv_insn_t riscv_program_read_ram(struct riscv_program *p, riscv_addr_t addr)
+{
+	if (addr < riscv_debug_buffer_addr(p->target))
+		return -1;
+	if ((size_t)addr > riscv_debug_buffer_addr(p->target) + (riscv_debug_buffer_size(p->target) * sizeof(p->debug_buffer[0])))
+		return -1;
+
+	int off = (addr - riscv_debug_buffer_addr(p->target)) / sizeof(p->debug_buffer[0]);
+	return p->debug_buffer[off];
+}
+
+void riscv_program_write_ram(struct riscv_program *p, riscv_addr_t addr, uint64_t d)
+{
+	if (addr < riscv_debug_buffer_addr(p->target))
+		return;
+	if ((size_t)addr > riscv_debug_buffer_addr(p->target) + (riscv_debug_buffer_size(p->target) * sizeof(p->debug_buffer[0])))
+		return;
+
+	int off = (addr - riscv_debug_buffer_addr(p->target)) / sizeof(p->debug_buffer[0]);
+	p->debug_buffer[off] = d;
+}
+
+int riscv_program_swr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno b, int offset)
+{
+	p->writes_memory = 1;
+	return riscv_program_insert(p, sw(d, b, offset));
+}
+
+int riscv_program_shr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno b, int offset)
+{
+	p->writes_memory = 1;
+	return riscv_program_insert(p, sh(d, b, offset));
+}
+
+int riscv_program_sbr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno b, int offset)
+{
+	p->writes_memory = 1;
+	return riscv_program_insert(p, sb(d, b, offset));
+}
+
+int riscv_program_lwr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno b, int offset)
+{
+	p->writes_memory = 1;
+	return riscv_program_insert(p, lw(d, b, offset));
+}
+
+int riscv_program_lhr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno b, int offset)
+{
+	p->writes_memory = 1;
+	return riscv_program_insert(p, lh(d, b, offset));
+}
+
+int riscv_program_lbr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno b, int offset)
+{
+	p->writes_memory = 1;
+	return riscv_program_insert(p, lb(d, b, offset));
+}
+
+int riscv_program_lx(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
+{
+	switch (p->target_xlen) {
+	case 64:  return riscv_program_ld(p, d, addr);
+	case 32:  return riscv_program_lw(p, d, addr);
+	}
+
+	LOG_ERROR("unknown xlen %d", p->target_xlen);
+	abort();
+	return -1;
+}
+
+int riscv_program_ld(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
+{
+	enum gdb_regno t = riscv_program_gah(p, addr) == 0 ? GDB_REGNO_X0 : d;
+	if (riscv_program_lah(p, d, addr) != ERROR_OK)
+		return ERROR_FAIL;
+	if (riscv_program_insert(p, ld(d, t, riscv_program_gal(p, addr))) != ERROR_OK)
+		return ERROR_FAIL;
+	return ERROR_OK;
+}
+
+int riscv_program_lw(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
+{
+	enum gdb_regno t = riscv_program_gah(p, addr) == 0 ? GDB_REGNO_X0 : d;
+	if (riscv_program_lah(p, d, addr) != ERROR_OK)
+		return ERROR_FAIL;
+	if (riscv_program_insert(p, lw(d, t, riscv_program_gal(p, addr))) != ERROR_OK)
+		return ERROR_FAIL;
+	return ERROR_OK;
+}
+
+int riscv_program_lh(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
+{
+	enum gdb_regno t = riscv_program_gah(p, addr) == 0 ? GDB_REGNO_X0 : d;
+	if (riscv_program_lah(p, d, addr) != ERROR_OK)
+		return ERROR_FAIL;
+	if (riscv_program_insert(p, lh(d, t, riscv_program_gal(p, addr))) != ERROR_OK)
+		return ERROR_FAIL;
+	return ERROR_OK;
+}
+
+int riscv_program_lb(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
+{
+	enum gdb_regno t = riscv_program_gah(p, addr) == 0 ? GDB_REGNO_X0 : d;
+	if (riscv_program_lah(p, t, addr) != ERROR_OK)
+		return ERROR_FAIL;
+	if (riscv_program_insert(p, lb(d, t, riscv_program_gal(p, addr))) != ERROR_OK)
+		return ERROR_FAIL;
+	return ERROR_OK;
+}
+
+int riscv_program_sx(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
+{
+	switch (p->target_xlen) {
+	case 64:  return riscv_program_sd(p, d, addr);
+	case 32:  return riscv_program_sw(p, d, addr);
+	}
+
+	LOG_ERROR("unknown xlen %d", p->target_xlen);
+	abort();
+	return -1;
+}
+
+int riscv_program_sd(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
+{
+	enum gdb_regno t = riscv_program_gah(p, addr) == 0
+		? GDB_REGNO_X0
+		: riscv_program_gettemp(p);
+	if (riscv_program_lah(p, t, addr) != ERROR_OK)
+		return ERROR_FAIL;
+	if (riscv_program_insert(p, sd(d, t, riscv_program_gal(p, addr))) != ERROR_OK)
+		return ERROR_FAIL;
+	riscv_program_puttemp(p, t);
+	p->writes_memory = true;
+	return ERROR_OK;
+}
+
+int riscv_program_sw(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
+{
+	enum gdb_regno t = riscv_program_gah(p, addr) == 0
+		? GDB_REGNO_X0
+		: riscv_program_gettemp(p);
+	if (riscv_program_lah(p, t, addr) != ERROR_OK)
+		return ERROR_FAIL;
+	if (riscv_program_insert(p, sw(d, t, riscv_program_gal(p, addr))) != ERROR_OK)
+		return ERROR_FAIL;
+	riscv_program_puttemp(p, t);
+	p->writes_memory = true;
+	return ERROR_OK;
+}
+
+int riscv_program_sh(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
+{
+	enum gdb_regno t = riscv_program_gah(p, addr) == 0
+		? GDB_REGNO_X0
+		: riscv_program_gettemp(p);
+	if (riscv_program_lah(p, t, addr) != ERROR_OK)
+		return ERROR_FAIL;
+	if (riscv_program_insert(p, sh(d, t, riscv_program_gal(p, addr))) != ERROR_OK)
+		return ERROR_FAIL;
+	riscv_program_puttemp(p, t);
+	p->writes_memory = true;
+	return ERROR_OK;
+}
+
+int riscv_program_sb(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
+{
+	enum gdb_regno t = riscv_program_gah(p, addr) == 0
+		? GDB_REGNO_X0
+		: riscv_program_gettemp(p);
+	if (riscv_program_lah(p, t, addr) != ERROR_OK)
+		return ERROR_FAIL;
+	if (riscv_program_insert(p, sb(d, t, riscv_program_gal(p, addr))) != ERROR_OK)
+		return ERROR_FAIL;
+	riscv_program_puttemp(p, t);
+	p->writes_memory = true;
+	return ERROR_OK;
+}
+
+int riscv_program_csrr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno csr)
+{
+	assert(csr >= GDB_REGNO_CSR0);
+	return riscv_program_insert(p, csrrs(d, GDB_REGNO_X0, csr - GDB_REGNO_CSR0));
+}
+
+int riscv_program_csrw(struct riscv_program *p, enum gdb_regno s, enum gdb_regno csr)
+{
+	assert(csr >= GDB_REGNO_CSR0);
+	return riscv_program_insert(p, csrrw(GDB_REGNO_X0, s, csr - GDB_REGNO_CSR0));
+}
+
+int riscv_program_csrrw(struct riscv_program *p, enum gdb_regno d, enum gdb_regno s, enum gdb_regno csr)
+{
+	assert(csr >= GDB_REGNO_CSR0);
+	return riscv_program_insert(p, csrrw(d, s, csr - GDB_REGNO_CSR0));
+}
+
+int riscv_program_fence_i(struct riscv_program *p)
+{
+	return riscv_program_insert(p, fence_i());
+}
+
+int riscv_program_fence(struct riscv_program *p)
+{
+	return riscv_program_insert(p, fence());
+}
+
+int riscv_program_ebreak(struct riscv_program *p)
+{
+	return riscv_program_insert(p, ebreak());
+}
+
+int riscv_program_lui(struct riscv_program *p, enum gdb_regno d, int32_t u)
+{
+	return riscv_program_insert(p, lui(d, u));
+}
+
+int riscv_program_addi(struct riscv_program *p, enum gdb_regno d, enum gdb_regno s, int16_t u)
+{
+	return riscv_program_insert(p, addi(d, s, u));
+}
+
+int riscv_program_fsd(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
+{
+	assert(d >= GDB_REGNO_FPR0);
+	assert(d <= GDB_REGNO_FPR31);
+	enum gdb_regno t = riscv_program_gah(p, addr) == 0
+		? GDB_REGNO_X0
+		: riscv_program_gettemp(p);
+	if (riscv_program_lah(p, t, addr) != ERROR_OK)
+		return ERROR_FAIL;
+	if (riscv_program_insert(p, fsd(d - GDB_REGNO_FPR0, t, riscv_program_gal(p, addr))) != ERROR_OK)
+		return ERROR_FAIL;
+	riscv_program_puttemp(p, t);
+	p->writes_memory = true;
+	return ERROR_OK;
+}
+
+int riscv_program_fld(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
+{
+	assert(d >= GDB_REGNO_FPR0);
+	assert(d <= GDB_REGNO_FPR31);
+	enum gdb_regno t = riscv_program_gah(p, addr) == 0 ? GDB_REGNO_X0 : d;
+	if (riscv_program_lah(p, t, addr) != ERROR_OK)
+		return ERROR_FAIL;
+	if (riscv_program_insert(p, fld(d - GDB_REGNO_FPR0, t, riscv_program_gal(p, addr))) != ERROR_OK)
+		return ERROR_FAIL;
+	return ERROR_OK;
+}
+
+int riscv_program_li(struct riscv_program *p, enum gdb_regno d, riscv_reg_t c)
+{
+	if (riscv_program_lui(p, d, c >> 12) != ERROR_OK)
+		return ERROR_FAIL;
+	if (riscv_program_addi(p, d, d, c & 0xFFF) != ERROR_OK)
+		return ERROR_FAIL;
+	return ERROR_OK;
+}
+
+int riscv_program_dont_restore_register(struct riscv_program *p, enum gdb_regno r)
+{
+	assert(r < RISCV_REGISTER_COUNT);
+	p->writes_xreg[r] = 0;
+	return ERROR_OK;
+}
+
+int riscv_program_do_restore_register(struct riscv_program *p, enum gdb_regno r)
+{
+	assert(r < RISCV_REGISTER_COUNT);
+	p->writes_xreg[r] = 1;
+	return ERROR_OK;
+}
+
+void riscv_program_reserve_register(struct riscv_program *p, enum gdb_regno r)
+{
+	assert(r < RISCV_REGISTER_COUNT);
+	assert(p->in_use[r] == 0);
+	p->in_use[r] = 1;
+}
+
+enum gdb_regno riscv_program_gettemp(struct riscv_program *p)
+{
+	for (size_t i = GDB_REGNO_S0; i <= GDB_REGNO_XPR31; ++i) {
+		if (p->in_use[i]) continue;
+
+		riscv_program_do_restore_register(p, i);
+		p->in_use[i] = 1;
+		return i;
+	}
+
+	LOG_ERROR("You've run out of temporary registers.  This is impossible.");
+	abort();
+}
+
+void riscv_program_puttemp(struct riscv_program *p, enum gdb_regno r)
+{
+	assert(r < RISCV_REGISTER_COUNT);
+	p->in_use[r] = 0;
+}
+
+/* Helper functions. */
+riscv_addr_t riscv_program_gah(struct riscv_program *p, riscv_addr_t addr)
+{
+	return addr >> 12;
+}
+
+riscv_addr_t riscv_program_gal(struct riscv_program *p, riscv_addr_t addr)
+{
+	return ((addr > 0) ? 1 : 0) * (abs(addr) & 0x7FF);
+}
+
+int riscv_program_lah(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
+{
+	riscv_addr_t ah = riscv_program_gah(p, addr);
+	if (ah == 0)
+		return ERROR_OK;
+	return riscv_program_lui(p, d, ah);
+}
+
+int riscv_program_lal(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
+{
+	riscv_addr_t al = riscv_program_gal(p, addr);
+	if (al == 0)
+		return ERROR_OK;
+	return riscv_program_addi(p, d, d, al);
+}
+
+int riscv_program_insert(struct riscv_program *p, riscv_insn_t i)
+{
+	LOG_DEBUG("instruction_count: %d (p=0x%p)", (int)p->instruction_count, p);
+
+	if (p->instruction_count + p->data_count + 1 > riscv_debug_buffer_size(p->target)) {
+		LOG_DEBUG("Unable to insert instruction:");
+		LOG_DEBUG("  instruction_count=%d", (int)p->instruction_count);
+		LOG_DEBUG("  data_count       =%d", (int)p->data_count);
+		LOG_DEBUG("  buffer size      =%d", (int)riscv_debug_buffer_size(p->target));
+		return ERROR_FAIL;
+	}
+
+	LOG_DEBUG("PROGBUF[%d] = DASM(0x%08x) [0x%08x]", (int)p->instruction_count, i, i);
+	p->debug_buffer[p->instruction_count] = i;
+	p->instruction_count++;
+	return ERROR_OK;
+}
diff --git a/src/target/riscv/program.h b/src/target/riscv/program.h
new file mode 100644
index 0000000..1efdb12
--- /dev/null
+++ b/src/target/riscv/program.h
@@ -0,0 +1,142 @@
+#ifndef TARGET__RISCV__PROGRAM_H
+#define TARGET__RISCV__PROGRAM_H
+
+#include "riscv.h"
+
+#define RISCV_MAX_DEBUG_BUFFER_SIZE 32
+#define RISCV_REGISTER_COUNT 32
+#define RISCV_DSCRATCH_COUNT 2
+
+/* The various RISC-V debug specifications all revolve around setting up
+ * program buffers and executing them on the target.  This structure contains a
+ * single program, which can then be executed on targets.  */
+struct riscv_program {
+	struct target *target;
+
+	uint32_t debug_buffer[RISCV_MAX_DEBUG_BUFFER_SIZE];
+
+	/* The debug buffer is allocated in two directions: instructions go at
+	 * the start, while data goes at the end.  When they meet in the middle
+	 * this blows up. */
+	size_t instruction_count;
+	size_t data_count;
+
+	/* Side effects of executing this program.  These must be accounted for
+	 * in order to maintain correct executing of the target system.  */
+	bool writes_xreg[RISCV_REGISTER_COUNT];
+	bool writes_memory;
+
+	/* When a register is used it will be set in this array. */
+	bool in_use[RISCV_REGISTER_COUNT];
+
+	/* Remembers the registers that have been saved into dscratch
+	 * registers.  These are restored */
+	enum gdb_regno dscratch_saved[RISCV_DSCRATCH_COUNT];
+
+	/* XLEN on the target. */
+	int target_xlen;
+};
+
+/* Initializes a program with the header. */
+int riscv_program_init(struct riscv_program *p, struct target *t);
+
+/* Executes a program, returning 0 if the program successfully executed.  Note
+ * that this may cause registers to be saved or restored, which could result to
+ * calls to things like riscv_save_register which itself could require a
+ * program to execute.  That's OK, just make sure this eventually terminates.
+ * */
+int riscv_program_exec(struct riscv_program *p, struct target *t);
+int riscv_program_load(struct riscv_program *p, struct target *t);
+
+/* Clears a program, removing all the state associated with it. */
+int riscv_program_clear(struct riscv_program *p, struct target *t);
+
+/* A lower level interface, you shouldn't use this unless you have a reason. */
+int riscv_program_insert(struct riscv_program *p, riscv_insn_t i);
+
+/* There is hardware support for saving at least one register.  This register
+ * doesn't need to be saved/restored the usual way, which is useful during
+ * early initialization when we can't save/restore arbitrary registerrs to host
+ * memory. */
+int riscv_program_save_to_dscratch(struct riscv_program *p, enum gdb_regno to_save);
+
+/* Allocates data of various sizes.  Either returns the absolute physical
+ * address or RISCV_PROGRAM_ALLOC_FAIL on failure. */
+riscv_addr_t riscv_program_alloc_data(struct riscv_program *p, size_t bytes);
+riscv_addr_t riscv_program_alloc_x(struct riscv_program *p);
+riscv_addr_t riscv_program_alloc_d(struct riscv_program *p);
+riscv_addr_t riscv_program_alloc_w(struct riscv_program *p);
+riscv_addr_t riscv_program_alloc_h(struct riscv_program *p);
+riscv_addr_t riscv_program_alloc_b(struct riscv_program *p);
+#define RISCV_PROGRAM_ALLOC_FAIL ((riscv_addr_t)(-1))
+
+/* Reads a word of memory from this program's internal view of the debug RAM.
+ * This is what you want to use to get data back from the program after it
+ * executes. */
+riscv_insn_t riscv_program_read_ram(struct riscv_program *p, riscv_addr_t addr);
+void riscv_program_write_ram(struct riscv_program *p, riscv_addr_t a, uint64_t d);
+
+/* Helpers to assembly various instructions.  Return 0 on success.  These might
+ * assembly into a multi-instruction sequence that overwrites some other
+ * register, but those will be properly saved and restored. */
+int riscv_program_lx(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr);
+int riscv_program_ld(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr);
+int riscv_program_lw(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr);
+int riscv_program_lh(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr);
+int riscv_program_lb(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr);
+
+int riscv_program_sx(struct riscv_program *p, enum gdb_regno s, riscv_addr_t addr);
+int riscv_program_sd(struct riscv_program *p, enum gdb_regno s, riscv_addr_t addr);
+int riscv_program_sw(struct riscv_program *p, enum gdb_regno s, riscv_addr_t addr);
+int riscv_program_sh(struct riscv_program *p, enum gdb_regno s, riscv_addr_t addr);
+int riscv_program_sb(struct riscv_program *p, enum gdb_regno s, riscv_addr_t addr);
+
+int riscv_program_lxr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno a, int o);
+int riscv_program_ldr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno a, int o);
+int riscv_program_lwr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno a, int o);
+int riscv_program_lhr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno a, int o);
+int riscv_program_lbr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno a, int o);
+
+int riscv_program_sxr(struct riscv_program *p, enum gdb_regno s, enum gdb_regno a, int o);
+int riscv_program_sdr(struct riscv_program *p, enum gdb_regno s, enum gdb_regno a, int o);
+int riscv_program_swr(struct riscv_program *p, enum gdb_regno s, enum gdb_regno a, int o);
+int riscv_program_shr(struct riscv_program *p, enum gdb_regno s, enum gdb_regno a, int o);
+int riscv_program_sbr(struct riscv_program *p, enum gdb_regno s, enum gdb_regno a, int o);
+
+int riscv_program_csrr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno csr);
+int riscv_program_csrw(struct riscv_program *p, enum gdb_regno s, enum gdb_regno csr);
+int riscv_program_csrrw(struct riscv_program *p, enum gdb_regno d, enum gdb_regno s, enum gdb_regno csr);
+
+int riscv_program_fence_i(struct riscv_program *p);
+int riscv_program_fence(struct riscv_program *p);
+int riscv_program_ebreak(struct riscv_program *p);
+
+int riscv_program_lui(struct riscv_program *p, enum gdb_regno d, int32_t u);
+int riscv_program_addi(struct riscv_program *p, enum gdb_regno d, enum gdb_regno s, int16_t i);
+
+int riscv_program_fsd(struct riscv_program *p, enum gdb_regno s, riscv_addr_t addr);
+int riscv_program_fld(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr);
+
+/* Assembler macros. */
+int riscv_program_li(struct riscv_program *p, enum gdb_regno d, riscv_reg_t c);
+int riscv_program_la(struct riscv_program *p, enum gdb_regno d, riscv_addr_t a);
+
+/* Register allocation.  The user is expected to have obtained temporary
+ * registers using these fuctions.  Additionally, there is an interface for
+ * reserving registers -- it's expected that this has been called as the first
+ * thing in the program's execution to reserve registers that can't be touched
+ * by the program's execution. */
+void riscv_program_reserve_register(struct riscv_program *p, enum gdb_regno r);
+enum gdb_regno riscv_program_gettemp(struct riscv_program *p);
+void riscv_program_puttemp(struct riscv_program *p, enum gdb_regno r);
+
+/* Executing a program usually causes the registers that get overwritten to be
+ * saved and restored.  Calling this prevents the given register from actually
+ * being restored as a result of all activity in this program. */
+int riscv_program_dont_restore_register(struct riscv_program *p, enum gdb_regno r);
+int riscv_program_do_restore_register(struct riscv_program *p, enum gdb_regno r);
+
+/* Addressing functions. */
+riscv_addr_t riscv_program_gah(struct riscv_program *p, riscv_addr_t addr);
+
+#endif
diff --git a/src/target/riscv/riscv-011.c b/src/target/riscv/riscv-011.c
index e940d20..10612e2 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)
@@ -1808,6 +1810,9 @@ static int examine(struct target *target)
 		return ERROR_FAIL;
 	}
 
+	RISCV_INFO(r);
+	r->hart_count = 1;
+
 	riscv011_info_t *info = get_info(target);
 	info->addrbits = get_field(dtmcontrol, DTMCONTROL_ADDRBITS);
 	info->dtmcontrol_idle = get_field(dtmcontrol, DTMCONTROL_IDLE);
@@ -1875,11 +1880,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 +1892,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 +1910,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 +2035,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 +2130,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 +2201,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 0957bdc..72e1e2c 100644
--- a/src/target/riscv/riscv-013.c
+++ b/src/target/riscv/riscv-013.c
@@ -21,6 +21,44 @@
 #include "helper/time_support.h"
 #include "riscv.h"
 #include "debug_defines.h"
+#include "rtos/rtos.h"
+#include "program.h"
+#include "asm.h"
+#include "batch.h"
+
+#define DMI_DATA1 (DMI_DATA0 + 1)
+
+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);
+static riscv_addr_t riscv013_progbuf_addr(struct target *target);
+static riscv_addr_t riscv013_progbuf_size(struct target *target);
+static riscv_addr_t riscv013_data_size(struct target *target);
+static riscv_addr_t riscv013_data_addr(struct target *target);
+static void riscv013_set_autoexec(struct target *target, int offset, bool enabled);
+static int riscv013_debug_buffer_register(struct target *target, riscv_addr_t addr);
+static void riscv013_clear_abstract_error(struct target *target);
+
+/* 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);
+static void riscv013_debug_buffer_enter(struct target *target, struct riscv_program *p);
+static void riscv013_debug_buffer_leave(struct target *target, struct riscv_program *p);
+static void riscv013_write_debug_buffer(struct target *target, int i, riscv_insn_t d);
+static riscv_insn_t riscv013_read_debug_buffer(struct target *target, int i);
+static int riscv013_execute_debug_buffer(struct target *target);
+static void riscv013_fill_dmi_write_u64(struct target *target, char *buf, int a, uint64_t d);
+static void riscv013_fill_dmi_read_u64(struct target *target, char *buf, int a);
+static int riscv013_dmi_write_u64_bits(struct target *target);
+static void riscv013_fill_dmi_nop_u64(struct target *target, char *buf);
 
 /**
  * Since almost everything can be accomplish by scanning the dbus register, all
@@ -40,6 +78,8 @@
 #define CSR_DCSR_CAUSE_STEP		4
 #define CSR_DCSR_CAUSE_HALT		5
 
+#define RISCV013_INFO(r) riscv013_info_t *r = get_info(target)
+
 /*** JTAG registers. ***/
 
 typedef enum {
@@ -78,27 +118,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 +144,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;
@@ -145,8 +162,6 @@ typedef struct {
 	// unique_id of the breakpoint/watchpoint that is using it.
 	int trigger_unique_id[MAX_HWBPS];
 
-	unsigned int trigger_count;
-
 	// Number of run-test/idle cycles the target requests we do after each dbus
 	// access.
 	unsigned int dtmcontrol_idle;
@@ -164,13 +179,10 @@ 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;
+	// Some memoized values
+	int progbuf_size, progbuf_addr, data_addr, data_size;
+} riscv013_info_t;
 
 static void dump_field(const struct scan_field *field)
 {
@@ -210,106 +222,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,19 +286,21 @@ static uint32_t dtmcontrol_scan(struct target *target, uint32_t out)
 	return in;
 }
 
-static void increase_ac_busy_delay(struct target *target)
+static void increase_dmi_busy_delay(struct target *target)
 {
 	riscv013_info_t *info = get_info(target);
-	info->ac_busy_delay += info->ac_busy_delay / 10 + 1;
+	info->dmi_busy_delay += info->dmi_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);
+
+	dtmcontrol_scan(target, DTM_DTMCS_DMIRESET);
 }
 
-static void increase_dmi_busy_delay(struct target *target)
+static void increase_ac_busy_delay(struct target *target)
 {
 	riscv013_info_t *info = get_info(target);
-	info->dmi_busy_delay += info->dmi_busy_delay / 10 + 1;
+	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);
@@ -421,7 +337,7 @@ static dmi_status_t dmi_scan(struct target *target, uint16_t *address_in,
 	/* Assume dbus is already selected. */
 	jtag_add_dr_scan(target->tap, 1, &field, TAP_IDLE);
 
-	int idle_count = info->dtmcontrol_idle + info->dmi_busy_delay;
+	int idle_count = info->dmi_busy_delay;
 	if (exec)
 		idle_count += info->ac_busy_delay;
 
@@ -450,27 +366,56 @@ 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;
 
 	unsigned i = 0;
+
+        // This first loop ensures that the read request was actually sent
+        // to the target. Note that if for some reason this stays busy,
+        // it is actually due to the Previous dmi_read or dmi_write.
 	for (i = 0; i < 256; i++) {
 		status = dmi_scan(target, NULL, NULL, DMI_OP_READ, address, 0,
 				false);
 		if (status == DMI_STATUS_BUSY) {
 			increase_dmi_busy_delay(target);
+		} else if (status == DMI_STATUS_SUCCESS) {
+			break;
 		} else {
+			LOG_ERROR("failed read from 0x%x, status=%d\n", address, status);
 			break;
 		}
 	}
 
-	status = dmi_scan(target, &address_in, &value, DMI_OP_NOP, address, 0,
-			false);
+        if (status != DMI_STATUS_SUCCESS) {
+                LOG_ERROR("Failed read from 0x%x; value=0x%" PRIx64 ", status=%d\n",
+                                address, value, status);
+                abort();
+        }
+
+         // This second loop ensures that we got the read
+         // data back. Note that NOP can result in a 'busy' result as well, but
+         // that would be noticed on the next DMI access we do.
+         for (i = 0; i < 256; i++) {
+           status = dmi_scan(target, &address_in, &value, DMI_OP_NOP, address, 0,
+                             false);
+           if (status == DMI_STATUS_BUSY) {
+             increase_dmi_busy_delay(target);
+           } else if (status == DMI_STATUS_SUCCESS) {
+             break;
+           } else {
+             LOG_ERROR("failed read (NOP) at 0x%x, status=%d\n", address, status);
+             break;
+           }
+        }
 
 	if (status != DMI_STATUS_SUCCESS) {
-		LOG_ERROR("failed read from 0x%x; value=0x%" PRIx64 ", status=%d\n",
+		LOG_ERROR("Failed read (NOP) from 0x%x; value=0x%" PRIx64 ", status=%d\n",
 				address, value, status);
+		abort();
 	}
 
 	return value;
@@ -478,33 +423,47 @@ 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) {
-		dmi_scan(target, NULL, NULL, DMI_OP_WRITE, address, value,
-				address == DMI_COMMAND);
-		status = dmi_scan(target, NULL, NULL, DMI_OP_NOP, 0, 0, false);
-		if (status == DMI_STATUS_BUSY) {
-			increase_dmi_busy_delay(target);
-		}
+
+         // The first loop ensures that we successfully sent the write request.
+        for (i = 0; i < 256; i++) {
+           status = dmi_scan(target, NULL, NULL, DMI_OP_WRITE, address, value,
+                             address == DMI_COMMAND);
+           if (status == DMI_STATUS_BUSY) {
+             increase_dmi_busy_delay(target);
+           } else if (status == DMI_STATUS_SUCCESS) {
+             break;
+           } else {
+             LOG_ERROR("failed write to 0x%x, status=%d\n", address, status);
+             break;
+           }
+        }
+
+         if (status != DMI_STATUS_SUCCESS) {
+           LOG_ERROR("Failed write to 0x%x;, status=%d\n",
+                     address, status);
+           abort();
+        }
+
+         // The second loop isn't strictly necessary, but would ensure that
+         // the write is complete/ has no non-busy errors before returning from this function.
+         for (i = 0; i < 256; i++) {
+           status = dmi_scan(target, NULL, NULL, DMI_OP_NOP, address, 0,
+                             false);
+           if (status == DMI_STATUS_BUSY) {
+             increase_dmi_busy_delay(target);
+           } else if (status == DMI_STATUS_SUCCESS) {
+             break;
+           } else {
+             LOG_ERROR("failed write (NOP) at 0x%x, status=%d\n", address, status);
+             break;
+           }
 	}
 	if (status != DMI_STATUS_SUCCESS) {
-		LOG_ERROR("failed to write 0x%" PRIx64 " to 0x%x; status=%d\n", value, address, status);
-	}
-}
-
-/** 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;
-	} else {
-		return ~0;
+		LOG_ERROR("failed to write (NOP) 0x%" PRIx64 " to 0x%x; status=%d\n", value, address, status);
+		abort();
 	}
 }
 
@@ -530,10 +489,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;
@@ -541,301 +518,82 @@ static int wait_for_idle(struct target *target, uint32_t *abstractcs)
 	}
 }
 
-static int execute_abstract_command(struct target *target, uint32_t command)
-{
-	dmi_write(target, DMI_COMMAND, command);
-
-	uint32_t abstractcs;
-	if (wait_for_idle(target, &abstractcs) != ERROR_OK)
-		return ERROR_FAIL;
+static int register_read_direct(struct target *target, uint64_t *value, uint32_t number);
 
-	if (get_field(abstractcs, DMI_ABSTRACTCS_CMDERR) != CMDERR_NONE) {
-		const char *errors[8] = {
-			"none",
-			"busy",
-			"not supported",
-			"exception",
-			"halt/resume",
-			"reserved",
-			"reserved",
-			"other" };
-		LOG_DEBUG("Abstract command 0x%x ended in error '%s' (abstractcs=0x%x)",
-				command, errors[get_field(abstractcs, DMI_ABSTRACTCS_CMDERR)],
-				abstractcs);
-		// Clear the error.
-		dmi_write(target, DMI_ABSTRACTCS, DMI_ABSTRACTCS_CMDERR);
-		return ERROR_FAIL;
-	}
-
-	return ERROR_OK;
-}
-
-/*** program "class" ***/
-/* This class allows a debug program to be built up piecemeal, and then be
- * executed. If necessary, the program is split up to fit in the program
- * buffer. */
-
-typedef struct {
-	uint8_t code[12 * 4];
-	unsigned length;
-	bool write;
-	unsigned regno;
-	uint64_t write_value;
-} program_t;
-
-static program_t *program_new(void)
-{
-	program_t *program = malloc(sizeof(program_t));
-	if (program) {
-		program->length = 0;
-		// Default to read zero.
-		program->write = false;
-		program->regno = 0x1000;
-	}
-	return program;
-}
-
-static void program_delete(program_t *program)
-{
-	free(program);
-}
-
-static void program_add32(program_t *program, uint32_t instruction)
-{
-	assert(program->length + 4 < sizeof(program->code));
-	program->code[program->length++] = instruction & 0xff;
-	program->code[program->length++] = (instruction >> 8) & 0xff;
-	program->code[program->length++] = (instruction >> 16) & 0xff;
-	program->code[program->length++] = (instruction >> 24) & 0xff;
-}
-
-static void program_set_read(program_t *program, unsigned reg_num)
-{
-	program->write = false;
-	program->regno = reg_number_to_no(reg_num);
-}
-
-static void program_set_write(program_t *program, unsigned reg_num, uint64_t value)
-{
-	program->write = true;
-	program->regno = reg_number_to_no(reg_num);
-	program->write_value = value;
-}
-
-/*** end of program class ***/
-
-static void write_program(struct target *target, const program_t *program)
-{
-	riscv013_info_t *info = get_info(target);
-
-	assert(program->length <= info->progsize * 4);
-	for (unsigned i = 0; i < program->length; i += 4) {
-		uint32_t value =
-			program->code[i] |
-			((uint32_t) program->code[i+1] << 8) |
-			((uint32_t) program->code[i+2] << 16) |
-			((uint32_t) program->code[i+3] << 24);
-		dmi_write(target, DMI_PROGBUF0 + i / 4, value);
-	}
-}
-
-static int execute_program(struct target *target, const program_t *program)
+static int register_write_direct(struct target *target, unsigned number,
+		uint64_t value)
 {
-	write_program(target, program);
-
-	uint32_t command = 0;
-	if (program->write) {
-		if (get_field(command, AC_ACCESS_REGISTER_SIZE) > 2) {
-			dmi_write(target, DMI_DATA1, program->write_value >> 32);
-		}
-		dmi_write(target, DMI_DATA0, program->write_value);
-		command |= AC_ACCESS_REGISTER_WRITE | AC_ACCESS_REGISTER_POSTEXEC;
+	struct riscv_program program;
+	riscv_program_init(&program, target);
+
+	riscv_addr_t input = riscv_program_alloc_d(&program);
+	riscv_program_write_ram(&program, input + 4, value >> 32);
+	riscv_program_write_ram(&program, input, value);
+
+	assert(GDB_REGNO_XPR0 == 0);
+	if (number <= GDB_REGNO_XPR31) {
+		riscv_program_lx(&program, number, input);
+	} else if (number >= GDB_REGNO_FPR0 && number <= GDB_REGNO_FPR31) {
+		riscv_program_fld(&program, number, input);
+	} else if (number >= GDB_REGNO_CSR0 && number <= GDB_REGNO_CSR4095) {
+		enum gdb_regno temp = riscv_program_gettemp(&program);
+		riscv_program_lx(&program, temp, input);
+		riscv_program_csrw(&program, temp, number);
 	} else {
-		command |= AC_ACCESS_REGISTER_PREEXEC;
+		LOG_ERROR("Unsupported register (enum gdb_regno)(%d)", number);
+		abort();
 	}
-	command |= abstract_register_size(xlen(target));
-	command |= program->regno;
-
-	return execute_abstract_command(target, command);
-}
-
-static int abstract_read_register(struct target *target,
-		uint64_t *value,
-		uint32_t reg_number, 
-		unsigned width)
-{
-	uint32_t command = abstract_register_size(width);
 
-	command |= reg_number_to_no(reg_number);
-
-	int result = execute_abstract_command(target, command);
-	if (result != ERROR_OK) {
-		return result;
+	int exec_out = riscv_program_exec(&program, target);
+	if (exec_out != ERROR_OK) {
+		LOG_ERROR("Unable to execute program");
+		return exec_out;
 	}
 
-	if (value) {
-		*value = 0;
-		switch (width) {
-			case 128:
-				LOG_ERROR("Ignoring top 64 bits from 128-bit register read.");
-			case 64:
-				*value |= ((uint64_t) dmi_read(target, DMI_DATA1)) << 32;
-			case 32:
-				*value |= dmi_read(target, DMI_DATA0);
-				break;
-		}
+	assert(GDB_REGNO_XPR0 == 0);
+	if (number <= GDB_REGNO_XPR31) {
+		uint64_t written_value;
+		register_read_direct(target, &written_value, number);
+		LOG_DEBUG("attempted to write 0x%016lx, actually wrote 0x%016lx", value, written_value);
+		assert(value == written_value);
 	}
 
 	return ERROR_OK;
 }
 
-static int abstract_write_register(struct target *target,
-		unsigned reg_number, 
-		unsigned width,
-		uint64_t value)
-{
-	uint32_t command = abstract_register_size(width);
-
-	command |= reg_number_to_no(reg_number);
-	command |= AC_ACCESS_REGISTER_WRITE;
-
-	switch (width) {
-		case 128:
-			LOG_ERROR("Ignoring top 64 bits from 128-bit register write.");
-		case 64:
-			dmi_write(target, DMI_DATA1, value >> 32);
-		case 32:
-			dmi_write(target, DMI_DATA0, value);
-			break;
-	}
-
-	int result = execute_abstract_command(target, command);
-	if (result != ERROR_OK) {
-		return result;
-	}
-
-	return ERROR_OK;
-}
-
-static int update_mstatus_actual(struct target *target)
-{
-	struct reg *mstatus_reg = &target->reg_cache->reg_list[REG_MSTATUS];
-	if (mstatus_reg->valid) {
-		// We previously made it valid.
-		return ERROR_OK;
-	}
-
-	LOG_DEBUG("Reading mstatus");
-
-	// Force reading the register. In that process mstatus_actual will be
-	// updated.
-	return register_get(&target->reg_cache->reg_list[REG_MSTATUS]);
-}
-
-static int register_write_direct(struct target *target, unsigned number,
-		uint64_t value)
-{
-	riscv013_info_t *info = get_info(target);
-	LOG_DEBUG("register 0x%x <- 0x%" PRIx64, number, value);
-
-	if (number == REG_MSTATUS) {
-		info->mstatus_actual = value;
-	}
-
-	int result = abstract_write_register(target, number, xlen(target), value);
-	if (result == ERROR_OK)
-		return result;
-
-	// Fall back to program buffer.
-	if (number >= REG_FPR0 && number <= REG_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, 
-					set_field(info->mstatus_actual, MSTATUS_FS, 1));
-			if (result != ERROR_OK)
-				return result;
-		}
-
-		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));
-		} else {
-			program_add32(program, fmv_s_x(number - REG_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) {
-		program_t *program = program_new();
-		program_add32(program, csrw(S0, number - REG_CSR0));
-		program_add32(program, ebreak());
-		program_set_write(program, S0, value);
-		result = execute_program(target, program);
-		program_delete(program);
-	} else {
-		return result;
-	}
-
-	return result;
-}
-
 /** Actually read registers from the target right now. */
 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));
-	if (result == ERROR_OK)
-		return result;
-
-	// Fall back to program buffer.
-	if (number >= REG_FPR0 && number <= REG_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,
-					set_field(info->mstatus_actual, MSTATUS_FS, 1));
-			if (result != ERROR_OK)
-				return result;
-		}
-		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));
-		} else {
-			program_add32(program, fmv_x_s(S0, number - REG_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) {
-		program_t *program = program_new();
-		program_add32(program, csrr(S0, number - REG_CSR0));
-		program_add32(program, ebreak());
-		program_set_read(program, S0);
-		result = execute_program(target, program);
-		program_delete(program);
+	struct riscv_program program;
+	riscv_program_init(&program, target);
+	riscv_addr_t output = riscv_program_alloc_d(&program);
+	riscv_program_write_ram(&program, output + 4, 0);
+	riscv_program_write_ram(&program, output, 0);
+
+	assert(GDB_REGNO_XPR0 == 0);
+	if (number <= GDB_REGNO_XPR31) {
+		riscv_program_sx(&program, number, output);
+	} else if (number >= GDB_REGNO_FPR0 && number <= GDB_REGNO_FPR31) {
+		riscv_program_fsd(&program, number, output);
+	} else if (number >= GDB_REGNO_CSR0 && number <= GDB_REGNO_CSR4095) {
+		enum gdb_regno temp = riscv_program_gettemp(&program);
+		riscv_program_csrr(&program, temp, number);
+		riscv_program_sx(&program, temp, output);
 	} else {
-		return result;
+		LOG_ERROR("Unsupported register (enum gdb_regno)(%d)", number);
+		abort();
 	}
 
-	if (result != ERROR_OK)
-		return result;
-
-	result = register_read_direct(target, value, S0);
-	if (result != ERROR_OK)
-		return result;
+	int exec_out = riscv_program_exec(&program, target);
+	if (exec_out != ERROR_OK) {
+		LOG_ERROR("Unable to execute program");
+		return exec_out;
+	}
 
+	*value = 0;
+	*value |= ((uint64_t)(riscv_program_read_ram(&program, output + 4))) << 32;
+	*value |= riscv_program_read_ram(&program, output);
 	LOG_DEBUG("register 0x%x = 0x%" PRIx64, number, *value);
-
 	return ERROR_OK;
 }
 
@@ -844,7 +602,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;
@@ -853,219 +611,20 @@ static int maybe_read_tselect(struct target *target)
 	return ERROR_OK;
 }
 
-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);
-		if (result != ERROR_OK)
-			return result;
-		info->tselect_dirty = true;
-	}
-
-	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->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)
 {
 	struct target *target = (struct target *) reg->arch_info;
-	riscv013_info_t *info = get_info(target);
-
-	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));
-		return ERROR_OK;
-	} else if (reg->number == REG_PC) {
-		buf_set_u32(reg->value, 0, 32, info->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));
-		return ERROR_OK;
-	} else {
-		uint64_t value;
-		int result = register_read_direct(target, &value, reg->number);
-		if (result != ERROR_OK) {
-			return result;
-		}
-		LOG_DEBUG("%s=0x%" PRIx64, reg->name, value);
-		buf_set_u64(reg->value, 0, xlen(target), value);
-
-		if (reg->number == REG_MSTATUS) {
-			info->mstatus_actual = value;
-			reg->valid = true;
-		}
-	}
-
+	uint64_t value = riscv_get_register(target, reg->number);
+	buf_set_u64(reg->value, 0, 64, value);
 	return ERROR_OK;
 }
 
 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);
-	} else {
-		return register_write_direct(target, number, value);
-	}
-
+	riscv_set_register(target, number, value);
 	return ERROR_OK;
 }
 
@@ -1073,7 +632,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];
@@ -1088,39 +647,59 @@ 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);
-
-	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->debug_buffer_enter = &riscv013_debug_buffer_enter;
+	generic_info->debug_buffer_leave = &riscv013_debug_buffer_leave;
+	generic_info->read_debug_buffer = &riscv013_read_debug_buffer;
+	generic_info->write_debug_buffer = &riscv013_write_debug_buffer;
+	generic_info->execute_debug_buffer = &riscv013_execute_debug_buffer;
+	generic_info->fill_dmi_write_u64 = &riscv013_fill_dmi_write_u64;
+	generic_info->fill_dmi_read_u64 = &riscv013_fill_dmi_read_u64;
+	generic_info->fill_dmi_nop_u64 = &riscv013_fill_dmi_nop_u64;
+	generic_info->dmi_write_u64_bits = &riscv013_dmi_write_u64_bits;
+
 	generic_info->version_specific = calloc(1, sizeof(riscv013_info_t));
 	if (!generic_info->version_specific)
 		return ERROR_FAIL;
 	riscv013_info_t *info = get_info(target);
 
+	info->progbuf_size = -1;
+	info->progbuf_addr = -1;
+	info->data_size = -1;
+	info->data_addr = -1;
+
+	info->dmi_busy_delay = 0;
+	info->ac_busy_delay = 0;
+
 	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;
@@ -1129,24 +708,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));
 
@@ -1164,20 +745,19 @@ static void deinit_target(struct target *target)
 static int add_trigger(struct target *target, struct trigger *trigger)
 {
 	riscv013_info_t *info = get_info(target);
-
 	maybe_read_tselect(target);
 
-	unsigned int i;
-	for (i = 0; i < info->trigger_count; i++) {
+	int i;
+	for (i = 0; i < riscv_count_triggers(target); i++) {
 		if (info->trigger_unique_id[i] != -1) {
 			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;
@@ -1189,7 +769,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);
@@ -1208,28 +788,28 @@ 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);
 		info->trigger_unique_id[i] = trigger->unique_id;
 		break;
 	}
-	if (i >= info->trigger_count) {
+	if (i >= riscv_count_triggers(target)) {
 		LOG_ERROR("Couldn't find an available hardware trigger.");
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 	}
@@ -1243,20 +823,20 @@ static int remove_trigger(struct target *target, struct trigger *trigger)
 
 	maybe_read_tselect(target);
 
-	unsigned int i;
-	for (i = 0; i < info->trigger_count; i++) {
+	int i;
+	for (i = 0; i < riscv_count_triggers(target); i++) {
 		if (info->trigger_unique_id[i] == trigger->unique_id) {
 			break;
 		}
 	}
-	if (i >= info->trigger_count) {
+	if (i >= riscv_count_triggers(target)) {
 		LOG_ERROR("Couldn't find the hardware resources used by hardware "
 				"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;
@@ -1319,7 +899,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;
@@ -1348,7 +927,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;
@@ -1389,73 +967,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.
@@ -1521,541 +1032,594 @@ 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;
-	}
-
-	for (unsigned i = 0; i < info->progsize; i++) {
-		dmi_write(target, DMI_PROGBUF0 + i, value);
-		value += 0x52534335;
-	}
-
-	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);
-		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))
+	/* Before doing anything else we must first enumerate the harts. */
+	RISCV_INFO(r);
+	if (riscv_rtos_enabled(target)) {
+		for (int i = 0; i < RISCV_MAX_HARTS; ++i) {
+			riscv_set_current_hartid(target, i);
+			uint32_t s = dmi_read(target, DMI_DMSTATUS);
+			if (get_field(s, DMI_DMSTATUS_ANYNONEXISTENT))
 				break;
+			r->hart_count = i + 1;
 		}
-		if (!get_field(dmstatus, DMI_DMSTATUS_ALLHALTED)) {
-			LOG_ERROR("hart didn't halt; dmstatus=0x%x", dmstatus);
-			return ERROR_FAIL;
-		}
+	} else {
+		r->hart_count = 1;
 	}
 
-	// TODO: do this using Quick Access, if supported.
+	LOG_DEBUG("Enumerated %d harts", r->hart_count);
 
-	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;
-	}
+	/* Halt every hart so we can probe them. */
+	riscv_halt_all_harts(target);
 
-	LOG_DEBUG("Discovered XLEN is %d", xlen(target));
+	/* Find the address of the program buffer, which must be done without
+	 * knowing anything about the target. */
+	for (int i = 0; i < riscv_count_harts(target); ++i) {
+		riscv_set_current_hartid(target, i);
 
-	// Update register list to match discovered XLEN.
-	update_reg_list(target);
+		/* Without knowing anything else we can at least mess with the
+		 * program buffer. */
+		r->debug_buffer_size[i] = riscv013_progbuf_size(target);
 
-	if (register_read_direct(target, &info->misa, REG_MISA) != ERROR_OK) {
-		LOG_ERROR("Failed to read misa.");
-		return ERROR_FAIL;
-	}
+		/* Guess this is a 32-bit system, we're probing it. */
+		r->xlen[i] = 32;
 
-	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);
-		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;
+		/* First find the low 32 bits of the program buffer.  This is
+		 * used to check for alignment. */
+		struct riscv_program program32;
+		riscv_program_init(&program32, target);
+		riscv_program_csrrw(&program32, GDB_REGNO_S0, GDB_REGNO_S0, GDB_REGNO_DSCRATCH);
+		riscv_program_insert(&program32, auipc(GDB_REGNO_S0));
+		riscv_program_insert(&program32, sw(GDB_REGNO_S0, GDB_REGNO_S0, -4));
+		riscv_program_csrrw(&program32, GDB_REGNO_S0, GDB_REGNO_S0, GDB_REGNO_DSCRATCH);
+		riscv_program_fence(&program32);
+		riscv_program_exec(&program32, target);
+
+		riscv_addr_t progbuf_addr = dmi_read(target, DMI_PROGBUF0) - 4;
+		if (get_field(dmi_read(target, DMI_ABSTRACTCS), DMI_ABSTRACTCS_CMDERR) != 0) {
+			LOG_ERROR("Unable to find the address of the program buffer on hart %d", i);
+			r->xlen[i] = -1;
+			continue;
 		}
-		if (!get_field(dmstatus, DMI_DMSTATUS_ALLRUNNING)) {
-			LOG_ERROR("hart didn't resume; dmstatus=0x%x", dmstatus);
-			return ERROR_FAIL;
+		r->debug_buffer_addr[i] = progbuf_addr;
+
+		/* Check to see if the core can execute 64 bit instructions.
+		 * In order to make this work we first need to */
+		int offset = (progbuf_addr % 8 == 0) ? -4 : 0;
+
+		struct riscv_program program64;
+		riscv_program_init(&program64, target);
+		riscv_program_csrrw(&program64, GDB_REGNO_S0, GDB_REGNO_S0, GDB_REGNO_DSCRATCH);
+		riscv_program_insert(&program64, auipc(GDB_REGNO_S0));
+		riscv_program_insert(&program64, sd(GDB_REGNO_S0, GDB_REGNO_S0, offset));
+		riscv_program_csrrw(&program64, GDB_REGNO_S0, GDB_REGNO_S0, GDB_REGNO_DSCRATCH);
+		riscv_program_fence(&program64);
+		riscv_program_exec(&program64, target);
+
+		if (get_field(dmi_read(target, DMI_ABSTRACTCS), DMI_ABSTRACTCS_CMDERR) == 0) {
+			r->debug_buffer_addr[i] =
+				(dmi_read(target, DMI_PROGBUF0 + (8 + offset) / 4) << 32)
+				+ dmi_read(target, DMI_PROGBUF0 + (4 + offset) / 4)
+				- 4;
+			r->xlen[i] = 64;
+		}
+
+		LOG_DEBUG("hart %d has XLEN=%d", i, r->xlen[i]);
+		LOG_DEBUG("found program buffer at 0x%08lx", (long)(r->debug_buffer_addr[i]));
+
+		if (riscv_program_gah(&program64, r->debug_buffer_addr[i])) {
+                	LOG_ERROR("This implementation will not work with hart %d with debug_buffer_addr of 0x%lx\n", i, 
+                            (long)r->debug_buffer_addr[i]);
+			abort();
+                }
+		
+		/* Check to see if we can use the data words as an extended
+		 * program buffer or not. */
+		if (r->debug_buffer_addr[i] + (4 * r->debug_buffer_size[i]) == riscv013_data_addr(target)) {
+			r->debug_buffer_size[i] += riscv013_data_size(target);
+			LOG_DEBUG("extending the debug buffer using data words, total size %d", r->debug_buffer_size[i]);
 		}
 	}
 
-	info->never_halted = true;
+	/* Then we check the number of triggers availiable to each hart. */
+	for (int i = 0; i < riscv_count_harts(target); ++i) {
+		for (uint32_t t = 0; t < RISCV_MAX_TRIGGERS; ++t) {
+			riscv_set_current_hartid(target, i);
 
-	int result = riscv013_poll(target);
-	if (result != ERROR_OK) {
-		return result;
+			r->trigger_count[i] = t;
+			register_write_direct(target, GDB_REGNO_TSELECT, t);
+			uint64_t tselect = t+1;
+			register_read_direct(target, &tselect, GDB_REGNO_TSELECT);
+			if (tselect != t)
+				break;
+		}
 	}
 
+	/* 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);
-
+        
+        // This print is used by some regression suites to know when
+        // they can connect with gdb/telnet.
+        // We will need to update those suites if we want to remove this line.
+        LOG_INFO("Examined RISC-V core");
 	return ERROR_OK;
 }
 
-static riscv_error_t handle_halt_routine(struct target *target)
+static int assert_reset(struct target *target)
 {
-	riscv013_info_t *info = get_info(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.
+  /*FIXME -- this only works for single-hart.*/
+  assert(!riscv_rtos_enabled(target));
+  RISCV_INFO(r);
+  assert(r->current_hartid == 0);
 
-	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);
-	}
+  select_dmi(target);
+  LOG_DEBUG("ASSERTING NDRESET");
+  uint32_t control = dmi_read(target, DMI_DMCONTROL);
+  control = set_field(control, DMI_DMCONTROL_NDMRESET, 1);
+  if (target->reset_halt) {
+    LOG_DEBUG("TARGET RESET HALT SET, ensuring halt is set during reset.");
+    control = set_field(control, DMI_DMCONTROL_HALTREQ, 1);
+  } else {
+    LOG_DEBUG("TARGET RESET HALT NOT SET");
+    control = set_field(control, DMI_DMCONTROL_HALTREQ, 0);
+  }
 
-	// 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);
+  dmi_write(target, DMI_DMCONTROL,
+	    control);
 
-	return RE_OK;
+  return ERROR_OK;
 }
 
-static int handle_halt(struct target *target, bool announce)
+static int deassert_reset(struct target *target)
 {
-	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;
-	}
+  RISCV_INFO(r);
+  RISCV013_INFO(info);
 
-	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;
+  select_dmi(target);
 
-		// 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.
+  /*FIXME -- this only works for Single Hart*/
+  assert(!riscv_rtos_enabled(target));
+  assert(r->current_hartid == 0);
 
-		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);
-			}
-		}
-	}
+  /*FIXME -- is there bookkeeping we need to do here*/
+  
+  uint32_t control = dmi_read(target, DMI_DMCONTROL);
 
-	if (announce) {
-		target_call_event_callbacks(target, TARGET_EVENT_HALTED);
-	}
+  // Clear the reset, but make sure haltreq is still set
+  if (target->reset_halt) {
+    control = set_field(control, DMI_DMCONTROL_HALTREQ, 1);
+  }
 
-	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]);
+  control = set_field(control, DMI_DMCONTROL_NDMRESET, 0);
+  dmi_write(target, DMI_DMCONTROL, control);
 
-	return ERROR_OK;
+  uint32_t status;
+  int dmi_busy_delay = info->dmi_busy_delay;
+  if (target->reset_halt) {
+    LOG_DEBUG("DEASSERTING RESET, waiting for hart to be halted.");
+    do {
+      status = dmi_read(target, DMI_DMSTATUS);
+    } while (get_field(status, DMI_DMSTATUS_ALLHALTED) == 0);
+  } else {
+    LOG_DEBUG("DEASSERTING RESET, waiting for hart to be running.");
+    do {
+      status = dmi_read(target, DMI_DMSTATUS);
+      if (get_field(status, DMI_DMSTATUS_ANYHALTED) ||
+	  get_field(status, DMI_DMSTATUS_ANYUNAVAIL)) {
+	LOG_ERROR("Unexpected hart status during reset.");
+	abort();
+      }
+    } while (get_field(status, DMI_DMSTATUS_ALLRUNNING) == 0);
+  }
+  info->dmi_busy_delay = dmi_busy_delay;
+  return ERROR_OK;
 }
 
-static int poll_target(struct target *target, bool announce)
+static int read_memory(struct target *target, uint32_t address,
+		uint32_t size, uint32_t count, uint8_t *buffer)
 {
-	select_dmi(target);
+	RISCV013_INFO(info);
 
-	// 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;
+	LOG_DEBUG("writing %d words of %d bytes to 0x%08lx", count, size, (long)address);
 
-	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;
+	select_dmi(target);
+	riscv_set_current_hartid(target, 0);
+
+	/* This program uses two temporary registers.  A word of data and the
+	 * associated address are stored at some location in memory.  The
+	 * program loads the word from that address and then increments the
+	 * address.  The debugger is expected to pull the memory word-by-word
+	 * from the chip with AUTOEXEC set in order to trigger program
+	 * execution on every word. */
+	uint64_t s0 = riscv_get_register(target, GDB_REGNO_S0);
+	uint64_t s1 = riscv_get_register(target, GDB_REGNO_S1);
+
+	struct riscv_program program;
+	riscv_program_init(&program, target);
+	riscv_addr_t r_data = riscv_program_alloc_w(&program);
+	riscv_addr_t r_addr = riscv_program_alloc_x(&program);
+	riscv_program_fence(&program);
+	riscv_program_lx(&program, GDB_REGNO_S0, r_addr);
+	riscv_program_addi(&program, GDB_REGNO_S0, GDB_REGNO_S0, size);
+	switch (size) {
+		case 1:
+			riscv_program_lbr(&program, GDB_REGNO_S1, GDB_REGNO_S0, 0);
+			break;
+		case 2:
+			riscv_program_lhr(&program, GDB_REGNO_S1, GDB_REGNO_S0, 0);
+			break;
+		case 4:
+			riscv_program_lwr(&program, GDB_REGNO_S1, GDB_REGNO_S0, 0);
+			break;
+		default:
+			LOG_ERROR("Unsupported size: %d", size);
+			return ERROR_FAIL;
+	}
+	riscv_program_sw(&program, GDB_REGNO_S1, r_data);
+	riscv_program_sx(&program, GDB_REGNO_S0, r_addr);
+
+#if 1
+	/* The first round through the program's execution we use the regular
+	 * program execution mechanism. */
+	switch (riscv_xlen(target)) {
+	case 64:
+		riscv_program_write_ram(&program, r_addr + 4, ((riscv_addr_t)(address - size)) >> 32);
+	case 32:
+		riscv_program_write_ram(&program, r_addr, (riscv_addr_t)(address - size));
+		break;
+	default:
+		LOG_ERROR("unknown XLEN %d", riscv_xlen(target));
+		return ERROR_FAIL;
 	}
 
-	if (get_field(dmstatus, DMI_DMSTATUS_ALLRUNNING)) {
-		target->state = TARGET_RUNNING;
-		return ERROR_OK;
+	if (riscv_program_exec(&program, target) != ERROR_OK) {
+		uint32_t acs = dmi_read(target, DMI_ABSTRACTCS);
+		LOG_ERROR("failed to execute program, abstractcs=0x%08x", acs);
+		riscv013_clear_abstract_error(target);
+		riscv_set_register(target, GDB_REGNO_S0, s0);
+		riscv_set_register(target, GDB_REGNO_S1, s1);
+		LOG_ERROR("  exiting with ERROR_FAIL");
+		return ERROR_FAIL;
 	}
 
-	if (get_field(dmstatus, DMI_DMSTATUS_ANYUNAVAIL)) {
-		target->state = TARGET_RESET;
-		return ERROR_OK;
+	uint32_t value = riscv_program_read_ram(&program, r_data);
+	LOG_DEBUG("M[0x%08lx] reads 0x%08lx", (long)address, (long)value);
+	switch (size) {
+	case 1:
+		buffer[0] = value;
+		break;
+	case 2:
+		buffer[0] = value;
+		buffer[1] = value >> 8;
+		break;
+	case 4:
+		buffer[0] = value;
+		buffer[1] = value >> 8;
+		buffer[2] = value >> 16;
+		buffer[3] = value >> 24;
+		break;
+	default:
+		LOG_ERROR("unsupported access size: %d", size);
+		return ERROR_FAIL;
+	}
+#else
+	/* The first round through the program's execution we use the regular
+	 * program execution mechanism. */
+	switch (riscv_xlen(target)) {
+	case 64:
+		riscv_program_write_ram(&program, r_addr + 4, ((riscv_addr_t)(address)) >> 32);
+	case 32:
+		riscv_program_write_ram(&program, r_addr, (riscv_addr_t)(address));
+		break;
+	default:
+		LOG_ERROR("unknown XLEN %d", riscv_xlen(target));
+		return ERROR_FAIL;
 	}
 
-	if (get_field(dmstatus, DMI_DMSTATUS_ANYNONEXISTENT)) {
-		LOG_ERROR("Hart disappeared!");
+	if (riscv_program_load(&program, target) != ERROR_OK) {
+		uint32_t acs = dmi_read(target, DMI_ABSTRACTCS);
+		LOG_ERROR("failed to execute program, abstractcs=0x%08x", acs);
+		riscv013_clear_abstract_error(target);
+		riscv_set_register(target, GDB_REGNO_S0, s0);
+		riscv_set_register(target, GDB_REGNO_S1, s1);
+		LOG_ERROR("  exiting with ERROR_FAIL");
 		return ERROR_FAIL;
 	}
+	uint32_t value;
+#endif
 
-	return ERROR_OK;
-}
+	/* The rest of this program is designed to be fast so it reads various
+	 * DMI registers directly. */
+	int d_data = (r_data - riscv_debug_buffer_addr(target)) / 4;
+	int d_addr = (r_addr - riscv_debug_buffer_addr(target)) / 4;
+
+	riscv013_set_autoexec(target, d_data, 1);
+
+	/* Copying memory might fail because we're going too quickly, in which
+	 * case we need to back off a bit and try again.  There's two
+	 * termination conditions to this loop: a non-BUSY error message, or
+	 * the data was all copied. */
+	riscv_addr_t cur_addr = 0xbadbeef;
+	riscv_addr_t fin_addr = address + (count * size);
+	riscv_addr_t prev_addr = 0;
+	LOG_DEBUG("writing until final address 0x%016lx", fin_addr);
+	while (count > 1 && (cur_addr = riscv_read_debug_buffer_x(target, d_addr)) < fin_addr) {
+		LOG_DEBUG("transferring burst starting at address 0x%016lx (previous burst was 0x%016lx)", cur_addr, prev_addr);
+		assert(prev_addr < cur_addr);
+		prev_addr = cur_addr;
+		riscv_addr_t start = (cur_addr - address) / size;
+                assert (cur_addr >= address);
+                struct riscv_batch *batch = riscv_batch_alloc(
+			target,
+			1024,
+			info->dmi_busy_delay + info->ac_busy_delay);
+
+#if 0
+		size_t reads = 1;
+		riscv_batch_add_dmi_read(batch, riscv013_debug_buffer_register(target, r_data));
+#else
+		size_t reads = 0;
+#endif
+		size_t rereads = reads;
+		for (riscv_addr_t i = start; i < count; ++i) {
+			size_t index = 
+				riscv_batch_add_dmi_read(
+					batch,
+					riscv013_debug_buffer_register(target, r_data));
+			assert(index == reads);
+			reads++;
+			if (riscv_batch_full(batch))
+				break;
+		}
 
-static int riscv013_poll(struct target *target)
-{
-	return poll_target(target, true);
-}
+		riscv_batch_run(batch);
 
-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);
+		for (size_t i = start; i < start + reads; ++i) {
+			riscv_addr_t offset = size*i;
+			riscv_addr_t t_addr = address + offset;
+			uint8_t *t_buffer = buffer + offset;
 
-	select_dmi(target);
+			uint64_t dmi_out = riscv_batch_get_dmi_read(batch, rereads);
+			value = get_field(dmi_out, DTM_DMI_DATA);
+			rereads++;
+
+			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:
+				LOG_ERROR("unsupported access size: %d", size);
+				return ERROR_FAIL;
+			}
 
-	if (!current) {
-		if (xlen(target) > 32) {
-			LOG_WARNING("Asked to resume at 32-bit PC on %d-bit target.",
-					xlen(target));
+			LOG_DEBUG("M[0x%08lx] reads 0x%08lx", (long)t_addr, (long)value);
 		}
-		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;
-	}
+		riscv_batch_free(batch);
 
-	return resume(target, debug_execution, false);
-}
+                // Note that if the scan resulted in a Busy DMI response, it
+                // is this read to abstractcs that will cause the dmi_busy_delay
+                // to be incremented if necessary. The loop condition above
+                // catches the case where no writes went through at all.
 
-static int assert_reset(struct target *target)
-{
-  select_dmi(target);
-  uint32_t control = DMI_DMCONTROL_DMACTIVE | DMI_DMCONTROL_NDMRESET;
-  if (target->reset_halt) {
-    LOG_DEBUG("TARGET RESET HALT SET, Requesting halt during reset.\n");
-    control |= DMI_DMCONTROL_HALTREQ;
-  }
-    
-  dmi_write(target, DMI_DMCONTROL,
-	    control);
+		uint32_t abstractcs = dmi_read(target, DMI_ABSTRACTCS);
+		switch (get_field(abstractcs, DMI_ABSTRACTCS_CMDERR)) {
+		case CMDERR_NONE:
+			LOG_DEBUG("successful (partial?) memory write");
+			break;
+		case CMDERR_BUSY:
+			LOG_DEBUG("memory write resulted in busy response");
+			riscv013_clear_abstract_error(target);
+			increase_ac_busy_delay(target);
+			break;
+		default:
+			LOG_ERROR("error when writing memory, abstractcs=0x%08lx", (long)abstractcs);
+			riscv013_set_autoexec(target, d_data, 0);
+			riscv_set_register(target, GDB_REGNO_S0, s0);
+			riscv_set_register(target, GDB_REGNO_S1, s1);
+			riscv013_clear_abstract_error(target);
+			return ERROR_FAIL;
+		}
+	}
 
-  if (!target->reset_halt) {
-    LOG_DEBUG("TARGET RESET HALT NOT SET, Requesting resume during reset.\n");
-    control = DMI_DMCONTROL_DMACTIVE | DMI_DMCONTROL_RESUMEREQ;
-    dmi_write(target, DMI_DMCONTROL, control);
-  }
-    
-  return ERROR_OK;
+	riscv013_set_autoexec(target, d_data, 0);
+	riscv_set_register(target, GDB_REGNO_S0, s0);
+	riscv_set_register(target, GDB_REGNO_S1, s1);
+	return ERROR_OK;
 }
 
-static int deassert_reset(struct target *target)
+static int write_memory(struct target *target, uint32_t address,
+		uint32_t size, uint32_t count, const uint8_t *buffer)
 {
-  select_dmi(target);
+	RISCV013_INFO(info);
 
-  // Note that we don't need to keep asserting
-  // haltreq since we already set it in assert_reset.
-  dmi_write(target, DMI_DMCONTROL, DMI_DMCONTROL_DMACTIVE);
-  
-  if (target->reset_halt) {
-    LOG_DEBUG("TARGET RESET HALT SET, waiting for hart to be halted.\n");
-    while (get_field(dmi_read(target, DMI_DMSTATUS), DMI_DMSTATUS_ALLHALTED) == 0) {
-    }
-    // This is necessary to re-read all the registers.
-    handle_halt(target, true);
-  } else {
-    LOG_DEBUG("TARGET RESET HALT NOT SET, waiting for hart to be running.\n");
-    while (get_field(dmi_read(target, DMI_DMSTATUS), DMI_DMSTATUS_ALLRUNNING) == 0) {
-    }
-  }
-  return ERROR_OK;
-}
+	LOG_DEBUG("writing %d words of %d bytes to 0x%08lx", count, size, (long)address);
 
-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);
+
+	/* This program uses two temporary registers.  A word of data and the
+	 * associated address are stored at some location in memory.  The
+	 * program stores the word to that address and then increments the
+	 * address.  The debugger is expected to feed the memory word-by-word
+	 * into the chip with AUTOEXEC set in order to trigger program
+	 * execution on every word. */
+	uint64_t s0 = riscv_get_register(target, GDB_REGNO_S0);
+	uint64_t s1 = riscv_get_register(target, GDB_REGNO_S1);
+
+	struct riscv_program program;
+	riscv_program_init(&program, target);
+	riscv_addr_t r_data = riscv_program_alloc_w(&program);
+	riscv_addr_t r_addr = riscv_program_alloc_x(&program);
+	riscv_program_fence(&program);
+	riscv_program_lx(&program, GDB_REGNO_S0, r_addr);
+	riscv_program_lw(&program, GDB_REGNO_S1, r_data);
+
+	switch (size) {
+		case 1:
+			riscv_program_sbr(&program, GDB_REGNO_S1, GDB_REGNO_S0, 0);
+			break;
+		case 2:
+			riscv_program_shr(&program, GDB_REGNO_S1, GDB_REGNO_S0, 0);
+			break;
+		case 4:
+			riscv_program_swr(&program, GDB_REGNO_S1, GDB_REGNO_S0, 0);
+			break;
+		default:
+			LOG_ERROR("Unsupported size: %d", size);
+			return ERROR_FAIL;
+	}
 
-        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;
-          }
-
-          // Set up autoexec s.t. each read of the the result that was in S1
-          // will start another run of reading the address pointed to by S0,
-          // copying it to S1, and storing S1 into Data 0.
-          if (count > 1) {
-            dmi_write(target, DMI_ABSTRACTAUTO, 0x1 << DMI_ABSTRACTAUTO_AUTOEXECDATA_OFFSET);
-          }
-          
-          uint32_t abstractcs;
-          for (uint32_t i = 0; i < count; i++) {
-
-            // On last iteration, turn off autoexec before reading the value
-            // so that we don't inadvertently read too far into memory.
-            if ((count > 1) && ((i + 1) == count)) {
-              dmi_write(target, DMI_ABSTRACTAUTO, 0);
-            }
-
-            uint32_t value = dmi_read(target, DMI_DATA0);
-            // If autoexec was set, the above dmi_read started an abstract command.
-            // If we just immediately loop and do another 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 ((count > 1) && ((i + 1) < count)) {
-              if (wait_for_idle(target, &abstractcs) != ERROR_OK) {
-                dmi_write(target, DMI_ABSTRACTAUTO, 0);
-                return ERROR_FAIL;
-              }
-            }
-            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;
-            }
-          }
-          
-          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("cmderr=%d", get_field(abstractcs, DMI_ABSTRACTCS_CMDERR));
-            return ERROR_FAIL;
-          } else {
-            return ERROR_OK;
-          }
-        }
-        // Should not get here.
-        assert(0);
-        return ERROR_OK;
-}
+	riscv_program_addi(&program, GDB_REGNO_S0, GDB_REGNO_S0, size);
+	riscv_program_sx(&program, GDB_REGNO_S0, r_addr);
 
-/**
- * If there was a DMI error, clear that error and return 1.
- * Otherwise return 0.
- */
-static int check_dmi_error(struct target *target)
-{
-	dmi_status_t status = dmi_scan(target, NULL, NULL, DMI_OP_NOP, 0, 0,
-			false);
-	if (status != DMI_STATUS_SUCCESS) {
-		// Clear errors.
-		dtmcontrol_scan(target, DTM_DTMCS_DMIRESET);
-		increase_dmi_busy_delay(target);
-		return 1;
+	/* The first round through the program's execution we use the regular
+	 * program execution mechanism. */
+	uint32_t value;
+	switch (size) {
+		case 1:
+			value = buffer[0];
+			break;
+		case 2:
+			value = buffer[0]
+				| ((uint32_t) buffer[1] << 8);
+			break;
+		case 4:
+			value = buffer[0]
+				| ((uint32_t) buffer[1] << 8)
+				| ((uint32_t) buffer[2] << 16)
+				| ((uint32_t) buffer[3] << 24);
+			break;
+		default:
+			LOG_ERROR("unsupported access size: %d", size);
+			return ERROR_FAIL;
 	}
-	return 0;
-}
 
-static int write_memory(struct target *target, uint32_t address,
-		uint32_t size, uint32_t count, const uint8_t *buffer)
-{
-	select_dmi(target);
+	switch (riscv_xlen(target)) {
+	case 64:
+		riscv_program_write_ram(&program, r_addr + 4, (uint64_t)address >> 32);
+	case 32:
+		riscv_program_write_ram(&program, r_addr, address);
+		break;
+	default:
+		LOG_ERROR("unknown XLEN %d", riscv_xlen(target));
+		return ERROR_FAIL;
+	}
+	riscv_program_write_ram(&program, r_data, value);
 
-	while (1) {
-		abstract_write_register(target, S0, xlen(target), address);
+	LOG_DEBUG("M[0x%08lx] writes 0x%08lx", (long)address, (long)value);
+
+	if (riscv_program_exec(&program, target) != ERROR_OK) {
+		uint32_t acs = dmi_read(target, DMI_ABSTRACTCS);
+		LOG_ERROR("failed to execute program, abstractcs=0x%08x", acs);
+		riscv013_clear_abstract_error(target);
+		riscv_set_register(target, GDB_REGNO_S0, s0);
+		riscv_set_register(target, GDB_REGNO_S1, s1);
+		LOG_ERROR("  exiting with ERROR_FAIL");
+		return ERROR_FAIL;
+	}
+
+	/* The rest of this program is designed to be fast so it reads various
+	 * DMI registers directly. */
+	int d_data = (r_data - riscv_debug_buffer_addr(target)) / 4;
+	int d_addr = (r_addr - riscv_debug_buffer_addr(target)) / 4;
+
+	riscv013_set_autoexec(target, d_data, 1);
+
+	/* Copying memory might fail because we're going too quickly, in which
+	 * case we need to back off a bit and try again.  There's two
+	 * termination conditions to this loop: a non-BUSY error message, or
+	 * the data was all copied. */
+	riscv_addr_t cur_addr = 0xbadbeef;
+	riscv_addr_t fin_addr = address + (count * size);
+	LOG_DEBUG("writing until final address 0x%016lx", fin_addr);
+	while ((cur_addr = riscv_read_debug_buffer_x(target, d_addr)) < fin_addr) {
+		LOG_DEBUG("transferring burst starting at address 0x%016lx", cur_addr);
+		riscv_addr_t start = (cur_addr - address) / size;
+                assert (cur_addr > address);
+                struct riscv_batch *batch = riscv_batch_alloc(
+			target,
+			1024,
+			info->dmi_busy_delay + info->ac_busy_delay);
+
+		for (riscv_addr_t i = start; i < count; ++i) {
+			riscv_addr_t offset = size*i;
+			riscv_addr_t t_addr = address + offset;
+			const uint8_t *t_buffer = buffer + offset;
 
-		program_t *program = program_new();
-		switch (size) {
-			case 1:
-				program_add32(program, sb(S1, S0, 0));
-				break;
-			case 2:
-				program_add32(program, sh(S1, S0, 0));
-				break;
-			case 4:
-				program_add32(program, sw(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);
-
-		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];
+					value = t_buffer[0];
 					break;
 				case 2:
-					value = buffer[2*i] | ((uint32_t) buffer[2*i+1] << 8);
+					value = t_buffer[0]
+						| ((uint32_t) t_buffer[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);
+					value = t_buffer[0]
+						| ((uint32_t) t_buffer[1] << 8)
+						| ((uint32_t) t_buffer[2] << 16)
+						| ((uint32_t) t_buffer[3] << 24);
 					break;
 				default:
+					LOG_ERROR("unsupported access size: %d", size);
 					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);
-			}
+
+			LOG_DEBUG("M[0x%08lx] writes 0x%08lx", (long)t_addr, (long)value);
+
+			riscv_batch_add_dmi_write(
+				batch,
+				riscv013_debug_buffer_register(target, r_data),
+				value);
+			if (riscv_batch_full(batch))
+				break;
 		}
-		int result = scans_execute(scans);
-		scans_delete(scans);
-		scans = NULL;
-		if (result != ERROR_OK)
-			return result;
 
-		int dmi_error = check_dmi_error(target);
+		riscv_batch_run(batch);
+		riscv_batch_free(batch);
+
+                // Note that if the scan resulted in a Busy DMI response, it
+                // is this read to abstractcs that will cause the dmi_busy_delay
+                // to be incremented if necessary. The loop condition above
+                // catches the case where no writes went through at all.
 
-		// 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);
+		switch (get_field(abstractcs, DMI_ABSTRACTCS_CMDERR)) {
+		case CMDERR_NONE:
+			LOG_DEBUG("successful (partial?) memory write");
+			break;
+		case CMDERR_BUSY:
+			LOG_DEBUG("memory write resulted in busy response");
+			riscv013_clear_abstract_error(target);
 			increase_ac_busy_delay(target);
-		} else if (cmderr) {
-			LOG_ERROR("cmderr=%d", get_field(abstractcs, DMI_ABSTRACTCS_CMDERR));
-			return ERROR_FAIL;
-		} else if (!dmi_error) {
 			break;
+		default:
+			LOG_ERROR("error when writing memory, abstractcs=0x%08lx", (long)abstractcs);
+			riscv013_set_autoexec(target, d_data, 0);
+			riscv013_clear_abstract_error(target);
+			riscv_set_register(target, GDB_REGNO_S0, s0);
+			riscv_set_register(target, GDB_REGNO_S1, s1);
+			return ERROR_FAIL;
 		}
 	}
 
+	riscv013_set_autoexec(target, d_data, 0);
+	riscv_set_register(target, GDB_REGNO_S0, s0);
+	riscv_set_register(target, GDB_REGNO_S1, s1);
 	return ERROR_OK;
 }
 
@@ -2072,12 +1636,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,
@@ -2093,3 +1655,365 @@ 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)
+{
+	LOG_DEBUG("reading register 0x%08x on hart %d", rid, hid);
+
+	riscv_set_current_hartid(target, hid);
+
+	uint64_t out;
+	riscv013_info_t *info = get_info(target);
+
+	if (rid <= GDB_REGNO_XPR31) {
+		register_read_direct(target, &out, rid);
+	} else if (rid == GDB_REGNO_PC) {
+		register_read_direct(target, &out, GDB_REGNO_DPC);
+		LOG_DEBUG("read PC from DPC: 0x%016lx", out);
+	} else if (rid == GDB_REGNO_PRIV) {
+		uint64_t dcsr;
+		register_read_direct(target, &dcsr, CSR_DCSR);
+		buf_set_u64((unsigned char *)&out, 0, 8, get_field(dcsr, CSR_DCSR_PRV));
+	} else {
+		int result = register_read_direct(target, &out, rid);
+		if (result != ERROR_OK) {
+			LOG_ERROR("Unable to read register %d", rid);
+			out = -1;
+		}
+
+		if (rid == GDB_REGNO_MSTATUS)
+			info->mstatus_actual = out;
+	}
+
+	return out;
+}
+
+static void riscv013_set_register(struct target *target, int hid, int rid, uint64_t value)
+{
+	LOG_DEBUG("writing register 0x%08x on hart %d", rid, hid);
+
+	riscv_set_current_hartid(target, hid);
+
+	if (rid <= GDB_REGNO_XPR31) {
+		register_write_direct(target, rid, value);
+	} else if (rid == GDB_REGNO_PC) {
+		LOG_DEBUG("writing PC to DPC: 0x%016lx", value);
+		register_write_direct(target, GDB_REGNO_DPC, value);
+		uint64_t actual_value;
+		register_read_direct(target, &actual_value, GDB_REGNO_DPC);
+		LOG_DEBUG("  actual DPC written: 0x%016lx", actual_value);
+		assert(value == actual_value);
+	} else if (rid == GDB_REGNO_PRIV) {
+		uint64_t dcsr;
+		register_read_direct(target, &dcsr, CSR_DCSR);
+		dcsr = set_field(dcsr, CSR_DCSR_PRV, value);
+		register_write_direct(target, CSR_DCSR, dcsr);
+	} else {
+		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)
+{
+}
+
+static bool riscv013_is_halted(struct target *target)
+{
+	uint32_t dmstatus = dmi_read(target, DMI_DMSTATUS);
+	if (get_field(dmstatus, DMI_DMSTATUS_ANYUNAVAIL))
+		LOG_ERROR("hart %d is unavailiable", riscv_current_hartid(target));
+	if (get_field(dmstatus, DMI_DMSTATUS_ANYNONEXISTENT))
+		LOG_ERROR("hart %d doesn't exist", riscv_current_hartid(target));
+	return get_field(dmstatus, DMI_DMSTATUS_ALLHALTED);
+}
+
+static enum riscv_halt_reason riscv013_halt_reason(struct target *target)
+{
+	uint64_t dcsr = riscv_get_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));
+	LOG_ERROR("  dcsr=0x%016lx", (long)dcsr);
+	abort();
+}
+
+void riscv013_debug_buffer_enter(struct target *target, struct riscv_program *program)
+{
+}
+
+void riscv013_debug_buffer_leave(struct target *target, struct riscv_program *program)
+{
+}
+
+void riscv013_write_debug_buffer(struct target *target, int index, riscv_insn_t data)
+{
+	if (index >= riscv013_progbuf_size(target))
+		return dmi_write(target, DMI_DATA0 + index - riscv013_progbuf_size(target), data);
+	return dmi_write(target, DMI_PROGBUF0 + index, data);
+}
+
+riscv_insn_t riscv013_read_debug_buffer(struct target *target, int index)
+{
+	if (index >= riscv013_progbuf_size(target))
+		return dmi_read(target, DMI_DATA0 + index - riscv013_progbuf_size(target));
+	return dmi_read(target, DMI_PROGBUF0 + index);
+}
+
+int riscv013_execute_debug_buffer(struct target *target)
+{
+	riscv013_clear_abstract_error(target);
+
+	uint32_t run_program = 0;
+	run_program = set_field(run_program, AC_ACCESS_REGISTER_SIZE, 2);
+	run_program = set_field(run_program, AC_ACCESS_REGISTER_POSTEXEC, 1);
+	run_program = set_field(run_program, AC_ACCESS_REGISTER_TRANSFER, 0);
+	run_program = set_field(run_program, AC_ACCESS_REGISTER_REGNO, 0x1000);
+	dmi_write(target, DMI_COMMAND, run_program);
+
+	{
+		uint32_t dmstatus = 0;
+		wait_for_idle(target, &dmstatus);
+	}
+
+	uint32_t cs = dmi_read(target, DMI_ABSTRACTCS);
+	if (get_field(cs, DMI_ABSTRACTCS_CMDERR) != 0) {
+		LOG_ERROR("unable to execute program: (abstractcs=0x%08x)", cs);
+		dmi_read(target, DMI_DMSTATUS);
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
+void riscv013_fill_dmi_write_u64(struct target *target, char *buf, int a, uint64_t d)
+{
+        RISCV013_INFO(info);
+        buf_set_u64((unsigned char *)buf, DTM_DMI_OP_OFFSET, DTM_DMI_OP_LENGTH, DMI_OP_WRITE);
+        buf_set_u64((unsigned char *)buf, DTM_DMI_DATA_OFFSET, DTM_DMI_DATA_LENGTH, d);
+        buf_set_u64((unsigned char *)buf, DTM_DMI_ADDRESS_OFFSET, info->abits, a);
+}
+
+void riscv013_fill_dmi_read_u64(struct target *target, char *buf, int a)
+{
+        RISCV013_INFO(info);
+        buf_set_u64((unsigned char *)buf, DTM_DMI_OP_OFFSET, DTM_DMI_OP_LENGTH, DMI_OP_READ);
+        buf_set_u64((unsigned char *)buf, DTM_DMI_DATA_OFFSET, DTM_DMI_DATA_LENGTH, 0);
+        buf_set_u64((unsigned char *)buf, DTM_DMI_ADDRESS_OFFSET, info->abits, a);
+}
+
+void riscv013_fill_dmi_nop_u64(struct target *target, char *buf)
+{
+        RISCV013_INFO(info);
+        buf_set_u64((unsigned char *)buf, DTM_DMI_OP_OFFSET, DTM_DMI_OP_LENGTH, DMI_OP_NOP);
+        buf_set_u64((unsigned char *)buf, DTM_DMI_DATA_OFFSET, DTM_DMI_DATA_LENGTH, 0);
+        buf_set_u64((unsigned char *)buf, DTM_DMI_ADDRESS_OFFSET, info->abits, 0);
+}
+
+int riscv013_dmi_write_u64_bits(struct target *target)
+{
+	RISCV013_INFO(info);
+	return info->abits + DTM_DMI_DATA_LENGTH + DTM_DMI_OP_LENGTH;
+}
+
+/* Helper Functions. */
+static void riscv013_on_step_or_resume(struct target *target, bool step)
+{
+	struct riscv_program program;
+	riscv_program_init(&program, target);
+	riscv_program_fence_i(&program);
+	if (riscv_program_exec(&program, target) != ERROR_OK)
+		LOG_ERROR("Unable to execute fence.i");
+
+	/* We want to twiddle some bits in the debug CSR so debugging works. */
+	uint64_t dcsr = riscv_get_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_set_register(target, GDB_REGNO_DCSR, dcsr);
+}
+
+static void riscv013_step_or_resume_current_hart(struct target *target, bool step)
+{
+	RISCV_INFO(r);
+	LOG_DEBUG("resuming hart %d (for step?=%d)", r->current_hartid, step);
+	assert(riscv_is_halted(target));
+
+	struct riscv_program program;
+	riscv_program_init(&program, target);
+	riscv_program_fence_i(&program);
+	if (riscv_program_exec(&program, target) != ERROR_OK)
+		abort();
+
+	/* 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);
+
+	for (size_t i = 0; i < 256; ++i) {
+		usleep(10);
+		uint32_t dmstatus = dmi_read(target, DMI_DMSTATUS);
+		if (get_field(dmstatus, DMI_DMSTATUS_ALLRESUMEACK) == 0)
+			continue;
+		if (step && get_field(dmstatus, DMI_DMSTATUS_ALLHALTED) == 0)
+			continue;
+
+		dmcontrol = set_field(dmcontrol, DMI_DMCONTROL_RESUMEREQ, 0);
+		dmi_write(target, DMI_DMCONTROL, dmcontrol);
+		return;
+	}
+
+	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);
+
+	if (step) {
+		LOG_ERROR("  was stepping, halting");
+		riscv013_halt_current_hart(target);
+		return;
+	}
+
+	abort();
+}
+
+riscv_addr_t riscv013_progbuf_addr(struct target *target)
+{
+	RISCV013_INFO(info);
+	assert(info->progbuf_addr != -1);
+	return info->progbuf_addr;
+}
+
+riscv_addr_t riscv013_progbuf_size(struct target *target)
+{
+	RISCV013_INFO(info);
+	if (info->progbuf_size == -1) {
+		uint32_t acs = dmi_read(target, DMI_ABSTRACTCS);
+		info->progbuf_size = get_field(acs, DMI_ABSTRACTCS_PROGSIZE);
+	}
+	return info->progbuf_size;
+}
+
+riscv_addr_t riscv013_data_size(struct target *target)
+{
+	RISCV013_INFO(info);
+	if (info->data_size == -1) {
+		uint32_t acs = dmi_read(target, DMI_HARTINFO);
+		info->data_size = get_field(acs, DMI_HARTINFO_DATASIZE);
+	}
+	return info->data_size;
+}
+
+riscv_addr_t riscv013_data_addr(struct target *target)
+{
+	RISCV013_INFO(info);
+	if (info->data_addr == -1) {
+		uint32_t acs = dmi_read(target, DMI_HARTINFO);
+		info->data_addr = get_field(acs, DMI_HARTINFO_DATAACCESS) ? get_field(acs, DMI_HARTINFO_DATAADDR) : 0;
+	}
+	return info->data_addr;
+}
+
+void riscv013_set_autoexec(struct target *target, int offset, bool enabled)
+{
+	if (offset >= riscv013_progbuf_size(target)) {
+		LOG_DEBUG("setting bit %d in AUTOEXECDATA to %d", offset, enabled);
+		uint32_t aa = dmi_read(target, DMI_ABSTRACTAUTO);
+		uint32_t aa_aed = get_field(aa, DMI_ABSTRACTAUTO_AUTOEXECDATA);
+		aa_aed &= ~(1 << (offset - riscv013_progbuf_size(target)));
+		aa_aed |= (enabled << (offset - riscv013_progbuf_size(target)));
+		aa = set_field(aa, DMI_ABSTRACTAUTO_AUTOEXECDATA, aa_aed);
+		dmi_write(target, DMI_ABSTRACTAUTO, aa);
+	} else {
+		LOG_DEBUG("setting bit %d in AUTOEXECPROGBUF to %d", offset, enabled);
+		uint32_t aa = dmi_read(target, DMI_ABSTRACTAUTO);
+		uint32_t aa_aed = get_field(aa, DMI_ABSTRACTAUTO_AUTOEXECPROGBUF);
+		aa_aed &= ~(1 << offset);
+		aa_aed |= (enabled << offset);
+		aa = set_field(aa, DMI_ABSTRACTAUTO_AUTOEXECPROGBUF, aa_aed);
+		dmi_write(target, DMI_ABSTRACTAUTO, aa);
+	}
+}
+
+int riscv013_debug_buffer_register(struct target *target, riscv_addr_t addr)
+{
+	if (addr >= riscv013_data_addr(target))
+		return DMI_DATA0 + (addr - riscv013_data_addr(target)) / 4;
+	else
+		return DMI_PROGBUF0 + (addr - riscv013_progbuf_addr(target)) / 4;
+}
+
+void riscv013_clear_abstract_error(struct target *target)
+{
+	uint32_t acs = dmi_read(target, DMI_ABSTRACTCS);
+	dmi_write(target, DMI_ABSTRACTCS, acs);
+}
diff --git a/src/target/riscv/riscv.c b/src/target/riscv/riscv.c
index ef6bca8..80859b6 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
@@ -333,7 +335,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);
@@ -379,7 +381,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:
@@ -398,6 +406,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];
 	}
 
@@ -477,7 +486,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);
@@ -494,12 +503,14 @@ static int riscv_run_algorithm(struct target *target, int num_mem_params,
 		int64_t now = timeval_ms();
 		if (now - start > timeout_ms) {
 			LOG_ERROR("Algorithm timed out after %d ms.", timeout_ms);
+			LOG_ERROR("  now   = 0x%08x", now);
+			LOG_ERROR("  start = 0x%08x", start);
 			riscv_halt(target);
-			riscv_poll(target);
+			riscv_openocd_poll(target);
 			return ERROR_TARGET_TIMEOUT;
 		}
 
-		int result = riscv_poll(target);
+		int result = riscv_openocd_poll(target);
 		if (result != ERROR_OK) {
 			return result;
 		}
@@ -517,12 +528,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;
 	}
@@ -530,7 +541,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;
 		}
@@ -575,7 +586,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,
@@ -602,3 +613,481 @@ 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);
+	riscv_set_current_hartid(target, hartid);
+
+	LOG_DEBUG("polling hart %d, target->state=%d (TARGET_HALTED=%d)", hartid, target->state, TARGET_HALTED);
+
+	/* If OpenOCD this we're running but this hart is halted then it's time
+	 * to raise an event. */
+	if (target->state != TARGET_HALTED && riscv_is_halted(target)) {
+		LOG_DEBUG("  triggered a halt");
+		r->on_halt(target);
+		return 1;
+	}
+
+	return 0;
+}
+
+/*** OpenOCD Interface ***/
+int riscv_openocd_poll(struct target *target)
+{
+	LOG_DEBUG("polling all harts");
+	int triggered_hart = -1;
+	if (riscv_rtos_enabled(target)) {
+		/* Check every hart for an event. */
+		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 just halted, target->state=%d", target->state);
+			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);
+	} else {
+		if (riscv_poll_hart(target, riscv_current_hartid(target)) == 0)
+			return ERROR_OK;
+
+		triggered_hart = riscv_current_hartid(target);
+		LOG_DEBUG("  hart %d halted", triggered_hart);
+	}
+
+	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;
+	}
+	
+	if (riscv_rtos_enabled(target)) {
+		target->rtos->current_threadid = triggered_hart + 1;
+		target->rtos->current_thread = triggered_hart + 1;
+	}
+
+	target->state = TARGET_HALTED;
+	target_call_event_callbacks(target, TARGET_EVENT_HALTED);
+	return ERROR_OK;
+}
+
+int riscv_openocd_halt(struct target *target)
+{
+	LOG_DEBUG("halting all harts");
+
+	int out = riscv_halt_all_harts(target);
+	if (out != ERROR_OK) {
+		LOG_ERROR("Unable to halt all harts");
+		return out;
+	}
+
+	register_cache_invalidate(target->reg_cache);
+	target->state = TARGET_HALTED;
+	target->debug_reason = DBG_REASON_DBGRQ;
+	target_call_event_callbacks(target, TARGET_EVENT_HALTED);
+	return out;
+}
+
+int riscv_openocd_resume(
+        struct target *target,
+        int current,
+        uint32_t address,
+        int handle_breakpoints,
+        int debug_execution
+) {
+	LOG_DEBUG("resuming all harts");
+
+	if (!current) {
+		riscv_set_register(target, GDB_REGNO_PC, address);
+	}
+
+	int out = riscv_resume_all_harts(target);
+	if (out != ERROR_OK) {
+		LOG_ERROR("unable to resume all harts");
+		return out;
+	}
+
+	register_cache_invalidate(target->reg_cache);
+	target->state = TARGET_RUNNING;
+	target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
+	return out;
+}
+
+int riscv_openocd_step(
+        struct target *target,
+        int current,
+        uint32_t address,
+        int handle_breakpoints
+) {
+	LOG_DEBUG("stepping rtos hart");
+
+	if (!current) {
+		riscv_set_register(target, GDB_REGNO_PC, address);
+	}
+
+	int out = riscv_step_rtos_hart(target);
+	if (out != ERROR_OK) {
+		LOG_ERROR("unable to step rtos hart");
+		return out;
+	}
+
+	register_cache_invalidate(target->reg_cache);
+	target->state = TARGET_RUNNING;
+	target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
+	target->state = TARGET_HALTED;
+	target->debug_reason = DBG_REASON_SINGLESTEP;
+	target_call_event_callbacks(target, TARGET_EVENT_HALTED);
+	return out;
+}
+
+/*** RISC-V Interface ***/
+
+void riscv_info_init(riscv_info_t *r)
+{
+	memset(r, 0, sizeof(*r));
+	r->dtm_version = 1;
+	r->registers_initialized = false;
+
+	for (size_t h = 0; h < RISCV_MAX_HARTS; ++h) {
+		r->xlen[h] = -1;
+		r->debug_buffer_addr[h] = -1;
+
+		for (size_t e = 0; e < RISCV_MAX_REGISTERS; ++e)
+			r->valid_saved_registers[h][e] = false;
+	}
+}
+
+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 (riscv_is_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 (!riscv_is_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);
+	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(riscv_is_halted(target));
+	r->on_step(target);
+	r->step_current_hart(target);
+	r->on_halt(target);
+	assert(riscv_is_halted(target));
+	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];
+}
+
+bool riscv_rtos_enabled(const struct target *target)
+{
+	return target->rtos != NULL;
+}
+
+void riscv_set_current_hartid(struct target *target, int hartid)
+{
+	RISCV_INFO(r);
+	r->current_hartid = hartid;
+	assert(riscv_rtos_enabled(target) || target->coreid == hartid);
+	int previous_hartid = riscv_current_hartid(target);
+	if (riscv_rtos_enabled(target))
+		r->select_current_hart(target);
+
+	/* This might get called during init, in which case we shouldn't be
+	 * setting up the register cache. */
+	if (!target_was_examined(target))
+		return;
+
+	/* Avoid invalidating the register cache all the time. */
+	if (r->registers_initialized && (!riscv_rtos_enabled(target) || (previous_hartid == hartid)) && target->reg_cache->reg_list[GDB_REGNO_XPR0].size == (long)riscv_xlen(target)) {
+		LOG_DEBUG("registers already initialized, skipping");
+		return;
+	} else
+		LOG_DEBUG("Initializing registers: xlen=%d", riscv_xlen(target));
+
+	/* Update the register list's widths. */
+	register_cache_invalidate(target->reg_cache);
+	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;
+		}
+	}
+
+	r->registers_initialized = true;
+}
+
+int riscv_current_hartid(const struct target *target)
+{
+	RISCV_INFO(r);
+	if (riscv_rtos_enabled(target))
+		return r->current_hartid;
+	else
+		return target->coreid;
+}
+
+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)
+{
+	LOG_DEBUG("setting RTOS hartid %d", hartid);
+	RISCV_INFO(r);
+	r->rtos_hartid = hartid;
+}
+
+int riscv_count_harts(struct target *target)
+{
+	if (target == NULL) return 1;
+	RISCV_INFO(r);
+	if (r == NULL) return 1;
+	return r->hart_count;
+}
+
+bool riscv_has_register(struct target *target, int hartid, int regid)
+{
+	return 1;
+}
+
+void riscv_set_register(struct target *target, enum gdb_regno r, riscv_reg_t v)
+{
+	return riscv_set_register_on_hart(target, riscv_current_hartid(target), r, v);
+}
+
+void riscv_set_register_on_hart(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);
+}
+
+riscv_reg_t riscv_get_register(struct target *target, enum gdb_regno r)
+{
+	return riscv_get_register_on_hart(target, riscv_current_hartid(target), r);
+}
+
+uint64_t riscv_get_register_on_hart(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);
+}
+
+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);
+}
+
+int riscv_count_triggers(struct target *target)
+{
+	return riscv_count_triggers_of_hart(target, riscv_current_hartid(target));
+}
+
+int riscv_count_triggers_of_hart(struct target *target, int hartid)
+{
+	RISCV_INFO(r);
+	assert(hartid < riscv_count_harts(target));
+	return r->trigger_count[hartid];
+}
+
+size_t riscv_debug_buffer_size(struct target *target)
+{
+	RISCV_INFO(r);
+	return r->debug_buffer_size[riscv_current_hartid(target)];
+}
+
+riscv_addr_t riscv_debug_buffer_addr(struct target *target)
+{
+	RISCV_INFO(r);
+	riscv_addr_t out = r->debug_buffer_addr[riscv_current_hartid(target)];
+	assert(out != -1);
+	return out;
+}
+
+int riscv_debug_buffer_enter(struct target *target, struct riscv_program *program)
+{
+	RISCV_INFO(r);
+	r->debug_buffer_enter(target, program);
+	return ERROR_OK;
+}
+
+int riscv_debug_buffer_leave(struct target *target, struct riscv_program *program)
+{
+	RISCV_INFO(r);
+	r->debug_buffer_leave(target, program);
+	return ERROR_OK;
+}
+
+int riscv_write_debug_buffer(struct target *target, int index, riscv_insn_t insn)
+{
+	RISCV_INFO(r);
+	r->write_debug_buffer(target, index, insn);
+	return ERROR_OK;
+}
+
+riscv_insn_t riscv_read_debug_buffer(struct target *target, int index)
+{
+	RISCV_INFO(r);
+	return r->read_debug_buffer(target, index);
+}
+
+riscv_addr_t riscv_read_debug_buffer_x(struct target *target, int index)
+{
+	riscv_addr_t out = 0;
+	switch (riscv_xlen(target)) {
+	case 64:
+		out |= (uint64_t)riscv_read_debug_buffer(target, index + 1) << 32;
+	case 32:
+		out |= riscv_read_debug_buffer(target, index + 0) <<  0;
+		break;
+	default:
+		LOG_ERROR("unsupported XLEN %d", riscv_xlen(target));
+		abort();
+	}
+	return out;
+}
+
+int riscv_execute_debug_buffer(struct target *target)
+{
+	RISCV_INFO(r);
+	return r->execute_debug_buffer(target);
+}
+
+void riscv_fill_dmi_write_u64(struct target *target, char *buf, int a, uint64_t d)
+{
+	RISCV_INFO(r);
+	r->fill_dmi_write_u64(target, buf, a, d);
+}
+
+void riscv_fill_dmi_read_u64(struct target *target, char *buf, int a)
+{
+	RISCV_INFO(r);
+	r->fill_dmi_read_u64(target, buf, a);
+}
+
+void riscv_fill_dmi_nop_u64(struct target *target, char *buf)
+{
+	RISCV_INFO(r);
+	r->fill_dmi_nop_u64(target, buf);
+}
+
+int riscv_dmi_write_u64_bits(struct target *target)
+{
+	RISCV_INFO(r);
+	return r->dmi_write_u64_bits(target);
+}
diff --git a/src/target/riscv/riscv.h b/src/target/riscv/riscv.h
index 2589b64..a38d04e 100644
--- a/src/target/riscv/riscv.h
+++ b/src/target/riscv/riscv.h
@@ -1,7 +1,16 @@
 #ifndef RISCV_H
 #define RISCV_H
 
+struct riscv_program;
+
+#include <stdint.h>
 #include "opcodes.h"
+#include "gdb_regs.h"
+
+/* The register cache is staticly allocated. */
+#define RISCV_MAX_HARTS 32
+#define RISCV_MAX_REGISTERS 5000
+#define RISCV_MAX_TRIGGERS 32
 
 extern struct target_type riscv011_target;
 extern struct target_type riscv013_target;
@@ -9,15 +18,87 @@ extern struct target_type riscv013_target;
 /*
  * Definitions shared by code supporting all RISC-V versions.
  */
+typedef uint64_t riscv_reg_t;
+typedef uint32_t riscv_insn_t;
+typedef  int64_t riscv_addr_t;
+
+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;
+
+	/* The number of harts on this system. */
+	int hart_count;
+
+	/* 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 number of triggers per hart. */
+	int trigger_count[RISCV_MAX_HARTS];
+
+	/* The address of the debug RAM buffer. */
+	riscv_addr_t debug_buffer_addr[RISCV_MAX_HARTS];
+
+	/* The number of entries in the debug buffer. */
+	int debug_buffer_size[RISCV_MAX_HARTS];
+
+	/* This avoids invalidating the register cache too often. */
+	bool registers_initialized;
+
+	/* 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);
+	void (*debug_buffer_enter)(struct target *target, struct riscv_program *program);
+	void (*debug_buffer_leave)(struct target *target, struct riscv_program *program);
+	void (*write_debug_buffer)(struct target *target, int i, riscv_insn_t d);
+	riscv_insn_t (*read_debug_buffer)(struct target *target, int i);
+	int (*execute_debug_buffer)(struct target *target);
+	int (*dmi_write_u64_bits)(struct target *target);
+	void (*fill_dmi_write_u64)(struct target *target, char *buf, int a, uint64_t d);
+	void (*fill_dmi_read_u64)(struct target *target, char *buf, int a);
+	void (*fill_dmi_nop_u64)(struct target *target, char *buf);
 } 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 +106,103 @@ 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);
+
+/* 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);
+
+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, enum gdb_regno i, riscv_reg_t v);
+void riscv_set_register_on_hart(struct target *target, int hid, enum gdb_regno rid, uint64_t v);
+riscv_reg_t riscv_get_register(struct target *target, enum gdb_regno i);
+riscv_reg_t riscv_get_register_on_hart(struct target *target, int hid, enum gdb_regno rid);
+
+/* Checks the state of the current hart -- "is_halted" checks the actual
+ * on-device register. */
+bool riscv_is_halted(struct target *target);
+enum riscv_halt_reason riscv_halt_reason(struct target *target, int hartid);
+
+/* Returns the number of triggers availiable to either the current hart or to
+ * the given hart. */
+int riscv_count_triggers(struct target *target);
+int riscv_count_triggers_of_hart(struct target *target, int hartid);
+
+/* These helper functions let the generic program interface get target-specific
+ * information. */
+size_t riscv_debug_buffer_size(struct target *target);
+riscv_addr_t riscv_debug_buffer_addr(struct target *target);
+
+int riscv_debug_buffer_enter(struct target *target, struct riscv_program *program);
+int riscv_debug_buffer_leave(struct target *target, struct riscv_program *program);
+
+riscv_insn_t riscv_read_debug_buffer(struct target *target, int index);
+riscv_addr_t riscv_read_debug_buffer_x(struct target *target, int index);
+int riscv_write_debug_buffer(struct target *target, int index, riscv_insn_t insn);
+int riscv_write_debug_buffer_x(struct target *target, int index, riscv_addr_t data);
+int riscv_execute_debug_buffer(struct target *target);
+
+void riscv_fill_dmi_nop_u64(struct target *target, char *buf);
+void riscv_fill_dmi_write_u64(struct target *target, char *buf, int a, uint64_t d);
+void riscv_fill_dmi_read_u64(struct target *target, char *buf, int a);
+int riscv_dmi_write_u64_bits(struct target *target);
 
 #endif
diff --git a/src/target/target.c b/src/target/target.c
index 42a8cac..7908a85 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -1088,7 +1088,7 @@ int target_add_breakpoint(struct target *target,
 		struct breakpoint *breakpoint)
 {
 	if ((target->state != TARGET_HALTED) && (breakpoint->type != BKPT_HARD)) {
-		LOG_WARNING("target %s is not halted", target_name(target));
+		LOG_WARNING("target %s is not halted (add breakpoint)", target_name(target));
 		return ERROR_TARGET_NOT_HALTED;
 	}
 	return target->type->add_breakpoint(target, breakpoint);
@@ -1098,7 +1098,7 @@ int target_add_context_breakpoint(struct target *target,
 		struct breakpoint *breakpoint)
 {
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target %s is not halted", target_name(target));
+		LOG_WARNING("target %s is not halted (add context breakpoint)", target_name(target));
 		return ERROR_TARGET_NOT_HALTED;
 	}
 	return target->type->add_context_breakpoint(target, breakpoint);
@@ -1108,7 +1108,7 @@ int target_add_hybrid_breakpoint(struct target *target,
 		struct breakpoint *breakpoint)
 {
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target %s is not halted", target_name(target));
+		LOG_WARNING("target %s is not halted (add hybrid breakpoint)", target_name(target));
 		return ERROR_TARGET_NOT_HALTED;
 	}
 	return target->type->add_hybrid_breakpoint(target, breakpoint);
@@ -1124,7 +1124,7 @@ int target_add_watchpoint(struct target *target,
 		struct watchpoint *watchpoint)
 {
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target %s is not halted", target_name(target));
+		LOG_WARNING("target %s is not halted (add watchpoint)", target_name(target));
 		return ERROR_TARGET_NOT_HALTED;
 	}
 	return target->type->add_watchpoint(target, watchpoint);
@@ -1138,7 +1138,7 @@ int target_hit_watchpoint(struct target *target,
 		struct watchpoint **hit_watchpoint)
 {
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target %s is not halted", target->cmd_name);
+		LOG_WARNING("target %s is not halted (hit watchpoint)", target->cmd_name);
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -1167,7 +1167,7 @@ int target_step(struct target *target,
 int target_get_gdb_fileio_info(struct target *target, struct gdb_fileio_info *fileio_info)
 {
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target %s is not halted", target->cmd_name);
+		LOG_WARNING("target %s is not halted (gdb fileio)", target->cmd_name);
 		return ERROR_TARGET_NOT_HALTED;
 	}
 	return target->type->get_gdb_fileio_info(target, fileio_info);
@@ -1176,7 +1176,7 @@ int target_get_gdb_fileio_info(struct target *target, struct gdb_fileio_info *fi
 int target_gdb_fileio_end(struct target *target, int retcode, int fileio_errno, bool ctrl_c)
 {
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target %s is not halted", target->cmd_name);
+		LOG_WARNING("target %s is not halted (gdb fileio end)", target->cmd_name);
 		return ERROR_TARGET_NOT_HALTED;
 	}
 	return target->type->gdb_fileio_end(target, retcode, fileio_errno, ctrl_c);
@@ -1186,7 +1186,7 @@ int target_profiling(struct target *target, uint32_t *samples,
 			uint32_t max_num_samples, uint32_t *num_samples, uint32_t seconds)
 {
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target %s is not halted", target->cmd_name);
+		LOG_WARNING("target %s is not halted (profiling)", target->cmd_name);
 		return ERROR_TARGET_NOT_HALTED;
 	}
 	return target->type->profiling(target, samples, max_num_samples,