Merge branch 'master' into from_upstream

Conflicts:
	tcl/target/gd32vf103.cfg

I kept our version, except I changed the flash device as happened in
mainline. Once this file settles down in mainline, we can copy it
wholesale into this fork.

Change-Id: I4c5b21fec0734b5e08eba392883e006a46386b1c

19 files changed, 553 insertions, 221 deletions

diff --git a/configure.ac b/configure.ac
index 066c14b..3245ecc 100644
--- a/configure.ac
+++ b/configure.ac
@@ -123,8 +123,10 @@ m4_define([USB1_ADAPTERS],
 	[[opendous], [eStick/opendous JTAG Programmer], [OPENDOUS]],
 	[[armjtagew], [Olimex ARM-JTAG-EW Programmer], [ARMJTAGEW]],
 	[[rlink], [Raisonance RLink JTAG Programmer], [RLINK]],
-	[[usbprog], [USBProg JTAG Programmer], [USBPROG]],
-	[[aice], [Andes JTAG Programmer], [AICE]]])
+	[[usbprog], [USBProg JTAG Programmer], [USBPROG]]])
+
+m4_define([DEPRECATED_USB1_ADAPTERS],
+	[[[aice], [Andes JTAG Programmer (deprecated)], [AICE]]])
 
 m4_define([HIDAPI_ADAPTERS],
 	[[[cmsis_dap], [CMSIS-DAP Compliant Debugger], [CMSIS_DAP_HID]],
@@ -257,6 +259,8 @@ AC_ARG_ADAPTERS([
   LIBJAYLINK_ADAPTERS
   ],[auto])
 
+AC_ARG_ADAPTERS([DEPRECATED_USB1_ADAPTERS],[no])
+
 AC_ARG_ENABLE([parport],
   AS_HELP_STRING([--enable-parport], [Enable building the pc parallel port driver]),
   [build_parport=$enableval], [build_parport=no])
@@ -661,6 +665,7 @@ m4_define([PROCESS_ADAPTERS], [
 ])
 
 PROCESS_ADAPTERS([USB1_ADAPTERS], ["x$use_libusb1" = "xyes"], [libusb-1.x])
+PROCESS_ADAPTERS([DEPRECATED_USB1_ADAPTERS], ["x$use_libusb1" = "xyes"], [libusb-1.x])
 PROCESS_ADAPTERS([HIDAPI_ADAPTERS], ["x$use_hidapi" = "xyes"], [hidapi])
 PROCESS_ADAPTERS([HIDAPI_USB1_ADAPTERS], ["x$use_hidapi" = "xyes" -a "x$use_libusb1" = "xyes"], [hidapi and libusb-1.x])
 PROCESS_ADAPTERS([LIBFTDI_ADAPTERS], ["x$use_libftdi" = "xyes"], [libftdi])
@@ -799,6 +804,7 @@ echo
 echo OpenOCD configuration summary
 echo --------------------------------------------------
 m4_foreach([adapter], [USB1_ADAPTERS,
+	DEPRECATED_USB1_ADAPTERS,
 	HIDAPI_ADAPTERS, HIDAPI_USB1_ADAPTERS, LIBFTDI_ADAPTERS,
 	LIBFTDI_USB1_ADAPTERS,
 	LIBGPIOD_ADAPTERS,
diff --git a/contrib/loaders/flash/gd32vf103/Makefile b/contrib/loaders/flash/gd32vf103/Makefile
new file mode 100644
index 0000000..2c34e08
--- /dev/null
+++ b/contrib/loaders/flash/gd32vf103/Makefile
@@ -0,0 +1,28 @@
+BIN2C = ../../../../src/helper/bin2char.sh
+
+CROSS_COMPILE ?= riscv-none-embed-
+
+CC=$(CROSS_COMPILE)gcc
+OBJCOPY=$(CROSS_COMPILE)objcopy
+OBJDUMP=$(CROSS_COMPILE)objdump
+
+CFLAGS = -march=rv32i -mabi=ilp32 -static -nostartfiles -nostdlib -Os -g -fPIC
+
+all: gd32vf103.inc
+
+.PHONY: clean
+
+%.elf: %.c
+	$(CC) $(CFLAGS) $< -o $@
+
+%.lst: %.elf
+	$(OBJDUMP) -S $< > $@
+
+%.bin: %.elf
+	$(OBJCOPY) -Obinary $< $@
+
+%.inc: %.bin
+	$(BIN2C) < $< > $@
+
+clean:
+	-rm -f *.elf *.lst *.bin *.inc
diff --git a/contrib/loaders/flash/gd32vf103/gd32vf103.c b/contrib/loaders/flash/gd32vf103/gd32vf103.c
new file mode 100644
index 0000000..69225a0
--- /dev/null
+++ b/contrib/loaders/flash/gd32vf103/gd32vf103.c
@@ -0,0 +1,33 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+
+#include <stdint.h>
+
+#define FLASH_BSY	(1 << 0)
+#define FLASH_PGERR	(1 << 2)
+#define FLASH_WRPRTERR	(1 << 4)
+
+void flash_write(volatile uint32_t *flash_sr,
+		uint32_t hwords_count,
+		uint16_t *buffer,
+		uint16_t *target_addr) __attribute__((naked));
+
+void flash_write(volatile uint32_t *flash_sr,
+		uint32_t hwords_count,
+		uint16_t *buffer,
+		uint16_t *target_addr)
+{
+	do {
+		*target_addr = *buffer++;
+
+		register uint32_t sr;
+		do {
+			sr = *flash_sr;
+		} while (sr & FLASH_BSY);
+
+		if (sr & (FLASH_PGERR | FLASH_WRPRTERR))
+			break;
+
+		target_addr++;
+	} while (--hwords_count);
+	asm("ebreak");
+}
diff --git a/contrib/loaders/flash/gd32vf103/gd32vf103.inc b/contrib/loaders/flash/gd32vf103/gd32vf103.inc
new file mode 100644
index 0000000..05eabff
--- /dev/null
+++ b/contrib/loaders/flash/gd32vf103/gd32vf103.inc
@@ -0,0 +1,4 @@
+/* Autogenerated with ../../../../src/helper/bin2char.sh */
+0x83,0x57,0x06,0x00,0x13,0x06,0x26,0x00,0x23,0x90,0xf6,0x00,0x83,0x27,0x05,0x00,
+0x13,0xf7,0x17,0x00,0xe3,0x1c,0x07,0xfe,0x93,0xf7,0x47,0x01,0x63,0x98,0x07,0x00,
+0x93,0x85,0xf5,0xff,0x93,0x86,0x26,0x00,0xe3,0x9c,0x05,0xfc,0x73,0x00,0x10,0x00,
diff --git a/doc/openocd.texi b/doc/openocd.texi
index 1870287..ac7ad88 100644
--- a/doc/openocd.texi
+++ b/doc/openocd.texi
@@ -4728,9 +4728,9 @@ specified, @xref{gdbportoverride,,option -gdb-port}.), and a fake ARM core will
 be emulated to comply to GDB remote protocol.
 @item @code{mips_m4k} -- a MIPS core.
 @item @code{mips_mips64} -- a MIPS64 core.
-@item @code{nds32_v2} -- this is an Andes NDS32 v2 core.
-@item @code{nds32_v3} -- this is an Andes NDS32 v3 core.
-@item @code{nds32_v3m} -- this is an Andes NDS32 v3m core.
+@item @code{nds32_v2} -- this is an Andes NDS32 v2 core (deprecated; would be removed in v0.13.0).
+@item @code{nds32_v3} -- this is an Andes NDS32 v3 core (deprecated; would be removed in v0.13.0).
+@item @code{nds32_v3m} -- this is an Andes NDS32 v3m core (deprecated; would be removed in v0.13.0).
 @item @code{or1k} -- this is an OpenRISC 1000 core.
 The current implementation supports three JTAG TAP cores:
 @itemize @minus
@@ -7293,6 +7293,7 @@ applied to all of them.
 All members of the STM32F0, STM32F1 and STM32F3 microcontroller families
 from STMicroelectronics and all members of the GD32F1x0, GD32F3x0 and GD32E23x microcontroller
 families from GigaDevice include internal flash and use ARM Cortex-M0/M3/M4/M23 cores.
+The driver also works with GD32VF103 powered by RISC-V core.
 The driver automatically recognizes a number of these chips using
 the chip identification register, and autoconfigures itself.
 
diff --git a/src/flash/nor/stm32f1x.c b/src/flash/nor/stm32f1x.c
index 6972bae..c750ff0 100644
--- a/src/flash/nor/stm32f1x.c
+++ b/src/flash/nor/stm32f1x.c
@@ -26,6 +26,8 @@
 #include "config.h"
 #endif
 
+#include <string.h>
+
 #include "imp.h"
 #include <helper/binarybuffer.h>
 #include <target/algorithm.h>
@@ -129,9 +131,8 @@ struct stm32x_flash_bank {
 };
 
 static int stm32x_mass_erase(struct flash_bank *bank);
-static int stm32x_get_device_id(struct flash_bank *bank, uint32_t *device_id);
 static int stm32x_write_block(struct flash_bank *bank, const uint8_t *buffer,
-		uint32_t address, uint32_t count);
+		uint32_t address, uint32_t hwords_count);
 
 /* flash bank stm32x <base> <size> 0 0 <target#>
  */
@@ -151,6 +152,9 @@ FLASH_BANK_COMMAND_HANDLER(stm32x_flash_bank_command)
 	stm32x_info->register_base = FLASH_REG_BASE_B0;
 	stm32x_info->user_bank_size = bank->size;
 
+	/* The flash write must be aligned to a halfword boundary */
+	bank->write_start_alignment = bank->write_end_alignment = 2;
+
 	return ERROR_OK;
 }
 
@@ -182,19 +186,19 @@ static int stm32x_wait_status_busy(struct flash_bank *bank, int timeout)
 			break;
 		if (timeout-- <= 0) {
 			LOG_ERROR("timed out waiting for flash");
-			return ERROR_FAIL;
+			return ERROR_FLASH_BUSY;
 		}
 		alive_sleep(1);
 	}
 
 	if (status & FLASH_WRPRTERR) {
 		LOG_ERROR("stm32x device protected");
-		retval = ERROR_FAIL;
+		retval = ERROR_FLASH_PROTECTED;
 	}
 
 	if (status & FLASH_PGERR) {
-		LOG_ERROR("stm32x device programming failed");
-		retval = ERROR_FAIL;
+		LOG_ERROR("stm32x device programming failed / flash not erased");
+		retval = ERROR_FLASH_OPERATION_FAILED;
 	}
 
 	/* Clear but report errors */
