Lots of RISC-V improvements.

This represents months of continuing RISC-V work, with too many changes
to list individually. Some improvements:
* Fixed memory leaks.
* Better handling of dbus timeouts.
* Add `riscv expose_custom` command.
* Somewhat deal with cache coherency.
* Deal with more timeouts during block memory accesses.
* Basic debug compliance test.
* Tell gdb which watchpoint hit.
* SMP support for use with -rtos hwthread
* Add `riscv set_ir`

Change-Id: Ica507ee2a57eaf51b578ab1d9b7de71512fdf47f
Signed-off-by: Tim Newsome <tim@sifive.com>
Reviewed-on: http://openocd.zylin.com/4922
Tested-by: jenkins
Reviewed-by: Philipp Guehring <pg@futureware.at>
Reviewed-by: Liviu Ionescu <ilg@livius.net>
Reviewed-by: Matthias Welwarsky <matthias@welwarsky.de>

8 files changed, 1783 insertions, 379 deletions

diff --git a/doc/openocd.texi b/doc/openocd.texi
index bbb9075..a17173c 100644
--- a/doc/openocd.texi
+++ b/doc/openocd.texi
@@ -9466,6 +9466,14 @@ command can be used if OpenOCD gets this wrong, or a target implements custom
 CSRs.
 @end deffn
 
+@deffn Command {riscv expose_custom} n0[-m0][,n1[-m1]]...
+The RISC-V Debug Specification allows targets to expose custom registers
+through abstract commands. (See Section 3.5.1.1 in that document.) This command
+configures a list of inclusive ranges of those registers to expose. Number 0
+indicates the first custom register, whose abstract command number is 0xc000.
+This command must be executed before `init`.
+@end deffn
+
 @deffn Command {riscv set_command_timeout_sec} [seconds]
 Set the wall-clock timeout (in seconds) for individual commands. The default
 should work fine for all but the slowest targets (eg. simulators).
@@ -9486,6 +9494,17 @@ When on, prefer to use System Bus Access to access memory.  When off, prefer to
 use the Program Buffer to access memory.
 @end deffn
 
+@deffn Command {riscv set_ir} (@option{idcode}|@option{dtmcs}|@option{dmi}) [value]
+Set the IR value for the specified JTAG register.  This is useful, for
+example, when using the existing JTAG interface on a Xilinx FPGA by
+way of BSCANE2 primitives that only permit a limited selection of IR
+values.
+
+When utilizing version 0.11 of the RISC-V Debug Specification,
+@option{dtmcs} and @option{dmi} set the IR values for the DTMCONTROL
+and DBUS registers, respectively.
+@end deffn
+
 @subsection RISC-V Authentication Commands
 
 The following commands can be used to authenticate to a RISC-V system. Eg.  a
diff --git a/src/target/riscv/batch.c b/src/target/riscv/batch.c
index 9327cb3..d041ed1 100644
--- a/src/target/riscv/batch.c
+++ b/src/target/riscv/batch.c
@@ -9,23 +9,20 @@
 #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);
+static void dump_field(int idle, 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));
+	struct riscv_batch *out = calloc(1, 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;
 }
 
@@ -51,7 +48,6 @@ int riscv_batch_run(struct riscv_batch *batch)
 
 	keep_alive();
 
-	LOG_DEBUG("running a batch of %ld scans", (long)batch->used_scans);
 	riscv_batch_add_nop(batch);
 
 	for (size_t i = 0; i < batch->used_scans; ++i) {
@@ -60,14 +56,13 @@ int riscv_batch_run(struct riscv_batch *batch)
 			jtag_add_runtest(batch->idle_count, TAP_IDLE);
 	}
 
-	LOG_DEBUG("executing queue");
 	if (jtag_execute_queue() != ERROR_OK) {
 		LOG_ERROR("Unable to execute JTAG queue");
 		return ERROR_FAIL;
 	}
 
 	for (size_t i = 0; i < batch->used_scans; ++i)
-		dump_field(batch->fields + i);
+		dump_field(batch->idle_count, batch->fields + i);
 
 	return ERROR_OK;
 }
@@ -98,13 +93,10 @@ size_t riscv_batch_add_dmi_read(struct riscv_batch *batch, unsigned address)
 	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. */
+	 * just sticking a NOP in there, but this should be coalesced away. */
 	riscv_batch_add_nop(batch);
 
 	batch->read_keys[batch->read_keys_used] = batch->used_scans - 1;
-	LOG_DEBUG("read key %u for batch 0x%p is %u (0x%p)",
-			(unsigned) batch->read_keys_used, batch, (unsigned) (batch->used_scans - 1),
-			batch->data_in + sizeof(uint64_t) * (batch->used_scans + 1));
 	return batch->read_keys_used++;
 }
 
@@ -135,10 +127,9 @@ void riscv_batch_add_nop(struct riscv_batch *batch)
 	riscv_fill_dmi_nop_u64(batch->target, (char *)field->in_value);
 	batch->last_scan = RISCV_SCAN_TYPE_NOP;
 	batch->used_scans++;
-	LOG_DEBUG("  added NOP with in_value=0x%p", field->in_value);
 }
 
-void dump_field(const struct scan_field *field)
+void dump_field(int idle, const struct scan_field *field)
 {
 	static const char * const op_string[] = {"-", "r", "w", "?"};
 	static const char * const status_string[] = {"+", "?", "F", "b"};
@@ -160,13 +151,13 @@ void dump_field(const struct scan_field *field)
 
 		log_printf_lf(LOG_LVL_DEBUG,
 				__FILE__, __LINE__, __PRETTY_FUNCTION__,
-				"%db %s %08x @%02x -> %s %08x @%02x",
-				field->num_bits,
+				"%db %di %s %08x @%02x -> %s %08x @%02x",
+				field->num_bits, idle,
 				op_string[out_op], out_data, out_address,
 				status_string[in_op], in_data, in_address);
 	} 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);
+				__FILE__, __LINE__, __PRETTY_FUNCTION__, "%db %di %s %08x @%02x -> ?",
+				field->num_bits, idle, op_string[out_op], out_data, out_address);
 	}
 }
diff --git a/src/target/riscv/opcodes.h b/src/target/riscv/opcodes.h
index dd51c80..de85aad 100644
--- a/src/target/riscv/opcodes.h
+++ b/src/target/riscv/opcodes.h
@@ -224,6 +224,9 @@ static uint32_t ebreak_c(void)
 	return MATCH_C_EBREAK;
 }
 
+static uint32_t wfi(void) __attribute__ ((unused));
+static uint32_t wfi(void) { return MATCH_WFI; }
+
 static uint32_t fence_i(void) __attribute__ ((unused));
 static uint32_t fence_i(void)
 {
diff --git a/src/target/riscv/program.h b/src/target/riscv/program.h
index d641be1..310460c 100644
--- a/src/target/riscv/program.h
+++ b/src/target/riscv/program.h
@@ -52,8 +52,8 @@ int riscv_program_insert(struct riscv_program *p, riscv_insn_t i);
  * memory. */
 int riscv_program_save_to_dscratch(struct riscv_program *p, enum gdb_regno to_save);
 
-/* Helpers to assembly various instructions.  Return 0 on success.  These might
- * assembly into a multi-instruction sequence that overwrites some other
+/* Helpers to assemble various instructions.  Return 0 on success.  These might
+ * assemble into a multi-instruction sequence that overwrites some other
  * register, but those will be properly saved and restored. */
 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);
