Merge commit 'ee31f1578a333a75737bc5b183cd4ae98cdaf798' into from_upstream

Conflicts:
	Makefile.am
	jimtcl
	src/helper/Makefile.am
	src/rtos/rtos.c
	src/rtos/rtos.h
	src/rtos/rtos_standard_stackings.c

Change-Id: I00c98d20089558744988184370a8cb7f95f03329

66 files changed, 2433 insertions, 1127 deletions

diff --git a/Makefile.am b/Makefile.am
index bc7ab21..c69507d 100644
--- a/Makefile.am
+++ b/Makefile.am
@@ -26,10 +26,14 @@ noinst_LTLIBRARIES =
 info_TEXINFOS =
 dist_man_MANS =
 EXTRA_DIST =
+DISTCLEANFILES =
 
 if INTERNAL_JIMTCL
 SUBDIRS += jimtcl
 DIST_SUBDIRS += jimtcl
+EXTRA_DIST += jimtcl/configure.gnu
+# jimtcl from 0.79 to 0.82 miss cleaning jsmn.o
+DISTCLEANFILES += jimtcl/jsmn/jsmn.o
 endif
 
 # common flags used in openocd build
@@ -131,18 +135,9 @@ uninstall-hook:
 
 distclean-local:
 	rm -rf Doxyfile doxygen
-	rm -f $(srcdir)/jimtcl/configure.gnu
-
-# We want every change to have Signed-off-by. This is tricky to enforce in
-# Travis, because it automatically makes temporary commits when merging. So
-# instead we have a hook that enforces this in each workspace. To make sure
-# that users actually use those hooks, we point git at them here.
-# If git fails for some reason, that's OK. It's probably because somebody is
-# building the source completely outside a git repo.
-all-local:
-	cd $(srcdir) && git config core.hooksPath ./git-hooks || true
-
-DISTCLEANFILES = doxygen.log
+	-rm -f $(srcdir)/jimtcl/configure.gnu
+
+DISTCLEANFILES += doxygen.log
 
 METASOURCES = AUTO
 
diff --git a/README b/README
index 2f71cfc..7d3f10d 100644
--- a/README
+++ b/README
@@ -240,8 +240,7 @@ Optional CMSIS-DAP adapter driver needs HIDAPI library.
 
 Optional linuxgpiod adapter driver needs libgpiod library.
 
-Optional JLink adapter driver needs libjaylink; build from git can
-retrieve libjaylink as git submodule.
+Optional J-Link adapter driver needs libjaylink library.
 
 Optional ARM disassembly needs capstone library.
 
