Merge pull request #986 from riscv/from_upstream

Merge up to 16e9b9c44fa62ea6eec99d1fb7bc43a8f1cc2f7e from upstream.

71 files changed, 3214 insertions, 449 deletions

diff --git a/HACKING b/HACKING
index 4935595..46db3b8 100644
--- a/HACKING
+++ b/HACKING
@@ -181,10 +181,6 @@ topics. It is possible because @c for/master is not a traditional Git
 branch.
   -# You will need to install this hook, we will look into a better solution:
 @code
-scp -p -P 29418 USERNAME@review.openocd.org:hooks/commit-msg .git/hooks/
-@endcode
-  Or with http only:
-@code
 wget https://review.openocd.org/tools/hooks/commit-msg
 mv commit-msg .git/hooks
 chmod +x .git/hooks/commit-msg
diff --git a/configure.ac b/configure.ac
index 341ee52..274be15 100644
--- a/configure.ac
+++ b/configure.ac
@@ -383,7 +383,7 @@ AC_ARG_ENABLE([internal-libjaylink],
   [use_internal_libjaylink=$enableval], [use_internal_libjaylink=no])
 
 AC_ARG_ENABLE([remote-bitbang],
-  AS_HELP_STRING([--enable-remote-bitbang], [Enable building support for the Remote Bitbang jtag driver]),
+  AS_HELP_STRING([--enable-remote-bitbang], [Enable building support for the Remote Bitbang driver]),
   [build_remote_bitbang=$enableval], [build_remote_bitbang=yes])
 
 AS_CASE(["${host_cpu}"],
@@ -599,9 +599,9 @@ AS_IF([test "x$use_internal_jimtcl" = "xyes"], [
 
 AS_IF([test "x$build_remote_bitbang" = "xyes"], [
   build_bitbang=yes
-  AC_DEFINE([BUILD_REMOTE_BITBANG], [1], [1 if you want the Remote Bitbang JTAG driver.])
+  AC_DEFINE([BUILD_REMOTE_BITBANG], [1], [1 if you want the Remote Bitbang driver.])
 ], [
-  AC_DEFINE([BUILD_REMOTE_BITBANG], [0], [0 if you don't want the Remote Bitbang JTAG driver.])
+  AC_DEFINE([BUILD_REMOTE_BITBANG], [0], [0 if you don't want the Remote Bitbang driver.])
 ])
 
 AS_IF([test "x$build_sysfsgpio" = "xyes"], [
diff --git a/contrib/firmware/angie/c/include/i2c.h b/contrib/firmware/angie/c/include/i2c.h
index 06185ef..d040492 100644
--- a/contrib/firmware/angie/c/include/i2c.h
+++ b/contrib/firmware/angie/c/include/i2c.h
@@ -19,6 +19,7 @@ void repeated_start(void);
 void stop_cd(void);
 void clock_cd(void);
 void send_ack(void);
+void send_nack(void);
 bool get_ack(void);
 
 uint8_t get_address(uint8_t adr, uint8_t rdwr);
diff --git a/contrib/firmware/angie/c/src/i2c.c b/contrib/firmware/angie/c/src/i2c.c
index a7004bf..5384010 100644
--- a/contrib/firmware/angie/c/src/i2c.c
+++ b/contrib/firmware/angie/c/src/i2c.c
@@ -60,6 +60,16 @@ void send_ack(void)
 	delay_us(1);
 }
 
+void send_nack(void)
+{
+	PIN_SDA = 1;
+	delay_us(1);
+	PIN_SCL = 1;
+	delay_us(1);
+	PIN_SCL = 0;
+	delay_us(1);
+}
+
 bool get_ack(void)
 {
 	PIN_SDA_DIR = 1;
diff --git a/contrib/firmware/angie/c/src/usb.c b/contrib/firmware/angie/c/src/usb.c
index 747fef1..1b7aa47 100644
--- a/contrib/firmware/angie/c/src/usb.c
+++ b/contrib/firmware/angie/c/src/usb.c
@@ -757,14 +757,13 @@ void i2c_recieve(void)
 	PIN_SDA_DIR = 0;
 	if (EP6FIFOBUF[0] == 1) {
 		uint8_t rdwr = EP6FIFOBUF[0];   //read
-		uint8_t reg_adr_check = EP6FIFOBUF[1];
-		uint8_t count = EP6FIFOBUF[2];  //request data count
+		uint8_t data_count = EP6FIFOBUF[1]; //data sent count
+		uint8_t count = EP6FIFOBUF[2];  //requested data count
 		uint8_t adr = EP6FIFOBUF[3];    //address
-		uint8_t reg_adr = EP6FIFOBUF[4];
 		uint8_t address = get_address(adr, rdwr);   //address byte (read command)
 		uint8_t address_2 = get_address(adr, 0);   //address byte 2 (write command)
 
-		printf("%d\n", address);
+		printf("%d\n", address - 1);
 
 		/*  start:   */
 		start_cd();
@@ -773,17 +772,15 @@ void i2c_recieve(void)
 		/*  ack:  */
 		uint8_t ack = get_ack();
 
-		delay_us(10);
-
 		/*   send data   */
-		if (reg_adr_check) { //if there is a byte reg
-			send_byte(reg_adr);
-			/*  ack():  */
-			ack = get_ack();
+		if (data_count) { //if there is a byte reg
+			for (uint8_t i = 0; i < data_count; i++) {
+				send_byte(EP6FIFOBUF[i + 4]);
+				/*  ack():  */
+				ack = get_ack();
+			}
 		}
 
-		delay_us(10);
-
 		/*  repeated start:  */
 		repeated_start();
 		/*  address:   */
@@ -791,23 +788,22 @@ void i2c_recieve(void)
 		/*  get ack:  */
 		ack = get_ack();
 
-		delay_us(10);
-
 		/*   receive data   */
-		for (uint8_t i = 0; i < count; i++) {
+		for (uint8_t i = 0; i < count - 1; i++) {
 			EP8FIFOBUF[i] = receive_byte();
 
-			/*  send ack:  */
+			/*  send ack: */
 			send_ack();
 		}
 
-		delay_ms(1);
+		EP8FIFOBUF[count - 1] = receive_byte();
+
+		/*  send Nack:  */
+		send_nack();
 
 		/*   stop   */
 		stop_cd();
 
-		delay_us(10);
-
 		EP8BCH = 0; //EP8
 		syncdelay(3);
 		EP8BCL = count; //EP8
diff --git a/contrib/loaders/trampoline/espressif/xtensa/Makefile b/contrib/loaders/trampoline/espressif/xtensa/Makefile
new file mode 100644
index 0000000..bd1f630
--- /dev/null
+++ b/contrib/loaders/trampoline/espressif/xtensa/Makefile
@@ -0,0 +1,38 @@
+# SPDX-License-Identifier: GPL-2.0-or-later
+
+# Espressif Xtensa Makefile to compile flasher stub wrapper
+# Copyright (C) 2023 Espressif Systems Ltd.
+
+# Prefix for Espressif xtensa cross compilers (can include a directory path)
+CROSS ?= xtensa-esp32-elf-
+
+APP_ARCH := xtensa
+APP_CHIP_PATH := $(shell pwd)
+SRCS := $(APP_CHIP_PATH)/esp_xtensa_stub_tramp_win.S
+
+BIN2C = ../../../../../src/helper/bin2char.sh
+BUILD_DIR = build
+
+APP = esp_xtensa_stub_tramp_win
+APP_OBJ = $(BUILD_DIR)/$(APP).o
+APP_BIN = $(BUILD_DIR)/$(APP).bin
+APP_CODE = $(APP).inc
+
+.PHONY: all clean
+
+all: $(BUILD_DIR) $(APP_OBJ) $(APP_CODE)
+
+$(BUILD_DIR):
+	$(Q) mkdir $@
+
+$(APP_OBJ): $(SRCS)
+	@echo "  CC   $^ -> $@"
+	$(Q) $(CROSS)gcc -c $(CFLAGS)  -o $@ $^
+
+$(APP_CODE): $(APP_OBJ)
+	@echo "  CC   $^ -> $@"
+	$(Q) $(CROSS)objcopy -O binary -j.text $^ $(APP_BIN)
+	$(Q) $(BIN2C) < $(APP_BIN) > $@
+
+clean:
+	$(Q) rm -rf $(BUILD_DIR)
diff --git a/contrib/loaders/trampoline/espressif/xtensa/esp_xtensa_stub_tramp_win.S b/contrib/loaders/trampoline/espressif/xtensa/esp_xtensa_stub_tramp_win.S
new file mode 100644
index 0000000..e0c827d
--- /dev/null
+++ b/contrib/loaders/trampoline/espressif/xtensa/esp_xtensa_stub_tramp_win.S
@@ -0,0 +1,41 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+
+/***************************************************************************
+ *   Xtensa flasher stub wrapper                                           *
+ *   Copyright (C) 2017 Espressif Systems Ltd.                             *
+ ***************************************************************************/
+
+/*
+ * Expects :
+ * a0 = zero
+ * a1 = stack_base + stack_size - 16, 16 bytes aligned
+ * a8 = address of the function to call
+ * Params :
+ * a2 = command arg0, result (out)
+ * a3 = command arg1
+ * a4 = command arg2
+ * a5 = command arg3
+ * a6 = command arg4
+ * Maximum 5 user args
+ */
+    .text
+
+    .align  4
+_stub_enter:
+    /* initialize initial stack frame for callx8 */
+    addi    a9, sp,  32 /* point 16 past extra save area */
+    s32e    a9, sp, -12 /* access to extra save area */
+    /* prepare args */
+    mov     a10, a2
+    mov     a11, a3
+    mov     a12, a4
+    mov     a13, a5
+    mov     a14, a6
+    /* call stub */
+    callx8  a8
+    /* prepare return value */
+    mov     a2, a10
+    break 0,0
+
+_idle_loop:
+    j _idle_loop
diff --git a/contrib/loaders/trampoline/espressif/xtensa/esp_xtensa_stub_tramp_win.inc b/contrib/loaders/trampoline/espressif/xtensa/esp_xtensa_stub_tramp_win.inc
new file mode 100644
index 0000000..1657223
--- /dev/null
+++ b/contrib/loaders/trampoline/espressif/xtensa/esp_xtensa_stub_tramp_win.inc
@@ -0,0 +1,3 @@
+/* Autogenerated with ../../../../../src/helper/bin2char.sh */
+0x92,0xc1,0x20,0x90,0xd1,0x49,0xad,0x02,0xbd,0x03,0xcd,0x04,0xdd,0x05,0x60,0xe6,
+0x20,0xe0,0x08,0x00,0x2d,0x0a,0x00,0x40,0x00,0x06,0xff,0xff,
diff --git a/contrib/remote_bitbang/remote_bitbang_sysfsgpio.c b/contrib/remote_bitbang/remote_bitbang_sysfsgpio.c
index 9294837..1588eb9 100644
--- a/contrib/remote_bitbang/remote_bitbang_sysfsgpio.c
+++ b/contrib/remote_bitbang/remote_bitbang_sysfsgpio.c
@@ -1,30 +1,31 @@
 // SPDX-License-Identifier: GPL-2.0-or-later
 
 /***************************************************************************
+ *   Copyright (C) 2021 by Manuel Wick <manuel@matronix.de>                *
  *   Copyright (C) 2013 Paul Fertser <fercerpav@gmail.com>                 *
  *   Copyright (C) 2012 by Creative Product Design, marc @ cpdesign.com.au *
  ***************************************************************************/
 
 /*
-  This is a test application to be used as a remote bitbang server for
-  the OpenOCD remote_bitbang interface driver.
-
-  To compile run:
-  gcc -Wall -ansi -pedantic -std=c99 -o remote_bitbang_sysfsgpio remote_bitbang_sysfsgpio.c
-
-
-  Usage example:
-
-  On Raspberry Pi run:
-  socat TCP6-LISTEN:7777,fork EXEC:"sudo ./remote_bitbang_sysfsgpio tck 11 tms 25 tdo 9 tdi 10"
-
-  On host run:
-  openocd -c "interface remote_bitbang; remote_bitbang host raspberrypi; remote_bitbang port 7777" \
-	  -f target/stm32f1x.cfg
-
-  Or if you want to test UNIX sockets, run both on Raspberry Pi:
-  socat UNIX-LISTEN:/tmp/remotebitbang-socket,fork EXEC:"sudo ./remote_bitbang_sysfsgpio tck 11 tms 25 tdo 9 tdi 10"
-  openocd -c "interface remote_bitbang; remote_bitbang host /tmp/remotebitbang-socket" -f target/stm32f1x.cfg
+ * This is a test application to be used as a remote bitbang server for
+ * the OpenOCD remote_bitbang interface driver.
+ *
+ * To compile run:
+ * gcc -Wall -ansi -pedantic -std=c99 -o remote_bitbang_sysfsgpio remote_bitbang_sysfsgpio.c
+ *
+ *
+ * Usage example:
+ *
+ * On Raspberry Pi run:
+ * socat TCP6-LISTEN:7777,fork EXEC:"sudo ./remote_bitbang_sysfsgpio tck 11 tms 25 tdo 9 tdi 10"
+ *
+ * On host run:
+ * openocd -c "adapter driver remote_bitbang; remote_bitbang host raspberrypi; remote_bitbang port 7777" \
+ *  -f target/stm32f1x.cfg
+ *
+ * Or if you want to test UNIX sockets, run both on Raspberry Pi:
+ * socat UNIX-LISTEN:/tmp/remotebitbang-socket,fork EXEC:"sudo ./remote_bitbang_sysfsgpio tck 11 tms 25 tdo 9 tdi 10"
+ * openocd -c "adapter driver remote_bitbang; remote_bitbang host /tmp/remotebitbang-socket" -f target/stm32f1x.cfg
 */
 
 #include <sys/types.h>
@@ -97,11 +98,14 @@ static void unexport_sysfs_gpio(int gpio)
  * If the gpio is an output, it is initialized according to init_high,
  * otherwise it is ignored.
  *
+ * When open_rw is set, the file descriptor will be open as read and write,
+ * e.g. for SWDIO (TMS) that is used as input and output.
+ *
  * If the gpio is already exported we just show a warning and continue; if
  * openocd happened to crash (or was killed by user) then the gpios will not
  * have been cleaned up.
  */
-static int setup_sysfs_gpio(int gpio, int is_output, int init_high)
+static int setup_sysfs_gpio(int gpio, int is_output, int init_high, int open_rw)
 {
 	char buf[40];
 	char gpiostr[4];
@@ -132,7 +136,9 @@ static int setup_sysfs_gpio(int gpio, int is_output, int init_high)
 	}
 
 	snprintf(buf, sizeof(buf), "/sys/class/gpio/gpio%d/value", gpio);
-	if (is_output)
+	if (open_rw)
+		ret = open(buf, O_RDWR | O_NONBLOCK | O_SYNC);
+	else if (is_output)
 		ret = open(buf, O_WRONLY | O_NONBLOCK | O_SYNC);
 	else
 		ret = open(buf, O_RDONLY | O_NONBLOCK | O_SYNC);
@@ -144,6 +150,37 @@ static int setup_sysfs_gpio(int gpio, int is_output, int init_high)
 }
 
 /*
+ * Change direction for gpio.
+ */
+static int change_dir_sysfs_gpio(int gpio, int is_output, int init_high)
+{
+	char buf[40];
+	int ret;
+
+	if (!is_gpio_valid(gpio))
+		return ERROR_OK;
+
+	snprintf(buf, sizeof(buf), "/sys/class/gpio/gpio%d/direction", gpio);
+	ret = open_write_close(buf, is_output ? (init_high ? "high" : "low") : "in");
+	if (ret < 0) {
+		LOG_ERROR("Couldn't set direction for gpio %d", gpio);
+		perror("sysfsgpio: ");
+		unexport_sysfs_gpio(gpio);
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
+/* gpio numbers for each gpio. Negative values are invalid */
+static int tck_gpio = -1;
+static int tms_gpio = -1;
+static int tdi_gpio = -1;
+static int tdo_gpio = -1;
+static int trst_gpio = -1;
+static int srst_gpio = -1;
+
+/*
  * file descriptors for /sys/class/gpio/gpioXX/value
  * Set up during init.
  */
@@ -155,6 +192,15 @@ static int trst_fd = -1;
 static int srst_fd = -1;
 
 /*
+ * GPIO state of /sys/class/gpio/gpioXX/value
+ */
+static int last_tck = -1;
+static int last_tms = -1;
+static int last_tms_drive = -1;
+static int last_tdi = -1;
+static int last_initialized = -1;
+
+/*
  * Bitbang interface read of TDO
  *
  * The sysfs value will read back either '0' or '1'. The trick here is to call
@@ -179,26 +225,22 @@ static int sysfsgpio_read(void)
 /*
  * Bitbang interface write of TCK, TMS, TDI
  *
- * Seeing as this is the only function where the outputs are changed,
- * we can cache the old value to avoid needlessly writing it.
+ * Output states are changed here and in sysfsgpio_write_swd,
+ * which are not used simultaneously, so we can cache the old
+ * value to avoid needlessly writing it.
  */
 static void sysfsgpio_write(int tck, int tms, int tdi)
 {
 	const char one[] = "1";
 	const char zero[] = "0";
 
-	static int last_tck;
-	static int last_tms;
-	static int last_tdi;
-
-	static int first_time;
 	size_t bytes_written;
 
-	if (!first_time) {
+	if (!last_initialized) {
 		last_tck = !tck;
 		last_tms = !tms;
 		last_tdi = !tdi;
-		first_time = 1;
+		last_initialized = 1;
 	}
 
 	if (tdi != last_tdi) {
@@ -251,13 +293,81 @@ static void sysfsgpio_reset(int trst, int srst)
 	}
 }
 
-/* gpio numbers for each gpio. Negative values are invalid */
-static int tck_gpio = -1;
-static int tms_gpio = -1;
-static int tdi_gpio = -1;
-static int tdo_gpio = -1;
-static int trst_gpio = -1;
-static int srst_gpio = -1;
+/*
+ * Bitbang interface set direction of SWDIO (TMS)
+ */
+static void sysfsgpio_swdio_drive(int is_output)
+{
+	int ret;
+
+	if (is_output != 0 && last_tms == -1)
+		last_tms = 0;
+
+	ret = change_dir_sysfs_gpio(tms_gpio, (is_output != 0) ? 1 : 0, last_tms);
+	if (ret != ERROR_OK)
+		LOG_WARNING("Failed to change SWDIO (TMS) direction to output");
+	else
+		last_tms_drive = (is_output != 0) ? 1 : 0;
+}
+
+/*
+ * Bitbang interface read of SWDIO (TMS)
+ *
+ * The sysfs value will read back either '0' or '1'. The trick here is to call
+ * lseek to bypass buffering in the sysfs kernel driver.
+ */
+static int sysfsgpio_swdio_read(void)
+{
+	char buf[1];
+
+	/* important to seek to signal sysfs of new read */
+	lseek(tms_fd, 0, SEEK_SET);
+	int ret = read(tms_fd, &buf, sizeof(buf));
+
+	if (ret < 0) {
+		LOG_WARNING("reading swdio (tms) failed");
+		return 0;
+	}
+
+	return buf[0];
+}
+
+/*
+ * Bitbang interface write of SWCLK (TCK) and SWDIO (TMS)
+ *
+ * Output states are changed here and in sysfsgpio_write, which
+ * are not used simultaneously, so we can cache the old value
+ * to avoid needlessly writing it.
+ */
+static void sysfsgpio_swd_write(int swclk, int swdio)
+{
+	static const char one[] = "1";
+	static const char zero[] = "0";
+
+	size_t bytes_written;
+
+	if (!last_initialized) {
+		last_tck = !swclk;
+		last_tms = !swdio;
+		last_initialized = 1;
+	}
+
+	if (last_tms_drive == 1 && swdio != last_tms) {
+		bytes_written = write(tms_fd, swdio ? &one : &zero, 1);
+		if (bytes_written != 1)
+			LOG_WARNING("writing swdio (tms) failed");
+	}
+
+	/* write clk last */
+	if (swclk != last_tck) {
+		bytes_written = write(tck_fd, swclk ? &one : &zero, 1);
+		if (bytes_written != 1)
+			LOG_WARNING("writing swclk (tck) failed");
+	}
+
+	last_tms = swdio;
+	last_tck = swclk;
+}
 
 /* helper func to close and cleanup files only if they were valid/ used */
 static void cleanup_fd(int fd, int gpio)
@@ -293,13 +403,21 @@ static void process_remote_protocol(void)
 			char d = c - 'r';
 			sysfsgpio_reset(!!(d & 2),
 					(d & 1));
-		} else if (c >= '0' && c <= '0' + 7) {/* Write */
+		} else if (c >= '0' && c <= '0' + 7) { /* Write */
 			char d = c - '0';
 			sysfsgpio_write(!!(d & 4),
 					!!(d & 2),
 					(d & 1));
 		} else if (c == 'R')
 			putchar(sysfsgpio_read());
+		else if (c == 'c') /* SWDIO read */
+			putchar(sysfsgpio_swdio_read());
+		else if (c == 'o' || c == 'O') /* SWDIO drive */
+			sysfsgpio_swdio_drive(c == 'o' ? 0 : 1);
+		else if (c >= 'd' && c <= 'g') { /* SWD write */
+			char d = c - 'd';
+			sysfsgpio_swd_write((d & 2), (d & 1));
+		}
 		else
 			LOG_ERROR("Unknown command '%c' received", c);
 	}
@@ -307,7 +425,7 @@ static void process_remote_protocol(void)
 
 int main(int argc, char *argv[])
 {
-	LOG_WARNING("SysfsGPIO remote_bitbang JTAG driver\n");
+	LOG_WARNING("SysfsGPIO remote_bitbang JTAG+SWD driver\n");
 
 	for (int i = 1; i < argc; i++) {
 		if (!strcmp(argv[i], "tck"))
@@ -349,36 +467,39 @@ int main(int argc, char *argv[])
 	 * Configure TDO as an input, and TDI, TCK, TMS, TRST, SRST
 	 * as outputs.  Drive TDI and TCK low, and TMS/TRST/SRST high.
 	 */
-	tck_fd = setup_sysfs_gpio(tck_gpio, 1, 0);
+	tck_fd = setup_sysfs_gpio(tck_gpio, 1, 0, 0);
 	if (tck_fd < 0)
 		goto out_error;
 
-	tms_fd = setup_sysfs_gpio(tms_gpio, 1, 1);
+	tms_fd = setup_sysfs_gpio(tms_gpio, 1, 1, 1);
 	if (tms_fd < 0)
 		goto out_error;
+	last_tms_drive = 0;
 
-	tdi_fd = setup_sysfs_gpio(tdi_gpio, 1, 0);
+	tdi_fd = setup_sysfs_gpio(tdi_gpio, 1, 0, 0);
 	if (tdi_fd < 0)
 		goto out_error;
 
-	tdo_fd = setup_sysfs_gpio(tdo_gpio, 0, 0);
+	tdo_fd = setup_sysfs_gpio(tdo_gpio, 0, 0, 0);
 	if (tdo_fd < 0)
 		goto out_error;
 
 	/* assume active low */
 	if (trst_gpio > 0) {
-		trst_fd = setup_sysfs_gpio(trst_gpio, 1, 1);
+		trst_fd = setup_sysfs_gpio(trst_gpio, 1, 1, 0);
 		if (trst_fd < 0)
 			goto out_error;
 	}
 
 	/* assume active low */
 	if (srst_gpio > 0) {
-		srst_fd = setup_sysfs_gpio(srst_gpio, 1, 1);
+		srst_fd = setup_sysfs_gpio(srst_gpio, 1, 1, 0);
 		if (srst_fd < 0)
 			goto out_error;
 	}
 
+	last_initialized = 0;
+
 	LOG_WARNING("SysfsGPIO nums: tck = %d, tms = %d, tdi = %d, tdo = %d",
 		 tck_gpio, tms_gpio, tdi_gpio, tdo_gpio);
 	LOG_WARNING("SysfsGPIO num: srst = %d", srst_gpio);
diff --git a/doc/manual/jtag/drivers/remote_bitbang.txt b/doc/manual/jtag/drivers/remote_bitbang.txt
index f394d73..7c8eee2 100644
--- a/doc/manual/jtag/drivers/remote_bitbang.txt
+++ b/doc/manual/jtag/drivers/remote_bitbang.txt
@@ -1,15 +1,19 @@
 /** @remote_bitbangpage OpenOCD Developer's Guide
 
-The remote_bitbang JTAG driver is used to drive JTAG from a remote process. The
-remote_bitbang driver communicates via TCP or UNIX sockets with some remote
-process using an ASCII encoding of the bitbang interface. The remote process
-presumably then drives the JTAG however it pleases. The remote process should
-act as a server, listening for connections from the openocd remote_bitbang
-driver.
+The remote_bitbang JTAG+SWD driver is used to drive JTAG and/or SWD from a
+remote process. The remote_bitbang driver communicates via TCP or UNIX
+sockets with some remote process using an ASCII encoding of the bitbang
+interface. The remote process presumably then drives the JTAG/SWD however
+it pleases. The remote process should act as a server, listening for
+connections from the openocd remote_bitbang driver.
 
 The remote bitbang driver is useful for debugging software running on
 processors which are being simulated.
 
+There also is an implementation of the server-side protocol for the
+Glasgow Debug Tool (https://github.com/glasgowEmbedded/Glasgow) through
+the jtag-openocd applet.
+
 The bitbang interface consists of the following functions.
 
 blink on
@@ -24,11 +28,20 @@ write tck tms tdi
 reset trst srst
 	Set the value of trst, srst.
 
+swdio_drive
+	Set the output enable of the bidirectional swdio (tms) pin
+
+swdio_read
+	Sample the value of swdio (tms).
+
+swd_write
+	Set the value of swclk (tck) and swdio (tms).
+
 An additional function, quit, is added to the remote_bitbang interface to
 indicate there will be no more requests and the connection with the remote
 driver should be closed.
 
-These five functions are encoded in ASCII by assigning a single character to
+These eight functions are encoded in ASCII by assigning a single character to
 each possible request. The assignments are:
 
 	B - Blink on
@@ -47,7 +60,14 @@ each possible request. The assignments are:
 	s - Reset 0 1
 	t - Reset 1 0
 	u - Reset 1 1
+	O - SWDIO drive 1
+	o - SWDIO drive 0
+	c - SWDIO read request
+	d - SWD write 0 0
+	e - SWD write 0 1
+	f - SWD write 1 0
+	g - SWD write 1 1
 
-The read response is encoded in ASCII as either digit 0 or 1.
+The read responses are encoded in ASCII as either digit 0 or 1.
 
  */
diff --git a/doc/openocd.texi b/doc/openocd.texi
index 1312c5f..1f145b9 100644
--- a/doc/openocd.texi
+++ b/doc/openocd.texi
@@ -2543,32 +2543,44 @@ and a specific set of GPIOs is used.
 ARM CMSIS-DAP compliant based adapter v1 (USB HID based)
 or v2 (USB bulk).
 
-@deffn {Config Command} {cmsis_dap_vid_pid} [vid pid]+
+@deffn {Config Command} {cmsis-dap vid_pid} [vid pid]+
 The vendor ID and product ID of the CMSIS-DAP device. If not specified
 the driver will attempt to auto detect the CMSIS-DAP device.
 Currently, up to eight [@var{vid}, @var{pid}] pairs may be given, e.g.
 @example
-cmsis_dap_vid_pid 0xc251 0xf001 0x0d28 0x0204
+cmsis-dap vid_pid 0xc251 0xf001 0x0d28 0x0204
 @end example
 @end deffn
 
-@deffn {Config Command} {cmsis_dap_backend} [@option{auto}|@option{usb_bulk}|@option{hid}]
+@deffn {Config Command} {cmsis-dap backend} [@option{auto}|@option{usb_bulk}|@option{hid}]
 Specifies how to communicate with the adapter:
 
 @itemize @minus
 @item @option{hid} Use HID generic reports - CMSIS-DAP v1
 @item @option{usb_bulk} Use USB bulk - CMSIS-DAP v2
 @item @option{auto} First try USB bulk CMSIS-DAP v2, if not found try HID CMSIS-DAP v1.
-This is the default if @command{cmsis_dap_backend} is not specified.
+This is the default if @command{cmsis-dap backend} is not specified.
 @end itemize
 @end deffn
 
-@deffn {Config Command} {cmsis_dap_usb interface} [number]
+@deffn {Config Command} {cmsis-dap usb interface} [number]
 Specifies the @var{number} of the USB interface to use in v2 mode (USB bulk).
 In most cases need not to be specified and interfaces are searched by
 interface string or for user class interface.
 @end deffn
 
+@deffn {Command} {cmsis-dap quirk} [@option{enable}|@option{disable}]
+Enables or disables the following workarounds of known CMSIS-DAP adapter
+quirks:
+@itemize @minus
+@item disconnect and re-connect before sending a switch sequence
+@item packets pipelining is suppressed, only one packet at a time is
+submitted to the adapter
+@end itemize
+The quirk workarounds are disabled by default.
+The command without a parameter displays current setting.
+@end deffn
+
 @deffn {Command} {cmsis-dap info}
 Display various device information, like hardware version, firmware version, current bus status.
 @end deffn
@@ -2839,9 +2851,9 @@ If not specified, default 0xFFFF is used.
 @end deffn
 
 @deffn {Interface Driver} {remote_bitbang}
-Drive JTAG from a remote process. This sets up a UNIX or TCP socket connection
-with a remote process and sends ASCII encoded bitbang requests to that process
-instead of directly driving JTAG.
+Drive JTAG and SWD from a remote process. This sets up a UNIX or TCP socket
+connection with a remote process and sends ASCII encoded bitbang requests to
+that process instead of directly driving JTAG and SWD.
 
 The remote_bitbang driver is useful for debugging software running on
 processors which are being simulated.
@@ -3020,7 +3032,7 @@ This driver is for Cypress Semiconductor's KitProg adapters. The KitProg is an
 SWD-only adapter that is designed to be used with Cypress's PSoC and PRoC device
 families, but it is possible to use it with some other devices. If you are using
 this adapter with a PSoC or a PRoC, you may need to add
-@command{kitprog_init_acquire_psoc} or @command{kitprog acquire_psoc} to your
+@command{kitprog init_acquire_psoc} or @command{kitprog acquire_psoc} to your
 configuration script.
 
 Note that this driver is for the proprietary KitProg protocol, not the CMSIS-DAP
@@ -3041,14 +3053,14 @@ versions only implement "SWD line reset". Second, due to a firmware quirk, an
 SWD sequence must be sent after every target reset in order to re-establish
 communications with the target.
 @item Due in part to the limitation above, KitProg devices with firmware below
-version 2.14 will need to use @command{kitprog_init_acquire_psoc} in order to
+version 2.14 will need to use @command{kitprog init_acquire_psoc} in order to
 communicate with PSoC 5LP devices. This is because, assuming debug is not
 disabled on the PSoC, the PSoC 5LP needs its JTAG interface switched to SWD
 mode before communication can begin, but prior to firmware 2.14, "JTAG to SWD"
 could only be sent with an acquisition sequence.
 @end itemize
 
-@deffn {Config Command} {kitprog_init_acquire_psoc}
+@deffn {Config Command} {kitprog init_acquire_psoc}
 Indicate that a PSoC acquisition sequence needs to be run during adapter init.
 Please be aware that the acquisition sequence hard-resets the target.
 @end deffn
@@ -9403,7 +9415,7 @@ Remove the breakpoint at @var{address} or all breakpoints.
 Remove data watchpoint on @var{address} or all watchpoints.
 @end deffn
 
-@deffn {Command} {wp} [address len [(@option{r}|@option{w}|@option{a}) [value [mask]]]]
+@deffn {Command} {wp} [address length [(@option{r}|@option{w}|@option{a}) [value [mask]]]]
 With no parameters, lists all active watchpoints.
 Else sets a data watchpoint on data from @var{address} for @var{length} bytes.
 The watch point is an "access" watchpoint unless
@@ -9477,8 +9489,9 @@ Return a list of all channels and their properties as Tcl list.
 The list can be manipulated easily from within scripts.
 @end deffn
 
-@deffn {Command} {rtt server start} port channel
-Start a TCP server on @var{port} for the channel @var{channel}.
+@deffn {Command} {rtt server start} port channel [message]
+Start a TCP server on @var{port} for the channel @var{channel}. When
+@var{message} is not empty, it will be sent to a client when it connects.
 @end deffn
 
 @deffn {Command} {rtt server stop} port
@@ -9508,7 +9521,7 @@ TCP/IP port 9090.
 @deffn {Command} {profile} seconds filename [start end]
 Profiling samples the CPU's program counter as quickly as possible,
 which is useful for non-intrusive stochastic profiling.
-Saves up to 10000 samples in @file{filename} using ``gmon.out''
+Saves up to 1000000 samples in @file{filename} using ``gmon.out''
 format. Optional @option{start} and @option{end} parameters allow to
 limit the address range.
 @end deffn
@@ -11590,16 +11603,18 @@ NOTE: @file{xtensa-core-XXX.cfg} must match the target Xtensa hardware
 connected to OpenOCD.
 
 Some example Xtensa configurations are bundled with OpenOCD for reference:
-@itemize @bullet
+@enumerate
 @item Cadence Palladium VDebug emulation target. The user can combine their
 @file{xtensa-core-XXX.cfg} with the provided
 @file{board/xtensa-palladium-vdebug.cfg} to debug an emulated Xtensa RTL design.
-@item NXP MIMXRT685-EVK evaluation kit. The relevant configuration files are
-@file{board/xtensa-rt685-jlink.cfg} and @file{board/xtensa-core-nxp_rt600.cfg}.
-Additional information is provided by
-@uref{https://www.nxp.com/design/development-boards/i-mx-evaluation-and-development-boards/i-mx-rt600-evaluation-kit:MIMXRT685-EVK,
-NXP}.
+@item NXP MIMXRT685-EVK evaluation kit. The relevant configuration files are:
+@itemize @bullet
+@item @file{board/xtensa-rt685-ext.cfg}
+@item @file{target/xtensa-core-nxp_rt600.cfg}
 @end itemize
+Additional information is available by searching for "i.MX RT600 Evaluation Kit"
+on @url{https://www.nxp.com}.
+@end enumerate
 
 @subsection Xtensa Configuration Commands
 
@@ -11624,6 +11639,11 @@ others may be common to both but have different valid ranges.
 Configure Xtensa target memory. Memory type determines access rights,
 where RAMs are read/write while ROMs are read-only. @var{baseaddr} and
 @var{bytes} are both integers, typically hexadecimal and decimal, respectively.
+
+NOTE: Some Xtensa memory types, such as system RAM/ROM or MMIO/device regions,
+can be added or modified after the Xtensa core has been generated. Additional
+@code{xtensa xtmem} definitions should be manually added to xtensa-core-XXX.cfg
+to keep OpenOCD's target address map consistent with the Xtensa configuration.
 @end deffn
 
 @deffn {Config Command} {xtensa xtmem} (@option{icache}|@option{dcache}) linebytes cachebytes ways [writeback]
@@ -11706,6 +11726,12 @@ Execute arbitrary instruction(s) provided as an ascii string.  The string repres
 number of instruction bytes, thus its length must be even.
 @end deffn
 
+@deffn {Command} {xtensa dm} (address) [value]
+Read or write Xtensa Debug Module (DM) registers.  @var{address} is required for both reads
+and writes and is a 4-byte-aligned value typically between 0 and 0x3ffc.  @var{value} is specified
+only for write accesses.
+@end deffn
+
 @subsection Xtensa Performance Monitor Configuration
 
 @deffn {Command} {xtensa perfmon_enable} <counter_id> <select> [mask] [kernelcnt] [tracelevel]
diff --git a/doc/usb_adapters/cmsis_dap/03eb_2111_atmel_edbg.txt b/doc/usb_adapters/cmsis_dap/03eb_2111_atmel_edbg.txt
new file mode 100644
index 0000000..e4dd6ce
--- /dev/null
+++ b/doc/usb_adapters/cmsis_dap/03eb_2111_atmel_edbg.txt
@@ -0,0 +1,211 @@
+# SPDX-License-Identifier: GPL-2.0-or-later OR GFDL-1.2-no-invariants-or-later
+
+# Optional comment
+
+Bus 001 Device 006: ID 03eb:2111 Atmel Corp. Xplained Pro board debugger and programmer
+Device Descriptor:
+  bLength                18
+  bDescriptorType         1
+  bcdUSB               2.00
+  bDeviceClass          239 Miscellaneous Device
+  bDeviceSubClass         2
+  bDeviceProtocol         1 Interface Association
+  bMaxPacketSize0        64
+  idVendor           0x03eb Atmel Corp.
+  idProduct          0x2111 Xplained Pro board debugger and programmer
+  bcdDevice            1.01
+  iManufacturer           1 Atmel Corp.
+  iProduct                2 EDBG CMSIS-DAP
+  iSerial                 3 ATMLxxxxxxxxxxxxxxxx
+  bNumConfigurations      1
+  Configuration Descriptor:
+    bLength                 9
+    bDescriptorType         2
+    wTotalLength       0x0082
+    bNumInterfaces          4
+    bConfigurationValue     1
+    iConfiguration          0
+    bmAttributes         0x80
+      (Bus Powered)
+    MaxPower              500mA
+    Interface Descriptor:
+      bLength                 9
+      bDescriptorType         4
+      bInterfaceNumber        0
+      bAlternateSetting       0
+      bNumEndpoints           2
+      bInterfaceClass         3 Human Interface Device
+      bInterfaceSubClass      0
+      bInterfaceProtocol      0
+      iInterface              4 EDBG CMSIS-DAP
+        HID Device Descriptor:
+          bLength                 9
+          bDescriptorType        33
+          bcdHID               1.11
+          bCountryCode            0 Not supported
+          bNumDescriptors         1
+          bDescriptorType        34 Report
+          wDescriptorLength      35
+          Report Descriptor: (length is 35)
+            Item(Global): Usage Page, data= [ 0x00 0xff ] 65280
+                            (null)
+            Item(Local ): Usage, data= [ 0x01 ] 1
+                            (null)
+            Item(Main  ): Collection, data= [ 0x01 ] 1
+                            Application
+            Item(Global): Logical Minimum, data= [ 0x00 ] 0
+            Item(Global): Logical Maximum, data= [ 0xff 0x00 ] 255
+            Item(Global): Report Size, data= [ 0x08 ] 8
+            Item(Global): Report Count, data= [ 0x00 0x02 ] 512
+            Item(Local ): Usage, data= [ 0x01 ] 1
+                            (null)
+            Item(Main  ): Input, data= [ 0x02 ] 2
+                            Data Variable Absolute No_Wrap Linear
+                            Preferred_State No_Null_Position Non_Volatile Bitfield
+            Item(Global): Report Count, data= [ 0x00 0x02 ] 512
+            Item(Local ): Usage, data= [ 0x01 ] 1
+                            (null)
+            Item(Main  ): Output, data= [ 0x02 ] 2
+                            Data Variable Absolute No_Wrap Linear
+                            Preferred_State No_Null_Position Non_Volatile Bitfield
+            Item(Global): Report Count, data= [ 0x04 ] 4
+            Item(Local ): Usage, data= [ 0x01 ] 1
+                            (null)
+            Item(Main  ): Feature, data= [ 0x02 ] 2
+                            Data Variable Absolute No_Wrap Linear
+                            Preferred_State No_Null_Position Non_Volatile Bitfield
+            Item(Main  ): End Collection, data=none
+      Endpoint Descriptor:
+        bLength                 7
+        bDescriptorType         5
+        bEndpointAddress     0x01  EP 1 OUT
+        bmAttributes            3
+          Transfer Type            Interrupt
+          Synch Type               None
+          Usage Type               Data
+        wMaxPacketSize     0x0200  1x 512 bytes
+        bInterval               1
+      Endpoint Descriptor:
+        bLength                 7
+        bDescriptorType         5
+        bEndpointAddress     0x82  EP 2 IN
+        bmAttributes            3
+          Transfer Type            Interrupt
+          Synch Type               None
+          Usage Type               Data
+        wMaxPacketSize     0x0200  1x 512 bytes
+        bInterval               1
+    Interface Association:
+      bLength                 8
+      bDescriptorType        11
+      bFirstInterface         1
+      bInterfaceCount         2
+      bFunctionClass          2 Communications
+      bFunctionSubClass       2 Abstract (modem)
+      bFunctionProtocol       1 AT-commands (v.25ter)
+      iFunction               6 EDBG Virtual COM Port
+    Interface Descriptor:
+      bLength                 9
+      bDescriptorType         4
+      bInterfaceNumber        1
+      bAlternateSetting       0
+      bNumEndpoints           1
+      bInterfaceClass         2 Communications
+      bInterfaceSubClass      2 Abstract (modem)
+      bInterfaceProtocol      1 AT-commands (v.25ter)
+      iInterface              0
+      CDC Header:
+        bcdCDC               1.10
+      CDC ACM:
+        bmCapabilities       0x06
+          sends break
+          line coding and serial state
+      CDC Union:
+        bMasterInterface        1
+        bSlaveInterface         2
+      CDC Call Management:
+        bmCapabilities       0x03
+          call management
+          use DataInterface
+        bDataInterface          2
+      Endpoint Descriptor:
+        bLength                 7
+        bDescriptorType         5
+        bEndpointAddress     0x83  EP 3 IN
+        bmAttributes            3
+          Transfer Type            Interrupt
+          Synch Type               None
+          Usage Type               Data
+        wMaxPacketSize     0x0040  1x 64 bytes
+        bInterval               8
+    Interface Descriptor:
+      bLength                 9
+      bDescriptorType         4
+      bInterfaceNumber        2
+      bAlternateSetting       0
+      bNumEndpoints           2
+      bInterfaceClass        10 CDC Data
+      bInterfaceSubClass      0
+      bInterfaceProtocol      0
+      iInterface              0
+      Endpoint Descriptor:
+        bLength                 7
+        bDescriptorType         5
+        bEndpointAddress     0x84  EP 4 IN
+        bmAttributes            2
+          Transfer Type            Bulk
+          Synch Type               None
+          Usage Type               Data
+        wMaxPacketSize     0x0040  1x 64 bytes
+        bInterval               0
+      Endpoint Descriptor:
+        bLength                 7
+        bDescriptorType         5
+        bEndpointAddress     0x05  EP 5 OUT
+        bmAttributes            2
+          Transfer Type            Bulk
+          Synch Type               None
+          Usage Type               Data
+        wMaxPacketSize     0x0040  1x 64 bytes
+        bInterval               0
+    Interface Descriptor:
+      bLength                 9
+      bDescriptorType         4
+      bInterfaceNumber        3
+      bAlternateSetting       0
+      bNumEndpoints           2
+      bInterfaceClass       255 Vendor Specific Class
+      bInterfaceSubClass    255 Vendor Specific Subclass
+      bInterfaceProtocol    255 Vendor Specific Protocol
+      iInterface              5 EDBG Data Gateway
+      Endpoint Descriptor:
+        bLength                 7
+        bDescriptorType         5
+        bEndpointAddress     0x87  EP 7 IN
+        bmAttributes            2
+          Transfer Type            Bulk
+          Synch Type               None
+          Usage Type               Data
+        wMaxPacketSize     0x0200  1x 512 bytes
+        bInterval             255
+      Endpoint Descriptor:
+        bLength                 7
+        bDescriptorType         5
+        bEndpointAddress     0x06  EP 6 OUT
+        bmAttributes            2
+          Transfer Type            Bulk
+          Synch Type               None
+          Usage Type               Data
+        wMaxPacketSize     0x0200  1x 512 bytes
+        bInterval             255
+Device Qualifier (for other device speed):
+  bLength                10
+  bDescriptorType         6
+  bcdUSB               2.00
+  bDeviceClass          239 Miscellaneous Device
+  bDeviceSubClass         2
+  bDeviceProtocol         1 Interface Association
+  bMaxPacketSize0        64
+  bNumConfigurations      1
+Device Status:     0x0000
+  (Bus Powered)
diff --git a/doc/usb_adapters/cmsis_dap/03eb_2169_atmel_edbg.txt b/doc/usb_adapters/cmsis_dap/03eb_2169_atmel_edbg.txt
new file mode 100644
index 0000000..1f7f26e
--- /dev/null
+++ b/doc/usb_adapters/cmsis_dap/03eb_2169_atmel_edbg.txt
@@ -0,0 +1,211 @@
+# SPDX-License-Identifier: GPL-2.0-or-later OR GFDL-1.2-no-invariants-or-later
+
+# Optional comment
+
+Bus 001 Device 005: ID 03eb:2169 Atmel Corp. EDBG CMSIS-DAP
+Device Descriptor:
+  bLength                18
+  bDescriptorType         1
+  bcdUSB               2.00
+  bDeviceClass          239 Miscellaneous Device
+  bDeviceSubClass         2
+  bDeviceProtocol         1 Interface Association
+  bMaxPacketSize0        64
+  idVendor           0x03eb Atmel Corp.
+  idProduct          0x2169
+  bcdDevice            1.01
+  iManufacturer           1 Atmel Corp.
+  iProduct                2 EDBG CMSIS-DAP
+  iSerial                 3 ATMLxxxxxxxxxxxxxxxx
+  bNumConfigurations      1
+  Configuration Descriptor:
+    bLength                 9
+    bDescriptorType         2
+    wTotalLength       0x0082
+    bNumInterfaces          4
+    bConfigurationValue     1
+    iConfiguration          0
+    bmAttributes         0x80
+      (Bus Powered)
+    MaxPower              500mA
+    Interface Descriptor:
+      bLength                 9
+      bDescriptorType         4
+      bInterfaceNumber        0
+      bAlternateSetting       0
+      bNumEndpoints           2
+      bInterfaceClass         3 Human Interface Device
+      bInterfaceSubClass      0
+      bInterfaceProtocol      0
+      iInterface              4 EDBG CMSIS-DAP
+        HID Device Descriptor:
+          bLength                 9
+          bDescriptorType        33
+          bcdHID               1.11
+          bCountryCode            0 Not supported
+          bNumDescriptors         1
+          bDescriptorType        34 Report
+          wDescriptorLength      35
+          Report Descriptor: (length is 35)
+            Item(Global): Usage Page, data= [ 0x00 0xff ] 65280
+                            (null)
+            Item(Local ): Usage, data= [ 0x01 ] 1
+                            (null)
+            Item(Main  ): Collection, data= [ 0x01 ] 1
+                            Application
+            Item(Global): Logical Minimum, data= [ 0x00 ] 0
+            Item(Global): Logical Maximum, data= [ 0xff 0x00 ] 255
+            Item(Global): Report Size, data= [ 0x08 ] 8
+            Item(Global): Report Count, data= [ 0x00 0x02 ] 512
+            Item(Local ): Usage, data= [ 0x01 ] 1
+                            (null)
+            Item(Main  ): Input, data= [ 0x02 ] 2
+                            Data Variable Absolute No_Wrap Linear
+                            Preferred_State No_Null_Position Non_Volatile Bitfield
+            Item(Global): Report Count, data= [ 0x00 0x02 ] 512
+            Item(Local ): Usage, data= [ 0x01 ] 1
+                            (null)
+            Item(Main  ): Output, data= [ 0x02 ] 2
+                            Data Variable Absolute No_Wrap Linear
+                            Preferred_State No_Null_Position Non_Volatile Bitfield
+            Item(Global): Report Count, data= [ 0x04 ] 4
+            Item(Local ): Usage, data= [ 0x01 ] 1
+                            (null)
+            Item(Main  ): Feature, data= [ 0x02 ] 2
+                            Data Variable Absolute No_Wrap Linear
+                            Preferred_State No_Null_Position Non_Volatile Bitfield
+            Item(Main  ): End Collection, data=none
+      Endpoint Descriptor:
+        bLength                 7
+        bDescriptorType         5
+        bEndpointAddress     0x01  EP 1 OUT
+        bmAttributes            3
+          Transfer Type            Interrupt
+          Synch Type               None
+          Usage Type               Data
+        wMaxPacketSize     0x0200  1x 512 bytes
+        bInterval               1
+      Endpoint Descriptor:
+        bLength                 7
+        bDescriptorType         5
+        bEndpointAddress     0x82  EP 2 IN
+        bmAttributes            3
+          Transfer Type            Interrupt
+          Synch Type               None
+          Usage Type               Data
+        wMaxPacketSize     0x0200  1x 512 bytes
+        bInterval               1
+    Interface Association:
+      bLength                 8
+      bDescriptorType        11
+      bFirstInterface         1
+      bInterfaceCount         2
+      bFunctionClass          2 Communications
+      bFunctionSubClass       2 Abstract (modem)
+      bFunctionProtocol       1 AT-commands (v.25ter)
+      iFunction               6 EDBG Virtual COM Port
+    Interface Descriptor:
+      bLength                 9
+      bDescriptorType         4
+      bInterfaceNumber        1
+      bAlternateSetting       0
+      bNumEndpoints           1
+      bInterfaceClass         2 Communications
+      bInterfaceSubClass      2 Abstract (modem)
+      bInterfaceProtocol      1 AT-commands (v.25ter)
+      iInterface              0
+      CDC Header:
+        bcdCDC               1.10
+      CDC ACM:
+        bmCapabilities       0x06
+          sends break
+          line coding and serial state
+      CDC Union:
+        bMasterInterface        1
+        bSlaveInterface         2
+      CDC Call Management:
+        bmCapabilities       0x03
+          call management
+          use DataInterface
+        bDataInterface          2
+      Endpoint Descriptor:
+        bLength                 7
+        bDescriptorType         5
+        bEndpointAddress     0x83  EP 3 IN
+        bmAttributes            3
+          Transfer Type            Interrupt
+          Synch Type               None
+          Usage Type               Data
+        wMaxPacketSize     0x0040  1x 64 bytes
+        bInterval               8
+    Interface Descriptor:
+      bLength                 9
+      bDescriptorType         4
+      bInterfaceNumber        2
+      bAlternateSetting       0
+      bNumEndpoints           2
+      bInterfaceClass        10 CDC Data
+      bInterfaceSubClass      0
+      bInterfaceProtocol      0
+      iInterface              0
+      Endpoint Descriptor:
+        bLength                 7
+        bDescriptorType         5
+        bEndpointAddress     0x84  EP 4 IN
+        bmAttributes            2
+          Transfer Type            Bulk
+          Synch Type               None
+          Usage Type               Data
+        wMaxPacketSize     0x0040  1x 64 bytes
+        bInterval               0
+      Endpoint Descriptor:
+        bLength                 7
+        bDescriptorType         5
+        bEndpointAddress     0x05  EP 5 OUT
+        bmAttributes            2
+          Transfer Type            Bulk
+          Synch Type               None
+          Usage Type               Data
+        wMaxPacketSize     0x0040  1x 64 bytes
+        bInterval               0
+    Interface Descriptor:
+      bLength                 9
+      bDescriptorType         4
+      bInterfaceNumber        3
+      bAlternateSetting       0
+      bNumEndpoints           2
+      bInterfaceClass         8 Mass Storage
+      bInterfaceSubClass      6 SCSI
+      bInterfaceProtocol     80 Bulk-Only
+      iInterface              0
+      Endpoint Descriptor:
+        bLength                 7
+        bDescriptorType         5
+        bEndpointAddress     0x87  EP 7 IN
+        bmAttributes            2
+          Transfer Type            Bulk
+          Synch Type               None
+          Usage Type               Data
+        wMaxPacketSize     0x0200  1x 512 bytes
+        bInterval               0
+      Endpoint Descriptor:
+        bLength                 7
+        bDescriptorType         5
+        bEndpointAddress     0x06  EP 6 OUT
+        bmAttributes            2
+          Transfer Type            Bulk
+          Synch Type               None
+          Usage Type               Data
+        wMaxPacketSize     0x0200  1x 512 bytes
+        bInterval               0
+Device Qualifier (for other device speed):
+  bLength                10
+  bDescriptorType         6
+  bcdUSB               2.00
+  bDeviceClass          239 Miscellaneous Device
+  bDeviceSubClass         2
+  bDeviceProtocol         1 Interface Association
+  bMaxPacketSize0        64
+  bNumConfigurations      1
+Device Status:     0x0000
+  (Bus Powered)
diff --git a/src/flash/nor/pic32mx.c b/src/flash/nor/pic32mx.c
index 9a1a634..0f3937c 100644
--- a/src/flash/nor/pic32mx.c
+++ b/src/flash/nor/pic32mx.c
@@ -866,10 +866,8 @@ COMMAND_HANDLER(pic32mx_handle_unlock_command)
 	struct mips_ejtag *ejtag_info;
 	int timeout = 10;
 
-	if (CMD_ARGC < 1) {
-		command_print(CMD, "pic32mx unlock <bank>");
+	if (CMD_ARGC != 1)
 		return ERROR_COMMAND_SYNTAX_ERROR;
-	}
 
 	struct flash_bank *bank;
 	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
@@ -932,7 +930,7 @@ static const struct command_registration pic32mx_exec_command_handlers[] = {
 		.name = "unlock",
 		.handler = pic32mx_handle_unlock_command,
 		.mode = COMMAND_EXEC,
-		.usage = "[bank_id]",
+		.usage = "bank_id",
 		.help = "Unlock/Erase entire device.",
 	},
 	COMMAND_REGISTRATION_DONE
diff --git a/src/flash/nor/stm32f2x.c b/src/flash/nor/stm32f2x.c
index 2e0d158..4e0f731 100644
--- a/src/flash/nor/stm32f2x.c
+++ b/src/flash/nor/stm32f2x.c
@@ -1540,10 +1540,8 @@ static int stm32x_mass_erase(struct flash_bank *bank)
 
 COMMAND_HANDLER(stm32x_handle_mass_erase_command)
 {
-	if (CMD_ARGC < 1) {
-		command_print(CMD, "stm32x mass_erase <bank>");
+	if (CMD_ARGC != 1)
 		return ERROR_COMMAND_SYNTAX_ERROR;
-	}
 
 	struct flash_bank *bank;
 	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
@@ -1566,10 +1564,8 @@ COMMAND_HANDLER(stm32f2x_handle_options_read_command)
 	struct flash_bank *bank;
 	struct stm32x_flash_bank *stm32x_info = NULL;
 
-	if (CMD_ARGC != 1) {
-		command_print(CMD, "stm32f2x options_read <bank>");
+	if (CMD_ARGC != 1)
 		return ERROR_COMMAND_SYNTAX_ERROR;
-	}
 
 	retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
 	if (retval != ERROR_OK)
@@ -1612,10 +1608,8 @@ COMMAND_HANDLER(stm32f2x_handle_options_write_command)
 	struct stm32x_flash_bank *stm32x_info = NULL;
 	uint16_t user_options, boot_addr0, boot_addr1, options_mask;
 
-	if (CMD_ARGC < 1) {
-		command_print(CMD, "stm32f2x options_write <bank> ...");
+	if (CMD_ARGC < 1)
 		return ERROR_COMMAND_SYNTAX_ERROR;
-	}
 
 	retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
 	if (retval != ERROR_OK)
@@ -1627,19 +1621,14 @@ COMMAND_HANDLER(stm32f2x_handle_options_write_command)
 
 	stm32x_info = bank->driver_priv;
 	if (stm32x_info->has_boot_addr) {
-		if (CMD_ARGC != 4) {
-			command_print(CMD, "stm32f2x options_write <bank> <user_options>"
-				" <boot_addr0> <boot_addr1>");
+		if (CMD_ARGC != 4)
 			return ERROR_COMMAND_SYNTAX_ERROR;
-		}
+
 		COMMAND_PARSE_NUMBER(u16, CMD_ARGV[2], boot_addr0);
 		COMMAND_PARSE_NUMBER(u16, CMD_ARGV[3], boot_addr1);
 		stm32x_info->option_bytes.boot_addr = boot_addr0 | (((uint32_t) boot_addr1) << 16);
-	} else {
-		if (CMD_ARGC != 2) {
-			command_print(CMD, "stm32f2x options_write <bank> <user_options>");
-			return ERROR_COMMAND_SYNTAX_ERROR;
-		}
+	} else if (CMD_ARGC != 2) {
+		return ERROR_COMMAND_SYNTAX_ERROR;
 	}
 
 	COMMAND_PARSE_NUMBER(u16, CMD_ARGV[1], user_options);
@@ -1674,10 +1663,8 @@ COMMAND_HANDLER(stm32f2x_handle_optcr2_write_command)
 	struct stm32x_flash_bank *stm32x_info = NULL;
 	uint32_t optcr2_pcrop;
 
-	if (CMD_ARGC != 2) {
-		command_print(CMD, "stm32f2x optcr2_write <bank> <optcr2_value>");
+	if (CMD_ARGC != 2)
 		return ERROR_COMMAND_SYNTAX_ERROR;
-	}
 
 	retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
 	if (retval != ERROR_OK)
@@ -1711,10 +1698,8 @@ COMMAND_HANDLER(stm32f2x_handle_optcr2_write_command)
 
 COMMAND_HANDLER(stm32x_handle_otp_command)
 {
-	if (CMD_ARGC < 2) {
-		command_print(CMD, "stm32x otp <bank> (enable|disable|show)");
+	if (CMD_ARGC != 2)
 		return ERROR_COMMAND_SYNTAX_ERROR;
-	}
 
 	struct flash_bank *bank;
 	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
@@ -1787,7 +1772,7 @@ static const struct command_registration stm32f2x_exec_command_handlers[] = {
 		.name = "otp",
 		.handler = stm32x_handle_otp_command,
 		.mode = COMMAND_EXEC,
-		.usage = "bank_id",
+		.usage = "bank_id (enable|disable|show)",
 		.help = "OTP (One Time Programmable) memory write enable/disable.",
 	},
 	COMMAND_REGISTRATION_DONE
diff --git a/src/flash/nor/stm32h7x.c b/src/flash/nor/stm32h7x.c
index 21618b3..c02fae9 100644
--- a/src/flash/nor/stm32h7x.c
+++ b/src/flash/nor/stm32h7x.c
@@ -1080,10 +1080,8 @@ flash_lock:
 
 COMMAND_HANDLER(stm32x_handle_mass_erase_command)
 {
-	if (CMD_ARGC < 1) {
-		command_print(CMD, "stm32h7x mass_erase <bank>");
+	if (CMD_ARGC != 1)
 		return ERROR_COMMAND_SYNTAX_ERROR;
-	}
 
 	struct flash_bank *bank;
 	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
@@ -1101,10 +1099,8 @@ COMMAND_HANDLER(stm32x_handle_mass_erase_command)
 
 COMMAND_HANDLER(stm32x_handle_option_read_command)
 {
-	if (CMD_ARGC < 2) {
-		command_print(CMD, "stm32h7x option_read <bank> <option_reg offset>");
+	if (CMD_ARGC != 2)
 		return ERROR_COMMAND_SYNTAX_ERROR;
-	}
 
 	struct flash_bank *bank;
 	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
@@ -1126,10 +1122,8 @@ COMMAND_HANDLER(stm32x_handle_option_read_command)
 
 COMMAND_HANDLER(stm32x_handle_option_write_command)
 {
-	if (CMD_ARGC < 3) {
-		command_print(CMD, "stm32h7x option_write <bank> <option_reg offset> <value> [mask]");
+	if (CMD_ARGC != 3 && CMD_ARGC != 4)
 		return ERROR_COMMAND_SYNTAX_ERROR;
-	}
 
 	struct flash_bank *bank;
 	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
diff --git a/src/flash/nor/stm32l4x.c b/src/flash/nor/stm32l4x.c
index 96757a9..0399385 100644
--- a/src/flash/nor/stm32l4x.c
+++ b/src/flash/nor/stm32l4x.c
@@ -2218,10 +2218,8 @@ err_lock:
 
 COMMAND_HANDLER(stm32l4_handle_mass_erase_command)
 {
-	if (CMD_ARGC < 1) {
-		command_print(CMD, "stm32l4x mass_erase <STM32L4 bank>");
+	if (CMD_ARGC != 1)
 		return ERROR_COMMAND_SYNTAX_ERROR;
-	}
 
 	struct flash_bank *bank;
 	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
@@ -2239,10 +2237,8 @@ COMMAND_HANDLER(stm32l4_handle_mass_erase_command)
 
 COMMAND_HANDLER(stm32l4_handle_option_read_command)
 {
-	if (CMD_ARGC < 2) {
-		command_print(CMD, "stm32l4x option_read <STM32L4 bank> <option_reg offset>");
+	if (CMD_ARGC != 2)
 		return ERROR_COMMAND_SYNTAX_ERROR;
-	}
 
 	struct flash_bank *bank;
 	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
@@ -2266,10 +2262,8 @@ COMMAND_HANDLER(stm32l4_handle_option_read_command)
 
 COMMAND_HANDLER(stm32l4_handle_option_write_command)
 {
-	if (CMD_ARGC < 3) {
-		command_print(CMD, "stm32l4x option_write <STM32L4 bank> <option_reg offset> <value> [mask]");
+	if (CMD_ARGC != 3 && CMD_ARGC != 4)
 		return ERROR_COMMAND_SYNTAX_ERROR;
-	}
 
 	struct flash_bank *bank;
 	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
@@ -2381,7 +2375,7 @@ COMMAND_HANDLER(stm32l4_handle_lock_command)
 {
 	struct target *target = NULL;
 
-	if (CMD_ARGC < 1)
+	if (CMD_ARGC != 1)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
 	struct flash_bank *bank;
@@ -2416,7 +2410,7 @@ COMMAND_HANDLER(stm32l4_handle_unlock_command)
 {
 	struct target *target = NULL;
 
-	if (CMD_ARGC < 1)
+	if (CMD_ARGC != 1)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
 	struct flash_bank *bank;
@@ -2520,7 +2514,7 @@ COMMAND_HANDLER(stm32l4_handle_wrp_info_command)
 
 COMMAND_HANDLER(stm32l4_handle_otp_command)
 {
-	if (CMD_ARGC < 2)
+	if (CMD_ARGC != 2)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
 	struct flash_bank *bank;
diff --git a/src/flash/nor/stmqspi.c b/src/flash/nor/stmqspi.c
index c9fc1bf..a1e1d34 100644
--- a/src/flash/nor/stmqspi.c
+++ b/src/flash/nor/stmqspi.c
@@ -646,21 +646,21 @@ COMMAND_HANDLER(stmqspi_handle_set)
 	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[index++], stmqspi_info->dev.size_in_bytes);
 	if (log2u(stmqspi_info->dev.size_in_bytes) < 8) {
 		command_print(CMD, "stmqspi: device size must be 2^n with n >= 8");
-		return ERROR_COMMAND_SYNTAX_ERROR;
+		return ERROR_COMMAND_ARGUMENT_INVALID;
 	}
 
 	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[index++], stmqspi_info->dev.pagesize);
 	if (stmqspi_info->dev.pagesize > stmqspi_info->dev.size_in_bytes ||
 		(log2u(stmqspi_info->dev.pagesize) < 0)) {
 		command_print(CMD, "stmqspi: page size must be 2^n and <= device size");
-		return ERROR_COMMAND_SYNTAX_ERROR;
+		return ERROR_COMMAND_ARGUMENT_INVALID;
 	}
 
 	COMMAND_PARSE_NUMBER(u8, CMD_ARGV[index++], stmqspi_info->dev.read_cmd);
 	if ((stmqspi_info->dev.read_cmd != 0x03) &&
 		(stmqspi_info->dev.read_cmd != 0x13)) {
 		command_print(CMD, "stmqspi: only 0x03/0x13 READ cmd allowed");
-		return ERROR_COMMAND_SYNTAX_ERROR;
+		return ERROR_COMMAND_ARGUMENT_INVALID;
 	}
 
 	COMMAND_PARSE_NUMBER(u8, CMD_ARGV[index++], stmqspi_info->dev.qread_cmd);
@@ -678,7 +678,7 @@ COMMAND_HANDLER(stmqspi_handle_set)
 		(stmqspi_info->dev.qread_cmd != 0xEE)) {
 		command_print(CMD, "stmqspi: only 0x0B/0x0C/0x3B/0x3C/"
 			"0x6B/0x6C/0xBB/0xBC/0xEB/0xEC/0xEE QREAD allowed");
-		return ERROR_COMMAND_SYNTAX_ERROR;
+		return ERROR_COMMAND_ARGUMENT_INVALID;
 	}
 
 	COMMAND_PARSE_NUMBER(u8, CMD_ARGV[index++], stmqspi_info->dev.pprog_cmd);
@@ -686,7 +686,7 @@ COMMAND_HANDLER(stmqspi_handle_set)
 		(stmqspi_info->dev.pprog_cmd != 0x12) &&
 		(stmqspi_info->dev.pprog_cmd != 0x32)) {
 		command_print(CMD, "stmqspi: only 0x02/0x12/0x32 PPRG cmd allowed");
-		return ERROR_COMMAND_SYNTAX_ERROR;
+		return ERROR_COMMAND_ARGUMENT_INVALID;
 	}
 
 	if (index < CMD_ARGC)
@@ -700,7 +700,7 @@ COMMAND_HANDLER(stmqspi_handle_set)
 			(stmqspi_info->dev.sectorsize < stmqspi_info->dev.pagesize) ||
 			(log2u(stmqspi_info->dev.sectorsize) < 0)) {
 			command_print(CMD, "stmqspi: sector size must be 2^n and <= device size");
-			return ERROR_COMMAND_SYNTAX_ERROR;
+			return ERROR_COMMAND_ARGUMENT_INVALID;
 		}
 
 		if (index < CMD_ARGC)
@@ -786,7 +786,7 @@ COMMAND_HANDLER(stmqspi_handle_cmd)
 	num_write = CMD_ARGC - 2;
 	if (num_write > max) {
 		LOG_ERROR("at most %d bytes may be sent", max);
-		return ERROR_COMMAND_SYNTAX_ERROR;
+		return ERROR_COMMAND_ARGUMENT_INVALID;
 	}
 
 	retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
@@ -811,7 +811,7 @@ COMMAND_HANDLER(stmqspi_handle_cmd)
 		if (stmqspi_info->saved_cr & BIT(SPI_DUAL_FLASH)) {
 			if ((num_write & 1) == 0) {
 				LOG_ERROR("number of data bytes to write must be even in dual mode");
-				return ERROR_COMMAND_SYNTAX_ERROR;
+				return ERROR_COMMAND_ARGUMENT_INVALID;
 			}
 		}
 	} else {
@@ -819,12 +819,12 @@ COMMAND_HANDLER(stmqspi_handle_cmd)
 		if (stmqspi_info->saved_cr & BIT(SPI_DUAL_FLASH)) {
 			if ((num_read & 1) != 0) {
 				LOG_ERROR("number of bytes to read must be even in dual mode");
-				return ERROR_COMMAND_SYNTAX_ERROR;
+				return ERROR_COMMAND_ARGUMENT_INVALID;
 			}
 		}
 		if ((num_write < 1) || (num_write > 5)) {
 			LOG_ERROR("one cmd and up to four addr bytes must be send when reading");
-			return ERROR_COMMAND_SYNTAX_ERROR;
+			return ERROR_COMMAND_ARGUMENT_INVALID;
 		}
 	}
 
diff --git a/src/flash/nor/xcf.c b/src/flash/nor/xcf.c
index 2870725..c253b22 100644
--- a/src/flash/nor/xcf.c
+++ b/src/flash/nor/xcf.c
@@ -597,7 +597,7 @@ static int xcf_probe(struct flash_bank *bank)
 	}
 
 	/* check idcode and alloc memory for sector table */
-	if (!bank->target->tap->hasidcode)
+	if (!bank->target->tap->has_idcode)
 		return ERROR_FLASH_OPERATION_FAILED;
 
 	/* guess number of blocks using chip ID */
diff --git a/src/helper/nvp.c b/src/helper/nvp.c
index 7a8abc2..a938716 100644
--- a/src/helper/nvp.c
+++ b/src/helper/nvp.c
@@ -14,7 +14,7 @@
  * Copyright 2009 David Brownell
  * Copyright (c) 2005-2011 Jim Tcl Project. All rights reserved.
  *
- * This file is extracted from jim_nvp.c, originally part of jim TCL code.
+ * This file is extracted from jim-nvp.c, originally part of jim TCL code.
  */
 
 #ifdef HAVE_CONFIG_H
diff --git a/src/helper/nvp.h b/src/helper/nvp.h
index 14bd9b0..1f4e3d1 100644
--- a/src/helper/nvp.h
+++ b/src/helper/nvp.h
@@ -14,7 +14,7 @@
  * Copyright 2009 David Brownell
  * Copyright (c) 2005-2011 Jim Tcl Project. All rights reserved.
  *
- * This file is extracted from jim_nvp.h, originally part of jim TCL code.
+ * This file is extracted from jim-nvp.h, originally part of jim TCL code.
  */
 
 #ifndef OPENOCD_HELPER_NVP_H
@@ -51,7 +51,7 @@
  *         returns &yn[0];
  *  result = nvp_name2value(yn, "no");
  *         returns &yn[1];
- *  result = jim_nvp_name2value(yn, "Blah");
+ *  result = nvp_name2value(yn, "Blah");
  *         returns &yn[4];
  * \endcode
  *
diff --git a/src/jtag/core.c b/src/jtag/core.c
index 5748011..e2af6c5 100644
--- a/src/jtag/core.c
+++ b/src/jtag/core.c
@@ -1049,7 +1049,7 @@ static int jtag_reset_callback(enum jtag_event event, void *priv)
 
 		/* current instruction is either BYPASS or IDCODE */
 		buf_set_ones(tap->cur_instr, tap->ir_length);
-		tap->bypass = 1;
+		tap->bypass = true;
 	}
 
 	return ERROR_OK;
@@ -1177,7 +1177,7 @@ static bool jtag_examine_chain_end(uint8_t *idcodes, unsigned count, unsigned ma
 static bool jtag_examine_chain_match_tap(const struct jtag_tap *tap)
 {
 
-	if (tap->expected_ids_cnt == 0 || !tap->hasidcode)
+	if (tap->expected_ids_cnt == 0 || !tap->has_idcode)
 		return true;
 
 	/* optionally ignore the JTAG version field - bits 28-31 of IDCODE */
@@ -1283,13 +1283,13 @@ static int jtag_examine_chain(void)
 			/* Zero for LSB indicates a device in bypass */
 			LOG_INFO("TAP %s does not have valid IDCODE (idcode=0x%" PRIx32 ")",
 					tap->dotted_name, idcode);
-			tap->hasidcode = false;
+			tap->has_idcode = false;
 			tap->idcode = 0;
 
 			bit_count += 1;
 		} else {
 			/* Friendly devices support IDCODE */
-			tap->hasidcode = true;
+			tap->has_idcode = true;
 			tap->idcode = idcode;
 			jtag_examine_chain_display(LOG_LVL_INFO, "tap/device found", tap->dotted_name, idcode);
 
@@ -1464,7 +1464,7 @@ void jtag_tap_init(struct jtag_tap *tap)
 	buf_set_u32(tap->expected_mask, 0, ir_len_bits, tap->ir_capture_mask);
 
 	/* TAP will be in bypass mode after jtag_validate_ircapture() */
-	tap->bypass = 1;
+	tap->bypass = true;
 	buf_set_ones(tap->cur_instr, tap->ir_length);
 
 	/* register the reset callback for the TAP */
diff --git a/src/jtag/drivers/cmsis_dap.c b/src/jtag/drivers/cmsis_dap.c
index 1e7a851..caacc9b 100644
--- a/src/jtag/drivers/cmsis_dap.c
+++ b/src/jtag/drivers/cmsis_dap.c
@@ -225,6 +225,12 @@ struct pending_scan_result {
 	unsigned int buffer_offset;
 };
 
+/* Read mode */
+enum cmsis_dap_blocking {
+	CMSIS_DAP_NON_BLOCKING,
+	CMSIS_DAP_BLOCKING
+};
+
 /* Each block in FIFO can contain up to pending_queue_len transfers */
 static unsigned int pending_queue_len;
 static unsigned int tfer_max_command_size;
@@ -315,7 +321,7 @@ static void cmsis_dap_flush_read(struct cmsis_dap *dap)
 	 * USB close/open so we need to flush up to 64 old packets
 	 * to be sure all buffers are empty */
 	for (i = 0; i < 64; i++) {
-		int retval = dap->backend->read(dap, 10);
+		int retval = dap->backend->read(dap, 10, NULL);
 		if (retval == ERROR_TIMEOUT_REACHED)
 			break;
 	}
@@ -326,10 +332,13 @@ static void cmsis_dap_flush_read(struct cmsis_dap *dap)
 /* Send a message and receive the reply */
 static int cmsis_dap_xfer(struct cmsis_dap *dap, int txlen)
 {
+	if (dap->write_count + dap->read_count) {
+		LOG_ERROR("internal: queue not empty before xfer");
+	}
 	if (dap->pending_fifo_block_count) {
 		LOG_ERROR("pending %u blocks, flushing", dap->pending_fifo_block_count);
 		while (dap->pending_fifo_block_count) {
-			dap->backend->read(dap, 10);
+			dap->backend->read(dap, 10, NULL);
 			dap->pending_fifo_block_count--;
 		}
 		dap->pending_fifo_put_idx = 0;
@@ -342,7 +351,7 @@ static int cmsis_dap_xfer(struct cmsis_dap *dap, int txlen)
 		return retval;
 
 	/* get reply */
-	retval = dap->backend->read(dap, LIBUSB_TIMEOUT_MS);
+	retval = dap->backend->read(dap, LIBUSB_TIMEOUT_MS, NULL);
 	if (retval < 0)
 		return retval;
 
@@ -356,6 +365,7 @@ static int cmsis_dap_xfer(struct cmsis_dap *dap, int txlen)
 		LOG_ERROR("CMSIS-DAP command mismatch. Sent 0x%" PRIx8
 			 " received 0x%" PRIx8, current_cmd, resp[0]);
 
+		dap->backend->cancel_all(dap);
 		cmsis_dap_flush_read(dap);
 		return ERROR_FAIL;
 	}
@@ -749,6 +759,22 @@ static int cmsis_dap_cmd_dap_swo_data(
 	return ERROR_OK;
 }
 
+static void cmsis_dap_swd_discard_all_pending(struct cmsis_dap *dap)
+{
+	for (unsigned int i = 0; i < MAX_PENDING_REQUESTS; i++)
+		dap->pending_fifo[i].transfer_count = 0;
+
+	dap->pending_fifo_put_idx = 0;
+	dap->pending_fifo_get_idx = 0;
+	dap->pending_fifo_block_count = 0;
+}
+
+static void cmsis_dap_swd_cancel_transfers(struct cmsis_dap *dap)
+{
+	dap->backend->cancel_all(dap);
+	cmsis_dap_flush_read(dap);
+	cmsis_dap_swd_discard_all_pending(dap);
+}
 
 static void cmsis_dap_swd_write_from_queue(struct cmsis_dap *dap)
 {
@@ -770,8 +796,10 @@ static void cmsis_dap_swd_write_from_queue(struct cmsis_dap *dap)
 		goto skip;
 	}
 
-	if (block->transfer_count == 0)
+	if (block->transfer_count == 0) {
+		LOG_ERROR("internal: write an empty queue?!");
 		goto skip;
+	}
 
 	bool block_cmd = !cmsis_dap_handle->swd_cmds_differ
 					 && block->transfer_count >= CMD_DAP_TFER_BLOCK_MIN_OPS;
@@ -831,14 +859,13 @@ static void cmsis_dap_swd_write_from_queue(struct cmsis_dap *dap)
 	if (retval < 0) {
 		queued_retval = retval;
 		goto skip;
-	} else {
-		queued_retval = ERROR_OK;
 	}
 
-	dap->pending_fifo_put_idx = (dap->pending_fifo_put_idx + 1) % dap->packet_count;
+	unsigned int packet_count = dap->quirk_mode ? 1 : dap->packet_count;
+	dap->pending_fifo_put_idx = (dap->pending_fifo_put_idx + 1) % packet_count;
 	dap->pending_fifo_block_count++;
-	if (dap->pending_fifo_block_count > dap->packet_count)
-		LOG_ERROR("too much pending writes %u", dap->pending_fifo_block_count);
+	if (dap->pending_fifo_block_count > packet_count)
+		LOG_ERROR("internal: too much pending writes %u", dap->pending_fifo_block_count);
 
 	return;
 
@@ -846,21 +873,47 @@ skip:
 	block->transfer_count = 0;
 }
 
-static void cmsis_dap_swd_read_process(struct cmsis_dap *dap, int timeout_ms)
+static void cmsis_dap_swd_read_process(struct cmsis_dap *dap, enum cmsis_dap_blocking blocking)
 {
+	int retval;
 	struct pending_request_block *block = &dap->pending_fifo[dap->pending_fifo_get_idx];
 
-	if (dap->pending_fifo_block_count == 0)
-		LOG_ERROR("no pending write");
+	if (dap->pending_fifo_block_count == 0) {
+		LOG_ERROR("internal: no pending write when reading?!");
+		return;
+	}
+
+	if (queued_retval != ERROR_OK) {
+		/* keep reading blocks until the pipeline is empty */
+		retval = dap->backend->read(dap, 10, NULL);
+		if (retval == ERROR_TIMEOUT_REACHED || retval == 0) {
+			/* timeout means that we flushed the pipeline,
+			 * we can safely discard remaining pending requests */
+			cmsis_dap_swd_discard_all_pending(dap);
+			return;
+		}
+		goto skip;
+	}
 
 	/* get reply */
-	int retval = dap->backend->read(dap, timeout_ms);
-	if (retval == ERROR_TIMEOUT_REACHED && timeout_ms < LIBUSB_TIMEOUT_MS)
+	struct timeval tv = {
+		.tv_sec = 0,
+		.tv_usec = 0
+	};
+	retval = dap->backend->read(dap, LIBUSB_TIMEOUT_MS, blocking ? NULL : &tv);
+	bool timeout = (retval == ERROR_TIMEOUT_REACHED || retval == 0);
+	if (timeout && blocking == CMSIS_DAP_NON_BLOCKING)
 		return;
 
 	if (retval <= 0) {
-		LOG_DEBUG("error reading data");
+		LOG_DEBUG("error reading adapter response");
 		queued_retval = ERROR_FAIL;
+		if (timeout) {
+			/* timeout means that we flushed the pipeline,
+			 * we can safely discard remaining pending requests */
+			cmsis_dap_swd_discard_all_pending(dap);
+			return;
+		}
 		goto skip;
 	}
 
@@ -868,8 +921,9 @@ static void cmsis_dap_swd_read_process(struct cmsis_dap *dap, int timeout_ms)
 	if (resp[0] != block->command) {
 		LOG_ERROR("CMSIS-DAP command mismatch. Expected 0x%x received 0x%" PRIx8,
 			block->command, resp[0]);
+		cmsis_dap_swd_cancel_transfers(dap);
 		queued_retval = ERROR_FAIL;
-		goto skip;
+		return;
 	}
 
 	unsigned int transfer_count;
@@ -895,12 +949,17 @@ static void cmsis_dap_swd_read_process(struct cmsis_dap *dap, int timeout_ms)
 		goto skip;
 	}
 
-	if (block->transfer_count != transfer_count)
+	if (block->transfer_count != transfer_count) {
 		LOG_ERROR("CMSIS-DAP transfer count mismatch: expected %d, got %d",
 			  block->transfer_count, transfer_count);
+		cmsis_dap_swd_cancel_transfers(dap);
+		queued_retval = ERROR_FAIL;
+		return;
+	}
 
-	LOG_DEBUG_IO("Received results of %d queued transactions FIFO index %u timeout %i",
-		 transfer_count, dap->pending_fifo_get_idx, timeout_ms);
+	LOG_DEBUG_IO("Received results of %d queued transactions FIFO index %u, %s mode",
+				 transfer_count, dap->pending_fifo_get_idx,
+				 blocking ? "blocking" : "nonblocking");
 
 	for (unsigned int i = 0; i < transfer_count; i++) {
 		struct pending_transfer_result *transfer = &(block->transfers[i]);
@@ -926,19 +985,22 @@ static void cmsis_dap_swd_read_process(struct cmsis_dap *dap, int timeout_ms)
 
 skip:
 	block->transfer_count = 0;
-	dap->pending_fifo_get_idx = (dap->pending_fifo_get_idx + 1) % dap->packet_count;
+	if (!dap->quirk_mode && dap->packet_count > 1)
+		dap->pending_fifo_get_idx = (dap->pending_fifo_get_idx + 1) % dap->packet_count;
 	dap->pending_fifo_block_count--;
 }
 
 static int cmsis_dap_swd_run_queue(void)
 {
-	if (cmsis_dap_handle->pending_fifo_block_count)
-		cmsis_dap_swd_read_process(cmsis_dap_handle, 0);
+	if (cmsis_dap_handle->write_count + cmsis_dap_handle->read_count) {
+		if (cmsis_dap_handle->pending_fifo_block_count)
+			cmsis_dap_swd_read_process(cmsis_dap_handle, CMSIS_DAP_NON_BLOCKING);
 
-	cmsis_dap_swd_write_from_queue(cmsis_dap_handle);
+		cmsis_dap_swd_write_from_queue(cmsis_dap_handle);
+	}
 
 	while (cmsis_dap_handle->pending_fifo_block_count)
-		cmsis_dap_swd_read_process(cmsis_dap_handle, LIBUSB_TIMEOUT_MS);
+		cmsis_dap_swd_read_process(cmsis_dap_handle, CMSIS_DAP_BLOCKING);
 
 	cmsis_dap_handle->pending_fifo_put_idx = 0;
 	cmsis_dap_handle->pending_fifo_get_idx = 0;
@@ -979,10 +1041,16 @@ static unsigned int cmsis_dap_tfer_resp_size(unsigned int write_count,
 
 static void cmsis_dap_swd_queue_cmd(uint8_t cmd, uint32_t *dst, uint32_t data)
 {
+	/* TARGETSEL register write cannot be queued */
+	if (swd_cmd(false, false, DP_TARGETSEL) == cmd) {
+		queued_retval = cmsis_dap_swd_run_queue();
+
+		cmsis_dap_metacmd_targetsel(data);
+		return;
+	}
+
 	/* Compute sizes of the DAP Transfer command and the expected response
 	 * for all queued and this operation */
-	bool targetsel_cmd = swd_cmd(false, false, DP_TARGETSEL) == cmd;
-
 	unsigned int write_count = cmsis_dap_handle->write_count;
 	unsigned int read_count = cmsis_dap_handle->read_count;
 	bool block_cmd;
@@ -1003,20 +1071,19 @@ static void cmsis_dap_swd_queue_cmd(uint8_t cmd, uint32_t *dst, uint32_t data)
 													block_cmd);
 	unsigned int max_transfer_count = block_cmd ? 65535 : 255;
 
-	/* Does the DAP Transfer command and the expected response fit into one packet?
-	 * Run the queue also before a targetsel - it cannot be queued */
+	/* Does the DAP Transfer command and also its expected response fit into one packet? */
 	if (cmd_size > tfer_max_command_size
 			|| resp_size > tfer_max_response_size
-			|| targetsel_cmd
 			|| write_count + read_count > max_transfer_count) {
 		if (cmsis_dap_handle->pending_fifo_block_count)
-			cmsis_dap_swd_read_process(cmsis_dap_handle, 0);
+			cmsis_dap_swd_read_process(cmsis_dap_handle, CMSIS_DAP_NON_BLOCKING);
 
 		/* Not enough room in the queue. Run the queue. */
 		cmsis_dap_swd_write_from_queue(cmsis_dap_handle);
 
-		if (cmsis_dap_handle->pending_fifo_block_count >= cmsis_dap_handle->packet_count)
-			cmsis_dap_swd_read_process(cmsis_dap_handle, LIBUSB_TIMEOUT_MS);
+		unsigned int packet_count = cmsis_dap_handle->quirk_mode ? 1 : cmsis_dap_handle->packet_count;
+		if (cmsis_dap_handle->pending_fifo_block_count >= packet_count)
+			cmsis_dap_swd_read_process(cmsis_dap_handle, CMSIS_DAP_BLOCKING);
 	}
 
 	assert(cmsis_dap_handle->pending_fifo[cmsis_dap_handle->pending_fifo_put_idx].transfer_count < pending_queue_len);
@@ -1024,11 +1091,6 @@ static void cmsis_dap_swd_queue_cmd(uint8_t cmd, uint32_t *dst, uint32_t data)
 	if (queued_retval != ERROR_OK)
 		return;
 
-	if (targetsel_cmd) {
-		cmsis_dap_metacmd_targetsel(data);
-		return;
-	}
-
 	struct pending_request_block *block = &cmsis_dap_handle->pending_fifo[cmsis_dap_handle->pending_fifo_put_idx];
 	struct pending_transfer_result *transfer = &(block->transfers[block->transfer_count]);
 	transfer->data = data;
@@ -1160,7 +1222,10 @@ static int cmsis_dap_swd_switch_seq(enum swd_special_seq seq)
 	unsigned int s_len;
 	int retval;
 
-	if (seq != LINE_RESET &&
+	if (swd_mode)
+		queued_retval = cmsis_dap_swd_run_queue();
+
+	if (cmsis_dap_handle->quirk_mode && seq != LINE_RESET &&
 			(output_pins & (SWJ_PIN_SRST | SWJ_PIN_TRST))
 				== (SWJ_PIN_SRST | SWJ_PIN_TRST)) {
 		/* Following workaround deasserts reset on most adapters.
@@ -1296,7 +1361,7 @@ static int cmsis_dap_init(void)
 	if (data[0] == 2) {  /* short */
 		uint16_t pkt_sz = data[1] + (data[2] << 8);
 		if (pkt_sz != cmsis_dap_handle->packet_size) {
-			free(cmsis_dap_handle->packet_buffer);
+			cmsis_dap_handle->backend->packet_buffer_free(cmsis_dap_handle);
 			retval = cmsis_dap_handle->backend->packet_buffer_alloc(cmsis_dap_handle, pkt_sz);
 			if (retval != ERROR_OK)
 				goto init_err;
@@ -2108,12 +2173,12 @@ COMMAND_HANDLER(cmsis_dap_handle_cmd_command)
 COMMAND_HANDLER(cmsis_dap_handle_vid_pid_command)
 {
 	if (CMD_ARGC > MAX_USB_IDS * 2) {
-		LOG_WARNING("ignoring extra IDs in cmsis_dap_vid_pid "
+		LOG_WARNING("ignoring extra IDs in cmsis-dap vid_pid "
 			"(maximum is %d pairs)", MAX_USB_IDS);
 		CMD_ARGC = MAX_USB_IDS * 2;
 	}
 	if (CMD_ARGC < 2 || (CMD_ARGC & 1)) {
-		LOG_WARNING("incomplete cmsis_dap_vid_pid configuration directive");
+		LOG_WARNING("incomplete cmsis-dap vid_pid configuration directive");
 		if (CMD_ARGC < 2)
 			return ERROR_COMMAND_SYNTAX_ERROR;
 		/* remove the incomplete trailing id */
@@ -2148,15 +2213,29 @@ COMMAND_HANDLER(cmsis_dap_handle_backend_command)
 				}
 			}
 
-			LOG_ERROR("invalid backend argument to cmsis_dap_backend <backend>");
+			command_print(CMD, "invalid backend argument to cmsis-dap backend <backend>");
+			return ERROR_COMMAND_ARGUMENT_INVALID;
 		}
 	} else {
-		LOG_ERROR("expected exactly one argument to cmsis_dap_backend <backend>");
+		return ERROR_COMMAND_SYNTAX_ERROR;
 	}
 
 	return ERROR_OK;
 }
 
+COMMAND_HANDLER(cmsis_dap_handle_quirk_command)
+{
+	if (CMD_ARGC > 1)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	if (CMD_ARGC == 1)
+		COMMAND_PARSE_ENABLE(CMD_ARGV[0], cmsis_dap_handle->quirk_mode);
+
+	command_print(CMD, "CMSIS-DAP quirk workarounds %s",
+				  cmsis_dap_handle->quirk_mode ? "enabled" : "disabled");
+	return ERROR_OK;
+}
+
 static const struct command_registration cmsis_dap_subcommand_handlers[] = {
 	{
 		.name = "info",
@@ -2172,35 +2251,30 @@ static const struct command_registration cmsis_dap_subcommand_handlers[] = {
 		.usage = "",
 		.help = "issue cmsis-dap command",
 	},
-	COMMAND_REGISTRATION_DONE
-};
-
-
-static const struct command_registration cmsis_dap_command_handlers[] = {
-	{
-		.name = "cmsis-dap",
-		.mode = COMMAND_ANY,
-		.help = "perform CMSIS-DAP management",
-		.usage = "<cmd>",
-		.chain = cmsis_dap_subcommand_handlers,
-	},
 	{
-		.name = "cmsis_dap_vid_pid",
+		.name = "vid_pid",
 		.handler = &cmsis_dap_handle_vid_pid_command,
 		.mode = COMMAND_CONFIG,
 		.help = "the vendor ID and product ID of the CMSIS-DAP device",
 		.usage = "(vid pid)*",
 	},
 	{
-		.name = "cmsis_dap_backend",
+		.name = "backend",
 		.handler = &cmsis_dap_handle_backend_command,
 		.mode = COMMAND_CONFIG,
 		.help = "set the communication backend to use (USB bulk or HID).",
 		.usage = "(auto | usb_bulk | hid)",
 	},
+	{
+		.name = "quirk",
+		.handler = &cmsis_dap_handle_quirk_command,
+		.mode = COMMAND_ANY,
+		.help = "allow expensive workarounds of known adapter quirks.",
+		.usage = "[enable | disable]",
+	},
 #if BUILD_CMSIS_DAP_USB
 	{
-		.name = "cmsis_dap_usb",
+		.name = "usb",
 		.chain = cmsis_dap_usb_subcommand_handlers,
 		.mode = COMMAND_ANY,
 		.help = "USB bulk backend-specific commands",
@@ -2210,6 +2284,18 @@ static const struct command_registration cmsis_dap_command_handlers[] = {
 	COMMAND_REGISTRATION_DONE
 };
 
+
+static const struct command_registration cmsis_dap_command_handlers[] = {
+	{
+		.name = "cmsis-dap",
+		.mode = COMMAND_ANY,
+		.help = "perform CMSIS-DAP management",
+		.usage = "<cmd>",
+		.chain = cmsis_dap_subcommand_handlers,
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
 static const struct swd_driver cmsis_dap_swd_driver = {
 	.init = cmsis_dap_swd_init,
 	.switch_seq = cmsis_dap_swd_switch_seq,
diff --git a/src/jtag/drivers/cmsis_dap.h b/src/jtag/drivers/cmsis_dap.h
index 16885a5..e47697d 100644
--- a/src/jtag/drivers/cmsis_dap.h
+++ b/src/jtag/drivers/cmsis_dap.h
@@ -52,7 +52,8 @@ struct cmsis_dap {
 	unsigned int pending_fifo_block_count;
 
 	uint16_t caps;
-	uint8_t mode;
+	bool quirk_mode;	/* enable expensive workarounds */
+
 	uint32_t swo_buf_sz;
 	bool trace_enabled;
 };
@@ -61,9 +62,12 @@ struct cmsis_dap_backend {
 	const char *name;
 	int (*open)(struct cmsis_dap *dap, uint16_t vids[], uint16_t pids[], const char *serial);
 	void (*close)(struct cmsis_dap *dap);
-	int (*read)(struct cmsis_dap *dap, int timeout_ms);
+	int (*read)(struct cmsis_dap *dap, int transfer_timeout_ms,
+			    struct timeval *wait_timeout);
 	int (*write)(struct cmsis_dap *dap, int len, int timeout_ms);
 	int (*packet_buffer_alloc)(struct cmsis_dap *dap, unsigned int pkt_sz);
+	void (*packet_buffer_free)(struct cmsis_dap *dap);
+	void (*cancel_all)(struct cmsis_dap *dap);
 };
 
 extern const struct cmsis_dap_backend cmsis_dap_hid_backend;
diff --git a/src/jtag/drivers/cmsis_dap_usb_bulk.c b/src/jtag/drivers/cmsis_dap_usb_bulk.c
index 6599c41..17e490f 100644
--- a/src/jtag/drivers/cmsis_dap_usb_bulk.c
+++ b/src/jtag/drivers/cmsis_dap_usb_bulk.c
@@ -28,8 +28,29 @@
 #include <libusb.h>
 #include <helper/log.h>
 #include <helper/replacements.h>
+#include <jtag/jtag.h>	/* ERROR_JTAG_DEVICE_ERROR only */
 
 #include "cmsis_dap.h"
+#include "libusb_helper.h"
+
+#if !defined(LIBUSB_API_VERSION) || (LIBUSB_API_VERSION < 0x01000105) \
+		|| defined(_WIN32) || defined(__CYGWIN__)
+	#define libusb_dev_mem_alloc(dev, sz) malloc(sz)
+	#define libusb_dev_mem_free(dev, buffer, sz) free(buffer)
+#endif
+
+enum {
+	CMSIS_DAP_TRANSFER_PENDING = 0,	/* must be 0, used in libusb_handle_events_completed */
+	CMSIS_DAP_TRANSFER_IDLE,
+	CMSIS_DAP_TRANSFER_COMPLETED
+};
+
+struct cmsis_dap_bulk_transfer {
+	struct libusb_transfer *transfer;
+	uint8_t *buffer;
+	int status;		/* either CMSIS_DAP_TRANSFER_ enum or error code */
+	int transferred;
+};
 
 struct cmsis_dap_backend_data {
 	struct libusb_context *usb_ctx;
@@ -37,12 +58,16 @@ struct cmsis_dap_backend_data {
 	unsigned int ep_out;
 	unsigned int ep_in;
 	int interface;
+
+	struct cmsis_dap_bulk_transfer command_transfers[MAX_PENDING_REQUESTS];
+	struct cmsis_dap_bulk_transfer response_transfers[MAX_PENDING_REQUESTS];
 };
 
 static int cmsis_dap_usb_interface = -1;
 
 static void cmsis_dap_usb_close(struct cmsis_dap *dap);
 static int cmsis_dap_usb_alloc(struct cmsis_dap *dap, unsigned int pkt_sz);
+static void cmsis_dap_usb_free(struct cmsis_dap *dap);
 
 static int cmsis_dap_usb_open(struct cmsis_dap *dap, uint16_t vids[], uint16_t pids[], const char *serial)
 {
@@ -358,6 +383,24 @@ static int cmsis_dap_usb_open(struct cmsis_dap *dap, uint16_t vids[], uint16_t p
 			dap->bdata->ep_in = ep_in;
 			dap->bdata->interface = interface_num;
 
+			for (unsigned int idx = 0; idx < MAX_PENDING_REQUESTS; idx++) {
+				dap->bdata->command_transfers[idx].status = CMSIS_DAP_TRANSFER_IDLE;
+				dap->bdata->command_transfers[idx].transfer = libusb_alloc_transfer(0);
+				if (!dap->bdata->command_transfers[idx].transfer) {
+					LOG_ERROR("unable to allocate USB transfer");
+					cmsis_dap_usb_close(dap);
+					return ERROR_FAIL;
+				}
+
+				dap->bdata->response_transfers[idx].status = CMSIS_DAP_TRANSFER_IDLE;
+				dap->bdata->response_transfers[idx].transfer = libusb_alloc_transfer(0);
+				if (!dap->bdata->response_transfers[idx].transfer) {
+					LOG_ERROR("unable to allocate USB transfer");
+					cmsis_dap_usb_close(dap);
+					return ERROR_FAIL;
+				}
+			}
+
 			err = cmsis_dap_usb_alloc(dap, packet_size);
 			if (err != ERROR_OK)
 				cmsis_dap_usb_close(dap);
@@ -376,65 +419,178 @@ static int cmsis_dap_usb_open(struct cmsis_dap *dap, uint16_t vids[], uint16_t p
 
 static void cmsis_dap_usb_close(struct cmsis_dap *dap)
 {
+	for (unsigned int i = 0; i < MAX_PENDING_REQUESTS; i++) {
+		libusb_free_transfer(dap->bdata->command_transfers[i].transfer);
+		libusb_free_transfer(dap->bdata->response_transfers[i].transfer);
+	}
+	cmsis_dap_usb_free(dap);
 	libusb_release_interface(dap->bdata->dev_handle, dap->bdata->interface);
 	libusb_close(dap->bdata->dev_handle);
 	libusb_exit(dap->bdata->usb_ctx);
 	free(dap->bdata);
 	dap->bdata = NULL;
-	free(dap->packet_buffer);
-	dap->packet_buffer = NULL;
 }
 
-static int cmsis_dap_usb_read(struct cmsis_dap *dap, int timeout_ms)
+static void LIBUSB_CALL cmsis_dap_usb_callback(struct libusb_transfer *transfer)
+{
+	struct cmsis_dap_bulk_transfer *tr;
+
+	tr = (struct cmsis_dap_bulk_transfer *)transfer->user_data;
+	if (transfer->status == LIBUSB_TRANSFER_COMPLETED) {
+		tr->status = CMSIS_DAP_TRANSFER_COMPLETED;
+		tr->transferred = transfer->actual_length;
+	} else if (transfer->status == LIBUSB_TRANSFER_TIMED_OUT) {
+		tr->status = ERROR_TIMEOUT_REACHED;
+	} else {
+		tr->status = ERROR_JTAG_DEVICE_ERROR;
+	}
+}
+
+static int cmsis_dap_usb_read(struct cmsis_dap *dap, int transfer_timeout_ms,
+							  struct timeval *wait_timeout)
 {
 	int transferred = 0;
 	int err;
+	struct cmsis_dap_bulk_transfer *tr;
+	tr = &dap->bdata->response_transfers[dap->pending_fifo_get_idx];
+
+	if (tr->status == CMSIS_DAP_TRANSFER_IDLE) {
+		libusb_fill_bulk_transfer(tr->transfer,
+								  dap->bdata->dev_handle, dap->bdata->ep_in,
+								  tr->buffer, dap->packet_size,
+								  &cmsis_dap_usb_callback, tr,
+								  transfer_timeout_ms);
+		LOG_DEBUG_IO("submit read @ %u", dap->pending_fifo_get_idx);
+		tr->status = CMSIS_DAP_TRANSFER_PENDING;
+		err = libusb_submit_transfer(tr->transfer);
+		if (err) {
+			tr->status = CMSIS_DAP_TRANSFER_IDLE;
+			LOG_ERROR("error submitting USB read: %s", libusb_strerror(err));
+			return ERROR_FAIL;
+		}
+	}
 
-	err = libusb_bulk_transfer(dap->bdata->dev_handle, dap->bdata->ep_in,
-							dap->packet_buffer, dap->packet_size, &transferred, timeout_ms);
-	if (err) {
-		if (err == LIBUSB_ERROR_TIMEOUT) {
-			return ERROR_TIMEOUT_REACHED;
-		} else {
-			LOG_ERROR("error reading data: %s", libusb_strerror(err));
+	struct timeval tv = {
+		.tv_sec = transfer_timeout_ms / 1000,
+		.tv_usec = transfer_timeout_ms % 1000 * 1000
+	};
+
+	while (tr->status == CMSIS_DAP_TRANSFER_PENDING) {
+		err = libusb_handle_events_timeout_completed(dap->bdata->usb_ctx,
+												 wait_timeout ? wait_timeout : &tv,
+												 &tr->status);
+		if (err) {
+			LOG_ERROR("error handling USB events: %s", libusb_strerror(err));
 			return ERROR_FAIL;
 		}
+		if (wait_timeout)
+			break;
+	}
+
+	if (tr->status < 0 || tr->status == CMSIS_DAP_TRANSFER_COMPLETED) {
+		/* Check related command request for an error */
+		struct cmsis_dap_bulk_transfer *tr_cmd;
+		tr_cmd = &dap->bdata->command_transfers[dap->pending_fifo_get_idx];
+		if (tr_cmd->status < 0) {
+			err = tr_cmd->status;
+			tr_cmd->status = CMSIS_DAP_TRANSFER_IDLE;
+			if (err != ERROR_TIMEOUT_REACHED)
+				LOG_ERROR("error writing USB data");
+			else
+				LOG_DEBUG("command write USB timeout @ %u", dap->pending_fifo_get_idx);
+
+			return err;
+		}
+		if (tr_cmd->status == CMSIS_DAP_TRANSFER_COMPLETED)
+			tr_cmd->status = CMSIS_DAP_TRANSFER_IDLE;
 	}
 
-	memset(&dap->packet_buffer[transferred], 0, dap->packet_buffer_size - transferred);
+	if (tr->status < 0) {
+		err = tr->status;
+		tr->status = CMSIS_DAP_TRANSFER_IDLE;
+		if (err != ERROR_TIMEOUT_REACHED)
+			LOG_ERROR("error reading USB data");
+		else
+			LOG_DEBUG("USB timeout @ %u", dap->pending_fifo_get_idx);
+
+		return err;
+	}
+
+	if (tr->status == CMSIS_DAP_TRANSFER_COMPLETED) {
+		transferred = tr->transferred;
+		LOG_DEBUG_IO("completed read @ %u, transferred %i",
+					 dap->pending_fifo_get_idx, transferred);
+		memcpy(dap->packet_buffer, tr->buffer, transferred);
+		memset(&dap->packet_buffer[transferred], 0, dap->packet_buffer_size - transferred);
+		tr->status = CMSIS_DAP_TRANSFER_IDLE;
+	}
 
 	return transferred;
 }
 
 static int cmsis_dap_usb_write(struct cmsis_dap *dap, int txlen, int timeout_ms)
 {
-	int transferred = 0;
 	int err;
+	struct cmsis_dap_bulk_transfer *tr;
+	tr = &dap->bdata->command_transfers[dap->pending_fifo_put_idx];
+
+	if (tr->status == CMSIS_DAP_TRANSFER_PENDING) {
+		LOG_ERROR("busy command USB transfer at %u", dap->pending_fifo_put_idx);
+		struct timeval tv = {
+			.tv_sec = timeout_ms / 1000,
+			.tv_usec = timeout_ms % 1000 * 1000
+		};
+		libusb_handle_events_timeout_completed(dap->bdata->usb_ctx, &tv, &tr->status);
+	}
+	if (tr->status < 0) {
+		if (tr->status != ERROR_TIMEOUT_REACHED)
+			LOG_ERROR("error writing USB data, late detect");
+		else
+			LOG_DEBUG("USB write timeout @ %u, late detect", dap->pending_fifo_get_idx);
+		tr->status = CMSIS_DAP_TRANSFER_IDLE;
+	}
+	if (tr->status == CMSIS_DAP_TRANSFER_COMPLETED) {
+		LOG_ERROR("USB write: late transfer competed");
+		tr->status = CMSIS_DAP_TRANSFER_IDLE;
+	}
+	if (tr->status != CMSIS_DAP_TRANSFER_IDLE) {
+		libusb_cancel_transfer(tr->transfer);
+		/* TODO: switch to less verbose errors and wait for USB working again */
+		return ERROR_JTAG_DEVICE_ERROR;
+	}
+
+	memcpy(tr->buffer, dap->packet_buffer, txlen);
 
-	/* skip the first byte that is only used by the HID backend */
-	err = libusb_bulk_transfer(dap->bdata->dev_handle, dap->bdata->ep_out,
-							dap->packet_buffer, txlen, &transferred, timeout_ms);
+	libusb_fill_bulk_transfer(tr->transfer,
+							  dap->bdata->dev_handle, dap->bdata->ep_out,
+							  tr->buffer, txlen,
+							  &cmsis_dap_usb_callback, tr,
+							  timeout_ms);
+
+	LOG_DEBUG_IO("submit write @ %u", dap->pending_fifo_put_idx);
+	tr->status = CMSIS_DAP_TRANSFER_PENDING;
+	err = libusb_submit_transfer(tr->transfer);
 	if (err) {
-		if (err == LIBUSB_ERROR_TIMEOUT) {
-			return ERROR_TIMEOUT_REACHED;
-		} else {
-			LOG_ERROR("error writing data: %s", libusb_strerror(err));
-			return ERROR_FAIL;
-		}
+		if (err == LIBUSB_ERROR_BUSY)
+			libusb_cancel_transfer(tr->transfer);
+		else
+			tr->status = CMSIS_DAP_TRANSFER_IDLE;
+
+		LOG_ERROR("error submitting USB write: %s", libusb_strerror(err));
+		return ERROR_FAIL;
 	}
 
-	return transferred;
+	return ERROR_OK;
 }
 
 static int cmsis_dap_usb_alloc(struct cmsis_dap *dap, unsigned int pkt_sz)
 {
-	uint8_t *buf = malloc(pkt_sz);
-	if (!buf) {
+	dap->packet_buffer = malloc(pkt_sz);
+	if (!dap->packet_buffer) {
 		LOG_ERROR("unable to allocate CMSIS-DAP packet buffer");
 		return ERROR_FAIL;
 	}
 
-	dap->packet_buffer = buf;
 	dap->packet_size = pkt_sz;
 	dap->packet_buffer_size = pkt_sz;
 	/* Prevent sending zero size USB packets */
@@ -443,9 +599,54 @@ static int cmsis_dap_usb_alloc(struct cmsis_dap *dap, unsigned int pkt_sz)
 	dap->command = dap->packet_buffer;
 	dap->response = dap->packet_buffer;
 
+	for (unsigned int i = 0; i < MAX_PENDING_REQUESTS; i++) {
+		dap->bdata->command_transfers[i].buffer =
+			libusb_dev_mem_alloc(dap->bdata->dev_handle, pkt_sz);
+		if (!dap->bdata->command_transfers[i].buffer) {
+			LOG_ERROR("unable to allocate CMSIS-DAP packet buffer");
+			return ERROR_FAIL;
+		}
+		dap->bdata->response_transfers[i].buffer =
+			libusb_dev_mem_alloc(dap->bdata->dev_handle, pkt_sz);
+		if (!dap->bdata->response_transfers[i].buffer) {
+			LOG_ERROR("unable to allocate CMSIS-DAP packet buffer");
+			return ERROR_FAIL;
+		}
+	}
+
 	return ERROR_OK;
 }
 
+static void cmsis_dap_usb_free(struct cmsis_dap *dap)
+{
+	for (unsigned int i = 0; i < MAX_PENDING_REQUESTS; i++) {
+		libusb_dev_mem_free(dap->bdata->dev_handle,
+			dap->bdata->command_transfers[i].buffer, dap->packet_size);
+		dap->bdata->command_transfers[i].buffer = NULL;
+		libusb_dev_mem_free(dap->bdata->dev_handle,
+			dap->bdata->response_transfers[i].buffer, dap->packet_size);
+		dap->bdata->response_transfers[i].buffer = NULL;
+	}
+
+	free(dap->packet_buffer);
+	dap->packet_buffer = NULL;
+	dap->command = NULL;
+	dap->response = NULL;
+}
+
+static void cmsis_dap_usb_cancel_all(struct cmsis_dap *dap)
+{
+	for (unsigned int i = 0; i < MAX_PENDING_REQUESTS; i++) {
+		if (dap->bdata->command_transfers[i].status == CMSIS_DAP_TRANSFER_PENDING)
+			libusb_cancel_transfer(dap->bdata->command_transfers[i].transfer);
+		if (dap->bdata->response_transfers[i].status == CMSIS_DAP_TRANSFER_PENDING)
+			libusb_cancel_transfer(dap->bdata->response_transfers[i].transfer);
+
+		dap->bdata->command_transfers[i].status = CMSIS_DAP_TRANSFER_IDLE;
+		dap->bdata->response_transfers[i].status = CMSIS_DAP_TRANSFER_IDLE;
+	}
+}
+
 COMMAND_HANDLER(cmsis_dap_handle_usb_interface_command)
 {
 	if (CMD_ARGC == 1)
@@ -474,4 +675,6 @@ const struct cmsis_dap_backend cmsis_dap_usb_backend = {
 	.read = cmsis_dap_usb_read,
 	.write = cmsis_dap_usb_write,
 	.packet_buffer_alloc = cmsis_dap_usb_alloc,
+	.packet_buffer_free = cmsis_dap_usb_free,
+	.cancel_all = cmsis_dap_usb_cancel_all,
 };
diff --git a/src/jtag/drivers/cmsis_dap_usb_hid.c b/src/jtag/drivers/cmsis_dap_usb_hid.c
index 52dfd76..98ccc3e 100644
--- a/src/jtag/drivers/cmsis_dap_usb_hid.c
+++ b/src/jtag/drivers/cmsis_dap_usb_hid.c
@@ -34,8 +34,39 @@ struct cmsis_dap_backend_data {
 	hid_device *dev_handle;
 };
 
+struct cmsis_dap_report_size {
+	unsigned short vid;
+	unsigned short pid;
+	unsigned int report_size;
+};
+
+static const struct cmsis_dap_report_size report_size_quirks[] = {
+	/* Third gen Atmel tools use a report size of 512 */
+    /* This list of PIDs comes from toolinfo.py in Microchip's pyedbglib. */
+	// Atmel JTAG-ICE 3
+	{ .vid = 0x03eb, .pid = 0x2140, .report_size = 512 },
+	// Atmel-ICE
+	{ .vid = 0x03eb, .pid = 0x2141, .report_size = 512 },
+	// Atmel Power Debugger
+	{ .vid = 0x03eb, .pid = 0x2144, .report_size = 512 },
+	// EDBG (found on Xplained Pro boards)
+	{ .vid = 0x03eb, .pid = 0x2111, .report_size = 512 },
+	// Zero (???)
+	{ .vid = 0x03eb, .pid = 0x2157, .report_size = 512 },
+	// EDBG with Mass Storage (found on Xplained Pro boards)
+	{ .vid = 0x03eb, .pid = 0x2169, .report_size = 512 },
+	// Commercially available EDBG (for third-party use)
+	{ .vid = 0x03eb, .pid = 0x216a, .report_size = 512 },
+	// Kraken (???)
+	{ .vid = 0x03eb, .pid = 0x2170, .report_size = 512 },
+
+	{ .vid = 0, .pid = 0, .report_size = 0 }
+};
+
+
 static void cmsis_dap_hid_close(struct cmsis_dap *dap);
 static int cmsis_dap_hid_alloc(struct cmsis_dap *dap, unsigned int pkt_sz);
+static void cmsis_dap_hid_free(struct cmsis_dap *dap);
 
 static int cmsis_dap_hid_open(struct cmsis_dap *dap, uint16_t vids[], uint16_t pids[], const char *serial)
 {
@@ -138,13 +169,15 @@ static int cmsis_dap_hid_open(struct cmsis_dap *dap, uint16_t vids[], uint16_t p
 
 	unsigned int packet_size = 64;
 
-	/* atmel cmsis-dap uses 512 byte reports */
-	/* except when it doesn't e.g. with mEDBG on SAMD10 Xplained
-	 * board */
+	/* Check for adapters that are known to have unusual report lengths. */
+	for (i = 0; report_size_quirks[i].vid != 0; i++) {
+		if (report_size_quirks[i].vid == target_vid &&
+		    report_size_quirks[i].pid == target_pid) {
+			packet_size = report_size_quirks[i].report_size;
+		}
+	}
 	/* TODO: HID report descriptor should be parsed instead of
-	 * hardcoding a match by VID */
-	if (target_vid == 0x03eb && target_pid != 0x2145 && target_pid != 0x2175)
-		packet_size = 512;
+	 * hardcoding a match by VID/PID */
 
 	dap->bdata->dev_handle = dev;
 
@@ -165,14 +198,21 @@ static void cmsis_dap_hid_close(struct cmsis_dap *dap)
 	hid_exit();
 	free(dap->bdata);
 	dap->bdata = NULL;
-	free(dap->packet_buffer);
-	dap->packet_buffer = NULL;
+	cmsis_dap_hid_free(dap);
 }
 
-static int cmsis_dap_hid_read(struct cmsis_dap *dap, int timeout_ms)
+static int cmsis_dap_hid_read(struct cmsis_dap *dap, int transfer_timeout_ms,
+							  struct timeval *wait_timeout)
 {
-	int retval = hid_read_timeout(dap->bdata->dev_handle, dap->packet_buffer, dap->packet_buffer_size, timeout_ms);
-
+	int timeout_ms;
+	if (wait_timeout)
+		timeout_ms = wait_timeout->tv_usec / 1000 + wait_timeout->tv_sec * 1000;
+	else
+		timeout_ms = transfer_timeout_ms;
+
+	int retval = hid_read_timeout(dap->bdata->dev_handle,
+								  dap->packet_buffer, dap->packet_buffer_size,
+								  timeout_ms);
 	if (retval == 0) {
 		return ERROR_TIMEOUT_REACHED;
 	} else if (retval == -1) {
@@ -222,6 +262,16 @@ static int cmsis_dap_hid_alloc(struct cmsis_dap *dap, unsigned int pkt_sz)
 	return ERROR_OK;
 }
 
+static void cmsis_dap_hid_free(struct cmsis_dap *dap)
+{
+	free(dap->packet_buffer);
+	dap->packet_buffer = NULL;
+}
+
+static void cmsis_dap_hid_cancel_all(struct cmsis_dap *dap)
+{
+}
+
 const struct cmsis_dap_backend cmsis_dap_hid_backend = {
 	.name = "hid",
 	.open = cmsis_dap_hid_open,
@@ -229,4 +279,6 @@ const struct cmsis_dap_backend cmsis_dap_hid_backend = {
 	.read = cmsis_dap_hid_read,
 	.write = cmsis_dap_hid_write,
 	.packet_buffer_alloc = cmsis_dap_hid_alloc,
+	.packet_buffer_free = cmsis_dap_hid_free,
+	.cancel_all = cmsis_dap_hid_cancel_all,
 };
diff --git a/src/jtag/drivers/driver.c b/src/jtag/drivers/driver.c
index 409b800..7732315 100644
--- a/src/jtag/drivers/driver.c
+++ b/src/jtag/drivers/driver.c
@@ -76,13 +76,13 @@ int interface_jtag_add_ir_scan(struct jtag_tap *active,
 
 		if (tap == active) {
 			/* if TAP is listed in input fields, copy the value */
-			tap->bypass = 0;
+			tap->bypass = false;
 
 			jtag_scan_field_clone(field, in_fields);
 		} else {
 			/* if a TAP isn't listed in input fields, set it to BYPASS */
 
-			tap->bypass = 1;
+			tap->bypass = true;
 
 			field->num_bits = tap->ir_length;
 			field->out_value = buf_set_ones(cmd_queue_alloc(DIV_ROUND_UP(tap->ir_length, 8)), tap->ir_length);
diff --git a/src/jtag/drivers/ftdi.c b/src/jtag/drivers/ftdi.c
index dd8cb16..880e90f 100644
--- a/src/jtag/drivers/ftdi.c
+++ b/src/jtag/drivers/ftdi.c
@@ -1213,7 +1213,7 @@ COMMAND_HANDLER(ftdi_handle_set_signal_command)
 		/* fallthrough */
 	default:
 		LOG_ERROR("unknown signal level '%s', use 0, 1 or z", CMD_ARGV[1]);
-		return ERROR_COMMAND_SYNTAX_ERROR;
+		return ERROR_COMMAND_ARGUMENT_INVALID;
 	}
 
 	return mpsse_flush(mpsse_ctx);
diff --git a/src/jtag/drivers/jlink.c b/src/jtag/drivers/jlink.c
index 243d1a4..5743d8d 100644
--- a/src/jtag/drivers/jlink.c
+++ b/src/jtag/drivers/jlink.c
@@ -966,19 +966,17 @@ COMMAND_HANDLER(jlink_usb_command)
 {
 	int tmp;
 
-	if (CMD_ARGC != 1) {
-		command_print(CMD, "Need exactly one argument for jlink usb");
+	if (CMD_ARGC != 1)
 		return ERROR_COMMAND_SYNTAX_ERROR;
-	}
 
 	if (sscanf(CMD_ARGV[0], "%i", &tmp) != 1) {
 		command_print(CMD, "Invalid USB address: %s", CMD_ARGV[0]);
-		return ERROR_FAIL;
+		return ERROR_COMMAND_ARGUMENT_INVALID;
 	}
 
 	if (tmp < JAYLINK_USB_ADDRESS_0 || tmp > JAYLINK_USB_ADDRESS_3) {
 		command_print(CMD, "Invalid USB address: %s", CMD_ARGV[0]);
-		return ERROR_FAIL;
+		return ERROR_COMMAND_ARGUMENT_INVALID;
 	}
 
 	usb_address = tmp;
@@ -1059,7 +1057,7 @@ COMMAND_HANDLER(jlink_handle_jlink_jtag_command)
 	} else if (CMD_ARGC == 1) {
 		if (sscanf(CMD_ARGV[0], "%i", &tmp) != 1) {
 			command_print(CMD, "Invalid argument: %s", CMD_ARGV[0]);
-			return ERROR_COMMAND_SYNTAX_ERROR;
+			return ERROR_COMMAND_ARGUMENT_INVALID;
 		}
 
 		switch (tmp) {
@@ -1071,10 +1069,9 @@ COMMAND_HANDLER(jlink_handle_jlink_jtag_command)
 				break;
 			default:
 				command_print(CMD, "Invalid argument: %s", CMD_ARGV[0]);
-				return ERROR_COMMAND_SYNTAX_ERROR;
+				return ERROR_COMMAND_ARGUMENT_INVALID;
 		}
 	} else {
-		command_print(CMD, "Need exactly one argument for jlink jtag");
 		return ERROR_COMMAND_SYNTAX_ERROR;
 	}
 
@@ -1086,10 +1083,8 @@ COMMAND_HANDLER(jlink_handle_target_power_command)
 	int ret;
 	int enable;
 
-	if (CMD_ARGC != 1) {
-		command_print(CMD, "Need exactly one argument for jlink targetpower");
+	if (CMD_ARGC != 1)
 		return ERROR_COMMAND_SYNTAX_ERROR;
-	}
 
 	if (!jaylink_has_cap(caps, JAYLINK_DEV_CAP_SET_TARGET_POWER)) {
 		command_print(CMD, "Target power supply is not supported by the "
@@ -1428,17 +1423,16 @@ COMMAND_HANDLER(jlink_handle_config_usb_address_command)
 	} else if (CMD_ARGC == 1) {
 		if (sscanf(CMD_ARGV[0], "%" SCNd8, &tmp) != 1) {
 			command_print(CMD, "Invalid USB address: %s", CMD_ARGV[0]);
-			return ERROR_FAIL;
+			return ERROR_COMMAND_ARGUMENT_INVALID;
 		}
 
 		if (tmp > JAYLINK_USB_ADDRESS_3) {
 			command_print(CMD, "Invalid USB address: %u", tmp);
-			return ERROR_FAIL;
+			return ERROR_COMMAND_ARGUMENT_INVALID;
 		}
 
 		tmp_config.usb_address = tmp;
 	} else {
-		command_print(CMD, "Need exactly one argument for jlink config usb");
 		return ERROR_COMMAND_SYNTAX_ERROR;
 	}
 
@@ -1470,13 +1464,11 @@ COMMAND_HANDLER(jlink_handle_config_target_power_command)
 			enable = false;
 		} else {
 			command_print(CMD, "Invalid argument: %s", CMD_ARGV[0]);
-			return ERROR_FAIL;
+			return ERROR_COMMAND_ARGUMENT_INVALID;
 		}
 
 		tmp_config.target_power = enable;
 	} else {
-		command_print(CMD, "Need exactly one argument for jlink config "
-			"targetpower");
 		return ERROR_COMMAND_SYNTAX_ERROR;
 	}
 
@@ -1510,7 +1502,7 @@ COMMAND_HANDLER(jlink_handle_config_mac_address_command)
 		if ((strlen(str) != 17) || (str[2] != ':' || str[5] != ':' ||
 				str[8] != ':' || str[11] != ':' || str[14] != ':')) {
 			command_print(CMD, "Invalid MAC address format");
-			return ERROR_COMMAND_SYNTAX_ERROR;
+			return ERROR_COMMAND_ARGUMENT_INVALID;
 		}
 
 		for (i = 5; i >= 0; i--) {
@@ -1520,17 +1512,16 @@ COMMAND_HANDLER(jlink_handle_config_mac_address_command)
 
 		if (!(addr[0] | addr[1] | addr[2] | addr[3] | addr[4] | addr[5])) {
 			command_print(CMD, "Invalid MAC address: zero address");
-			return ERROR_COMMAND_SYNTAX_ERROR;
+			return ERROR_COMMAND_ARGUMENT_INVALID;
 		}
 
 		if (!(0x01 & addr[0])) {
 			command_print(CMD, "Invalid MAC address: multicast address");
-			return ERROR_COMMAND_SYNTAX_ERROR;
+			return ERROR_COMMAND_ARGUMENT_INVALID;
 		}
 
 		memcpy(tmp_config.mac_address, addr, sizeof(addr));
 	} else {
-		command_print(CMD, "Need exactly one argument for jlink config mac");
 		return ERROR_COMMAND_SYNTAX_ERROR;
 	}
 
@@ -1592,20 +1583,26 @@ COMMAND_HANDLER(jlink_handle_config_ip_address_command)
 	if (!CMD_ARGC) {
 		show_config_ip_address(CMD);
 	} else {
-		if (!string_to_ip(CMD_ARGV[0], ip_address, &i))
-			return ERROR_COMMAND_SYNTAX_ERROR;
+		if (!string_to_ip(CMD_ARGV[0], ip_address, &i)) {
+			command_print(CMD, "invalid IPv4 address");
+			return ERROR_COMMAND_ARGUMENT_INVALID;
+		}
 
 		len = strlen(CMD_ARGV[0]);
 
 		/* Check for format A.B.C.D/E. */
 		if (i < len) {
-			if (CMD_ARGV[0][i] != '/')
-				return ERROR_COMMAND_SYNTAX_ERROR;
+			if (CMD_ARGV[0][i] != '/') {
+				command_print(CMD, "missing network mask");
+				return ERROR_COMMAND_ARGUMENT_INVALID;
+			}
 
 			COMMAND_PARSE_NUMBER(u8, CMD_ARGV[0] + i + 1, subnet_bits);
 		} else if (CMD_ARGC > 1) {
-			if (!string_to_ip(CMD_ARGV[1], (uint8_t *)&subnet_mask, &i))
-				return ERROR_COMMAND_SYNTAX_ERROR;
+			if (!string_to_ip(CMD_ARGV[1], (uint8_t *)&subnet_mask, &i)) {
+				command_print(CMD, "invalid subnet mask");
+				return ERROR_COMMAND_ARGUMENT_INVALID;
+			}
 		}
 
 		if (!subnet_mask)
diff --git a/src/jtag/drivers/jtag_vpi.c b/src/jtag/drivers/jtag_vpi.c
index f19e9ab..c2b3b08 100644
--- a/src/jtag/drivers/jtag_vpi.c
+++ b/src/jtag/drivers/jtag_vpi.c
@@ -593,10 +593,8 @@ static int jtag_vpi_quit(void)
 
 COMMAND_HANDLER(jtag_vpi_set_port)
 {
-	if (CMD_ARGC == 0) {
-		LOG_ERROR("Command \"jtag_vpi set_port\" expects 1 argument (TCP port number)");
+	if (CMD_ARGC == 0)
 		return ERROR_COMMAND_SYNTAX_ERROR;
-	}
 
 	COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], server_port);
 	LOG_INFO("jtag_vpi: server port set to %u", server_port);
@@ -607,10 +605,8 @@ COMMAND_HANDLER(jtag_vpi_set_port)
 COMMAND_HANDLER(jtag_vpi_set_address)
 {
 
-	if (CMD_ARGC == 0) {
-		LOG_ERROR("Command \"jtag_vpi set_address\" expects 1 argument (IP address)");
+	if (CMD_ARGC == 0)
 		return ERROR_COMMAND_SYNTAX_ERROR;
-	}
 
 	free(server_address);
 	server_address = strdup(CMD_ARGV[0]);
@@ -621,10 +617,8 @@ COMMAND_HANDLER(jtag_vpi_set_address)
 
 COMMAND_HANDLER(jtag_vpi_stop_sim_on_exit_handler)
 {
-	if (CMD_ARGC != 1) {
-		LOG_ERROR("Command \"jtag_vpi stop_sim_on_exit\" expects 1 argument (on|off)");
+	if (CMD_ARGC != 1)
 		return ERROR_COMMAND_SYNTAX_ERROR;
-	}
 
 	COMMAND_PARSE_ON_OFF(CMD_ARGV[0], stop_sim_on_exit);
 	return ERROR_OK;
diff --git a/src/jtag/drivers/kitprog.c b/src/jtag/drivers/kitprog.c
index e126a9c..c0d2adc 100644
--- a/src/jtag/drivers/kitprog.c
+++ b/src/jtag/drivers/kitprog.c
@@ -879,6 +879,13 @@ static const struct command_registration kitprog_subcommand_handlers[] = {
 		.usage = "",
 		.help = "try to acquire a PSoC",
 	},
+	{
+		.name = "init_acquire_psoc",
+		.handler = &kitprog_handle_init_acquire_psoc_command,
+		.mode = COMMAND_CONFIG,
+		.help = "try to acquire a PSoC during init",
+		.usage = "",
+	},
 	COMMAND_REGISTRATION_DONE
 };
 
@@ -890,13 +897,6 @@ static const struct command_registration kitprog_command_handlers[] = {
 		.usage = "<cmd>",
 		.chain = kitprog_subcommand_handlers,
 	},
-	{
-		.name = "kitprog_init_acquire_psoc",
-		.handler = &kitprog_handle_init_acquire_psoc_command,
-		.mode = COMMAND_CONFIG,
-		.help = "try to acquire a PSoC during init",
-		.usage = "",
-	},
 	COMMAND_REGISTRATION_DONE
 };
 
diff --git a/src/jtag/drivers/remote_bitbang.c b/src/jtag/drivers/remote_bitbang.c
index 36bcbe1..261c30d 100644
--- a/src/jtag/drivers/remote_bitbang.c
+++ b/src/jtag/drivers/remote_bitbang.c
@@ -3,6 +3,8 @@
 /***************************************************************************
  *   Copyright (C) 2011 by Richard Uhler                                   *
  *   ruhler@mit.edu                                                        *
+ *                                                                         *
+ *   Copyright (C) 2021 by Manuel Wick <manuel@matronix.de>                *
  ***************************************************************************/
 
 #ifdef HAVE_CONFIG_H
@@ -220,11 +222,35 @@ static int remote_bitbang_blink(int on)
 	return remote_bitbang_queue(c, FLUSH_SEND_BUF);
 }
 
+static void remote_bitbang_swdio_drive(bool is_output)
+{
+	char c = is_output ? 'O' : 'o';
+	if (remote_bitbang_queue(c, FLUSH_SEND_BUF) == ERROR_FAIL)
+		LOG_ERROR("Error setting direction for swdio");
+}
+
+static int remote_bitbang_swdio_read(void)
+{
+	if (remote_bitbang_queue('c', FLUSH_SEND_BUF) != ERROR_FAIL)
+		return remote_bitbang_read_sample();
+	else
+		return BB_ERROR;
+}
+
+static int remote_bitbang_swd_write(int swclk, int swdio)
+{
+	char c = 'd' + ((swclk ? 0x2 : 0x0) | (swdio ? 0x1 : 0x0));
+	return remote_bitbang_queue(c, NO_FLUSH);
+}
+
 static struct bitbang_interface remote_bitbang_bitbang = {
 	.buf_size = sizeof(remote_bitbang_recv_buf) - 1,
 	.sample = &remote_bitbang_sample,
 	.read_sample = &remote_bitbang_read_sample,
 	.write = &remote_bitbang_write,
+	.swdio_read = &remote_bitbang_swdio_read,
+	.swdio_drive = &remote_bitbang_swdio_drive,
+	.swd_write = &remote_bitbang_swd_write,
 	.blink = &remote_bitbang_blink,
 };
 
@@ -349,6 +375,8 @@ COMMAND_HANDLER(remote_bitbang_handle_remote_bitbang_host_command)
 	return ERROR_COMMAND_SYNTAX_ERROR;
 }
 
+static const char * const remote_bitbang_transports[] = { "jtag", "swd", NULL };
+
 static const struct command_registration remote_bitbang_subcommand_handlers[] = {
 	{
 		.name = "port",
@@ -401,7 +429,7 @@ static struct jtag_interface remote_bitbang_interface = {
 
 struct adapter_driver remote_bitbang_adapter_driver = {
 	.name = "remote_bitbang",
-	.transports = jtag_only,
+	.transports = remote_bitbang_transports,
 	.commands = remote_bitbang_command_handlers,
 
 	.init = &remote_bitbang_init,
@@ -409,4 +437,5 @@ struct adapter_driver remote_bitbang_adapter_driver = {
 	.reset = &remote_bitbang_reset,
 
 	.jtag_ops = &remote_bitbang_interface,
+	.swd_ops = &bitbang_swd,
 };
diff --git a/src/jtag/drivers/rshim.c b/src/jtag/drivers/rshim.c
index 6170e86..21fc7fd 100644
--- a/src/jtag/drivers/rshim.c
+++ b/src/jtag/drivers/rshim.c
@@ -434,10 +434,8 @@ static void rshim_disconnect(struct adiv5_dap *dap)
 
 COMMAND_HANDLER(rshim_dap_device_command)
 {
-	if (CMD_ARGC != 1) {
-		command_print(CMD, "Too many arguments");
+	if (CMD_ARGC != 1)
 		return ERROR_COMMAND_SYNTAX_ERROR;
-	}
 
 	free(rshim_dev_path);
 	rshim_dev_path = strdup(CMD_ARGV[0]);
diff --git a/src/jtag/hla/hla_interface.c b/src/jtag/hla/hla_interface.c
index f4bfeb1..9c8d0fa 100644
--- a/src/jtag/hla/hla_interface.c
+++ b/src/jtag/hla/hla_interface.c
@@ -100,7 +100,7 @@ int hl_interface_init_target(struct target *t)
 	}
 
 	t->tap->priv = &hl_if;
-	t->tap->hasidcode = 1;
+	t->tap->has_idcode = true;
 
 	return ERROR_OK;
 }
diff --git a/src/jtag/jtag.h b/src/jtag/jtag.h
index 04d1b4a..1d1c495 100644
--- a/src/jtag/jtag.h
+++ b/src/jtag/jtag.h
@@ -115,7 +115,7 @@ struct jtag_tap {
 	uint32_t idcode; /**< device identification code */
 	/** not all devices have idcode,
 	 * we'll discover this during chain examination */
-	bool hasidcode;
+	bool has_idcode;
 
 	/** Array of expected identification codes */
 	uint32_t *expected_ids;
@@ -131,7 +131,7 @@ struct jtag_tap {
 	/** current instruction */
 	uint8_t *cur_instr;
 	/** Bypass register selected */
-	int bypass;
+	bool bypass;
 
 	struct jtag_tap_event_action *event_action;
 
diff --git a/src/jtag/startup.tcl b/src/jtag/startup.tcl
index 085c89b..4eca677 100644
--- a/src/jtag/startup.tcl
+++ b/src/jtag/startup.tcl
@@ -1108,6 +1108,30 @@ proc "am335xgpio led_on_state" {state} {
 	}
 }
 
+lappend _telnet_autocomplete_skip "cmsis_dap_backend"
+proc "cmsis_dap_backend" {backend} {
+	echo "DEPRECATED! use 'cmsis-dap backend', not 'cmsis_dap_backend'"
+	eval cmsis-dap backend $backend
+}
+
+lappend _telnet_autocomplete_skip "cmsis_dap_vid_pid"
+proc "cmsis_dap_vid_pid" {args} {
+	echo "DEPRECATED! use 'cmsis-dap vid_pid', not 'cmsis_dap_vid_pid'"
+	eval cmsis-dap vid_pid $args
+}
+
+lappend _telnet_autocomplete_skip "cmsis_dap_usb"
+proc "cmsis_dap_usb" {args} {
+	echo "DEPRECATED! use 'cmsis-dap usb', not 'cmsis_dap_usb'"
+	eval cmsis-dap usb $args
+}
+
+lappend _telnet_autocomplete_skip "kitprog_init_acquire_psoc"
+proc "kitprog_init_acquire_psoc" {} {
+	echo "DEPRECATED! use 'kitprog init_acquire_psoc', not 'kitprog_init_acquire_psoc'"
+	eval kitprog init_acquire_psoc
+}
+
 lappend _telnet_autocomplete_skip "pld device"
 proc "pld device" {driver tap_name {opt 0}} {
 	echo "DEPRECATED! use 'pld create ...', not 'pld device ...'"
diff --git a/src/pld/intel.c b/src/pld/intel.c
index 8422c94..a39e16c 100644
--- a/src/pld/intel.c
+++ b/src/pld/intel.c
@@ -157,7 +157,7 @@ static int intel_check_for_unique_id(struct intel_pld_device *intel_info)
 
 static int intel_check_config(struct intel_pld_device *intel_info)
 {
-	if (!intel_info->tap->hasidcode) {
+	if (!intel_info->tap->has_idcode) {
 		LOG_ERROR("no IDCODE");
 		return ERROR_FAIL;
 	}
diff --git a/src/pld/lattice.c b/src/pld/lattice.c
index cd72d3c..2997cdc 100644
--- a/src/pld/lattice.c
+++ b/src/pld/lattice.c
@@ -81,7 +81,7 @@ static int lattice_check_device_family(struct lattice_pld_device *lattice_device
 	if (lattice_device->family != LATTICE_UNKNOWN && lattice_device->preload_length != 0)
 		return ERROR_OK;
 
-	if (!lattice_device->tap || !lattice_device->tap->hasidcode)
+	if (!lattice_device->tap || !lattice_device->tap->has_idcode)
 		return ERROR_FAIL;
 
 	for (size_t i = 0; i < ARRAY_SIZE(lattice_devices); ++i) {
@@ -280,7 +280,7 @@ static int lattice_load_command(struct pld_device *pld_device, const char *filen
 		return ERROR_FAIL;
 	struct jtag_tap *tap = lattice_device->tap;
 
-	if (!tap || !tap->hasidcode)
+	if (!tap || !tap->has_idcode)
 		return ERROR_FAIL;
 
 	int retval = lattice_check_device_family(lattice_device);
diff --git a/src/rtt/tcl.c b/src/rtt/tcl.c
index f949aa1..2b8822f 100644
--- a/src/rtt/tcl.c
+++ b/src/rtt/tcl.c
@@ -17,7 +17,7 @@
 
 COMMAND_HANDLER(handle_rtt_setup_command)
 {
-struct rtt_source source;
+	struct rtt_source source;
 
 	if (CMD_ARGC != 3)
 		return ERROR_COMMAND_SYNTAX_ERROR;
diff --git a/src/server/rtt_server.c b/src/server/rtt_server.c
index df2247b..9769153 100644
--- a/src/server/rtt_server.c
+++ b/src/server/rtt_server.c
@@ -25,6 +25,7 @@
 
 struct rtt_service {
 	unsigned int channel;
+	char *hello_message;
 };
 
 static int read_callback(unsigned int channel, const uint8_t *buffer,
@@ -65,6 +66,9 @@ static int rtt_new_connection(struct connection *connection)
 	if (ret != ERROR_OK)
 		return ret;
 
+	if (service->hello_message)
+		connection_write(connection, service->hello_message, strlen(service->hello_message));
+
 	return ERROR_OK;
 }
 
@@ -117,16 +121,30 @@ COMMAND_HANDLER(handle_rtt_start_command)
 	int ret;
 	struct rtt_service *service;
 
-	if (CMD_ARGC != 2)
+	if (CMD_ARGC < 2 || CMD_ARGC > 3)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
-	service = malloc(sizeof(struct rtt_service));
+	service = calloc(1, sizeof(struct rtt_service));
 
 	if (!service)
 		return ERROR_FAIL;
 
 	COMMAND_PARSE_NUMBER(uint, CMD_ARGV[1], service->channel);
 
+	if (CMD_ARGC >= 3) {
+		const char *hello_message = CMD_ARGV[2];
+		size_t hello_length = strlen(hello_message);
+
+		service->hello_message = malloc(hello_length + 2);
+		if (!service->hello_message) {
+			LOG_ERROR("Out of memory");
+			free(service);
+			return ERROR_FAIL;
+		}
+		strcpy(service->hello_message, hello_message);
+		service->hello_message[hello_length] = '\n';
+		service->hello_message[hello_length + 1] = '\0';
+	}
 	ret = add_service(&rtt_service_driver, CMD_ARGV[0], CONNECTION_LIMIT_UNLIMITED, service);
 
 	if (ret != ERROR_OK) {
@@ -153,7 +171,7 @@ static const struct command_registration rtt_server_subcommand_handlers[] = {
 		.handler = handle_rtt_start_command,
 		.mode = COMMAND_ANY,
 		.help = "Start a RTT server",
-		.usage = "<port> <channel>"
+		.usage = "<port> <channel> [message]"
 	},
 	{
 		.name = "stop",
diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index db60243..1c056a0 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -105,7 +105,7 @@ static int aarch64_restore_system_control_reg(struct target *target)
 		if (target_mode != ARM_MODE_ANY)
 			armv8_dpm_modeswitch(&armv8->dpm, target_mode);
 
-		retval = armv8->dpm.instr_write_data_r0(&armv8->dpm, instr, aarch64->system_control_reg);
+		retval = armv8->dpm.instr_write_data_r0_64(&armv8->dpm, instr, aarch64->system_control_reg);
 		if (retval != ERROR_OK)
 			return retval;
 
@@ -182,7 +182,7 @@ static int aarch64_mmu_modify(struct target *target, int enable)
 	if (target_mode != ARM_MODE_ANY)
 		armv8_dpm_modeswitch(&armv8->dpm, target_mode);
 
-	retval = armv8->dpm.instr_write_data_r0(&armv8->dpm, instr,
+	retval = armv8->dpm.instr_write_data_r0_64(&armv8->dpm, instr,
 				aarch64->system_control_reg_curr);
 
 	if (target_mode != ARM_MODE_ANY)
@@ -1055,14 +1055,14 @@ static int aarch64_post_debug_entry(struct target *target)
 	if (target_mode != ARM_MODE_ANY)
 		armv8_dpm_modeswitch(&armv8->dpm, target_mode);
 
-	retval = armv8->dpm.instr_read_data_r0(&armv8->dpm, instr, &aarch64->system_control_reg);
+	retval = armv8->dpm.instr_read_data_r0_64(&armv8->dpm, instr, &aarch64->system_control_reg);
 	if (retval != ERROR_OK)
 		return retval;
 
 	if (target_mode != ARM_MODE_ANY)
 		armv8_dpm_modeswitch(&armv8->dpm, ARM_MODE_ANY);
 
-	LOG_DEBUG("System_register: %8.8" PRIx32, aarch64->system_control_reg);
+	LOG_DEBUG("System_register: %8.8" PRIx64, aarch64->system_control_reg);
 	aarch64->system_control_reg_curr = aarch64->system_control_reg;
 
 	if (armv8->armv8_mmu.armv8_cache.info == -1) {
diff --git a/src/target/aarch64.h b/src/target/aarch64.h
index 2721fe7..b265e82 100644
--- a/src/target/aarch64.h
+++ b/src/target/aarch64.h
@@ -43,8 +43,8 @@ struct aarch64_common {
 	struct armv8_common armv8_common;
 
 	/* Context information */
-	uint32_t system_control_reg;
-	uint32_t system_control_reg_curr;
+	uint64_t system_control_reg;
+	uint64_t system_control_reg_curr;
 
 	/* Breakpoint register pairs */
 	int brp_num_context;
diff --git a/src/target/armv8_cache.c b/src/target/armv8_cache.c
index cf71119..66d4e00 100644
--- a/src/target/armv8_cache.c
+++ b/src/target/armv8_cache.c
@@ -243,7 +243,7 @@ static int  armv8_flush_all_data(struct target *target)
 		foreach_smp_target(head, target->smp_targets) {
 			struct target *curr = head->target;
 			if (curr->state == TARGET_HALTED) {
-				LOG_INFO("Wait flushing data l1 on core %" PRId32, curr->coreid);
+				LOG_TARGET_INFO(curr, "Wait flushing data l1.");
 				retval = _armv8_flush_all_data(curr);
 			}
 		}
@@ -286,8 +286,9 @@ static struct armv8_cachesize decode_cache_reg(uint32_t cache_reg)
 	size.index = (cache_reg >> 13) & 0x7fff;
 	size.way = ((cache_reg >> 3) & 0x3ff);
 
-	while (((size.way << i) & 0x80000000) == 0)
-		i++;
+	if (size.way != 0)
+		while (((size.way << i) & 0x80000000) == 0)
+			i++;
 	size.way_shift = i;
 
 	return size;
diff --git a/src/target/breakpoints.c b/src/target/breakpoints.c
index d069c6b..77f7673 100644
--- a/src/target/breakpoints.c
+++ b/src/target/breakpoints.c
@@ -53,7 +53,7 @@ static int breakpoint_add_internal(struct target *target,
 			 * breakpoint" ... check all the parameters before
 			 * succeeding.
 			 */
-			LOG_ERROR("Duplicate Breakpoint address: " TARGET_ADDR_FMT " (BP %" PRIu32 ")",
+			LOG_TARGET_ERROR(target, "Duplicate Breakpoint address: " TARGET_ADDR_FMT " (BP %" PRIu32 ")",
 				address, breakpoint->unique_id);
 			return ERROR_TARGET_DUPLICATE_BREAKPOINT;
 		}
@@ -84,16 +84,15 @@ static int breakpoint_add_internal(struct target *target,
 		default:
 			reason = "unknown reason";
 fail:
-			LOG_ERROR("can't add breakpoint: %s", reason);
+			LOG_TARGET_ERROR(target, "can't add breakpoint: %s", reason);
 			free((*breakpoint_p)->orig_instr);
 			free(*breakpoint_p);
 			*breakpoint_p = NULL;
 			return retval;
 	}
 
-	LOG_DEBUG("[%d] added %s breakpoint at " TARGET_ADDR_FMT
+	LOG_TARGET_DEBUG(target, "added %s breakpoint at " TARGET_ADDR_FMT
 			" of length 0x%8.8x, (BPID: %" PRIu32 ")",
-		target->coreid,
 		breakpoint_type_strings[(*breakpoint_p)->type],
 		(*breakpoint_p)->address, (*breakpoint_p)->length,
 		(*breakpoint_p)->unique_id);
@@ -135,14 +134,14 @@ static int context_breakpoint_add_internal(struct target *target,
 	(*breakpoint_p)->unique_id = bpwp_unique_id++;
 	retval = target_add_context_breakpoint(target, *breakpoint_p);
 	if (retval != ERROR_OK) {
-		LOG_ERROR("could not add breakpoint");
+		LOG_TARGET_ERROR(target, "could not add breakpoint");
 		free((*breakpoint_p)->orig_instr);
 		free(*breakpoint_p);
 		*breakpoint_p = NULL;
 		return retval;
 	}
 
-	LOG_DEBUG("added %s Context breakpoint at 0x%8.8" PRIx32 " of length 0x%8.8x, (BPID: %" PRIu32 ")",
+	LOG_TARGET_DEBUG(target, "added %s Context breakpoint at 0x%8.8" PRIx32 " of length 0x%8.8x, (BPID: %" PRIu32 ")",
 		breakpoint_type_strings[(*breakpoint_p)->type],
 		(*breakpoint_p)->asid, (*breakpoint_p)->length,
 		(*breakpoint_p)->unique_id);
@@ -166,11 +165,11 @@ static int hybrid_breakpoint_add_internal(struct target *target,
 			 * breakpoint" ... check all the parameters before
 			 * succeeding.
 			 */
-			LOG_ERROR("Duplicate Hybrid Breakpoint asid: 0x%08" PRIx32 " (BP %" PRIu32 ")",
+			LOG_TARGET_ERROR(target, "Duplicate Hybrid Breakpoint asid: 0x%08" PRIx32 " (BP %" PRIu32 ")",
 				asid, breakpoint->unique_id);
 			return ERROR_TARGET_DUPLICATE_BREAKPOINT;
 		} else if ((breakpoint->address == address) && (breakpoint->asid == 0)) {
-			LOG_ERROR("Duplicate Breakpoint IVA: " TARGET_ADDR_FMT " (BP %" PRIu32 ")",
+			LOG_TARGET_ERROR(target, "Duplicate Breakpoint IVA: " TARGET_ADDR_FMT " (BP %" PRIu32 ")",
 				address, breakpoint->unique_id);
 			return ERROR_TARGET_DUPLICATE_BREAKPOINT;
 
@@ -191,13 +190,13 @@ static int hybrid_breakpoint_add_internal(struct target *target,
 
 	retval = target_add_hybrid_breakpoint(target, *breakpoint_p);
 	if (retval != ERROR_OK) {
-		LOG_ERROR("could not add breakpoint");
+		LOG_TARGET_ERROR(target, "could not add breakpoint");
 		free((*breakpoint_p)->orig_instr);
 		free(*breakpoint_p);
 		*breakpoint_p = NULL;
 		return retval;
 	}
-	LOG_DEBUG(
+	LOG_TARGET_DEBUG(target,
 		"added %s Hybrid breakpoint at address " TARGET_ADDR_FMT " of length 0x%8.8x, (BPID: %" PRIu32 ")",
 		breakpoint_type_strings[(*breakpoint_p)->type],
 		(*breakpoint_p)->address,
@@ -307,7 +306,7 @@ static int breakpoint_free(struct target *data_target, struct target *breakpoint
 		return retval;
 	}
 
-	LOG_DEBUG("free BPID: %" PRIu32 " --> %d", breakpoint->unique_id, retval);
+	LOG_TARGET_DEBUG(data_target, "free BPID: %" PRIu32 " --> %d", breakpoint->unique_id, retval);
 	(*breakpoint_p) = breakpoint->next;
 	free(breakpoint->orig_instr);
 	free(breakpoint);
@@ -445,7 +444,7 @@ static int watchpoint_free(struct target *target, struct watchpoint *watchpoint_
 		return retval;
 	}
 
-	LOG_DEBUG("free WPID: %d --> %d", watchpoint->unique_id, retval);
+	LOG_TARGET_DEBUG(target, "free WPID: %d --> %d", watchpoint->unique_id, retval);
 	(*watchpoint_p) = watchpoint->next;
 	free(watchpoint);
 
@@ -556,7 +555,7 @@ static int watchpoint_add_internal(struct target *target, target_addr_t address,
 				|| watchpoint->value != value
 				|| watchpoint->mask != mask
 				|| watchpoint->rw != rw) {
-				LOG_ERROR("address " TARGET_ADDR_FMT
+				LOG_TARGET_ERROR(target, "address " TARGET_ADDR_FMT
 					" already has watchpoint %d",
 					address, watchpoint->unique_id);
 				return ERROR_FAIL;
@@ -590,7 +589,7 @@ static int watchpoint_add_internal(struct target *target, target_addr_t address,
 		default:
 			reason = "unrecognized error";
 bye:
-			LOG_ERROR("can't add %s watchpoint at " TARGET_ADDR_FMT ", %s",
+			LOG_TARGET_ERROR(target, "can't add %s watchpoint at " TARGET_ADDR_FMT ", %s",
 				watchpoint_rw_strings[(*watchpoint_p)->rw],
 				address, reason);
 			free(*watchpoint_p);
@@ -598,9 +597,8 @@ bye:
 			return retval;
 	}
 
-	LOG_DEBUG("[%d] added %s watchpoint at " TARGET_ADDR_FMT
+	LOG_TARGET_DEBUG(target, "added %s watchpoint at " TARGET_ADDR_FMT
 			" of length 0x%8.8" PRIx32 " (WPID: %d)",
-		target->coreid,
 		watchpoint_rw_strings[(*watchpoint_p)->rw],
 		(*watchpoint_p)->address,
 		(*watchpoint_p)->length,
@@ -718,7 +716,7 @@ int watchpoint_hit(struct target *target, enum watchpoint_rw *rw,
 	*rw = hit_watchpoint->rw;
 	*address = hit_watchpoint->address;
 
-	LOG_DEBUG("Found hit watchpoint at " TARGET_ADDR_FMT " (WPID: %d)",
+	LOG_TARGET_DEBUG(target, "Found hit watchpoint at " TARGET_ADDR_FMT " (WPID: %d)",
 		hit_watchpoint->address,
 		hit_watchpoint->unique_id);
 
diff --git a/src/target/cortex_a.c b/src/target/cortex_a.c
index ba3349d..7fa0c4e 100644
--- a/src/target/cortex_a.c
+++ b/src/target/cortex_a.c
@@ -2989,29 +2989,29 @@ static int cortex_a_examine_first(struct target *target)
 				    armv7a->debug_base + CPUDBG_PRSR, &dbg_osreg);
 	if (retval != ERROR_OK)
 		return retval;
-	LOG_DEBUG("target->coreid %" PRId32 " DBGPRSR  0x%" PRIx32, target->coreid, dbg_osreg);
+	LOG_TARGET_DEBUG(target, "DBGPRSR  0x%" PRIx32, dbg_osreg);
 
 	if ((dbg_osreg & PRSR_POWERUP_STATUS) == 0) {
-		LOG_ERROR("target->coreid %" PRId32 " powered down!", target->coreid);
+		LOG_TARGET_ERROR(target, "powered down!");
 		target->state = TARGET_UNKNOWN; /* TARGET_NO_POWER? */
 		return ERROR_TARGET_INIT_FAILED;
 	}
 
 	if (dbg_osreg & PRSR_STICKY_RESET_STATUS)
-		LOG_DEBUG("target->coreid %" PRId32 " was reset!", target->coreid);
+		LOG_TARGET_DEBUG(target, "was reset!");
 
 	/* Read DBGOSLSR and check if OSLK is implemented */
 	retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
 				armv7a->debug_base + CPUDBG_OSLSR, &dbg_osreg);
 	if (retval != ERROR_OK)
 		return retval;
-	LOG_DEBUG("target->coreid %" PRId32 " DBGOSLSR 0x%" PRIx32, target->coreid, dbg_osreg);
+	LOG_TARGET_DEBUG(target, "DBGOSLSR 0x%" PRIx32, dbg_osreg);
 
 	/* check if OS Lock is implemented */
 	if ((dbg_osreg & OSLSR_OSLM) == OSLSR_OSLM0 || (dbg_osreg & OSLSR_OSLM) == OSLSR_OSLM1) {
 		/* check if OS Lock is set */
 		if (dbg_osreg & OSLSR_OSLK) {
-			LOG_DEBUG("target->coreid %" PRId32 " OSLock set! Trying to unlock", target->coreid);
+			LOG_TARGET_DEBUG(target, "OSLock set! Trying to unlock");
 
 			retval = mem_ap_write_atomic_u32(armv7a->debug_ap,
 							armv7a->debug_base + CPUDBG_OSLAR,
@@ -3022,8 +3022,7 @@ static int cortex_a_examine_first(struct target *target)
 
 			/* if we fail to access the register or cannot reset the OSLK bit, bail out */
 			if (retval != ERROR_OK || (dbg_osreg & OSLSR_OSLK) != 0) {
-				LOG_ERROR("target->coreid %" PRId32 " OSLock sticky, core not powered?",
-						target->coreid);
+				LOG_TARGET_ERROR(target, "OSLock sticky, core not powered?");
 				target->state = TARGET_UNKNOWN; /* TARGET_NO_POWER? */
 				return ERROR_TARGET_INIT_FAILED;
 			}
@@ -3036,13 +3035,11 @@ static int cortex_a_examine_first(struct target *target)
 		return retval;
 
 	if (dbg_idpfr1 & 0x000000f0) {
-		LOG_DEBUG("target->coreid %" PRId32 " has security extensions",
-				target->coreid);
+		LOG_TARGET_DEBUG(target, "has security extensions");
 		armv7a->arm.core_type = ARM_CORE_TYPE_SEC_EXT;
 	}
 	if (dbg_idpfr1 & 0x0000f000) {
-		LOG_DEBUG("target->coreid %" PRId32 " has virtualization extensions",
-				target->coreid);
+		LOG_TARGET_DEBUG(target, "has virtualization extensions");
 		/*
 		 * overwrite and simplify the checks.
 		 * virtualization extensions require implementation of security extension
diff --git a/src/target/dsp563xx.c b/src/target/dsp563xx.c
index 16fd149..1e71803 100644
--- a/src/target/dsp563xx.c
+++ b/src/target/dsp563xx.c
@@ -912,7 +912,7 @@ static int dsp563xx_examine(struct target *target)
 {
 	uint32_t chip;
 
-	if (target->tap->hasidcode == false) {
+	if (!target->tap->has_idcode) {
 		LOG_ERROR("no IDCODE present on device");
 		return ERROR_COMMAND_SYNTAX_ERROR;
 	}
diff --git a/src/target/espressif/Makefile.am b/src/target/espressif/Makefile.am
index 776818f..cf82ee9 100644
--- a/src/target/espressif/Makefile.am
+++ b/src/target/espressif/Makefile.am
@@ -10,6 +10,8 @@ noinst_LTLIBRARIES += %D%/libespressif.la
 	%D%/esp_xtensa_semihosting.h \
 	%D%/esp_xtensa_apptrace.c \
 	%D%/esp_xtensa_apptrace.h \
+	%D%/esp_xtensa_algorithm.c \
+	%D%/esp_xtensa_algorithm.h \
 	%D%/esp32_apptrace.c \
 	%D%/esp32_apptrace.h \
 	%D%/esp32.c \
@@ -21,4 +23,6 @@ noinst_LTLIBRARIES += %D%/libespressif.la
 	%D%/esp32_sysview.h \
 	%D%/segger_sysview.h \
 	%D%/esp_semihosting.c \
-	%D%/esp_semihosting.h
+	%D%/esp_semihosting.h \
+	%D%/esp_algorithm.c \
+	%D%/esp_algorithm.h
diff --git a/src/target/espressif/esp.c b/src/target/espressif/esp.c
index 9583d64..600f6d7 100644
--- a/src/target/espressif/esp.c
+++ b/src/target/espressif/esp.c
@@ -14,6 +14,16 @@
 #include "target/target.h"
 #include "esp.h"
 
+int esp_common_init(struct esp_common *esp, const struct esp_algorithm_hw *algo_hw)
+{
+	if (!esp)
+		return ERROR_FAIL;
+
+	esp->algo_hw = algo_hw;
+
+	return ERROR_OK;
+}
+
 int esp_dbgstubs_table_read(struct target *target, struct esp_dbg_stubs *dbg_stubs)
 {
 	uint32_t table_size, table_start_id, desc_entry_id, gcov_entry_id;
diff --git a/src/target/espressif/esp.h b/src/target/espressif/esp.h
index 3ba2b8b..6e0a2d2 100644
--- a/src/target/espressif/esp.h
+++ b/src/target/espressif/esp.h
@@ -77,9 +77,11 @@ struct esp_dbg_stubs {
 };
 
 struct esp_common {
+	const struct esp_algorithm_hw *algo_hw;
 	struct esp_dbg_stubs dbg_stubs;
 };
 
+int esp_common_init(struct esp_common *esp, const struct esp_algorithm_hw *algo_hw);
 int esp_dbgstubs_table_read(struct target *target, struct esp_dbg_stubs *dbg_stubs);
 
 #endif	/* OPENOCD_TARGET_ESP_H */
diff --git a/src/target/espressif/esp32.c b/src/target/espressif/esp32.c
index b510f28..324aa39 100644
--- a/src/target/espressif/esp32.c
+++ b/src/target/espressif/esp32.c
@@ -484,6 +484,10 @@ struct target_type esp32_target = {
 	.get_gdb_arch = xtensa_get_gdb_arch,
 	.get_gdb_reg_list = xtensa_get_gdb_reg_list,
 
+	.run_algorithm = xtensa_run_algorithm,
+	.start_algorithm = xtensa_start_algorithm,
+	.wait_algorithm = xtensa_wait_algorithm,
+
 	.add_breakpoint = esp_xtensa_breakpoint_add,
 	.remove_breakpoint = esp_xtensa_breakpoint_remove,
 
diff --git a/src/target/espressif/esp32s2.c b/src/target/espressif/esp32s2.c
index dadc130..2abde47 100644
--- a/src/target/espressif/esp32s2.c
+++ b/src/target/espressif/esp32s2.c
@@ -521,6 +521,10 @@ struct target_type esp32s2_target = {
 	.get_gdb_arch = xtensa_get_gdb_arch,
 	.get_gdb_reg_list = xtensa_get_gdb_reg_list,
 
+	.run_algorithm = xtensa_run_algorithm,
+	.start_algorithm = xtensa_start_algorithm,
+	.wait_algorithm = xtensa_wait_algorithm,
+
 	.add_breakpoint = esp_xtensa_breakpoint_add,
 	.remove_breakpoint = esp_xtensa_breakpoint_remove,
 
diff --git a/src/target/espressif/esp32s3.c b/src/target/espressif/esp32s3.c
index 5036956..22e1630 100644
--- a/src/target/espressif/esp32s3.c
+++ b/src/target/espressif/esp32s3.c
@@ -405,6 +405,10 @@ struct target_type esp32s3_target = {
 	.get_gdb_arch = xtensa_get_gdb_arch,
 	.get_gdb_reg_list = xtensa_get_gdb_reg_list,
 
+	.run_algorithm = xtensa_run_algorithm,
+	.start_algorithm = xtensa_start_algorithm,
+	.wait_algorithm = xtensa_wait_algorithm,
+
 	.add_breakpoint = esp_xtensa_breakpoint_add,
 	.remove_breakpoint = esp_xtensa_breakpoint_remove,
 
diff --git a/src/target/espressif/esp_algorithm.c b/src/target/espressif/esp_algorithm.c
new file mode 100644
index 0000000..79f610b
--- /dev/null
+++ b/src/target/espressif/esp_algorithm.c
@@ -0,0 +1,595 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+
+/***************************************************************************
+ *   Espressif chips common algorithm API for OpenOCD                      *
+ *   Copyright (C) 2022 Espressif Systems Ltd.                             *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <helper/align.h>
+#include <target/algorithm.h>
+#include <target/target.h>
+#include "esp_algorithm.h"
+
+#define DEFAULT_ALGORITHM_TIMEOUT_MS    40000	/* ms */
+
+static int esp_algorithm_read_stub_logs(struct target *target, struct esp_algorithm_stub *stub)
+{
+	if (!stub || stub->log_buff_addr == 0 || stub->log_buff_size == 0)
+		return ERROR_FAIL;
+
+	uint32_t len = 0;
+	int retval = target_read_u32(target, stub->log_buff_addr, &len);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* sanity check. log_buff_size = sizeof(len) + sizeof(log_buff) */
+	if (len == 0 || len > stub->log_buff_size - 4)
+		return ERROR_FAIL;
+
+	uint8_t *log_buff = calloc(1, len);
+	if (!log_buff) {
+		LOG_ERROR("Failed to allocate memory for the stub log!");
+		return ERROR_FAIL;
+	}
+	retval = target_read_memory(target, stub->log_buff_addr + 4, 1, len, log_buff);
+	if (retval == ERROR_OK)
+		LOG_OUTPUT("%*.*s", len, len, log_buff);
+	free(log_buff);
+	return retval;
+}
+
+static int esp_algorithm_run_image(struct target *target,
+	struct esp_algorithm_run_data *run,
+	uint32_t num_args,
+	va_list ap)
+{
+	struct working_area **mem_handles = NULL;
+
+	if (!run || !run->hw)
+		return ERROR_FAIL;
+
+	int retval = run->hw->algo_init(target, run, num_args, ap);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* allocate memory arguments and fill respective reg params */
+	if (run->mem_args.count > 0) {
+		mem_handles = calloc(run->mem_args.count, sizeof(*mem_handles));
+		if (!mem_handles) {
+			LOG_ERROR("Failed to alloc target mem handles!");
+			retval = ERROR_FAIL;
+			goto _cleanup;
+		}
+		/* alloc memory args target buffers */
+		for (uint32_t i = 0; i < run->mem_args.count; i++) {
+			/* small hack: if we need to update some reg param this field holds
+			 * appropriate user argument number, */
+			/* otherwise should hold UINT_MAX */
+			uint32_t usr_param_num = run->mem_args.params[i].address;
+			static struct working_area *area;
+			retval = target_alloc_working_area(target, run->mem_args.params[i].size, &area);
+			if (retval != ERROR_OK) {
+				LOG_ERROR("Failed to alloc target buffer!");
+				retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+				goto _cleanup;
+			}
+			mem_handles[i] = area;
+			run->mem_args.params[i].address = area->address;
+			if (usr_param_num != UINT_MAX) /* if we need update some register param with mem param value */
+				esp_algorithm_user_arg_set_uint(run, usr_param_num, run->mem_args.params[i].address);
+		}
+	}
+
+	if (run->usr_func_init) {
+		retval = run->usr_func_init(target, run, run->usr_func_arg);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("Failed to prepare algorithm host side args stub (%d)!", retval);
+			goto _cleanup;
+		}
+	}
+
+	LOG_DEBUG("Algorithm start @ " TARGET_ADDR_FMT ", stack %d bytes @ " TARGET_ADDR_FMT,
+		run->stub.tramp_mapped_addr, run->stack_size, run->stub.stack_addr);
+	retval = target_start_algorithm(target,
+		run->mem_args.count, run->mem_args.params,
+		run->reg_args.count, run->reg_args.params,
+		run->stub.tramp_mapped_addr, 0,
+		run->stub.ainfo);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("Failed to start algorithm (%d)!", retval);
+		goto _cleanup;
+	}
+
+	if (run->usr_func) {
+		/* give target algorithm stub time to init itself, then user func can communicate to it safely */
+		alive_sleep(100);
+		retval = run->usr_func(target, run->usr_func_arg);
+		if (retval != ERROR_OK)
+			LOG_ERROR("Failed to exec algorithm user func (%d)!", retval);
+	}
+	uint32_t timeout_ms = 0;	/* do not wait if 'usr_func' returned error */
+	if (retval == ERROR_OK)
+		timeout_ms = run->timeout_ms ? run->timeout_ms : DEFAULT_ALGORITHM_TIMEOUT_MS;
+	LOG_DEBUG("Wait algorithm completion");
+	retval = target_wait_algorithm(target,
+		run->mem_args.count, run->mem_args.params,
+		run->reg_args.count, run->reg_args.params,
+		0, timeout_ms,
+		run->stub.ainfo);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("Failed to wait algorithm (%d)!", retval);
+		/* target has been forced to stop in target_wait_algorithm() */
+	}
+	esp_algorithm_read_stub_logs(target, &run->stub);
+
+	if (run->usr_func_done)
+		run->usr_func_done(target, run, run->usr_func_arg);
+
+	if (retval != ERROR_OK) {
+		LOG_ERROR("Algorithm run failed (%d)!", retval);
+	} else {
+		run->ret_code = esp_algorithm_user_arg_get_uint(run, 0);
+		LOG_DEBUG("Got algorithm RC 0x%" PRIx32, run->ret_code);
+	}
+
+_cleanup:
+	/* free memory arguments */
+	if (mem_handles) {
+		for (uint32_t i = 0; i < run->mem_args.count; i++) {
+			if (mem_handles[i])
+				target_free_working_area(target, mem_handles[i]);
+		}
+		free(mem_handles);
+	}
+	run->hw->algo_cleanup(target, run);
+
+	return retval;
+}
+
+static int esp_algorithm_run_debug_stub(struct target *target,
+	struct esp_algorithm_run_data *run,
+	uint32_t num_args,
+	va_list ap)
+{
+	if (!run || !run->hw)
+		return ERROR_FAIL;
+
+	int retval = run->hw->algo_init(target, run, num_args, ap);
+	if (retval != ERROR_OK)
+		return retval;
+
+	LOG_DEBUG("Algorithm start @ " TARGET_ADDR_FMT ", stack %d bytes @ " TARGET_ADDR_FMT,
+		run->stub.tramp_mapped_addr, run->stack_size, run->stub.stack_addr);
+	retval = target_start_algorithm(target,
+		run->mem_args.count, run->mem_args.params,
+		run->reg_args.count, run->reg_args.params,
+		run->stub.tramp_mapped_addr, 0,
+		run->stub.ainfo);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("Failed to start algorithm (%d)!", retval);
+		goto _cleanup;
+	}
+
+	uint32_t timeout_ms = 0;	/* do not wait if 'usr_func' returned error */
+	if (retval == ERROR_OK)
+		timeout_ms = run->timeout_ms ? run->timeout_ms : DEFAULT_ALGORITHM_TIMEOUT_MS;
+	LOG_DEBUG("Wait algorithm completion");
+	retval = target_wait_algorithm(target,
+		run->mem_args.count, run->mem_args.params,
+		run->reg_args.count, run->reg_args.params,
+		0, timeout_ms,
+		run->stub.ainfo);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("Failed to wait algorithm (%d)!", retval);
+		/* target has been forced to stop in target_wait_algorithm() */
+	}
+
+	if (retval != ERROR_OK) {
+		LOG_ERROR("Algorithm run failed (%d)!", retval);
+	} else {
+		run->ret_code = esp_algorithm_user_arg_get_uint(run, 0);
+		LOG_DEBUG("Got algorithm RC 0x%" PRIx32, run->ret_code);
+	}
+
+_cleanup:
+	run->hw->algo_cleanup(target, run);
+
+	return retval;
+}
+
+static void reverse_binary(const uint8_t *src, uint8_t *dest, size_t length)
+{
+	size_t remaining = length % 4;
+	size_t offset = 0;
+	size_t aligned_len = ALIGN_UP(length, 4);
+
+	if (remaining > 0) {
+		/* Put extra bytes to the beginning with padding */
+		memset(dest + remaining, 0xFF, 4 - remaining);
+		for (size_t i = 0; i < remaining; i++)
+			dest[i] = src[length - remaining + i];
+		length -= remaining; /* reverse the others */
+		offset = 4;
+	}
+
+	for (size_t i = offset; i < aligned_len; i += 4) {
+		dest[i + 0] = src[length - i + offset - 4];
+		dest[i + 1] = src[length - i + offset - 3];
+		dest[i + 2] = src[length - i + offset - 2];
+		dest[i + 3] = src[length - i + offset - 1];
+	}
+}
+
+static int load_section_from_image(struct target *target,
+	struct esp_algorithm_run_data *run,
+	int section_num,
+	bool reverse)
+{
+	if (!run)
+		return ERROR_FAIL;
+
+	struct imagesection *section = &run->image.image.sections[section_num];
+	uint32_t sec_wr = 0;
+	uint8_t buf[1024];
+
+	assert(sizeof(buf) % 4 == 0);
+
+	while (sec_wr < section->size) {
+		uint32_t nb = section->size - sec_wr > sizeof(buf) ? sizeof(buf) : section->size - sec_wr;
+		size_t size_read = 0;
+		int retval = image_read_section(&run->image.image, section_num, sec_wr, nb, buf, &size_read);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("Failed to read stub section (%d)!", retval);
+			return retval;
+		}
+
+		if (reverse) {
+			size_t aligned_len = ALIGN_UP(size_read, 4);
+			uint8_t reversed_buf[aligned_len];
+
+			/* Send original size to allow padding */
+			reverse_binary(buf, reversed_buf, size_read);
+
+			/*
+				The address range accessed via the instruction bus is in reverse order (word-wise) compared to access
+				via the data bus. That is to say, address
+				0x3FFE_0000 and 0x400B_FFFC access the same word
+				0x3FFE_0004 and 0x400B_FFF8 access the same word
+				0x3FFE_0008 and 0x400B_FFF4 access the same word
+				...
+				The data bus and instruction bus of the CPU are still both little-endian,
+				so the byte order of individual words is not reversed between address spaces.
+				For example, address
+				0x3FFE_0000 accesses the least significant byte in the word accessed by 0x400B_FFFC.
+				0x3FFE_0001 accesses the second least significant byte in the word accessed by 0x400B_FFFC.
+				0x3FFE_0002 accesses the second most significant byte in the word accessed by 0x400B_FFFC.
+				For more details, please refer to ESP32 TRM, Internal SRAM1 section.
+			*/
+			retval = target_write_buffer(target, run->image.dram_org - sec_wr - aligned_len, aligned_len, reversed_buf);
+			if (retval != ERROR_OK) {
+				LOG_ERROR("Failed to write stub section!");
+				return retval;
+			}
+		} else {
+			retval = target_write_buffer(target, section->base_address + sec_wr, size_read, buf);
+			if (retval != ERROR_OK) {
+				LOG_ERROR("Failed to write stub section!");
+				return retval;
+			}
+		}
+
+		sec_wr += size_read;
+	}
+
+	return ERROR_OK;
+}
+
+/*
+ * Configuration:
+ * ----------------------------
+ * The linker scripts defines the memory layout for the stub code.
+ * The OpenOCD script specifies the workarea address and it's size
+ * Sections defined in the linker are organized to share the same addresses with the workarea.
+ * code and data sections are located in Internal SRAM1 and OpenOCD fills these sections using the data bus.
+ */
+int esp_algorithm_load_func_image(struct target *target, struct esp_algorithm_run_data *run)
+{
+	int retval;
+	size_t tramp_sz = 0;
+	const uint8_t *tramp = NULL;
+	struct duration algo_time;
+	bool alloc_code_working_area = true;
+
+	if (!run || !run->hw)
+		return ERROR_FAIL;
+
+	if (duration_start(&algo_time) != 0) {
+		LOG_ERROR("Failed to start algo time measurement!");
+		return ERROR_FAIL;
+	}
+
+	if (run->hw->stub_tramp_get) {
+		tramp = run->hw->stub_tramp_get(target, &tramp_sz);
+		if (!tramp)
+			return ERROR_FAIL;
+	}
+
+	LOG_DEBUG("stub: base 0x%x, start 0x%" PRIx32 ", %d sections",
+		run->image.image.base_address_set ? (unsigned int)run->image.image.base_address : 0,
+		run->image.image.start_address,
+		run->image.image.num_sections);
+	run->stub.entry = run->image.image.start_address;
+
+	/* [code + trampoline] + <padding> + [data] */
+
+	/* ESP32 has reversed memory region. It will use the last part of DRAM, the others will use the first part.
+	 * To avoid complexity for the backup/restore process, we will allocate a workarea for all IRAM region from
+	 * the beginning. In that case no need to have a padding area.
+	 */
+	if (run->image.reverse) {
+		if (target_alloc_working_area(target, run->image.iram_len, &run->stub.code) != ERROR_OK) {
+			LOG_ERROR("no working area available, can't alloc space for stub code!");
+			retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+			goto _on_error;
+		}
+		alloc_code_working_area = false;
+	}
+
+	uint32_t code_size = 0;
+
+	/* Load code section */
+	for (unsigned int i = 0; i < run->image.image.num_sections; i++) {
+		struct imagesection *section = &run->image.image.sections[i];
+
+		if (section->size == 0)
+			continue;
+
+		if (section->flags & ESP_IMAGE_ELF_PHF_EXEC) {
+			LOG_DEBUG("addr " TARGET_ADDR_FMT ", sz %d, flags %" PRIx64,
+				section->base_address, section->size, section->flags);
+
+			if (alloc_code_working_area) {
+				retval = target_alloc_working_area(target, section->size, &run->stub.code);
+				if (retval != ERROR_OK) {
+					LOG_ERROR("no working area available, can't alloc space for stub code!");
+					retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+					goto _on_error;
+				}
+			}
+
+			if (section->base_address == 0) {
+				section->base_address = run->stub.code->address;
+				/* sanity check, stub is compiled to be run from working area */
+			} else if (run->stub.code->address != section->base_address) {
+				LOG_ERROR("working area " TARGET_ADDR_FMT " and stub code section " TARGET_ADDR_FMT
+					" address mismatch!",
+					section->base_address,
+					run->stub.code->address);
+				retval = ERROR_FAIL;
+				goto _on_error;
+			}
+
+			retval = load_section_from_image(target, run, i, run->image.reverse);
+			if (retval != ERROR_OK)
+				goto _on_error;
+
+			code_size += ALIGN_UP(section->size, 4);
+			break; /* Stub has one executable text section */
+		}
+	}
+
+	/* If exists, load trampoline to the code area */
+	if (tramp) {
+		if (run->stub.tramp_addr == 0) {
+			if (alloc_code_working_area) {
+				/* alloc trampoline in code working area */
+				if (target_alloc_working_area(target, tramp_sz, &run->stub.tramp) != ERROR_OK) {
+					LOG_ERROR("no working area available, can't alloc space for stub jumper!");
+					retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+					goto _on_error;
+				}
+				run->stub.tramp_addr = run->stub.tramp->address;
+			}
+		}
+
+		size_t al_tramp_size = ALIGN_UP(tramp_sz, 4);
+
+		if (run->image.reverse) {
+			target_addr_t reversed_tramp_addr = run->image.dram_org - code_size;
+			uint8_t reversed_tramp[al_tramp_size];
+
+			/* Send original size to allow padding */
+			reverse_binary(tramp, reversed_tramp, tramp_sz);
+			run->stub.tramp_addr = reversed_tramp_addr - al_tramp_size;
+			LOG_DEBUG("Write reversed tramp to addr " TARGET_ADDR_FMT ", sz %zu", run->stub.tramp_addr, al_tramp_size);
+			retval = target_write_buffer(target, run->stub.tramp_addr, al_tramp_size, reversed_tramp);
+		} else {
+			LOG_DEBUG("Write tramp to addr " TARGET_ADDR_FMT ", sz %zu", run->stub.tramp_addr, tramp_sz);
+			retval = target_write_buffer(target, run->stub.tramp_addr, tramp_sz, tramp);
+		}
+
+		if (retval != ERROR_OK) {
+			LOG_ERROR("Failed to write stub jumper!");
+			goto _on_error;
+		}
+
+		run->stub.tramp_mapped_addr = run->image.iram_org + code_size;
+		code_size += al_tramp_size;
+		LOG_DEBUG("Tramp mapped to addr " TARGET_ADDR_FMT, run->stub.tramp_mapped_addr);
+	}
+
+	/* allocate dummy space until the data address */
+	if (alloc_code_working_area) {
+		/* we dont need to restore padding area. */
+		uint32_t backup_working_area_prev = target->backup_working_area;
+		target->backup_working_area = 0;
+		if (target_alloc_working_area(target, run->image.iram_len - code_size, &run->stub.padding) != ERROR_OK) {
+			LOG_ERROR("no working area available, can't alloc space for stub code!");
+			retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+			goto _on_error;
+		}
+		target->backup_working_area = backup_working_area_prev;
+	}
+
+	/*  Load the data section */
+	for (unsigned int i = 0; i < run->image.image.num_sections; i++) {
+		struct imagesection *section = &run->image.image.sections[i];
+
+		if (section->size == 0)
+			continue;
+
+		if (!(section->flags & ESP_IMAGE_ELF_PHF_EXEC)) {
+			LOG_DEBUG("addr " TARGET_ADDR_FMT ", sz %d, flags %" PRIx64, section->base_address, section->size,
+				section->flags);
+			/* target_alloc_working_area() aligns the whole working area size to 4-byte boundary.
+			   We alloc one area for both DATA and BSS, so align each of them ourselves. */
+			uint32_t data_sec_sz = ALIGN_UP(section->size, 4);
+			LOG_DEBUG("DATA sec size %" PRIu32 " -> %" PRIu32, section->size, data_sec_sz);
+			uint32_t bss_sec_sz = ALIGN_UP(run->image.bss_size, 4);
+			LOG_DEBUG("BSS sec size %" PRIu32 " -> %" PRIu32, run->image.bss_size, bss_sec_sz);
+			if (target_alloc_working_area(target, data_sec_sz + bss_sec_sz, &run->stub.data) != ERROR_OK) {
+				LOG_ERROR("no working area available, can't alloc space for stub data!");
+				retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+				goto _on_error;
+			}
+			if (section->base_address == 0) {
+				section->base_address = run->stub.data->address;
+				/* sanity check, stub is compiled to be run from working area */
+			} else if (run->stub.data->address != section->base_address) {
+				LOG_ERROR("working area " TARGET_ADDR_FMT
+					" and stub data section " TARGET_ADDR_FMT
+					" address mismatch!",
+					section->base_address,
+					run->stub.data->address);
+				retval = ERROR_FAIL;
+				goto _on_error;
+			}
+
+			retval = load_section_from_image(target, run, i, false);
+			if (retval != ERROR_OK)
+				goto _on_error;
+		}
+	}
+
+	/* stack */
+	if (run->stub.stack_addr == 0 && run->stack_size > 0) {
+		/* allocate stack in data working area */
+		if (target_alloc_working_area(target, run->stack_size, &run->stub.stack) != ERROR_OK) {
+			LOG_ERROR("no working area available, can't alloc stub stack!");
+			retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+			goto _on_error;
+		}
+		run->stub.stack_addr = run->stub.stack->address + run->stack_size;
+	}
+
+	if (duration_measure(&algo_time) != 0) {
+		LOG_ERROR("Failed to stop algo run measurement!");
+		retval = ERROR_FAIL;
+		goto _on_error;
+	}
+	LOG_DEBUG("Stub loaded in %g ms", duration_elapsed(&algo_time) * 1000);
+	return ERROR_OK;
+
+_on_error:
+	esp_algorithm_unload_func_image(target, run);
+	return retval;
+}
+
+int esp_algorithm_unload_func_image(struct target *target, struct esp_algorithm_run_data *run)
+{
+	if (!run)
+		return ERROR_FAIL;
+
+	target_free_all_working_areas(target);
+
+	run->stub.tramp = NULL;
+	run->stub.stack = NULL;
+	run->stub.code = NULL;
+	run->stub.data = NULL;
+	run->stub.padding = NULL;
+
+	return ERROR_OK;
+}
+
+int esp_algorithm_exec_func_image_va(struct target *target,
+	struct esp_algorithm_run_data *run,
+	uint32_t num_args,
+	va_list ap)
+{
+	if (!run || !run->image.image.start_address_set || run->image.image.start_address == 0)
+		return ERROR_FAIL;
+
+	return esp_algorithm_run_image(target, run, num_args, ap);
+}
+
+int esp_algorithm_load_onboard_func(struct target *target, target_addr_t func_addr, struct esp_algorithm_run_data *run)
+{
+	int res;
+	const uint8_t *tramp = NULL;
+	size_t tramp_sz = 0;
+	struct duration algo_time;
+
+	if (!run || !run->hw)
+		return ERROR_FAIL;
+
+	if (duration_start(&algo_time) != 0) {
+		LOG_ERROR("Failed to start algo time measurement!");
+		return ERROR_FAIL;
+	}
+
+	if (run->hw->stub_tramp_get) {
+		tramp = run->hw->stub_tramp_get(target, &tramp_sz);
+		if (!tramp)
+			return ERROR_FAIL;
+	}
+
+	if (tramp_sz > run->on_board.code_buf_size) {
+		LOG_ERROR("Stub tramp size %zu bytes exceeds target buf size %d bytes!",
+			tramp_sz, run->on_board.code_buf_size);
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+
+	if (run->stack_size > run->on_board.min_stack_size) {
+		LOG_ERROR("Algorithm stack size not fit into the allocated target stack!");
+		return ERROR_FAIL;
+	}
+
+	run->stub.stack_addr = run->on_board.min_stack_addr + run->stack_size;
+	run->stub.tramp_addr = run->on_board.code_buf_addr;
+	run->stub.tramp_mapped_addr = run->stub.tramp_addr;
+	run->stub.entry = func_addr;
+
+	if (tramp) {
+		res = target_write_buffer(target, run->stub.tramp_addr, tramp_sz, tramp);
+		if (res != ERROR_OK) {
+			LOG_ERROR("Failed to write stub jumper!");
+			esp_algorithm_unload_onboard_func(target, run);
+			return res;
+		}
+	}
+
+	if (duration_measure(&algo_time) != 0) {
+		LOG_ERROR("Failed to stop algo run measurement!");
+		return ERROR_FAIL;
+	}
+	LOG_DEBUG("Stub loaded in %g ms", duration_elapsed(&algo_time) * 1000);
+
+	return ERROR_OK;
+}
+
+int esp_algorithm_unload_onboard_func(struct target *target, struct esp_algorithm_run_data *run)
+{
+	return ERROR_OK;
+}
+
+int esp_algorithm_exec_onboard_func_va(struct target *target,
+	struct esp_algorithm_run_data *run,
+	uint32_t num_args,
+	va_list ap)
+{
+	return esp_algorithm_run_debug_stub(target, run, num_args, ap);
+}
diff --git a/src/target/espressif/esp_algorithm.h b/src/target/espressif/esp_algorithm.h
new file mode 100644
index 0000000..11d2757
--- /dev/null
+++ b/src/target/espressif/esp_algorithm.h
@@ -0,0 +1,420 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+
+/***************************************************************************
+ *   Espressif chips common algorithm API for OpenOCD                      *
+ *   Copyright (C) 2022 Espressif Systems Ltd.                             *
+ ***************************************************************************/
+
+#ifndef OPENOCD_TARGET_ESP_ALGORITHM_H
+#define OPENOCD_TARGET_ESP_ALGORITHM_H
+
+#include "helper/log.h"
+#include "helper/binarybuffer.h"
+#include <helper/time_support.h>
+#include <target/algorithm.h>
+#include <target/image.h>
+
+/**
+ * API defined below allows executing pieces of code on target without breaking the execution of the running program.
+ * This functionality can be useful for various debugging and maintenance procedures.
+ * @note ESP flashing code to load flasher stub on target and write/read/erase flash.
+ * Also ESP GCOV command uses some of these functions to run onboard routines to dump coverage info.
+ * Stub entry function can take up to 5 arguments and should be of the following form:
+ *
+ * int stub_entry([uint32_t a1 [, uint32_t a2 [, uint32_t a3 [, uint32_t a4 [, uint32_t a5]]]]]);
+ *
+ * The general scheme of stub code execution is shown below.
+ *
+ *  -------                                                    -----------   (initial frame)    ----
+ * |       | -------(registers, stub entry, stub args)------> |trampoline | ---(stub args)---> |    |
+ * |       |                                                  |           |                    |    |
+ * |OpenOCD| <----------(stub-specific communications)---------------------------------------> |stub|
+ * |       |                                                  |           |                    |    |
+ * |       | <---------(target halted event, ret code)------- |tramp break| <---(ret code)---- |    |
+ *  -------                                                    -----------                      ----
+ *
+ * Procedure of executing stub on target includes:
+ * 1) User prepares struct esp_algorithm_run_data and calls one of algorithm_run_xxx() functions.
+ * 2) Routine allocates all necessary stub code and data sections.
+ * 3) If a user specifies an initializer func esp_algorithm_usr_func_init_t it is called just before the stub starts.
+ * 4) If user specifies stub communication func esp_algorithm_usr_func_t (@see esp_flash_write/read in ESP flash driver)
+ *    it is called just after the stub starts. When communication with stub is finished this function must return.
+ * 5) OpenOCD waits for the stub to finish (hit exit breakpoint).
+ * 6) If the user specified arguments cleanup func esp_algorithm_usr_func_done_t,
+ *    it is called just after the stub finishes.
+ *
+ * There are two options to run code on target under OpenOCD control:
+ * - Run externally compiled stub code.
+ * - Run onboard pre-compiled code. @note For ESP chips debug stubs must be enabled in target code @see ESP IDF docs.
+ * The main difference between the execution of external stub code and target built-in functions is that
+ * in the latter case working areas can not be used to allocate target memory for code and data because they can overlap
+ * with code and data involved in onboard function execution. For example, if memory allocated in the working area
+ * for the stub stack will overlap with some on-board data used by the stub the stack will get overwritten.
+ * The same stands for allocations in target code space.
+ *
+ * External Code Execution
+ * -----------------------
+ * To run external code on the target user should use esp_algorithm_run_func_image().
+ * In this case all necessary memory (code/data) is allocated in working areas that have fixed configuration
+ * defined in target TCL file. Stub code is actually a standalone program, so all its segments must have known
+ * addresses due to position-dependent code nature. So stub must be linked in such a way that its code segment
+ * starts at the beginning of the working area for code space defined in TCL. The same restriction must be applied
+ * to stub's data segment and base addresses of working area for data space. @see ESP stub flasher LD scripts.
+ * Also in order to simplify memory allocation BSS section must follow the DATA section in the stub image.
+ * The size of the BSS section must be specified in the bss_size field of struct algorithm_image.
+ * Sample stub memory map is shown below.
+ *  ___________________________________________
+ * | data space working area start             |
+ * |                                           |
+ * | <stub .data segment>                      |
+ * |___________________________________________|
+ * | stub .bss start                           |
+ * |                                           |
+ * | <stub .bss segment of size 'bss_size'>    |
+ * |___________________________________________|
+ * | stub stack base                           |
+ * |                                           |
+ * | <stub stack>                              |
+ * |___________________________________________|
+ * |                                           |
+ * | <stub mem arg1>                           |
+ * |___________________________________________|
+ * |                                           |
+ * | <stub mem arg2>                           |
+ * |___________________________________________|
+ *  ___________________________________________
+ * | code space working area start             |
+ * |                                           |
+ * | <stub .text segment>                      |
+ * |___________________________________________|
+ * |                                           |
+ * | <stub trampoline with exit breakpoint>    |
+ * |___________________________________________|
+ *
+ * For example on how to execute external code with memory arguments @see esp_algo_flash_blank_check in
+ * ESP flash driver.
+ *
+ * On-Board Code Execution
+ * -----------------------
+ * To run on-board code on the target user should use esp_algorithm_run_onboard_func().
+ * On-board code execution process does not need to allocate target memory for stub code and data,
+ * Because the stub is pre-compiled to the code running on the target.
+ * But it still needs memory for stub trampoline, stack, and memory arguments.
+ * Working areas can not be used due to possible memory layout conflicts with on-board stub code and data.
+ * Debug stubs functionality provided by ESP IDF allows OpenOCD to overcome the above problem.
+ * It provides a special descriptor which provides info necessary to safely allocate memory on target.
+ * @see struct esp_dbg_stubs_desc.
+ * That info is also used to locate memory for stub trampoline code.
+ * User can execute target function at any address, but @see ESP IDF debug stubs also provide a way to pass to the host
+ * an entry address of pre-defined registered stub functions.
+ * For example of an on-board code execution @see esp32_cmd_gcov() in ESP32 apptrace module.
+*/
+
+/**
+ * Algorithm image data.
+ * Helper struct to work with algorithms consisting of code and data segments.
+ */
+struct esp_algorithm_image {
+	/** Image. */
+	struct image image;
+	/** BSS section size. */
+	uint32_t bss_size;
+	/** IRAM start address in the linker script */
+	uint32_t iram_org;
+	/** Total reserved IRAM size */
+	uint32_t iram_len;
+	/** DRAM start address in the linker script */
+	uint32_t dram_org;
+	/** Total reserved DRAM size */
+	uint32_t dram_len;
+	/** IRAM DRAM address range reversed or not */
+	bool reverse;
+};
+
+#define ESP_IMAGE_ELF_PHF_EXEC			0x1
+
+/**
+ * Algorithm stub data.
+ */
+struct esp_algorithm_stub {
+	/** Entry addr. */
+	target_addr_t entry;
+	/** Working area for code segment. */
+	struct working_area *code;
+	/** Working area for data segment. */
+	struct working_area *data;
+	/** Working area for trampoline. */
+	struct working_area *tramp;
+	/** Working area for padding between code and data area. */
+	struct working_area *padding;
+	/** Address of the target buffer for stub trampoline. If zero tramp->address will be used. */
+	target_addr_t tramp_addr;
+	/** Tramp code area will be filled from dbus.
+	 *  We need to map it to the ibus to be able to initialize PC register to start algorithm execution from.
+	 */
+	target_addr_t tramp_mapped_addr;
+	/** Working area for stack. */
+	struct working_area *stack;
+	/** Address of the target buffer for stack. If zero tramp->address will be used. */
+	target_addr_t stack_addr;
+	/** Address of the log buffer */
+	target_addr_t log_buff_addr;
+	/** Size of the log buffer */
+	uint32_t log_buff_size;
+	/** Algorithm's arch-specific info. */
+	void *ainfo;
+};
+
+/**
+ * Algorithm stub in-memory arguments.
+ */
+struct esp_algorithm_mem_args {
+	/** Memory params. */
+	struct mem_param *params;
+	/** Number of memory params. */
+	uint32_t count;
+};
+
+/**
+ * Algorithm stub register arguments.
+ */
+struct esp_algorithm_reg_args {
+	/** Algorithm register params. User args start from user_first_reg_param */
+	struct reg_param *params;
+	/** Number of register params. */
+	uint32_t count;
+	/** The first several reg_params can be used by stub itself (e.g. for trampoline).
+	 * This is the index of the first reg_param available for user to pass args to algorithm stub. */
+	uint32_t first_user_param;
+};
+
+struct esp_algorithm_run_data;
+
+/**
+ * @brief Algorithm run function.
+ *
+ * @param target Pointer to target.
+ * @param run    Pointer to algo run data.
+ * @param arg    Function specific argument.
+ *
+ * @return ERROR_OK on success, otherwise ERROR_XXX.
+ */
+typedef int (*esp_algorithm_func_t)(struct target *target, struct esp_algorithm_run_data *run, void *arg);
+
+/**
+ * @brief Host part of algorithm.
+ *        This function will be called while stub is running on target.
+ *        It can be used for communication with stub.
+ *
+ * @param target  Pointer to target.
+ * @param usr_arg Function specific argument.
+ *
+ * @return ERROR_OK on success, otherwise ERROR_XXX.
+ */
+typedef int (*esp_algorithm_usr_func_t)(struct target *target, void *usr_arg);
+
+/**
+ * @brief Algorithm's arguments setup function.
+ *        This function will be called just before stub start.
+ *        It must return when all operations with running stub are completed.
+ *        It can be used to prepare stub memory parameters.
+ *
+ * @param target  Pointer to target.
+ * @param run     Pointer to algo run data.
+ * @param usr_arg Function specific argument. The same as for esp_algorithm_usr_func_t.
+ *
+ * @return ERROR_OK on success, otherwise ERROR_XXX.
+ */
+typedef int (*esp_algorithm_usr_func_init_t)(struct target *target,
+	struct esp_algorithm_run_data *run,
+	void *usr_arg);
+
+/**
+ * @brief Algorithm's arguments cleanup function.
+ *        This function will be called just after stub exit.
+ *        It can be used to cleanup stub memory parameters.
+ *
+ * @param target  Pointer to target.
+ * @param run     Pointer to algo run data.
+ * @param usr_arg Function specific argument. The same as for esp_algorithm_usr_func_t.
+ *
+ * @return ERROR_OK on success, otherwise ERROR_XXX.
+ */
+typedef void (*esp_algorithm_usr_func_done_t)(struct target *target,
+	struct esp_algorithm_run_data *run,
+	void *usr_arg);
+
+struct esp_algorithm_hw {
+	int (*algo_init)(struct target *target, struct esp_algorithm_run_data *run, uint32_t num_args, va_list ap);
+	int (*algo_cleanup)(struct target *target, struct esp_algorithm_run_data *run);
+	const uint8_t *(*stub_tramp_get)(struct target *target, size_t *size);
+};
+
+/**
+ * Algorithm run data.
+ */
+struct esp_algorithm_run_data {
+	/** Algorithm completion timeout in ms. If 0, default value will be used */
+	uint32_t timeout_ms;
+	/** Algorithm stack size. */
+	uint32_t stack_size;
+	/** Algorithm register arguments. */
+	struct esp_algorithm_reg_args reg_args;
+	/** Algorithm memory arguments. */
+	struct esp_algorithm_mem_args mem_args;
+	/** Algorithm arch-specific info. For Xtensa this should point to struct xtensa_algorithm. */
+	void *arch_info;
+	/** Algorithm return code. */
+	int32_t ret_code;
+	/** Stub. */
+	struct esp_algorithm_stub stub;
+	union {
+		struct {
+			/** Size of the pre-alocated on-board buffer for stub's code. */
+			uint32_t code_buf_size;
+			/** Address of pre-compiled target buffer for stub trampoline. */
+			target_addr_t code_buf_addr;
+			/** Size of the pre-alocated on-board buffer for stub's stack. */
+			uint32_t min_stack_size;
+			/** Pre-compiled target buffer's addr for stack. */
+			target_addr_t min_stack_addr;
+		} on_board;
+		struct esp_algorithm_image image;
+	};
+	/** Host side algorithm function argument. */
+	void *usr_func_arg;
+	/** Host side algorithm function. */
+	esp_algorithm_usr_func_t usr_func;
+	/** Host side algorithm function setup routine. */
+	esp_algorithm_usr_func_init_t usr_func_init;
+	/** Host side algorithm function cleanup routine. */
+	esp_algorithm_usr_func_done_t usr_func_done;
+	/** Algorithm run function: see algorithm_run_xxx for example. */
+	esp_algorithm_func_t algo_func;
+	/** HW specific API */
+	const struct esp_algorithm_hw *hw;
+};
+
+int esp_algorithm_load_func_image(struct target *target, struct esp_algorithm_run_data *run);
+int esp_algorithm_unload_func_image(struct target *target, struct esp_algorithm_run_data *run);
+
+int esp_algorithm_exec_func_image_va(struct target *target,
+	struct esp_algorithm_run_data *run,
+	uint32_t num_args,
+	va_list ap);
+
+/**
+ * @brief Loads and runs stub from specified image.
+ *        This function should be used to run external stub code on target.
+ *
+ * @param target   Pointer to target.
+ * @param run      Pointer to algo run data.
+ * @param num_args Number of stub arguments that follow.
+ *
+ * @return ERROR_OK on success, otherwise ERROR_XXX. Stub return code is in run->ret_code.
+ */
+static inline int esp_algorithm_run_func_image_va(struct target *target,
+	struct esp_algorithm_run_data *run,
+	uint32_t num_args,
+	va_list ap)
+{
+	int ret = esp_algorithm_load_func_image(target, run);
+	if (ret != ERROR_OK)
+		return ret;
+	ret = esp_algorithm_exec_func_image_va(target, run, num_args, ap);
+	int rc = esp_algorithm_unload_func_image(target, run);
+	return ret != ERROR_OK ? ret : rc;
+}
+
+static inline int esp_algorithm_run_func_image(struct target *target,
+	struct esp_algorithm_run_data *run,
+	uint32_t num_args,
+	...)
+{
+	va_list ap;
+	va_start(ap, num_args);
+	int retval = esp_algorithm_run_func_image_va(target, run, num_args, ap);
+	va_end(ap);
+	return retval;
+}
+
+int esp_algorithm_load_onboard_func(struct target *target,
+	target_addr_t func_addr,
+	struct esp_algorithm_run_data *run);
+int esp_algorithm_unload_onboard_func(struct target *target, struct esp_algorithm_run_data *run);
+int esp_algorithm_exec_onboard_func_va(struct target *target,
+	struct esp_algorithm_run_data *run,
+	uint32_t num_args,
+	va_list ap);
+
+/**
+ * @brief Runs pre-compiled on-board function.
+ *        This function should be used to run on-board stub code.
+ *
+ * @param target     Pointer to target.
+ * @param run        Pointer to algo run data.
+ * @param func_entry Address of the function to run.
+ * @param num_args   Number of function arguments that follow.
+ *
+ * @return ERROR_OK on success, otherwise ERROR_XXX. Stub return code is in run->ret_code.
+ */
+static inline int esp_algorithm_run_onboard_func_va(struct target *target,
+	struct esp_algorithm_run_data *run,
+	target_addr_t func_addr,
+	uint32_t num_args,
+	va_list ap)
+{
+	int ret = esp_algorithm_load_onboard_func(target, func_addr, run);
+	if (ret != ERROR_OK)
+		return ret;
+	ret = esp_algorithm_exec_onboard_func_va(target, run, num_args, ap);
+	if (ret != ERROR_OK)
+		return ret;
+	return esp_algorithm_unload_onboard_func(target, run);
+}
+
+static inline int esp_algorithm_run_onboard_func(struct target *target,
+	struct esp_algorithm_run_data *run,
+	target_addr_t func_addr,
+	uint32_t num_args,
+	...)
+{
+	va_list ap;
+	va_start(ap, num_args);
+	int retval = esp_algorithm_run_onboard_func_va(target, run, func_addr, num_args, ap);
+	va_end(ap);
+	return retval;
+}
+
+/**
+ * @brief Set the value of an argument passed via registers to the stub main function.
+ */
+static inline void esp_algorithm_user_arg_set_uint(struct esp_algorithm_run_data *run,
+	int arg_num,
+	uint64_t val)
+{
+	struct reg_param *param = &run->reg_args.params[run->reg_args.first_user_param + arg_num];
+
+	assert(param->size <= 64);
+
+	if (param->size <= 32)
+		buf_set_u32(param->value, 0, param->size, val);
+	else
+		buf_set_u64(param->value, 0, param->size, val);
+}
+
+/**
+ * @brief Get the value of an argument passed via registers from the stub main function.
+ */
+static inline uint64_t esp_algorithm_user_arg_get_uint(struct esp_algorithm_run_data *run, int arg_num)
+{
+	struct reg_param *param = &run->reg_args.params[run->reg_args.first_user_param + arg_num];
+
+	assert(param->size <= 64);
+
+	if (param->size <= 32)
+		return buf_get_u32(param->value, 0, param->size);
+	return buf_get_u64(param->value, 0, param->size);
+}
+
+#endif	/* OPENOCD_TARGET_ESP_ALGORITHM_H */
diff --git a/src/target/espressif/esp_xtensa.c b/src/target/espressif/esp_xtensa.c
index 0bd2cdd..11895d2 100644
--- a/src/target/espressif/esp_xtensa.c
+++ b/src/target/espressif/esp_xtensa.c
@@ -12,10 +12,12 @@
 #include <stdbool.h>
 #include <stdint.h>
 #include <target/smp.h>
-#include "esp_xtensa_apptrace.h"
 #include <target/register.h>
+#include "esp.h"
 #include "esp_xtensa.h"
+#include "esp_xtensa_apptrace.h"
 #include "esp_semihosting.h"
+#include "esp_xtensa_algorithm.h"
 
 #define ESP_XTENSA_DBGSTUBS_UPDATE_DATA_ENTRY(_e_) \
 	do { \
@@ -68,6 +70,10 @@ int esp_xtensa_init_arch_info(struct target *target,
 	int ret = xtensa_init_arch_info(target, &esp_xtensa->xtensa, dm_cfg);
 	if (ret != ERROR_OK)
 		return ret;
+	ret = esp_common_init(&esp_xtensa->esp, &xtensa_algo_hw);
+	if (ret != ERROR_OK)
+		return ret;
+
 	esp_xtensa->semihost.ops = (struct esp_semihost_ops *)semihost_ops;
 	esp_xtensa->apptrace.hw = &esp_xtensa_apptrace_hw;
 	return ERROR_OK;
diff --git a/src/target/espressif/esp_xtensa_algorithm.c b/src/target/espressif/esp_xtensa_algorithm.c
new file mode 100644
index 0000000..68005cb
--- /dev/null
+++ b/src/target/espressif/esp_xtensa_algorithm.c
@@ -0,0 +1,140 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+
+/***************************************************************************
+ *   Module to run arbitrary code on Xtensa using OpenOCD                  *
+ *   Copyright (C) 2019 Espressif Systems Ltd.                             *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <target/xtensa/xtensa.h>
+#include "esp_xtensa_algorithm.h"
+
+static int esp_xtensa_algo_init(struct target *target, struct esp_algorithm_run_data *run,
+	uint32_t num_args, va_list ap);
+static int esp_xtensa_algo_cleanup(struct target *target, struct esp_algorithm_run_data *run);
+static const uint8_t *esp_xtensa_stub_tramp_get(struct target *target, size_t *size);
+
+const struct esp_algorithm_hw xtensa_algo_hw = {
+	.algo_init = esp_xtensa_algo_init,
+	.algo_cleanup = esp_xtensa_algo_cleanup,
+	.stub_tramp_get = esp_xtensa_stub_tramp_get,
+};
+
+/* Generated from contrib/loaders/trampoline/espressif/xtensa/esp_xtensa_stub_tramp_win.S */
+static const uint8_t esp_xtensa_stub_tramp_win[] = {
+#include "../../../contrib/loaders/trampoline/espressif/xtensa/esp_xtensa_stub_tramp_win.inc"
+};
+
+static const uint8_t *esp_xtensa_stub_tramp_get(struct target *target, size_t *size)
+{
+	struct xtensa *xtensa = target_to_xtensa(target);
+
+	if (!xtensa->core_config->windowed) {
+		LOG_ERROR("Running stubs is not supported for cores without windowed registers option!");
+		return NULL;
+	}
+	*size = sizeof(esp_xtensa_stub_tramp_win);
+	return esp_xtensa_stub_tramp_win;
+}
+
+static int esp_xtensa_algo_regs_init_start(struct target *target, struct esp_algorithm_run_data *run)
+{
+	uint32_t stack_addr = run->stub.stack_addr;
+
+	LOG_TARGET_DEBUG(target, "Check stack addr 0x%x", stack_addr);
+	if (stack_addr & 0xFUL) {
+		stack_addr &= ~0xFUL;
+		LOG_TARGET_DEBUG(target, "Adjust stack addr to 0x%x", stack_addr);
+	}
+	stack_addr -= 16;
+	struct reg_param *params = run->reg_args.params;
+	init_reg_param(&params[0], "a0", 32, PARAM_OUT);		/*TODO: move to tramp */
+	init_reg_param(&params[1], "a1", 32, PARAM_OUT);
+	init_reg_param(&params[2], "a8", 32, PARAM_OUT);
+	init_reg_param(&params[3], "windowbase", 32, PARAM_OUT);	/*TODO: move to tramp */
+	init_reg_param(&params[4], "windowstart", 32, PARAM_OUT);	/*TODO: move to tramp */
+	init_reg_param(&params[5], "ps", 32, PARAM_OUT);
+	buf_set_u32(params[0].value, 0, 32, 0);	/* a0 TODO: move to tramp */
+	buf_set_u32(params[1].value, 0, 32, stack_addr);	/* a1 */
+	buf_set_u32(params[2].value, 0, 32, run->stub.entry);	/* a8 */
+	buf_set_u32(params[3].value, 0, 32, 0x0);	/* initial window base TODO: move to tramp */
+	buf_set_u32(params[4].value, 0, 32, 0x1);	/* initial window start TODO: move to tramp */
+	buf_set_u32(params[5].value, 0, 32, 0x60025);	/* enable WOE, UM and debug interrupts level (6) */
+	return ERROR_OK;
+}
+
+static int esp_xtensa_algo_init(struct target *target, struct esp_algorithm_run_data *run,
+	uint32_t num_args, va_list ap)
+{
+	enum xtensa_mode core_mode = XT_MODE_ANY;
+	static const char *const arg_regs[] = { "a2", "a3", "a4", "a5", "a6" };
+
+	if (!run)
+		return ERROR_FAIL;
+
+	if (num_args > ARRAY_SIZE(arg_regs)) {
+		LOG_ERROR("Too many algo user args %u! Max %zu args are supported.", num_args, ARRAY_SIZE(arg_regs));
+		return ERROR_FAIL;
+	}
+
+	struct xtensa_algorithm *ainfo = calloc(1, sizeof(struct xtensa_algorithm));
+	if (!ainfo) {
+		LOG_ERROR("Unable to allocate memory");
+		return ERROR_FAIL;
+	}
+
+	if (run->arch_info) {
+		struct xtensa_algorithm *xtensa_algo = run->arch_info;
+		core_mode = xtensa_algo->core_mode;
+	}
+
+	run->reg_args.first_user_param = ESP_XTENSA_STUB_ARGS_FUNC_START;
+	run->reg_args.count = run->reg_args.first_user_param + num_args;
+	if (num_args == 0)
+		run->reg_args.count++;	/* a2 reg is used as the 1st arg and return code */
+	LOG_DEBUG("reg params count %d (%d/%d).",
+		run->reg_args.count,
+		run->reg_args.first_user_param,
+		num_args);
+	run->reg_args.params = calloc(run->reg_args.count, sizeof(struct reg_param));
+	if (!run->reg_args.params) {
+		free(ainfo);
+		LOG_ERROR("Unable to allocate memory");
+		return ERROR_FAIL;
+	}
+
+	esp_xtensa_algo_regs_init_start(target, run);
+
+	init_reg_param(&run->reg_args.params[run->reg_args.first_user_param + 0], "a2", 32, PARAM_IN_OUT);
+
+	if (num_args > 0) {
+		uint32_t arg = va_arg(ap, uint32_t);
+		esp_algorithm_user_arg_set_uint(run, 0, arg);
+		LOG_DEBUG("Set arg[0] = %d (%s)", arg, run->reg_args.params[run->reg_args.first_user_param + 0].reg_name);
+	} else {
+		esp_algorithm_user_arg_set_uint(run, 0, 0);
+	}
+
+	for (unsigned int i = 1; i < num_args; i++) {
+		uint32_t arg = va_arg(ap, uint32_t);
+		init_reg_param(&run->reg_args.params[run->reg_args.first_user_param + i], (char *)arg_regs[i], 32, PARAM_OUT);
+		esp_algorithm_user_arg_set_uint(run, i, arg);
+		LOG_DEBUG("Set arg[%d] = %d (%s)", i, arg, run->reg_args.params[run->reg_args.first_user_param + i].reg_name);
+	}
+
+	ainfo->core_mode = core_mode;
+	run->stub.ainfo = ainfo;
+	return ERROR_OK;
+}
+
+static int esp_xtensa_algo_cleanup(struct target *target, struct esp_algorithm_run_data *run)
+{
+	free(run->stub.ainfo);
+	for (uint32_t i = 0; i < run->reg_args.count; i++)
+		destroy_reg_param(&run->reg_args.params[i]);
+	free(run->reg_args.params);
+	return ERROR_OK;
+}
diff --git a/src/target/espressif/esp_xtensa_algorithm.h b/src/target/espressif/esp_xtensa_algorithm.h
new file mode 100644
index 0000000..36fa1a3
--- /dev/null
+++ b/src/target/espressif/esp_xtensa_algorithm.h
@@ -0,0 +1,19 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+
+/***************************************************************************
+ *   Module to run arbitrary code on Xtensa using OpenOCD                  *
+ *   Copyright (C) 2019 Espressif Systems Ltd.                             *
+ ***************************************************************************/
+
+#ifndef OPENOCD_TARGET_ESP_XTENSA_ALGO_H
+#define OPENOCD_TARGET_ESP_XTENSA_ALGO_H
+
+#include <target/xtensa/xtensa.h>
+#include <target/espressif/esp_algorithm.h>
+
+/** Index of the first user-defined algo arg. @see algorithm_stub */
+#define ESP_XTENSA_STUB_ARGS_FUNC_START             6
+
+extern const struct esp_algorithm_hw xtensa_algo_hw;
+
+#endif	/* OPENOCD_TARGET_XTENSA_ALGO_H */
diff --git a/src/target/espressif/esp_xtensa_smp.c b/src/target/espressif/esp_xtensa_smp.c
index 1d70be9..f883b1c 100644
--- a/src/target/espressif/esp_xtensa_smp.c
+++ b/src/target/espressif/esp_xtensa_smp.c
@@ -16,6 +16,7 @@
 #include <target/semihosting_common.h>
 #include "esp_xtensa_smp.h"
 #include "esp_xtensa_semihosting.h"
+#include "esp_algorithm.h"
 
 /*
 Multiprocessor stuff common:
@@ -495,6 +496,83 @@ int esp_xtensa_smp_watchpoint_remove(struct target *target, struct watchpoint *w
 	return ERROR_OK;
 }
 
+int esp_xtensa_smp_run_func_image(struct target *target, struct esp_algorithm_run_data *run, uint32_t num_args, ...)
+{
+	struct target *run_target = target;
+	struct target_list *head;
+	va_list ap;
+	uint32_t smp_break = 0;
+	int res;
+
+	if (target->smp) {
+		/* find first HALTED and examined core */
+		foreach_smp_target(head, target->smp_targets) {
+			run_target = head->target;
+			if (target_was_examined(run_target) && run_target->state == TARGET_HALTED)
+				break;
+		}
+		if (!head) {
+			LOG_ERROR("Failed to find HALTED core!");
+			return ERROR_FAIL;
+		}
+
+		res = esp_xtensa_smp_smpbreak_disable(run_target, &smp_break);
+		if (res != ERROR_OK)
+			return res;
+	}
+
+	va_start(ap, num_args);
+	int algo_res = esp_algorithm_run_func_image_va(run_target, run, num_args, ap);
+	va_end(ap);
+
+	if (target->smp) {
+		res = esp_xtensa_smp_smpbreak_restore(run_target, smp_break);
+		if (res != ERROR_OK)
+			return res;
+	}
+	return algo_res;
+}
+
+int esp_xtensa_smp_run_onboard_func(struct target *target,
+	struct esp_algorithm_run_data *run,
+	uint32_t func_addr,
+	uint32_t num_args,
+	...)
+{
+	struct target *run_target = target;
+	struct target_list *head;
+	va_list ap;
+	uint32_t smp_break = 0;
+	int res;
+
+	if (target->smp) {
+		/* find first HALTED and examined core */
+		foreach_smp_target(head, target->smp_targets) {
+			run_target = head->target;
+			if (target_was_examined(run_target) && run_target->state == TARGET_HALTED)
+				break;
+		}
+		if (!head) {
+			LOG_ERROR("Failed to find HALTED core!");
+			return ERROR_FAIL;
+		}
+		res = esp_xtensa_smp_smpbreak_disable(run_target, &smp_break);
+		if (res != ERROR_OK)
+			return res;
+	}
+
+	va_start(ap, num_args);
+	int algo_res = esp_algorithm_run_onboard_func_va(run_target, run, func_addr, num_args, ap);
+	va_end(ap);
+
+	if (target->smp) {
+		res = esp_xtensa_smp_smpbreak_restore(run_target, smp_break);
+		if (res != ERROR_OK)
+			return res;
+	}
+	return algo_res;
+}
+
 int esp_xtensa_smp_init_arch_info(struct target *target,
 	struct esp_xtensa_smp_common *esp_xtensa_smp,
 	struct xtensa_debug_module_config *dm_cfg,
@@ -746,7 +824,7 @@ COMMAND_HANDLER(esp_xtensa_smp_cmd_perfmon_dump)
 		struct target *curr;
 		foreach_smp_target(head, target->smp_targets) {
 			curr = head->target;
-			LOG_INFO("CPU%d:", curr->coreid);
+			LOG_TARGET_INFO(curr, ":");
 			int ret = CALL_COMMAND_HANDLER(xtensa_cmd_perfmon_dump_do,
 				target_to_xtensa(curr));
 			if (ret != ERROR_OK)
diff --git a/src/target/espressif/esp_xtensa_smp.h b/src/target/espressif/esp_xtensa_smp.h
index 4e4f3b3..39afd8a 100644
--- a/src/target/espressif/esp_xtensa_smp.h
+++ b/src/target/espressif/esp_xtensa_smp.h
@@ -9,6 +9,7 @@
 #define OPENOCD_TARGET_XTENSA_ESP_SMP_H
 
 #include "esp_xtensa.h"
+#include "esp_algorithm.h"
 
 struct esp_xtensa_smp_chip_ops {
 	int (*poll)(struct target *target);
@@ -47,7 +48,12 @@ int esp_xtensa_smp_init_arch_info(struct target *target,
 	struct xtensa_debug_module_config *dm_cfg,
 	const struct esp_xtensa_smp_chip_ops *chip_ops,
 	const struct esp_semihost_ops *semihost_ops);
-
+int esp_xtensa_smp_run_func_image(struct target *target, struct esp_algorithm_run_data *run, uint32_t num_args, ...);
+int esp_xtensa_smp_run_onboard_func(struct target *target,
+	struct esp_algorithm_run_data *run,
+	uint32_t func_addr,
+	uint32_t num_args,
+	...);
 extern const struct command_registration esp_xtensa_smp_command_handlers[];
 extern const struct command_registration esp_xtensa_smp_xtensa_command_handlers[];
 extern const struct command_registration esp_xtensa_smp_esp_command_handlers[];
diff --git a/src/target/mips32.h b/src/target/mips32.h
index d072eb9..fc89624 100644
--- a/src/target/mips32.h
+++ b/src/target/mips32.h
@@ -380,7 +380,8 @@ struct mips32_algorithm {
 #define MIPS32_OP_XORI	0x0Eu
 #define MIPS32_OP_XOR	0x26u
 #define MIPS32_OP_SLTU	0x2Bu
-#define MIPS32_OP_SRL	0x03u
+#define MIPS32_OP_SRL	0x02u
+#define MIPS32_OP_SRA	0x03u
 #define MIPS32_OP_SYNCI	0x1Fu
 #define MIPS32_OP_SLL	0x00u
 #define MIPS32_OP_SLTI	0x0Au
@@ -439,7 +440,8 @@ struct mips32_algorithm {
 #define MIPS32_ISA_SLL(dst, src, sa)		MIPS32_R_INST(MIPS32_OP_SPECIAL, 0, src, dst, sa, MIPS32_OP_SLL)
 #define MIPS32_ISA_SLTI(tar, src, val)		MIPS32_I_INST(MIPS32_OP_SLTI, src, tar, val)
 #define MIPS32_ISA_SLTU(dst, src, tar)		MIPS32_R_INST(MIPS32_OP_SPECIAL, src, tar, dst, 0, MIPS32_OP_SLTU)
-#define MIPS32_ISA_SRL(reg, src, off)		MIPS32_R_INST(0, 0, src, reg, off, MIPS32_OP_SRL)
+#define MIPS32_ISA_SRA(reg, src, off)		MIPS32_R_INST(MIPS32_OP_SPECIAL, 0, src, reg, off, MIPS32_OP_SRA)
+#define MIPS32_ISA_SRL(reg, src, off)		MIPS32_R_INST(MIPS32_OP_SPECIAL, 0, src, reg, off, MIPS32_OP_SRL)
 #define MIPS32_ISA_SYNC				0xFu
 #define MIPS32_ISA_SYNCI(off, base)		MIPS32_I_INST(MIPS32_OP_REGIMM, base, MIPS32_OP_SYNCI, off)
 
diff --git a/src/target/mips32_pracc.c b/src/target/mips32_pracc.c
index 9f0d87c..db50ef9 100644
--- a/src/target/mips32_pracc.c
+++ b/src/target/mips32_pracc.c
@@ -842,12 +842,12 @@ int mips32_pracc_write_regs(struct mips32_common *mips32)
 	};
 
 	uint32_t cp0_write_data[] = {
+		/* status */
+		c0rs[0],
 		/* lo */
 		gprs[32],
 		/* hi */
 		gprs[33],
-		/* status */
-		c0rs[0],
 		/* badvaddr */
 		c0rs[1],
 		/* cause */
@@ -856,6 +856,9 @@ int mips32_pracc_write_regs(struct mips32_common *mips32)
 		c0rs[3],
 	};
 
+	/* Write CP0 Status Register first, changes on EXL or ERL bits
+	 * may lead to different behaviour on writing to other CP0 registers.
+	 */
 	for (size_t i = 0; i < ARRAY_SIZE(cp0_write_code); i++) {
 		/* load CP0 value in $1 */
 		pracc_add_li32(&ctx, 1, cp0_write_data[i], 0);
diff --git a/src/target/mips_m4k.c b/src/target/mips_m4k.c
index 0a06bb1..ad98089 100644
--- a/src/target/mips_m4k.c
+++ b/src/target/mips_m4k.c
@@ -142,7 +142,7 @@ static int mips_m4k_halt_smp(struct target *target)
 			ret = mips_m4k_halt(curr);
 
 		if (ret != ERROR_OK) {
-			LOG_ERROR("halt failed target->coreid: %" PRId32, curr->coreid);
+			LOG_TARGET_ERROR(curr, "halt failed.");
 			retval = ret;
 		}
 	}
@@ -412,8 +412,8 @@ static int mips_m4k_restore_smp(struct target *target, uint32_t address, int han
 						   handle_breakpoints, 0);
 
 			if (ret != ERROR_OK) {
-				LOG_ERROR("target->coreid :%" PRId32 " failed to resume at address :0x%" PRIx32,
-						  curr->coreid, address);
+				LOG_TARGET_ERROR(curr, "failed to resume at address: 0x%" PRIx32,
+						address);
 				retval = ret;
 			}
 		}
diff --git a/src/target/smp.c b/src/target/smp.c
index effc63f..50b19d0 100644
--- a/src/target/smp.c
+++ b/src/target/smp.c
@@ -132,6 +132,9 @@ COMMAND_HANDLER(handle_smp_gdb_command)
 {
 	struct target *target = get_current_target(CMD_CTX);
 	int retval = ERROR_OK;
+
+	LOG_WARNING(DEPRECATED_MSG);
+
 	if (!list_empty(target->smp_targets)) {
 		if (CMD_ARGC == 1) {
 			int coreid = 0;
diff --git a/src/target/target.c b/src/target/target.c
index cfd0641..216dcb2 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -299,23 +299,6 @@ const char *target_reset_mode_name(enum target_reset_mode reset_mode)
 	return cp;
 }
 
-/* determine the number of the new target */
-static int new_target_number(void)
-{
-	struct target *t;
-	int x;
-
-	/* number is 0 based */
-	x = -1;
-	t = all_targets;
-	while (t) {
-		if (x < t->target_number)
-			x = t->target_number;
-		t = t->next;
-	}
-	return x + 1;
-}
-
 static void append_to_list_all_targets(struct target *target)
 {
 	struct target **t = &all_targets;
@@ -451,7 +434,7 @@ void target_buffer_set_u16_array(struct target *target, uint8_t *buffer, uint32_
 		target_buffer_set_u16(target, &buffer[i * 2], srcbuf[i]);
 }
 
-/* return a pointer to a configured target; id is name or number */
+/* return a pointer to a configured target; id is name or index in all_targets */
 struct target *get_target(const char *id)
 {
 	struct target *target;
@@ -464,36 +447,17 @@ struct target *get_target(const char *id)
 			return target;
 	}
 
-	/* It's OK to remove this fallback sometime after August 2010 or so */
-
-	/* no match, try as number */
-	unsigned num;
-	if (parse_uint(id, &num) != ERROR_OK)
+	/* try as index */
+	unsigned int index, counter;
+	if (parse_uint(id, &index) != ERROR_OK)
 		return NULL;
 
-	for (target = all_targets; target; target = target->next) {
-		if (target->target_number == (int)num) {
-			LOG_WARNING("use '%s' as target identifier, not '%u'",
-					target_name(target), num);
-			return target;
-		}
-	}
-
-	return NULL;
-}
+	for (target = all_targets, counter = index;
+			target && counter;
+			target = target->next, --counter)
+		;
 
-/* returns a pointer to the n-th configured target */
-struct target *get_target_by_num(int num)
-{
-	struct target *target = all_targets;
-
-	while (target) {
-		if (target->target_number == num)
-			return target;
-		target = target->next;
-	}
-
-	return NULL;
+	return target;
 }
 
 struct target *get_current_target(struct command_context *cmd_ctx)
@@ -712,10 +676,14 @@ static int default_check_reset(struct target *target)
  * Keep in sync */
 int target_examine_one(struct target *target)
 {
+	LOG_TARGET_DEBUG(target, "Examination started");
+
 	target_call_event_callbacks(target, TARGET_EVENT_EXAMINE_START);
 
 	int retval = target->type->examine(target);
 	if (retval != ERROR_OK) {
+		LOG_TARGET_ERROR(target, "Examination failed");
+		LOG_TARGET_DEBUG(target, "examine() returned error code %d", retval);
 		target_reset_examined(target);
 		target_call_event_callbacks(target, TARGET_EVENT_EXAMINE_FAIL);
 		return retval;
@@ -725,6 +693,7 @@ int target_examine_one(struct target *target)
 	target_set_examined(target);
 	target_call_event_callbacks(target, TARGET_EVENT_EXAMINE_END);
 
+	LOG_TARGET_INFO(target, "Examination succeed");
 	return ERROR_OK;
 }
 
@@ -2222,6 +2191,9 @@ uint32_t target_get_working_area_avail(struct target *target)
 
 static void target_destroy(struct target *target)
 {
+	breakpoint_remove_all(target);
+	watchpoint_remove_all(target);
+
 	if (target->type->deinit_target)
 		target->type->deinit_target(target);
 
@@ -2850,10 +2822,10 @@ COMMAND_HANDLER(handle_targets_command)
 		}
 	}
 
-	struct target *target = all_targets;
+	unsigned int index = 0;
 	command_print(CMD, "    TargetName         Type       Endian TapName            State       ");
 	command_print(CMD, "--  ------------------ ---------- ------ ------------------ ------------");
-	while (target) {
+	for (struct target *target = all_targets; target; target = target->next, ++index) {
 		const char *state;
 		char marker = ' ';
 
@@ -2868,7 +2840,7 @@ COMMAND_HANDLER(handle_targets_command)
 		/* keep columns lined up to match the headers above */
 		command_print(CMD,
 				"%2d%c %-18s %-10s %-6s %-18s %s",
-				target->target_number,
+				index,
 				marker,
 				target_name(target),
 				target_type_name(target),
@@ -2876,7 +2848,6 @@ COMMAND_HANDLER(handle_targets_command)
 					target->endianness)->name,
 				target->tap->dotted_name,
 				state);
-		target = target->next;
 	}
 
 	return retval;
@@ -3973,7 +3944,7 @@ static int handle_bp_command_set(struct command_invocation *cmd,
 
 	} else if (addr == 0) {
 		if (!target->type->add_context_breakpoint) {
-			LOG_ERROR("Context breakpoint not available");
+			LOG_TARGET_ERROR(target, "Context breakpoint not available");
 			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 		}
 		retval = context_breakpoint_add(target, asid, length, hw);
@@ -3983,7 +3954,7 @@ static int handle_bp_command_set(struct command_invocation *cmd,
 
 	} else {
 		if (!target->type->add_hybrid_breakpoint) {
-			LOG_ERROR("Hybrid breakpoint not available");
+			LOG_TARGET_ERROR(target, "Hybrid breakpoint not available");
 			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 		}
 		retval = hybrid_breakpoint_add(target, addr, asid, length, hw);
@@ -4120,7 +4091,7 @@ COMMAND_HANDLER(handle_wp_command)
 			type = WPT_ACCESS;
 			break;
 		default:
-			LOG_ERROR("invalid watchpoint mode ('%c')", CMD_ARGV[2][0]);
+			LOG_TARGET_ERROR(target, "invalid watchpoint mode ('%c')", CMD_ARGV[2][0]);
 			return ERROR_COMMAND_SYNTAX_ERROR;
 		}
 		/* fall through */
@@ -4136,7 +4107,7 @@ COMMAND_HANDLER(handle_wp_command)
 	int retval = watchpoint_add(target, addr, length, type,
 			data_value, data_mask);
 	if (retval != ERROR_OK)
-		LOG_ERROR("Failure setting watchpoints");
+		LOG_TARGET_ERROR(target, "Failure setting watchpoints");
 
 	return retval;
 }
@@ -4329,7 +4300,7 @@ COMMAND_HANDLER(handle_profile_command)
 	if ((CMD_ARGC != 2) && (CMD_ARGC != 4))
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
-	const uint32_t MAX_PROFILE_SAMPLE_NUM = 10000;
+	const uint32_t MAX_PROFILE_SAMPLE_NUM = 1000000;
 	uint32_t offset;
 	uint32_t num_of_samples;
 	int retval = ERROR_OK;
@@ -5062,8 +5033,7 @@ void target_handle_event(struct target *target, enum target_event e)
 
 	for (teap = target->event_action; teap; teap = teap->next) {
 		if (teap->event == e) {
-			LOG_DEBUG("target(%d): %s (%s) event: %d (%s) action: %s",
-					   target->target_number,
+			LOG_DEBUG("target: %s (%s) event: %d (%s) action: %s",
 					   target_name(target),
 					   target_type_name(target),
 					   e,
@@ -5482,13 +5452,13 @@ no_params:
 				e = jim_getopt_wide(goi, &w);
 				if (e != JIM_OK)
 					return e;
-				/* make this exactly 1 or 0 */
-				target->backup_working_area = (!!w);
+				/* make this boolean */
+				target->backup_working_area = (w != 0);
 			} else {
 				if (goi->argc != 0)
 					goto no_params;
 			}
-			Jim_SetResult(goi->interp, Jim_NewIntObj(goi->interp, target->backup_working_area));
+			Jim_SetResult(goi->interp, Jim_NewIntObj(goi->interp, target->backup_working_area ? 1 : 0));
 			/* loop for more e*/
 			break;
 
@@ -5848,8 +5818,7 @@ COMMAND_HANDLER(handle_target_event_list)
 	struct target *target = get_current_target(CMD_CTX);
 	struct target_event_action *teap = target->event_action;
 
-	command_print(CMD, "Event actions for target (%d) %s\n",
-				   target->target_number,
+	command_print(CMD, "Event actions for target %s\n",
 				   target_name(target));
 	command_print(CMD, "%-25s | Body", "Event");
 	command_print(CMD, "------------------------- | "
@@ -5883,7 +5852,17 @@ COMMAND_HANDLER(handle_target_debug_reason)
 
 	struct target *target = get_current_target(CMD_CTX);
 
-	command_print(CMD, "%s", debug_reason_name(target));
+
+	const char *debug_reason = nvp_value2name(nvp_target_debug_reason,
+		target->debug_reason)->name;
+
+	if (!debug_reason) {
+		command_print(CMD, "bug: invalid debug reason (%d)",
+			target->debug_reason);
+		return ERROR_FAIL;
+	}
+
+	command_print(CMD, "%s", debug_reason);
 
 	return ERROR_OK;
 }
@@ -6188,9 +6167,6 @@ static int target_create(struct jim_getopt_info *goi)
 	/* set empty smp cluster */
 	target->smp_targets = &empty_smp_targets;
 
-	/* set target number */
-	target->target_number = new_target_number();
-
 	/* allocate memory for each unique target type */
 	target->type = malloc(sizeof(struct target_type));
 	if (!target->type) {
@@ -6207,7 +6183,7 @@ static int target_create(struct jim_getopt_info *goi)
 	target->working_area        = 0x0;
 	target->working_area_size   = 0x0;
 	target->working_areas       = NULL;
-	target->backup_working_area = 0;
+	target->backup_working_area = false;
 
 	target->state               = TARGET_UNKNOWN;
 	target->debug_reason        = DBG_REASON_UNDEFINED;
@@ -7095,7 +7071,7 @@ static const struct command_registration target_exec_command_handlers[] = {
 		.handler = handle_wp_command,
 		.mode = COMMAND_EXEC,
 		.help = "list (no params) or create watchpoints",
-		.usage = "[address length [('r'|'w'|'a') value [mask]]]",
+		.usage = "[address length [('r'|'w'|'a') [value [mask]]]]",
 	},
 	{
 		.name = "rwp",
diff --git a/src/target/target.h b/src/target/target.h
index b8f3b01..8b2e362 100644
--- a/src/target/target.h
+++ b/src/target/target.h
@@ -118,7 +118,6 @@ enum target_register_class {
 struct target {
 	struct target_type *type;			/* target type definition (name, access functions) */
 	char *cmd_name;				/* tcl Name of target */
-	int target_number;					/* DO NOT USE!  field to be removed in 2010 */
 	struct jtag_tap *tap;				/* where on the jtag chain is this */
 	int32_t coreid;						/* which device on the TAP? */
 
@@ -152,7 +151,7 @@ struct target {
 	bool working_area_phys_spec;		/* physical address specified? */
 	target_addr_t working_area_phys;			/* physical address */
 	uint32_t working_area_size;			/* size in bytes */
-	uint32_t backup_working_area;		/* whether the content of the working area has to be preserved */
+	bool backup_working_area;			/* whether the content of the working area has to be preserved */
 	struct working_area *working_areas;/* list of allocated working areas */
 	enum target_debug_reason debug_reason;/* reason why the target entered debug state */
 	enum target_endianness endianness;	/* target endianness */
@@ -418,7 +417,6 @@ int target_call_timer_callbacks_now(void);
  */
 int64_t target_timer_next_event(void);
 
-struct target *get_target_by_num(int num);
 struct target *get_current_target(struct command_context *cmd_ctx);
 struct target *get_current_target_or_null(struct command_context *cmd_ctx);
 struct target *get_target(const char *id);
diff --git a/src/target/xtensa/xtensa.c b/src/target/xtensa/xtensa.c
index c575b53..d2ca32c 100644
--- a/src/target/xtensa/xtensa.c
+++ b/src/target/xtensa/xtensa.c
@@ -16,6 +16,7 @@
 #include <helper/time_support.h>
 #include <helper/align.h>
 #include <target/register.h>
+#include <target/algorithm.h>
 
 #include "xtensa_chip.h"
 #include "xtensa.h"
@@ -822,7 +823,7 @@ int xtensa_examine(struct target *target)
 	struct xtensa *xtensa = target_to_xtensa(target);
 	unsigned int cmd = PWRCTL_DEBUGWAKEUP(xtensa) | PWRCTL_MEMWAKEUP(xtensa) | PWRCTL_COREWAKEUP(xtensa);
 
-	LOG_DEBUG("coreid = %d", target->coreid);
+	LOG_TARGET_DEBUG(target, "");
 
 	if (xtensa->core_config->core_type == XT_UNDEF) {
 		LOG_ERROR("XTensa core not configured; is xtensa-core-openocd.cfg missing?");
@@ -1096,7 +1097,7 @@ int xtensa_assert_reset(struct target *target)
 {
 	struct xtensa *xtensa = target_to_xtensa(target);
 
-	LOG_TARGET_DEBUG(target, "target_number=%i, begin", target->target_number);
+	LOG_TARGET_DEBUG(target, " begin");
 	xtensa_queue_pwr_reg_write(xtensa,
 		XDMREG_PWRCTL,
 		PWRCTL_JTAGDEBUGUSE(xtensa) | PWRCTL_DEBUGWAKEUP(xtensa) | PWRCTL_MEMWAKEUP(xtensa) |
@@ -2635,6 +2636,214 @@ int xtensa_watchpoint_remove(struct target *target, struct watchpoint *watchpoin
 	return ERROR_OK;
 }
 
+int xtensa_start_algorithm(struct target *target,
+	int num_mem_params, struct mem_param *mem_params,
+	int num_reg_params, struct reg_param *reg_params,
+	target_addr_t entry_point, target_addr_t exit_point,
+	void *arch_info)
+{
+	struct xtensa *xtensa = target_to_xtensa(target);
+	struct xtensa_algorithm *algorithm_info = arch_info;
+	int retval = ERROR_OK;
+	bool usr_ps = false;
+
+	/* NOTE: xtensa_run_algorithm requires that each algorithm uses a software breakpoint
+	 * at the exit point */
+
+	if (target->state != TARGET_HALTED) {
+		LOG_WARNING("Target not halted!");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	for (unsigned int i = 0; i < xtensa->core_cache->num_regs; i++) {
+		struct reg *reg = &xtensa->core_cache->reg_list[i];
+		buf_cpy(reg->value, xtensa->algo_context_backup[i], reg->size);
+	}
+	/* save debug reason, it will be changed */
+	algorithm_info->ctx_debug_reason = target->debug_reason;
+	/* write mem params */
+	for (int i = 0; i < num_mem_params; i++) {
+		if (mem_params[i].direction != PARAM_IN) {
+			retval = target_write_buffer(target, mem_params[i].address,
+				mem_params[i].size,
+				mem_params[i].value);
+			if (retval != ERROR_OK)
+				return retval;
+		}
+	}
+	/* write reg params */
+	for (int i = 0; i < num_reg_params; i++) {
+		if (reg_params[i].size > 32) {
+			LOG_ERROR("BUG: not supported register size (%d)", reg_params[i].size);
+			return ERROR_FAIL;
+		}
+		struct reg *reg = register_get_by_name(xtensa->core_cache, reg_params[i].reg_name, 0);
+		if (!reg) {
+			LOG_ERROR("BUG: register '%s' not found", reg_params[i].reg_name);
+			return ERROR_FAIL;
+		}
+		if (reg->size != reg_params[i].size) {
+			LOG_ERROR("BUG: register '%s' size doesn't match reg_params[i].size", reg_params[i].reg_name);
+			return ERROR_FAIL;
+		}
+		if (memcmp(reg_params[i].reg_name, "ps", 3)) {
+			usr_ps = true;
+		} else {
+			unsigned int reg_id = xtensa->eps_dbglevel_idx;
+			assert(reg_id < xtensa->core_cache->num_regs && "Attempt to access non-existing reg!");
+			reg = &xtensa->core_cache->reg_list[reg_id];
+		}
+		xtensa_reg_set_value(reg, buf_get_u32(reg_params[i].value, 0, reg->size));
+		reg->valid = 1;
+	}
+	/* ignore custom core mode if custom PS value is specified */
+	if (!usr_ps) {
+		unsigned int eps_reg_idx = xtensa->eps_dbglevel_idx;
+		xtensa_reg_val_t ps = xtensa_reg_get(target, eps_reg_idx);
+		enum xtensa_mode core_mode = XT_PS_RING_GET(ps);
+		if (algorithm_info->core_mode != XT_MODE_ANY && algorithm_info->core_mode != core_mode) {
+			LOG_DEBUG("setting core_mode: 0x%x", algorithm_info->core_mode);
+			xtensa_reg_val_t new_ps = (ps & ~XT_PS_RING_MSK) | XT_PS_RING(algorithm_info->core_mode);
+			/* save previous core mode */
+			/* TODO: core_mode is not restored for now. Can be added to the end of wait_algorithm */
+			algorithm_info->core_mode = core_mode;
+			xtensa_reg_set(target, eps_reg_idx, new_ps);
+			xtensa->core_cache->reg_list[eps_reg_idx].valid = 1;
+		}
+	}
+
+	return xtensa_resume(target, 0, entry_point, 1, 1);
+}
+
+/** Waits for an algorithm in the target. */
+int xtensa_wait_algorithm(struct target *target,
+	int num_mem_params, struct mem_param *mem_params,
+	int num_reg_params, struct reg_param *reg_params,
+	target_addr_t exit_point, unsigned int timeout_ms,
+	void *arch_info)
+{
+	struct xtensa *xtensa = target_to_xtensa(target);
+	struct xtensa_algorithm *algorithm_info = arch_info;
+	int retval = ERROR_OK;
+	xtensa_reg_val_t pc;
+
+	/* NOTE: xtensa_run_algorithm requires that each algorithm uses a software breakpoint
+	 * at the exit point */
+
+	retval = target_wait_state(target, TARGET_HALTED, timeout_ms);
+	/* If the target fails to halt due to the breakpoint, force a halt */
+	if (retval != ERROR_OK || target->state != TARGET_HALTED) {
+		retval = target_halt(target);
+		if (retval != ERROR_OK)
+			return retval;
+		retval = target_wait_state(target, TARGET_HALTED, 500);
+		if (retval != ERROR_OK)
+			return retval;
+		LOG_TARGET_ERROR(target, "not halted %d, pc 0x%" PRIx32 ", ps 0x%" PRIx32, retval,
+			xtensa_reg_get(target, XT_REG_IDX_PC),
+			xtensa_reg_get(target, xtensa->eps_dbglevel_idx));
+		return ERROR_TARGET_TIMEOUT;
+	}
+	pc = xtensa_reg_get(target, XT_REG_IDX_PC);
+	if (exit_point && pc != exit_point) {
+		LOG_ERROR("failed algorithm halted at 0x%" PRIx32 ", expected " TARGET_ADDR_FMT, pc, exit_point);
+		return ERROR_TARGET_TIMEOUT;
+	}
+	/* Copy core register values to reg_params[] */
+	for (int i = 0; i < num_reg_params; i++) {
+		if (reg_params[i].direction != PARAM_OUT) {
+			struct reg *reg = register_get_by_name(xtensa->core_cache, reg_params[i].reg_name, 0);
+			if (!reg) {
+				LOG_ERROR("BUG: register '%s' not found", reg_params[i].reg_name);
+				return ERROR_FAIL;
+			}
+			if (reg->size != reg_params[i].size) {
+				LOG_ERROR("BUG: register '%s' size doesn't match reg_params[i].size", reg_params[i].reg_name);
+				return ERROR_FAIL;
+			}
+			buf_set_u32(reg_params[i].value, 0, 32, xtensa_reg_get_value(reg));
+		}
+	}
+	/* Read memory values to mem_params */
+	LOG_DEBUG("Read mem params");
+	for (int i = 0; i < num_mem_params; i++) {
+		LOG_DEBUG("Check mem param @ " TARGET_ADDR_FMT, mem_params[i].address);
+		if (mem_params[i].direction != PARAM_OUT) {
+			LOG_DEBUG("Read mem param @ " TARGET_ADDR_FMT, mem_params[i].address);
+			retval = target_read_buffer(target, mem_params[i].address, mem_params[i].size, mem_params[i].value);
+			if (retval != ERROR_OK)
+				return retval;
+		}
+	}
+
+	/* avoid gdb keep_alive warning */
+	keep_alive();
+
+	for (int i = xtensa->core_cache->num_regs - 1; i >= 0; i--) {
+		struct reg *reg = &xtensa->core_cache->reg_list[i];
+		if (i == XT_REG_IDX_PS) {
+			continue;	/* restore mapped reg number of PS depends on NDEBUGLEVEL */
+		} else if (i == XT_REG_IDX_DEBUGCAUSE) {
+			/*FIXME: restoring DEBUGCAUSE causes exception when executing corresponding
+			* instruction in DIR */
+			LOG_DEBUG("Skip restoring register %s: 0x%8.8" PRIx32 " -> 0x%8.8" PRIx32,
+				xtensa->core_cache->reg_list[i].name,
+				buf_get_u32(reg->value, 0, 32),
+				buf_get_u32(xtensa->algo_context_backup[i], 0, 32));
+			buf_cpy(xtensa->algo_context_backup[i], reg->value, reg->size);
+			xtensa->core_cache->reg_list[i].dirty = 0;
+			xtensa->core_cache->reg_list[i].valid = 0;
+		} else if (memcmp(xtensa->algo_context_backup[i], reg->value, reg->size / 8)) {
+			if (reg->size <= 32) {
+				LOG_DEBUG("restoring register %s: 0x%8.8" PRIx32 " -> 0x%8.8" PRIx32,
+					xtensa->core_cache->reg_list[i].name,
+					buf_get_u32(reg->value, 0, reg->size),
+					buf_get_u32(xtensa->algo_context_backup[i], 0, reg->size));
+			} else if (reg->size <= 64) {
+				LOG_DEBUG("restoring register %s: 0x%8.8" PRIx64 " -> 0x%8.8" PRIx64,
+					xtensa->core_cache->reg_list[i].name,
+					buf_get_u64(reg->value, 0, reg->size),
+					buf_get_u64(xtensa->algo_context_backup[i], 0, reg->size));
+			} else {
+				LOG_DEBUG("restoring register %s %u-bits", xtensa->core_cache->reg_list[i].name, reg->size);
+			}
+			buf_cpy(xtensa->algo_context_backup[i], reg->value, reg->size);
+			xtensa->core_cache->reg_list[i].dirty = 1;
+			xtensa->core_cache->reg_list[i].valid = 1;
+		}
+	}
+	target->debug_reason = algorithm_info->ctx_debug_reason;
+
+	retval = xtensa_write_dirty_registers(target);
+	if (retval != ERROR_OK)
+		LOG_ERROR("Failed to write dirty regs (%d)!", retval);
+
+	return retval;
+}
+
+int xtensa_run_algorithm(struct target *target,
+	int num_mem_params, struct mem_param *mem_params,
+	int num_reg_params, struct reg_param *reg_params,
+	target_addr_t entry_point, target_addr_t exit_point,
+	unsigned int timeout_ms, void *arch_info)
+{
+	int retval = xtensa_start_algorithm(target,
+		num_mem_params, mem_params,
+		num_reg_params, reg_params,
+		entry_point, exit_point,
+		arch_info);
+
+	if (retval == ERROR_OK) {
+		retval = xtensa_wait_algorithm(target,
+			num_mem_params, mem_params,
+			num_reg_params, reg_params,
+			exit_point, timeout_ms,
+			arch_info);
+	}
+
+	return retval;
+}
+
 static int xtensa_build_reg_cache(struct target *target)
 {
 	struct xtensa *xtensa = target_to_xtensa(target);
@@ -3978,6 +4187,38 @@ COMMAND_HANDLER(xtensa_cmd_smpbreak)
 		get_current_target(CMD_CTX));
 }
 
+COMMAND_HELPER(xtensa_cmd_dm_rw_do, struct xtensa *xtensa)
+{
+	if (CMD_ARGC == 1) {
+		// read: xtensa dm addr
+		uint32_t addr = strtoul(CMD_ARGV[0], NULL, 0);
+		uint32_t val;
+		int res = xtensa_dm_read(&xtensa->dbg_mod, addr, &val);
+		if (res == ERROR_OK)
+			command_print(CMD, "xtensa DM(0x%08" PRIx32 ") -> 0x%08" PRIx32, addr, val);
+		else
+			command_print(CMD, "xtensa DM(0x%08" PRIx32 ") : read ERROR %" PRId32, addr, res);
+		return res;
+	} else if (CMD_ARGC == 2) {
+		// write: xtensa dm addr value
+		uint32_t addr = strtoul(CMD_ARGV[0], NULL, 0);
+		uint32_t val = strtoul(CMD_ARGV[1], NULL, 0);
+		int res = xtensa_dm_write(&xtensa->dbg_mod, addr, val);
+		if (res == ERROR_OK)
+			command_print(CMD, "xtensa DM(0x%08" PRIx32 ") <- 0x%08" PRIx32, addr, val);
+		else
+			command_print(CMD, "xtensa DM(0x%08" PRIx32 ") : write ERROR %" PRId32, addr, res);
+		return res;
+	}
+	return ERROR_COMMAND_SYNTAX_ERROR;
+}
+
+COMMAND_HANDLER(xtensa_cmd_dm_rw)
+{
+	return CALL_COMMAND_HANDLER(xtensa_cmd_dm_rw_do,
+		target_to_xtensa(get_current_target(CMD_CTX)));
+}
+
 COMMAND_HELPER(xtensa_cmd_tracestart_do, struct xtensa *xtensa)
 {
 	struct xtensa_trace_status trace_status;
@@ -4235,6 +4476,13 @@ static const struct command_registration xtensa_any_command_handlers[] = {
 		.usage = "[none|breakinout|runstall] | [BreakIn] [BreakOut] [RunStallIn] [DebugModeOut]",
 	},
 	{
+		.name = "dm",
+		.handler = xtensa_cmd_dm_rw,
+		.mode = COMMAND_ANY,
+		.help = "Xtensa DM read/write",
+		.usage = "addr [value]"
+	},
+	{
 		.name = "perfmon_enable",
 		.handler = xtensa_cmd_perfmon_enable,
 		.mode = COMMAND_EXEC,
diff --git a/src/target/xtensa/xtensa.h b/src/target/xtensa/xtensa.h
index 4216ae2..3b37122 100644
--- a/src/target/xtensa/xtensa.h
+++ b/src/target/xtensa/xtensa.h
@@ -222,6 +222,16 @@ struct xtensa_sw_breakpoint {
 	uint8_t insn_sz;	/* 2 or 3 bytes */
 };
 
+/**
+ * Xtensa algorithm data.
+ */
+struct xtensa_algorithm {
+	/** User can set this to specify which core mode algorithm should be run in. */
+	enum xtensa_mode core_mode;
+	/** Used internally to backup and restore debug_reason. */
+	enum target_debug_reason ctx_debug_reason;
+};
+
 #define XTENSA_COMMON_MAGIC 0x54E4E555U
 
 /**
@@ -395,6 +405,21 @@ int xtensa_breakpoint_add(struct target *target, struct breakpoint *breakpoint);
 int xtensa_breakpoint_remove(struct target *target, struct breakpoint *breakpoint);
 int xtensa_watchpoint_add(struct target *target, struct watchpoint *watchpoint);
 int xtensa_watchpoint_remove(struct target *target, struct watchpoint *watchpoint);
+int xtensa_start_algorithm(struct target *target,
+	int num_mem_params, struct mem_param *mem_params,
+	int num_reg_params, struct reg_param *reg_params,
+	target_addr_t entry_point, target_addr_t exit_point,
+	void *arch_info);
+int xtensa_wait_algorithm(struct target *target,
+	int num_mem_params, struct mem_param *mem_params,
+	int num_reg_params, struct reg_param *reg_params,
+	target_addr_t exit_point, unsigned int timeout_ms,
+	void *arch_info);
+int xtensa_run_algorithm(struct target *target,
+	int num_mem_params, struct mem_param *mem_params,
+	int num_reg_params, struct reg_param *reg_params,
+	target_addr_t entry_point, target_addr_t exit_point,
+	unsigned int timeout_ms, void *arch_info);
 void xtensa_set_permissive_mode(struct target *target, bool state);
 const char *xtensa_get_gdb_arch(struct target *target);
 int xtensa_gdb_query_custom(struct target *target, const char *packet, char **response_p);
diff --git a/src/target/xtensa/xtensa_debug_module.c b/src/target/xtensa/xtensa_debug_module.c
index 31d7a94..8045779 100644
--- a/src/target/xtensa/xtensa_debug_module.c
+++ b/src/target/xtensa/xtensa_debug_module.c
@@ -34,6 +34,16 @@ static const struct xtensa_dm_pwr_reg_offsets xdm_pwr_regs[XDMREG_PWRNUM] =
 static const struct xtensa_dm_reg_offsets xdm_regs[XDMREG_NUM] =
 	XTENSA_DM_REG_OFFSETS;
 
+static enum xtensa_dm_reg xtensa_dm_regaddr_to_id(uint32_t addr)
+{
+	enum xtensa_dm_reg id;
+	uint32_t addr_masked = (addr & (XTENSA_DM_APB_ALIGN - 1));
+	for (id = XDMREG_TRAXID; id < XDMREG_NUM; id++)
+		if (xdm_regs[id].apb == addr_masked)
+			break;
+	return id;
+}
+
 static void xtensa_dm_add_set_ir(struct xtensa_debug_module *dm, uint8_t value)
 {
 	struct scan_field field;
@@ -285,6 +295,34 @@ int xtensa_dm_core_status_clear(struct xtensa_debug_module *dm, xtensa_dsr_t bit
 	return xtensa_dm_queue_execute(dm);
 }
 
+int xtensa_dm_read(struct xtensa_debug_module *dm, uint32_t addr, uint32_t *val)
+{
+	enum xtensa_dm_reg reg = xtensa_dm_regaddr_to_id(addr);
+	uint8_t buf[sizeof(uint32_t)];
+	if (reg < XDMREG_NUM) {
+		xtensa_dm_queue_enable(dm);
+		dm->dbg_ops->queue_reg_read(dm, reg, buf);
+		xtensa_dm_queue_tdi_idle(dm);
+		int res = xtensa_dm_queue_execute(dm);
+		if (res == ERROR_OK && val)
+			*val = buf_get_u32(buf, 0, 32);
+		return res;
+	}
+	return ERROR_FAIL;
+}
+
+int xtensa_dm_write(struct xtensa_debug_module *dm, uint32_t addr, uint32_t val)
+{
+	enum xtensa_dm_reg reg = xtensa_dm_regaddr_to_id(addr);
+	if (reg < XDMREG_NUM) {
+		xtensa_dm_queue_enable(dm);
+		dm->dbg_ops->queue_reg_write(dm, reg, val);
+		xtensa_dm_queue_tdi_idle(dm);
+		return xtensa_dm_queue_execute(dm);
+	}
+	return ERROR_FAIL;
+}
+
 int xtensa_dm_trace_start(struct xtensa_debug_module *dm, struct xtensa_trace_start_config *cfg)
 {
 	/*Turn off trace unit so we can start a new trace. */
diff --git a/src/target/xtensa/xtensa_debug_module.h b/src/target/xtensa/xtensa_debug_module.h
index 46b2935..495da2a 100644
--- a/src/target/xtensa/xtensa_debug_module.h
+++ b/src/target/xtensa/xtensa_debug_module.h
@@ -75,6 +75,22 @@ enum xtensa_dm_reg {
 	XDMREG_DELAYCNT,
 	XDMREG_MEMADDRSTART,
 	XDMREG_MEMADDREND,
+	XDMREG_EXTTIMELO,
+	XDMREG_EXTTIMEHI,
+	XDMREG_TRAXRSVD48,
+	XDMREG_TRAXRSVD4C,
+	XDMREG_TRAXRSVD50,
+	XDMREG_TRAXRSVD54,
+	XDMREG_TRAXRSVD58,
+	XDMREG_TRAXRSVD5C,
+	XDMREG_TRAXRSVD60,
+	XDMREG_TRAXRSVD64,
+	XDMREG_TRAXRSVD68,
+	XDMREG_TRAXRSVD6C,
+	XDMREG_TRAXRSVD70,
+	XDMREG_TRAXRSVD74,
+	XDMREG_CONFIGID0,
+	XDMREG_CONFIGID1,
 
 	/* Performance Monitor Registers */
 	XDMREG_PMG,
@@ -168,6 +184,22 @@ struct xtensa_dm_reg_offsets {
 	{ .nar = 0x07, .apb = 0x001c },	/* XDMREG_DELAYCNT */		\
 	{ .nar = 0x08, .apb = 0x0020 },	/* XDMREG_MEMADDRSTART */	\
 	{ .nar = 0x09, .apb = 0x0024 },	/* XDMREG_MEMADDREND */		\
+	{ .nar = 0x10, .apb = 0x0040 },	/* XDMREG_EXTTIMELO */		\
+	{ .nar = 0x11, .apb = 0x0044 },	/* XDMREG_EXTTIMEHI */		\
+	{ .nar = 0x12, .apb = 0x0048 },	/* XDMREG_TRAXRSVD48 */		\
+	{ .nar = 0x13, .apb = 0x004c },	/* XDMREG_TRAXRSVD4C */		\
+	{ .nar = 0x14, .apb = 0x0050 },	/* XDMREG_TRAXRSVD50 */		\
+	{ .nar = 0x15, .apb = 0x0054 },	/* XDMREG_TRAXRSVD54 */		\
+	{ .nar = 0x16, .apb = 0x0058 },	/* XDMREG_TRAXRSVD58 */		\
+	{ .nar = 0x17, .apb = 0x005c },	/* XDMREG_TRAXRSVD5C */		\
+	{ .nar = 0x18, .apb = 0x0060 },	/* XDMREG_TRAXRSVD60 */		\
+	{ .nar = 0x19, .apb = 0x0064 },	/* XDMREG_TRAXRSVD64 */		\
+	{ .nar = 0x1a, .apb = 0x0068 },	/* XDMREG_TRAXRSVD68 */		\
+	{ .nar = 0x1b, .apb = 0x006c },	/* XDMREG_TRAXRSVD6C */		\
+	{ .nar = 0x1c, .apb = 0x0070 },	/* XDMREG_TRAXRSVD70 */		\
+	{ .nar = 0x1d, .apb = 0x0074 },	/* XDMREG_TRAXRSVD74 */		\
+	{ .nar = 0x1e, .apb = 0x0078 },	/* XDMREG_CONFIGID0 */		\
+	{ .nar = 0x1f, .apb = 0x007c },	/* XDMREG_CONFIGID1 */		\
 																\
 	/* Performance Monitor Registers */							\
 	{ .nar = 0x20, .apb = 0x1000 },	/* XDMREG_PMG */			\
@@ -297,6 +329,11 @@ struct xtensa_dm_reg_offsets {
 #define DEBUGCAUSE_DI               BIT(5)	/* Debug Interrupt */
 #define DEBUGCAUSE_VALID            BIT(31)	/* Pseudo-value to trigger reread (NX only) */
 
+/* TRAXID */
+#define TRAXID_PRODNO_TRAX          0		/* TRAXID.PRODNO value for TRAX module */
+#define TRAXID_PRODNO_SHIFT         28
+#define TRAXID_PRODNO_MASK          0xf
+
 #define TRAXCTRL_TREN               BIT(0)	/* Trace enable. Tracing starts on 0->1 */
 #define TRAXCTRL_TRSTP              BIT(1)	/* Trace Stop. Make 1 to stop trace. */
 #define TRAXCTRL_PCMEN              BIT(2)	/* PC match enable */
@@ -512,6 +549,9 @@ static inline xtensa_dsr_t xtensa_dm_core_status_get(struct xtensa_debug_module
 	return dm->core_status.dsr;
 }
 
+int xtensa_dm_read(struct xtensa_debug_module *dm, uint32_t addr, uint32_t *val);
+int xtensa_dm_write(struct xtensa_debug_module *dm, uint32_t addr, uint32_t val);
+
 int xtensa_dm_device_id_read(struct xtensa_debug_module *dm);
 static inline xtensa_ocdid_t xtensa_dm_device_id_get(struct xtensa_debug_module *dm)
 {
diff --git a/tcl/target/at91sama5d2.cfg b/tcl/target/at91sama5d2.cfg
index 65e5217..30ddc92 100644
--- a/tcl/target/at91sama5d2.cfg
+++ b/tcl/target/at91sama5d2.cfg
@@ -1,5 +1,7 @@
 # SPDX-License-Identifier: GPL-2.0-or-later
 #
+# SAMA5D2 devices support both JTAG and SWD transports.
+#
 # The JTAG connection is disabled at reset, and during the ROM Code execution.
 # It is re-enabled when the ROM code jumps in the boot file copied from an
 # external Flash memory into the internalSRAM, or when the ROM code launches
@@ -12,14 +14,28 @@
 # - if enabled, boundary Scan mode is activated. JTAG ID Code value is 0x05B3F03F.
 # - if disabled, ICE mode is activated. Debug Port JTAG IDCODE value is 0x5BA00477
 #
+
+source [find target/swj-dp.tcl]
+
+#jtag scan chain
+if { [info exists CPUTAPID] } {
+	set _CPUTAPID $CPUTAPID
+} else {
+	if { [using_jtag] } {
+		set _CPUTAPID 0x5ba00477
+	} else {
+		# SWD IDCODE (single drop, arm)
+		set _CPUTAPID 0x5ba02477
+	}
+}
+
 if { [info exists CHIPNAME] } {
 	set  _CHIPNAME $CHIPNAME
 } else {
 	set  _CHIPNAME at91sama5d2
 }
 
-jtag newtap $_CHIPNAME cpu -irlen 4 -ircapture 0x01 -irmask 0x0f \
-	-expected-id 0x5ba00477
+swj_newdap $_CHIPNAME cpu -irlen 4 -ircapture 0x1 -irmask 0xf -expected-id $_CPUTAPID
 
 # Cortex-A5 target
 set _TARGETNAME $_CHIPNAME.cpu_a5
diff --git a/tcl/target/gd32vf103.cfg b/tcl/target/gd32vf103.cfg
index 6262697..77fdff7 100644
--- a/tcl/target/gd32vf103.cfg
+++ b/tcl/target/gd32vf103.cfg
@@ -13,6 +13,13 @@ transport select jtag
 
 reset_config srst_nogate
 
+# The smallest RAM size 6kB (GD32VF103C4/T4/R4)
+if { [info exists WORKAREASIZE] } {
+   set _WORKAREASIZE $WORKAREASIZE
+} else {
+   set _WORKAREASIZE 0x1800
+}
+
 set _CHIPNAME gd32vf103
 # The vendor's configuration expects an ID of 0x1e200a6d, but this one is what
 # I have on my board (Sipeed Longan Nano, GD32VF103CBT6).
@@ -29,7 +36,7 @@ proc default_mem_access {} {
 
 default_mem_access
 
-$_TARGETNAME configure -work-area-phys 0x20000000 -work-area-size 0x1000 -work-area-backup 1
+$_TARGETNAME configure -work-area-phys 0x20000000 -work-area-size $_WORKAREASIZE -work-area-backup 0
 
 set _FLASHNAME $_CHIPNAME.flash
 flash bank $_FLASHNAME stm32f1x 0x08000000 0 0 0 $_TARGETNAME
@@ -107,3 +114,74 @@ proc init_reset { mode } {
 		jtag arp_init-reset
 	}
 }
+
+# On this chip, ndmreset (the debug module bit that triggers a software reset)
+# doesn't work. So for JTAG connections without an SRST, we need to trigger a
+# reset manually. This is an undocumented reset sequence that's used by the
+# JTAG flashing script in the vendor-supplied GD32VF103 PlatformIO plugin:
+#
+#   https://github.com/sipeed/platform-gd32v/commit/f9cbb44819bc05dd2010cc815c32be0486800cc2
+#
+$_TARGETNAME configure -event reset-assert {
+	set dmcontrol 		0x10
+	set dmcontrol_dmactive	[expr {1 << 0}]
+	set dmcontrol_ackhavereset [expr {1 << 28}]
+	set dmcontrol_haltreq	[expr {1 << 31}]
+
+	global _RESETMODE
+
+	# If hardware NRST signal is connected and configured (reset_config srst_only)
+	# the device has been recently reset in 'jtag arp_init-reset', therefore
+	# DM_DMSTATUS_ANYHAVERESET reads 1.
+	# The following 'halt' command checks this status bit
+	# and shows 'Hart 0 unexpectedly reset!' if set.
+	# Prevent this message by sending an acknowledge first.
+	set val [expr {$dmcontrol_dmactive | $dmcontrol_ackhavereset}]
+	riscv dmi_write $dmcontrol $val
+
+	# Halt the core so that we can write to memory. We do this first so
+	# that it doesn't clobber our dmcontrol configuration.
+	halt
+
+	# Set haltreq appropriately for the type of reset we're doing. This
+	# replicates what the generic RISC-V reset_assert() function would
+	# do if we weren't overriding it. The $_RESETMODE hack sucks, but
+	# it's the least invasive way to determine whether we need to halt.
+	#
+	# If we didn't override the generic handler, we'd actually still have
+	# to do this: the default handler sets ndmreset, which prevents memory
+	# access even though it doesn't actually trigger a reset on this chip.
+	# So we'd need to unset it here, which involves a write to dmcontrol,
+	# Since haltreq is write-only and there's no way to leave it unchanged,
+	# we'd have to figure out its proper value anyway.
+	set val $dmcontrol_dmactive
+	if {$_RESETMODE ne "run"} {
+		set val [expr {$val | $dmcontrol_haltreq}]
+	}
+	riscv dmi_write $dmcontrol $val
+
+	# Unlock 0xe0042008 so that the next write triggers a reset
+	mww 0xe004200c 0x4b5a6978
+
+	# We need to trigger the reset using abstract memory access, since
+	# progbuf access tries to read a status code out of a core register
+	# after the write happens, which fails when the core is in reset.
+	riscv set_mem_access abstract
+
+	# Go!
+	mww 0xe0042008 0x1
+
+	# Put the memory access mode back to what it was.
+	default_mem_access
+}
+
+# Capture the mode of a given reset so that we can use it later in the
+# reset-assert handler.
+proc init_reset { mode } {
+	global _RESETMODE
+	set _RESETMODE $mode
+
+	if {[using_jtag]} {
+		jtag arp_init-reset
+	}
+}