Merge pull request #131 from riscv/small_progbuf

Support program buffers that are just 2 instructions large

7 files changed, 780 insertions, 1018 deletions

diff --git a/src/target/riscv/batch.c b/src/target/riscv/batch.c
index 1e6a4be..1e6db42 100644
--- a/src/target/riscv/batch.c
+++ b/src/target/riscv/batch.c
@@ -49,13 +49,12 @@ void riscv_batch_run(struct riscv_batch *batch)
 		return;
 	}
 
-  keep_alive();
+	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) {
-		dump_field(batch->fields + i);
 		jtag_add_dr_scan(batch->target->tap, 1, batch->fields + i, TAP_IDLE);
 		if (batch->idle_count > 0)
 			jtag_add_runtest(batch->idle_count, TAP_IDLE);
diff --git a/src/target/riscv/debug_defines.h b/src/target/riscv/debug_defines.h
index 6e88a55..6067dbe 100644
--- a/src/target/riscv/debug_defines.h
+++ b/src/target/riscv/debug_defines.h
@@ -173,12 +173,6 @@
 #define CSR_DCSR_EBREAKM_LENGTH             1
 #define CSR_DCSR_EBREAKM                    (0x1U << CSR_DCSR_EBREAKM_OFFSET)
 /*
-* When 1, {\tt ebreak} instructions in Hypervisor Mode enter Debug Mode.
- */
-#define CSR_DCSR_EBREAKH_OFFSET             14
-#define CSR_DCSR_EBREAKH_LENGTH             1
-#define CSR_DCSR_EBREAKH                    (0x1U << CSR_DCSR_EBREAKH_OFFSET)
-/*
 * When 1, {\tt ebreak} instructions in Supervisor Mode enter Debug Mode.
  */
 #define CSR_DCSR_EBREAKS_OFFSET             13
@@ -207,9 +201,10 @@
 /*
 * 0: Increment counters as usual.
 *
-* 1: Don't increment any counters while in Debug Mode.  This includes
-* the {\tt cycle} and {\tt instret} CSRs. This is preferred for most
-* debugging scenarios.
+* 1: Don't increment any counters while in Debug Mode or on {\tt
+* ebreak} instructions that cause entry into Debug Mode.  These
+* counters include the {\tt cycle} and {\tt instret} CSRs. This is
+* preferred for most debugging scenarios.
 *
 * An implementation may choose not to support writing to this bit.
 * The debugger must read back the value it writes to check whether
@@ -312,9 +307,9 @@
 *
 * This bit is only writable from Debug Mode.
  */
-#define CSR_TDATA1_HMODE_OFFSET             XLEN-5
-#define CSR_TDATA1_HMODE_LENGTH             1
-#define CSR_TDATA1_HMODE                    (0x1ULL << CSR_TDATA1_HMODE_OFFSET)
+#define CSR_TDATA1_DMODE_OFFSET             XLEN-5
+#define CSR_TDATA1_DMODE_LENGTH             1
+#define CSR_TDATA1_DMODE                    (0x1ULL << CSR_TDATA1_DMODE_OFFSET)
 /*
 * Trigger-specific data.
  */
@@ -390,7 +385,7 @@
 * 0: Raise a breakpoint exception. (Used when software wants to use
 * the trigger module without an external debugger attached.)
 *
-* 1: Enter Debug Mode. (Only supported when \Fhmode is 1.)
+* 1: Enter Debug Mode. (Only supported when \Fdmode is 1.)
 *
 * 2: Start tracing.
 *
@@ -532,7 +527,7 @@
 * 0: Raise a breakpoint exception. (Used when software wants to use the
 * trigger module without an external debugger attached.)
 *
-* 1: Enter Debug Mode. (Only supported when \Fhmode is 1.)
+* 1: Enter Debug Mode. (Only supported when \Fdmode is 1.)
 *
 * 2: Start tracing.
 *
@@ -549,6 +544,30 @@
 #define CSR_ICOUNT_ACTION                   (0x3fULL << CSR_ICOUNT_ACTION_OFFSET)
 #define DMI_DMSTATUS                        0x11
 /*
+* If 1, then there is an implicit {\tt ebreak} instruction at the
+* non-existent word immediately after the Program Buffer. This saves
+* the debugger from having to write the {\tt ebreak} itself, and
+* allows the Program Buffer to be one word smaller.
+*
+* This must be 1 when \Fprogbufsize is 1.
+ */
+#define DMI_DMSTATUS_IMPEBREAK_OFFSET       22
+#define DMI_DMSTATUS_IMPEBREAK_LENGTH       1
+#define DMI_DMSTATUS_IMPEBREAK              (0x1U << DMI_DMSTATUS_IMPEBREAK_OFFSET)
+/*
+* Gets set if the Debug Module was accessed incorrectly.
+*
+* 0 (none): No error.
+*
+* 1 (badaddr): There was an access to an unimplemented Debug Module
+* address.
+*
+* 7 (other): An access failed for another reason.
+ */
+#define DMI_DMSTATUS_DMERR_OFFSET           18
+#define DMI_DMSTATUS_DMERR_LENGTH           3
+#define DMI_DMSTATUS_DMERR                  (0x7U << DMI_DMSTATUS_DMERR_OFFSET)
+/*
 * This field is 1 when all currently selected harts have acknowledged the previous \Fresumereq.
  */
 #define DMI_DMSTATUS_ALLRESUMEACK_OFFSET    17
@@ -629,6 +648,13 @@
 #define DMI_DMSTATUS_AUTHBUSY_OFFSET        6
 #define DMI_DMSTATUS_AUTHBUSY_LENGTH        1
 #define DMI_DMSTATUS_AUTHBUSY               (0x1U << DMI_DMSTATUS_AUTHBUSY_OFFSET)
+/*
+* 0: \Rdevtreeaddrzero--\Rdevtreeaddrthree hold information which
+* is not relevant to the Device Tree.
+*
+* 1: \Rdevtreeaddrzero--\Rdevtreeaddrthree registers hold the address of the
+* Device Tree.
+ */
 #define DMI_DMSTATUS_DEVTREEVALID_OFFSET    4
 #define DMI_DMSTATUS_DEVTREEVALID_LENGTH    1
 #define DMI_DMSTATUS_DEVTREEVALID           (0x1U << DMI_DMSTATUS_DEVTREEVALID_OFFSET)
@@ -654,7 +680,6 @@
 *
 * Writing 1 or 0 has no effect on a hart which is already halted, but
 * the bit should be cleared to 0 before the hart is resumed.
-* Setting both \Fhaltreq and \Fresumereq leads to undefined behavior.
 *
 * Writes apply to the new value of \Fhartsel and \Fhasel.
  */
@@ -664,7 +689,8 @@
 /*
 * Resume request signal for all currently selected harts. When set to 1,
 * each selected hart will resume if it is currently halted.
-* Setting both \Fhaltreq and \Fresumereq leads to undefined behavior.
+*
+* This bit is ignored while \Fhaltreq is set.
 *
 * Writes apply to the new value of \Fhartsel and \Fhasel.
  */
@@ -710,11 +736,12 @@
 #define DMI_DMCONTROL_HARTSEL               (0x3ffU << DMI_DMCONTROL_HARTSEL_OFFSET)
 /*
 * This bit controls the reset signal from the DM to the rest of the
-* system. To perform a system reset the debugger writes 1,
+* system. The signal should reset every part of the system, including
+* every hart, except for the DM and any logic required to access the
+* DM.
+* To perform a system reset the debugger writes 1,
 * and then writes 0
-* to deassert the reset. This bit must not reset the Debug Module
-* registers. What it does reset is platform-specific (it may
-* reset nothing).
+* to deassert the reset.
  */
 #define DMI_DMCONTROL_NDMRESET_OFFSET       1
 #define DMI_DMCONTROL_NDMRESET_LENGTH       1
@@ -778,7 +805,7 @@
 * shadowing the {\tt data} registers.
 *
 * If \Fdataaccess is 1: Signed address of RAM where the {\tt data}
-* registers are shadowed.
+* registers are shadowed, to be used to access relative to \Rzero.
  */
 #define DMI_HARTINFO_DATAADDR_OFFSET        0
 #define DMI_HARTINFO_DATAADDR_LENGTH        12
@@ -891,13 +918,10 @@
 #define DMI_ABSTRACTCS                      0x16
 /*
 * Size of the Program Buffer, in 32-bit words. Valid sizes are 0 - 16.
-*
-* TODO: Explain what can be done with each size of the buffer, to suggest
-* why you would want more or less words.
  */
-#define DMI_ABSTRACTCS_PROGSIZE_OFFSET      24
-#define DMI_ABSTRACTCS_PROGSIZE_LENGTH      5
-#define DMI_ABSTRACTCS_PROGSIZE             (0x1fU << DMI_ABSTRACTCS_PROGSIZE_OFFSET)
+#define DMI_ABSTRACTCS_PROGBUFSIZE_OFFSET   24
+#define DMI_ABSTRACTCS_PROGBUFSIZE_LENGTH   5
+#define DMI_ABSTRACTCS_PROGBUFSIZE          (0x1fU << DMI_ABSTRACTCS_PROGBUFSIZE_OFFSET)
 /*
 * 1: An abstract command is currently being executed.
 *
@@ -1013,24 +1037,22 @@
 *
 * 4: 128-bit
 *
-* If an unsupported system bus access size is written here,
-* the DM may not perform the access, or may perform the access
-* with any access size.
+* If an unsupported system bus access size is written here, the DM
+* does not perform the access and sberror is set to 3.
  */
 #define DMI_SBCS_SBACCESS_OFFSET            17
 #define DMI_SBCS_SBACCESS_LENGTH            3
 #define DMI_SBCS_SBACCESS                   (0x7U << DMI_SBCS_SBACCESS_OFFSET)
 /*
-* When 1, the internal address value (used by the system bus master)
-* is incremented by the access size (in bytes) selected in \Fsbaccess
-* after every system bus access.
+* When 1, {\tt sbaddress} is incremented by the access size (in
+* bytes) selected in \Fsbaccess after every system bus access.
  */
 #define DMI_SBCS_SBAUTOINCREMENT_OFFSET     16
 #define DMI_SBCS_SBAUTOINCREMENT_LENGTH     1
 #define DMI_SBCS_SBAUTOINCREMENT            (0x1U << DMI_SBCS_SBAUTOINCREMENT_OFFSET)
 /*
-* When 1, every read from \Rsbdatazero automatically triggers a system
-* bus read at the new address.
+* When 1, every read from \Rsbdatazero automatically triggers a
+* system bus read at the (possibly auto-incremented) address.
  */
 #define DMI_SBCS_SBAUTOREAD_OFFSET          15
 #define DMI_SBCS_SBAUTOREAD_LENGTH          1
@@ -1052,8 +1074,7 @@
 *
 * 4: The system bus master was busy when one of the
 * {\tt sbaddress} or {\tt sbdata} registers was written,
-* or the {\tt sbdata} register was read when it had
-* stale data.
+* or \Rsbdatazero was read when it had stale data.
  */
 #define DMI_SBCS_SBERROR_OFFSET             12
 #define DMI_SBCS_SBERROR_LENGTH             3
@@ -1097,54 +1118,54 @@
 #define DMI_SBCS_SBACCESS8                  (0x1U << DMI_SBCS_SBACCESS8_OFFSET)
 #define DMI_SBADDRESS0                      0x39
 /*
-* Accesses bits 31:0 of the internal address.
+* Accesses bits 31:0 of the physical address in {\tt sbaddress}.
  */
 #define DMI_SBADDRESS0_ADDRESS_OFFSET       0
 #define DMI_SBADDRESS0_ADDRESS_LENGTH       32
 #define DMI_SBADDRESS0_ADDRESS              (0xffffffffU << DMI_SBADDRESS0_ADDRESS_OFFSET)
 #define DMI_SBADDRESS1                      0x3a
 /*
-* Accesses bits 63:32 of the internal address (if the system address
-* bus is that wide).
+* Accesses bits 63:32 of the physical address in {\tt sbaddress} (if
+* the system address bus is that wide).
  */
 #define DMI_SBADDRESS1_ADDRESS_OFFSET       0
 #define DMI_SBADDRESS1_ADDRESS_LENGTH       32
 #define DMI_SBADDRESS1_ADDRESS              (0xffffffffU << DMI_SBADDRESS1_ADDRESS_OFFSET)
 #define DMI_SBADDRESS2                      0x3b
 /*
-* Accesses bits 95:64 of the internal address (if the system address
-* bus is that wide).
+* Accesses bits 95:64 of the physical address in {\tt sbaddress} (if
+* the system address bus is that wide).
  */
 #define DMI_SBADDRESS2_ADDRESS_OFFSET       0
 #define DMI_SBADDRESS2_ADDRESS_LENGTH       32
 #define DMI_SBADDRESS2_ADDRESS              (0xffffffffU << DMI_SBADDRESS2_ADDRESS_OFFSET)
 #define DMI_SBDATA0                         0x3c
 /*
-* Accesses bits 31:0 of the internal data.
+* Accesses bits 31:0 of {\tt sbdata}.
  */
 #define DMI_SBDATA0_DATA_OFFSET             0
 #define DMI_SBDATA0_DATA_LENGTH             32
 #define DMI_SBDATA0_DATA                    (0xffffffffU << DMI_SBDATA0_DATA_OFFSET)
 #define DMI_SBDATA1                         0x3d
 /*
-* Accesses bits 63:32 of the internal data (if the system bus is
-* that wide).
+* Accesses bits 63:32 of {\tt sbdata} (if the system bus is that
+* wide).
  */
 #define DMI_SBDATA1_DATA_OFFSET             0
 #define DMI_SBDATA1_DATA_LENGTH             32
 #define DMI_SBDATA1_DATA                    (0xffffffffU << DMI_SBDATA1_DATA_OFFSET)
 #define DMI_SBDATA2                         0x3e
 /*
-* Accesses bits 95:64 of the internal data (if the system bus is
-* that wide).
+* Accesses bits 95:64 of {\tt sbdata} (if the system bus is that
+* wide).
  */
 #define DMI_SBDATA2_DATA_OFFSET             0
 #define DMI_SBDATA2_DATA_LENGTH             32
 #define DMI_SBDATA2_DATA                    (0xffffffffU << DMI_SBDATA2_DATA_OFFSET)
 #define DMI_SBDATA3                         0x3f
 /*
-* Accesses bits 127:96 of the internal data (if the system bus is
-* that wide).
+* Accesses bits 127:96 of {\tt sbdata} (if the system bus is that
+* wide).
  */
 #define DMI_SBDATA3_DATA_OFFSET             0
 #define DMI_SBDATA3_DATA_LENGTH             32
@@ -1188,6 +1209,9 @@
 * 0: Don't do the operation specified by \Fwrite.
 *
 * 1: Do the operation specified by \Fwrite.
+*
+* This bit can be used to just execute the Program Buffer without
+* having to worry about placing valid values into \Fsize or \Fregno.
  */
 #define AC_ACCESS_REGISTER_TRANSFER_OFFSET  17
 #define AC_ACCESS_REGISTER_TRANSFER_LENGTH  1
@@ -1223,8 +1247,9 @@
 /*
 * Contains the privilege level the hart was operating in when Debug
 * Mode was entered. The encoding is described in Table
-* \ref{tab:privlevel}. A user can write this value to change the
-* hart's privilege level when exiting Debug Mode.
+* \ref{tab:privlevel}, and matches the privilege level encoding from
+* the RISC-V Privileged ISA Specification. A user can write this
+* value to change the hart's privilege level when exiting Debug Mode.
  */
 #define VIRT_PRIV_PRV_OFFSET                0
 #define VIRT_PRIV_PRV_LENGTH                2
diff --git a/src/target/riscv/program.c b/src/target/riscv/program.c
index d052574..e7238dd 100644
--- a/src/target/riscv/program.c
+++ b/src/target/riscv/program.c
@@ -11,44 +11,39 @@
 #include "asm.h"
 #include "encoding.h"
 
-riscv_addr_t riscv_program_gal(struct riscv_program *p, riscv_addr_t addr);
-int riscv_program_lah(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr);
-int riscv_program_lal(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr);
-
 /* Program interface. */
 int riscv_program_init(struct riscv_program *p, struct target *target)
 {
 	memset(p, 0, sizeof(*p));
 	p->target = target;
 	p->instruction_count = 0;
-	p->data_count = 0;
-	p->writes_memory = 0;
 	p->target_xlen = riscv_xlen(target);
 	for (size_t i = 0; i < RISCV_REGISTER_COUNT; ++i) {
 		p->writes_xreg[i] = 0;
-		p->in_use[i] = 0;
 	}
 
 	for(size_t i = 0; i < RISCV_MAX_DEBUG_BUFFER_SIZE; ++i)
 		p->debug_buffer[i] = -1;
 
-	if (riscv_debug_buffer_enter(target, p) != ERROR_OK) {
-		LOG_ERROR("unable to write progam buffer enter code");
-		return ERROR_FAIL;
-	}
+	return ERROR_OK;
+}
 
+int riscv_program_write(struct riscv_program *program)
+{
+	for (unsigned i = 0; i < program->instruction_count; ++i) {
+		LOG_DEBUG("%p: debug_buffer[%02x] = DASM(0x%08x)", program, i, program->debug_buffer[i]);
+		if (riscv_write_debug_buffer(program->target, i,
+					program->debug_buffer[i]) != ERROR_OK)
+			return ERROR_FAIL;
+	}
 	return ERROR_OK;
 }
 
+/** Add ebreak and execute the program. */
 int riscv_program_exec(struct riscv_program *p, struct target *t)
 {
 	keep_alive();
 
-	if (riscv_debug_buffer_leave(t, p) != ERROR_OK) {
-		LOG_ERROR("unable to write program buffer exit code");
-		return ERROR_FAIL;
-	}
-
 	riscv_reg_t saved_registers[GDB_REGNO_XPR31 + 1];
 	for (size_t i = GDB_REGNO_XPR0 + 1; i <= GDB_REGNO_XPR31; ++i) {
 		if (p->writes_xreg[i]) {
@@ -57,14 +52,6 @@ int riscv_program_exec(struct riscv_program *p, struct target *t)
 		}
 	}
 
-	if (p->writes_memory && (riscv_program_fence(p) != ERROR_OK)) {
-		LOG_ERROR("Unable to write fence");
-		for(size_t i = 0; i < riscv_debug_buffer_size(p->target); ++i)
-			LOG_ERROR("ram[%02x]: DASM(0x%08lx) [0x%08lx]", (int)i, (long)p->debug_buffer[i], (long)p->debug_buffer[i]);
-		abort();
-		return ERROR_FAIL;
-	}
-
 	if (riscv_program_ebreak(p) != ERROR_OK) {
 		LOG_ERROR("Unable to write ebreak");
 		for(size_t i = 0; i < riscv_debug_buffer_size(p->target); ++i)
@@ -73,16 +60,8 @@ int riscv_program_exec(struct riscv_program *p, struct target *t)
 		return ERROR_FAIL;
 	}
 
-	for (unsigned i = 0; i < riscv_debug_buffer_size(p->target); ++i) {
-		if (i < p->instruction_count) {
-			LOG_DEBUG("%p: debug_buffer[%02x] = DASM(0x%08x)", p, i, p->debug_buffer[i]);
-			riscv_write_debug_buffer(t, i, p->debug_buffer[i]);
-		}
-		if (i >= riscv_debug_buffer_size(p->target) - p->data_count) {
-			LOG_DEBUG("%p: debug_buffer[%02x] = 0x%08x", p, i, p->debug_buffer[i]);
-			riscv_write_debug_buffer(t, i, p->debug_buffer[i]);
-		}
-	}
+	if (riscv_program_write(p) != ERROR_OK)
+		return ERROR_FAIL;
 
 	if (riscv_execute_debug_buffer(t) != ERROR_OK) {
 		LOG_ERROR("Unable to execute program %p", p);
@@ -90,7 +69,7 @@ int riscv_program_exec(struct riscv_program *p, struct target *t)
 	}
 
 	for (size_t i = 0; i < riscv_debug_buffer_size(p->target); ++i)
-		if (i >= riscv_debug_buffer_size(p->target) - p->data_count)
+		if (i >= riscv_debug_buffer_size(p->target))
 			p->debug_buffer[i] = riscv_read_debug_buffer(t, i);
 
 	for (size_t i = GDB_REGNO_XPR0; i <= GDB_REGNO_XPR31; ++i)
@@ -100,236 +79,39 @@ int riscv_program_exec(struct riscv_program *p, struct target *t)
 	return ERROR_OK;
 }
 
-riscv_addr_t riscv_program_alloc_data(struct riscv_program *p, size_t bytes)
-{
-	riscv_addr_t addr = 
-		riscv_debug_buffer_addr(p->target) 
-		+ riscv_debug_buffer_size(p->target) * sizeof(p->debug_buffer[0])
-		- p->data_count * sizeof(p->debug_buffer[0])
-		- bytes;
-	while (addr % bytes != 0) addr--;
-
-	riscv_addr_t ptop =
-		riscv_debug_buffer_addr(p->target)
-		+ p->instruction_count * sizeof(p->debug_buffer[0]);
-
-	if (addr <= ptop) {
-		LOG_ERROR("unable to allocate %d bytes", (int)bytes);
-		return RISCV_PROGRAM_ALLOC_FAIL;
-	}
-
-	p->data_count = 
-		+ riscv_debug_buffer_size(p->target)
-		- (addr - riscv_debug_buffer_addr(p->target)) / sizeof(p->debug_buffer[0]);
-	return addr;
-}
-
-riscv_addr_t riscv_program_alloc_x(struct riscv_program *p)
-{
-	return riscv_program_alloc_data(p, p->target_xlen / 8);
-}
-
-riscv_addr_t riscv_program_alloc_d(struct riscv_program *p)
-{
-	return riscv_program_alloc_data(p, 8);
-}
-
-riscv_addr_t riscv_program_alloc_w(struct riscv_program *p)
-{
-	return riscv_program_alloc_data(p, 4);
-}
-
-riscv_addr_t riscv_program_alloc_h(struct riscv_program *p)
-{
-	return riscv_program_alloc_data(p, 2);
-}
-
-riscv_addr_t riscv_program_alloc_b(struct riscv_program *p)
-{
-	return riscv_program_alloc_data(p, 1);
-}
-
-riscv_insn_t riscv_program_read_ram(struct riscv_program *p, riscv_addr_t addr)
-{
-	if (addr < riscv_debug_buffer_addr(p->target))
-		return -1;
-	if ((size_t)addr > riscv_debug_buffer_addr(p->target) + (riscv_debug_buffer_size(p->target) * sizeof(p->debug_buffer[0])))
-		return -1;
-
-	int off = (addr - riscv_debug_buffer_addr(p->target)) / sizeof(p->debug_buffer[0]);
-	return p->debug_buffer[off];
-}
-
-void riscv_program_write_ram(struct riscv_program *p, riscv_addr_t addr, uint64_t d)
-{
-	if (addr < riscv_debug_buffer_addr(p->target))
-		return;
-	if ((size_t)addr > riscv_debug_buffer_addr(p->target) + (riscv_debug_buffer_size(p->target) * sizeof(p->debug_buffer[0])))
-		return;
-
-	int off = (addr - riscv_debug_buffer_addr(p->target)) / sizeof(p->debug_buffer[0]);
-	p->debug_buffer[off] = d;
-}
-
 int riscv_program_swr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno b, int offset)
 {
-	p->writes_memory = 1;
 	return riscv_program_insert(p, sw(d, b, offset));
 }
 
 int riscv_program_shr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno b, int offset)
 {
-	p->writes_memory = 1;
 	return riscv_program_insert(p, sh(d, b, offset));
 }
 
 int riscv_program_sbr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno b, int offset)
 {
-	p->writes_memory = 1;
 	return riscv_program_insert(p, sb(d, b, offset));
 }
 
 int riscv_program_lwr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno b, int offset)
 {
-	p->writes_memory = 1;
 	return riscv_program_insert(p, lw(d, b, offset));
 }
 
 int riscv_program_lhr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno b, int offset)
 {
-	p->writes_memory = 1;
 	return riscv_program_insert(p, lh(d, b, offset));
 }
 
 int riscv_program_lbr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno b, int offset)
 {
-	p->writes_memory = 1;
 	return riscv_program_insert(p, lb(d, b, offset));
 }
 
