From bb2cceac2513fa782f28f3c660a6f64a3a1cddce Mon Sep 17 00:00:00 2001
From: Palmer Dabbelt <palmer@dabbelt.com>
Date: Wed, 12 Apr 2017 16:29:39 -0700
Subject: [PATCH] Replace the 0.13-specific "program_t" with a generic one

This new version changes how we handle temporary registers: rather than
tracking them by hand (like in the old code), they're now tracked as
part of assembling programs.  The register save/restore assertion that
used to fire all the time no longer exists.
---
 src/rtos/riscv_debug.c           |  14 +-
 src/target/Makefile.am           |   3 +-
 src/target/riscv/debug_defines.h | 197 ++------
 src/target/riscv/gdb_regs.h      |   3 +
 src/target/riscv/opcodes.h       |  18 +-
 src/target/riscv/program.c       | 459 ++++++++++++++++++
 src/target/riscv/program.h       | 135 ++++++
 src/target/riscv/riscv-013.c     | 809 +++++++++++++++++--------------
 src/target/riscv/riscv.c         | 138 ++++--
 src/target/riscv/riscv.h         |  40 +-
 10 files changed, 1252 insertions(+), 564 deletions(-)
 create mode 100644 src/target/riscv/program.c
 create mode 100644 src/target/riscv/program.h

