Merge commit '9f23a1d7c1e27c556ef9787b9d3f263f5c1ecf24' into from_upstream

Conflicts:
	HACKING
	src/target/riscv/riscv-013.c

Change-Id: I43ccb143cae8daa39212d66a8824ae3ad2af6fef

73 files changed, 3094 insertions, 727 deletions

diff --git a/src/flash/nand/core.h b/src/flash/nand/core.h
index 19c53d1..137298c 100644
--- a/src/flash/nand/core.h
+++ b/src/flash/nand/core.h
@@ -179,6 +179,7 @@ enum oob_formats {
 	NAND_OOB_YAFFS2 = 0x100,/* when writing, use YAFFS2 OOB layout */
 };
 
+extern struct nand_device *nand_devices;
 
 struct nand_device *get_nand_device_by_num(int num);
 
@@ -202,6 +203,8 @@ int nand_calculate_ecc(struct nand_device *nand,
 		       const uint8_t *dat, uint8_t *ecc_code);
 int nand_calculate_ecc_kw(struct nand_device *nand,
 			  const uint8_t *dat, uint8_t *ecc_code);
+int nand_correct_data(struct nand_device *nand, u_char *dat,
+		u_char *read_ecc, u_char *calc_ecc);
 
 int nand_register_commands(struct command_context *cmd_ctx);
 
diff --git a/src/flash/nand/lpc32xx.c b/src/flash/nand/lpc32xx.c
index f8b59b3..1fdae9f 100644
--- a/src/flash/nand/lpc32xx.c
+++ b/src/flash/nand/lpc32xx.c
@@ -24,8 +24,6 @@
 static int lpc32xx_reset(struct nand_device *nand);
 static int lpc32xx_controller_ready(struct nand_device *nand, int timeout);
 static int lpc32xx_tc_ready(struct nand_device *nand, int timeout);
-extern int nand_correct_data(struct nand_device *nand, u_char *dat,
-		u_char *read_ecc, u_char *calc_ecc);
 
 /* These are offset with the working area in IRAM when using DMA to
  * read/write data to the SLC controller.
diff --git a/src/flash/nand/tcl.c b/src/flash/nand/tcl.c
index 4bb15fa..67a6277 100644
--- a/src/flash/nand/tcl.c
+++ b/src/flash/nand/tcl.c
@@ -17,9 +17,6 @@
 #include "fileio.h"
 #include <target/target.h>
 
-/* to be removed */
-extern struct nand_device *nand_devices;
-
 COMMAND_HANDLER(handle_nand_list_command)
 {
 	struct nand_device *p;
diff --git a/src/flash/nor/Makefile.am b/src/flash/nor/Makefile.am
index f04f0d2..534a7a8 100644
--- a/src/flash/nor/Makefile.am
+++ b/src/flash/nor/Makefile.am
@@ -54,6 +54,7 @@ NOR_DRIVERS = \
 	%D%/psoc4.c \
 	%D%/psoc5lp.c \
 	%D%/psoc6.c \
+	%D%/qn908x.c \
 	%D%/renesas_rpchf.c \
 	%D%/rp2040.c \
 	%D%/rsl10.c \
diff --git a/src/flash/nor/core.h b/src/flash/nor/core.h
index 8c26ba0..80911f7 100644
--- a/src/flash/nor/core.h
+++ b/src/flash/nor/core.h
@@ -253,6 +253,19 @@ int get_flash_bank_by_num(unsigned int num, struct flash_bank **bank);
 COMMAND_HELPER(flash_command_get_bank, unsigned name_index,
 		struct flash_bank **bank);
 /**
+ * Retrieves @a bank from a command argument, reporting errors parsing
+ * the bank identifier or retrieving the specified bank.  The bank
+ * may be identified by its bank number or by @c name.instance, where
+ * @a instance is driver-specific.
+ * @param name_index The index to the string in args containing the
+ * bank identifier.
+ * @param bank On output, contains a pointer to the bank or NULL.
+ * @param do_probe Does auto-probing when set, otherwise without probing.
+ * @returns ERROR_OK on success, or an error indicating the problem.
+ */
+COMMAND_HELPER(flash_command_get_bank_probe_optional, unsigned int name_index,
+		struct flash_bank **bank, bool do_probe);
+/**
  * Returns the flash bank like get_flash_bank_by_num(), without probing.
  * @param num The flash bank number.
  * @returns A struct flash_bank for flash bank @a num, or NULL.
diff --git a/src/flash/nor/driver.h b/src/flash/nor/driver.h
index fcb9ea7..514fe8a 100644
--- a/src/flash/nor/driver.h
+++ b/src/flash/nor/driver.h
@@ -285,6 +285,7 @@ extern const struct flash_driver psoc5lp_eeprom_flash;
 extern const struct flash_driver psoc5lp_flash;
 extern const struct flash_driver psoc5lp_nvl_flash;
 extern const struct flash_driver psoc6_flash;
+extern const struct flash_driver qn908x_flash;
 extern const struct flash_driver renesas_rpchf_flash;
 extern const struct flash_driver rp2040_flash;
 extern const struct flash_driver rsl10_flash;
diff --git a/src/flash/nor/drivers.c b/src/flash/nor/drivers.c
index a0e135f..02b1511 100644
--- a/src/flash/nor/drivers.c
+++ b/src/flash/nor/drivers.c
@@ -62,6 +62,7 @@ static const struct flash_driver * const flash_drivers[] = {
 	&psoc5lp_eeprom_flash,
 	&psoc5lp_nvl_flash,
 	&psoc6_flash,
+	&qn908x_flash,
 	&renesas_rpchf_flash,
 	&rp2040_flash,
 	&sh_qspi_flash,
diff --git a/src/flash/nor/jtagspi.c b/src/flash/nor/jtagspi.c
index c176ca8..6bb3af9 100644
--- a/src/flash/nor/jtagspi.c
+++ b/src/flash/nor/jtagspi.c
@@ -41,7 +41,11 @@ FLASH_BANK_COMMAND_HANDLER(jtagspi_flash_bank_command)
 	bank->sectors = NULL;
 	bank->driver_priv = info;
 
-	info->tap = NULL;
+	if (!bank->target->tap) {
+		LOG_ERROR("Target has no JTAG tap");
+		return ERROR_FAIL;
+	}
+	info->tap = bank->target->tap;
 	info->probed = false;
 	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[6], info->ir);
 
@@ -161,7 +165,12 @@ COMMAND_HANDLER(jtagspi_handle_set)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 	}
 
-	retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	/* calling flash_command_get_bank without probing because handle_set is used
+	   to set device parameters if not autodetected. So probing would fail
+	   anyhow.
+	*/
+	retval = CALL_COMMAND_HANDLER(flash_command_get_bank_probe_optional, 0,
+		&bank, false);
 	if (ERROR_OK != retval)
 		return retval;
 	info = bank->driver_priv;
@@ -292,52 +301,50 @@ COMMAND_HANDLER(jtagspi_handle_set)
 COMMAND_HANDLER(jtagspi_handle_cmd)
 {
 	struct flash_bank *bank;
-	unsigned int index = 1;
-	const int max = 21;
-	uint8_t num_write, num_read, write_buffer[max], read_buffer[1 << CHAR_BIT];
-	uint8_t data, *ptr;
-	char temp[4], output[(2 + max + (1 << CHAR_BIT)) * 3 + 8];
-	int retval;
+	const unsigned int max = 20;
+	uint8_t cmd_byte, num_read, write_buffer[max], read_buffer[1 << CHAR_BIT];
 
 	LOG_DEBUG("%s", __func__);
 
-	if (CMD_ARGC < 3) {
-		command_print(CMD, "jtagspi: not enough arguments");
+	if (CMD_ARGC < 3)
 		return ERROR_COMMAND_SYNTAX_ERROR;
-	}
 
-	num_write = CMD_ARGC - 2;
+	uint8_t num_write = CMD_ARGC - 3;
 	if (num_write > max) {
-		LOG_ERROR("at most %d bytes may be send", max);
-		return ERROR_COMMAND_SYNTAX_ERROR;
+		command_print(CMD, "at most %d bytes may be send", max);
+		return ERROR_COMMAND_ARGUMENT_INVALID;
 	}
 
-	retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
-	if (ERROR_OK != retval)
+	/* calling flash_command_get_bank without probing because we like to be
+	   able to send commands before auto-probing occurred. For example sending
+	   "release from power down" is needed before probing when flash is in
+	   power down mode.
+	 */
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank_probe_optional, 0,
+								&bank, false);
+	if (retval != ERROR_OK)
 		return retval;
 
-	COMMAND_PARSE_NUMBER(u8, CMD_ARGV[index++], num_read);
+	COMMAND_PARSE_NUMBER(u8, CMD_ARGV[1], num_read);
+	COMMAND_PARSE_NUMBER(u8, CMD_ARGV[2], cmd_byte);
 
-	snprintf(output, sizeof(output), "spi: ");
-	for (ptr = &write_buffer[0] ; index < CMD_ARGC; index++) {
-		COMMAND_PARSE_NUMBER(u8, CMD_ARGV[index], data);
-		*ptr++ = data;
-		snprintf(temp, sizeof(temp), "%02" PRIx8 " ", data);
-		strncat(output, temp, sizeof(output) - strlen(output) - 1);
-	}
-	strncat(output, "-> ", sizeof(output) - strlen(output) - 1);
+	for (unsigned int i = 0; i < num_write; i++)
+		COMMAND_PARSE_NUMBER(u8, CMD_ARGV[i + 3], write_buffer[i]);
 
 	/* process command */
-	ptr = &read_buffer[0];
-	jtagspi_cmd(bank, write_buffer[0], &write_buffer[1], num_write - 1, ptr, -num_read);
+	retval = jtagspi_cmd(bank, cmd_byte, write_buffer, num_write, read_buffer, -num_read);
 	if (retval != ERROR_OK)
 		return retval;
 
-	for ( ; num_read > 0; num_read--) {
-		snprintf(temp, sizeof(temp), "%02" PRIx8 " ", *ptr++);
-		strncat(output, temp, sizeof(output) - strlen(output) - 1);
-	}
-	command_print(CMD, "%s", output);
+	command_print_sameline(CMD, "spi: %02" PRIx8, cmd_byte);
+
+	for (unsigned int i = 0; i < num_write; i++)
+		command_print_sameline(CMD, " %02" PRIx8, write_buffer[i]);
+
+	command_print_sameline(CMD, " ->");
+
+	for (unsigned int i = 0; i < num_read; i++)
+		command_print_sameline(CMD, " %02" PRIx8, read_buffer[i]);
 
 	return ERROR_OK;
 }
@@ -381,12 +388,6 @@ static int jtagspi_probe(struct flash_bank *bank)
 	}
 	info->probed = false;
 
-	if (!bank->target->tap) {
-		LOG_ERROR("Target has no JTAG tap");
-		return ERROR_FAIL;
-	}
-	info->tap = bank->target->tap;
-
 	jtagspi_cmd(bank, SPIFLASH_READ_ID, NULL, 0, in_buf, -3);
 	/* the table in spi.c has the manufacturer byte (first) as the lsb */
 	id = le_to_h_u24(in_buf);
@@ -518,7 +519,7 @@ static int jtagspi_bulk_erase(struct flash_bank *bank)
 	if (retval != ERROR_OK)
 		return retval;
 
-	jtagspi_cmd(bank, info->dev.chip_erase_cmd, NULL, 0, NULL, 0);
+	retval = jtagspi_cmd(bank, info->dev.chip_erase_cmd, NULL, 0, NULL, 0);
 	if (retval != ERROR_OK)
 		return retval;
 
diff --git a/src/flash/nor/psoc4.c b/src/flash/nor/psoc4.c
index fb462c1..1064fa9 100644
--- a/src/flash/nor/psoc4.c
+++ b/src/flash/nor/psoc4.c
@@ -779,7 +779,7 @@ static int psoc4_probe(struct flash_bank *bank)
 		flash_size_in_kb = psoc4_info->user_bank_size / 1024;
 	}
 
-	char macros_txt[20] = "";
+	char macros_txt[22] = "";
 	if (num_macros > 1)
 		snprintf(macros_txt, sizeof(macros_txt), " in %" PRIu32 " macros", num_macros);
 