-int riscv_program_lx(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
-{
-	switch (p->target_xlen) {
-	case 64:  return riscv_program_ld(p, d, addr);
-	case 32:  return riscv_program_lw(p, d, addr);
-	}
-
-	LOG_ERROR("unknown xlen %d", p->target_xlen);
-	abort();
-	return -1;
-}
-
-int riscv_program_ld(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
-{
-	enum gdb_regno t = riscv_program_gah(p, addr) == 0 ? GDB_REGNO_X0 : d;
-	if (riscv_program_lah(p, d, addr) != ERROR_OK)
-		return ERROR_FAIL;
-	if (riscv_program_insert(p, ld(d, t, riscv_program_gal(p, addr))) != ERROR_OK)
-		return ERROR_FAIL;
-	return ERROR_OK;
-}
-
-int riscv_program_lw(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
-{
-	enum gdb_regno t = riscv_program_gah(p, addr) == 0 ? GDB_REGNO_X0 : d;
-	if (riscv_program_lah(p, d, addr) != ERROR_OK)
-		return ERROR_FAIL;
-	if (riscv_program_insert(p, lw(d, t, riscv_program_gal(p, addr))) != ERROR_OK)
-		return ERROR_FAIL;
-	return ERROR_OK;
-}
-
-int riscv_program_lh(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
-{
-	enum gdb_regno t = riscv_program_gah(p, addr) == 0 ? GDB_REGNO_X0 : d;
-	if (riscv_program_lah(p, d, addr) != ERROR_OK)
-		return ERROR_FAIL;
-	if (riscv_program_insert(p, lh(d, t, riscv_program_gal(p, addr))) != ERROR_OK)
-		return ERROR_FAIL;
-	return ERROR_OK;
-}
-
-int riscv_program_lb(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
-{
-	enum gdb_regno t = riscv_program_gah(p, addr) == 0 ? GDB_REGNO_X0 : d;
-	if (riscv_program_lah(p, t, addr) != ERROR_OK)
-		return ERROR_FAIL;
-	if (riscv_program_insert(p, lb(d, t, riscv_program_gal(p, addr))) != ERROR_OK)
-		return ERROR_FAIL;
-	return ERROR_OK;
-}
-
-int riscv_program_sx(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
-{
-	switch (p->target_xlen) {
-	case 64:  return riscv_program_sd(p, d, addr);
-	case 32:  return riscv_program_sw(p, d, addr);
-	}
-
-	LOG_ERROR("unknown xlen %d", p->target_xlen);
-	abort();
-	return -1;
-}
-
-int riscv_program_sd(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
-{
-	enum gdb_regno t = riscv_program_gah(p, addr) == 0
-		? GDB_REGNO_X0
-		: riscv_program_gettemp(p);
-	if (riscv_program_lah(p, t, addr) != ERROR_OK)
-		return ERROR_FAIL;
-	if (riscv_program_insert(p, sd(d, t, riscv_program_gal(p, addr))) != ERROR_OK)
-		return ERROR_FAIL;
-	riscv_program_puttemp(p, t);
-	p->writes_memory = true;
-	return ERROR_OK;
-}
-
-int riscv_program_sw(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
-{
-	enum gdb_regno t = riscv_program_gah(p, addr) == 0
-		? GDB_REGNO_X0
-		: riscv_program_gettemp(p);
-	if (riscv_program_lah(p, t, addr) != ERROR_OK)
-		return ERROR_FAIL;
-	if (riscv_program_insert(p, sw(d, t, riscv_program_gal(p, addr))) != ERROR_OK)
-		return ERROR_FAIL;
-	riscv_program_puttemp(p, t);
-	p->writes_memory = true;
-	return ERROR_OK;
-}
-
-int riscv_program_sh(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
-{
-	enum gdb_regno t = riscv_program_gah(p, addr) == 0
-		? GDB_REGNO_X0
-		: riscv_program_gettemp(p);
-	if (riscv_program_lah(p, t, addr) != ERROR_OK)
-		return ERROR_FAIL;
-	if (riscv_program_insert(p, sh(d, t, riscv_program_gal(p, addr))) != ERROR_OK)
-		return ERROR_FAIL;
-	riscv_program_puttemp(p, t);
-	p->writes_memory = true;
-	return ERROR_OK;
-}
-
-int riscv_program_sb(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
-{
-	enum gdb_regno t = riscv_program_gah(p, addr) == 0
-		? GDB_REGNO_X0
-		: riscv_program_gettemp(p);
-	if (riscv_program_lah(p, t, addr) != ERROR_OK)
-		return ERROR_FAIL;
-	if (riscv_program_insert(p, sb(d, t, riscv_program_gal(p, addr))) != ERROR_OK)
-		return ERROR_FAIL;
-	riscv_program_puttemp(p, t);
-	p->writes_memory = true;
-	return ERROR_OK;
-}
-
 int riscv_program_csrr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno csr)
 {
-	assert(csr >= GDB_REGNO_CSR0);
+	assert(csr >= GDB_REGNO_CSR0 && csr <= GDB_REGNO_CSR4095);
 	return riscv_program_insert(p, csrrs(d, GDB_REGNO_X0, csr - GDB_REGNO_CSR0));
 }
 
@@ -339,12 +121,6 @@ int riscv_program_csrw(struct riscv_program *p, enum gdb_regno s, enum gdb_regno
 	return riscv_program_insert(p, csrrw(GDB_REGNO_X0, s, csr - GDB_REGNO_CSR0));
 }
 
-int riscv_program_csrrw(struct riscv_program *p, enum gdb_regno d, enum gdb_regno s, enum gdb_regno csr)
-{
-	assert(csr >= GDB_REGNO_CSR0);
-	return riscv_program_insert(p, csrrw(d, s, csr - GDB_REGNO_CSR0));
-}
-
 int riscv_program_fence_i(struct riscv_program *p)
 {
 	return riscv_program_insert(p, fence_i());
@@ -357,158 +133,27 @@ int riscv_program_fence(struct riscv_program *p)
 
 int riscv_program_ebreak(struct riscv_program *p)
 {
+	struct target *target = p->target;
+	RISCV_INFO(r);
+	if (p->instruction_count == riscv_debug_buffer_size(p->target) &&
+			r->impebreak) {
+		return ERROR_OK;
+	}
 	return riscv_program_insert(p, ebreak());
 }
 
-int riscv_program_lui(struct riscv_program *p, enum gdb_regno d, int32_t u)
-{
-	return riscv_program_insert(p, lui(d, u));
-}
-
 int riscv_program_addi(struct riscv_program *p, enum gdb_regno d, enum gdb_regno s, int16_t u)
 {
 	return riscv_program_insert(p, addi(d, s, u));
 }
 
-int riscv_program_fsx(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
-{
-	assert(d >= GDB_REGNO_FPR0);
-	assert(d <= GDB_REGNO_FPR31);
-	enum gdb_regno t = riscv_program_gah(p, addr) == 0
-		? GDB_REGNO_X0
-		: riscv_program_gettemp(p);
-	if (riscv_program_lah(p, t, addr) != ERROR_OK)
-		return ERROR_FAIL;
-	uint32_t instruction;
-	switch (p->target->reg_cache->reg_list[GDB_REGNO_FPR0].size) {
-		case 64:
-			instruction = fsd(d - GDB_REGNO_FPR0, t, riscv_program_gal(p, addr));
-			break;
-		case 32:
-			instruction = fsw(d - GDB_REGNO_FPR0, t, riscv_program_gal(p, addr));
-			break;
-		default:
-			return ERROR_FAIL;
-	}
-	if (riscv_program_insert(p, instruction) != ERROR_OK)
-		return ERROR_FAIL;
-	riscv_program_puttemp(p, t);
-	p->writes_memory = true;
-	return ERROR_OK;
-}
-
-int riscv_program_flx(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
-{
-	assert(d >= GDB_REGNO_FPR0);
-	assert(d <= GDB_REGNO_FPR31);
-	enum gdb_regno t = riscv_program_gah(p, addr) == 0 ? GDB_REGNO_X0 : d;
-	if (riscv_program_lah(p, t, addr) != ERROR_OK)
-		return ERROR_FAIL;
-	uint32_t instruction;
-	switch (p->target->reg_cache->reg_list[GDB_REGNO_FPR0].size) {
-		case 64:
-			instruction = fld(d - GDB_REGNO_FPR0, t, riscv_program_gal(p, addr));
-			break;
-		case 32:
-			instruction = flw(d - GDB_REGNO_FPR0, t, riscv_program_gal(p, addr));
-			break;
-		default:
-			return ERROR_FAIL;
-	}
-	if (riscv_program_insert(p, instruction) != ERROR_OK)
-		return ERROR_FAIL;
-	return ERROR_OK;
-}
-
-int riscv_program_li(struct riscv_program *p, enum gdb_regno d, riscv_reg_t c)
-{
-	if (riscv_program_lui(p, d, c >> 12) != ERROR_OK)
-		return ERROR_FAIL;
-	if (riscv_program_addi(p, d, d, c & 0xFFF) != ERROR_OK)
-		return ERROR_FAIL;
-	return ERROR_OK;
-}
-
-int riscv_program_dont_restore_register(struct riscv_program *p, enum gdb_regno r)
-{
-	assert(r < RISCV_REGISTER_COUNT);
-	p->writes_xreg[r] = 0;
-	return ERROR_OK;
-}
-
-int riscv_program_do_restore_register(struct riscv_program *p, enum gdb_regno r)
-{
-	assert(r < RISCV_REGISTER_COUNT);
-	p->writes_xreg[r] = 1;
-	return ERROR_OK;
-}
-
-void riscv_program_reserve_register(struct riscv_program *p, enum gdb_regno r)
-{
-	assert(r < RISCV_REGISTER_COUNT);
-	assert(p->in_use[r] == 0);
-	p->in_use[r] = 1;
-}
-
-enum gdb_regno riscv_program_gettemp(struct riscv_program *p)
-{
-	for (size_t i = GDB_REGNO_S0; i <= GDB_REGNO_XPR31; ++i) {
-		if (p->in_use[i]) continue;
-
-		riscv_program_do_restore_register(p, i);
-		p->in_use[i] = 1;
-		return i;
-	}
-
-	LOG_ERROR("You've run out of temporary registers.  This is impossible.");
-	abort();
-}
-
-void riscv_program_puttemp(struct riscv_program *p, enum gdb_regno r)
-{
-	assert(r < RISCV_REGISTER_COUNT);
-	p->in_use[r] = 0;
-}
-
-/* Helper functions. */
-riscv_addr_t riscv_program_gah(struct riscv_program *p, riscv_addr_t addr)
-{
-	return addr >> 12;
-}
-
-riscv_addr_t riscv_program_gal(struct riscv_program *p, riscv_addr_t addr)
-{
-	if (addr > 0) {
-	    return (addr & 0x7FF);
-	} else {
-	    return 0;
-	}
-}
-
-int riscv_program_lah(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
-{
-	riscv_addr_t ah = riscv_program_gah(p, addr);
-	if (ah == 0)
-		return ERROR_OK;
-	return riscv_program_lui(p, d, ah);
-}
-
-int riscv_program_lal(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
-{
-	riscv_addr_t al = riscv_program_gal(p, addr);
-	if (al == 0)
-		return ERROR_OK;
-	return riscv_program_addi(p, d, d, al);
-}
-
 int riscv_program_insert(struct riscv_program *p, riscv_insn_t i)
 {
-	if (p->instruction_count + p->data_count + 1 > riscv_debug_buffer_size(p->target)) {
+	if (p->instruction_count >= riscv_debug_buffer_size(p->target)) {
 		LOG_ERROR("Unable to insert instruction:");
 		LOG_ERROR("  instruction_count=%d", (int)p->instruction_count);
-		LOG_ERROR("  data_count       =%d", (int)p->data_count);
 		LOG_ERROR("  buffer size      =%d", (int)riscv_debug_buffer_size(p->target));
-		return ERROR_FAIL;
+		abort();
 	}
 
 	p->debug_buffer[p->instruction_count] = i;
diff --git a/src/target/riscv/program.h b/src/target/riscv/program.h
index ac1127e..d641be1 100644
--- a/src/target/riscv/program.h
+++ b/src/target/riscv/program.h
@@ -15,23 +15,12 @@ struct riscv_program {
 
 	uint32_t debug_buffer[RISCV_MAX_DEBUG_BUFFER_SIZE];
 
-	/* The debug buffer is allocated in two directions: instructions go at
-	 * the start, while data goes at the end.  When they meet in the middle
-	 * this blows up. */
+	/* Number of 32-bit instructions in the program. */
 	size_t instruction_count;
-	size_t data_count;
 
 	/* Side effects of executing this program.  These must be accounted for
 	 * in order to maintain correct executing of the target system.  */
 	bool writes_xreg[RISCV_REGISTER_COUNT];
-	bool writes_memory;
-
-	/* When a register is used it will be set in this array. */
-	bool in_use[RISCV_REGISTER_COUNT];
-
-	/* Remembers the registers that have been saved into dscratch
-	 * registers.  These are restored */
-	enum gdb_regno dscratch_saved[RISCV_DSCRATCH_COUNT];
 
 	/* XLEN on the target. */
 	int target_xlen;
@@ -40,6 +29,9 @@ struct riscv_program {
 /* Initializes a program with the header. */
 int riscv_program_init(struct riscv_program *p, struct target *t);
 
+/* Write the program to the program buffer. */
+int riscv_program_write(struct riscv_program *program);
+
 /* Executes a program, returning 0 if the program successfully executed.  Note
  * that this may cause registers to be saved or restored, which could result to
  * calls to things like riscv_save_register which itself could require a
@@ -60,83 +52,24 @@ 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);
 
-/* Allocates data of various sizes.  Either returns the absolute physical
- * address or RISCV_PROGRAM_ALLOC_FAIL on failure. */
-riscv_addr_t riscv_program_alloc_data(struct riscv_program *p, size_t bytes);
-riscv_addr_t riscv_program_alloc_x(struct riscv_program *p);
-riscv_addr_t riscv_program_alloc_d(struct riscv_program *p);
-riscv_addr_t riscv_program_alloc_w(struct riscv_program *p);
-riscv_addr_t riscv_program_alloc_h(struct riscv_program *p);
-riscv_addr_t riscv_program_alloc_b(struct riscv_program *p);
-#define RISCV_PROGRAM_ALLOC_FAIL ((riscv_addr_t)(-1))
-
-/* Reads a word of memory from this program's internal view of the debug RAM.
- * This is what you want to use to get data back from the program after it
- * executes. */
-riscv_insn_t riscv_program_read_ram(struct riscv_program *p, riscv_addr_t addr);
-void riscv_program_write_ram(struct riscv_program *p, riscv_addr_t a, uint64_t d);
-
 /* Helpers to assembly various instructions.  Return 0 on success.  These might
  * assembly into a multi-instruction sequence that overwrites some other
  * register, but those will be properly saved and restored. */
-int riscv_program_lx(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr);
-int riscv_program_ld(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr);
-int riscv_program_lw(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr);
-int riscv_program_lh(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr);
-int riscv_program_lb(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr);
-
-int riscv_program_sx(struct riscv_program *p, enum gdb_regno s, riscv_addr_t addr);
-int riscv_program_sd(struct riscv_program *p, enum gdb_regno s, riscv_addr_t addr);
-int riscv_program_sw(struct riscv_program *p, enum gdb_regno s, riscv_addr_t addr);
-int riscv_program_sh(struct riscv_program *p, enum gdb_regno s, riscv_addr_t addr);
-int riscv_program_sb(struct riscv_program *p, enum gdb_regno s, riscv_addr_t addr);
-
-int riscv_program_lxr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno a, int o);
-int riscv_program_ldr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno a, int o);
 int riscv_program_lwr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno a, int o);
 int riscv_program_lhr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno a, int o);
 int riscv_program_lbr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno a, int o);
 
-int riscv_program_sxr(struct riscv_program *p, enum gdb_regno s, enum gdb_regno a, int o);
-int riscv_program_sdr(struct riscv_program *p, enum gdb_regno s, enum gdb_regno a, int o);
 int riscv_program_swr(struct riscv_program *p, enum gdb_regno s, enum gdb_regno a, int o);
 int riscv_program_shr(struct riscv_program *p, enum gdb_regno s, enum gdb_regno a, int o);
 int riscv_program_sbr(struct riscv_program *p, enum gdb_regno s, enum gdb_regno a, int o);
 
 int riscv_program_csrr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno csr);
 int riscv_program_csrw(struct riscv_program *p, enum gdb_regno s, enum gdb_regno csr);
-int riscv_program_csrrw(struct riscv_program *p, enum gdb_regno d, enum gdb_regno s, enum gdb_regno csr);
 
 int riscv_program_fence_i(struct riscv_program *p);
 int riscv_program_fence(struct riscv_program *p);
 int riscv_program_ebreak(struct riscv_program *p);
 
-int riscv_program_lui(struct riscv_program *p, enum gdb_regno d, int32_t u);
 int riscv_program_addi(struct riscv_program *p, enum gdb_regno d, enum gdb_regno s, int16_t i);
 
-int riscv_program_fsx(struct riscv_program *p, enum gdb_regno s, riscv_addr_t addr);
-int riscv_program_flx(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr);
-
-/* Assembler macros. */
-int riscv_program_li(struct riscv_program *p, enum gdb_regno d, riscv_reg_t c);
-int riscv_program_la(struct riscv_program *p, enum gdb_regno d, riscv_addr_t a);
-
-/* Register allocation.  The user is expected to have obtained temporary
- * registers using these fuctions.  Additionally, there is an interface for
- * reserving registers -- it's expected that this has been called as the first
- * thing in the program's execution to reserve registers that can't be touched
- * by the program's execution. */
-void riscv_program_reserve_register(struct riscv_program *p, enum gdb_regno r);
-enum gdb_regno riscv_program_gettemp(struct riscv_program *p);
-void riscv_program_puttemp(struct riscv_program *p, enum gdb_regno r);
-
-/* Executing a program usually causes the registers that get overwritten to be
- * saved and restored.  Calling this prevents the given register from actually
- * being restored as a result of all activity in this program. */
-int riscv_program_dont_restore_register(struct riscv_program *p, enum gdb_regno r);
-int riscv_program_do_restore_register(struct riscv_program *p, enum gdb_regno r);
-
-/* Addressing functions. */
-riscv_addr_t riscv_program_gah(struct riscv_program *p, riscv_addr_t addr);
-
 #endif
diff --git a/src/target/riscv/riscv-013.c b/src/target/riscv/riscv-013.c
index 4ab9a2e..1768b62 100644
--- a/src/target/riscv/riscv-013.c
+++ b/src/target/riscv/riscv-013.c
@@ -28,16 +28,10 @@
 #include "batch.h"
 
 #define DMI_DATA1 (DMI_DATA0 + 1)
+#define DMI_PROGBUF1 (DMI_PROGBUF0 + 1)
 
 static void riscv013_on_step_or_resume(struct target *target, bool step);
 static void riscv013_step_or_resume_current_hart(struct target *target, bool step);
-static riscv_addr_t riscv013_progbuf_addr(struct target *target);
-static riscv_addr_t riscv013_progbuf_size(struct target *target);
-static riscv_addr_t riscv013_data_size(struct target *target);
-static riscv_addr_t riscv013_data_addr(struct target *target);
-static void riscv013_set_autoexec(struct target *target, unsigned index,
-		bool enabled);
-static int riscv013_debug_buffer_register(struct target *target, riscv_addr_t addr);
 static void riscv013_clear_abstract_error(struct target *target);
 
 /* Implementations of the functions in riscv_info_t. */
@@ -52,8 +46,6 @@ static void riscv013_on_step(struct target *target);
 static void riscv013_on_resume(struct target *target);
 static bool riscv013_is_halted(struct target *target);
 static enum riscv_halt_reason riscv013_halt_reason(struct target *target);
-static void riscv013_debug_buffer_enter(struct target *target, struct riscv_program *p);
-static void riscv013_debug_buffer_leave(struct target *target, struct riscv_program *p);
 static void riscv013_write_debug_buffer(struct target *target, unsigned index,
 		riscv_insn_t d);
 static riscv_insn_t riscv013_read_debug_buffer(struct target *target, unsigned
@@ -63,6 +55,13 @@ static void riscv013_fill_dmi_write_u64(struct target *target, char *buf, int a,
 static void riscv013_fill_dmi_read_u64(struct target *target, char *buf, int a);
 static int riscv013_dmi_write_u64_bits(struct target *target);
 static void riscv013_fill_dmi_nop_u64(struct target *target, char *buf);
+static int register_read_direct(struct target *target, uint64_t *value, uint32_t number);
+static int register_write_direct(struct target *target, unsigned number,
+		uint64_t value);
+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);
 
 /**
  * Since almost everything can be accomplish by scanning the dbus register, all
@@ -135,18 +134,28 @@ struct memory_cache_line {
 	bool dirty;
 };
 
+typedef enum {
+	YNM_MAYBE,
+	YNM_YES,
+	YNM_NO
+} yes_no_maybe_t;
+
 typedef struct {
 	/* Number of address bits in the dbus register. */
 	unsigned abits;
 	/* Number of abstract command data registers. */
 	unsigned datacount;
 	/* Number of words in the Program Buffer. */
-	unsigned progsize;
+	unsigned progbufsize;
 	/* The value that mstatus actually has on the target right now. This is not
 	 * the value we present to the user. That one may be stored in the
 	 * reg_cache. */
 	uint64_t mstatus_actual;
 
+	yes_no_maybe_t progbuf_writable;
+	/* We only need the address so that we know the alignment of the buffer. */
+	riscv_addr_t progbuf_address;
+
 	/* Single buffer that contains all register names, instead of calling
 	 * malloc for each register. Needs to be freed when reg_list is freed. */
 	char *reg_names;
@@ -171,9 +180,6 @@ typedef struct {
 
 	bool need_strict_step;
 
-	// Some memoized values
-	int progbuf_size, progbuf_addr, data_addr, data_size;
-
 	bool abstract_read_csr_supported;
 	bool abstract_write_csr_supported;
 	bool abstract_read_fpr_supported;
@@ -182,7 +188,12 @@ typedef struct {
 	// When a function returns some error due to a failure indicated by the
 	// target in cmderr, the caller can look here to see what that error was.
 	// (Compare with errno.)
-	unsigned cmderr;
+	uint8_t cmderr;
+
+	// Some fields from hartinfo.
+	uint8_t datasize;
+	uint8_t dataaccess;
+	int16_t dataaddr;
 } riscv013_info_t;
 
 static void decode_dmi(char *text, unsigned address, unsigned data)
@@ -200,6 +211,7 @@ static void decode_dmi(char *text, unsigned address, unsigned data)
 		{ DMI_DMCONTROL, DMI_DMCONTROL_NDMRESET, "ndmreset" },
 		{ DMI_DMCONTROL, DMI_DMCONTROL_DMACTIVE, "dmactive" },
 
+		{ DMI_DMSTATUS, DMI_DMSTATUS_IMPEBREAK, "impebreak" },
 		{ DMI_DMSTATUS, DMI_DMSTATUS_ALLRESUMEACK, "allresumeack" },
 		{ DMI_DMSTATUS, DMI_DMSTATUS_ANYRESUMEACK, "anyresumeack" },
 		{ DMI_DMSTATUS, DMI_DMSTATUS_ALLNONEXISTENT, "allnonexistent" },
@@ -215,7 +227,7 @@ static void decode_dmi(char *text, unsigned address, unsigned data)
 		{ DMI_DMSTATUS, DMI_DMSTATUS_DEVTREEVALID, "devtreevalid" },
 		{ DMI_DMSTATUS, DMI_DMSTATUS_VERSION, "version" },
 
-		{ DMI_ABSTRACTCS, DMI_ABSTRACTCS_PROGSIZE, "progsize" },
+		{ DMI_ABSTRACTCS, DMI_ABSTRACTCS_PROGBUFSIZE, "progbufsize" },
 		{ DMI_ABSTRACTCS, DMI_ABSTRACTCS_BUSY, "busy" },
 		{ DMI_ABSTRACTCS, DMI_ABSTRACTCS_CMDERR, "cmderr" },
 		{ DMI_ABSTRACTCS, DMI_ABSTRACTCS_DATACOUNT, "datacount" },
@@ -635,11 +647,12 @@ static int write_abstract_arg(struct target *target, unsigned index,
 	return ERROR_OK;
 }
 
-static int register_read_abstract(struct target *target, uint64_t *value,
-		uint32_t number, unsigned size)
+/**
+ * @size in bits
+ */
+static uint32_t access_register_command(uint32_t number, unsigned size,
+		uint32_t flags)
 {
-	RISCV013_INFO(r);
-
 	uint32_t command = set_field(0, DMI_COMMAND_CMDTYPE, 0);
 	switch (size) {
 		case 32:
@@ -649,45 +662,58 @@ static int register_read_abstract(struct target *target, uint64_t *value,
 			command = set_field(command, AC_ACCESS_REGISTER_SIZE, 3);
 			break;
 		default:
-			LOG_ERROR("Unsupported abstract register read size: %d", size);
-			return ERROR_FAIL;
+			assert(0);
 	}
-	command = set_field(command, AC_ACCESS_REGISTER_POSTEXEC, 0);
-	command = set_field(command, AC_ACCESS_REGISTER_TRANSFER, 1);
-	command = set_field(command, AC_ACCESS_REGISTER_WRITE, 0);
 
 	if (number <= GDB_REGNO_XPR31) {
 		command = set_field(command, AC_ACCESS_REGISTER_REGNO,
 				0x1000 + number - GDB_REGNO_XPR0);
 	} else if (number >= GDB_REGNO_FPR0 && number <= GDB_REGNO_FPR31) {
-		if (!r->abstract_read_fpr_supported)
-			return ERROR_FAIL;
 		command = set_field(command, AC_ACCESS_REGISTER_REGNO,
 				0x1020 + number - GDB_REGNO_FPR0);
 	} else if (number >= GDB_REGNO_CSR0 && number <= GDB_REGNO_CSR4095) {
-		if (!r->abstract_read_csr_supported)
-			return ERROR_FAIL;
 		command = set_field(command, AC_ACCESS_REGISTER_REGNO,
 				number - GDB_REGNO_CSR0);
 	} else {
-		return ERROR_FAIL;
+		assert(0);
 	}
 
+	command |= flags;
+
+	return command;
+}
+
+static int register_read_abstract(struct target *target, uint64_t *value,
+		uint32_t number, unsigned size)
+{
+	RISCV013_INFO(info);
+
+	if (number >= GDB_REGNO_FPR0 && number <= GDB_REGNO_FPR31 &&
+			!info->abstract_read_fpr_supported)
+		return ERROR_FAIL;
+	if (number >= GDB_REGNO_CSR0 && number <= GDB_REGNO_CSR4095 &&
+			!info->abstract_read_csr_supported)
+		return ERROR_FAIL;
+
+	uint32_t command = access_register_command(number, size,
+			AC_ACCESS_REGISTER_TRANSFER);
+
 	int result = execute_abstract_command(target, command);
 	if (result != ERROR_OK) {
-		if (r->cmderr == CMDERR_NOT_SUPPORTED) {
+		if (info->cmderr == CMDERR_NOT_SUPPORTED) {
 			if (number >= GDB_REGNO_FPR0 && number <= GDB_REGNO_FPR31) {
-				r->abstract_read_fpr_supported = false;
+				info->abstract_read_fpr_supported = false;
 				LOG_INFO("Disabling abstract command reads from FPRs.");
 			} else if (number >= GDB_REGNO_CSR0 && number <= GDB_REGNO_CSR4095) {
-				r->abstract_read_csr_supported = false;
+				info->abstract_read_csr_supported = false;
 				LOG_INFO("Disabling abstract command reads from CSRs.");
 			}
 		}
 		return result;
 	}
 
-	*value = read_abstract_arg(target, 0);
+	if (value)
+		*value = read_abstract_arg(target, 0);
 
 	return ERROR_OK;
 }
@@ -697,38 +723,16 @@ static int register_write_abstract(struct target *target, uint32_t number,
 {
 	RISCV013_INFO(info);
 
-	uint32_t command = set_field(0, DMI_COMMAND_CMDTYPE, 0);
-	switch (size) {
-		case 32:
-			command = set_field(command, AC_ACCESS_REGISTER_SIZE, 2);
-			break;
-		case 64:
-			command = set_field(command, AC_ACCESS_REGISTER_SIZE, 3);
-			break;
-		default:
-			LOG_ERROR("Unsupported abstract register read size: %d", size);
-			return ERROR_FAIL;
-	}
-	command = set_field(command, AC_ACCESS_REGISTER_POSTEXEC, 0);
-	command = set_field(command, AC_ACCESS_REGISTER_TRANSFER, 1);
-	command = set_field(command, AC_ACCESS_REGISTER_WRITE, 1);
-
-	if (number <= GDB_REGNO_XPR31) {
-		command = set_field(command, AC_ACCESS_REGISTER_REGNO,
-				0x1000 + number - GDB_REGNO_XPR0);
-	} else if (number >= GDB_REGNO_FPR0 && number <= GDB_REGNO_FPR31) {
-		if (!info->abstract_read_fpr_supported)
-			return ERROR_FAIL;
-		command = set_field(command, AC_ACCESS_REGISTER_REGNO,
-				0x1020 + number - GDB_REGNO_FPR0);
-	} else if (number >= GDB_REGNO_CSR0 && number <= GDB_REGNO_CSR4095) {
-		if (!info->abstract_read_csr_supported)
-			return ERROR_FAIL;
-		command = set_field(command, AC_ACCESS_REGISTER_REGNO,
-				number - GDB_REGNO_CSR0);
-	} else {
+	if (number >= GDB_REGNO_FPR0 && number <= GDB_REGNO_FPR31 &&
+			!info->abstract_write_fpr_supported)
+		return ERROR_FAIL;
+	if (number >= GDB_REGNO_CSR0 && number <= GDB_REGNO_CSR4095 &&
+			!info->abstract_write_csr_supported)
 		return ERROR_FAIL;
-	}
+
+	uint32_t command = access_register_command(number, size,
+			AC_ACCESS_REGISTER_TRANSFER |
+			AC_ACCESS_REGISTER_WRITE);
 
 	if (write_abstract_arg(target, 0, value) != ERROR_OK) {
 		return ERROR_FAIL;
@@ -751,6 +755,201 @@ static int register_write_abstract(struct target *target, uint32_t number,
 	return ERROR_OK;
 }
 
+static int examine_progbuf(struct target *target)
+{
+	riscv013_info_t *info = get_info(target);
+
+	if (info->progbuf_writable != YNM_MAYBE)
+		return ERROR_OK;
+
+	// Figure out if progbuf is writable.
+
+	if (info->progbufsize < 1) {
+		info->progbuf_writable = YNM_NO;
+		LOG_INFO("No program buffer present.");
+		return ERROR_OK;
+	}
+
+	uint64_t s0;
+	if (register_read_direct(target, &s0, GDB_REGNO_S0) != ERROR_OK)
+		return ERROR_FAIL;
+
+	struct riscv_program program;
+	riscv_program_init(&program, target);
+	riscv_program_insert(&program, auipc(S0));
+	if (riscv_program_exec(&program, target) != ERROR_OK)
+		return ERROR_FAIL;
+
+	if (register_read_direct(target, &info->progbuf_address, GDB_REGNO_S0) != ERROR_OK)
+		return ERROR_FAIL;
+
+	riscv_program_init(&program, target);
+	riscv_program_insert(&program, sw(S0, S0, 0));
+	int result = riscv_program_exec(&program, target);
+
+	if (register_write_direct(target, GDB_REGNO_S0, s0) != ERROR_OK)
+		return ERROR_FAIL;
+
+	if (result != ERROR_OK) {
+		// This program might have failed if the program buffer is not
+		// writable.
+		info->progbuf_writable = YNM_NO;
+		return ERROR_OK;
+	}
+
+	uint32_t written = dmi_read(target, DMI_PROGBUF0);
+	if (written == (uint32_t) info->progbuf_address) {
+		LOG_INFO("progbuf is writable at 0x%" TARGET_PRIxADDR,
+				info->progbuf_address);
+		info->progbuf_writable = YNM_YES;
+
+	} else {
+		LOG_INFO("progbuf is not writeable at 0x%" TARGET_PRIxADDR,
+				info->progbuf_address);
+		info->progbuf_writable = YNM_NO;
+	}
+
+	return ERROR_OK;
+}
+
+typedef enum {
+	SPACE_DMI_DATA,
+	SPACE_DMI_PROGBUF,
+	SPACE_DMI_RAM
+} memory_space_t;
+
+typedef struct {
+	// How can the debugger access this memory?
+	memory_space_t memory_space;
+	// Memory address to access the scratch memory from the hart.
+	riscv_addr_t hart_address;
+	// Memory address to access the scratch memory from the debugger.
+	riscv_addr_t debug_address;
+} scratch_mem_t;
+
+/**
+ * Find some scratch memory to be used with the given program.
+ */
+static int scratch_find(struct target *target,
+		scratch_mem_t *scratch,
+		struct riscv_program *program,
+		unsigned size_bytes)
+{
+	riscv013_info_t *info = get_info(target);
+
+	riscv_addr_t alignment = 1;
+	while (alignment < size_bytes)
+		alignment *= 2;
+
+	if (info->dataaccess == 1) {
+		// Sign extend dataaddr.
+		scratch->hart_address = info->dataaddr;
+		if (info->dataaddr & (1<<11)) {
+			scratch->hart_address |= 0xfffffffffffff000ULL;
+		}
+		// Align.
+		scratch->hart_address = (scratch->hart_address + alignment - 1) & ~(alignment - 1);
+
+		if ((size_bytes + scratch->hart_address - info->dataaddr + 3) / 4 >=
+				info->datasize) {
+			scratch->memory_space = SPACE_DMI_DATA;
+			scratch->debug_address = (scratch->hart_address - info->dataaddr) / 4;
+			return ERROR_OK;
+		}
+	}
+
+	if (examine_progbuf(target) != ERROR_OK)
+		return ERROR_FAIL;
+
+	// Allow for ebreak at the end of the program.
+	unsigned program_size = (program->instruction_count + 1 ) * 4;
+	scratch->hart_address = (info->progbuf_address + program_size + alignment - 1) &
+		~(alignment - 1);
+	if ((size_bytes + scratch->hart_address - info->progbuf_address + 3) / 4 >=
+			info->progbufsize) {
+		scratch->memory_space = SPACE_DMI_PROGBUF;
+		scratch->debug_address = (scratch->hart_address - info->progbuf_address) / 4;
+		return ERROR_OK;
+	}
+
+	if (riscv_use_scratch_ram) {
+		scratch->hart_address = (riscv_use_scratch_ram + alignment - 1) &
+			~(alignment - 1);
+		scratch->memory_space = SPACE_DMI_RAM;
+		scratch->debug_address = scratch->hart_address;
+		return ERROR_OK;
+	}
+
+	LOG_ERROR("Couldn't find %d bytes of scratch RAM to use. Please configure "
+			"an address with 'riscv set_scratch_ram'.", size_bytes);
+	return ERROR_FAIL;
+}
+
+static int scratch_read64(struct target *target, scratch_mem_t *scratch,
+		uint64_t *value)
+{
+	switch (scratch->memory_space) {
+		case SPACE_DMI_DATA:
+			*value = dmi_read(target, DMI_DATA0 + scratch->debug_address);
+			*value |= ((uint64_t) dmi_read(target, DMI_DATA1 +
+						scratch->debug_address)) << 32;
+			break;
+		case SPACE_DMI_PROGBUF:
+			*value = dmi_read(target, DMI_PROGBUF0 + scratch->debug_address);
+			*value |= ((uint64_t) dmi_read(target, DMI_PROGBUF1 +
+						scratch->debug_address)) << 32;
+			break;
+		case SPACE_DMI_RAM:
+			{
+				uint8_t buffer[8];
+				if (read_memory(target, scratch->debug_address, 4, 2, buffer) != ERROR_OK)
+					return ERROR_FAIL;
+				*value = buffer[0] |
+					(((uint64_t) buffer[1]) << 8) |
+					(((uint64_t) buffer[2]) << 16) |
+					(((uint64_t) buffer[3]) << 24) |
+					(((uint64_t) buffer[4]) << 32) |
+					(((uint64_t) buffer[5]) << 40) |
+					(((uint64_t) buffer[6]) << 48) |
+					(((uint64_t) buffer[7]) << 56);
+			}
+			break;
+	}
+	return ERROR_OK;
+}
+
+static int scratch_write64(struct target *target, scratch_mem_t *scratch,
+		uint64_t value)
+{
+	switch (scratch->memory_space) {
+		case SPACE_DMI_DATA:
+			dmi_write(target, DMI_DATA0 + scratch->debug_address, value);
+			dmi_write(target, DMI_DATA1 + scratch->debug_address, value >> 32);
+			break;
+		case SPACE_DMI_PROGBUF:
+			dmi_write(target, DMI_PROGBUF0 + scratch->debug_address, value);
+			dmi_write(target, DMI_PROGBUF1 + scratch->debug_address, value >> 32);
+			break;
+		case SPACE_DMI_RAM:
+			{
+				uint8_t buffer[8] = {
+					value,
+					value >> 8,
+					value >> 16,
+					value >> 24,
+					value >> 32,
+					value >> 40,
+					value >> 48,
+					value >> 56
+				};
+				if (write_memory(target, scratch->debug_address, 4, 2, buffer) != ERROR_OK)
+					return ERROR_FAIL;
+			}
+			break;
+	}
+	return ERROR_OK;
+}
+
 static int register_write_direct(struct target *target, unsigned number,
 		uint64_t value)
 {
@@ -763,34 +962,55 @@ static int register_write_direct(struct target *target, unsigned number,
 		return ERROR_OK;
 
 	struct riscv_program program;
-
 	riscv_program_init(&program, target);
 
-	riscv_addr_t input = riscv_program_alloc_d(&program);
-	riscv_program_write_ram(&program, input + 4, value >> 32);
-	riscv_program_write_ram(&program, input, value);
+	uint64_t s0;
+	if (register_read_direct(target, &s0, GDB_REGNO_S0) != ERROR_OK)
+		return ERROR_FAIL;
+
+	if (number >= GDB_REGNO_FPR0 && number <= GDB_REGNO_FPR31 &&
+			supports_extension(target, 'D') &&
+			riscv_xlen(target) < 64) {
+		/* There are no instructions to move all the bits from a register, so
+		 * we need to use some scratch RAM. */
+		riscv_program_insert(&program, fld(number - GDB_REGNO_FPR0, S0, 0));
+
+		scratch_mem_t scratch;
+		if (scratch_find(target, &scratch, &program, 8) != ERROR_OK)
+			return ERROR_FAIL;
+
+		if (register_write_direct(target, GDB_REGNO_S0, scratch.hart_address)
+				!= ERROR_OK)
+			return ERROR_FAIL;
+
+		if (scratch_write64(target, &scratch, value) != ERROR_OK)
+			return ERROR_FAIL;
 
-	assert(GDB_REGNO_XPR0 == 0);
-	if (number <= GDB_REGNO_XPR31) {
-		riscv_program_lx(&program, number, input);
-	} else if (number >= GDB_REGNO_FPR0 && number <= GDB_REGNO_FPR31) {
-		riscv_program_flx(&program, number, input);
-	} else if (number >= GDB_REGNO_CSR0 && number <= GDB_REGNO_CSR4095) {
-		enum gdb_regno temp = riscv_program_gettemp(&program);
-		riscv_program_lx(&program, temp, input);
-		riscv_program_csrw(&program, temp, number);
 	} else {
-		LOG_ERROR("Unsupported register (enum gdb_regno)(%d)", number);
-		abort();
+		if (register_write_direct(target, GDB_REGNO_S0, value) != ERROR_OK)
+			return ERROR_FAIL;
+
+		if (number >= GDB_REGNO_FPR0 && number <= GDB_REGNO_FPR31) {
+			if (supports_extension(target, 'D')) {
+				riscv_program_insert(&program, fmv_d_x(number - GDB_REGNO_FPR0, S0));
+			} else {
+				riscv_program_insert(&program, fmv_s_x(number - GDB_REGNO_FPR0, S0));
+			}
+		} else if (number >= GDB_REGNO_CSR0 && number <= GDB_REGNO_CSR4095) {
+			riscv_program_csrw(&program, S0, number);
+		} else {
+			LOG_ERROR("Unsupported register (enum gdb_regno)(%d)", number);
+			abort();
+		}
 	}
 
 	int exec_out = riscv_program_exec(&program, target);
-	if (exec_out != ERROR_OK) {
-		riscv013_clear_abstract_error(target);
+
+	// Restore S0.
+	if (register_write_direct(target, GDB_REGNO_S0, s0) != ERROR_OK)
 		return ERROR_FAIL;
-	}
 
-	return ERROR_OK;
+	return exec_out;
 }
 
 /** Actually read registers from the target right now. */
@@ -800,41 +1020,72 @@ static int register_read_direct(struct target *target, uint64_t *value, uint32_t
 			riscv_xlen(target));
 
 	if (result != ERROR_OK) {
+		assert(number != GDB_REGNO_S0);
+
+		result = ERROR_OK;
+
 		struct riscv_program program;
 		riscv_program_init(&program, target);
-		riscv_addr_t output = riscv_program_alloc_d(&program);
-		riscv_program_write_ram(&program, output + 4, 0);
-		riscv_program_write_ram(&program, output, 0);
-
-		assert(GDB_REGNO_XPR0 == 0);
-		if (number <= GDB_REGNO_XPR31) {
-			riscv_program_sx(&program, number, output);
-		} else if (number >= GDB_REGNO_FPR0 && number <= GDB_REGNO_FPR31) {
-			riscv_program_fsx(&program, number, output);
+
+		scratch_mem_t scratch;
+		bool use_scratch = false;
+
+		uint64_t s0;
+		if (register_read_direct(target, &s0, GDB_REGNO_S0) != ERROR_OK)
+			return ERROR_FAIL;
+
+		// Write program to move data into s0.
+
+		if (number >= GDB_REGNO_FPR0 && number <= GDB_REGNO_FPR31) {
+			// TODO: Possibly set F in mstatus.
+			if (supports_extension(target, 'D') && riscv_xlen(target) < 64) {
+				/* There are no instructions to move all the bits from a
+				 * register, so we need to use some scratch RAM. */
+				riscv_program_insert(&program, fsd(number - GDB_REGNO_FPR0, S0,
+							0));
+
+				if (scratch_find(target, &scratch, &program, 8) != ERROR_OK)
+					return ERROR_FAIL;
+				use_scratch = true;
+
+				if (register_write_direct(target, GDB_REGNO_S0,
+							scratch.hart_address) != ERROR_OK)
+					return ERROR_FAIL;
+			} else if (supports_extension(target, 'D')) {
+				riscv_program_insert(&program, fmv_x_d(S0, number - GDB_REGNO_FPR0));
+			} else {
+				riscv_program_insert(&program, fmv_x_s(S0, number - GDB_REGNO_FPR0));
+			}
 		} else if (number >= GDB_REGNO_CSR0 && number <= GDB_REGNO_CSR4095) {
-			LOG_DEBUG("reading CSR index=0x%03x", number - GDB_REGNO_CSR0);
-			enum gdb_regno temp = riscv_program_gettemp(&program);
-			riscv_program_csrr(&program, temp, number);
-			riscv_program_sx(&program, temp, output);
+			riscv_program_csrr(&program, S0, number);
 		} else {
 			LOG_ERROR("Unsupported register (enum gdb_regno)(%d)", number);
 			abort();
 		}
 
-		int exec_out = riscv_program_exec(&program, target);
-		if (exec_out != ERROR_OK) {
-			riscv013_clear_abstract_error(target);
-			return ERROR_FAIL;
+		// Execute program.
+		result = riscv_program_exec(&program, target);
+
+		if (use_scratch) {
+			if (scratch_read64(target, &scratch, value) != ERROR_OK)
+				return ERROR_FAIL;
+		} else {
+			// Read S0
+			if (register_read_direct(target, value, GDB_REGNO_S0) != ERROR_OK)
+				return ERROR_FAIL;
 		}
 
-		*value = 0;
-		*value |= ((uint64_t)(riscv_program_read_ram(&program, output + 4))) << 32;
-		*value |= riscv_program_read_ram(&program, output);
+		// Restore S0.
+		if (register_write_direct(target, GDB_REGNO_S0, s0) != ERROR_OK)
+			return ERROR_FAIL;
 	}
 
-	LOG_DEBUG("[%d] reg[0x%x] = 0x%" PRIx64, riscv_current_hartid(target),
-			number, *value);
-	return ERROR_OK;
+	if (result == ERROR_OK) {
+		LOG_DEBUG("[%d] reg[0x%x] = 0x%" PRIx64, riscv_current_hartid(target),
+				number, *value);
+	}
+
+	return result;
 }
 
 /*** OpenOCD target functions. ***/
@@ -890,8 +1141,6 @@ static int init_target(struct command_context *cmd_ctx,
 	generic_info->on_resume = &riscv013_on_resume;
 	generic_info->on_step = &riscv013_on_step;
 	generic_info->halt_reason = &riscv013_halt_reason;
-	generic_info->debug_buffer_enter = &riscv013_debug_buffer_enter;
-	generic_info->debug_buffer_leave = &riscv013_debug_buffer_leave;
 	generic_info->read_debug_buffer = &riscv013_read_debug_buffer;
 	generic_info->write_debug_buffer = &riscv013_write_debug_buffer;
 	generic_info->execute_debug_buffer = &riscv013_execute_debug_buffer;
@@ -904,10 +1153,7 @@ static int init_target(struct command_context *cmd_ctx,
 		return ERROR_FAIL;
 	riscv013_info_t *info = get_info(target);
 
-	info->progbuf_size = -1;
-	info->progbuf_addr = -1;
-	info->data_size = -1;
-	info->data_addr = -1;
+	info->progbufsize = -1;
 
 	info->dmi_busy_delay = 0;
 	info->ac_busy_delay = 0;
@@ -996,7 +1242,6 @@ static int examine(struct target *target)
 	info->abits = get_field(dtmcontrol, DTM_DTMCS_ABITS);
 	info->dtmcontrol_idle = get_field(dtmcontrol, DTM_DTMCS_IDLE);
 
-	uint32_t dmcontrol = dmi_read(target, DMI_DMCONTROL);
 	uint32_t dmstatus = dmi_read(target, DMI_DMSTATUS);
 	if (get_field(dmstatus, DMI_DMSTATUS_VERSION) != 2) {
 		LOG_ERROR("OpenOCD only supports Debug Module version 2, not %d "
@@ -1007,10 +1252,17 @@ static int examine(struct target *target)
 	// Reset the Debug Module.
 	dmi_write(target, DMI_DMCONTROL, 0);
 	dmi_write(target, DMI_DMCONTROL, DMI_DMCONTROL_DMACTIVE);
-	dmcontrol = dmi_read(target, DMI_DMCONTROL);
+	uint32_t dmcontrol = dmi_read(target, DMI_DMCONTROL);
+
+	uint32_t hartinfo = dmi_read(target, DMI_HARTINFO);
 
 	LOG_DEBUG("dmcontrol: 0x%08x", dmcontrol);
 	LOG_DEBUG("dmstatus:  0x%08x", dmstatus);
+	LOG_DEBUG("hartinfo:  0x%08x", hartinfo);
+
+	info->datasize = get_field(hartinfo, DMI_HARTINFO_DATASIZE);
+	info->dataaccess = get_field(hartinfo, DMI_HARTINFO_DATAACCESS);
+	info->dataaddr = get_field(hartinfo, DMI_HARTINFO_DATAADDR);
 
 	if (!get_field(dmcontrol, DMI_DMCONTROL_DMACTIVE)) {
 		LOG_ERROR("Debug Module did not become active. dmcontrol=0x%x",
@@ -1037,10 +1289,12 @@ static int examine(struct target *target)
 	// Check that abstract data registers are accessible.
 	uint32_t abstractcs = dmi_read(target, DMI_ABSTRACTCS);
 	info->datacount = get_field(abstractcs, DMI_ABSTRACTCS_DATACOUNT);
-	info->progsize = get_field(abstractcs, DMI_ABSTRACTCS_PROGSIZE);
+	info->progbufsize = get_field(abstractcs, DMI_ABSTRACTCS_PROGBUFSIZE);
 
 	/* Before doing anything else we must first enumerate the harts. */
 	RISCV_INFO(r);
+	r->impebreak = get_field(dmstatus, DMI_DMSTATUS_IMPEBREAK);
+
 	int original_coreid = target->coreid;
 	for (int i = 0; i < RISCV_MAX_HARTS; ++i) {
 		/* Fake being a non-RTOS targeted to this core so we can see if
@@ -1074,80 +1328,21 @@ static int examine(struct target *target)
 
 		/* Without knowing anything else we can at least mess with the
 		 * program buffer. */
-		r->debug_buffer_size[i] = riscv013_progbuf_size(target);
-
-		/* Guess this is a 32-bit system, we're probing it. */
-		r->xlen[i] = 32;
-
-		/* First find the low 32 bits of the program buffer.  This is
-		 * used to check for alignment. */
-		struct riscv_program program32;
-		riscv_program_init(&program32, target);
-		riscv_program_csrrw(&program32, GDB_REGNO_S0, GDB_REGNO_S0, GDB_REGNO_DSCRATCH);
-		riscv_program_insert(&program32, auipc(GDB_REGNO_S0));
-		riscv_program_insert(&program32, sw(GDB_REGNO_S0, GDB_REGNO_S0, -4));
-		riscv_program_csrrw(&program32, GDB_REGNO_S0, GDB_REGNO_S0, GDB_REGNO_DSCRATCH);
-		riscv_program_fence(&program32);
-		riscv_program_exec(&program32, target);
-
-		riscv_addr_t progbuf_addr = dmi_read(target, DMI_PROGBUF0) - 4;
-		if (get_field(dmi_read(target, DMI_ABSTRACTCS), DMI_ABSTRACTCS_CMDERR) != 0) {
-			LOG_ERROR("Unable to find the address of the program buffer on hart %d", i);
-			r->xlen[i] = -1;
-			continue;
-		}
-		r->debug_buffer_addr[i] = progbuf_addr;
-
-		/* Check to see if the core can execute 64 bit instructions.
-		 * In order to make this work we first need to */
-		int offset = (progbuf_addr % 8 == 0) ? -4 : 0;
-
-		/* This program uses a temporary register. If the core can not
-		 * execute 64 bit instruction, the original value of temporary
-		 * register (s0) will not be restored due to an exception.
-		 * So we have to save it and restore manually in that case.
-		 * If the core can execute 64 bit instruction, the saved value
-		 * is wrong, because it was read with 32 bit lw instruction,
-		 * but the value of s0 will be restored by the reverse swap
-		 * of s0 and dscratch registers. */
-		uint64_t s0 = riscv_get_register(target, GDB_REGNO_S0);
-
-		struct riscv_program program64;
-		riscv_program_init(&program64, target);
-		riscv_program_csrrw(&program64, GDB_REGNO_S0, GDB_REGNO_S0, GDB_REGNO_DSCRATCH);
-		riscv_program_insert(&program64, auipc(GDB_REGNO_S0));
-		riscv_program_insert(&program64, sd(GDB_REGNO_S0, GDB_REGNO_S0, offset));
-		riscv_program_csrrw(&program64, GDB_REGNO_S0, GDB_REGNO_S0, GDB_REGNO_DSCRATCH);
-		riscv_program_fence(&program64);
-		int result = riscv_program_exec(&program64, target);
+		r->debug_buffer_size[i] = info->progbufsize;
 
+		int result = register_read_abstract(target, NULL, GDB_REGNO_S0, 64);
 		if (result == ERROR_OK) {
-			r->debug_buffer_addr[i] =
-				(dmi_read(target, DMI_PROGBUF0 + (8 + offset) / 4) << 32)
-				+ dmi_read(target, DMI_PROGBUF0 + (4 + offset) / 4)
-				- 4;
 			r->xlen[i] = 64;
 		} else {
-			riscv_set_register(target, GDB_REGNO_S0, s0);
+			r->xlen[i] = 32;
 		}
 
+		r->misa = riscv_get_register_on_hart(target, i, GDB_REGNO_MISA);
+
 		/* Display this as early as possible to help people who are using
 		 * really slow simulators. */
-		LOG_DEBUG(" hart %d: XLEN=%d, program buffer at 0x%" PRIx64, i,
-				r->xlen[i], r->debug_buffer_addr[i]);
-
-		if (riscv_program_gah(&program64, r->debug_buffer_addr[i])) {
-			LOG_ERROR("This implementation will not work with hart %d with debug_buffer_addr of 0x%lx", i,
-					(long)r->debug_buffer_addr[i]);
-			abort();
-		}
-
-		/* Check to see if we can use the data words as an extended
-		 * program buffer or not. */
-		if (r->debug_buffer_addr[i] + (4 * r->debug_buffer_size[i]) == riscv013_data_addr(target)) {
-			r->debug_buffer_size[i] += riscv013_data_size(target);
-			LOG_DEBUG("extending the debug buffer using data words, total size %d", r->debug_buffer_size[i]);
-		}
+		LOG_DEBUG(" hart %d: XLEN=%d, misa=0x%" PRIx64, i, r->xlen[i],
+				r->misa);
 	}
 
 	/* Then we check the number of triggers availiable to each hart. */
@@ -1169,8 +1364,7 @@ static int examine(struct target *target)
 			riscv_count_harts(target));
 	for (int i = 0; i < riscv_count_harts(target); ++i) {
 		if (riscv_hart_enabled(target, i)) {
-			LOG_INFO(" hart %d: XLEN=%d, program buffer at 0x%" PRIx64
-					", %d triggers", i, r->xlen[i], r->debug_buffer_addr[i],
+			LOG_INFO(" hart %d: XLEN=%d, %d triggers", i, r->xlen[i],
 					r->trigger_count[i]);
 		} else {
 			LOG_INFO(" hart %d: currently disabled", i);
@@ -1293,6 +1487,9 @@ static int deassert_reset(struct target *target)
 	return ERROR_OK;
 }
 
+/**
+ * @size in bytes
+ */
 static void write_to_buf(uint8_t *buffer, uint64_t value, unsigned size)
 {
 	switch (size) {
@@ -1314,6 +1511,16 @@ 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;
+}
+
 /**
  * Read the requested memory, taking care to execute every read exactly once,
  * even if cmderr=busy is encountered.
@@ -1328,21 +1535,21 @@ static int read_memory(struct target *target, target_addr_t address,
 
 	select_dmi(target);
 
-	/* This program uses two temporary registers.  A word of data and the
-	 * associated address are stored at some location in memory.  The
-	 * program loads the word from that address and then increments the
-	 * address.  The debugger is expected to pull the memory word-by-word
-	 * from the chip with AUTOEXEC set in order to trigger program
-	 * execution on every word. */
-	uint64_t s0 = riscv_get_register(target, GDB_REGNO_S0);
-	uint64_t s1 = riscv_get_register(target, GDB_REGNO_S1);
+	/* s0 holds the next address to write to
+	 * s1 holds the next data value to write
+	 */
+	uint64_t s0, s1;
+	if (register_read_direct(target, &s0, GDB_REGNO_S0) != ERROR_OK)
+		return ERROR_FAIL;
+	if (register_read_direct(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);
-	riscv_addr_t r_data = riscv_program_alloc_w(&program);
-	riscv_addr_t r_addr = riscv_program_alloc_x(&program);
-	riscv_program_fence(&program);
-	riscv_program_lx(&program, GDB_REGNO_S0, r_addr);
 	switch (size) {
 		case 1:
 			riscv_program_lbr(&program, GDB_REGNO_S1, GDB_REGNO_S0, 0);
@@ -1358,70 +1565,51 @@ static int read_memory(struct target *target, target_addr_t address,
 			return ERROR_FAIL;
 	}
 	riscv_program_addi(&program, GDB_REGNO_S0, GDB_REGNO_S0, size);
-	riscv_program_sw(&program, GDB_REGNO_S1, r_data);
-	riscv_program_sx(&program, GDB_REGNO_S0, r_addr);
-
-	/* The first round through the program's execution we use the regular
-	 * program execution mechanism. */
-	switch (riscv_xlen(target)) {
-	case 64:
-		riscv_program_write_ram(&program, r_addr + 4, ((riscv_addr_t) address) >> 32);
-	case 32:
-		riscv_program_write_ram(&program, r_addr, (riscv_addr_t) address);
-		break;
-	default:
-		LOG_ERROR("unknown XLEN %d", riscv_xlen(target));
-		return ERROR_FAIL;
-	}
 
-	if (riscv_program_exec(&program, target) != ERROR_OK) {
-		uint32_t acs = dmi_read(target, DMI_ABSTRACTCS);
-		LOG_ERROR("failed to execute program, abstractcs=0x%08x", acs);
-		riscv013_clear_abstract_error(target);
-		riscv_set_register(target, GDB_REGNO_S0, s0);
-		riscv_set_register(target, GDB_REGNO_S1, s1);
-		LOG_ERROR("  exiting with ERROR_FAIL");
+	if (riscv_program_ebreak(&program) != ERROR_OK)
 		return ERROR_FAIL;
-	}
+	riscv_program_write(&program);
 
-	// Program has been executed once. d_addr contains address+size, and d_data
-	// contains *address.
+	// Write address to S0, and execute buffer.
+	if (register_write_direct(target, GDB_REGNO_S0, address) != ERROR_OK)
+		return ERROR_FAIL;
+	uint32_t command = access_register_command(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;
 
-	/* The rest of this program is designed to be fast so it reads various
-	 * DMI registers directly. */
-	int d_data = (r_data - riscv_debug_buffer_addr(target)) / 4;
-	int d_addr = (r_addr - riscv_debug_buffer_addr(target)) / 4;
+	// First read has just triggered. Result is in s1.
 
-	riscv013_set_autoexec(target, d_data, 1);
+	dmi_write(target, DMI_ABSTRACTAUTO,
+			1 << DMI_ABSTRACTAUTO_AUTOEXECDATA_OFFSET);
 
-	/* Copying memory might fail because we're going too quickly, in which
-	 * case we need to back off a bit and try again.  There's two
-	 * termination conditions to this loop: a non-BUSY error message, or
-	 * the data was all copied. */
-	riscv_addr_t cur_addr = riscv_read_debug_buffer_x(target, d_addr);
+	// 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 fin_addr = address + (count * size);
-	LOG_DEBUG("reading until final address 0x%" PRIx64, fin_addr);
-	while (cur_addr < fin_addr) {
-		// Invariant:
-		// d_data contains *addr
-		// d_addr contains addr + size
-
-		unsigned start = (cur_addr - address) / size;
-		LOG_DEBUG("creating burst to read address 0x%" TARGET_PRIxADDR
-				" up to 0x%" TARGET_PRIxADDR "; start=0x%d", cur_addr, fin_addr, start);
-		assert(cur_addr >= address && cur_addr < fin_addr);
-		struct riscv_batch *batch = riscv_batch_alloc(
-			target,
-			32,
-			info->dmi_busy_delay + info->ac_busy_delay);
+	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);
+		// 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]
+
+		LOG_DEBUG("creating burst to read from 0x%" TARGET_PRIxADDR
+				" up to 0x%" TARGET_PRIxADDR, read_addr, fin_addr);
+		assert(read_addr >= address && read_addr < fin_addr);
+		struct riscv_batch *batch = riscv_batch_alloc(target, 32,
+				info->dmi_busy_delay + info->ac_busy_delay);
 
 		size_t reads = 0;
-		for (riscv_addr_t addr = cur_addr; addr < fin_addr; addr += size) {
-			size_t const index =
-				riscv_batch_add_dmi_read(
-						batch,
-						riscv013_debug_buffer_register(target, r_data));
-			assert(index == reads);
+		for (riscv_addr_t addr = read_addr; addr < fin_addr; addr += size) {
+			riscv_batch_add_dmi_read(batch, DMI_DATA0);
 
 			reads++;
 			if (riscv_batch_full(batch))
@@ -1437,62 +1625,126 @@ static int read_memory(struct target *target, target_addr_t address,
 			abstractcs = dmi_read(target, DMI_ABSTRACTCS);
 		info->cmderr = get_field(abstractcs, DMI_ABSTRACTCS_CMDERR);
 
+		unsigned cmderr = info->cmderr;
+		riscv_addr_t next_read_addr;
+		uint32_t dmi_data0 = -1;
 		switch (info->cmderr) {
-		case CMDERR_NONE:
-			LOG_DEBUG("successful (partial?) memory read");
-			break;
-		case CMDERR_BUSY:
-			LOG_DEBUG("memory read resulted in busy response");
-			increase_ac_busy_delay(target);
-			riscv013_clear_abstract_error(target);
-			break;
-		default:
-			LOG_ERROR("error when reading memory, abstractcs=0x%08lx", (long)abstractcs);
-			riscv013_set_autoexec(target, d_data, 0);
-			riscv_set_register(target, GDB_REGNO_S0, s0);
-			riscv_set_register(target, GDB_REGNO_S1, s1);
-			riscv013_clear_abstract_error(target);
-			riscv_batch_free(batch);
-			return ERROR_FAIL;
-		}
+			case CMDERR_NONE:
+				LOG_DEBUG("successful (partial?) memory read");
+				next_read_addr = read_addr + reads * size;
+				break;
+			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);
+
+				// This is definitely a good version of the value that we
+				// attempted to read when we discovered that the target was
+				// busy.
+				dmi_data0 = dmi_read(target, DMI_DATA0);
+
+				// Clobbers DMI_DATA0.
+				if (register_read_direct(target, &next_read_addr, GDB_REGNO_S0) != ERROR_OK)
+					return ERROR_FAIL;
+				// 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);
 
-		// Figure out how far we managed to read.
-		riscv_addr_t next_addr = riscv_read_debug_buffer_x(target, d_addr);
-		LOG_DEBUG("Batch read [0x%" TARGET_PRIxADDR ", 0x%" TARGET_PRIxADDR
-				"); reads=%d", cur_addr, next_addr, (unsigned) reads);
-		assert(next_addr >= address && next_addr <= fin_addr);
-		assert(info->cmderr != CMDERR_NONE ||
-				next_addr == cur_addr + reads * size);
+				dmi_write(target, DMI_ABSTRACTAUTO,
+						1 << DMI_ABSTRACTAUTO_AUTOEXECDATA_OFFSET);
+				break;
+			default:
+				LOG_ERROR("error when reading memory, abstractcs=0x%08lx", (long)abstractcs);
+				dmi_write(target, DMI_ABSTRACTAUTO, 0);
+				riscv_set_register(target, GDB_REGNO_S0, s0);
+				riscv_set_register(target, GDB_REGNO_S1, s1);
+				riscv013_clear_abstract_error(target);
+				riscv_batch_free(batch);
+				return ERROR_FAIL;
+		}
 
 		// Now read whatever we got out of the batch.
-		unsigned rereads = 0;
-		for (riscv_addr_t addr = cur_addr - size; addr < next_addr - size;
-				addr += size) {
-			riscv_addr_t offset = addr - address;
+		for (size_t i = 0; i < reads; i++) {
+			if (read_addr >= next_read_addr) {
+				break;
+			}
+
+			read_addr += size;
+
+			if (skip > 0) {
+				skip--;
+				continue;
+			}
 
-			uint64_t dmi_out = riscv_batch_get_dmi_read(batch, rereads);
+			riscv_addr_t offset = receive_addr - address;
+			uint64_t dmi_out = riscv_batch_get_dmi_read(batch, i);
 			uint32_t value = get_field(dmi_out, DTM_DMI_DATA);
 			write_to_buf(buffer + offset, value, size);
+			LOG_DEBUG("M[0x%" TARGET_PRIxADDR "] reads 0x%08x", receive_addr,
+					value);
 
-			rereads++;
-
-			LOG_DEBUG("M[0x%" TARGET_PRIxADDR "] reads 0x%08x", addr, value);
+			receive_addr += size;
 		}
 		riscv_batch_free(batch);
 
-		cur_addr = next_addr;
+		if (cmderr == CMDERR_BUSY) {
+			riscv_addr_t offset = receive_addr - address;
+			write_to_buf(buffer + offset, dmi_data0, size);
+			LOG_DEBUG("M[0x%" TARGET_PRIxADDR "] reads 0x%08x", receive_addr,
+					dmi_data0);
+			read_addr += size;
+			receive_addr += size;
+		}
 	}
 
-	riscv013_set_autoexec(target, d_data, 0);
+	dmi_write(target, DMI_ABSTRACTAUTO, 0);
 
-	// Read the last word.
+	if (count > 1) {
+		// Read the penultimate word.
+		uint64_t value = dmi_read(target, DMI_DATA0);
+		write_to_buf(buffer + receive_addr - address, value, size);
+		LOG_DEBUG("M[0x%" TARGET_PRIxADDR "] reads 0x%" PRIx64, receive_addr, value);
+		receive_addr += size;
+	}
 
-	// Access debug buffer without executing a program. This
-	// address logic was taken from program.c.
-	uint32_t value = riscv013_read_debug_buffer(target, d_data);
-	riscv_addr_t addr = cur_addr - size;
-	write_to_buf(buffer + addr - address, value, size);
-	LOG_DEBUG("M[0x%" TARGET_PRIxADDR "] reads 0x%08x", addr, value);
+	// Read the last word.
+	uint64_t value;
+	if (register_read_direct(target, &value, GDB_REGNO_S1) != ERROR_OK)
+		return ERROR_FAIL;
+	write_to_buf(buffer + receive_addr - address, value, size);
+	LOG_DEBUG("M[0x%" TARGET_PRIxADDR "] reads 0x%" PRIx64, receive_addr, value);
+	receive_addr += size;
 
 	riscv_set_register(target, GDB_REGNO_S0, s0);
 	riscv_set_register(target, GDB_REGNO_S1, s1);
@@ -1508,22 +1760,19 @@ static int write_memory(struct target *target, target_addr_t address,
 
 	select_dmi(target);
 
-	/* This program uses two temporary registers.  A word of data and the
-	 * associated address are stored at some location in memory.  The
-	 * program stores the word to that address and then increments the
-	 * address.  The debugger is expected to feed the memory word-by-word
-	 * into the chip with AUTOEXEC set in order to trigger program
-	 * execution on every word. */
-	uint64_t s0 = riscv_get_register(target, GDB_REGNO_S0);
-	uint64_t s1 = riscv_get_register(target, GDB_REGNO_S1);
+	/* s0 holds the next address to write to
+	 * s1 holds the next data value to write
+	 */
 
+	uint64_t s0, s1;
+	if (register_read_direct(target, &s0, GDB_REGNO_S0) != ERROR_OK)
+		return ERROR_FAIL;
+	if (register_read_direct(target, &s1, GDB_REGNO_S1) != ERROR_OK)
+		return ERROR_FAIL;
+
+	// Write the program (store, increment)
 	struct riscv_program program;
 	riscv_program_init(&program, target);
-	riscv_addr_t r_data = riscv_program_alloc_w(&program);
-	riscv_addr_t r_addr = riscv_program_alloc_x(&program);
-	riscv_program_fence(&program);
-	riscv_program_lx(&program, GDB_REGNO_S0, r_addr);
-	riscv_program_lw(&program, GDB_REGNO_S1, r_data);
 
 	switch (size) {
 		case 1:
@@ -1541,83 +1790,31 @@ static int write_memory(struct target *target, target_addr_t address,
 	}
 
 	riscv_program_addi(&program, GDB_REGNO_S0, GDB_REGNO_S0, size);
-	riscv_program_sx(&program, GDB_REGNO_S0, r_addr);
-
-	/* The first round through the program's execution we use the regular
-	 * program execution mechanism. */
-	uint32_t value;
-	switch (size) {
-		case 1:
-			value = buffer[0];
-			break;
-		case 2:
-			value = buffer[0]
-				| ((uint32_t) buffer[1] << 8);
-			break;
-		case 4:
-			value = buffer[0]
-				| ((uint32_t) buffer[1] << 8)
-				| ((uint32_t) buffer[2] << 16)
-				| ((uint32_t) buffer[3] << 24);
-			break;
-		default:
-			LOG_ERROR("unsupported access size: %d", size);
-			return ERROR_FAIL;
-	}
-
-	switch (riscv_xlen(target)) {
-	case 64:
-		riscv_program_write_ram(&program, r_addr + 4, (uint64_t)address >> 32);
-	case 32:
-		riscv_program_write_ram(&program, r_addr, address);
-		break;
-	default:
-		LOG_ERROR("unknown XLEN %d", riscv_xlen(target));
-		return ERROR_FAIL;
-	}
-	riscv_program_write_ram(&program, r_data, value);
-
-	LOG_DEBUG("M[0x%08lx] writes 0x%08x", (long)address, value);
 
-	if (riscv_program_exec(&program, target) != ERROR_OK) {
-		uint32_t acs = dmi_read(target, DMI_ABSTRACTCS);
-		LOG_ERROR("failed to execute program, abstractcs=0x%08x", acs);
-		riscv013_clear_abstract_error(target);
-		riscv_set_register(target, GDB_REGNO_S0, s0);
-		riscv_set_register(target, GDB_REGNO_S1, s1);
-		LOG_ERROR("  exiting with ERROR_FAIL");
+	if (riscv_program_ebreak(&program) != ERROR_OK)
 		return ERROR_FAIL;
-	}
-
-	/* The rest of this program is designed to be fast so it reads various
-	 * DMI registers directly. */
-	int d_data = (r_data - riscv_debug_buffer_addr(target)) / 4;
-	int d_addr = (r_addr - riscv_debug_buffer_addr(target)) / 4;
-
-	riscv013_set_autoexec(target, d_data, 1);
+	riscv_program_write(&program);
 
-	/* Copying memory might fail because we're going too quickly, in which
-	 * case we need to back off a bit and try again.  There's two
-	 * termination conditions to this loop: a non-BUSY error message, or
-	 * the data was all copied. */
-	riscv_addr_t cur_addr = 0xbadbeef;
+	riscv_addr_t cur_addr = address;
 	riscv_addr_t fin_addr = address + (count * size);
+	bool setup_needed = true;
 	LOG_DEBUG("writing until final address 0x%016" PRIx64, fin_addr);
-	while ((cur_addr = riscv_read_debug_buffer_x(target, d_addr)) < fin_addr) {
+	while (cur_addr < fin_addr) {
 		LOG_DEBUG("transferring burst starting at address 0x%016" PRIx64,
 				cur_addr);
-		riscv_addr_t start = (cur_addr - address) / size;
-		assert (cur_addr > address);
+
 		struct riscv_batch *batch = riscv_batch_alloc(
-			target,
-			32,
-			info->dmi_busy_delay + info->ac_busy_delay);
+				target,
+				32,
+				info->dmi_busy_delay + info->ac_busy_delay);
 
-		for (riscv_addr_t i = start; i < count; ++i) {
-			riscv_addr_t offset = size*i;
-			riscv_addr_t t_addr = address + offset;
+		/* To write another word, we put it in S1 and execute the program. */
+		unsigned start = (cur_addr - address) / size;
+		for (unsigned i = start; i < count; ++i) {
+			unsigned offset = size*i;
 			const uint8_t *t_buffer = buffer + offset;
 
+			uint32_t value;
 			switch (size) {
 				case 1:
 					value = t_buffer[0];
@@ -1637,14 +1834,37 @@ static int write_memory(struct target *target, target_addr_t address,
 					return ERROR_FAIL;
 			}
 
-			LOG_DEBUG("M[0x%08lx] writes 0x%08x", (long)t_addr, value);
+			LOG_DEBUG("M[0x%08" PRIx64 "] writes 0x%08x", address + offset, value);
+			cur_addr += size;
 
-			riscv_batch_add_dmi_write(
-				batch,
-				riscv013_debug_buffer_register(target, r_data),
-				value);
-			if (riscv_batch_full(batch))
-				break;
+			if (setup_needed) {
+				if (register_write_direct(target, GDB_REGNO_S0,
+							address + offset) != ERROR_OK)
+					return ERROR_FAIL;
+
+				// Write value.
+				dmi_write(target, DMI_DATA0, value);
+
+				// Write and execute command that moves value into S1 and
+				// executes program buffer.
+				uint32_t command = access_register_command(GDB_REGNO_S1, 32, 
+						AC_ACCESS_REGISTER_POSTEXEC |
+						AC_ACCESS_REGISTER_TRANSFER |
+						AC_ACCESS_REGISTER_WRITE);
+				int result = execute_abstract_command(target, command);
+				if (result != ERROR_OK)
+					return result;
+
+				// Turn on autoexec
+				dmi_write(target, DMI_ABSTRACTAUTO,
+						1 << DMI_ABSTRACTAUTO_AUTOEXECDATA_OFFSET);
+
+				setup_needed = false;
+			} else {
+				riscv_batch_add_dmi_write(batch, DMI_DATA0, value);
+				if (riscv_batch_full(batch))
+					break;
+			}
 		}
 
 		riscv_batch_run(batch);
@@ -1652,35 +1872,47 @@ static int write_memory(struct target *target, target_addr_t address,
 
 		// Note that if the scan resulted in a Busy DMI response, it
 		// is this read to abstractcs that will cause the dmi_busy_delay
-		// to be incremented if necessary. The loop condition above
-		// catches the case where no writes went through at all.
+		// to be incremented if necessary.
 
 		uint32_t abstractcs = dmi_read(target, DMI_ABSTRACTCS);
 		while (get_field(abstractcs, DMI_ABSTRACTCS_BUSY))
 			abstractcs = dmi_read(target, DMI_ABSTRACTCS);
 		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);
-			break;
-		default:
-			LOG_ERROR("error when writing memory, abstractcs=0x%08lx", (long)abstractcs);
-			riscv013_set_autoexec(target, d_data, 0);
-			riscv013_clear_abstract_error(target);
-			riscv_set_register(target, GDB_REGNO_S0, s0);
-			riscv_set_register(target, GDB_REGNO_S1, s1);
-			return ERROR_FAIL;
+			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);
+				if (register_read_direct(target, &cur_addr, GDB_REGNO_S0) != ERROR_OK)
+					return ERROR_FAIL;
+				setup_needed = true;
+				break;
+
+			default:
+				LOG_ERROR("error when writing memory, abstractcs=0x%08lx", (long)abstractcs);
+				dmi_write(target, DMI_ABSTRACTAUTO, 0);
+				riscv013_clear_abstract_error(target);
+				riscv_set_register(target, GDB_REGNO_S0, s0);
+				riscv_set_register(target, GDB_REGNO_S1, s1);
+				return ERROR_FAIL;
 		}
 	}
 
-	riscv013_set_autoexec(target, d_data, 0);
-	riscv_set_register(target, GDB_REGNO_S0, s0);
-	riscv_set_register(target, GDB_REGNO_S1, s1);
+	dmi_write(target, DMI_ABSTRACTAUTO, 0);
+
+	if (register_write_direct(target, GDB_REGNO_S1, s1) != ERROR_OK)
+		return ERROR_FAIL;
+	if (register_write_direct(target, GDB_REGNO_S0, s0) != ERROR_OK)
+		return ERROR_FAIL;
+
+	if (execute_fence(target) != ERROR_OK)
+		return ERROR_FAIL;
+
 	return ERROR_OK;
 }
 
@@ -1860,25 +2092,13 @@ static enum riscv_halt_reason riscv013_halt_reason(struct target *target)
 	abort();
 }
 
-void riscv013_debug_buffer_enter(struct target *target, struct riscv_program *program)
-{
-}
-
-void riscv013_debug_buffer_leave(struct target *target, struct riscv_program *program)
-{
-}
-
 void riscv013_write_debug_buffer(struct target *target, unsigned index, riscv_insn_t data)
 {
-	if (index >= riscv013_progbuf_size(target))
-		return dmi_write(target, DMI_DATA0 + index - riscv013_progbuf_size(target), data);
 	return dmi_write(target, DMI_PROGBUF0 + index, data);
 }
 
 riscv_insn_t riscv013_read_debug_buffer(struct target *target, unsigned index)
 {
-	if (index >= riscv013_progbuf_size(target))
-		return dmi_read(target, DMI_DATA0 + index - riscv013_progbuf_size(target));
 	return dmi_read(target, DMI_PROGBUF0 + index);
 }
 
@@ -1936,7 +2156,6 @@ static void riscv013_on_step_or_resume(struct target *target, bool step)
 	uint64_t dcsr = riscv_get_register(target, GDB_REGNO_DCSR);
 	dcsr = set_field(dcsr, CSR_DCSR_STEP, step);
 	dcsr = set_field(dcsr, CSR_DCSR_EBREAKM, 1);
-	dcsr = set_field(dcsr, CSR_DCSR_EBREAKH, 1);
 	dcsr = set_field(dcsr, CSR_DCSR_EBREAKS, 1);
 	dcsr = set_field(dcsr, CSR_DCSR_EBREAKU, 1);
 	riscv_set_register(target, GDB_REGNO_DCSR, dcsr);
@@ -1987,72 +2206,6 @@ static void riscv013_step_or_resume_current_hart(struct target *target, bool ste
 	abort();
 }
 
-riscv_addr_t riscv013_progbuf_addr(struct target *target)
-{
-	RISCV013_INFO(info);
-	assert(info->progbuf_addr != -1);
-	return info->progbuf_addr;
-}
-
-riscv_addr_t riscv013_progbuf_size(struct target *target)
-{
-	RISCV013_INFO(info);
-	if (info->progbuf_size == -1) {
-		uint32_t acs = dmi_read(target, DMI_ABSTRACTCS);
-		info->progbuf_size = get_field(acs, DMI_ABSTRACTCS_PROGSIZE);
-	}
-	return info->progbuf_size;
-}
-
-riscv_addr_t riscv013_data_size(struct target *target)
-{
-	RISCV013_INFO(info);
-	if (info->data_size == -1) {
-		uint32_t acs = dmi_read(target, DMI_HARTINFO);
-		info->data_size = get_field(acs, DMI_HARTINFO_DATASIZE);
-	}
-	return info->data_size;
-}
-
-riscv_addr_t riscv013_data_addr(struct target *target)
-{
-	RISCV013_INFO(info);
-	if (info->data_addr == -1) {
-		uint32_t acs = dmi_read(target, DMI_HARTINFO);
-		info->data_addr = get_field(acs, DMI_HARTINFO_DATAACCESS) ? get_field(acs, DMI_HARTINFO_DATAADDR) : 0;
-	}
-	return info->data_addr;
-}
-
-void riscv013_set_autoexec(struct target *target, unsigned index, bool enabled)
-{
-	if (index >= riscv013_progbuf_size(target)) {
-		LOG_DEBUG("setting bit %d in AUTOEXECDATA to %d", index, enabled);
-		uint32_t aa = dmi_read(target, DMI_ABSTRACTAUTO);
-		uint32_t aa_aed = get_field(aa, DMI_ABSTRACTAUTO_AUTOEXECDATA);
-		aa_aed &= ~(1 << (index - riscv013_progbuf_size(target)));
-		aa_aed |= (enabled << (index - riscv013_progbuf_size(target)));
-		aa = set_field(aa, DMI_ABSTRACTAUTO_AUTOEXECDATA, aa_aed);
-		dmi_write(target, DMI_ABSTRACTAUTO, aa);
-	} else {
-		LOG_DEBUG("setting bit %d in AUTOEXECPROGBUF to %d", index, enabled);
-		uint32_t aa = dmi_read(target, DMI_ABSTRACTAUTO);
-		uint32_t aa_aed = get_field(aa, DMI_ABSTRACTAUTO_AUTOEXECPROGBUF);
-		aa_aed &= ~(1 << index);
-		aa_aed |= (enabled << index);
-		aa = set_field(aa, DMI_ABSTRACTAUTO_AUTOEXECPROGBUF, aa_aed);
-		dmi_write(target, DMI_ABSTRACTAUTO, aa);
-	}
-}
-
-int riscv013_debug_buffer_register(struct target *target, riscv_addr_t addr)
-{
-	if (addr >= riscv013_data_addr(target))
-		return DMI_DATA0 + (addr - riscv013_data_addr(target)) / 4;
-	else
-		return DMI_PROGBUF0 + (addr - riscv013_progbuf_addr(target)) / 4;
-}
-
 void riscv013_clear_abstract_error(struct target *target)
 {
 	// Wait for busy to go away.
diff --git a/src/target/riscv/riscv.c b/src/target/riscv/riscv.c
index a8fc437..b9be133 100644
--- a/src/target/riscv/riscv.c
+++ b/src/target/riscv/riscv.c
@@ -200,6 +200,9 @@ int riscv_command_timeout_sec = DEFAULT_COMMAND_TIMEOUT_SEC;
 /* Wall-clock timeout after reset. Settable via RISC-V Target commands.*/
 int riscv_reset_timeout_sec = DEFAULT_RESET_TIMEOUT_SEC;
 
+bool riscv_use_scratch_ram = false;
+uint64_t riscv_scratch_ram_address = 0;
+
 static uint32_t dtmcontrol_scan(struct target *target, uint32_t out)
 {
 	struct scan_field field;
@@ -1120,8 +1123,8 @@ int riscv_openocd_step(
 }
 
 /* Command Handlers */
-COMMAND_HANDLER(riscv_set_command_timeout_sec) {
-
+COMMAND_HANDLER(riscv_set_command_timeout_sec)
+{
 	if (CMD_ARGC != 1) {
 		LOG_ERROR("Command takes exactly 1 parameter");
 		return ERROR_COMMAND_SYNTAX_ERROR;
@@ -1137,11 +1140,11 @@ COMMAND_HANDLER(riscv_set_command_timeout_sec) {
 	return ERROR_OK;
 }
 
-COMMAND_HANDLER(riscv_set_reset_timeout_sec) {
-
+COMMAND_HANDLER(riscv_set_reset_timeout_sec)
+{
 	if (CMD_ARGC != 1) {
-			LOG_ERROR("Command takes exactly 1 parameter");
-			return ERROR_COMMAND_SYNTAX_ERROR;
+		LOG_ERROR("Command takes exactly 1 parameter");
+		return ERROR_COMMAND_SYNTAX_ERROR;
 	}
 	int timeout = atoi(CMD_ARGV[0]);
 	if (timeout <= 0){
@@ -1153,6 +1156,29 @@ COMMAND_HANDLER(riscv_set_reset_timeout_sec) {
 	return ERROR_OK;
 }
 
+COMMAND_HANDLER(riscv_set_scratch_ram)
+{
+	if (CMD_ARGC != 1) {
+		LOG_ERROR("Command takes exactly 1 parameter");
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+	if (!strcmp(CMD_ARGV[0], "none")) {
+		riscv_use_scratch_ram = false;
+		return ERROR_OK;
+	}
+
+	long long unsigned int address;
+	int result = sscanf(CMD_ARGV[0], "%Lx", &address);
+	if (result != (int) strlen(CMD_ARGV[0])) {
+		LOG_ERROR("%s is not a valid address for command.", CMD_ARGV[0]);
+		riscv_use_scratch_ram = false;
+		return ERROR_FAIL;
+	}
+
+	riscv_scratch_ram_address = address;
+	riscv_use_scratch_ram = true;
+	return ERROR_OK;
+}
 
 static const struct command_registration riscv_exec_command_handlers[] = {
 	{
@@ -1161,14 +1187,21 @@ static const struct command_registration riscv_exec_command_handlers[] = {
 		.mode = COMMAND_ANY,
 		.usage = "riscv set_command_timeout_sec [sec]",
 		.help = "Set the wall-clock timeout (in seconds) for individual commands"
-	 },
-	 {
+	},
+	{
 		.name = "set_reset_timeout_sec",
 		.handler = riscv_set_reset_timeout_sec,
 		.mode = COMMAND_ANY,
 		.usage = "riscv set_reset_timeout_sec [sec]",
 		.help = "Set the wall-clock timeout (in seconds) after reset is deasserted"
 	},
+	{
+		.name = "set_scratch_ram",
+		.handler = riscv_set_scratch_ram,
+		.mode = COMMAND_ANY,
+		.usage = "riscv set_scratch_ram none|[address]",
+		.help = "Set address of 16 bytes of scratch RAM the debugger can use, or 'none'."
+	},
 	COMMAND_REGISTRATION_DONE
 };
 
@@ -1235,7 +1268,6 @@ void riscv_info_init(struct target *target, riscv_info_t *r)
 
 	for (size_t h = 0; h < RISCV_MAX_HARTS; ++h) {
 		r->xlen[h] = -1;
-		r->debug_buffer_addr[h] = -1;
 
 		for (size_t e = 0; e < RISCV_MAX_REGISTERS; ++e)
 			r->valid_saved_registers[h][e] = false;
@@ -1483,28 +1515,6 @@ size_t riscv_debug_buffer_size(struct target *target)
 	return r->debug_buffer_size[riscv_current_hartid(target)];
 }
 
-riscv_addr_t riscv_debug_buffer_addr(struct target *target)
-{
-	RISCV_INFO(r);
-	riscv_addr_t out = r->debug_buffer_addr[riscv_current_hartid(target)];
-	assert((out & 3) == 0);
-	return out;
-}
-
-int riscv_debug_buffer_enter(struct target *target, struct riscv_program *program)
-{
-	RISCV_INFO(r);
-	r->debug_buffer_enter(target, program);
-	return ERROR_OK;
-}
-
-int riscv_debug_buffer_leave(struct target *target, struct riscv_program *program)
-{
-	RISCV_INFO(r);
-	r->debug_buffer_leave(target, program);
-	return ERROR_OK;
-}
-
 int riscv_write_debug_buffer(struct target *target, int index, riscv_insn_t insn)
 {
 	RISCV_INFO(r);
diff --git a/src/target/riscv/riscv.h b/src/target/riscv/riscv.h
index 1197cbf..4ff6127 100644
--- a/src/target/riscv/riscv.h
+++ b/src/target/riscv/riscv.h
@@ -72,15 +72,15 @@ typedef struct {
 	 * target controls, while otherwise only a single hart is controlled. */
 	int trigger_unique_id[RISCV_MAX_HWBPS];
 
-	/* The address of the debug RAM buffer. */
-	riscv_addr_t debug_buffer_addr[RISCV_MAX_HARTS];
-
 	/* The number of entries in the debug buffer. */
 	int debug_buffer_size[RISCV_MAX_HARTS];
 
 	/* This avoids invalidating the register cache too often. */
 	bool registers_initialized;
 
+	/* This hart contains an implicit ebreak at the end of the program buffer. */
+	bool impebreak;
+
 	/* Helper functions that target the various RISC-V debug spec
 	 * implementations. */
 	riscv_reg_t (*get_register)(struct target *, int hartid, int regid);
@@ -95,8 +95,6 @@ typedef struct {
 	void (*on_resume)(struct target *target);
 	void (*on_step)(struct target *target);
 	enum riscv_halt_reason (*halt_reason)(struct target *target);
-	void (*debug_buffer_enter)(struct target *target, struct riscv_program *program);
-	void (*debug_buffer_leave)(struct target *target, struct riscv_program *program);
 	void (*write_debug_buffer)(struct target *target, unsigned index,
 			riscv_insn_t d);
 	riscv_insn_t (*read_debug_buffer)(struct target *target, unsigned index);
@@ -113,6 +111,9 @@ extern int riscv_command_timeout_sec;
 /* Wall-clock timeout after reset. Settable via RISC-V Target commands.*/
 extern int riscv_reset_timeout_sec;
 
+extern bool riscv_use_scratch_ram;
+extern uint64_t riscv_scratch_ram_address;
+
 /* Everything needs the RISC-V specific info structure, so here's a nice macro
  * that provides that. */
 static inline riscv_info_t *riscv_info(const struct target *target) __attribute__((unused));
@@ -213,10 +214,6 @@ int riscv_count_triggers_of_hart(struct target *target, int hartid);
 /* These helper functions let the generic program interface get target-specific
  * information. */
 size_t riscv_debug_buffer_size(struct target *target);
-riscv_addr_t riscv_debug_buffer_addr(struct target *target);
-
-int riscv_debug_buffer_enter(struct target *target, struct riscv_program *program);
-int riscv_debug_buffer_leave(struct target *target, struct riscv_program *program);
 
 riscv_insn_t riscv_read_debug_buffer(struct target *target, int index);
 riscv_addr_t riscv_read_debug_buffer_x(struct target *target, int index);