diff --git a/src/target/riscv/riscv-011.c b/src/target/riscv/riscv-011.c
index bd3f159..eded862 100644
--- a/src/target/riscv/riscv-011.c
+++ b/src/target/riscv/riscv-011.c
@@ -358,6 +358,15 @@ static void add_dbus_scan(const struct target *target, struct scan_field *field,
 		uint16_t address, uint64_t data)
 {
 	riscv011_info_t *info = get_info(target);
+	RISCV_INFO(r);
+
+	if (r->reset_delays_wait >= 0) {
+		r->reset_delays_wait--;
+		if (r->reset_delays_wait < 0) {
+			info->dbus_busy_delay = 0;
+			info->interrupt_high_delay = 0;
+		}
+	}
 
 	field->num_bits = info->addrbits + DBUS_OP_SIZE + DBUS_DATA_SIZE;
 	field->in_value = in_value;
@@ -1408,12 +1417,6 @@ static int strict_step(struct target *target, bool announce)
 
 	LOG_DEBUG("enter");
 
-	struct breakpoint *breakpoint = target->breakpoints;
-	while (breakpoint) {
-		riscv_remove_breakpoint(target, breakpoint);
-		breakpoint = breakpoint->next;
-	}
-
 	struct watchpoint *watchpoint = target->watchpoints;
 	while (watchpoint) {
 		riscv_remove_watchpoint(target, watchpoint);
@@ -1424,12 +1427,6 @@ static int strict_step(struct target *target, bool announce)
 	if (result != ERROR_OK)
 		return result;
 
-	breakpoint = target->breakpoints;
-	while (breakpoint) {
-		riscv_add_breakpoint(target, breakpoint);
-		breakpoint = breakpoint->next;
-	}
-
 	watchpoint = target->watchpoints;
 	while (watchpoint) {
 		riscv_add_watchpoint(target, watchpoint);
@@ -1463,7 +1460,7 @@ static int step(struct target *target, int current, target_addr_t address,
 		if (result != ERROR_OK)
 			return result;
 	} else {
-		return resume(target, 0, true);
+		return full_step(target, false);
 	}
 
 	return ERROR_OK;
@@ -1676,7 +1673,7 @@ static riscv_error_t handle_halt_routine(struct target *target)
 				break;
 			default:
 				LOG_ERROR("Got invalid bus access status: %d", status);
-				return ERROR_FAIL;
+				goto error;
 		}
 		if (data & DMCONTROL_INTERRUPT) {
 			interrupt_set++;
@@ -1850,7 +1847,7 @@ static int handle_halt(struct target *target, bool announce)
 			target->debug_reason = DBG_REASON_BREAKPOINT;
 			break;
 		case DCSR_CAUSE_HWBP:
-			target->debug_reason = DBG_REASON_WPTANDBKPT;
+			target->debug_reason = DBG_REASON_WATCHPOINT;
 			/* 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;
diff --git a/src/target/riscv/riscv-013.c b/src/target/riscv/riscv-013.c
index 4acd427..5683e5a 100644
--- a/src/target/riscv/riscv-013.c
+++ b/src/target/riscv/riscv-013.c
@@ -64,6 +64,13 @@ static int read_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, uint8_t *buffer);
 static int write_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, const uint8_t *buffer);
+static int riscv013_test_sba_config_reg(struct target *target, target_addr_t legal_address,
+		uint32_t num_words, target_addr_t illegal_address, bool run_sbbusyerror_test);
+void write_memory_sba_simple(struct target *target, target_addr_t addr, uint32_t* write_data,
+		uint32_t write_size, uint32_t sbcs);
+void read_memory_sba_simple(struct target *target, target_addr_t addr,
+		uint32_t *rd_buf, uint32_t read_size, uint32_t sbcs);
+static int	riscv013_test_compliance(struct target *target);
 
 /**
  * Since almost everything can be accomplish by scanning the dbus register, all
@@ -169,7 +176,7 @@ typedef struct {
 
 	/* Number of run-test/idle cycles the target requests we do after each dbus
 	 * access. */
-	unsigned int dtmcontrol_idle;
+	unsigned int dtmcs_idle;
 
 	/* This value is incremented every time a dbus access comes back as "busy".
 	 * It's used to determine how many run-test/idle cycles to feed the target
@@ -187,8 +194,6 @@ typedef struct {
 	 * go low. */
 	unsigned int ac_busy_delay;
 
-	bool need_strict_step;
-
 	bool abstract_read_csr_supported;
 	bool abstract_write_csr_supported;
 	bool abstract_read_fpr_supported;
@@ -351,7 +356,7 @@ static void decode_dmi(char *text, unsigned address, unsigned data)
 	}
 }
 
-static void dump_field(const struct scan_field *field)
+static void dump_field(int idle, const struct scan_field *field)
 {
 	static const char * const op_string[] = {"-", "r", "w", "?"};
 	static const char * const status_string[] = {"+", "?", "F", "b"};
@@ -371,8 +376,8 @@ static void dump_field(const struct scan_field *field)
 
 	log_printf_lf(LOG_LVL_DEBUG,
 			__FILE__, __LINE__, "scan",
-			"%db %s %08x @%02x -> %s %08x @%02x",
-			field->num_bits,
+			"%db %di %s %08x @%02x -> %s %08x @%02x",
+			field->num_bits, idle,
 			op_string[out_op], out_data, out_address,
 			status_string[in_op], in_data, in_address);
 
@@ -390,16 +395,7 @@ static void dump_field(const struct scan_field *field)
 
 static void select_dmi(struct target *target)
 {
-	static uint8_t ir_dmi[1] = {DTM_DMI};
-	struct scan_field field = {
-		.num_bits = target->tap->ir_length,
-		.out_value = ir_dmi,
-		.in_value = NULL,
-		.check_value = NULL,
-		.check_mask = NULL
-	};
-
-	jtag_add_ir_scan(target->tap, &field, TAP_IDLE);
+	jtag_add_ir_scan(target->tap, &select_dbus, TAP_IDLE);
 }
 
 static uint32_t dtmcontrol_scan(struct target *target, uint32_t out)
@@ -436,8 +432,8 @@ static void increase_dmi_busy_delay(struct target *target)
 {
 	riscv013_info_t *info = get_info(target);
 	info->dmi_busy_delay += info->dmi_busy_delay / 10 + 1;
-	LOG_DEBUG("dtmcontrol_idle=%d, dmi_busy_delay=%d, ac_busy_delay=%d",
-			info->dtmcontrol_idle, info->dmi_busy_delay,
+	LOG_DEBUG("dtmcs_idle=%d, dmi_busy_delay=%d, ac_busy_delay=%d",
+			info->dtmcs_idle, info->dmi_busy_delay,
 			info->ac_busy_delay);
 
 	dtmcontrol_scan(target, DTM_DTMCS_DMIRESET);
@@ -452,14 +448,27 @@ static dmi_status_t dmi_scan(struct target *target, uint32_t *address_in,
 		bool exec)
 {
 	riscv013_info_t *info = get_info(target);
-	uint8_t in[8] = {0};
-	uint8_t out[8];
+	RISCV_INFO(r);
+	unsigned num_bits = info->abits + DTM_DMI_OP_LENGTH + DTM_DMI_DATA_LENGTH;
+	size_t num_bytes = (num_bits + 7) / 8;
+	uint8_t in[num_bytes];
+	uint8_t out[num_bytes];
 	struct scan_field field = {
-		.num_bits = info->abits + DTM_DMI_OP_LENGTH + DTM_DMI_DATA_LENGTH,
+		.num_bits = num_bits,
 		.out_value = out,
 		.in_value = in
 	};
 
+	if (r->reset_delays_wait >= 0) {
+		r->reset_delays_wait--;
+		if (r->reset_delays_wait < 0) {
+			info->dmi_busy_delay = 0;
+			info->ac_busy_delay = 0;
+		}
+	}
+
+	memset(in, 0, num_bytes);
+
 	assert(info->abits != 0);
 
 	buf_set_u32(out, DTM_DMI_OP_OFFSET, DTM_DMI_OP_LENGTH, op);
@@ -488,19 +497,25 @@ static dmi_status_t dmi_scan(struct target *target, uint32_t *address_in,
 	if (address_in)
 		*address_in = buf_get_u32(in, DTM_DMI_ADDRESS_OFFSET, info->abits);
 
-	dump_field(&field);
+	dump_field(idle_count, &field);
 
 	return buf_get_u32(in, DTM_DMI_OP_OFFSET, DTM_DMI_OP_LENGTH);
 }
 
-static int dmi_op_timeout(struct target *target, uint32_t *data_in, int dmi_op,
-		uint32_t address, uint32_t data_out, int timeout_sec)
+/* If dmi_busy_encountered is non-NULL, this function will use it to tell the
+ * caller whether DMI was ever busy during this call. */
+static int dmi_op_timeout(struct target *target, uint32_t *data_in,
+		bool *dmi_busy_encountered, int dmi_op, uint32_t address,
+		uint32_t data_out, int timeout_sec, bool exec)
 {
 	select_dmi(target);
 
 	dmi_status_t status;
 	uint32_t address_in;
 
+	if (dmi_busy_encountered)
+		*dmi_busy_encountered = false;
+
 	const char *op_name;
 	switch (dmi_op) {
 		case DMI_OP_NOP:
@@ -522,9 +537,11 @@ static int dmi_op_timeout(struct target *target, uint32_t *data_in, int dmi_op,
 	 * stays busy, it is actually due to the previous access. */
 	while (1) {
 		status = dmi_scan(target, NULL, NULL, dmi_op, address, data_out,
-				false);
+				exec);
 		if (status == DMI_STATUS_BUSY) {
 			increase_dmi_busy_delay(target);
+			if (dmi_busy_encountered)
+				*dmi_busy_encountered = true;
 		} else if (status == DMI_STATUS_SUCCESS) {
 			break;
 		} else {
@@ -573,11 +590,12 @@ static int dmi_op_timeout(struct target *target, uint32_t *data_in, int dmi_op,
 	return ERROR_OK;
 }
 
-static int dmi_op(struct target *target, uint32_t *data_in, int dmi_op,
-		uint32_t address, uint32_t data_out)
+static int dmi_op(struct target *target, uint32_t *data_in,
+		bool *dmi_busy_encountered, int dmi_op, uint32_t address,
+		uint32_t data_out, bool exec)
 {
-	int result = dmi_op_timeout(target, data_in, dmi_op, address, data_out,
-			riscv_command_timeout_sec);
+	int result = dmi_op_timeout(target, data_in, dmi_busy_encountered, dmi_op,
+			address, data_out, riscv_command_timeout_sec, exec);
 	if (result == ERROR_TIMEOUT_REACHED) {
 		LOG_ERROR("DMI operation didn't complete in %d seconds. The target is "
 				"either really slow or broken. You could increase the "
@@ -590,19 +608,29 @@ static int dmi_op(struct target *target, uint32_t *data_in, int dmi_op,
 
 static int dmi_read(struct target *target, uint32_t *value, uint32_t address)
 {
-	return dmi_op(target, value, DMI_OP_READ, address, 0);
+	return dmi_op(target, value, NULL, DMI_OP_READ, address, 0, false);
+}
+
+static int dmi_read_exec(struct target *target, uint32_t *value, uint32_t address)
+{
+	return dmi_op(target, value, NULL, DMI_OP_READ, address, 0, true);
 }
 
 static int dmi_write(struct target *target, uint32_t address, uint32_t value)
 {
-	return dmi_op(target, NULL, DMI_OP_WRITE, address, value);
+	return dmi_op(target, NULL, NULL, DMI_OP_WRITE, address, value, false);
+}
+
+static int dmi_write_exec(struct target *target, uint32_t address, uint32_t value)
+{
+	return dmi_op(target, NULL, NULL, DMI_OP_WRITE, address, value, true);
 }
 
 int dmstatus_read_timeout(struct target *target, uint32_t *dmstatus,
 		bool authenticated, unsigned timeout_sec)
 {
-	int result = dmi_op_timeout(target, dmstatus, DMI_OP_READ, DMI_DMSTATUS, 0,
-			timeout_sec);
+	int result = dmi_op_timeout(target, dmstatus, NULL, DMI_OP_READ,
+			DMI_DMSTATUS, 0, timeout_sec, false);
 	if (result != ERROR_OK)
 		return result;
 	if (authenticated && !get_field(*dmstatus, DMI_DMSTATUS_AUTHENTICATED)) {
@@ -625,8 +653,8 @@ static void increase_ac_busy_delay(struct target *target)
 {
 	riscv013_info_t *info = get_info(target);
 	info->ac_busy_delay += info->ac_busy_delay / 10 + 1;
-	LOG_DEBUG("dtmcontrol_idle=%d, dmi_busy_delay=%d, ac_busy_delay=%d",
-			info->dtmcontrol_idle, info->dmi_busy_delay,
+	LOG_DEBUG("dtmcs_idle=%d, dmi_busy_delay=%d, ac_busy_delay=%d",
+			info->dtmcs_idle, info->dmi_busy_delay,
 			info->ac_busy_delay);
 }
 
@@ -687,8 +715,25 @@ static int wait_for_idle(struct target *target, uint32_t *abstractcs)
 static int execute_abstract_command(struct target *target, uint32_t command)
 {
 	RISCV013_INFO(info);
-	LOG_DEBUG("command=0x%x", command);
-	dmi_write(target, DMI_COMMAND, command);
+	if (debug_level >= LOG_LVL_DEBUG) {
+		switch (get_field(command, DMI_COMMAND_CMDTYPE)) {
+			case 0:
+				LOG_DEBUG("command=0x%x; access register, size=%d, postexec=%d, "
+						"transfer=%d, write=%d, regno=0x%x",
+						command,
+						8 << get_field(command, AC_ACCESS_REGISTER_SIZE),
+						get_field(command, AC_ACCESS_REGISTER_POSTEXEC),
+						get_field(command, AC_ACCESS_REGISTER_TRANSFER),
+						get_field(command, AC_ACCESS_REGISTER_WRITE),
+						get_field(command, AC_ACCESS_REGISTER_REGNO));
+				break;
+			default:
+				LOG_DEBUG("command=0x%x", command);
+				break;
+		}
+	}
+
+	dmi_write_exec(target, DMI_COMMAND, command);
 
 	uint32_t abstractcs = 0;
 	wait_for_idle(target, &abstractcs);
@@ -744,10 +789,10 @@ static int write_abstract_arg(struct target *target, unsigned index,
 }
 
 /**
- * @size in bits
+ * @par size in bits
  */
-static uint32_t access_register_command(uint32_t number, unsigned size,
-		uint32_t flags)
+static uint32_t access_register_command(struct target *target, uint32_t number,
+		unsigned size, uint32_t flags)
 {
 	uint32_t command = set_field(0, DMI_COMMAND_CMDTYPE, 0);
 	switch (size) {
@@ -770,8 +815,13 @@ static uint32_t access_register_command(uint32_t number, unsigned size,
 	} else if (number >= GDB_REGNO_CSR0 && number <= GDB_REGNO_CSR4095) {
 		command = set_field(command, AC_ACCESS_REGISTER_REGNO,
 				number - GDB_REGNO_CSR0);
-	} else {
-		assert(0);
+	} else if (number >= GDB_REGNO_COUNT) {
+		/* Custom register. */
+		assert(target->reg_cache->reg_list[number].arch_info);
+		riscv_reg_info_t *reg_info = target->reg_cache->reg_list[number].arch_info;
+		assert(reg_info);
+		command = set_field(command, AC_ACCESS_REGISTER_REGNO,
+				0xc000 + reg_info->custom_number);
 	}
 
 	command |= flags;
@@ -791,7 +841,7 @@ static int register_read_abstract(struct target *target, uint64_t *value,
 			!info->abstract_read_csr_supported)
 		return ERROR_FAIL;
 
-	uint32_t command = access_register_command(number, size,
+	uint32_t command = access_register_command(target, number, size,
 			AC_ACCESS_REGISTER_TRANSFER);
 
 	int result = execute_abstract_command(target, command);
@@ -826,7 +876,7 @@ static int register_write_abstract(struct target *target, uint32_t number,
 			!info->abstract_write_csr_supported)
 		return ERROR_FAIL;
 
-	uint32_t command = access_register_command(number, size,
+	uint32_t command = access_register_command(target, number, size,
 			AC_ACCESS_REGISTER_TRANSFER |
 			AC_ACCESS_REGISTER_WRITE);
 
@@ -1087,7 +1137,7 @@ static int register_write_direct(struct target *target, unsigned number,
 	RISCV013_INFO(info);
 	RISCV_INFO(r);
 
-	LOG_DEBUG("[%d] reg[0x%x] <- 0x%" PRIx64, riscv_current_hartid(target),
+	LOG_DEBUG("{%d} reg[0x%x] <- 0x%" PRIx64, riscv_current_hartid(target),
 			number, value);
 
 	int result = register_write_abstract(target, number, value,
@@ -1095,7 +1145,6 @@ static int register_write_direct(struct target *target, unsigned number,
 	if (result == ERROR_OK && target->reg_cache) {
 		struct reg *reg = &target->reg_cache->reg_list[number];
 		buf_set_u64(reg->value, 0, reg->size, value);
-		reg->valid = true;
 	}
 	if (result == ERROR_OK || info->progbufsize + r->impebreak < 2 ||
 			!riscv_is_halted(target))
@@ -1154,7 +1203,6 @@ static int register_write_direct(struct target *target, unsigned number,
 	if (exec_out == ERROR_OK && target->reg_cache) {
 		struct reg *reg = &target->reg_cache->reg_list[number];
 		buf_set_u64(reg->value, 0, reg->size, value);
-		reg->valid = true;
 	}
 
 	if (use_scratch)
@@ -1174,24 +1222,12 @@ static int register_read(struct target *target, uint64_t *value, uint32_t number
 		*value = 0;
 		return ERROR_OK;
 	}
-	if (target->reg_cache &&
-			(number <= GDB_REGNO_XPR31 ||
-			 (number >= GDB_REGNO_FPR0 && number <= GDB_REGNO_FPR31))) {
-		/* Only check the cache for registers that we know won't spontaneously
-		 * change. */
-		struct reg *reg = &target->reg_cache->reg_list[number];
-		if (reg && reg->valid) {
-			*value = buf_get_u64(reg->value, 0, reg->size);
-			return ERROR_OK;
-		}
-	}
 	int result = register_read_direct(target, value, number);
 	if (result != ERROR_OK)
 		return ERROR_FAIL;
 	if (target->reg_cache) {
 		struct reg *reg = &target->reg_cache->reg_list[number];
 		buf_set_u64(reg->value, 0, reg->size, *value);
-		reg->valid = true;
 	}
 	return ERROR_OK;
 }
@@ -1284,7 +1320,7 @@ static int register_read_direct(struct target *target, uint64_t *value, uint32_t
 	}
 
 	if (result == ERROR_OK) {
-		LOG_DEBUG("[%d] reg[0x%x] = 0x%" PRIx64, riscv_current_hartid(target),
+		LOG_DEBUG("{%d} reg[0x%x] = 0x%" PRIx64, riscv_current_hartid(target),
 				number, *value);
 	}
 
@@ -1321,6 +1357,7 @@ static void deinit_target(struct target *target)
 	LOG_DEBUG("riscv_deinit_target()");
 	riscv_info_t *info = (riscv_info_t *) target->arch_info;
 	free(info->version_specific);
+	/* TODO: free register arch_info */
 	info->version_specific = NULL;
 }
 
@@ -1347,17 +1384,7 @@ static int examine(struct target *target)
 
 	riscv013_info_t *info = get_info(target);
 	info->abits = get_field(dtmcontrol, DTM_DTMCS_ABITS);
-	info->dtmcontrol_idle = get_field(dtmcontrol, DTM_DTMCS_IDLE);
-
-	uint32_t dmstatus;
-	if (dmstatus_read(target, &dmstatus, false) != ERROR_OK)
-		return ERROR_FAIL;
-	LOG_DEBUG("dmstatus:  0x%08x", dmstatus);
-	if (get_field(dmstatus, DMI_DMSTATUS_VERSION) != 2) {
-		LOG_ERROR("OpenOCD only supports Debug Module version 2, not %d "
-				"(dmstatus=0x%x)", get_field(dmstatus, DMI_DMSTATUS_VERSION), dmstatus);
-		return ERROR_FAIL;
-	}
+	info->dtmcs_idle = get_field(dtmcontrol, DTM_DTMCS_IDLE);
 
 	/* Reset the Debug Module. */
 	dm013_info_t *dm = get_dm(target);
@@ -1379,6 +1406,16 @@ static int examine(struct target *target)
 		return ERROR_FAIL;
 	}
 
+	uint32_t dmstatus;
+	if (dmstatus_read(target, &dmstatus, false) != ERROR_OK)
+		return ERROR_FAIL;
+	LOG_DEBUG("dmstatus:  0x%08x", dmstatus);
+	if (get_field(dmstatus, DMI_DMSTATUS_VERSION) != 2) {
+		LOG_ERROR("OpenOCD only supports Debug Module version 2, not %d "
+				"(dmstatus=0x%x)", get_field(dmstatus, DMI_DMSTATUS_VERSION), dmstatus);
+		return ERROR_FAIL;
+	}
+
 	uint32_t hartsel =
 		(get_field(dmcontrol, DMI_DMCONTROL_HARTSELHI) <<
 		 DMI_DMCONTROL_HARTSELLO_LENGTH) |
@@ -1454,7 +1491,8 @@ static int examine(struct target *target)
 			dmi_write(target, DMI_DMCONTROL,
 					set_hartsel(DMI_DMCONTROL_DMACTIVE | DMI_DMCONTROL_ACKHAVERESET, i));
 
-		if (!riscv_is_halted(target)) {
+		bool halted = riscv_is_halted(target);
+		if (!halted) {
 			if (riscv013_halt_current_hart(target) != ERROR_OK) {
 				LOG_ERROR("Fatal: Hart %d failed to halt during examine()", i);
 				return ERROR_FAIL;
@@ -1484,6 +1522,9 @@ static int examine(struct target *target)
 		 * really slow simulators. */
 		LOG_DEBUG(" hart %d: XLEN=%d, misa=0x%" PRIx64, i, r->xlen[i],
 				r->misa[i]);
+
+		if (!halted)
+			riscv013_resume_current_hart(target);
 	}
 
 	LOG_DEBUG("Enumerated %d harts", r->hart_count);
@@ -1493,11 +1534,6 @@ static int examine(struct target *target)
 		return ERROR_FAIL;
 	}
 
-	/* Resumes all the harts, so the debugger can later pause them. */
-	/* TODO: Only do this if the harts were halted to start with. */
-	riscv_resume_all_harts(target);
-	target->state = TARGET_RUNNING;
-
 	target_set_examined(target);
 
 	/* Some regression suites rely on seeing 'Examined RISC-V core' to know
@@ -1579,6 +1615,8 @@ static int init_target(struct command_context *cmd_ctx,
 	generic_info->authdata_write = &riscv013_authdata_write;
 	generic_info->dmi_read = &dmi_read;
 	generic_info->dmi_write = &dmi_write;
+	generic_info->test_sba_config_reg = &riscv013_test_sba_config_reg;
+	generic_info->test_compliance = &riscv013_test_compliance;
 	generic_info->version_specific = calloc(1, sizeof(riscv013_info_t));
 	if (!generic_info->version_specific)
 		return ERROR_FAIL;
@@ -1722,7 +1760,7 @@ static int deassert_reset(struct target *target)
 }
 
 /**
- * @size in bytes
+ * @par size in bytes
  */
 static void write_to_buf(uint8_t *buffer, uint64_t value, unsigned size)
 {
@@ -1750,13 +1788,38 @@ static void write_to_buf(uint8_t *buffer, uint64_t value, unsigned size)
 
 static int execute_fence(struct target *target)
 {
-	struct riscv_program program;
-	riscv_program_init(&program, target);
-	riscv_program_fence(&program);
-	int result = riscv_program_exec(&program, target);
-	if (result != ERROR_OK)
-		LOG_ERROR("Unable to execute fence");
-	return result;
+	int old_hartid = riscv_current_hartid(target);
+
+	/* FIXME: For non-coherent systems we need to flush the caches right
+	 * here, but there's no ISA-defined way of doing that. */
+	{
+		struct riscv_program program;
+		riscv_program_init(&program, target);
+		riscv_program_fence_i(&program);
+		riscv_program_fence(&program);
+		int result = riscv_program_exec(&program, target);
+		if (result != ERROR_OK)
+			LOG_DEBUG("Unable to execute pre-fence");
+	}
+
+	for (int i = 0; i < riscv_count_harts(target); ++i) {
+		if (!riscv_hart_enabled(target, i))
+			continue;
+
+		riscv_set_current_hartid(target, i);
+
+		struct riscv_program program;
+		riscv_program_init(&program, target);
+		riscv_program_fence_i(&program);
+		riscv_program_fence(&program);
+		int result = riscv_program_exec(&program, target);
+		if (result != ERROR_OK)
+			LOG_DEBUG("Unable to execute fence on hart %d", i);
+	}
+
+	riscv_set_current_hartid(target, old_hartid);
+
+	return ERROR_OK;
 }
 
 static void log_memory_access(target_addr_t address, uint64_t value,
@@ -2015,89 +2078,76 @@ static int read_memory_bus_v1(struct target *target, target_addr_t address,
 	return ERROR_OK;
 }
 
+static int batch_run(const struct target *target, struct riscv_batch *batch)
+{
+	RISCV013_INFO(info);
+	RISCV_INFO(r);
+	if (r->reset_delays_wait >= 0) {
+		r->reset_delays_wait -= batch->used_scans;
+		if (r->reset_delays_wait <= 0) {
+			batch->idle_count = 0;
+			info->dmi_busy_delay = 0;
+			info->ac_busy_delay = 0;
+		}
+	}
+	return riscv_batch_run(batch);
+}
+
 /**
  * Read the requested memory, taking care to execute every read exactly once,
  * even if cmderr=busy is encountered.
  */
-static int read_memory_progbuf(struct target *target, target_addr_t address,
+static int read_memory_progbuf_inner(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	RISCV013_INFO(info);
 
 	int result = ERROR_OK;
 
-	LOG_DEBUG("reading %d words of %d bytes from 0x%" TARGET_PRIxADDR, count,
-			size, address);
-
-	select_dmi(target);
-
-	/* s0 holds the next address to write to
-	 * s1 holds the next data value to write
-	 */
-	uint64_t s0, s1;
-	if (register_read(target, &s0, GDB_REGNO_S0) != ERROR_OK)
-		return ERROR_FAIL;
-	if (register_read(target, &s1, GDB_REGNO_S1) != ERROR_OK)
+	/* Write address to S0, and execute buffer. */
+	result = register_write_direct(target, GDB_REGNO_S0, address);
+	if (result != ERROR_OK)
+		goto error;
+	uint32_t command = access_register_command(target, GDB_REGNO_S1,
+			riscv_xlen(target),
+			AC_ACCESS_REGISTER_TRANSFER | AC_ACCESS_REGISTER_POSTEXEC);
+	if (execute_abstract_command(target, command) != ERROR_OK)
 		return ERROR_FAIL;
 
-	if (execute_fence(target) != ERROR_OK)
-		return ERROR_FAIL;
+	/* First read has just triggered. Result is in s1. */
 
-	/* Write the program (load, increment) */
-	struct riscv_program program;
-	riscv_program_init(&program, target);
-	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);
+	if (count == 1) {
+		uint64_t value;
+		if (register_read_direct(target, &value, GDB_REGNO_S1) != ERROR_OK)
 			return ERROR_FAIL;
+		write_to_buf(buffer, value, size);
+		log_memory_access(address, value, size, true);
+		return ERROR_OK;
 	}
-	riscv_program_addi(&program, GDB_REGNO_S0, GDB_REGNO_S0, size);
 
-	if (riscv_program_ebreak(&program) != ERROR_OK)
-		return ERROR_FAIL;
-	riscv_program_write(&program);
-
-	/* Write address to S0, and execute buffer. */
-	result = register_write_direct(target, GDB_REGNO_S0, address);
-	if (result != ERROR_OK)
+	if (dmi_write(target, DMI_ABSTRACTAUTO,
+			1 << DMI_ABSTRACTAUTO_AUTOEXECDATA_OFFSET) != ERROR_OK)
 		goto error;
-	uint32_t command = access_register_command(GDB_REGNO_S1, riscv_xlen(target),
-				AC_ACCESS_REGISTER_TRANSFER |
-				AC_ACCESS_REGISTER_POSTEXEC);
-	result = execute_abstract_command(target, command);
-	if (result != ERROR_OK)
+	/* Read garbage from dmi_data0, which triggers another execution of the
+	 * program. Now dmi_data0 contains the first good result, and s1 the next
+	 * memory value. */
+	if (dmi_read_exec(target, NULL, DMI_DATA0) != ERROR_OK)
 		goto error;
 
-	/* First read has just triggered. Result is in s1. */
-
-	dmi_write(target, DMI_ABSTRACTAUTO,
-			1 << DMI_ABSTRACTAUTO_AUTOEXECDATA_OFFSET);
-
 	/* read_addr is the next address that the hart will read from, which is the
 	 * value in s0. */
-	riscv_addr_t read_addr = address + size;
-	/* The next address that we need to receive data for. */
-	riscv_addr_t receive_addr = address;
+	riscv_addr_t read_addr = address + 2 * size;
 	riscv_addr_t fin_addr = address + (count * size);
-	unsigned skip = 1;
 	while (read_addr < fin_addr) {
-		LOG_DEBUG("read_addr=0x%" PRIx64 ", receive_addr=0x%" PRIx64
-				", fin_addr=0x%" PRIx64, read_addr, receive_addr, fin_addr);
+		LOG_DEBUG("read_addr=0x%" PRIx64 ", fin_addr=0x%" PRIx64, read_addr,
+				fin_addr);
 		/* The pipeline looks like this:
 		 * memory -> s1 -> dm_data0 -> debugger
-		 * It advances every time the debugger reads dmdata0.
-		 * So at any time the debugger has just read mem[s0 - 3*size],
-		 * dm_data0 contains mem[s0 - 2*size]
-		 * s1 contains mem[s0-size] */
+		 * Right now:
+		 * s0 contains read_addr
+		 * s1 contains mem[read_addr-size]
+		 * dm_data0 contains[read_addr-size*2]
+		 */
 
 		LOG_DEBUG("creating burst to read from 0x%" PRIx64
 				" up to 0x%" PRIx64, read_addr, fin_addr);
@@ -2114,10 +2164,11 @@ static int read_memory_progbuf(struct target *target, target_addr_t address,
 				break;
 		}
 
-		riscv_batch_run(batch);
+		batch_run(target, batch);
 
 		/* Wait for the target to finish performing the last abstract command,
-		 * and update our copy of cmderr. */
+		 * and update our copy of cmderr. If we see that DMI is busy here,
+		 * dmi_busy_delay will be incremented. */
 		uint32_t abstractcs;
 		if (dmi_read(target, &abstractcs, DMI_ABSTRACTCS) != ERROR_OK)
 			return ERROR_FAIL;
@@ -2126,9 +2177,8 @@ static int read_memory_progbuf(struct target *target, target_addr_t address,
 				return ERROR_FAIL;
 		info->cmderr = get_field(abstractcs, DMI_ABSTRACTCS_CMDERR);
 
-		unsigned cmderr = info->cmderr;
 		riscv_addr_t next_read_addr;
-		uint32_t dmi_data0 = -1;
+		unsigned ignore_last = 0;
 		switch (info->cmderr) {
 			case CMDERR_NONE:
 				LOG_DEBUG("successful (partial?) memory read");
@@ -2137,38 +2187,12 @@ static int read_memory_progbuf(struct target *target, target_addr_t address,
 			case CMDERR_BUSY:
 				LOG_DEBUG("memory read resulted in busy response");
 
-				/*
-				 * If you want to exercise this code path, apply the following patch to spike:
---- a/riscv/debug_module.cc
-+++ b/riscv/debug_module.cc
-@@ -1,3 +1,5 @@
-+#include <unistd.h>
-+
- #include <cassert>
-
- #include "debug_module.h"
-@@ -398,6 +400,15 @@ bool debug_module_t::perform_abstract_command()
-       // Since the next instruction is what we will use, just use nother NOP
-       // to get there.
-       write32(debug_abstract, 1, addi(ZERO, ZERO, 0));
-+
-+      if (abstractauto.autoexecdata &&
-+          program_buffer[0] == 0x83 &&
-+          program_buffer[1] == 0x24 &&
-+          program_buffer[2] == 0x04 &&
-+          program_buffer[3] == 0 &&
-+          rand() < RAND_MAX / 10) {
-+        usleep(1000000);
-+      }
-     } else {
-       write32(debug_abstract, 1, ebreak());
-     }
-				 */
 				increase_ac_busy_delay(target);
 				riscv013_clear_abstract_error(target);
 
 				dmi_write(target, DMI_ABSTRACTAUTO, 0);
 
+				uint32_t dmi_data0;
 				/* This is definitely a good version of the value that we
 				 * attempted to read when we discovered that the target was
 				 * busy. */
@@ -2177,23 +2201,30 @@ static int read_memory_progbuf(struct target *target, target_addr_t address,
 					goto error;
 				}
 
-				/* Clobbers DMI_DATA0. */
+				/* See how far we got, clobbering dmi_data0. */
 				result = register_read_direct(target, &next_read_addr,
 						GDB_REGNO_S0);
 				if (result != ERROR_OK) {
 					riscv_batch_free(batch);
 					goto error;
 				}
+				write_to_buf(buffer + next_read_addr - 2 * size - address, dmi_data0, size);
+				log_memory_access(next_read_addr - 2 * size, dmi_data0, size, true);
+
 				/* Restore the command, and execute it.
 				 * Now DMI_DATA0 contains the next value just as it would if no
 				 * error had occurred. */
-				dmi_write(target, DMI_COMMAND, command);
+				dmi_write_exec(target, DMI_COMMAND, command);
+				next_read_addr += size;
 
 				dmi_write(target, DMI_ABSTRACTAUTO,
 						1 << DMI_ABSTRACTAUTO_AUTOEXECDATA_OFFSET);
+
+				ignore_last = 1;
+
 				break;
 			default:
-				LOG_ERROR("error when reading memory, abstractcs=0x%08lx", (long)abstractcs);
+				LOG_DEBUG("error when reading memory, abstractcs=0x%08lx", (long)abstractcs);
 				riscv013_clear_abstract_error(target);
 				riscv_batch_free(batch);
 				result = ERROR_FAIL;
@@ -2201,34 +2232,43 @@ static int read_memory_progbuf(struct target *target, target_addr_t address,
 		}
 
 		/* Now read whatever we got out of the batch. */
+		dmi_status_t status = DMI_STATUS_SUCCESS;
 		for (size_t i = 0; i < reads; i++) {
-			if (read_addr >= next_read_addr)
-				break;
-
-			read_addr += size;
-
-			if (skip > 0) {
-				skip--;
+			riscv_addr_t receive_addr = read_addr + (i-2) * size;
+			assert(receive_addr < address + size * count);
+			if (receive_addr < address)
 				continue;
-			}
+			if (receive_addr > next_read_addr - (3 + ignore_last) * size)
+				break;
 
-			riscv_addr_t offset = receive_addr - address;
 			uint64_t dmi_out = riscv_batch_get_dmi_read(batch, i);
+			status = get_field(dmi_out, DTM_DMI_OP);
+			if (status != DMI_STATUS_SUCCESS) {
+				/* If we're here because of busy count, dmi_busy_delay will
+				 * already have been increased and busy state will have been
+				 * cleared in dmi_read(). */
+				/* In at least some implementations, we issue a read, and then
+				 * can get busy back when we try to scan out the read result,
+				 * and the actual read value is lost forever. Since this is
+				 * rare in any case, we return error here and rely on our
+				 * caller to reread the entire block. */
+				LOG_WARNING("Batch memory read encountered DMI error %d. "
+						"Falling back on slower reads.", status);
+				riscv_batch_free(batch);
+				result = ERROR_FAIL;
+				goto error;
+			}
 			uint32_t value = get_field(dmi_out, DTM_DMI_DATA);
+			riscv_addr_t offset = receive_addr - address;
 			write_to_buf(buffer + offset, value, size);
 			log_memory_access(receive_addr, value, size, true);
 
 			receive_addr += size;
 		}
-		riscv_batch_free(batch);
 
-		if (cmderr == CMDERR_BUSY) {
-			riscv_addr_t offset = receive_addr - address;
-			write_to_buf(buffer + offset, dmi_data0, size);
-			log_memory_access(receive_addr, dmi_data0, size, true);
-			read_addr += size;
-			receive_addr += size;
-		}
+		read_addr = next_read_addr;
+
+		riscv_batch_free(batch);
 	}
 
 	dmi_write(target, DMI_ABSTRACTAUTO, 0);
@@ -2237,10 +2277,9 @@ static int read_memory_progbuf(struct target *target, target_addr_t address,
 		/* Read the penultimate word. */
 		uint32_t value;
 		if (dmi_read(target, &value, DMI_DATA0) != ERROR_OK)
-			goto error;
-		write_to_buf(buffer + receive_addr - address, value, size);
-		log_memory_access(receive_addr, value, size, true);
-		receive_addr += size;
+			return ERROR_FAIL;
+		write_to_buf(buffer + size * (count-2), value, size);
+		log_memory_access(address + size * (count-2), value, size, true);
 	}
 
 	/* Read the last word. */
@@ -2248,16 +2287,95 @@ static int read_memory_progbuf(struct target *target, target_addr_t address,
 	result = register_read_direct(target, &value, GDB_REGNO_S1);
 	if (result != ERROR_OK)
 		goto error;
-	write_to_buf(buffer + receive_addr - address, value, size);
-	log_memory_access(receive_addr, value, size, true);
+	write_to_buf(buffer + size * (count-1), value, size);
+	log_memory_access(address + size * (count-1), value, size, true);
 
-	riscv_set_register(target, GDB_REGNO_S0, s0);
-	riscv_set_register(target, GDB_REGNO_S1, s1);
 	return ERROR_OK;
 
 error:
 	dmi_write(target, DMI_ABSTRACTAUTO, 0);
 
+	return result;
+}
+
+/**
+ * Read the requested memory, silently handling memory access errors.
+ */
+static int read_memory_progbuf(struct target *target, target_addr_t address,
+		uint32_t size, uint32_t count, uint8_t *buffer)
+{
+	int result = ERROR_OK;
+
+	LOG_DEBUG("reading %d words of %d bytes from 0x%" TARGET_PRIxADDR, count,
+			size, address);
+
+	select_dmi(target);
+
+	memset(buffer, 0, count*size);
+
+	/* s0 holds the next address to write to
+	 * s1 holds the next data value to write
+	 */
+	uint64_t s0, s1;
+	if (register_read(target, &s0, GDB_REGNO_S0) != ERROR_OK)
+		return ERROR_FAIL;
+	if (register_read(target, &s1, GDB_REGNO_S1) != ERROR_OK)
+		return ERROR_FAIL;
+
+	if (execute_fence(target) != ERROR_OK)
+		return ERROR_FAIL;
+
+	/* Write the program (load, increment) */
+	struct riscv_program program;
+	riscv_program_init(&program, target);
+	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_addi(&program, GDB_REGNO_S0, GDB_REGNO_S0, size);
+
+	if (riscv_program_ebreak(&program) != ERROR_OK)
+		return ERROR_FAIL;
+	riscv_program_write(&program);
+
+	result = read_memory_progbuf_inner(target, address, size, count, buffer);
+
+	if (result != ERROR_OK) {
+		/* The full read did not succeed, so we will try to read each word individually. */
+		/* This will not be fast, but reading outside actual memory is a special case anyway. */
+		/* It will make the toolchain happier, especially Eclipse Memory View as it reads ahead. */
+		target_addr_t address_i = address;
+		uint32_t size_i = size;
+		uint32_t count_i = 1;
+		uint8_t *buffer_i = buffer;
+
+		for (uint32_t i = 0; i < count; i++, address_i += size_i, buffer_i += size_i) {
+			/* TODO: This is much slower than it needs to be because we end up
+			 * writing the address to read for every word we read. */
+			result = read_memory_progbuf_inner(target, address_i, size_i, count_i, buffer_i);
+
+			/* The read of a single word failed, so we will just return 0 for that instead */
+			if (result != ERROR_OK) {
+				LOG_DEBUG("error reading single word of %d bytes from 0x%" TARGET_PRIxADDR,
+						size_i, address_i);
+
+				uint64_t value_i = 0;
+				write_to_buf(buffer_i, value_i, size_i);
+			}
+		}
+		result = ERROR_OK;
+	}
+
 	riscv_set_register(target, GDB_REGNO_S0, s0);
 	riscv_set_register(target, GDB_REGNO_S1, s1);
 	return result;
@@ -2558,7 +2676,8 @@ static int write_memory_progbuf(struct target *target, target_addr_t address,
 
 				/* Write and execute command that moves value into S1 and
 				 * executes program buffer. */
-				uint32_t command = access_register_command(GDB_REGNO_S1, 32,
+				uint32_t command = access_register_command(target,
+						GDB_REGNO_S1, 32,
 						AC_ACCESS_REGISTER_POSTEXEC |
 						AC_ACCESS_REGISTER_TRANSFER |
 						AC_ACCESS_REGISTER_WRITE);
@@ -2580,7 +2699,7 @@ static int write_memory_progbuf(struct target *target, target_addr_t address,
 			}
 		}
 
-		result = riscv_batch_run(batch);
+		result = batch_run(target, batch);
 		riscv_batch_free(batch);
 		if (result != ERROR_OK)
 			goto error;
@@ -2590,33 +2709,34 @@ static int write_memory_progbuf(struct target *target, target_addr_t address,
 		 * to be incremented if necessary. */
 
 		uint32_t abstractcs;
-		if (dmi_read(target, &abstractcs, DMI_ABSTRACTCS) != ERROR_OK)
+		bool dmi_busy_encountered;
+		if (dmi_op(target, &abstractcs, &dmi_busy_encountered, DMI_OP_READ,
+					DMI_ABSTRACTCS, 0, false) != ERROR_OK)
 			goto error;
 		while (get_field(abstractcs, DMI_ABSTRACTCS_BUSY))
 			if (dmi_read(target, &abstractcs, DMI_ABSTRACTCS) != ERROR_OK)
 				return ERROR_FAIL;
 		info->cmderr = get_field(abstractcs, DMI_ABSTRACTCS_CMDERR);
-		switch (info->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);
-
-				dmi_write(target, DMI_ABSTRACTAUTO, 0);
-				result = register_read_direct(target, &cur_addr, GDB_REGNO_S0);
-				if (result != ERROR_OK)
-					goto error;
-				setup_needed = true;
-				break;
-
-			default:
-				LOG_ERROR("error when writing memory, abstractcs=0x%08lx", (long)abstractcs);
-				riscv013_clear_abstract_error(target);
-				result = ERROR_FAIL;
+		if (info->cmderr == CMDERR_NONE && !dmi_busy_encountered) {
+			LOG_DEBUG("successful (partial?) memory write");
+		} else if (info->cmderr == CMDERR_BUSY || dmi_busy_encountered) {
+			if (info->cmderr == CMDERR_BUSY)
+				LOG_DEBUG("Memory write resulted in abstract command busy response.");
+			else if (dmi_busy_encountered)
+				LOG_DEBUG("Memory write resulted in DMI busy response.");
+			riscv013_clear_abstract_error(target);
+			increase_ac_busy_delay(target);
+
+			dmi_write(target, DMI_ABSTRACTAUTO, 0);
+			result = register_read_direct(target, &cur_addr, GDB_REGNO_S0);
+			if (result != ERROR_OK)
 				goto error;
+			setup_needed = true;
+		} else {
+			LOG_ERROR("error when writing memory, abstractcs=0x%08lx", (long)abstractcs);
+			riscv013_clear_abstract_error(target);
+			result = ERROR_FAIL;
+			goto error;
 		}
 	}
 
@@ -2696,7 +2816,7 @@ static int riscv013_get_register(struct target *target,
 	int result = ERROR_OK;
 	if (rid == GDB_REGNO_PC) {
 		result = register_read(target, value, GDB_REGNO_DPC);
-		LOG_DEBUG("read PC from DPC: 0x%016" PRIx64, *value);
+		LOG_DEBUG("read PC from DPC: 0x%" PRIx64, *value);
 	} else if (rid == GDB_REGNO_PRIV) {
 		uint64_t dcsr;
 		result = register_read(target, &dcsr, GDB_REGNO_DCSR);
@@ -2720,7 +2840,7 @@ static int riscv013_set_register(struct target *target, int hid, int rid, uint64
 	if (rid <= GDB_REGNO_XPR31) {
 		return register_write_direct(target, rid, value);
 	} else if (rid == GDB_REGNO_PC) {
-		LOG_DEBUG("writing PC to DPC: 0x%016" PRIx64, value);
+		LOG_DEBUG("writing PC to DPC: 0x%" PRIx64, value);
 		register_write_direct(target, GDB_REGNO_DPC, value);
 		uint64_t actual_value;
 		register_read_direct(target, &actual_value, GDB_REGNO_DPC);
@@ -2864,6 +2984,7 @@ static enum riscv_halt_reason riscv013_halt_reason(struct target *target)
 		 * already set when we connected. Force enumeration now, which has the
 		 * side effect of clearing any triggers we did not set. */
 		riscv_enumerate_triggers(target);
+		LOG_DEBUG("{%d} halted because of trigger", target->coreid);
 		return RISCV_HALT_TRIGGER;
 	case CSR_DCSR_CAUSE_STEP:
 		return RISCV_HALT_SINGLESTEP;
@@ -2924,6 +3045,357 @@ void riscv013_fill_dmi_nop_u64(struct target *target, char *buf)
 	buf_set_u64((unsigned char *)buf, DTM_DMI_ADDRESS_OFFSET, info->abits, 0);
 }
 
+/* Helper function for riscv013_test_sba_config_reg */
+static int get_max_sbaccess(struct target *target)
+{
+	RISCV013_INFO(info);
+
+	uint32_t sbaccess128 = get_field(info->sbcs, DMI_SBCS_SBACCESS128);
+	uint32_t sbaccess64 = get_field(info->sbcs, DMI_SBCS_SBACCESS64);
+	uint32_t sbaccess32 = get_field(info->sbcs, DMI_SBCS_SBACCESS32);
+	uint32_t sbaccess16 = get_field(info->sbcs, DMI_SBCS_SBACCESS16);
+	uint32_t sbaccess8 = get_field(info->sbcs, DMI_SBCS_SBACCESS8);
+
+	if (sbaccess128)
+		return 4;
+	else if (sbaccess64)
+		return 3;
+	else if (sbaccess32)
+		return 2;
+	else if (sbaccess16)
+		return 1;
+	else if (sbaccess8)
+		return 0;
+	else
+		return -1;
+}
+
+static uint32_t get_num_sbdata_regs(struct target *target)
+{
+	RISCV013_INFO(info);
+
+	uint32_t sbaccess128 = get_field(info->sbcs, DMI_SBCS_SBACCESS128);
+	uint32_t sbaccess64 = get_field(info->sbcs, DMI_SBCS_SBACCESS64);
+	uint32_t sbaccess32 = get_field(info->sbcs, DMI_SBCS_SBACCESS32);
+
+	if (sbaccess128)
+		return 4;
+	else if (sbaccess64)
+		return 2;
+	else if (sbaccess32)
+		return 1;
+	else
+		return 0;
+}
+
+static int riscv013_test_sba_config_reg(struct target *target,
+		target_addr_t legal_address, uint32_t num_words,
+		target_addr_t illegal_address, bool run_sbbusyerror_test)
+{
+	LOG_INFO("Testing System Bus Access as defined by RISC-V Debug Spec v0.13");
+
+	uint32_t tests_failed = 0;
+
+	uint32_t rd_val;
+	uint32_t sbcs_orig;
+	dmi_read(target, &sbcs_orig, DMI_SBCS);
+
+	uint32_t sbcs = sbcs_orig;
+	bool test_passed;
+
+	int max_sbaccess = get_max_sbaccess(target);
+
+	if (max_sbaccess == -1) {
+		LOG_ERROR("System Bus Access not supported in this config.");
+		return ERROR_FAIL;
+	}
+
+	if (get_field(sbcs, DMI_SBCS_SBVERSION) != 1) {
+		LOG_ERROR("System Bus Access unsupported SBVERSION (%d). Only version 1 is supported.",
+				get_field(sbcs, DMI_SBCS_SBVERSION));
+		return ERROR_FAIL;
+	}
+
+	uint32_t num_sbdata_regs = get_num_sbdata_regs(target);
+
+	uint32_t rd_buf[num_sbdata_regs];
+
+	/* Test 1: Simple write/read test */
+	test_passed = true;
+	sbcs = set_field(sbcs_orig, DMI_SBCS_SBAUTOINCREMENT, 0);
+	dmi_write(target, DMI_SBCS, sbcs);
+
+	uint32_t test_patterns[4] = {0xdeadbeef, 0xfeedbabe, 0x12345678, 0x08675309};
+	for (uint32_t sbaccess = 0; sbaccess <= (uint32_t)max_sbaccess; sbaccess++) {
+		sbcs = set_field(sbcs, DMI_SBCS_SBACCESS, sbaccess);
+		dmi_write(target, DMI_SBCS, sbcs);
+
+		uint32_t compare_mask = (sbaccess == 0) ? 0xff : (sbaccess == 1) ? 0xffff : 0xffffffff;
+
+		for (uint32_t i = 0; i < num_words; i++) {
+			uint32_t addr = legal_address + (i << sbaccess);
+			uint32_t wr_data[num_sbdata_regs];
+			for (uint32_t j = 0; j < num_sbdata_regs; j++)
+				wr_data[j] = test_patterns[j] + i;
+			write_memory_sba_simple(target, addr, wr_data, num_sbdata_regs, sbcs);
+		}
+
+		for (uint32_t i = 0; i < num_words; i++) {
+			uint32_t addr = legal_address + (i << sbaccess);
+			read_memory_sba_simple(target, addr, rd_buf, num_sbdata_regs, sbcs);
+			for (uint32_t j = 0; j < num_sbdata_regs; j++) {
+				if (((test_patterns[j]+i)&compare_mask) != (rd_buf[j]&compare_mask)) {
+					LOG_ERROR("System Bus Access Test 1: Error reading non-autoincremented address %x,"
+							"expected val = %x, read val = %x", addr, test_patterns[j]+i, rd_buf[j]);
+					test_passed = false;
+					tests_failed++;
+				}
+			}
+		}
+	}
+	if (test_passed)
+		LOG_INFO("System Bus Access Test 1: Simple write/read test PASSED.");
+
+	/* Test 2: Address autoincrement test */
+	target_addr_t curr_addr;
+	target_addr_t prev_addr;
+	test_passed = true;
+	sbcs = set_field(sbcs_orig, DMI_SBCS_SBAUTOINCREMENT, 1);
+	dmi_write(target, DMI_SBCS, sbcs);
+
+	for (uint32_t sbaccess = 0; sbaccess <= (uint32_t)max_sbaccess; sbaccess++) {
+		sbcs = set_field(sbcs, DMI_SBCS_SBACCESS, sbaccess);
+		dmi_write(target, DMI_SBCS, sbcs);
+
+		dmi_write(target, DMI_SBADDRESS0, legal_address);
+		read_sbcs_nonbusy(target, &sbcs);
+		curr_addr = legal_address;
+		for (uint32_t i = 0; i < num_words; i++) {
+			prev_addr = curr_addr;
+			read_sbcs_nonbusy(target, &sbcs);
+			curr_addr = sb_read_address(target);
+			if ((curr_addr - prev_addr != (uint32_t)(1 << sbaccess)) && (i != 0)) {
+				LOG_ERROR("System Bus Access Test 2: Error with address auto-increment, sbaccess = %x.", sbaccess);
+				test_passed = false;
+				tests_failed++;
+			}
+			dmi_write(target, DMI_SBDATA0, i);
+		}
+
+		read_sbcs_nonbusy(target, &sbcs);
+
+		dmi_write(target, DMI_SBADDRESS0, legal_address);
+
+		uint32_t val;
+		sbcs = set_field(sbcs, DMI_SBCS_SBREADONDATA, 1);
+		dmi_write(target, DMI_SBCS, sbcs);
+		dmi_read(target, &val, DMI_SBDATA0); /* Dummy read to trigger first system bus read */
+		curr_addr = legal_address;
+		for (uint32_t i = 0; i < num_words; i++) {
+			prev_addr = curr_addr;
+			read_sbcs_nonbusy(target, &sbcs);
+			curr_addr = sb_read_address(target);
+			if ((curr_addr - prev_addr != (uint32_t)(1 << sbaccess)) && (i != 0)) {
+				LOG_ERROR("System Bus Access Test 2: Error with address auto-increment, sbaccess = %x", sbaccess);
+				test_passed = false;
+				tests_failed++;
+			}
+			dmi_read(target, &val, DMI_SBDATA0);
+			read_sbcs_nonbusy(target, &sbcs);
+			if (i != val) {
+				LOG_ERROR("System Bus Access Test 2: Error reading auto-incremented address,"
+						"expected val = %x, read val = %x.", i, val);
+				test_passed = false;
+				tests_failed++;
+			}
+		}
+	}
+	if (test_passed)
+		LOG_INFO("System Bus Access Test 2: Address auto-increment test PASSED.");
+
+	/* Test 3: Read from illegal address */
+	read_memory_sba_simple(target, illegal_address, rd_buf, 1, sbcs_orig);
+
+	dmi_read(target, &rd_val, DMI_SBCS);
+	if (get_field(rd_val, DMI_SBCS_SBERROR) == 2) {
+		sbcs = set_field(sbcs_orig, DMI_SBCS_SBERROR, 2);
+		dmi_write(target, DMI_SBCS, sbcs);
+		dmi_read(target, &rd_val, DMI_SBCS);
+		if (get_field(rd_val, DMI_SBCS_SBERROR) == 0)
+			LOG_INFO("System Bus Access Test 3: Illegal address read test PASSED.");
+		else
+			LOG_ERROR("System Bus Access Test 3: Illegal address read test FAILED, unable to clear to 0.");
+	} else {
+		LOG_ERROR("System Bus Access Test 3: Illegal address read test FAILED, unable to set error code.");
+	}
+
+	/* Test 4: Write to illegal address */
+	write_memory_sba_simple(target, illegal_address, test_patterns, 1, sbcs_orig);
+
+	dmi_read(target, &rd_val, DMI_SBCS);
+	if (get_field(rd_val, DMI_SBCS_SBERROR) == 2) {
+		sbcs = set_field(sbcs_orig, DMI_SBCS_SBERROR, 2);
+		dmi_write(target, DMI_SBCS, sbcs);
+		dmi_read(target, &rd_val, DMI_SBCS);
+		if (get_field(rd_val, DMI_SBCS_SBERROR) == 0)
+			LOG_INFO("System Bus Access Test 4: Illegal address write test PASSED.");
+		else {
+			LOG_ERROR("System Bus Access Test 4: Illegal address write test FAILED, unable to clear to 0.");
+			tests_failed++;
+		}
+	} else {
+		LOG_ERROR("System Bus Access Test 4: Illegal address write test FAILED, unable to set error code.");
+		tests_failed++;
+	}
+
+	/* Test 5: Write with unsupported sbaccess size */
+	uint32_t sbaccess128 = get_field(sbcs_orig, DMI_SBCS_SBACCESS128);
+
+	if (sbaccess128) {
+		LOG_INFO("System Bus Access Test 5: SBCS sbaccess error test PASSED, all sbaccess sizes supported.");
+	} else {
+		sbcs = set_field(sbcs_orig, DMI_SBCS_SBACCESS, 4);
+
+		write_memory_sba_simple(target, legal_address, test_patterns, 1, sbcs);
+
+		dmi_read(target, &rd_val, DMI_SBCS);
+		if (get_field(rd_val, DMI_SBCS_SBERROR) == 4) {
+			sbcs = set_field(sbcs_orig, DMI_SBCS_SBERROR, 4);
+			dmi_write(target, DMI_SBCS, sbcs);
+			dmi_read(target, &rd_val, DMI_SBCS);
+			if (get_field(rd_val, DMI_SBCS_SBERROR) == 0)
+				LOG_INFO("System Bus Access Test 5: SBCS sbaccess error test PASSED.");
+			else {
+				LOG_ERROR("System Bus Access Test 5: SBCS sbaccess error test FAILED, unable to clear to 0.");
+				tests_failed++;
+			}
+		} else {
+			LOG_ERROR("System Bus Access Test 5: SBCS sbaccess error test FAILED, unable to set error code.");
+			tests_failed++;
+		}
+	}
+
+	/* Test 6: Write to misaligned address */
+	sbcs = set_field(sbcs_orig, DMI_SBCS_SBACCESS, 1);
+
+	write_memory_sba_simple(target, legal_address+1, test_patterns, 1, sbcs);
+
+	dmi_read(target, &rd_val, DMI_SBCS);
+	if (get_field(rd_val, DMI_SBCS_SBERROR) == 3) {
+		sbcs = set_field(sbcs_orig, DMI_SBCS_SBERROR, 3);
+		dmi_write(target, DMI_SBCS, sbcs);
+		dmi_read(target, &rd_val, DMI_SBCS);
+		if (get_field(rd_val, DMI_SBCS_SBERROR) == 0)
+			LOG_INFO("System Bus Access Test 6: SBCS address alignment error test PASSED");
+		else {
+			LOG_ERROR("System Bus Access Test 6: SBCS address alignment error test FAILED, unable to clear to 0.");
+			tests_failed++;
+		}
+	} else {
+		LOG_ERROR("System Bus Access Test 6: SBCS address alignment error test FAILED, unable to set error code.");
+		tests_failed++;
+	}
+
+	/* Test 7: Set sbbusyerror, only run this case in simulation as it is likely
+	 * impossible to hit otherwise */
+	if (run_sbbusyerror_test) {
+		sbcs = set_field(sbcs_orig, DMI_SBCS_SBREADONADDR, 1);
+		dmi_write(target, DMI_SBCS, sbcs);
+
+		for (int i = 0; i < 16; i++)
+			dmi_write(target, DMI_SBDATA0, 0xdeadbeef);
+
+		for (int i = 0; i < 16; i++)
+			dmi_write(target, DMI_SBADDRESS0, legal_address);
+
+		dmi_read(target, &rd_val, DMI_SBCS);
+		if (get_field(rd_val, DMI_SBCS_SBBUSYERROR)) {
+			sbcs = set_field(sbcs_orig, DMI_SBCS_SBBUSYERROR, 1);
+			dmi_write(target, DMI_SBCS, sbcs);
+			dmi_read(target, &rd_val, DMI_SBCS);
+			if (get_field(rd_val, DMI_SBCS_SBBUSYERROR) == 0)
+				LOG_INFO("System Bus Access Test 7: SBCS sbbusyerror test PASSED.");
+			else {
+				LOG_ERROR("System Bus Access Test 7: SBCS sbbusyerror test FAILED, unable to clear to 0.");
+				tests_failed++;
+			}
+		} else {
+			LOG_ERROR("System Bus Access Test 7: SBCS sbbusyerror test FAILED, unable to set error code.");
+			tests_failed++;
+		}
+	}
+
+	if (tests_failed == 0) {
+		LOG_INFO("ALL TESTS PASSED");
+		return ERROR_OK;
+	} else {
+		LOG_ERROR("%d TESTS FAILED", tests_failed);
+		return ERROR_FAIL;
+	}
+
+}
+
+void write_memory_sba_simple(struct target *target, target_addr_t addr,
+		uint32_t *write_data, uint32_t write_size, uint32_t sbcs)
+{
+	RISCV013_INFO(info);
+
+	uint32_t rd_sbcs;
+	uint32_t masked_addr;
+
+	uint32_t sba_size = get_field(info->sbcs, DMI_SBCS_SBASIZE);
+
+	read_sbcs_nonbusy(target, &rd_sbcs);
+
+	uint32_t sbcs_no_readonaddr = set_field(sbcs, DMI_SBCS_SBREADONADDR, 0);
+	dmi_write(target, DMI_SBCS, sbcs_no_readonaddr);
+
+	for (uint32_t i = 0; i < sba_size/32; i++) {
+		masked_addr = (addr >> 32*i) & 0xffffffff;
+
+		if (i != 3)
+			dmi_write(target, DMI_SBADDRESS0+i, masked_addr);
+		else
+			dmi_write(target, DMI_SBADDRESS3, masked_addr);
+	}
+
+	/* Write SBDATA registers starting with highest address, since write to
+	 * SBDATA0 triggers write */
+	for (int i = write_size-1; i >= 0; i--)
+		dmi_write(target, DMI_SBDATA0+i, write_data[i]);
+}
+
+void read_memory_sba_simple(struct target *target, target_addr_t addr,
+		uint32_t *rd_buf, uint32_t read_size, uint32_t sbcs)
+{
+	RISCV013_INFO(info);
+
+	uint32_t rd_sbcs;
+	uint32_t masked_addr;
+
+	uint32_t sba_size = get_field(info->sbcs, DMI_SBCS_SBASIZE);
+
+	read_sbcs_nonbusy(target, &rd_sbcs);
+
+	uint32_t sbcs_readonaddr = set_field(sbcs, DMI_SBCS_SBREADONADDR, 1);
+	dmi_write(target, DMI_SBCS, sbcs_readonaddr);
+
+	/* Write addresses starting with highest address register */
+	for (int i = sba_size/32-1; i >= 0; i--) {
+		masked_addr = (addr >> 32*i) & 0xffffffff;
+
+		if (i != 3)
+			dmi_write(target, DMI_SBADDRESS0+i, masked_addr);
+		else
+			dmi_write(target, DMI_SBADDRESS3, masked_addr);
+	}
+
+	read_sbcs_nonbusy(target, &rd_sbcs);
+
+	for (uint32_t i = 0; i < read_size; i++)
+		dmi_read(target, &(rd_buf[i]), DMI_SBDATA0+i);
+}
+
 int riscv013_dmi_write_u64_bits(struct target *target)
 {
 	RISCV013_INFO(info);
@@ -2934,16 +3406,8 @@ static int maybe_execute_fence_i(struct target *target)
 {
 	RISCV013_INFO(info);
 	RISCV_INFO(r);
-	if (info->progbufsize + r->impebreak >= 2) {
-		struct riscv_program program;
-		riscv_program_init(&program, target);
-		if (riscv_program_fence_i(&program) != ERROR_OK)
-			return ERROR_FAIL;
-		if (riscv_program_exec(&program, target) != ERROR_OK) {
-			LOG_ERROR("Failed to execute fence.i");
-			return ERROR_FAIL;
-		}
-	}
+	if (info->progbufsize + r->impebreak >= 3)
+		return execute_fence(target);
 	return ERROR_OK;
 }
 
@@ -3034,3 +3498,459 @@ void riscv013_clear_abstract_error(struct target *target)
 	/* Clear the error status. */
 	dmi_write(target, DMI_ABSTRACTCS, abstractcs & DMI_ABSTRACTCS_CMDERR);
 }
+
+#define COMPLIANCE_TEST(b, message) \
+{                                   \
+	int pass = 0;		    \
+	if (b) {		    \
+		pass = 1;	    \
+		passed_tests++;     \
+	}			    \
+	LOG_INFO("%s test %d (%s)\n", (pass) ? "PASSED" : "FAILED",  total_tests, message); \
+	assert(pass);		    \
+	total_tests++;		    \
+}
+
+#define COMPLIANCE_MUST_PASS(b) COMPLIANCE_TEST(ERROR_OK == (b), "Regular calls must return ERROR_OK")
+
+#define COMPLIANCE_READ(target, addr, value) COMPLIANCE_MUST_PASS(dmi_read(target, addr, value))
+#define COMPLIANCE_WRITE(target, addr, value) COMPLIANCE_MUST_PASS(dmi_write(target, addr, value))
+
+#define COMPLIANCE_CHECK_RO(target, addr)                               \
+{                                                                       \
+	uint32_t orig;                                                      \
+	uint32_t inverse;                                                   \
+	COMPLIANCE_READ(target, &orig, addr);                               \
+	COMPLIANCE_WRITE(target, addr, ~orig);                              \
+	COMPLIANCE_READ(target, &inverse, addr);                            \
+	COMPLIANCE_TEST(orig == inverse, "Register must be read-only");     \
+}
+
+int riscv013_test_compliance(struct target *target)
+{
+	LOG_INFO("Testing Compliance against RISC-V Debug Spec v0.13");
+
+	if (!riscv_rtos_enabled(target)) {
+		LOG_ERROR("Please run with -rtos riscv to run compliance test.");
+		return ERROR_FAIL;
+	}
+
+	int total_tests = 0;
+	int passed_tests = 0;
+
+	uint32_t dmcontrol_orig = DMI_DMCONTROL_DMACTIVE;
+	uint32_t dmcontrol;
+	uint32_t testvar;
+	uint32_t testvar_read;
+	riscv_reg_t value;
+	RISCV013_INFO(info);
+
+	/* All the bits of HARTSEL are covered by the examine sequence. */
+
+	/* hartreset */
+	/* This field is optional. Either we can read and write it to 1/0,
+	or it is tied to 0. This check doesn't really do anything, but
+	it does attempt to set the bit to 1 and then back to 0, which needs to
+	work if its implemented. */
+	COMPLIANCE_WRITE(target, DMI_DMCONTROL, set_field(dmcontrol_orig, DMI_DMCONTROL_HARTRESET, 1));
+	COMPLIANCE_WRITE(target, DMI_DMCONTROL, set_field(dmcontrol_orig, DMI_DMCONTROL_HARTRESET, 0));
+	COMPLIANCE_READ(target, &dmcontrol, DMI_DMCONTROL);
+	COMPLIANCE_TEST((get_field(dmcontrol, DMI_DMCONTROL_HARTRESET) == 0),
+			"DMCONTROL.hartreset can be 0 or RW.");
+
+	/* hasel */
+	COMPLIANCE_WRITE(target, DMI_DMCONTROL, set_field(dmcontrol_orig, DMI_DMCONTROL_HASEL, 1));
+	COMPLIANCE_WRITE(target, DMI_DMCONTROL, set_field(dmcontrol_orig, DMI_DMCONTROL_HASEL, 0));
+	COMPLIANCE_READ(target, &dmcontrol, DMI_DMCONTROL);
+	COMPLIANCE_TEST((get_field(dmcontrol, DMI_DMCONTROL_HASEL) == 0),
+			"DMCONTROL.hasel can be 0 or RW.");
+	/* TODO: test that hamask registers exist if hasel does. */
+
+	/* haltreq */
+	COMPLIANCE_MUST_PASS(riscv_halt_all_harts(target));
+	/* This bit is not actually readable according to the spec, so nothing to check.*/
+
+	/* DMSTATUS */
+	COMPLIANCE_CHECK_RO(target, DMI_DMSTATUS);
+
+	/* resumereq */
+	/* This bit is not actually readable according to the spec, so nothing to check.*/
+	COMPLIANCE_MUST_PASS(riscv_resume_all_harts(target));
+
+	/* Halt all harts again so the test can continue.*/
+	COMPLIANCE_MUST_PASS(riscv_halt_all_harts(target));
+
+	/* HARTINFO: Read-Only. This is per-hart, so need to adjust hartsel. */
+	uint32_t hartinfo;
+	COMPLIANCE_READ(target, &hartinfo, DMI_HARTINFO);
+	for (int hartsel = 0; hartsel < riscv_count_harts(target); hartsel++) {
+		COMPLIANCE_MUST_PASS(riscv_set_current_hartid(target, hartsel));
+
+		COMPLIANCE_CHECK_RO(target, DMI_HARTINFO);
+
+		/* $dscratch CSRs */
+		uint32_t nscratch = get_field(hartinfo, DMI_HARTINFO_NSCRATCH);
+		for (unsigned int d = 0; d < nscratch; d++) {
+			riscv_reg_t testval, testval_read;
+			/* Because DSCRATCH is not guaranteed to last across PB executions, need to put
+			this all into one PB execution. Which may not be possible on all implementations.*/
+			if (info->progbufsize >= 5) {
+				for (testval = 0x0011223300112233;
+						 testval != 0xDEAD;
+						 testval = testval == 0x0011223300112233 ? ~testval : 0xDEAD) {
+					COMPLIANCE_TEST(register_write_direct(target, GDB_REGNO_S0, testval) == ERROR_OK,
+							"Need to be able to write S0 in order to test DSCRATCH.");
+					struct riscv_program program32;
+					riscv_program_init(&program32, target);
+					riscv_program_csrw(&program32, GDB_REGNO_S0, GDB_REGNO_DSCRATCH + d);
+					riscv_program_csrr(&program32, GDB_REGNO_S1, GDB_REGNO_DSCRATCH + d);
+					riscv_program_fence(&program32);
+					riscv_program_ebreak(&program32);
+					COMPLIANCE_TEST(riscv_program_exec(&program32, target) == ERROR_OK,
+							"Accessing DSCRATCH with program buffer should succeed.");
+					COMPLIANCE_TEST(register_read_direct(target, &testval_read, GDB_REGNO_S1) == ERROR_OK,
+							"Need to be able to read S1 in order to test DSCRATCH.");
+					if (riscv_xlen(target) > 32) {
+						COMPLIANCE_TEST(testval == testval_read,
+								"All DSCRATCH registers in HARTINFO must be R/W.");
+					} else {
+						COMPLIANCE_TEST(testval_read == (testval & 0xFFFFFFFF),
+								"All DSCRATCH registers in HARTINFO must be R/W.");
+					}
+				}
+			}
+		}
+		/* TODO: dataaccess */
+		if (get_field(hartinfo, DMI_HARTINFO_DATAACCESS)) {
+			/* TODO: Shadowed in memory map. */
+			/* TODO: datasize */
+			/* TODO: dataaddr */
+		} else {
+			/* TODO: Shadowed in CSRs. */
+			/* TODO: datasize */
+			/* TODO: dataaddr */
+		}
+
+	}
+
+	/* HALTSUM -- TODO: More than 32 harts. Would need to loop over this to set hartsel */
+	/* TODO: HALTSUM2, HALTSUM3 */
+	/* HALTSUM0 */
+	uint32_t expected_haltsum0 = 0;
+	for (int i = 0; i < MIN(riscv_count_harts(target), 32); i++)
+		expected_haltsum0 |= (1 << i);
+
+	COMPLIANCE_READ(target, &testvar_read, DMI_HALTSUM0);
+	COMPLIANCE_TEST(testvar_read == expected_haltsum0,
+			"HALTSUM0 should report summary of up to 32 halted harts");
+
+	COMPLIANCE_WRITE(target, DMI_HALTSUM0, 0xffffffff);
+	COMPLIANCE_READ(target, &testvar_read, DMI_HALTSUM0);
+	COMPLIANCE_TEST(testvar_read == expected_haltsum0, "HALTSUM0 should be R/O");
+
+	COMPLIANCE_WRITE(target, DMI_HALTSUM0, 0x0);
+	COMPLIANCE_READ(target, &testvar_read, DMI_HALTSUM0);
+	COMPLIANCE_TEST(testvar_read == expected_haltsum0, "HALTSUM0 should be R/O");
+
+	/* HALTSUM1 */
+	uint32_t expected_haltsum1 = 0;
+	for (int i = 0; i < MIN(riscv_count_harts(target), 1024); i += 32)
+		expected_haltsum1 |= (1 << (i/32));
+
+	COMPLIANCE_READ(target, &testvar_read, DMI_HALTSUM1);
+	COMPLIANCE_TEST(testvar_read == expected_haltsum1,
+			"HALTSUM1 should report summary of up to 1024 halted harts");
+
+	COMPLIANCE_WRITE(target, DMI_HALTSUM1, 0xffffffff);
+	COMPLIANCE_READ(target, &testvar_read, DMI_HALTSUM1);
+	COMPLIANCE_TEST(testvar_read == expected_haltsum1, "HALTSUM1 should be R/O");
+
+	COMPLIANCE_WRITE(target, DMI_HALTSUM1, 0x0);
+	COMPLIANCE_READ(target, &testvar_read, DMI_HALTSUM1);
+	COMPLIANCE_TEST(testvar_read == expected_haltsum1, "HALTSUM1 should be R/O");
+
+	/* TODO: HAWINDOWSEL */
+
+	/* TODO: HAWINDOW */
+
+	/* ABSTRACTCS */
+
+	uint32_t abstractcs;
+	COMPLIANCE_READ(target, &abstractcs, DMI_ABSTRACTCS);
+
+	/* Check that all reported Data Words are really R/W */
+	for (int invert = 0; invert < 2; invert++) {
+		for (unsigned int i = 0; i < get_field(abstractcs, DMI_ABSTRACTCS_DATACOUNT); i++) {
+			testvar = (i + 1) * 0x11111111;
+			if (invert)
+				testvar = ~testvar;
+			COMPLIANCE_WRITE(target, DMI_DATA0 + i, testvar);
+		}
+		for (unsigned int i = 0; i < get_field(abstractcs, DMI_ABSTRACTCS_DATACOUNT); i++) {
+			testvar = (i + 1) * 0x11111111;
+			if (invert)
+				testvar = ~testvar;
+			COMPLIANCE_READ(target, &testvar_read, DMI_DATA0 + i);
+			COMPLIANCE_TEST(testvar_read == testvar, "All reported DATA words must be R/W");
+		}
+	}
+
+	/* Check that all reported ProgBuf words are really R/W */
+	for (int invert = 0; invert < 2; invert++) {
+		for (unsigned int i = 0; i < get_field(abstractcs, DMI_ABSTRACTCS_PROGBUFSIZE); i++) {
+			testvar = (i + 1) * 0x11111111;
+			if (invert)
+				testvar = ~testvar;
+			COMPLIANCE_WRITE(target, DMI_PROGBUF0 + i, testvar);
+		}
+		for (unsigned int i = 0; i < get_field(abstractcs, DMI_ABSTRACTCS_PROGBUFSIZE); i++) {
+			testvar = (i + 1) * 0x11111111;
+			if (invert)
+				testvar = ~testvar;
+			COMPLIANCE_READ(target, &testvar_read, DMI_PROGBUF0 + i);
+			COMPLIANCE_TEST(testvar_read == testvar, "All reported PROGBUF words must be R/W");
+		}
+	}
+
+	/* TODO: Cause and clear all error types */
+
+	/* COMMAND
+	According to the spec, this register is only W, so can't really check the read result.
+	But at any rate, this is not legal and should cause an error. */
+	COMPLIANCE_WRITE(target, DMI_COMMAND, 0xAAAAAAAA);
+	COMPLIANCE_READ(target, &testvar_read, DMI_ABSTRACTCS);
+	COMPLIANCE_TEST(get_field(testvar_read, DMI_ABSTRACTCS_CMDERR) == CMDERR_NOT_SUPPORTED, \
+			"Illegal COMMAND should result in UNSUPPORTED");
+	COMPLIANCE_WRITE(target, DMI_ABSTRACTCS, DMI_ABSTRACTCS_CMDERR);
+
+	COMPLIANCE_WRITE(target, DMI_COMMAND, 0x55555555);
+	COMPLIANCE_READ(target, &testvar_read, DMI_ABSTRACTCS);
+	COMPLIANCE_TEST(get_field(testvar_read, DMI_ABSTRACTCS_CMDERR) == CMDERR_NOT_SUPPORTED, \
+			"Illegal COMMAND should result in UNSUPPORTED");
+	COMPLIANCE_WRITE(target, DMI_ABSTRACTCS, DMI_ABSTRACTCS_CMDERR);
+
+	/* Basic Abstract Commands */
+	for (unsigned int i = 1; i < 32; i = i << 1) {
+		riscv_reg_t testval =	i | ((i + 1ULL) << 32);
+		riscv_reg_t testval_read;
+		COMPLIANCE_TEST(ERROR_OK == register_write_direct(target, GDB_REGNO_ZERO + i, testval),
+				"GPR Writes should be supported.");
+		COMPLIANCE_MUST_PASS(write_abstract_arg(target, 0, 0xDEADBEEFDEADBEEF, 64));
+		COMPLIANCE_TEST(ERROR_OK == register_read_direct(target, &testval_read, GDB_REGNO_ZERO + i),
+				"GPR Reads should be supported.");
+		if (riscv_xlen(target) > 32) {
+			/* Dummy comment to satisfy linter, since removing the brances here doesn't actually compile. */
+			COMPLIANCE_TEST(testval == testval_read, "GPR Reads and writes should be supported.");
+		} else {
+			/* Dummy comment to satisfy linter, since removing the brances here doesn't actually compile. */
+			COMPLIANCE_TEST((testval & 0xFFFFFFFF) == testval_read, "GPR Reads and writes should be supported.");
+		}
+	}
+
+	/* ABSTRACTAUTO
+	See which bits are actually writable */
+	COMPLIANCE_WRITE(target, DMI_ABSTRACTAUTO, 0xFFFFFFFF);
+	uint32_t abstractauto;
+	uint32_t busy;
+	COMPLIANCE_READ(target, &abstractauto, DMI_ABSTRACTAUTO);
+	COMPLIANCE_WRITE(target, DMI_ABSTRACTAUTO, 0x0);
+	if (abstractauto > 0) {
+		/* This mechanism only works when you have a reasonable sized progbuf, which is not
+		a true compliance requirement. */
+		if (info->progbufsize >= 3) {
+
+			testvar = 0;
+			COMPLIANCE_TEST(ERROR_OK == register_write_direct(target, GDB_REGNO_S0, 0),
+					"Need to be able to write S0 to test ABSTRACTAUTO");
+			struct riscv_program program;
+			COMPLIANCE_MUST_PASS(riscv_program_init(&program, target));
+			/* This is also testing that WFI() is a NOP during debug mode. */
+			COMPLIANCE_MUST_PASS(riscv_program_insert(&program, wfi()));
+			COMPLIANCE_MUST_PASS(riscv_program_addi(&program, GDB_REGNO_S0, GDB_REGNO_S0, 1));
+			COMPLIANCE_MUST_PASS(riscv_program_ebreak(&program));
+			COMPLIANCE_WRITE(target, DMI_ABSTRACTAUTO, 0x0);
+			COMPLIANCE_MUST_PASS(riscv_program_exec(&program, target));
+			testvar++;
+			COMPLIANCE_WRITE(target, DMI_ABSTRACTAUTO, 0xFFFFFFFF);
+			COMPLIANCE_READ(target, &abstractauto, DMI_ABSTRACTAUTO);
+			uint32_t autoexec_data = get_field(abstractauto, DMI_ABSTRACTAUTO_AUTOEXECDATA);
+			uint32_t autoexec_progbuf = get_field(abstractauto, DMI_ABSTRACTAUTO_AUTOEXECPROGBUF);
+			for (unsigned int i = 0; i < 12; i++) {
+				COMPLIANCE_READ(target, &testvar_read, DMI_DATA0 + i);
+				do {
+					COMPLIANCE_READ(target, &testvar_read, DMI_ABSTRACTCS);
+					busy = get_field(testvar_read, DMI_ABSTRACTCS_BUSY);
+				} while (busy);
+				if (autoexec_data & (1 << i)) {
+					COMPLIANCE_TEST(i < get_field(abstractcs, DMI_ABSTRACTCS_DATACOUNT),
+							"AUTOEXEC may be writable up to DATACOUNT bits.");
+					testvar++;
+				}
+			}
+			for (unsigned int i = 0; i < 16; i++) {
+				COMPLIANCE_READ(target, &testvar_read, DMI_PROGBUF0 + i);
+				do {
+					COMPLIANCE_READ(target, &testvar_read, DMI_ABSTRACTCS);
+					busy = get_field(testvar_read, DMI_ABSTRACTCS_BUSY);
+				} while (busy);
+				if (autoexec_progbuf & (1 << i)) {
+					COMPLIANCE_TEST(i < get_field(abstractcs, DMI_ABSTRACTCS_PROGBUFSIZE),
+							"AUTOEXEC may be writable up to PROGBUFSIZE bits.");
+					testvar++;
+				}
+			}
+
+			COMPLIANCE_WRITE(target, DMI_ABSTRACTAUTO, 0);
+			COMPLIANCE_TEST(ERROR_OK == register_read_direct(target, &value, GDB_REGNO_S0),
+					"Need to be able to read S0 to test ABSTRACTAUTO");
+
+			COMPLIANCE_TEST(testvar == value,
+					"ABSTRACTAUTO should cause COMMAND to run the expected number of times.");
+		}
+	}
+
+	/* Single-Step each hart. */
+	for (int hartsel = 0; hartsel < riscv_count_harts(target); hartsel++) {
+		COMPLIANCE_MUST_PASS(riscv_set_current_hartid(target, hartsel));
+		COMPLIANCE_MUST_PASS(riscv013_on_step(target));
+		COMPLIANCE_MUST_PASS(riscv013_step_current_hart(target));
+		COMPLIANCE_TEST(riscv_halt_reason(target, hartsel) == RISCV_HALT_SINGLESTEP,
+				"Single Step should result in SINGLESTEP");
+	}
+
+	/* Core Register Tests */
+	uint64_t bogus_dpc = 0xdeadbeef;
+	for (int hartsel = 0; hartsel < riscv_count_harts(target); hartsel++) {
+		COMPLIANCE_MUST_PASS(riscv_set_current_hartid(target, hartsel));
+
+		/* DCSR Tests */
+		COMPLIANCE_MUST_PASS(register_write_direct(target, GDB_REGNO_DCSR, 0x0));
+		COMPLIANCE_MUST_PASS(register_read_direct(target, &value, GDB_REGNO_DCSR));
+		COMPLIANCE_TEST(value != 0,	"Not all bits in DCSR are writable by Debugger");
+		COMPLIANCE_MUST_PASS(register_write_direct(target, GDB_REGNO_DCSR, 0xFFFFFFFF));
+		COMPLIANCE_MUST_PASS(register_read_direct(target, &value, GDB_REGNO_DCSR));
+		COMPLIANCE_TEST(value != 0,	"At least some bits in DCSR must be 1");
+
+		/* DPC. Note that DPC is sign-extended. */
+		riscv_reg_t dpcmask = 0xFFFFFFFCUL;
+		riscv_reg_t dpc;
+
+		if (riscv_xlen(target) > 32)
+			dpcmask |= (0xFFFFFFFFULL << 32);
+
+		if (riscv_supports_extension(target, riscv_current_hartid(target), 'C'))
+			dpcmask |= 0x2;
+
+		COMPLIANCE_MUST_PASS(register_write_direct(target, GDB_REGNO_DPC, dpcmask));
+		COMPLIANCE_MUST_PASS(register_read_direct(target, &dpc, GDB_REGNO_DPC));
+		COMPLIANCE_TEST(dpcmask == dpc,
+				"DPC must be sign-extended to XLEN and writable to all-1s (except the least significant bits)");
+		COMPLIANCE_MUST_PASS(register_write_direct(target, GDB_REGNO_DPC, 0));
+		COMPLIANCE_MUST_PASS(register_read_direct(target, &dpc, GDB_REGNO_DPC));
+		COMPLIANCE_TEST(dpc == 0, "DPC must be writable to 0.");
+		if (hartsel == 0)
+			bogus_dpc = dpc; /* For a later test step */
+	}
+
+	/* NDMRESET
+	Asserting non-debug module reset should not reset Debug Module state.
+	But it should reset Hart State, e.g. DPC should get a different value.
+	Also make sure that DCSR reports cause of 'HALT' even though previously we single-stepped.
+	*/
+
+	/* Write some registers. They should not be impacted by ndmreset. */
+	COMPLIANCE_WRITE(target, DMI_COMMAND, 0xFFFFFFFF);
+
+	for (unsigned int i = 0; i < get_field(abstractcs, DMI_ABSTRACTCS_PROGBUFSIZE); i++) {
+		testvar = (i + 1) * 0x11111111;
+		COMPLIANCE_WRITE(target, DMI_PROGBUF0 + i, testvar);
+	}
+
+	for (unsigned int i = 0; i < get_field(abstractcs, DMI_ABSTRACTCS_DATACOUNT); i++) {
+		testvar = (i + 1) * 0x11111111;
+		COMPLIANCE_WRITE(target, DMI_DATA0 + i, testvar);
+	}
+
+	COMPLIANCE_WRITE(target, DMI_ABSTRACTAUTO, 0xFFFFFFFF);
+	COMPLIANCE_READ(target, &abstractauto, DMI_ABSTRACTAUTO);
+
+	/* Pulse reset. */
+	target->reset_halt = true;
+	COMPLIANCE_MUST_PASS(riscv_set_current_hartid(target, 0));
+	COMPLIANCE_TEST(ERROR_OK == assert_reset(target), "Must be able to assert NDMRESET");
+	COMPLIANCE_TEST(ERROR_OK == deassert_reset(target), "Must be able to deassert NDMRESET");
+
+	/* Verify that most stuff is not affected by ndmreset. */
+	COMPLIANCE_READ(target, &testvar_read, DMI_ABSTRACTCS);
+	COMPLIANCE_TEST(get_field(testvar_read, DMI_ABSTRACTCS_CMDERR)	== CMDERR_NOT_SUPPORTED,
+			"NDMRESET should not affect DMI_ABSTRACTCS");
+	COMPLIANCE_READ(target, &testvar_read, DMI_ABSTRACTAUTO);
+	COMPLIANCE_TEST(testvar_read == abstractauto, "NDMRESET should not affect DMI_ABSTRACTAUTO");
+
+	/* Clean up to avoid future test failures */
+	COMPLIANCE_WRITE(target, DMI_ABSTRACTCS, DMI_ABSTRACTCS_CMDERR);
+	COMPLIANCE_WRITE(target, DMI_ABSTRACTAUTO, 0);
+
+	for (unsigned int i = 0; i < get_field(abstractcs, DMI_ABSTRACTCS_PROGBUFSIZE); i++) {
+		testvar = (i + 1) * 0x11111111;
+		COMPLIANCE_READ(target, &testvar_read, DMI_PROGBUF0 + i);
+		COMPLIANCE_TEST(testvar_read == testvar, "PROGBUF words must not be affected by NDMRESET");
+	}
+
+	for (unsigned int i = 0; i < get_field(abstractcs, DMI_ABSTRACTCS_DATACOUNT); i++) {
+		testvar = (i + 1) * 0x11111111;
+		COMPLIANCE_READ(target, &testvar_read, DMI_DATA0 + i);
+		COMPLIANCE_TEST(testvar_read == testvar, "DATA words must not be affected by NDMRESET");
+	}
+
+	/* Verify that DPC *is* affected by ndmreset. Since we don't know what it *should* be,
+	just verify that at least it's not the bogus value anymore. */
+
+	COMPLIANCE_TEST(bogus_dpc != 0xdeadbeef, "BOGUS DPC should have been set somehow (bug in compliance test)");
+	COMPLIANCE_MUST_PASS(register_read_direct(target, &value, GDB_REGNO_DPC));
+	COMPLIANCE_TEST(bogus_dpc != value, "NDMRESET should move DPC to reset value.");
+
+	COMPLIANCE_TEST(riscv_halt_reason(target, 0) == RISCV_HALT_INTERRUPT,
+			"After NDMRESET halt, DCSR should report cause of halt");
+
+	/* DMACTIVE -- deasserting DMACTIVE should reset all the above values. */
+
+	/* Toggle dmactive */
+	COMPLIANCE_WRITE(target, DMI_DMCONTROL, 0);
+	COMPLIANCE_WRITE(target, DMI_DMCONTROL, DMI_DMCONTROL_DMACTIVE);
+	COMPLIANCE_READ(target, &testvar_read, DMI_ABSTRACTCS);
+	COMPLIANCE_TEST(get_field(testvar_read, DMI_ABSTRACTCS_CMDERR)	== 0, "ABSTRACTCS.cmderr should reset to 0");
+	COMPLIANCE_READ(target, &testvar_read, DMI_ABSTRACTAUTO);
+	COMPLIANCE_TEST(testvar_read == 0, "ABSTRACTAUTO should reset to 0");
+
+	for (unsigned int i = 0; i < get_field(abstractcs, DMI_ABSTRACTCS_PROGBUFSIZE); i++) {
+		COMPLIANCE_READ(target, &testvar_read, DMI_PROGBUF0 + i);
+		COMPLIANCE_TEST(testvar_read == 0, "PROGBUF words should reset to 0");
+	}
+
+	for (unsigned int i = 0; i < get_field(abstractcs, DMI_ABSTRACTCS_DATACOUNT); i++) {
+		COMPLIANCE_READ(target, &testvar_read, DMI_DATA0 + i);
+		COMPLIANCE_TEST(testvar_read == 0, "DATA words should reset to 0");
+	}
+
+	/*
+	* TODO:
+	* DCSR.cause priorities
+	* DCSR.stoptime/stopcycle
+	* DCSR.stepie
+	* DCSR.ebreak
+	* DCSR.prv
+	*/
+
+	/* Halt every hart for any follow-up tests*/
+	COMPLIANCE_MUST_PASS(riscv_halt_all_harts(target));
+
+	uint32_t failed_tests = total_tests - passed_tests;
+	if (total_tests == passed_tests) {
+		LOG_INFO("ALL TESTS PASSED\n");
+		return ERROR_OK;
+	} else {
+		LOG_INFO("%d TESTS FAILED\n", failed_tests);
+		return ERROR_FAIL;
+	}
+}
diff --git a/src/target/riscv/riscv.c b/src/target/riscv/riscv.c
index 9a6b938..7bae390 100644
--- a/src/target/riscv/riscv.c
+++ b/src/target/riscv/riscv.c
@@ -154,17 +154,17 @@ typedef enum slot {
 #define MAX_HWBPS			16
 #define DRAM_CACHE_SIZE		16
 
-uint8_t ir_dtmcontrol[1] = {DTMCONTROL};
+uint8_t ir_dtmcontrol[4] = {DTMCONTROL};
 struct scan_field select_dtmcontrol = {
 	.in_value = NULL,
 	.out_value = ir_dtmcontrol
 };
-uint8_t ir_dbus[1] = {DBUS};
+uint8_t ir_dbus[4] = {DBUS};
 struct scan_field select_dbus = {
 	.in_value = NULL,
 	.out_value = ir_dbus
 };
-uint8_t ir_idcode[1] = {0x1};
+uint8_t ir_idcode[4] = {0x1};
 struct scan_field select_idcode = {
 	.in_value = NULL,
 	.out_value = ir_idcode
@@ -187,13 +187,17 @@ int riscv_reset_timeout_sec = DEFAULT_RESET_TIMEOUT_SEC;
 
 bool riscv_prefer_sba;
 
+typedef struct {
+	uint16_t low, high;
+} range_t;
+
 /* In addition to the ones in the standard spec, we'll also expose additional
  * CSRs in this list.
  * The list is either NULL, or a series of ranges (inclusive), terminated with
  * 1,0. */
-struct {
-	uint16_t low, high;
-} *expose_csr;
+range_t *expose_csr;
+/* Same, but for custom registers. */
+range_t *expose_custom;
 
 static uint32_t dtmcontrol_scan(struct target *target, uint32_t out)
 {
@@ -262,6 +266,8 @@ static int riscv_init_target(struct command_context *cmd_ctx,
 
 	riscv_semihosting_init(target);
 
+	target->debug_reason = DBG_REASON_DBGRQ;
+
 	return ERROR_OK;
 }
 
@@ -272,8 +278,21 @@ static void riscv_deinit_target(struct target *target)
 	if (tt) {
 		tt->deinit_target(target);
 		riscv_info_t *info = (riscv_info_t *) target->arch_info;
+		free(info->reg_names);
 		free(info);
 	}
+	/* Free the shared structure use for most registers. */
+	if (target->reg_cache) {
+		if (target->reg_cache->reg_list) {
+			if (target->reg_cache->reg_list[0].arch_info)
+				free(target->reg_cache->reg_list[0].arch_info);
+			/* Free the ones we allocated separately. */
+			for (unsigned i = GDB_REGNO_COUNT; i < target->reg_cache->num_regs; i++)
+				free(target->reg_cache->reg_list[i].arch_info);
+			free(target->reg_cache->reg_list);
+		}
+		free(target->reg_cache);
+	}
 	target->arch_info = NULL;
 }
 
@@ -470,8 +489,8 @@ static int add_trigger(struct target *target, struct trigger *trigger)
 		if (result != ERROR_OK)
 			continue;
 
-		LOG_DEBUG("Using trigger %d (type %d) for bp %d", i, type,
-				trigger->unique_id);
+		LOG_DEBUG("[%d] Using trigger %d (type %d) for bp %d", target->coreid,
+				i, type, trigger->unique_id);
 		r->trigger_unique_id[i] = trigger->unique_id;
 		break;
 	}
@@ -493,19 +512,31 @@ static int add_trigger(struct target *target, struct trigger *trigger)
 
 int riscv_add_breakpoint(struct target *target, struct breakpoint *breakpoint)
 {
+	LOG_DEBUG("[%d] @0x%" TARGET_PRIxADDR, target->coreid, breakpoint->address);
+	assert(breakpoint);
 	if (breakpoint->type == BKPT_SOFT) {
-		if (target_read_memory(target, breakpoint->address, breakpoint->length, 1,
+		/** @todo check RVC for size/alignment */
+		if (!(breakpoint->length == 4 || breakpoint->length == 2)) {
+			LOG_ERROR("Invalid breakpoint length %d", breakpoint->length);
+			return ERROR_FAIL;
+		}
+
+		if (0 != (breakpoint->address % 2)) {
+			LOG_ERROR("Invalid breakpoint alignment for address 0x%" TARGET_PRIxADDR, breakpoint->address);
+			return ERROR_FAIL;
+		}
+
+		if (target_read_memory(target, breakpoint->address, 2, breakpoint->length / 2,
 					breakpoint->orig_instr) != ERROR_OK) {
 			LOG_ERROR("Failed to read original instruction at 0x%" TARGET_PRIxADDR,
 					breakpoint->address);
 			return ERROR_FAIL;
 		}
 
-		int retval;
-		if (breakpoint->length == 4)
-			retval = target_write_u32(target, breakpoint->address, ebreak());
-		else
-			retval = target_write_u16(target, breakpoint->address, ebreak_c());
+		uint8_t buff[4];
+		buf_set_u32(buff, 0, breakpoint->length * CHAR_BIT, breakpoint->length == 4 ? ebreak() : ebreak_c());
+		int const retval = target_write_memory(target, breakpoint->address, 2, breakpoint->length / 2, buff);
+
 		if (retval != ERROR_OK) {
 			LOG_ERROR("Failed to write %d-byte breakpoint instruction at 0x%"
 					TARGET_PRIxADDR, breakpoint->length, breakpoint->address);
@@ -515,17 +546,15 @@ int riscv_add_breakpoint(struct target *target, struct breakpoint *breakpoint)
 	} else if (breakpoint->type == BKPT_HARD) {
 		struct trigger trigger;
 		trigger_from_breakpoint(&trigger, breakpoint);
-		int result = add_trigger(target, &trigger);
+		int const result = add_trigger(target, &trigger);
 		if (result != ERROR_OK)
 			return result;
-
 	} else {
 		LOG_INFO("OpenOCD only supports hardware and software breakpoints.");
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 	}
 
 	breakpoint->set = true;
-
 	return ERROR_OK;
 }
 
@@ -557,7 +586,8 @@ static int remove_trigger(struct target *target, struct trigger *trigger)
 				"trigger.");
 		return ERROR_FAIL;
 	}
-	LOG_DEBUG("Stop using resource %d for bp %d", i, trigger->unique_id);
+	LOG_DEBUG("[%d] Stop using resource %d for bp %d", target->coreid, i,
+			trigger->unique_id);
 	for (int hartid = first_hart; hartid < riscv_count_harts(target); ++hartid) {
 		if (!riscv_hart_enabled(target, hartid))
 			continue;
@@ -578,7 +608,7 @@ int riscv_remove_breakpoint(struct target *target,
 		struct breakpoint *breakpoint)
 {
 	if (breakpoint->type == BKPT_SOFT) {
-		if (target_write_memory(target, breakpoint->address, breakpoint->length, 1,
+		if (target_write_memory(target, breakpoint->address, 2, breakpoint->length / 2,
 					breakpoint->orig_instr) != ERROR_OK) {
 			LOG_ERROR("Failed to restore instruction for %d-byte breakpoint at "
 					"0x%" TARGET_PRIxADDR, breakpoint->length, breakpoint->address);
@@ -632,6 +662,8 @@ int riscv_add_watchpoint(struct target *target, struct watchpoint *watchpoint)
 int riscv_remove_watchpoint(struct target *target,
 		struct watchpoint *watchpoint)
 {
+	LOG_DEBUG("[%d] @0x%" TARGET_PRIxADDR, target->coreid, watchpoint->address);
+
 	struct trigger trigger;
 	trigger_from_watchpoint(&trigger, watchpoint);
 
@@ -643,6 +675,89 @@ int riscv_remove_watchpoint(struct target *target,
 	return ERROR_OK;
 }
 
+/* Sets *hit_watchpoint to the first watchpoint identified as causing the
+ * current halt.
+ *
+ * The GDB server uses this information to tell GDB what data address has
+ * been hit, which enables GDB to print the hit variable along with its old
+ * and new value. */
+int riscv_hit_watchpoint(struct target *target, struct watchpoint **hit_watchpoint)
+{
+	struct watchpoint *wp = target->watchpoints;
+
+	LOG_DEBUG("Current hartid = %d", riscv_current_hartid(target));
+
+	/*TODO instead of disassembling the instruction that we think caused the
+	 * trigger, check the hit bit of each watchpoint first. The hit bit is
+	 * simpler and more reliable to check but as it is optional and relatively
+	 * new, not all hardware will implement it  */
+	riscv_reg_t dpc;
+	riscv_get_register(target, &dpc, GDB_REGNO_DPC);
+	const uint8_t length = 4;
+	LOG_DEBUG("dpc is 0x%" PRIx64, dpc);
+
+	/* fetch the instruction at dpc */
+	uint8_t buffer[length];
+	if (target_read_buffer(target, dpc, length, buffer) != ERROR_OK) {
+		LOG_ERROR("Failed to read instruction at dpc 0x%" PRIx64, dpc);
+		return ERROR_FAIL;
+	}
+
+	uint32_t instruction = 0;
+
+	for (int i = 0; i < length; i++) {
+		LOG_DEBUG("Next byte is %x", buffer[i]);
+		instruction += (buffer[i] << 8 * i);
+	}
+	LOG_DEBUG("Full instruction is %x", instruction);
+
+	/* find out which memory address is accessed by the instruction at dpc */
+	/* opcode is first 7 bits of the instruction */
+	uint8_t opcode = instruction & 0x7F;
+	uint32_t rs1;
+	int16_t imm;
+	riscv_reg_t mem_addr;
+
+	if (opcode == MATCH_LB || opcode == MATCH_SB) {
+		rs1 = (instruction & 0xf8000) >> 15;
+		riscv_get_register(target, &mem_addr, rs1);
+
+		if (opcode == MATCH_SB) {
+			LOG_DEBUG("%x is store instruction", instruction);
+			imm = ((instruction & 0xf80) >> 7) | ((instruction & 0xfe000000) >> 20);
+		} else {
+			LOG_DEBUG("%x is load instruction", instruction);
+			imm = (instruction & 0xfff00000) >> 20;
+		}
+		/* sign extend 12-bit imm to 16-bits */
+		if (imm & (1 << 11))
+			imm |= 0xf000;
+		mem_addr += imm;
+		LOG_DEBUG("memory address=0x%" PRIx64, mem_addr);
+	} else {
+		LOG_DEBUG("%x is not a RV32I load or store", instruction);
+		return ERROR_FAIL;
+	}
+
+	while (wp) {
+		/*TODO support length/mask */
+		if (wp->address == mem_addr) {
+			*hit_watchpoint = wp;
+			LOG_DEBUG("Hit address=%" TARGET_PRIxADDR, wp->address);
+			return ERROR_OK;
+		}
+		wp = wp->next;
+	}
+
+	/* No match found - either we hit a watchpoint caused by an instruction that
+	 * this function does not yet disassemble, or we hit a breakpoint.
+	 *
+	 * OpenOCD will behave as if this function had never been implemented i.e.
+	 * report the halt to GDB with no address information. */
+	return ERROR_FAIL;
+}
+
+
 static int oldriscv_step(struct target *target, int current, uint32_t address,
 		int handle_breakpoints)
 {
@@ -718,13 +833,15 @@ static int old_or_new_riscv_halt(struct target *target)
 
 static int riscv_assert_reset(struct target *target)
 {
+	LOG_DEBUG("[%d]", target->coreid);
 	struct target_type *tt = get_target_type(target);
+	riscv_invalidate_register_cache(target);
 	return tt->assert_reset(target);
 }
 
 static int riscv_deassert_reset(struct target *target)
 {
-	LOG_DEBUG("RISCV DEASSERT RESET");
+	LOG_DEBUG("[%d]", target->coreid);
 	struct target_type *tt = get_target_type(target);
 	return tt->deassert_reset(target);
 }
@@ -745,8 +862,28 @@ static int old_or_new_riscv_resume(
 		int handle_breakpoints,
 		int debug_execution
 ){
-	RISCV_INFO(r);
 	LOG_DEBUG("handle_breakpoints=%d", handle_breakpoints);
+	if (target->smp) {
+		struct target_list *targets = target->head;
+		int result = ERROR_OK;
+		while (targets) {
+			struct target *t = targets->target;
+			riscv_info_t *r = riscv_info(t);
+			if (r->is_halted == NULL) {
+				if (oldriscv_resume(t, current, address, handle_breakpoints,
+							debug_execution) != ERROR_OK)
+					result = ERROR_FAIL;
+			} else {
+				if (riscv_openocd_resume(t, current, address,
+							handle_breakpoints, debug_execution) != ERROR_OK)
+					result = ERROR_FAIL;
+			}
+			targets = targets->next;
+		}
+		return result;
+	}
+
+	RISCV_INFO(r);
 	if (r->is_halted == NULL)
 		return oldriscv_resume(target, current, address, handle_breakpoints, debug_execution);
 	else
@@ -756,9 +893,11 @@ static int old_or_new_riscv_resume(
 static int riscv_select_current_hart(struct target *target)
 {
 	RISCV_INFO(r);
-	if (r->rtos_hartid != -1 && riscv_rtos_enabled(target))
+	if (riscv_rtos_enabled(target)) {
+		if (r->rtos_hartid == -1)
+			r->rtos_hartid = target->rtos->current_threadid - 1;
 		return riscv_set_current_hartid(target, r->rtos_hartid);
-	else
+	} else
 		return riscv_set_current_hartid(target, target->coreid);
 }
 
@@ -780,13 +919,13 @@ static int riscv_write_memory(struct target *target, target_addr_t address,
 	return tt->write_memory(target, address, size, count, buffer);
 }
 
-static int riscv_get_gdb_reg_list(struct target *target,
+static int riscv_get_gdb_reg_list_internal(struct target *target,
 		struct reg **reg_list[], int *reg_list_size,
-		enum target_register_class reg_class)
+		enum target_register_class reg_class, bool read)
 {
 	RISCV_INFO(r);
-	LOG_DEBUG("reg_class=%d", reg_class);
-	LOG_DEBUG("rtos_hartid=%d current_hartid=%d", r->rtos_hartid, r->current_hartid);
+	LOG_DEBUG("rtos_hartid=%d, current_hartid=%d, reg_class=%d, read=%d",
+			r->rtos_hartid, r->current_hartid, reg_class, read);
 
 	if (!target->reg_cache) {
 		LOG_ERROR("Target not initialized. Return ERROR_FAIL.");
@@ -798,10 +937,10 @@ static int riscv_get_gdb_reg_list(struct target *target,
 
 	switch (reg_class) {
 		case REG_CLASS_GENERAL:
-			*reg_list_size = 32;
+			*reg_list_size = 33;
 			break;
 		case REG_CLASS_ALL:
-			*reg_list_size = GDB_REGNO_COUNT;
+			*reg_list_size = target->reg_cache->num_regs;
 			break;
 		default:
 			LOG_ERROR("Unsupported reg_class: %d", reg_class);
@@ -816,11 +955,28 @@ static int riscv_get_gdb_reg_list(struct target *target,
 		assert(!target->reg_cache->reg_list[i].valid ||
 				target->reg_cache->reg_list[i].size > 0);
 		(*reg_list)[i] = &target->reg_cache->reg_list[i];
+		if (read && !target->reg_cache->reg_list[i].valid) {
+			if (target->reg_cache->reg_list[i].type->get(
+						&target->reg_cache->reg_list[i]) != ERROR_OK)
+				/* This function is called when first connecting to gdb,
+				 * resulting in an attempt to read all kinds of registers which
+				 * probably will fail. Ignore these failures, and when
+				 * encountered stop reading to save time. */
+				read = false;
+		}
 	}
 
 	return ERROR_OK;
 }
 
+static int riscv_get_gdb_reg_list(struct target *target,
+		struct reg **reg_list[], int *reg_list_size,
+		enum target_register_class reg_class)
+{
+	return riscv_get_gdb_reg_list_internal(target, reg_list, reg_list_size,
+			reg_class, true);
+}
+
 static int riscv_arch_state(struct target *target)
 {
 	struct target_type *tt = get_target_type(target);
@@ -853,7 +1009,7 @@ static int riscv_run_algorithm(struct target *target, int num_mem_params,
 
 	uint64_t saved_regs[32];
 	for (int i = 0; i < num_reg_params; i++) {
-		if (mem_params[i].direction == PARAM_IN)
+		if (reg_params[i].direction == PARAM_IN)
 			continue;
 
 		LOG_DEBUG("save %s", reg_params[i].reg_name);
@@ -1000,6 +1156,30 @@ static enum riscv_poll_hart riscv_poll_hart(struct target *target, int hartid)
 	return RPH_NO_CHANGE;
 }
 
+int set_debug_reason(struct target *target, int hartid)
+{
+	switch (riscv_halt_reason(target, hartid)) {
+		case RISCV_HALT_BREAKPOINT:
+			target->debug_reason = DBG_REASON_BREAKPOINT;
+			break;
+		case RISCV_HALT_TRIGGER:
+			target->debug_reason = DBG_REASON_WATCHPOINT;
+			break;
+		case RISCV_HALT_INTERRUPT:
+			target->debug_reason = DBG_REASON_DBGRQ;
+			break;
+		case RISCV_HALT_SINGLESTEP:
+			target->debug_reason = DBG_REASON_SINGLESTEP;
+			break;
+		case RISCV_HALT_UNKNOWN:
+			target->debug_reason = DBG_REASON_UNDEFINED;
+			break;
+		case RISCV_HALT_ERROR:
+			return ERROR_FAIL;
+	}
+	return ERROR_OK;
+}
+
 /*** OpenOCD Interface ***/
 int riscv_openocd_poll(struct target *target)
 {
@@ -1034,6 +1214,64 @@ int riscv_openocd_poll(struct target *target)
 		 * harts. */
 		for (int i = 0; i < riscv_count_harts(target); ++i)
 			riscv_halt_one_hart(target, i);
+
+	} else if (target->smp) {
+		bool halt_discovered = false;
+		bool newly_halted[128] = {0};
+		unsigned i = 0;
+		for (struct target_list *list = target->head; list != NULL;
+				list = list->next, i++) {
+			struct target *t = list->target;
+			riscv_info_t *r = riscv_info(t);
+			assert(i < DIM(newly_halted));
+			enum riscv_poll_hart out = riscv_poll_hart(t, r->current_hartid);
+			switch (out) {
+				case RPH_NO_CHANGE:
+					break;
+				case RPH_DISCOVERED_RUNNING:
+					t->state = TARGET_RUNNING;
+					break;
+				case RPH_DISCOVERED_HALTED:
+					halt_discovered = true;
+					newly_halted[i] = true;
+					t->state = TARGET_HALTED;
+					if (set_debug_reason(t, r->current_hartid) != ERROR_OK)
+						return ERROR_FAIL;
+					break;
+				case RPH_ERROR:
+					return ERROR_FAIL;
+			}
+		}
+
+		if (halt_discovered) {
+			LOG_DEBUG("Halt other targets in this SMP group.");
+			i = 0;
+			for (struct target_list *list = target->head; list != NULL;
+					list = list->next, i++) {
+				struct target *t = list->target;
+				riscv_info_t *r = riscv_info(t);
+				if (t->state != TARGET_HALTED) {
+					if (riscv_halt_one_hart(t, r->current_hartid) != ERROR_OK)
+						return ERROR_FAIL;
+					t->state = TARGET_HALTED;
+					if (set_debug_reason(t, r->current_hartid) != ERROR_OK)
+						return ERROR_FAIL;
+					newly_halted[i] = true;
+				}
+			}
+
+			/* Now that we have all our ducks in a row, tell the higher layers
+			 * what just happened. */
+			i = 0;
+			for (struct target_list *list = target->head; list != NULL;
+					list = list->next, i++) {
+				struct target *t = list->target;
+				if (newly_halted[i])
+					target_call_event_callbacks(t, TARGET_EVENT_HALTED);
+			}
+		}
+		return ERROR_OK;
+
 	} else {
 		enum riscv_poll_hart out = riscv_poll_hart(target,
 				riscv_current_hartid(target));
@@ -1047,29 +1285,13 @@ int riscv_openocd_poll(struct target *target)
 	}
 
 	target->state = TARGET_HALTED;
-	switch (riscv_halt_reason(target, halted_hart)) {
-	case RISCV_HALT_BREAKPOINT:
-		target->debug_reason = DBG_REASON_BREAKPOINT;
-		break;
-	case RISCV_HALT_TRIGGER:
-		target->debug_reason = DBG_REASON_WATCHPOINT;
-		break;
-	case RISCV_HALT_INTERRUPT:
-		target->debug_reason = DBG_REASON_DBGRQ;
-		break;
-	case RISCV_HALT_SINGLESTEP:
-		target->debug_reason = DBG_REASON_SINGLESTEP;
-		break;
-	case RISCV_HALT_UNKNOWN:
-		target->debug_reason = DBG_REASON_UNDEFINED;
-		break;
-	case RISCV_HALT_ERROR:
+	if (set_debug_reason(target, halted_hart) != ERROR_OK)
 		return ERROR_FAIL;
-	}
 
 	if (riscv_rtos_enabled(target)) {
 		target->rtos->current_threadid = halted_hart + 1;
 		target->rtos->current_thread = halted_hart + 1;
+		riscv_set_rtos_hartid(target, halted_hart);
 	}
 
 	target->state = TARGET_HALTED;
@@ -1087,25 +1309,39 @@ int riscv_openocd_poll(struct target *target)
 int riscv_openocd_halt(struct target *target)
 {
 	RISCV_INFO(r);
+	int result;
 
-	LOG_DEBUG("halting all harts");
+	LOG_DEBUG("[%d] halting all harts", target->coreid);
 
-	int out = riscv_halt_all_harts(target);
-	if (out != ERROR_OK) {
-		LOG_ERROR("Unable to halt all harts");
-		return out;
+	if (target->smp) {
+		LOG_DEBUG("Halt other targets in this SMP group.");
+		struct target_list *targets = target->head;
+		result = ERROR_OK;
+		while (targets) {
+			struct target *t = targets->target;
+			targets = targets->next;
+			if (t->state != TARGET_HALTED) {
+				if (riscv_halt_all_harts(t) != ERROR_OK)
+					result = ERROR_FAIL;
+			}
+		}
+	} else {
+		result = riscv_halt_all_harts(target);
 	}
 
-	register_cache_invalidate(target->reg_cache);
 	if (riscv_rtos_enabled(target)) {
-		target->rtos->current_threadid = r->rtos_hartid + 1;
-		target->rtos->current_thread = r->rtos_hartid + 1;
+		if (r->rtos_hartid != -1) {
+			LOG_DEBUG("halt requested on RTOS hartid %d", r->rtos_hartid);
+			target->rtos->current_threadid = r->rtos_hartid + 1;
+			target->rtos->current_thread = r->rtos_hartid + 1;
+		} else
+			LOG_DEBUG("halt requested, but no known RTOS hartid");
 	}
 
 	target->state = TARGET_HALTED;
 	target->debug_reason = DBG_REASON_DBGRQ;
 	target_call_event_callbacks(target, TARGET_EVENT_HALTED);
-	return out;
+	return result;
 }
 
 int riscv_openocd_resume(
@@ -1230,6 +1466,25 @@ COMMAND_HANDLER(riscv_set_reset_timeout_sec)
 	return ERROR_OK;
 }
 
+COMMAND_HANDLER(riscv_test_compliance) {
+
+	struct target *target = get_current_target(CMD_CTX);
+
+	RISCV_INFO(r);
+
+	if (CMD_ARGC > 0) {
+		LOG_ERROR("Command does not take any parameters.");
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	if (r->test_compliance) {
+		return r->test_compliance(target);
+	} else {
+		LOG_ERROR("This target does not support this command (may implement an older version of the spec).");
+		return ERROR_FAIL;
+	}
+}
+
 COMMAND_HANDLER(riscv_set_prefer_sba)
 {
 	if (CMD_ARGC != 1) {
@@ -1253,20 +1508,15 @@ void parse_error(const char *string, char c, unsigned position)
 	LOG_ERROR("%s", buf);
 }
 
-COMMAND_HANDLER(riscv_set_expose_csrs)
+int parse_ranges(range_t **ranges, const char **argv)
 {
-	if (CMD_ARGC != 1) {
-		LOG_ERROR("Command takes exactly 1 parameter");
-		return ERROR_COMMAND_SYNTAX_ERROR;
-	}
-
 	for (unsigned pass = 0; pass < 2; pass++) {
 		unsigned range = 0;
 		unsigned low = 0;
 		bool parse_low = true;
 		unsigned high = 0;
-		for (unsigned i = 0; i == 0 || CMD_ARGV[0][i-1]; i++) {
-			char c = CMD_ARGV[0][i];
+		for (unsigned i = 0; i == 0 || argv[0][i-1]; i++) {
+			char c = argv[0][i];
 			if (isspace(c)) {
 				/* Ignore whitespace. */
 				continue;
@@ -1280,13 +1530,13 @@ COMMAND_HANDLER(riscv_set_expose_csrs)
 					parse_low = false;
 				} else if (c == ',' || c == 0) {
 					if (pass == 1) {
-						expose_csr[range].low = low;
-						expose_csr[range].high = low;
+						(*ranges)[range].low = low;
+						(*ranges)[range].high = low;
 					}
 					low = 0;
 					range++;
 				} else {
-					parse_error(CMD_ARGV[0], c, i);
+					parse_error(argv[0], c, i);
 					return ERROR_COMMAND_SYNTAX_ERROR;
 				}
 
@@ -1297,31 +1547,52 @@ COMMAND_HANDLER(riscv_set_expose_csrs)
 				} else if (c == ',' || c == 0) {
 					parse_low = true;
 					if (pass == 1) {
-						expose_csr[range].low = low;
-						expose_csr[range].high = high;
+						(*ranges)[range].low = low;
+						(*ranges)[range].high = high;
 					}
 					low = 0;
 					high = 0;
 					range++;
 				} else {
-					parse_error(CMD_ARGV[0], c, i);
+					parse_error(argv[0], c, i);
 					return ERROR_COMMAND_SYNTAX_ERROR;
 				}
 			}
 		}
 
 		if (pass == 0) {
-			if (expose_csr)
-				free(expose_csr);
-			expose_csr = calloc(range + 2, sizeof(*expose_csr));
+			if (*ranges)
+				free(*ranges);
+			*ranges = calloc(range + 2, sizeof(range_t));
 		} else {
-			expose_csr[range].low = 1;
-			expose_csr[range].high = 0;
+			(*ranges)[range].low = 1;
+			(*ranges)[range].high = 0;
 		}
 	}
+
 	return ERROR_OK;
 }
 
+COMMAND_HANDLER(riscv_set_expose_csrs)
+{
+	if (CMD_ARGC != 1) {
+		LOG_ERROR("Command takes exactly 1 parameter");
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	return parse_ranges(&expose_csr, CMD_ARGV);
+}
+
+COMMAND_HANDLER(riscv_set_expose_custom)
+{
+	if (CMD_ARGC != 1) {
+		LOG_ERROR("Command takes exactly 1 parameter");
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	return parse_ranges(&expose_custom, CMD_ARGV);
+}
+
 COMMAND_HANDLER(riscv_authdata_read)
 {
 	if (CMD_ARGC != 0) {
@@ -1429,8 +1700,86 @@ COMMAND_HANDLER(riscv_dmi_write)
 	}
 }
 
+COMMAND_HANDLER(riscv_test_sba_config_reg)
+{
+	if (CMD_ARGC != 4) {
+		LOG_ERROR("Command takes exactly 4 arguments");
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	struct target *target = get_current_target(CMD_CTX);
+	RISCV_INFO(r);
+
+	target_addr_t legal_address;
+	uint32_t num_words;
+	target_addr_t illegal_address;
+	bool run_sbbusyerror_test;
+
+	COMMAND_PARSE_NUMBER(target_addr, CMD_ARGV[0], legal_address);
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], num_words);
+	COMMAND_PARSE_NUMBER(target_addr, CMD_ARGV[2], illegal_address);
+	COMMAND_PARSE_ON_OFF(CMD_ARGV[3], run_sbbusyerror_test);
+
+	if (r->test_sba_config_reg) {
+		return r->test_sba_config_reg(target, legal_address, num_words,
+				illegal_address, run_sbbusyerror_test);
+	} else {
+		LOG_ERROR("test_sba_config_reg is not implemented for this target.");
+		return ERROR_FAIL;
+	}
+}
+
+COMMAND_HANDLER(riscv_reset_delays)
+{
+	int wait = 0;
+
+	if (CMD_ARGC > 1) {
+		LOG_ERROR("Command takes at most one argument");
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	if (CMD_ARGC == 1)
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], wait);
+
+	struct target *target = get_current_target(CMD_CTX);
+	RISCV_INFO(r);
+	r->reset_delays_wait = wait;
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(riscv_set_ir)
+{
+	if (CMD_ARGC != 2) {
+		LOG_ERROR("Command takes exactly 2 arguments");
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	uint32_t value;
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], value);
+
+	if (!strcmp(CMD_ARGV[0], "idcode")) {
+		buf_set_u32(ir_idcode, 0, 32, value);
+		return ERROR_OK;
+	} else if (!strcmp(CMD_ARGV[0], "dtmcs")) {
+		buf_set_u32(ir_dtmcontrol, 0, 32, value);
+		return ERROR_OK;
+	} else if (!strcmp(CMD_ARGV[0], "dmi")) {
+		buf_set_u32(ir_dbus, 0, 32, value);
+		return ERROR_OK;
+	} else {
+		return ERROR_FAIL;
+	}
+}
+
 static const struct command_registration riscv_exec_command_handlers[] = {
 	{
+		.name = "test_compliance",
+		.handler = riscv_test_compliance,
+		.mode = COMMAND_EXEC,
+		.usage = "riscv test_compliance",
+		.help = "Runs a basic compliance test suite against the RISC-V Debug Spec."
+	},
+	{
 		.name = "set_command_timeout_sec",
 		.handler = riscv_set_command_timeout_sec,
 		.mode = COMMAND_ANY,
@@ -1462,6 +1811,15 @@ static const struct command_registration riscv_exec_command_handlers[] = {
 				"`init`."
 	},
 	{
+		.name = "expose_custom",
+		.handler = riscv_set_expose_custom,
+		.mode = COMMAND_ANY,
+		.usage = "riscv expose_custom n0[-m0][,n1[-m1]]...",
+		.help = "Configure a list of inclusive ranges for custom registers to "
+			"expose. custom0 is accessed as abstract register number 0xc000, "
+			"etc. This must be executed before `init`."
+	},
+	{
 		.name = "authdata_read",
 		.handler = riscv_authdata_read,
 		.mode = COMMAND_ANY,
@@ -1489,6 +1847,36 @@ static const struct command_registration riscv_exec_command_handlers[] = {
 		.usage = "riscv dmi_write address value",
 		.help = "Perform a 32-bit DMI write of value at address."
 	},
+	{
+		.name = "test_sba_config_reg",
+		.handler = riscv_test_sba_config_reg,
+		.mode = COMMAND_ANY,
+		.usage = "riscv test_sba_config_reg legal_address num_words"
+			"illegal_address run_sbbusyerror_test[on/off]",
+		.help = "Perform a series of tests on the SBCS register."
+			"Inputs are a legal, 128-byte aligned address and a number of words to"
+			"read/write starting at that address (i.e., address range [legal address,"
+			"legal_address+word_size*num_words) must be legally readable/writable)"
+			", an illegal, 128-byte aligned address for error flag/handling cases,"
+			"and whether sbbusyerror test should be run."
+	},
+	{
+		.name = "reset_delays",
+		.handler = riscv_reset_delays,
+		.mode = COMMAND_ANY,
+		.usage = "reset_delays [wait]",
+		.help = "OpenOCD learns how many Run-Test/Idle cycles are required "
+			"between scans to avoid encountering the target being busy. This "
+			"command resets those learned values after `wait` scans. It's only "
+			"useful for testing OpenOCD itself."
+	},
+	{
+		.name = "set_ir",
+		.handler = riscv_set_ir,
+		.mode = COMMAND_ANY,
+		.usage = "riscv set_ir_idcode [idcode|dtmcs|dmi] value",
+		.help = "Set IR value for specified JTAG register."
+	},
 	COMMAND_REGISTRATION_DONE
 };
 
@@ -1594,6 +1982,7 @@ struct target_type riscv_target = {
 
 	.add_watchpoint = riscv_add_watchpoint,
 	.remove_watchpoint = riscv_remove_watchpoint,
+	.hit_watchpoint = riscv_hit_watchpoint,
 
 	.arch_state = riscv_arch_state,
 
@@ -1632,6 +2021,8 @@ int riscv_halt_all_harts(struct target *target)
 		riscv_halt_one_hart(target, i);
 	}
 
+	riscv_invalidate_register_cache(target);
+
 	return ERROR_OK;
 }
 
@@ -1646,7 +2037,9 @@ int riscv_halt_one_hart(struct target *target, int hartid)
 		return ERROR_OK;
 	}
 
-	return r->halt_current_hart(target);
+	int result = r->halt_current_hart(target);
+	register_cache_invalidate(target->reg_cache);
+	return result;
 }
 
 int riscv_resume_all_harts(struct target *target)
@@ -1684,7 +2077,7 @@ int riscv_step_rtos_hart(struct target *target)
 	if (riscv_rtos_enabled(target)) {
 		hartid = r->rtos_hartid;
 		if (hartid == -1) {
-			LOG_USER("GDB has asked me to step \"any\" thread, so I'm stepping hart 0.");
+			LOG_DEBUG("GDB has asked me to step \"any\" thread, so I'm stepping hart 0.");
 			hartid = 0;
 		}
 	}
@@ -1734,9 +2127,10 @@ int riscv_xlen_of_hart(const struct target *target, int hartid)
 	return r->xlen[hartid];
 }
 
+extern struct rtos_type riscv_rtos;
 bool riscv_rtos_enabled(const struct target *target)
 {
-	return target->rtos != NULL;
+	return false;
 }
 
 int riscv_set_current_hartid(struct target *target, int hartid)
@@ -1754,19 +2148,9 @@ int riscv_set_current_hartid(struct target *target, int hartid)
 
 	/* This might get called during init, in which case we shouldn't be
 	 * setting up the register cache. */
-	if (!target_was_examined(target))
-		return ERROR_OK;
+	if (target_was_examined(target) && riscv_rtos_enabled(target))
+		riscv_invalidate_register_cache(target);
 
-	/* Avoid invalidating the register cache all the time. */
-	if (r->registers_initialized
-			&& (!riscv_rtos_enabled(target) || (previous_hartid == hartid))
-			&& target->reg_cache->reg_list[GDB_REGNO_ZERO].size == (unsigned)riscv_xlen(target)
-			&& (!riscv_rtos_enabled(target) || (r->rtos_hartid != -1))) {
-		return ERROR_OK;
-	} else
-		LOG_DEBUG("Initializing registers: xlen=%d", riscv_xlen(target));
-
-	riscv_invalidate_register_cache(target);
 	return ERROR_OK;
 }
 
@@ -1774,8 +2158,9 @@ void riscv_invalidate_register_cache(struct target *target)
 {
 	RISCV_INFO(r);
 
+	LOG_DEBUG("[%d]", target->coreid);
 	register_cache_invalidate(target->reg_cache);
-	for (size_t i = 0; i < GDB_REGNO_COUNT; ++i) {
+	for (size_t i = 0; i < target->reg_cache->num_regs; ++i) {
 		struct reg *reg = &target->reg_cache->reg_list[i];
 		reg->valid = false;
 	}
@@ -1830,7 +2215,7 @@ int riscv_set_register_on_hart(struct target *target, int hartid,
 		enum gdb_regno regid, uint64_t value)
 {
 	RISCV_INFO(r);
-	LOG_DEBUG("[%d] %s <- %" PRIx64, hartid, gdb_regno_name(regid), value);
+	LOG_DEBUG("{%d} %s <- %" PRIx64, hartid, gdb_regno_name(regid), value);
 	assert(r->set_register);
 	return r->set_register(target, hartid, regid, value);
 }
@@ -1846,8 +2231,17 @@ int riscv_get_register_on_hart(struct target *target, riscv_reg_t *value,
 		int hartid, enum gdb_regno regid)
 {
 	RISCV_INFO(r);
+
+	struct reg *reg = &target->reg_cache->reg_list[regid];
+
+	if (reg && reg->valid && hartid == riscv_current_hartid(target)) {
+		*value = buf_get_u64(reg->value, 0, reg->size);
+		return ERROR_OK;
+	}
+
 	int result = r->get_register(target, value, hartid, regid);
-	LOG_DEBUG("[%d] %s: %" PRIx64, hartid, gdb_regno_name(regid), *value);
+
+	LOG_DEBUG("{%d} %s: %" PRIx64, hartid, gdb_regno_name(regid), *value);
 	return result;
 }
 
@@ -2048,24 +2442,42 @@ const char *gdb_regno_name(enum gdb_regno regno)
 
 static int register_get(struct reg *reg)
 {
-	struct target *target = (struct target *) reg->arch_info;
+	riscv_reg_info_t *reg_info = reg->arch_info;
+	struct target *target = reg_info->target;
 	uint64_t value;
 	int result = riscv_get_register(target, &value, reg->number);
 	if (result != ERROR_OK)
 		return result;
 	buf_set_u64(reg->value, 0, reg->size, value);
+	/* CSRs (and possibly other extension) registers may change value at any
+	 * time. */
+	if (reg->number <= GDB_REGNO_XPR31 ||
+			(reg->number >= GDB_REGNO_FPR0 && reg->number <= GDB_REGNO_FPR31) ||
+			reg->number == GDB_REGNO_PC)
+		reg->valid = true;
+	LOG_DEBUG("[%d]{%d} read 0x%" PRIx64 " from %s (valid=%d)",
+			target->coreid, riscv_current_hartid(target), value, reg->name,
+			reg->valid);
 	return ERROR_OK;
 }
 
 static int register_set(struct reg *reg, uint8_t *buf)
 {
-	struct target *target = (struct target *) reg->arch_info;
+	riscv_reg_info_t *reg_info = reg->arch_info;
+	struct target *target = reg_info->target;
 
 	uint64_t value = buf_get_u64(buf, 0, reg->size);
 
-	LOG_DEBUG("write 0x%" PRIx64 " to %s", value, reg->name);
+	LOG_DEBUG("[%d]{%d} write 0x%" PRIx64 " to %s (valid=%d)",
+			target->coreid, riscv_current_hartid(target), value, reg->name,
+			reg->valid);
 	struct reg *r = &target->reg_cache->reg_list[reg->number];
-	r->valid = true;
+	/* CSRs (and possibly other extension) registers may change value at any
+	 * time. */
+	if (reg->number <= GDB_REGNO_XPR31 ||
+			(reg->number >= GDB_REGNO_FPR0 && reg->number <= GDB_REGNO_FPR31) ||
+			reg->number == GDB_REGNO_PC)
+		r->valid = true;
 	memcpy(r->value, buf, (r->size + 7) / 8);
 
 	riscv_set_register(target, reg->number, value);
@@ -2101,12 +2513,26 @@ int riscv_init_registers(struct target *target)
 	target->reg_cache->name = "RISC-V Registers";
 	target->reg_cache->num_regs = GDB_REGNO_COUNT;
 
-	target->reg_cache->reg_list = calloc(GDB_REGNO_COUNT, sizeof(struct reg));
+	if (expose_custom) {
+		for (unsigned i = 0; expose_custom[i].low <= expose_custom[i].high; i++) {
+			for (unsigned number = expose_custom[i].low;
+					number <= expose_custom[i].high;
+					number++)
+				target->reg_cache->num_regs++;
+		}
+	}
+
+	LOG_DEBUG("create register cache for %d registers",
+			target->reg_cache->num_regs);
+
+	target->reg_cache->reg_list =
+		calloc(target->reg_cache->num_regs, sizeof(struct reg));
 
 	const unsigned int max_reg_name_len = 12;
 	if (info->reg_names)
 		free(info->reg_names);
-	info->reg_names = calloc(1, GDB_REGNO_COUNT * max_reg_name_len);
+	info->reg_names =
+		calloc(target->reg_cache->num_regs, max_reg_name_len);
 	char *reg_name = info->reg_names;
 
 	static struct reg_feature feature_cpu = {
@@ -2121,6 +2547,9 @@ int riscv_init_registers(struct target *target)
 	static struct reg_feature feature_virtual = {
 		.name = "org.gnu.gdb.riscv.virtual"
 	};
+	static struct reg_feature feature_custom = {
+		.name = "org.gnu.gdb.riscv.custom"
+	};
 
 	static struct reg_data_type type_ieee_single = {
 		.type = REG_TYPE_IEEE_SINGLE,
@@ -2139,18 +2568,24 @@ int riscv_init_registers(struct target *target)
 	qsort(csr_info, DIM(csr_info), sizeof(*csr_info), cmp_csr_info);
 	unsigned csr_info_index = 0;
 
-	/* When gdb request register N, gdb_get_register_packet() assumes that this
+	unsigned custom_range_index = 0;
+	int custom_within_range = 0;
+
+	riscv_reg_info_t *shared_reg_info = calloc(1, sizeof(riscv_reg_info_t));
+	shared_reg_info->target = target;
+
+	/* When gdb requests register N, gdb_get_register_packet() assumes that this
 	 * is register at index N in reg_list. So if there are certain registers
 	 * that don't exist, we need to leave holes in the list (or renumber, but
 	 * it would be nice not to have yet another set of numbers to translate
 	 * between). */
-	for (uint32_t number = 0; number < GDB_REGNO_COUNT; number++) {
+	for (uint32_t number = 0; number < target->reg_cache->num_regs; number++) {
 		struct reg *r = &target->reg_cache->reg_list[number];
 		r->dirty = false;
 		r->valid = false;
 		r->exist = true;
 		r->type = &riscv_reg_arch_type;
-		r->arch_info = target;
+		r->arch_info = shared_reg_info;
 		r->number = number;
 		r->size = riscv_xlen(target);
 		/* r->size is set in riscv_invalidate_register_cache, maybe because the
@@ -2510,11 +2945,35 @@ int riscv_init_registers(struct target *target)
 			r->group = "general";
 			r->feature = &feature_virtual;
 			r->size = 8;
+
+		} else {
+			/* Custom registers. */
+			assert(expose_custom);
+
+			range_t *range = &expose_custom[custom_range_index];
+			assert(range->low <= range->high);
+			unsigned custom_number = range->low + custom_within_range;
+
+			r->group = "custom";
+			r->feature = &feature_custom;
+			r->arch_info = calloc(1, sizeof(riscv_reg_info_t));
+			assert(r->arch_info);
+			((riscv_reg_info_t *) r->arch_info)->target = target;
+			((riscv_reg_info_t *) r->arch_info)->custom_number = custom_number;
+			sprintf(reg_name, "custom%d", custom_number);
+
+			custom_within_range++;
+			if (custom_within_range > range->high - range->low) {
+				custom_within_range = 0;
+				custom_range_index++;
+			}
 		}
+
 		if (reg_name[0])
 			r->name = reg_name;
 		reg_name += strlen(reg_name) + 1;
-		assert(reg_name < info->reg_names + GDB_REGNO_COUNT * max_reg_name_len);
+		assert(reg_name < info->reg_names + target->reg_cache->num_regs *
+				max_reg_name_len);
 		r->value = &info->reg_cache_values[number];
 	}
 
diff --git a/src/target/riscv/riscv.h b/src/target/riscv/riscv.h
index 31f3cf6..59414fc 100644
--- a/src/target/riscv/riscv.h
+++ b/src/target/riscv/riscv.h
@@ -36,6 +36,11 @@ enum riscv_halt_reason {
 };
 
 typedef struct {
+	struct target *target;
+	unsigned custom_number;
+} riscv_reg_info_t;
+
+typedef struct {
 	unsigned dtm_version;
 
 	struct command_context *cmd_ctx;
@@ -91,6 +96,10 @@ typedef struct {
 
 	bool triggers_enumerated;
 
+	/* Decremented every scan, and when it reaches 0 we clear the learned
+	 * delays, causing them to be relearned. Used for testing. */
+	int reset_delays_wait;
+
 	/* Helper functions that target the various RISC-V debug spec
 	 * implementations. */
 	int (*get_register)(struct target *target,
@@ -120,6 +129,11 @@ typedef struct {
 
 	int (*dmi_read)(struct target *target, uint32_t *value, uint32_t address);
 	int (*dmi_write)(struct target *target, uint32_t address, uint32_t value);
+
+	int (*test_sba_config_reg)(struct target *target, target_addr_t legal_address,
+			uint32_t num_words, target_addr_t illegal_address, bool run_sbbusyerror_test);
+
+	int (*test_compliance)(struct target *target);
 } riscv_info_t;
 
 /* Wall-clock timeout for a command/access. Settable via RISC-V Target commands.*/
@@ -137,11 +151,11 @@ 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 uint8_t ir_dtmcontrol[4];
 extern struct scan_field select_dtmcontrol;
-extern uint8_t ir_dbus[1];
+extern uint8_t ir_dbus[4];
 extern struct scan_field select_dbus;
-extern uint8_t ir_idcode[1];
+extern uint8_t ir_idcode[4];
 extern struct scan_field select_idcode;
 
 /*** OpenOCD Interface */
@@ -253,6 +267,7 @@ int riscv_remove_breakpoint(struct target *target,
 int riscv_add_watchpoint(struct target *target, struct watchpoint *watchpoint);
 int riscv_remove_watchpoint(struct target *target,
 		struct watchpoint *watchpoint);
+int riscv_hit_watchpoint(struct target *target, struct watchpoint **hit_wp_address);
 
 int riscv_init_registers(struct target *target);