@@ -258,36 +262,39 @@ static int stm32x_erase_options(struct flash_bank *bank)
 	int retval = target_write_u32(target, STM32_FLASH_KEYR_B0, KEY1);
 	if (retval != ERROR_OK)
 		return retval;
-
 	retval = target_write_u32(target, STM32_FLASH_KEYR_B0, KEY2);
 	if (retval != ERROR_OK)
-		return retval;
+		goto flash_lock;
 
 	/* unlock option flash registers */
 	retval = target_write_u32(target, STM32_FLASH_OPTKEYR_B0, KEY1);
 	if (retval != ERROR_OK)
-		return retval;
+		goto flash_lock;
 	retval = target_write_u32(target, STM32_FLASH_OPTKEYR_B0, KEY2);
 	if (retval != ERROR_OK)
-		return retval;
+		goto flash_lock;
 
 	/* erase option bytes */
 	retval = target_write_u32(target, STM32_FLASH_CR_B0, FLASH_OPTER | FLASH_OPTWRE);
 	if (retval != ERROR_OK)
-		return retval;
+		goto flash_lock;
 	retval = target_write_u32(target, STM32_FLASH_CR_B0, FLASH_OPTER | FLASH_STRT | FLASH_OPTWRE);
 	if (retval != ERROR_OK)
-		return retval;
+		goto flash_lock;
 
 	retval = stm32x_wait_status_busy(bank, FLASH_ERASE_TIMEOUT);
 	if (retval != ERROR_OK)
-		return retval;
+		goto flash_lock;
 
 	/* clear read protection option byte
 	 * this will also force a device unlock if set */
 	stm32x_info->option_bytes.rdp = stm32x_info->default_rdp;
 
 	return ERROR_OK;
+
+flash_lock:
+	target_write_u32(target, STM32_FLASH_CR_B0, FLASH_LOCK);
+	return retval;
 }
 
 static int stm32x_write_options(struct flash_bank *bank)
@@ -303,20 +310,20 @@ static int stm32x_write_options(struct flash_bank *bank)
 		return retval;
 	retval = target_write_u32(target, STM32_FLASH_KEYR_B0, KEY2);
 	if (retval != ERROR_OK)
-		return retval;
+		goto flash_lock;
 
 	/* unlock option flash registers */
 	retval = target_write_u32(target, STM32_FLASH_OPTKEYR_B0, KEY1);
 	if (retval != ERROR_OK)
-		return retval;
+		goto flash_lock;
 	retval = target_write_u32(target, STM32_FLASH_OPTKEYR_B0, KEY2);
 	if (retval != ERROR_OK)
-		return retval;
+		goto flash_lock;
 
 	/* program option bytes */
 	retval = target_write_u32(target, STM32_FLASH_CR_B0, FLASH_OPTPG | FLASH_OPTWRE);
 	if (retval != ERROR_OK)
-		return retval;
+		goto flash_lock;
 
 	uint8_t opt_bytes[16];
 
@@ -329,18 +336,20 @@ static int stm32x_write_options(struct flash_bank *bank)
 	target_buffer_set_u16(target, opt_bytes + 12, (stm32x_info->option_bytes.protection >> 16) & 0xff);
 	target_buffer_set_u16(target, opt_bytes + 14, (stm32x_info->option_bytes.protection >> 24) & 0xff);
 
+	/* Block write is preferred in favour of operation with ancient ST-Link
+	 * firmwares without 16-bit memory access. See
+	 * 480: flash: stm32f1x: write option bytes using the loader
+	 * https://review.openocd.org/c/openocd/+/480
+	 */
 	retval = stm32x_write_block(bank, opt_bytes, STM32_OB_RDP, sizeof(opt_bytes) / 2);
-	if (retval != ERROR_OK) {
-		if (retval == ERROR_TARGET_RESOURCE_NOT_AVAILABLE)
-			LOG_ERROR("working area required to erase options bytes");
-		return retval;
-	}
-
-	retval = target_write_u32(target, STM32_FLASH_CR_B0, FLASH_LOCK);
-	if (retval != ERROR_OK)
-		return retval;
 
-	return ERROR_OK;
+flash_lock:
+	{
+		int retval2 = target_write_u32(target, STM32_FLASH_CR_B0, FLASH_LOCK);
+		if (retval == ERROR_OK)
+			retval = retval2;
+	}
+	return retval;
 }
 
 static int stm32x_protect_check(struct flash_bank *bank)