diff --git a/src/flash/nor/qn908x.c b/src/flash/nor/qn908x.c
new file mode 100644
index 0000000..8cd7a2f
--- /dev/null
+++ b/src/flash/nor/qn908x.c
@@ -0,0 +1,1197 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/***************************************************************************
+ *   Copyright (C) 2020 iosabi                                             *
+ *   iosabi <iosabi@protonmail.com>                                        *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "imp.h"
+
+#include <helper/binarybuffer.h>
+#include <helper/bits.h>
+#include <helper/crc32.h>
+#include <helper/time_support.h>
+#include <helper/types.h>
+
+/* The QN908x has two flash regions, one is the main flash region holding the
+ * user code and the second one is a small (0x800 bytes) "Flash information
+ * page" that can't be written to by the user. This page contains information
+ * programmed at the factory.
+ *
+ * The main flash region is normally 512 KiB, there's a field in the "Flash
+ * information page" that allows to specify for 256 KiB size chips. However, at
+ * the time of writing, none of the variants in the market have 256 KiB.
+ *
+ * The flash is divided into blocks of 256 KiB each, therefore containing two
+ * blocks. A block is subdivided into pages, of 2048 bytes. A page is the
+ * smallest region that can be erased or protected independently, although it
+ * is also possible to erase a whole block or both blocks. A page is subdivided
+ * into 8 rows of 64 words (32-bit words). The word subdivision is only
+ * relevant because DMA can write multiple words in the same row in the same
+ * flash program operation.
+ *
+ * For the Flash information page we are only interested in the last
+ * 0x100 bytes which contain a CRC-32 checksum of that 0x100 bytes long region
+ * and a field stating the size of the flash. This is also a good check that
+ * we are dealing with the right chip/flash configuration and is used in the
+ * probe() function.
+ */
+#define QN908X_FLASH_BASE						0x01000000
+
+#define QN908X_FLASH_PAGE_SIZE					2048
+#define QN908X_FLASH_PAGES_PER_BLOCK			128
+#define QN908X_FLASH_MAX_BLOCKS					2
+#define QN908X_FLASH_BLOCK_SIZE \
+		(QN908X_FLASH_PAGES_PER_BLOCK * QN908X_FLASH_PAGE_SIZE)
+#define QN908X_FLASH_IRQ_VECTOR_CHECKSUM_POS	0x1c
+#define QN908X_FLASH_IRQ_VECTOR_CHECKSUM_SIZE	4
+#define QN908X_FLASH_IRQ_VECTOR_CHECKSUM_END	\
+	(QN908X_FLASH_IRQ_VECTOR_CHECKSUM_POS + QN908X_FLASH_IRQ_VECTOR_CHECKSUM_SIZE)
+
+
+/* Flash information page memory fields. */
+#define QN908X_INFO_PAGE_BASE					0x210b0000u
+#define QN908X_INFO_PAGE_CRC32					(QN908X_INFO_PAGE_BASE + 0x700)
+#define QN908X_INFO_PAGE_CRC_START				(QN908X_INFO_PAGE_BASE + 0x704)
+#define QN908X_INFO_PAGE_BOOTLOADER_VER			(QN908X_INFO_PAGE_BASE + 0x704)
+#define QN908X_INFO_PAGE_FLASH_SIZE				(QN908X_INFO_PAGE_BASE + 0x708)
+#define QN908X_INFO_PAGE_BLUETOOTH_ADDR			(QN908X_INFO_PAGE_BASE + 0x7fa)
+#define QN908X_INFO_PAGE_CRC_END				(QN908X_INFO_PAGE_BASE + 0x800)
+
+
+/* Possible values of the QN908X_INFO_PAGE_FLASH_SIZE field. */
+enum qn908x_info_page_flash_size {
+	QN908X_FLASH_SIZE_512K					= 0xfffff0ff,
+	QN908X_FLASH_SIZE_256K					= 0xffffe0ff,
+};
+
+/* QN908x "Flash memory controller", described in section 28 of the user
+ * manual. In the NXP SDK this peripheral is called "FLASH", however we use the
+ * name "FMC" (Flash Memory Controller) here when referring to the controller
+ * to avoid confusion with other "flash" terms in OpenOCD. */
+#define QN908X_FMC_BASE 0x40081000u
+#define QN908X_FMC_INI_RD_EN						(QN908X_FMC_BASE + 0x00)
+#define QN908X_FMC_ERASE_CTRL						(QN908X_FMC_BASE + 0x04)
+#define QN908X_FMC_ERASE_TIME						(QN908X_FMC_BASE + 0x08)
+#define QN908X_FMC_TIME_CTRL						(QN908X_FMC_BASE + 0x0c)
+#define QN908X_FMC_SMART_CTRL						(QN908X_FMC_BASE + 0x10)
+#define QN908X_FMC_INT_STAT							(QN908X_FMC_BASE + 0x18)
+#define QN908X_FMC_LOCK_STAT_0						(QN908X_FMC_BASE + 0x20)
+#define QN908X_FMC_LOCK_STAT_1						(QN908X_FMC_BASE + 0x24)
+#define QN908X_FMC_LOCK_STAT_2						(QN908X_FMC_BASE + 0x28)
+#define QN908X_FMC_LOCK_STAT_3						(QN908X_FMC_BASE + 0x2c)
+#define QN908X_FMC_LOCK_STAT_4						(QN908X_FMC_BASE + 0x30)
+#define QN908X_FMC_LOCK_STAT_5						(QN908X_FMC_BASE + 0x34)
+#define QN908X_FMC_LOCK_STAT_6						(QN908X_FMC_BASE + 0x38)
+#define QN908X_FMC_LOCK_STAT_7						(QN908X_FMC_BASE + 0x3c)
+#define QN908X_FMC_LOCK_STAT_8						(QN908X_FMC_BASE + 0x40)
+#define QN908X_FMC_STATUS1							(QN908X_FMC_BASE + 0x48)
+#define QN908X_FMC_DEBUG_PASSWORD					(QN908X_FMC_BASE + 0xa8)
+#define QN908X_FMC_ERASE_PASSWORD					(QN908X_FMC_BASE + 0xac)
+
+#define QN908X_FMC_INI_RD_EN_INI_RD_EN_MASK			BIT(0)
+
+#define QN908X_FMC_STATUS1_FSH_ERA_BUSY_L_MASK		BIT(9)
+#define QN908X_FMC_STATUS1_FSH_WR_BUSY_L_MASK		BIT(10)
+#define QN908X_FMC_STATUS1_FSH_ERA_BUSY_H_MASK		BIT(12)
+#define QN908X_FMC_STATUS1_FSH_WR_BUSY_H_MASK		BIT(13)
+#define QN908X_FMC_STATUS1_INI_RD_DONE_MASK			BIT(15)
+#define QN908X_FMC_STATUS1_FSH_STA_MASK				BIT(26)
+
+#define QN908X_FMC_ERASE_CTRL_PAGE_IDXL_SHIFT		0
+#define QN908X_FMC_ERASE_CTRL_PAGE_IDXH_SHIFT		8
+#define QN908X_FMC_ERASE_CTRL_HALF_ERASEL_EN_SHIFT	28
+#define QN908X_FMC_ERASE_CTRL_HALF_ERASEH_EN_SHIFT	29
+#define QN908X_FMC_ERASE_CTRL_PAGE_ERASEL_EN_SHIFT	30
+#define QN908X_FMC_ERASE_CTRL_PAGE_ERASEH_EN_SHIFT	31
+
+#define QN908X_FMC_INT_STAT_AHBL_INT_MASK			BIT(0)
+#define QN908X_FMC_INT_STAT_LOCKL_INT_MASK			BIT(1)
+#define QN908X_FMC_INT_STAT_ERASEL_INT_MASK			BIT(2)
+#define QN908X_FMC_INT_STAT_WRITEL_INT_MASK			BIT(3)
+#define QN908X_FMC_INT_STAT_WR_BUFL_INT_MASK		BIT(4)
+#define QN908X_FMC_INT_STAT_WRITE_FAIL_L_INT_MASK	BIT(5)
+#define QN908X_FMC_INT_STAT_ERASE_FAIL_L_INT_MASK	BIT(6)
+#define QN908X_FMC_INT_STAT_AHBH_INT_MASK			BIT(8)
+#define QN908X_FMC_INT_STAT_LOCKH_INT_MASK			BIT(9)
+#define QN908X_FMC_INT_STAT_ERASEH_INT_MASK			BIT(10)
+#define QN908X_FMC_INT_STAT_WRITEH_INT_MASK			BIT(11)
+#define QN908X_FMC_INT_STAT_WR_BUFH_INT_MASK		BIT(12)
+#define QN908X_FMC_INT_STAT_WRITE_FAIL_H_INT_MASK	BIT(13)
+#define QN908X_FMC_INT_STAT_ERASE_FAIL_H_INT_MASK	BIT(14)
+
+#define QN908X_FMC_SMART_CTRL_PRGML_EN_MASK			BIT(0)
+#define QN908X_FMC_SMART_CTRL_PRGMH_EN_MASK			BIT(1)
+#define QN908X_FMC_SMART_CTRL_SMART_WRITEL_EN_MASK	BIT(2)
+#define QN908X_FMC_SMART_CTRL_SMART_WRITEH_EN_MASK	BIT(3)
+#define QN908X_FMC_SMART_CTRL_SMART_ERASEL_EN_MASK	BIT(4)
+#define QN908X_FMC_SMART_CTRL_SMART_ERASEH_EN_MASK	BIT(5)
+#define QN908X_FMC_SMART_CTRL_MAX_WRITE_MASK		0xf00u
+#define QN908X_FMC_SMART_CTRL_MAX_WRITE_SHIFT		8u
+#define QN908X_FMC_SMART_CTRL_MAX_WRITE(x) \
+	(((uint32_t)(((uint32_t)(x)) << QN908X_FMC_SMART_CTRL_MAX_WRITE_SHIFT)) \
+	& QN908X_FMC_SMART_CTRL_MAX_WRITE_MASK)
+#define QN908X_FMC_SMART_CTRL_MAX_ERASE_MASK		0x3f000u
+#define QN908X_FMC_SMART_CTRL_MAX_ERASE_SHIFT		12u
+#define QN908X_FMC_SMART_CTRL_MAX_ERASE(x) \
+	(((uint32_t)(((uint32_t)(x)) << QN908X_FMC_SMART_CTRL_MAX_ERASE_SHIFT)) \
+	& QN908X_FMC_SMART_CTRL_MAX_ERASE_MASK)
+
+#define QN908X_FMC_SMART_CTRL_MAX_ERASE_RETRIES		9
+#define QN908X_FMC_SMART_CTRL_MAX_WRITE_RETRIES		9
+
+#define QN908X_FMC_TIME_CTRL_PRGM_CYCLE_MASK		0xfffu
+#define QN908X_FMC_TIME_CTRL_PRGM_CYCLE_SHIFT		0u
+#define QN908X_FMC_TIME_CTRL_PRGM_CYCLE(x) \
+	(((uint32_t)(((uint32_t)(x)) << QN908X_FMC_TIME_CTRL_PRGM_CYCLE_SHIFT)) \
+	& QN908X_FMC_TIME_CTRL_PRGM_CYCLE_MASK)
+#define QN908X_FMC_TIME_CTRL_TIME_BASE_MASK			0xff000u
+#define QN908X_FMC_TIME_CTRL_TIME_BASE_SHIFT		12u
+#define QN908X_FMC_TIME_CTRL_TIME_BASE(x) \
+	(((uint32_t)(((uint32_t)(x)) << QN908X_FMC_TIME_CTRL_TIME_BASE_SHIFT)) \
+	& QN908X_FMC_TIME_CTRL_TIME_BASE_MASK)
+
+#define QN908X_FMC_LOCK_STAT_8_MASS_ERASE_LOCK_EN	BIT(0)
+#define QN908X_FMC_LOCK_STAT_8_FSH_PROTECT_EN		BIT(1)
+#define QN908X_FMC_LOCK_STAT_8_MEM_PROTECT_EN		BIT(2)
+#define QN908X_FMC_LOCK_STAT_8_PROTECT_ANY			(BIT(1) | BIT(2))
+
+/* See Table 418 "Flash lock and protect description" in the user manual */
+#define QN908X_FLASH_LOCK_ADDR						(QN908X_FLASH_BASE + 0x7f820)
+/* Allow mass erase */
+#define QN908X_FLASH_LOCK_ENABLE_MASS_ERASE			BIT(0)
+/* disallow flash access from SWD */
+#define QN908X_FLASH_LOCK_ENABLE_FLASH_PROTECTION	BIT(1)
+/* disallow SRAM access from SWD */
+#define QN908X_FLASH_LOCK_ENABLE_MEMORY_PROTECTION	BIT(2)
+
+/* Page lock information located at the beginning of the last page. */
+struct qn908x_flash_page_lock {
+	uint8_t bits[QN908X_FLASH_MAX_BLOCKS * QN908X_FLASH_PAGES_PER_BLOCK / 8];
+	uint8_t protection;
+	uint8_t _reserved[3];
+	/* nvds_size is unused here, but we need to preserve it across erases
+	 * when locking and unlocking pages. */
+	uint8_t nvds_size[4];
+} __attribute__ ((packed));
+
+/* Clock configuration is stored in the SYSCON. */
+#define QN908X_SYSCON_BASE						0x40000000u
+#define QN908X_SYSCON_CLK_EN					(QN908X_SYSCON_BASE + 0x00cu)
+#define QN908X_SYSCON_CLK_CTRL					(QN908X_SYSCON_BASE + 0x010u)
+#define QN908X_SYSCON_CHIP_ID					(QN908X_SYSCON_BASE + 0x108u)
+#define QN908X_SYSCON_XTAL_CTRL					(QN908X_SYSCON_BASE + 0x180u)
+
+/* Internal 16MHz / 8MHz clock used by the erase operation. */
+#define QN908X_SYSCON_CLK_EN_CLK_DP_EN_MASK		BIT(21)
+
+#define SYSCON_XTAL_CTRL_XTAL_DIV_MASK			BIT(31)
+
+#define SYSCON_CLK_CTRL_AHB_DIV_MASK			0x1FFF0u
+#define SYSCON_CLK_CTRL_AHB_DIV_SHIFT			4u
+#define SYSCON_CLK_CTRL_CLK_XTAL_SEL_MASK		BIT(19)
+#define SYSCON_CLK_CTRL_CLK_OSC32M_DIV_MASK		BIT(20)
+#define SYSCON_CLK_CTRL_SYS_CLK_SEL_MASK		0xC0000000u
+#define SYSCON_CLK_CTRL_SYS_CLK_SEL_SHIFT		30u
+
+#define CLOCK_16MHZ								16000000u
+#define CLOCK_32MHZ								32000000u
+#define CLOCK_32KHZ								32000u
+
+/* Watchdog block registers */
+#define QN908X_WDT_BASE							0x40001000u
+#define QN908X_WDT_CTRL							(QN908X_WDT_BASE + 0x08u)
+#define QN908X_WDT_LOCK							(QN908X_WDT_BASE + 0x20u)
+
+struct qn908x_flash_bank {
+	/* The number of flash blocks. Initially set to zero until the flash
+	 * is probed. This determines the size of the flash. */
+	unsigned int num_blocks;
+
+	unsigned int user_bank_size;
+	bool calc_checksum;
+
+	/* Whether we allow to flash an image that disables SWD access, potentially
+	 * bricking the device since the image can't be reflashed from SWD. */
+	bool allow_swd_disabled;
+
+	bool page_lock_loaded;
+	struct qn908x_flash_page_lock page_lock;
+};
+
+/* 500 ms timeout. */
+#define QN908X_DEFAULT_TIMEOUT_MS 500
+
+/* Forward declaration of commands. */
+static int qn908x_probe(struct flash_bank *bank);
+static int qn908x_write(struct flash_bank *bank, const uint8_t *buffer,
+		uint32_t offset, uint32_t count);
+
+/* Update the value of a register with a mask. This helper allows to read a
+ * register, modify a subset of the bits and write back the value, which is a
+ * common operation when modifying only a bit filed in a register. */
+static int qn908x_update_reg(struct target *target, target_addr_t reg,
+		uint32_t mask, uint32_t value)
+{
+	uint32_t orig_value = 0;
+	uint32_t new_value;
+	int retval;
+	if (mask != 0xffffffff) {
+		/* No need to read the old value if we request a mask of 32 bits. */
+		retval = target_read_u32(target, reg, &orig_value);
+		if (retval != ERROR_OK) {
+			LOG_DEBUG("Error reading reg at " TARGET_ADDR_FMT
+					": %d", reg, retval);
+			return retval;
+		}
+	}
+	new_value = (orig_value & ~mask) | (value & mask);
+	retval = target_write_u32(target, reg, new_value);
+	if (retval != ERROR_OK) {
+		LOG_DEBUG("Error writing reg at " TARGET_ADDR_FMT " with 0x%08"
+				PRIx32 ": %d", reg, new_value, retval);
+		return retval;
+	}
+	if (mask == 0xffffffff) {
+		LOG_DEBUG("Updated reg at " TARGET_ADDR_FMT ": ?? -> 0x%.08"
+				PRIx32 "", reg, new_value);
+	} else {
+		LOG_DEBUG("Updated reg at " TARGET_ADDR_FMT ": 0x%.08" PRIx32
+				" -> 0x%.08" PRIx32, reg, orig_value, new_value);
+	}
+	return ERROR_OK;
+}
+
+/* Load lock bit and protection bit and load redundancy page info.
+ * This populates the LOCK_STAT_n registers with the values from the lock page,
+ * making protection bit changes to the last page effective. */
+static int qn908x_load_lock_stat(struct target *target)
+{
+	int retval = target_write_u32(target, QN908X_FMC_INI_RD_EN,
+			QN908X_FMC_INI_RD_EN_INI_RD_EN_MASK);
+	if (retval != ERROR_OK)
+		return retval;
+
+	uint32_t status1;
+	const uint32_t status_mask = QN908X_FMC_STATUS1_FSH_STA_MASK
+			| QN908X_FMC_STATUS1_INI_RD_DONE_MASK;
+	do {
+		retval = target_read_u32(target, QN908X_FMC_STATUS1, &status1);
+		if (retval != ERROR_OK)
+			return retval;
+	} while ((status1 & status_mask) != QN908X_FMC_STATUS1_INI_RD_DONE_MASK);
+
+	for (int i = 0; i <= 8; i++) {
+		uint32_t addr = QN908X_FMC_LOCK_STAT_0 + i * 4;
+		uint32_t lock_stat;
+		if (target_read_u32(target, addr, &lock_stat) == ERROR_OK)
+			LOG_DEBUG("LOCK_STAT_%d = 0x%08" PRIx32, i, lock_stat);
+	}
+	return ERROR_OK;
+}
+
+/* Initializes the FMC controller registers for allowing writing. */
+static int qn908x_init_flash(struct target *target)
+{
+	/* Determine the current clock configuration. */
+	uint32_t clk_ctrl;
+	int retval = target_read_u32(target, QN908X_SYSCON_CLK_CTRL, &clk_ctrl);
+	if (retval != ERROR_OK)
+		return retval;
+
+	uint32_t clk_sel = (clk_ctrl & SYSCON_CLK_CTRL_SYS_CLK_SEL_MASK)
+			>> SYSCON_CLK_CTRL_SYS_CLK_SEL_SHIFT;
+	LOG_DEBUG("Clock clk_sel=0x%08" PRIu32, clk_sel);
+
+	/* Core clock frequency. */
+	uint32_t core_freq = 0;
+	switch (clk_sel) {
+	case 0:	/* RCO 32 MHz */
+		core_freq = (clk_ctrl & SYSCON_CLK_CTRL_CLK_OSC32M_DIV_MASK) ?
+			CLOCK_16MHZ : CLOCK_32MHZ;
+		break;
+	case 1:	/* Xin frequency */
+	{
+		uint32_t clk_xtal;
+		retval = target_read_u32(target, QN908X_SYSCON_XTAL_CTRL, &clk_xtal);
+		if (retval != ERROR_OK)
+			return retval;
+		core_freq	= (clk_ctrl & SYSCON_CLK_CTRL_CLK_XTAL_SEL_MASK)
+					&& (clk_xtal & SYSCON_XTAL_CTRL_XTAL_DIV_MASK)
+					? CLOCK_32MHZ : CLOCK_16MHZ;
+	}
+	break;
+	case 2:	/* 32 Kz */
+		core_freq = CLOCK_32KHZ;
+		break;
+	default:
+		return ERROR_FAIL;
+	}
+
+	uint32_t ahb_div = (clk_ctrl & SYSCON_CLK_CTRL_AHB_DIV_MASK)
+			>> SYSCON_CLK_CTRL_AHB_DIV_SHIFT;
+	uint32_t ahb_freq = core_freq / (ahb_div + 1);
+
+	LOG_DEBUG("Core freq: %" PRIu32 " Hz | AHB freq: %" PRIu32 " Hz",
+			core_freq, ahb_freq);
+
+	/* TIME_BASE is 2uS at the current AHB clock speed. */
+	retval = target_write_u32(target, QN908X_FMC_TIME_CTRL,
+			QN908X_FMC_TIME_CTRL_TIME_BASE(2 * ahb_freq / 1000000) |
+			QN908X_FMC_TIME_CTRL_PRGM_CYCLE(30));
+	if (retval != ERROR_OK)
+		return retval;
+
+	return qn908x_load_lock_stat(target);
+}
+
+/* flash bank qn908x <base> <size> 0 0 <target#> [calc_checksum] */
+FLASH_BANK_COMMAND_HANDLER(qn908x_flash_bank_command)
+{
+	struct qn908x_flash_bank *qn908x_info;
+
+	if (CMD_ARGC < 6 || CMD_ARGC > 7)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	if (bank->base != QN908X_FLASH_BASE) {
+		LOG_ERROR("Address " TARGET_ADDR_FMT
+			" is an invalid bank address (try 0x%08" PRIx32 ")",
+			bank->base, QN908X_FLASH_BASE);
+		return ERROR_COMMAND_ARGUMENT_INVALID;
+	}
+
+	qn908x_info = malloc(sizeof(struct qn908x_flash_bank));
+
+	if (!qn908x_info)
+		return ERROR_FAIL;
+
+	bank->driver_priv = qn908x_info;
+	qn908x_info->num_blocks = 0;
+	qn908x_info->user_bank_size = bank->size;
+	qn908x_info->page_lock_loaded = false;
+	qn908x_info->allow_swd_disabled = false;
+
+	qn908x_info->calc_checksum = false;
+	if (CMD_ARGC == 7) {
+		if (strcmp(CMD_ARGV[6], "calc_checksum")) {
+			free(qn908x_info);
+			return ERROR_COMMAND_ARGUMENT_INVALID;
+		}
+		qn908x_info->calc_checksum = true;
+	}
+
+	return ERROR_OK;
+}
+
+static int qn908x_read_page_lock(struct flash_bank *bank)
+{
+	struct qn908x_flash_bank *qn908x_info = bank->driver_priv;
+
+	if (bank->target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* The last page of the flash contains the "Flash lock and protect"
+	 * information. It is not clear where this is located on chips with only
+	 * one block. */
+	uint32_t prot_offset = qn908x_info->num_blocks * QN908X_FLASH_BLOCK_SIZE
+			- QN908X_FLASH_PAGE_SIZE;
+
+	int retval = target_read_memory(bank->target, bank->base + prot_offset, 4,
+			sizeof(qn908x_info->page_lock) / 4,
+			(void *)(&qn908x_info->page_lock));
+	if (retval != ERROR_OK)
+		return retval;
+	LOG_DEBUG("Flash protection = 0x%02" PRIx8,
+			qn908x_info->page_lock.protection);
+
+	qn908x_info->page_lock_loaded = true;
+	return ERROR_OK;
+}
+
+static int qn908x_busy_check(struct target *target)
+{
+	uint32_t status1;
+	int retval = target_read_u32(target, QN908X_FMC_STATUS1, &status1);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if ((status1 & (QN908X_FMC_STATUS1_FSH_ERA_BUSY_L_MASK
+					| QN908X_FMC_STATUS1_FSH_WR_BUSY_L_MASK
+					| QN908X_FMC_STATUS1_FSH_ERA_BUSY_H_MASK
+					| QN908X_FMC_STATUS1_FSH_WR_BUSY_H_MASK)))
+		return ERROR_FLASH_BUSY;
+	return ERROR_OK;
+}
+
+static int qn908x_status_check(struct target *target)
+{
+	uint32_t int_stat;
+	int retval = target_read_u32(target, QN908X_FMC_INT_STAT, &int_stat);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* The error bits for block 0 and block 1 have the exact same layout, only
+	 * that block 1 error bits are shifted by 8 bits. We use this fact to
+	 * loop over the blocks */
+	for (unsigned int block = 0; block <= 1; block++) {
+		unsigned int shift = (block) ? 8 : 0;
+		if (int_stat & (QN908X_FMC_INT_STAT_AHBL_INT_MASK << shift)) {
+			LOG_ERROR("AHB error on block %u", block);
+			return ERROR_FAIL;
+		}
+
+		if (int_stat & (QN908X_FMC_INT_STAT_LOCKL_INT_MASK << shift)) {
+			LOG_ERROR("Locked page being accessed error on block %u", block);
+			return ERROR_FAIL;
+		}
+
+		if (int_stat & (QN908X_FMC_INT_STAT_WRITE_FAIL_L_INT_MASK << shift)) {
+			LOG_ERROR("Smart write on block %u failed", block);
+			return ERROR_FAIL;
+		}
+
+		if ((int_stat & (QN908X_FMC_INT_STAT_ERASE_FAIL_L_INT_MASK << shift))
+				|| (int_stat & (QN908X_FMC_INT_STAT_ERASE_FAIL_H_INT_MASK << shift))) {
+			LOG_ERROR("Smart erase on block %u failed", block);
+			return ERROR_FAIL;
+		}
+	}
+
+	return ERROR_OK;
+}
+
+static int qn908x_wait_for_idle(struct target *target, int64_t timeout_ms)
+{
+	int64_t ms_start = timeval_ms();
+
+	int busy = ERROR_FLASH_BUSY;
+	while (busy != ERROR_OK) {
+		busy = qn908x_busy_check(target);
+		if (busy != ERROR_OK && busy != ERROR_FLASH_BUSY)
+			return busy;
+		if (timeval_ms() - ms_start > timeout_ms) {
+			LOG_ERROR("Timeout waiting to be idle.");
+			return ERROR_TIMEOUT_REACHED;
+		}
+	}
+	return ERROR_OK;
+}
+
+/* Set up the chip to perform an erase (page or block) operation. */
+static int qn908x_setup_erase(struct target *target)
+{
+	int retval;
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* Enable 8MHz clock. */
+	retval = qn908x_update_reg(target, QN908X_SYSCON_CLK_EN,
+			QN908X_SYSCON_CLK_EN_CLK_DP_EN_MASK,
+			QN908X_SYSCON_CLK_EN_CLK_DP_EN_MASK);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Set ERASE_TIME to 2ms for smart erase. */
+	retval = qn908x_update_reg(target, QN908X_FMC_ERASE_TIME,
+			(1u << 20) - 1,
+			2000 * 8);	/* 2000 uS * 8 MHz = x cycles */
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Set up smart erase. SWD can only perform smart erase. */
+	uint32_t ctrl_val	= QN908X_FMC_SMART_CTRL_SMART_ERASEH_EN_MASK
+						| QN908X_FMC_SMART_CTRL_SMART_ERASEL_EN_MASK
+						| QN908X_FMC_SMART_CTRL_MAX_ERASE(QN908X_FMC_SMART_CTRL_MAX_ERASE_RETRIES)
+						| QN908X_FMC_SMART_CTRL_MAX_WRITE(QN908X_FMC_SMART_CTRL_MAX_WRITE_RETRIES);
+	retval = target_write_u32(target, QN908X_FMC_SMART_CTRL, ctrl_val);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = qn908x_wait_for_idle(target, QN908X_DEFAULT_TIMEOUT_MS);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return ERROR_OK;
+}
+
+static int qn908x_erase(struct flash_bank *bank, unsigned int first,
+		unsigned int last)
+{
+	struct qn908x_flash_bank *qn908x_info = bank->driver_priv;
+	int retval = ERROR_OK;
+
+	if (!qn908x_info->num_blocks) {
+		if (qn908x_probe(bank) != ERROR_OK)
+			return ERROR_FLASH_BANK_NOT_PROBED;
+	}
+
+	retval = qn908x_setup_erase(bank->target);
+	if (retval != ERROR_OK)
+		return retval;
+
+	for (unsigned int i = first; i <= last; i++) {
+		if (i >= bank->num_sectors)
+			return ERROR_FLASH_SECTOR_INVALID;
+		uint32_t block_idx = i / QN908X_FLASH_PAGES_PER_BLOCK;
+		uint32_t page_idx = i % QN908X_FLASH_PAGES_PER_BLOCK;
+		if (block_idx >= qn908x_info->num_blocks)
+			return ERROR_FLASH_SECTOR_INVALID;
+
+		LOG_DEBUG("Erasing page %" PRIu32 " of block %" PRIu32,
+			page_idx, block_idx);
+
+		/* Depending on the block the page we are erasing is located we
+		 * need to use a different set of bits in the registers. */
+		uint32_t ctrl_page_idx_shift = block_idx ?
+			QN908X_FMC_ERASE_CTRL_PAGE_IDXH_SHIFT :
+			QN908X_FMC_ERASE_CTRL_PAGE_IDXL_SHIFT;
+		uint32_t ctrl_erase_en_shift = block_idx ?
+			QN908X_FMC_ERASE_CTRL_PAGE_ERASEH_EN_SHIFT :
+			QN908X_FMC_ERASE_CTRL_PAGE_ERASEL_EN_SHIFT;
+
+		retval = target_write_u32(bank->target, QN908X_FMC_ERASE_CTRL,
+				BIT(ctrl_erase_en_shift) | (page_idx << ctrl_page_idx_shift));
+		if (retval != ERROR_OK)
+			return retval;
+
+		retval = qn908x_wait_for_idle(bank->target, QN908X_DEFAULT_TIMEOUT_MS);
+		if (retval != ERROR_OK)
+			return retval;
+
+		retval = qn908x_status_check(bank->target);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	return retval;
+}
+
+static int qn908x_protect(struct flash_bank *bank, int set, unsigned int first,
+		unsigned int last)
+{
+	struct qn908x_flash_bank *qn908x_info = bank->driver_priv;
+
+	if (bank->target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (!qn908x_info->page_lock_loaded) {
+		int retval = qn908x_read_page_lock(bank);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	/* Use [first, last) interval open on the right side from now on. */
+	last++;
+	/* We use sectors as prot_blocks. */
+	bool needs_update = false;
+	for (unsigned int i = first; i < last; i++) {
+		if (set != (((qn908x_info->page_lock.bits[i / 8] >> (i % 8)) & 1) ^ 1))
+			needs_update = true;
+	}
+
+	/* Check that flash protection still allows SWD access to flash and RAM,
+	 * otherwise we won't be able to re-flash this chip from SWD unless we do a
+	 * mass erase. */
+	if (qn908x_info->page_lock.protection & QN908X_FMC_LOCK_STAT_8_PROTECT_ANY) {
+		LOG_WARNING("SWD flash/RAM access disabled in the Flash lock and "
+			"protect descriptor. You might need to issue a mass_erase to "
+			"regain SWD access to this chip after reboot.");
+	}
+
+	if (!needs_update)
+		return ERROR_OK;
+
+	int last_page = qn908x_info->num_blocks * QN908X_FLASH_PAGES_PER_BLOCK - 1;
+	int retval;
+
+	if (qn908x_info->page_lock.bits[sizeof(qn908x_info->page_lock.bits) - 1] & 0x80) {
+		/* A bit 1 in the MSB in the page_lock.bits array means that the last
+		 * page is unlocked, so we can just erase it. */
+		retval = qn908x_erase(bank, last_page, last_page);
+		if (retval != ERROR_OK)
+			return retval;
+	} else {
+		/* TODO: The last page is locked and we can't erase unless we use the
+		 * ERASE_PASSWORD from code running on the device. For this we need to
+		 * copy a little program to RAM and execute the erase command from
+		 * there since there's no way to override the page protection from
+		 * SWD. */
+		LOG_ERROR("Unprotecting the last page is not supported. Issue a "
+			"\"qn908x mass_erase\" command to erase the whole flash, "
+			"including the last page and its protection.");
+		return ERROR_FAIL;
+	}
+
+	for (unsigned int i = first / 8; i < (last + 7) / 8; i++) {
+		/* first_mask contains a bit set if the bit corresponds to a block id
+		 * that is larger or equal than first. This is basically 0xff in all
+		 * cases except potentially the first iteration. */
+		uint8_t first_mask	= (first <= i * 8)
+							? 0xff : 0xff ^ ((1u << (first - i * 8)) - 1);
+		/* Similar to first_mask, this contains a bit set if the corresponding
+		 * is smaller than last. */
+		uint8_t last_mask	= (i * 8 + 8 <= last)
+							? 0xff : ((1u << (last - i * 8)) - 1);
+
+		uint8_t mask = first_mask & last_mask;
+		LOG_DEBUG("protect set=%d bits[%d] with mask=0x%02x", set, i, mask);
+		/* To "set" the protection bit means to clear the bit in the page_lock
+		 * bit array. */
+		if (set)
+			qn908x_info->page_lock.bits[i] &= ~mask;
+		else
+			qn908x_info->page_lock.bits[i] |= mask;
+	}
+
+	retval = qn908x_write(bank, (void *)(&qn908x_info->page_lock),
+			last_page * QN908X_FLASH_PAGE_SIZE, sizeof(qn908x_info->page_lock));
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Reload the lock_stat to make the changes effective. */
+	retval = qn908x_load_lock_stat(bank->target);
+	if (retval != ERROR_OK)
+		return retval;
+
+	for (unsigned int i = first; i < last; i++)
+		bank->sectors[i].is_protected = set;
+
+	return ERROR_OK;
+}
+
+static int qn908x_write(struct flash_bank *bank, const uint8_t *buffer,
+		uint32_t offset, uint32_t count)
+{
+	struct qn908x_flash_bank *qn908x_info = bank->driver_priv;
+	int retval = ERROR_OK;
+
+	if (bank->target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* The flash infrastructure was requested to align writes to 32 bit */
+	assert(((offset % 4) == 0) && ((count % 4) == 0));
+
+	/* Compute the calc_checksum even if it wasn't requested. */
+	uint32_t checksum = 0;
+	if (offset == 0 && count >= 0x20) {
+		for (int i = 0; i < 7; i++)
+			checksum += buf_get_u32(buffer + (i * 4), 0, 32);
+		checksum = 0 - checksum;
+		LOG_DEBUG("computed image checksum: 0x%8.8" PRIx32, checksum);
+		uint32_t stored_checksum = buf_get_u32(buffer + 7 * 4, 0, 32);
+		if (checksum != stored_checksum) {
+			LOG_WARNING("Image vector table checksum mismatch: expected 0x%08"
+					PRIx32 " but found 0x%08" PRIx32,
+					checksum, stored_checksum);
+			if (!qn908x_info->calc_checksum)
+				LOG_WARNING("This device will not boot, use calc_checksum in "
+						"the flash bank.");
+			else
+				LOG_WARNING("Updating checksum, verification will fail.");
+		}
+	}
+
+	/* Check the Code Read Protection (CRP) word for invalid values or not
+	 * allowed ones. */
+	if (offset <= 0x20 && offset + count >= 0x24) {
+		uint32_t crp = buf_get_u32(buffer + 0x20 - offset, 0, 32);
+		/* 2-bit fields at bits 10, 12, 14, 16 and 18 must not be 00 or 11. */
+		for (int i = 10; i <= 18; i += 2) {
+			uint32_t field = (crp >> i) & 3;
+			if (field == 0 || field == 3) {
+				LOG_DEBUG("Code Read Protection = 0x%08" PRIx32, crp);
+				LOG_ERROR("The Code Read Protection (CRP) field at bit %d is "
+						"invalid (%" PRIu32 "). An invalid value could make "
+						"the flash inaccessible.", i, field);
+				return ERROR_FAIL;
+			}
+		}
+
+		uint32_t swd_allowed = (crp >> 18) & 3;
+		if (swd_allowed != 2) {
+			LOG_WARNING("The Code Read Protection (CRP) in this image "
+					"(0x%08" PRIx32 ") is disabling the SWD access, which is "
+					"currently used by OpenOCD to flash this device. After "
+					"reboot, this device will not be accessible to OpenOCD "
+					"anymore.", crp);
+			if (!qn908x_info->allow_swd_disabled) {
+				LOG_ERROR("Disabling SWD is not allowed, run "
+						"\"qn908x allow_brick\" before if you really want to "
+						"disable SWD. You won't be able to access this chip "
+						"anymore from OpenOCD.");
+				return ERROR_FAIL;
+			}
+		}
+	}
+
+	retval = qn908x_wait_for_idle(bank->target, QN908X_DEFAULT_TIMEOUT_MS);
+	if (retval != ERROR_OK)
+		return retval;
+
+	uint32_t smart_ctrl	= QN908X_FMC_SMART_CTRL_SMART_WRITEL_EN_MASK
+						| QN908X_FMC_SMART_CTRL_PRGML_EN_MASK
+						| QN908X_FMC_SMART_CTRL_MAX_WRITE(QN908X_FMC_SMART_CTRL_MAX_WRITE_RETRIES);
+	if (qn908x_info->num_blocks > 1) {
+		smart_ctrl	|= QN908X_FMC_SMART_CTRL_SMART_WRITEH_EN_MASK
+					| QN908X_FMC_SMART_CTRL_PRGMH_EN_MASK;
+	}
+	retval = target_write_u32(bank->target, QN908X_FMC_SMART_CTRL, smart_ctrl);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Write data page-wise, as suggested in the examples in section
+	 * 28.5.2 "Flash write" of user manual UM11023 in revision 1.1
+	 * (February 2018). */
+	while (count > 0) {
+		uint32_t next_offset = (offset & ~(QN908X_FLASH_PAGE_SIZE - 1)) + QN908X_FLASH_PAGE_SIZE;
+		uint32_t chunk_len = next_offset - offset;
+		if (chunk_len > count)
+			chunk_len = count;
+
+		if (offset == 0
+				&& chunk_len >= QN908X_FLASH_IRQ_VECTOR_CHECKSUM_END
+				&& qn908x_info->calc_checksum) {
+			/* write data prior to checksum */
+			retval = target_write_buffer(bank->target, bank->base,
+					QN908X_FLASH_IRQ_VECTOR_CHECKSUM_POS, buffer);
+			if (retval != ERROR_OK)
+				return retval;
+
+			/* write computed crc checksum instead of provided data */
+			retval = target_write_u32(bank->target, bank->base + QN908X_FLASH_IRQ_VECTOR_CHECKSUM_POS, checksum);
+			if (retval != ERROR_OK)
+				return retval;
+
+			retval = target_write_buffer(bank->target,
+					bank->base + QN908X_FLASH_IRQ_VECTOR_CHECKSUM_END,
+					chunk_len - QN908X_FLASH_IRQ_VECTOR_CHECKSUM_END,
+					buffer + QN908X_FLASH_IRQ_VECTOR_CHECKSUM_END);
+		} else {
+			retval = target_write_buffer(bank->target, bank->base + offset,
+					chunk_len, buffer);
+		}
+
+		if (retval != ERROR_OK)
+			return retval;
+
+		keep_alive();
+		buffer += chunk_len;
+		count -= chunk_len;
+		offset = next_offset;
+
+		/* Wait for FMC to complete write */
+		retval = qn908x_wait_for_idle(bank->target, QN908X_DEFAULT_TIMEOUT_MS);
+		if (retval != ERROR_OK)
+			return retval;
+
+		/* Check if FMC reported any errors */
+		retval = qn908x_status_check(bank->target);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	return retval;
+}
+
+static int is_flash_protected(struct flash_bank *bank, bool *is_protected)
+{
+	int retval;
+	uint32_t lock_stat;
+	retval = target_read_u32(bank->target, QN908X_FMC_LOCK_STAT_8, &lock_stat);
+	if (retval)
+		return retval;
+
+	*is_protected = false;
+	if (lock_stat & QN908X_FMC_LOCK_STAT_8_PROTECT_ANY)
+		*is_protected = true;
+
+	return ERROR_OK;
+}
+
+static int qn908x_probe(struct flash_bank *bank)
+{
+	int retval;
+	struct qn908x_flash_bank *qn908x_info = bank->driver_priv;
+	uint8_t info_page[QN908X_INFO_PAGE_CRC_END - QN908X_INFO_PAGE_CRC_START];
+	qn908x_info->num_blocks = 0;
+
+	/* When the SWD access to the RAM is locked by the LOCK_STAT_8 register we
+	 * can't access the info page to verify the chip/bank version and it will
+	 * read all zeros. This situation prevents the bank from being initialized
+	 * at all so no other operation can be performed. The only option to
+	 * re-flash the chip is to perform a mass_erase from SWD, which can be
+	 * performed even if the mass_erase operation is locked as well.
+	 * We attempt to read the info page and redirect the user to perform a
+	 * mass_erase if we detect this situation. */
+	retval = target_read_memory(bank->target, QN908X_INFO_PAGE_CRC_START,
+			sizeof(uint32_t), sizeof(info_page) / sizeof(uint32_t),
+			info_page);
+	if (retval != ERROR_OK)
+		return retval;
+
+	const uint32_t crc_seed = 0xffffffff;
+	/* The QN908x uses the standard little endian CRC32 polynomial and all ones
+	 * as seed. The CRC32 is however finalized by one last xor operation that
+	 * is not part of the common CRC32 implementation, so we do that by hand */
+	uint32_t computed_crc = crc32_le(CRC32_POLY_LE, crc_seed,
+			info_page, sizeof(info_page));
+	computed_crc ^= crc_seed;
+	uint32_t read_crc;
+	retval = target_read_u32(bank->target, QN908X_INFO_PAGE_CRC32, &read_crc);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (computed_crc != read_crc) {
+		uint32_t info_page_or = 0;
+		for (unsigned int i = 0; i < sizeof(info_page); i++)
+			info_page_or |= info_page[i];
+		bool is_protected;
+		retval = is_flash_protected(bank, &is_protected);
+		if (retval != ERROR_OK)
+			return retval;
+
+		if (info_page_or == 0 && is_protected) {
+			LOG_ERROR("The flash or memory in this chip is protected and "
+				"cannot be accessed from the SWD interface. However, a "
+				"\"qn908x mass_erase\" can erase the device and lift this "
+				"protection.");
+			return ERROR_FAIL;
+		}
+
+		LOG_ERROR("Flash information page CRC32 mismatch, found 0x%08"
+			PRIx32 " but computed 0x%08" PRIx32 ". Flash size unknown",
+			read_crc, computed_crc);
+		return ERROR_FAIL;
+	}
+
+	uint32_t flash_size_fld = target_buffer_get_u32(bank->target,
+			info_page + (QN908X_INFO_PAGE_FLASH_SIZE - QN908X_INFO_PAGE_CRC_START));
+
+	switch (flash_size_fld) {
+	case QN908X_FLASH_SIZE_512K:
+		qn908x_info->num_blocks = 2;
+		break;
+	case QN908X_FLASH_SIZE_256K:
+		qn908x_info->num_blocks = 1;
+		break;
+	default:
+		LOG_ERROR("Unknown Flash size field: 0x%08" PRIx32,
+			flash_size_fld);
+		return ERROR_FAIL;
+	}
+
+	bank->size = qn908x_info->num_blocks * QN908X_FLASH_BLOCK_SIZE;
+	bank->write_start_alignment = 4;
+	bank->write_end_alignment = 4;
+
+	/* The flash supports erasing and protecting individual pages. */
+	bank->num_sectors = qn908x_info->num_blocks *
+		QN908X_FLASH_PAGES_PER_BLOCK;
+	bank->sectors = alloc_block_array(0, QN908X_FLASH_PAGE_SIZE,
+			bank->num_sectors);
+	if (!bank->sectors)
+		return ERROR_FAIL;
+
+	retval = qn908x_init_flash(bank->target);
+	if (retval != ERROR_OK)
+		return retval;
+
+	LOG_INFO("Detected flash size: %d KiB", bank->size / 1024);
+
+	return ERROR_OK;
+}
+
+static int qn908x_auto_probe(struct flash_bank *bank)
+{
+	struct qn908x_flash_bank *qn908x_info = bank->driver_priv;
+	if (qn908x_info->num_blocks != 0)
+		return ERROR_OK;
+	LOG_DEBUG("auto_probe");
+	return qn908x_probe(bank);
+}
+
+static int qn908x_protect_check(struct flash_bank *bank)
+{
+	struct qn908x_flash_bank *qn908x_info = bank->driver_priv;
+
+	int retval = qn908x_read_page_lock(bank);
+	if (retval != ERROR_OK)
+		return retval;
+
+	for (uint32_t i = 0;
+			i < qn908x_info->num_blocks * QN908X_FLASH_PAGES_PER_BLOCK;
+			i++) {
+		/* A bit 0 in page_lock means page is locked. */
+		bank->sectors[i].is_protected =
+			((qn908x_info->page_lock.bits[i / 8] >> (i % 8)) & 1) ^ 1;
+	}
+	return ERROR_OK;
+}
+
+static int qn908x_get_info(struct flash_bank *bank,
+		struct command_invocation *cmd)
+{
+	uint32_t bootloader_version;
+	uint32_t chip_id;
+	uint8_t bluetooth[6];
+	int retval;
+	struct qn908x_flash_bank *qn908x_info = bank->driver_priv;
+
+	retval = target_read_u32(bank->target, QN908X_SYSCON_CHIP_ID, &chip_id);
+	if (retval != ERROR_OK) {
+		command_print_sameline(cmd, "Cannot read QN908x chip ID.");
+		return retval;
+	}
+	retval = target_read_u32(bank->target, QN908X_INFO_PAGE_BOOTLOADER_VER,
+			&bootloader_version);
+	if (retval != ERROR_OK) {
+		command_print_sameline(cmd, "Cannot read from QN908x info page.");
+		return retval;
+	}
+
+	retval = target_read_memory(bank->target, QN908X_INFO_PAGE_BLUETOOTH_ADDR,
+			1, sizeof(bluetooth), bluetooth);
+	if (retval != ERROR_OK) {
+		command_print_sameline(cmd, "Cannot read QN908x bluetooth L2 address.");
+		return retval;
+	}
+
+	command_print_sameline(cmd, "qn908x: chip id: 0x%" PRIx32, chip_id);
+
+	command_print_sameline(cmd, " bdaddr: "
+			"%02" PRIx8 ":%02" PRIx8 ":%02" PRIx8
+			":%02" PRIx8 ":%02" PRIx8 ":%02" PRIx8,
+			bluetooth[0], bluetooth[1], bluetooth[2],
+			bluetooth[3], bluetooth[4], bluetooth[5]);
+
+	command_print_sameline(cmd, " bootloader: %08" PRIx32, bootloader_version);
+
+	command_print_sameline(cmd, " blocks: %" PRIu32, qn908x_info->num_blocks);
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(qn908x_handle_allow_brick_command)
+{
+	int retval;
+
+	struct target *target = get_current_target(CMD_CTX);
+	struct flash_bank *bank = NULL;
+
+	if (CMD_ARGC != 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	retval = get_flash_bank_by_addr(target, QN908X_FLASH_BASE, true, &bank);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* If get_flash_bank_by_addr() did not find the flash bank, it should have
+	 * returned and error code instead of ERROR_OK */
+	assert(bank);
+	struct qn908x_flash_bank *qn908x_info = bank->driver_priv;
+
+	LOG_WARNING("Flashing images that disable SWD in qn908x is now allowed.");
+	qn908x_info->allow_swd_disabled = true;
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(qn908x_handle_disable_wdog_command)
+{
+	int retval;
+	struct target *target = get_current_target(CMD_CTX);
+
+	if (CMD_ARGC != 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	if (target->state != TARGET_HALTED) {
+		command_print(CMD, "Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* To change any value in the watchdog block (WDT) we need to first write
+	 * 0x1ACCE551 to the LOCK register, and we can then set it back to any other
+	 * value to prevent accidental changes to the watchdog. */
+	retval = target_write_u32(target, QN908X_WDT_LOCK, 0x1ACCE551);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = target_write_u32(target, QN908X_WDT_CTRL, 0);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return target_write_u32(target, QN908X_WDT_LOCK, 0);
+}
+
+COMMAND_HANDLER(qn908x_handle_mass_erase_command)
+{
+	int retval;
+	bool keep_lock = false;
+	if (CMD_ARGC > 1)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	if (CMD_ARGC == 1) {
+		if (strcmp("keep_lock", CMD_ARGV[0]))
+			return ERROR_COMMAND_ARGUMENT_INVALID;
+		keep_lock = true;
+	}
+
+	/* This operation can be performed without probing the bank since it is the
+	 * only way to unlock a chip when the flash and ram have been locked. */
+	struct target *target = get_current_target(CMD_CTX);
+
+	retval = qn908x_setup_erase(target);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Check the mass-erase locking status for information purposes only. This
+	 * lock applies to both the SWD and the code running in the core but can be
+	 * bypassed in either case. */
+	uint32_t lock_stat_8;
+	retval = target_read_u32(target, QN908X_FMC_LOCK_STAT_8, &lock_stat_8);
+	LOG_DEBUG("LOCK_STAT_8 before erasing: 0x%" PRIx32, lock_stat_8);
+	if (retval != ERROR_OK)
+		return retval;
+	if ((lock_stat_8 & QN908X_FMC_LOCK_STAT_8_MASS_ERASE_LOCK_EN) == 0) {
+		LOG_INFO("mass_erase disabled by Flash lock and protection, forcing "
+				"mass_erase.");
+	}
+	/* Set the DEBUG_PASSWORD so we can force the mass erase from the SWD. We do
+	 * this regardless of the lock status. */
+	retval = target_write_u32(target, QN908X_FMC_DEBUG_PASSWORD, 0xCA1E093F);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Erase both halves of the flash at the same time. These are actually done
+	 * sequentially but we need to send the command to erase both blocks since
+	 * doing so in a locked flash will change the LOCK_STAT_8 register to 0x01,
+	 * allowing us to access the (now erase) flash an memory. Erasing only one
+	 * block at a time does not reset the LOCK_STAT_8 register and therefore
+	 * will not grant access to program the chip. */
+	uint32_t erase_cmd = (1u << QN908X_FMC_ERASE_CTRL_HALF_ERASEH_EN_SHIFT) |
+		(1u << QN908X_FMC_ERASE_CTRL_HALF_ERASEL_EN_SHIFT);
+	LOG_DEBUG("Erasing both blocks with command 0x%" PRIx32, erase_cmd);
+
+	retval = target_write_u32(target, QN908X_FMC_ERASE_CTRL, erase_cmd);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = qn908x_wait_for_idle(target, QN908X_DEFAULT_TIMEOUT_MS);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = qn908x_status_check(target);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Set the debug password back to 0 to avoid accidental mass_erase. */
+	retval = target_write_u32(target, QN908X_FMC_DEBUG_PASSWORD, 0);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* At this point the flash is erased and we are able to write to the flash
+	 * since the LOCK_STAT_8 gets updated to 0x01 after the mass_erase. However,
+	 * after a hard reboot this value will be realoaded from flash which after
+	 * an erase is 0xff. This means that after flashing an image that doesn't
+	 * set the protection bits we end up with a chip that we can't debug. We
+	 * update this value to 0x01 unless "keep_lock" is passed to allow the SWD
+	 * interface to debug the flash and RAM after a hard reset. */
+	if (keep_lock)
+		return retval;
+
+	retval = qn908x_init_flash(target);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Unlock access to RAM and FLASH in the last page of the flash and
+	 * reloading */
+	retval = qn908x_wait_for_idle(target, QN908X_DEFAULT_TIMEOUT_MS);
+	if (retval != ERROR_OK)
+		return retval;
+
+	uint32_t smart_ctrl = QN908X_FMC_SMART_CTRL_SMART_WRITEH_EN_MASK |
+		QN908X_FMC_SMART_CTRL_PRGMH_EN_MASK;
+	retval = target_write_u32(target, QN908X_FMC_SMART_CTRL, smart_ctrl);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = target_write_u32(target, QN908X_FLASH_LOCK_ADDR,
+			QN908X_FLASH_LOCK_ENABLE_MASS_ERASE);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = qn908x_wait_for_idle(target, QN908X_DEFAULT_TIMEOUT_MS);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Force a page_lock reload after the mass_erase . */
+	retval = qn908x_load_lock_stat(target);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return retval;
+}
+
+static const struct command_registration qn908x_exec_command_handlers[] = {
+	{
+		.name		= "allow_brick",
+		.handler	= qn908x_handle_allow_brick_command,
+		.mode		= COMMAND_EXEC,
+		.help		= "Allow writing images that disable SWD access in their "
+				"Code Read Protection (CRP) word. Warning: This can make your "
+				"chip inaccessible from OpenOCD or any other SWD debugger.",
+		.usage		= "",
+	},
+	{
+		.name		= "disable_wdog",
+		.handler	= qn908x_handle_disable_wdog_command,
+		.mode		= COMMAND_EXEC,
+		.help		= "Disabled the watchdog (WDT).",
+		.usage		= "",
+	},
+	{
+		.name		= "mass_erase",
+		.handler	= qn908x_handle_mass_erase_command,
+		.mode		= COMMAND_EXEC,
+		.help		= "Erase the whole flash chip.",
+		.usage		= "[keep_lock]",
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+static const struct command_registration qn908x_command_handlers[] = {
+	{
+		.name		= "qn908x",
+		.mode		= COMMAND_ANY,
+		.help		= "qn908x flash controller commands",
+		.usage		= "",
+		.chain		= qn908x_exec_command_handlers,
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+const struct flash_driver qn908x_flash = {
+	.name				= "qn908x",
+	.commands			= qn908x_command_handlers,
+	.flash_bank_command	= qn908x_flash_bank_command,
+	.info				= qn908x_get_info,
+	.erase				= qn908x_erase,
+	.protect			= qn908x_protect,
+	.write				= qn908x_write,
+	.read				= default_flash_read,
+	.probe				= qn908x_probe,
+	.auto_probe			= qn908x_auto_probe,
+	.erase_check		= default_flash_blank_check,
+	.protect_check		= qn908x_protect_check,
+	.free_driver_priv	= default_flash_free_driver_priv,
+};
diff --git a/src/flash/nor/rp2040.c b/src/flash/nor/rp2040.c
index 6c18c7b..b2ebd9c 100644
--- a/src/flash/nor/rp2040.c
+++ b/src/flash/nor/rp2040.c
@@ -91,7 +91,7 @@ static uint32_t rp2040_lookup_symbol(struct target *target, uint32_t tag, uint16
 }
 
 static int rp2040_call_rom_func(struct target *target, struct rp2040_flash_bank *priv,
-		uint16_t func_offset, uint32_t argdata[], unsigned int n_args, int timeout_ms)
+		uint16_t func_offset, uint32_t argdata[], unsigned int n_args, unsigned int timeout_ms)
 {
 	char *regnames[4] = { "r0", "r1", "r2", "r3" };
 
@@ -312,7 +312,7 @@ static int rp2040_flash_erase(struct flash_bank *bank, unsigned int first, unsig
 	an optional larger "block" (size and command provided in args).
 	*/
 
-	int timeout_ms = 2000 * (last - first) + 1000;
+	unsigned int timeout_ms = 2000 * (last - first) + 1000;
 	err = rp2040_call_rom_func(target, priv, priv->jump_flash_range_erase,
 							args, ARRAY_SIZE(args), timeout_ms);
 
diff --git a/src/flash/nor/stm32f2x.c b/src/flash/nor/stm32f2x.c
index 36b7a0d..dcaf260 100644
--- a/src/flash/nor/stm32f2x.c
+++ b/src/flash/nor/stm32f2x.c
@@ -230,11 +230,11 @@ static int stm32x_otp_enable(struct flash_bank *bank)
 	struct stm32x_flash_bank *stm32x_info = bank->driver_priv;
 
 	if (!stm32x_info->otp_unlocked) {
-		LOG_INFO("OTP memory bank #%u is is enabled for write commands.",
+		LOG_INFO("OTP memory bank #%u is enabled for write commands.",
 			 bank->bank_number);
 		stm32x_info->otp_unlocked = true;
 	} else {
-		LOG_WARNING("OTP memory bank #%u is is already enabled for write commands.",
+		LOG_WARNING("OTP memory bank #%u is already enabled for write commands.",
 			    bank->bank_number);
 	}
 	return ERROR_OK;
@@ -659,8 +659,10 @@ static int stm32x_protect(struct flash_bank *bank, int set, unsigned int first,
 	}
 
 	if (stm32x_is_otp(bank)) {
-		if (!set)
+		if (!set) {
+			LOG_ERROR("OTP protection can only be enabled");
 			return ERROR_COMMAND_ARGUMENT_INVALID;
+		}
 
 		return stm32x_otp_protect(bank, first, last);
 	}
diff --git a/src/flash/nor/stm32l4x.c b/src/flash/nor/stm32l4x.c
index 02e737b..934deec 100644
--- a/src/flash/nor/stm32l4x.c
+++ b/src/flash/nor/stm32l4x.c
@@ -80,6 +80,12 @@
  * http://www.st.com/resource/en/reference_manual/dm00451556.pdf
  */
 
+/* STM32C0xxx series for reference.
+ *
+ * RM0490 (STM32C0x1)
+ * http://www.st.com/resource/en/reference_manual/dm00781702.pdf
+ */
+
 /* STM32G0xxx series for reference.
  *
  * RM0444 (STM32G0x1)
@@ -263,7 +269,7 @@ struct stm32l4_wrp {
 };
 
 /* human readable list of families this drivers supports (sorted alphabetically) */
-static const char *device_families = "STM32G0/G4/L4/L4+/L5/U5/WB/WL";
+static const char *device_families = "STM32C0/G0/G4/L4/L4+/L5/U5/WB/WL";
 
 static const struct stm32l4_rev stm32l47_l48xx_revs[] = {
 	{ 0x1000, "1" }, { 0x1001, "2" }, { 0x1003, "3" }, { 0x1007, "4" }
@@ -273,6 +279,15 @@ static const struct stm32l4_rev stm32l43_l44xx_revs[] = {
 	{ 0x1000, "A" }, { 0x1001, "Z" }, { 0x2001, "Y" },
 };
 
+
+static const struct stm32l4_rev stm32c01xx_revs[] = {
+	{ 0x1000, "A" }, { 0x1001, "Z" },
+};
+
+static const struct stm32l4_rev stm32c03xx_revs[] = {
+	{ 0x1000, "A" }, { 0x1001, "Z" },
+};
+
 static const struct stm32l4_rev stm32g05_g06xx_revs[] = {
 	{ 0x1000, "A" },
 };
@@ -372,6 +387,30 @@ static const struct stm32l4_part_info stm32l4_parts[] = {
 	  .otp_size              = 1024,
 	},
 	{
+	  .id                    = DEVID_STM32C01XX,
+	  .revs                  = stm32c01xx_revs,
+	  .num_revs              = ARRAY_SIZE(stm32c01xx_revs),
+	  .device_str            = "STM32C01xx",
+	  .max_flash_size_kb     = 32,
+	  .flags                 = F_NONE,
+	  .flash_regs_base       = 0x40022000,
+	  .fsize_addr            = 0x1FFF75A0,
+	  .otp_base              = 0x1FFF7000,
+	  .otp_size              = 1024,
+	},
+	{
+	  .id                    = DEVID_STM32C03XX,
+	  .revs                  = stm32c03xx_revs,
+	  .num_revs              = ARRAY_SIZE(stm32c03xx_revs),
+	  .device_str            = "STM32C03xx",
+	  .max_flash_size_kb     = 32,
+	  .flags                 = F_NONE,
+	  .flash_regs_base       = 0x40022000,
+	  .fsize_addr            = 0x1FFF75A0,
+	  .otp_base              = 0x1FFF7000,
+	  .otp_size              = 1024,
+	},
+	{
 	  .id                    = DEVID_STM32G05_G06XX,
 	  .revs                  = stm32g05_g06xx_revs,
 	  .num_revs              = ARRAY_SIZE(stm32g05_g06xx_revs),
@@ -1855,6 +1894,8 @@ static int stm32l4_probe(struct flash_bank *bank)
 		}
 		break;
 	case DEVID_STM32L43_L44XX:
+	case DEVID_STM32C01XX:
+	case DEVID_STM32C03XX:
 	case DEVID_STM32G05_G06XX:
 	case DEVID_STM32G07_G08XX:
 	case DEVID_STM32L45_L46XX:
diff --git a/src/flash/nor/stm32l4x.h b/src/flash/nor/stm32l4x.h
index 06cc66d..278038d 100644
--- a/src/flash/nor/stm32l4x.h
+++ b/src/flash/nor/stm32l4x.h
@@ -87,6 +87,8 @@
 /* Supported device IDs */
 #define DEVID_STM32L47_L48XX	0x415
 #define DEVID_STM32L43_L44XX	0x435
+#define DEVID_STM32C01XX		0x443
+#define DEVID_STM32C03XX		0x453
 #define DEVID_STM32G05_G06XX	0x456
 #define DEVID_STM32G07_G08XX	0x460
 #define DEVID_STM32L49_L4AXX	0x461
diff --git a/src/flash/nor/tcl.c b/src/flash/nor/tcl.c
index 7723a17..1c4e154 100644
--- a/src/flash/nor/tcl.c
+++ b/src/flash/nor/tcl.c
@@ -19,7 +19,7 @@
  * Implements Tcl commands used to access NOR flash facilities.
  */
 
-static COMMAND_HELPER(flash_command_get_bank_maybe_probe, unsigned name_index,
+COMMAND_HELPER(flash_command_get_bank_probe_optional, unsigned int name_index,
 	       struct flash_bank **bank, bool do_probe)
 {
 	const char *name = CMD_ARGV[name_index];
@@ -51,7 +51,7 @@ static COMMAND_HELPER(flash_command_get_bank_maybe_probe, unsigned name_index,
 COMMAND_HELPER(flash_command_get_bank, unsigned name_index,
 	struct flash_bank **bank)
 {
-	return CALL_COMMAND_HANDLER(flash_command_get_bank_maybe_probe,
+	return CALL_COMMAND_HANDLER(flash_command_get_bank_probe_optional,
 				    name_index, bank, true);
 }
 
@@ -157,7 +157,7 @@ COMMAND_HANDLER(handle_flash_probe_command)
 	if (CMD_ARGC != 1)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
-	retval = CALL_COMMAND_HANDLER(flash_command_get_bank_maybe_probe, 0, &p, false);
+	retval = CALL_COMMAND_HANDLER(flash_command_get_bank_probe_optional, 0, &p, false);
 	if (retval != ERROR_OK)
 		return retval;
 
diff --git a/src/hello.c b/src/hello.c
index 4a4ce01..4e27d4d 100644
--- a/src/hello.c
+++ b/src/hello.c
@@ -8,6 +8,7 @@
 #include <config.h>
 #endif
 #include <helper/log.h>
+#include "hello.h"
 
 COMMAND_HANDLER(handle_foo_command)
 {
diff --git a/src/helper/log.c b/src/helper/log.c
index e6a70a3..a4fc53d 100644
--- a/src/helper/log.c
+++ b/src/helper/log.c
@@ -19,6 +19,7 @@
 #include "command.h"
 #include "replacements.h"
 #include "time_support.h"
+#include <server/gdb_server.h>
 #include <server/server.h>
 
 #include <stdarg.h>
@@ -399,9 +400,7 @@ char *alloc_printf(const char *format, ...)
 
 static void gdb_timeout_warning(int64_t delta_time)
 {
-	extern int gdb_actual_connections;
-
-	if (gdb_actual_connections)
+	if (gdb_get_actual_connections())
 		LOG_WARNING("keep_alive() was not invoked in the "
 			"%d ms timelimit. GDB alive packet not "
 			"sent! (%" PRId64 " ms). Workaround: increase "
diff --git a/src/helper/replacements.c b/src/helper/replacements.c
index b30dbd5..782d975 100644
--- a/src/helper/replacements.c
+++ b/src/helper/replacements.c
@@ -10,10 +10,18 @@
  *   Copyright (C) 2008 by Spencer Oliver                                  *
  *   spen@spen-soft.co.uk                                                  *
  ***************************************************************************/
-/* DANGER!!!! These must be defined *BEFORE* replacements.h and the malloc() macro!!!! */
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+/* define IN_REPLACEMENTS_C before include replacements.h */
+#define IN_REPLACEMENTS_C
+#include "replacements.h"
 
 #include <stdlib.h>
 #include <string.h>
+
 /*
  * clear_malloc
  *
@@ -41,10 +49,6 @@ void *fill_malloc(size_t size)
 	return t;
 }
 
-#define IN_REPLACEMENTS_C
-#ifdef HAVE_CONFIG_H
-#include "config.h"
-#endif
 #ifdef HAVE_STRINGS_H
 #include <strings.h>
 #endif
diff --git a/src/helper/replacements.h b/src/helper/replacements.h
index 9eac4d2..6e30b62 100644
--- a/src/helper/replacements.h
+++ b/src/helper/replacements.h
@@ -66,12 +66,10 @@ int gettimeofday(struct timeval *tv, struct timezone *tz);
 
 #endif
 
-#ifndef IN_REPLACEMENTS_C
-/**** clear_malloc & fill_malloc ****/
 void *clear_malloc(size_t size);
 void *fill_malloc(size_t size);
-#endif
 
+#ifndef IN_REPLACEMENTS_C
 /*
  * Now you have 3 ways for the malloc function:
  *
@@ -100,6 +98,7 @@ void *fill_malloc(size_t size);
 
 /* #define malloc(_a) clear_malloc(_a)
  * #define malloc(_a) fill_malloc(_a) */
+#endif  /* IN_REPLACEMENTS_C */
 
 /* GNU extensions to the C library that may be missing on some systems */
 #ifndef HAVE_STRNDUP
diff --git a/src/helper/types.h b/src/helper/types.h
index 587ed22..53249e5 100644
--- a/src/helper/types.h
+++ b/src/helper/types.h
@@ -177,44 +177,44 @@ static inline void h_u64_to_be(uint8_t *buf, uint64_t val)
 
 static inline void h_u32_to_le(uint8_t *buf, uint32_t val)
 {
-	buf[3] = (uint8_t) (val >> 24);
-	buf[2] = (uint8_t) (val >> 16);
-	buf[1] = (uint8_t) (val >> 8);
-	buf[0] = (uint8_t) (val >> 0);
+	buf[3] = (val >> 24) & 0xff;
+	buf[2] = (val >> 16) & 0xff;
+	buf[1] = (val >> 8) & 0xff;
+	buf[0] = (val >> 0) & 0xff;
 }
 
 static inline void h_u32_to_be(uint8_t *buf, uint32_t val)
 {
-	buf[0] = (uint8_t) (val >> 24);
-	buf[1] = (uint8_t) (val >> 16);
-	buf[2] = (uint8_t) (val >> 8);
-	buf[3] = (uint8_t) (val >> 0);
+	buf[0] = (val >> 24) & 0xff;
+	buf[1] = (val >> 16) & 0xff;
+	buf[2] = (val >> 8) & 0xff;
+	buf[3] = (val >> 0) & 0xff;
 }
 
 static inline void h_u24_to_le(uint8_t *buf, unsigned int val)
 {
-	buf[2] = (uint8_t) (val >> 16);
-	buf[1] = (uint8_t) (val >> 8);
-	buf[0] = (uint8_t) (val >> 0);
+	buf[2] = (val >> 16) & 0xff;
+	buf[1] = (val >> 8) & 0xff;
+	buf[0] = (val >> 0) & 0xff;
 }
 
 static inline void h_u24_to_be(uint8_t *buf, unsigned int val)
 {
-	buf[0] = (uint8_t) (val >> 16);
-	buf[1] = (uint8_t) (val >> 8);
-	buf[2] = (uint8_t) (val >> 0);
+	buf[0] = (val >> 16) & 0xff;
+	buf[1] = (val >> 8) & 0xff;
+	buf[2] = (val >> 0) & 0xff;
 }
 
 static inline void h_u16_to_le(uint8_t *buf, uint16_t val)
 {
-	buf[1] = (uint8_t) (val >> 8);
-	buf[0] = (uint8_t) (val >> 0);
+	buf[1] = (val >> 8) & 0xff;
+	buf[0] = (val >> 0) & 0xff;
 }
 
 static inline void h_u16_to_be(uint8_t *buf, uint16_t val)
 {
-	buf[0] = (uint8_t) (val >> 8);
-	buf[1] = (uint8_t) (val >> 0);
+	buf[0] = (val >> 8) & 0xff;
+	buf[1] = (val >> 0) & 0xff;
 }
 
 /**
diff --git a/src/jtag/drivers/bcm2835gpio.c b/src/jtag/drivers/bcm2835gpio.c
index 879ca3c..39e4af3 100644
--- a/src/jtag/drivers/bcm2835gpio.c
+++ b/src/jtag/drivers/bcm2835gpio.c
@@ -19,7 +19,8 @@
 
 #include <sys/mman.h>
 
-static uint32_t bcm2835_peri_base = 0x20000000;
+static char *bcm2835_peri_mem_dev;
+static off_t bcm2835_peri_base = 0x20000000;
 #define BCM2835_GPIO_BASE	(bcm2835_peri_base + 0x200000) /* GPIO controller */
 
 #define BCM2835_PADS_GPIO_0_27		(bcm2835_peri_base + 0x100000)
@@ -57,6 +58,14 @@ static struct initial_gpio_state {
 } initial_gpio_state[ADAPTER_GPIO_IDX_NUM];
 static uint32_t initial_drive_strength_etc;
 
+static inline const char *bcm2835_get_mem_dev(void)
+{
+	if (bcm2835_peri_mem_dev)
+		return bcm2835_peri_mem_dev;
+
+	return "/dev/gpiomem";
+}
+
 static inline void bcm2835_gpio_synchronize(void)
 {
 	/* Ensure that previous writes to GPIO registers are flushed out of
@@ -300,13 +309,29 @@ COMMAND_HANDLER(bcm2835gpio_handle_speed_coeffs)
 	return ERROR_OK;
 }
 
+COMMAND_HANDLER(bcm2835gpio_handle_peripheral_mem_dev)
+{
+	if (CMD_ARGC == 1) {
+		free(bcm2835_peri_mem_dev);
+		bcm2835_peri_mem_dev = strdup(CMD_ARGV[0]);
+	}
+
+	command_print(CMD, "BCM2835 GPIO: peripheral_mem_dev = %s",
+				  bcm2835_get_mem_dev());
+	return ERROR_OK;
+}
+
 COMMAND_HANDLER(bcm2835gpio_handle_peripheral_base)
 {
-	if (CMD_ARGC == 1)
-		COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], bcm2835_peri_base);
+	uint64_t tmp_base;
+	if (CMD_ARGC == 1) {
+		COMMAND_PARSE_NUMBER(u64, CMD_ARGV[0], tmp_base);
+		bcm2835_peri_base = (off_t)tmp_base;
+	}
 
-	command_print(CMD, "BCM2835 GPIO: peripheral_base = 0x%08x",
-				  bcm2835_peri_base);
+	tmp_base = bcm2835_peri_base;
+	command_print(CMD, "BCM2835 GPIO: peripheral_base = 0x%08" PRIu64,
+				  tmp_base);
 	return ERROR_OK;
 }
 
@@ -319,10 +344,17 @@ static const struct command_registration bcm2835gpio_subcommand_handlers[] = {
 		.usage = "[SPEED_COEFF SPEED_OFFSET]",
 	},
 	{
+		.name = "peripheral_mem_dev",
+		.handler = &bcm2835gpio_handle_peripheral_mem_dev,
+		.mode = COMMAND_CONFIG,
+		.help = "device to map memory mapped GPIOs from.",
+		.usage = "[device]",
+	},
+	{
 		.name = "peripheral_base",
 		.handler = &bcm2835gpio_handle_peripheral_base,
 		.mode = COMMAND_CONFIG,
-		.help = "peripheral base to access GPIOs (RPi1 0x20000000, RPi2 0x3F000000).",
+		.help = "peripheral base to access GPIOs, not needed with /dev/gpiomem.",
 		.usage = "[base]",
 	},
 
@@ -409,13 +441,16 @@ static int bcm2835gpio_init(void)
 		return ERROR_JTAG_INIT_FAILED;
 	}
 
-	dev_mem_fd = open("/dev/gpiomem", O_RDWR | O_SYNC);
-	if (dev_mem_fd < 0) {
-		LOG_DEBUG("Cannot open /dev/gpiomem, fallback to /dev/mem");
-		dev_mem_fd = open("/dev/mem", O_RDWR | O_SYNC);
-	}
+	bool is_gpiomem = strcmp(bcm2835_get_mem_dev(), "/dev/gpiomem") == 0;
+	bool pad_mapping_possible = !is_gpiomem;
+
+	dev_mem_fd = open(bcm2835_get_mem_dev(), O_RDWR | O_SYNC);
 	if (dev_mem_fd < 0) {
-		LOG_ERROR("open: %s", strerror(errno));
+		LOG_ERROR("open %s: %s", bcm2835_get_mem_dev(), strerror(errno));
+		/* TODO: add /dev/mem specific doc and refer to it
+		 * if (!is_gpiomem && (errno == EACCES || errno == EPERM))
+		 *	LOG_INFO("Consult the user's guide chapter 4.? how to set permissions and capabilities");
+		 */
 		return ERROR_JTAG_INIT_FAILED;
 	}
 
@@ -428,21 +463,28 @@ static int bcm2835gpio_init(void)
 		return ERROR_JTAG_INIT_FAILED;
 	}
 
-	pads_base = mmap(NULL, sysconf(_SC_PAGE_SIZE), PROT_READ | PROT_WRITE,
+	/* TODO: move pads config to a separate utility */
+	if (pad_mapping_possible) {
+		pads_base = mmap(NULL, sysconf(_SC_PAGE_SIZE), PROT_READ | PROT_WRITE,
 				MAP_SHARED, dev_mem_fd, BCM2835_PADS_GPIO_0_27);
 
-	if (pads_base == MAP_FAILED) {
-		LOG_ERROR("mmap: %s", strerror(errno));
-		bcm2835gpio_munmap();
-		close(dev_mem_fd);
-		return ERROR_JTAG_INIT_FAILED;
+		if (pads_base == MAP_FAILED) {
+			LOG_ERROR("mmap pads: %s", strerror(errno));
+			LOG_WARNING("Continuing with unchanged GPIO pad settings (drive strength and slew rate)");
+		}
+	} else {
+		pads_base = MAP_FAILED;
 	}
 
 	close(dev_mem_fd);
 
-	/* set 4mA drive strength, slew rate limited, hysteresis on */
-	initial_drive_strength_etc = pads_base[BCM2835_PADS_GPIO_0_27_OFFSET] & 0x1f;
-	pads_base[BCM2835_PADS_GPIO_0_27_OFFSET] = 0x5a000008 + 1;
+	if (pads_base != MAP_FAILED) {
+		/* set 4mA drive strength, slew rate limited, hysteresis on */
+		initial_drive_strength_etc = pads_base[BCM2835_PADS_GPIO_0_27_OFFSET] & 0x1f;
+LOG_INFO("initial pads conf %08x", pads_base[BCM2835_PADS_GPIO_0_27_OFFSET]);
+		pads_base[BCM2835_PADS_GPIO_0_27_OFFSET] = 0x5a000008 + 1;
+LOG_INFO("pads conf set to %08x", pads_base[BCM2835_PADS_GPIO_0_27_OFFSET]);
+	}
 
 	/* Configure JTAG/SWD signals. Default directions and initial states are handled
 	 * by adapter.c and "adapter gpio" command.
@@ -513,9 +555,12 @@ static int bcm2835gpio_quit(void)
 	restore_gpio(ADAPTER_GPIO_IDX_SRST);
 	restore_gpio(ADAPTER_GPIO_IDX_LED);
 
-	/* Restore drive strength. MSB is password ("5A") */
-	pads_base[BCM2835_PADS_GPIO_0_27_OFFSET] = 0x5A000000 | initial_drive_strength_etc;
+	if (pads_base != MAP_FAILED) {
+		/* Restore drive strength. MSB is password ("5A") */
+		pads_base[BCM2835_PADS_GPIO_0_27_OFFSET] = 0x5A000000 | initial_drive_strength_etc;
+	}
 	bcm2835gpio_munmap();
+	free(bcm2835_peri_mem_dev);
 
 	return ERROR_OK;
 }
diff --git a/src/jtag/drivers/vdebug.c b/src/jtag/drivers/vdebug.c
index 9223be2..f6d99c6 100644
--- a/src/jtag/drivers/vdebug.c
+++ b/src/jtag/drivers/vdebug.c
@@ -53,7 +53,7 @@
 #include "helper/log.h"
 #include "helper/list.h"
 
-#define VD_VERSION 44
+#define VD_VERSION 46
 #define VD_BUFFER_LEN 4024
 #define VD_CHEADER_LEN 24
 #define VD_SHEADER_LEN 16
@@ -272,7 +272,7 @@ static int vdebug_socket_open(char *server_addr, uint32_t port)
 		LOG_ERROR("socket_open: cannot resolve address %s, error %d", server_addr, vdebug_socket_error());
 		rc = VD_ERR_SOC_ADDR;
 	} else {
-		buf_set_u32((uint8_t *)ainfo->ai_addr->sa_data, 0, 16, htons(port));
+		h_u16_to_be((uint8_t *)ainfo->ai_addr->sa_data, port);
 		if (connect(hsock, ainfo->ai_addr, sizeof(struct sockaddr)) < 0) {
 			LOG_ERROR("socket_open: cannot connect to %s:%d, error %d", server_addr, port, vdebug_socket_error());
 			rc = VD_ERR_SOC_CONN;
@@ -942,10 +942,10 @@ static int vdebug_jtag_tlr(tap_state_t state, uint8_t f_flush)
 {
 	int rc = ERROR_OK;
 
-	uint8_t cur = tap_get_state();
+	tap_state_t cur = tap_get_state();
 	uint8_t tms_pre = tap_get_tms_path(cur, state);
 	uint8_t num_pre = tap_get_tms_path_len(cur, state);
-	LOG_INFO("tlr  from %" PRIx8 " to %" PRIx8, cur, state);
+	LOG_INFO("tlr  from %x to %x", cur, state);
 	if (cur != state) {
 		rc = vdebug_jtag_shift_tap(vdc.hsocket, pbuf, num_pre, tms_pre, 0, NULL, 0, 0, NULL, f_flush);
 		tap_set_state(state);
@@ -958,7 +958,7 @@ static int vdebug_jtag_scan(struct scan_command *cmd, uint8_t f_flush)
 {
 	int rc = ERROR_OK;
 
-	uint8_t cur = tap_get_state();
+	tap_state_t cur = tap_get_state();
 	uint8_t state = cmd->ir_scan ? TAP_IRSHIFT : TAP_DRSHIFT;
 	uint8_t tms_pre = tap_get_tms_path(cur, state);
 	uint8_t num_pre = tap_get_tms_path_len(cur, state);
@@ -988,7 +988,7 @@ static int vdebug_jtag_scan(struct scan_command *cmd, uint8_t f_flush)
 
 static int vdebug_jtag_runtest(int cycles, tap_state_t state, uint8_t f_flush)
 {
-	uint8_t cur = tap_get_state();
+	tap_state_t cur = tap_get_state();
 	uint8_t tms_pre = tap_get_tms_path(cur, state);
 	uint8_t num_pre = tap_get_tms_path_len(cur, state);
 	LOG_DEBUG("idle len:%d state cur:%x end:%x", cycles, cur, state);
@@ -1125,7 +1125,7 @@ static int vdebug_dap_queue_ap_abort(struct adiv5_dap *dap, uint8_t *ack)
 
 static int vdebug_dap_run(struct adiv5_dap *dap)
 {
-	if (pbuf->waddr)
+	if (le_to_h_u16(pbuf->waddr))
 		return vdebug_run_reg_queue(vdc.hsocket, pbuf, le_to_h_u16(pbuf->waddr));
 
 	return ERROR_OK;
diff --git a/src/jtag/hla/hla_transport.c b/src/jtag/hla/hla_transport.c
index 004e9f0..08ee18f 100644
--- a/src/jtag/hla/hla_transport.c
+++ b/src/jtag/hla/hla_transport.c
@@ -37,8 +37,15 @@ static const struct command_registration hl_swd_transport_subcommand_handlers[]
 	{
 	 .name = "newdap",
 	 .mode = COMMAND_CONFIG,
-	 .jim_handler = jim_jtag_newtap,
+	 .handler = handle_jtag_newtap,
 	 .help = "declare a new SWD DAP",
+	 .usage = "basename dap_type ['-irlen' count] "
+			"['-enable'|'-disable'] "
+			"['-expected_id' number] "
+			"['-ignore-version'] "
+			"['-ignore-bypass'] "
+			"['-ircapture' number] "
+			"['-mask' number]",
 	 },
 	COMMAND_REGISTRATION_DONE
 };
@@ -58,11 +65,16 @@ static const struct command_registration hl_transport_jtag_subcommand_handlers[]
 	{
 	 .name = "newtap",
 	 .mode = COMMAND_CONFIG,
-	 .jim_handler = jim_jtag_newtap,
+	 .handler = handle_jtag_newtap,
 	 .help = "Create a new TAP instance named basename.tap_type, "
 	 "and appends it to the scan chain.",
 	 .usage = "basename tap_type '-irlen' count "
-	 "['-expected_id' number]",
+			"['-enable'|'-disable'] "
+			"['-expected_id' number] "
+			"['-ignore-version'] "
+			"['-ignore-bypass'] "
+			"['-ircapture' number] "
+			"['-mask' number]",
 	 },
 	{
 	 .name = "init",
@@ -85,12 +97,18 @@ static const struct command_registration hl_transport_jtag_subcommand_handlers[]
 	{
 	 .name = "tapisenabled",
 	 .mode = COMMAND_EXEC,
-	 .jim_handler = jim_jtag_tap_enabler,
+	 .handler = handle_jtag_tap_enabler,
+	 .help = "Returns a Tcl boolean (0/1) indicating whether "
+		"the TAP is enabled (1) or not (0).",
+	 .usage = "tap_name",
 	 },
 	{
 	 .name = "tapenable",
 	 .mode = COMMAND_EXEC,
-	 .jim_handler = jim_jtag_tap_enabler,
+	 .handler = handle_jtag_tap_enabler,
+	 .help = "Try to enable the specified TAP using the "
+		"'tap-enable' TAP event.",
+	 .usage = "tap_name",
 	 },
 	{
 	 .name = "tapdisable",
diff --git a/src/jtag/interface.c b/src/jtag/interface.c
index bc9ff3e..1230bb1 100644
--- a/src/jtag/interface.c
+++ b/src/jtag/interface.c
@@ -372,7 +372,7 @@ tap_state_t tap_state_by_name(const char *name)
 	tap_state_name(a), tap_state_name(b), astr, bstr)
 
 tap_state_t jtag_debug_state_machine_(const void *tms_buf, const void *tdi_buf,
-	unsigned tap_bits, tap_state_t next_state)
+	unsigned int tap_bits, tap_state_t next_state)
 {
 	const uint8_t *tms_buffer;
 	const uint8_t *tdi_buffer;
diff --git a/src/jtag/interface.h b/src/jtag/interface.h
index a8d9ee4..5004493 100644
--- a/src/jtag/interface.h
+++ b/src/jtag/interface.h
@@ -147,6 +147,8 @@ void tap_use_new_tms_table(bool use_new);
 /** @returns True if new TMS table is active; false otherwise. */
 bool tap_uses_new_tms_table(void);
 
+tap_state_t jtag_debug_state_machine_(const void *tms_buf, const void *tdi_buf,
+		unsigned int tap_len, tap_state_t start_tap_state);
 
 /**
  * @brief Prints verbose TAP state transitions for the given TMS/TDI buffers.
@@ -159,10 +161,6 @@ bool tap_uses_new_tms_table(void);
 static inline tap_state_t jtag_debug_state_machine(const void *tms_buf,
 		const void *tdi_buf, unsigned tap_len, tap_state_t start_tap_state)
 {
-	/* Private declaration */
-	tap_state_t jtag_debug_state_machine_(const void *tms_buf, const void *tdi_buf,
-			unsigned tap_len, tap_state_t start_tap_state);
-
 	if (LOG_LEVEL_IS(LOG_LVL_DEBUG_IO))
 		return jtag_debug_state_machine_(tms_buf, tdi_buf, tap_len, start_tap_state);
 	else
@@ -364,4 +362,42 @@ int adapter_config_trace(bool enabled, enum tpiu_pin_protocol pin_protocol,
 		unsigned int traceclkin_freq, uint16_t *prescaler);
 int adapter_poll_trace(uint8_t *buf, size_t *size);
 
+extern struct adapter_driver am335xgpio_adapter_driver;
+extern struct adapter_driver amt_jtagaccel_adapter_driver;
+extern struct adapter_driver armjtagew_adapter_driver;
+extern struct adapter_driver at91rm9200_adapter_driver;
+extern struct adapter_driver bcm2835gpio_adapter_driver;
+extern struct adapter_driver buspirate_adapter_driver;
+extern struct adapter_driver cmsis_dap_adapter_driver;
+extern struct adapter_driver dummy_adapter_driver;
+extern struct adapter_driver ep93xx_adapter_driver;
+extern struct adapter_driver esp_usb_adapter_driver;
+extern struct adapter_driver ft232r_adapter_driver;
+extern struct adapter_driver ftdi_adapter_driver;
+extern struct adapter_driver gw16012_adapter_driver;
+extern struct adapter_driver hl_adapter_driver;
+extern struct adapter_driver imx_gpio_adapter_driver;
+extern struct adapter_driver jlink_adapter_driver;
+extern struct adapter_driver jtag_dpi_adapter_driver;
+extern struct adapter_driver jtag_vpi_adapter_driver;
+extern struct adapter_driver kitprog_adapter_driver;
+extern struct adapter_driver linuxgpiod_adapter_driver;
+extern struct adapter_driver opendous_adapter_driver;
+extern struct adapter_driver openjtag_adapter_driver;
+extern struct adapter_driver osbdm_adapter_driver;
+extern struct adapter_driver parport_adapter_driver;
+extern struct adapter_driver presto_adapter_driver;
+extern struct adapter_driver remote_bitbang_adapter_driver;
+extern struct adapter_driver rlink_adapter_driver;
+extern struct adapter_driver rshim_dap_adapter_driver;
+extern struct adapter_driver stlink_dap_adapter_driver;
+extern struct adapter_driver sysfsgpio_adapter_driver;
+extern struct adapter_driver ulink_adapter_driver;
+extern struct adapter_driver usb_blaster_adapter_driver;
+extern struct adapter_driver usbprog_adapter_driver;
+extern struct adapter_driver vdebug_adapter_driver;
+extern struct adapter_driver vsllink_adapter_driver;
+extern struct adapter_driver xds110_adapter_driver;
+extern struct adapter_driver xlnx_pcie_xvc_adapter_driver;
+
 #endif /* OPENOCD_JTAG_INTERFACE_H */
diff --git a/src/jtag/interfaces.c b/src/jtag/interfaces.c
index 12848be..48a194f 100644
--- a/src/jtag/interfaces.c
+++ b/src/jtag/interfaces.c
@@ -24,125 +24,13 @@
 #include "interfaces.h"
 
 /** @file
- * This file includes declarations for all built-in jtag interfaces,
- * which are then listed in the adapter_drivers array.
+ * This file collects all the built-in JTAG interfaces in the adapter_drivers
+ * array.
  *
  * Dynamic loading can be implemented be searching for shared libraries
  * that contain an adapter_driver structure that can added to this list.
  */
 
-#if BUILD_PARPORT == 1
-extern struct adapter_driver parport_adapter_driver;
-#endif
-#if BUILD_DUMMY == 1
-extern struct adapter_driver dummy_adapter_driver;
-#endif
-#if BUILD_FTDI == 1
-extern struct adapter_driver ftdi_adapter_driver;
-#endif
-#if BUILD_USB_BLASTER == 1 || BUILD_USB_BLASTER_2 == 1
-extern struct adapter_driver usb_blaster_adapter_driver;
-#endif
-#if BUILD_ESP_USB_JTAG == 1
-extern struct adapter_driver esp_usb_adapter_driver;
-#endif
-#if BUILD_JTAG_VPI == 1
-extern struct adapter_driver jtag_vpi_adapter_driver;
-#endif
-#if BUILD_VDEBUG == 1
-extern struct adapter_driver vdebug_adapter_driver;
-#endif
-#if BUILD_JTAG_DPI == 1
-extern struct adapter_driver jtag_dpi_adapter_driver;
-#endif
-#if BUILD_FT232R == 1
-extern struct adapter_driver ft232r_adapter_driver;
-#endif
-#if BUILD_AMTJTAGACCEL == 1
-extern struct adapter_driver amt_jtagaccel_adapter_driver;
-#endif
-#if BUILD_EP93XX == 1
-extern struct adapter_driver ep93xx_adapter_driver;
-#endif
-#if BUILD_AT91RM9200 == 1
-extern struct adapter_driver at91rm9200_adapter_driver;
-#endif
-#if BUILD_GW16012 == 1
-extern struct adapter_driver gw16012_adapter_driver;
-#endif
-#if BUILD_PRESTO
-extern struct adapter_driver presto_adapter_driver;
-#endif
-#if BUILD_USBPROG == 1
-extern struct adapter_driver usbprog_adapter_driver;
-#endif
-#if BUILD_OPENJTAG == 1
-extern struct adapter_driver openjtag_adapter_driver;
-#endif
-#if BUILD_JLINK == 1
-extern struct adapter_driver jlink_adapter_driver;
-#endif
-#if BUILD_VSLLINK == 1
-extern struct adapter_driver vsllink_adapter_driver;
-#endif
-#if BUILD_RLINK == 1
-extern struct adapter_driver rlink_adapter_driver;
-#endif
-#if BUILD_ULINK == 1
-extern struct adapter_driver ulink_adapter_driver;
-#endif
-#if BUILD_ARMJTAGEW == 1
-extern struct adapter_driver armjtagew_adapter_driver;
-#endif
-#if BUILD_BUSPIRATE == 1
-extern struct adapter_driver buspirate_adapter_driver;
-#endif
-#if BUILD_REMOTE_BITBANG == 1
-extern struct adapter_driver remote_bitbang_adapter_driver;
-#endif
-#if BUILD_HLADAPTER == 1
-extern struct adapter_driver hl_adapter_driver;
-#endif
-#if BUILD_OSBDM == 1
-extern struct adapter_driver osbdm_adapter_driver;
-#endif
-#if BUILD_OPENDOUS == 1
-extern struct adapter_driver opendous_adapter_driver;
-#endif
-#if BUILD_SYSFSGPIO == 1
-extern struct adapter_driver sysfsgpio_adapter_driver;
-#endif
-#if BUILD_LINUXGPIOD == 1
-extern struct adapter_driver linuxgpiod_adapter_driver;
-#endif
-#if BUILD_XLNX_PCIE_XVC == 1
-extern struct adapter_driver xlnx_pcie_xvc_adapter_driver;
-#endif
-#if BUILD_BCM2835GPIO == 1
-extern struct adapter_driver bcm2835gpio_adapter_driver;
-#endif
-#if BUILD_CMSIS_DAP_USB == 1 || BUILD_CMSIS_DAP_HID == 1
-extern struct adapter_driver cmsis_dap_adapter_driver;
-#endif
-#if BUILD_KITPROG == 1
-extern struct adapter_driver kitprog_adapter_driver;
-#endif
-#if BUILD_IMX_GPIO == 1
-extern struct adapter_driver imx_gpio_adapter_driver;
-#endif
-#if BUILD_XDS110 == 1
-extern struct adapter_driver xds110_adapter_driver;
-#endif
-#if BUILD_HLADAPTER_STLINK == 1
-extern struct adapter_driver stlink_dap_adapter_driver;
-#endif
-#if BUILD_RSHIM == 1
-extern struct adapter_driver rshim_dap_adapter_driver;
-#endif
-#if BUILD_AM335XGPIO == 1
-extern struct adapter_driver am335xgpio_adapter_driver;
-#endif
-
 /**
  * The list of built-in JTAG interfaces, containing entries for those
  * drivers that were enabled by the @c configure script.
diff --git a/src/jtag/jtag.h b/src/jtag/jtag.h
index 4f94e99..04d1b4a 100644
--- a/src/jtag/jtag.h
+++ b/src/jtag/jtag.h
@@ -12,6 +12,7 @@
 #define OPENOCD_JTAG_JTAG_H
 
 #include <helper/binarybuffer.h>
+#include <helper/command.h>
 #include <helper/log.h>
 #include <helper/replacements.h>
 
@@ -602,6 +603,6 @@ void jtag_poll_unmask(bool saved);
 
 #include <jtag/minidriver.h>
 
-int jim_jtag_newtap(Jim_Interp *interp, int argc, Jim_Obj *const *argv);
+__COMMAND_HANDLER(handle_jtag_newtap);
 
 #endif /* OPENOCD_JTAG_JTAG_H */
diff --git a/src/jtag/tcl.c b/src/jtag/tcl.c
index 934b603..85a66aa 100644
--- a/src/jtag/tcl.c
+++ b/src/jtag/tcl.c
@@ -31,6 +31,8 @@
 #include <strings.h>
 #endif
 
+#include <helper/command.h>
+#include <helper/nvp.h>
 #include <helper/time_support.h>
 #include "transport/transport.h"
 
@@ -376,39 +378,6 @@ static int jtag_tap_configure_cmd(struct jim_getopt_info *goi, struct jtag_tap *
 	return JIM_OK;
 }
 
-static int is_bad_irval(int ir_length, jim_wide w)
-{
-	jim_wide v = 1;
-
-	v <<= ir_length;
-	v -= 1;
-	v = ~v;
-	return (w & v) != 0;
-}
-
-static int jim_newtap_expected_id(struct jim_nvp *n, struct jim_getopt_info *goi,
-	struct jtag_tap *tap)
-{
-	jim_wide w;
-	int e = jim_getopt_wide(goi, &w);
-	if (e != JIM_OK) {
-		Jim_SetResultFormatted(goi->interp, "option: %s bad parameter", n->name);
-		return e;
-	}
-
-	uint32_t *p = realloc(tap->expected_ids,
-			      (tap->expected_ids_cnt + 1) * sizeof(uint32_t));
-	if (!p) {
-		Jim_SetResultFormatted(goi->interp, "no memory");
-		return JIM_ERR;
-	}
-
-	tap->expected_ids = p;
-	tap->expected_ids[tap->expected_ids_cnt++] = w;
-
-	return JIM_OK;
-}
-
 #define NTAP_OPT_IRLEN     0
 #define NTAP_OPT_IRMASK    1
 #define NTAP_OPT_IRCAPTURE 2
@@ -418,166 +387,155 @@ static int jim_newtap_expected_id(struct jim_nvp *n, struct jim_getopt_info *goi
 #define NTAP_OPT_VERSION   6
 #define NTAP_OPT_BYPASS    7
 
-static int jim_newtap_ir_param(struct jim_nvp *n, struct jim_getopt_info *goi,
-	struct jtag_tap *tap)
-{
-	jim_wide w;
-	int e = jim_getopt_wide(goi, &w);
-	if (e != JIM_OK) {
-		Jim_SetResultFormatted(goi->interp,
-			"option: %s bad parameter", n->name);
-		return e;
-	}
-	switch (n->value) {
-	    case NTAP_OPT_IRLEN:
-		    if (w > (jim_wide) (8 * sizeof(tap->ir_capture_value))) {
-			    LOG_WARNING("%s: huge IR length %d",
-				    tap->dotted_name, (int) w);
-		    }
-		    tap->ir_length = w;
-		    break;
-	    case NTAP_OPT_IRMASK:
-		    if (is_bad_irval(tap->ir_length, w)) {
-			    LOG_ERROR("%s: IR mask %x too big",
-				    tap->dotted_name,
-				    (int) w);
-			    return JIM_ERR;
-		    }
-		    if ((w & 3) != 3)
-			    LOG_WARNING("%s: nonstandard IR mask", tap->dotted_name);
-		    tap->ir_capture_mask = w;
-		    break;
-	    case NTAP_OPT_IRCAPTURE:
-		    if (is_bad_irval(tap->ir_length, w)) {
-			    LOG_ERROR("%s: IR capture %x too big",
-				    tap->dotted_name, (int) w);
-			    return JIM_ERR;
-		    }
-		    if ((w & 3) != 1)
-			    LOG_WARNING("%s: nonstandard IR value",
-				    tap->dotted_name);
-		    tap->ir_capture_value = w;
-		    break;
-	    default:
-		    return JIM_ERR;
-	}
-	return JIM_OK;
-}
+static const struct nvp jtag_newtap_opts[] = {
+	{ .name = "-irlen",          .value = NTAP_OPT_IRLEN },
+	{ .name = "-irmask",         .value = NTAP_OPT_IRMASK },
+	{ .name = "-ircapture",      .value = NTAP_OPT_IRCAPTURE },
+	{ .name = "-enable",         .value = NTAP_OPT_ENABLED },
+	{ .name = "-disable",        .value = NTAP_OPT_DISABLED },
+	{ .name = "-expected-id",    .value = NTAP_OPT_EXPECTED_ID },
+	{ .name = "-ignore-version", .value = NTAP_OPT_VERSION },
+	{ .name = "-ignore-bypass",  .value = NTAP_OPT_BYPASS },
+	{ .name = NULL,              .value = -1 },
+};
 
-static int jim_newtap_cmd(struct jim_getopt_info *goi)
+static COMMAND_HELPER(handle_jtag_newtap_args, struct jtag_tap *tap)
 {
-	struct jtag_tap *tap;
-	int x;
-	int e;
-	struct jim_nvp *n;
-	char *cp;
-	const struct jim_nvp opts[] = {
-		{ .name = "-irlen",       .value = NTAP_OPT_IRLEN },
-		{ .name = "-irmask",       .value = NTAP_OPT_IRMASK },
-		{ .name = "-ircapture",       .value = NTAP_OPT_IRCAPTURE },
-		{ .name = "-enable",       .value = NTAP_OPT_ENABLED },
-		{ .name = "-disable",       .value = NTAP_OPT_DISABLED },
-		{ .name = "-expected-id",       .value = NTAP_OPT_EXPECTED_ID },
-		{ .name = "-ignore-version",       .value = NTAP_OPT_VERSION },
-		{ .name = "-ignore-bypass",       .value = NTAP_OPT_BYPASS },
-		{ .name = NULL,       .value = -1 },
-	};
-
-	tap = calloc(1, sizeof(struct jtag_tap));
-	if (!tap) {
-		Jim_SetResultFormatted(goi->interp, "no memory");
-		return JIM_ERR;
-	}
+	/* we expect CHIP + TAP + OPTIONS */
+	if (CMD_ARGC < 2)
+		return ERROR_COMMAND_SYNTAX_ERROR;
 
-	/*
-	 * we expect CHIP + TAP + OPTIONS
-	 * */
-	if (goi->argc < 3) {
-		Jim_SetResultFormatted(goi->interp, "Missing CHIP TAP OPTIONS ....");
-		free(tap);
-		return JIM_ERR;
+	tap->chip = strdup(CMD_ARGV[0]);
+	tap->tapname = strdup(CMD_ARGV[1]);
+	tap->dotted_name = alloc_printf("%s.%s", CMD_ARGV[0], CMD_ARGV[1]);
+	if (!tap->chip || !tap->tapname || !tap->dotted_name) {
+		LOG_ERROR("Out of memory");
+		return ERROR_FAIL;
 	}
-
-	const char *tmp;
-	jim_getopt_string(goi, &tmp, NULL);
-	tap->chip = strdup(tmp);
-
-	jim_getopt_string(goi, &tmp, NULL);
-	tap->tapname = strdup(tmp);
-
-	/* name + dot + name + null */
-	x = strlen(tap->chip) + 1 + strlen(tap->tapname) + 1;
-	cp = malloc(x);
-	sprintf(cp, "%s.%s", tap->chip, tap->tapname);
-	tap->dotted_name = cp;
+	CMD_ARGC -= 2;
+	CMD_ARGV += 2;
 
 	LOG_DEBUG("Creating New Tap, Chip: %s, Tap: %s, Dotted: %s, %d params",
-		tap->chip, tap->tapname, tap->dotted_name, goi->argc);
+		  tap->chip, tap->tapname, tap->dotted_name, CMD_ARGC);
 
-	/* IEEE specifies that the two LSBs of an IR scan are 01, so make
+	/*
+	 * IEEE specifies that the two LSBs of an IR scan are 01, so make
 	 * that the default.  The "-ircapture" and "-irmask" options are only
 	 * needed to cope with nonstandard TAPs, or to specify more bits.
 	 */
 	tap->ir_capture_mask = 0x03;
 	tap->ir_capture_value = 0x01;
 
-	while (goi->argc) {
-		e = jim_getopt_nvp(goi, opts, &n);
-		if (e != JIM_OK) {
-			jim_getopt_nvp_unknown(goi, opts, 0);
-			free(cp);
-			free(tap);
-			return e;
-		}
-		LOG_DEBUG("Processing option: %s", n->name);
+	while (CMD_ARGC) {
+		const struct nvp *n = nvp_name2value(jtag_newtap_opts, CMD_ARGV[0]);
+		CMD_ARGC--;
+		CMD_ARGV++;
 		switch (n->value) {
-		    case NTAP_OPT_ENABLED:
-			    tap->disabled_after_reset = false;
-			    break;
-		    case NTAP_OPT_DISABLED:
-			    tap->disabled_after_reset = true;
-			    break;
-		    case NTAP_OPT_EXPECTED_ID:
-			    e = jim_newtap_expected_id(n, goi, tap);
-			    if (e != JIM_OK) {
-				    free(cp);
-				    free(tap);
-				    return e;
-			    }
-			    break;
-		    case NTAP_OPT_IRLEN:
-		    case NTAP_OPT_IRMASK:
-		    case NTAP_OPT_IRCAPTURE:
-			    e = jim_newtap_ir_param(n, goi, tap);
-			    if (e != JIM_OK) {
-				    free(cp);
-				    free(tap);
-				    return e;
-			    }
-			    break;
-		    case NTAP_OPT_VERSION:
-			    tap->ignore_version = true;
-			    break;
-		    case NTAP_OPT_BYPASS:
-			    tap->ignore_bypass = true;
-			    break;
-		}	/* switch (n->value) */
-	}	/* while (goi->argc) */
+	    case NTAP_OPT_ENABLED:
+		    tap->disabled_after_reset = false;
+		    break;
+
+	    case NTAP_OPT_DISABLED:
+		    tap->disabled_after_reset = true;
+		    break;
+
+		case NTAP_OPT_EXPECTED_ID:
+			if (!CMD_ARGC)
+				return ERROR_COMMAND_ARGUMENT_INVALID;
+
+			tap->expected_ids = realloc(tap->expected_ids,
+										(tap->expected_ids_cnt + 1) * sizeof(uint32_t));
+			if (!tap->expected_ids) {
+				LOG_ERROR("Out of memory");
+				return ERROR_FAIL;
+			}
+
+			uint32_t id;
+			COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], id);
+			CMD_ARGC--;
+			CMD_ARGV++;
+			tap->expected_ids[tap->expected_ids_cnt++] = id;
+
+			break;
+
+		case NTAP_OPT_IRLEN:
+			if (!CMD_ARGC)
+				return ERROR_COMMAND_ARGUMENT_INVALID;
+
+			COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], tap->ir_length);
+			CMD_ARGC--;
+			CMD_ARGV++;
+			if (tap->ir_length > (int)(8 * sizeof(tap->ir_capture_value)))
+				LOG_WARNING("%s: huge IR length %d", tap->dotted_name, tap->ir_length);
+			break;
+
+		case NTAP_OPT_IRMASK:
+			if (!CMD_ARGC)
+				return ERROR_COMMAND_ARGUMENT_INVALID;
+
+			COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], tap->ir_capture_mask);
+			CMD_ARGC--;
+			CMD_ARGV++;
+			if ((tap->ir_capture_mask & 3) != 3)
+				LOG_WARNING("%s: nonstandard IR mask", tap->dotted_name);
+			break;
+
+		case NTAP_OPT_IRCAPTURE:
+			if (!CMD_ARGC)
+				return ERROR_COMMAND_ARGUMENT_INVALID;
+
+			COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], tap->ir_capture_value);
+			CMD_ARGC--;
+			CMD_ARGV++;
+			if ((tap->ir_capture_value & 3) != 1)
+				LOG_WARNING("%s: nonstandard IR value", tap->dotted_name);
+			break;
+
+		case NTAP_OPT_VERSION:
+			tap->ignore_version = true;
+			break;
+
+		case NTAP_OPT_BYPASS:
+			tap->ignore_bypass = true;
+			break;
+
+		default:
+			nvp_unknown_command_print(CMD, jtag_newtap_opts, NULL, CMD_ARGV[-1]);
+			return ERROR_COMMAND_ARGUMENT_INVALID;
+		}
+	}
 
 	/* default is enabled-after-reset */
 	tap->enabled = !tap->disabled_after_reset;
 
-	/* Did all the required option bits get cleared? */
-	if (!transport_is_jtag() || tap->ir_length != 0) {
-		jtag_tap_init(tap);
-		return JIM_OK;
+	if (transport_is_jtag() && tap->ir_length == 0) {
+		command_print(CMD, "newtap: %s missing IR length", tap->dotted_name);
+		return ERROR_COMMAND_ARGUMENT_INVALID;
 	}
 
-	Jim_SetResultFormatted(goi->interp,
-		"newtap: %s missing IR length",
-		tap->dotted_name);
-	jtag_tap_free(tap);
-	return JIM_ERR;
+	return ERROR_OK;
+}
+
+__COMMAND_HANDLER(handle_jtag_newtap)
+{
+	struct jtag_tap *tap = calloc(1, sizeof(struct jtag_tap));
+	if (!tap) {
+		LOG_ERROR("Out of memory");
+		return ERROR_FAIL;
+	}
+
+	int retval = CALL_COMMAND_HANDLER(handle_jtag_newtap_args, tap);
+	if (retval != ERROR_OK) {
+		free(tap->chip);
+		free(tap->tapname);
+		free(tap->dotted_name);
+		free(tap->expected_ids);
+		free(tap);
+		return retval;
+	}
+
+	jtag_tap_init(tap);
+	return ERROR_OK;
 }
 
 static void jtag_tap_handle_event(struct jtag_tap *tap, enum jtag_event e)
@@ -643,17 +601,10 @@ COMMAND_HANDLER(handle_jtag_arp_init_reset)
 	return ERROR_OK;
 }
 
-int jim_jtag_newtap(Jim_Interp *interp, int argc, Jim_Obj *const *argv)
-{
-	struct jim_getopt_info goi;
-	jim_getopt_setup(&goi, interp, argc-1, argv + 1);
-	return jim_newtap_cmd(&goi);
-}
-
 static bool jtag_tap_enable(struct jtag_tap *t)
 {
 	if (t->enabled)
-		return false;
+		return true;
 	jtag_tap_handle_event(t, JTAG_TAP_EVENT_ENABLE);
 	if (!t->enabled)
 		return false;
@@ -668,7 +619,7 @@ static bool jtag_tap_enable(struct jtag_tap *t)
 static bool jtag_tap_disable(struct jtag_tap *t)
 {
 	if (!t->enabled)
-		return false;
+		return true;
 	jtag_tap_handle_event(t, JTAG_TAP_EVENT_DISABLE);
 	if (t->enabled)
 		return false;
@@ -681,42 +632,36 @@ static bool jtag_tap_disable(struct jtag_tap *t)
 	return true;
 }
 
-int jim_jtag_tap_enabler(Jim_Interp *interp, int argc, Jim_Obj *const *argv)
+__COMMAND_HANDLER(handle_jtag_tap_enabler)
 {
-	struct command *c = jim_to_command(interp);
-	const char *cmd_name = c->name;
-	struct jim_getopt_info goi;
-	jim_getopt_setup(&goi, interp, argc-1, argv + 1);
-	if (goi.argc != 1) {
-		Jim_SetResultFormatted(goi.interp, "usage: %s <name>", cmd_name);
-		return JIM_ERR;
-	}
-
-	struct jtag_tap *t;
+	if (CMD_ARGC != 1)
+		return ERROR_COMMAND_SYNTAX_ERROR;
 
-	t = jtag_tap_by_jim_obj(goi.interp, goi.argv[0]);
-	if (!t)
-		return JIM_ERR;
+	struct jtag_tap *t = jtag_tap_by_string(CMD_ARGV[0]);
+	if (!t) {
+		command_print(CMD, "Tap '%s' could not be found", CMD_ARGV[0]);
+		return ERROR_COMMAND_ARGUMENT_INVALID;
+	}
 
-	if (strcasecmp(cmd_name, "tapisenabled") == 0) {
+	if (strcmp(CMD_NAME, "tapisenabled") == 0) {
 		/* do nothing, just return the value */
-	} else if (strcasecmp(cmd_name, "tapenable") == 0) {
+	} else if (strcmp(CMD_NAME, "tapenable") == 0) {
 		if (!jtag_tap_enable(t)) {
-			LOG_WARNING("failed to enable tap %s", t->dotted_name);
-			return JIM_ERR;
+			command_print(CMD, "failed to enable tap %s", t->dotted_name);
+			return ERROR_FAIL;
 		}
-	} else if (strcasecmp(cmd_name, "tapdisable") == 0) {
+	} else if (strcmp(CMD_NAME, "tapdisable") == 0) {
 		if (!jtag_tap_disable(t)) {
-			LOG_WARNING("failed to disable tap %s", t->dotted_name);
-			return JIM_ERR;
+			command_print(CMD, "failed to disable tap %s", t->dotted_name);
+			return ERROR_FAIL;
 		}
 	} else {
-		LOG_ERROR("command '%s' unknown", cmd_name);
-		return JIM_ERR;
+		command_print(CMD, "command '%s' unknown", CMD_NAME);
+		return ERROR_FAIL;
 	}
-	bool e = t->enabled;
-	Jim_SetResult(goi.interp, Jim_NewIntObj(goi.interp, e));
-	return JIM_OK;
+
+	command_print(CMD, "%d", t->enabled ? 1 : 0);
+	return ERROR_OK;
 }
 
 int jim_jtag_configure(Jim_Interp *interp, int argc, Jim_Obj *const *argv)
@@ -798,7 +743,7 @@ static const struct command_registration jtag_subcommand_handlers[] = {
 	{
 		.name = "newtap",
 		.mode = COMMAND_CONFIG,
-		.jim_handler = jim_jtag_newtap,
+		.handler = handle_jtag_newtap,
 		.help = "Create a new TAP instance named basename.tap_type, "
 			"and appends it to the scan chain.",
 		.usage = "basename tap_type '-irlen' count "
@@ -812,7 +757,7 @@ static const struct command_registration jtag_subcommand_handlers[] = {
 	{
 		.name = "tapisenabled",
 		.mode = COMMAND_EXEC,
-		.jim_handler = jim_jtag_tap_enabler,
+		.handler = handle_jtag_tap_enabler,
 		.help = "Returns a Tcl boolean (0/1) indicating whether "
 			"the TAP is enabled (1) or not (0).",
 		.usage = "tap_name",
@@ -820,7 +765,7 @@ static const struct command_registration jtag_subcommand_handlers[] = {
 	{
 		.name = "tapenable",
 		.mode = COMMAND_EXEC,
-		.jim_handler = jim_jtag_tap_enabler,
+		.handler = handle_jtag_tap_enabler,
 		.help = "Try to enable the specified TAP using the "
 			"'tap-enable' TAP event.",
 		.usage = "tap_name",
@@ -828,7 +773,7 @@ static const struct command_registration jtag_subcommand_handlers[] = {
 	{
 		.name = "tapdisable",
 		.mode = COMMAND_EXEC,
-		.jim_handler = jim_jtag_tap_enabler,
+		.handler = handle_jtag_tap_enabler,
 		.help = "Try to disable the specified TAP using the "
 			"'tap-disable' TAP event.",
 		.usage = "tap_name",
diff --git a/src/jtag/tcl.h b/src/jtag/tcl.h
index d67c085..66867ab 100644
--- a/src/jtag/tcl.h
+++ b/src/jtag/tcl.h
@@ -18,9 +18,10 @@
 #ifndef OPENOCD_JTAG_TCL_H
 #define OPENOCD_JTAG_TCL_H
 
+#include <helper/command.h>
+
 int jim_jtag_configure(Jim_Interp *interp, int argc,
 		Jim_Obj * const *argv);
-int jim_jtag_tap_enabler(Jim_Interp *interp, int argc,
-		Jim_Obj * const *argv);
+__COMMAND_HANDLER(handle_jtag_tap_enabler);
 
 #endif /* OPENOCD_JTAG_TCL_H */
diff --git a/src/pld/lattice_bit.c b/src/pld/lattice_bit.c
index 562b17d..796adce 100644
--- a/src/pld/lattice_bit.c
+++ b/src/pld/lattice_bit.c
@@ -29,7 +29,7 @@ static int lattice_read_bit_file(struct lattice_bit_file *bit_file, const char *
 	if (retval != ERROR_OK)
 		return retval;
 
-	bit_file->part = 0;
+	bit_file->part = NULL;
 	bit_file->has_id = false;
 	enum read_bit_state state = SEEK_HEADER_START;
 	for (size_t pos = 1; pos < bit_file->raw_bit.length && state != DONE; ++pos) {
diff --git a/src/pld/pld.c b/src/pld/pld.c
index 7dd2cec..07b575f 100644
--- a/src/pld/pld.c
+++ b/src/pld/pld.c
@@ -156,8 +156,6 @@ COMMAND_HANDLER(handle_pld_load_command)
 	if (retval != ERROR_OK) {
 		command_print(CMD, "failed loading file %s to pld device %u",
 			CMD_ARGV[1], dev_id);
-		switch (retval) {
-		}
 		return retval;
 	} else {
 		gettimeofday(&end, NULL);
diff --git a/src/pld/virtex2.c b/src/pld/virtex2.c
index 3c174ae..fd0725a 100644
--- a/src/pld/virtex2.c
+++ b/src/pld/virtex2.c
@@ -241,9 +241,13 @@ COMMAND_HANDLER(virtex2_handle_read_stat_command)
 		return ERROR_FAIL;
 	}
 
-	virtex2_read_stat(device, &status);
+	int retval = virtex2_read_stat(device, &status);
+	if (retval != ERROR_OK) {
+		command_print(CMD, "cannot read virtex2 status register");
+		return retval;
+	}
 
-	command_print(CMD, "virtex2 status register: 0x%8.8" PRIx32 "", status);
+	command_print(CMD, "virtex2 status register: 0x%8.8" PRIx32, status);
 
 	return ERROR_OK;
 }
diff --git a/src/rtos/eCos.c b/src/rtos/eCos.c
index 963bb61..10ed128 100644
--- a/src/rtos/eCos.c
+++ b/src/rtos/eCos.c
@@ -1161,9 +1161,6 @@ static bool ecos_detect_rtos(struct target *target)
 	return false;
 }
 
-extern int rtos_thread_packet(struct connection *connection,
-		const char *packet, int packet_size);
-
 /* Since we should never have 0 as a valid eCos thread ID we use $Hg0 as the
  * indicator of a new session as regards flushing any cached state. */
 static int ecos_packet_hook(struct connection *connection,
diff --git a/src/rtos/hwthread.c b/src/rtos/hwthread.c
index 61ceb66..763a97d 100644
--- a/src/rtos/hwthread.c
+++ b/src/rtos/hwthread.c
@@ -35,8 +35,6 @@ struct target *hwthread_swbp_target(struct rtos *rtos, target_addr_t address,
 
 #define HW_THREAD_NAME_STR_SIZE (32)
 
-extern int rtos_thread_packet(struct connection *connection, const char *packet, int packet_size);
-
 static inline threadid_t threadid_from_target(const struct target *target)
 {
 	return target->coreid + 1;
diff --git a/src/rtos/rtos.c b/src/rtos/rtos.c
index b4f6a1f..faf30bd 100644
--- a/src/rtos/rtos.c
+++ b/src/rtos/rtos.c
@@ -37,8 +37,6 @@ static const struct rtos_type *rtos_types[] = {
 
 static int rtos_try_next(struct target *target);
 
-int rtos_thread_packet(struct connection *connection, const char *packet, int packet_size);
-
 int rtos_smp_init(struct target *target)
 {
 	if (target->rtos->type->smp_init)
diff --git a/src/rtos/rtos.h b/src/rtos/rtos.h
index 1ec7674..210f4fc 100644
--- a/src/rtos/rtos.h
+++ b/src/rtos/rtos.h
@@ -157,6 +157,7 @@ int rtos_generic_stack_write_reg(struct target *target,
 								target_addr_t stack_ptr,
 								uint32_t reg_num, uint8_t *reg_value);
 int gdb_thread_packet(struct connection *connection, char const *packet, int packet_size);
+int rtos_thread_packet(struct connection *connection, const char *packet, int packet_size);
 int rtos_get_gdb_reg(struct connection *connection, int reg_num);
 int rtos_get_gdb_reg_list(struct connection *connection);
 int rtos_update_threads(struct target *target);
diff --git a/src/rtos/uCOS-III.c b/src/rtos/uCOS-III.c
index 21be8ff..4d704a4 100644
--- a/src/rtos/uCOS-III.c
+++ b/src/rtos/uCOS-III.c
@@ -29,6 +29,12 @@
 struct ucos_iii_params {
 	const char *target_name;
 	const unsigned char pointer_width;
+	size_t threadid_start;
+	const struct rtos_register_stacking *stacking_info;
+};
+
+struct ucos_iii_private {
+	const struct ucos_iii_params *params;
 	symbol_address_t thread_stack_offset;
 	symbol_address_t thread_name_offset;
 	symbol_address_t thread_state_offset;
@@ -36,40 +42,22 @@ struct ucos_iii_params {
 	symbol_address_t thread_prev_offset;
 	symbol_address_t thread_next_offset;
 	bool thread_offsets_updated;
-	size_t threadid_start;
-	const struct rtos_register_stacking *stacking_info;
 	size_t num_threads;
-	symbol_address_t threads[];
+	symbol_address_t threads[UCOS_III_MAX_THREADS];
 };
 
 static const struct ucos_iii_params ucos_iii_params_list[] = {
 	{
-		"cortex_m",							/* target_name */
-		sizeof(uint32_t),					/* pointer_width */
-		0,									/* thread_stack_offset */
-		0,									/* thread_name_offset */
-		0,									/* thread_state_offset */
-		0,									/* thread_priority_offset */
-		0,									/* thread_prev_offset */
-		0,									/* thread_next_offset */
-		false,								/* thread_offsets_updated */
-		1,									/* threadid_start */
-		&rtos_ucos_iii_cortex_m_stacking,	/* stacking_info */
-		0,									/* num_threads */
+		.target_name    = "cortex_m",
+		.pointer_width  = sizeof(uint32_t),
+		.threadid_start = 1,
+		.stacking_info  = &rtos_ucos_iii_cortex_m_stacking,
 	},
 	{
-		"esirisc",							/* target_name */
-		sizeof(uint32_t),					/* pointer_width */
-		0,									/* thread_stack_offset */
-		0,									/* thread_name_offset */
-		0,									/* thread_state_offset */
-		0,									/* thread_priority_offset */
-		0,									/* thread_prev_offset */
-		0,									/* thread_next_offset */
-		false,								/* thread_offsets_updated */
-		1,									/* threadid_start */
-		&rtos_ucos_iii_esi_risc_stacking,	/* stacking_info */
-		0,									/* num_threads */
+		.target_name    = "esirisc",
+		.pointer_width  = sizeof(uint32_t),
+		.threadid_start = 1,
+		.stacking_info  = &rtos_ucos_iii_esi_risc_stacking,
 	},
 };
 
@@ -118,7 +106,7 @@ static const char * const ucos_iii_thread_state_list[] = {
 static int ucos_iii_find_or_create_thread(struct rtos *rtos, symbol_address_t thread_address,
 		threadid_t *threadid)
 {
-	struct ucos_iii_params *params = rtos->rtos_specific_params;
+	struct ucos_iii_private *params = rtos->rtos_specific_params;
 	size_t thread_index;
 
 	for (thread_index = 0; thread_index < params->num_threads; thread_index++)
@@ -133,17 +121,17 @@ static int ucos_iii_find_or_create_thread(struct rtos *rtos, symbol_address_t th
 	params->threads[thread_index] = thread_address;
 	params->num_threads++;
 found:
-	*threadid = thread_index + params->threadid_start;
+	*threadid = thread_index + params->params->threadid_start;
 	return ERROR_OK;
 }
 
 static int ucos_iii_find_thread_address(struct rtos *rtos, threadid_t threadid,
 		symbol_address_t *thread_address)
 {
-	struct ucos_iii_params *params = rtos->rtos_specific_params;
+	struct ucos_iii_private *params = rtos->rtos_specific_params;
 	size_t thread_index;
 
-	thread_index = threadid - params->threadid_start;
+	thread_index = threadid - params->params->threadid_start;
 	if (thread_index >= params->num_threads) {
 		LOG_ERROR("uCOS-III: failed to find thread address");
 		return ERROR_FAIL;
@@ -155,7 +143,7 @@ static int ucos_iii_find_thread_address(struct rtos *rtos, threadid_t threadid,
 
 static int ucos_iii_find_last_thread_address(struct rtos *rtos, symbol_address_t *thread_address)
 {
-	struct ucos_iii_params *params = rtos->rtos_specific_params;
+	struct ucos_iii_private *params = rtos->rtos_specific_params;
 	int retval;
 
 	/* read the thread list head */
@@ -163,7 +151,7 @@ static int ucos_iii_find_last_thread_address(struct rtos *rtos, symbol_address_t
 
 	retval = target_read_memory(rtos->target,
 			rtos->symbols[UCOS_III_VAL_OS_TASK_DBG_LIST_PTR].address,
-			params->pointer_width,
+			params->params->pointer_width,
 			1,
 			(void *)&thread_list_address);
 	if (retval != ERROR_OK) {
@@ -177,7 +165,7 @@ static int ucos_iii_find_last_thread_address(struct rtos *rtos, symbol_address_t
 
 		retval = target_read_memory(rtos->target,
 				thread_list_address + params->thread_next_offset,
-				params->pointer_width,
+				params->params->pointer_width,
 				1,
 				(void *)&thread_list_address);
 		if (retval != ERROR_OK) {
@@ -191,7 +179,7 @@ static int ucos_iii_find_last_thread_address(struct rtos *rtos, symbol_address_t
 
 static int ucos_iii_update_thread_offsets(struct rtos *rtos)
 {
-	struct ucos_iii_params *params = rtos->rtos_specific_params;
+	struct ucos_iii_private *params = rtos->rtos_specific_params;
 
 	if (params->thread_offsets_updated)
 		return ERROR_OK;
@@ -231,7 +219,7 @@ static int ucos_iii_update_thread_offsets(struct rtos *rtos)
 
 		int retval = target_read_memory(rtos->target,
 				rtos->symbols[thread_offset_map->symbol_value].address,
-				params->pointer_width,
+				params->params->pointer_width,
 				1,
 				(void *)thread_offset_map->thread_offset);
 		if (retval != ERROR_OK) {
@@ -252,7 +240,7 @@ static bool ucos_iii_detect_rtos(struct target *target)
 
 static int ucos_iii_reset_handler(struct target *target, enum target_reset_mode reset_mode, void *priv)
 {
-	struct ucos_iii_params *params = target->rtos->rtos_specific_params;
+	struct ucos_iii_private *params = target->rtos->rtos_specific_params;
 
 	params->thread_offsets_updated = false;
 	params->num_threads = 0;
@@ -262,17 +250,17 @@ static int ucos_iii_reset_handler(struct target *target, enum target_reset_mode
 
 static int ucos_iii_create(struct target *target)
 {
-	struct ucos_iii_params *params;
+	struct ucos_iii_private *params;
 
 	for (size_t i = 0; i < ARRAY_SIZE(ucos_iii_params_list); i++)
 		if (strcmp(ucos_iii_params_list[i].target_name, target->type->name) == 0) {
-			params = malloc(sizeof(*params) + (UCOS_III_MAX_THREADS * sizeof(*params->threads)));
+			params = calloc(1, sizeof(*params));
 			if (!params) {
 				LOG_ERROR("uCOS-III: out of memory");
 				return ERROR_FAIL;
 			}
 
-			memcpy(params, &ucos_iii_params_list[i], sizeof(ucos_iii_params_list[i]));
+			params->params = &ucos_iii_params_list[i];
 			target->rtos->rtos_specific_params = (void *)params;
 
 			target_register_reset_callback(ucos_iii_reset_handler, NULL);
@@ -286,7 +274,7 @@ static int ucos_iii_create(struct target *target)
 
 static int ucos_iii_update_threads(struct rtos *rtos)
 {
-	struct ucos_iii_params *params = rtos->rtos_specific_params;
+	struct ucos_iii_private *params = rtos->rtos_specific_params;
 	int retval;
 
 	if (!rtos->symbols) {
@@ -340,7 +328,7 @@ static int ucos_iii_update_threads(struct rtos *rtos)
 
 	retval = target_read_memory(rtos->target,
 			rtos->symbols[UCOS_III_VAL_OS_TCB_CUR_PTR].address,
-			params->pointer_width,
+			params->params->pointer_width,
 			1,
 			(void *)&current_thread_address);
 	if (retval != ERROR_OK) {
@@ -396,7 +384,7 @@ static int ucos_iii_update_threads(struct rtos *rtos)
 
 		retval = target_read_memory(rtos->target,
 				thread_address + params->thread_name_offset,
-				params->pointer_width,
+				params->params->pointer_width,
 				1,
 				(void *)&thread_name_address);
 		if (retval != ERROR_OK) {
@@ -450,7 +438,7 @@ static int ucos_iii_update_threads(struct rtos *rtos)
 		/* read previous thread address */
 		retval = target_read_memory(rtos->target,
 				thread_address + params->thread_prev_offset,
-				params->pointer_width,
+				params->params->pointer_width,
 				1,
 				(void *)&thread_address);
 		if (retval != ERROR_OK) {
@@ -465,7 +453,7 @@ static int ucos_iii_update_threads(struct rtos *rtos)
 static int ucos_iii_get_thread_reg_list(struct rtos *rtos, threadid_t threadid,
 		struct rtos_reg **reg_list, int *num_regs)
 {
-	struct ucos_iii_params *params = rtos->rtos_specific_params;
+	struct ucos_iii_private *params = rtos->rtos_specific_params;
 	int retval;
 
 	/* find thread address for threadid */
@@ -482,7 +470,7 @@ static int ucos_iii_get_thread_reg_list(struct rtos *rtos, threadid_t threadid,
 
 	retval = target_read_memory(rtos->target,
 			thread_address + params->thread_stack_offset,
-			params->pointer_width,
+			params->params->pointer_width,
 			1,
 			(void *)&stack_address);
 	if (retval != ERROR_OK) {
@@ -491,7 +479,7 @@ static int ucos_iii_get_thread_reg_list(struct rtos *rtos, threadid_t threadid,
 	}
 
 	return rtos_generic_stack_read(rtos->target,
-			params->stacking_info,
+			params->params->stacking_info,
 			stack_address,
 			reg_list,
 			num_regs);
diff --git a/src/server/gdb_server.c b/src/server/gdb_server.c
index aa23938..8409012 100644
--- a/src/server/gdb_server.c
+++ b/src/server/gdb_server.c
@@ -117,7 +117,7 @@ static void gdb_sig_halted(struct connection *connection);
 
 /* number of gdb connections, mainly to suppress gdb related debugging spam
  * in helper/log.c when no gdb connections are actually active */
-int gdb_actual_connections;
+static int gdb_actual_connections;
 
 /* set if we are sending a memory map to gdb
  * via qXfer:memory-map:read packet */
@@ -186,6 +186,9 @@ static bool gdb_connection_includes_target(struct connection *connection, struct
 
 static int gdb_last_signal(struct target *target)
 {
+	LOG_TARGET_DEBUG(target, "Debug reason is: %s",
+			target_debug_reason_str(target->debug_reason));
+
 	switch (target->debug_reason) {
 		case DBG_REASON_DBGRQ:
 			return 0x2;		/* SIGINT */
@@ -200,8 +203,9 @@ static int gdb_last_signal(struct target *target)
 		case DBG_REASON_NOTHALTED:
 			return 0x0;		/* no signal... shouldn't happen */
 		default:
-			LOG_USER("undefined debug reason %d - target needs reset",
-					target->debug_reason);
+			LOG_USER("undefined debug reason %d (%s) - target needs reset",
+					target->debug_reason,
+					target_debug_reason_str(target->debug_reason));
 			return 0x0;
 	}
 }
@@ -272,39 +276,20 @@ static int gdb_get_char_inner(struct connection *connection, int *next_char)
 		}
 
 #ifdef _WIN32
-		errno = WSAGetLastError();
-
-		switch (errno) {
-			case WSAEWOULDBLOCK:
-				usleep(1000);
-				break;
-			case WSAECONNABORTED:
-				gdb_con->closed = true;
-				return ERROR_SERVER_REMOTE_CLOSED;
-			case WSAECONNRESET:
-				gdb_con->closed = true;
-				return ERROR_SERVER_REMOTE_CLOSED;
-			default:
-				LOG_ERROR("read: %d", errno);
-				exit(-1);
-		}
+		bool retry = (WSAGetLastError() == WSAEWOULDBLOCK);
 #else
-		switch (errno) {
-			case EAGAIN:
-				usleep(1000);
-				break;
-			case ECONNABORTED:
-				gdb_con->closed = true;
-				return ERROR_SERVER_REMOTE_CLOSED;
-			case ECONNRESET:
-				gdb_con->closed = true;
-				return ERROR_SERVER_REMOTE_CLOSED;
-			default:
-				LOG_ERROR("read: %s", strerror(errno));
-				gdb_con->closed = true;
-				return ERROR_SERVER_REMOTE_CLOSED;
-		}
+		bool retry = (errno == EAGAIN);
 #endif
+
+		if (retry) {
+			// Try again after a delay
+			usleep(1000);
+		} else {
+			// Print error and close the socket
+			log_socket_error("GDB");
+			gdb_con->closed = true;
+			return ERROR_SERVER_REMOTE_CLOSED;
+		}
 	}
 
 #ifdef _DEBUG_GDB_IO_
@@ -844,6 +829,7 @@ static void gdb_signal_reply(struct target *target, struct connection *connectio
 		}
 
 		if (gdb_connection->ctrl_c) {
+			LOG_TARGET_DEBUG(target, "Responding with signal 2 (SIGINT) to debugger due to Ctrl-C");
 			signal_var = 0x2;
 		} else
 			signal_var = gdb_last_signal(ct);
@@ -4201,3 +4187,8 @@ void gdb_service_free(void)
 	free(gdb_port);
 	free(gdb_port_next);
 }
+
+int gdb_get_actual_connections(void)
+{
+	return gdb_actual_connections;
+}
diff --git a/src/server/gdb_server.h b/src/server/gdb_server.h
index e27aad7..4288ceb 100644
--- a/src/server/gdb_server.h
+++ b/src/server/gdb_server.h
@@ -20,6 +20,7 @@
 struct image;
 struct reg;
 #include <target/target.h>
+#include <server/server.h>
 
 #define GDB_BUFFER_SIZE 16384
 
@@ -29,6 +30,8 @@ void gdb_service_free(void);
 
 int gdb_put_packet(struct connection *connection, char *buffer, int len);
 
+int gdb_get_actual_connections(void);
+
 static inline struct target *get_target_from_connection(struct connection *connection)
 {
 	struct gdb_service *gdb_service = connection->service->priv;
diff --git a/src/target/adi_v5_dapdirect.c b/src/target/adi_v5_dapdirect.c
index e1c00b7..575092c 100644
--- a/src/target/adi_v5_dapdirect.c
+++ b/src/target/adi_v5_dapdirect.c
@@ -58,8 +58,15 @@ static const struct command_registration dapdirect_jtag_subcommand_handlers[] =
 	{
 		.name = "newtap",
 		.mode = COMMAND_CONFIG,
-		.jim_handler = jim_jtag_newtap,
-		.help = "declare a new TAP"
+		.handler = handle_jtag_newtap,
+		.help = "declare a new TAP",
+		.usage = "basename tap_type '-irlen' count "
+			"['-enable'|'-disable'] "
+			"['-expected_id' number] "
+			"['-ignore-version'] "
+			"['-ignore-bypass'] "
+			"['-ircapture' number] "
+			"['-mask' number]",
 	},
 	{
 		.name = "init",
@@ -82,12 +89,18 @@ static const struct command_registration dapdirect_jtag_subcommand_handlers[] =
 	{
 		.name = "tapisenabled",
 		.mode = COMMAND_EXEC,
-		.jim_handler = jim_jtag_tap_enabler,
+		.handler = handle_jtag_tap_enabler,
+		.help = "Returns a Tcl boolean (0/1) indicating whether "
+			"the TAP is enabled (1) or not (0).",
+		.usage = "tap_name",
 	},
 	{
 		.name = "tapenable",
 		.mode = COMMAND_EXEC,
-		.jim_handler = jim_jtag_tap_enabler,
+		.handler = handle_jtag_tap_enabler,
+		.help = "Try to enable the specified TAP using the "
+			"'tap-enable' TAP event.",
+		.usage = "tap_name",
 	},
 	{
 		.name = "tapdisable",
@@ -135,8 +148,15 @@ static const struct command_registration dapdirect_swd_subcommand_handlers[] = {
 	{
 		.name = "newdap",
 		.mode = COMMAND_CONFIG,
-		.jim_handler = jim_jtag_newtap,
+		.handler = handle_jtag_newtap,
 		.help = "declare a new SWD DAP",
+		.usage = "basename dap_type ['-irlen' count] "
+			"['-enable'|'-disable'] "
+			"['-expected_id' number] "
+			"['-ignore-version'] "
+			"['-ignore-bypass'] "
+			"['-ircapture' number] "
+			"['-mask' number]",
 	},
 	COMMAND_REGISTRATION_DONE
 };
diff --git a/src/target/adi_v5_swd.c b/src/target/adi_v5_swd.c
index 653f91f..275a501 100644
--- a/src/target/adi_v5_swd.c
+++ b/src/target/adi_v5_swd.c
@@ -657,9 +657,16 @@ static const struct command_registration swd_commands[] = {
 		 * REVISIT can we verify "just one SWD DAP" here/early?
 		 */
 		.name = "newdap",
-		.jim_handler = jim_jtag_newtap,
+		.handler = handle_jtag_newtap,
 		.mode = COMMAND_CONFIG,
-		.help = "declare a new SWD DAP"
+		.help = "declare a new SWD DAP",
+		.usage = "basename dap_type ['-irlen' count] "
+			"['-enable'|'-disable'] "
+			"['-expected_id' number] "
+			"['-ignore-version'] "
+			"['-ignore-bypass'] "
+			"['-ircapture' number] "
+			"['-mask' number]",
 	},
 	COMMAND_REGISTRATION_DONE
 };
diff --git a/src/target/arm.h b/src/target/arm.h
index de46ffb..fd61d5f 100644
--- a/src/target/arm.h
+++ b/src/target/arm.h
@@ -292,14 +292,14 @@ int armv4_5_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,
-		int timeout_ms, void *arch_info);
+		unsigned int timeout_ms, void *arch_info);
 int armv4_5_run_algorithm_inner(struct target *target,
 		int num_mem_params, struct mem_param *mem_params,
 		int num_reg_params, struct reg_param *reg_params,
 		uint32_t entry_point, uint32_t exit_point,
-		int timeout_ms, void *arch_info,
+		unsigned int timeout_ms, void *arch_info,
 		int (*run_it)(struct target *target, uint32_t exit_point,
-				int timeout_ms, void *arch_info));
+				unsigned int timeout_ms, void *arch_info));
 
 int arm_checksum_memory(struct target *target,
 		target_addr_t address, uint32_t count, uint32_t *checksum);
diff --git a/src/target/arm7_9_common.c b/src/target/arm7_9_common.c
index 0632290..f60777d 100644
--- a/src/target/arm7_9_common.c
+++ b/src/target/arm7_9_common.c
@@ -2518,7 +2518,7 @@ static const uint8_t *dcc_buffer;
 
 static int arm7_9_dcc_completion(struct target *target,
 	uint32_t exit_point,
-	int timeout_ms,
+	unsigned int timeout_ms,
 	void *arch_info)
 {
 	int retval = ERROR_OK;
diff --git a/src/target/arm_adi_v5.h b/src/target/arm_adi_v5.h
index 3eddbc0..90d28bc 100644
--- a/src/target/arm_adi_v5.h
+++ b/src/target/arm_adi_v5.h
@@ -455,6 +455,9 @@ enum ap_type {
 	AP_TYPE_AHB5H_AP = AP_REG_IDR_VALUE(ARM_ID, AP_REG_IDR_CLASS_MEM_AP, 8),  /* AHB5 with enhanced HPROT Memory-AP */
 };
 
+extern const struct dap_ops jtag_dp_ops;
+extern const struct dap_ops swd_dap_ops;
+
 /* Check the ap->cfg_reg Long Address field (bit 1)
  *
  * 0b0: The AP only supports physical addresses 32 bits or smaller
diff --git a/src/target/arm_dap.c b/src/target/arm_dap.c
index bc9d962..84cc6c7 100644
--- a/src/target/arm_dap.c
+++ b/src/target/arm_dap.c
@@ -20,8 +20,6 @@
 
 static LIST_HEAD(all_dap);
 
-extern const struct dap_ops swd_dap_ops;
-extern const struct dap_ops jtag_dp_ops;
 extern struct adapter_driver *adapter_driver;
 
 /* DAP command support */
diff --git a/src/target/arm_dpm.c b/src/target/arm_dpm.c
index 5f7e929..fd6fb26 100644
--- a/src/target/arm_dpm.c
+++ b/src/target/arm_dpm.c
@@ -1050,7 +1050,7 @@ int arm_dpm_setup(struct arm_dpm *dpm)
 {
 	struct arm *arm = dpm->arm;
 	struct target *target = arm->target;
-	struct reg_cache *cache = 0;
+	struct reg_cache *cache = NULL;
 
 	arm->dpm = dpm;
 
diff --git a/src/target/armv4_5.c b/src/target/armv4_5.c
index 9586adc..f35d67a 100644
--- a/src/target/armv4_5.c
+++ b/src/target/armv4_5.c
@@ -1252,7 +1252,7 @@ int arm_get_gdb_reg_list(struct target *target,
 /* wait for execution to complete and check exit point */
 static int armv4_5_run_algorithm_completion(struct target *target,
 	uint32_t exit_point,
-	int timeout_ms,
+	unsigned int timeout_ms,
 	void *arch_info)
 {
 	int retval;
@@ -1286,9 +1286,9 @@ int armv4_5_run_algorithm_inner(struct target *target,
 	int num_mem_params, struct mem_param *mem_params,
 	int num_reg_params, struct reg_param *reg_params,
 	uint32_t entry_point, uint32_t exit_point,
-	int timeout_ms, void *arch_info,
+	unsigned int timeout_ms, void *arch_info,
 	int (*run_it)(struct target *target, uint32_t exit_point,
-	int timeout_ms, void *arch_info))
+	unsigned int timeout_ms, void *arch_info))
 {
 	struct arm *arm = target_to_arm(target);
 	struct arm_algorithm *arm_algorithm_info = arch_info;
@@ -1474,7 +1474,7 @@ int armv4_5_run_algorithm(struct target *target,
 	struct reg_param *reg_params,
 	target_addr_t entry_point,
 	target_addr_t exit_point,
-	int timeout_ms,
+	unsigned int timeout_ms,
 	void *arch_info)
 {
 	return armv4_5_run_algorithm_inner(target,
@@ -1535,7 +1535,7 @@ int arm_checksum_memory(struct target *target,
 	buf_set_u32(reg_params[1].value, 0, 32, count);
 
 	/* 20 second timeout/megabyte */
-	int timeout = 20000 * (1 + (count / (1024 * 1024)));
+	unsigned int timeout = 20000 * (1 + (count / (1024 * 1024)));
 
 	/* armv4 must exit using a hardware breakpoint */
 	if (arm->arch == ARM_ARCH_V4)
diff --git a/src/target/armv7m.c b/src/target/armv7m.c
index 5745681..8c9ff90 100644
--- a/src/target/armv7m.c
+++ b/src/target/armv7m.c
@@ -484,7 +484,7 @@ int armv7m_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,
-	int timeout_ms, void *arch_info)
+	unsigned int timeout_ms, void *arch_info)
 {
 	int retval;
 
@@ -622,7 +622,7 @@ int armv7m_start_algorithm(struct target *target,
 int armv7m_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, int timeout_ms,
+	target_addr_t exit_point, unsigned int timeout_ms,
 	void *arch_info)
 {
 	struct armv7m_common *armv7m = target_to_armv7m(target);
@@ -909,7 +909,7 @@ int armv7m_checksum_memory(struct target *target,
 	buf_set_u32(reg_params[0].value, 0, 32, address);
 	buf_set_u32(reg_params[1].value, 0, 32, count);
 
-	int timeout = 20000 * (1 + (count / (1024 * 1024)));
+	unsigned int timeout = 20000 * (1 + (count / (1024 * 1024)));
 
 	retval = target_run_algorithm(target, 0, NULL, 2, reg_params, crc_algorithm->address,
 			crc_algorithm->address + (sizeof(cortex_m_crc_code) - 6),
@@ -1016,7 +1016,7 @@ int armv7m_blank_check_memory(struct target *target,
 	buf_set_u32(reg_params[1].value, 0, 32, erased_word);
 
 	/* assume CPU clk at least 1 MHz */
-	int timeout = (timed_out ? 30000 : 2000) + total_size * 3 / 1000;
+	unsigned int timeout = (timed_out ? 30000 : 2000) + total_size * 3 / 1000;
 
 	retval = target_run_algorithm(target,
 				0, NULL,
diff --git a/src/target/armv7m.h b/src/target/armv7m.h
index 188bd56..8693404 100644
--- a/src/target/armv7m.h
+++ b/src/target/armv7m.h
@@ -314,7 +314,7 @@ int armv7m_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,
-		int timeout_ms, void *arch_info);
+		unsigned int timeout_ms, void *arch_info);
 
 int armv7m_start_algorithm(struct target *target,
 		int num_mem_params, struct mem_param *mem_params,
@@ -325,7 +325,7 @@ int armv7m_start_algorithm(struct target *target,
 int armv7m_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, int timeout_ms,
+		target_addr_t exit_point, unsigned int timeout_ms,
 		void *arch_info);
 
 int armv7m_invalidate_core_regs(struct target *target);
diff --git a/src/target/armv8.c b/src/target/armv8.c
index ffed263..e647c3b 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -1682,7 +1682,7 @@ struct reg_cache *armv8_build_reg_cache(struct target *target)
 			LOG_ERROR("unable to allocate reg type list");
 
 		if (i == ARMV8_PAUTH_CMASK || i == ARMV8_PAUTH_DMASK)
-			reg_list[i].hidden = !armv8->enable_pauth;
+			reg_list[i].exist = armv8->enable_pauth;
 	}
 
 	arm->cpsr = reg_list + ARMV8_XPSR;
diff --git a/src/target/cortex_m.c b/src/target/cortex_m.c
index 8e55014..ebc3bac 100644
--- a/src/target/cortex_m.c
+++ b/src/target/cortex_m.c
@@ -801,15 +801,11 @@ static int cortex_m_debug_entry(struct target *target)
 		return retval;
 
 	/* examine PE security state */
-	bool secure_state = false;
+	uint32_t dscsr = 0;
 	if (armv7m->arm.arch == ARM_ARCH_V8M) {
-		uint32_t dscsr;
-
 		retval = mem_ap_read_u32(armv7m->debug_ap, DCB_DSCSR, &dscsr);
 		if (retval != ERROR_OK)
 			return retval;
-
-		secure_state = (dscsr & DSCSR_CDS) == DSCSR_CDS;
 	}
 
 	/* Load all registers to arm.core_cache */
@@ -857,6 +853,7 @@ static int cortex_m_debug_entry(struct target *target)
 	if (armv7m->exception_number)
 		cortex_m_examine_exception_reason(target);
 
+	bool secure_state = (dscsr & DSCSR_CDS) == DSCSR_CDS;
 	LOG_TARGET_DEBUG(target, "entered debug state in core mode: %s at PC 0x%" PRIx32
 			", cpu in %s state, target->state: %s",
 		arm_mode_name(arm->core_mode),
diff --git a/src/target/dsp563xx.c b/src/target/dsp563xx.c
index 9db54fb..af60345 100644
--- a/src/target/dsp563xx.c
+++ b/src/target/dsp563xx.c
@@ -1374,7 +1374,7 @@ static int dsp563xx_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,
-	int timeout_ms, void *arch_info)
+	unsigned int timeout_ms, void *arch_info)
 {
 	int i;
 	int retval = ERROR_OK;
diff --git a/src/target/espressif/Makefile.am b/src/target/espressif/Makefile.am
index c1759ed..14625d4 100644
--- a/src/target/espressif/Makefile.am
+++ b/src/target/espressif/Makefile.am
@@ -15,5 +15,8 @@ noinst_LTLIBRARIES += %D%/libespressif.la
        %D%/esp32.c \
        %D%/esp32s2.c \
        %D%/esp32s3.c \
+       %D%/esp32_sysview.c \
+       %D%/esp32_sysview.h \
+       %D%/segger_sysview.h \
        %D%/esp_semihosting.c \
        %D%/esp_semihosting.h
diff --git a/src/target/espressif/esp32_apptrace.c b/src/target/espressif/esp32_apptrace.c
index 291503e..8842241 100644
--- a/src/target/espressif/esp32_apptrace.c
+++ b/src/target/espressif/esp32_apptrace.c
@@ -32,6 +32,8 @@
 #include "esp_xtensa_smp.h"
 #include "esp_xtensa_apptrace.h"
 #include "esp32_apptrace.h"
+#include "esp32_sysview.h"
+#include "segger_sysview.h"
 
 #define ESP32_APPTRACE_USER_BLOCK_CORE(_v_)     ((_v_) >> 15)
 #define ESP32_APPTRACE_USER_BLOCK_LEN(_v_)      ((_v_) & ~BIT(15))
@@ -82,6 +84,8 @@ static int esp32_apptrace_safe_halt_targets(struct esp32_apptrace_cmd_ctx *ctx,
 static struct esp32_apptrace_block *esp32_apptrace_free_block_get(struct esp32_apptrace_cmd_ctx *ctx);
 static int esp32_apptrace_handle_trace_block(struct esp32_apptrace_cmd_ctx *ctx,
 	struct esp32_apptrace_block *block);
+static int esp32_sysview_start(struct esp32_apptrace_cmd_ctx *ctx);
+static int esp32_sysview_stop(struct esp32_apptrace_cmd_ctx *ctx);
 
 static const bool s_time_stats_enable = true;
 
@@ -1118,10 +1122,7 @@ static int esp32_apptrace_poll(void *priv)
 			return ERROR_FAIL;
 		}
 	}
-	res =
-		ctx->hw->data_read(ctx->cpus[fired_target_num],
-		target_state[fired_target_num].data_len,
-		block->data,
+	res = ctx->hw->data_read(ctx->cpus[fired_target_num], target_state[fired_target_num].data_len, block->data,
 		target_state[fired_target_num].block_id,
 		/* do not ack target data in sync mode,
 		   esp32_apptrace_handle_trace_block() can write response data and will do ack thereafter */
@@ -1215,6 +1216,11 @@ static int esp32_apptrace_poll(void *priv)
 	return ERROR_OK;
 }
 
+static inline bool is_sysview_mode(int mode)
+{
+	return mode == ESP_APPTRACE_CMD_MODE_SYSVIEW || mode ==	ESP_APPTRACE_CMD_MODE_SYSVIEW_MCORE;
+}
+
 static void esp32_apptrace_cmd_stop(struct esp32_apptrace_cmd_ctx *ctx)
 {
 	if (duration_measure(&ctx->read_time) != 0)
@@ -1222,7 +1228,12 @@ static void esp32_apptrace_cmd_stop(struct esp32_apptrace_cmd_ctx *ctx)
 	int res = target_unregister_timer_callback(esp32_apptrace_poll, ctx);
 	if (res != ERROR_OK)
 		LOG_ERROR("Failed to unregister target timer handler (%d)!", res);
-
+	if (is_sysview_mode(ctx->mode)) {
+		/* stop tracing */
+		res = esp32_sysview_stop(ctx);
+		if (res != ERROR_OK)
+			LOG_ERROR("sysview: Failed to stop tracing!");
+	}
 	/* data processor is alive, so wait for all received blocks to be processed */
 	res = esp32_apptrace_wait_tracing_finished(ctx);
 	if (res != ERROR_OK)
@@ -1236,7 +1247,191 @@ static void esp32_apptrace_cmd_stop(struct esp32_apptrace_cmd_ctx *ctx)
 		LOG_ERROR("Failed to cleanup cmd ctx (%d)!", res);
 }
 
-int esp32_cmd_apptrace_generic(struct command_invocation *cmd, int mode, const char **argv, int argc)
+/* this function must be called after connecting to targets */
+static int esp32_sysview_start(struct esp32_apptrace_cmd_ctx *ctx)
+{
+	uint8_t cmds[] = { SEGGER_SYSVIEW_COMMAND_ID_START };
+	uint32_t fired_target_num = 0;
+	struct esp32_apptrace_target_state target_state[ESP32_APPTRACE_MAX_CORES_NUM] = {0};
+	struct esp32_sysview_cmd_data *cmd_data = ctx->cmd_priv;
+
+	/* get current block id */
+	int res = esp32_apptrace_get_data_info(ctx, target_state, &fired_target_num);
+	if (res != ERROR_OK) {
+		LOG_ERROR("sysview: Failed to read target data info!");
+		return res;
+	}
+	if (fired_target_num == UINT32_MAX) {
+		/* it can happen that there is no pending target data, but block was switched
+		 * in this case block_ids on both CPUs are equal, so select the first one */
+		fired_target_num = 0;
+	}
+	/* start tracing */
+	res = esp_apptrace_usr_block_write(ctx->hw, ctx->cpus[fired_target_num], target_state[fired_target_num].block_id,
+		cmds, sizeof(cmds));
+	if (res != ERROR_OK) {
+		LOG_ERROR("sysview: Failed to start tracing!");
+		return res;
+	}
+	cmd_data->sv_trace_running = 1;
+	return res;
+}
+
+static int esp32_sysview_stop(struct esp32_apptrace_cmd_ctx *ctx)
+{
+	uint32_t old_block_id, fired_target_num = 0, empty_target_num = 0;
+	struct esp32_apptrace_target_state target_state[ESP32_APPTRACE_MAX_CORES_NUM];
+	struct esp32_sysview_cmd_data *cmd_data = ctx->cmd_priv;
+	uint8_t cmds[] = { SEGGER_SYSVIEW_COMMAND_ID_STOP };
+	struct duration wait_time;
+
+	struct esp32_apptrace_block *block = esp32_apptrace_free_block_get(ctx);
+	if (!block) {
+		LOG_ERROR("Failed to get free block for data on (%s)!", target_name(ctx->cpus[fired_target_num]));
+		return ERROR_FAIL;
+	}
+
+	/* halt all CPUs (not only one), otherwise it can happen that there is no target data and
+	 * while we are queueing commands another CPU switches tracing block */
+	int res = esp32_apptrace_safe_halt_targets(ctx, target_state);
+	if (res != ERROR_OK) {
+		LOG_ERROR("sysview: Failed to halt targets (%d)!", res);
+		return res;
+	}
+	/* it can happen that there is no pending target data
+	 * in this case block_ids on both CPUs are equal, so the first one will be selected */
+	for (unsigned int k = 0; k < ctx->cores_num; k++) {
+		if (target_state[k].data_len) {
+			fired_target_num = k;
+			break;
+		}
+	}
+	if (target_state[fired_target_num].data_len) {
+		/* read pending data without ack, they will be acked when stop command is queued */
+		res = ctx->hw->data_read(ctx->cpus[fired_target_num], target_state[fired_target_num].data_len, block->data,
+			target_state[fired_target_num].block_id,
+			false /*no ack target data*/);
+		if (res != ERROR_OK) {
+			LOG_ERROR("sysview: Failed to read data on (%s)!", target_name(ctx->cpus[fired_target_num]));
+			return res;
+		}
+		/* process data */
+		block->data_len = target_state[fired_target_num].data_len;
+		res = esp32_apptrace_handle_trace_block(ctx, block);
+		if (res != ERROR_OK) {
+			LOG_ERROR("Failed to process trace block %" PRId32 " bytes!", block->data_len);
+			return res;
+		}
+	}
+	/* stop tracing and ack target data */
+	res = esp_apptrace_usr_block_write(ctx->hw, ctx->cpus[fired_target_num], target_state[fired_target_num].block_id,
+		cmds,
+		sizeof(cmds));
+	if (res != ERROR_OK) {
+		LOG_ERROR("sysview: Failed to stop tracing!");
+		return res;
+	}
+	if (ctx->cores_num > 1) {
+		empty_target_num = fired_target_num ? 0 : 1;
+		/* ack target data on another CPU */
+		res = ctx->hw->ctrl_reg_write(ctx->cpus[empty_target_num], target_state[fired_target_num].block_id,
+			0 /*target data ack*/,
+			true /*host connected*/,
+			false /*no host data*/);
+		if (res != ERROR_OK) {
+			LOG_ERROR("sysview: Failed to ack data on target '%s' (%d)!",
+				target_name(ctx->cpus[empty_target_num]), res);
+			return res;
+		}
+	}
+	/* resume targets to allow command processing */
+	LOG_INFO("Resume targets");
+	bool smp_resumed = false;
+	for (unsigned int k = 0; k < ctx->cores_num; k++) {
+		if (smp_resumed && ctx->cpus[k]->smp) {
+			/* in SMP mode we need to call target_resume for one core only */
+			continue;
+		}
+		res = target_resume(ctx->cpus[k], 1, 0, 1, 0);
+		if (res != ERROR_OK) {
+			LOG_ERROR("sysview: Failed to resume target '%s' (%d)!", target_name(ctx->cpus[k]), res);
+			return res;
+		}
+		if (ctx->cpus[k]->smp)
+			smp_resumed = true;
+	}
+	/* wait for block switch (command sent), so we can disconnect from targets */
+	old_block_id = target_state[fired_target_num].block_id;
+	if (duration_start(&wait_time) != 0) {
+		LOG_ERROR("Failed to start trace stop timeout measurement!");
+		return ERROR_FAIL;
+	}
+	/* we are waiting for the last data from tracing block and also there can be data in the pended
+	 * data buffer */
+	/* so we are expecting two TRX block switches at most or stopping due to timeout */
+	while (cmd_data->sv_trace_running) {
+		res = esp32_apptrace_get_data_info(ctx, target_state, &fired_target_num);
+		if (res != ERROR_OK) {
+			LOG_ERROR("sysview: Failed to read targets data info!");
+			return res;
+		}
+		if (fired_target_num == UINT32_MAX) {
+			/* it can happen that there is no pending (last) target data, but block was
+			 * switched */
+			/* in this case block_ids on both CPUs are equal, so select the first one */
+			fired_target_num = 0;
+		}
+		if (target_state[fired_target_num].block_id != old_block_id) {
+			if (target_state[fired_target_num].data_len) {
+				/* read last data and ack them */
+				res = ctx->hw->data_read(ctx->cpus[fired_target_num],
+					target_state[fired_target_num].data_len,
+					block->data,
+					target_state[fired_target_num].block_id,
+					true /*ack target data*/);
+				if (res != ERROR_OK) {
+					LOG_ERROR("sysview: Failed to read last data on (%s)!", target_name(ctx->cpus[fired_target_num]));
+				} else {
+					if (ctx->cores_num > 1) {
+						/* ack target data on another CPU */
+						empty_target_num = fired_target_num ? 0 : 1;
+						res = ctx->hw->ctrl_reg_write(ctx->cpus[empty_target_num],
+							target_state[fired_target_num].block_id,
+							0 /*all read*/,
+							true /*host connected*/,
+							false /*no host data*/);
+						if (res != ERROR_OK) {
+							LOG_ERROR("sysview: Failed to ack data on target '%s' (%d)!",
+								target_name(ctx->cpus[empty_target_num]), res);
+							return res;
+						}
+					}
+					/* process data */
+					block->data_len = target_state[fired_target_num].data_len;
+					res = esp32_apptrace_handle_trace_block(ctx, block);
+					if (res != ERROR_OK) {
+						LOG_ERROR("Failed to process trace block %" PRId32 " bytes!",
+							block->data_len);
+						return res;
+					}
+				}
+				old_block_id = target_state[fired_target_num].block_id;
+			}
+		}
+		if (duration_measure(&wait_time) != 0) {
+			LOG_ERROR("Failed to start trace stop timeout measurement!");
+			return ERROR_FAIL;
+		}
+		const float stop_tmo = LOG_LEVEL_IS(LOG_LVL_DEBUG) ? 30.0 : 0.5;
+		if (duration_elapsed(&wait_time) >= stop_tmo) {
+			LOG_INFO("Stop waiting for the last data due to timeout.");
+			break;
+		}
+	}
+	return res;
+}
+
+static int esp32_cmd_apptrace_generic(struct command_invocation *cmd, int mode, const char **argv, int argc)
 {
 	static struct esp32_apptrace_cmd_ctx s_at_cmd_ctx;
 	struct esp32_apptrace_cmd_data *cmd_data;
@@ -1264,17 +1459,39 @@ int esp32_cmd_apptrace_generic(struct command_invocation *cmd, int mode, const c
 	old_state = target->state;
 
 	if (strcmp(argv[0], "start") == 0) {
-		res = esp32_apptrace_cmd_init(&s_at_cmd_ctx,
-			cmd,
-			mode,
-			&argv[1],
-			argc - 1);
-		if (res != ERROR_OK) {
-			command_print(cmd, "Failed to init cmd ctx (%d)!", res);
-			return res;
+		if (is_sysview_mode(mode)) {
+			/* init cmd context */
+			res = esp32_sysview_cmd_init(&s_at_cmd_ctx,
+				cmd,
+				mode,
+				mode == ESP_APPTRACE_CMD_MODE_SYSVIEW_MCORE,
+				&argv[1],
+				argc - 1);
+			if (res != ERROR_OK) {
+				command_print(cmd, "Failed to init cmd ctx (%d)!", res);
+				return res;
+			}
+			cmd_data = s_at_cmd_ctx.cmd_priv;
+			if (cmd_data->skip_len != 0) {
+				s_at_cmd_ctx.running = 0;
+				esp32_sysview_cmd_cleanup(&s_at_cmd_ctx);
+				command_print(cmd, "Data skipping not supported!");
+				return ERROR_FAIL;
+			}
+			s_at_cmd_ctx.process_data = esp32_sysview_process_data;
+		} else {
+			res = esp32_apptrace_cmd_init(&s_at_cmd_ctx,
+				cmd,
+				mode,
+				&argv[1],
+				argc - 1);
+			if (res != ERROR_OK) {
+				command_print(cmd, "Failed to init cmd ctx (%d)!", res);
+				return res;
+			}
+			cmd_data = s_at_cmd_ctx.cmd_priv;
+			s_at_cmd_ctx.process_data = esp32_apptrace_process_data;
 		}
-		cmd_data = s_at_cmd_ctx.cmd_priv;
-		s_at_cmd_ctx.process_data = esp32_apptrace_process_data;
 		s_at_cmd_ctx.auto_clean = esp32_apptrace_cmd_stop;
 		if (cmd_data->wait4halt) {
 			res = esp32_apptrace_wait4halt(&s_at_cmd_ctx, target);
@@ -1288,6 +1505,17 @@ int esp32_cmd_apptrace_generic(struct command_invocation *cmd, int mode, const c
 			command_print(cmd, "Failed to connect to targets (%d)!", res);
 			goto _on_start_error;
 		}
+		if (is_sysview_mode(mode)) {
+			/* start tracing */
+			res = esp32_sysview_start(&s_at_cmd_ctx);
+			if (res != ERROR_OK) {
+				esp32_apptrace_connect_targets(&s_at_cmd_ctx, false, old_state == TARGET_RUNNING);
+				s_at_cmd_ctx.running = 0;
+				esp32_apptrace_cmd_cleanup(&s_at_cmd_ctx);
+				command_print(cmd, "sysview: Failed to start tracing!");
+				return res;
+			}
+		}
 		res = target_register_timer_callback(esp32_apptrace_poll,
 			cmd_data->poll_period,
 			TARGET_TIMER_TYPE_PERIODIC,
@@ -1309,6 +1537,10 @@ int esp32_cmd_apptrace_generic(struct command_invocation *cmd, int mode, const c
 		esp32_apptrace_print_stats(&s_at_cmd_ctx);
 		return ERROR_OK;
 	} else if (strcmp(argv[0], "dump") == 0) {
+		if (is_sysview_mode(mode)) {
+			command_print(cmd, "Not supported!");
+			return ERROR_FAIL;
+		}
 		/* [dump outfile] - post-mortem dump without connection to targets */
 		res = esp32_apptrace_cmd_init(&s_at_cmd_ctx,
 			cmd,
@@ -1349,7 +1581,10 @@ int esp32_cmd_apptrace_generic(struct command_invocation *cmd, int mode, const c
 
 _on_start_error:
 	s_at_cmd_ctx.running = 0;
-	esp32_apptrace_cmd_cleanup(&s_at_cmd_ctx);
+	if (is_sysview_mode(mode))
+		esp32_sysview_cmd_cleanup(&s_at_cmd_ctx);
+	else
+		esp32_apptrace_cmd_cleanup(&s_at_cmd_ctx);
 	return res;
 }
 
@@ -1358,6 +1593,16 @@ COMMAND_HANDLER(esp32_cmd_apptrace)
 	return esp32_cmd_apptrace_generic(CMD, ESP_APPTRACE_CMD_MODE_GEN, CMD_ARGV, CMD_ARGC);
 }
 
+COMMAND_HANDLER(esp32_cmd_sysview)
+{
+	return esp32_cmd_apptrace_generic(CMD, ESP_APPTRACE_CMD_MODE_SYSVIEW, CMD_ARGV, CMD_ARGC);
+}
+
+COMMAND_HANDLER(esp32_cmd_sysview_mcore)
+{
+	return esp32_cmd_apptrace_generic(CMD, ESP_APPTRACE_CMD_MODE_SYSVIEW_MCORE, CMD_ARGV, CMD_ARGC);
+}
+
 const struct command_registration esp32_apptrace_command_handlers[] = {
 	{
 		.name = "apptrace",
@@ -1366,7 +1611,25 @@ const struct command_registration esp32_apptrace_command_handlers[] = {
 		.help =
 			"App Tracing: application level trace control. Starts, stops or queries tracing process status.",
 		.usage =
-			"[start <destination> [poll_period [trace_size [stop_tmo [wait4halt [skip_size]]]]] | [stop] | [status] | [dump <destination>]",
+			"(start <destination> [poll_period [trace_size [stop_tmo [wait4halt [skip_size]]]]) | (stop) | (status) | (dump <destination>)",
+	},
+	{
+		.name = "sysview",
+		.handler = esp32_cmd_sysview,
+		.mode = COMMAND_EXEC,
+		.help =
+			"App Tracing: SEGGER SystemView compatible trace control. Starts, stops or queries tracing process status.",
+		.usage =
+			"(start file://<outfile1> [file://<outfile2>] [poll_period [trace_size [stop_tmo [wait4halt [skip_size]]]]) | (stop) | (status)",
+	},
+	{
+		.name = "sysview_mcore",
+		.handler = esp32_cmd_sysview_mcore,
+		.mode = COMMAND_EXEC,
+		.help =
+			"App Tracing: Espressif multi-core SystemView trace control. Starts, stops or queries tracing process status.",
+		.usage =
+			"(start file://<outfile> [poll_period [trace_size [stop_tmo [wait4halt [skip_size]]]]) | (stop) | (status)",
 	},
 	COMMAND_REGISTRATION_DONE
 };
diff --git a/src/target/espressif/esp32_sysview.c b/src/target/espressif/esp32_sysview.c
new file mode 100644
index 0000000..2fe2157
--- /dev/null
+++ b/src/target/espressif/esp32_sysview.c
@@ -0,0 +1,554 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+
+/***************************************************************************
+ *   ESP32 sysview tracing module                                          *
+ *   Copyright (C) 2020 Espressif Systems Ltd.                             *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <helper/log.h>
+#include "esp32_apptrace.h"
+#include "esp32_sysview.h"
+#include "segger_sysview.h"
+
+/* in SystemView mode core ID is passed in event ID field */
+#define ESP32_SYSVIEW_USER_BLOCK_CORE(_v_)  (0)	/* not used */
+#define ESP32_SYSVIEW_USER_BLOCK_LEN(_v_)   (_v_)
+#define ESP32_SYSVIEW_USER_BLOCK_HDR_SZ     2
+
+struct esp_sysview_target2host_hdr {
+	uint8_t block_sz;
+	uint8_t wr_sz;
+};
+#define SYSVIEW_BLOCK_SIZE_OFFSET       0
+#define SYSVIEW_WR_SIZE_OFFSET          1
+
+static int esp_sysview_trace_header_write(struct esp32_apptrace_cmd_ctx *ctx, bool mcore_format);
+static int esp32_sysview_core_id_get(struct target *target, uint8_t *hdr_buf);
+static uint32_t esp32_sysview_usr_block_len_get(struct target *target, uint8_t *hdr_buf, uint32_t *wr_len);
+
+int esp32_sysview_cmd_init(struct esp32_apptrace_cmd_ctx *cmd_ctx,
+	struct command_invocation *cmd,
+	int mode,
+	bool mcore_format,
+	const char **argv,
+	int argc)
+{
+	struct esp32_sysview_cmd_data *cmd_data;
+
+	if (argc < 1) {
+		command_print(cmd, "Not enough args! Need trace data destination!");
+		return ERROR_FAIL;
+	}
+
+	int res = esp32_apptrace_cmd_ctx_init(cmd_ctx, cmd, mode);
+	if (res != ERROR_OK)
+		return res;
+
+	int core_num = cmd_ctx->cores_num;
+
+	if (!mcore_format && argc < core_num) {
+		command_print(cmd, "Not enough args! Need %d trace data destinations!", core_num);
+		res = ERROR_FAIL;
+		goto on_error;
+	}
+
+	cmd_data = calloc(1, sizeof(*cmd_data));
+	if (!cmd_data) {
+		command_print(cmd, "No memory for command data!");
+		res = ERROR_FAIL;
+		goto on_error;
+	}
+	cmd_ctx->cmd_priv = cmd_data;
+	cmd_data->mcore_format = mcore_format;
+
+	/*outfile1 [outfile2] [poll_period [trace_size [stop_tmo [wait4halt [skip_size]]]]] */
+	int dests_num = esp32_apptrace_dest_init(cmd_data->data_dests, argv, !mcore_format ? core_num : 1);
+	if (!mcore_format && dests_num < core_num) {
+		command_print(cmd, "Not enough args! Need %d trace data destinations!", core_num);
+		free(cmd_data);
+		res = ERROR_FAIL;
+		goto on_error;
+	}
+	cmd_data->apptrace.max_len = UINT32_MAX;
+	cmd_data->apptrace.poll_period = 0 /*ms*/;
+	cmd_ctx->stop_tmo = -1.0;	/* infinite */
+	if (argc > dests_num) {
+		/* parse remaining args */
+		esp32_apptrace_cmd_args_parse(cmd_ctx,
+			&cmd_data->apptrace,
+			&argv[dests_num],
+			argc - dests_num);
+	}
+	LOG_USER("App trace params: from %d cores, size %u bytes, stop_tmo %g s, "
+		"poll period %u ms, wait_rst %d, skip %u bytes",
+		cmd_ctx->cores_num,
+		cmd_data->apptrace.max_len,
+		cmd_ctx->stop_tmo,
+		cmd_data->apptrace.poll_period,
+		cmd_data->apptrace.wait4halt,
+		cmd_data->apptrace.skip_len);
+
+	cmd_ctx->trace_format.hdr_sz = ESP32_SYSVIEW_USER_BLOCK_HDR_SZ;
+	cmd_ctx->trace_format.core_id_get = esp32_sysview_core_id_get;
+	cmd_ctx->trace_format.usr_block_len_get = esp32_sysview_usr_block_len_get;
+
+	res = esp_sysview_trace_header_write(cmd_ctx, mcore_format);
+	if (res != ERROR_OK) {
+		command_print(cmd, "Failed to write trace header (%d)!", res);
+		esp32_apptrace_dest_cleanup(cmd_data->data_dests, core_num);
+		free(cmd_data);
+		return res;
+	}
+	return ERROR_OK;
+on_error:
+	cmd_ctx->running = 0;
+	esp32_apptrace_cmd_ctx_cleanup(cmd_ctx);
+	return res;
+}
+
+int esp32_sysview_cmd_cleanup(struct esp32_apptrace_cmd_ctx *cmd_ctx)
+{
+	struct esp32_sysview_cmd_data *cmd_data = cmd_ctx->cmd_priv;
+
+	esp32_apptrace_dest_cleanup(cmd_data->data_dests, cmd_ctx->cores_num);
+	free(cmd_data);
+	cmd_ctx->cmd_priv = NULL;
+	esp32_apptrace_cmd_ctx_cleanup(cmd_ctx);
+	return ERROR_OK;
+}
+
+static int esp32_sysview_core_id_get(struct target *target, uint8_t *hdr_buf)
+{
+	/* for sysview compressed apptrace header is used, so core id is encoded in sysview packet */
+	return 0;
+}
+
+static uint32_t esp32_sysview_usr_block_len_get(struct target *target, uint8_t *hdr_buf, uint32_t *wr_len)
+{
+	*wr_len = ESP32_SYSVIEW_USER_BLOCK_LEN(hdr_buf[SYSVIEW_WR_SIZE_OFFSET]);
+	return ESP32_SYSVIEW_USER_BLOCK_LEN(hdr_buf[SYSVIEW_BLOCK_SIZE_OFFSET]);
+}
+
+static int esp_sysview_trace_header_write(struct esp32_apptrace_cmd_ctx *ctx, bool mcore_format)
+{
+	struct esp32_sysview_cmd_data *cmd_data = ctx->cmd_priv;
+	char *hdr_str;
+	int dests_num;
+
+	if (!mcore_format) {
+		hdr_str = ";\n"
+			"; Version     " SYSVIEW_MIN_VER_STRING "\n"
+			"; Author      Espressif Inc\n"
+			";\n";
+		dests_num = ctx->cores_num;
+	} else {
+		hdr_str = ";\n"
+			"; Version     " SYSVIEW_MIN_VER_STRING "\n"
+			"; Author      Espressif Inc\n"
+			"; ESP_Extension\n"
+			";\n";
+		dests_num = 1;
+	}
+
+	int hdr_len = strlen(hdr_str);
+	for (int i = 0; i < dests_num; i++) {
+		int res = cmd_data->data_dests[i].write(cmd_data->data_dests[i].priv,
+			(uint8_t *)hdr_str,
+			hdr_len);
+		if (res != ERROR_OK) {
+			LOG_ERROR("sysview: Failed to write %u bytes to dest %d!", hdr_len, i);
+			return ERROR_FAIL;
+		}
+	}
+	return ERROR_OK;
+}
+
+static void sysview_encode_u32(uint8_t **dest, uint32_t val)
+{
+	uint8_t *sv_ptr = *dest;
+	while (val > 0x7F) {
+		*sv_ptr++ = (uint8_t)(val | 0x80);
+		val >>= 7;
+	}
+	*sv_ptr++ = (uint8_t)val;
+	*dest = sv_ptr;
+}
+
+static uint32_t esp_sysview_decode_u32(uint8_t **ptr)
+{
+	uint32_t val = 0;
+	for (int k = 0;; k++, (*ptr)++) {
+		if (**ptr & 0x80) {
+			val |= (uint32_t)(**ptr & ~0x80) << 7 * k;
+		} else {
+			val |= (uint32_t)**ptr << 7 * k;
+			(*ptr)++;
+			break;
+		}
+	}
+	return val;
+}
+
+static uint16_t esp_sysview_decode_plen(uint8_t **ptr)
+{
+	uint16_t payload_len = 0;
+	uint8_t *p = *ptr;
+	/* here pkt points to encoded payload length */
+	if (*p & 0x80) {
+		payload_len = *(p + 1);	/* higher part */
+		payload_len = (payload_len << 7) | (*p & ~0x80);/* lower 7 bits */
+		p += 2;	/* payload len (2 bytes) */
+	} else {
+		payload_len = *p;
+		p++;	/* payload len (1 byte) */
+	}
+	*ptr = p;
+
+	return payload_len;
+}
+
+static uint16_t esp_sysview_get_predef_payload_len(uint16_t id, uint8_t *pkt)
+{
+	uint16_t len;
+	uint8_t *ptr = pkt;
+
+	switch (id) {
+	case SYSVIEW_EVTID_OVERFLOW:
+	case SYSVIEW_EVTID_ISR_ENTER:
+	case SYSVIEW_EVTID_TASK_START_EXEC:
+	case SYSVIEW_EVTID_TASK_START_READY:
+	case SYSVIEW_EVTID_TASK_CREATE:
+	case SYSVIEW_EVTID_SYSTIME_CYCLES:
+	case SYSVIEW_EVTID_USER_START:
+	case SYSVIEW_EVTID_USER_STOP:
+	case SYSVIEW_EVTID_TIMER_ENTER:
+		/*ENCODE_U32 */
+		esp_sysview_decode_u32(&ptr);
+		len = ptr - pkt;
+		break;
+	case SYSVIEW_EVTID_TASK_STOP_READY:
+	case SYSVIEW_EVTID_SYSTIME_US:
+		/*2*ENCODE_U32 */
+		esp_sysview_decode_u32(&ptr);
+		esp_sysview_decode_u32(&ptr);
+		len = ptr - pkt;
+		break;
+	case SYSVIEW_EVTID_SYSDESC:
+		/*str(128 + 1) */
+		len = *ptr + 1;
+		break;
+	case SYSVIEW_EVTID_TASK_INFO:
+	case SYSVIEW_EVTID_MODULEDESC:
+		/*2*ENCODE_U32 + str */
+		esp_sysview_decode_u32(&ptr);
+		esp_sysview_decode_u32(&ptr);
+		/* TODO: add support for strings longer then 255 bytes */
+		len = ptr - pkt + *ptr + 1;
+		break;
+	case SYSVIEW_EVTID_STACK_INFO:
+		/*4*ENCODE_U32 */
+		esp_sysview_decode_u32(&ptr);
+		esp_sysview_decode_u32(&ptr);
+		esp_sysview_decode_u32(&ptr);
+		esp_sysview_decode_u32(&ptr);
+		len = ptr - pkt;
+		break;
+	case SYSVIEW_EVTID_ISR_EXIT:
+	case SYSVIEW_EVTID_TASK_STOP_EXEC:
+	case SYSVIEW_EVTID_TRACE_START:
+	case SYSVIEW_EVTID_TRACE_STOP:
+	case SYSVIEW_EVTID_IDLE:
+	case SYSVIEW_EVTID_ISR_TO_SCHEDULER:
+	case SYSVIEW_EVTID_TIMER_EXIT:
+		len = 0;
+		break;
+
+	/*case SYSVIEW_EVTID_NOP: */
+	default:
+		LOG_ERROR("sysview: Unsupported predef event %d!", id);
+		len = 0;
+	}
+	return len;
+}
+
+static uint16_t esp_sysview_parse_packet(uint8_t *pkt_buf,
+	uint32_t *pkt_len,
+	unsigned int *pkt_core_id,
+	uint32_t *delta,
+	uint32_t *delta_len,
+	bool clear_core_bit)
+{
+	uint8_t *pkt = pkt_buf;
+	uint16_t event_id = 0, payload_len = 0;
+
+	*pkt_core_id = 0;
+	*pkt_len = 0;
+	/* 1-2 byte of message type, 0-2  byte of payload length, payload, 1-5 bytes of timestamp. */
+	if (*pkt & 0x80) {
+		if (*(pkt + 1) & (1 << 6)) {
+			if (clear_core_bit)
+				*(pkt + 1) &= ~(1 << 6);	/* clear core_id bit */
+			*pkt_core_id = 1;
+		}
+		event_id = *(pkt + 1) & ~(1 << 6);	/* higher part */
+		event_id = (event_id << 7) | (*pkt & ~0x80);	/* lower 7 bits */
+		pkt += 2;	/* event_id (2 bytes) */
+		/* here pkt points to encoded payload length */
+		payload_len = esp_sysview_decode_plen(&pkt);
+	} else {
+		if (*pkt & (1 << 6)) {
+			if (clear_core_bit)
+				*pkt &= ~(1 << 6);	/* clear core_id bit */
+			*pkt_core_id = 1;
+		}
+		/* event_id (1 byte) */
+		event_id = *pkt & ~(1 << 6);
+		pkt++;
+		if (event_id < 24)
+			payload_len = esp_sysview_get_predef_payload_len(event_id, pkt);
+		else
+			payload_len = esp_sysview_decode_plen(&pkt);
+	}
+	pkt += payload_len;
+	uint8_t *delta_start = pkt;
+	*delta = esp_sysview_decode_u32(&pkt);
+	*delta_len = pkt - delta_start;
+	*pkt_len = pkt - pkt_buf;
+	LOG_DEBUG("sysview: evt %d len %d plen %d dlen %d",
+		event_id,
+		*pkt_len,
+		payload_len,
+		*delta_len);
+	return event_id;
+}
+
+static int esp32_sysview_write_packet(struct esp32_sysview_cmd_data *cmd_data,
+	int pkt_core_id, uint32_t pkt_len, uint8_t *pkt_buf, uint32_t delta_len, uint8_t *delta_buf)
+{
+	if (!cmd_data->data_dests[pkt_core_id].write)
+		return ERROR_FAIL;
+
+	int res = cmd_data->data_dests[pkt_core_id].write(cmd_data->data_dests[pkt_core_id].priv, pkt_buf, pkt_len);
+
+	if (res != ERROR_OK) {
+		LOG_ERROR("sysview: Failed to write %u bytes to dest %d!", pkt_len, pkt_core_id);
+		return res;
+	}
+	if (delta_len) {
+		/* write packet with modified delta */
+		res = cmd_data->data_dests[pkt_core_id].write(cmd_data->data_dests[pkt_core_id].priv, delta_buf, delta_len);
+		if (res != ERROR_OK) {
+			LOG_ERROR("sysview: Failed to write %u bytes of delta to dest %d!", delta_len, pkt_core_id);
+			return res;
+		}
+	}
+	return ERROR_OK;
+}
+
+static int esp32_sysview_process_packet(struct esp32_apptrace_cmd_ctx *ctx,
+	unsigned int pkt_core_id, uint16_t event_id, uint32_t delta, uint32_t delta_len,
+	uint32_t pkt_len, uint8_t *pkt_buf)
+{
+	struct esp32_sysview_cmd_data *cmd_data = ctx->cmd_priv;
+	int pkt_core_changed = 0;
+	uint32_t new_delta_len = 0;
+	uint8_t new_delta_buf[10];
+	uint32_t wr_len = pkt_len;
+
+	if (ctx->cores_num > 1) {
+		if (cmd_data->sv_last_core_id == pkt_core_id) {
+			/* if this packet is for the same core as the prev one acc delta and write packet unmodified */
+			cmd_data->sv_acc_time_delta += delta;
+		} else {
+			/* if this packet is for another core then prev one set acc delta to the packet's delta */
+			uint8_t *delta_ptr = new_delta_buf;
+			sysview_encode_u32(&delta_ptr, delta + cmd_data->sv_acc_time_delta);
+			cmd_data->sv_acc_time_delta = delta;
+			wr_len -= delta_len;
+			new_delta_len = delta_ptr - new_delta_buf;
+			pkt_core_changed = 1;
+		}
+		cmd_data->sv_last_core_id = pkt_core_id;
+	}
+	if (pkt_core_id >= ctx->cores_num) {
+		LOG_WARNING("sysview: invalid core ID in packet %d, must be less then %d! Event id %d",
+			pkt_core_id,
+			ctx->cores_num,
+			event_id);
+		return ERROR_FAIL;
+	}
+	int res = esp32_sysview_write_packet(cmd_data,
+		pkt_core_id,
+		wr_len,
+		pkt_buf,
+		new_delta_len,
+		new_delta_buf);
+	if (res != ERROR_OK)
+		return res;
+	for (unsigned int i = 0; i < ctx->cores_num; i++) {
+		if (pkt_core_id == i)
+			continue;
+		switch (event_id) {
+		/* messages below should be sent to trace destinations for all cores */
+		case SYSVIEW_EVTID_TRACE_START:
+		case SYSVIEW_EVTID_TRACE_STOP:
+		case SYSVIEW_EVTID_SYSTIME_CYCLES:
+		case SYSVIEW_EVTID_SYSTIME_US:
+		case SYSVIEW_EVTID_SYSDESC:
+		case SYSVIEW_EVTID_TASK_INFO:
+		case SYSVIEW_EVTID_STACK_INFO:
+		case SYSVIEW_EVTID_MODULEDESC:
+		case SYSVIEW_EVTID_INIT:
+		case SYSVIEW_EVTID_NUMMODULES:
+		case SYSVIEW_EVTID_OVERFLOW:
+		case SYSVIEW_EVTID_TASK_START_READY:
+			/* if packet's source core has changed */
+			wr_len = pkt_len;
+			if (pkt_core_changed) {
+				/* clone packet with unmodified delta */
+				new_delta_len = 0;
+			} else {
+				/* clone packet with modified delta */
+				uint8_t *delta_ptr = new_delta_buf;
+				sysview_encode_u32(&delta_ptr, cmd_data->sv_acc_time_delta /*delta has been accumulated above*/);
+				wr_len -= delta_len;
+				new_delta_len = delta_ptr - new_delta_buf;
+			}
+			LOG_DEBUG("sysview: Redirect %d bytes of event %d to dest %d", wr_len, event_id, i);
+			res = esp32_sysview_write_packet(cmd_data,
+				i,
+				wr_len,
+				pkt_buf,
+				new_delta_len,
+				new_delta_buf);
+			if (res != ERROR_OK)
+				return res;
+			/* messages above are cloned to trace files for both cores,
+			* so reset acc time delta, both files have actual delta
+			* info */
+			cmd_data->sv_acc_time_delta = 0;
+			break;
+		default:
+			break;
+		}
+	}
+	return ERROR_OK;
+}
+
+int esp32_sysview_process_data(struct esp32_apptrace_cmd_ctx *ctx,
+	unsigned int core_id,
+	uint8_t *data,
+	uint32_t data_len)
+{
+	struct esp32_sysview_cmd_data *cmd_data = ctx->cmd_priv;
+
+	LOG_DEBUG("sysview: Read from target %d bytes [%x %x %x %x]",
+		data_len,
+		data[0],
+		data[1],
+		data[2],
+		data[3]);
+	int res;
+	uint32_t processed = 0;
+	if (core_id >= ctx->cores_num) {
+		LOG_ERROR("sysview: Invalid core id %d in user block!", core_id);
+		return ERROR_FAIL;
+	}
+	if (cmd_data->mcore_format)
+		core_id = 0;
+	if (ctx->tot_len == 0) {
+		/* handle sync seq */
+		if (data_len < SYSVIEW_SYNC_LEN) {
+			LOG_ERROR("sysview: Invalid init seq len %d!", data_len);
+			return ERROR_FAIL;
+		}
+		LOG_DEBUG("sysview: Process %d sync bytes", SYSVIEW_SYNC_LEN);
+		uint8_t sync_seq[SYSVIEW_SYNC_LEN] = { 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0 };
+		if (memcmp(data, sync_seq, SYSVIEW_SYNC_LEN) != 0) {
+			LOG_ERROR("sysview: Invalid init seq [%x %x %x %x %x %x %x %x %x %x]",
+				data[0], data[1], data[2], data[3], data[4], data[5], data[6],
+				data[7], data[8], data[9]);
+			return ERROR_FAIL;
+		}
+		res = cmd_data->data_dests[core_id].write(cmd_data->data_dests[core_id].priv,
+			data,
+			SYSVIEW_SYNC_LEN);
+		if (res != ERROR_OK) {
+			LOG_ERROR("sysview: Failed to write %u sync bytes to dest %d!",
+				SYSVIEW_SYNC_LEN,
+				core_id);
+			return res;
+		}
+		if (!cmd_data->mcore_format) {
+			for (unsigned int i = 0; i < ctx->cores_num; i++) {
+				if (core_id == i)
+					continue;
+				res =
+					cmd_data->data_dests[i].write(cmd_data->data_dests[i].priv,
+					data,
+					SYSVIEW_SYNC_LEN);
+				if (res != ERROR_OK) {
+					LOG_ERROR("sysview: Failed to write %u sync bytes to dest %d!", SYSVIEW_SYNC_LEN, core_id ? 0 : 1);
+					return res;
+				}
+			}
+		}
+		ctx->tot_len += SYSVIEW_SYNC_LEN;
+		processed += SYSVIEW_SYNC_LEN;
+	}
+	while (processed < data_len) {
+		unsigned int pkt_core_id;
+		uint32_t delta_len = 0;
+		uint32_t pkt_len = 0, delta = 0;
+		uint16_t event_id = esp_sysview_parse_packet(data + processed,
+			&pkt_len,
+			&pkt_core_id,
+			&delta,
+			&delta_len,
+			!cmd_data->mcore_format);
+		LOG_DEBUG("sysview: Process packet: core %d, %d id, %d bytes [%x %x %x %x]",
+			pkt_core_id,
+			event_id,
+			pkt_len,
+			data[processed + 0],
+			data[processed + 1],
+			data[processed + 2],
+			data[processed + 3]);
+		if (!cmd_data->mcore_format) {
+			res = esp32_sysview_process_packet(ctx,
+				pkt_core_id,
+				event_id,
+				delta,
+				delta_len,
+				pkt_len,
+				data + processed);
+			if (res != ERROR_OK)
+				return res;
+		} else {
+			res = cmd_data->data_dests[0].write(cmd_data->data_dests[0].priv, data + processed, pkt_len);
+			if (res != ERROR_OK) {
+				LOG_ERROR("sysview: Failed to write %u bytes to dest %d!", pkt_len, 0);
+				return res;
+			}
+		}
+		if (event_id == SYSVIEW_EVTID_TRACE_STOP)
+			cmd_data->sv_trace_running = 0;
+		ctx->tot_len += pkt_len;
+		processed += pkt_len;
+	}
+	LOG_USER("%u ", ctx->tot_len);
+	/* check for stop condition */
+	if (ctx->tot_len > cmd_data->apptrace.skip_len &&
+		(ctx->tot_len - cmd_data->apptrace.skip_len >= cmd_data->apptrace.max_len)) {
+		ctx->running = 0;
+		if (duration_measure(&ctx->read_time) != 0) {
+			LOG_ERROR("Failed to stop trace read time measure!");
+			return ERROR_FAIL;
+		}
+	}
+	return ERROR_OK;
+}
diff --git a/src/target/espressif/esp32_sysview.h b/src/target/espressif/esp32_sysview.h
new file mode 100644
index 0000000..230ce46
--- /dev/null
+++ b/src/target/espressif/esp32_sysview.h
@@ -0,0 +1,38 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+
+/***************************************************************************
+ *   ESP32 sysview tracing module                                          *
+ *   Copyright (C) 2020 Espressif Systems Ltd.                             *
+ ***************************************************************************/
+
+#ifndef OPENOCD_TARGET_ESP32_SYSVIEW_H
+#define OPENOCD_TARGET_ESP32_SYSVIEW_H
+
+#include <stdint.h>
+#include "esp32_apptrace.h"
+
+struct esp32_sysview_cmd_data {
+	/* Should be the first field. Generic apptrace command handling code accesses it */
+	struct esp32_apptrace_cmd_data apptrace;
+	struct esp32_apptrace_dest data_dests[ESP32_APPTRACE_MAX_CORES_NUM];
+	bool mcore_format;
+	uint32_t sv_acc_time_delta;
+	unsigned int sv_last_core_id;
+	int sv_trace_running;
+};
+
+struct esp32_apptrace_cmd_ctx;
+
+int esp32_sysview_cmd_init(struct esp32_apptrace_cmd_ctx *cmd_ctx,
+	struct command_invocation *cmd,
+	int mode,
+	bool mcore_format,
+	const char **argv,
+	int argc);
+int esp32_sysview_cmd_cleanup(struct esp32_apptrace_cmd_ctx *cmd_ctx);
+int esp32_sysview_process_data(struct esp32_apptrace_cmd_ctx *ctx,
+	unsigned int core_id,
+	uint8_t *data,
+	uint32_t data_len);
+
+#endif	/* OPENOCD_TARGET_ESP32_SYSVIEW_H */
diff --git a/src/target/espressif/segger_sysview.h b/src/target/espressif/segger_sysview.h
new file mode 100644
index 0000000..d149cab
--- /dev/null
+++ b/src/target/espressif/segger_sysview.h
@@ -0,0 +1,79 @@
+/* SPDX-License-Identifier: BSD-1-Clause */
+/* SPDX-FileCopyrightText: (c) 1995-2021 SEGGER Microcontroller GmbH. All rights reserved. */
+/* SPDX-FileContributor: 2023 Espressif Systems (Shanghai) CO LTD */
+
+/*
+* The contend below is extracted from files SEGGER_SYSVIEW.h and SEGGER_SYSVIEW_Int.h in:
+* https://www.segger.com/downloads/systemview/systemview_target_src
+* SystemView version: 3.42
+*/
+
+#ifndef OPENOCD_TARGET_SEGGER_SYSVIEW_H
+#define OPENOCD_TARGET_SEGGER_SYSVIEW_H
+
+#define SYSVIEW_EVTID_NOP                 0	/* Dummy packet. */
+#define SYSVIEW_EVTID_OVERFLOW            1
+#define SYSVIEW_EVTID_ISR_ENTER           2
+#define SYSVIEW_EVTID_ISR_EXIT            3
+#define SYSVIEW_EVTID_TASK_START_EXEC     4
+#define SYSVIEW_EVTID_TASK_STOP_EXEC      5
+#define SYSVIEW_EVTID_TASK_START_READY    6
+#define SYSVIEW_EVTID_TASK_STOP_READY     7
+#define SYSVIEW_EVTID_TASK_CREATE         8
+#define SYSVIEW_EVTID_TASK_INFO           9
+#define SYSVIEW_EVTID_TRACE_START         10
+#define SYSVIEW_EVTID_TRACE_STOP          11
+#define SYSVIEW_EVTID_SYSTIME_CYCLES      12
+#define SYSVIEW_EVTID_SYSTIME_US          13
+#define SYSVIEW_EVTID_SYSDESC             14
+#define SYSVIEW_EVTID_USER_START          15
+#define SYSVIEW_EVTID_USER_STOP           16
+#define SYSVIEW_EVTID_IDLE                17
+#define SYSVIEW_EVTID_ISR_TO_SCHEDULER    18
+#define SYSVIEW_EVTID_TIMER_ENTER         19
+#define SYSVIEW_EVTID_TIMER_EXIT          20
+#define SYSVIEW_EVTID_STACK_INFO          21
+#define SYSVIEW_EVTID_MODULEDESC          22
+
+#define SYSVIEW_EVTID_INIT                24
+#define SYSVIEW_EVTID_NAME_RESOURCE       25
+#define SYSVIEW_EVTID_PRINT_FORMATTED     26
+#define SYSVIEW_EVTID_NUMMODULES          27
+#define SYSVIEW_EVTID_END_CALL            28
+#define SYSVIEW_EVTID_TASK_TERMINATE      29
+
+#define SYSVIEW_EVTID_EX                  31
+//
+// SystemView extended events. Sent with ID 31.
+//
+#define SYSVIEW_EVTID_EX_MARK             0
+#define SYSVIEW_EVTID_EX_NAME_MARKER      1
+#define SYSVIEW_EVTID_EX_HEAP_DEFINE      2
+#define SYSVIEW_EVTID_EX_HEAP_ALLOC       3
+#define SYSVIEW_EVTID_EX_HEAP_ALLOC_EX    4
+#define SYSVIEW_EVTID_EX_HEAP_FREE        5
+
+#define SYSVIEW_SYNC_LEN                  10
+
+#define SYSVIEW_EVENT_ID_MAX             (200)
+
+//
+// Commands that Host can send to target
+//
+enum {
+	SEGGER_SYSVIEW_COMMAND_ID_START = 1,
+	SEGGER_SYSVIEW_COMMAND_ID_STOP,
+	SEGGER_SYSVIEW_COMMAND_ID_GET_SYSTIME,
+	SEGGER_SYSVIEW_COMMAND_ID_GET_TASKLIST,
+	SEGGER_SYSVIEW_COMMAND_ID_GET_SYSDESC,
+	SEGGER_SYSVIEW_COMMAND_ID_GET_NUMMODULES,
+	SEGGER_SYSVIEW_COMMAND_ID_GET_MODULEDESC,
+	SEGGER_SYSVIEW_COMMAND_ID_HEARTBEAT = 127,
+	// Extended commands: Commands >= 128 have a second parameter
+	SEGGER_SYSVIEW_COMMAND_ID_GET_MODULE = 128
+};
+
+/* Minimum compatible SEGGER SystemView tool version */
+#define SYSVIEW_MIN_VER_STRING			"SEGGER SystemViewer V2.42"
+
+#endif
diff --git a/src/target/image.c b/src/target/image.c
index 6aa609d..f8de7a2 100644
--- a/src/target/image.c
+++ b/src/target/image.c
@@ -407,10 +407,12 @@ static int image_elf32_read_headers(struct image *image)
 		return ERROR_FILEIO_OPERATION_FAILED;
 	}
 
-	/* count useful segments (loadable) */
+	/* count useful segments (loadable), ignore BSS section */
 	image->num_sections = 0;
 	for (i = 0; i < elf->segment_count; i++)
-		if (field32(elf, elf->segments32[i].p_type) == PT_LOAD)
+		if ((field32(elf,
+			elf->segments32[i].p_type) == PT_LOAD) &&
+			(field32(elf, elf->segments32[i].p_filesz) != 0))
 			image->num_sections++;
 
 	if (image->num_sections == 0) {
@@ -447,8 +449,10 @@ static int image_elf32_read_headers(struct image *image)
 	}
 
 	for (i = 0, j = 0; i < elf->segment_count; i++) {
-		if (field32(elf, elf->segments32[i].p_type) == PT_LOAD) {
-			image->sections[j].size = field32(elf, elf->segments32[i].p_memsz);
+		if ((field32(elf,
+			elf->segments32[i].p_type) == PT_LOAD) &&
+			(field32(elf, elf->segments32[i].p_filesz) != 0)) {
+			image->sections[j].size = field32(elf, elf->segments32[i].p_filesz);
 			if (load_to_vaddr)
 				image->sections[j].base_address = field32(elf,
 						elf->segments32[i].p_vaddr);
@@ -528,10 +532,12 @@ static int image_elf64_read_headers(struct image *image)
 		return ERROR_FILEIO_OPERATION_FAILED;
 	}
 
-	/* count useful segments (loadable) */
+	/* count useful segments (loadable), ignore BSS section */
 	image->num_sections = 0;
 	for (i = 0; i < elf->segment_count; i++)
-		if (field32(elf, elf->segments64[i].p_type) == PT_LOAD)
+		if ((field32(elf,
+			elf->segments64[i].p_type) == PT_LOAD) &&
+			(field64(elf, elf->segments64[i].p_filesz) != 0))
 			image->num_sections++;
 
 	if (image->num_sections == 0) {
@@ -568,8 +574,10 @@ static int image_elf64_read_headers(struct image *image)
 	}
 
 	for (i = 0, j = 0; i < elf->segment_count; i++) {
-		if (field32(elf, elf->segments64[i].p_type) == PT_LOAD) {
-			image->sections[j].size = field64(elf, elf->segments64[i].p_memsz);
+		if ((field32(elf,
+			elf->segments64[i].p_type) == PT_LOAD) &&
+			(field64(elf, elf->segments64[i].p_filesz) != 0)) {
+			image->sections[j].size = field64(elf, elf->segments64[i].p_filesz);
 			if (load_to_vaddr)
 				image->sections[j].base_address = field64(elf,
 						elf->segments64[i].p_vaddr);
@@ -643,8 +651,6 @@ static int image_elf32_read_section(struct image *image,
 {
 	struct image_elf *elf = image->type_private;
 	Elf32_Phdr *segment = (Elf32_Phdr *)image->sections[section].private;
-	uint32_t filesz = field32(elf, segment->p_filesz);
-	uint32_t memsz = field32(elf, segment->p_memsz);
 	size_t read_size, really_read;
 	int retval;
 
@@ -653,9 +659,9 @@ static int image_elf32_read_section(struct image *image,
 	LOG_DEBUG("load segment %d at 0x%" TARGET_PRIxADDR " (sz = 0x%" PRIx32 ")", section, offset, size);
 
 	/* read initialized data in current segment if any */
-	if (offset < filesz) {
+	if (offset < field32(elf, segment->p_filesz)) {
 		/* maximal size present in file for the current segment */
-		read_size = MIN(size, filesz - offset);
+		read_size = MIN(size, field32(elf, segment->p_filesz) - offset);
 		LOG_DEBUG("read elf: size = 0x%zx at 0x%" TARGET_PRIxADDR "", read_size,
 			field32(elf, segment->p_offset) + offset);
 		/* read initialized area of the segment */
@@ -669,8 +675,6 @@ static int image_elf32_read_section(struct image *image,
 			LOG_ERROR("cannot read ELF segment content, read failed");
 			return retval;
 		}
-		buffer += read_size;
-		offset += read_size;
 		size -= read_size;
 		*size_read += read_size;
 		/* need more data ? */
@@ -678,13 +682,6 @@ static int image_elf32_read_section(struct image *image,
 			return ERROR_OK;
 	}
 
-	/* clear bss in current segment if any */
-	if (offset >= filesz) {
-		uint32_t memset_size = MIN(size, memsz - filesz);
-		memset(buffer, 0, memset_size);
-		*size_read += memset_size;
-	}
-
 	return ERROR_OK;
 }
 
@@ -697,8 +694,6 @@ static int image_elf64_read_section(struct image *image,
 {
 	struct image_elf *elf = image->type_private;
 	Elf64_Phdr *segment = (Elf64_Phdr *)image->sections[section].private;
-	uint64_t filesz = field64(elf, segment->p_filesz);
-	uint64_t memsz = field64(elf, segment->p_memsz);
 	size_t read_size, really_read;
 	int retval;
 
@@ -707,9 +702,9 @@ static int image_elf64_read_section(struct image *image,
 	LOG_DEBUG("load segment %d at 0x%" TARGET_PRIxADDR " (sz = 0x%" PRIx32 ")", section, offset, size);
 
 	/* read initialized data in current segment if any */
-	if (offset < filesz) {
+	if (offset < field64(elf, segment->p_filesz)) {
 		/* maximal size present in file for the current segment */
-		read_size = MIN(size, filesz - offset);
+		read_size = MIN(size, field64(elf, segment->p_filesz) - offset);
 		LOG_DEBUG("read elf: size = 0x%zx at 0x%" TARGET_PRIxADDR "", read_size,
 			field64(elf, segment->p_offset) + offset);
 		/* read initialized area of the segment */
@@ -723,8 +718,6 @@ static int image_elf64_read_section(struct image *image,
 			LOG_ERROR("cannot read ELF segment content, read failed");
 			return retval;
 		}
-		buffer += read_size;
-		offset += read_size;
 		size -= read_size;
 		*size_read += read_size;
 		/* need more data ? */
@@ -732,13 +725,6 @@ static int image_elf64_read_section(struct image *image,
 			return ERROR_OK;
 	}
 
-	/* clear bss in current segment if any */
-	if (offset >= filesz) {
-		uint64_t memset_size = MIN(size, memsz - filesz);
-		memset(buffer, 0, memset_size);
-		*size_read += memset_size;
-	}
-
 	return ERROR_OK;
 }
 
diff --git a/src/target/mips32.c b/src/target/mips32.c
index f593b5f..1a34f73 100644
--- a/src/target/mips32.c
+++ b/src/target/mips32.c
@@ -384,7 +384,7 @@ int mips32_init_arch_info(struct target *target, struct mips32_common *mips32, s
 
 /* run to exit point. return error if exit point was not reached. */
 static int mips32_run_and_wait(struct target *target, target_addr_t entry_point,
-		int timeout_ms, target_addr_t exit_point, struct mips32_common *mips32)
+		unsigned int timeout_ms, target_addr_t exit_point, struct mips32_common *mips32)
 {
 	uint32_t pc;
 	int retval;
@@ -418,7 +418,7 @@ static int mips32_run_and_wait(struct target *target, target_addr_t entry_point,
 int mips32_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, int timeout_ms, void *arch_info)
+		target_addr_t exit_point, unsigned int timeout_ms, void *arch_info)
 {
 	struct mips32_common *mips32 = target_to_mips32(target);
 	struct mips32_algorithm *mips32_algorithm_info = arch_info;
@@ -803,7 +803,7 @@ int mips32_checksum_memory(struct target *target, target_addr_t address,
 	init_reg_param(&reg_params[1], "r5", 32, PARAM_OUT);
 	buf_set_u32(reg_params[1].value, 0, 32, count);
 
-	int timeout = 20000 * (1 + (count / (1024 * 1024)));
+	unsigned int timeout = 20000 * (1 + (count / (1024 * 1024)));
 
 	retval = target_run_algorithm(target, 0, NULL, 2, reg_params, crc_algorithm->address,
 				      crc_algorithm->address + (sizeof(mips_crc_code) - 4), timeout, &mips32_info);
diff --git a/src/target/mips32.h b/src/target/mips32.h
index 8837da1..81b6d64 100644
--- a/src/target/mips32.h
+++ b/src/target/mips32.h
@@ -400,7 +400,7 @@ int mips32_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,
-		int timeout_ms, void *arch_info);
+		unsigned int timeout_ms, void *arch_info);
 
 int mips32_configure_break_unit(struct target *target);
 
diff --git a/src/target/mips64.c b/src/target/mips64.c
index 773b92d..37f3685 100644
--- a/src/target/mips64.c
+++ b/src/target/mips64.c
@@ -459,7 +459,7 @@ int mips64_init_arch_info(struct target *target, struct mips64_common *mips64,
 int mips64_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, int timeout_ms, void *arch_info)
+			 target_addr_t exit_point, unsigned int timeout_ms, void *arch_info)
 {
 	/* TODO */
 	return ERROR_OK;
diff --git a/src/target/mips64.h b/src/target/mips64.h
index 9079c80..ae0811c 100644
--- a/src/target/mips64.h
+++ b/src/target/mips64.h
@@ -213,7 +213,7 @@ int mips64_build_reg_cache(struct target *target);
 int mips64_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,
-	int timeout_ms, void *arch_info);
+	unsigned int timeout_ms, void *arch_info);
 int mips64_configure_break_unit(struct target *target);
 int mips64_enable_interrupts(struct target *target, bool enable);
 int mips64_examine(struct target *target);
diff --git a/src/target/riscv/riscv.c b/src/target/riscv/riscv.c
index d3031e8..b2f0633 100644
--- a/src/target/riscv/riscv.c
+++ b/src/target/riscv/riscv.c
@@ -2573,7 +2573,7 @@ static int riscv_arch_state(struct target *target)
 static int riscv_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, int timeout_ms, void *arch_info)
+		target_addr_t exit_point, unsigned int timeout_ms, void *arch_info)
 {
 	RISCV_INFO(info);
 
diff --git a/src/target/semihosting_common.c b/src/target/semihosting_common.c
index dc0dae2..6c91876 100644
--- a/src/target/semihosting_common.c
+++ b/src/target/semihosting_common.c
@@ -39,6 +39,7 @@
 
 #include <helper/binarybuffer.h>
 #include <helper/log.h>
+#include <server/gdb_server.h>
 #include <sys/stat.h>
 
 /**
@@ -92,9 +93,6 @@ static int semihosting_common_fileio_info(struct target *target,
 static int semihosting_common_fileio_end(struct target *target, int result,
 	int fileio_errno, bool ctrl_c);
 
-/* Attempts to include gdb_server.h failed. */
-extern int gdb_actual_connections;
-
 /**
  * Initialize common semihosting support.
  *
@@ -493,7 +491,7 @@ int semihosting_common(struct target *target)
 					int code = semihosting_get_field(target, 1, fields);
 
 					if (type == ADP_STOPPED_APPLICATION_EXIT) {
-						if (!gdb_actual_connections)
+						if (!gdb_get_actual_connections())
 							exit(code);
 						else {
 							fprintf(stderr,
@@ -508,7 +506,7 @@ int semihosting_common(struct target *target)
 				}
 			} else {
 				if (semihosting->param == ADP_STOPPED_APPLICATION_EXIT) {
-					if (!gdb_actual_connections)
+					if (!gdb_get_actual_connections())
 						exit(0);
 					else {
 						fprintf(stderr,
@@ -517,14 +515,14 @@ int semihosting_common(struct target *target)
 				} else if (semihosting->param == ADP_STOPPED_RUN_TIME_ERROR) {
 					/* Chosen more or less arbitrarily to have a nicer message,
 					 * otherwise all other return the same exit code 1. */
-					if (!gdb_actual_connections)
+					if (!gdb_get_actual_connections())
 						exit(1);
 					else {
 						fprintf(stderr,
 							"semihosting: *** application exited with error ***\n");
 					}
 				} else {
-					if (!gdb_actual_connections)
+					if (!gdb_get_actual_connections())
 						exit(1);
 					else {
 						fprintf(stderr,
@@ -584,7 +582,7 @@ int semihosting_common(struct target *target)
 				int code = semihosting_get_field(target, 1, fields);
 
 				if (type == ADP_STOPPED_APPLICATION_EXIT) {
-					if (!gdb_actual_connections)
+					if (!gdb_get_actual_connections())
 						exit(code);
 					else {
 						fprintf(stderr,
@@ -781,7 +779,8 @@ int semihosting_common(struct target *target)
 				if (retval != ERROR_OK)
 					return retval;
 				int fd = semihosting_get_field(target, 0, fields);
-				semihosting->result = isatty(fd);
+				// isatty() on Windows may return any non-zero value if fd is a terminal
+				semihosting->result = isatty(fd) ? 1 : 0;
 				semihosting->sys_errno = errno;
 				LOG_DEBUG("isatty(%d)=%" PRId64, fd, semihosting->result);
 			}
diff --git a/src/target/stm8.c b/src/target/stm8.c
index 9fd6509..91a59d7 100644
--- a/src/target/stm8.c
+++ b/src/target/stm8.c
@@ -1784,7 +1784,7 @@ static int stm8_checksum_memory(struct target *target, target_addr_t address,
 
 /* run to exit point. return error if exit point was not reached. */
 static int stm8_run_and_wait(struct target *target, uint32_t entry_point,
-		int timeout_ms, uint32_t exit_point, struct stm8_common *stm8)
+		unsigned int timeout_ms, uint32_t exit_point, struct stm8_common *stm8)
 {
 	uint32_t pc;
 	int retval;
@@ -1819,7 +1819,7 @@ static int stm8_run_and_wait(struct target *target, uint32_t entry_point,
 static int stm8_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, int timeout_ms, void *arch_info)
+		target_addr_t exit_point, unsigned int timeout_ms, void *arch_info)
 {
 	struct stm8_common *stm8 = target_to_stm8(target);
 
diff --git a/src/target/target.c b/src/target/target.c
index c592a71..4443584 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -210,7 +210,7 @@ static const struct jim_nvp nvp_target_event[] = {
 	{ .name = NULL, .value = -1 }
 };
 
-static const struct jim_nvp nvp_target_state[] = {
+static const struct nvp nvp_target_state[] = {
 	{ .name = "unknown", .value = TARGET_UNKNOWN },
 	{ .name = "running", .value = TARGET_RUNNING },
 	{ .name = "halted",  .value = TARGET_HALTED },
@@ -265,7 +265,7 @@ const char *debug_reason_name(struct target *t)
 const char *target_state_name(struct target *t)
 {
 	const char *cp;
-	cp = jim_nvp_value2name_simple(nvp_target_state, t->state)->name;
+	cp = nvp_value2name(nvp_target_state, t->state)->name;
 	if (!cp) {
 		LOG_ERROR("Invalid target state: %d", (int)(t->state));
 		cp = "(*BUG*unknown*BUG*)";
@@ -819,7 +819,7 @@ int target_run_algorithm(struct target *target,
 		int num_mem_params, struct mem_param *mem_params,
 		int num_reg_params, struct reg_param *reg_param,
 		target_addr_t entry_point, target_addr_t exit_point,
-		int timeout_ms, void *arch_info)
+		unsigned int timeout_ms, void *arch_info)
 {
 	int retval = ERROR_FAIL;
 
@@ -903,7 +903,7 @@ done:
 int target_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, int timeout_ms,
+		target_addr_t exit_point, unsigned int timeout_ms,
 		void *arch_info)
 {
 	int retval = ERROR_FAIL;
@@ -3230,7 +3230,7 @@ COMMAND_HANDLER(handle_wait_halt_command)
  *
  * After 500ms, keep_alive() is invoked
  */
-int target_wait_state(struct target *target, enum target_state state, int ms)
+int target_wait_state(struct target *target, enum target_state state, unsigned int ms)
 {
 	int retval;
 	int64_t then = 0, cur;
@@ -3247,7 +3247,7 @@ int target_wait_state(struct target *target, enum target_state state, int ms)
 			once = false;
 			then = timeval_ms();
 			LOG_DEBUG("waiting for target %s...",
-				jim_nvp_value2name_simple(nvp_target_state, state)->name);
+				nvp_value2name(nvp_target_state, state)->name);
 		}
 
 		if (cur-then > 500)
@@ -3255,7 +3255,7 @@ int target_wait_state(struct target *target, enum target_state state, int ms)
 
 		if ((cur-then) > ms) {
 			LOG_ERROR("timed out while waiting for target %s",
-				jim_nvp_value2name_simple(nvp_target_state, state)->name);
+				nvp_value2name(nvp_target_state, state)->name);
 			return ERROR_FAIL;
 		}
 	}
@@ -5635,55 +5635,40 @@ static int jim_target_array2mem(Jim_Interp *interp,
 	return target_array2mem(interp, target, argc - 1, argv + 1);
 }
 
-static int jim_target_tap_disabled(Jim_Interp *interp)
-{
-	Jim_SetResultFormatted(interp, "[TAP is disabled]");
-	return JIM_ERR;
-}
-
-static int jim_target_examine(Jim_Interp *interp, int argc, Jim_Obj *const *argv)
+COMMAND_HANDLER(handle_target_examine)
 {
 	bool allow_defer = false;
 
-	struct jim_getopt_info goi;
-	jim_getopt_setup(&goi, interp, argc - 1, argv + 1);
-	if (goi.argc > 1) {
-		const char *cmd_name = Jim_GetString(argv[0], NULL);
-		Jim_SetResultFormatted(goi.interp,
-				"usage: %s ['allow-defer']", cmd_name);
-		return JIM_ERR;
-	}
-	if (goi.argc > 0 &&
-	    strcmp(Jim_GetString(argv[1], NULL), "allow-defer") == 0) {
-		/* consume it */
-		Jim_Obj *obj;
-		int e = jim_getopt_obj(&goi, &obj);
-		if (e != JIM_OK)
-			return e;
+	if (CMD_ARGC > 1)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	if (CMD_ARGC == 1) {
+		if (strcmp(CMD_ARGV[0], "allow-defer"))
+			return ERROR_COMMAND_ARGUMENT_INVALID;
 		allow_defer = true;
 	}
 
-	struct command_context *cmd_ctx = current_command_context(interp);
-	assert(cmd_ctx);
-	struct target *target = get_current_target(cmd_ctx);
-	if (!target->tap->enabled)
-		return jim_target_tap_disabled(interp);
+	struct target *target = get_current_target(CMD_CTX);
+	if (!target->tap->enabled) {
+		command_print(CMD, "[TAP is disabled]");
+		return ERROR_FAIL;
+	}
 
 	if (allow_defer && target->defer_examine) {
 		LOG_INFO("Deferring arp_examine of %s", target_name(target));
 		LOG_INFO("Use arp_examine command to examine it manually!");
-		return JIM_OK;
+		return ERROR_OK;
 	}
 
-	int e = target->type->examine(target);
-	if (e != ERROR_OK) {
+	int retval = target->type->examine(target);
+	if (retval != ERROR_OK) {
 		target_reset_examined(target);
-		return JIM_ERR;
+		return retval;
 	}
 
 	target_set_examined(target);
 
-	return JIM_OK;
+	return ERROR_OK;
 }
 
 COMMAND_HANDLER(handle_target_was_examined)
@@ -5791,45 +5776,35 @@ COMMAND_HANDLER(handle_target_halt)
 	return target->type->halt(target);
 }
 
-static int jim_target_wait_state(Jim_Interp *interp, int argc, Jim_Obj *const *argv)
+COMMAND_HANDLER(handle_target_wait_state)
 {
-	struct jim_getopt_info goi;
-	jim_getopt_setup(&goi, interp, argc - 1, argv + 1);
+	if (CMD_ARGC != 2)
+		return ERROR_COMMAND_SYNTAX_ERROR;
 
-	/* params:  <name>  statename timeoutmsecs */
-	if (goi.argc != 2) {
-		const char *cmd_name = Jim_GetString(argv[0], NULL);
-		Jim_SetResultFormatted(goi.interp,
-				"%s <state_name> <timeout_in_msec>", cmd_name);
-		return JIM_ERR;
+	const struct nvp *n = nvp_name2value(nvp_target_state, CMD_ARGV[0]);
+	if (!n->name) {
+		nvp_unknown_command_print(CMD, nvp_target_state, NULL, CMD_ARGV[0]);
+		return ERROR_COMMAND_ARGUMENT_INVALID;
 	}
 
-	struct jim_nvp *n;
-	int e = jim_getopt_nvp(&goi, nvp_target_state, &n);
-	if (e != JIM_OK) {
-		jim_getopt_nvp_unknown(&goi, nvp_target_state, 1);
-		return e;
+	unsigned int a;
+	COMMAND_PARSE_NUMBER(uint, CMD_ARGV[1], a);
+
+	struct target *target = get_current_target(CMD_CTX);
+	if (!target->tap->enabled) {
+		command_print(CMD, "[TAP is disabled]");
+		return ERROR_FAIL;
 	}
-	jim_wide a;
-	e = jim_getopt_wide(&goi, &a);
-	if (e != JIM_OK)
-		return e;
-	struct command_context *cmd_ctx = current_command_context(interp);
-	assert(cmd_ctx);
-	struct target *target = get_current_target(cmd_ctx);
-	if (!target->tap->enabled)
-		return jim_target_tap_disabled(interp);
 
-	e = target_wait_state(target, n->value, a);
-	if (e != ERROR_OK) {
-		Jim_Obj *obj = Jim_NewIntObj(interp, e);
-		Jim_SetResultFormatted(goi.interp,
-				"target: %s wait %s fails (%#s) %s",
+	int retval = target_wait_state(target, n->value, a);
+	if (retval != ERROR_OK) {
+		command_print(CMD,
+				"target: %s wait %s fails (%d) %s",
 				target_name(target), n->name,
-				obj, target_strerror_safe(e));
-		return JIM_ERR;
+				retval, target_strerror_safe(retval));
+		return retval;
 	}
-	return JIM_OK;
+	return ERROR_OK;
 }
 /* List for human, Events defined for this target.
  * scripts/programs should use 'name cget -event NAME'
@@ -6021,7 +5996,7 @@ static const struct command_registration target_instance_command_handlers[] = {
 	{
 		.name = "arp_examine",
 		.mode = COMMAND_EXEC,
-		.jim_handler = jim_target_examine,
+		.handler = handle_target_examine,
 		.help = "used internally for reset processing",
 		.usage = "['allow-defer']",
 	},
@@ -6070,8 +6045,9 @@ static const struct command_registration target_instance_command_handlers[] = {
 	{
 		.name = "arp_waitstate",
 		.mode = COMMAND_EXEC,
-		.jim_handler = jim_target_wait_state,
+		.handler = handle_target_wait_state,
 		.help = "used internally for reset processing",
+		.usage = "statename timeoutmsecs",
 	},
 	{
 		.name = "invoke-event",
@@ -7173,3 +7149,29 @@ static int target_register_user_commands(struct command_context *cmd_ctx)
 
 	return register_commands(cmd_ctx, NULL, target_exec_command_handlers);
 }
+
+const char *target_debug_reason_str(enum target_debug_reason reason)
+{
+	switch (reason) {
+		case DBG_REASON_DBGRQ:
+			return "DBGRQ";
+		case DBG_REASON_BREAKPOINT:
+			return "BREAKPOINT";
+		case DBG_REASON_WATCHPOINT:
+			return "WATCHPOINT";
+		case DBG_REASON_WPTANDBKPT:
+			return "WPTANDBKPT";
+		case DBG_REASON_SINGLESTEP:
+			return "SINGLESTEP";
+		case DBG_REASON_NOTHALTED:
+			return "NOTHALTED";
+		case DBG_REASON_EXIT:
+			return "EXIT";
+		case DBG_REASON_EXC_CATCH:
+			return "EXC_CATCH";
+		case DBG_REASON_UNDEFINED:
+			return "UNDEFINED";
+		default:
+			return "UNKNOWN!";
+	}
+}
diff --git a/src/target/target.h b/src/target/target.h
index da4612c..b8f3b01 100644
--- a/src/target/target.h
+++ b/src/target/target.h
@@ -553,7 +553,7 @@ int target_run_algorithm(struct target *target,
 		int num_mem_params, struct mem_param *mem_params,
 		int num_reg_params, struct reg_param *reg_param,
 		target_addr_t entry_point, target_addr_t exit_point,
-		int timeout_ms, void *arch_info);
+		unsigned int timeout_ms, void *arch_info);
 
 /**
  * Starts an algorithm in the background on the @a target given.
@@ -574,7 +574,7 @@ int target_start_algorithm(struct target *target,
 int target_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, int timeout_ms,
+		target_addr_t exit_point, unsigned int timeout_ms,
 		void *arch_info);
 
 /**
@@ -666,7 +666,7 @@ int target_checksum_memory(struct target *target,
 int target_blank_check_memory(struct target *target,
 		struct target_memory_check_block *blocks, int num_blocks,
 		uint8_t erased_value);
-int target_wait_state(struct target *target, enum target_state state, int ms);
+int target_wait_state(struct target *target, enum target_state state, unsigned int ms);
 
 /**
  * Obtain file-I/O information from target for GDB to do syscall.
@@ -809,4 +809,6 @@ extern bool get_target_reset_nag(void);
 
 #define TARGET_DEFAULT_POLLING_INTERVAL		100
 
+const char *target_debug_reason_str(enum target_debug_reason reason);
+
 #endif /* OPENOCD_TARGET_TARGET_H */
diff --git a/src/target/target_type.h b/src/target/target_type.h
index 5186e9c..678ce0f 100644
--- a/src/target/target_type.h
+++ b/src/target/target_type.h
@@ -181,7 +181,7 @@ struct target_type {
 	int (*run_algorithm)(struct target *target, int num_mem_params,
 			struct mem_param *mem_params, int num_reg_params,
 			struct reg_param *reg_param, target_addr_t entry_point,
-			target_addr_t exit_point, int timeout_ms, void *arch_info);
+			target_addr_t exit_point, unsigned int timeout_ms, void *arch_info);
 	int (*start_algorithm)(struct target *target, int num_mem_params,
 			struct mem_param *mem_params, int num_reg_params,
 			struct reg_param *reg_param, target_addr_t entry_point,
@@ -189,7 +189,7 @@ struct target_type {
 	int (*wait_algorithm)(struct target *target, int num_mem_params,
 			struct mem_param *mem_params, int num_reg_params,
 			struct reg_param *reg_param, target_addr_t exit_point,
-			int timeout_ms, void *arch_info);
+			unsigned int timeout_ms, void *arch_info);
 
 	const struct command_registration *commands;
 
diff --git a/src/target/xtensa/Makefile.am b/src/target/xtensa/Makefile.am
index 94c7c4a..22504e7 100644
--- a/src/target/xtensa/Makefile.am
+++ b/src/target/xtensa/Makefile.am
@@ -8,4 +8,6 @@ noinst_LTLIBRARIES += %D%/libxtensa.la
        %D%/xtensa_chip.h \
        %D%/xtensa_debug_module.c \
        %D%/xtensa_debug_module.h \
+       %D%/xtensa_fileio.c \
+       %D%/xtensa_fileio.h \
        %D%/xtensa_regs.h
diff --git a/src/target/xtensa/xtensa.c b/src/target/xtensa/xtensa.c
index 5880637..431c36a 100644
--- a/src/target/xtensa/xtensa.c
+++ b/src/target/xtensa/xtensa.c
@@ -1544,6 +1544,7 @@ int xtensa_prepare_resume(struct target *target,
 		LOG_TARGET_WARNING(target, "target not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
+	xtensa->halt_request = false;
 
 	if (address && !current) {
 		xtensa_reg_set(target, XT_REG_IDX_PC, address);
diff --git a/src/target/xtensa/xtensa_chip.c b/src/target/xtensa/xtensa_chip.c
index c62992f..668aa3a 100644
--- a/src/target/xtensa/xtensa_chip.c
+++ b/src/target/xtensa/xtensa_chip.c
@@ -15,6 +15,7 @@
 #include <target/arm_adi_v5.h>
 #include <rtos/rtos.h>
 #include "xtensa_chip.h"
+#include "xtensa_fileio.h"
 
 int xtensa_chip_init_arch_info(struct target *target, void *arch_info,
 	struct xtensa_debug_module_config *dm_cfg)
@@ -30,7 +31,10 @@ int xtensa_chip_init_arch_info(struct target *target, void *arch_info,
 
 int xtensa_chip_target_init(struct command_context *cmd_ctx, struct target *target)
 {
-	return xtensa_target_init(cmd_ctx, target);
+	int ret = xtensa_target_init(cmd_ctx, target);
+	if (ret != ERROR_OK)
+		return ret;
+	return xtensa_fileio_init(target);
 }
 
 int xtensa_chip_arch_state(struct target *target)
@@ -45,10 +49,12 @@ static int xtensa_chip_poll(struct target *target)
 
 	if (old_state != TARGET_HALTED && target->state == TARGET_HALTED) {
 		/*Call any event callbacks that are applicable */
-		if (old_state == TARGET_DEBUG_RUNNING)
+		if (old_state == TARGET_DEBUG_RUNNING) {
 			target_call_event_callbacks(target, TARGET_EVENT_DEBUG_HALTED);
-		else
+		} else {
+			xtensa_fileio_detect_proc(target);
 			target_call_event_callbacks(target, TARGET_EVENT_HALTED);
+		}
 	}
 
 	return ret;
@@ -193,4 +199,7 @@ struct target_type xtensa_chip_target = {
 	.gdb_query_custom = xtensa_gdb_query_custom,
 
 	.commands = xtensa_command_handlers,
+
+	.get_gdb_fileio_info = xtensa_get_gdb_fileio_info,
+	.gdb_fileio_end = xtensa_gdb_fileio_end,
 };
diff --git a/src/target/xtensa/xtensa_fileio.c b/src/target/xtensa/xtensa_fileio.c
new file mode 100644
index 0000000..7628fbe
--- /dev/null
+++ b/src/target/xtensa/xtensa_fileio.c
@@ -0,0 +1,195 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+
+/***************************************************************************
+ *   Xtensa Target File-I/O Support for OpenOCD                            *
+ *   Copyright (C) 2020-2023 Cadence Design Systems, Inc.                  *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "xtensa_chip.h"
+#include "xtensa_fileio.h"
+#include "xtensa.h"
+
+#define XTENSA_SYSCALL(x)		XT_INS_BREAK(x, 1, 14)
+#define XTENSA_SYSCALL_SZ		3
+#define XTENSA_SYSCALL_LEN_MAX	255
+
+
+int xtensa_fileio_init(struct target *target)
+{
+	char *idmem = malloc(XTENSA_SYSCALL_LEN_MAX + 1);
+	target->fileio_info = malloc(sizeof(struct gdb_fileio_info));
+	if (!idmem || !target->fileio_info) {
+		LOG_TARGET_ERROR(target, "Out of memory!");
+		free(idmem);
+		free(target->fileio_info);
+		return ERROR_FAIL;
+	}
+	target->fileio_info->identifier = idmem;
+	return ERROR_OK;
+}
+
+/**
+ * Checks for and processes an Xtensa File-IO request.
+ *
+ * Return ERROR_OK if request was found and handled; or
+ * return ERROR_FAIL if no request was detected.
+ */
+int xtensa_fileio_detect_proc(struct target *target)
+{
+	struct xtensa *xtensa = target_to_xtensa(target);
+	int retval;
+
+	xtensa_reg_val_t dbg_cause = xtensa_cause_get(target);
+	if ((dbg_cause & (DEBUGCAUSE_BI | DEBUGCAUSE_BN)) == 0 || xtensa->halt_request)
+		return ERROR_FAIL;
+
+	uint8_t brk_insn_buf[sizeof(uint32_t)] = {0};
+	xtensa_reg_val_t pc = xtensa_reg_get(target, XT_REG_IDX_PC);
+	retval = target_read_memory(target,
+		pc,
+		XTENSA_SYSCALL_SZ,
+		1,
+		(uint8_t *)brk_insn_buf);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("Failed to read break instruction!");
+		return ERROR_FAIL;
+	}
+	if (buf_get_u32(brk_insn_buf, 0, 32) != XTENSA_SYSCALL(xtensa))
+		return ERROR_FAIL;
+
+	LOG_TARGET_DEBUG(target, "File-I/O: syscall breakpoint found at 0x%x", pc);
+	xtensa->proc_syscall = true;
+	return ERROR_OK;
+}
+
+int xtensa_get_gdb_fileio_info(struct target *target, struct gdb_fileio_info *fileio_info)
+{
+	/* fill syscall parameters to file-I/O info */
+	if (!fileio_info) {
+		LOG_ERROR("File-I/O data structure uninitialized");
+		return ERROR_FAIL;
+	}
+
+	struct xtensa *xtensa = target_to_xtensa(target);
+	if (!xtensa->proc_syscall)
+		return ERROR_FAIL;
+
+	xtensa_reg_val_t syscall = xtensa_reg_get(target, XTENSA_SYSCALL_OP_REG);
+	xtensa_reg_val_t arg0 = xtensa_reg_get(target, XT_REG_IDX_A6);
+	xtensa_reg_val_t arg1 = xtensa_reg_get(target, XT_REG_IDX_A3);
+	xtensa_reg_val_t arg2 = xtensa_reg_get(target, XT_REG_IDX_A4);
+	xtensa_reg_val_t arg3 = xtensa_reg_get(target, XT_REG_IDX_A5);
+	int retval = ERROR_OK;
+
+	LOG_TARGET_DEBUG(target, "File-I/O: syscall 0x%x 0x%x 0x%x 0x%x 0x%x",
+		syscall, arg0, arg1, arg2, arg3);
+
+	switch (syscall) {
+	case XTENSA_SYSCALL_OPEN:
+		snprintf(fileio_info->identifier, XTENSA_SYSCALL_LEN_MAX, "open");
+		fileio_info->param_1 = arg0;	// pathp
+		fileio_info->param_2 = arg3;	// len
+		fileio_info->param_3 = arg1;	// flags
+		fileio_info->param_4 = arg2;	// mode
+		break;
+	case XTENSA_SYSCALL_CLOSE:
+		snprintf(fileio_info->identifier, XTENSA_SYSCALL_LEN_MAX, "close");
+		fileio_info->param_1 = arg0;	// fd
+		break;
+	case XTENSA_SYSCALL_READ:
+		snprintf(fileio_info->identifier, XTENSA_SYSCALL_LEN_MAX, "read");
+		fileio_info->param_1 = arg0;	// fd
+		fileio_info->param_2 = arg1;	// bufp
+		fileio_info->param_3 = arg2;	// count
+		break;
+	case XTENSA_SYSCALL_WRITE:
+		snprintf(fileio_info->identifier, XTENSA_SYSCALL_LEN_MAX, "write");
+		fileio_info->param_1 = arg0;	// fd
+		fileio_info->param_2 = arg1;	// bufp
+		fileio_info->param_3 = arg2;	// count
+		break;
+	case XTENSA_SYSCALL_LSEEK:
+		snprintf(fileio_info->identifier, XTENSA_SYSCALL_LEN_MAX, "lseek");
+		fileio_info->param_1 = arg0;	// fd
+		fileio_info->param_2 = arg1;	// offset
+		fileio_info->param_3 = arg2;	// flags
+		break;
+	case XTENSA_SYSCALL_RENAME:
+		snprintf(fileio_info->identifier, XTENSA_SYSCALL_LEN_MAX, "rename");
+		fileio_info->param_1 = arg0;	// old pathp
+		fileio_info->param_2 = arg3;	// old len
+		fileio_info->param_3 = arg1;	// new pathp
+		fileio_info->param_4 = arg2;	// new len
+		break;
+	case XTENSA_SYSCALL_UNLINK:
+		snprintf(fileio_info->identifier, XTENSA_SYSCALL_LEN_MAX, "unlink");
+		fileio_info->param_1 = arg0;	// pathnamep
+		fileio_info->param_2 = arg1;	// len
+		break;
+	case XTENSA_SYSCALL_STAT:
+		snprintf(fileio_info->identifier, XTENSA_SYSCALL_LEN_MAX, "stat");
+		fileio_info->param_1 = arg0;	// pathnamep
+		fileio_info->param_2 = arg2;	// len
+		fileio_info->param_3 = arg1;	// bufp
+		break;
+	case XTENSA_SYSCALL_FSTAT:
+		snprintf(fileio_info->identifier, XTENSA_SYSCALL_LEN_MAX, "fstat");
+		fileio_info->param_1 = arg0;	// fd
+		fileio_info->param_2 = arg1;	// bufp
+		break;
+	case XTENSA_SYSCALL_GETTIMEOFDAY:
+		snprintf(fileio_info->identifier, XTENSA_SYSCALL_LEN_MAX, "gettimeofday");
+		fileio_info->param_1 = arg0;	// tvp
+		fileio_info->param_2 = arg1;	// tzp
+		break;
+	case XTENSA_SYSCALL_ISATTY:
+		snprintf(fileio_info->identifier, XTENSA_SYSCALL_LEN_MAX, "isatty");
+		fileio_info->param_1 = arg0;	// fd
+		break;
+	case XTENSA_SYSCALL_SYSTEM:
+		snprintf(fileio_info->identifier, XTENSA_SYSCALL_LEN_MAX, "system");
+		fileio_info->param_1 = arg0;	// cmdp
+		fileio_info->param_2 = arg1;	// len
+		break;
+	default:
+		snprintf(fileio_info->identifier, XTENSA_SYSCALL_LEN_MAX, "unknown");
+		LOG_TARGET_DEBUG(target, "File-I/O: syscall unknown (%d), pc=0x%08X",
+			syscall, xtensa_reg_get(target, XT_REG_IDX_PC));
+		LOG_INFO("File-I/O: syscall unknown (%d), pc=0x%08X",
+			syscall, xtensa_reg_get(target, XT_REG_IDX_PC));
+		retval = ERROR_FAIL;
+		break;
+	}
+
+	return retval;
+}
+
+int xtensa_gdb_fileio_end(struct target *target, int retcode, int fileio_errno, bool ctrl_c)
+{
+	struct xtensa *xtensa = target_to_xtensa(target);
+	if (!xtensa->proc_syscall)
+		return ERROR_FAIL;
+
+	LOG_TARGET_DEBUG(target, "File-I/O: syscall return code: 0x%x, errno: 0x%x , ctrl_c: %s",
+		retcode, fileio_errno, ctrl_c ? "true" : "false");
+
+	/* If interrupt was requested before FIO completion (ERRNO==4), halt and repeat
+	 * syscall. Otherwise, set File-I/O Ax and underlying ARx registers, increment PC.
+	 * NOTE: sporadic cases of ((ERRNO==4) && !ctrl_c) were observed; most have ctrl_c.
+	 */
+	if (fileio_errno != 4) {
+		xtensa_reg_set_deep_relgen(target, XTENSA_SYSCALL_RETVAL_REG, retcode);
+		xtensa_reg_set_deep_relgen(target, XTENSA_SYSCALL_ERRNO_REG, fileio_errno);
+
+		xtensa_reg_val_t pc = xtensa_reg_get(target, XT_REG_IDX_PC);
+		xtensa_reg_set(target, XT_REG_IDX_PC, pc + XTENSA_SYSCALL_SZ);
+	}
+
+	xtensa->proc_syscall = false;
+	xtensa->halt_request = true;
+	return ctrl_c ? ERROR_FAIL : ERROR_OK;
+}
diff --git a/src/target/xtensa/xtensa_fileio.h b/src/target/xtensa/xtensa_fileio.h
new file mode 100644
index 0000000..5e1af5f
--- /dev/null
+++ b/src/target/xtensa/xtensa_fileio.h
@@ -0,0 +1,37 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+
+/***************************************************************************
+ *   Xtensa Target File-I/O Support for OpenOCD                            *
+ *   Copyright (C) 2020-2023 Cadence Design Systems, Inc.                  *
+ ***************************************************************************/
+
+#ifndef OPENOCD_TARGET_XTENSA_FILEIO_H
+#define OPENOCD_TARGET_XTENSA_FILEIO_H
+
+#include <target/target.h>
+#include <helper/command.h>
+#include "xtensa.h"
+
+#define XTENSA_SYSCALL_OP_REG		XT_REG_IDX_A2
+#define XTENSA_SYSCALL_RETVAL_REG	XT_REG_IDX_A2
+#define XTENSA_SYSCALL_ERRNO_REG	XT_REG_IDX_A3
+
+#define XTENSA_SYSCALL_OPEN			(-2)
+#define XTENSA_SYSCALL_CLOSE		(-3)
+#define XTENSA_SYSCALL_READ			(-4)
+#define XTENSA_SYSCALL_WRITE		(-5)
+#define XTENSA_SYSCALL_LSEEK		(-6)
+#define XTENSA_SYSCALL_RENAME		(-7)
+#define XTENSA_SYSCALL_UNLINK		(-8)
+#define XTENSA_SYSCALL_STAT			(-9)
+#define XTENSA_SYSCALL_FSTAT		(-10)
+#define XTENSA_SYSCALL_GETTIMEOFDAY	(-11)
+#define XTENSA_SYSCALL_ISATTY		(-12)
+#define XTENSA_SYSCALL_SYSTEM		(-13)
+
+int xtensa_fileio_init(struct target *target);
+int xtensa_fileio_detect_proc(struct target *target);
+int xtensa_get_gdb_fileio_info(struct target *target, struct gdb_fileio_info *fileio_info);
+int xtensa_gdb_fileio_end(struct target *target, int retcode, int fileio_errno, bool ctrl_c);
+
+#endif	/* OPENOCD_TARGET_XTENSA_FILEIO_H */