Add memory sample feature (#541)

* Add memory sampling feature.

Currently only gets 10 samples per second, but the overall scaffolding
looks like it works.

Change-Id: I25a2bbcba322f2101c3de598c225f83c902680fa

* Basic memory sample speed-ups.

977 samples/second.

Change-Id: I6ea874f25051aca1cbe3aa2918567a4ee316c4be

* Add base64 dumping of sample buffer.

We can't just dump raw data, because the API we use to get data to the
"user" uses NULL-terminated strings.

Change-Id: I3f33faaa485a74735c13cdaad685e336c1e2095f

* WIP on optimizing PC sampling.

1k samples per second on my laptop, which is roughly double what it was.

Change-Id: I6a77df8aa53118e44928f96d22210df84be45eda

* WIP

Change-Id: I4300692355cb0cf997ec59ab5ca71543b295abb0

* Use small batch to sample memory.

5k samples/second. No error checking.

Change-Id: I8a7f08e49cb153699021e27f8006beb0e6db70ee

* Collect memory samples near continuously.

Rewrite OpenOCD's core loop to get rid of the fixed 100ms delay.
Now collecting 15k samples/second.

Change-Id: Iba5e73e96e8d226a0b5777ecac19453c152dc634

* Fix build.

Change-Id: If2fe7a0c77e0d6545c93fa0d4a013c50a9b9d896

* Fix the mess I left after resolving conflicts.

Change-Id: I96abd47a7834bf8f5e005ba63020f0a0cc429548

* Support 64-bit address in memory sampling.

* Support sampling 64-bit values.

* Better error reporting. WIP on 64-bit support.

* Speed up single 32-bit memory sample.

21k samples/second.

* WIP on review feedback.

Change-Id: I00e453fd685d173b0206d925090beb06c1f057ca

* Make memory sample buffers/config per-target.

Change-Id: I5c2f5997795c7a434e71b36ca4c712623daf993c

* Document, and add bucket clear option.

Change-Id: I922b883adfa787fb4f5a894db872d04fda126cbd
Signed-off-by: Tim Newsome <tim@sifive.com>

* Fix whitespace.

Change-Id: Iabfeb0068d7138d9b252ac127d1b1f949cf19632
Signed-off-by: Tim Newsome <tim@sifive.com>

* Document sample buffer full behavior.

Change-Id: Ib3c30d34b1f9f30cf403afda8fdccb850bc8b4df
Signed-off-by: Tim Newsome <tim@sifive.com>

* Actually clear the sample buffer in dump_sample_buf.

Change-Id: Ifda22643f1e58f69a6382abc90474659d7330ac5
Signed-off-by: Tim Newsome <tim@sifive.com>

* Use compatible string formatting.

Change-Id: Ia5e5333e036c1dbe457bc977fcee41983b9a9b77
Signed-off-by: Tim Newsome <tim@sifive.com>

11 files changed, 696 insertions, 84 deletions

diff --git a/src/helper/Makefile.am b/src/helper/Makefile.am
index 2b3523f..ad6ec11 100644
--- a/src/helper/Makefile.am
+++ b/src/helper/Makefile.am
@@ -30,7 +30,9 @@ noinst_LTLIBRARIES += %D%/libhelper.la
 	%D%/system.h \
 	%D%/jep106.h \
 	%D%/jep106.inc \
-	%D%/jim-nvp.h
+	%D%/jim-nvp.h \
+	%D%/base64.c \
+	%D%/base64.h
 
 if IOUTIL
 %C%_libhelper_la_SOURCES += %D%/ioutil.c
diff --git a/src/helper/base64.c b/src/helper/base64.c
new file mode 100644
index 0000000..effa8ac
--- /dev/null
+++ b/src/helper/base64.c
@@ -0,0 +1,157 @@
+/*
+ * Base64 encoding/decoding (RFC1341)
+ * Copyright (c) 2005-2011, Jouni Malinen <j@w1.fi>
+ *
+ * This software may be distributed under the terms of the BSD license.
+ * See README for more details.
+ */
+
+
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "base64.h"
+
+static const unsigned char base64_table[65] =
+	"ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";
+
+/**
+ * base64_encode - Base64 encode
+ * @src: Data to be encoded
+ * @len: Length of the data to be encoded
+ * @out_len: Pointer to output length variable, or %NULL if not used
+ * Returns: Allocated buffer of out_len bytes of encoded data,
+ * or %NULL on failure
+ *
+ * Caller is responsible for freeing the returned buffer. Returned buffer is
+ * nul terminated to make it easier to use as a C string. The nul terminator is
+ * not included in out_len.
+ */
+unsigned char *base64_encode(const unsigned char *src, size_t len,
+			      size_t *out_len)
+{
+	unsigned char *out, *pos;
+	const unsigned char *end, *in;
+	size_t olen;
+	int line_len;
+
+	olen = len * 4 / 3 + 4; /* 3-byte blocks to 4-byte */
+	olen += olen / 72; /* line feeds */
+	olen++; /* nul termination */
+	if (olen < len)
+		return NULL; /* integer overflow */
+	out = malloc(olen);
+	if (out == NULL)
+		return NULL;
+
+	end = src + len;
+	in = src;
+	pos = out;
+	line_len = 0;
+	while (end - in >= 3) {
+		*pos++ = base64_table[in[0] >> 2];
+		*pos++ = base64_table[((in[0] & 0x03) << 4) | (in[1] >> 4)];
+		*pos++ = base64_table[((in[1] & 0x0f) << 2) | (in[2] >> 6)];
+		*pos++ = base64_table[in[2] & 0x3f];
+		in += 3;
+		line_len += 4;
+		if (line_len >= 72) {
+			*pos++ = '\n';
+			line_len = 0;
+		}
+	}
+
+	if (end - in) {
+		*pos++ = base64_table[in[0] >> 2];
+		if (end - in == 1) {
+			*pos++ = base64_table[(in[0] & 0x03) << 4];
+			*pos++ = '=';
+		} else {
+			*pos++ = base64_table[((in[0] & 0x03) << 4) |
+					      (in[1] >> 4)];
+			*pos++ = base64_table[(in[1] & 0x0f) << 2];
+		}
+		*pos++ = '=';
+		line_len += 4;
+	}
+
+	if (line_len)
+		*pos++ = '\n';
+
+	*pos = '\0';
+	if (out_len)
+		*out_len = pos - out;
+	return out;
+}
+
+
+/**
+ * base64_decode - Base64 decode
+ * @src: Data to be decoded
+ * @len: Length of the data to be decoded
+ * @out_len: Pointer to output length variable
+ * Returns: Allocated buffer of out_len bytes of decoded data,
+ * or %NULL on failure
+ *
+ * Caller is responsible for freeing the returned buffer.
+ */
+unsigned char *base64_decode(const unsigned char *src, size_t len,
+			      size_t *out_len)
+{
+	unsigned char dtable[256], *out, *pos, block[4], tmp;
+	size_t i, count, olen;
+	int pad = 0;
+
+	memset(dtable, 0x80, 256);
+	for (i = 0; i < sizeof(base64_table) - 1; i++)
+		dtable[base64_table[i]] = (unsigned char) i;
+	dtable['='] = 0;
+
+	count = 0;
+	for (i = 0; i < len; i++) {
+		if (dtable[src[i]] != 0x80)
+			count++;
+	}
+
+	if (count == 0 || count % 4)
+		return NULL;
+
+	olen = count / 4 * 3;
+	pos = out = malloc(olen);
+	if (out == NULL)
+		return NULL;
+
+	count = 0;
+	for (i = 0; i < len; i++) {
+		tmp = dtable[src[i]];
+		if (tmp == 0x80)
+			continue;
+
+		if (src[i] == '=')
+			pad++;
+		block[count] = tmp;
+		count++;
+		if (count == 4) {
+			*pos++ = (block[0] << 2) | (block[1] >> 4);
+			*pos++ = (block[1] << 4) | (block[2] >> 2);
+			*pos++ = (block[2] << 6) | block[3];
+			count = 0;
+			if (pad) {
+				if (pad == 1)
+					pos--;
+				else if (pad == 2)
+					pos -= 2;
+				else {
+					/* Invalid padding */
+					free(out);
+					return NULL;
+				}
+				break;
+			}
+		}
+	}
+
+	*out_len = pos - out;
+	return out;
+}
diff --git a/src/helper/base64.h b/src/helper/base64.h
new file mode 100644
index 0000000..dbabb60
--- /dev/null
+++ b/src/helper/base64.h
@@ -0,0 +1,17 @@
+/*
+ * Base64 encoding/decoding (RFC1341)
+ * Copyright (c) 2005, Jouni Malinen <j@w1.fi>
+ *
+ * This software may be distributed under the terms of the BSD license.
+ * See README for more details.
+ */
+
+#ifndef BASE64_H
+#define BASE64_H
+
+unsigned char *base64_encode(const unsigned char *src, size_t len,
+			      size_t *out_len);
+unsigned char *base64_decode(const unsigned char *src, size_t len,
+			      size_t *out_len);
+
+#endif /* BASE64_H */
diff --git a/src/helper/command.c b/src/helper/command.c
index 271e7b9..cfaa1f7 100644
--- a/src/helper/command.c
+++ b/src/helper/command.c
@@ -195,7 +195,7 @@ struct command_context *current_command_context(Jim_Interp *interp)
 static int script_command_run(Jim_Interp *interp,
 	int argc, Jim_Obj * const *argv, struct command *c)
 {
-	target_call_timer_callbacks_now();
+	target_call_timer_callbacks_now(NULL);
 	LOG_USER_N("%s", "");	/* Keep GDB connection alive*/
 
 	unsigned nwords;
@@ -1201,7 +1201,7 @@ COMMAND_HANDLER(handle_sleep_command)
 	if (!busy) {
 		int64_t then = timeval_ms();
 		while (timeval_ms() - then < (int64_t)duration) {
-			target_call_timer_callbacks_now();
+			target_call_timer_callbacks_now(NULL);
 			usleep(1000);
 		}
 	} else
diff --git a/src/server/gdb_server.c b/src/server/gdb_server.c
index 4c5f32a..c83347a 100644
--- a/src/server/gdb_server.c
+++ b/src/server/gdb_server.c
@@ -2634,13 +2634,13 @@ static int gdb_query_packet(struct connection *connection,
 
 			/* We want to print all debug output to GDB connection */
 			log_add_callback(gdb_log_callback, connection);
-			target_call_timer_callbacks_now();
+			target_call_timer_callbacks_now(NULL);
 			/* some commands need to know the GDB connection, make note of current
 			 * GDB connection. */
 			current_gdb_connection = gdb_connection;
 			command_run_line(cmd_ctx, cmd);
 			current_gdb_connection = NULL;
-			target_call_timer_callbacks_now();
+			target_call_timer_callbacks_now(NULL);
 			log_remove_callback(gdb_log_callback, connection);
 			free(cmd);
 		}
diff --git a/src/server/server.c b/src/server/server.c
index 23a15ef..19cf16a 100644
--- a/src/server/server.c
+++ b/src/server/server.c
@@ -33,6 +33,7 @@
 #include "openocd.h"
 #include "tcl_server.h"
 #include "telnet_server.h"
+#include "time_support.h"
 
 #include <signal.h>
 
@@ -441,6 +442,8 @@ int server_loop(struct command_context *command_context)
 	/* used in accept() */
 	int retval;
 
+	int64_t next_event = timeval_ms() + polling_period;
+
 #ifndef _WIN32
 	if (signal(SIGPIPE, SIG_IGN) == SIG_ERR)
 		LOG_ERROR("couldn't set SIGPIPE to SIG_IGN");
@@ -482,7 +485,12 @@ int server_loop(struct command_context *command_context)
 			retval = socket_select(fd_max + 1, &read_fds, NULL, NULL, &tv);
 		} else {
 			/* Every 100ms, can be changed with "poll_period" command */
-			tv.tv_usec = polling_period * 1000;
+			int timeout_ms = next_event - timeval_ms();
+			if (timeout_ms < 0)
+				timeout_ms = 0;
+			else if (timeout_ms > polling_period)
+				timeout_ms = polling_period;
+			tv.tv_usec = timeout_ms * 1000;
 			/* Only while we're sleeping we'll let others run */
 			openocd_sleep_prelude();
 			kept_alive();
@@ -515,7 +523,7 @@ int server_loop(struct command_context *command_context)
 		if (retval == 0) {
 			/* We only execute these callbacks when there was nothing to do or we timed
 			 *out */
-			target_call_timer_callbacks();
+			target_call_timer_callbacks(&next_event);
 			process_jim_events(command_context);
 
 			FD_ZERO(&read_fds);	/* eCos leaves read_fds unchanged in this case!  */
diff --git a/src/target/riscv/riscv-013.c b/src/target/riscv/riscv-013.c
index 9064002..2f9bc01 100644
--- a/src/target/riscv/riscv-013.c
+++ b/src/target/riscv/riscv-013.c
@@ -2023,6 +2023,220 @@ static int riscv013_set_register_buf(struct target *target,
 	return result;
 }
 
+static uint32_t sb_sbaccess(unsigned size_bytes)
+{
+	switch (size_bytes) {
+		case 1:
+			return set_field(0, DM_SBCS_SBACCESS, 0);
+		case 2:
+			return set_field(0, DM_SBCS_SBACCESS, 1);
+		case 4:
+			return set_field(0, DM_SBCS_SBACCESS, 2);
+		case 8:
+			return set_field(0, DM_SBCS_SBACCESS, 3);
+		case 16:
+			return set_field(0, DM_SBCS_SBACCESS, 4);
+	}
+	assert(0);
+	return 0;	/* Make mingw happy. */
+}
+
+static int sb_write_address(struct target *target, target_addr_t address,
+							bool ensure_success)
+{
+	RISCV013_INFO(info);
+	unsigned sbasize = get_field(info->sbcs, DM_SBCS_SBASIZE);
+	/* There currently is no support for >64-bit addresses in OpenOCD. */
+	if (sbasize > 96)
+		dmi_op(target, NULL, NULL, DMI_OP_WRITE, DM_SBADDRESS3, 0, false, false);
+	if (sbasize > 64)
+		dmi_op(target, NULL, NULL, DMI_OP_WRITE, DM_SBADDRESS2, 0, false, false);
+	if (sbasize > 32)
+		dmi_op(target, NULL, NULL, DMI_OP_WRITE, DM_SBADDRESS1, address >> 32, false, false);
+	return dmi_op(target, NULL, NULL, DMI_OP_WRITE, DM_SBADDRESS0, address,
+				  false, ensure_success);
+}
+
+static int batch_run(const struct target *target, struct riscv_batch *batch)
+{
+	RISCV013_INFO(info);
+	RISCV_INFO(r);
+	if (r->reset_delays_wait >= 0) {
+		r->reset_delays_wait -= batch->used_scans;
+		if (r->reset_delays_wait <= 0) {
+			batch->idle_count = 0;
+			info->dmi_busy_delay = 0;
+			info->ac_busy_delay = 0;
+		}
+	}
+	return riscv_batch_run(batch);
+}
+
+static int sba_supports_access(struct target *target, unsigned size_bytes)
+{
+	RISCV013_INFO(info);
+	switch (size_bytes) {
+		case 1:
+			return get_field(info->sbcs, DM_SBCS_SBACCESS8);
+		case 2:
+			return get_field(info->sbcs, DM_SBCS_SBACCESS16);
+		case 4:
+			return get_field(info->sbcs, DM_SBCS_SBACCESS32);
+		case 8:
+			return get_field(info->sbcs, DM_SBCS_SBACCESS64);
+		case 16:
+			return get_field(info->sbcs, DM_SBCS_SBACCESS128);
+		default:
+			return 0;
+	}
+}
+
+static int sample_memory_bus_v1(struct target *target,
+								riscv_sample_buf_t *buf,
+								const riscv_sample_config_t *config,
+								int64_t until_ms)
+{
+	RISCV013_INFO(info);
+	unsigned sbasize = get_field(info->sbcs, DM_SBCS_SBASIZE);
+	if (sbasize > 64) {
+		LOG_ERROR("Memory sampling is only implemented for sbasize <= 64.");
+		return ERROR_NOT_IMPLEMENTED;
+	}
+
+	if (get_field(info->sbcs, DM_SBCS_SBVERSION) != 1) {
+		LOG_ERROR("Memory sampling is only implemented for SBA version 1.");
+		return ERROR_NOT_IMPLEMENTED;
+	}
+
+	uint32_t sbcs = 0;
+	uint32_t sbcs_valid = false;
+
+	uint32_t sbaddress0 = 0;
+	bool sbaddress0_valid = false;
+	uint32_t sbaddress1 = 0;
+	bool sbaddress1_valid = false;
+
+	/* How often to read each value in a batch. */
+	const unsigned repeat = 5;
+
+	unsigned enabled_count = 0;
+	for (unsigned i = 0; i < DIM(config->bucket); i++) {
+		if (config->bucket[i].enabled)
+			enabled_count++;
+	}
+
+	while (timeval_ms() < until_ms) {
+		/*
+		 * batch_run() adds to the batch, so we can't simply reuse the same
+		 * batch over and over. So we create a new one every time through the
+		 * loop.
+		 */
+		struct riscv_batch *batch = riscv_batch_alloc(
+			target, 1 + enabled_count * 5 * repeat,
+			info->dmi_busy_delay + info->bus_master_read_delay);
+
+		unsigned result_bytes = 0;
+		for (unsigned n = 0; n < repeat; n++) {
+			for (unsigned i = 0; i < DIM(config->bucket); i++) {
+				if (config->bucket[i].enabled) {
+					if (!sba_supports_access(target, config->bucket[i].size_bytes)) {
+						LOG_ERROR("Hardware does not support SBA access for %d-byte memory sampling.",
+								config->bucket[i].size_bytes);
+						return ERROR_NOT_IMPLEMENTED;
+					}
+
+					uint32_t sbcs_write = DM_SBCS_SBREADONADDR;
+					if (enabled_count == 1)
+						sbcs_write |= DM_SBCS_SBREADONDATA;
+					sbcs_write |= sb_sbaccess(config->bucket[i].size_bytes);
+					if (!sbcs_valid || sbcs_write != sbcs) {
+						riscv_batch_add_dmi_write(batch, DM_SBCS, sbcs_write);
+						sbcs = sbcs_write;
+						sbcs_valid = true;
+					}
+
+					if (sbasize > 32 &&
+							(!sbaddress1_valid ||
+							sbaddress1 != config->bucket[i].address >> 32)) {
+						sbaddress1 = config->bucket[i].address >> 32;
+						riscv_batch_add_dmi_write(batch, DM_SBADDRESS1, sbaddress1);
+						sbaddress1_valid = true;
+					}
+					if (!sbaddress0_valid ||
+							sbaddress0 != (config->bucket[i].address & 0xffffffff)) {
+						sbaddress0 = config->bucket[i].address;
+						riscv_batch_add_dmi_write(batch, DM_SBADDRESS0, sbaddress0);
+						sbaddress0_valid = true;
+					}
+					if (config->bucket[i].size_bytes > 4)
+						riscv_batch_add_dmi_read(batch, DM_SBDATA1);
+					riscv_batch_add_dmi_read(batch, DM_SBDATA0);
+					result_bytes += 1 + config->bucket[i].size_bytes;
+				}
+			}
+		}
+
+		if (buf->used + result_bytes >= buf->size) {
+			riscv_batch_free(batch);
+			break;
+		}
+
+		size_t sbcs_key = riscv_batch_add_dmi_read(batch, DM_SBCS);
+
+		int result = batch_run(target, batch);
+		if (result != ERROR_OK)
+			return result;
+
+		uint32_t sbcs_read = riscv_batch_get_dmi_read_data(batch, sbcs_key);
+		if (get_field(sbcs_read, DM_SBCS_SBBUSYERROR)) {
+			/* Discard this batch (too much hassle to try to recover partial
+			 * data) and try again with a larger delay. */
+			info->bus_master_read_delay += info->bus_master_read_delay / 10 + 1;
+			dmi_write(target, DM_SBCS, DM_SBCS_SBBUSYERROR | DM_SBCS_SBERROR);
+			riscv_batch_free(batch);
+			continue;
+		}
+		if (get_field(sbcs_read, DM_SBCS_SBERROR)) {
+			/* The memory we're sampling was unreadable, somehow. Give up. */
+			dmi_write(target, DM_SBCS, DM_SBCS_SBBUSYERROR | DM_SBCS_SBERROR);
+			riscv_batch_free(batch);
+			return ERROR_FAIL;
+		}
+
+		unsigned read = 0;
+		for (unsigned n = 0; n < repeat; n++) {
+			for (unsigned i = 0; i < DIM(config->bucket); i++) {
+				if (config->bucket[i].enabled) {
+					assert(i < RISCV_SAMPLE_BUF_TIMESTAMP);
+					uint64_t value = 0;
+					if (config->bucket[i].size_bytes > 4)
+						value = ((uint64_t) riscv_batch_get_dmi_read_data(batch, read++)) << 32;
+					value |= riscv_batch_get_dmi_read_data(batch, read++);
+
+					buf->buf[buf->used] = i;
+					buf_set_u64(buf->buf + buf->used + 1, 0, config->bucket[i].size_bytes * 8, value);
+					buf->used += 1 + config->bucket[i].size_bytes;
+				}
+			}
+		}
+
+		riscv_batch_free(batch);
+	}
+
+	return ERROR_OK;
+}
+
+static int sample_memory(struct target *target,
+						 riscv_sample_buf_t *buf,
+						 riscv_sample_config_t *config,
+						 int64_t until_ms)
+{
+	if (!config->enabled)
+		return ERROR_OK;
+
+	return sample_memory_bus_v1(target, buf, config, until_ms);
+}
+
 static int init_target(struct command_context *cmd_ctx,
 		struct target *target)
 {
@@ -2062,6 +2276,7 @@ static int init_target(struct command_context *cmd_ctx,
 	generic_info->version_specific = calloc(1, sizeof(riscv013_info_t));
 	if (!generic_info->version_specific)
 		return ERROR_FAIL;
+	generic_info->sample_memory = sample_memory;
 	riscv013_info_t *info = get_info(target);
 
 	info->progbufsize = -1;
@@ -2299,24 +2514,6 @@ static int read_memory_bus_word(struct target *target, target_addr_t address,
 	return ERROR_OK;
 }
 
-static uint32_t sb_sbaccess(unsigned size_bytes)
-{
-	switch (size_bytes) {
-		case 1:
-			return set_field(0, DM_SBCS_SBACCESS, 0);
-		case 2:
-			return set_field(0, DM_SBCS_SBACCESS, 1);
-		case 4:
-			return set_field(0, DM_SBCS_SBACCESS, 2);
-		case 8:
-			return set_field(0, DM_SBCS_SBACCESS, 3);
-		case 16:
-			return set_field(0, DM_SBCS_SBACCESS, 4);
-	}
-	assert(0);
-	return 0;	/* Make mingw happy. */
-}
-
 static target_addr_t sb_read_address(struct target *target)
 {
 	RISCV013_INFO(info);
@@ -2333,20 +2530,6 @@ static target_addr_t sb_read_address(struct target *target)
 	return address;
 }
 
-static int sb_write_address(struct target *target, target_addr_t address)
-{
-	RISCV013_INFO(info);
-	unsigned sbasize = get_field(info->sbcs, DM_SBCS_SBASIZE);
-	/* There currently is no support for >64-bit addresses in OpenOCD. */
-	if (sbasize > 96)
-		dmi_write(target, DM_SBADDRESS3, 0);
-	if (sbasize > 64)
-		dmi_write(target, DM_SBADDRESS2, 0);
-	if (sbasize > 32)
-		dmi_write(target, DM_SBADDRESS1, address >> 32);
-	return dmi_write(target, DM_SBADDRESS0, address);
-}
-
 static int read_sbcs_nonbusy(struct target *target, uint32_t *sbcs)
 {
 	time_t start = time(NULL);
@@ -2474,6 +2657,10 @@ static int read_memory_bus_v0(struct target *target, target_addr_t address,
 		}
 	}
 
+	uint32_t sbcs;
+	if (dmi_read(target, &sbcs, DM_SBCS) != ERROR_OK)
+		return ERROR_FAIL;
+
 	return ERROR_OK;
 }
 
@@ -2503,7 +2690,7 @@ static int read_memory_bus_v1(struct target *target, target_addr_t address,
 			return ERROR_FAIL;
 
 		/* This address write will trigger the first read. */
-		if (sb_write_address(target, next_address) != ERROR_OK)
+		if (sb_write_address(target, next_address, true) != ERROR_OK)
 			return ERROR_FAIL;
 
 		if (info->bus_master_read_delay) {
@@ -2613,21 +2800,6 @@ static int read_memory_bus_v1(struct target *target, target_addr_t address,
 	return ERROR_OK;
 }
 
-static int batch_run(const struct target *target, struct riscv_batch *batch)
-{
-	RISCV013_INFO(info);
-	RISCV_INFO(r);
-	if (r->reset_delays_wait >= 0) {
-		r->reset_delays_wait -= batch->used_scans;
-		if (r->reset_delays_wait <= 0) {
-			batch->idle_count = 0;
-			info->dmi_busy_delay = 0;
-			info->ac_busy_delay = 0;
-		}
-	}
-	return riscv_batch_run(batch);
-}
-
 static void log_mem_access_result(struct target *target, bool success, int method, bool read)
 {
 	RISCV_INFO(r);
@@ -2708,11 +2880,7 @@ static bool mem_should_skip_sysbus(struct target *target, target_addr_t address,
 	assert(skip_reason);
 
 	RISCV013_INFO(info);
-	if ((!get_field(info->sbcs, DM_SBCS_SBACCESS8) || size != 1) &&
-			(!get_field(info->sbcs, DM_SBCS_SBACCESS16) || size != 2) &&
-			(!get_field(info->sbcs, DM_SBCS_SBACCESS32) || size != 4) &&
-			(!get_field(info->sbcs, DM_SBCS_SBACCESS64) || size != 8) &&
-			(!get_field(info->sbcs, DM_SBCS_SBACCESS128) || size != 16)) {
+	if (!sba_supports_access(target, size)) {
 		LOG_DEBUG("Skipping mem %s via system bus - unsupported size.",
 				read ? "read" : "write");
 		*skip_reason = "skipped (unsupported size)";
@@ -3475,7 +3643,7 @@ static int write_memory_bus_v1(struct target *target, target_addr_t address,
 
 	int result;
 
-	sb_write_address(target, next_address);
+	sb_write_address(target, next_address, true);
 	while (next_address < end_address) {
 		LOG_DEBUG("transferring burst starting at address 0x%" TARGET_PRIxADDR,
 				next_address);
@@ -3848,7 +4016,7 @@ struct target_type riscv013_target = {
 
 	.write_memory = write_memory,
 
-	.arch_state = arch_state,
+	.arch_state = arch_state
 };
 
 /*** 0.13-specific implementations of various RISC-V helper functions. ***/
diff --git a/src/target/riscv/riscv.c b/src/target/riscv/riscv.c
index 37be7d4..02ec811 100644
--- a/src/target/riscv/riscv.c
+++ b/src/target/riscv/riscv.c
@@ -15,6 +15,7 @@
 #include "jtag/jtag.h"
 #include "target/register.h"
 #include "target/breakpoints.h"
+#include "helper/base64.h"
 #include "helper/time_support.h"
 #include "riscv.h"
 #include "gdb_regs.h"
@@ -255,6 +256,21 @@ virt2phys_info_t sv48 = {
 	.pa_ppn_mask = {0x1ff, 0x1ff, 0x1ff, 0x1ffff},
 };
 
+void riscv_sample_buf_maybe_add_timestamp(struct target *target)
+{
+	RISCV_INFO(r);
+	uint32_t now = timeval_ms() & 0xffffffff;
+	if (r->sample_buf.used + 5 < r->sample_buf.size &&
+			(r->sample_buf.used == 0 || r->sample_buf.last_timestamp != now)) {
+		r->sample_buf.buf[r->sample_buf.used++] = RISCV_SAMPLE_BUF_TIMESTAMP;
+		r->sample_buf.buf[r->sample_buf.used++] = now & 0xff;
+		r->sample_buf.buf[r->sample_buf.used++] = (now >> 8) & 0xff;
+		r->sample_buf.buf[r->sample_buf.used++] = (now >> 16) & 0xff;
+		r->sample_buf.buf[r->sample_buf.used++] = (now >> 24) & 0xff;
+		r->sample_buf.last_timestamp = now;
+	}
+}
+
 static int riscv_resume_go_all_harts(struct target *target);
 
 void select_dmi_via_bscan(struct target *target)
@@ -2126,11 +2142,58 @@ int set_debug_reason(struct target *target, enum riscv_halt_reason halt_reason)
 	return ERROR_OK;
 }
 
+int sample_memory(struct target *target)
+{
+	RISCV_INFO(r);
+
+	if (!r->sample_buf.buf || !r->sample_config.enabled)
+		return ERROR_OK;
+
+	LOG_DEBUG("buf used/size: %d/%d", r->sample_buf.used, r->sample_buf.size);
+
+	uint64_t start = timeval_ms();
+	riscv_sample_buf_maybe_add_timestamp(target);
+	int result = ERROR_OK;
+	if (r->sample_memory) {
+		result = r->sample_memory(target, &r->sample_buf, &r->sample_config,
+									  start + TARGET_DEFAULT_POLLING_INTERVAL);
+		if (result != ERROR_NOT_IMPLEMENTED)
+			goto exit;
+	}
+
+	/* Default slow path. */
+	while (timeval_ms() - start < TARGET_DEFAULT_POLLING_INTERVAL) {
+		for (unsigned i = 0; i < DIM(r->sample_config.bucket); i++) {
+			if (r->sample_config.bucket[i].enabled &&
+					r->sample_buf.used + 1 + r->sample_config.bucket[i].size_bytes < r->sample_buf.size) {
+				assert(i < RISCV_SAMPLE_BUF_TIMESTAMP);
+				r->sample_buf.buf[r->sample_buf.used] = i;
+				result = riscv_read_phys_memory(
+					target, r->sample_config.bucket[i].address,
+					r->sample_config.bucket[i].size_bytes, 1,
+					r->sample_buf.buf + r->sample_buf.used + 1);
+				if (result == ERROR_OK)
+					r->sample_buf.used += 1 + r->sample_config.bucket[i].size_bytes;
+				else
+					goto exit;
+			}
+		}
+	}
+
+exit:
+	if (result != ERROR_OK) {
+		LOG_INFO("Turning off memory sampling because it failed.");
+		r->sample_config.enabled = false;
+	}
+	return result;
+}
+
 /*** OpenOCD Interface ***/
 int riscv_openocd_poll(struct target *target)
 {
 	LOG_DEBUG("polling all harts");
 	int halted_hart = -1;
+
 	if (riscv_rtos_enabled(target)) {
 		/* Check every hart for an event. */
 		for (int i = 0; i < riscv_count_harts(target); ++i) {
@@ -2171,14 +2234,12 @@ int riscv_openocd_poll(struct target *target)
 
 	} else if (target->smp) {
 		unsigned halts_discovered = 0;
-		unsigned total_targets = 0;
 		bool newly_halted[RISCV_MAX_HARTS] = {0};
 		unsigned should_remain_halted = 0;
 		unsigned should_resume = 0;
 		unsigned i = 0;
 		for (struct target_list *list = target->head; list != NULL;
 				list = list->next, i++) {
-			total_targets++;
 			struct target *t = list->target;
 			riscv_info_t *r = riscv_info(t);
 			assert(i < DIM(newly_halted));
@@ -2238,13 +2299,27 @@ int riscv_openocd_poll(struct target *target)
 			LOG_DEBUG("resume all");
 			riscv_resume(target, true, 0, 0, 0, false);
 		}
+
+		/* Sample memory if any target is running. */
+		for (struct target_list *list = target->head; list != NULL;
+				list = list->next, i++) {
+			struct target *t = list->target;
+			if (t->state == TARGET_RUNNING) {
+				sample_memory(target);
+				break;
+			}
+		}
+
 		return ERROR_OK;
 
 	} else {
 		enum riscv_poll_hart out = riscv_poll_hart(target,
 				riscv_current_hartid(target));
-		if (out == RPH_NO_CHANGE || out == RPH_DISCOVERED_RUNNING)
+		if (out == RPH_NO_CHANGE || out == RPH_DISCOVERED_RUNNING) {
+			if (target->state == TARGET_RUNNING)
+				sample_memory(target);
 			return ERROR_OK;
+		}
 		else if (out == RPH_ERROR)
 			return ERROR_FAIL;
 
@@ -2918,8 +2993,160 @@ COMMAND_HANDLER(handle_repeat_read)
 	return result;
 }
 
+COMMAND_HANDLER(handle_memory_sample_command)
+{
+	struct target *target = get_current_target(CMD_CTX);
+	RISCV_INFO(r);
+
+	if (CMD_ARGC == 0) {
+		command_print(CMD, "Memory sample configuration for %s:", target_name(target));
+		for (unsigned i = 0; i < DIM(r->sample_config.bucket); i++) {
+			if (r->sample_config.bucket[i].enabled) {
+				command_print(CMD, "bucket %d; address=0x%" TARGET_PRIxADDR "; size=%d", i,
+							  r->sample_config.bucket[i].address,
+							  r->sample_config.bucket[i].size_bytes);
+			} else {
+				command_print(CMD, "bucket %d; disabled", i);
+			}
+		}
+		return ERROR_OK;
+	}
+
+	if (CMD_ARGC < 2) {
+		LOG_ERROR("Command requires at least bucket and address arguments.");
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	if (riscv_rtos_enabled(target)) {
+		LOG_ERROR("Memory sampling is not supported with `-rtos riscv`.");
+		return ERROR_FAIL;
+	}
+
+	uint32_t bucket;
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], bucket);
+	if (bucket > DIM(r->sample_config.bucket)) {
+		LOG_ERROR("Max bucket number is %d.", (unsigned) DIM(r->sample_config.bucket));
+		return ERROR_COMMAND_ARGUMENT_INVALID;
+	}
+
+	if (!strcmp(CMD_ARGV[1], "clear")) {
+		r->sample_config.bucket[bucket].enabled = false;
+	} else {
+		COMMAND_PARSE_ADDRESS(CMD_ARGV[1], r->sample_config.bucket[bucket].address);
+
+		if (CMD_ARGC > 2) {
+			COMMAND_PARSE_NUMBER(u32, CMD_ARGV[2], r->sample_config.bucket[bucket].size_bytes);
+			if (r->sample_config.bucket[bucket].size_bytes != 4 &&
+					r->sample_config.bucket[bucket].size_bytes != 8) {
+				LOG_ERROR("Only 4-byte and 8-byte sizes are supported.");
+				return ERROR_COMMAND_ARGUMENT_INVALID;
+			}
+		} else {
+			r->sample_config.bucket[bucket].size_bytes = 4;
+		}
+
+		r->sample_config.bucket[bucket].enabled = true;
+	}
+
+	if (!r->sample_buf.buf) {
+		r->sample_buf.size = 1024 * 1024;
+		r->sample_buf.buf = malloc(r->sample_buf.size);
+	}
+
+	/* Clear the buffer when the configuration is changed. */
+	r->sample_buf.used = 0;
+
+	r->sample_config.enabled = true;
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(handle_dump_sample_buf_command)
+{
+	struct target *target = get_current_target(CMD_CTX);
+	RISCV_INFO(r);
+
+	if (CMD_ARGC > 1) {
+		LOG_ERROR("Command takes at most 1 arguments.");
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+	bool base64 = false;
+	if (CMD_ARGC > 0) {
+		if (!strcmp(CMD_ARGV[0], "base64")) {
+			base64 = true;
+		} else {
+			LOG_ERROR("Unknown argument: %s", CMD_ARGV[0]);
+			return ERROR_COMMAND_SYNTAX_ERROR;
+		}
+	}
+
+	int result = ERROR_OK;
+	if (base64) {
+		unsigned char *encoded = base64_encode(r->sample_buf.buf,
+									  r->sample_buf.used, NULL);
+		if (!encoded) {
+			LOG_ERROR("Failed base64 encode!");
+			result = ERROR_FAIL;
+			goto error;
+		}
+		command_print(CMD, "%s", encoded);
+		free(encoded);
+	} else {
+		unsigned i = 0;
+		while (i < r->sample_buf.used) {
+			uint8_t command = r->sample_buf.buf[i++];
+			if (command == RISCV_SAMPLE_BUF_TIMESTAMP) {
+				uint32_t timestamp = buf_get_u32(r->sample_buf.buf + i, 0, 32);
+				i += 4;
+				command_print(CMD, "timestamp: %u", timestamp);
+			} else if (command < DIM(r->sample_config.bucket)) {
+				command_print_sameline(CMD, "0x%" TARGET_PRIxADDR ": ",
+									   r->sample_config.bucket[command].address);
+				if (r->sample_config.bucket[command].size_bytes == 4) {
+					uint32_t value = buf_get_u32(r->sample_buf.buf + i, 0, 32);
+					i += 4;
+					command_print(CMD, "0x%08" PRIx32, value);
+				} else if (r->sample_config.bucket[command].size_bytes == 8) {
+					uint64_t value = buf_get_u64(r->sample_buf.buf + i, 0, 64);
+					i += 8;
+					command_print(CMD, "0x%016" PRIx64, value);
+				} else {
+					LOG_ERROR("Found invalid size in bucket %d: %d", command,
+							  r->sample_config.bucket[command].size_bytes);
+					result = ERROR_FAIL;
+					goto error;
+				}
+			} else {
+				LOG_ERROR("Found invalid command byte in sample buf: 0x%2x at offset 0x%x",
+					command, i - 1);
+				result = ERROR_FAIL;
+				goto error;
+			}
+		}
+	}
+
+error:
+	/* Clear the sample buffer even when there was an error. */
+	r->sample_buf.used = 0;
+	return result;
+}
+
 static const struct command_registration riscv_exec_command_handlers[] = {
 	{
+		.name = "dump_sample_buf",
+		.handler = handle_dump_sample_buf_command,
+		.mode = COMMAND_ANY,
+		.usage = "riscv dump_sample_buf [base64]",
+		.help = "Print the contents of the sample buffer, and clear the buffer."
+	},
+	{
+		.name = "memory_sample",
+		.handler = handle_memory_sample_command,
+		.mode = COMMAND_ANY,
+		.usage = "riscv memory_sample bucket address|clear [size=4]",
+		.help = "Causes OpenOCD to frequently read size bytes at the given address."
+	},
+	{
 		.name = "repeat_read",
 		.handler = handle_repeat_read,
 		.mode = COMMAND_ANY,
diff --git a/src/target/riscv/riscv.h b/src/target/riscv/riscv.h
index c9dc266..9940cfb 100644
--- a/src/target/riscv/riscv.h
+++ b/src/target/riscv/riscv.h
@@ -60,6 +60,23 @@ typedef struct {
 	unsigned custom_number;
 } riscv_reg_info_t;
 
+#define RISCV_SAMPLE_BUF_TIMESTAMP	0x80
+typedef struct {
+	uint8_t *buf;
+	unsigned used;
+	unsigned size;
+	uint32_t last_timestamp;
+} riscv_sample_buf_t;
+
+typedef struct {
+	bool enabled;
+	struct {
+		bool enabled;
+		target_addr_t address;
+		uint32_t size_bytes;
+	} bucket[16];
+} riscv_sample_config_t;
+
 typedef struct {
 	struct list_head list;
 	uint16_t low, high;
@@ -169,6 +186,11 @@ typedef struct {
 
 	int (*test_compliance)(struct target *target);
 
+	int (*sample_memory)(struct target *target,
+						 riscv_sample_buf_t *buf,
+						 riscv_sample_config_t *config,
+						 int64_t until_ms);
+
 	int (*read_memory)(struct target *target, target_addr_t address,
 			uint32_t size, uint32_t count, uint8_t *buffer, uint32_t increment);
 
@@ -211,6 +233,9 @@ typedef struct {
 	 * Custom registers are for non-standard extensions and use abstract register numbers
 	 * from range 0xc000 ... 0xffff. */
 	struct list_head expose_custom;
+
+	riscv_sample_config_t sample_config;
+	riscv_sample_buf_t sample_buf;
 } riscv_info_t;
 
 typedef struct {
diff --git a/src/target/target.c b/src/target/target.c
index 3395b2a..ca1ded3 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -158,7 +158,7 @@ static struct target_event_callback *target_event_callbacks;
 static struct target_timer_callback *target_timer_callbacks;
 LIST_HEAD(target_reset_callback_list);
 LIST_HEAD(target_trace_callback_list);
-static const int polling_interval = 100;
+static const int polling_interval = TARGET_DEFAULT_POLLING_INTERVAL;
 
 static const Jim_Nvp nvp_assert[] = {
 	{ .name = "assert", NVP_ASSERT },
@@ -674,7 +674,7 @@ static int target_process_reset(struct command_invocation *cmd, enum target_rese
 	}
 
 	/* We want any events to be processed before the prompt */
-	retval = target_call_timer_callbacks_now();
+	retval = target_call_timer_callbacks_now(NULL);
 
 	for (target = all_targets; target; target = target->next) {
 		target->type->check_reset(target);
@@ -1534,8 +1534,7 @@ int target_register_timer_callback(int (*callback)(void *priv),
 	(*callbacks_p)->time_ms = time_ms;
 	(*callbacks_p)->removed = false;
 
-	gettimeofday(&(*callbacks_p)->when, NULL);
-	timeval_add_time(&(*callbacks_p)->when, 0, time_ms * 1000);
+	(*callbacks_p)->when = timeval_ms() + time_ms;
 
 	(*callbacks_p)->priv = priv;
 	(*callbacks_p)->next = NULL;
@@ -1669,15 +1668,14 @@ int target_call_trace_callbacks(struct target *target, size_t len, uint8_t *data
 }
 
 static int target_timer_callback_periodic_restart(
-		struct target_timer_callback *cb, struct timeval *now)
+		struct target_timer_callback *cb, int64_t *now)
 {
-	cb->when = *now;
-	timeval_add_time(&cb->when, 0, cb->time_ms * 1000L);
+	cb->when = *now + cb->time_ms;
 	return ERROR_OK;
 }
 
 static int target_call_timer_callback(struct target_timer_callback *cb,
-		struct timeval *now)
+		int64_t *now)
 {
 	cb->callback(cb->priv);
 
@@ -1687,7 +1685,7 @@ static int target_call_timer_callback(struct target_timer_callback *cb,
 	return target_unregister_timer_callback(cb->callback, cb->priv);
 }
 
-static int target_call_timer_callbacks_check_time(int checktime)
+static int target_call_timer_callbacks_check_time(int64_t *next_event, int checktime)
 {
 	static bool callback_processing;
 
@@ -1699,8 +1697,9 @@ static int target_call_timer_callbacks_check_time(int checktime)
 
 	keep_alive();
 
-	struct timeval now;
-	gettimeofday(&now, NULL);
+	int64_t now = timeval_ms();
+	if (next_event)
+		*next_event = now + 1000;
 
 	/* Store an address of the place containing a pointer to the
 	 * next item; initially, that's a standalone "root of the
@@ -1716,11 +1715,14 @@ static int target_call_timer_callbacks_check_time(int checktime)
 
 		bool call_it = (*callback)->callback &&
 			((!checktime && (*callback)->type == TARGET_TIMER_TYPE_PERIODIC) ||
-			 timeval_compare(&now, &(*callback)->when) >= 0);
+			 now >= (*callback)->when);
 
 		if (call_it)
 			target_call_timer_callback(*callback, &now);
 
+		if (next_event && (*callback)->when < *next_event)
+			*next_event = (*callback)->when;
+
 		callback = &(*callback)->next;
 	}
 
@@ -1728,15 +1730,19 @@ static int target_call_timer_callbacks_check_time(int checktime)
 	return ERROR_OK;
 }
 
-int target_call_timer_callbacks(void)
+/**
+ * This function updates *next_event with the time (according to timeval_ms())
+ * that it would next need to be called in order to run all callbacks on time.
+ */
+int target_call_timer_callbacks(int64_t *next_event)
 {
-	return target_call_timer_callbacks_check_time(1);
+	return target_call_timer_callbacks_check_time(next_event, 1);
 }
 
 /* invoke periodic callbacks immediately */
-int target_call_timer_callbacks_now(void)
+int target_call_timer_callbacks_now(int64_t *next_event)
 {
-	return target_call_timer_callbacks_check_time(0);
+	return target_call_timer_callbacks_check_time(next_event, 0);
 }
 
 /* Prints the working area layout for debug purposes */
diff --git a/src/target/target.h b/src/target/target.h
index b6f8d5d..4e8897c 100644
--- a/src/target/target.h
+++ b/src/target/target.h
@@ -329,7 +329,7 @@ struct target_timer_callback {
 	unsigned int time_ms;
 	enum target_timer_type type;
 	bool removed;
-	struct timeval when;
+	int64_t when;	/* output of timeval_ms() */
 	void *priv;
 	struct target_timer_callback *next;
 };
@@ -397,12 +397,12 @@ int target_call_trace_callbacks(struct target *target, size_t len, uint8_t *data
 int target_register_timer_callback(int (*callback)(void *priv),
 		unsigned int time_ms, enum target_timer_type type, void *priv);
 int target_unregister_timer_callback(int (*callback)(void *priv), void *priv);
-int target_call_timer_callbacks(void);
+int target_call_timer_callbacks(int64_t *next_event);
 /**
  * Invoke this to ensure that e.g. polling timer callbacks happen before
  * a synchronous command completes.
  */
-int target_call_timer_callbacks_now(void);
+int target_call_timer_callbacks_now(int64_t *next_event);
 
 struct target *get_target_by_num(int num);
 struct target *get_current_target(struct command_context *cmd_ctx);
@@ -771,4 +771,6 @@ void target_handle_md_output(struct command_invocation *cmd,
 
 extern bool get_target_reset_nag(void);
 
+#define TARGET_DEFAULT_POLLING_INTERVAL		100
+
 #endif /* OPENOCD_TARGET_TARGET_H */