@@ -384,31 +393,33 @@ static int stm32x_erase(struct flash_bank *bank, unsigned int first,
 		return retval;
 	retval = target_write_u32(target, stm32x_get_flash_reg(bank, STM32_FLASH_KEYR), KEY2);
 	if (retval != ERROR_OK)
-		return retval;
+		goto flash_lock;
 
 	for (unsigned int i = first; i <= last; i++) {
 		retval = target_write_u32(target, stm32x_get_flash_reg(bank, STM32_FLASH_CR), FLASH_PER);
 		if (retval != ERROR_OK)
-			return retval;
+			goto flash_lock;
 		retval = target_write_u32(target, stm32x_get_flash_reg(bank, STM32_FLASH_AR),
 				bank->base + bank->sectors[i].offset);
 		if (retval != ERROR_OK)
-			return retval;
+			goto flash_lock;
 		retval = target_write_u32(target,
 				stm32x_get_flash_reg(bank, STM32_FLASH_CR), FLASH_PER | FLASH_STRT);
 		if (retval != ERROR_OK)
-			return retval;
+			goto flash_lock;
 
 		retval = stm32x_wait_status_busy(bank, FLASH_ERASE_TIMEOUT);
 		if (retval != ERROR_OK)
-			return retval;
+			goto flash_lock;
 	}
 
-	retval = target_write_u32(target, stm32x_get_flash_reg(bank, STM32_FLASH_CR), FLASH_LOCK);
-	if (retval != ERROR_OK)
-		return retval;
-
-	return ERROR_OK;
+flash_lock:
+	{
+		int retval2 = target_write_u32(target, stm32x_get_flash_reg(bank, STM32_FLASH_CR), FLASH_LOCK);
+		if (retval == ERROR_OK)
+			retval = retval2;
+	}
+	return retval;
 }
 
 static int stm32x_protect(struct flash_bank *bank, int set, unsigned int first,
@@ -442,17 +453,16 @@ static int stm32x_protect(struct flash_bank *bank, int set, unsigned int first,
 	return stm32x_write_options(bank);
 }
 
-static int stm32x_write_block(struct flash_bank *bank, const uint8_t *buffer,
-		uint32_t address, uint32_t count)
+static int stm32x_write_block_async(struct flash_bank *bank, const uint8_t *buffer,
+		uint32_t address, uint32_t hwords_count)
 {
 	struct stm32x_flash_bank *stm32x_info = bank->driver_priv;
 	struct target *target = bank->target;
-	uint32_t buffer_size = 16384;
+	uint32_t buffer_size;
 	struct working_area *write_algorithm;
 	struct working_area *source;
-	struct reg_param reg_params[5];
 	struct armv7m_algorithm armv7m_info;
-	int retval = ERROR_OK;
+	int retval;
 
 	static const uint8_t stm32x_flash_write_code[] = {
 #include "../../../contrib/loaders/flash/stm32/stm32f1x.inc"
@@ -473,19 +483,28 @@ static int stm32x_write_block(struct flash_bank *bank, const uint8_t *buffer,
 	}
 
 	/* memory buffer */
-	while (target_alloc_working_area_try(target, buffer_size, &source) != ERROR_OK) {
-		buffer_size /= 2;
-		buffer_size &= ~3UL; /* Make sure it's 4 byte aligned */
-		if (buffer_size <= 256) {
-			/* we already allocated the writing code, but failed to get a
-			 * buffer, free the algorithm */
-			target_free_working_area(target, write_algorithm);
-
-			LOG_WARNING("no large enough working area available, can't do block memory writes");
-			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
-		}
+	buffer_size = target_get_working_area_avail(target);
+	buffer_size = MIN(hwords_count * 2, MAX(buffer_size, 256));
+	/* Normally we allocate all available working area.
+	 * MIN shrinks buffer_size if the size of the written block is smaller.
+	 * MAX prevents using async algo if the available working area is smaller
+	 * than 256, the following allocation fails with
+	 * ERROR_TARGET_RESOURCE_NOT_AVAILABLE and slow flashing takes place.
+	 */
+
+	retval = target_alloc_working_area(target, buffer_size, &source);
+	/* Allocated size is always 32-bit word aligned */
+	if (retval != ERROR_OK) {
+		target_free_working_area(target, write_algorithm);
+		LOG_WARNING("no large enough working area available, can't do block memory writes");
+		/* target_alloc_working_area() may return ERROR_FAIL if area backup fails:
+		 * convert any error to ERROR_TARGET_RESOURCE_NOT_AVAILABLE
+		 */
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 	}
 
+	struct reg_param reg_params[5];
+
 	init_reg_param(&reg_params[0], "r0", 32, PARAM_IN_OUT);	/* flash base (in), status (out) */
 	init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);	/* count (halfword-16bit) */
 	init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);	/* buffer start */
@@ -493,7 +512,7 @@ static int stm32x_write_block(struct flash_bank *bank, const uint8_t *buffer,
 	init_reg_param(&reg_params[4], "r4", 32, PARAM_IN_OUT);	/* target address */
 
 	buf_set_u32(reg_params[0].value, 0, 32, stm32x_info->register_base);
-	buf_set_u32(reg_params[1].value, 0, 32, count);
+	buf_set_u32(reg_params[1].value, 0, 32, hwords_count);
 	buf_set_u32(reg_params[2].value, 0, 32, source->address);
 	buf_set_u32(reg_params[3].value, 0, 32, source->address + source->size);
 	buf_set_u32(reg_params[4].value, 0, 32, address);
@@ -501,126 +520,235 @@ static int stm32x_write_block(struct flash_bank *bank, const uint8_t *buffer,
 	armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
 	armv7m_info.core_mode = ARM_MODE_THREAD;
 
-	retval = target_run_flash_async_algorithm(target, buffer, count, 2,
+	retval = target_run_flash_async_algorithm(target, buffer, hwords_count, 2,
 			0, NULL,
-			5, reg_params,
+			ARRAY_SIZE(reg_params), reg_params,
 			source->address, source->size,
 			write_algorithm->address, 0,
 			&armv7m_info);
 
 	if (retval == ERROR_FLASH_OPERATION_FAILED) {
-		LOG_ERROR("flash write failed at address 0x%"PRIx32,
-				buf_get_u32(reg_params[4].value, 0, 32));
-
-		if (buf_get_u32(reg_params[0].value, 0, 32) & FLASH_PGERR) {
-			LOG_ERROR("flash memory not erased before writing");
-			/* Clear but report errors */
-			target_write_u32(target, stm32x_get_flash_reg(bank, STM32_FLASH_SR), FLASH_PGERR);
-		}
+		/* Actually we just need to check for programming errors
+		 * stm32x_wait_status_busy also reports error and clears status bits.
+		 *
+		 * Target algo returns flash status in r0 only if properly finished.
+		 * It is safer to re-read status register.
+		 */
+		int retval2 = stm32x_wait_status_busy(bank, 5);
+		if (retval2 != ERROR_OK)
+			retval = retval2;
 
-		if (buf_get_u32(reg_params[0].value, 0, 32) & FLASH_WRPRTERR) {
-			LOG_ERROR("flash memory write protected");
-			/* Clear but report errors */
-			target_write_u32(target, stm32x_get_flash_reg(bank, STM32_FLASH_SR), FLASH_WRPRTERR);
-		}
+		LOG_ERROR("flash write failed just before address 0x%"PRIx32,
+				buf_get_u32(reg_params[4].value, 0, 32));
 	}
 
+	for (unsigned int i = 0; i < ARRAY_SIZE(reg_params); i++)
+		destroy_reg_param(&reg_params[i]);
+
 	target_free_working_area(target, source);
 	target_free_working_area(target, write_algorithm);
 
-	destroy_reg_param(&reg_params[0]);
-	destroy_reg_param(&reg_params[1]);
-	destroy_reg_param(&reg_params[2]);
-	destroy_reg_param(&reg_params[3]);
-	destroy_reg_param(&reg_params[4]);
-
 	return retval;
 }
 
-static int stm32x_write(struct flash_bank *bank, const uint8_t *buffer,
-		uint32_t offset, uint32_t count)
+static int stm32x_write_block_riscv(struct flash_bank *bank, const uint8_t *buffer,
+		uint32_t address, uint32_t hwords_count)
 {
 	struct target *target = bank->target;
-	uint8_t *new_buffer = NULL;
+	uint32_t buffer_size;
+	struct working_area *write_algorithm;
+	struct working_area *source;
+	static const uint8_t gd32vf103_flash_write_code[] = {
+#include "../../../contrib/loaders/flash/gd32vf103/gd32vf103.inc"
+	};
 
-	if (bank->target->state != TARGET_HALTED) {
-		LOG_ERROR("Target not halted");
-		return ERROR_TARGET_NOT_HALTED;
+	/* flash write code */
+	if (target_alloc_working_area(target, sizeof(gd32vf103_flash_write_code),
+			&write_algorithm) != ERROR_OK) {
+		LOG_WARNING("no working area available, can't do block memory writes");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 	}
 
-	if (offset & 0x1) {
-		LOG_ERROR("offset 0x%" PRIx32 " breaks required 2-byte alignment", offset);
-		return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
+	int retval = target_write_buffer(target, write_algorithm->address,
+			sizeof(gd32vf103_flash_write_code), gd32vf103_flash_write_code);
+	if (retval != ERROR_OK) {
+		target_free_working_area(target, write_algorithm);
+		return retval;
 	}
 
-	/* If there's an odd number of bytes, the data has to be padded. Duplicate
-	 * the buffer and use the normal code path with a single block write since
-	 * it's probably cheaper than to special case the last odd write using
-	 * discrete accesses. */
-	if (count & 1) {
-		new_buffer = malloc(count + 1);
-		if (!new_buffer) {
-			LOG_ERROR("odd number of bytes to write and no memory for padding buffer");
-			return ERROR_FAIL;
+	/* memory buffer */
+	buffer_size = target_get_working_area_avail(target);
+	buffer_size = MIN(hwords_count * 2, MAX(buffer_size, 256));
+
+	retval = target_alloc_working_area(target, buffer_size, &source);
+	/* Allocated size is always word aligned */
+	if (retval != ERROR_OK) {
+		target_free_working_area(target, write_algorithm);
+		LOG_WARNING("no large enough working area available, can't do block memory writes");
+		/* target_alloc_working_area() may return ERROR_FAIL if area backup fails:
+		 * convert any error to ERROR_TARGET_RESOURCE_NOT_AVAILABLE
+		 */
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+
+	struct reg_param reg_params[4];
+
+	init_reg_param(&reg_params[0], "a0", 32, PARAM_OUT);	/* poiner to FLASH_SR */
+	init_reg_param(&reg_params[1], "a1", 32, PARAM_OUT);	/* count (halfword-16bit) */
+	init_reg_param(&reg_params[2], "a2", 32, PARAM_OUT);	/* buffer start */
+	init_reg_param(&reg_params[3], "a3", 32, PARAM_IN_OUT);	/* target address */
+
+	while (hwords_count > 0) {
+		uint32_t thisrun_hwords = source->size / 2;
+
+		/* Limit to the amount of data we actually want to write */
+		if (thisrun_hwords > hwords_count)
+			thisrun_hwords = hwords_count;
+
+		/* Write data to buffer */
+		retval = target_write_buffer(target, source->address,
+					thisrun_hwords * 2, buffer);
+		if (retval != ERROR_OK)
+			break;
+
+		buf_set_u32(reg_params[0].value, 0, 32, stm32x_get_flash_reg(bank, STM32_FLASH_SR));
+		buf_set_u32(reg_params[1].value, 0, 32, thisrun_hwords);
+		buf_set_u32(reg_params[2].value, 0, 32, source->address);
+		buf_set_u32(reg_params[3].value, 0, 32, address);
+
+		retval = target_run_algorithm(target,
+				0, NULL,
+				ARRAY_SIZE(reg_params), reg_params,
+				write_algorithm->address,
+				write_algorithm->address + sizeof(gd32vf103_flash_write_code) - 4,
+				10000, NULL);
+
+		if (retval != ERROR_OK) {
+			LOG_ERROR("Failed to execute algorithm at 0x%" TARGET_PRIxADDR ": %d",
+					write_algorithm->address, retval);
+			break;
+		}
+
+		/* Actually we just need to check for programming errors
+		 * stm32x_wait_status_busy also reports error and clears status bits
+		 */
+		retval = stm32x_wait_status_busy(bank, 5);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("flash write failed at address 0x%"PRIx32,
+					buf_get_u32(reg_params[3].value, 0, 32));
+			break;
 		}
-		LOG_INFO("odd number of bytes to write, padding with 0xff");
-		buffer = memcpy(new_buffer, buffer, count);
-		new_buffer[count++] = 0xff;
+
+		/* Update counters */
+		buffer += thisrun_hwords * 2;
+		address += thisrun_hwords * 2;
+		hwords_count -= thisrun_hwords;
 	}
 
-	uint32_t words_remaining = count / 2;
-	int retval, retval2;
+	for (unsigned int i = 0; i < ARRAY_SIZE(reg_params); i++)
+		destroy_reg_param(&reg_params[i]);
 
-	/* unlock flash registers */
-	retval = target_write_u32(target, stm32x_get_flash_reg(bank, STM32_FLASH_KEYR), KEY1);
-	if (retval != ERROR_OK)
-		goto cleanup;
-	retval = target_write_u32(target, stm32x_get_flash_reg(bank, STM32_FLASH_KEYR), KEY2);
-	if (retval != ERROR_OK)
-		goto cleanup;
+	target_free_working_area(target, source);
+	target_free_working_area(target, write_algorithm);
 
-	retval = target_write_u32(target, stm32x_get_flash_reg(bank, STM32_FLASH_CR), FLASH_PG);
-	if (retval != ERROR_OK)
-		goto cleanup;
+	return retval;
+}
 
-	/* try using a block write */
-	retval = stm32x_write_block(bank, buffer, bank->base + offset, words_remaining);
+/** Writes a block to flash either using target algorithm
+ *  or use fallback, host controlled halfword-by-halfword access.
+ *  Flash controller must be unlocked before this call.
+ */
+static int stm32x_write_block(struct flash_bank *bank,
+		const uint8_t *buffer, uint32_t address, uint32_t hwords_count)
+{
+	struct target *target = bank->target;
+
+	/* The flash write must be aligned to a halfword boundary.
+	 * The flash infrastructure ensures it, do just a security check
+	 */
+	assert(address % 2 == 0);
+
+	int retval;
+	struct arm *arm = target_to_arm(target);
+	if (is_arm(arm)) {
+		/* try using a block write - on ARM architecture or... */
+		retval = stm32x_write_block_async(bank, buffer, address, hwords_count);
+	} else {
+		/* ... RISC-V architecture */
+		retval = stm32x_write_block_riscv(bank, buffer, address, hwords_count);
+	}
 
 	if (retval == ERROR_TARGET_RESOURCE_NOT_AVAILABLE) {
 		/* if block write failed (no sufficient working area),
 		 * we use normal (slow) single halfword accesses */
 		LOG_WARNING("couldn't use block writes, falling back to single memory accesses");
 
-		while (words_remaining > 0) {
-			retval = target_write_memory(target, bank->base + offset, 2, 1, buffer);
+		while (hwords_count > 0) {
+			retval = target_write_memory(target, address, 2, 1, buffer);
 			if (retval != ERROR_OK)
-				goto reset_pg_and_lock;
+				return retval;
 
 			retval = stm32x_wait_status_busy(bank, 5);
 			if (retval != ERROR_OK)
-				goto reset_pg_and_lock;
+				return retval;
 
-			words_remaining--;
+			hwords_count--;
 			buffer += 2;
-			offset += 2;
+			address += 2;
 		}
 	}
+	return retval;
+}
+
+static int stm32x_write(struct flash_bank *bank, const uint8_t *buffer,
+		uint32_t offset, uint32_t count)
+{
+	struct target *target = bank->target;
+
+	if (bank->target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* The flash write must be aligned to a halfword boundary.
+	 * The flash infrastructure ensures it, do just a security check
+	 */
+	assert(offset % 2 == 0);
+	assert(count % 2 == 0);
+
+	int retval, retval2;
+
+	/* unlock flash registers */
+	retval = target_write_u32(target, stm32x_get_flash_reg(bank, STM32_FLASH_KEYR), KEY1);
+	if (retval != ERROR_OK)
+		return retval;
+	retval = target_write_u32(target, stm32x_get_flash_reg(bank, STM32_FLASH_KEYR), KEY2);
+	if (retval != ERROR_OK)
+		goto reset_pg_and_lock;
+
+	/* enable flash programming */
+	retval = target_write_u32(target, stm32x_get_flash_reg(bank, STM32_FLASH_CR), FLASH_PG);
+	if (retval != ERROR_OK)
+		goto reset_pg_and_lock;
+
+	/* write to flash */
+	retval = stm32x_write_block(bank, buffer, bank->base + offset, count / 2);
 
 reset_pg_and_lock:
 	retval2 = target_write_u32(target, stm32x_get_flash_reg(bank, STM32_FLASH_CR), FLASH_LOCK);
 	if (retval == ERROR_OK)
 		retval = retval2;
 
-cleanup:
-	free(new_buffer);
 	return retval;
 }
 
-static int stm32x_get_device_id(struct flash_bank *bank, uint32_t *device_id)
-{
-	struct target *target = bank->target;
-	uint32_t device_id_register = 0;
+struct stm32x_property_addr {
+	uint32_t device_id;
+	uint32_t flash_size;
+};
 
+static int stm32x_get_property_addr(struct target *target, struct stm32x_property_addr *addr)
+{
 	if (!target_was_examined(target)) {
 		LOG_ERROR("Target not examined yet");
 		return ERROR_TARGET_NOT_EXAMINED;
@@ -628,63 +756,61 @@ static int stm32x_get_device_id(struct flash_bank *bank, uint32_t *device_id)
 
 	switch (cortex_m_get_partno_safe(target)) {
 	case CORTEX_M0_PARTNO: /* STM32F0x devices */
-		device_id_register = 0x40015800;
-		break;
+		addr->device_id = 0x40015800;
+		addr->flash_size = 0x1FFFF7CC;
+		return ERROR_OK;
 	case CORTEX_M3_PARTNO: /* STM32F1x devices */
-		device_id_register = 0xE0042000;
-		break;
+		addr->device_id = 0xE0042000;
+		addr->flash_size = 0x1FFFF7E0;
+		return ERROR_OK;
 	case CORTEX_M4_PARTNO: /* STM32F3x devices */
-		device_id_register = 0xE0042000;
-		break;
+		addr->device_id = 0xE0042000;
+		addr->flash_size = 0x1FFFF7CC;
+		return ERROR_OK;
 	case CORTEX_M23_PARTNO: /* GD32E23x devices */
-		device_id_register = 0x40015800;
-		break;
+		addr->device_id = 0x40015800;
+		addr->flash_size = 0x1FFFF7E0;
+		return ERROR_OK;
+	case CORTEX_M_PARTNO_INVALID:
+		/* Check for GD32VF103 with RISC-V CPU */
+		if (strcmp(target_type_name(target), "riscv") == 0
+				&& target_address_bits(target) == 32) {
+			/* There is nothing like arm common_magic in riscv_info_t
+			 * check text name of target and if target is 32-bit
+			 */
+			addr->device_id = 0xE0042000;
+			addr->flash_size = 0x1FFFF7E0;
+			return ERROR_OK;
+		}
+		/* fallthrough */
 	default:
 		LOG_ERROR("Cannot identify target as a stm32x");
 		return ERROR_FAIL;
 	}
+}
 
-	/* read stm32 device id register */
-	int retval = target_read_u32(target, device_id_register, device_id);
+static int stm32x_get_device_id(struct flash_bank *bank, uint32_t *device_id)
+{
+	struct target *target = bank->target;
+	struct stm32x_property_addr addr;
+
+	int retval = stm32x_get_property_addr(target, &addr);
 	if (retval != ERROR_OK)
 		return retval;
 
-	return retval;
+	return target_read_u32(target, addr.device_id, device_id);
 }
 
 static int stm32x_get_flash_size(struct flash_bank *bank, uint16_t *flash_size_in_kb)
 {
 	struct target *target = bank->target;
-	uint32_t flash_size_reg;
+	struct stm32x_property_addr addr;
 
-	if (!target_was_examined(target)) {
-		LOG_ERROR("Target not examined yet");
-		return ERROR_TARGET_NOT_EXAMINED;
-	}
-
-	switch (cortex_m_get_partno_safe(target)) {
-	case CORTEX_M0_PARTNO: /* STM32F0x devices */
-		flash_size_reg = 0x1FFFF7CC;
-		break;
-	case CORTEX_M3_PARTNO: /* STM32F1x devices */
-		flash_size_reg = 0x1FFFF7E0;
-		break;
-	case CORTEX_M4_PARTNO: /* STM32F3x devices */
-		flash_size_reg = 0x1FFFF7CC;
-		break;
-	case CORTEX_M23_PARTNO: /* GD32E23x devices */
-		flash_size_reg = 0x1FFFF7E0;
-		break;
-	default:
-		LOG_ERROR("Cannot identify target as a stm32x");
-		return ERROR_FAIL;
-	}
-
-	int retval = target_read_u16(target, flash_size_reg, flash_size_in_kb);
+	int retval = stm32x_get_property_addr(target, &addr);
 	if (retval != ERROR_OK)
 		return retval;
 
-	return retval;
+	return target_read_u16(target, addr.flash_size, flash_size_in_kb);
 }
 
 static int stm32x_probe(struct flash_bank *bank)
@@ -770,15 +896,20 @@ static int stm32x_probe(struct flash_bank *bank)
 			stm32x_info->user_data_offset = 16;
 			stm32x_info->option_offset = 6;
 			max_flash_size_in_kb = 64;
+			stm32x_info->can_load_options = true;
 			break;
 		case 0x1704: /* gd32f3x0 */
 			stm32x_info->user_data_offset = 16;
 			stm32x_info->option_offset = 6;
+			stm32x_info->can_load_options = true;
+			break;
+		case 0x1906: /* gd32vf103 */
 			break;
 		case 0x1909: /* gd32e23x */
 			stm32x_info->user_data_offset = 16;
 			stm32x_info->option_offset = 6;
 			max_flash_size_in_kb = 64;
+			stm32x_info->can_load_options = true;
 			break;
 		}
 		break;
@@ -990,6 +1121,10 @@ static int get_stm32x_info(struct flash_bank *bank, struct command_invocation *c
 			device_str = "GD32F3x0";
 			break;
 
+		case 0x1906:
+			device_str = "GD32VF103";
+			break;
+
 		case 0x1909: /* gd32e23x */
 			device_str = "GD32E23x";
 			break;
@@ -1473,8 +1608,10 @@ COMMAND_HANDLER(stm32x_handle_options_load_command)
 	if (retval != ERROR_OK)
 		return retval;
 	retval = target_write_u32(target, stm32x_get_flash_reg(bank, STM32_FLASH_KEYR), KEY2);
-	if (retval != ERROR_OK)
+	if (retval != ERROR_OK) {
+		(void)target_write_u32(target, stm32x_get_flash_reg(bank, STM32_FLASH_CR), FLASH_LOCK);
 		return retval;
+	}
 
 	/* force re-load of option bytes - generates software reset */
 	retval = target_write_u32(target, stm32x_get_flash_reg(bank, STM32_FLASH_CR), FLASH_OBL_LAUNCH);
@@ -1499,26 +1636,26 @@ static int stm32x_mass_erase(struct flash_bank *bank)
 		return retval;
 	retval = target_write_u32(target, stm32x_get_flash_reg(bank, STM32_FLASH_KEYR), KEY2);
 	if (retval != ERROR_OK)
-		return retval;
+		goto flash_lock;
 
 	/* mass erase flash memory */
 	retval = target_write_u32(target, stm32x_get_flash_reg(bank, STM32_FLASH_CR), FLASH_MER);
 	if (retval != ERROR_OK)
-		return retval;
+		goto flash_lock;
 	retval = target_write_u32(target, stm32x_get_flash_reg(bank, STM32_FLASH_CR),
 			FLASH_MER | FLASH_STRT);
 	if (retval != ERROR_OK)
-		return retval;
+		goto flash_lock;
 
 	retval = stm32x_wait_status_busy(bank, FLASH_ERASE_TIMEOUT);
-	if (retval != ERROR_OK)
-		return retval;
-
-	retval = target_write_u32(target, stm32x_get_flash_reg(bank, STM32_FLASH_CR), FLASH_LOCK);
-	if (retval != ERROR_OK)
-		return retval;
 
-	return ERROR_OK;
+flash_lock:
+	{
+		int retval2 = target_write_u32(target, stm32x_get_flash_reg(bank, STM32_FLASH_CR), FLASH_LOCK);
+		if (retval == ERROR_OK)
+			retval = retval2;
+	}
+	return retval;
 }
 
 COMMAND_HANDLER(stm32x_handle_mass_erase_command)
diff --git a/src/jtag/drivers/bcm2835gpio.c b/src/jtag/drivers/bcm2835gpio.c
index fd6c28b..b7a4d99 100644
--- a/src/jtag/drivers/bcm2835gpio.c
+++ b/src/jtag/drivers/bcm2835gpio.c
@@ -565,6 +565,13 @@ static int bcm2835gpio_init(void)
 	}
 
 	if (transport_is_swd()) {
+		/* Make buffer an output before the GPIO connected to it */
+		if (swdio_dir_gpio != -1) {
+			swdio_dir_gpio_mode = MODE_GPIO(swdio_dir_gpio);
+			GPIO_SET = 1 << swdio_dir_gpio;
+			OUT_GPIO(swdio_dir_gpio);
+		}
+
 		swclk_gpio_mode = MODE_GPIO(swclk_gpio);
 		swdio_gpio_mode = MODE_GPIO(swdio_gpio);
 
@@ -580,12 +587,6 @@ static int bcm2835gpio_init(void)
 		OUT_GPIO(srst_gpio);
 	}
 
-	if (swdio_dir_gpio != -1) {
-		swdio_dir_gpio_mode = MODE_GPIO(swdio_dir_gpio);
-		GPIO_SET = 1 << swdio_dir_gpio;
-		OUT_GPIO(swdio_dir_gpio);
-	}
-
 	LOG_DEBUG("saved pinmux settings: tck %d tms %d tdi %d "
 		  "tdo %d trst %d srst %d", tck_gpio_mode, tms_gpio_mode,
 		  tdi_gpio_mode, tdo_gpio_mode, trst_gpio_mode, srst_gpio_mode);
diff --git a/src/jtag/drivers/cmsis_dap.c b/src/jtag/drivers/cmsis_dap.c
index 63407be..eaa65ab 100644
--- a/src/jtag/drivers/cmsis_dap.c
+++ b/src/jtag/drivers/cmsis_dap.c
@@ -1405,18 +1405,18 @@ static void debug_parse_cmsis_buf(const uint8_t *cmd, int cmdlen)
 	for (int i = 0; i < cmdlen; ++i)
 		printf(" %02x", cmd[i]);
 	printf("\n");
-	switch (cmd[1]) {
+	switch (cmd[0]) {
 		case CMD_DAP_JTAG_SEQ: {
-			printf("cmsis-dap jtag sequence command %02x (n=%d)\n", cmd[1], cmd[2]);
+			printf("cmsis-dap jtag sequence command %02x (n=%d)\n", cmd[0], cmd[1]);
 			/*
-			 * #2 = number of sequences
-			 * #3 = sequence info 1
-			 * #4...4+n_bytes-1 = sequence 1
+			 * #1 = number of sequences
+			 * #2 = sequence info 1
+			 * #3...4+n_bytes-1 = sequence 1
 			 * #4+n_bytes = sequence info 2
 			 * #5+n_bytes = sequence 2 (single bit)
 			 */
-			int pos = 3;
-			for (int seq = 0; seq < cmd[2]; ++seq) {
+			int pos = 2;
+			for (int seq = 0; seq < cmd[1]; ++seq) {
 				uint8_t info = cmd[pos++];
 				int len = info & DAP_JTAG_SEQ_TCK;
 				if (len == 0)
diff --git a/src/jtag/drivers/linuxgpiod.c b/src/jtag/drivers/linuxgpiod.c
index 9f9f27a..288035f 100644
--- a/src/jtag/drivers/linuxgpiod.c
+++ b/src/jtag/drivers/linuxgpiod.c
@@ -27,6 +27,7 @@ static int trst_gpio = -1;
 static int srst_gpio = -1;
 static int swclk_gpio = -1;
 static int swdio_gpio = -1;
+static int swdio_dir_gpio = -1;
 static int led_gpio = -1;
 static int gpiochip = -1;
 static int tck_gpiochip = -1;
@@ -37,6 +38,7 @@ static int trst_gpiochip = -1;
 static int srst_gpiochip = -1;
 static int swclk_gpiochip = -1;
 static int swdio_gpiochip = -1;
+static int swdio_dir_gpiochip = -1;
 static int led_gpiochip = -1;
 
 static struct gpiod_chip *gpiod_chip_tck;
@@ -47,6 +49,7 @@ static struct gpiod_chip *gpiod_chip_trst;
 static struct gpiod_chip *gpiod_chip_srst;
 static struct gpiod_chip *gpiod_chip_swclk;
 static struct gpiod_chip *gpiod_chip_swdio;
+static struct gpiod_chip *gpiod_chip_swdio_dir;
 static struct gpiod_chip *gpiod_chip_led;
 
 static struct gpiod_line *gpiod_tck;
@@ -56,6 +59,7 @@ static struct gpiod_line *gpiod_tdo;
 static struct gpiod_line *gpiod_trst;
 static struct gpiod_line *gpiod_swclk;
 static struct gpiod_line *gpiod_swdio;
+static struct gpiod_line *gpiod_swdio_dir;
 static struct gpiod_line *gpiod_srst;
 static struct gpiod_line *gpiod_led;
 
@@ -63,6 +67,7 @@ static int last_swclk;
 static int last_swdio;
 static bool last_stored;
 static bool swdio_input;
+static bool swdio_dir_is_active_high = true;
 
 /* Bitbang interface read of TDO */
 static bb_value_t linuxgpiod_read(void)
@@ -152,6 +157,11 @@ static void linuxgpiod_swdio_drive(bool is_output)
 	gpiod_line_release(gpiod_swdio);
 
 	if (is_output) {
+		if (gpiod_swdio_dir) {
+			retval = gpiod_line_set_value(gpiod_swdio_dir, swdio_dir_is_active_high ? 1 : 0);
+			if (retval < 0)
+				LOG_WARNING("Fail set swdio_dir");
+		}
 		retval = gpiod_line_request_output(gpiod_swdio, "OpenOCD", 1);
 		if (retval < 0)
 			LOG_WARNING("Fail request_output line swdio");
@@ -159,6 +169,11 @@ static void linuxgpiod_swdio_drive(bool is_output)
 		retval = gpiod_line_request_input(gpiod_swdio, "OpenOCD");
 		if (retval < 0)
 			LOG_WARNING("Fail request_input line swdio");
+		if (gpiod_swdio_dir) {
+			retval = gpiod_line_set_value(gpiod_swdio_dir, swdio_dir_is_active_high ? 0 : 1);
+			if (retval < 0)
+				LOG_WARNING("Fail set swdio_dir");
+		}
 	}
 
 	last_stored = false;
@@ -297,6 +312,8 @@ static int linuxgpiod_quit(void)
 		gpiod_chip_close(gpiod_chip_srst);
 	if (gpiod_chip_swdio != NULL)
 		gpiod_chip_close(gpiod_chip_swdio);
+	if (gpiod_chip_swdio_dir != NULL)
+		gpiod_chip_close(gpiod_chip_swdio_dir);
 	if (gpiod_chip_swclk != NULL)
 		gpiod_chip_close(gpiod_chip_swclk);
 	if (gpiod_chip_trst != NULL)
@@ -451,10 +468,26 @@ static int linuxgpiod_init(void)
 			goto out_error;
 		}
 
+		if (is_gpio_valid(swdio_dir_gpio)) {
+			gpiod_chip_swdio_dir = gpiod_chip_open_by_number(swdio_dir_gpiochip);
+			if (!gpiod_chip_swdio_dir) {
+				LOG_ERROR("Cannot open LinuxGPIOD swdio_dir_gpiochip %d", swdio_dir_gpiochip);
+				goto out_error;
+			}
+		}
+
 		gpiod_swclk = helper_get_output_line("swclk", gpiod_chip_swclk, swclk_gpio, 1);
 		if (!gpiod_swclk)
 			goto out_error;
 
+		/* Set buffer direction before making SWDIO an output */
+		if (is_gpio_valid(swdio_dir_gpio)) {
+			gpiod_swdio_dir = helper_get_output_line("swdio_dir", gpiod_chip_swdio_dir, swdio_dir_gpio,
+					swdio_dir_is_active_high ? 1 : 0);
+			if (!gpiod_swdio_dir)
+				goto out_error;
+		}
+
 		gpiod_swdio = helper_get_output_line("swdio", gpiod_chip_swdio, swdio_gpio, 1);
 		if (!gpiod_swdio)
 			goto out_error;
@@ -593,6 +626,12 @@ COMMAND_HANDLER(linuxgpiod_handle_swd_gpionum_swdio)
 			&swdio_gpio);
 }
 
+COMMAND_HANDLER(linuxgpiod_handle_swd_gpionum_swdio_dir)
+{
+	return CALL_COMMAND_HANDLER(linuxgpiod_helper_gpionum, "swdio_dir", &swdio_dir_gpiochip,
+			&swdio_dir_gpio);
+}
+
 COMMAND_HANDLER(linuxgpiod_handle_gpionum_led)
 {
 	return CALL_COMMAND_HANDLER(linuxgpiod_helper_gpionum, "led", &led_gpiochip,
@@ -611,6 +650,7 @@ COMMAND_HANDLER(linuxgpiod_handle_gpiochip)
 		srst_gpiochip = gpiochip;
 		swclk_gpiochip = gpiochip;
 		swdio_gpiochip = gpiochip;
+		swdio_dir_gpiochip = gpiochip;
 		led_gpiochip = gpiochip;
 	}
 
@@ -690,6 +730,13 @@ static const struct command_registration linuxgpiod_subcommand_handlers[] = {
 		.usage = "[chip] swdio",
 	},
 	{
+		.name = "swdio_dir_num",
+		.handler = linuxgpiod_handle_swd_gpionum_swdio_dir,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio chip number (optional) and gpio number for swdio_dir.",
+		.usage = "[chip] swdio_dir",
+	},
+	{
 		.name = "led_num",
 		.handler = linuxgpiod_handle_gpionum_led,
 		.mode = COMMAND_CONFIG,
diff --git a/src/rtos/zephyr.c b/src/rtos/zephyr.c
index 6305116..7f3325f 100644
--- a/src/rtos/zephyr.c
+++ b/src/rtos/zephyr.c
@@ -375,15 +375,15 @@ static const struct symbol_table_elem zephyr_symbol_list[] = {
 		.optional = false
 	},
 	{
-		.symbol_name = "_kernel_openocd_offsets",
+		.symbol_name = "_kernel_thread_info_offsets",
 		.optional = false
 	},
 	{
-		.symbol_name = "_kernel_openocd_size_t_size",
+		.symbol_name = "_kernel_thread_info_size_t_size",
 		.optional = false
 	},
 	{
-		.symbol_name = "_kernel_openocd_num_offsets",
+		.symbol_name = "_kernel_thread_info_num_offsets",
 		.optional = true
 	},
 	{
diff --git a/src/server/gdb_server.c b/src/server/gdb_server.c
index 07dfaec..da65527 100644
--- a/src/server/gdb_server.c
+++ b/src/server/gdb_server.c
@@ -2768,11 +2768,52 @@ static int gdb_query_packet(struct connection *connection,
 			/* some commands need to know the GDB connection, make note of current
 			 * GDB connection. */
 			current_gdb_connection = gdb_connection;
-			command_run_line(cmd_ctx, cmd);
+			struct target *saved_target_override = cmd_ctx->current_target_override;
+			cmd_ctx->current_target_override = target;
+
+			int retval = Jim_EvalObj(cmd_ctx->interp, Jim_NewStringObj(cmd_ctx->interp, cmd, -1));
+
+			cmd_ctx->current_target_override = saved_target_override;
 			current_gdb_connection = NULL;
 			target_call_timer_callbacks_now();
 			gdb_connection->output_flag = GDB_OUTPUT_NO;
 			free(cmd);
+			if (retval == JIM_RETURN)
+				retval = cmd_ctx->interp->returnCode;
+			int lenmsg;
+			const char *cretmsg = Jim_GetString(Jim_GetResult(cmd_ctx->interp), &lenmsg);
+			char *retmsg;
+			if (lenmsg && cretmsg[lenmsg - 1] != '\n') {
+				retmsg = alloc_printf("%s\n", cretmsg);
+				lenmsg++;
+			} else {
+				retmsg = strdup(cretmsg);
+			}
+			if (!retmsg)
+				return ERROR_GDB_BUFFER_TOO_SMALL;
+
+			if (retval == JIM_OK) {
+				if (lenmsg) {
+					char *hex_buffer = malloc(lenmsg * 2 + 1);
+					if (!hex_buffer) {
+						free(retmsg);
+						return ERROR_GDB_BUFFER_TOO_SMALL;
+					}
+
+					size_t pkt_len = hexify(hex_buffer, (const uint8_t *)retmsg, lenmsg,
+											lenmsg * 2 + 1);
+					gdb_put_packet(connection, hex_buffer, pkt_len);
+					free(hex_buffer);
+				} else {
+					gdb_put_packet(connection, "OK", 2);
+				}
+			} else {
+				if (lenmsg)
+					gdb_output_con(connection, retmsg);
+				gdb_send_error(connection, retval);
+			}
+			free(retmsg);
+			return ERROR_OK;
 		}
 		gdb_put_packet(connection, "OK", 2);
 		return ERROR_OK;
diff --git a/src/target/a64_disassembler.c b/src/target/a64_disassembler.c
index bd78129..58ddf60 100644
--- a/src/target/a64_disassembler.c
+++ b/src/target/a64_disassembler.c
@@ -26,7 +26,7 @@
 
 #if HAVE_CAPSTONE
 
-#include <capstone/capstone.h>
+#include <capstone.h>
 
 static void print_opcode(struct command_invocation *cmd, const cs_insn *insn)
 {
diff --git a/src/target/arm_cti.c b/src/target/arm_cti.c
index c776e9c..96927bf 100644
--- a/src/target/arm_cti.c
+++ b/src/target/arm_cti.c
@@ -435,8 +435,13 @@ static int cti_configure(struct jim_getopt_info *goi, struct arm_cti *cti)
 	/* parse config or cget options ... */
 	while (goi->argc > 0) {
 		int e = adiv5_jim_mem_ap_spot_configure(&cti->spot, goi);
+
+		if (e == JIM_CONTINUE)
+			Jim_SetResultFormatted(goi->interp, "unknown option '%s'",
+				Jim_String(goi->argv[0]));
+
 		if (e != JIM_OK)
-			return e;
+			return JIM_ERR;
 	}
 
 	if (!cti->spot.dap) {
diff --git a/src/target/arm_disassembler.c b/src/target/arm_disassembler.c
index 6618593..d3d27a9 100644
--- a/src/target/arm_disassembler.c
+++ b/src/target/arm_disassembler.c
@@ -27,7 +27,7 @@
 #include <helper/log.h>
 
 #if HAVE_CAPSTONE
-#include <capstone/capstone.h>
+#include <capstone.h>
 #endif
 
 /*
diff --git a/src/target/image.c b/src/target/image.c
index eafa73e..130ea6c 100644
--- a/src/target/image.c
+++ b/src/target/image.c
@@ -596,7 +596,7 @@ static int image_elf64_read_headers(struct image *image)
 				image->sections[j].base_address = field64(elf,
 						elf->segments64[i].p_paddr);
 			image->sections[j].private = &elf->segments64[i];
-			image->sections[j].flags = field32(elf, elf->segments64[i].p_flags);
+			image->sections[j].flags = field64(elf, elf->segments64[i].p_flags);
 			j++;
 		}
 	}
@@ -1168,7 +1168,7 @@ int image_read_section(struct image *image,
 	return ERROR_OK;
 }
 
-int image_add_section(struct image *image, target_addr_t base, uint32_t size, int flags, uint8_t const *data)
+int image_add_section(struct image *image, target_addr_t base, uint32_t size, uint64_t flags, uint8_t const *data)
 {
 	struct imagesection *section;
 
diff --git a/src/target/image.h b/src/target/image.h
index 5b5d11f..bf06064 100644
--- a/src/target/image.h
+++ b/src/target/image.h
@@ -52,7 +52,7 @@ enum image_type {
 struct imagesection {
 	target_addr_t base_address;
 	uint32_t size;
-	int flags;
+	uint64_t flags;
 	void *private;		/* private data */
 };
 
@@ -108,7 +108,7 @@ int image_read_section(struct image *image, int section, target_addr_t offset,
 void image_close(struct image *image);
 
 int image_add_section(struct image *image, target_addr_t base, uint32_t size,
-		int flags, uint8_t const *data);
+		uint64_t flags, uint8_t const *data);
 
 int image_calculate_checksum(const uint8_t *buffer, uint32_t nbytes,
 		uint32_t *checksum);
diff --git a/tcl/target/gd32vf103.cfg b/tcl/target/gd32vf103.cfg
index c7af88d..b00e5e9 100644
--- a/tcl/target/gd32vf103.cfg
+++ b/tcl/target/gd32vf103.cfg
@@ -22,7 +22,7 @@ default_mem_access
 $_TARGETNAME configure -work-area-phys 0x20000000 -work-area-size 0x1000 -work-area-backup 1
 
 set _FLASHNAME $_CHIPNAME.flash
-flash bank $_FLASHNAME gd32vf103 0x08000000 0 0 0 $_TARGETNAME
+flash bank $_FLASHNAME stm32f1x 0x08000000 0 0 0 $_TARGETNAME
 
 # Address 0 is only aliased to main flash when the chip is not running its
 # built-in bootloader. When it is, it's instead aliased to a read only section
diff --git a/tcl/target/stm32f4x.cfg b/tcl/target/stm32f4x.cfg
index 2228de7..aa2816e 100644
--- a/tcl/target/stm32f4x.cfg
+++ b/tcl/target/stm32f4x.cfg
@@ -1,7 +1,7 @@
 # script for stm32f4x family
 
 #
-# stm32 devices support both JTAG and SWD transports.
+# stm32f4 devices support both JTAG and SWD transports.
 #
 source [find target/swj-dp.tcl]
 source [find mem_helper.tcl]
diff --git a/tcl/target/stm32l4x.cfg b/tcl/target/stm32l4x.cfg
index 5899791..9bd7e37 100644
--- a/tcl/target/stm32l4x.cfg
+++ b/tcl/target/stm32l4x.cfg
@@ -15,11 +15,11 @@ if { [info exists CHIPNAME] } {
 set _ENDIAN little
 
 # Work-area is a space in RAM used for flash programming
-# Smallest current target has 64kB ram, use 32kB by default to avoid surprises
+# By default use 40kB (Available RAM in smallest device STM32L412)
 if { [info exists WORKAREASIZE] } {
    set _WORKAREASIZE $WORKAREASIZE
 } else {
-   set _WORKAREASIZE 0x8000
+   set _WORKAREASIZE 0xa000
 }
 
 #jtag scan chain
@@ -38,6 +38,8 @@ if { [info exists CPUTAPID] } {
 swj_newdap $_CHIPNAME cpu -irlen 4 -ircapture 0x1 -irmask 0xf -expected-id $_CPUTAPID
 dap create $_CHIPNAME.dap -chain-position $_CHIPNAME.cpu
 
+tpiu create $_CHIPNAME.tpiu -dap $_CHIPNAME.dap -ap-num 0 -baseaddr 0xE0040000
+
 if {[using_jtag]} {
    jtag newtap $_CHIPNAME bs -irlen 5
 }
@@ -47,8 +49,9 @@ target create $_TARGETNAME cortex_m -endian $_ENDIAN -dap $_CHIPNAME.dap
 
 $_TARGETNAME configure -work-area-phys 0x20000000 -work-area-size $_WORKAREASIZE -work-area-backup 0
 
-flash bank $_CHIPNAME.flash stm32l4x 0x08000000 0 0 0 $_TARGETNAME
-flash bank $_CHIPNAME.otp   stm32l4x 0x1fff7000 0 0 0 $_TARGETNAME
+set _FLASHNAME $_CHIPNAME.flash
+flash bank $_FLASHNAME stm32l4x 0x08000000 0 0 0 $_TARGETNAME
+flash bank $_CHIPNAME.otp stm32l4x 0x1fff7000 0 0 0 $_TARGETNAME
 
 if { [info exists QUADSPI] && $QUADSPI } {
    set a [llength [flash list]]
@@ -88,12 +91,54 @@ if {![using_hla]} {
    cortex_m reset_config sysresetreq
 }
 
+$_TARGETNAME configure -event examine-end {
+	# Enable debug during low power modes (uses more power)
+	# DBGMCU_CR |= DBG_STANDBY | DBG_STOP | DBG_SLEEP
+	mmw 0xE0042004 0x00000007 0
+
+	# Stop watchdog counters during halt
+	# DBGMCU_APB1_FZ |= DBG_IWDG_STOP | DBG_WWDG_STOP
+	mmw 0xE0042008 0x00001800 0
+}
+
+proc proc_post_enable {_chipname} {
+	targets $_chipname.cpu
+
+	if { [$_chipname.tpiu cget -protocol] eq "sync" } {
+		switch [$_chipname.tpiu cget -port-width] {
+			1 {
+				mmw 0xE0042004 0x00000060 0x000000c0
+				mmw 0x48001020 0x00000000 0x0000ff00
+				mmw 0x48001000 0x000000a0 0x000000f0
+				mmw 0x48001008 0x000000f0 0x00000000
+			  }
+			2 {
+				mmw 0xE0042004 0x000000a0 0x000000c0
+				mmw 0x48001020 0x00000000 0x000fff00
+				mmw 0x48001000 0x000002a0 0x000003f0
+				mmw 0x48001008 0x000003f0 0x00000000
+			  }
+			4 {
+				mmw 0xE0042004 0x000000e0 0x000000c0
+				mmw 0x48001020 0x00000000 0x0fffff00
+				mmw 0x48001000 0x00002aa0 0x00003ff0
+				mmw 0x48001008 0x00003ff0 0x00000000
+			  }
+		}
+	} else {
+		mmw 0xE0042004 0x00000020 0x000000c0
+	}
+}
+
+$_CHIPNAME.tpiu configure -event post-enable "proc_post_enable $_CHIPNAME"
+
 $_TARGETNAME configure -event reset-init {
 	# CPU comes out of reset with MSI_ON | MSI_RDY | MSI Range 6 (4 MHz).
 	# Use MSI 24 MHz clock, compliant even with VOS == 2.
 	# 3 WS compliant with VOS == 2 and 24 MHz.
 	mww 0x40022000 0x00000103   ;# FLASH_ACR = PRFTBE | 3(Latency)
 	mww 0x40021000 0x00000099   ;# RCC_CR = MSI_ON | MSIRGSEL | MSI Range 9
+
 	# Boost JTAG frequency
 	adapter speed 4000
 }
@@ -102,19 +147,3 @@ $_TARGETNAME configure -event reset-start {
 	# Reset clock is MSI (4 MHz)
 	adapter speed 500
 }
-
-$_TARGETNAME configure -event examine-end {
-	# DBGMCU_CR |= DBG_STANDBY | DBG_STOP | DBG_SLEEP
-	mmw 0xE0042004 0x00000007 0
-
-	# Stop watchdog counters during halt
-	# DBGMCU_APB1_FZ |= DBG_IWDG_STOP | DBG_WWDG_STOP
-	mmw 0xE0042008 0x00001800 0
-}
-
-$_TARGETNAME configure -event trace-config {
-	# Set TRACE_IOEN; TRACE_MODE is set to async; when using sync
-	# change this value accordingly to configure trace pins
-	# assignment
-	mmw 0xE0042004 0x00000020 0
-}