diff --git a/TODO b/TODO
index ebb6c99..e4dded0 100644
--- a/TODO
+++ b/TODO
@@ -202,8 +202,6 @@ https://lists.berlios.de/pipermail/openocd-development/2009-October/011506.html
 - MC1322x support (JW/DE?)
   - integrate and test support from JW (and DE?)
   - get working with a known good interface (i.e. not today's jlink)
-- AT91SAM92xx:
-  - improvements for unknown-board-atmel-at91sam9260.cfg (RD)
 - STR9x: (ZW)
   - improvements to str912.cfg to be more general purpose
 - AVR: (SQ)
diff --git a/configure.ac b/configure.ac
index a32fe89..b72253e 100644
--- a/configure.ac
+++ b/configure.ac
@@ -572,9 +572,9 @@ AS_IF([test "x$enable_buspirate" != "xno"], [
 AS_IF([test "x$use_internal_jimtcl" = "xyes"], [
   AS_IF([test -f "$srcdir/jimtcl/configure"], [
     AS_IF([test "x$use_internal_jimtcl_maintainer" = "xyes"], [
-      jimtcl_config_options="--disable-install-jim --with-ext=json --maintainer"
+      jimtcl_config_options="--disable-install-jim --with-ext=json --minimal --disable-ssl --maintainer"
     ], [
-      jimtcl_config_options="--disable-install-jim --with-ext=json"
+      jimtcl_config_options="--disable-install-jim --with-ext=json --minimal --disable-ssl"
     ])
     AX_CONFIG_SUBDIR_OPTION([jimtcl], [$jimtcl_config_options])
   ], [
diff --git a/doc/openocd.texi b/doc/openocd.texi
index fb2eaa9..a64d483 100644
--- a/doc/openocd.texi
+++ b/doc/openocd.texi
@@ -5031,7 +5031,7 @@ The value should normally correspond to a static mapping for the
 @var{rtos_type} can be one of @option{auto}, @option{none}, @option{eCos},
 @option{ThreadX}, @option{FreeRTOS}, @option{linux}, @option{ChibiOS},
 @option{embKernel}, @option{mqx}, @option{uCOS-III}, @option{nuttx},
-@option{RIOT}, @option{Zephyr}
+@option{RIOT}, @option{Zephyr}, @option{rtkernel}
 @xref{gdbrtossupport,,RTOS Support}.
 
 @item @code{-defer-examine} -- skip target examination at initial JTAG chain
@@ -11651,7 +11651,8 @@ In a debug session using JTAG for its transport protocol,
 OpenOCD supports running such test files.
 
 @deffn {Command} {svf} @file{filename} [@option{-tap @var{tapname}}] [@option{[-]quiet}] @
-                     [@option{[-]nil}] [@option{[-]progress}] [@option{[-]ignore_error}]
+                     [@option{[-]nil}] [@option{[-]progress}] [@option{[-]ignore_error}] @
+                     [@option{-noreset}] [@option{-addcycles @var{cyclecount}}]
 This issues a JTAG reset (Test-Logic-Reset) and then
 runs the SVF script from @file{filename}.
 
@@ -11670,6 +11671,10 @@ on the real interface;
 @item @option{[-]progress} enable progress indication;
 @item @option{[-]ignore_error} continue execution despite TDO check
 errors.
+@item @option{-noreset} omit JTAG reset (Test-Logic-Reset) before executing
+content of the SVF file;
+@item @option{-addcycles @var{cyclecount}} inject @var{cyclecount} number of
+additional TCLK cycles after each SDR scan instruction;
 @end itemize
 @end deffn
 
@@ -12085,6 +12090,7 @@ Currently supported rtos's include:
 @item @option{RIOT}
 @item @option{hwthread} (This is not an actual RTOS. @xref{usingopenocdsmpwithgdb,,Using OpenOCD SMP with GDB}.)
 @item @option{Zephyr}
+@item @option{rtkernel}
 @end itemize
 
 At any time, it's possible to drop the selected RTOS using:
@@ -12104,7 +12110,7 @@ _tx_thread_current_ptr, _tx_thread_created_ptr, _tx_thread_created_count.
 @raggedright
 pxCurrentTCB, pxReadyTasksLists, xDelayedTaskList1, xDelayedTaskList2,
 pxDelayedTaskList, pxOverflowDelayedTaskList, xPendingReadyList,
-uxCurrentNumberOfTasks, uxTopUsedPriority.
+uxCurrentNumberOfTasks, uxTopUsedPriority, xSchedulerRunning.
 @end raggedright
 @item linux symbols
 init_task.
@@ -12126,6 +12132,8 @@ _tcb_name_offset.
 @end raggedright
 @item Zephyr symbols
 _kernel, _kernel_openocd_offsets, _kernel_openocd_size_t_size
+@item rtkernel symbols
+Multiple struct offsets.
 @end table
 
 For most RTOS supported the above symbols will be exported by default. However for
diff --git a/jimtcl b/jimtcl
-Subproject 70b007b63669a709b0e8aef34a22658047815cc
+Subproject 1933e5457b9512d39ebbe11ed32578aada149f4
diff --git a/src/flash/nor/at91samd.c b/src/flash/nor/at91samd.c
index 33e86c7..416f077 100644
--- a/src/flash/nor/at91samd.c
+++ b/src/flash/nor/at91samd.c
@@ -243,8 +243,12 @@ static const struct samd_part saml21_parts[] = {
 	{ 0x1F, "SAMR30E18A", 256, 32 },
 
     /* SAMR34/R35 parts have integrated SAML21 with a lora radio */
-	{ 0x28, "SAMR34J18", 256, 32 },
-	{ 0x2B, "SAMR35J18", 256, 32 },
+	{ 0x28, "SAMR34J18", 256, 40 },
+	{ 0x29, "SAMR34J17", 128, 24 },
+	{ 0x2A, "SAMR34J16", 64, 12 },
+	{ 0x2B, "SAMR35J18", 256, 40 },
+	{ 0x2C, "SAMR35J17", 128, 24 },
+	{ 0x2D, "SAMR35J16", 64, 12 },
 };
 
 /* Known SAML22 parts. */
diff --git a/src/flash/nor/spi.c b/src/flash/nor/spi.c
index 373a9a1..3bcaa9f 100644
--- a/src/flash/nor/spi.c
+++ b/src/flash/nor/spi.c
@@ -23,7 +23,14 @@
   * from device datasheets and Linux SPI flash drivers. */
 const struct flash_device flash_devices[] = {
 	/* name, read_cmd, qread_cmd, pprog_cmd, erase_cmd, chip_erase_cmd, device_id,
-	 * pagesize, sectorsize, size_in_bytes */
+	 * pagesize, sectorsize, size_in_bytes
+	 * note: device id is usually 3 bytes long, however the unused highest byte counts
+	 * continuation codes for manufacturer id as per JEP106xx */
+	FLASH_ID("st m25pe10",          0x03, 0x00, 0x02, 0xd8, 0x00, 0x00118020, 0x100, 0x10000, 0x20000),
+	FLASH_ID("st m25pe20",          0x03, 0x00, 0x02, 0xd8, 0x00, 0x00128020, 0x100, 0x10000, 0x40000),
+	FLASH_ID("st m25pe40",          0x03, 0x00, 0x02, 0xd8, 0x00, 0x00138020, 0x100, 0x10000, 0x80000),
+	FLASH_ID("st m25pe80",          0x03, 0x00, 0x02, 0xd8, 0xc7, 0x00148020, 0x100, 0x10000, 0x100000),
+	FLASH_ID("st m25pe16",          0x03, 0x00, 0x02, 0xd8, 0xc7, 0x00158020, 0x100, 0x10000, 0x200000),
 	FLASH_ID("st m25p05",           0x03, 0x00, 0x02, 0xd8, 0xc7, 0x00102020, 0x80,  0x8000,  0x10000),
 	FLASH_ID("st m25p10",           0x03, 0x00, 0x02, 0xd8, 0xc7, 0x00112020, 0x80,  0x8000,  0x20000),
 	FLASH_ID("st m25p20",           0x03, 0x00, 0x02, 0xd8, 0xc7, 0x00122020, 0x100, 0x10000, 0x40000),
@@ -51,12 +58,28 @@ const struct flash_device flash_devices[] = {
 	FLASH_ID("cyp s25fl064l",       0x03, 0x00, 0x02, 0xd8, 0xc7, 0x00176001, 0x100, 0x10000, 0x800000),
 	FLASH_ID("cyp s25fl128l",       0x03, 0x00, 0x02, 0xd8, 0xc7, 0x00186001, 0x100, 0x10000, 0x1000000),
 	FLASH_ID("cyp s25fl256l",       0x13, 0x00, 0x12, 0xdc, 0xc7, 0x00196001, 0x100, 0x10000, 0x2000000),
+	FLASH_ID("cyp s28hl256t",       0x13, 0xec, 0x12, 0xdc, 0xc7, 0x00195a34, 0x100, 0x40000, 0x2000000), /* page! */
+	FLASH_ID("cyp s28hs256t",       0x13, 0xec, 0x12, 0xdc, 0xc7, 0x00195b34, 0x100, 0x40000, 0x2000000), /* page! */
+	FLASH_ID("cyp s28hl512t",       0x13, 0xec, 0x12, 0xdc, 0xc7, 0x001a5a34, 0x100, 0x40000, 0x4000000), /* page! */
+	FLASH_ID("cyp s28hs512t",       0x13, 0xec, 0x12, 0xdc, 0xc7, 0x001a5b34, 0x100, 0x40000, 0x4000000), /* page! */
+	FLASH_ID("cyp s28hl01gt",       0x13, 0xec, 0x12, 0xdc, 0xc7, 0x001b5a34, 0x100, 0x40000, 0x8000000), /* page! */
+	FLASH_ID("cyp s28hs01gt",       0x13, 0xec, 0x12, 0xdc, 0xc7, 0x001b5b34, 0x100, 0x40000, 0x8000000), /* page! */
 	FLASH_ID("atmel 25f512",        0x03, 0x00, 0x02, 0x52, 0xc7, 0x0065001f, 0x80,  0x8000,  0x10000),
 	FLASH_ID("atmel 25f1024",       0x03, 0x00, 0x02, 0x52, 0x62, 0x0060001f, 0x100, 0x8000,  0x20000),
 	FLASH_ID("atmel 25f2048",       0x03, 0x00, 0x02, 0x52, 0x62, 0x0063001f, 0x100, 0x10000, 0x40000),
 	FLASH_ID("atmel 25f4096",       0x03, 0x00, 0x02, 0x52, 0x62, 0x0064001f, 0x100, 0x10000, 0x80000),
 	FLASH_ID("atmel 25fs040",       0x03, 0x00, 0x02, 0xd7, 0xc7, 0x0004661f, 0x100, 0x10000, 0x80000),
+	FLASH_ID("adesto 25sf041b",     0x03, 0x00, 0x02, 0xd8, 0xc7, 0x0001841f, 0x100, 0x10000, 0x80000),
 	FLASH_ID("adesto 25df081a",     0x03, 0x00, 0x02, 0xd8, 0xc7, 0x0001451f, 0x100, 0x10000, 0x100000),
+	FLASH_ID("adesto 25sf081b",     0x03, 0x00, 0x02, 0xd8, 0xc7, 0x0001851f, 0x100, 0x10000, 0x100000),
+	FLASH_ID("adesto 25sf161b",     0x03, 0x00, 0x02, 0xd8, 0xc7, 0x0001861f, 0x100, 0x10000, 0x200000),
+	FLASH_ID("adesto 25df321b",     0x03, 0x00, 0x02, 0xd8, 0xc7, 0x0001471f, 0x100, 0x10000, 0x400000),
+	FLASH_ID("adesto 25sf321b",     0x03, 0x00, 0x02, 0xd8, 0xc7, 0x0001871f, 0x100, 0x10000, 0x400000),
+	FLASH_ID("adesto 25xf641b",     0x03, 0x00, 0x02, 0xd8, 0xc7, 0x0001881f, 0x100, 0x10000, 0x800000), /* sf/qf */
+	FLASH_ID("adesto 25xf128a",     0x03, 0x00, 0x02, 0xd8, 0xc7, 0x0001891f, 0x100, 0x10000, 0x1000000), /* sf/qf */
+	FLASH_ID("adesto xp032",        0x03, 0x0b, 0x02, 0xd8, 0xc7, 0x0700a743, 0x100, 0x10000, 0x400000), /* 4-byte */
+	FLASH_ID("adesto xp064b",       0x03, 0x0b, 0x02, 0xd8, 0xc7, 0x0001a81f, 0x100, 0x10000, 0x800000), /* 4-byte */
+	FLASH_ID("adesto xp128",        0x03, 0x0b, 0x02, 0xd8, 0xc7, 0x0000a91f, 0x100, 0x10000, 0x1000000), /* 4-byte */
 	FLASH_ID("mac 25l512",          0x03, 0x00, 0x02, 0xd8, 0xc7, 0x001020c2, 0x010, 0x10000, 0x10000),
 	FLASH_ID("mac 25l1005",         0x03, 0x00, 0x02, 0xd8, 0xc7, 0x001120c2, 0x010, 0x10000, 0x20000),
 	FLASH_ID("mac 25l2005",         0x03, 0x00, 0x02, 0xd8, 0xc7, 0x001220c2, 0x010, 0x10000, 0x40000),
@@ -78,6 +101,9 @@ const struct flash_device flash_devices[] = {
 	FLASH_ID("mac 25r3235f",        0x03, 0x00, 0x02, 0xd8, 0xc7, 0x001628c2, 0x100, 0x10000, 0x400000),
 	FLASH_ID("mac 25r6435f",        0x03, 0x00, 0x02, 0xd8, 0xc7, 0x001728c2, 0x100, 0x10000, 0x800000),
 	FLASH_ID("mac 25u1635e",        0x03, 0xeb, 0x02, 0x20, 0xc7, 0x003525c2, 0x100, 0x1000,  0x100000),
+	FLASH_ID("mac 66l1g45g",        0x13, 0xec, 0x12, 0xdc, 0xc7, 0x001b20c2, 0x100, 0x10000, 0x8000000),
+	FLASH_ID("mac 66um1g45g",       0x13, 0xec, 0x12, 0xdc, 0xc7, 0x003b80c2, 0x100, 0x10000, 0x8000000),
+	FLASH_ID("mac 66lm1g45g",       0x13, 0xec, 0x12, 0xdc, 0xc7, 0x003b85c2, 0x100, 0x10000, 0x8000000),
 	FLASH_ID("micron n25q032",      0x03, 0xeb, 0x02, 0xd8, 0xc7, 0x0016ba20, 0x100, 0x10000, 0x400000),
 	FLASH_ID("micron n25q064",      0x03, 0xeb, 0x02, 0xd8, 0xc7, 0x0017ba20, 0x100, 0x10000, 0x800000),
 	FLASH_ID("micron n25q128",      0x03, 0xeb, 0x02, 0xd8, 0xc7, 0x0018ba20, 0x100, 0x10000, 0x1000000),
@@ -102,6 +128,10 @@ const struct flash_device flash_devices[] = {
 	FLASH_ID("win w25q256fv/jv",    0x03, 0xeb, 0x02, 0xd8, 0xc7, 0x001940ef, 0x100, 0x10000, 0x2000000),
 	FLASH_ID("win w25q256fv",       0x03, 0xeb, 0x02, 0xd8, 0xc7, 0x001960ef, 0x100, 0x10000, 0x2000000), /* QPI mode */
 	FLASH_ID("win w25q256jv",       0x03, 0x00, 0x02, 0xd8, 0xc7, 0x001970ef, 0x100, 0x10000, 0x2000000),
+	FLASH_ID("win w25q512jv",       0x03, 0x00, 0x02, 0xd8, 0xc7, 0x002040ef, 0x100, 0x10000, 0x4000000),
+	FLASH_ID("win w25q01jv",        0x13, 0x00, 0x12, 0xdc, 0xc7, 0x002140ef, 0x100, 0x10000, 0x8000000),
+	FLASH_ID("win w25q01jv-dtr",    0x03, 0xeb, 0x02, 0xd8, 0xc7, 0x002170ef, 0x100, 0x10000, 0x8000000),
+	FLASH_ID("win w25q02jv-dtr",    0x03, 0xeb, 0x02, 0xd8, 0xc7, 0x002270ef, 0x100, 0x10000, 0x10000000),
 	FLASH_ID("gd gd25q512",         0x03, 0x00, 0x02, 0x20, 0xc7, 0x001040c8, 0x100, 0x1000,  0x10000),
 	FLASH_ID("gd gd25q10",          0x03, 0x00, 0x02, 0x20, 0xc7, 0x001140c8, 0x100, 0x1000,  0x20000),
 	FLASH_ID("gd gd25q20",          0x03, 0x00, 0x02, 0x20, 0xc7, 0x001240c8, 0x100, 0x1000,  0x40000),
@@ -109,9 +139,17 @@ const struct flash_device flash_devices[] = {
 	FLASH_ID("gd gd25q16c",         0x03, 0x00, 0x02, 0xd8, 0xc7, 0x001540c8, 0x100, 0x10000, 0x200000),
 	FLASH_ID("gd gd25q32c",         0x03, 0x00, 0x02, 0xd8, 0xc7, 0x001640c8, 0x100, 0x10000, 0x400000),
 	FLASH_ID("gd gd25q64c",         0x03, 0x00, 0x02, 0xd8, 0xc7, 0x001740c8, 0x100, 0x10000, 0x800000),
-	FLASH_ID("gd gd25q128c",        0x03, 0xeb, 0x02, 0xd8, 0xc7, 0x001840c8, 0x100, 0x10000, 0x1000000),
+	FLASH_ID("gd gd25q128c",        0x03, 0x0b, 0x02, 0xd8, 0xc7, 0x001840c8, 0x100, 0x10000, 0x1000000),
 	FLASH_ID("gd gd25q256c",        0x13, 0x00, 0x12, 0xdc, 0xc7, 0x001940c8, 0x100, 0x10000, 0x2000000),
 	FLASH_ID("gd gd25q512mc",       0x13, 0x00, 0x12, 0xdc, 0xc7, 0x002040c8, 0x100, 0x10000, 0x4000000),
+	FLASH_ID("gd gd25lx256e",       0x13, 0x0c, 0x12, 0xdc, 0xc7, 0x001968c8, 0x100, 0x10000, 0x2000000),
+	FLASH_ID("zbit zb25vq16",       0x03, 0x00, 0x02, 0xd8, 0xc7, 0x0015605e, 0x100, 0x10000, 0x200000),
+	FLASH_ID("zbit zb25vq32",       0x03, 0x0b, 0x02, 0xd8, 0xc7, 0x0016405e, 0x100, 0x10000, 0x400000),
+	FLASH_ID("zbit zb25vq32",       0x03, 0x0b, 0x02, 0xd8, 0xc7, 0x0016605e, 0x100, 0x10000, 0x400000), /* QPI mode */
+	FLASH_ID("zbit zb25vq64",       0x03, 0x0b, 0x02, 0xd8, 0xc7, 0x0017405e, 0x100, 0x10000, 0x800000),
+	FLASH_ID("zbit zb25vq64",       0x03, 0x0b, 0x02, 0xd8, 0xc7, 0x0017605e, 0x100, 0x10000, 0x800000), /* QPI mode */
+	FLASH_ID("zbit zb25vq128",      0x03, 0x0b, 0x02, 0xd8, 0xc7, 0x0018405e, 0x100, 0x10000, 0x1000000),
+	FLASH_ID("zbit zb25vq128",      0x03, 0x0b, 0x02, 0xd8, 0xc7, 0x0018605e, 0x100, 0x10000, 0x1000000), /* QPI mode */
 	FLASH_ID("issi is25lq040b",     0x03, 0xeb, 0x02, 0x20, 0xc7, 0x0013409d, 0x100, 0x1000,  0x80000),
 	FLASH_ID("issi is25lp032",      0x03, 0x00, 0x02, 0xd8, 0xc7, 0x0016609d, 0x100, 0x10000, 0x400000),
 	FLASH_ID("issi is25lp064",      0x03, 0x00, 0x02, 0xd8, 0xc7, 0x0017609d, 0x100, 0x10000, 0x800000),
@@ -121,6 +159,18 @@ const struct flash_device flash_devices[] = {
 	FLASH_ID("issi is25wp256d",     0x13, 0xec, 0x12, 0xdc, 0xc7, 0x0019709d, 0x100, 0x10000, 0x2000000),
 	FLASH_ID("issi is25lp512m",     0x13, 0xec, 0x12, 0xdc, 0xc7, 0x001a609d, 0x100, 0x10000, 0x4000000),
 	FLASH_ID("issi is25wp512m",     0x13, 0xec, 0x12, 0xdc, 0xc7, 0x001a709d, 0x100, 0x10000, 0x4000000),
+	FLASH_ID("xtx xt25f02e",        0x03, 0x00, 0x02, 0xd8, 0xc7, 0x0012400b, 0x100, 0x10000, 0x40000),
+	FLASH_ID("xtx xt25f04d",        0x03, 0x00, 0x02, 0xd8, 0xc7, 0x0013400b, 0x100, 0x10000, 0x80000),
+	FLASH_ID("xtx xt25f08b",        0x03, 0x00, 0x02, 0xd8, 0xc7, 0x0014400b, 0x100, 0x10000, 0x100000),
+	FLASH_ID("xtx xt25f16b",        0x03, 0x00, 0x02, 0xd8, 0xc7, 0x0015400b, 0x100, 0x10000, 0x200000),
+	FLASH_ID("xtx xt25f32b",        0x03, 0x0b, 0x02, 0xd8, 0xc7, 0x0016400b, 0x100, 0x10000, 0x200000),
+	FLASH_ID("xtx xt25f64b",        0x03, 0x0b, 0x02, 0xd8, 0xc7, 0x0017400b, 0x100, 0x10000, 0x400000),
+	FLASH_ID("xtx xt25f128b",       0x03, 0x0b, 0x02, 0xd8, 0xc7, 0x0018400b, 0x100, 0x10000, 0x800000),
+	FLASH_ID("xtx xt25q08d",        0x03, 0x0b, 0x02, 0xd8, 0xc7, 0x0014600b, 0x100, 0x10000, 0x100000),
+	FLASH_ID("xtx xt25q16b",        0x03, 0x0b, 0x02, 0xd8, 0xc7, 0x0015600b, 0x100, 0x10000, 0x200000),
+	FLASH_ID("xtx xt25q32b",        0x03, 0x0b, 0x02, 0xd8, 0xc7, 0x0016600b, 0x100, 0x10000, 0x400000), /* exists ? */
+	FLASH_ID("xtx xt25q64b",        0x03, 0x0b, 0x02, 0xd8, 0xc7, 0x0017600b, 0x100, 0x10000, 0x800000),
+	FLASH_ID("xtx xt25q128b",       0x03, 0x0b, 0x02, 0xd8, 0xc7, 0x0018600b, 0x100, 0x10000, 0x1000000),
 
 	/* FRAM, no erase commands, no write page or sectors */
 	FRAM_ID("fu mb85rs16n",         0x03, 0,    0x02, 0x00010104, 0x800),
@@ -131,13 +181,30 @@ const struct flash_device flash_devices[] = {
 	FRAM_ID("fu mb85rs512t",        0x03, 0,    0x02, 0x00032604, 0x10000),
 	FRAM_ID("fu mb85rs1mt",         0x03, 0,    0x02, 0x00032704, 0x20000),
 	FRAM_ID("fu mb85rs2mta",        0x03, 0,    0x02, 0x00034804, 0x40000),
-	FRAM_ID("cyp fm25v01a",         0x03, 0,    0x02, 0x000821c2, 0x4000),
-	FRAM_ID("cyp fm25v02",          0x03, 0,    0x02, 0x000022c2, 0x8000),
-	FRAM_ID("cyp fm25v02a",         0x03, 0,    0x02, 0x000822c2, 0x8000),
-	FRAM_ID("cyp fm25v05",          0x03, 0,    0x02, 0x000023c2, 0x10000),
-	FRAM_ID("cyp fm25v10",          0x03, 0,    0x02, 0x000024c2, 0x20000),
-	FRAM_ID("cyp fm25v20a",         0x03, 0,    0x02, 0x000825c2, 0x40000),
-	FRAM_ID("cyp fm25v40",          0x03, 0,    0x02, 0x004026c2, 0x80000),
+	FRAM_ID("cyp fm25v01a",         0x03, 0,    0x02, 0x060821c2, 0x4000),
+	FRAM_ID("cyp fm25v02",          0x03, 0,    0x02, 0x060022c2, 0x8000),
+	FRAM_ID("cyp fm25v02a",         0x03, 0,    0x02, 0x060822c2, 0x8000),
+	FRAM_ID("cyp fm25v05",          0x03, 0,    0x02, 0x060023c2, 0x10000),
+	FRAM_ID("cyp fm25v10",          0x03, 0,    0x02, 0x060024c2, 0x20000),
+	FRAM_ID("cyp fm25v20a",         0x03, 0,    0x02, 0x060825c2, 0x40000),
+	FRAM_ID("cyp fm25v40",          0x03, 0,    0x02, 0x064026c2, 0x80000),
+	FRAM_ID("cyp cy15b102qn",       0x03, 0,    0x02, 0x06002ac2, 0x40000),
+	FRAM_ID("cyp cy15v102qn",       0x03, 0,    0x02, 0x06042ac2, 0x40000),
+	FRAM_ID("cyp cy15b104qi",       0x03, 0,    0x02, 0x06012dc2, 0x80000),
+	FRAM_ID("cyp cy15b104qi",       0x03, 0,    0x02, 0x06a12dc2, 0x80000),
+	FRAM_ID("cyp cy15v104qi",       0x03, 0,    0x02, 0x06052dc2, 0x80000),
+	FRAM_ID("cyp cy15v104qi",       0x03, 0,    0x02, 0x06a52dc2, 0x80000),
+	FRAM_ID("cyp cy15b104qn",       0x03, 0,    0x02, 0x06402cc2, 0x80000),
+	FRAM_ID("cyp cy15b108qi",       0x03, 0,    0x02, 0x06012fc2, 0x100000),
+	FRAM_ID("cyp cy15b108qi",       0x03, 0,    0x02, 0x06a12fc2, 0x100000),
+	FRAM_ID("cyp cy15v108qi",       0x03, 0,    0x02, 0x06052fc2, 0x100000),
+	FRAM_ID("cyp cy15v108qi",       0x03, 0,    0x02, 0x06a52fc2, 0x100000),
+	FRAM_ID("cyp cy15b108qn",       0x03, 0,    0x02, 0x06012ec2, 0x100000),
+	FRAM_ID("cyp cy15b108qn",       0x03, 0,    0x02, 0x06032ec2, 0x100000),
+	FRAM_ID("cyp cy15b108qn",       0x03, 0,    0x02, 0x06a12ec2, 0x100000),
+	FRAM_ID("cyp cy15v108qn",       0x03, 0,    0x02, 0x06052ec2, 0x100000),
+	FRAM_ID("cyp cy15v108qn",       0x03, 0,    0x02, 0x06072ec2, 0x100000),
+	FRAM_ID("cyp cy15v108qn",       0x03, 0,    0x02, 0x06a52ec2, 0x100000),
 
 	FLASH_ID(NULL,                  0,    0,    0,    0,    0,    0,          0,     0,       0)
 };
diff --git a/src/flash/nor/stm32l4x.c b/src/flash/nor/stm32l4x.c
index c67dbf7..02e737b 100644
--- a/src/flash/nor/stm32l4x.c
+++ b/src/flash/nor/stm32l4x.c
@@ -318,7 +318,7 @@ static const struct stm32l4_rev stm32l4p_l4qxx_revs[] = {
 };
 
 static const struct stm32l4_rev stm32l55_l56xx_revs[] = {
-	{ 0x1000, "A" }, { 0x2000, "B" },
+	{ 0x1000, "A" }, { 0x2000, "B" }, { 0x2001, "Z" },
 };
 
 static const struct stm32l4_rev stm32g49_g4axx_revs[] = {
diff --git a/src/helper/Makefile.am b/src/helper/Makefile.am
index 26ff0bd..39d93d6 100644
--- a/src/helper/Makefile.am
+++ b/src/helper/Makefile.am
@@ -16,6 +16,7 @@ noinst_LTLIBRARIES += %D%/libhelper.la
 	%D%/util.c \
 	%D%/jep106.c \
 	%D%/jim-nvp.c \
+	%D%/nvp.c \
 	%D%/align.h \
 	%D%/binarybuffer.h \
 	%D%/bits.h \
@@ -34,7 +35,9 @@ noinst_LTLIBRARIES += %D%/libhelper.la
 	%D%/jep106.inc \
 	%D%/jim-nvp.h \
 	%D%/base64.c \
-	%D%/base64.h
+	%D%/base64.h \
+	%D%/nvp.h \
+	%D%/compiler.h
 
 STARTUP_TCL_SRCS += %D%/startup.tcl
 EXTRA_DIST += \
diff --git a/src/helper/command.c b/src/helper/command.c
index ca66cf7..b358e18 100644
--- a/src/helper/command.c
+++ b/src/helper/command.c
@@ -18,9 +18,6 @@
 #include "config.h"
 #endif
 
-/* see Embedded-HOWTO.txt in Jim Tcl project hosted on BerliOS*/
-#define JIM_EMBEDDED
-
 /* @todo the inclusion of target.h here is a layering violation */
 #include <jtag/jtag.h>
 #include <target/target.h>
@@ -543,8 +540,16 @@ static int run_command(struct command_context *context,
 		if (retval != ERROR_OK)
 			LOG_DEBUG("Command '%s' failed with error code %d",
 						words[0], retval);
-		/* Use the command output as the Tcl result */
-		Jim_SetResult(context->interp, cmd.output);
+		/*
+		 * Use the command output as the Tcl result.
+		 * Drop last '\n' to allow command output concatenation
+		 * while keep using command_print() everywhere.
+		 */
+		const char *output_txt = Jim_String(cmd.output);
+		int len = strlen(output_txt);
+		if (len && output_txt[len - 1] == '\n')
+			--len;
+		Jim_SetResultString(context->interp, output_txt, len);
 	}
 	Jim_DecrRefCount(context->interp, cmd.output);
 
diff --git a/src/helper/command.h b/src/helper/command.h
index 478e5c8..42cb9cb 100644
--- a/src/helper/command.h
+++ b/src/helper/command.h
@@ -370,10 +370,21 @@ struct command_context *copy_command_context(struct command_context *cmd_ctx);
  */
 void command_done(struct command_context *context);
 
+/*
+ * command_print() and command_print_sameline() are used to produce the TCL
+ * output of OpenOCD commands. command_print() automatically adds a '\n' at
+ * the end or the format string. Use command_print_sameline() to avoid the
+ * trailing '\n', e.g. to concatenate the command output in the same line.
+ * The very last '\n' of the command is stripped away (see run_command()).
+ * For commands that strictly require a '\n' as last output character, add
+ * it explicitly with either an empty command_print() or with a '\n' in the
+ * last command_print() and add a comment to document it.
+ */
 void command_print(struct command_invocation *cmd, const char *format, ...)
 __attribute__ ((format (PRINTF_ATTRIBUTE_FORMAT, 2, 3)));
 void command_print_sameline(struct command_invocation *cmd, const char *format, ...)
 __attribute__ ((format (PRINTF_ATTRIBUTE_FORMAT, 2, 3)));
+
 int command_run_line(struct command_context *context, char *line);
 int command_run_linef(struct command_context *context, const char *format, ...)
 __attribute__ ((format (PRINTF_ATTRIBUTE_FORMAT, 2, 3)));
diff --git a/src/helper/compiler.h b/src/helper/compiler.h
new file mode 100644
index 0000000..33a075d
--- /dev/null
+++ b/src/helper/compiler.h
@@ -0,0 +1,52 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+
+/*
+ * This file contains compiler specific workarounds to handle different
+ * compilers and different compiler versions.
+ * Inspired by Linux's include/linux/compiler_attributes.h
+ * and file sys/cdefs.h in libc and newlib.
+ */
+
+#ifndef OPENOCD_HELPER_COMPILER_H
+#define OPENOCD_HELPER_COMPILER_H
+
+/*
+ * __has_attribute is supported on gcc >= 5, clang >= 2.9 and icc >= 17.
+ */
+#ifndef __has_attribute
+# define __has_attribute(x) 0
+#endif
+
+/*
+ * The __returns_nonnull function attribute marks the return type of the function
+ * as always being non-null.
+ */
+#ifndef __returns_nonnull
+# if __has_attribute(__returns_nonnull__)
+#  define __returns_nonnull __attribute__((__returns_nonnull__))
+# else
+#  define __returns_nonnull
+# endif
+#endif
+
+/*
+ * The __nonnull function attribute marks pointer parameters that
+ * must not be NULL.
+ *
+ * clang for Apple defines
+ * #define __nonnull _Nonnull
+ * that is a per argument attribute, incompatible with the gcc per function attribute __nonnull__.
+ * Undefine it to keep compatibility among compilers.
+ */
+#if defined(__clang__) && defined(__APPLE__)
+# undef __nonnull
+#endif
+#ifndef __nonnull
+# if __has_attribute(__nonnull__)
+#  define __nonnull(params) __attribute__ ((__nonnull__ params))
+# else
+#  define __nonnull(params)
+# endif
+#endif
+
+#endif /* OPENOCD_HELPER_COMPILER_H */
diff --git a/src/helper/nvp.c b/src/helper/nvp.c
new file mode 100644
index 0000000..7a8abc2
--- /dev/null
+++ b/src/helper/nvp.c
@@ -0,0 +1,67 @@
+// SPDX-License-Identifier: BSD-2-Clause-Views
+
+/*
+ * Copyright 2005 Salvatore Sanfilippo <antirez@invece.org>
+ * Copyright 2005 Clemens Hintze <c.hintze@gmx.net>
+ * Copyright 2005 patthoyts - Pat Thoyts <patthoyts@users.sf.net>
+ * Copyright 2008 oharboe - Øyvind Harboe - oyvind.harboe@zylin.com
+ * Copyright 2008 Andrew Lunn <andrew@lunn.ch>
+ * Copyright 2008 Duane Ellis <openocd@duaneellis.com>
+ * Copyright 2008 Uwe Klein <uklein@klein-messgeraete.de>
+ * Copyright 2008 Steve Bennett <steveb@workware.net.au>
+ * Copyright 2009 Nico Coesel <ncoesel@dealogic.nl>
+ * Copyright 2009 Zachary T Welch zw@superlucidity.net
+ * Copyright 2009 David Brownell
+ * Copyright (c) 2005-2011 Jim Tcl Project. All rights reserved.
+ *
+ * This file is extracted from jim_nvp.c, originally part of jim TCL code.
+ */
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <string.h>
+
+#include <helper/command.h>
+#include <helper/nvp.h>
+
+const struct nvp *nvp_name2value(const struct nvp *p, const char *name)
+{
+	while (p->name) {
+		if (strcmp(name, p->name) == 0)
+			break;
+		p++;
+	}
+	return p;
+}
+
+const struct nvp *nvp_value2name(const struct nvp *p, int value)
+{
+	while (p->name) {
+		if (value == p->value)
+			break;
+		p++;
+	}
+	return p;
+}
+
+void nvp_unknown_command_print(struct command_invocation *cmd, const struct nvp *nvp,
+	const char *param_name, const char *param_value)
+{
+	if (param_name)
+		command_print_sameline(cmd, "%s: Unknown: %s, try one of: ", param_name, param_value);
+	else
+		command_print_sameline(cmd, "Unknown param: %s, try one of: ", param_value);
+
+	while (nvp->name) {
+		if ((nvp + 1)->name)
+			command_print_sameline(cmd, "%s, ", nvp->name);
+		else
+			command_print(cmd, "or %s", nvp->name);
+
+		nvp++;
+	}
+
+	/* We assume nvp to be not empty and loop has been taken; no need to add a '\n' */
+}
diff --git a/src/helper/nvp.h b/src/helper/nvp.h
new file mode 100644
index 0000000..14bd9b0
--- /dev/null
+++ b/src/helper/nvp.h
@@ -0,0 +1,77 @@
+/* SPDX-License-Identifier: BSD-2-Clause-Views */
+
+/*
+ * Copyright 2005 Salvatore Sanfilippo <antirez@invece.org>
+ * Copyright 2005 Clemens Hintze <c.hintze@gmx.net>
+ * Copyright 2005 patthoyts - Pat Thoyts <patthoyts@users.sf.net>
+ * Copyright 2008 oharboe - Øyvind Harboe - oyvind.harboe@zylin.com
+ * Copyright 2008 Andrew Lunn <andrew@lunn.ch>
+ * Copyright 2008 Duane Ellis <openocd@duaneellis.com>
+ * Copyright 2008 Uwe Klein <uklein@klein-messgeraete.de>
+ * Copyright 2008 Steve Bennett <steveb@workware.net.au>
+ * Copyright 2009 Nico Coesel <ncoesel@dealogic.nl>
+ * Copyright 2009 Zachary T Welch zw@superlucidity.net
+ * Copyright 2009 David Brownell
+ * Copyright (c) 2005-2011 Jim Tcl Project. All rights reserved.
+ *
+ * This file is extracted from jim_nvp.h, originally part of jim TCL code.
+ */
+
+#ifndef OPENOCD_HELPER_NVP_H
+#define OPENOCD_HELPER_NVP_H
+
+#include <helper/compiler.h>
+
+/** Name Value Pairs, aka: NVP
+ *   -  Given a string - return the associated int.
+ *   -  Given a number - return the associated string.
+ *   .
+ *
+ * Very useful when the number is not a simple index into an array of
+ * known string, or there may be multiple strings (aliases) that mean then same
+ * thing.
+ *
+ * An NVP Table is terminated with ".name = NULL".
+ *
+ * During the 'name2value' operation, if no matching string is found
+ * the pointer to the terminal element (with p->name == NULL) is returned.
+ *
+ * Example:
+ * \code
+ *      const struct nvp yn[] = {
+ *          { "yes", 1 },
+ *          { "no" , 0 },
+ *          { "yep", 1 },
+ *          { "nope", 0 },
+ *          { NULL, -1 },
+ *      };
+ *
+ *  struct nvp *result;
+ *  result = nvp_name2value(yn, "yes");
+ *         returns &yn[0];
+ *  result = nvp_name2value(yn, "no");
+ *         returns &yn[1];
+ *  result = jim_nvp_name2value(yn, "Blah");
+ *         returns &yn[4];
+ * \endcode
+ *
+ * During the number2name operation, the first matching value is returned.
+ */
+
+struct nvp {
+	const char *name;
+	int value;
+};
+
+struct command_invocation;
+
+/* Name Value Pairs Operations */
+const struct nvp *nvp_name2value(const struct nvp *nvp_table, const char *name)
+	__returns_nonnull __nonnull((1));
+const struct nvp *nvp_value2name(const struct nvp *nvp_table, int v)
+	__returns_nonnull __nonnull((1));
+
+void nvp_unknown_command_print(struct command_invocation *cmd, const struct nvp *nvp,
+	const char *param_name, const char *param_value);
+
+#endif /* OPENOCD_HELPER_NVP_H */
diff --git a/src/jtag/drivers/bcm2835gpio.c b/src/jtag/drivers/bcm2835gpio.c
index 5aa1a99..635d9a5 100644
--- a/src/jtag/drivers/bcm2835gpio.c
+++ b/src/jtag/drivers/bcm2835gpio.c
@@ -66,6 +66,12 @@ static inline void bcm2835_gpio_synchronize(void)
 	__sync_synchronize();
 }
 
+static inline void bcm2835_delay(void)
+{
+	for (unsigned int i = 0; i < jtag_delay; i++)
+		asm volatile ("");
+}
+
 static bool is_gpio_config_valid(enum adapter_gpio_config_index idx)
 {
 	/* Only chip 0 is supported, accept unset value (-1) too */
@@ -178,8 +184,7 @@ static int bcm2835gpio_write(int tck, int tms, int tdi)
 	GPIO_CLR = clear;
 	bcm2835_gpio_synchronize();
 
-	for (unsigned int i = 0; i < jtag_delay; i++)
-		asm volatile ("");
+	bcm2835_delay();
 
 	return ERROR_OK;
 }
@@ -199,8 +204,7 @@ static int bcm2835gpio_swd_write_fast(int swclk, int swdio)
 	GPIO_CLR = clear;
 	bcm2835_gpio_synchronize();
 
-	for (unsigned int i = 0; i < jtag_delay; i++)
-		asm volatile ("");
+	bcm2835_delay();
 
 	return ERROR_OK;
 }
@@ -211,8 +215,7 @@ static int bcm2835gpio_swd_write_generic(int swclk, int swdio)
 	set_gpio_value(&adapter_gpio_config[ADAPTER_GPIO_IDX_SWDIO], swdio);
 	set_gpio_value(&adapter_gpio_config[ADAPTER_GPIO_IDX_SWCLK], swclk); /* Write clock last */
 
-	for (unsigned int i = 0; i < jtag_delay; ++i)
-		asm volatile ("");
+	bcm2835_delay();
 
 	return ERROR_OK;
 }
@@ -264,7 +267,8 @@ static int bcm2835gpio_khz(int khz, int *jtag_speed)
 		LOG_DEBUG("BCM2835 GPIO: RCLK not supported");
 		return ERROR_FAIL;
 	}
-	*jtag_speed = speed_coeff/khz - speed_offset;
+	*jtag_speed = DIV_ROUND_UP(speed_coeff, khz) - speed_offset;
+	LOG_DEBUG("jtag_delay %d", *jtag_speed);
 	if (*jtag_speed < 0)
 		*jtag_speed = 0;
 	return ERROR_OK;
@@ -272,7 +276,9 @@ static int bcm2835gpio_khz(int khz, int *jtag_speed)
 
 static int bcm2835gpio_speed_div(int speed, int *khz)
 {
-	*khz = speed_coeff/(speed + speed_offset);
+	int divisor = speed + speed_offset;
+	/* divide with roundig to the closest */
+	*khz = (speed_coeff + divisor / 2) / divisor;
 	return ERROR_OK;
 }
 
diff --git a/src/jtag/drivers/bitbang.c b/src/jtag/drivers/bitbang.c
index 2ab0a2a..665dbf3 100644
--- a/src/jtag/drivers/bitbang.c
+++ b/src/jtag/drivers/bitbang.c
@@ -525,7 +525,19 @@ static void bitbang_swd_write_reg(uint8_t cmd, uint32_t value, uint32_t ap_delay
 		bitbang_swd_exchange(false, &cmd, 0, 8);
 
 		bitbang_interface->swdio_drive(false);
-		bitbang_swd_exchange(true, trn_ack_data_parity_trn, 0, 1 + 3 + 1);
+		bitbang_swd_exchange(true, trn_ack_data_parity_trn, 0, 1 + 3);
+
+		/* Avoid a glitch on SWDIO when changing the direction to output.
+		 * To keep performance penalty minimal, pre-write the first data
+		 * bit to SWDIO GPIO output buffer while clocking the turnaround bit.
+		 * Following swdio_drive(true) outputs the pre-written value
+		 * and the same value is rewritten by the next swd_write()
+		 * instead of glitching SWDIO
+		 * HiZ/pull-up --------------> 0 -------------> 1
+		 *           swdio_drive(true)   swd_write(0,1)
+		 * in case of data bit 0 = 1
+		 */
+		bitbang_swd_exchange(false, trn_ack_data_parity_trn, 1 + 3 + 1, 1);
 		bitbang_interface->swdio_drive(true);
 		bitbang_swd_exchange(false, trn_ack_data_parity_trn, 1 + 3 + 1, 32 + 1);
 
diff --git a/src/jtag/drivers/cmsis_dap.c b/src/jtag/drivers/cmsis_dap.c
index 0c42a7f..1e7a851 100644
--- a/src/jtag/drivers/cmsis_dap.c
+++ b/src/jtag/drivers/cmsis_dap.c
@@ -1001,12 +1001,14 @@ static void cmsis_dap_swd_queue_cmd(uint8_t cmd, uint32_t *dst, uint32_t data)
 													block_cmd);
 	unsigned int resp_size = cmsis_dap_tfer_resp_size(write_count, read_count,
 													block_cmd);
+	unsigned int max_transfer_count = block_cmd ? 65535 : 255;
 
 	/* Does the DAP Transfer command and the expected response fit into one packet?
 	 * Run the queue also before a targetsel - it cannot be queued */
 	if (cmd_size > tfer_max_command_size
 			|| resp_size > tfer_max_response_size
-			|| targetsel_cmd) {
+			|| targetsel_cmd
+			|| write_count + read_count > max_transfer_count) {
 		if (cmsis_dap_handle->pending_fifo_block_count)
 			cmsis_dap_swd_read_process(cmsis_dap_handle, 0);
 
diff --git a/src/jtag/drivers/ftdi.c b/src/jtag/drivers/ftdi.c
index d2dd893..9c8b80f 100644
--- a/src/jtag/drivers/ftdi.c
+++ b/src/jtag/drivers/ftdi.c
@@ -735,13 +735,8 @@ static int ftdi_initialize(void)
 		return ERROR_JTAG_INIT_FAILED;
 	}
 
-	for (int i = 0; ftdi_vid[i] || ftdi_pid[i]; i++) {
-		mpsse_ctx = mpsse_open(&ftdi_vid[i], &ftdi_pid[i], ftdi_device_desc,
+	mpsse_ctx = mpsse_open(ftdi_vid, ftdi_pid, ftdi_device_desc,
 				adapter_get_required_serial(), adapter_usb_get_location(), ftdi_channel);
-		if (mpsse_ctx)
-			break;
-	}
-
 	if (!mpsse_ctx)
 		return ERROR_JTAG_INIT_FAILED;
 
diff --git a/src/jtag/drivers/jlink.c b/src/jtag/drivers/jlink.c
index 0a96ac2..243d1a4 100644
--- a/src/jtag/drivers/jlink.c
+++ b/src/jtag/drivers/jlink.c
@@ -1976,6 +1976,8 @@ struct pending_scan_result {
 	void *buffer;
 	/** Offset in the destination buffer */
 	unsigned buffer_offset;
+	/** SWD command */
+	uint8_t swd_cmd;
 };
 
 #define MAX_PENDING_SCAN_RESULTS 256
@@ -2179,12 +2181,13 @@ static int jlink_swd_run_queue(void)
 	}
 
 	for (i = 0; i < pending_scan_results_length; i++) {
+		/* Devices do not reply to DP_TARGETSEL write cmd, ignore received ack */
+		bool check_ack = swd_cmd_returns_ack(pending_scan_results_buffer[i].swd_cmd);
 		int ack = buf_get_u32(tdo_buffer, pending_scan_results_buffer[i].first, 3);
-
-		if (ack != SWD_ACK_OK) {
+		if (check_ack && ack != SWD_ACK_OK) {
 			LOG_DEBUG("SWD ack not OK: %d %s", ack,
 				  ack == SWD_ACK_WAIT ? "WAIT" : ack == SWD_ACK_FAULT ? "FAULT" : "JUNK");
-			queued_retval = ack == SWD_ACK_WAIT ? ERROR_WAIT : ERROR_FAIL;
+			queued_retval = swd_ack_to_error_code(ack);
 			goto skip;
 		} else if (pending_scan_results_buffer[i].length) {
 			uint32_t data = buf_get_u32(tdo_buffer, 3 + pending_scan_results_buffer[i].first, 32);
@@ -2221,6 +2224,7 @@ static void jlink_swd_queue_cmd(uint8_t cmd, uint32_t *dst, uint32_t data, uint3
 	if (queued_retval != ERROR_OK)
 		return;
 
+	pending_scan_results_buffer[pending_scan_results_length].swd_cmd = cmd;
 	cmd |= SWD_CMD_START | SWD_CMD_PARK;
 
 	jlink_queue_data_out(&cmd, 8);
diff --git a/src/jtag/drivers/libusb_helper.c b/src/jtag/drivers/libusb_helper.c
index 53dfd50..4b098b4 100644
--- a/src/jtag/drivers/libusb_helper.c
+++ b/src/jtag/drivers/libusb_helper.c
@@ -50,7 +50,7 @@ static int jtag_libusb_error(int err)
 	}
 }
 
-static bool jtag_libusb_match_ids(struct libusb_device_descriptor *dev_desc,
+bool jtag_libusb_match_ids(struct libusb_device_descriptor *dev_desc,
 		const uint16_t vids[], const uint16_t pids[])
 {
 	for (unsigned i = 0; vids[i]; i++) {
diff --git a/src/jtag/drivers/libusb_helper.h b/src/jtag/drivers/libusb_helper.h
index 172c345..799e3e6 100644
--- a/src/jtag/drivers/libusb_helper.h
+++ b/src/jtag/drivers/libusb_helper.h
@@ -30,6 +30,8 @@
 typedef char * (*adapter_get_alternate_serial_fn)(struct libusb_device_handle *device,
 		struct libusb_device_descriptor *dev_desc);
 
+bool jtag_libusb_match_ids(struct libusb_device_descriptor *dev_desc,
+		const uint16_t vids[], const uint16_t pids[]);
 int jtag_libusb_open(const uint16_t vids[], const uint16_t pids[],
 		struct libusb_device_handle **out,
 		adapter_get_alternate_serial_fn adapter_get_alternate_serial);
diff --git a/src/jtag/drivers/mpsse.c b/src/jtag/drivers/mpsse.c
index 18aeb38..9f2fdde 100644
--- a/src/jtag/drivers/mpsse.c
+++ b/src/jtag/drivers/mpsse.c
@@ -13,6 +13,7 @@
 #include "helper/log.h"
 #include "helper/replacements.h"
 #include "helper/time_support.h"
+#include "libusb_helper.h"
 #include <libusb.h>
 
 /* Compatibility define for older libusb-1.0 */
@@ -148,7 +149,7 @@ static bool device_location_equal(struct libusb_device *device, const char *loca
  * Set any field to 0 as a wildcard. If the device is found true is returned, with ctx containing
  * the already opened handle. ctx->interface must be set to the desired interface (channel) number
  * prior to calling this function. */
-static bool open_matching_device(struct mpsse_ctx *ctx, const uint16_t *vid, const uint16_t *pid,
+static bool open_matching_device(struct mpsse_ctx *ctx, const uint16_t vids[], const uint16_t pids[],
 	const char *product, const char *serial, const char *location)
 {
 	struct libusb_device **list;
@@ -169,9 +170,7 @@ static bool open_matching_device(struct mpsse_ctx *ctx, const uint16_t *vid, con
 			continue;
 		}
 
-		if (vid && *vid != desc.idVendor)
-			continue;
-		if (pid && *pid != desc.idProduct)
+		if (!jtag_libusb_match_ids(&desc, vids, pids))
 			continue;
 
 		err = libusb_open(device, &ctx->usb_dev);
@@ -203,7 +202,7 @@ static bool open_matching_device(struct mpsse_ctx *ctx, const uint16_t *vid, con
 	libusb_free_device_list(list, 1);
 
 	if (!found) {
-		LOG_ERROR("no device found");
+		/* The caller reports detailed error desc */
 		return false;
 	}
 
@@ -307,7 +306,7 @@ error:
 	return false;
 }
 
-struct mpsse_ctx *mpsse_open(const uint16_t *vid, const uint16_t *pid, const char *description,
+struct mpsse_ctx *mpsse_open(const uint16_t vids[], const uint16_t pids[], const char *description,
 	const char *serial, const char *location, int channel)
 {
 	struct mpsse_ctx *ctx = calloc(1, sizeof(*ctx));
@@ -343,14 +342,9 @@ struct mpsse_ctx *mpsse_open(const uint16_t *vid, const uint16_t *pid, const cha
 		goto error;
 	}
 
-	if (!open_matching_device(ctx, vid, pid, description, serial, location)) {
-		/* Four hex digits plus terminating zero each */
-		char vidstr[5];
-		char pidstr[5];
-		LOG_ERROR("unable to open ftdi device with vid %s, pid %s, description '%s', "
+	if (!open_matching_device(ctx, vids, pids, description, serial, location)) {
+		LOG_ERROR("unable to open ftdi device with description '%s', "
 				"serial '%s' at bus location '%s'",
-				vid ? sprintf(vidstr, "%04x", *vid), vidstr : "*",
-				pid ? sprintf(pidstr, "%04x", *pid), pidstr : "*",
 				description ? description : "*",
 				serial ? serial : "*",
 				location ? location : "*");
diff --git a/src/jtag/drivers/openjtag.c b/src/jtag/drivers/openjtag.c
index 6be9507..12ea463 100644
--- a/src/jtag/drivers/openjtag.c
+++ b/src/jtag/drivers/openjtag.c
@@ -742,16 +742,18 @@ static void openjtag_execute_runtest(struct jtag_command *cmd)
 		tap_set_state(TAP_IDLE);
 	}
 
-	if (cmd->cmd.runtest->num_cycles > 16)
-		LOG_WARNING("num_cycles > 16 on run test");
-
 	if (openjtag_variant != OPENJTAG_VARIANT_CY7C65215 ||
 		cmd->cmd.runtest->num_cycles) {
 		uint8_t command;
-		command = 7;
-		command |= ((cmd->cmd.runtest->num_cycles - 1) & 0x0F) << 4;
+		int cycles = cmd->cmd.runtest->num_cycles;
 
-		openjtag_add_byte(command);
+		do {
+			command = 7;
+			command |= (((cycles > 16 ? 16 : cycles) - 1) & 0x0F) << 4;
+
+			openjtag_add_byte(command);
+			cycles -= 16;
+		} while (cycles > 0);
 	}
 
 	tap_set_end_state(end_state);
diff --git a/src/rtos/FreeRTOS.c b/src/rtos/FreeRTOS.c
index fbb698b..bf2896c 100644
--- a/src/rtos/FreeRTOS.c
+++ b/src/rtos/FreeRTOS.c
@@ -236,7 +236,7 @@ static int freertos_get_thread_reg_value(struct rtos *rtos, threadid_t thread_id
 static int freertos_set_reg(struct rtos *rtos, uint32_t reg_num, uint8_t *reg_value);
 static int freertos_get_symbol_list_to_lookup(struct symbol_table_elem *symbol_list[]);
 
-struct rtos_type freertos_rtos = {
+const struct rtos_type freertos_rtos = {
 	.name = "FreeRTOS",
 
 	.detect_rtos = freertos_detect_rtos,
diff --git a/src/rtos/Makefile.am b/src/rtos/Makefile.am
index fc3ab8b..0796910 100644
--- a/src/rtos/Makefile.am
+++ b/src/rtos/Makefile.am
@@ -21,6 +21,7 @@ noinst_LTLIBRARIES += %D%/librtos.la
 	%D%/mqx.c \
 	%D%/uCOS-III.c \
 	%D%/nuttx.c \
+	%D%/rtkernel.c \
 	%D%/hwthread.c \
 	%D%/zephyr.c \
 	%D%/riot.c \
@@ -33,5 +34,4 @@ noinst_LTLIBRARIES += %D%/librtos.la
 	%D%/rtos_mqx_stackings.h \
 	%D%/rtos_riot_stackings.h \
 	%D%/rtos_ucos_iii_stackings.h \
-	%D%/rtos_nuttx_stackings.h \
-	%D%/nuttx_header.h
+	%D%/rtos_nuttx_stackings.h
diff --git a/src/rtos/chibios.c b/src/rtos/chibios.c
index 8319cc8..68fe8a1 100644
--- a/src/rtos/chibios.c
+++ b/src/rtos/chibios.c
@@ -97,7 +97,7 @@ static int chibios_get_thread_reg_list(struct rtos *rtos, int64_t thread_id,
 		struct rtos_reg **reg_list, int *num_regs);
 static int chibios_get_symbol_list_to_lookup(struct symbol_table_elem *symbol_list[]);
 
-struct rtos_type chibios_rtos = {
+const struct rtos_type chibios_rtos = {
 	.name = "chibios",
 
 	.detect_rtos = chibios_detect_rtos,
diff --git a/src/rtos/embKernel.c b/src/rtos/embKernel.c
index c1b5723..a03b039 100644
--- a/src/rtos/embKernel.c
+++ b/src/rtos/embKernel.c
@@ -27,7 +27,7 @@ static int embkernel_get_thread_reg_list(struct rtos *rtos, int64_t thread_id,
 		struct rtos_reg **reg_list, int *num_regs);
 static int embkernel_get_symbol_list_to_lookup(struct symbol_table_elem *symbol_list[]);
 
-struct rtos_type embkernel_rtos = {
+const struct rtos_type embkernel_rtos = {
 		.name = "embKernel",
 		.detect_rtos = embkernel_detect_rtos,
 		.create = embkernel_create,
diff --git a/src/rtos/mqx.c b/src/rtos/mqx.c
index 8d483ed..d9b6942 100644
--- a/src/rtos/mqx.c
+++ b/src/rtos/mqx.c
@@ -498,7 +498,7 @@ static int mqx_get_symbol_list_to_lookup(struct symbol_table_elem *symbol_list[]
 	return ERROR_OK;
 }
 
-struct rtos_type mqx_rtos = {
+const struct rtos_type mqx_rtos = {
 	.name = "mqx",
 	.detect_rtos = mqx_detect_rtos,
 	.create = mqx_create,
diff --git a/src/rtos/nuttx.c b/src/rtos/nuttx.c
index 993ff84..0616af0 100644
--- a/src/rtos/nuttx.c
+++ b/src/rtos/nuttx.c
@@ -18,53 +18,60 @@
 #include "rtos.h"
 #include "helper/log.h"
 #include "helper/types.h"
-#include "server/gdb_server.h"
-
-#include "nuttx_header.h"
+#include "target/register.h"
 #include "rtos_nuttx_stackings.h"
 
-int rtos_thread_packet(struct connection *connection, const char *packet, int packet_size);
-
-#ifdef CONFIG_DISABLE_SIGNALS
-#define SIG_QUEUE_NUM 0
-#else
-#define SIG_QUEUE_NUM 1
-#endif /* CONFIG_DISABLE_SIGNALS */
-
-#ifdef CONFIG_DISABLE_MQUEUE
-#define M_QUEUE_NUM 0
-#else
-#define M_QUEUE_NUM 2
-#endif /* CONFIG_DISABLE_MQUEUE */
-
-#ifdef CONFIG_PAGING
-#define PAGING_QUEUE_NUM 1
-#else
-#define PAGING_QUEUE_NUM 0
-#endif /* CONFIG_PAGING */
+#define NAME_SIZE       32
+#define EXTRAINFO_SIZE  256
 
+/* Only 32-bit CPUs are supported by the current implementation.  Supporting
+ * other CPUs will require reading this information from the target and
+ * adapting the code accordingly.
+ */
+#define PTR_WIDTH 4
 
-#define TASK_QUEUE_NUM (6 + SIG_QUEUE_NUM + M_QUEUE_NUM + PAGING_QUEUE_NUM)
+struct nuttx_params {
+	const char *target_name;
+	const struct rtos_register_stacking *stacking;
+	const struct rtos_register_stacking *(*select_stackinfo)(struct target *target);
+};
 
+/*
+ * struct tcbinfo_s is located in the sched.h
+ * https://github.com/apache/nuttx/blob/master/include/nuttx/sched.h
+ */
+#define TCBINFO_TARGET_SIZE 22
+struct tcbinfo {
+	uint16_t pid_off;			/* Offset of tcb.pid                */
+	uint16_t state_off;			/* Offset of tcb.task_state         */
+	uint16_t pri_off;			/* Offset of tcb.sched_priority     */
+	uint16_t name_off;			/* Offset of tcb.name               */
+	uint16_t regs_off;			/* Offset of tcb.regs               */
+	uint16_t basic_num;			/* Num of genernal regs             */
+	uint16_t total_num;			/* Num of regs in tcbinfo.reg_offs  */
+	target_addr_t xcpreg_off;	/* Offset pointer of xcp.regs       */
+};
 
-/* see nuttx/sched/os_start.c */
-static char *nuttx_symbol_list[] = {
-	"g_readytorun",            /* 0: must be top of this array */
-	"g_tasklisttable",
-	NULL
+struct symbols {
+	const char *name;
+	bool optional;
 };
 
-/* see nuttx/include/nuttx/sched.h */
-struct tcb {
-	uint32_t flink;
-	uint32_t blink;
-	uint8_t  dat[512];
+/* Used to index the list of retrieved symbols. See nuttx_symbol_list for the order. */
+enum nuttx_symbol_vals {
+	NX_SYM_READYTORUN = 0,
+	NX_SYM_PIDHASH,
+	NX_SYM_NPIDHASH,
+	NX_SYM_TCB_INFO,
 };
 
-static struct {
-	uint32_t addr;
-	uint32_t prio;
-} g_tasklist[TASK_QUEUE_NUM];
+static const struct symbols nuttx_symbol_list[] = {
+	{ "g_readytorun", false },
+	{ "g_pidhash", false },
+	{ "g_npidhash", false },
+	{ "g_tcbinfo", false },
+	{ NULL, false }
+};
 
 static char *task_state_str[] = {
 	"INVALID",
@@ -73,261 +80,363 @@ static char *task_state_str[] = {
 	"RUNNING",
 	"INACTIVE",
 	"WAIT_SEM",
-#ifndef CONFIG_DISABLE_SIGNALS
 	"WAIT_SIG",
-#endif /* CONFIG_DISABLE_SIGNALS */
-#ifndef CONFIG_DISABLE_MQUEUE
 	"WAIT_MQNOTEMPTY",
 	"WAIT_MQNOTFULL",
-#endif /* CONFIG_DISABLE_MQUEUE */
-#ifdef CONFIG_PAGING
 	"WAIT_PAGEFILL",
-#endif /* CONFIG_PAGING */
+	"STOPPED",
 };
 
-static int pid_offset = PID;
-static int state_offset = STATE;
-static int name_offset =  NAME;
-static int xcpreg_offset = XCPREG;
-static int name_size = NAME_SIZE;
+static const struct rtos_register_stacking *cortexm_select_stackinfo(struct target *target);
+
+static const struct nuttx_params nuttx_params_list[] = {
+	{
+		.target_name = "cortex_m",
+		.stacking = NULL,
+		.select_stackinfo = cortexm_select_stackinfo,
+	},
+	{
+		.target_name = "hla_target",
+		.stacking = NULL,
+		.select_stackinfo = cortexm_select_stackinfo,
+	},
+	{
+		.target_name = "esp32",
+		.stacking = &nuttx_esp32_stacking,
+	},
+	{
+		.target_name = "esp32s2",
+		.stacking = &nuttx_esp32s2_stacking,
+	},
+	{
+		.target_name = "esp32s3",
+		.stacking = &nuttx_esp32s3_stacking,
+	},
+	{
+		.target_name = "esp32c3",
+		.stacking = &nuttx_riscv_stacking,
+	},
+};
 
-static int rcmd_offset(const char *cmd, const char *name)
+static bool cortexm_hasfpu(struct target *target)
 {
-	if (strncmp(cmd, name, strlen(name)))
-		return -1;
+	uint32_t cpacr;
+	struct armv7m_common *armv7m_target = target_to_armv7m(target);
 
-	if (strlen(cmd) <= strlen(name) + 1)
-		return -1;
+	if (!is_armv7m(armv7m_target) || armv7m_target->fp_feature == FP_NONE)
+		return false;
 
-	return atoi(cmd + strlen(name));
+	int retval = target_read_u32(target, FPU_CPACR, &cpacr);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("Could not read CPACR register to check FPU state");
+		return false;
+	}
+
+	return cpacr & 0x00F00000;
 }
 
-static int nuttx_thread_packet(struct connection *connection,
-	char const *packet, int packet_size)
+static const struct rtos_register_stacking *cortexm_select_stackinfo(struct target *target)
 {
-	char cmd[GDB_BUFFER_SIZE / 2 + 1] = ""; /* Extra byte for null-termination */
-
-	if (!strncmp(packet, "qRcmd", 5)) {
-		size_t len = unhexify((uint8_t *)cmd, packet + 6, sizeof(cmd));
-		int offset;
-
-		if (len <= 0)
-			goto pass;
-
-		offset = rcmd_offset(cmd, "nuttx.pid_offset");
-
-		if (offset >= 0) {
-			LOG_INFO("pid_offset: %d", offset);
-			pid_offset = offset;
-			goto retok;
-		}
-
-		offset = rcmd_offset(cmd, "nuttx.state_offset");
+	return cortexm_hasfpu(target) ? &nuttx_stacking_cortex_m_fpu : &nuttx_stacking_cortex_m;
+}
 
-		if (offset >= 0) {
-			LOG_INFO("state_offset: %d", offset);
-			state_offset = offset;
-			goto retok;
-		}
+static bool nuttx_detect_rtos(struct target *target)
+{
+	if (target->rtos->symbols &&
+		target->rtos->symbols[NX_SYM_READYTORUN].address != 0 &&
+		target->rtos->symbols[NX_SYM_PIDHASH].address != 0)
+		return true;
+	return false;
+}
 
-		offset = rcmd_offset(cmd, "nuttx.name_offset");
+static int nuttx_create(struct target *target)
+{
+	const struct nuttx_params *param;
+	unsigned int i;
 
-		if (offset >= 0) {
-			LOG_INFO("name_offset: %d", offset);
-			name_offset = offset;
-			goto retok;
+	for (i = 0; i < ARRAY_SIZE(nuttx_params_list); i++) {
+		param = &nuttx_params_list[i];
+		if (strcmp(target_type_name(target), param->target_name) == 0) {
+			LOG_INFO("Detected target \"%s\"", param->target_name);
+			break;
 		}
+	}
 
-		offset = rcmd_offset(cmd, "nuttx.xcpreg_offset");
-
-		if (offset >= 0) {
-			LOG_INFO("xcpreg_offset: %d", offset);
-			xcpreg_offset = offset;
-			goto retok;
-		}
+	if (i >= ARRAY_SIZE(nuttx_params_list)) {
+		LOG_ERROR("Could not find \"%s\" target in NuttX compatibility list", target_type_name(target));
+		return JIM_ERR;
+	}
 
-		offset = rcmd_offset(cmd, "nuttx.name_size");
+	/* We found a target in our list, copy its reference. */
+	target->rtos->rtos_specific_params = (void *)param;
 
-		if (offset >= 0) {
-			LOG_INFO("name_size: %d", offset);
-			name_size = offset;
-			goto retok;
-		}
-	}
-pass:
-	return rtos_thread_packet(connection, packet, packet_size);
-retok:
-	gdb_put_packet(connection, "OK", 2);
-	return ERROR_OK;
+	return JIM_OK;
 }
 
-
-static bool nuttx_detect_rtos(struct target *target)
+static int nuttx_smp_init(struct target *target)
 {
-	if ((target->rtos->symbols) &&
-			(target->rtos->symbols[0].address != 0) &&
-			(target->rtos->symbols[1].address != 0)) {
-		return true;
-	}
-	return false;
+	/* Return OK for now so that the initialisation sequence doesn't stop.
+	 * SMP case will be implemented later. */
+	return ERROR_OK;
 }
 
-static int nuttx_create(struct target *target)
+static target_addr_t target_buffer_get_addr(struct target *target, const uint8_t *buffer)
 {
-
-	target->rtos->gdb_thread_packet = nuttx_thread_packet;
-	LOG_INFO("target type name = %s", target->type->name);
-	return 0;
+#if PTR_WIDTH == 8
+	return target_buffer_get_u64(target, buffer);
+#else
+	return target_buffer_get_u32(target, buffer);
+#endif
 }
 
 static int nuttx_update_threads(struct rtos *rtos)
 {
-	uint32_t thread_count;
-	struct tcb tcb;
-	int ret;
-	uint32_t head;
-	uint32_t tcb_addr;
-	uint32_t i;
+	struct tcbinfo tcbinfo;
+	uint32_t pidhashaddr, npidhash, tcbaddr;
+	uint16_t pid;
 	uint8_t state;
 
 	if (!rtos->symbols) {
-		LOG_ERROR("No symbols for NuttX");
-		return -3;
+		LOG_ERROR("No symbols for nuttx");
+		return ERROR_FAIL;
 	}
 
-	/* free previous thread details */
+	/* Free previous thread details */
 	rtos_free_threadlist(rtos);
 
-	ret = target_read_buffer(rtos->target, rtos->symbols[1].address,
-		sizeof(g_tasklist), (uint8_t *)&g_tasklist);
-	if (ret) {
-		LOG_ERROR("target_read_buffer : ret = %d\n", ret);
+	/* NuttX provides a hash table that keeps track of all the TCBs.
+	 * We first read its size from g_npidhash and its address from g_pidhash.
+	 * Its content is then read from these values.
+	 */
+	int ret = target_read_u32(rtos->target, rtos->symbols[NX_SYM_NPIDHASH].address, &npidhash);
+	if (ret != ERROR_OK) {
+		LOG_ERROR("Failed to read g_npidhash: ret = %d", ret);
 		return ERROR_FAIL;
 	}
 
-	thread_count = 0;
+	LOG_DEBUG("Hash table size (g_npidhash) = %" PRId32, npidhash);
+
+	ret = target_read_u32(rtos->target, rtos->symbols[NX_SYM_PIDHASH].address, &pidhashaddr);
+	if (ret != ERROR_OK) {
+		LOG_ERROR("Failed to read g_pidhash address: ret = %d", ret);
+		return ERROR_FAIL;
+	}
 
-	for (i = 0; i < TASK_QUEUE_NUM; i++) {
+	LOG_DEBUG("Hash table address (g_pidhash) = %" PRIx32, pidhashaddr);
+
+	uint8_t *pidhash = malloc(npidhash * PTR_WIDTH);
+	if (!pidhash) {
+		LOG_ERROR("Failed to allocate pidhash");
+		return ERROR_FAIL;
+	}
 
-		if (g_tasklist[i].addr == 0)
+	ret = target_read_buffer(rtos->target, pidhashaddr, PTR_WIDTH * npidhash, pidhash);
+	if (ret != ERROR_OK) {
+		LOG_ERROR("Failed to read tcbhash: ret = %d", ret);
+		goto errout;
+	}
+
+	/* NuttX provides a struct that contains TCB offsets for required members.
+	 * Read its content from g_tcbinfo.
+	 */
+	uint8_t buff[TCBINFO_TARGET_SIZE];
+	ret = target_read_buffer(rtos->target, rtos->symbols[NX_SYM_TCB_INFO].address, sizeof(buff), buff);
+	if (ret != ERROR_OK) {
+		LOG_ERROR("Failed to read tcbinfo: ret = %d", ret);
+		goto errout;
+	}
+	tcbinfo.pid_off = target_buffer_get_u16(rtos->target, buff);
+	tcbinfo.state_off = target_buffer_get_u16(rtos->target, buff + 2);
+	tcbinfo.pri_off = target_buffer_get_u16(rtos->target, buff + 4);
+	tcbinfo.name_off = target_buffer_get_u16(rtos->target, buff + 6);
+	tcbinfo.regs_off = target_buffer_get_u16(rtos->target, buff + 8);
+	tcbinfo.basic_num = target_buffer_get_u16(rtos->target, buff + 10);
+	tcbinfo.total_num = target_buffer_get_u16(rtos->target, buff + 12);
+	tcbinfo.xcpreg_off = target_buffer_get_addr(rtos->target, buff + 14);
+
+	/* The head of the g_readytorun list is the currently running task.
+	 * Reading in a temporary variable first to avoid endianness issues,
+	 * rtos->current_thread is int64_t. */
+	uint32_t current_thread;
+	ret = target_read_u32(rtos->target, rtos->symbols[NX_SYM_READYTORUN].address, &current_thread);
+	if (ret != ERROR_OK) {
+		LOG_ERROR("Failed to read g_readytorun: ret = %d", ret);
+		goto errout;
+	}
+	rtos->current_thread = current_thread;
+
+	uint32_t thread_count = 0;
+
+	for (unsigned int i = 0; i < npidhash; i++) {
+		tcbaddr = target_buffer_get_u32(rtos->target, &pidhash[i * PTR_WIDTH]);
+
+		if (!tcbaddr)
 			continue;
 
-		ret = target_read_u32(rtos->target, g_tasklist[i].addr,
-			&head);
+		ret = target_read_u16(rtos->target, tcbaddr + tcbinfo.pid_off, &pid);
+		if (ret != ERROR_OK) {
+			LOG_ERROR("Failed to read PID of TCB@0x%x from pidhash[%d]: ret = %d",
+				tcbaddr, i, ret);
+			goto errout;
+		}
 
-		if (ret) {
-			LOG_ERROR("target_read_u32 : ret = %d\n", ret);
-			return ERROR_FAIL;
+		ret = target_read_u8(rtos->target, tcbaddr + tcbinfo.state_off, &state);
+		if (ret != ERROR_OK) {
+			LOG_ERROR("Failed to read state of TCB@0x%x from pidhash[%d]: ret = %d",
+				tcbaddr, i, ret);
+			goto errout;
 		}
 
-		/* readytorun head is current thread */
-		if (g_tasklist[i].addr == rtos->symbols[0].address)
-			rtos->current_thread = head;
+		struct thread_detail *new_thread_details = realloc(rtos->thread_details,
+			sizeof(struct thread_detail) * (thread_count + 1));
+		if (!new_thread_details) {
+			ret = ERROR_FAIL;
+			goto errout;
+		}
 
+		struct thread_detail *thread = &new_thread_details[thread_count];
+		thread->threadid = tcbaddr;
+		thread->exists = true;
+		thread->extra_info_str = NULL;
 
-		tcb_addr = head;
-		while (tcb_addr) {
-			struct thread_detail *thread;
-			ret = target_read_buffer(rtos->target, tcb_addr,
-				sizeof(tcb), (uint8_t *)&tcb);
-			if (ret) {
-				LOG_ERROR("target_read_buffer : ret = %d\n",
-					ret);
-				return ERROR_FAIL;
-			}
-			thread_count++;
-
-			rtos->thread_details = realloc(rtos->thread_details,
-				sizeof(struct thread_detail) * thread_count);
-			thread = &rtos->thread_details[thread_count - 1];
-			thread->threadid = tcb_addr;
-			thread->exists = true;
-
-			state = tcb.dat[state_offset - 8];
-			thread->extra_info_str = NULL;
-			if (state < ARRAY_SIZE(task_state_str)) {
-				thread->extra_info_str = malloc(256);
-				snprintf(thread->extra_info_str, 256, "pid:%d, %s",
-				    tcb.dat[pid_offset - 8] |
-				    tcb.dat[pid_offset - 8 + 1] << 8,
-				    task_state_str[state]);
-			}
+		rtos->thread_details = new_thread_details;
+		thread_count++;
 
-			if (name_offset) {
-				thread->thread_name_str = malloc(name_size + 1);
-				snprintf(thread->thread_name_str, name_size,
-				    "%s", (char *)&tcb.dat[name_offset - 8]);
-			} else {
-				thread->thread_name_str = malloc(sizeof("None"));
-				strcpy(thread->thread_name_str, "None");
+		if (state < ARRAY_SIZE(task_state_str)) {
+			thread->extra_info_str = malloc(EXTRAINFO_SIZE);
+			if (!thread->extra_info_str) {
+				ret = ERROR_FAIL;
+				goto errout;
 			}
+			snprintf(thread->extra_info_str, EXTRAINFO_SIZE, "pid:%d, %s",
+				pid,
+				task_state_str[state]);
+		}
 
-			tcb_addr = tcb.flink;
+		if (tcbinfo.name_off) {
+			thread->thread_name_str = calloc(NAME_SIZE + 1, sizeof(char));
+			if (!thread->thread_name_str) {
+				ret = ERROR_FAIL;
+				goto errout;
+			}
+			ret = target_read_buffer(rtos->target, tcbaddr + tcbinfo.name_off,
+				sizeof(char) * NAME_SIZE, (uint8_t *)thread->thread_name_str);
+			if (ret != ERROR_OK) {
+				LOG_ERROR("Failed to read thread's name: ret = %d", ret);
+				goto errout;
+			}
+		} else {
+			thread->thread_name_str = strdup("None");
 		}
 	}
-	rtos->thread_count = thread_count;
 
-	return 0;
+	ret = ERROR_OK;
+	rtos->thread_count = thread_count;
+errout:
+	free(pidhash);
+	return ret;
 }
 
-
-/*
- * thread_id = tcb address;
- */
-static int nuttx_get_thread_reg_list(struct rtos *rtos, int64_t thread_id,
+static int nuttx_getreg_current_thread(struct rtos *rtos,
 	struct rtos_reg **reg_list, int *num_regs)
 {
-	int retval;
-
-	/* Check for armv7m with *enabled* FPU, i.e. a Cortex-M4F */
-	bool cm4_fpu_enabled = false;
-	struct armv7m_common *armv7m_target = target_to_armv7m(rtos->target);
-	if (is_armv7m(armv7m_target)) {
-		if (armv7m_target->fp_feature == FPV4_SP) {
-			/* Found ARM v7m target which includes a FPU */
-			uint32_t cpacr;
-
-			retval = target_read_u32(rtos->target, FPU_CPACR, &cpacr);
-			if (retval != ERROR_OK) {
-				LOG_ERROR("Could not read CPACR register to check FPU state");
-				return -1;
-			}
+	struct reg **gdb_reg_list;
+
+	/* Registers for currently running thread are not on task's stack and
+	 * should be retrieved from reg caches via target_get_gdb_reg_list */
+	int ret = target_get_gdb_reg_list(rtos->target, &gdb_reg_list, num_regs,
+		REG_CLASS_GENERAL);
+	if (ret != ERROR_OK) {
+		LOG_ERROR("target_get_gdb_reg_list failed %d", ret);
+		return ret;
+	}
 
-			/* Check if CP10 and CP11 are set to full access. */
-			if (cpacr & 0x00F00000) {
-				/* Found target with enabled FPU */
-				cm4_fpu_enabled = 1;
-			}
+	*reg_list = calloc(*num_regs, sizeof(struct rtos_reg));
+	if (!(*reg_list)) {
+		LOG_ERROR("Failed to alloc memory for %d", *num_regs);
+		free(gdb_reg_list);
+		return ERROR_FAIL;
+	}
+
+	for (int i = 0; i < *num_regs; i++) {
+		(*reg_list)[i].number = gdb_reg_list[i]->number;
+		(*reg_list)[i].size = gdb_reg_list[i]->size;
+		memcpy((*reg_list)[i].value, gdb_reg_list[i]->value, ((*reg_list)[i].size + 7) / 8);
+	}
+
+	free(gdb_reg_list);
+
+	return ERROR_OK;
+}
+
+static int nuttx_getregs_fromstack(struct rtos *rtos, int64_t thread_id,
+	struct rtos_reg **reg_list, int *num_regs)
+{
+	uint16_t xcpreg_off;
+	uint32_t regsaddr;
+	const struct nuttx_params *priv = rtos->rtos_specific_params;
+	const struct rtos_register_stacking *stacking = priv->stacking;
+
+	if (!stacking) {
+		if (priv->select_stackinfo) {
+			stacking = priv->select_stackinfo(rtos->target);
+		} else {
+			LOG_ERROR("Can't find a way to get stacking info");
+			return ERROR_FAIL;
 		}
 	}
 
-	const struct rtos_register_stacking *stacking;
-	if (cm4_fpu_enabled)
-		stacking = &nuttx_stacking_cortex_m_fpu;
-	else
-		stacking = &nuttx_stacking_cortex_m;
+	int ret = target_read_u16(rtos->target,
+		rtos->symbols[NX_SYM_TCB_INFO].address + offsetof(struct tcbinfo, regs_off),
+		&xcpreg_off);
+	if (ret != ERROR_OK) {
+		LOG_ERROR("Failed to read registers' offset: ret = %d", ret);
+		return ERROR_FAIL;
+	}
+
+	ret = target_read_u32(rtos->target, thread_id + xcpreg_off, &regsaddr);
+	if (ret != ERROR_OK) {
+		LOG_ERROR("Failed to read registers' address: ret = %d", ret);
+		return ERROR_FAIL;
+	}
 
-	return rtos_generic_stack_read(rtos->target, stacking,
-	    (uint32_t)thread_id + xcpreg_offset, reg_list, num_regs);
+	return rtos_generic_stack_read(rtos->target, stacking, regsaddr, reg_list, num_regs);
 }
 
-static int nuttx_get_symbol_list_to_lookup(struct symbol_table_elem *symbol_list[])
+static int nuttx_get_thread_reg_list(struct rtos *rtos, int64_t thread_id,
+	struct rtos_reg **reg_list, int *num_regs)
 {
-	unsigned int i;
+	if (!rtos) {
+		LOG_ERROR("NUTTX: out of memory");
+		return ERROR_FAIL;
+	}
+
+	if (thread_id == rtos->current_thread)
+		return nuttx_getreg_current_thread(rtos, reg_list, num_regs);
+	return nuttx_getregs_fromstack(rtos, thread_id, reg_list, num_regs);
+}
 
-	*symbol_list = (struct symbol_table_elem *) calloc(1,
-		sizeof(struct symbol_table_elem) * ARRAY_SIZE(nuttx_symbol_list));
+static int nuttx_get_symbol_list_to_lookup(struct symbol_table_elem *symbol_list[])
+{
+	*symbol_list = calloc(ARRAY_SIZE(nuttx_symbol_list), sizeof(**symbol_list));
+	if (!*symbol_list) {
+		LOG_ERROR("NUTTX: out of memory");
+		return ERROR_FAIL;
+	}
 
-	for (i = 0; i < ARRAY_SIZE(nuttx_symbol_list); i++)
-		(*symbol_list)[i].symbol_name = nuttx_symbol_list[i];
+	for (unsigned int i = 0; i < ARRAY_SIZE(nuttx_symbol_list); i++) {
+		(*symbol_list)[i].symbol_name = nuttx_symbol_list[i].name;
+		(*symbol_list)[i].optional = nuttx_symbol_list[i].optional;
+	}
 
-	return 0;
+	return ERROR_OK;
 }
 
-struct rtos_type nuttx_rtos = {
+const struct rtos_type nuttx_rtos = {
 	.name = "nuttx",
 	.detect_rtos = nuttx_detect_rtos,
 	.create = nuttx_create,
+	.smp_init = nuttx_smp_init,
 	.update_threads = nuttx_update_threads,
 	.get_thread_reg_list = nuttx_get_thread_reg_list,
 	.get_symbol_list_to_lookup = nuttx_get_symbol_list_to_lookup,
diff --git a/src/rtos/nuttx_header.h b/src/rtos/nuttx_header.h
deleted file mode 100644
index 3436df1..0000000
--- a/src/rtos/nuttx_header.h
+++ /dev/null
@@ -1,60 +0,0 @@
-/* SPDX-License-Identifier: GPL-2.0-or-later */
-
-/***************************************************************************
- *   Copyright 2016,2017 Sony Video & Sound Products Inc.                  *
- *   Masatoshi Tateishi - Masatoshi.Tateishi@jp.sony.com                   *
- *   Masayuki Ishikawa - Masayuki.Ishikawa@jp.sony.com                     *
- ***************************************************************************/
-
-#ifndef OPENOCD_RTOS_NUTTX_HEADER_H
-#define OPENOCD_RTOS_NUTTX_HEADER_H
-
-/*  gdb script to update the header file
-  according to kernel version and build option
-  before executing function awareness
-  kernel symbol must be loaded : symbol nuttx
-
-define awareness
- set logging off
- set logging file nuttx_header.h
- set logging on
-
- printf "#define PID  %p\n",&((struct tcb_s *)(0))->pid
- printf "#define XCPREG  %p\n",&((struct tcb_s *)(0))->xcp.regs
- printf "#define STATE %p\n",&((struct tcb_s *)(0))->task_state
- printf "#define NAME %p\n",&((struct tcb_s *)(0))->name
- printf "#define NAME_SIZE %d\n",sizeof(((struct tcb_s *)(0))->name)
- end
-
-
- OR ~/.gdbinit
-
-
-define hookpost-file
-
- if &g_readytorun != 0
-  eval "monitor nuttx.pid_offset %d", &((struct tcb_s *)(0))->pid
-  eval "monitor nuttx.xcpreg_offset %d", &((struct tcb_s *)(0))->xcp.regs
-  eval "monitor nuttx.state_offset %d", &((struct tcb_s *)(0))->task_state
-  eval "monitor nuttx.name_offset %d", &((struct tcb_s *)(0))->name
-  eval "monitor nuttx.name_size %d", sizeof(((struct tcb_s *)(0))->name)
- end
-
-end
-
-*/
-
-/* default offset */
-#define PID  0xc
-#define XCPREG  0x70
-#define STATE 0x19
-#define NAME 0xb8
-#define NAME_SIZE 32
-
-/* defconfig of nuttx */
-/* #define CONFIG_DISABLE_SIGNALS */
-#define CONFIG_DISABLE_MQUEUE
-/* #define CONFIG_PAGING */
-
-
-#endif /* OPENOCD_RTOS_NUTTX_HEADER_H */
diff --git a/src/rtos/rtkernel.c b/src/rtos/rtkernel.c
new file mode 100644
index 0000000..ba1de25
--- /dev/null
+++ b/src/rtos/rtkernel.c
@@ -0,0 +1,384 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+
+/***************************************************************************
+ *   Copyright (C) 2016-2023 by Andreas Fritiofson                         *
+ *   andreas.fritiofson@gmail.com                                          *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <helper/time_support.h>
+#include <jtag/jtag.h>
+#include "target/target.h"
+#include "target/target_type.h"
+#include "rtos.h"
+#include "helper/log.h"
+#include "helper/types.h"
+#include "rtos_standard_stackings.h"
+#include "target/armv7m.h"
+#include "target/cortex_m.h"
+
+#define ST_DEAD     BIT(0)    /* Task is waiting to be deleted */
+#define ST_WAIT     BIT(1)    /* Task is blocked: */
+#define ST_SEM      BIT(2)    /*  on semaphore */
+#define ST_MTX      BIT(3)    /*  on mutex */
+#define ST_SIG      BIT(4)    /*  on signal */
+#define ST_DLY      BIT(5)    /*  on timer */
+#define ST_FLAG     BIT(6)    /*  on flag */
+#define ST_FLAG_ALL BIT(7)    /*  on flag and flag mode is "ALL" */
+#define ST_MBOX     BIT(8)    /*  on mailbox */
+#define ST_STP      BIT(9)    /*  self stopped */
+#define ST_SUSPEND  BIT(10)   /* Task is suspended */
+#define ST_TT       BIT(11)   /* Time triggered task */
+#define ST_TT_YIELD BIT(12)   /* Time triggered task that yields */
+#define ST_CREATE   BIT(13)   /* Task was created by task_create() */
+
+struct rtkernel_params {
+	const char *target_name;
+	const struct rtos_register_stacking *stacking_info_cm3;
+	const struct rtos_register_stacking *stacking_info_cm4f;
+	const struct rtos_register_stacking *stacking_info_cm4f_fpu;
+};
+
+static const struct rtkernel_params rtkernel_params_list[] = {
+	{
+		"cortex_m",			/* target_name */
+		&rtos_standard_cortex_m3_stacking,	/* stacking_info */
+		&rtos_standard_cortex_m4f_stacking,
+		&rtos_standard_cortex_m4f_fpu_stacking,
+	},
+	{
+		"hla_target",			/* target_name */
+		&rtos_standard_cortex_m3_stacking,	/* stacking_info */
+		&rtos_standard_cortex_m4f_stacking,
+		&rtos_standard_cortex_m4f_fpu_stacking,
+	},
+};
+
+enum rtkernel_symbol_values {
+	sym_os_state = 0,
+	sym___off_os_state2chain = 1,
+	sym___off_os_state2current = 2,
+	sym___off_task2chain = 3,
+	sym___off_task2magic = 4,
+	sym___off_task2stack = 5,
+	sym___off_task2state = 6,
+	sym___off_task2name = 7,
+	sym___val_task_magic = 8,
+};
+
+struct symbols {
+	const char *name;
+	bool optional;
+};
+
+static const struct symbols rtkernel_symbol_list[] = {
+	{ "os_state", false },
+	{ "__off_os_state2chain", false },
+	{ "__off_os_state2current", false },
+	{ "__off_task2chain", false },
+	{ "__off_task2magic", false },
+	{ "__off_task2stack", false },
+	{ "__off_task2state", false },
+	{ "__off_task2name", false },
+	{ "__val_task_magic", false },
+	{ NULL, false }
+};
+
+static void *realloc_preserve(void *ptr, size_t old_size, size_t new_size)
+{
+	void *new_ptr = malloc(new_size);
+
+	if (new_ptr) {
+		memcpy(new_ptr, ptr, MIN(old_size, new_size));
+		free(ptr);
+	}
+
+	return new_ptr;
+}
+
+static int rtkernel_add_task(struct rtos *rtos, uint32_t task, uint32_t current_task)
+{
+	int retval;
+	int new_thread_count = rtos->thread_count + 1;
+	struct thread_detail *new_thread_details = realloc_preserve(rtos->thread_details,
+			rtos->thread_count * sizeof(struct thread_detail),
+			new_thread_count * sizeof(struct thread_detail));
+	if (!new_thread_details) {
+		LOG_ERROR("Error growing memory to %d threads", new_thread_count);
+		return ERROR_FAIL;
+	}
+	rtos->thread_details = new_thread_details;
+	struct thread_detail *thread = &new_thread_details[rtos->thread_count];
+
+	*thread = (struct thread_detail){ .threadid = task, .exists = true };
+
+	/* Read the task name */
+	uint32_t name;
+	retval = target_read_u32(rtos->target, task + rtos->symbols[sym___off_task2name].address, &name);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("Could not read task name pointer from target");
+		return retval;
+	}
+	uint8_t tmp_str[33];
+	retval = target_read_buffer(rtos->target, name, sizeof(tmp_str) - 1, tmp_str);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("Error reading task name from target");
+		return retval;
+	}
+	tmp_str[sizeof(tmp_str) - 1] = '\0';
+	LOG_DEBUG("task name at 0x%" PRIx32 ", value \"%s\"", name, tmp_str);
+
+	if (tmp_str[0] != '\0')
+		thread->thread_name_str = strdup((char *)tmp_str);
+	else
+		thread->thread_name_str = strdup("No Name");
+
+	/* Read the task state */
+	uint16_t state;
+	retval = target_read_u16(rtos->target, task + rtos->symbols[sym___off_task2state].address, &state);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("Could not read task state from target");
+		return retval;
+	}
+
+	LOG_DEBUG("task state 0x%" PRIx16, state);
+
+	char state_str[64] = "";
+	if (state & ST_TT)
+		strcat(state_str, "TT|");
+	if (task == current_task) {
+		strcat(state_str, "RUN");
+	} else {
+		if (state & (ST_TT | ST_TT_YIELD))
+			strcat(state_str, "YIELD");
+		else if (state & ST_DEAD)
+			strcat(state_str, "DEAD");
+		else if (state & ST_WAIT)
+			strcat(state_str, "WAIT");
+		else if (state & ST_SUSPEND)
+			strcat(state_str, "SUSP");
+		else
+			strcat(state_str, "READY");
+	}
+	if (state & ST_SEM)
+		strcat(state_str, "|SEM");
+	if (state & ST_MTX)
+		strcat(state_str, "|MTX");
+	if (state & ST_SIG)
+		strcat(state_str, "|SIG");
+	if (state & ST_DLY)
+		strcat(state_str, "|DLY");
+	if ((state & ST_FLAG) || (state & ST_FLAG_ALL))
+		strcat(state_str, "|FLAG");
+	if (state & ST_FLAG_ALL)
+		strcat(state_str, "_ALL");
+	if (state & ST_MBOX)
+		strcat(state_str, "|MBOX");
+	if (state & ST_STP)
+		strcat(state_str, "|STP");
+
+	thread->extra_info_str = strdup(state_str);
+
+	rtos->thread_count = new_thread_count;
+	if (task == current_task)
+		rtos->current_thread = task;
+	return ERROR_OK;
+}
+
+static int rtkernel_verify_task(struct rtos *rtos, uint32_t task)
+{
+	int retval;
+	uint32_t magic;
+	retval = target_read_u32(rtos->target, task + rtos->symbols[sym___off_task2magic].address, &magic);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("Could not read task magic from target");
+		return retval;
+	}
+	if (magic != rtos->symbols[sym___val_task_magic].address) {
+		LOG_ERROR("Invalid task found (magic=0x%" PRIx32 ")", magic);
+		return ERROR_FAIL;
+	}
+	return retval;
+}
+
+static int rtkernel_update_threads(struct rtos *rtos)
+{
+	/* wipe out previous thread details if any */
+	/* do this first because rtos layer does not check our retval */
+	rtos_free_threadlist(rtos);
+	rtos->current_thread = 0;
+
+	if (!rtos->symbols) {
+		LOG_ERROR("No symbols for rt-kernel");
+		return -3;
+	}
+
+	/* read the current task */
+	uint32_t current_task;
+	int retval = target_read_u32(rtos->target,
+			rtos->symbols[sym_os_state].address + rtos->symbols[sym___off_os_state2current].address,
+			&current_task);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("Error reading current task");
+		return retval;
+	}
+	LOG_DEBUG("current task is 0x%" PRIx32, current_task);
+
+	retval = rtkernel_verify_task(rtos, current_task);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("Current task is invalid");
+		return retval;
+	}
+
+	/* loop through kernel task list */
+	uint32_t chain = rtos->symbols[sym_os_state].address + rtos->symbols[sym___off_os_state2chain].address;
+	LOG_DEBUG("chain start at 0x%" PRIx32, chain);
+
+	uint32_t next = chain;
+	for (;;) {
+		retval = target_read_u32(rtos->target, next, &next);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("Could not read rt-kernel data structure from target");
+			return retval;
+		}
+		LOG_DEBUG("next entry at 0x%" PRIx32, next);
+		if (next == chain) {
+			LOG_DEBUG("end of chain detected");
+			break;
+		}
+		uint32_t task = next - rtos->symbols[sym___off_task2chain].address;
+		LOG_DEBUG("found task at 0x%" PRIx32, task);
+
+		retval = rtkernel_verify_task(rtos, task);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("Invalid task found");
+			return retval;
+		}
+
+		retval = rtkernel_add_task(rtos, task, current_task);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("Could not add task to rtos system");
+			return retval;
+		}
+	}
+	return ERROR_OK;
+}
+
+static int rtkernel_get_thread_reg_list(struct rtos *rtos, int64_t thread_id,
+		struct rtos_reg **reg_list, int *num_regs)
+{
+	uint32_t stack_ptr = 0;
+
+	if (!rtos)
+		return -1;
+
+	if (thread_id == 0)
+		return -2;
+
+	if (!rtos->rtos_specific_params)
+		return -1;
+
+	const struct rtkernel_params *param = rtos->rtos_specific_params;
+
+	/* Read the stack pointer */
+	int retval = target_read_u32(rtos->target, thread_id + rtos->symbols[sym___off_task2stack].address, &stack_ptr);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("Error reading stack pointer from rtkernel thread");
+		return retval;
+	}
+	LOG_DEBUG("stack pointer at 0x%" PRIx64 ", value 0x%" PRIx32,
+			thread_id + rtos->symbols[sym___off_task2stack].address,
+			stack_ptr);
+
+	/* Adjust stack pointer to ignore non-standard BASEPRI register stacking */
+	stack_ptr += 4;
+
+	/* Check for armv7m with *enabled* FPU, i.e. a Cortex M4F */
+	bool cm4_fpu_enabled = false;
+	struct armv7m_common *armv7m_target = target_to_armv7m(rtos->target);
+	if (is_armv7m(armv7m_target)) {
+		if (armv7m_target->fp_feature != FP_NONE) {
+			/* Found ARM v7m target which includes a FPU */
+			uint32_t cpacr;
+
+			retval = target_read_u32(rtos->target, FPU_CPACR, &cpacr);
+			if (retval != ERROR_OK) {
+				LOG_ERROR("Could not read CPACR register to check FPU state");
+				return -1;
+			}
+
+			/* Check if CP10 and CP11 are set to full access. */
+			if (cpacr & 0x00F00000) {
+				/* Found target with enabled FPU */
+				cm4_fpu_enabled = true;
+			}
+		}
+	}
+
+	if (!cm4_fpu_enabled) {
+		LOG_DEBUG("cm3 stacking");
+		return rtos_generic_stack_read(rtos->target, param->stacking_info_cm3, stack_ptr, reg_list, num_regs);
+	}
+
+	/* Read the LR to decide between stacking with or without FPU */
+	uint32_t lr_svc;
+	retval = target_read_u32(rtos->target, stack_ptr + 0x20, &lr_svc);
+	if (retval != ERROR_OK) {
+		LOG_OUTPUT("Error reading stack frame from rtkernel thread\r\n");
+		return retval;
+	}
+
+	if ((lr_svc & 0x10) == 0) {
+		LOG_DEBUG("cm4f_fpu stacking");
+		return rtos_generic_stack_read(rtos->target, param->stacking_info_cm4f_fpu, stack_ptr, reg_list, num_regs);
+	}
+
+	LOG_DEBUG("cm4f stacking");
+	return rtos_generic_stack_read(rtos->target, param->stacking_info_cm4f, stack_ptr, reg_list, num_regs);
+}
+
+static int rtkernel_get_symbol_list_to_lookup(struct symbol_table_elem *symbol_list[])
+{
+	*symbol_list = calloc(ARRAY_SIZE(rtkernel_symbol_list), sizeof(struct symbol_table_elem));
+	if (!*symbol_list)
+		return ERROR_FAIL;
+
+	for (size_t i = 0; i < ARRAY_SIZE(rtkernel_symbol_list); i++) {
+		(*symbol_list)[i].symbol_name = rtkernel_symbol_list[i].name;
+		(*symbol_list)[i].optional = rtkernel_symbol_list[i].optional;
+	}
+
+	return ERROR_OK;
+}
+
+static bool rtkernel_detect_rtos(struct target *target)
+{
+	return (target->rtos->symbols) &&
+			(target->rtos->symbols[sym___off_os_state2chain].address != 0);
+}
+
+static int rtkernel_create(struct target *target)
+{
+	for (size_t i = 0; i < ARRAY_SIZE(rtkernel_params_list); i++) {
+		if (strcmp(rtkernel_params_list[i].target_name, target->type->name) == 0) {
+			target->rtos->rtos_specific_params = (void *)&rtkernel_params_list[i];
+			return 0;
+		}
+	}
+
+	LOG_ERROR("Could not find target in rt-kernel compatibility list");
+	return -1;
+}
+
+const struct rtos_type rtkernel_rtos = {
+	.name = "rtkernel",
+
+	.detect_rtos = rtkernel_detect_rtos,
+	.create = rtkernel_create,
+	.update_threads = rtkernel_update_threads,
+	.get_thread_reg_list = rtkernel_get_thread_reg_list,
+	.get_symbol_list_to_lookup = rtkernel_get_symbol_list_to_lookup,
+};
diff --git a/src/rtos/rtos.c b/src/rtos/rtos.c
index c9da33b..8bfe3d5 100644
--- a/src/rtos/rtos.c
+++ b/src/rtos/rtos.c
@@ -17,21 +17,22 @@
 #include "server/gdb_server.h"
 
 /* RTOSs */
-extern struct rtos_type freertos_rtos;
-extern struct rtos_type threadx_rtos;
-extern struct rtos_type ecos_rtos;
-extern struct rtos_type linux_rtos;
-extern struct rtos_type chibios_rtos;
-extern struct rtos_type chromium_ec_rtos;
-extern struct rtos_type embkernel_rtos;
-extern struct rtos_type mqx_rtos;
-extern struct rtos_type ucos_iii_rtos;
-extern struct rtos_type nuttx_rtos;
-extern struct rtos_type hwthread_rtos;
-extern struct rtos_type riot_rtos;
-extern struct rtos_type zephyr_rtos;
-
-static struct rtos_type *rtos_types[] = {
+extern const struct rtos_type freertos_rtos;
+extern const struct rtos_type threadx_rtos;
+extern const struct rtos_type ecos_rtos;
+extern const struct rtos_type linux_rtos;
+extern const struct rtos_type chibios_rtos;
+extern const struct rtos_type chromium_ec_rtos;
+extern const struct rtos_type embkernel_rtos;
+extern const struct rtos_type mqx_rtos;
+extern const struct rtos_type ucos_iii_rtos;
+extern const struct rtos_type nuttx_rtos;
+extern const struct rtos_type hwthread_rtos;
+extern const struct rtos_type riot_rtos;
+extern const struct rtos_type zephyr_rtos;
+extern const struct rtos_type rtkernel_rtos;
+
+static const struct rtos_type *rtos_types[] = {
 	&threadx_rtos,
 	&freertos_rtos,
 	&ecos_rtos,
@@ -44,6 +45,7 @@ static struct rtos_type *rtos_types[] = {
 	&nuttx_rtos,
 	&riot_rtos,
 	&zephyr_rtos,
+	&rtkernel_rtos,
 	/* keep this as last, as it always matches with rtos auto */
 	&hwthread_rtos,
 	NULL
@@ -71,7 +73,7 @@ static int rtos_target_for_threadid(struct connection *connection,
 	return ERROR_OK;
 }
 
-static int os_alloc(struct target *target, struct rtos_type *ostype,
+static int os_alloc(struct target *target, const struct rtos_type *ostype,
 					struct command_context *cmd_ctx)
 {
 	struct rtos *os = target->rtos = calloc(1, sizeof(struct rtos));
@@ -103,7 +105,7 @@ static void os_free(struct target *target)
 	target->rtos = NULL;
 }
 
-static int os_alloc_create(struct target *target, struct rtos_type *ostype,
+static int os_alloc_create(struct target *target, const struct rtos_type *ostype,
 						   struct command_context *cmd_ctx)
 {
 	int ret = os_alloc(target, ostype, cmd_ctx);
@@ -657,7 +659,10 @@ int rtos_generic_stack_read(struct target *target,
 
 	if (stacking->stack_growth_direction == 1)
 		address -= stacking->stack_registers_size;
-	retval = target_read_buffer(target, address, stacking->stack_registers_size, stack_data);
+	if (stacking->read_stack)
+		retval = stacking->read_stack(target, address, stacking, stack_data);
+	else
+		retval = target_read_buffer(target, address, stacking->stack_registers_size, stack_data);
 	if (retval != ERROR_OK) {
 		free(stack_data);
 		LOG_ERROR("Error reading stack frame from thread");
@@ -800,7 +805,7 @@ int rtos_generic_stack_write_reg(struct target *target,
 static int rtos_try_next(struct target *target)
 {
 	struct rtos *os = target->rtos;
-	struct rtos_type **type = rtos_types;
+	const struct rtos_type **type = rtos_types;
 
 	if (!os)
 		return 0;
diff --git a/src/rtos/rtos.h b/src/rtos/rtos.h
index 6695915..f36288d 100644
--- a/src/rtos/rtos.h
+++ b/src/rtos/rtos.h
@@ -126,7 +126,15 @@ struct rtos_register_stacking {
 	/* Total number of registers on the stack, including the general ones. This
 	 * may be 0 if there are no additional registers on the stack beyond the
 	 * general ones. */
-	unsigned total_register_count;
+	unsigned int total_register_count;
+
+	/* Optional field for targets which may have to implement their own stack read function.
+	 * Because stack format can be weird or stack data needed to be edited before passing to the gdb.
+	 */
+	int (*read_stack)(struct target *target,
+		int64_t stack_ptr,
+		const struct rtos_register_stacking *stacking,
+		uint8_t *stack_data);
 };
 
 #define GDB_THREAD_PACKET_NOT_CONSUMED (-40)
diff --git a/src/rtos/rtos_chibios_stackings.c b/src/rtos/rtos_chibios_stackings.c
index e2fe0a2..c0816ac 100644
--- a/src/rtos/rtos_chibios_stackings.c
+++ b/src/rtos/rtos_chibios_stackings.c
@@ -14,6 +14,7 @@
 
 #include "rtos.h"
 #include "target/armv7m.h"
+#include "rtos_chibios_stackings.h"
 
 static const struct stack_register_offset rtos_chibios_arm_v7m_stack_offsets[ARMV7M_NUM_CORE_REGS] = {
 	{ ARMV7M_R0,   -1,   32 },		/* r0   */
diff --git a/src/rtos/rtos_chibios_stackings.h b/src/rtos/rtos_chibios_stackings.h
index 23ad44a..e909451 100644
--- a/src/rtos/rtos_chibios_stackings.h
+++ b/src/rtos/rtos_chibios_stackings.h
@@ -8,10 +8,6 @@
 #ifndef OPENOCD_RTOS_RTOS_CHIBIOS_STACKINGS_H
 #define OPENOCD_RTOS_RTOS_CHIBIOS_STACKINGS_H
 
-#ifdef HAVE_CONFIG_H
-#include "config.h"
-#endif
-
 #include "rtos.h"
 
 extern const struct rtos_register_stacking rtos_chibios_arm_v7m_stacking;
diff --git a/src/rtos/rtos_ecos_stackings.c b/src/rtos/rtos_ecos_stackings.c
index 86e1765..cae2712 100644
--- a/src/rtos/rtos_ecos_stackings.c
+++ b/src/rtos/rtos_ecos_stackings.c
@@ -5,8 +5,9 @@
 #endif
 
 #include "rtos.h"
-#include "rtos_standard_stackings.h"
 #include "target/armv7m.h"
+#include "rtos_standard_stackings.h"
+#include "rtos_ecos_stackings.h"
 
 /* For Cortex-M eCos applications the actual thread context register layout can
  * be different between active threads of an application depending on whether
diff --git a/src/rtos/rtos_ecos_stackings.h b/src/rtos/rtos_ecos_stackings.h
index 0375e2d..a6bcf1a 100644
--- a/src/rtos/rtos_ecos_stackings.h
+++ b/src/rtos/rtos_ecos_stackings.h
@@ -3,10 +3,6 @@
 #ifndef OPENOCD_RTOS_RTOS_ECOS_STACKINGS_H
 #define OPENOCD_RTOS_RTOS_ECOS_STACKINGS_H
 
-#ifdef HAVE_CONFIG_H
-#include "config.h"
-#endif
-
 #include "rtos.h"
 
 extern const struct rtos_register_stacking rtos_ecos_cortex_m3_stacking;
diff --git a/src/rtos/rtos_embkernel_stackings.c b/src/rtos/rtos_embkernel_stackings.c
index 809b622..b98628a 100644
--- a/src/rtos/rtos_embkernel_stackings.c
+++ b/src/rtos/rtos_embkernel_stackings.c
@@ -12,6 +12,7 @@
 #include "rtos.h"
 #include "target/armv7m.h"
 #include "rtos_standard_stackings.h"
+#include "rtos_embkernel_stackings.h"
 
 static const struct stack_register_offset rtos_embkernel_cortex_m_stack_offsets[ARMV7M_NUM_CORE_REGS] = {
 	{ ARMV7M_R0,   0x24, 32 },		/* r0   */
diff --git a/src/rtos/rtos_embkernel_stackings.h b/src/rtos/rtos_embkernel_stackings.h
index 972bce6..87bd0e7 100644
--- a/src/rtos/rtos_embkernel_stackings.h
+++ b/src/rtos/rtos_embkernel_stackings.h
@@ -8,10 +8,6 @@
 #ifndef OPENOCD_RTOS_RTOS_EMBKERNEL_STACKINGS_H
 #define OPENOCD_RTOS_RTOS_EMBKERNEL_STACKINGS_H
 
-#ifdef HAVE_CONFIG_H
-#include "config.h"
-#endif
-
 #include "rtos.h"
 
 extern const struct rtos_register_stacking rtos_embkernel_cortex_m_stacking;
diff --git a/src/rtos/rtos_mqx_stackings.c b/src/rtos/rtos_mqx_stackings.c
index 8c8fd20..5ab743b 100644
--- a/src/rtos/rtos_mqx_stackings.c
+++ b/src/rtos/rtos_mqx_stackings.c
@@ -11,7 +11,7 @@
 
 #include "rtos.h"
 #include "target/armv7m.h"
-
+#include "rtos_mqx_stackings.h"
 
 /*
  * standard exception stack
diff --git a/src/rtos/rtos_mqx_stackings.h b/src/rtos/rtos_mqx_stackings.h
index f86c05a..faa741d 100644
--- a/src/rtos/rtos_mqx_stackings.h
+++ b/src/rtos/rtos_mqx_stackings.h
@@ -8,10 +8,6 @@
 #ifndef OPENOCD_RTOS_RTOS_MQX_STACKINGS_H
 #define OPENOCD_RTOS_RTOS_MQX_STACKINGS_H
 
-#ifdef HAVE_CONFIG_H
-#include "config.h"
-#endif
-
 #include "rtos.h"
 
 extern const struct rtos_register_stacking rtos_mqx_arm_v7m_stacking;
diff --git a/src/rtos/rtos_nuttx_stackings.c b/src/rtos/rtos_nuttx_stackings.c
index b59b135..b70cccb 100644
--- a/src/rtos/rtos_nuttx_stackings.c
+++ b/src/rtos/rtos_nuttx_stackings.c
@@ -108,3 +108,361 @@ const struct rtos_register_stacking nuttx_riscv_stacking = {
 	.calculate_process_stack = rtos_generic_stack_align8,
 	.register_offsets = nuttx_stack_offsets_riscv,
 };
+
+static int nuttx_esp_xtensa_stack_read(struct target *target,
+	int64_t stack_ptr, const struct rtos_register_stacking *stacking,
+	uint8_t *stack_data)
+{
+	int retval = target_read_buffer(target, stack_ptr, stacking->stack_registers_size, stack_data);
+	if (retval != ERROR_OK)
+		return retval;
+
+	stack_data[4] &= ~0x10;	/* Clear exception bit in PS */
+
+	return ERROR_OK;
+}
+
+static const struct stack_register_offset nuttx_stack_offsets_esp32[] = {
+	{ 0, 0x00, 32 },		/* PC */
+	{ 1, 0x08, 32 },		/* A0 */
+	{ 2, 0x0c, 32 },		/* A1 */
+	{ 3, 0x10, 32 },		/* A2 */
+	{ 4, 0x14, 32 },		/* A3 */
+	{ 5, 0x18, 32 },		/* A4 */
+	{ 6, 0x1c, 32 },		/* A5 */
+	{ 7, 0x20, 32 },		/* A6 */
+	{ 8, 0x24, 32 },		/* A7 */
+	{ 9, 0x28, 32 },		/* A8 */
+	{ 10, 0x2c, 32 },		/* A9 */
+	{ 11, 0x30, 32 },		/* A10 */
+	{ 12, 0x34, 32 },		/* A11 */
+	{ 13, 0x38, 32 },		/* A12 */
+	{ 14, 0x3c, 32 },		/* A13 */
+	{ 15, 0x40, 32 },		/* A14 */
+	{ 16, 0x44, 32 },		/* A15 */
+	/* A16-A63 aren't in the stack frame because they've been flushed to the stack earlier */
+	{ 17, -1, 32 },			/* A16 */
+	{ 18, -1, 32 },			/* A17 */
+	{ 19, -1, 32 },			/* A18 */
+	{ 20, -1, 32 },			/* A19 */
+	{ 21, -1, 32 },			/* A20 */
+	{ 22, -1, 32 },			/* A21 */
+	{ 23, -1, 32 },			/* A22 */
+	{ 24, -1, 32 },			/* A23 */
+	{ 25, -1, 32 },			/* A24 */
+	{ 26, -1, 32 },			/* A25 */
+	{ 27, -1, 32 },			/* A26 */
+	{ 28, -1, 32 },			/* A27 */
+	{ 29, -1, 32 },			/* A28 */
+	{ 30, -1, 32 },			/* A29 */
+	{ 31, -1, 32 },			/* A30 */
+	{ 32, -1, 32 },			/* A31 */
+	{ 33, -1, 32 },			/* A32 */
+	{ 34, -1, 32 },			/* A33 */
+	{ 35, -1, 32 },			/* A34 */
+	{ 36, -1, 32 },			/* A35 */
+	{ 37, -1, 32 },			/* A36 */
+	{ 38, -1, 32 },			/* A37 */
+	{ 39, -1, 32 },			/* A38 */
+	{ 40, -1, 32 },			/* A39 */
+	{ 41, -1, 32 },			/* A40 */
+	{ 42, -1, 32 },			/* A41 */
+	{ 43, -1, 32 },			/* A42 */
+	{ 44, -1, 32 },			/* A43 */
+	{ 45, -1, 32 },			/* A44 */
+	{ 46, -1, 32 },			/* A45 */
+	{ 47, -1, 32 },			/* A46 */
+	{ 48, -1, 32 },			/* A47 */
+	{ 49, -1, 32 },			/* A48 */
+	{ 50, -1, 32 },			/* A49 */
+	{ 51, -1, 32 },			/* A50 */
+	{ 52, -1, 32 },			/* A51 */
+	{ 53, -1, 32 },			/* A52 */
+	{ 54, -1, 32 },			/* A53 */
+	{ 55, -1, 32 },			/* A54 */
+	{ 56, -1, 32 },			/* A55 */
+	{ 57, -1, 32 },			/* A56 */
+	{ 58, -1, 32 },			/* A57 */
+	{ 59, -1, 32 },			/* A58 */
+	{ 60, -1, 32 },			/* A59 */
+	{ 61, -1, 32 },			/* A60 */
+	{ 62, -1, 32 },			/* A61 */
+	{ 63, -1, 32 },			/* A62 */
+	{ 64, -1, 32 },			/* A63 */
+	{ 65, 0x58, 32 },		/* lbeg */
+	{ 66, 0x5c, 32 },		/* lend */
+	{ 67, 0x60, 32 },		/* lcount */
+	{ 68, 0x48, 32 },		/* SAR */
+	{ 69, -1, 32 },			/* windowbase */
+	{ 70, -1, 32 },			/* windowstart */
+	{ 71, -1, 32 },			/* configid0 */
+	{ 72, -1, 32 },			/* configid1 */
+	{ 73, 0x04, 32 },		/* PS */
+	{ 74, -1, 32 },			/* threadptr */
+	{ 75, -1, 32 },			/* br */
+	{ 76, 0x54, 32 },		/* scompare1 */
+	{ 77, -1, 32 },			/* acclo */
+	{ 78, -1, 32 },			/* acchi */
+	{ 79, -1, 32 },			/* m0 */
+	{ 80, -1, 32 },			/* m1 */
+	{ 81, -1, 32 },			/* m2 */
+	{ 82, -1, 32 },			/* m3 */
+	{ 83, -1, 32 },			/* expstate */
+	{ 84, -1, 32 },			/* f64r_lo */
+	{ 85, -1, 32 },			/* f64r_hi */
+	{ 86, -1, 32 },			/* f64s */
+	{ 87, -1, 32 },			/* f0 */
+	{ 88, -1, 32 },			/* f1 */
+	{ 89, -1, 32 },			/* f2 */
+	{ 90, -1, 32 },			/* f3 */
+	{ 91, -1, 32 },			/* f4 */
+	{ 92, -1, 32 },			/* f5 */
+	{ 93, -1, 32 },			/* f6 */
+	{ 94, -1, 32 },			/* f7 */
+	{ 95, -1, 32 },			/* f8 */
+	{ 96, -1, 32 },			/* f9 */
+	{ 97, -1, 32 },			/* f10 */
+	{ 98, -1, 32 },			/* f11 */
+	{ 99, -1, 32 },			/* f12 */
+	{ 100, -1, 32 },		/* f13 */
+	{ 101, -1, 32 },		/* f14 */
+	{ 102, -1, 32 },		/* f15 */
+	{ 103, -1, 32 },		/* fcr */
+	{ 104, -1, 32 },		/* fsr */
+};
+
+const struct rtos_register_stacking nuttx_esp32_stacking = {
+	.stack_registers_size = 26 * 4,
+	.stack_growth_direction = -1,
+	.num_output_registers = ARRAY_SIZE(nuttx_stack_offsets_esp32),
+	.calculate_process_stack = rtos_generic_stack_align8,
+	.register_offsets = nuttx_stack_offsets_esp32,
+	.read_stack = nuttx_esp_xtensa_stack_read,
+};
+
+static const struct stack_register_offset nuttx_stack_offsets_esp32s2[] = {
+	{ 0, 0x00, 32 },		/* PC */
+	{ 1, 0x08, 32 },		/* A0 */
+	{ 2, 0x0c, 32 },		/* A1 */
+	{ 3, 0x10, 32 },		/* A2 */
+	{ 4, 0x14, 32 },		/* A3 */
+	{ 5, 0x18, 32 },		/* A4 */
+	{ 6, 0x1c, 32 },		/* A5 */
+	{ 7, 0x20, 32 },		/* A6 */
+	{ 8, 0x24, 32 },		/* A7 */
+	{ 9, 0x28, 32 },		/* A8 */
+	{ 10, 0x2c, 32 },		/* A9 */
+	{ 11, 0x30, 32 },		/* A10 */
+	{ 12, 0x34, 32 },		/* A11 */
+	{ 13, 0x38, 32 },		/* A12 */
+	{ 14, 0x3c, 32 },		/* A13 */
+	{ 15, 0x40, 32 },		/* A14 */
+	{ 16, 0x44, 32 },		/* A15 */
+	/* A16-A63 aren't in the stack frame because they've been flushed to the stack earlier */
+	{ 17, -1, 32 },			/* A16 */
+	{ 18, -1, 32 },			/* A17 */
+	{ 19, -1, 32 },			/* A18 */
+	{ 20, -1, 32 },			/* A19 */
+	{ 21, -1, 32 },			/* A20 */
+	{ 22, -1, 32 },			/* A21 */
+	{ 23, -1, 32 },			/* A22 */
+	{ 24, -1, 32 },			/* A23 */
+	{ 25, -1, 32 },			/* A24 */
+	{ 26, -1, 32 },			/* A25 */
+	{ 27, -1, 32 },			/* A26 */
+	{ 28, -1, 32 },			/* A27 */
+	{ 29, -1, 32 },			/* A28 */
+	{ 30, -1, 32 },			/* A29 */
+	{ 31, -1, 32 },			/* A30 */
+	{ 32, -1, 32 },			/* A31 */
+	{ 33, -1, 32 },			/* A32 */
+	{ 34, -1, 32 },			/* A33 */
+	{ 35, -1, 32 },			/* A34 */
+	{ 36, -1, 32 },			/* A35 */
+	{ 37, -1, 32 },			/* A36 */
+	{ 38, -1, 32 },			/* A37 */
+	{ 39, -1, 32 },			/* A38 */
+	{ 40, -1, 32 },			/* A39 */
+	{ 41, -1, 32 },			/* A40 */
+	{ 42, -1, 32 },			/* A41 */
+	{ 43, -1, 32 },			/* A42 */
+	{ 44, -1, 32 },			/* A43 */
+	{ 45, -1, 32 },			/* A44 */
+	{ 46, -1, 32 },			/* A45 */
+	{ 47, -1, 32 },			/* A46 */
+	{ 48, -1, 32 },			/* A47 */
+	{ 49, -1, 32 },			/* A48 */
+	{ 50, -1, 32 },			/* A49 */
+	{ 51, -1, 32 },			/* A50 */
+	{ 52, -1, 32 },			/* A51 */
+	{ 53, -1, 32 },			/* A52 */
+	{ 54, -1, 32 },			/* A53 */
+	{ 55, -1, 32 },			/* A54 */
+	{ 56, -1, 32 },			/* A55 */
+	{ 57, -1, 32 },			/* A56 */
+	{ 58, -1, 32 },			/* A57 */
+	{ 59, -1, 32 },			/* A58 */
+	{ 60, -1, 32 },			/* A59 */
+	{ 61, -1, 32 },			/* A60 */
+	{ 62, -1, 32 },			/* A61 */
+	{ 63, -1, 32 },			/* A62 */
+	{ 64, -1, 32 },			/* A63 */
+	{ 65, 0x48, 32 },		/* SAR */
+	{ 66, -1, 32 },			/* windowbase */
+	{ 67, -1, 32 },			/* windowstart */
+	{ 68, -1, 32 },			/* configid0 */
+	{ 69, -1, 32 },			/* configid1 */
+	{ 70, 0x04, 32 },		/* PS */
+	{ 71, -1, 32 },			/* threadptr */
+	{ 72, -1, 32 },			/* gpio_out */
+};
+
+const struct rtos_register_stacking nuttx_esp32s2_stacking = {
+	.stack_registers_size = 25 * 4,
+	.stack_growth_direction = -1,
+	.num_output_registers = ARRAY_SIZE(nuttx_stack_offsets_esp32s2),
+	.calculate_process_stack = rtos_generic_stack_align8,
+	.register_offsets = nuttx_stack_offsets_esp32s2,
+	.read_stack = nuttx_esp_xtensa_stack_read,
+};
+
+static const struct stack_register_offset nuttx_stack_offsets_esp32s3[] = {
+	{ 0, 0x00, 32 },		/* PC */
+	{ 1, 0x08, 32 },		/* A0 */
+	{ 2, 0x0c, 32 },		/* A1 */
+	{ 3, 0x10, 32 },		/* A2 */
+	{ 4, 0x14, 32 },		/* A3 */
+	{ 5, 0x18, 32 },		/* A4 */
+	{ 6, 0x1c, 32 },		/* A5 */
+	{ 7, 0x20, 32 },		/* A6 */
+	{ 8, 0x24, 32 },		/* A7 */
+	{ 9, 0x28, 32 },		/* A8 */
+	{ 10, 0x2c, 32 },		/* A9 */
+	{ 11, 0x30, 32 },		/* A10 */
+	{ 12, 0x34, 32 },		/* A11 */
+	{ 13, 0x38, 32 },		/* A12 */
+	{ 14, 0x3c, 32 },		/* A13 */
+	{ 15, 0x40, 32 },		/* A14 */
+	{ 16, 0x44, 32 },		/* A15 */
+	/* A16-A63 aren't in the stack frame because they've been flushed to the stack earlier */
+	{ 17, -1, 32 },			/* A16 */
+	{ 18, -1, 32 },			/* A17 */
+	{ 19, -1, 32 },			/* A18 */
+	{ 20, -1, 32 },			/* A19 */
+	{ 21, -1, 32 },			/* A20 */
+	{ 22, -1, 32 },			/* A21 */
+	{ 23, -1, 32 },			/* A22 */
+	{ 24, -1, 32 },			/* A23 */
+	{ 25, -1, 32 },			/* A24 */
+	{ 26, -1, 32 },			/* A25 */
+	{ 27, -1, 32 },			/* A26 */
+	{ 28, -1, 32 },			/* A27 */
+	{ 29, -1, 32 },			/* A28 */
+	{ 30, -1, 32 },			/* A29 */
+	{ 31, -1, 32 },			/* A30 */
+	{ 32, -1, 32 },			/* A31 */
+	{ 33, -1, 32 },			/* A32 */
+	{ 34, -1, 32 },			/* A33 */
+	{ 35, -1, 32 },			/* A34 */
+	{ 36, -1, 32 },			/* A35 */
+	{ 37, -1, 32 },			/* A36 */
+	{ 38, -1, 32 },			/* A37 */
+	{ 39, -1, 32 },			/* A38 */
+	{ 40, -1, 32 },			/* A39 */
+	{ 41, -1, 32 },			/* A40 */
+	{ 42, -1, 32 },			/* A41 */
+	{ 43, -1, 32 },			/* A42 */
+	{ 44, -1, 32 },			/* A43 */
+	{ 45, -1, 32 },			/* A44 */
+	{ 46, -1, 32 },			/* A45 */
+	{ 47, -1, 32 },			/* A46 */
+	{ 48, -1, 32 },			/* A47 */
+	{ 49, -1, 32 },			/* A48 */
+	{ 50, -1, 32 },			/* A49 */
+	{ 51, -1, 32 },			/* A50 */
+	{ 52, -1, 32 },			/* A51 */
+	{ 53, -1, 32 },			/* A52 */
+	{ 54, -1, 32 },			/* A53 */
+	{ 55, -1, 32 },			/* A54 */
+	{ 56, -1, 32 },			/* A55 */
+	{ 57, -1, 32 },			/* A56 */
+	{ 58, -1, 32 },			/* A57 */
+	{ 59, -1, 32 },			/* A58 */
+	{ 60, -1, 32 },			/* A59 */
+	{ 61, -1, 32 },			/* A60 */
+	{ 62, -1, 32 },			/* A61 */
+	{ 63, -1, 32 },			/* A62 */
+	{ 64, -1, 32 },			/* A63 */
+	{ 65, 0x58, 32 },		/* lbeg */
+	{ 66, 0x5c, 32 },		/* lend */
+	{ 67, 0x60, 32 },		/* lcount */
+	{ 68, 0x48, 32 },		/* SAR */
+	{ 69, -1, 32 },			/* windowbase */
+	{ 70, -1, 32 },			/* windowstart */
+	{ 71, -1, 32 },			/* configid0 */
+	{ 72, -1, 32 },			/* configid1 */
+	{ 73, 0x04, 32 },		/* PS */
+	{ 74, -1, 32 },			/* threadptr */
+	{ 75, -1, 32 },			/* br */
+	{ 76, 0x54, 32 },		/* scompare1 */
+	{ 77, -1, 32 },			/* acclo */
+	{ 78, -1, 32 },			/* acchi */
+	{ 79, -1, 32 },			/* m0 */
+	{ 80, -1, 32 },			/* m1 */
+	{ 81, -1, 32 },			/* m2 */
+	{ 82, -1, 32 },			/* m3 */
+	{ 83, -1, 32 },			/* gpio_out */
+	{ 84, -1, 32 },			/* f0 */
+	{ 85, -1, 32 },			/* f1 */
+	{ 86, -1, 32 },			/* f2 */
+	{ 87, -1, 32 },			/* f3 */
+	{ 88, -1, 32 },			/* f4 */
+	{ 89, -1, 32 },			/* f5 */
+	{ 90, -1, 32 },			/* f6 */
+	{ 91, -1, 32 },			/* f7 */
+	{ 92, -1, 32 },			/* f8 */
+	{ 93, -1, 32 },			/* f9 */
+	{ 94, -1, 32 },			/* f10 */
+	{ 95, -1, 32 },			/* f11 */
+	{ 96, -1, 32 },			/* f12 */
+	{ 97, -1, 32 },			/* f13 */
+	{ 98, -1, 32 },			/* f14 */
+	{ 99, -1, 32 },			/* f15 */
+	{ 100, -1, 32 },		/* fcr */
+	{ 101, -1, 32 },		/* fsr */
+	{ 102, -1, 32 },		/* accx_0 */
+	{ 103, -1, 32 },		/* accx_1 */
+	{ 104, -1, 32 },		/* qacc_h_0 */
+	{ 105, -1, 32 },		/* qacc_h_1 */
+	{ 106, -1, 32 },		/* qacc_h_2 */
+	{ 107, -1, 32 },		/* qacc_h_3 */
+	{ 108, -1, 32 },		/* qacc_h_4 */
+	{ 109, -1, 32 },		/* qacc_l_0 */
+	{ 110, -1, 32 },		/* qacc_l_1 */
+	{ 111, -1, 32 },		/* qacc_l_2 */
+	{ 112, -1, 32 },		/* qacc_l_3 */
+	{ 113, -1, 32 },		/* qacc_l_4 */
+	{ 114, -1, 32 },		/* sar_byte */
+	{ 115, -1, 32 },		/* fft_bit_width */
+	{ 116, -1, 32 },		/* ua_state_0 */
+	{ 117, -1, 32 },		/* ua_state_1 */
+	{ 118, -1, 32 },		/* ua_state_2 */
+	{ 119, -1, 32 },		/* ua_state_3 */
+	{ 120, -1, 128 },		/* q0 */
+	{ 121, -1, 128 },		/* q1 */
+	{ 122, -1, 128 },		/* q2 */
+	{ 123, -1, 128 },		/* q3 */
+	{ 124, -1, 128 },		/* q4 */
+	{ 125, -1, 128 },		/* q5 */
+	{ 126, -1, 128 },		/* q6 */
+	{ 127, -1, 128 },		/* q7 */
+};
+
+const struct rtos_register_stacking nuttx_esp32s3_stacking = {
+	.stack_registers_size = 26 * 4,
+	.stack_growth_direction = -1,
+	.num_output_registers = ARRAY_SIZE(nuttx_stack_offsets_esp32s3),
+	.calculate_process_stack = rtos_generic_stack_align8,
+	.register_offsets = nuttx_stack_offsets_esp32s3,
+	.read_stack = nuttx_esp_xtensa_stack_read,
+};
diff --git a/src/rtos/rtos_nuttx_stackings.h b/src/rtos/rtos_nuttx_stackings.h
index 2e5f092..213a060 100644
--- a/src/rtos/rtos_nuttx_stackings.h
+++ b/src/rtos/rtos_nuttx_stackings.h
@@ -8,5 +8,8 @@
 extern const struct rtos_register_stacking nuttx_stacking_cortex_m;
 extern const struct rtos_register_stacking nuttx_stacking_cortex_m_fpu;
 extern const struct rtos_register_stacking nuttx_riscv_stacking;
+extern const struct rtos_register_stacking nuttx_esp32_stacking;
+extern const struct rtos_register_stacking nuttx_esp32s2_stacking;
+extern const struct rtos_register_stacking nuttx_esp32s3_stacking;
 
 #endif	/* INCLUDED_RTOS_NUTTX_STACKINGS_H */
diff --git a/src/rtos/rtos_riot_stackings.c b/src/rtos/rtos_riot_stackings.c
index e717e8c..e467621 100644
--- a/src/rtos/rtos_riot_stackings.c
+++ b/src/rtos/rtos_riot_stackings.c
@@ -12,6 +12,7 @@
 #include "rtos.h"
 #include "target/armv7m.h"
 #include "rtos_standard_stackings.h"
+#include "rtos_riot_stackings.h"
 
 /* This works for the M0 and M34 stackings as xPSR is in a fixed
  * location
diff --git a/src/rtos/rtos_riot_stackings.h b/src/rtos/rtos_riot_stackings.h
index 3b6c5f4..ebd5337 100644
--- a/src/rtos/rtos_riot_stackings.h
+++ b/src/rtos/rtos_riot_stackings.h
@@ -8,14 +8,9 @@
 #ifndef OPENOCD_RTOS_RTOS_RIOT_STACKINGS_H
 #define OPENOCD_RTOS_RTOS_RIOT_STACKINGS_H
 
-#ifdef HAVE_CONFIG_H
-#include "config.h"
-#endif
-
 #include "rtos.h"
 
 extern const struct rtos_register_stacking rtos_riot_cortex_m0_stacking;
 extern const struct rtos_register_stacking rtos_riot_cortex_m34_stacking;
 
 #endif	/* OPENOCD_RTOS_RTOS_RIOT_STACKINGS_H */
-
diff --git a/src/rtos/rtos_standard_stackings.c b/src/rtos/rtos_standard_stackings.c
index 7f0c3c7..0ca664f 100644
--- a/src/rtos/rtos_standard_stackings.c
+++ b/src/rtos/rtos_standard_stackings.c
@@ -12,6 +12,7 @@
 #include "rtos.h"
 #include "target/armv7m.h"
 #include "target/riscv/riscv.h"
+#include "rtos_standard_stackings.h"
 
 static const struct stack_register_offset rtos_standard_cortex_m3_stack_offsets[ARMV7M_NUM_CORE_REGS] = {
 	{ ARMV7M_R0,   0x20, 32 },		/* r0   */
diff --git a/src/rtos/rtos_standard_stackings.h b/src/rtos/rtos_standard_stackings.h
index 70ea034..40a3503 100644
--- a/src/rtos/rtos_standard_stackings.h
+++ b/src/rtos/rtos_standard_stackings.h
@@ -8,10 +8,6 @@
 #ifndef OPENOCD_RTOS_RTOS_STANDARD_STACKINGS_H
 #define OPENOCD_RTOS_RTOS_STANDARD_STACKINGS_H
 
-#ifdef HAVE_CONFIG_H
-#include "config.h"
-#endif
-
 #include "rtos.h"
 
 extern const struct rtos_register_stacking rtos_standard_cortex_m3_stacking;
diff --git a/src/rtos/rtos_ucos_iii_stackings.c b/src/rtos/rtos_ucos_iii_stackings.c
index 9ba5288..f1e2482 100644
--- a/src/rtos/rtos_ucos_iii_stackings.c
+++ b/src/rtos/rtos_ucos_iii_stackings.c
@@ -9,11 +9,11 @@
 #include "config.h"
 #endif
 
-#include <helper/types.h>
-#include <rtos/rtos.h>
-#include <rtos/rtos_standard_stackings.h>
-#include <target/armv7m.h>
-#include <target/esirisc.h>
+#include "rtos.h"
+#include "target/armv7m.h"
+#include "target/esirisc.h"
+#include "rtos_standard_stackings.h"
+#include "rtos_ucos_iii_stackings.h"
 
 static const struct stack_register_offset rtos_ucos_iii_cortex_m_stack_offsets[] = {
 	{ ARMV7M_R0,   0x20, 32 },	/* r0   */
diff --git a/src/rtos/rtos_ucos_iii_stackings.h b/src/rtos/rtos_ucos_iii_stackings.h
index 831c68e..dfe60b2 100644
--- a/src/rtos/rtos_ucos_iii_stackings.h
+++ b/src/rtos/rtos_ucos_iii_stackings.h
@@ -8,11 +8,7 @@
 #ifndef OPENOCD_RTOS_RTOS_UCOS_III_STACKINGS_H
 #define OPENOCD_RTOS_RTOS_UCOS_III_STACKINGS_H
 
-#ifdef HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include <rtos/rtos.h>
+#include "rtos.h"
 
 extern const struct rtos_register_stacking rtos_ucos_iii_cortex_m_stacking;
 extern const struct rtos_register_stacking rtos_ucos_iii_esi_risc_stacking;
diff --git a/src/rtos/zephyr.c b/src/rtos/zephyr.c
index b00b4b3..934a8dd 100644
--- a/src/rtos/zephyr.c
+++ b/src/rtos/zephyr.c
@@ -785,7 +785,7 @@ static int zephyr_get_symbol_list_to_lookup(struct symbol_table_elem **symbol_li
 	return ERROR_OK;
 }
 
-struct rtos_type zephyr_rtos = {
+const struct rtos_type zephyr_rtos = {
 	.name = "Zephyr",
 
 	.detect_rtos = zephyr_detect_rtos,
diff --git a/src/svf/svf.c b/src/svf/svf.c
index a537431..7195880 100644
--- a/src/svf/svf.c
+++ b/src/svf/svf.c
@@ -22,6 +22,7 @@
 #include "svf.h"
 #include "helper/system.h"
 #include <helper/time_support.h>
+#include <stdbool.h>
 
 /* SVF command */
 enum svf_command {
@@ -139,6 +140,9 @@ static const struct svf_statemove svf_statemoves[] = {
 #define XXR_TDO				(1 << 1)
 #define XXR_MASK			(1 << 2)
 #define XXR_SMASK			(1 << 3)
+
+#define SVF_MAX_ADDCYCLES	255
+
 struct svf_xxr_para {
 	int len;
 	int data_mask;
@@ -220,6 +224,8 @@ static int svf_buffer_index, svf_buffer_size;
 static int svf_quiet;
 static int svf_nil;
 static int svf_ignore_error;
+static bool svf_noreset;
+static int svf_addcycles;
 
 /* Targeting particular tap */
 static int svf_tap_is_specified;
@@ -343,7 +349,7 @@ int svf_add_statemove(tap_state_t state_to)
 COMMAND_HANDLER(handle_svf_command)
 {
 #define SVF_MIN_NUM_OF_OPTIONS 1
-#define SVF_MAX_NUM_OF_OPTIONS 5
+#define SVF_MAX_NUM_OF_OPTIONS 8
 	int command_num = 0;
 	int ret = ERROR_OK;
 	int64_t time_measure_ms;
@@ -363,8 +369,18 @@ COMMAND_HANDLER(handle_svf_command)
 	svf_nil = 0;
 	svf_progress_enabled = 0;
 	svf_ignore_error = 0;
+	svf_noreset = false;
+	svf_addcycles = 0;
+
 	for (unsigned int i = 0; i < CMD_ARGC; i++) {
-		if (strcmp(CMD_ARGV[i], "-tap") == 0) {
+		if (strcmp(CMD_ARGV[i], "-addcycles") == 0) {
+			svf_addcycles = atoi(CMD_ARGV[i + 1]);
+			if (svf_addcycles > SVF_MAX_ADDCYCLES) {
+				command_print(CMD, "addcycles: %s out of range", CMD_ARGV[i + 1]);
+				return ERROR_FAIL;
+			}
+			i++;
+		} else if (strcmp(CMD_ARGV[i], "-tap") == 0) {
 			tap = jtag_tap_by_string(CMD_ARGV[i+1]);
 			if (!tap) {
 				command_print(CMD, "Tap: %s unknown", CMD_ARGV[i+1]);
@@ -382,6 +398,8 @@ COMMAND_HANDLER(handle_svf_command)
 		else if ((strcmp(CMD_ARGV[i],
 				  "ignore_error") == 0) || (strcmp(CMD_ARGV[i], "-ignore_error") == 0))
 			svf_ignore_error = 1;
+		else if (strcmp(CMD_ARGV[i], "-noreset") == 0)
+			svf_noreset = true;
 		else {
 			svf_fd = fopen(CMD_ARGV[i], "r");
 			if (!svf_fd) {
@@ -424,7 +442,7 @@ COMMAND_HANDLER(handle_svf_command)
 
 	memcpy(&svf_para, &svf_para_init, sizeof(svf_para));
 
-	if (!svf_nil) {
+	if (!svf_nil && !svf_noreset) {
 		/* TAP_RESET */
 		jtag_add_tlr();
 	}
@@ -1189,6 +1207,9 @@ xxr_common:
 							svf_para.dr_end_state);
 				}
 
+				if (svf_addcycles)
+					jtag_add_clocks(svf_addcycles);
+
 				svf_buffer_index += (i + 7) >> 3;
 			} else if (command == SIR) {
 				/* check buffer size first, reallocate if necessary */
@@ -1545,7 +1566,7 @@ static const struct command_registration svf_command_handlers[] = {
 		.handler = handle_svf_command,
 		.mode = COMMAND_EXEC,
 		.help = "Runs a SVF file.",
-		.usage = "[-tap device.tap] <file> [quiet] [nil] [progress] [ignore_error]",
+		.usage = "[-tap device.tap] <file> [quiet] [nil] [progress] [ignore_error] [-noreset] [-addcycles numcycles]",
 	},
 	COMMAND_REGISTRATION_DONE
 };
diff --git a/src/target/arc.h b/src/target/arc.h
index bb70a59..a351802 100644
--- a/src/target/arc.h
+++ b/src/target/arc.h
@@ -253,16 +253,6 @@ struct arc_common {
 		}				\
 	} while (0)
 
-#define JIM_CHECK_RETVAL(action)		\
-	do {					\
-		int __retval = (action);	\
-		if (__retval != JIM_OK) {	\
-			LOG_DEBUG("error while calling \"%s\"",	\
-				# action);     \
-			return __retval;	\
-		}				\
-	} while (0)
-
 static inline struct arc_common *target_to_arc(struct target *target)
 {
 	return target->arch_info;
diff --git a/src/target/arc_cmd.c b/src/target/arc_cmd.c
index 7a80046..e7760b0 100644
--- a/src/target/arc_cmd.c
+++ b/src/target/arc_cmd.c
@@ -13,6 +13,7 @@
 #endif
 
 #include "arc.h"
+#include <helper/nvp.h>
 
 /* --------------------------------------------------------------------------
  *
@@ -22,14 +23,6 @@
  * ------------------------------------------------------------------------- */
 
 
-static int arc_cmd_jim_get_uint32(struct jim_getopt_info *goi, uint32_t *value)
-{
-	jim_wide value_wide;
-	JIM_CHECK_RETVAL(jim_getopt_wide(goi, &value_wide));
-	*value = (uint32_t)value_wide;
-	return JIM_OK;
-}
-
 enum add_reg_types {
 	CFG_ADD_REG_TYPE_FLAG,
 	CFG_ADD_REG_TYPE_STRUCT,
@@ -40,7 +33,7 @@ enum add_reg_type_flags {
 	CFG_ADD_REG_TYPE_FLAGS_FLAG,
 };
 
-static struct jim_nvp nvp_add_reg_type_flags_opts[] = {
+static const struct nvp nvp_add_reg_type_flags_opts[] = {
 	{ .name = "-name",  .value = CFG_ADD_REG_TYPE_FLAGS_NAME },
 	{ .name = "-flag",  .value = CFG_ADD_REG_TYPE_FLAGS_FLAG },
 	{ .name = NULL,     .value = -1 }
@@ -62,113 +55,113 @@ static const char *validate_register(const struct arc_reg_desc * const reg, bool
 	return NULL;
 }
 
-/* Helper function to read the name of register type or register from
- * configure files  */
-static int jim_arc_read_reg_name_field(struct jim_getopt_info *goi,
-	const char **name, int *name_len)
+static COMMAND_HELPER(arc_handle_add_reg_type_flags_ops, struct arc_reg_data_type *type)
 {
-	int e = JIM_OK;
+	struct reg_data_type_flags_field *fields = type->reg_type_flags_field;
+	struct arc_reg_bitfield *bitfields = type->bitfields;
+	struct reg_data_type_flags *flags = &type->data_type_flags;
+	unsigned int cur_field = 0;
 
-	if (!goi->argc) {
-		Jim_WrongNumArgs(goi->interp, goi->argc, goi->argv, "-name <name> ...");
-		return JIM_ERR;
-	}
-	e = jim_getopt_string(goi, name, name_len);
-	return e;
-}
+	while (CMD_ARGC) {
+		const struct nvp *n = nvp_name2value(nvp_add_reg_type_flags_opts, CMD_ARGV[0]);
+		CMD_ARGC--;
+		CMD_ARGV++;
+		switch (n->value) {
+		case CFG_ADD_REG_TYPE_FLAGS_NAME:
+			if (!CMD_ARGC)
+				return ERROR_COMMAND_ARGUMENT_INVALID;
 
-/* Helper function to read bitfields/flags of register type. */
-static int jim_arc_read_reg_type_field(struct jim_getopt_info *goi, const char **field_name, int *field_name_len,
-	 struct arc_reg_bitfield *bitfields, int cur_field, int type)
-{
-		jim_wide start_pos, end_pos;
+			const char *name = CMD_ARGV[0];
+			CMD_ARGC--;
+			CMD_ARGV++;
+
+			if (strlen(name) >= REG_TYPE_MAX_NAME_LENGTH) {
+				command_print(CMD, "Reg type name is too big.");
+				return ERROR_COMMAND_ARGUMENT_INVALID;
+			}
 
-		int e = JIM_OK;
-		if ((type == CFG_ADD_REG_TYPE_STRUCT && goi->argc < 3) ||
-		 (type == CFG_ADD_REG_TYPE_FLAG && goi->argc < 2)) {
-			Jim_SetResultFormatted(goi->interp, "Not enough arguments after -flag/-bitfield");
-			return JIM_ERR;
+			strcpy((void *)type->data_type.id, name);
+			break;
+
+		case CFG_ADD_REG_TYPE_FLAGS_FLAG:
+			if (CMD_ARGC < 2)
+				return ERROR_COMMAND_ARGUMENT_INVALID;
+
+			uint32_t val;
+			const char *field_name = CMD_ARGV[0];
+			COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], val);
+			CMD_ARGC -= 2;
+			CMD_ARGV += 2;
+			bitfields[cur_field].bitfield.start = val;
+			bitfields[cur_field].bitfield.end = val;
+
+			if (strlen(field_name) >= REG_TYPE_MAX_NAME_LENGTH) {
+				command_print(CMD, "Reg type field_name is too big.");
+				return ERROR_COMMAND_ARGUMENT_INVALID;
+			}
+
+			fields[cur_field].name = bitfields[cur_field].name;
+			strcpy(bitfields[cur_field].name, field_name);
+
+			fields[cur_field].bitfield = &bitfields[cur_field].bitfield;
+			if (cur_field > 0)
+				fields[cur_field - 1].next = &fields[cur_field];
+			else
+				flags->fields = fields;
+
+			cur_field += 1;
+			break;
+
+		default:
+			nvp_unknown_command_print(CMD, nvp_add_reg_type_flags_opts, NULL, CMD_ARGV[-1]);
+			return ERROR_COMMAND_ARGUMENT_INVALID;
 		}
+	}
 
-		e = jim_getopt_string(goi, field_name, field_name_len);
-		if (e != JIM_OK)
-					return e;
-
-		/* read start position of bitfield/flag */
-		e = jim_getopt_wide(goi, &start_pos);
-		if (e != JIM_OK)
-					return e;
-
-		end_pos = start_pos;
-
-		/* Check if any arguments remain,
-		 * set bitfields[cur_field].end if flag is multibit */
-		if (goi->argc > 0)
-			/* Check current argv[0], if it is equal to "-flag",
-			 * than bitfields[cur_field].end remains start */
-			if ((strcmp(Jim_String(goi->argv[0]), "-flag") && type == CFG_ADD_REG_TYPE_FLAG)
-					|| (type == CFG_ADD_REG_TYPE_STRUCT)) {
-								e = jim_getopt_wide(goi, &end_pos);
-								if (e != JIM_OK) {
-									Jim_SetResultFormatted(goi->interp, "Error reading end position");
-									return e;
-								}
-							}
-
-		bitfields[cur_field].bitfield.start = start_pos;
-		bitfields[cur_field].bitfield.end = end_pos;
-		if ((end_pos != start_pos) || (type == CFG_ADD_REG_TYPE_STRUCT))
-			bitfields[cur_field].bitfield.type = REG_TYPE_INT;
-	return e;
+	if (!type->data_type.id) {
+		command_print(CMD, "-name is a required option");
+		return ERROR_COMMAND_ARGUMENT_INVALID;
+	}
+
+	return ERROR_OK;
 }
 
-static int jim_arc_add_reg_type_flags(Jim_Interp *interp, int argc,
-	Jim_Obj * const *argv)
+COMMAND_HANDLER(arc_handle_add_reg_type_flags)
 {
-	struct jim_getopt_info goi;
-	JIM_CHECK_RETVAL(jim_getopt_setup(&goi, interp, argc-1, argv+1));
+	int retval;
 
 	LOG_DEBUG("-");
 
-	struct command_context *ctx;
-	struct target *target;
-
-	ctx = current_command_context(interp);
-	assert(ctx);
-	target = get_current_target(ctx);
+	struct target *target = get_current_target(CMD_CTX);
 	if (!target) {
-		Jim_SetResultFormatted(goi.interp, "No current target");
-		return JIM_ERR;
+		command_print(CMD, "No current target");
+		return ERROR_FAIL;
 	}
 
-	int e = JIM_OK;
-
 	/* Check if the amount of arguments is not zero */
-	if (goi.argc <= 0) {
-		Jim_SetResultFormatted(goi.interp, "The command has no arguments");
-		return JIM_ERR;
-	}
+	if (CMD_ARGC == 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
 
 	/* Estimate number of registers as (argc - 2)/3 as each -flag option has 2
 	 * arguments while -name is required. */
-	unsigned int fields_sz = (goi.argc - 2) / 3;
-	unsigned int cur_field = 0;
+	unsigned int fields_sz = (CMD_ARGC - 2) / 3;
 
 	/* The maximum amount of bitfields is 32 */
 	if (fields_sz > 32) {
-		Jim_SetResultFormatted(goi.interp, "The amount of bitfields exceed 32");
-		return JIM_ERR;
+		command_print(CMD, "The amount of bitfields exceed 32");
+		return ERROR_COMMAND_ARGUMENT_INVALID;
 	}
 
 	struct arc_reg_data_type *type = calloc(1, sizeof(*type));
-	struct reg_data_type_flags *flags = &type->data_type_flags;
 	struct reg_data_type_flags_field *fields = calloc(fields_sz, sizeof(*fields));
-	type->reg_type_flags_field = fields;
 	struct arc_reg_bitfield *bitfields = calloc(fields_sz, sizeof(*bitfields));
-	if (!(type && fields && bitfields)) {
-		Jim_SetResultFormatted(goi.interp, "Failed to allocate memory.");
+	if (!type || !fields || !bitfields) {
+		LOG_ERROR("Out of memory");
+		retval = ERROR_FAIL;
 		goto fail;
 	}
+	struct reg_data_type_flags *flags = &type->data_type_flags;
+	type->reg_type_flags_field = fields;
 
 	/* Initialize type */
 	type->bitfields = bitfields;
@@ -178,92 +171,22 @@ static int jim_arc_add_reg_type_flags(Jim_Interp *interp, int argc,
 	type->data_type.reg_type_flags = flags;
 	flags->size = 4; /* For now ARC has only 32-bit registers */
 
-	while (goi.argc > 0 && e == JIM_OK) {
-		struct jim_nvp *n;
-		e = jim_getopt_nvp(&goi, nvp_add_reg_type_flags_opts, &n);
-		if (e != JIM_OK) {
-			jim_getopt_nvp_unknown(&goi, nvp_add_reg_type_flags_opts, 0);
-			continue;
-		}
-
-		switch (n->value) {
-			case CFG_ADD_REG_TYPE_FLAGS_NAME:
-			{
-				const char *name = NULL;
-				int name_len = 0;
-
-				e = jim_arc_read_reg_name_field(&goi, &name, &name_len);
-				if (e != JIM_OK) {
-					Jim_SetResultFormatted(goi.interp, "Unable to read reg name.");
-					goto fail;
-				}
-
-				if (name_len > REG_TYPE_MAX_NAME_LENGTH) {
-					Jim_SetResultFormatted(goi.interp, "Reg type name is too big.");
-					goto fail;
-				}
-
-				strncpy((void *)type->data_type.id, name, name_len);
-				if (!type->data_type.id) {
-					Jim_SetResultFormatted(goi.interp, "Unable to setup reg type name.");
-					goto fail;
-				}
-
-				break;
-			}
-
-			case CFG_ADD_REG_TYPE_FLAGS_FLAG:
-			{
-				const char *field_name = NULL;
-				int field_name_len = 0;
-
-				e = jim_arc_read_reg_type_field(&goi, &field_name, &field_name_len, bitfields,
-									cur_field, CFG_ADD_REG_TYPE_FLAG);
-				if (e != JIM_OK) {
-					Jim_SetResultFormatted(goi.interp, "Unable to add reg_type_flag field.");
-					goto fail;
-				}
-
-				if (field_name_len > REG_TYPE_MAX_NAME_LENGTH) {
-					Jim_SetResultFormatted(goi.interp, "Reg type field_name_len is too big.");
-					goto fail;
-				}
-
-				fields[cur_field].name = bitfields[cur_field].name;
-				strncpy(bitfields[cur_field].name, field_name, field_name_len);
-				if (!fields[cur_field].name) {
-					Jim_SetResultFormatted(goi.interp, "Unable to setup field name. ");
-					goto fail;
-				}
-
-				fields[cur_field].bitfield = &(bitfields[cur_field].bitfield);
-				if (cur_field > 0)
-					fields[cur_field - 1].next = &(fields[cur_field]);
-				else
-					flags->fields = fields;
-
-				cur_field += 1;
-				break;
-			}
-		}
-	}
-
-	if (!type->data_type.id) {
-		Jim_SetResultFormatted(goi.interp, "-name is a required option");
+	retval = CALL_COMMAND_HANDLER(arc_handle_add_reg_type_flags_ops, type);
+	if (retval != ERROR_OK)
 		goto fail;
-	}
 
 	arc_reg_data_type_add(target, type);
 
 	LOG_DEBUG("added flags type {name=%s}", type->data_type.id);
 
-	return JIM_OK;
+	return ERROR_OK;
+
 fail:
 	free(type);
 	free(fields);
 	free(bitfields);
 
-	return JIM_ERR;
+	return retval;
 }
 
 /* Add struct register data type */
@@ -272,43 +195,30 @@ enum add_reg_type_struct {
 	CFG_ADD_REG_TYPE_STRUCT_BITFIELD,
 };
 
-static struct jim_nvp nvp_add_reg_type_struct_opts[] = {
+static const struct nvp nvp_add_reg_type_struct_opts[] = {
 	{ .name = "-name",     .value = CFG_ADD_REG_TYPE_STRUCT_NAME },
 	{ .name = "-bitfield", .value = CFG_ADD_REG_TYPE_STRUCT_BITFIELD },
 	{ .name = NULL,     .value = -1 }
 };
 
-static int jim_arc_set_aux_reg(Jim_Interp *interp, int argc, Jim_Obj * const *argv)
+COMMAND_HANDLER(arc_handle_set_aux_reg)
 {
+	if (CMD_ARGC != 2)
+		return ERROR_COMMAND_SYNTAX_ERROR;
 
-	struct command_context *context;
-	struct target *target;
-	uint32_t regnum;
-	uint32_t value;
-
-	struct jim_getopt_info goi;
-	JIM_CHECK_RETVAL(jim_getopt_setup(&goi, interp, argc-1, argv+1));
-
-	if (goi.argc != 2) {
-		Jim_SetResultFormatted(goi.interp,
-			"usage: %s <aux_reg_num> <aux_reg_value>", Jim_GetString(argv[0], NULL));
-		return JIM_ERR;
-	}
-
-	context = current_command_context(interp);
-	assert(context);
-
-	target = get_current_target(context);
+	struct target *target = get_current_target(CMD_CTX);
 	if (!target) {
-		Jim_SetResultFormatted(goi.interp, "No current target");
-		return JIM_ERR;
+		command_print(CMD, "No current target");
+		return ERROR_FAIL;
 	}
 
 	/* Register number */
-	JIM_CHECK_RETVAL(arc_cmd_jim_get_uint32(&goi, &regnum));
+	uint32_t regnum;
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], regnum);
 
 	/* Register value */
-	JIM_CHECK_RETVAL(arc_cmd_jim_get_uint32(&goi, &value));
+	uint32_t value;
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], value);
 
 	struct arc_common *arc = target_to_arc(target);
 	assert(arc);
@@ -318,121 +228,87 @@ static int jim_arc_set_aux_reg(Jim_Interp *interp, int argc, Jim_Obj * const *ar
 	return ERROR_OK;
 }
 
-static int jim_arc_get_aux_reg(Jim_Interp *interp, int argc, Jim_Obj * const *argv)
+COMMAND_HANDLER(arc_handle_get_aux_reg)
 {
-	struct command_context *context;
-	struct target *target;
-	uint32_t regnum;
-	uint32_t value;
-
-	struct jim_getopt_info goi;
-	JIM_CHECK_RETVAL(jim_getopt_setup(&goi, interp, argc-1, argv+1));
-
-	if (goi.argc != 1) {
-		Jim_SetResultFormatted(goi.interp,
-			"usage: %s <aux_reg_num>", Jim_GetString(argv[0], NULL));
-		return JIM_ERR;
-	}
-
-	context = current_command_context(interp);
-	assert(context);
+	if (CMD_ARGC != 1)
+		return ERROR_COMMAND_SYNTAX_ERROR;
 
-	target = get_current_target(context);
+	struct target *target = get_current_target(CMD_CTX);
 	if (!target) {
-		Jim_SetResultFormatted(goi.interp, "No current target");
-		return JIM_ERR;
+		command_print(CMD, "No current target");
+		return ERROR_FAIL;
 	}
 
 	/* Register number */
-	JIM_CHECK_RETVAL(arc_cmd_jim_get_uint32(&goi, &regnum));
+	uint32_t regnum;
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], regnum);
 
 	struct arc_common *arc = target_to_arc(target);
 	assert(arc);
 
+	uint32_t value;
 	CHECK_RETVAL(arc_jtag_read_aux_reg_one(&arc->jtag_info, regnum, &value));
-	Jim_SetResultInt(interp, value);
+
+	command_print(CMD, "0x%" PRIx32, value);
 
 	return ERROR_OK;
 }
 
-static int jim_arc_get_core_reg(Jim_Interp *interp, int argc, Jim_Obj * const *argv)
+COMMAND_HANDLER(arc_handle_get_core_reg)
 {
-	struct command_context *context;
-	struct target *target;
-	uint32_t regnum;
-	uint32_t value;
-
-	struct jim_getopt_info goi;
-	JIM_CHECK_RETVAL(jim_getopt_setup(&goi, interp, argc-1, argv+1));
-
-	if (goi.argc != 1) {
-		Jim_SetResultFormatted(goi.interp,
-			"usage: %s <core_reg_num>", Jim_GetString(argv[0], NULL));
-		return JIM_ERR;
-	}
-
-	context = current_command_context(interp);
-	assert(context);
+	if (CMD_ARGC != 1)
+		return ERROR_COMMAND_SYNTAX_ERROR;
 
-	target = get_current_target(context);
+	struct target *target = get_current_target(CMD_CTX);
 	if (!target) {
-		Jim_SetResultFormatted(goi.interp, "No current target");
-		return JIM_ERR;
+		command_print(CMD, "No current target");
+		return ERROR_FAIL;
 	}
 
 	/* Register number */
-	JIM_CHECK_RETVAL(arc_cmd_jim_get_uint32(&goi, &regnum));
+	uint32_t regnum;
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], regnum);
 	if (regnum > CORE_REG_MAX_NUMBER || regnum == ARC_R61 || regnum == ARC_R62) {
-		Jim_SetResultFormatted(goi.interp, "Core register number %i "
+		command_print(CMD, "Core register number %i "
 			"is invalid. Must less then 64 and not 61 and 62.", regnum);
-		return JIM_ERR;
+		return ERROR_COMMAND_ARGUMENT_INVALID;
 	}
 
 	struct arc_common *arc = target_to_arc(target);
 	assert(arc);
 
 	/* Read value */
+	uint32_t value;
 	CHECK_RETVAL(arc_jtag_read_core_reg_one(&arc->jtag_info, regnum, &value));
-	Jim_SetResultInt(interp, value);
+
+	command_print(CMD, "0x%" PRIx32, value);
 
 	return ERROR_OK;
 }
 
-static int jim_arc_set_core_reg(Jim_Interp *interp, int argc, Jim_Obj * const *argv)
+COMMAND_HANDLER(arc_handle_set_core_reg)
 {
-	struct command_context *context;
-	struct target *target;
-	uint32_t regnum;
-	uint32_t value;
-
-	struct jim_getopt_info goi;
-	JIM_CHECK_RETVAL(jim_getopt_setup(&goi, interp, argc-1, argv+1));
-
-	if (goi.argc != 2) {
-		Jim_SetResultFormatted(goi.interp,
-			"usage: %s <core_reg_num> <core_reg_value>", Jim_GetString(argv[0], NULL));
-		return JIM_ERR;
-	}
-
-	context = current_command_context(interp);
-	assert(context);
+	if (CMD_ARGC != 2)
+		return ERROR_COMMAND_SYNTAX_ERROR;
 
-	target = get_current_target(context);
+	struct target *target = get_current_target(CMD_CTX);
 	if (!target) {
-		Jim_SetResultFormatted(goi.interp, "No current target");
-		return JIM_ERR;
+		command_print(CMD, "No current target");
+		return ERROR_FAIL;
 	}
 
 	/* Register number */
-	JIM_CHECK_RETVAL(arc_cmd_jim_get_uint32(&goi, &regnum));
+	uint32_t regnum;
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], regnum);
 	if (regnum > CORE_REG_MAX_NUMBER || regnum == ARC_R61 || regnum == ARC_R62) {
-		Jim_SetResultFormatted(goi.interp, "Core register number %i "
+		command_print(CMD, "Core register number %i "
 			"is invalid. Must less then 64 and not 61 and 62.", regnum);
-		return JIM_ERR;
+		return ERROR_COMMAND_ARGUMENT_INVALID;
 	}
 
 	/* Register value */
-	JIM_CHECK_RETVAL(arc_cmd_jim_get_uint32(&goi, &value));
+	uint32_t value;
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], value);
 
 	struct arc_common *arc = target_to_arc(target);
 	assert(arc);
@@ -445,7 +321,7 @@ static int jim_arc_set_core_reg(Jim_Interp *interp, int argc, Jim_Obj * const *a
 static const struct command_registration arc_jtag_command_group[] = {
 	{
 		.name = "get-aux-reg",
-		.jim_handler = jim_arc_get_aux_reg,
+		.handler = arc_handle_get_aux_reg,
 		.mode = COMMAND_EXEC,
 		.help = "Get AUX register by number. This command does a "
 			"raw JTAG request that bypasses OpenOCD register cache "
@@ -455,7 +331,7 @@ static const struct command_registration arc_jtag_command_group[] = {
 	},
 	{
 		.name = "set-aux-reg",
-		.jim_handler = jim_arc_set_aux_reg,
+		.handler = arc_handle_set_aux_reg,
 		.mode = COMMAND_EXEC,
 		.help = "Set AUX register by number. This command does a "
 			"raw JTAG request that bypasses OpenOCD register cache "
@@ -465,7 +341,7 @@ static const struct command_registration arc_jtag_command_group[] = {
 	},
 	{
 		.name = "get-core-reg",
-		.jim_handler = jim_arc_get_core_reg,
+		.handler = arc_handle_get_core_reg,
 		.mode = COMMAND_EXEC,
 		.help = "Get/Set core register by number. This command does a "
 			"raw JTAG request that bypasses OpenOCD register cache "
@@ -475,7 +351,7 @@ static const struct command_registration arc_jtag_command_group[] = {
 	},
 	{
 		.name = "set-core-reg",
-		.jim_handler = jim_arc_set_core_reg,
+		.handler = arc_handle_set_core_reg,
 		.mode = COMMAND_EXEC,
 		.help = "Get/Set core register by number. This command does a "
 			"raw JTAG request that bypasses OpenOCD register cache "
@@ -488,53 +364,117 @@ static const struct command_registration arc_jtag_command_group[] = {
 
 
 /* This function supports only bitfields. */
-static int jim_arc_add_reg_type_struct(Jim_Interp *interp, int argc,
-	Jim_Obj * const *argv)
+static COMMAND_HELPER(arc_handle_add_reg_type_struct_opts, struct arc_reg_data_type *type)
 {
-	struct jim_getopt_info goi;
-	JIM_CHECK_RETVAL(jim_getopt_setup(&goi, interp, argc-1, argv+1));
+	struct reg_data_type_struct_field *fields = type->reg_type_struct_field;
+	struct arc_reg_bitfield *bitfields = type->bitfields;
+	struct reg_data_type_struct *struct_type = &type->data_type_struct;
+	unsigned int cur_field = 0;
 
-	LOG_DEBUG("-");
+	while (CMD_ARGC) {
+		const struct nvp *n = nvp_name2value(nvp_add_reg_type_struct_opts, CMD_ARGV[0]);
+		CMD_ARGC--;
+		CMD_ARGV++;
+		switch (n->value) {
+		case CFG_ADD_REG_TYPE_STRUCT_NAME:
+			if (!CMD_ARGC)
+				return ERROR_COMMAND_ARGUMENT_INVALID;
 
-	struct command_context *ctx;
-	struct target *target;
+			const char *name = CMD_ARGV[0];
+			CMD_ARGC--;
+			CMD_ARGV++;
 
-	ctx = current_command_context(interp);
-	assert(ctx);
-	target = get_current_target(ctx);
-	if (!target) {
-		Jim_SetResultFormatted(goi.interp, "No current target");
-		return JIM_ERR;
+			if (strlen(name) >= REG_TYPE_MAX_NAME_LENGTH) {
+				command_print(CMD, "Reg type name is too big.");
+				return ERROR_COMMAND_ARGUMENT_INVALID;
+			}
+
+			strcpy((void *)type->data_type.id, name);
+			break;
+
+		case CFG_ADD_REG_TYPE_STRUCT_BITFIELD:
+			if (CMD_ARGC < 3)
+				return ERROR_COMMAND_ARGUMENT_INVALID;
+
+			uint32_t start_pos, end_pos;
+			const char *field_name = CMD_ARGV[0];
+			COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], start_pos);
+			COMMAND_PARSE_NUMBER(u32, CMD_ARGV[2], end_pos);
+			CMD_ARGC -= 3;
+			CMD_ARGV += 3;
+			bitfields[cur_field].bitfield.start = start_pos;
+			bitfields[cur_field].bitfield.end = end_pos;
+			bitfields[cur_field].bitfield.type = REG_TYPE_INT;
+
+			if (strlen(field_name) >= REG_TYPE_MAX_NAME_LENGTH) {
+				command_print(CMD, "Reg type field_name is too big.");
+				return ERROR_COMMAND_ARGUMENT_INVALID;
+			}
+
+			fields[cur_field].name = bitfields[cur_field].name;
+			strcpy(bitfields[cur_field].name, field_name);
+
+			fields[cur_field].bitfield = &bitfields[cur_field].bitfield;
+			fields[cur_field].use_bitfields = true;
+			if (cur_field > 0)
+				fields[cur_field - 1].next = &fields[cur_field];
+			else
+				struct_type->fields = fields;
+
+			cur_field += 1;
+
+			break;
+
+		default:
+			nvp_unknown_command_print(CMD, nvp_add_reg_type_struct_opts, NULL, CMD_ARGV[-1]);
+			return ERROR_COMMAND_ARGUMENT_INVALID;
+		}
+	}
+
+	if (!type->data_type.id) {
+		command_print(CMD, "-name is a required option");
+		return ERROR_COMMAND_ARGUMENT_INVALID;
 	}
 
-	int e = JIM_OK;
+	return ERROR_OK;
+}
 
-	/* Check if the amount of arguments is not zero */
-	if (goi.argc <= 0) {
-		Jim_SetResultFormatted(goi.interp, "The command has no arguments");
-		return JIM_ERR;
+COMMAND_HANDLER(arc_handle_add_reg_type_struct)
+{
+	int retval;
+
+	LOG_DEBUG("-");
+
+	struct target *target = get_current_target(CMD_CTX);
+	if (!target) {
+		command_print(CMD, "No current target");
+		return ERROR_FAIL;
 	}
 
+	/* Check if the amount of arguments is not zero */
+	if (CMD_ARGC == 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
 	/* Estimate number of registers as (argc - 2)/4 as each -bitfield option has 3
 	 * arguments while -name is required. */
-	unsigned int fields_sz = (goi.argc - 2) / 4;
-	unsigned int cur_field = 0;
+	unsigned int fields_sz = (CMD_ARGC - 2) / 4;
 
 	/* The maximum amount of bitfields is 32 */
 	if (fields_sz > 32) {
-			Jim_SetResultFormatted(goi.interp, "The amount of bitfields exceed 32");
-			return JIM_ERR;
+		command_print(CMD, "The amount of bitfields exceed 32");
+		return ERROR_COMMAND_ARGUMENT_INVALID;
 	}
 
 	struct arc_reg_data_type *type = calloc(1, sizeof(*type));
-	struct reg_data_type_struct *struct_type = &type->data_type_struct;
 	struct reg_data_type_struct_field *fields = calloc(fields_sz, sizeof(*fields));
-	type->reg_type_struct_field = fields;
 	struct arc_reg_bitfield *bitfields = calloc(fields_sz, sizeof(*bitfields));
-	if (!(type && fields && bitfields)) {
-		Jim_SetResultFormatted(goi.interp, "Failed to allocate memory.");
+	if (!type || !fields || !bitfields) {
+		LOG_ERROR("Out of memory");
+		retval = ERROR_FAIL;
 		goto fail;
 	}
+	struct reg_data_type_struct *struct_type = &type->data_type_struct;
+	type->reg_type_struct_field = fields;
 
 	/* Initialize type */
 	type->data_type.id = type->data_type_id;
@@ -544,91 +484,22 @@ static int jim_arc_add_reg_type_struct(Jim_Interp *interp, int argc,
 	type->data_type.reg_type_struct = struct_type;
 	struct_type->size = 4; /* For now ARC has only 32-bit registers */
 
-	while (goi.argc > 0 && e == JIM_OK) {
-		struct jim_nvp *n;
-		e = jim_getopt_nvp(&goi, nvp_add_reg_type_struct_opts, &n);
-		if (e != JIM_OK) {
-			jim_getopt_nvp_unknown(&goi, nvp_add_reg_type_struct_opts, 0);
-			continue;
-		}
-
-		switch (n->value) {
-			case CFG_ADD_REG_TYPE_STRUCT_NAME:
-			{
-				const char *name = NULL;
-				int name_len = 0;
-
-				e = jim_arc_read_reg_name_field(&goi, &name, &name_len);
-				if (e != JIM_OK) {
-					Jim_SetResultFormatted(goi.interp, "Unable to read reg name.");
-					goto fail;
-				}
-
-				if (name_len > REG_TYPE_MAX_NAME_LENGTH) {
-					Jim_SetResultFormatted(goi.interp, "Reg type name is too big.");
-					goto fail;
-				}
-
-				strncpy((void *)type->data_type.id, name, name_len);
-				if (!type->data_type.id) {
-					Jim_SetResultFormatted(goi.interp, "Unable to setup reg type name.");
-					goto fail;
-				}
-
-				break;
-			}
-			case CFG_ADD_REG_TYPE_STRUCT_BITFIELD:
-			{
-				const char *field_name = NULL;
-				int field_name_len = 0;
-				e = jim_arc_read_reg_type_field(&goi, &field_name, &field_name_len, bitfields,
-									cur_field, CFG_ADD_REG_TYPE_STRUCT);
-				if (e != JIM_OK) {
-					Jim_SetResultFormatted(goi.interp, "Unable to add reg_type_struct field.");
-					goto fail;
-				}
-
-				if (field_name_len > REG_TYPE_MAX_NAME_LENGTH) {
-					Jim_SetResultFormatted(goi.interp, "Reg type field_name_len is too big.");
-					goto fail;
-				}
-
-				fields[cur_field].name = bitfields[cur_field].name;
-				strncpy(bitfields[cur_field].name, field_name, field_name_len);
-				if (!fields[cur_field].name) {
-					Jim_SetResultFormatted(goi.interp, "Unable to setup field name. ");
-					goto fail;
-				}
-
-				fields[cur_field].bitfield = &(bitfields[cur_field].bitfield);
-				fields[cur_field].use_bitfields = true;
-				if (cur_field > 0)
-					fields[cur_field - 1].next = &(fields[cur_field]);
-				else
-					struct_type->fields = fields;
-
-				cur_field += 1;
-
-				break;
-			}
-		}
-	}
-
-	if (!type->data_type.id) {
-		Jim_SetResultFormatted(goi.interp, "-name is a required option");
+	retval = CALL_COMMAND_HANDLER(arc_handle_add_reg_type_struct_opts, type);
+	if (retval != ERROR_OK)
 		goto fail;
-	}
 
 	arc_reg_data_type_add(target, type);
+
 	LOG_DEBUG("added struct type {name=%s}", type->data_type.id);
-	return JIM_OK;
+
+	return ERROR_OK;
 
 fail:
-			free(type);
-			free(fields);
-			free(bitfields);
+	free(type);
+	free(fields);
+	free(bitfields);
 
-			return JIM_ERR;
+	return retval;
 }
 
 /* Add register */
@@ -642,7 +513,7 @@ enum opts_add_reg {
 	CFG_ADD_REG_GENERAL,
 };
 
-static struct jim_nvp opts_nvp_add_reg[] = {
+static const struct nvp opts_nvp_add_reg[] = {
 	{ .name = "-name",    .value = CFG_ADD_REG_NAME },
 	{ .name = "-num",     .value = CFG_ADD_REG_ARCH_NUM },
 	{ .name = "-core",    .value = CFG_ADD_REG_IS_CORE },
@@ -660,155 +531,133 @@ void free_reg_desc(struct arc_reg_desc *r)
 	free(r);
 }
 
-static int jim_arc_add_reg(Jim_Interp *interp, int argc, Jim_Obj * const *argv)
+static COMMAND_HELPER(arc_handle_add_reg_do, struct arc_reg_desc *reg)
 {
-	struct jim_getopt_info goi;
-	JIM_CHECK_RETVAL(jim_getopt_setup(&goi, interp, argc-1, argv+1));
-
-	struct arc_reg_desc *reg = calloc(1, sizeof(*reg));
-	if (!reg) {
-		Jim_SetResultFormatted(goi.interp, "Failed to allocate memory.");
-		return JIM_ERR;
-	}
-
 	/* There is no architecture number that we could treat as invalid, so
 	 * separate variable required to ensure that arch num has been set. */
 	bool arch_num_set = false;
 	const char *type_name = "int"; /* Default type */
-	int type_name_len = strlen(type_name);
-	int e = ERROR_OK;
 
 	/* At least we need to specify 4 parameters: name, number and gdb_feature,
 	 * which means there should be 6 arguments. Also there can be additional parameters
 	 * "-type <type>", "-g" and  "-core" or "-bcr" which makes maximum 10 parameters. */
-	if (goi.argc < 6 || goi.argc > 10) {
-		free_reg_desc(reg);
-		Jim_SetResultFormatted(goi.interp,
-			"Should be at least 6 arguments and not greater than 10: "
-			" -name <name> -num <num> -feature <gdb_feature> "
-			" [-type <type_name>] [-core|-bcr] [-g].");
-		return JIM_ERR;
-	}
+	if (CMD_ARGC < 6 || CMD_ARGC > 10)
+		return ERROR_COMMAND_SYNTAX_ERROR;
 
 	/* Parse options. */
-	while (goi.argc > 0) {
-		struct jim_nvp *n;
-		e = jim_getopt_nvp(&goi, opts_nvp_add_reg, &n);
-		if (e != JIM_OK) {
-			jim_getopt_nvp_unknown(&goi, opts_nvp_add_reg, 0);
-			free_reg_desc(reg);
-			return e;
-		}
-
+	while (CMD_ARGC) {
+		const struct nvp *n = nvp_name2value(opts_nvp_add_reg, CMD_ARGV[0]);
+		CMD_ARGC--;
+		CMD_ARGV++;
 		switch (n->value) {
-			case CFG_ADD_REG_NAME:
-			{
-				const char *reg_name = NULL;
-				int reg_name_len = 0;
-
-				e = jim_arc_read_reg_name_field(&goi, &reg_name, &reg_name_len);
-				if (e != JIM_OK) {
-					Jim_SetResultFormatted(goi.interp, "Unable to read register name.");
-					free_reg_desc(reg);
-					return e;
-				}
-
-				reg->name = strndup(reg_name, reg_name_len);
-				break;
+		case CFG_ADD_REG_NAME:
+			if (!CMD_ARGC)
+				return ERROR_COMMAND_ARGUMENT_INVALID;
+
+			reg->name = strdup(CMD_ARGV[0]);
+			if (!reg->name) {
+				LOG_ERROR("Out of memory");
+				return ERROR_FAIL;
 			}
-			case CFG_ADD_REG_IS_CORE:
-				reg->is_core = true;
-				break;
-			case CFG_ADD_REG_IS_BCR:
-				reg->is_bcr = true;
-				break;
-			case CFG_ADD_REG_ARCH_NUM:
-			{
-				jim_wide archnum;
-
-				if (!goi.argc) {
-					free_reg_desc(reg);
-					Jim_WrongNumArgs(interp, goi.argc, goi.argv, "-num <int> ...");
-					return JIM_ERR;
-				}
-
-				e = jim_getopt_wide(&goi, &archnum);
-				if (e != JIM_OK) {
-					free_reg_desc(reg);
-					return e;
-				}
-
-				reg->arch_num = archnum;
-				arch_num_set = true;
-				break;
-			}
-			case CFG_ADD_REG_GDB_FEATURE:
-			{
-				const char *feature = NULL;
-				int feature_len = 0;
-
-				e = jim_arc_read_reg_name_field(&goi, &feature, &feature_len);
-				if (e != JIM_OK) {
-					Jim_SetResultFormatted(goi.interp, "Unable to read gdb_feature.");
-					free_reg_desc(reg);
-					return e;
-				}
-
-				reg->gdb_xml_feature = strndup(feature, feature_len);
-				break;
+
+			CMD_ARGC--;
+			CMD_ARGV++;
+			break;
+
+		case CFG_ADD_REG_IS_CORE:
+			reg->is_core = true;
+			break;
+
+		case CFG_ADD_REG_IS_BCR:
+			reg->is_bcr = true;
+			break;
+
+		case CFG_ADD_REG_ARCH_NUM:
+			if (!CMD_ARGC)
+				return ERROR_COMMAND_ARGUMENT_INVALID;
+
+			COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], reg->arch_num);
+			CMD_ARGC--;
+			CMD_ARGV++;
+
+			arch_num_set = true;
+			break;
+
+		case CFG_ADD_REG_GDB_FEATURE:
+			if (!CMD_ARGC)
+				return ERROR_COMMAND_ARGUMENT_INVALID;
+
+			reg->gdb_xml_feature = strdup(CMD_ARGV[0]);
+			if (!reg->gdb_xml_feature) {
+				LOG_ERROR("Out of memory");
+				return ERROR_FAIL;
 			}
-			case CFG_ADD_REG_TYPE:
-				e = jim_arc_read_reg_name_field(&goi, &type_name, &type_name_len);
-				if (e != JIM_OK) {
-					Jim_SetResultFormatted(goi.interp, "Unable to read register type.");
-					free_reg_desc(reg);
-					return e;
-				}
-
-				break;
-			case CFG_ADD_REG_GENERAL:
-				reg->is_general = true;
-				break;
-			default:
-				LOG_DEBUG("Error: Unknown parameter");
-				free_reg_desc(reg);
-				return JIM_ERR;
+
+			CMD_ARGC--;
+			CMD_ARGV++;
+			break;
+
+		case CFG_ADD_REG_TYPE:
+			if (!CMD_ARGC)
+				return ERROR_COMMAND_ARGUMENT_INVALID;
+
+			type_name = CMD_ARGV[0];
+			CMD_ARGC--;
+			CMD_ARGV++;
+			break;
+
+		case CFG_ADD_REG_GENERAL:
+			reg->is_general = true;
+			break;
+
+		default:
+			nvp_unknown_command_print(CMD, opts_nvp_add_reg, NULL, CMD_ARGV[-1]);
+			return ERROR_COMMAND_ARGUMENT_INVALID;
 		}
 	}
 
 	/* Check that required fields are set */
 	const char * const errmsg = validate_register(reg, arch_num_set);
 	if (errmsg) {
-		Jim_SetResultFormatted(goi.interp, errmsg);
-		free_reg_desc(reg);
-		return JIM_ERR;
+		command_print(CMD, "%s", errmsg);
+		return ERROR_COMMAND_ARGUMENT_INVALID;
 	}
 
 	/* Add new register */
-	struct command_context *ctx;
-	struct target *target;
-
-	ctx = current_command_context(interp);
-	assert(ctx);
-	target = get_current_target(ctx);
+	struct target *target = get_current_target(CMD_CTX);
 	if (!target) {
-		Jim_SetResultFormatted(goi.interp, "No current target");
-		free_reg_desc(reg);
-		return JIM_ERR;
+		command_print(CMD, "No current target");
+		return ERROR_FAIL;
 	}
 
 	reg->target = target;
 
-	e = arc_reg_add(target, reg, type_name, type_name_len);
-	if (e == ERROR_ARC_REGTYPE_NOT_FOUND) {
-		Jim_SetResultFormatted(goi.interp,
+	int retval = arc_reg_add(target, reg, type_name, strlen(type_name));
+	if (retval == ERROR_ARC_REGTYPE_NOT_FOUND) {
+		command_print(CMD,
 			"Cannot find type `%s' for register `%s'.",
 			type_name, reg->name);
+		return retval;
+	}
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(arc_handle_add_reg)
+{
+	struct arc_reg_desc *reg = calloc(1, sizeof(*reg));
+	if (!reg) {
+		LOG_ERROR("Out of memory");
+		return ERROR_FAIL;
+	}
+
+	int retval = CALL_COMMAND_HANDLER(arc_handle_add_reg_do, reg);
+	if (retval != ERROR_OK) {
 		free_reg_desc(reg);
-		return JIM_ERR;
+		return retval;
 	}
 
-	return e;
+	return ERROR_OK;
 }
 
 /* arc set-reg-exists ($reg_name)+
@@ -818,7 +667,7 @@ COMMAND_HANDLER(arc_set_reg_exists)
 	struct target * const target = get_current_target(CMD_CTX);
 	if (!target) {
 		command_print(CMD, "Unable to get current target.");
-		return JIM_ERR;
+		return ERROR_FAIL;
 	}
 
 	if (!CMD_ARGC) {
@@ -838,64 +687,45 @@ COMMAND_HANDLER(arc_set_reg_exists)
 		r->exist = true;
 	}
 
-	return JIM_OK;
+	return ERROR_OK;
 }
 
 /* arc reg-field  ($reg_name) ($reg_field)
  * Reads struct type register field */
-static int jim_arc_get_reg_field(Jim_Interp *interp, int argc, Jim_Obj * const *argv)
+COMMAND_HANDLER(arc_handle_get_reg_field)
 {
-	struct jim_getopt_info goi;
-	const char *reg_name, *field_name;
-	uint32_t value;
-	int retval;
-
-	JIM_CHECK_RETVAL(jim_getopt_setup(&goi, interp, argc-1, argv+1));
-
-	LOG_DEBUG("Reading register field");
-	if (goi.argc != 2) {
-		if (!goi.argc)
-			Jim_WrongNumArgs(interp, goi.argc, goi.argv, "<regname> <fieldname>");
-		else if (goi.argc == 1)
-			Jim_WrongNumArgs(interp, goi.argc, goi.argv, "<fieldname>");
-		else
-			Jim_WrongNumArgs(interp, goi.argc, goi.argv, "<regname> <fieldname>");
+	if (CMD_ARGC != 2)
 		return ERROR_COMMAND_SYNTAX_ERROR;
-	}
-
-	JIM_CHECK_RETVAL(jim_getopt_string(&goi, &reg_name, NULL));
-	JIM_CHECK_RETVAL(jim_getopt_string(&goi, &field_name, NULL));
-	assert(reg_name);
-	assert(field_name);
 
-	struct command_context * const ctx = current_command_context(interp);
-	assert(ctx);
-	struct target * const target = get_current_target(ctx);
+	struct target *target = get_current_target(CMD_CTX);
 	if (!target) {
-		Jim_SetResultFormatted(goi.interp, "No current target");
-		return JIM_ERR;
+		command_print(CMD, "No current target");
+		return ERROR_FAIL;
 	}
 
-	retval = arc_reg_get_field(target, reg_name, field_name, &value);
+	const char *reg_name = CMD_ARGV[0];
+	const char *field_name = CMD_ARGV[1];
+	uint32_t value;
+	int retval = arc_reg_get_field(target, reg_name, field_name, &value);
 
 	switch (retval) {
 		case ERROR_OK:
 			break;
 		case ERROR_ARC_REGISTER_NOT_FOUND:
-			Jim_SetResultFormatted(goi.interp,
+			command_print(CMD,
 				"Register `%s' has not been found.", reg_name);
 			return ERROR_COMMAND_ARGUMENT_INVALID;
 		case ERROR_ARC_REGISTER_IS_NOT_STRUCT:
-			Jim_SetResultFormatted(goi.interp,
+			command_print(CMD,
 				"Register `%s' must have 'struct' type.", reg_name);
 			return ERROR_COMMAND_ARGUMENT_INVALID;
 		case ERROR_ARC_REGISTER_FIELD_NOT_FOUND:
-			Jim_SetResultFormatted(goi.interp,
+			command_print(CMD,
 				"Field `%s' has not been found in register `%s'.",
 				field_name, reg_name);
 			return ERROR_COMMAND_ARGUMENT_INVALID;
 		case ERROR_ARC_FIELD_IS_NOT_BITFIELD:
-			Jim_SetResultFormatted(goi.interp,
+			command_print(CMD,
 				"Field `%s' is not a 'bitfield' field in a structure.",
 				field_name);
 			return ERROR_COMMAND_ARGUMENT_INVALID;
@@ -904,9 +734,9 @@ static int jim_arc_get_reg_field(Jim_Interp *interp, int argc, Jim_Obj * const *
 			return retval;
 	}
 
-	Jim_SetResultInt(interp, value);
+	command_print(CMD, "0x%" PRIx32, value);
 
-	return JIM_OK;
+	return ERROR_OK;
 }
 
 COMMAND_HANDLER(arc_l1_cache_disable_auto_cmd)
@@ -929,27 +759,17 @@ COMMAND_HANDLER(arc_l2_cache_disable_auto_cmd)
 		&arc->has_l2cache, "target has l2 cache enabled");
 }
 
-static int jim_handle_actionpoints_num(Jim_Interp *interp, int argc,
-	Jim_Obj * const *argv)
+COMMAND_HANDLER(arc_handle_actionpoints_num)
 {
-	struct jim_getopt_info goi;
-	jim_getopt_setup(&goi, interp, argc - 1, argv + 1);
-
 	LOG_DEBUG("-");
 
-	if (goi.argc >= 2) {
-		Jim_WrongNumArgs(interp, goi.argc, goi.argv, "[<unsigned integer>]");
-		return JIM_ERR;
-	}
-
-	struct command_context *context = current_command_context(interp);
-	assert(context);
-
-	struct target *target = get_current_target(context);
+	if (CMD_ARGC >= 2)
+		return ERROR_COMMAND_SYNTAX_ERROR;
 
+	struct target *target = get_current_target(CMD_CTX);
 	if (!target) {
-		Jim_SetResultFormatted(goi.interp, "No current target");
-		return JIM_ERR;
+		command_print(CMD, "No current target");
+		return ERROR_FAIL;
 	}
 
 	struct arc_common *arc = target_to_arc(target);
@@ -958,19 +778,19 @@ static int jim_handle_actionpoints_num(Jim_Interp *interp, int argc,
 	 * "actionpoint reset, initiated by arc_set_actionpoints_num.  */
 	uint32_t ap_num = arc->actionpoints_num;
 
-	if (goi.argc == 1) {
-		JIM_CHECK_RETVAL(arc_cmd_jim_get_uint32(&goi, &ap_num));
+	if (CMD_ARGC == 1) {
+		COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], ap_num);
 		int e = arc_set_actionpoints_num(target, ap_num);
 		if (e != ERROR_OK) {
-			Jim_SetResultFormatted(goi.interp,
+			command_print(CMD,
 				"Failed to set number of actionpoints");
-			return JIM_ERR;
+			return e;
 		}
 	}
 
-	Jim_SetResultInt(interp, ap_num);
+	command_print(CMD, "%" PRIu32, ap_num);
 
-	return JIM_OK;
+	return ERROR_OK;
 }
 
 /* ----- Exported target commands ------------------------------------------ */
@@ -1008,7 +828,7 @@ static const struct command_registration arc_cache_group_handlers[] = {
 static const struct command_registration arc_core_command_handlers[] = {
 	{
 		.name = "add-reg-type-flags",
-		.jim_handler = jim_arc_add_reg_type_flags,
+		.handler = arc_handle_add_reg_type_flags,
 		.mode = COMMAND_CONFIG,
 		.usage = "-name <string> -flag <name> <position> "
 			"[-flag <name> <position>]...",
@@ -1018,7 +838,7 @@ static const struct command_registration arc_core_command_handlers[] = {
 	},
 	{
 		.name = "add-reg-type-struct",
-		.jim_handler = jim_arc_add_reg_type_struct,
+		.handler = arc_handle_add_reg_type_struct,
 		.mode = COMMAND_CONFIG,
 		.usage = "-name <string> -bitfield <name> <start> <end> "
 			"[-bitfield <name> <start> <end>]...",
@@ -1030,7 +850,7 @@ static const struct command_registration arc_core_command_handlers[] = {
 	},
 	{
 		.name = "add-reg",
-		.jim_handler = jim_arc_add_reg,
+		.handler = arc_handle_add_reg,
 		.mode = COMMAND_CONFIG,
 		.usage = "-name <string> -num <int> -feature <string> [-gdbnum <int>] "
 			"[-core|-bcr] [-type <type_name>] [-g]",
@@ -1049,7 +869,7 @@ static const struct command_registration arc_core_command_handlers[] = {
 	},
 	{
 		.name = "get-reg-field",
-		.jim_handler = jim_arc_get_reg_field,
+		.handler = arc_handle_get_reg_field,
 		.mode = COMMAND_ANY,
 		.usage = "<regname> <field_name>",
 		.help = "Returns value of field in a register with 'struct' type.",
@@ -1070,7 +890,7 @@ static const struct command_registration arc_core_command_handlers[] = {
 	},
 	{
 		.name = "num-actionpoints",
-		.jim_handler = jim_handle_actionpoints_num,
+		.handler = arc_handle_actionpoints_num,
 		.mode = COMMAND_ANY,
 		.usage = "[<unsigned integer>]",
 		.help = "Prints or sets amount of actionpoints in the processor.",
diff --git a/src/target/image.c b/src/target/image.c
index f8de7a2..6aa609d 100644
--- a/src/target/image.c
+++ b/src/target/image.c
@@ -407,12 +407,10 @@ static int image_elf32_read_headers(struct image *image)
 		return ERROR_FILEIO_OPERATION_FAILED;
 	}
 
-	/* count useful segments (loadable), ignore BSS section */
+	/* count useful segments (loadable) */
 	image->num_sections = 0;
 	for (i = 0; i < elf->segment_count; i++)
-		if ((field32(elf,
-			elf->segments32[i].p_type) == PT_LOAD) &&
-			(field32(elf, elf->segments32[i].p_filesz) != 0))
+		if (field32(elf, elf->segments32[i].p_type) == PT_LOAD)
 			image->num_sections++;
 
 	if (image->num_sections == 0) {
@@ -449,10 +447,8 @@ 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) &&
-			(field32(elf, elf->segments32[i].p_filesz) != 0)) {
-			image->sections[j].size = field32(elf, elf->segments32[i].p_filesz);
+		if (field32(elf, elf->segments32[i].p_type) == PT_LOAD) {
+			image->sections[j].size = field32(elf, elf->segments32[i].p_memsz);
 			if (load_to_vaddr)
 				image->sections[j].base_address = field32(elf,
 						elf->segments32[i].p_vaddr);
@@ -532,12 +528,10 @@ static int image_elf64_read_headers(struct image *image)
 		return ERROR_FILEIO_OPERATION_FAILED;
 	}
 
-	/* count useful segments (loadable), ignore BSS section */
+	/* count useful segments (loadable) */
 	image->num_sections = 0;
 	for (i = 0; i < elf->segment_count; i++)
-		if ((field32(elf,
-			elf->segments64[i].p_type) == PT_LOAD) &&
-			(field64(elf, elf->segments64[i].p_filesz) != 0))
+		if (field32(elf, elf->segments64[i].p_type) == PT_LOAD)
 			image->num_sections++;
 
 	if (image->num_sections == 0) {
@@ -574,10 +568,8 @@ 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) &&
-			(field64(elf, elf->segments64[i].p_filesz) != 0)) {
-			image->sections[j].size = field64(elf, elf->segments64[i].p_filesz);
+		if (field32(elf, elf->segments64[i].p_type) == PT_LOAD) {
+			image->sections[j].size = field64(elf, elf->segments64[i].p_memsz);
 			if (load_to_vaddr)
 				image->sections[j].base_address = field64(elf,
 						elf->segments64[i].p_vaddr);
@@ -651,6 +643,8 @@ 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;
 
@@ -659,9 +653,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 < field32(elf, segment->p_filesz)) {
+	if (offset < filesz) {
 		/* maximal size present in file for the current segment */
-		read_size = MIN(size, field32(elf, segment->p_filesz) - offset);
+		read_size = MIN(size, 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 */
@@ -675,6 +669,8 @@ 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 ? */
@@ -682,6 +678,13 @@ 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;
 }
 
@@ -694,6 +697,8 @@ 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;
 
@@ -702,9 +707,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 < field64(elf, segment->p_filesz)) {
+	if (offset < filesz) {
 		/* maximal size present in file for the current segment */
-		read_size = MIN(size, field64(elf, segment->p_filesz) - offset);
+		read_size = MIN(size, 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 */
@@ -718,6 +723,8 @@ 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 ? */
@@ -725,6 +732,13 @@ 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/xtensa/xtensa.c b/src/target/xtensa/xtensa.c
index b57e2d6..fcd0048 100644
--- a/src/target/xtensa/xtensa.c
+++ b/src/target/xtensa/xtensa.c
@@ -165,6 +165,7 @@
 #define XT_SR_DDR         (xtensa_regs[XT_REG_IDX_DDR].reg_num)
 #define XT_SR_PS          (xtensa_regs[XT_REG_IDX_PS].reg_num)
 #define XT_SR_WB          (xtensa_regs[XT_REG_IDX_WINDOWBASE].reg_num)
+#define XT_REG_A0         (xtensa_regs[XT_REG_IDX_AR0].reg_num)
 #define XT_REG_A3         (xtensa_regs[XT_REG_IDX_AR3].reg_num)
 #define XT_REG_A4         (xtensa_regs[XT_REG_IDX_AR4].reg_num)
 
@@ -173,6 +174,7 @@
 #define XT_EPC_REG_NUM_BASE         (0xb0U)	/* (EPC1 - 1), for adding DBGLEVEL */
 #define XT_PC_REG_NUM_VIRTUAL       (0xffU)	/* Marker for computing PC (EPC[DBGLEVEL) */
 #define XT_PC_DBREG_NUM_BASE        (0x20U)	/* External (i.e., GDB) access */
+#define XT_NX_IBREAKC_BASE          (0xc0U)	/* (IBREAKC0..IBREAKC1) for NX */
 
 #define XT_SW_BREAKPOINTS_MAX_NUM       32
 #define XT_HW_IBREAK_MAX_NUM            2
@@ -476,7 +478,9 @@ static enum xtensa_reg_id xtensa_windowbase_offset_to_canonical(struct xtensa *x
 		LOG_ERROR("Error: can't convert register %d to non-windowbased register!", reg_idx);
 		return -1;
 	}
-	return ((idx + windowbase * 4) & (xtensa->core_config->aregs_num - 1)) + XT_REG_IDX_AR0;
+	/* Each windowbase value represents 4 registers on LX and 8 on NX */
+	int base_inc = (xtensa->core_config->core_type == XT_LX) ? 4 : 8;
+	return ((idx + windowbase * base_inc) & (xtensa->core_config->aregs_num - 1)) + XT_REG_IDX_AR0;
 }
 
 static enum xtensa_reg_id xtensa_canonical_to_windowbase_offset(struct xtensa *xtensa,
@@ -526,26 +530,29 @@ static int xtensa_queue_pwr_reg_write(struct xtensa *xtensa, unsigned int reg, u
 static int xtensa_window_state_save(struct target *target, uint32_t *woe)
 {
 	struct xtensa *xtensa = target_to_xtensa(target);
-	int woe_dis;
+	unsigned int woe_sr = (xtensa->core_config->core_type == XT_LX) ? XT_SR_PS : XT_SR_WB;
+	uint32_t woe_dis;
 	uint8_t woe_buf[4];
 
 	if (xtensa->core_config->windowed) {
-		/* Save PS (LX) and disable window overflow exceptions prior to AR save */
-		xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, XT_SR_PS, XT_REG_A3));
+		/* Save PS (LX) or WB (NX) and disable window overflow exceptions prior to AR save */
+		xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, woe_sr, XT_REG_A3));
 		xtensa_queue_exec_ins(xtensa, XT_INS_WSR(xtensa, XT_SR_DDR, XT_REG_A3));
 		xtensa_queue_dbg_reg_read(xtensa, XDMREG_DDR, woe_buf);
 		int res = xtensa_dm_queue_execute(&xtensa->dbg_mod);
 		if (res != ERROR_OK) {
-			LOG_ERROR("Failed to read PS (%d)!", res);
+			LOG_TARGET_ERROR(target, "Failed to read %s (%d)!",
+				(woe_sr == XT_SR_PS) ? "PS" : "WB", res);
 			return res;
 		}
 		xtensa_core_status_check(target);
 		*woe = buf_get_u32(woe_buf, 0, 32);
-		woe_dis = *woe & ~XT_PS_WOE_MSK;
-		LOG_DEBUG("Clearing PS.WOE (0x%08" PRIx32 " -> 0x%08" PRIx32 ")", *woe, woe_dis);
+		woe_dis = *woe & ~((woe_sr == XT_SR_PS) ? XT_PS_WOE_MSK : XT_WB_S_MSK);
+		LOG_TARGET_DEBUG(target, "Clearing %s (0x%08" PRIx32 " -> 0x%08" PRIx32 ")",
+			(woe_sr == XT_SR_PS) ? "PS.WOE" : "WB.S", *woe, woe_dis);
 		xtensa_queue_dbg_reg_write(xtensa, XDMREG_DDR, woe_dis);
 		xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, XT_SR_DDR, XT_REG_A3));
-		xtensa_queue_exec_ins(xtensa, XT_INS_WSR(xtensa, XT_SR_PS, XT_REG_A3));
+		xtensa_queue_exec_ins(xtensa, XT_INS_WSR(xtensa, woe_sr, XT_REG_A3));
 	}
 	return ERROR_OK;
 }
@@ -554,12 +561,14 @@ static int xtensa_window_state_save(struct target *target, uint32_t *woe)
 static void xtensa_window_state_restore(struct target *target, uint32_t woe)
 {
 	struct xtensa *xtensa = target_to_xtensa(target);
+	unsigned int woe_sr = (xtensa->core_config->core_type == XT_LX) ? XT_SR_PS : XT_SR_WB;
 	if (xtensa->core_config->windowed) {
 		/* Restore window overflow exception state */
 		xtensa_queue_dbg_reg_write(xtensa, XDMREG_DDR, woe);
 		xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, XT_SR_DDR, XT_REG_A3));
-		xtensa_queue_exec_ins(xtensa, XT_INS_WSR(xtensa, XT_SR_PS, XT_REG_A3));
-		LOG_DEBUG("Restored PS.WOE (0x%08" PRIx32 ")", woe);
+		xtensa_queue_exec_ins(xtensa, XT_INS_WSR(xtensa, woe_sr, XT_REG_A3));
+		LOG_TARGET_DEBUG(target, "Restored %s (0x%08" PRIx32 ")",
+			(woe_sr == XT_SR_PS) ? "PS.WOE" : "WB", woe);
 	}
 }
 
@@ -596,6 +605,10 @@ static int xtensa_write_dirty_registers(struct target *target)
 	bool preserve_a3 = false;
 	uint8_t a3_buf[4];
 	xtensa_reg_val_t a3 = 0, woe;
+	unsigned int ms_idx = (xtensa->core_config->core_type == XT_NX) ?
+		xtensa->nx_reg_idx[XT_NX_REG_IDX_MS] : reg_list_size;
+	xtensa_reg_val_t ms;
+	bool restore_ms = false;
 
 	LOG_TARGET_DEBUG(target, "start");
 
@@ -627,13 +640,25 @@ static int xtensa_write_dirty_registers(struct target *target)
 					} else if (rlist[ridx].type == XT_REG_FR) {
 						xtensa_queue_exec_ins(xtensa, XT_INS_WFR(xtensa, reg_num, XT_REG_A3));
 					} else {/*SFR */
-						if (reg_num == XT_PC_REG_NUM_VIRTUAL)
-							/* reg number of PC for debug interrupt depends on NDEBUGLEVEL
-							 **/
-							reg_num =
-								(XT_EPC_REG_NUM_BASE +
-								xtensa->core_config->debug.irq_level);
-						xtensa_queue_exec_ins(xtensa, XT_INS_WSR(xtensa, reg_num, XT_REG_A3));
+						if (reg_num == XT_PC_REG_NUM_VIRTUAL) {
+							if (xtensa->core_config->core_type == XT_LX) {
+								/* reg number of PC for debug interrupt depends on NDEBUGLEVEL */
+								reg_num = (XT_EPC_REG_NUM_BASE + xtensa->core_config->debug.irq_level);
+								xtensa_queue_exec_ins(xtensa, XT_INS_WSR(xtensa, reg_num, XT_REG_A3));
+							} else {
+								/* NX PC set through issuing a jump instruction */
+								xtensa_queue_exec_ins(xtensa, XT_INS_JX(xtensa, XT_REG_A3));
+							}
+						} else if (i == ms_idx) {
+							/* MS must be restored after ARs.  This ensures ARs remain in correct
+							 * order even for reversed register groups (overflow/underflow).
+							 */
+							ms = regval;
+							restore_ms = true;
+							LOG_TARGET_DEBUG(target, "Delaying MS write: 0x%x", ms);
+						} else {
+							xtensa_queue_exec_ins(xtensa, XT_INS_WSR(xtensa, reg_num, XT_REG_A3));
+						}
 					}
 				}
 				reg_list[i].dirty = false;
@@ -648,12 +673,12 @@ static int xtensa_write_dirty_registers(struct target *target)
 		xtensa_queue_dbg_reg_write(xtensa, XDMREG_DDR, regval);
 		xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, XT_SR_DDR, XT_REG_A3));
 		xtensa_queue_exec_ins(xtensa, XT_INS_WSR(xtensa,
-				xtensa_regs[XT_REG_IDX_CPENABLE].reg_num,
-				XT_REG_A3));
+			xtensa_regs[XT_REG_IDX_CPENABLE].reg_num,
+			XT_REG_A3));
 		reg_list[XT_REG_IDX_CPENABLE].dirty = false;
 	}
 
-	preserve_a3 = (xtensa->core_config->windowed);
+	preserve_a3 = (xtensa->core_config->windowed) || (xtensa->core_config->core_type == XT_NX);
 	if (preserve_a3) {
 		/* Save (windowed) A3 for scratch use */
 		xtensa_queue_exec_ins(xtensa, XT_INS_WSR(xtensa, XT_SR_DDR, XT_REG_A3));
@@ -670,7 +695,12 @@ static int xtensa_write_dirty_registers(struct target *target)
 		if (res != ERROR_OK)
 			return res;
 		/* Grab the windowbase, we need it. */
-		windowbase = xtensa_reg_get(target, XT_REG_IDX_WINDOWBASE);
+		uint32_t wb_idx = (xtensa->core_config->core_type == XT_LX) ?
+			XT_REG_IDX_WINDOWBASE : xtensa->nx_reg_idx[XT_NX_REG_IDX_WB];
+		windowbase = xtensa_reg_get(target, wb_idx);
+		if (xtensa->core_config->core_type == XT_NX)
+			windowbase = (windowbase & XT_WB_P_MSK) >> XT_WB_P_SHIFT;
+
 		/* Check if there are mismatches between the ARx and corresponding Ax registers.
 		 * When the user sets a register on a windowed config, xt-gdb may set the ARx
 		 * register directly.  Thus we take ARx as priority over Ax if both are dirty
@@ -748,10 +778,12 @@ static int xtensa_write_dirty_registers(struct target *target)
 					}
 				}
 			}
-			/*Now rotate the window so we'll see the next 16 registers. The final rotate
-			 * will wraparound, */
-			/*leaving us in the state we were. */
-			xtensa_queue_exec_ins(xtensa, XT_INS_ROTW(xtensa, 4));
+
+			/* Now rotate the window so we'll see the next 16 registers. The final rotate
+			 * will wraparound, leaving us in the state we were.
+			 * Each ROTW rotates 4 registers on LX and 8 on NX */
+			int rotw_arg = (xtensa->core_config->core_type == XT_LX) ? 4 : 2;
+			xtensa_queue_exec_ins(xtensa, XT_INS_ROTW(xtensa, rotw_arg));
 		}
 
 		xtensa_window_state_restore(target, woe);
@@ -760,6 +792,14 @@ static int xtensa_write_dirty_registers(struct target *target)
 			xtensa->scratch_ars[s].intval = false;
 	}
 
+	if (restore_ms) {
+		uint32_t ms_regno = xtensa->optregs[ms_idx - XT_NUM_REGS].reg_num;
+		xtensa_queue_dbg_reg_write(xtensa, XDMREG_DDR, ms);
+		xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, XT_SR_DDR, XT_REG_A3));
+		xtensa_queue_exec_ins(xtensa, XT_INS_WSR(xtensa, ms_regno, XT_REG_A3));
+		LOG_TARGET_DEBUG(target, "Delayed MS (0x%x) write complete: 0x%x", ms_regno, ms);
+	}
+
 	if (preserve_a3) {
 		xtensa_queue_dbg_reg_write(xtensa, XDMREG_DDR, a3);
 		xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, XT_SR_DDR, XT_REG_A3));
@@ -877,10 +917,41 @@ static inline void xtensa_reg_set_value(struct reg *reg, xtensa_reg_val_t value)
 	reg->dirty = true;
 }
 
+static int xtensa_imprecise_exception_occurred(struct target *target)
+{
+	struct xtensa *xtensa = target_to_xtensa(target);
+	for (enum xtensa_nx_reg_idx idx = XT_NX_REG_IDX_IEVEC; idx <= XT_NX_REG_IDX_MESR; idx++) {
+		enum xtensa_reg_id ridx = xtensa->nx_reg_idx[idx];
+		if (xtensa->nx_reg_idx[idx]) {
+			xtensa_reg_val_t reg = xtensa_reg_get(target, xtensa->nx_reg_idx[idx]);
+			if (reg & XT_IMPR_EXC_MSK) {
+				LOG_TARGET_DEBUG(target, "Imprecise exception: %s: 0x%x",
+					xtensa->core_cache->reg_list[ridx].name, reg);
+				return true;
+			}
+		}
+	}
+	return false;
+}
+
+static void xtensa_imprecise_exception_clear(struct target *target)
+{
+	struct xtensa *xtensa = target_to_xtensa(target);
+	for (enum xtensa_nx_reg_idx idx = XT_NX_REG_IDX_IEVEC; idx <= XT_NX_REG_IDX_MESRCLR; idx++) {
+		enum xtensa_reg_id ridx = xtensa->nx_reg_idx[idx];
+		if (ridx && idx != XT_NX_REG_IDX_MESR) {
+			xtensa_reg_val_t value = (idx == XT_NX_REG_IDX_MESRCLR) ? XT_MESRCLR_IMPR_EXC_MSK : 0;
+			xtensa_reg_set(target, ridx, value);
+			LOG_TARGET_DEBUG(target, "Imprecise exception: clearing %s (0x%x)",
+				xtensa->core_cache->reg_list[ridx].name, value);
+		}
+	}
+}
+
 int xtensa_core_status_check(struct target *target)
 {
 	struct xtensa *xtensa = target_to_xtensa(target);
-	int res, needclear = 0;
+	int res, needclear = 0, needimprclear = 0;
 
 	xtensa_dm_core_status_read(&xtensa->dbg_mod);
 	xtensa_dsr_t dsr = xtensa_dm_core_status_get(&xtensa->dbg_mod);
@@ -904,11 +975,20 @@ int xtensa_core_status_check(struct target *target)
 				dsr);
 		needclear = 1;
 	}
+	if (xtensa->core_config->core_type == XT_NX && (xtensa_imprecise_exception_occurred(target))) {
+		if (!xtensa->suppress_dsr_errors)
+			LOG_TARGET_ERROR(target,
+				"%s: Imprecise exception occurred!", target_name(target));
+		needclear = 1;
+		needimprclear = 1;
+	}
 	if (needclear) {
 		res = xtensa_dm_core_status_clear(&xtensa->dbg_mod,
 			OCDDSR_EXECEXCEPTION | OCDDSR_EXECOVERRUN);
 		if (res != ERROR_OK && !xtensa->suppress_dsr_errors)
 			LOG_TARGET_ERROR(target, "clearing DSR failed!");
+		if (xtensa->core_config->core_type == XT_NX && needimprclear)
+			xtensa_imprecise_exception_clear(target);
 		return ERROR_FAIL;
 	}
 	return ERROR_OK;
@@ -934,8 +1014,12 @@ void xtensa_reg_set(struct target *target, enum xtensa_reg_id reg_id, xtensa_reg
 void xtensa_reg_set_deep_relgen(struct target *target, enum xtensa_reg_id a_idx, xtensa_reg_val_t value)
 {
 	struct xtensa *xtensa = target_to_xtensa(target);
+	uint32_t wb_idx = (xtensa->core_config->core_type == XT_LX) ?
+		XT_REG_IDX_WINDOWBASE : xtensa->nx_reg_idx[XT_NX_REG_IDX_WB];
 	uint32_t windowbase = (xtensa->core_config->windowed ?
-		xtensa_reg_get(target, XT_REG_IDX_WINDOWBASE) : 0);
+		xtensa_reg_get(target, wb_idx) : 0);
+	if (xtensa->core_config->core_type == XT_NX)
+		windowbase = (windowbase & XT_WB_P_MSK) >> XT_WB_P_SHIFT;
 	int ar_idx = xtensa_windowbase_offset_to_canonical(xtensa, a_idx, windowbase);
 	xtensa_reg_set(target, a_idx, value);
 	xtensa_reg_set(target, ar_idx, value);
@@ -944,14 +1028,68 @@ void xtensa_reg_set_deep_relgen(struct target *target, enum xtensa_reg_id a_idx,
 /* Read cause for entering halted state; return bitmask in DEBUGCAUSE_* format */
 uint32_t xtensa_cause_get(struct target *target)
 {
-	return xtensa_reg_get(target, XT_REG_IDX_DEBUGCAUSE);
+	struct xtensa *xtensa = target_to_xtensa(target);
+	if (xtensa->core_config->core_type == XT_LX) {
+		/* LX cause in DEBUGCAUSE */
+		return xtensa_reg_get(target, XT_REG_IDX_DEBUGCAUSE);
+	}
+	if (xtensa->nx_stop_cause & DEBUGCAUSE_VALID)
+		return xtensa->nx_stop_cause;
+
+	/* NX cause determined from DSR.StopCause */
+	if (xtensa_dm_core_status_read(&xtensa->dbg_mod) != ERROR_OK) {
+		LOG_TARGET_ERROR(target, "Read DSR error");
+	} else {
+		uint32_t dsr = xtensa_dm_core_status_get(&xtensa->dbg_mod);
+		/* NX causes are prioritized; only 1 bit can be set */
+		switch ((dsr & OCDDSR_STOPCAUSE) >> OCDDSR_STOPCAUSE_SHIFT) {
+		case OCDDSR_STOPCAUSE_DI:
+			xtensa->nx_stop_cause = DEBUGCAUSE_DI;
+			break;
+		case OCDDSR_STOPCAUSE_SS:
+			xtensa->nx_stop_cause = DEBUGCAUSE_IC;
+			break;
+		case OCDDSR_STOPCAUSE_IB:
+			xtensa->nx_stop_cause = DEBUGCAUSE_IB;
+			break;
+		case OCDDSR_STOPCAUSE_B:
+		case OCDDSR_STOPCAUSE_B1:
+			xtensa->nx_stop_cause = DEBUGCAUSE_BI;
+			break;
+		case OCDDSR_STOPCAUSE_BN:
+			xtensa->nx_stop_cause = DEBUGCAUSE_BN;
+			break;
+		case OCDDSR_STOPCAUSE_DB0:
+		case OCDDSR_STOPCAUSE_DB1:
+			xtensa->nx_stop_cause = DEBUGCAUSE_DB;
+			break;
+		default:
+			LOG_TARGET_ERROR(target, "Unknown stop cause (DSR: 0x%08x)", dsr);
+			break;
+		}
+		if (xtensa->nx_stop_cause)
+			xtensa->nx_stop_cause |= DEBUGCAUSE_VALID;
+	}
+	return xtensa->nx_stop_cause;
 }
 
 void xtensa_cause_clear(struct target *target)
 {
 	struct xtensa *xtensa = target_to_xtensa(target);
-	xtensa_reg_set(target, XT_REG_IDX_DEBUGCAUSE, 0);
-	xtensa->core_cache->reg_list[XT_REG_IDX_DEBUGCAUSE].dirty = false;
+	if (xtensa->core_config->core_type == XT_LX) {
+		xtensa_reg_set(target, XT_REG_IDX_DEBUGCAUSE, 0);
+		xtensa->core_cache->reg_list[XT_REG_IDX_DEBUGCAUSE].dirty = false;
+	} else {
+		/* NX DSR.STOPCAUSE is not writeable; clear cached copy but leave it valid */
+		xtensa->nx_stop_cause = DEBUGCAUSE_VALID;
+	}
+}
+
+void xtensa_cause_reset(struct target *target)
+{
+	/* Clear DEBUGCAUSE_VALID to trigger re-read (on NX) */
+	struct xtensa *xtensa = target_to_xtensa(target);
+	xtensa->nx_stop_cause = 0;
 }
 
 int xtensa_assert_reset(struct target *target)
@@ -1008,9 +1146,11 @@ int xtensa_fetch_all_regs(struct target *target)
 	struct xtensa *xtensa = target_to_xtensa(target);
 	struct reg *reg_list = xtensa->core_cache->reg_list;
 	unsigned int reg_list_size = xtensa->core_cache->num_regs;
-	xtensa_reg_val_t cpenable = 0, windowbase = 0, a3;
+	xtensa_reg_val_t cpenable = 0, windowbase = 0, a0 = 0, a3;
+	unsigned int ms_idx = reg_list_size;
+	uint32_t ms = 0;
 	uint32_t woe;
-	uint8_t a3_buf[4];
+	uint8_t a0_buf[4], a3_buf[4], ms_buf[4];
 	bool debug_dsrs = !xtensa->regs_fetched || LOG_LEVEL_IS(LOG_LVL_DEBUG);
 
 	union xtensa_reg_val_u *regvals = calloc(reg_list_size, sizeof(*regvals));
@@ -1030,6 +1170,25 @@ int xtensa_fetch_all_regs(struct target *target)
 	/* Save (windowed) A3 so cache matches physical AR3; A3 usable as scratch */
 	xtensa_queue_exec_ins(xtensa, XT_INS_WSR(xtensa, XT_SR_DDR, XT_REG_A3));
 	xtensa_queue_dbg_reg_read(xtensa, XDMREG_DDR, a3_buf);
+	if (xtensa->core_config->core_type == XT_NX) {
+		/* Save (windowed) A0 as well--it will be required for reading PC */
+		xtensa_queue_exec_ins(xtensa, XT_INS_WSR(xtensa, XT_SR_DDR, XT_REG_A0));
+		xtensa_queue_dbg_reg_read(xtensa, XDMREG_DDR, a0_buf);
+
+		/* Set MS.DispSt, clear MS.DE prior to accessing ARs.  This ensures ARs remain
+		 * in correct order even for reversed register groups (overflow/underflow).
+		 */
+		ms_idx = xtensa->nx_reg_idx[XT_NX_REG_IDX_MS];
+		uint32_t ms_regno = xtensa->optregs[ms_idx - XT_NUM_REGS].reg_num;
+		xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, ms_regno, XT_REG_A3));
+		xtensa_queue_exec_ins(xtensa, XT_INS_WSR(xtensa, XT_SR_DDR, XT_REG_A3));
+		xtensa_queue_dbg_reg_read(xtensa, XDMREG_DDR, ms_buf);
+		LOG_TARGET_DEBUG(target, "Overriding MS (0x%x): 0x%x", ms_regno, XT_MS_DISPST_DBG);
+		xtensa_queue_dbg_reg_write(xtensa, XDMREG_DDR, XT_MS_DISPST_DBG);
+		xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, XT_SR_DDR, XT_REG_A3));
+		xtensa_queue_exec_ins(xtensa, XT_INS_WSR(xtensa, ms_regno, XT_REG_A3));
+	}
+
 	int res = xtensa_window_state_save(target, &woe);
 	if (res != ERROR_OK)
 		goto xtensa_fetch_all_regs_done;
@@ -1052,11 +1211,13 @@ int xtensa_fetch_all_regs(struct target *target)
 						dsrs[XT_REG_IDX_AR0 + i + j].buf);
 			}
 		}
-		if (xtensa->core_config->windowed)
+		if (xtensa->core_config->windowed) {
 			/* Now rotate the window so we'll see the next 16 registers. The final rotate
-			 * will wraparound, */
-			/* leaving us in the state we were. */
-			xtensa_queue_exec_ins(xtensa, XT_INS_ROTW(xtensa, 4));
+			 * will wraparound, leaving us in the state we were.
+			 * Each ROTW rotates 4 registers on LX and 8 on NX */
+			int rotw_arg = (xtensa->core_config->core_type == XT_LX) ? 4 : 2;
+			xtensa_queue_exec_ins(xtensa, XT_INS_ROTW(xtensa, rotw_arg));
+		}
 	}
 	xtensa_window_state_restore(target, woe);
 
@@ -1074,6 +1235,10 @@ int xtensa_fetch_all_regs(struct target *target)
 	xtensa_core_status_check(target);
 
 	a3 = buf_get_u32(a3_buf, 0, 32);
+	if (xtensa->core_config->core_type == XT_NX) {
+		a0 = buf_get_u32(a0_buf, 0, 32);
+		ms = buf_get_u32(ms_buf, 0, 32);
+	}
 
 	if (xtensa->core_config->coproc) {
 		cpenable = buf_get_u32(regvals[XT_REG_IDX_CPENABLE].buf, 0, 32);
@@ -1104,17 +1269,30 @@ int xtensa_fetch_all_regs(struct target *target)
 				break;
 			case XT_REG_SPECIAL:
 				if (reg_num == XT_PC_REG_NUM_VIRTUAL) {
-					/* reg number of PC for debug interrupt depends on NDEBUGLEVEL */
-					reg_num = XT_EPC_REG_NUM_BASE + xtensa->core_config->debug.irq_level;
-				} else if (reg_num == xtensa_regs[XT_REG_IDX_PS].reg_num) {
+					if (xtensa->core_config->core_type == XT_LX) {
+						/* reg number of PC for debug interrupt depends on NDEBUGLEVEL */
+						reg_num = XT_EPC_REG_NUM_BASE + xtensa->core_config->debug.irq_level;
+						xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, reg_num, XT_REG_A3));
+					} else {
+						/* NX PC read through CALL0(0) and reading A0 */
+						xtensa_queue_exec_ins(xtensa, XT_INS_CALL0(xtensa, 0));
+						xtensa_queue_exec_ins(xtensa, XT_INS_WSR(xtensa, XT_SR_DDR, XT_REG_A0));
+						xtensa_queue_dbg_reg_read(xtensa, XDMREG_DDR, regvals[i].buf);
+						xtensa_queue_dbg_reg_read(xtensa, XDMREG_DSR, dsrs[i].buf);
+						reg_fetched = false;
+					}
+				} else if ((xtensa->core_config->core_type == XT_LX)
+					&& (reg_num == xtensa_regs[XT_REG_IDX_PS].reg_num)) {
 					/* reg number of PS for debug interrupt depends on NDEBUGLEVEL */
 					reg_num = XT_EPS_REG_NUM_BASE + xtensa->core_config->debug.irq_level;
+					xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, reg_num, XT_REG_A3));
 				} else if (reg_num == xtensa_regs[XT_REG_IDX_CPENABLE].reg_num) {
 					/* CPENABLE already read/updated; don't re-read */
 					reg_fetched = false;
 					break;
+				} else {
+					xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, reg_num, XT_REG_A3));
 				}
-				xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, reg_num, XT_REG_A3));
 				break;
 			default:
 				reg_fetched = false;
@@ -1154,9 +1332,15 @@ int xtensa_fetch_all_regs(struct target *target)
 		}
 	}
 
-	if (xtensa->core_config->windowed)
+	if (xtensa->core_config->windowed) {
 		/* We need the windowbase to decode the general addresses. */
-		windowbase = buf_get_u32(regvals[XT_REG_IDX_WINDOWBASE].buf, 0, 32);
+		uint32_t wb_idx = (xtensa->core_config->core_type == XT_LX) ?
+			XT_REG_IDX_WINDOWBASE : xtensa->nx_reg_idx[XT_NX_REG_IDX_WB];
+		windowbase = buf_get_u32(regvals[wb_idx].buf, 0, 32);
+		if (xtensa->core_config->core_type == XT_NX)
+			windowbase = (windowbase & XT_WB_P_MSK) >> XT_WB_P_SHIFT;
+	}
+
 	/* Decode the result and update the cache. */
 	for (unsigned int i = 0; i < reg_list_size; i++) {
 		struct xtensa_reg_desc *rlist = (i < XT_NUM_REGS) ? xtensa_regs : xtensa->optregs;
@@ -1180,6 +1364,16 @@ int xtensa_fetch_all_regs(struct target *target)
 				bool is_dirty = (i == XT_REG_IDX_CPENABLE);
 				if (xtensa_extra_debug_log)
 					LOG_INFO("Register %s: 0x%X", reg_list[i].name, regval);
+				if (rlist[ridx].reg_num == XT_PC_REG_NUM_VIRTUAL &&
+					xtensa->core_config->core_type == XT_NX) {
+					/* A0 from prior CALL0 points to next instruction; decrement it */
+					regval -= 3;
+					is_dirty = 1;
+				} else if (i == ms_idx) {
+					LOG_TARGET_DEBUG(target, "Caching MS: 0x%x", ms);
+					regval = ms;
+					is_dirty = 1;
+				}
 				xtensa_reg_set(target, i, regval);
 				reg_list[i].dirty = is_dirty;	/*always do this _after_ xtensa_reg_set! */
 			}
@@ -1214,6 +1408,11 @@ int xtensa_fetch_all_regs(struct target *target)
 	/* We have used A3 (XT_REG_RELGEN) as a scratch register.  Restore and flag for write-back. */
 	xtensa_reg_set(target, XT_REG_IDX_A3, a3);
 	xtensa_mark_register_dirty(xtensa, XT_REG_IDX_A3);
+	if (xtensa->core_config->core_type == XT_NX) {
+		xtensa_reg_set(target, XT_REG_IDX_A0, a0);
+		xtensa_mark_register_dirty(xtensa, XT_REG_IDX_A0);
+	}
+
 	xtensa->regs_fetched = true;
 xtensa_fetch_all_regs_done:
 	free(regvals);
@@ -1262,7 +1461,7 @@ int xtensa_get_gdb_reg_list(struct target *target,
 			struct xtensa_reg_desc *rlist = (i < XT_NUM_REGS) ? xtensa_regs : xtensa->optregs;
 			unsigned int ridx = (i < XT_NUM_REGS) ? i : i - XT_NUM_REGS;
 			int sparse_idx = rlist[ridx].dbreg_num;
-			if (i == XT_REG_IDX_PS) {
+			if (i == XT_REG_IDX_PS && xtensa->core_config->core_type == XT_LX) {
 				if (xtensa->eps_dbglevel_idx == 0) {
 					LOG_ERROR("eps_dbglevel_idx not set\n");
 					return ERROR_FAIL;
@@ -1372,10 +1571,13 @@ int xtensa_prepare_resume(struct target *target,
 		if (xtensa->hw_brps[slot]) {
 			/* Write IBREAKA[slot] and set bit #slot in IBREAKENABLE */
 			xtensa_reg_set(target, XT_REG_IDX_IBREAKA0 + slot, xtensa->hw_brps[slot]->address);
+			if (xtensa->core_config->core_type == XT_NX)
+				xtensa_reg_set(target, xtensa->nx_reg_idx[XT_NX_REG_IDX_IBREAKC0] + slot, XT_IBREAKC_FB);
 			bpena |= BIT(slot);
 		}
 	}
-	xtensa_reg_set(target, XT_REG_IDX_IBREAKENABLE, bpena);
+	if (xtensa->core_config->core_type == XT_LX)
+		xtensa_reg_set(target, XT_REG_IDX_IBREAKENABLE, bpena);
 
 	/* Here we write all registers to the targets */
 	int res = xtensa_write_dirty_registers(target);
@@ -1390,6 +1592,7 @@ int xtensa_do_resume(struct target *target)
 
 	LOG_TARGET_DEBUG(target, "start");
 
+	xtensa_cause_reset(target);
 	xtensa_queue_exec_ins(xtensa, XT_INS_RFDO(xtensa));
 	int res = xtensa_dm_queue_execute(&xtensa->dbg_mod);
 	if (res != ERROR_OK) {
@@ -1467,13 +1670,14 @@ int xtensa_do_step(struct target *target, int current, target_addr_t address, in
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
-	if (xtensa->eps_dbglevel_idx == 0) {
-		LOG_ERROR("eps_dbglevel_idx not set\n");
+	if (xtensa->eps_dbglevel_idx == 0 && xtensa->core_config->core_type == XT_LX) {
+		LOG_TARGET_ERROR(target, "eps_dbglevel_idx not set\n");
 		return ERROR_FAIL;
 	}
 
 	/* Save old ps (EPS[dbglvl] on LX), pc */
-	oldps = xtensa_reg_get(target, xtensa->eps_dbglevel_idx);
+	oldps = xtensa_reg_get(target, (xtensa->core_config->core_type == XT_LX) ?
+		xtensa->eps_dbglevel_idx : XT_REG_IDX_PS);
 	oldpc = xtensa_reg_get(target, XT_REG_IDX_PC);
 
 	cause = xtensa_cause_get(target);
@@ -1542,7 +1746,7 @@ int xtensa_do_step(struct target *target, int current, target_addr_t address, in
 	if (!handle_breakpoints && (cause & (DEBUGCAUSE_BI | DEBUGCAUSE_BN)))
 		/* handle normal SW breakpoint */
 		xtensa_cause_clear(target);	/* so we don't recurse into the same routine */
-	if ((oldps & 0xf) >= icountlvl) {
+	if (xtensa->core_config->core_type == XT_LX && ((oldps & 0xf) >= icountlvl)) {
 		/* Lower interrupt level to allow stepping, but flag eps[dbglvl] to be restored */
 		ps_lowered = true;
 		uint32_t newps = (oldps & ~0xf) | (icountlvl - 1);
@@ -1554,10 +1758,16 @@ int xtensa_do_step(struct target *target, int current, target_addr_t address, in
 			oldps);
 	}
 	do {
-		xtensa_reg_set(target, XT_REG_IDX_ICOUNTLEVEL, icountlvl);
-		xtensa_reg_set(target, XT_REG_IDX_ICOUNT, icount_val);
+		if (xtensa->core_config->core_type == XT_LX) {
+			xtensa_reg_set(target, XT_REG_IDX_ICOUNTLEVEL, icountlvl);
+			xtensa_reg_set(target, XT_REG_IDX_ICOUNT, icount_val);
+		} else {
+			xtensa_queue_dbg_reg_write(xtensa, XDMREG_DCRSET, OCDDCR_STEPREQUEST);
+		}
 
-		/* Now ICOUNT is set, we can resume as if we were going to run */
+		/* Now that ICOUNT (LX) or DCR.StepRequest (NX) is set,
+		 * we can resume as if we were going to run
+		 */
 		res = xtensa_prepare_resume(target, current, address, 0, 0);
 		if (res != ERROR_OK) {
 			LOG_TARGET_ERROR(target, "Failed to prepare resume for single step");
@@ -2108,6 +2318,22 @@ int xtensa_poll(struct target *target)
 				OCDDSR_DEBUGPENDBREAK | OCDDSR_DEBUGINTBREAK | OCDDSR_DEBUGPENDTRAX |
 				OCDDSR_DEBUGINTTRAX |
 				OCDDSR_DEBUGPENDHOST | OCDDSR_DEBUGINTHOST);
+			if (xtensa->core_config->core_type == XT_NX) {
+				/* Enable imprecise exceptions while in halted state */
+				xtensa_reg_val_t ps = xtensa_reg_get(target, XT_REG_IDX_PS);
+				xtensa_reg_val_t newps = ps & ~(XT_PS_DIEXC_MSK);
+				xtensa_mark_register_dirty(xtensa, XT_REG_IDX_PS);
+				LOG_TARGET_DEBUG(target, "Enabling PS.DIEXC: 0x%08x -> 0x%08x", ps, newps);
+				xtensa_queue_dbg_reg_write(xtensa, XDMREG_DDR, newps);
+				xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, XT_SR_DDR, XT_REG_A3));
+				xtensa_queue_exec_ins(xtensa, XT_INS_WSR(xtensa, XT_SR_PS, XT_REG_A3));
+				res = xtensa_dm_queue_execute(&xtensa->dbg_mod);
+				if (res != ERROR_OK) {
+					LOG_TARGET_ERROR(target, "Failed to write PS.DIEXC (%d)!", res);
+					return res;
+				}
+				xtensa_core_status_check(target);
+			}
 		}
 	} else {
 		target->debug_reason = DBG_REASON_NOTHALTED;
@@ -2326,6 +2552,8 @@ int xtensa_breakpoint_remove(struct target *target, struct breakpoint *breakpoin
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 	}
 	xtensa->hw_brps[slot] = NULL;
+	if (xtensa->core_config->core_type == XT_NX)
+		xtensa_reg_set(target, xtensa->nx_reg_idx[XT_NX_REG_IDX_IBREAKC0] + slot, 0);
 	LOG_TARGET_DEBUG(target, "cleared HW breakpoint %u @ " TARGET_ADDR_FMT, slot, breakpoint->address);
 	return ERROR_OK;
 }
@@ -3073,8 +3301,10 @@ COMMAND_HELPER(xtensa_cmd_xtdef_do, struct xtensa *xtensa)
 	const char *core_name = CMD_ARGV[0];
 	if (strcasecmp(core_name, "LX") == 0) {
 		xtensa->core_config->core_type = XT_LX;
+	} else if (strcasecmp(core_name, "NX") == 0) {
+		xtensa->core_config->core_type = XT_NX;
 	} else {
-		LOG_ERROR("xtdef [LX]\n");
+		LOG_ERROR("xtdef [LX|NX]\n");
 		return ERROR_COMMAND_SYNTAX_ERROR;
 	}
 	return ERROR_OK;
@@ -3456,6 +3686,33 @@ COMMAND_HELPER(xtensa_cmd_xtreg_do, struct xtensa *xtensa)
 			xtensa->eps_dbglevel_idx = XT_NUM_REGS + xtensa->num_optregs - 1;
 			LOG_DEBUG("Setting PS (%s) index to %d", rptr->name, xtensa->eps_dbglevel_idx);
 		}
+		if (xtensa->core_config->core_type == XT_NX) {
+			enum xtensa_nx_reg_idx idx = XT_NX_REG_IDX_NUM;
+			if (strcmp(rptr->name, "ibreakc0") == 0)
+				idx = XT_NX_REG_IDX_IBREAKC0;
+			else if (strcmp(rptr->name, "wb") == 0)
+				idx = XT_NX_REG_IDX_WB;
+			else if (strcmp(rptr->name, "ms") == 0)
+				idx = XT_NX_REG_IDX_MS;
+			else if (strcmp(rptr->name, "ievec") == 0)
+				idx = XT_NX_REG_IDX_IEVEC;
+			else if (strcmp(rptr->name, "ieextern") == 0)
+				idx = XT_NX_REG_IDX_IEEXTERN;
+			else if (strcmp(rptr->name, "mesr") == 0)
+				idx = XT_NX_REG_IDX_MESR;
+			else if (strcmp(rptr->name, "mesrclr") == 0)
+				idx = XT_NX_REG_IDX_MESRCLR;
+			if (idx < XT_NX_REG_IDX_NUM) {
+				if (xtensa->nx_reg_idx[idx] != 0) {
+					LOG_ERROR("nx_reg_idx[%d] previously set to %d",
+						idx, xtensa->nx_reg_idx[idx]);
+					return ERROR_FAIL;
+				}
+				xtensa->nx_reg_idx[idx] = XT_NUM_REGS + xtensa->num_optregs - 1;
+				LOG_DEBUG("NX reg %s: index %d (%d)",
+					rptr->name, xtensa->nx_reg_idx[idx], idx);
+			}
+		}
 	} else if (strcmp(rptr->name, "cpenable") == 0) {
 		xtensa->core_config->coproc = true;
 	}
@@ -3640,6 +3897,12 @@ COMMAND_HELPER(xtensa_cmd_mask_interrupts_do, struct xtensa *xtensa)
 		command_print(CMD, "Current ISR step mode: %s", st);
 		return ERROR_OK;
 	}
+
+	if (xtensa->core_config->core_type == XT_NX) {
+		command_print(CMD, "ERROR: ISR step mode only supported on Xtensa LX");
+		return ERROR_FAIL;
+	}
+
 	/* Masking is ON -> interrupts during stepping are OFF, and vice versa */
 	if (!strcasecmp(CMD_ARGV[0], "off"))
 		state = XT_STEPPING_ISR_ON;
diff --git a/src/target/xtensa/xtensa.h b/src/target/xtensa/xtensa.h
index 4d98f3a..4216ae2 100644
--- a/src/target/xtensa/xtensa.h
+++ b/src/target/xtensa/xtensa.h
@@ -35,6 +35,7 @@
 
 #define XT_ISNS_SZ_MAX                          3
 
+/* PS register bits (LX) */
 #define XT_PS_RING(_v_)                         ((uint32_t)((_v_) & 0x3) << 6)
 #define XT_PS_RING_MSK                          (0x3 << 6)
 #define XT_PS_RING_GET(_v_)                     (((_v_) >> 6) & 0x3)
@@ -42,6 +43,31 @@
 #define XT_PS_OWB_MSK                           (0xF << 8)
 #define XT_PS_WOE_MSK                           BIT(18)
 
+/* PS register bits (NX) */
+#define XT_PS_DIEXC_MSK                         BIT(2)
+
+/* MS register bits (NX) */
+#define XT_MS_DE_MSK                            BIT(5)
+#define XT_MS_DISPST_MSK                        (0x1f)
+#define XT_MS_DISPST_DBG                        (0x10)
+
+/* WB register bits (NX) */
+#define XT_WB_P_SHIFT                           (0)
+#define XT_WB_P_MSK                             (0x7U << XT_WB_P_SHIFT)
+#define XT_WB_C_SHIFT                           (4)
+#define XT_WB_C_MSK                             (0x7U << XT_WB_C_SHIFT)
+#define XT_WB_N_SHIFT                           (8)
+#define XT_WB_N_MSK                             (0x7U << XT_WB_N_SHIFT)
+#define XT_WB_S_SHIFT                           (30)
+#define XT_WB_S_MSK                             (0x3U << XT_WB_S_SHIFT)
+
+/* IBREAKC register bits (NX) */
+#define XT_IBREAKC_FB                           (0x80000000)
+
+/* Definitions for imprecise exception registers (NX) */
+#define XT_IMPR_EXC_MSK                         (0x00000013)
+#define XT_MESRCLR_IMPR_EXC_MSK                 (0x00000090)
+
 #define XT_LOCAL_MEM_REGIONS_NUM_MAX            8
 
 #define XT_AREGS_NUM_MAX                        64
@@ -79,6 +105,7 @@ struct xtensa_keyval_info_s {
 enum xtensa_type {
 	XT_UNDEF = 0,
 	XT_LX,
+	XT_NX,
 };
 
 struct xtensa_cache_config {
@@ -167,6 +194,17 @@ enum xtensa_stepping_isr_mode {
 	XT_STEPPING_ISR_ON,		/* interrupts are enabled during stepping */
 };
 
+enum xtensa_nx_reg_idx {
+	XT_NX_REG_IDX_IBREAKC0 = 0,
+	XT_NX_REG_IDX_WB,
+	XT_NX_REG_IDX_MS,
+	XT_NX_REG_IDX_IEVEC,		/* IEVEC, IEEXTERN, and MESR must be contiguous */
+	XT_NX_REG_IDX_IEEXTERN,
+	XT_NX_REG_IDX_MESR,
+	XT_NX_REG_IDX_MESRCLR,
+	XT_NX_REG_IDX_NUM
+};
+
 /* Only supported in cores with in-CPU MMU. None of Espressif chips as of now. */
 enum xtensa_mode {
 	XT_MODE_RING0,
@@ -232,6 +270,8 @@ struct xtensa {
 	uint8_t come_online_probes_num;
 	bool proc_syscall;
 	bool halt_request;
+	uint32_t nx_stop_cause;
+	uint32_t nx_reg_idx[XT_NX_REG_IDX_NUM];
 	struct xtensa_keyval_info_s scratch_ars[XT_AR_SCRATCH_NUM];
 	bool regs_fetched;	/* true after first register fetch completed successfully */
 };
diff --git a/src/target/xtensa/xtensa_debug_module.h b/src/target/xtensa/xtensa_debug_module.h
index b382e03..46b2935 100644
--- a/src/target/xtensa/xtensa_debug_module.h
+++ b/src/target/xtensa/xtensa_debug_module.h
@@ -246,6 +246,7 @@ struct xtensa_dm_reg_offsets {
 #define OCDDCR_ENABLEOCD            BIT(0)
 #define OCDDCR_DEBUGINTERRUPT       BIT(1)
 #define OCDDCR_INTERRUPTALLCONDS    BIT(2)
+#define OCDDCR_STEPREQUEST          BIT(3)		/* NX only */
 #define OCDDCR_BREAKINEN            BIT(16)
 #define OCDDCR_BREAKOUTEN           BIT(17)
 #define OCDDCR_DEBUGSWACTIVE        BIT(20)
@@ -259,6 +260,8 @@ struct xtensa_dm_reg_offsets {
 #define OCDDSR_EXECBUSY             BIT(2)
 #define OCDDSR_EXECOVERRUN          BIT(3)
 #define OCDDSR_STOPPED              BIT(4)
+#define OCDDSR_STOPCAUSE            (0xF << 5)	/* NX only */
+#define OCDDSR_STOPCAUSE_SHIFT      (5)			/* NX only */
 #define OCDDSR_COREWROTEDDR         BIT(10)
 #define OCDDSR_COREREADDDR          BIT(11)
 #define OCDDSR_HOSTWROTEDDR         BIT(14)
@@ -275,12 +278,24 @@ struct xtensa_dm_reg_offsets {
 #define OCDDSR_BREAKINITI           BIT(26)
 #define OCDDSR_DBGMODPOWERON        BIT(31)
 
+/* NX stop cause */
+#define OCDDSR_STOPCAUSE_DI         (0)		/* Debug Interrupt */
+#define OCDDSR_STOPCAUSE_SS         (1)		/* Single-step completed */
+#define OCDDSR_STOPCAUSE_IB         (2)		/* HW breakpoint (IBREAKn match) */
+#define OCDDSR_STOPCAUSE_B1         (4)		/* SW breakpoint (BREAK.1 instruction) */
+#define OCDDSR_STOPCAUSE_BN         (5)		/* SW breakpoint (BREAK.N instruction) */
+#define OCDDSR_STOPCAUSE_B          (6)		/* SW breakpoint (BREAK instruction) */
+#define OCDDSR_STOPCAUSE_DB0        (8)		/* HW watchpoint (DBREAK0 match) */
+#define OCDDSR_STOPCAUSE_DB1        (9)		/* HW watchpoint (DBREAK0 match) */
+
+/* LX stop cause */
 #define DEBUGCAUSE_IC               BIT(0)	/* ICOUNT exception */
 #define DEBUGCAUSE_IB               BIT(1)	/* IBREAK exception */
 #define DEBUGCAUSE_DB               BIT(2)	/* DBREAK exception */
 #define DEBUGCAUSE_BI               BIT(3)	/* BREAK instruction encountered */
 #define DEBUGCAUSE_BN               BIT(4)	/* BREAK.N instruction encountered */
 #define DEBUGCAUSE_DI               BIT(5)	/* Debug Interrupt */
+#define DEBUGCAUSE_VALID            BIT(31)	/* Pseudo-value to trigger reread (NX only) */
 
 #define TRAXCTRL_TREN               BIT(0)	/* Trace enable. Tracing starts on 0->1 */
 #define TRAXCTRL_TRSTP              BIT(1)	/* Trace Stop. Make 1 to stop trace. */
diff --git a/tcl/board/at91sam9g20-ek.cfg b/tcl/board/at91sam9g20-ek.cfg
index a5831cd..4740471 100644
--- a/tcl/board/at91sam9g20-ek.cfg
+++ b/tcl/board/at91sam9g20-ek.cfg
@@ -7,10 +7,6 @@
 #												#
 #################################################################################################
 
-# FIXME use some standard target config, maybe create one from this
-#
-#	source [find target/...cfg]
-
 source [find target/at91sam9g20.cfg]
 
 set _FLASHTYPE nandflash_cs3
diff --git a/tcl/interface/ftdi/ashling-opella-ld-jtag.cfg b/tcl/interface/ftdi/ashling-opella-ld-jtag.cfg
index 6256aa0..6256aa0 100755..100644
--- a/tcl/interface/ftdi/ashling-opella-ld-jtag.cfg
+++ b/tcl/interface/ftdi/ashling-opella-ld-jtag.cfg
diff --git a/tcl/interface/ftdi/ashling-opella-ld-swd.cfg b/tcl/interface/ftdi/ashling-opella-ld-swd.cfg
index 4a4e4e0..4a4e4e0 100755..100644
--- a/tcl/interface/ftdi/ashling-opella-ld-swd.cfg
+++ b/tcl/interface/ftdi/ashling-opella-ld-swd.cfg
diff --git a/tcl/interface/raspberrypi-gpio-connector.cfg b/tcl/interface/raspberrypi-gpio-connector.cfg
new file mode 100644
index 0000000..eff73fc
--- /dev/null
+++ b/tcl/interface/raspberrypi-gpio-connector.cfg
@@ -0,0 +1,42 @@
+# SPDX-License-Identifier: GPL-2.0-or-later
+
+#
+# Config for Raspberry Pi GPIO header
+#
+# This is best used with a fast enough buffer but also
+# is suitable for direct connection if the target voltage
+# matches RPi's 3.3V and the cable is short enough.
+#
+# Do not forget the GND connection, e.g. pin 20 of the GPIO header.
+#
+
+# GPIO 25 (pin 22) previously used for TMS/SWDIO is pulled-down by default.
+# The JTAG/SWD specification requires pull-up at the target board
+# for either signal. Connecting the signal pulled-up on the target
+# to the pull-down on the adapter is not a good idea.
+# GPIO 8 is pulled-up by default.
+echo "Warn : TMS/SWDIO moved to GPIO 8 (pin 24). Check the wiring please!"
+
+# Each of the JTAG lines need a gpio number set: tck tms tdi tdo
+# Header pin numbers: 23 24 19 21
+adapter gpio tck -chip 0 11
+adapter gpio tms -chip 0 8
+adapter gpio tdi -chip 0 10
+adapter gpio tdo -chip 0 9
+
+# Each of the SWD lines need a gpio number set: swclk swdio
+# Header pin numbers: 23 24
+adapter gpio swclk -chip 0 11
+adapter gpio swdio -chip 0 8
+
+# If you define trst or srst, use appropriate reset_config
+# Header pin numbers: TRST - 26, SRST - 18
+
+# adapter gpio trst -chip 0 7
+# reset_config trst_only
+
+# adapter gpio srst -chip 0 24
+# reset_config srst_only srst_push_pull
+
+# or if you have both connected,
+# reset_config trst_and_srst srst_push_pull
diff --git a/tcl/interface/raspberrypi-native.cfg b/tcl/interface/raspberrypi-native.cfg
index 02a3563..7224723 100644
--- a/tcl/interface/raspberrypi-native.cfg
+++ b/tcl/interface/raspberrypi-native.cfg
@@ -1,44 +1,71 @@
 # SPDX-License-Identifier: GPL-2.0-or-later
 
-#
-# Config for using Raspberry Pi's expansion header
-#
-# This is best used with a fast enough buffer but also
-# is suitable for direct connection if the target voltage
-# matches RPi's 3.3V and the cable is short enough.
-#
-# Do not forget the GND connection, pin 6 of the expansion header.
-#
+# Config for Raspberry Pi used as a bitbang adapter.
+# https://www.raspberrypi.com/documentation/computers/raspberry-pi.html
+
+# Supports all models with 40-pin or 26-pin GPIO connector up to Raspberry Pi 4 B
+# also supports Raspberry Pi Zero, Zero W and Zero 2 W.
+
+# Adapter speed calibration is computed from cpufreq/scaling_max_freq.
+# Adjusts automatically if CPU is overclocked.
 
 adapter driver bcm2835gpio
 
-bcm2835gpio peripheral_base 0x20000000
+proc read_file { name } {
+	if {[catch {open $name r} fd]} {
+		return ""
+	}
+	set result [read $fd]
+	close $fd
+	return $result
+}
 
-# Transition delay calculation: SPEED_COEFF/khz - SPEED_OFFSET
-# These depend on system clock, calibrated for stock 700MHz
-# bcm2835gpio speed SPEED_COEFF SPEED_OFFSET
-bcm2835gpio speed_coeffs 113714 28
+proc measure_clock {} {
+	set result [exec vcgencmd measure_clock arm]
+	set clock_hz [lindex [split $result "="] 1]
+	expr { $clock_hz / 1000 }
+}
+
+proc get_max_cpu_clock { default } {
+	set clock [read_file /sys/devices/system/cpu/cpu0/cpufreq/scaling_max_freq]
+	if { $clock > 100000 } {
+		return $clock
+	}
 
-# Each of the JTAG lines need a gpio number set: tck tms tdi tdo
-# Header pin numbers: 23 22 19 21
-adapter gpio tck -chip 0 11
-adapter gpio tms -chip 0 25
-adapter gpio tdi -chip 0 10
-adapter gpio tdo -chip 0 9
+	# cpufreq not available. As the last resort try Broadcom's proprietary utility
+	if {![catch measure_clock clock] && $clock > 100000} {
+		return $clock
+	}
 
-# Each of the SWD lines need a gpio number set: swclk swdio
-# Header pin numbers: 23 22
-adapter gpio swclk -chip 0 11
-adapter gpio swdio -chip 0 25
+	echo "WARNING: Host CPU clock unknown."
+	echo "WARNING: Using the highest possible value $default kHz as a safe default."
+	echo "WARNING: Expect JTAG/SWD clock significantly slower than requested."
 
-# If you define trst or srst, use appropriate reset_config
-# Header pin numbers: TRST - 26, SRST - 18
+	return $default
+}
 
-# adapter gpio trst -chip 0 7
-# reset_config trst_only
+set compat [read_file /proc/device-tree/compatible]
+set clocks_per_timing_loop 4
 
-# adapter gpio srst -chip 0 24
-# reset_config srst_only srst_push_pull
+if {[string match *bcm2711* $compat]} {
+	set speed_offset 52
+} elseif {[string match *bcm2837* $compat] || [string match *bcm2710* $compat]} {
+	set speed_offset 34
+} elseif {[string match *bcm2836* $compat] || [string match *bcm2709* $compat]} {
+	set speed_offset 36
+} elseif {[string match *bcm2835* $compat] || [string match *bcm2708* $compat]} {
+	set clocks_per_timing_loop 6
+	set speed_offset 32
+} else {
+	set speed_offset 32
+	echo "WARNING: Unknown type of the host SoC. Expect JTAG/SWD clock slower than requested."
+}
+
+set clock [get_max_cpu_clock 2000000]
+set speed_coeff [expr { $clock / $clocks_per_timing_loop }]
+
+# Transition delay calculation: SPEED_COEFF/khz - SPEED_OFFSET
+# The coefficients depend on system clock and CPU frequency scaling.
+bcm2835gpio speed_coeffs $speed_coeff $speed_offset
 
-# or if you have both connected,
-# reset_config trst_and_srst srst_push_pull
+source [find interface/raspberrypi-gpio-connector.cfg]
diff --git a/tcl/interface/raspberrypi2-native.cfg b/tcl/interface/raspberrypi2-native.cfg
index d5edded..fe9186f 100644
--- a/tcl/interface/raspberrypi2-native.cfg
+++ b/tcl/interface/raspberrypi2-native.cfg
@@ -1,44 +1,6 @@
 # SPDX-License-Identifier: GPL-2.0-or-later
 
-#
-# Config for using Raspberry Pi's expansion header
-#
-# This is best used with a fast enough buffer but also
-# is suitable for direct connection if the target voltage
-# matches RPi's 3.3V and the cable is short enough.
-#
-# Do not forget the GND connection, pin 6 of the expansion header.
-#
+echo "WARNING: interface/raspberrypi2-native.cfg is deprecated."
+echo "WARNING: Please use interface/raspberrypi-native.cfg for all Raspberry Pi models."
 
-adapter driver bcm2835gpio
-
-bcm2835gpio peripheral_base 0x3F000000
-
-# Transition delay calculation: SPEED_COEFF/khz - SPEED_OFFSET
-# These depend on system clock, calibrated for scaling_max_freq 900MHz
-# bcm2835gpio speed SPEED_COEFF SPEED_OFFSET
-bcm2835gpio speed_coeffs 225000 36
-
-# Each of the JTAG lines need a gpio number set: tck tms tdi tdo
-# Header pin numbers: 23 22 19 21
-adapter gpio tck -chip 0 11
-adapter gpio tms -chip 0 25
-adapter gpio tdi -chip 0 10
-adapter gpio tdo -chip 0 9
-
-# Each of the SWD lines need a gpio number set: swclk swdio
-# Header pin numbers: 23 22
-adapter gpio swclk -chip 0 11
-adapter gpio swdio -chip 0 25
-
-# If you define trst or srst, use appropriate reset_config
-# Header pin numbers: TRST - 26, SRST - 18
-
-# adapter gpio trst -chip 0 7
-# reset_config trst_only
-
-# adapter gpio srst -chip 0 24
-# reset_config srst_only srst_push_pull
-
-# or if you have both connected,
-# reset_config trst_and_srst srst_push_pull
+source [find interface/raspberrypi-native.cfg]
diff --git a/tcl/target/rp2040.cfg b/tcl/target/rp2040.cfg
index 0593e03..de76b4e 100644
--- a/tcl/target/rp2040.cfg
+++ b/tcl/target/rp2040.cfg
@@ -26,6 +26,13 @@ if { [info exists CPUTAPID] } {
 	set _CPUTAPID 0x01002927
 }
 
+# Set to '1' to start rescue mode
+if { [info exists RESCUE] } {
+	set _RESCUE $RESCUE
+} else {
+	set _RESCUE 0
+}
+
 # Set to '0' or '1' for single core configuration, 'SMP' for -rtos hwthread
 # handling of both cores, anything else for isolated debugging of both cores
 if { [info exists USE_CORE] } {
@@ -37,6 +44,29 @@ set _BOTH_CORES [expr { $_USE_CORE != 0 && $_USE_CORE != 1 }]
 
 swj_newdap $_CHIPNAME cpu -expected-id $_CPUTAPID
 
+# The rescue debug port uses the DP CTRL/STAT bit DBGPWRUPREQ to reset the
+# PSM (power on state machine) of the RP2040 with a flag set in the
+# VREG_AND_POR_CHIP_RESET register. Once the reset is released
+# (by clearing the DBGPWRUPREQ flag), the bootrom will run, see this flag,
+# and halt. Allowing the user to load some fresh code, rather than loading
+# the potentially broken code stored in flash
+if { $_RESCUE } {
+	dap create $_CHIPNAME.rescue_dap -chain-position $_CHIPNAME.cpu -dp-id $_CPUTAPID -instance-id 0xf -ignore-syspwrupack
+	init
+
+	# Clear DBGPWRUPREQ
+	$_CHIPNAME.rescue_dap dpreg 0x4 0x00000000
+
+	# Verifying CTRL/STAT is 0
+	set _CTRLSTAT [$_CHIPNAME.rescue_dap dpreg 0x4]
+	if {[expr {$_CTRLSTAT & 0xf0000000}]} {
+		echo "Rescue failed, DP CTRL/STAT readback $_CTRLSTAT"
+	} else {
+		echo "Now restart OpenOCD without RESCUE flag and load code to RP2040"
+	}
+	shutdown
+}
+
 # core 0
 if { $_USE_CORE != 1 } {
 	dap create $_CHIPNAME.dap0 -chain-position $_CHIPNAME.cpu -dp-id $_CPUTAPID -instance-id 0
diff --git a/tcl/tools/test_cpu_speed.tcl b/tcl/tools/test_cpu_speed.tcl
index cef2bbb..f1a3fb3 100644
--- a/tcl/tools/test_cpu_speed.tcl
+++ b/tcl/tools/test_cpu_speed.tcl
@@ -18,7 +18,7 @@ proc cortex_m_test_cpu_speed { address { timeout 200 } { cycles_per_loop 4 } } {
 	halt
 
 	# Backup registers and memory.
-	set backup_regs [get_reg -force {pc r0 xPSR}]
+	set backup_regs [get_reg -force {pc r0 xpsr}]
 	set backup_mem [read_memory $address 16 3]
 
 	# We place the following code at the given address to measure the