diff --git a/src/rtos/riscv_debug.c b/src/rtos/riscv_debug.c
index 4161f5875..3677466d8 100644
--- a/src/rtos/riscv_debug.c
+++ b/src/rtos/riscv_debug.c
@@ -213,14 +213,9 @@ static int riscv_gdb_thread_packet(struct connection *connection, const char *pa
 		return JIM_OK;
 
 	case 'R':
-	{
-		char *packet_str = malloc(packet_size + 1);
-		memset(packet_str, '\0', packet_size + 1);
-		memcpy(packet_str, packet, packet_size);
-		LOG_WARNING("riscv_gdb_thread_packet(%s): unimplemented", packet_str);
-		gdb_put_packet(connection, NULL, 0);
+		gdb_put_packet(connection, "E00", 3);
 		return JIM_OK;
-	}
+
 	default:
 		LOG_ERROR("Unknown packet of type 0x%2.2x", packet[0]);
 		gdb_put_packet(connection, NULL, 0);
@@ -285,10 +280,10 @@ static int riscv_get_thread_reg_list(struct rtos *rtos, int64_t thread_id, char
 	*hex_reg_list[0] = '\0';
 	for (size_t i = 0; i < n_regs; ++i) {
 		if (riscv_has_register(rtos->target, thread_id, i)) {
-			uint64_t reg_value = riscv_get_register(rtos->target, thread_id - 1, i);
+			uint64_t reg_value = riscv_get_register_on_hart(rtos->target, thread_id - 1, i);
 			for (size_t byte = 0; byte < xlen / 8; ++byte) {
 				uint8_t reg_byte = reg_value >> (byte * 8);
-				char hex[3];
+				char hex[3] = {'x', 'x', 'x'};
 				snprintf(hex, 3, "%02x", reg_byte);
 				strncat(*hex_reg_list, hex, hex_reg_list_length);
 			}
@@ -297,6 +292,7 @@ static int riscv_get_thread_reg_list(struct rtos *rtos, int64_t thread_id, char
 				strncat(*hex_reg_list, "xx", hex_reg_list_length);
 		}
 	}
+	LOG_DEBUG(*hex_reg_list);
 	return JIM_OK;
 }
 
diff --git a/src/target/Makefile.am b/src/target/Makefile.am
index 44299fc15..07b7cdac6 100644
--- a/src/target/Makefile.am
+++ b/src/target/Makefile.am
@@ -138,7 +138,8 @@ INTEL_IA32_SRC = \
 RISCV_SRC = \
 	riscv/riscv-011.c \
 	riscv/riscv-013.c \
-	riscv/riscv.c
+	riscv/riscv.c \
+	riscv/program.c
 
 noinst_HEADERS = \
 	algorithm.h \
diff --git a/src/target/riscv/debug_defines.h b/src/target/riscv/debug_defines.h
index ec535dbb6..2fb541b8e 100644
--- a/src/target/riscv/debug_defines.h
+++ b/src/target/riscv/debug_defines.h
@@ -26,15 +26,28 @@
 #define DTM_IDCODE_1                        (0x1 << DTM_IDCODE_1_OFFSET)
 #define DTM_DTMCS                           0x10
 /*
-* Writing 1 to this bit resets the DMI controller, clearing any
-* sticky error state.
+* Writing 1 to this bit does a hard reset of the DTM,
+* causing the DTM to forget about any outstanding DMI transactions.
+* In general this should only be used when the Debugger has
+* reason to expect that the outstanding DMI transaction will never
+* complete (e.g. a reset condition caused an inflight DMI transaction to
+* be cancelled).
+ */
+#define DTM_DTMCS_DMIHARDRESET_OFFSET       17
+#define DTM_DTMCS_DMIHARDRESET_LENGTH       1
+#define DTM_DTMCS_DMIHARDRESET              (0x1 << DTM_DTMCS_DMIHARDRESET_OFFSET)
+/*
+* Writing 1 to this bit clears the sticky error state
+* and allows the DTM to retry or complete the previous
+* transaction.
  */
 #define DTM_DTMCS_DMIRESET_OFFSET           16
 #define DTM_DTMCS_DMIRESET_LENGTH           1
 #define DTM_DTMCS_DMIRESET                  (0x1 << DTM_DTMCS_DMIRESET_OFFSET)
 /*
-* This is the minimum number of cycles a debugger should spend in
-* Run-Test/Idle after every DMI scan to avoid a 'busy'
+* This is a hint to the debugger of the minimum number of
+* cycles a debugger should spend in
+* Run-Test/Idle after every DMI scan to avoid a `busy'
 * return code (\Fdmistat of 3). A debugger must still
 * check \Fdmistat when necessary.
 *
@@ -146,26 +159,26 @@
 #define CSR_DCSR_XDEBUGVER_LENGTH           2
 #define CSR_DCSR_XDEBUGVER                  (0x3 << CSR_DCSR_XDEBUGVER_OFFSET)
 /*
-* When 1, {\tt ebreak} instructions in Machine Mode enter Halt Mode.
+* When 1, {\tt ebreak} instructions in Machine Mode enter Debug Mode.
  */
 #define CSR_DCSR_EBREAKM_OFFSET             15
 #define CSR_DCSR_EBREAKM_LENGTH             1
 #define CSR_DCSR_EBREAKM                    (0x1 << CSR_DCSR_EBREAKM_OFFSET)
 /*
-* When 1, {\tt ebreak} instructions in Hypervisor Mode enter Halt Mode.
+* When 1, {\tt ebreak} instructions in Hypervisor Mode enter Debug Mode.
  */
 #define CSR_DCSR_EBREAKH_OFFSET             14
 #define CSR_DCSR_EBREAKH_LENGTH             1
 #define CSR_DCSR_EBREAKH                    (0x1 << CSR_DCSR_EBREAKH_OFFSET)
 /*
-* When 1, {\tt ebreak} instructions in Supervisor Mode enter Halt Mode.
+* When 1, {\tt ebreak} instructions in Supervisor Mode enter Debug Mode.
  */
 #define CSR_DCSR_EBREAKS_OFFSET             13
 #define CSR_DCSR_EBREAKS_LENGTH             1
 #define CSR_DCSR_EBREAKS                    (0x1 << CSR_DCSR_EBREAKS_OFFSET)
 /*
 * When 1, {\tt ebreak} instructions in User/Application Mode enter
-* Halt Mode.
+* Debug Mode.
  */
 #define CSR_DCSR_EBREAKU_OFFSET             12
 #define CSR_DCSR_EBREAKU_LENGTH             1
@@ -173,7 +186,7 @@
 /*
 * 0: Increment counters as usual.
 *
-* 1: Don't increment any counters while in Halt Mode.  This includes
+* 1: Don't increment any counters while in Debug Mode.  This includes
 * the {\tt cycle} and {\tt instret} CSRs. This is preferred for most
 * debugging scenarios.
 *
@@ -187,7 +200,7 @@
 /*
 * 0: Increment timers as usual.
 *
-* 1: Don't increment any hart-local timers while in Halt Mode.
+* 1: Don't increment any hart-local timers while in Debug Mode.
 *
 * An implementation may choose not to support writing to this bit.
 * The debugger must read back the value it writes to check whether
@@ -197,29 +210,27 @@
 #define CSR_DCSR_STOPTIME_LENGTH            1
 #define CSR_DCSR_STOPTIME                   (0x1 << CSR_DCSR_STOPTIME_OFFSET)
 /*
-* Explains why Halt Mode was entered.
+* Explains why Debug Mode was entered.
 *
-* When there are multiple reasons to enter Halt Mode in a single
+* When there are multiple reasons to enter Debug Mode in a single
 * cycle, the cause with the highest priority is the one written.
 *
-* 1: A software breakpoint was hit. (priority 3)
+* 1: An {\tt ebreak} instruction was executed. (priority 3)
 *
 * 2: The Trigger Module caused a halt. (priority 4)
 *
-* 3: The debug interrupt was asserted by the Debug Module. (priority 2)
+* 3: \Fhaltreq was set. (priority 2)
 *
 * 4: The hart single stepped because \Fstep was set. (priority 1)
 *
-* 5: \Fhaltreq was set. (priority 0)
-*
 * Other values are reserved for future use.
  */
 #define CSR_DCSR_CAUSE_OFFSET               6
 #define CSR_DCSR_CAUSE_LENGTH               3
 #define CSR_DCSR_CAUSE                      (0x7 << CSR_DCSR_CAUSE_OFFSET)
 /*
-* When set and not in Halt Mode, the hart will only execute a single
-* instruction, and then enter Halt Mode. Interrupts are disabled
+* When set and not in Debug Mode, the hart will only execute a single
+* instruction, and then enter Debug Mode. Interrupts are disabled
 * when this bit is set.
  */
 #define CSR_DCSR_STEP_OFFSET                2
@@ -227,9 +238,9 @@
 #define CSR_DCSR_STEP                       (0x1 << CSR_DCSR_STEP_OFFSET)
 /*
 * Contains the privilege level the hart was operating in when Debug
-* Mode was entered. The encoding is describe in Table
+* Mode was entered. The encoding is described in Table
 * \ref{tab:privlevel}.  A debugger can change this value to change
-* the hart's privilege level when exiting Halt Mode.
+* the hart's privilege level when exiting Debug Mode.
 *
 * Not all privilege levels are supported on all harts. If the
 * encoding written is not supported or the debugger is not allowed to
@@ -247,9 +258,9 @@
 #define CSR_PRIV                            virtual
 /*
 * Contains the privilege level the hart was operating in when Debug
-* Mode was entered. The encoding is describe in Table
+* Mode was entered. The encoding is described in Table
 * \ref{tab:privlevel}. A user can write this value to change the
-* hart's privilege level when exiting Halt Mode.
+* hart's privilege level when exiting Debug Mode.
  */
 #define CSR_PRIV_PRV_OFFSET                 0
 #define CSR_PRIV_PRV_LENGTH                 2
@@ -283,10 +294,10 @@
 * 0: Both Debug and M Mode can write the {\tt tdata} registers at the
 * selected \Rtselect.
 *
-* 1: Only Halt Mode can write the {\tt tdata} registers at the
+* 1: Only Debug Mode can write the {\tt tdata} registers at the
 * selected \Rtselect.  Writes from other modes are ignored.
 *
-* This bit is only writable from Halt Mode.
+* This bit is only writable from Debug Mode.
  */
 #define CSR_TDATA1_HMODE_OFFSET             XLEN-5
 #define CSR_TDATA1_HMODE_LENGTH             1
@@ -366,7 +377,7 @@
 * 0: Raise a breakpoint exception. (Used when software wants to use
 * the trigger module without an external debugger attached.)
 *
-* 1: Enter Halt Mode. (Only supported when \Fhmode is 1.)
+* 1: Enter Debug Mode. (Only supported when \Fhmode is 1.)
 *
 * 2: Start tracing.
 *
@@ -504,10 +515,10 @@
 /*
 * Determines what happens when this trigger matches.
 *
-* 0: Raise a debug exception. (Used when software wants to use the
+* 0: Raise a breakpoint exception. (Used when software wants to use the
 * trigger module without an external debugger attached.)
 *
-* 1: Enter Halt Mode. (Only supported when \Fhmode is 1.)
+* 1: Enter Debug Mode. (Only supported when \Fhmode is 1.)
 *
 * 2: Start tracing.
 *
@@ -524,6 +535,18 @@
 #define CSR_ICOUNT_ACTION                   (0x3fL << CSR_ICOUNT_ACTION_OFFSET)
 #define DMI_DMSTATUS                        0x11
 /*
+* This field is 1 when all currently selected harts have acknowledged the previous \Fresumereq.
+ */
+#define DMI_DMSTATUS_ALLRESUMEACK_OFFSET    17
+#define DMI_DMSTATUS_ALLRESUMEACK_LENGTH    1
+#define DMI_DMSTATUS_ALLRESUMEACK           (0x1 << DMI_DMSTATUS_ALLRESUMEACK_OFFSET)
+/*
+* This field is 1 when any currently selected hart has acknowledged the previous \Fresumereq.
+ */
+#define DMI_DMSTATUS_ANYRESUMEACK_OFFSET    16
+#define DMI_DMSTATUS_ANYRESUMEACK_LENGTH    1
+#define DMI_DMSTATUS_ANYRESUMEACK           (0x1 << DMI_DMSTATUS_ANYRESUMEACK_OFFSET)
+/*
 * This field is 1 when all currently selected harts do not exist in this system.
  */
 #define DMI_DMSTATUS_ALLNONEXISTENT_OFFSET  15
@@ -617,7 +640,7 @@
 #define DMI_DMSTATUS_VERSIONLO              (0x3 << DMI_DMSTATUS_VERSIONLO_OFFSET)
 #define DMI_DMCONTROL                       0x10
 /*
-* Halt request signal for all currently selected harts. When 1, the
+* Halt request signal for all currently selected harts. When set to 1, the
 * hart will halt if it is not currently halted.
 * Setting both \Fhaltreq and \Fresumereq leads to undefined behavior.
 *
@@ -627,7 +650,7 @@
 #define DMI_DMCONTROL_HALTREQ_LENGTH        1
 #define DMI_DMCONTROL_HALTREQ               (0x1 << DMI_DMCONTROL_HALTREQ_OFFSET)
 /*
-* Resume request signal for all currently selected harts. When 1,
+* Resume request signal for all currently selected harts. When set to 1,
 * the hart will resume if it is currently halted.
 * Setting both \Fhaltreq and \Fresumereq leads to undefined behavior.
 *
@@ -942,31 +965,12 @@
 #define DMI_DATA0_DATA_OFFSET               0
 #define DMI_DATA0_DATA_LENGTH               32
 #define DMI_DATA0_DATA                      (0xffffffff << DMI_DATA0_DATA_OFFSET)
-#define DMI_DATA1                           0x05
-#define DMI_DATA2                           0x06
-#define DMI_DATA3                           0x07
-#define DMI_DATA4                           0x08
-#define DMI_DATA5                           0x09
-#define DMI_DATA6                           0x0a
-#define DMI_DATA7                           0x0b
-#define DMI_DATA8                           0x0c
-#define DMI_DATA9                           0x0d
-#define DMI_DATA10                          0x0e
 #define DMI_DATA11                          0x0f
 #define DMI_PROGBUF0                        0x20
 #define DMI_PROGBUF0_DATA_OFFSET            0
 #define DMI_PROGBUF0_DATA_LENGTH            32
 #define DMI_PROGBUF0_DATA                   (0xffffffff << DMI_PROGBUF0_DATA_OFFSET)
-#define DMI_PROGBUF1                        0x21
-#define DMI_PROGBUF2                        0x22
-#define DMI_PROGBUF3                        0x23
-#define DMI_PROGBUF4                        0x24
-#define DMI_PROGBUF5                        0x25
-#define DMI_PROGBUF6                        0x26
-#define DMI_PROGBUF7                        0x27
-#define DMI_PROGBUF8                        0x28
-#define DMI_PROGBUF9                        0x29
-#define DMI_PROGBUF10                       0x2a
+#define DMI_PROGBUF15                       0x2f
 #define DMI_AUTHDATA                        0x30
 #define DMI_AUTHDATA_DATA_OFFSET            0
 #define DMI_AUTHDATA_DATA_LENGTH            32
@@ -1233,93 +1237,6 @@
 #define DMI_SBDATA3_DATA_OFFSET             0
 #define DMI_SBDATA3_DATA_LENGTH             32
 #define DMI_SBDATA3_DATA                    (0xffffffff << DMI_SBDATA3_DATA_OFFSET)
-#define SERINFO                             0x280
-/*
-* Like \Fserialzero.
- */
-#define SERINFO_SERIAL7_OFFSET              7
-#define SERINFO_SERIAL7_LENGTH              1
-#define SERINFO_SERIAL7                     (0x1 << SERINFO_SERIAL7_OFFSET)
-/*
-* Like \Fserialzero.
- */
-#define SERINFO_SERIAL6_OFFSET              6
-#define SERINFO_SERIAL6_LENGTH              1
-#define SERINFO_SERIAL6                     (0x1 << SERINFO_SERIAL6_OFFSET)
-/*
-* Like \Fserialzero.
- */
-#define SERINFO_SERIAL5_OFFSET              5
-#define SERINFO_SERIAL5_LENGTH              1
-#define SERINFO_SERIAL5                     (0x1 << SERINFO_SERIAL5_OFFSET)
-/*
-* Like \Fserialzero.
- */
-#define SERINFO_SERIAL4_OFFSET              4
-#define SERINFO_SERIAL4_LENGTH              1
-#define SERINFO_SERIAL4                     (0x1 << SERINFO_SERIAL4_OFFSET)
-/*
-* Like \Fserialzero.
- */
-#define SERINFO_SERIAL3_OFFSET              3
-#define SERINFO_SERIAL3_LENGTH              1
-#define SERINFO_SERIAL3                     (0x1 << SERINFO_SERIAL3_OFFSET)
-/*
-* Like \Fserialzero.
- */
-#define SERINFO_SERIAL2_OFFSET              2
-#define SERINFO_SERIAL2_LENGTH              1
-#define SERINFO_SERIAL2                     (0x1 << SERINFO_SERIAL2_OFFSET)
-/*
-* Like \Fserialzero.
- */
-#define SERINFO_SERIAL1_OFFSET              1
-#define SERINFO_SERIAL1_LENGTH              1
-#define SERINFO_SERIAL1                     (0x1 << SERINFO_SERIAL1_OFFSET)
-/*
-* 1 means serial interface 0 is supported.
- */
-#define SERINFO_SERIAL0_OFFSET              0
-#define SERINFO_SERIAL0_LENGTH              1
-#define SERINFO_SERIAL0                     (0x1 << SERINFO_SERIAL0_OFFSET)
-#define SERSEND0                            0x200
-#define SERRECV0                            0x204
-#define SERSTAT0                            0x208
-/*
-* Send ready. 1 when the core-to-debugger queue is not full. 0
-* otherwise.
- */
-#define SERSTAT0_SENDR_OFFSET               1
-#define SERSTAT0_SENDR_LENGTH               1
-#define SERSTAT0_SENDR                      (0x1 << SERSTAT0_SENDR_OFFSET)
-/*
-* Receive ready. 1 when the debugger-to-core queue is not empty. 0
-* otherwise.
- */
-#define SERSTAT0_RECVR_OFFSET               0
-#define SERSTAT0_RECVR_LENGTH               1
-#define SERSTAT0_RECVR                      (0x1 << SERSTAT0_RECVR_OFFSET)
-#define SERSEND1                            0x210
-#define SERRECV1                            0x214
-#define SERSTAT1                            0x218
-#define SERSEND2                            0x220
-#define SERRECV2                            0x224
-#define SERSTAT2                            0x228
-#define SERSEND3                            0x230
-#define SERRECV3                            0x234
-#define SERSTAT3                            0x238
-#define SERSEND4                            0x240
-#define SERRECV4                            0x244
-#define SERSTAT4                            0x248
-#define SERSEND5                            0x250
-#define SERRECV5                            0x254
-#define SERSTAT5                            0x258
-#define SERSEND6                            0x260
-#define SERRECV6                            0x264
-#define SERSTAT6                            0x268
-#define SERSEND7                            0x274
-#define SERRECV7                            0x278
-#define SERSTAT7                            0x27c
 #define TRACE                               0x728
 /*
 * 1 if the trace buffer has wrapped since the last time \Fdiscard was
@@ -1454,14 +1371,6 @@
 #define AC_ACCESS_REGISTER_SIZE             (0x7 << AC_ACCESS_REGISTER_SIZE_OFFSET)
 /*
 * When 1, execute the program in the Program Buffer exactly once
-* before performing the transfer.
-* \textbf{WARNING: preexec is considered for removal.}
- */
-#define AC_ACCESS_REGISTER_PREEXEC_OFFSET   19
-#define AC_ACCESS_REGISTER_PREEXEC_LENGTH   1
-#define AC_ACCESS_REGISTER_PREEXEC          (0x1 << AC_ACCESS_REGISTER_PREEXEC_OFFSET)
-/*
-* When 1, execute the program in the Program Buffer exactly once
 * after performing the transfer, if any.
  */
 #define AC_ACCESS_REGISTER_POSTEXEC_OFFSET  18
diff --git a/src/target/riscv/gdb_regs.h b/src/target/riscv/gdb_regs.h
index a12c36f4b..e366f7d36 100644
--- a/src/target/riscv/gdb_regs.h
+++ b/src/target/riscv/gdb_regs.h
@@ -3,6 +3,8 @@
 
 enum gdb_regno {
         GDB_REGNO_XPR0 = 0,
+	GDB_REGNO_X0 = GDB_REGNO_XPR0 + 0,
+	GDB_REGNO_ZERO = GDB_REGNO_XPR0 + 0,
 	GDB_REGNO_S0 = GDB_REGNO_XPR0 + 8,
 	GDB_REGNO_S1 = GDB_REGNO_XPR0 + 9,
         GDB_REGNO_XPR31 = GDB_REGNO_XPR0 + 31,
@@ -16,6 +18,7 @@ enum gdb_regno {
         GDB_REGNO_MISA = CSR_MISA + GDB_REGNO_CSR0,
         GDB_REGNO_DPC = CSR_DPC + GDB_REGNO_CSR0,
         GDB_REGNO_DCSR = CSR_DCSR + GDB_REGNO_CSR0,
+	GDB_REGNO_DSCRATCH = CSR_DSCRATCH + GDB_REGNO_CSR0,
         GDB_REGNO_MSTATUS = CSR_MSTATUS + GDB_REGNO_CSR0,
         GDB_REGNO_CSR4095 = GDB_REGNO_CSR0 + 4095,
         GDB_REGNO_PRIV = 4161,
diff --git a/src/target/riscv/opcodes.h b/src/target/riscv/opcodes.h
index f5fac02fb..3e1f8a28e 100644
--- a/src/target/riscv/opcodes.h
+++ b/src/target/riscv/opcodes.h
@@ -125,6 +125,16 @@ static uint32_t csrr(unsigned int rd, unsigned int csr) {
   return (csr << 20) | (rd << 7) | MATCH_CSRRS;
 }
 
+static uint32_t csrrs(unsigned int rd, unsigned int rs, unsigned int csr) __attribute__ ((unused));
+static uint32_t csrrs(unsigned int rd, unsigned int rs, unsigned int csr) {
+  return (csr << 20) | (rs << 15) | (rd << 7) | MATCH_CSRRS;
+}
+
+static uint32_t csrrw(unsigned int rd, unsigned int rs, unsigned int csr) __attribute__ ((unused));
+static uint32_t csrrw(unsigned int rd, unsigned int rs, unsigned int csr) {
+  return (csr << 20) | (rs << 15) | (rd << 7) | MATCH_CSRRW;
+}
+
 static uint32_t fsw(unsigned int src, unsigned int base, uint16_t offset) __attribute__ ((unused));
 static uint32_t fsw(unsigned int src, unsigned int base, uint16_t offset)
 {
@@ -206,7 +216,6 @@ static uint32_t fence_i(void)
   return MATCH_FENCE_I;
 }
 
-/*
 static uint32_t lui(unsigned int dest, uint32_t imm) __attribute__ ((unused));
 static uint32_t lui(unsigned int dest, uint32_t imm)
 {
@@ -215,6 +224,7 @@ static uint32_t lui(unsigned int dest, uint32_t imm)
     MATCH_LUI;
 }
 
+/*
 static uint32_t csrci(unsigned int csr, uint16_t imm) __attribute__ ((unused));
 static uint32_t csrci(unsigned int csr, uint16_t imm) {
   return (csr << 20) |
@@ -277,3 +287,9 @@ static uint32_t fence(void)
 {
 	return MATCH_FENCE;
 }
+
+static uint32_t auipc(unsigned int dest) __attribute__((unused));
+static uint32_t auipc(unsigned int dest)
+{
+	return MATCH_AUIPC | (dest << 7);
+}
diff --git a/src/target/riscv/program.c b/src/target/riscv/program.c
new file mode 100644
index 000000000..182b1e38d
--- /dev/null
+++ b/src/target/riscv/program.c
@@ -0,0 +1,459 @@
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "target/target.h"
+#include "riscv.h"
+#include "program.h"
+#include "helper/log.h"
+
+#include "asm.h"
+#include "encoding.h"
+
+riscv_addr_t riscv_program_gah(struct riscv_program *p, riscv_addr_t addr);
+riscv_addr_t riscv_program_gal(struct riscv_program *p, riscv_addr_t addr);
+int riscv_program_lah(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr);
+int riscv_program_lal(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr);
+
+/* Program interface. */
+int riscv_program_init(struct riscv_program *p, struct target *target)
+{
+	LOG_DEBUG("riscv_program_init: p=0x%016lx", p);
+
+	memset(p, 0, sizeof(*p));
+	p->target = target;
+	p->instruction_count = 0;
+	p->data_count = 0;
+	p->writes_memory = 0;
+	p->target_xlen = riscv_xlen(target);
+	for (size_t i = 0; i < RISCV_REGISTER_COUNT; ++i) {
+		p->writes_xreg[i] = 0;
+		p->in_use[i] = 0;
+	}
+
+	for(size_t i = 0; i < RISCV_MAX_DEBUG_BUFFER_SIZE; ++i)
+		p->debug_buffer[i] = -1;
+
+	if (riscv_debug_buffer_enter(target, p) != ERROR_OK) {
+		LOG_ERROR("unable to write progam buffer enter code");
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
+int riscv_program_exec(struct riscv_program *p, struct target *t)
+{
+	if (riscv_debug_buffer_leave(t, p) != ERROR_OK) {
+		LOG_ERROR("unable to write program buffer exit code");
+		return ERROR_FAIL;
+	}
+
+	riscv_reg_t saved_registers[GDB_REGNO_XPR31 + 1];
+	for (size_t i = GDB_REGNO_XPR0 + 1; i <= GDB_REGNO_XPR31; ++i) {
+		if (p->writes_xreg[i]) {
+			LOG_DEBUG("Saving register %d as used by program", i);
+			saved_registers[i] = riscv_get_register(t, i);
+		}
+	}
+
+	if (p->writes_memory && (riscv_program_fence(p) != ERROR_OK)) {
+		LOG_ERROR("Unable to write fence");
+		for(size_t i = 0; i < riscv_debug_buffer_size(p->target); ++i)
+			LOG_ERROR("ram[%02x]: DASM(0x%08lx) [0x%08lx]", i, p->debug_buffer[i], p->debug_buffer[i]);
+		abort();
+		return ERROR_FAIL;
+	}
+
+	if (riscv_program_ebreak(p) != ERROR_OK) {
+		LOG_ERROR("Unable to write ebreak");
+		for(size_t i = 0; i < riscv_debug_buffer_size(p->target); ++i)
+			LOG_ERROR("ram[%02x]: DASM(0x%08lx) [0x%08lx]", i, p->debug_buffer[i], p->debug_buffer[i]);
+		abort();
+		return ERROR_FAIL;
+	}
+
+	for (size_t i = 0; i < riscv_debug_buffer_size(p->target); ++i) {
+		LOG_DEBUG("Executing program 0x%016lx: debug_buffer[%02x] = DASM(0x%08lx)", p, i, p->debug_buffer[i]);
+		riscv_write_debug_buffer(t, i, p->debug_buffer[i]);
+	}
+
+	riscv_execute_debug_buffer(t);
+
+	for (size_t i = 0; i < riscv_debug_buffer_size(p->target); ++i)
+		p->debug_buffer[i] = riscv_read_debug_buffer(t, i);
+
+	for (size_t i = GDB_REGNO_XPR0; i <= GDB_REGNO_XPR31; ++i)
+		if (p->writes_xreg[i])
+			riscv_set_register(t, i, saved_registers[i]);
+
+	return ERROR_OK;
+}
+
+riscv_addr_t riscv_program_alloc_data(struct riscv_program *p, size_t bytes)
+{
+	LOG_DEBUG("allocating %d bytes of data", bytes);
+
+	riscv_addr_t addr = 
+		riscv_debug_buffer_addr(p->target) 
+		+ riscv_debug_buffer_size(p->target) * sizeof(p->debug_buffer[0])
+		- p->data_count * sizeof(p->debug_buffer[0])
+		- bytes;
+	while (addr % bytes != 0) addr--;
+
+	riscv_addr_t ptop =
+		riscv_debug_buffer_addr(p->target)
+		+ p->instruction_count * sizeof(p->debug_buffer[0]);
+
+	if (addr <= ptop) {
+		LOG_DEBUG("unable to allocate %d bytes", bytes);
+		return RISCV_PROGRAM_ALLOC_FAIL;
+	}
+
+	LOG_DEBUG("allocated %d bytes at 0x%08lx", bytes, addr);
+	p->data_count = 
+		+ riscv_debug_buffer_size(p->target)
+		- (addr - riscv_debug_buffer_addr(p->target)) / sizeof(p->debug_buffer[0]);
+	return addr;
+}
+
+riscv_addr_t riscv_program_alloc_x(struct riscv_program *p)
+{
+	return riscv_program_alloc_data(p, p->target_xlen / 8);
+}
+
+riscv_addr_t riscv_program_alloc_d(struct riscv_program *p)
+{
+	return riscv_program_alloc_data(p, 8);
+}
+
+riscv_addr_t riscv_program_alloc_w(struct riscv_program *p)
+{
+	return riscv_program_alloc_data(p, 4);
+}
+
+riscv_addr_t riscv_program_alloc_h(struct riscv_program *p)
+{
+	return riscv_program_alloc_data(p, 2);
+}
+
+riscv_addr_t riscv_program_alloc_b(struct riscv_program *p)
+{
+	return riscv_program_alloc_data(p, 1);
+}
+
+riscv_insn_t riscv_program_read_ram(struct riscv_program *p, riscv_addr_t addr)
+{
+	if (addr < riscv_debug_buffer_addr(p->target))
+		return -1;
+	if (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 (addr > riscv_debug_buffer_addr(p->target) + (riscv_debug_buffer_size(p->target) * sizeof(p->debug_buffer[0])))
+		return;
+
+	int off = (addr - riscv_debug_buffer_addr(p->target)) / sizeof(p->debug_buffer[0]);
+	p->debug_buffer[off] = d;
+}
+
+int riscv_program_swr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno b, int offset)
+{
+	p->writes_memory = 1;
+	return riscv_program_insert(p, sw(d, b, offset));
+}
+
+int riscv_program_shr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno b, int offset)
+{
+	p->writes_memory = 1;
+	return riscv_program_insert(p, sh(d, b, offset));
+}
+
+int riscv_program_sbr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno b, int offset)
+{
+	p->writes_memory = 1;
+	return riscv_program_insert(p, sb(d, b, offset));
+}
+
+int riscv_program_lwr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno b, int offset)
+{
+	p->writes_memory = 1;
+	return riscv_program_insert(p, lw(d, b, offset));
+}
+
+int riscv_program_lhr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno b, int offset)
+{
+	p->writes_memory = 1;
+	return riscv_program_insert(p, lh(d, b, offset));
+}
+
+int riscv_program_lbr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno b, int offset)
+{
+	p->writes_memory = 1;
+	return riscv_program_insert(p, lb(d, b, offset));
+}
+
+int riscv_program_lx(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
+{
+	switch (p->target_xlen) {
+	case 64:  return riscv_program_ld(p, d, addr);
+	case 32:  return riscv_program_lw(p, d, addr);
+	}
+
+	LOG_ERROR("unknown xlen %d", p->target_xlen);
+	abort();
+	return -1;
+}
+
+int riscv_program_ld(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
+{
+	enum gdb_regno t = riscv_program_gah(p, addr) == 0 ? GDB_REGNO_X0 : d;
+	if (riscv_program_lah(p, d, addr) != ERROR_OK)
+		return ERROR_FAIL;
+	if (riscv_program_insert(p, ld(d, t, riscv_program_gal(p, addr))) != ERROR_OK)
+		return ERROR_FAIL;
+	return ERROR_OK;
+}
+
+int riscv_program_lw(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
+{
+	enum gdb_regno t = riscv_program_gah(p, addr) == 0 ? GDB_REGNO_X0 : d;
+	if (riscv_program_lah(p, d, addr) != ERROR_OK)
+		return ERROR_FAIL;
+	if (riscv_program_insert(p, lw(d, t, riscv_program_gal(p, addr))) != ERROR_OK)
+		return ERROR_FAIL;
+	return ERROR_OK;
+}
+
+int riscv_program_lh(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
+{
+	enum gdb_regno t = riscv_program_gah(p, addr) == 0 ? GDB_REGNO_X0 : d;
+	if (riscv_program_lah(p, d, addr) != ERROR_OK)
+		return ERROR_FAIL;
+	if (riscv_program_insert(p, lh(d, t, riscv_program_gal(p, addr))) != ERROR_OK)
+		return ERROR_FAIL;
+	return ERROR_OK;
+}
+
+int riscv_program_lb(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
+{
+	enum gdb_regno t = riscv_program_gah(p, addr) == 0 ? GDB_REGNO_X0 : d;
+	if (riscv_program_lah(p, t, addr) != ERROR_OK)
+		return ERROR_FAIL;
+	if (riscv_program_insert(p, lb(d, t, riscv_program_gal(p, addr))) != ERROR_OK)
+		return ERROR_FAIL;
+	return ERROR_OK;
+}
+
+int riscv_program_sx(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
+{
+	switch (p->target_xlen) {
+	case 64:  return riscv_program_sd(p, d, addr);
+	case 32:  return riscv_program_sw(p, d, addr);
+	}
+
+	LOG_ERROR("unknown xlen %d", p->target_xlen);
+	abort();
+	return -1;
+}
+
+int riscv_program_sd(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
+{
+	enum gdb_regno t = riscv_program_gah(p, addr) == 0
+		? GDB_REGNO_X0
+		: riscv_program_gettemp(p);
+	if (riscv_program_lah(p, t, addr) != ERROR_OK)
+		return ERROR_FAIL;
+	if (riscv_program_insert(p, sd(d, t, riscv_program_gal(p, addr))) != ERROR_OK)
+		return ERROR_FAIL;
+	riscv_program_puttemp(p, t);
+	p->writes_memory = true;
+	return ERROR_OK;
+}
+
+int riscv_program_sw(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
+{
+	enum gdb_regno t = riscv_program_gah(p, addr) == 0
+		? GDB_REGNO_X0
+		: riscv_program_gettemp(p);
+	if (riscv_program_lah(p, t, addr) != ERROR_OK)
+		return ERROR_FAIL;
+	if (riscv_program_insert(p, sw(d, t, riscv_program_gal(p, addr))) != ERROR_OK)
+		return ERROR_FAIL;
+	riscv_program_puttemp(p, t);
+	p->writes_memory = true;
+	return ERROR_OK;
+}
+
+int riscv_program_sh(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
+{
+	enum gdb_regno t = riscv_program_gah(p, addr) == 0
+		? GDB_REGNO_X0
+		: riscv_program_gettemp(p);
+	if (riscv_program_lah(p, t, addr) != ERROR_OK)
+		return ERROR_FAIL;
+	if (riscv_program_insert(p, sh(d, t, riscv_program_gal(p, addr))) != ERROR_OK)
+		return ERROR_FAIL;
+	riscv_program_puttemp(p, t);
+	p->writes_memory = true;
+	return ERROR_OK;
+}
+
+int riscv_program_sb(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
+{
+	enum gdb_regno t = riscv_program_gah(p, addr) == 0
+		? GDB_REGNO_X0
+		: riscv_program_gettemp(p);
+	if (riscv_program_lah(p, t, addr) != ERROR_OK)
+		return ERROR_FAIL;
+	if (riscv_program_insert(p, sb(d, t, riscv_program_gal(p, addr))) != ERROR_OK)
+		return ERROR_FAIL;
+	riscv_program_puttemp(p, t);
+	p->writes_memory = true;
+	return ERROR_OK;
+}
+
+int riscv_program_csrr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno csr)
+{
+	assert(csr >= GDB_REGNO_CSR0);
+	return riscv_program_insert(p, csrrs(d, GDB_REGNO_X0, csr - GDB_REGNO_CSR0));
+}
+
+int riscv_program_csrw(struct riscv_program *p, enum gdb_regno s, enum gdb_regno csr)
+{
+	assert(csr >= GDB_REGNO_CSR0);
+	return riscv_program_insert(p, csrrw(GDB_REGNO_X0, s, csr - GDB_REGNO_CSR0));
+}
+
+int riscv_program_csrrw(struct riscv_program *p, enum gdb_regno d, enum gdb_regno s, enum gdb_regno csr)
+{
+	assert(csr >= GDB_REGNO_CSR0);
+	return riscv_program_insert(p, csrrw(d, s, csr - GDB_REGNO_CSR0));
+}
+
+int riscv_program_fence_i(struct riscv_program *p)
+{
+	return riscv_program_insert(p, fence_i());
+}
+
+int riscv_program_fence(struct riscv_program *p)
+{
+	return riscv_program_insert(p, fence());
+}
+
+int riscv_program_ebreak(struct riscv_program *p)
+{
+	return riscv_program_insert(p, ebreak());
+}
+
+int riscv_program_lui(struct riscv_program *p, enum gdb_regno d, int32_t u)
+{
+	return riscv_program_insert(p, lui(d, u));
+}
+
+int riscv_program_addi(struct riscv_program *p, enum gdb_regno d, enum gdb_regno s, int16_t u)
+{
+	return riscv_program_insert(p, addi(d, s, u));
+}
+
+int riscv_program_li(struct riscv_program *p, enum gdb_regno d, riscv_reg_t c)
+{
+	if (riscv_program_lui(p, d, c >> 12) != ERROR_OK)
+		return ERROR_FAIL;
+	if (riscv_program_addi(p, d, d, c & 0xFFF) != ERROR_OK)
+		return ERROR_FAIL;
+	return ERROR_OK;
+}
+
+int riscv_program_dont_restore_register(struct riscv_program *p, enum gdb_regno r)
+{
+	assert(r < RISCV_REGISTER_COUNT);
+	p->writes_xreg[r] = 0;
+	return ERROR_OK;
+}
+
+int riscv_program_do_restore_register(struct riscv_program *p, enum gdb_regno r)
+{
+	assert(r < RISCV_REGISTER_COUNT);
+	p->writes_xreg[r] = 1;
+	return ERROR_OK;
+}
+
+void riscv_program_reserve_register(struct riscv_program *p, enum gdb_regno r)
+{
+	assert(r < RISCV_REGISTER_COUNT);
+	assert(p->in_use[r] == 0);
+	p->in_use[r] = 1;
+}
+
+enum gdb_regno riscv_program_gettemp(struct riscv_program *p)
+{
+	for (size_t i = GDB_REGNO_S0; i <= GDB_REGNO_XPR31; ++i) {
+		if (p->in_use[i]) continue;
+
+		riscv_program_do_restore_register(p, i);
+		p->in_use[i] = 1;
+		return i;
+	}
+
+	LOG_ERROR("You've run out of temporary registers.  This is impossible.");
+	abort();
+}
+
+void riscv_program_puttemp(struct riscv_program *p, enum gdb_regno r)
+{
+	assert(r < RISCV_REGISTER_COUNT);
+	p->in_use[r] = 0;
+}
+
+/* Helper functions. */
+riscv_addr_t riscv_program_gah(struct riscv_program *p, riscv_addr_t addr)
+{
+	return addr >> 12;
+}
+
+riscv_addr_t riscv_program_gal(struct riscv_program *p, riscv_addr_t addr)
+{
+	return ((addr > 0) ? 1 : 0) * (abs(addr) & 0x7FF);
+}
+
+int riscv_program_lah(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
+{
+	riscv_addr_t ah = riscv_program_gah(p, addr);
+	if (ah == 0)
+		return ERROR_OK;
+	return riscv_program_lui(p, d, ah);
+}
+
+int riscv_program_lal(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr)
+{
+	riscv_addr_t al = riscv_program_gal(p, addr);
+	if (al == 0)
+		return ERROR_OK;
+	return riscv_program_addi(p, d, d, al);
+}
+
+int riscv_program_insert(struct riscv_program *p, riscv_insn_t i)
+{
+	LOG_DEBUG("instruction_count: %d (p=0x%016lx)", p->instruction_count, p);
+
+	if (p->instruction_count + p->data_count + 1 > riscv_debug_buffer_size(p->target)) {
+		LOG_DEBUG("Unable to insert instruction:");
+		LOG_DEBUG("  instruction_count=%d", p->instruction_count);
+		LOG_DEBUG("  data_count       =%d", p->data_count);
+		LOG_DEBUG("  buffer size      =%d", riscv_debug_buffer_size(p->target));
+		return ERROR_FAIL;
+	}
+
+	LOG_DEBUG("PROGBUF[%d] = DASM(0x%08x) [0x%08x]", p->instruction_count, i, i);
+	p->debug_buffer[p->instruction_count] = i;
+	p->instruction_count++;
+	return ERROR_OK;
+}
diff --git a/src/target/riscv/program.h b/src/target/riscv/program.h
new file mode 100644
index 000000000..5d51714ca
--- /dev/null
+++ b/src/target/riscv/program.h
@@ -0,0 +1,135 @@
+#ifndef TARGET__RISCV__PROGRAM_H
+#define TARGET__RISCV__PROGRAM_H
+
+#include "riscv.h"
+
+#define RISCV_MAX_DEBUG_BUFFER_SIZE 32
+#define RISCV_REGISTER_COUNT 32
+#define RISCV_DSCRATCH_COUNT 2
+
+/* The various RISC-V debug specifications all revolve around setting up
+ * program buffers and executing them on the target.  This structure contains a
+ * single program, which can then be executed on targets.  */
+struct riscv_program {
+	struct target *target;
+
+	uint32_t debug_buffer[RISCV_MAX_DEBUG_BUFFER_SIZE];
+
+	/* The debug buffer is allocated in two directions: instructions go at
+	 * the start, while data goes at the end.  When they meet in the middle
+	 * this blows up. */
+	size_t instruction_count;
+	size_t data_count;
+
+	/* Side effects of executing this program.  These must be accounted for
+	 * in order to maintain correct executing of the target system.  */
+	bool writes_xreg[RISCV_REGISTER_COUNT];
+	bool writes_memory;
+
+	/* When a register is used it will be set in this array. */
+	bool in_use[RISCV_REGISTER_COUNT];
+
+	/* Remembers the registers that have been saved into dscratch
+	 * registers.  These are restored */
+	enum gdb_regno dscratch_saved[RISCV_DSCRATCH_COUNT];
+
+	/* XLEN on the target. */
+	int target_xlen;
+};
+
+/* Initializes a program with the header. */
+int riscv_program_init(struct riscv_program *p, struct target *t);
+
+/* Executes a program, returning 0 if the program successfully executed.  Note
+ * that this may cause registers to be saved or restored, which could result to
+ * calls to things like riscv_save_register which itself could require a
+ * program to execute.  That's OK, just make sure this eventually terminates.
+ * */
+int riscv_program_exec(struct riscv_program *p, struct target *t);
+
+/* Clears a program, removing all the state associated with it. */
+int riscv_program_clear(struct riscv_program *p, struct target *t);
+
+/* A lower level interface, you shouldn't use this unless you have a reason. */
+int riscv_program_insert(struct riscv_program *p, riscv_insn_t i);
+
+/* There is hardware support for saving at least one register.  This register
+ * doesn't need to be saved/restored the usual way, which is useful during
+ * early initialization when we can't save/restore arbitrary registerrs to host
+ * memory. */
+int riscv_program_save_to_dscratch(struct riscv_program *p, enum gdb_regno to_save);
+
+/* Allocates data of various sizes.  Either returns the absolute physical
+ * address or RISCV_PROGRAM_ALLOC_FAIL on failure. */
+riscv_addr_t riscv_program_alloc_data(struct riscv_program *p, size_t bytes);
+riscv_addr_t riscv_program_alloc_x(struct riscv_program *p);
+riscv_addr_t riscv_program_alloc_d(struct riscv_program *p);
+riscv_addr_t riscv_program_alloc_w(struct riscv_program *p);
+riscv_addr_t riscv_program_alloc_h(struct riscv_program *p);
+riscv_addr_t riscv_program_alloc_b(struct riscv_program *p);
+#define RISCV_PROGRAM_ALLOC_FAIL ((riscv_addr_t)(-1))
+
+/* Reads a word of memory from this program's internal view of the debug RAM.
+ * This is what you want to use to get data back from the program after it
+ * executes. */
+riscv_insn_t riscv_program_read_ram(struct riscv_program *p, riscv_addr_t addr);
+void riscv_program_write_ram(struct riscv_program *p, riscv_addr_t a, uint64_t d);
+
+/* Helpers to assembly various instructions.  Return 0 on success.  These might
+ * assembly into a multi-instruction sequence that overwrites some other
+ * register, but those will be properly saved and restored. */
+int riscv_program_lx(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr);
+int riscv_program_ld(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr);
+int riscv_program_lw(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr);
+int riscv_program_lh(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr);
+int riscv_program_lb(struct riscv_program *p, enum gdb_regno d, riscv_addr_t addr);
+
+int riscv_program_sx(struct riscv_program *p, enum gdb_regno s, riscv_addr_t addr);
+int riscv_program_sd(struct riscv_program *p, enum gdb_regno s, riscv_addr_t addr);
+int riscv_program_sw(struct riscv_program *p, enum gdb_regno s, riscv_addr_t addr);
+int riscv_program_sh(struct riscv_program *p, enum gdb_regno s, riscv_addr_t addr);
+int riscv_program_sb(struct riscv_program *p, enum gdb_regno s, riscv_addr_t addr);
+
+int riscv_program_lxr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno a, int o);
+int riscv_program_ldr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno a, int o);
+int riscv_program_lwr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno a, int o);
+int riscv_program_lhr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno a, int o);
+int riscv_program_lbr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno a, int o);
+
+int riscv_program_sxr(struct riscv_program *p, enum gdb_regno s, enum gdb_regno a, int o);
+int riscv_program_sdr(struct riscv_program *p, enum gdb_regno s, enum gdb_regno a, int o);
+int riscv_program_swr(struct riscv_program *p, enum gdb_regno s, enum gdb_regno a, int o);
+int riscv_program_shr(struct riscv_program *p, enum gdb_regno s, enum gdb_regno a, int o);
+int riscv_program_sbr(struct riscv_program *p, enum gdb_regno s, enum gdb_regno a, int o);
+
+int riscv_program_csrr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno csr);
+int riscv_program_csrw(struct riscv_program *p, enum gdb_regno s, enum gdb_regno csr);
+int riscv_program_csrrw(struct riscv_program *p, enum gdb_regno d, enum gdb_regno s, enum gdb_regno csr);
+
+int riscv_program_fence_i(struct riscv_program *p);
+int riscv_program_fence(struct riscv_program *p);
+int riscv_program_ebreak(struct riscv_program *p);
+
+int riscv_program_lui(struct riscv_program *p, enum gdb_regno d, int32_t u);
+int riscv_program_addi(struct riscv_program *p, enum gdb_regno d, enum gdb_regno s, int16_t i);
+
+/* 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);
+
+#endif
diff --git a/src/target/riscv/riscv-013.c b/src/target/riscv/riscv-013.c
index 6a1da10fa..6508629b6 100644
--- a/src/target/riscv/riscv-013.c
+++ b/src/target/riscv/riscv-013.c
@@ -22,9 +22,16 @@
 #include "riscv.h"
 #include "debug_defines.h"
 #include "rtos/rtos.h"
+#include "program.h"
+#include "asm.h"
+
+#define DMI_DATA1 (DMI_DATA0 + 1)
 
 static void riscv013_on_step_or_resume(struct target *target, bool step);
 static void riscv013_step_or_resume_current_hart(struct target *target, bool step);
+static size_t riscv013_progbuf_size(struct target *target);
+static size_t riscv013_data_size(struct target *target);
+static size_t riscv013_data_addr(struct target *target);
 
 /* Implementations of the functions in riscv_info_t. */
 static riscv_reg_t riscv013_get_register(struct target *target, int hartid, int regid);
@@ -38,6 +45,11 @@ static void riscv013_on_step(struct target *target);
 static void riscv013_on_resume(struct target *target);
 static bool riscv013_is_halted(struct target *target);
 static enum riscv_halt_reason riscv013_halt_reason(struct target *target);
+static void riscv013_debug_buffer_enter(struct target *target, struct riscv_program *p);
+static void riscv013_debug_buffer_leave(struct target *target, struct riscv_program *p);
+static void riscv013_write_debug_buffer(struct target *target, int i, riscv_insn_t d);
+static riscv_insn_t riscv013_read_debug_buffer(struct target *target, int i);
+static void riscv013_execute_debug_buffer(struct target *target);
 
 /**
  * Since almost everything can be accomplish by scanning the dbus register, all
@@ -57,6 +69,8 @@ static enum riscv_halt_reason riscv013_halt_reason(struct target *target);
 #define CSR_DCSR_CAUSE_STEP		4
 #define CSR_DCSR_CAUSE_HALT		5
 
+#define RISCV013_INFO(r) riscv013_info_t *r = get_info(target)
+
 /*** JTAG registers. ***/
 
 typedef enum {
@@ -156,6 +170,9 @@ typedef struct {
 	unsigned int ac_busy_delay;
 
 	bool need_strict_step;
+
+	// Some memoized values
+	int progbuf_size, progbuf_addr, data_addr, data_size;
 } riscv013_info_t;
 
 static void dump_field(const struct scan_field *field)
@@ -470,98 +487,6 @@ static int execute_abstract_command(struct target *target, uint32_t command)
 	return ERROR_OK;
 }
 
-/*** program "class" ***/
-/* This class allows a debug program to be built up piecemeal, and then be
- * executed. If necessary, the program is split up to fit in the program
- * buffer. */
-
-typedef struct {
-	uint8_t code[12 * 4];
-	unsigned length;
-	bool write;
-	unsigned regno;
-	uint64_t write_value;
-} program_t;
-
-static void program_add32(program_t *program, uint32_t instruction);
-
-static program_t *program_new(void)
-{
-	program_t *program = malloc(sizeof(program_t));
-	if (program) {
-		program->length = 0;
-		// Default to read zero.
-		program->write = false;
-		program->regno = 0x1000;
-	}
-	program_add32(program, fence_i());
-	return program;
-}
-
-static void program_delete(program_t *program)
-{
-	free(program);
-}
-
-static void program_add32(program_t *program, uint32_t instruction)
-{
-	assert(program->length + 4 < sizeof(program->code));
-	program->code[program->length++] = instruction & 0xff;
-	program->code[program->length++] = (instruction >> 8) & 0xff;
-	program->code[program->length++] = (instruction >> 16) & 0xff;
-	program->code[program->length++] = (instruction >> 24) & 0xff;
-}
-
-static void program_set_read(program_t *program, unsigned reg_num)
-{
-	program->write = false;
-	program->regno = reg_number_to_no(reg_num);
-}
-
-static void program_set_write(program_t *program, unsigned reg_num, uint64_t value)
-{
-	program->write = true;
-	program->regno = reg_number_to_no(reg_num);
-	program->write_value = value;
-}
-
-/*** end of program class ***/
-
-static void write_program(struct target *target, const program_t *program)
-{
-	riscv013_info_t *info = get_info(target);
-
-	assert(program->length <= info->progsize * 4);
-	for (unsigned i = 0; i < program->length; i += 4) {
-		uint32_t value =
-			program->code[i] |
-			((uint32_t) program->code[i+1] << 8) |
-			((uint32_t) program->code[i+2] << 16) |
-			((uint32_t) program->code[i+3] << 24);
-		dmi_write(target, DMI_PROGBUF0 + i / 4, value);
-	}
-}
-
-static int execute_program(struct target *target, const program_t *program)
-{
-	write_program(target, program);
-
-	uint32_t command = 0;
-	if (program->write) {
-		if (get_field(command, AC_ACCESS_REGISTER_SIZE) > 2) {
-			dmi_write(target, DMI_DATA1, program->write_value >> 32);
-		}
-		dmi_write(target, DMI_DATA0, program->write_value);
-		command |= AC_ACCESS_REGISTER_WRITE | AC_ACCESS_REGISTER_POSTEXEC;
-	} else {
-		command |= AC_ACCESS_REGISTER_PREEXEC;
-	}
-	command |= abstract_register_size(riscv_xlen(target));
-	command |= program->regno;
-
-	return execute_abstract_command(target, command);
-}
-
 static int abstract_read_register(struct target *target,
 		uint64_t *value,
 		uint32_t reg_number, 
@@ -570,6 +495,7 @@ static int abstract_read_register(struct target *target,
 	uint32_t command = abstract_register_size(width);
 
 	command |= reg_number_to_no(reg_number);
+	command |= AC_ACCESS_REGISTER_TRANSFER;
 
 	int result = execute_abstract_command(target, command);
 	if (result != ERROR_OK) {
@@ -601,6 +527,7 @@ static int abstract_write_register(struct target *target,
 
 	command |= reg_number_to_no(reg_number);
 	command |= AC_ACCESS_REGISTER_WRITE;
+	command |= AC_ACCESS_REGISTER_TRANSFER;
 
 	switch (width) {
 		case 128:
@@ -635,110 +562,92 @@ static int update_mstatus_actual(struct target *target)
 	return register_get(&target->reg_cache->reg_list[GDB_REGNO_MSTATUS]);
 }
 
+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)
 {
-	riscv013_info_t *info = get_info(target);
-	LOG_DEBUG("register 0x%x <- 0x%" PRIx64, number, value);
+	struct riscv_program program;
+	riscv_program_init(&program, target);
 
-	if (number == GDB_REGNO_MSTATUS) {
-		info->mstatus_actual = value;
+	riscv_addr_t input = riscv_program_alloc_x(&program);
+	switch (riscv_xlen(target)) {
+	case 64:
+		riscv_program_write_ram(&program, input + 4, value >> 32);
+	case 32:
+		riscv_program_write_ram(&program, input, value);
+		break;
+	default:
+		LOG_ERROR("Unknown XLEN: %d\n", riscv_xlen(target));
+		abort();
 	}
 
-	int result = abstract_write_register(target, number, riscv_xlen(target), value);
-	if (result == ERROR_OK)
-		return result;
-
-	// Fall back to program buffer.
-	if (number >= GDB_REGNO_FPR0 && number <= GDB_REGNO_FPR31) {
-		result = update_mstatus_actual(target);
-		if (result != ERROR_OK) {
-			return result;
-		}
-		if ((info->mstatus_actual & MSTATUS_FS) == 0) {
-			result = register_write_direct(target, GDB_REGNO_MSTATUS, 
-					set_field(info->mstatus_actual, MSTATUS_FS, 1));
-			if (result != ERROR_OK)
-				return result;
-		}
-
-		program_t *program = program_new();
-		// TODO: Fully support D extension on RV32.
-		if (supports_extension(target, 'D') && riscv_xlen(target) >= 64) {
-			program_add32(program, fmv_d_x(number - GDB_REGNO_FPR0, S0));
-		} else {
-			program_add32(program, fmv_s_x(number - GDB_REGNO_FPR0, S0));
-		}
-		program_add32(program, ebreak());
-		program_set_write(program, S0, value);
-		result = execute_program(target, program);
-		program_delete(program);
+	if (number >= GDB_REGNO_XPR0 && number <= GDB_REGNO_XPR31) {
+		riscv_program_lx(&program, number, input);
+	} else if (number >= GDB_REGNO_FPR0 && number <= GDB_REGNO_FPR31) {
+		LOG_ERROR("FIXME: I don't support floating-point");
+		abort();
 	} else if (number >= GDB_REGNO_CSR0 && number <= GDB_REGNO_CSR4095) {
-		program_t *program = program_new();
-		program_add32(program, csrw(S0, number - GDB_REGNO_CSR0));
-		program_add32(program, ebreak());
-		program_set_write(program, S0, value);
-		result = execute_program(target, program);
-		program_delete(program);
+		enum gdb_regno temp = riscv_program_gettemp(&program);
+		riscv_program_lx(&program, temp, input);
+		riscv_program_csrw(&program, temp, number);
 	} else {
-		return result;
+		LOG_ERROR("Unsupported register (enum gdb_regno)(%d)", number);
+		abort();
 	}
 
-	return result;
+	int exec_out = riscv_program_exec(&program, target);
+	if (exec_out != ERROR_OK) {
+		LOG_ERROR("Unable to execute program");
+		return exec_out;
+	}
+
+	if (number >= GDB_REGNO_XPR0 && number <= GDB_REGNO_XPR31) {
+		uint64_t written_value;
+		register_read_direct(target, &written_value, number);
+		LOG_DEBUG("attempted to write 0x%016lx, actually wrote 0x%016lx", value, written_value);
+		assert(value == written_value);
+	}
+
+	return ERROR_OK;
 }
 
 /** Actually read registers from the target right now. */
 static int register_read_direct(struct target *target, uint64_t *value, uint32_t number)
 {
-	riscv013_info_t *info = get_info(target);
-	int result = abstract_read_register(target, value, number, riscv_xlen(target));
-	if (result == ERROR_OK)
-		return result;
+	struct riscv_program program;
+	riscv_program_init(&program, target);
+	riscv_addr_t output = riscv_program_alloc_x(&program);
 
-	// Fall back to program buffer.
-	if (number >= GDB_REGNO_FPR0 && number <= GDB_REGNO_FPR31) {
-		result = update_mstatus_actual(target);
-		if (result != ERROR_OK) {
-			return result;
-		}
-		if ((info->mstatus_actual & MSTATUS_FS) == 0) {
-			result = register_write_direct(target, GDB_REGNO_MSTATUS,
-					set_field(info->mstatus_actual, MSTATUS_FS, 1));
-			if (result != ERROR_OK)
-				return result;
-		}
-		LOG_DEBUG("mstatus_actual=0x%lx", info->mstatus_actual);
-
-		program_t *program = program_new();
-		if (supports_extension(target, 'D') && riscv_xlen(target) >= 64) {
-			program_add32(program, fmv_x_d(S0, number - GDB_REGNO_FPR0));
-		} else {
-			program_add32(program, fmv_x_s(S0, number - GDB_REGNO_FPR0));
-		}
-		program_add32(program, ebreak());
-		program_set_read(program, S0);
-		result = execute_program(target, program);
-		program_delete(program);
+	if (number >= GDB_REGNO_XPR0 && number <= GDB_REGNO_XPR31) {
+		riscv_program_sx(&program, number, output);
+	} else if (number >= GDB_REGNO_FPR0 && number <= GDB_REGNO_FPR31) {
+		LOG_ERROR("FIXME: I don't support floating-point");
+		abort();
 	} else if (number >= GDB_REGNO_CSR0 && number <= GDB_REGNO_CSR4095) {
-		program_t *program = program_new();
-		program_add32(program, csrr(S0, number - GDB_REGNO_CSR0));
-		program_add32(program, ebreak());
-		program_set_read(program, S0);
-		result = execute_program(target, program);
-		program_delete(program);
+		enum gdb_regno temp = riscv_program_gettemp(&program);
+		riscv_program_csrr(&program, temp, number);
+		riscv_program_sx(&program, temp, output);
 	} else {
-		return result;
+		LOG_ERROR("Unsupported register (enum gdb_regno)(%d)", number);
+		abort();
 	}
 
-	if (result != ERROR_OK)
-		return result;
+	int exec_out = riscv_program_exec(&program, target);
+	if (exec_out != ERROR_OK) {
+		LOG_ERROR("Unable to execute program");
+		return exec_out;
+	}
 
-	result = register_read_direct(target, value, S0);
-	if (result != ERROR_OK)
-		return result;
+	*value = 0;
+	switch (riscv_xlen(target)) {
+	case 64:
+		*value |= ((uint64_t)(riscv_program_read_ram(&program, output + 4))) << 32;
+	case 32:
+		*value |= riscv_program_read_ram(&program, output);
+	}
 
 	LOG_DEBUG("register 0x%x = 0x%" PRIx64, number, *value);
-
 	return ERROR_OK;
 }
 
@@ -776,54 +685,15 @@ static int register_get(struct reg *reg)
 {
 	struct target *target = (struct target *) reg->arch_info;
 	riscv013_info_t *info = get_info(target);
-
-	maybe_write_tselect(target);
-
-	if (reg->number <= GDB_REGNO_XPR31) {
-		register_read_direct(target, reg->value, reg->number);
-		return ERROR_OK;
-	} else if (reg->number == GDB_REGNO_PC) {
-		buf_set_u32(reg->value, 0, 32, riscv_peek_register(target, GDB_REGNO_DPC));
-		reg->valid = true;
-		return ERROR_OK;
-	} else if (reg->number == GDB_REGNO_PRIV) {
-		uint64_t dcsr = riscv_peek_register(target, CSR_DCSR);
-		buf_set_u64(reg->value, 0, 8, get_field(dcsr, CSR_DCSR_PRV));
-		riscv_overwrite_register(target, CSR_DCSR, dcsr);
-		return ERROR_OK;
-	} else {
-		uint64_t value;
-		int result = register_read_direct(target, &value, reg->number);
-		if (result != ERROR_OK) {
-			return result;
-		}
-		LOG_DEBUG("%s=0x%" PRIx64, reg->name, value);
-		buf_set_u64(reg->value, 0, riscv_xlen(target), value);
-
-		if (reg->number == GDB_REGNO_MSTATUS) {
-			info->mstatus_actual = value;
-			reg->valid = true;
-		}
-	}
-
+	uint64_t value = riscv_get_register(target, reg->number);
+	buf_set_u64(reg->value, 0, 64, value);
 	return ERROR_OK;
 }
 
 static int register_write(struct target *target, unsigned int number,
 		uint64_t value)
 {
-	maybe_write_tselect(target);
-
-	if (number == GDB_REGNO_PC) {
-		riscv_overwrite_register(target, GDB_REGNO_DPC, value);
-	} else if (number == GDB_REGNO_PRIV) {
-		uint64_t dcsr = riscv_peek_register(target, CSR_DCSR);
-		dcsr = set_field(dcsr, CSR_DCSR_PRV, value);
-		riscv_overwrite_register(target, GDB_REGNO_DCSR, dcsr);
-	} else {
-		return register_write_direct(target, number, value);
-	}
-
+	riscv_set_register(target, number, value);
 	return ERROR_OK;
 }
 
@@ -864,12 +734,22 @@ 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;
 
 	generic_info->version_specific = calloc(1, sizeof(riscv013_info_t));
 	if (!generic_info->version_specific)
 		return ERROR_FAIL;
 	riscv013_info_t *info = get_info(target);
 
+	info->progbuf_size = -1;
+	info->progbuf_addr = -1;
+	info->data_size = -1;
+	info->data_addr = -1;
+
 	target->reg_cache = calloc(1, sizeof(*target->reg_cache));
 	target->reg_cache->name = "RISC-V registers";
 	target->reg_cache->num_regs = GDB_REGNO_COUNT;
@@ -1234,19 +1114,66 @@ static int examine(struct target *target)
 	/* Halt every hart so we can probe them. */
 	riscv_halt_all_harts(target);
 
-	/* Examines every hart, first checking XLEN. */
+	/* Find the address of the program buffer, which must be done without
+	 * knowing anything about the target. */
 	for (int i = 0; i < riscv_count_harts(target); ++i) {
 		riscv_set_current_hartid(target, i);
 
-		if (abstract_read_register(target, NULL, S0, 128) == ERROR_OK) {
-			r->xlen[i] = 128;
-		} else if (abstract_read_register(target, NULL, S0, 64) == ERROR_OK) {
+		/* 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;
+
+		struct riscv_program program64;
+		riscv_program_init(&program64, target);
+		riscv_program_csrrw(&program64, GDB_REGNO_S0, GDB_REGNO_S0, GDB_REGNO_DSCRATCH);
+		riscv_program_insert(&program64, auipc(GDB_REGNO_S0));
+		riscv_program_insert(&program64, sd(GDB_REGNO_S0, GDB_REGNO_S0, offset));
+		riscv_program_csrrw(&program64, GDB_REGNO_S0, GDB_REGNO_S0, GDB_REGNO_DSCRATCH);
+		riscv_program_fence(&program64);
+		riscv_program_exec(&program64, target);
+
+		if (get_field(dmi_read(target, DMI_ABSTRACTCS), DMI_ABSTRACTCS_CMDERR) == 0) {
+			r->debug_buffer_addr[i] =
+				(dmi_read(target, DMI_PROGBUF0 + (8 + offset) / 4) << 32)
+				+ dmi_read(target, DMI_PROGBUF0 + (4 + offset) / 4)
+				- 4;
 			r->xlen[i] = 64;
-		} else if (abstract_read_register(target, NULL, S0, 32) == ERROR_OK) {
-			r->xlen[i] = 32;
-		} else {
-			LOG_ERROR("Failed to discover size using abstract register reads.");
-			return ERROR_FAIL;
+		}
+
+		LOG_DEBUG("hart %d has XLEN=%d", i, r->xlen[i]);
+		LOG_DEBUG("found program buffer at 0x%08lx", (long)(r->debug_buffer_addr[i]));
+
+		/* 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]);
 		}
 	}
 
@@ -1303,69 +1230,99 @@ static int read_memory(struct target *target, uint32_t address,
 	select_dmi(target);
 	riscv_set_current_hartid(target, 0);
 
-	for (uint32_t i = 0; i < count; ++i) {
-		uint32_t offset = i*size;
-		uint32_t t_addr = address + offset;
-		uint8_t *t_buffer = buffer + offset;
+	uint64_t s0 = riscv_get_register(target, GDB_REGNO_S0);
 
-		abstract_write_register(target, S0, riscv_xlen(target), t_addr);
-
-		program_t *program = program_new();
-		switch (size) {
+	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_lx(&program, GDB_REGNO_S0, r_addr);
+	switch (size) {
 		case 1:
-			program_add32(program, lb(S1, S0, 0));
+			riscv_program_lbr(&program, GDB_REGNO_S0, GDB_REGNO_S0, 0);
 			break;
 		case 2:
-			program_add32(program, lh(S1, S0, 0));
+			riscv_program_lhr(&program, GDB_REGNO_S0, GDB_REGNO_S0, 0);
 			break;
 		case 4:
-			program_add32(program, lw(S1, S0, 0));
+			riscv_program_lwr(&program, GDB_REGNO_S0, GDB_REGNO_S0, 0);
 			break;
 		default:
 			LOG_ERROR("Unsupported size: %d", size);
 			return ERROR_FAIL;
-		}
-		program_add32(program, fence());
-		program_add32(program, ebreak());
-		program_set_read(program, S1);
-		write_program(target, program);
-		execute_program(target, program);
-		uint32_t abstractcs;
-		wait_for_idle(target, &abstractcs);
-		program_delete(program);
+	}
+	riscv_program_sw(&program, GDB_REGNO_S0, r_data);
+	program.writes_memory = false;
 
-		uint32_t value = dmi_read(target, DMI_DATA0);
-		switch (size) {
-		case 1:
-			t_buffer[0] = value;
-			break;
-		case 2:
-		  t_buffer[0] = value;
-		  t_buffer[1] = value >> 8;
-		  break;
-		case 4:
-		  t_buffer[0] = value;
-		  t_buffer[1] = value >> 8;
-		  t_buffer[2] = value >> 16;
-		  t_buffer[3] = value >> 24;
-		  break;
-		default:
-		  return ERROR_FAIL;
+	for (uint32_t i = 0; i < count; ++i) {
+		uint32_t offset = size*i;
+		uint32_t t_addr = address + offset;
+		uint8_t *t_buffer = buffer + offset;
+
+		uint64_t value;
+		if (i == 0) {
+			switch (riscv_xlen(target)) {
+			case 64:
+				riscv_program_write_ram(&program, r_addr + 4, t_addr >> 32);
+			case 32:
+				riscv_program_write_ram(&program, r_addr, t_addr);
+			}
+
+			if (riscv_program_exec(&program, target) != ERROR_OK) {
+				LOG_ERROR("failed to execute program");
+				return ERROR_FAIL;
+			}
+
+			value = riscv_program_read_ram(&program, r_data);
+		} else {
+			int d_addr = (r_addr - riscv_debug_buffer_addr(target)) / 4;
+			int d_data = (r_data - riscv_debug_buffer_addr(target)) / 4;
+
+			switch (riscv_xlen(target)) {
+			case 64:
+				riscv_write_debug_buffer(target, d_addr + 1, t_addr >> 32);
+			case 32:
+				riscv_write_debug_buffer(target, d_addr, t_addr);
+			}
+
+			if (riscv_execute_debug_buffer(target) != ERROR_OK) {
+				LOG_ERROR("failed to execute program");
+				return ERROR_FAIL;
+			}
+
+			value = riscv_read_debug_buffer(target, d_data);
 		}
 
-		if (check_dmi_error(target)) {
-			LOG_ERROR("DMI error");
-			return ERROR_FAIL;
+                switch (size) {
+                case 1:
+                        t_buffer[0] = value;
+                        break;
+                case 2:
+                        t_buffer[0] = value;
+                        t_buffer[1] = value >> 8;
+                        break;
+                case 4:
+                        t_buffer[0] = value;
+                        t_buffer[1] = value >> 8;
+                        t_buffer[2] = value >> 16;
+                        t_buffer[3] = value >> 24;
+                        break;
+                default:
+                        LOG_ERROR("unsupported access size: %d", size);
+                        return ERROR_FAIL;
 		}
+
+		LOG_DEBUG("M[0x%08lx] reads 0x%08lx", t_addr, value);
 	}
 
-	program_t *program = program_new();
-	program_add32(program, ebreak());
-	program_add32(program, ebreak());
-	program_add32(program, ebreak());
-	program_add32(program, ebreak());
-	write_program(target, program);
-	program_delete(program);
+	riscv_set_register(target, GDB_REGNO_S0, s0);
+
+	{
+		struct riscv_program fprogram;
+		riscv_program_init(&fprogram, target);
+		riscv_program_fence(&fprogram);
+		riscv_program_exec(&fprogram, target);
+	}
 
 	return ERROR_OK;
 }
@@ -1376,30 +1333,38 @@ static int write_memory(struct target *target, uint32_t address,
 	select_dmi(target);
 	riscv_set_current_hartid(target, 0);
 
+	uint64_t s0 = riscv_get_register(target, GDB_REGNO_S0);
+	uint64_t s1 = riscv_get_register(target, GDB_REGNO_S1);
+
+	struct riscv_program program;
+	riscv_program_init(&program, target);
+	riscv_addr_t r_data = riscv_program_alloc_w(&program);
+	riscv_addr_t r_addr = riscv_program_alloc_x(&program);
+	riscv_program_lx(&program, GDB_REGNO_S0, r_addr);
+	riscv_program_lw(&program, GDB_REGNO_S1, r_data);
+
+	switch (size) {
+		case 1:
+			riscv_program_sbr(&program, GDB_REGNO_S1, GDB_REGNO_S0, 0);
+			break;
+		case 2:
+			riscv_program_shr(&program, GDB_REGNO_S1, GDB_REGNO_S0, 0);
+			break;
+		case 4:
+			riscv_program_swr(&program, GDB_REGNO_S1, GDB_REGNO_S0, 0);
+			break;
+		default:
+			LOG_ERROR("Unsupported size: %d", size);
+			return ERROR_FAIL;
+	}
+
+	program.writes_memory = false;
+
 	for (uint32_t i = 0; i < count; ++i) {
 		uint32_t offset = size*i;
 		uint32_t t_addr = address + offset;
 		const uint8_t *t_buffer = buffer + offset;
 
-		abstract_write_register(target, S0, riscv_xlen(target), t_addr);
-		program_t *program = program_new();
-		switch (size) {
-			case 1:
-				program_add32(program, sb(S1, S0, 0));
-				break;
-			case 2:
-				program_add32(program, sh(S1, S0, 0));
-				break;
-			case 4:
-				program_add32(program, sw(S1, S0, 0));
-				break;
-			default:
-				LOG_ERROR("Unsupported size: %d", size);
-				return ERROR_FAIL;
-		}
-		program_add32(program, fence());
-		program_add32(program, ebreak());
-
 		uint32_t value;
 		switch (size) {
 			case 1:
@@ -1415,30 +1380,54 @@ static int write_memory(struct target *target, uint32_t address,
 					((uint32_t) t_buffer[3] << 24);
 				break;
 			default:
+				LOG_ERROR("unsupported access size: %d", size);
 				return ERROR_FAIL;
 		}
-		abstract_write_register(target, S1, riscv_xlen(target), value);
-		program_set_write(program, S1, value);
 
-		write_program(target, program);
-		execute_program(target, program);
-		uint32_t abstractcs;
-		wait_for_idle(target, &abstractcs);
-		program_delete(program);
+		LOG_DEBUG("M[0x%08lx] writes 0x%08lx", t_addr, value);
 
-		if (check_dmi_error(target)) {
-			LOG_ERROR("DMI error");
-			return ERROR_FAIL;
+		if (i == 0) {
+			switch (riscv_xlen(target)) {
+			case 64:
+				riscv_program_write_ram(&program, r_addr + 4, t_addr >> 32);
+			case 32:
+				riscv_program_write_ram(&program, r_addr, t_addr);
+			}
+			riscv_program_write_ram(&program, r_data, value);
+
+			if (riscv_program_exec(&program, target) != ERROR_OK) {
+				LOG_ERROR("failed to execute program");
+				return ERROR_FAIL;
+			}
+		} else {
+			int d_addr = (r_addr - riscv_debug_buffer_addr(target)) / 4;
+			int d_data = (r_data - riscv_debug_buffer_addr(target)) / 4;
+
+			switch (riscv_xlen(target)) {
+			case 64:
+				riscv_write_debug_buffer(target, d_addr + 1, t_addr >> 32);
+			case 32:
+				riscv_write_debug_buffer(target, d_addr, t_addr);
+			}
+			riscv_write_debug_buffer(target, d_data, value);
+
+			if (riscv_execute_debug_buffer(target) != ERROR_OK) {
+				LOG_ERROR("failed to execute program");
+				return ERROR_FAIL;
+			}
 		}
 	}
 
-	program_t *program = program_new();
-	program_add32(program, ebreak());
-	program_add32(program, ebreak());
-	program_add32(program, ebreak());
-	program_add32(program, ebreak());
-	write_program(target, program);
-	program_delete(program);
+	riscv_set_register(target, GDB_REGNO_S0, s0);
+	riscv_set_register(target, GDB_REGNO_S1, s1);
+
+	{
+		struct riscv_program fprogram;
+		riscv_program_init(&fprogram, target);
+		riscv_program_fence(&fprogram);
+		riscv_program_exec(&fprogram, target);
+	}
+
 	return ERROR_OK;
 }
 
@@ -1478,18 +1467,62 @@ struct target_type riscv013_target =
 /*** 0.13-specific implementations of various RISC-V hepler functions. ***/
 static riscv_reg_t riscv013_get_register(struct target *target, int hid, int rid)
 {
+	LOG_DEBUG("reading register 0x%08x on hart %d", rid, hid);
+
 	riscv_set_current_hartid(target, hid);
 
 	uint64_t out;
-	register_read_direct(target, &out, rid);
+	riscv013_info_t *info = get_info(target);
+
+	if (rid <= GDB_REGNO_XPR31) {
+		register_read_direct(target, &out, rid);
+	} else if (rid == GDB_REGNO_PC) {
+		register_read_direct(target, &out, GDB_REGNO_DPC);
+		LOG_DEBUG("read PC from DPC: 0x%016lx", out);
+	} else if (rid == GDB_REGNO_PRIV) {
+		uint64_t dcsr;
+		register_read_direct(target, &dcsr, CSR_DCSR);
+		buf_set_u64(&out, 0, 8, get_field(dcsr, CSR_DCSR_PRV));
+	} else {
+		int result = register_read_direct(target, &out, rid);
+		if (result != ERROR_OK) {
+			LOG_ERROR("Whoops");
+			abort();
+		}
+
+		if (rid == GDB_REGNO_MSTATUS)
+			info->mstatus_actual = out;
+	}
+
 	return out;
 }
 
 static void riscv013_set_register(struct target *target, int hid, int rid, uint64_t value)
 {
+	LOG_DEBUG("writing register 0x%08x on hart %d", rid, hid);
+
 	riscv_set_current_hartid(target, hid);
 
-	register_write_direct(target, rid, value);
+	uint64_t out;
+	riscv013_info_t *info = get_info(target);
+
+	if (rid <= GDB_REGNO_XPR31) {
+		register_write_direct(target, rid, value);
+	} else if (rid == GDB_REGNO_PC) {
+		LOG_DEBUG("writing PC to DPC: 0x%016lx", value);
+		register_write_direct(target, GDB_REGNO_DPC, value);
+		uint64_t actual_value;
+		register_read_direct(target, &actual_value, GDB_REGNO_DPC);
+		LOG_DEBUG("  actual DPC written: 0x%016lx", actual_value);
+		assert(value == actual_value);
+	} else if (rid == GDB_REGNO_PRIV) {
+		uint64_t dcsr;
+		register_read_direct(target, &dcsr, CSR_DCSR);
+		dcsr = set_field(dcsr, CSR_DCSR_PRV, value);
+		register_write_direct(target, CSR_DCSR, &dcsr);
+	} else {
+		register_write_direct(target, rid, value);
+	}
 }
 
 static void riscv013_select_current_hart(struct target *target)
@@ -1551,23 +1584,21 @@ static void riscv013_on_step(struct target *target)
 
 static void riscv013_on_halt(struct target *target)
 {
-	RISCV_INFO(r);
-	LOG_DEBUG("saving register state for hart %d", r->current_hartid);
-	riscv_save_register(target, GDB_REGNO_S0);
-	riscv_save_register(target, GDB_REGNO_S1);
-	riscv_save_register(target, GDB_REGNO_DPC);
-	riscv_save_register(target, GDB_REGNO_DCSR);
 }
 
 static bool riscv013_is_halted(struct target *target)
 {
 	uint32_t dmstatus = dmi_read(target, DMI_DMSTATUS);
+	if (get_field(dmstatus, DMI_DMSTATUS_ANYUNAVAIL))
+		LOG_ERROR("hart %d is unavailiable", riscv_current_hartid(target));
+	if (get_field(dmstatus, DMI_DMSTATUS_ANYNONEXISTENT))
+		LOG_ERROR("hart %d doesn't exist", riscv_current_hartid(target));
 	return get_field(dmstatus, DMI_DMSTATUS_ALLHALTED);
 }
 
 static enum riscv_halt_reason riscv013_halt_reason(struct target *target)
 {
-	uint64_t dcsr = riscv_peek_register(target, GDB_REGNO_DCSR);
+	uint64_t dcsr = riscv_get_register(target, GDB_REGNO_DCSR);
 	switch (get_field(dcsr, CSR_DCSR_CAUSE)) {
 	case CSR_DCSR_CAUSE_SWBP:
 	case CSR_DCSR_CAUSE_TRIGGER:
@@ -1580,37 +1611,76 @@ static enum riscv_halt_reason riscv013_halt_reason(struct target *target)
 	}
 
 	LOG_ERROR("Unknown DCSR cause field: %x", (int)get_field(dcsr, CSR_DCSR_CAUSE));
+	LOG_ERROR("  dcsr=0x%08x", dcsr);
 	abort();
 }
 
+void riscv013_debug_buffer_enter(struct target *target, struct riscv_program *program)
+{
+}
+
+void riscv013_debug_buffer_leave(struct target *target, struct riscv_program *program)
+{
+}
+
+void riscv013_write_debug_buffer(struct target *target, int index, riscv_insn_t data)
+{
+	if (index >= riscv013_progbuf_size(target))
+		return dmi_write(target, DMI_DATA0 + index - riscv013_progbuf_size(target), data);
+	return dmi_write(target, DMI_PROGBUF0 + index, data);
+}
+
+riscv_insn_t riscv013_read_debug_buffer(struct target *target, int index)
+{
+	if (index >= riscv013_progbuf_size(target))
+		return dmi_read(target, DMI_DATA0 + index - riscv013_progbuf_size(target));
+	return dmi_read(target, DMI_PROGBUF0 + index);
+}
+
+void riscv013_execute_debug_buffer(struct target *target)
+{
+	uint32_t abstractcs = dmi_read(target, DMI_ABSTRACTCS);
+	set_field(abstractcs, DMI_ABSTRACTCS_CMDERR, 0);
+	dmi_write(target, DMI_ABSTRACTCS, abstractcs);
+
+	uint32_t run_program = 0;
+	run_program = set_field(run_program, AC_ACCESS_REGISTER_SIZE, 2);
+	run_program = set_field(run_program, AC_ACCESS_REGISTER_POSTEXEC, 1);
+	run_program = set_field(run_program, AC_ACCESS_REGISTER_TRANSFER, 0);
+	run_program = set_field(run_program, AC_ACCESS_REGISTER_REGNO, 0x1000);
+	dmi_write(target, DMI_COMMAND, run_program);
+
+	{
+		uint32_t dmstatus = 0;
+		wait_for_idle(target, &dmstatus);
+	}
+
+	uint32_t cs = dmi_read(target, DMI_ABSTRACTCS);
+	if (get_field(cs, DMI_ABSTRACTCS_CMDERR) != 0) {
+		LOG_ERROR("unable to execute program: (abstractcs=0x%08x)", cs);
+		dmi_read(target, DMI_DMSTATUS);
+	}
+}
+
 /* Helper Functions. */
 static void riscv013_on_step_or_resume(struct target *target, bool step)
 {
 	RISCV_INFO(r);
-	LOG_DEBUG("restoring register state for hart %d", r->current_hartid);
 
-	program_t *program = program_new();
-	program_add32(program, fence_i());
-	program_add32(program, ebreak());
-	write_program(target, program);
-	if (execute_program(target, program) != ERROR_OK) {
+	struct riscv_program program;
+	riscv_program_init(&program, target);
+	riscv_program_fence_i(&program);
+	if (riscv_program_exec(&program, target) != ERROR_OK)
 		LOG_ERROR("Unable to execute fence.i");
-	}
-	program_delete(program);
 
 	/* We want to twiddle some bits in the debug CSR so debugging works. */
-	uint64_t dcsr = riscv_peek_register(target, GDB_REGNO_DCSR);
+	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_overwrite_register(target, GDB_REGNO_DCSR, dcsr);
-
-	riscv_restore_register(target, GDB_REGNO_DCSR);
-	riscv_restore_register(target, GDB_REGNO_DPC);
-	riscv_restore_register(target, GDB_REGNO_S1);
-	riscv_restore_register(target, GDB_REGNO_S0);
+	riscv_set_register(target, GDB_REGNO_DCSR, dcsr);
 }
 
 static void riscv013_step_or_resume_current_hart(struct target *target, bool step)
@@ -1619,36 +1689,71 @@ static void riscv013_step_or_resume_current_hart(struct target *target, bool ste
 	LOG_DEBUG("resuming hart %d (for step?=%d)", r->current_hartid, step);
 	assert(riscv_is_halted(target));
 
+	struct riscv_program program;
+	riscv_program_init(&program, target);
+	riscv_program_fence_i(&program);
+	if (riscv_program_exec(&program, target) != ERROR_OK)
+		abort();
+
 	/* Issue the halt command, and then wait for the current hart to halt. */
 	uint32_t dmcontrol = dmi_read(target, DMI_DMCONTROL);
 	dmcontrol = set_field(dmcontrol, DMI_DMCONTROL_RESUMEREQ, 1);
 	dmi_write(target, DMI_DMCONTROL, dmcontrol);
 
-#if 1
-	/* FIXME: ... well, after a short time. */
-	usleep(100);
-#else
-	/* FIXME: there's a race condition in stepping now, so just return
-	 * right away... */
 	for (size_t i = 0; i < 256; ++i) {
-		if (!riscv_is_halted(target))
-			break;
-	}
-
-	if (riscv_is_halted(target)) {
+		usleep(10);
 		uint32_t dmstatus = dmi_read(target, DMI_DMSTATUS);
-		dmcontrol = dmi_read(target, DMI_DMCONTROL);
+		if (get_field(dmstatus, DMI_DMSTATUS_ALLRESUMEACK) == 0)
+			continue;
+		if (step && get_field(dmstatus, DMI_DMSTATUS_ALLHALTED) == 0)
+			continue;
 
-		LOG_ERROR("unable to resume hart %d", r->current_hartid);
-		LOG_ERROR("  dmcontrol=0x%08x", dmcontrol);
-		LOG_ERROR("  dmstatus =0x%08x", dmstatus);
-		abort();
+		dmcontrol = set_field(dmcontrol, DMI_DMCONTROL_RESUMEREQ, 0);
+		dmi_write(target, DMI_DMCONTROL, dmcontrol);
+		return;
 	}
-#endif
 
-	dmcontrol = set_field(dmcontrol, DMI_DMCONTROL_RESUMEREQ, 0);
-	dmi_write(target, DMI_DMCONTROL, dmcontrol);
+	uint32_t dmstatus = dmi_read(target, DMI_DMSTATUS);
+	dmcontrol = dmi_read(target, DMI_DMCONTROL);
+	LOG_ERROR("unable to resume hart %d", r->current_hartid);
+	LOG_ERROR("  dmcontrol=0x%08x", dmcontrol);
+	LOG_ERROR("  dmstatus =0x%08x", dmstatus);
 
-	/* When stepping we need to go and restore the relevant registers. */
-	if (step) riscv013_on_halt(target);
+	if (step) {
+		LOG_ERROR("  was stepping, halting");
+		riscv013_halt_current_hart(target);
+		return;
+	}
+
+	abort();
+}
+
+size_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;
+}
+
+size_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;
+}
+
+size_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;
 }
diff --git a/src/target/riscv/riscv.c b/src/target/riscv/riscv.c
index c1855b0d7..58ad93754 100644
--- a/src/target/riscv/riscv.c
+++ b/src/target/riscv/riscv.c
@@ -618,9 +618,17 @@ static int riscv_poll_hart(struct target *target, int hartid)
 	RISCV_INFO(r);
 	LOG_DEBUG("polling hart %d", hartid);
 
-	/* If there's no new event then there's nothing to do. */
+	/* Polling can only detect one state change: a hart that was previously
+	 * running but has gone to sleep.  A state change in the other
+	 * direction is invalid and indicates that one of the previous calls
+	 * didn't correctly block. */
 	riscv_set_current_hartid(target, hartid);
-	assert((riscv_was_halted(target) && riscv_is_halted(target)) || !riscv_was_halted(target));
+	if (riscv_was_halted(target) && !riscv_is_halted(target)) {
+		LOG_ERROR("unexpected wakeup on hart %d", hartid);
+		abort();
+	}
+
+	/* If there's no new event then there's nothing to do. */
 	if (riscv_was_halted(target) || !riscv_is_halted(target))
 		return 0;
 
@@ -696,7 +704,9 @@ int riscv_openocd_halt(struct target *target)
 	if (out != ERROR_OK)
 		return out;
 
-	target->state = TARGET_HALTED;
+	/* Don't change the target state right here, it'll get updated by the
+	 * poll. */
+	riscv_openocd_poll(target);
 	return out;
 }
 
@@ -717,6 +727,7 @@ int riscv_openocd_resume(
 		return out;
 
 	target->state = TARGET_RUNNING;
+	riscv_openocd_poll(target);
 	return out;
 }
 
@@ -738,7 +749,7 @@ int riscv_openocd_step(
 		return out;
 
 	/* step_rtos_hart blocks until the hart has actually stepped, but we
-	 * need to cycle through OpenOCD to  */
+	 * need to cycle through OpenOCD to actually get this to trigger. */
 	target->state = TARGET_RUNNING;
 	riscv_openocd_poll(target);
 
@@ -756,55 +767,15 @@ void riscv_info_init(riscv_info_t *r)
 	r->rtos_enabled = true;
 
 	for (size_t h = 0; h < RISCV_MAX_HARTS; ++h) {
-		/* FIXME: I need to rip out Tim's probing sequence, as it
-		 * disrupts the running code.  For now, I'm just hard-coding
-		 * XLEN to 64 for all cores at reset. */
-		r->xlen[h] = 64;
+		r->xlen[h] = -1;
 		r->hart_state[h] = RISCV_HART_UNKNOWN;
+		r->debug_buffer_addr[h] = -1;
 
 		for (size_t e = 0; e < RISCV_MAX_REGISTERS; ++e)
 			r->valid_saved_registers[h][e] = false;
 	}
 }
 
-void riscv_save_register(struct target *target, int regno)
-{
-	RISCV_INFO(r);
-	int hartno = r->current_hartid;
-	LOG_DEBUG("riscv_save_register(%d, %d)", hartno, regno);
-	assert(r->valid_saved_registers[hartno][regno] == false);
-	r->valid_saved_registers[hartno][regno] = true;
-	r->saved_registers[hartno][regno] = riscv_get_register(target, hartno, regno);
-}
-
-uint64_t riscv_peek_register(struct target *target, int regno)
-{
-	RISCV_INFO(r);
-	int hartno = r->current_hartid;
-	LOG_DEBUG("riscv_peek_register(%d, %d)", hartno, regno);
-	assert(r->valid_saved_registers[hartno][regno] == true);
-	return r->saved_registers[hartno][regno];
-}
-
-void riscv_overwrite_register(struct target *target, int regno, uint64_t newval)
-{
-	RISCV_INFO(r);
-	int hartno = r->current_hartid;
-	LOG_DEBUG("riscv_overwrite_register(%d, %d)", hartno, regno);
-	assert(r->valid_saved_registers[hartno][regno] == true);
-	r->saved_registers[hartno][regno] = newval;
-}
-
-void riscv_restore_register(struct target *target, int regno)
-{
-	RISCV_INFO(r);
-	int hartno = r->current_hartid;
-	LOG_DEBUG("riscv_restore_register(%d, %d)", hartno, regno);
-	assert(r->valid_saved_registers[hartno][regno] == true);
-	r->valid_saved_registers[hartno][regno] = false;
-	riscv_set_register(target, hartno, regno, r->saved_registers[hartno][regno]);
-}
-
 int riscv_halt_all_harts(struct target *target)
 {
 	if (riscv_rtos_enabled(target)) {
@@ -837,6 +808,8 @@ int riscv_halt_one_hart(struct target *target, int hartid)
 	}
 
 	r->halt_current_hart(target);
+	/* Here we don't actually update 'hart_state' because we want poll to
+	 * pick that up.  We can't actually wait until */
 	return ERROR_OK;
 }
 
@@ -894,6 +867,10 @@ int riscv_step_rtos_hart(struct target *target)
 	assert(r->hart_state[hartid] == RISCV_HART_HALTED);
 	r->on_step(target);
 	r->step_current_hart(target);
+	r->hart_state[hartid] = RISCV_HART_RUNNING;
+	r->on_halt(target);
+	r->hart_state[hartid] = RISCV_HART_HALTED;
+	assert(riscv_is_halted(target));
 	return ERROR_OK;
 }
 
@@ -924,11 +901,16 @@ bool riscv_rtos_enabled(const struct target *target)
 void riscv_set_current_hartid(struct target *target, int hartid)
 {
 	RISCV_INFO(r);
-	register_cache_invalidate(target->reg_cache);
 	r->current_hartid = hartid;
 	r->select_current_hart(target);
 
+	/* This might get called during init, in which case we shouldn't be
+	 * setting up the register cache. */
+	if (!target_was_examined(target))
+		return;
+
 	/* Update the register list's widths. */
+	register_cache_invalidate(target->reg_cache);
 	for (size_t i = 0; i < GDB_REGNO_COUNT; ++i) {
 		struct reg *reg = &target->reg_cache->reg_list[i];
 
@@ -979,14 +961,24 @@ bool riscv_has_register(struct target *target, int hartid, int regid)
 	return 1;
 }
 
-void riscv_set_register(struct target *target, int hartid, enum gdb_regno regid, uint64_t value)
+void riscv_set_register(struct target *target, enum gdb_regno r, riscv_reg_t v)
+{
+	return riscv_set_register_on_hart(target, riscv_current_hartid(target), r, v);
+}
+
+void riscv_set_register_on_hart(struct target *target, int hartid, enum gdb_regno regid, uint64_t value)
 {
 	RISCV_INFO(r);
 	LOG_DEBUG("writing register %d on hart %d", regid, hartid);
 	return r->set_register(target, hartid, regid, value);
 }
 
-uint64_t riscv_get_register(struct target *target, int hartid, enum gdb_regno regid)
+riscv_reg_t riscv_get_register(struct target *target, enum gdb_regno r)
+{
+	return riscv_get_register_on_hart(target, riscv_current_hartid(target), r);
+}
+
+uint64_t riscv_get_register_on_hart(struct target *target, int hartid, enum gdb_regno regid)
 {
 	RISCV_INFO(r);
 	LOG_DEBUG("reading register %d on hart %d", regid, hartid);
@@ -1025,3 +1017,51 @@ int riscv_count_triggers_of_hart(struct target *target, int hartid)
 	assert(hartid < riscv_count_harts(target));
 	return r->trigger_count[hartid];
 }
+
+size_t riscv_debug_buffer_size(struct target *target)
+{
+	RISCV_INFO(r);
+	return r->debug_buffer_size[riscv_current_hartid(target)];
+}
+
+riscv_addr_t riscv_debug_buffer_addr(struct target *target)
+{
+	RISCV_INFO(r);
+	riscv_addr_t out = r->debug_buffer_addr[riscv_current_hartid(target)];
+	assert(out != -1);
+	return out;
+}
+
+int riscv_debug_buffer_enter(struct target *target, struct riscv_program *program)
+{
+	RISCV_INFO(r);
+	r->debug_buffer_enter(target, program);
+	return ERROR_OK;
+}
+
+int riscv_debug_buffer_leave(struct target *target, struct riscv_program *program)
+{
+	RISCV_INFO(r);
+	r->debug_buffer_leave(target, program);
+	return ERROR_OK;
+}
+
+int riscv_write_debug_buffer(struct target *target, int index, riscv_insn_t insn)
+{
+	RISCV_INFO(r);
+	r->write_debug_buffer(target, index, insn);
+	return ERROR_OK;
+}
+
+riscv_insn_t riscv_read_debug_buffer(struct target *target, int index)
+{
+	RISCV_INFO(r);
+	return r->read_debug_buffer(target, index);
+}
+
+int riscv_execute_debug_buffer(struct target *target)
+{
+	RISCV_INFO(r);
+	r->execute_debug_buffer(target);
+	return ERROR_OK;
+}
diff --git a/src/target/riscv/riscv.h b/src/target/riscv/riscv.h
index 73fee415e..84ed411db 100644
--- a/src/target/riscv/riscv.h
+++ b/src/target/riscv/riscv.h
@@ -1,6 +1,9 @@
 #ifndef RISCV_H
 #define RISCV_H
 
+struct riscv_program;
+
+#include <stdint.h>
 #include "opcodes.h"
 #include "gdb_regs.h"
 
@@ -16,6 +19,8 @@ extern struct target_type riscv013_target;
  * Definitions shared by code supporting all RISC-V versions.
  */
 typedef uint64_t riscv_reg_t;
+typedef uint32_t riscv_insn_t;
+typedef  int64_t riscv_addr_t;
 
 enum riscv_hart_state {
 	RISCV_HART_UNKNOWN,
@@ -68,6 +73,12 @@ typedef struct {
 	/* The number of triggers per hart. */
 	int trigger_count[RISCV_MAX_HARTS];
 
+	/* The address of the debug RAM buffer. */
+	riscv_addr_t debug_buffer_addr[RISCV_MAX_HARTS];
+
+	/* The number of entries in the debug buffer. */
+	int debug_buffer_size[RISCV_MAX_HARTS];
+
 	/* Helper functions that target the various RISC-V debug spec
 	 * implementations. */
 	riscv_reg_t (*get_register)(struct target *, int, int);
@@ -81,6 +92,11 @@ 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, int i, riscv_insn_t d);
+	riscv_insn_t (*read_debug_buffer)(struct target *target, int i);
+	void (*execute_debug_buffer)(struct target *target);
 } riscv_info_t;
 
 /* Everything needs the RISC-V specific info structure, so here's a nice macro
@@ -122,12 +138,6 @@ int riscv_openocd_step(
 /* Initializes the shared RISC-V structure. */
 void riscv_info_init(riscv_info_t *r);
 
-/* Functions that save and restore registers.  */
-void riscv_save_register(struct target *target, int regno);
-uint64_t riscv_peek_register(struct target *target, int regno);
-void riscv_overwrite_register(struct target *target, int regno, uint64_t newval);
-void riscv_restore_register(struct target *target, int regno);
-
 /* Run control, possibly for multiple harts.  The _all_harts versions resume
  * all the enabled harts, which when running in RTOS mode is all the harts on
  * the system. */
@@ -172,8 +182,10 @@ bool riscv_has_register(struct target *target, int hartid, int regid);
 
 /* Returns the value of the given register on the given hart.  32-bit registers
  * are zero extended to 64 bits.  */
-void riscv_set_register(struct target *target, int hid, enum gdb_regno rid, uint64_t v);
-riscv_reg_t riscv_get_register(struct target *target, int hid, enum gdb_regno rid);
+void riscv_set_register(struct target *target, enum gdb_regno i, riscv_reg_t v);
+void riscv_set_register_on_hart(struct target *target, int hid, enum gdb_regno rid, uint64_t v);
+riscv_reg_t riscv_get_register(struct target *target, enum gdb_regno i);
+riscv_reg_t riscv_get_register_on_hart(struct target *target, int hid, enum gdb_regno rid);
 
 /* Checks the state of the current hart -- "is_halted" checks the actual
  * on-device register, while "was_halted" checks the machine's state. */
@@ -186,4 +198,16 @@ enum riscv_halt_reason riscv_halt_reason(struct target *target, int hartid);
 int riscv_count_triggers(struct target *target);
 int riscv_count_triggers_of_hart(struct target *target, int hartid);
 
+/* These helper functions let the generic program interface get target-specific
+ * information. */
+size_t riscv_debug_buffer_size(struct target *target);
+riscv_addr_t riscv_debug_buffer_addr(struct target *target);
+
+int riscv_debug_buffer_enter(struct target *target, struct riscv_program *program);
+int riscv_debug_buffer_leave(struct target *target, struct riscv_program *program);
+
+riscv_insn_t riscv_read_debug_buffer(struct target *target, int index);
+int riscv_write_debug_buffer(struct target *target, int index, riscv_insn_t insn);
+int riscv_execute_debug_buffer(struct target *target);
+
 #endif