Merge commit 'd1b882f2c014258be5397067e45848fa5465b78b' into from_upstream

Conflicts:
	doc/openocd.texi
	src/target/riscv/riscv-013.c
	src/target/riscv/riscv.c

Change-Id: I8cd557a10c3d5beeaed05ecc05d4c325a9ee7e70

62 files changed, 7137 insertions, 439 deletions

diff --git a/.github/workflows/snapshot.yml b/.github/workflows/snapshot.yml
index 972011c..9d356ae 100644
--- a/.github/workflows/snapshot.yml
+++ b/.github/workflows/snapshot.yml
@@ -22,7 +22,7 @@ jobs:
       - run: ./bootstrap
       - name: Prepare libusb1
         env:
-          LIBUSB1_VER: 1.0.24
+          LIBUSB1_VER: 1.0.26
         run: |
           mkdir -p $DL_DIR && cd $DL_DIR
           wget "https://github.com/libusb/libusb/releases/download/v${LIBUSB1_VER}/libusb-${LIBUSB1_VER}.tar.bz2"
@@ -30,7 +30,7 @@ jobs:
           echo "LIBUSB1_SRC=$PWD/libusb-${LIBUSB1_VER}" >> $GITHUB_ENV
       - name: Prepare hidapi
         env:
-          HIDAPI_VER: 0.10.1
+          HIDAPI_VER: 0.11.2
         run: |
           mkdir -p $DL_DIR && cd $DL_DIR
           wget "https://github.com/libusb/hidapi/archive/hidapi-${HIDAPI_VER}.tar.gz"
diff --git a/.gitignore b/.gitignore
index 955ca3c..f5aa68a 100644
--- a/.gitignore
+++ b/.gitignore
@@ -68,6 +68,7 @@ doxygen
 doxygen.log
 Doxyfile
 libtool
+*-libtool
 Makefile
 !contrib/loaders/**/Makefile
 stamp-h1
diff --git a/HACKING b/HACKING
index 1eeb1a2..5d110fb 100644
--- a/HACKING
+++ b/HACKING
@@ -197,11 +197,11 @@ while(!done) {
 @endcode
    \note use "git add ." before commit to add new files.
 
-   Comment template, notice the short first line w/topic. The topic field
-   should identify the main part or subsystem the patch touches. Check
-   git log for examples.
-@code
-topic: Short comment
+   Commit message template, notice the short first line.
+   The field '<c>specify touched area</c>'
+   should identify the main part or subsystem the patch touches.
+@code{.unparsed}
+specify touched area: short comment
 <blank line>
 Longer comments over several lines, explaining (where applicable) the
 reason for the patch and the general idea the solution is based on,
@@ -209,6 +209,32 @@ any major design decisions, etc...
 <blank line>
 Signed-off-by: ...
 @endcode
+   Examples:
+@code{.unparsed}
+flash/nor/atsame5: add SAME59 support
+
+Add new device ID
+@endcode
+@code{.unparsed}
+flash/nor: flash driver for XYZ123
+
+Add new flash driver for internal flash of ...
+@endcode
+@code{.unparsed}
+target/cortex_m: fix segmentation fault in cmd 'soft_reset_halt'
+
+soft_reset_halt command failed reproducibly under following conditions: ...
+Test for NULL pointer and return error ...
+
+Reported-by: John Reporter <rep9876@gmail.com>
+Fixes: 123456789abc ("target: the commit where the problem started")
+BugLink: https://sourceforge.net/p/openocd/tickets/999/
+@endcode
+@code{.unparsed}
+doc: fix typos
+@endcode
+   See "git log" for more examples.
+
 -# Next you need to make sure that your patches
    are on top of the latest stuff on the server and
    that there are no conflicts:
diff --git a/configure.ac b/configure.ac
index 081e774..687fffa 100644
--- a/configure.ac
+++ b/configure.ac
@@ -299,10 +299,14 @@ AS_CASE(["${host_cpu}"],
     AC_ARG_ENABLE([imx_gpio],
       AS_HELP_STRING([--enable-imx_gpio], [Enable building support for bitbanging on NXP IMX processors]),
       [build_imx_gpio=$enableval], [build_imx_gpio=no])
+    AC_ARG_ENABLE([am335xgpio],
+      AS_HELP_STRING([--enable-am335xgpio], [Enable building support for bitbanging on AM335x (as found in Beaglebones)]),
+      [build_am335xgpio=$enableval], [build_am335xgpio=no])
   ],
   [
     build_bcm2835gpio=no
     build_imx_gpio=no
+    build_am335xgpio=no
 ])
 
 AS_CASE(["${host_cpu}"],
@@ -508,6 +512,13 @@ AS_IF([test "x$build_imx_gpio" = "xyes"], [
   AC_DEFINE([BUILD_IMX_GPIO], [0], [0 if you don't want imx_gpio.])
 ])
 
+AS_IF([test "x$build_am335xgpio" = "xyes"], [
+  build_bitbang=yes
+  AC_DEFINE([BUILD_AM335XGPIO], [1], [1 if you want am335xgpio.])
+], [
+  AC_DEFINE([BUILD_AM335XGPIO], [0], [0 if you don't want am335xgpio.])
+])
+
 AS_IF([test "x$parport_use_ppdev" = "xyes"], [
   AC_DEFINE([PARPORT_USE_PPDEV], [1], [1 if you want parport to use ppdev.])
 ], [
@@ -560,9 +571,9 @@ AS_IF([test "x$enable_buspirate" != "xno"], [
 AS_IF([test "x$use_internal_jimtcl" = "xyes"], [
   AS_IF([test -f "$srcdir/jimtcl/configure.ac"], [
     AS_IF([test "x$use_internal_jimtcl_maintainer" = "xyes"], [
-      jimtcl_config_options="--disable-install-jim --maintainer"
+      jimtcl_config_options="--disable-install-jim --with-ext=json --maintainer"
     ], [
-      jimtcl_config_options="--disable-install-jim"
+      jimtcl_config_options="--disable-install-jim --with-ext=json"
     ])
     AX_CONFIG_SUBDIR_OPTION([jimtcl], [$jimtcl_config_options])
   ], [
@@ -712,6 +723,7 @@ AM_CONDITIONAL([EP93XX], [test "x$build_ep93xx" = "xyes"])
 AM_CONDITIONAL([AT91RM9200], [test "x$build_at91rm9200" = "xyes"])
 AM_CONDITIONAL([BCM2835GPIO], [test "x$build_bcm2835gpio" = "xyes"])
 AM_CONDITIONAL([IMX_GPIO], [test "x$build_imx_gpio" = "xyes"])
+AM_CONDITIONAL([AM335XGPIO], [test "x$build_am335xgpio" = "xyes"])
 AM_CONDITIONAL([BITBANG], [test "x$build_bitbang" = "xyes"])
 AM_CONDITIONAL([JTAG_VPI], [test "x$build_jtag_vpi" = "xyes"])
 AM_CONDITIONAL([VDEBUG], [test "x$build_vdebug" = "xyes"])
diff --git a/contrib/cross-build.sh b/contrib/cross-build.sh
index 856551d..2275f68 100755
--- a/contrib/cross-build.sh
+++ b/contrib/cross-build.sh
@@ -154,7 +154,7 @@ if [ -d $CAPSTONE_SRC ] ; then
   sed -i '1s;^;prefix=/usr \
 exec_prefix=${prefix} \
 libdir=${exec_prefix}/lib \
-includedir=${prefix}/include\n\n;' $CAPSTONE_PC_FILE
+includedir=${prefix}/include/capstone\n\n;' $CAPSTONE_PC_FILE
 fi
 
 
diff --git a/doc/openocd.texi b/doc/openocd.texi
index a5d561c..99cae9a 100644
--- a/doc/openocd.texi
+++ b/doc/openocd.texi
@@ -584,6 +584,9 @@ produced, PDF schematics are easily found and it is easy to make.
 @item @b{imx_gpio}
 @* A NXP i.MX-based board (e.g. Wandboard) using the GPIO pins (should work on any i.MX processor).
 
+@item @b{am335xgpio}
+@* A Texas Instruments AM335x-based board (e.g. BeagleBone Black) using the GPIO pins of the expansion headers.
+
 @item @b{jtag_vpi}
 @* A JTAG driver acting as a client for the JTAG VPI server interface.
 @* Link: @url{http://github.com/fjullien/jtag_vpi}
@@ -2120,6 +2123,15 @@ corresponding subsystems:
 @deffnx {Config Command} {pld init}
 @deffnx {Command} {tpiu init}
 @end deffn
+
+At last, @command{init} executes all the commands that are specified in
+the TCL list @var{post_init_commands}. The commands are executed in the
+same order they occupy in the list. If one of the commands fails, then
+the error is propagated and OpenOCD fails too.
+@example
+lappend post_init_commands @{echo "OpenOCD successfully initialized."@}
+lappend post_init_commands @{echo "Have fun with OpenOCD !"@}
+@end example
 @end deffn
 
 @deffn {Config Command} {noinit}
@@ -3342,11 +3354,180 @@ pinout.
 @end deffn
 
 
+@deffn {Interface Driver} {am335xgpio} The AM335x SoC is present in BeagleBone
+Black and BeagleBone Green single-board computers which expose some of the GPIOs
+on the two expansion headers.
+
+For maximum performance the driver accesses memory-mapped GPIO peripheral
+registers directly. The memory mapping requires read and write permission to
+kernel memory; if /dev/gpiomem exists it will be used, otherwise /dev/mem will
+be used. The driver restores the GPIO state on exit.
+
+All four GPIO ports are available. GPIOs numbered 0 to 31 are mapped to GPIO port
+0, GPIO numbers 32 to 63 are mapped to GPIO port 1 and so on.
+
+See @file{interface/beaglebone-swd-native.cfg} for a sample configuration file.
+
+@deffn {Config Command} {am335xgpio jtag_nums} @var{tck} @var{tms} @var{tdi} @var{tdo}
+Set JTAG transport GPIO numbers for TCK, TMS, TDI, and TDO (in that order).
+Must be specified to enable JTAG transport. These pins can also be specified
+individually.
+@end deffn
+
+@deffn {Config Command} {am335xgpio tck_num} @var{tck}
+Set TCK GPIO number. Must be specified to enable JTAG transport. Can also be
+specified using the configuration command @command{am335xgpio jtag_nums}.
+@end deffn
+
+@deffn {Config Command} {am335xgpio tms_num} @var{tms}
+Set TMS GPIO number. Must be specified to enable JTAG transport. Can also be
+specified using the configuration command @command{am335xgpio jtag_nums}.
+@end deffn
+
+@deffn {Config Command} {am335xgpio tdo_num} @var{tdo}
+Set TDO GPIO number. Must be specified to enable JTAG transport. Can also be
+specified using the configuration command @command{am335xgpio jtag_nums}.
+@end deffn
+
+@deffn {Config Command} {am335xgpio tdi_num} @var{tdi}
+Set TDI GPIO number. Must be specified to enable JTAG transport. Can also be
+specified using the configuration command @command{am335xgpio jtag_nums}.
+@end deffn
+
+@deffn {Config Command} {am335xgpio swd_nums} @var{swclk} @var{swdio}
+Set SWD transport GPIO numbers for SWCLK and SWDIO (in that order). Must be
+specified to enable SWD transport. These pins can also be specified individually.
+@end deffn
+
+@deffn {Config Command} {am335xgpio swclk_num} @var{swclk}
+Set SWCLK GPIO number. Must be specified to enable SWD transport. Can also be
+specified using the configuration command @command{am335xgpio swd_nums}.
+@end deffn
+
+@deffn {Config Command} {am335xgpio swdio_num} @var{swdio}
+Set SWDIO GPIO number. Must be specified to enable SWD transport. Can also be
+specified using the configuration command @command{am335xgpio swd_nums}.
+@end deffn
+
+@deffn {Config Command} {am335xgpio swdio_dir_num} @var{swdio_dir}
+Set SWDIO direction control pin GPIO number. If specified, this pin can be used
+to control the direction of an external buffer on the SWDIO pin. The direction
+control state can be set with the command @command{am335xgpio
+swdio_dir_output_state}. If not specified this feature is disabled.
+@end deffn
+
+@deffn {Config Command} {am335xgpio swdio_dir_output_state} @var{output_state}
+Set the state required for an external SWDIO buffer to be an output. Valid
+values are @option{on} (default) and @option{off}.
+@end deffn
+
+@deffn {Config Command} {am335xgpio srst_num} @var{srst}
+Set SRST GPIO number. Must be specified to enable SRST.
+@end deffn
+
+@deffn {Config Command} {am335xgpio trst_num} @var{trst}
+Set TRST GPIO number. Must be specified to enable TRST.
+@end deffn
+
+@deffn {Config Command} {am335xgpio led_num} @var{led}
+Set activity LED GPIO number. If not specified an activity LED is not enabled.
+@end deffn
+
+@deffn {Config Command} {am335xgpio led_on_state} @var{on_state}
+Set required logic level for the LED to be on. Valid values are @option{on}
+(default) and @option{off}.
+@end deffn
+
+@deffn {Config Command} {am335xgpio speed_coeffs} @var{speed_coeff} @var{speed_offset}
+Set SPEED_COEFF and SPEED_OFFSET for delay calculations. If unspecified
+speed_coeff defaults to 600000 and speed_offset defaults to 575.
+@end deffn
+
+@end deffn
+
+
 @deffn {Interface Driver} {linuxgpiod}
 Linux provides userspace access to GPIO through libgpiod since Linux kernel version v4.6.
-The driver emulates either JTAG and SWD transport through bitbanging.
+The driver emulates either JTAG or SWD transport through bitbanging.
 
 See @file{interface/dln-2-gpiod.cfg} for a sample config.
+
+@deffn {Config Command} {linuxgpiod gpiochip} @var{chip}
+Set the GPIO chip number for all GPIOs used by linuxgpiod. If GPIOs use
+different GPIO chips then the individual GPIO configuration commands (i.e., not
+@command{linuxgpiod jtag_nums} or @command{linuxgpiod swd_nums}) can be used to
+set chip numbers independently for each GPIO.
+@end deffn
+
+@deffn {Config Command} {linuxgpiod jtag_nums} @var{tck} @var{tms} @var{tdi} @var{tdo}
+Set JTAG transport GPIO numbers for TCK, TMS, TDI, and TDO (in that order). Must
+be specified to enable JTAG transport. These pins can also be specified
+individually.
+@end deffn
+
+@deffn {Config Command} {linuxgpiod tck_num} [@var{chip}] @var{tck}
+Set TCK GPIO number, and optionally TCK chip number. Must be specified to enable
+JTAG transport. Can also be specified using the configuration command
+@command{linuxgpiod jtag_nums}.
+@end deffn
+
+@deffn {Config Command} {linuxgpiod tms_num} [@var{chip}] @var{tms}
+Set TMS GPIO number, and optionally TMS chip number. Must be specified to enable
+JTAG transport. Can also be specified using the configuration command
+@command{linuxgpiod jtag_nums}.
+@end deffn
+
+@deffn {Config Command} {linuxgpiod tdo_num} [@var{chip}] @var{tdo}
+Set TDO GPIO number, and optionally TDO chip number. Must be specified to enable
+JTAG transport. Can also be specified using the configuration command
+@command{linuxgpiod jtag_nums}.
+@end deffn
+
+@deffn {Config Command} {linuxgpiod tdi_num} [@var{chip}] @var{tdi}
+Set TDI GPIO number, and optionally TDI chip number. Must be specified to enable
+JTAG transport. Can also be specified using the configuration command
+@command{linuxgpiod jtag_nums}.
+@end deffn
+
+@deffn {Config Command} {linuxgpiod trst_num} [@var{chip}] @var{trst}
+Set TRST GPIO number, and optionally TRST chip number. Must be specified to
+enable TRST.
+@end deffn
+
+@deffn {Config Command} {linuxgpiod swd_nums} @var{swclk} @var{swdio}
+Set SWD transport GPIO numbers for SWCLK and SWDIO (in that order). Must be
+specified to enable SWD transport. These pins can also be specified
+individually.
+@end deffn
+
+@deffn {Config Command} {linuxgpiod swclk_num} [@var{chip}] @var{swclk}
+Set SWCLK GPIO number, and optionally SWCLK chip number. Must be specified to
+enable SWD transport. Can also be specified using the configuration command
+@command{linuxgpiod swd_nums}.
+@end deffn
+
+@deffn {Config Command} {linuxgpiod swdio_num} [@var{chip}] @var{swdio}
+Set SWDIO GPIO number, and optionally SWDIO chip number. Must be specified to
+enable SWD transport. Can also be specified using the configuration command
+@command{linuxgpiod swd_nums}.
+@end deffn
+
+@deffn {Config Command} {linuxgpiod swdio_dir_num} [@var{chip}] @var{swdio_dir}
+Set SWDIO direction control GPIO number, and optionally SWDIO direction control
+chip number. If specified, this GPIO can be used to control the direction of an
+external buffer connected to the SWDIO GPIO (set=output mode, clear=input mode).
+@end deffn
+
+@deffn {Config Command} {linuxgpiod srst_num} [@var{chip}] @var{srst}
+Set SRST GPIO number, and optionally SRST chip number. Must be specified to
+enable SRST.
+@end deffn
+
+@deffn {Config Command} {linuxgpiod led_num} [@var{chip}] @var{led}
+Set activity LED GPIO number, and optionally activity LED chip number. If not
+specified an activity LED is not enabled.
+@end deffn
+
 @end deffn
 
 
@@ -4745,6 +4926,7 @@ compact Thumb2 instruction set. Supports also ARMv6-M and ARMv8-M cores
 The current implementation supports eSi-32xx cores.
 @item @code{esp32c2} -- this is an Espressif SoC with single RISC-V core.
 @item @code{esp32c3} -- this is an Espressif SoC with single RISC-V core.
+@item @code{esp32s2} -- this is an Espressif SoC with single Xtensa core.
 @item @code{fa526} -- resembles arm920 (w/o Thumb).
 @item @code{feroceon} -- resembles arm926.
 @item @code{hla_target} -- a Cortex-M alternative to work with HL adapters like ST-Link.
@@ -6449,7 +6631,7 @@ The AVR 8-bit microcontrollers from Atmel integrate flash memory.
 @end deffn
 
 @deffn {Flash Driver} {bluenrg-x}
-STMicroelectronics BlueNRG-1, BlueNRG-2 and BlueNRG-LP Bluetooth low energy wireless system-on-chip. They include ARM Cortex-M0/M0+ core and internal flash memory.
+STMicroelectronics BlueNRG-1, BlueNRG-2 and BlueNRG-LP/LPS Bluetooth low energy wireless system-on-chip. They include ARM Cortex-M0/M0+ core and internal flash memory.
 The driver automatically recognizes these chips using
 the chip identification registers, and autoconfigures itself.
 
@@ -8464,18 +8646,19 @@ Close the OpenOCD server, disconnecting all clients (GDB, telnet,
 other). If option @option{error} is used, OpenOCD will return a
 non-zero exit code to the parent process.
 
-Like any TCL commands, also @command{shutdown} can be redefined, e.g.:
+If user types CTRL-C or kills OpenOCD, the command @command{shutdown}
+will be automatically executed to cause OpenOCD to exit.
+
+It is possible to specify, in the TCL list @var{pre_shutdown_commands} , a
+set of commands to be automatically executed before @command{shutdown} , e.g.:
 @example
-# redefine shutdown
-rename shutdown original_shutdown
-proc shutdown @{@} @{
-    puts "This is my implementation of shutdown"
-    # my own stuff before exit OpenOCD
-    original_shutdown
-@}
+lappend pre_shutdown_commands @{echo "Goodbye, my friend ..."@}
+lappend pre_shutdown_commands @{echo "see you soon !"@}
 @end example
-If user types CTRL-C or kills OpenOCD, either the command @command{shutdown}
-or its replacement will be automatically executed before OpenOCD exits.
+The commands in the list will be executed (in the same order they occupy
+in the list) before OpenOCD exits. If one of the commands in the list
+fails, then the remaining commands are not executed anymore while OpenOCD
+will proceed to quit.
 @end deffn
 
 @anchor{debuglevel}
@@ -9473,6 +9656,12 @@ is valid during the run of the event handlers and is accessible with this
 command.
 @end deffn
 
+@deffn {Command} {arm semihosting_basedir} [dir]
+@cindex ARM semihosting
+Set the base directory for semihosting I/O, either an absolute path or a path relative to OpenOCD working directory.
+Use "." for the current directory.
+@end deffn
+
 @section ARMv4 and ARMv5 Architecture
 @cindex ARMv4
 @cindex ARMv5
@@ -10956,6 +11145,94 @@ STMicroelectronics, based on a proprietary 8-bit core architecture.
 OpenOCD supports debugging STM8 through the STMicroelectronics debug
 protocol SWIM, @pxref{swimtransport,,SWIM}.
 
+@section Xtensa Architecture
+Xtensa processors are based on a modular, highly flexible 32-bit RISC architecture
+that can easily scale from a tiny, cache-less controller or task engine to a high-performance
+SIMD/VLIW DSP provided by Cadence.
+@url{https://www.cadence.com/en_US/home/tools/ip/tensilica-ip/tensilica-xtensa-controllers-and-extensible-processors.html}.
+
+OpenOCD supports generic Xtensa processors implementation which can be customized by
+simply providing vendor-specific core configuration which controls every configurable
+Xtensa architecture option, e.g. number of address registers, exceptions, reduced
+size instructions support, memory banks configuration etc. Also OpenOCD supports SMP
+configurations for Xtensa processors with any number of cores and allows to configure
+their debug signals interconnection (so-called "break/stall networks") which control how
+debug signals are distributed among cores. Xtensa "break networks" are compatible with
+ARM's Cross Trigger Interface (CTI). For debugging code on Xtensa chips OpenOCD
+uses JTAG protocol. Currently OpenOCD implements several Epsressif Xtensa-based chips of
+@uref{https://www.espressif.com/en/products/socs, ESP32 family}.
+
+@subsection General Xtensa Commands
+
+@deffn {Command} {xtensa set_permissive} (0|1)
+By default accessing memory beyond defined regions is forbidden. This commnd controls memory access address check.
+When set to (1), skips access controls and address range check before read/write memory.
+@end deffn
+
+@deffn {Command} {xtensa maskisr} (on|off)
+Selects whether interrupts will be disabled during stepping over single instruction. The default configuration is (off).
+@end deffn
+
+@deffn {Command} {xtensa smpbreak} [none|breakinout|runstall] | [BreakIn] [BreakOut] [RunStallIn] [DebugModeOut]
+Configures debug signals connection ("break network") for currently selected core.
+@itemize @bullet
+@item @code{none} - Core's "break/stall network" is disconnected. Core is not affected by any debug
+signal from other cores.
+@item @code{breakinout} - Core's "break network" is fully connected (break inputs and outputs are enabled).
+Core will receive debug break signals from other cores and send such signals to them. For example when another core
+is stopped due to breakpoint hit this core will be stopped too and vice versa.
+@item @code{runstall} - Core's "stall network" is fully connected (stall inputs and outputs are enabled).
+This feature is not well implemented and tested yet.
+@item @code{BreakIn} - Core's "break-in" signal is enabled.
+Core will receive debug break signals from other cores. For example when another core is
+stopped due to breakpoint hit this core will be stopped too.
+@item @code{BreakOut} - Core's "break-out" signal is enabled.
+Core will send debug break signal to other cores. For example when this core is
+stopped due to breakpoint hit other cores with enabled break-in signals will be stopped too.
+@item @code{RunStallIn} - Core's "runstall-in" signal is enabled.
+This feature is not well implemented and tested yet.
+@item @code{DebugModeOut} - Core's "debugmode-out" signal is enabled.
+This feature is not well implemented and tested yet.
+@end itemize
+@end deffn
+
+@deffn {Command} {xtensa perfmon_enable} <counter_id> <select> [mask] [kernelcnt] [tracelevel]
+Enable and start performance counter.
+@itemize @bullet
+@item @code{counter_id} - Counter ID (0-1).
+@item @code{select} - Selects performance metric to be counted by the counter,
+e.g. 0 - CPU cycles, 2 - retired instructions.
+@item @code{mask} - Selects input subsets to be counted (counter will
+increment only once even if more than one condition corresponding to a mask bit occurs).
+@item @code{kernelcnt} - 0 - count events with "CINTLEVEL <= tracelevel",
+1 - count events with "CINTLEVEL > tracelevel".
+@item @code{tracelevel} - Compares this value to "CINTLEVEL" when deciding
+whether to count.
+@end itemize
+@end deffn
+
+@deffn {Command} {xtensa perfmon_dump} (counter_id)
+Dump performance counter value. If no argument specified, dumps all counters.
+@end deffn
+
+@deffn {Command} {xtensa tracestart} [pc <pcval>/[<maskbitcount>]] [after <n> [ins|words]]
+Set up and start a HW trace. Optionally set PC address range to trigger tracing stop when reached during program execution.
+This command also allows to specify the amount of data to capture after stop trigger activation.
+@itemize @bullet
+@item @code{pcval} - PC value which will trigger trace data collection stop.
+@item @code{maskbitcount} - PC value mask.
+@item @code{n} - Maximum number of instructions/words to capture after trace stop trigger.
+@end itemize
+@end deffn
+
+@deffn {Command} {xtensa tracestop}
+Stop current trace as started by the tracestart command.
+@end deffn
+
+@deffn {Command} {xtensa tracedump} <outfile>
+Dump trace memory to a file.
+@end deffn
+
 @anchor{softwaredebugmessagesandtracing}
 @section Software Debug Messages and Tracing
 @cindex Linux-ARM DCC support
@@ -11756,57 +12033,6 @@ The @command{step} and @command{stepi} commands can be used to step a specific c
 while other cores are free-running or remain halted, depending on the
 scheduler-locking mode configured in GDB.
 
-@section Legacy SMP core switching support
-@quotation Note
-This method is deprecated in favor of the @emph{hwthread} pseudo RTOS.
-@end quotation
-
-For SMP support following GDB serial protocol packet have been defined :
-@itemize @bullet
-@item j - smp status request
-@item J - smp set request
-@end itemize
-
-OpenOCD implements :
-@itemize @bullet
-@item @option{jc} packet for reading core id displayed by
-GDB connection. Reply is @option{XXXXXXXX} (8 hex digits giving core id) or
- @option{E01} for target not smp.
-@item @option{JcXXXXXXXX} (8 hex digits) packet for setting core id displayed at next GDB continue
-(core id -1 is reserved for returning to normal resume mode). Reply @option{E01}
-for target not smp or @option{OK} on success.
-@end itemize
-
-Handling of this packet within GDB can be done :
-@itemize @bullet
-@item by the creation of an internal variable (i.e @option{_core}) by mean
-of function allocate_computed_value allowing following GDB command.
-@example
-set $_core 1
-#Jc01 packet is sent
-print $_core
-#jc packet is sent and result is affected in $
-@end example
-
-@item by the usage of GDB maintenance command as described in following example (2 cpus in SMP with
-core id 0 and 1 @pxref{definecputargetsworkinginsmp,,Define CPU targets working in SMP}).
-
-@example
-# toggle0 : force display of coreid 0
-define toggle0
-maint packet Jc0
-continue
-main packet Jc-1
-end
-# toggle1 : force display of coreid 1
-define toggle1
-maint packet Jc1
-continue
-main packet Jc-1
-end
-@end example
-@end itemize
-
 @node Tcl Scripting API
 @chapter Tcl Scripting API
 @cindex Tcl Scripting API
@@ -12374,7 +12600,7 @@ It sort of works like this:
 When the command ``proc'' is parsed (which creates a procedure
 function) it gets 3 parameters on the command line. @b{1} the name of
 the proc (function), @b{2} the list of parameters, and @b{3} the body
-of the function. Not the choice of words: LIST and BODY. The PROC
+of the function. Note the choice of words: LIST and BODY. The PROC
 command stores these items in a table somewhere so it can be found by
 ``LookupCommand()''
 
diff --git a/src/flash/nor/bluenrg-x.c b/src/flash/nor/bluenrg-x.c
index 60eccef..16075ec 100644
--- a/src/flash/nor/bluenrg-x.c
+++ b/src/flash/nor/bluenrg-x.c
@@ -35,6 +35,8 @@
 #define JTAG_IDCODE_REG(bluenrgx_info)      (bluenrgx_info->flash_ptr->jtag_idcode_reg)
 #define FLASH_PAGE_SIZE(bluenrgx_info)      (bluenrgx_info->flash_ptr->flash_page_size)
 
+#define FLASH_SIZE_REG_MASK (0xFFFF)
+
 struct flash_ctrl_priv_data {
 	uint32_t die_id_reg;
 	uint32_t jtag_idcode_reg;
@@ -75,6 +77,16 @@ static const struct flash_ctrl_priv_data flash_priv_data_lp = {
 	.part_name = "BLUENRG-LP",
 };
 
+static const struct flash_ctrl_priv_data flash_priv_data_lps = {
+	.die_id_reg = 0x40000000,
+	.jtag_idcode_reg = 0x40000004,
+	.flash_base = 0x10040000,
+	.flash_regs_base = 0x40001000,
+	.flash_page_size = 2048,
+	.jtag_idcode = 0x02028041,
+	.part_name = "BLUENRG-LPS",
+};
+
 struct bluenrgx_flash_bank {
 	bool probed;
 	uint32_t die_id;
@@ -84,8 +96,8 @@ struct bluenrgx_flash_bank {
 static const struct flash_ctrl_priv_data *flash_ctrl[] = {
 	&flash_priv_data_1,
 	&flash_priv_data_2,
-	&flash_priv_data_lp
-};
+	&flash_priv_data_lp,
+	&flash_priv_data_lps};
 
 /* flash_bank bluenrg-x 0 0 0 0 <target#> */
 FLASH_BANK_COMMAND_HANDLER(bluenrgx_flash_bank_command)
@@ -377,7 +389,7 @@ static int bluenrgx_probe(struct flash_bank *bank)
 	if (retval != ERROR_OK)
 		return retval;
 
-	if (idcode != flash_priv_data_lp.jtag_idcode) {
+	if ((idcode != flash_priv_data_lp.jtag_idcode) && (idcode != flash_priv_data_lps.jtag_idcode)) {
 		retval = target_read_u32(bank->target, BLUENRG2_JTAG_REG, &idcode);
 		if (retval != ERROR_OK)
 			return retval;
@@ -395,6 +407,7 @@ static int bluenrgx_probe(struct flash_bank *bank)
 		}
 	}
 	retval = bluenrgx_read_flash_reg(bank, FLASH_SIZE_REG, &size_info);
+	size_info = size_info & FLASH_SIZE_REG_MASK;
 	if (retval != ERROR_OK)
 		return retval;
 
diff --git a/src/flash/nor/core.c b/src/flash/nor/core.c
index 30d387a..f4ff5df 100644
--- a/src/flash/nor/core.c
+++ b/src/flash/nor/core.c
@@ -429,7 +429,11 @@ int default_flash_blank_check(struct flash_bank *bank)
 			bank->sectors[i].is_erased = block_array[i].result;
 		retval = ERROR_OK;
 	} else {
-		LOG_USER("Running slow fallback erase check - add working memory");
+		if (retval == ERROR_NOT_IMPLEMENTED)
+			LOG_USER("Running slow fallback erase check");
+		else
+			LOG_USER("Running slow fallback erase check - add working memory");
+
 		retval = default_flash_mem_blank_check(bank);
 	}
 	free(block_array);
diff --git a/src/flash/nor/numicro.c b/src/flash/nor/numicro.c
index cb7c1df..3487508 100644
--- a/src/flash/nor/numicro.c
+++ b/src/flash/nor/numicro.c
@@ -1385,13 +1385,6 @@ static int numicro_writeblock(struct flash_bank *bank, const uint8_t *buffer,
 	init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);    /* faddr */
 	init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);    /* number of words to program */
 
-	struct armv7m_common *armv7m = target_to_armv7m(target);
-	if (!armv7m) {
-		/* something is very wrong if armv7m is NULL */
-		LOG_ERROR("unable to get armv7m target");
-		return retval;
-	}
-
 	/* write code buffer and use Flash programming code within NuMicro     */
 	/* Set breakpoint to 0 with time-out of 1000 ms                        */
 	while (count > 0) {
diff --git a/src/flash/nor/stm32l4x.c b/src/flash/nor/stm32l4x.c
index 7b35a06..0a9d59c 100644
--- a/src/flash/nor/stm32l4x.c
+++ b/src/flash/nor/stm32l4x.c
@@ -1078,7 +1078,7 @@ static int stm32l4_get_all_wrpxy(struct flash_bank *bank, enum stm32_bank_id dev
 	if (dev_bank_id != STM32_BANK1 && stm32l4_info->dual_bank_mode)
 		wrp2y_sectors_offset = stm32l4_info->bank1_sectors;
 
-	if (wrp2y_sectors_offset > -1) {
+	if (wrp2y_sectors_offset >= 0) {
 		/* get WRP2AR */
 		ret = stm32l4_get_one_wrpxy(bank, &wrpxy[(*n_wrp)++], STM32_FLASH_WRP2AR_INDEX, wrp2y_sectors_offset);
 		if (ret != ERROR_OK)
@@ -1220,49 +1220,11 @@ err_lock:
 	return retval2;
 }
 
-static int stm32l4_protect(struct flash_bank *bank, int set, unsigned int first, unsigned int last)
+static int stm32l4_protect_same_bank(struct flash_bank *bank, enum stm32_bank_id bank_id, int set,
+		unsigned int first, unsigned int last)
 {
-	struct target *target = bank->target;
-	struct stm32l4_flash_bank *stm32l4_info = bank->driver_priv;
-	int ret = ERROR_OK;
 	unsigned int i;
 
-	if (stm32l4_is_otp(bank)) {
-		LOG_ERROR("cannot protect/unprotect OTP memory");
-		return ERROR_FLASH_OPER_UNSUPPORTED;
-	}
-
-	if (target->state != TARGET_HALTED) {
-		LOG_ERROR("Target not halted");
-		return ERROR_TARGET_NOT_HALTED;
-	}
-
-	/* the requested sectors could be located into bank1 and/or bank2 */
-	bool use_bank2 = false;
-	if (last >= stm32l4_info->bank1_sectors) {
-		if (first < stm32l4_info->bank1_sectors) {
-			/* the requested sectors for (un)protection are shared between
-			 * bank 1 and 2, then split the operation */
-
-			/*  1- deal with bank 1 sectors */
-			LOG_DEBUG("The requested sectors for %s are shared between bank 1 and 2",
-					set ? "protection" : "unprotection");
-			ret = stm32l4_protect(bank, set, first, stm32l4_info->bank1_sectors - 1);
-			if (ret != ERROR_OK)
-				return ret;
-
-			/*  2- then continue with bank 2 sectors */
-			first = stm32l4_info->bank1_sectors;
-		}
-
-		use_bank2 = true;
-	}
-
-	/* refresh the sectors' protection */
-	ret = stm32l4_protect_check(bank);
-	if (ret != ERROR_OK)
-		return ret;
-
 	/* check if the desired protection is already configured */
 	for (i = first; i <= last; i++) {
 		if (bank->sectors[i].is_protected != set)
@@ -1278,7 +1240,7 @@ static int stm32l4_protect(struct flash_bank *bank, int set, unsigned int first,
 	unsigned int n_wrp;
 	struct stm32l4_wrp wrpxy[4];
 
-	ret = stm32l4_get_all_wrpxy(bank, use_bank2 ? STM32_BANK2 : STM32_BANK1, wrpxy, &n_wrp);
+	int ret = stm32l4_get_all_wrpxy(bank, bank_id, wrpxy, &n_wrp);
 	if (ret != ERROR_OK)
 		return ret;
 
@@ -1349,6 +1311,40 @@ static int stm32l4_protect(struct flash_bank *bank, int set, unsigned int first,
 	return stm32l4_write_all_wrpxy(bank, wrpxy, n_wrp);
 }
 
+static int stm32l4_protect(struct flash_bank *bank, int set, unsigned int first, unsigned int last)
+{
+	struct target *target = bank->target;
+	struct stm32l4_flash_bank *stm32l4_info = bank->driver_priv;
+
+	if (stm32l4_is_otp(bank)) {
+		LOG_ERROR("cannot protect/unprotect OTP memory");
+		return ERROR_FLASH_OPER_UNSUPPORTED;
+	}
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* refresh the sectors' protection */
+	int ret = stm32l4_protect_check(bank);
+	if (ret != ERROR_OK)
+		return ret;
+
+	/* the requested sectors could be located into bank1 and/or bank2 */
+	if (last < stm32l4_info->bank1_sectors) {
+		return stm32l4_protect_same_bank(bank, STM32_BANK1, set, first, last);
+	} else if (first >= stm32l4_info->bank1_sectors) {
+		return stm32l4_protect_same_bank(bank, STM32_BANK2, set, first, last);
+	} else {
+		ret = stm32l4_protect_same_bank(bank, STM32_BANK1, set, first, stm32l4_info->bank1_sectors - 1);
+		if (ret != ERROR_OK)
+			return ret;
+
+		return stm32l4_protect_same_bank(bank, STM32_BANK2, set, stm32l4_info->bank1_sectors, last);
+	}
+}
+
 /* count is the size divided by stm32l4_info->data_width */
 static int stm32l4_write_block(struct flash_bank *bank, const uint8_t *buffer,
 	uint32_t offset, uint32_t count)
@@ -1987,6 +1983,15 @@ static int stm32l4_probe(struct flash_bank *bank)
 		return ERROR_FAIL;
 	}
 
+	/* ensure that at least there is 1 flash sector / page */
+	if (num_pages == 0) {
+		if (stm32l4_info->user_bank_size)
+			LOG_ERROR("The specified flash size is less than page size");
+
+		LOG_ERROR("Flash pages count cannot be zero");
+		return ERROR_FAIL;
+	}
+
 	LOG_INFO("flash mode : %s-bank", stm32l4_info->dual_bank_mode ? "dual" : "single");
 
 	const int gap_size_kb = stm32l4_info->hole_sectors * page_size_kb;
diff --git a/src/helper/startup.tcl b/src/helper/startup.tcl
index 71f489d..6389262 100644
--- a/src/helper/startup.tcl
+++ b/src/helper/startup.tcl
@@ -28,4 +28,26 @@ proc script {filename} {
 add_help_text script "filename of OpenOCD script (tcl) to run"
 add_usage_text script "<file>"
 
+# Run a list of post-init commands
+# Each command should be added with 'lappend post_init_commands command'
+lappend _telnet_autocomplete_skip _run_post_init_commands
+proc _run_post_init_commands {} {
+	if {[info exists ::post_init_commands]} {
+		foreach cmd $::post_init_commands {
+			eval $cmd
+		}
+	}
+}
+
+# Run a list of pre-shutdown commands
+# Each command should be added with 'lappend pre_shutdown_commands command'
+lappend _telnet_autocomplete_skip _run_pre_shutdown_commands
+proc _run_pre_shutdown_commands {} {
+	if {[info exists ::pre_shutdown_commands]} {
+		foreach cmd $::pre_shutdown_commands {
+			eval $cmd
+		}
+	}
+}
+
 #########
diff --git a/src/jtag/drivers/Makefile.am b/src/jtag/drivers/Makefile.am
index 887f99b..d05b7b9 100644
--- a/src/jtag/drivers/Makefile.am
+++ b/src/jtag/drivers/Makefile.am
@@ -185,6 +185,9 @@ endif
 if XDS110
 DRIVERFILES += %D%/xds110.c
 endif
+if AM335XGPIO
+DRIVERFILES += %D%/am335xgpio.c
+endif
 
 DRIVERHEADERS = \
 	%D%/bitbang.h \
diff --git a/src/jtag/drivers/am335xgpio.c b/src/jtag/drivers/am335xgpio.c
new file mode 100644
index 0000000..e04c44c
--- /dev/null
+++ b/src/jtag/drivers/am335xgpio.c
@@ -0,0 +1,733 @@
+/***************************************************************************
+ *   Copyright (C) 2022 by Steve Marple, stevemarple@googlemail.com        *
+ *                                                                         *
+ *   Based on bcm2835gpio.c and linuxgpiod.c                               *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <jtag/interface.h>
+#include <transport/transport.h>
+#include "bitbang.h"
+
+#include <sys/mman.h>
+
+/*
+ * GPIO register base addresses. Values taken from "AM335x and AMIC110 Sitara
+ * Processors Technical Reference Manual", Chapter 2 Memory Map.
+ */
+#define AM335XGPIO_NUM_GPIO_PORTS 4
+#define AM335XGPIO_GPIO0_HW_ADDR 0x44E07000
+#define AM335XGPIO_GPIO1_HW_ADDR 0x4804C000
+#define AM335XGPIO_GPIO2_HW_ADDR 0x481AC000
+#define AM335XGPIO_GPIO3_HW_ADDR 0x481AE000
+
+/* 32-bit offsets from GPIO port base address. Values taken from "AM335x and
+ * AMIC110 Sitara Processors Technical Reference Manual", Chapter 25
+ * General-Purpose Input/Output.
+ */
+#define AM335XGPIO_GPIO_OE_OFFSET (0x134 / 4)
+#define AM335XGPIO_GPIO_DATAIN_OFFSET (0x138 / 4)
+#define AM335XGPIO_GPIO_DATAOUT_OFFSET (0x13C / 4)  /* DATAOUT register uses 0 for output, 1 for input */
+#define AM335XGPIO_GPIO_CLEARDATAOUT_OFFSET (0x190 / 4)
+#define AM335XGPIO_GPIO_SETDATAOUT_OFFSET (0x194 / 4)
+
+/* GPIOs are integer values; need to map to a port module, and the pin within
+ * that module. GPIOs 0 to 31 map to GPIO0, 32 to 63 to GPIO1 etc. This scheme
+ * matches that used by Linux on the BeagleBone.
+ */
+#define AM335XGPIO_PORT_NUM(gpio_num) ((gpio_num) / 32)
+#define AM335XGPIO_BIT_NUM(gpio_num) ((gpio_num) % 32)
+#define AM335XGPIO_BIT_MASK(gpio_num) BIT(AM335XGPIO_BIT_NUM(gpio_num))
+
+#define AM335XGPIO_READ_REG(gpio_num, offset) \
+	(*(am335xgpio_gpio_port_mmap_addr[AM335XGPIO_PORT_NUM(gpio_num)] + (offset)))
+
+#define AM335XGPIO_WRITE_REG(gpio_num, offset, value) \
+	(*(am335xgpio_gpio_port_mmap_addr[AM335XGPIO_PORT_NUM(gpio_num)] + (offset)) = (value))
+
+#define AM335XGPIO_SET_REG_BITS(gpio_num, offset, bit_mask) \
+	(*(am335xgpio_gpio_port_mmap_addr[AM335XGPIO_PORT_NUM(gpio_num)] + (offset)) |= (bit_mask))
+
+#define AM335XGPIO_CLEAR_REG_BITS(gpio_num, offset, bit_mask) \
+	(*(am335xgpio_gpio_port_mmap_addr[AM335XGPIO_PORT_NUM(gpio_num)] + (offset)) &= ~(bit_mask))
+
+enum amx335gpio_gpio_mode {
+	AM335XGPIO_GPIO_MODE_INPUT,
+	AM335XGPIO_GPIO_MODE_OUTPUT, /* To set output mode but not state */
+	AM335XGPIO_GPIO_MODE_OUTPUT_LOW,
+	AM335XGPIO_GPIO_MODE_OUTPUT_HIGH,
+};
+
+static const uint32_t am335xgpio_gpio_port_hw_addr[AM335XGPIO_NUM_GPIO_PORTS] = {
+	AM335XGPIO_GPIO0_HW_ADDR,
+	AM335XGPIO_GPIO1_HW_ADDR,
+	AM335XGPIO_GPIO2_HW_ADDR,
+	AM335XGPIO_GPIO3_HW_ADDR,
+};
+
+/* Memory-mapped address pointers */
+static volatile uint32_t *am335xgpio_gpio_port_mmap_addr[AM335XGPIO_NUM_GPIO_PORTS];
+
+static int dev_mem_fd;
+
+/* GPIO numbers for each signal. Negative values are invalid */
+static int tck_gpio = -1;
+static enum amx335gpio_gpio_mode tck_gpio_mode;
+static int tms_gpio = -1;
+static enum amx335gpio_gpio_mode tms_gpio_mode;
+static int tdi_gpio = -1;
+static enum amx335gpio_gpio_mode tdi_gpio_mode;
+static int tdo_gpio = -1;
+static enum amx335gpio_gpio_mode tdo_gpio_mode;
+static int trst_gpio = -1;
+static enum amx335gpio_gpio_mode trst_gpio_mode;
+static int srst_gpio = -1;
+static enum amx335gpio_gpio_mode srst_gpio_mode;
+static int swclk_gpio = -1;
+static enum amx335gpio_gpio_mode swclk_gpio_mode;
+static int swdio_gpio = -1;
+static enum amx335gpio_gpio_mode swdio_gpio_mode;
+static int swdio_dir_gpio = -1;
+static enum amx335gpio_gpio_mode swdio_dir_gpio_mode;
+static int led_gpio = -1;
+static enum amx335gpio_gpio_mode led_gpio_mode = -1;
+
+static bool swdio_dir_is_active_high = true; /* Active state means output */
+static bool led_is_active_high = true;
+
+/* Transition delay coefficients */
+static int speed_coeff = 600000;
+static int speed_offset = 575;
+static unsigned int jtag_delay;
+
+static int is_gpio_valid(int gpio_num)
+{
+	return gpio_num >= 0 && gpio_num < (32 * AM335XGPIO_NUM_GPIO_PORTS);
+}
+
+static int get_gpio_value(int gpio_num)
+{
+	unsigned int shift = AM335XGPIO_BIT_NUM(gpio_num);
+	return (AM335XGPIO_READ_REG(gpio_num, AM335XGPIO_GPIO_DATAIN_OFFSET) >> shift) & 1;
+}
+
+static void set_gpio_value(int gpio_num, int value)
+{
+	if (value)
+		AM335XGPIO_WRITE_REG(gpio_num, AM335XGPIO_GPIO_SETDATAOUT_OFFSET, AM335XGPIO_BIT_MASK(gpio_num));
+	else
+		AM335XGPIO_WRITE_REG(gpio_num, AM335XGPIO_GPIO_CLEARDATAOUT_OFFSET, AM335XGPIO_BIT_MASK(gpio_num));
+}
+
+static enum amx335gpio_gpio_mode get_gpio_mode(int gpio_num)
+{
+	if (AM335XGPIO_READ_REG(gpio_num, AM335XGPIO_GPIO_OE_OFFSET) & AM335XGPIO_BIT_MASK(gpio_num)) {
+		return AM335XGPIO_GPIO_MODE_INPUT;
+	} else {
+		/* Return output level too so that pin mode can be fully restored */
+		if (AM335XGPIO_READ_REG(gpio_num, AM335XGPIO_GPIO_DATAOUT_OFFSET) & AM335XGPIO_BIT_MASK(gpio_num))
+			return AM335XGPIO_GPIO_MODE_OUTPUT_HIGH;
+		else
+			return AM335XGPIO_GPIO_MODE_OUTPUT_LOW;
+	}
+}
+
+static void set_gpio_mode(int gpio_num, enum amx335gpio_gpio_mode gpio_mode)
+{
+	if (gpio_mode == AM335XGPIO_GPIO_MODE_INPUT) {
+		AM335XGPIO_SET_REG_BITS(gpio_num, AM335XGPIO_GPIO_OE_OFFSET, AM335XGPIO_BIT_MASK(gpio_num));
+		return;
+	}
+
+	if (gpio_mode == AM335XGPIO_GPIO_MODE_OUTPUT_LOW)
+		set_gpio_value(gpio_num, 0);
+	if (gpio_mode == AM335XGPIO_GPIO_MODE_OUTPUT_HIGH)
+		set_gpio_value(gpio_num, 1);
+
+	if (gpio_mode == AM335XGPIO_GPIO_MODE_OUTPUT ||
+		gpio_mode == AM335XGPIO_GPIO_MODE_OUTPUT_LOW ||
+		gpio_mode == AM335XGPIO_GPIO_MODE_OUTPUT_HIGH) {
+			AM335XGPIO_CLEAR_REG_BITS(gpio_num, AM335XGPIO_GPIO_OE_OFFSET, AM335XGPIO_BIT_MASK(gpio_num));
+	}
+}
+
+static const char *get_gpio_mode_name(enum amx335gpio_gpio_mode gpio_mode)
+{
+	switch (gpio_mode) {
+	case AM335XGPIO_GPIO_MODE_INPUT:
+		return "input";
+	case AM335XGPIO_GPIO_MODE_OUTPUT:
+		return "output";
+	case AM335XGPIO_GPIO_MODE_OUTPUT_LOW:
+		return "output (low)";
+	case AM335XGPIO_GPIO_MODE_OUTPUT_HIGH:
+		return "output (high)";
+	default:
+		return "unknown";
+	}
+}
+
+static bb_value_t am335xgpio_read(void)
+{
+	return get_gpio_value(tdo_gpio) ? BB_HIGH : BB_LOW;
+}
+
+static int am335xgpio_write(int tck, int tms, int tdi)
+{
+	set_gpio_value(tdi_gpio, tdi);
+	set_gpio_value(tms_gpio, tms);
+	set_gpio_value(tck_gpio, tck); /* Write clock last */
+
+	for (unsigned int i = 0; i < jtag_delay; ++i)
+		asm volatile ("");
+
+	return ERROR_OK;
+}
+
+static int am335xgpio_swd_write(int swclk, int swdio)
+{
+	set_gpio_value(swdio_gpio, swdio);
+	set_gpio_value(swclk_gpio, swclk); /* Write clock last */
+
+	for (unsigned int i = 0; i < jtag_delay; ++i)
+		asm volatile ("");
+
+	return ERROR_OK;
+}
+
+/* (1) assert or (0) deassert reset lines */
+static int am335xgpio_reset(int trst, int srst)
+{
+	/* assume active low */
+	if (is_gpio_valid(srst_gpio)) {
+		if (jtag_get_reset_config() & RESET_SRST_PUSH_PULL)
+			set_gpio_mode(srst_gpio, srst ? AM335XGPIO_GPIO_MODE_OUTPUT_LOW : AM335XGPIO_GPIO_MODE_OUTPUT_HIGH);
+		else
+			set_gpio_mode(srst_gpio, srst ? AM335XGPIO_GPIO_MODE_OUTPUT_LOW : AM335XGPIO_GPIO_MODE_INPUT);
+	}
+
+	/* assume active low */
+	if (is_gpio_valid(trst_gpio)) {
+		if (jtag_get_reset_config() & RESET_TRST_OPEN_DRAIN)
+			set_gpio_mode(trst_gpio, trst ? AM335XGPIO_GPIO_MODE_OUTPUT_LOW : AM335XGPIO_GPIO_MODE_INPUT);
+		else
+			set_gpio_mode(trst_gpio, trst ? AM335XGPIO_GPIO_MODE_OUTPUT_LOW : AM335XGPIO_GPIO_MODE_OUTPUT_HIGH);
+	}
+
+	LOG_DEBUG("am335xgpio_reset(%d, %d), trst_gpio: %d (%s), srst_gpio: %d (%s)",
+		trst, srst,
+		trst_gpio, get_gpio_mode_name(get_gpio_mode(trst_gpio)),
+		srst_gpio, get_gpio_mode_name(get_gpio_mode(srst_gpio)));
+	return ERROR_OK;
+}
+
+static void am335xgpio_swdio_drive(bool is_output)
+{
+	if (is_output) {
+		set_gpio_value(swdio_dir_gpio, swdio_dir_is_active_high ? 1 : 0);
+		set_gpio_mode(swdio_gpio, AM335XGPIO_GPIO_MODE_OUTPUT);
+	} else {
+		set_gpio_mode(swdio_gpio, AM335XGPIO_GPIO_MODE_INPUT);
+		set_gpio_value(swdio_dir_gpio, swdio_dir_is_active_high ? 0 : 1);
+	}
+}
+
+static int am335xgpio_swdio_read(void)
+{
+	return get_gpio_value(swdio_gpio);
+}
+
+static int am335xgpio_blink(int on)
+{
+	if (is_gpio_valid(led_gpio))
+		set_gpio_value(led_gpio, (!on ^ led_is_active_high) ? 1 : 0);
+
+	return ERROR_OK;
+}
+
+static struct bitbang_interface am335xgpio_bitbang = {
+	.read = am335xgpio_read,
+	.write = am335xgpio_write,
+	.swdio_read = am335xgpio_swdio_read,
+	.swdio_drive = am335xgpio_swdio_drive,
+	.swd_write = am335xgpio_swd_write,
+	.blink = am335xgpio_blink
+};
+
+static int am335xgpio_khz(int khz, int *jtag_speed)
+{
+	if (!khz) {
+		LOG_DEBUG("RCLK not supported");
+		return ERROR_FAIL;
+	}
+	*jtag_speed = speed_coeff / khz - speed_offset;
+	if (*jtag_speed < 0)
+		*jtag_speed = 0;
+	return ERROR_OK;
+}
+
+static int am335xgpio_speed_div(int speed, int *khz)
+{
+	*khz = speed_coeff / (speed + speed_offset);
+	return ERROR_OK;
+}
+
+static int am335xgpio_speed(int speed)
+{
+	jtag_delay = speed;
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(am335xgpio_handle_jtag_gpionums)
+{
+	if (CMD_ARGC == 4) {
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], tck_gpio);
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[1], tms_gpio);
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[2], tdi_gpio);
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[3], tdo_gpio);
+	} else if (CMD_ARGC != 0) {
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	command_print(CMD, "AM335x GPIO config: tck = %d, tms = %d, tdi = %d, tdo = %d",
+			tck_gpio, tms_gpio, tdi_gpio, tdo_gpio);
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(am335xgpio_handle_jtag_gpionum_tck)
+{
+	if (CMD_ARGC == 1)
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], tck_gpio);
+
+	command_print(CMD, "AM335x GPIO config: tck = %d", tck_gpio);
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(am335xgpio_handle_jtag_gpionum_tms)
+{
+	if (CMD_ARGC == 1)
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], tms_gpio);
+
+	command_print(CMD, "AM335x GPIO config: tms = %d", tms_gpio);
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(am335xgpio_handle_jtag_gpionum_tdo)
+{
+	if (CMD_ARGC == 1)
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], tdo_gpio);
+
+	command_print(CMD, "AM335x GPIO config: tdo = %d", tdo_gpio);
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(am335xgpio_handle_jtag_gpionum_tdi)
+{
+	if (CMD_ARGC == 1)
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], tdi_gpio);
+
+	command_print(CMD, "AM335x GPIO config: tdi = %d", tdi_gpio);
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(am335xgpio_handle_jtag_gpionum_srst)
+{
+	if (CMD_ARGC == 1)
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], srst_gpio);
+
+	command_print(CMD, "AM335x GPIO config: srst = %d", srst_gpio);
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(am335xgpio_handle_jtag_gpionum_trst)
+{
+	if (CMD_ARGC == 1)
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], trst_gpio);
+
+	command_print(CMD, "AM335x GPIO config: trst = %d", trst_gpio);
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(am335xgpio_handle_swd_gpionums)
+{
+	if (CMD_ARGC == 2) {
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], swclk_gpio);
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[1], swdio_gpio);
+	} else if (CMD_ARGC != 0) {
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	command_print(CMD, "AM335x GPIO config: swclk = %d, swdio = %d", swclk_gpio, swdio_gpio);
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(am335xgpio_handle_swd_gpionum_swclk)
+{
+	if (CMD_ARGC == 1)
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], swclk_gpio);
+
+	command_print(CMD, "AM335x GPIO config: swclk = %d", swclk_gpio);
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(am335xgpio_handle_swd_gpionum_swdio)
+{
+	if (CMD_ARGC == 1)
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], swdio_gpio);
+
+	command_print(CMD, "AM335x GPIO config: swdio = %d", swdio_gpio);
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(am335xgpio_handle_swd_gpionum_swdio_dir)
+{
+	if (CMD_ARGC == 1)
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], swdio_dir_gpio);
+
+	command_print(CMD, "AM335x GPIO config: swdio_dir = %d", swdio_dir_gpio);
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(am335xgpio_handle_swd_dir_output_state)
+{
+	if (CMD_ARGC == 1)
+		COMMAND_PARSE_BOOL(CMD_ARGV[0], swdio_dir_is_active_high, "high", "low");
+
+	command_print(CMD, "AM335x GPIO config: swdio_dir_output_state = %s", swdio_dir_is_active_high ? "high" : "low");
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(am335xgpio_handle_gpionum_led)
+{
+	if (CMD_ARGC == 1)
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], led_gpio);
+
+	command_print(CMD, "AM335x GPIO config: led = %d", led_gpio);
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(am335xgpio_handle_led_on_state)
+{
+	if (CMD_ARGC == 1)
+		COMMAND_PARSE_BOOL(CMD_ARGV[0], led_is_active_high, "high", "low");
+
+	command_print(CMD, "AM335x GPIO config: led_on_state = %s", led_is_active_high ? "high" : "low");
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(am335xgpio_handle_speed_coeffs)
+{
+	if (CMD_ARGC == 2) {
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], speed_coeff);
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[1], speed_offset);
+	}
+
+	command_print(CMD, "AM335x GPIO config: speed_coeffs = %d, speed_offset = %d",
+				  speed_coeff, speed_offset);
+	return ERROR_OK;
+}
+
+static const struct command_registration am335xgpio_subcommand_handlers[] = {
+	{
+		.name = "jtag_nums",
+		.handler = am335xgpio_handle_jtag_gpionums,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio numbers for tck, tms, tdi, tdo (in that order).",
+		.usage = "[tck tms tdi tdo]",
+	},
+	{
+		.name = "tck_num",
+		.handler = am335xgpio_handle_jtag_gpionum_tck,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio number for tck.",
+		.usage = "[tck]",
+	},
+	{
+		.name = "tms_num",
+		.handler = am335xgpio_handle_jtag_gpionum_tms,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio number for tms.",
+		.usage = "[tms]",
+	},
+	{
+		.name = "tdo_num",
+		.handler = am335xgpio_handle_jtag_gpionum_tdo,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio number for tdo.",
+		.usage = "[tdo]",
+	},
+	{
+		.name = "tdi_num",
+		.handler = am335xgpio_handle_jtag_gpionum_tdi,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio number for tdi.",
+		.usage = "[tdi]",
+	},
+	{
+		.name = "swd_nums",
+		.handler = am335xgpio_handle_swd_gpionums,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio numbers for swclk, swdio (in that order).",
+		.usage = "[swclk swdio]",
+	},
+	{
+		.name = "swclk_num",
+		.handler = am335xgpio_handle_swd_gpionum_swclk,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio number for swclk.",
+		.usage = "[swclk]",
+	},
+	{
+		.name = "swdio_num",
+		.handler = am335xgpio_handle_swd_gpionum_swdio,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio number for swdio.",
+		.usage = "[swdio]",
+	},
+	{
+		.name = "swdio_dir_num",
+		.handler = am335xgpio_handle_swd_gpionum_swdio_dir,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio number for swdio direction control pin.",
+		.usage = "[swdio_dir]",
+	},
+	{
+		.name = "swdio_dir_output_state",
+		.handler = am335xgpio_handle_swd_dir_output_state,
+		.mode = COMMAND_CONFIG,
+		.help = "required state for swdio_dir pin to select SWDIO buffer to be output.",
+		.usage = "['off'|'on']",
+	},
+	{
+		.name = "srst_num",
+		.handler = am335xgpio_handle_jtag_gpionum_srst,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio number for srst.",
+		.usage = "[srst]",
+	},
+	{
+		.name = "trst_num",
+		.handler = am335xgpio_handle_jtag_gpionum_trst,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio number for trst.",
+		.usage = "[trst]",
+	},
+	{
+		.name = "led_num",
+		.handler = am335xgpio_handle_gpionum_led,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio number for led.",
+		.usage = "[led]",
+	},
+	{
+		.name = "led_on_state",
+		.handler = am335xgpio_handle_led_on_state,
+		.mode = COMMAND_CONFIG,
+		.help = "required state for led pin to turn on LED.",
+		.usage = "['off'|'on']",
+	},
+	{
+		.name = "speed_coeffs",
+		.handler = am335xgpio_handle_speed_coeffs,
+		.mode = COMMAND_CONFIG,
+		.help = "SPEED_COEFF and SPEED_OFFSET for delay calculations.",
+		.usage = "[SPEED_COEFF SPEED_OFFSET]",
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+static const struct command_registration am335xgpio_command_handlers[] = {
+	{
+		.name = "am335xgpio",
+		.mode = COMMAND_ANY,
+		.help = "perform am335xgpio management",
+		.chain = am335xgpio_subcommand_handlers,
+		.usage = "",
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+static const char * const am335xgpio_transports[] = { "jtag", "swd", NULL };
+
+static struct jtag_interface am335xgpio_interface = {
+	.supported = DEBUG_CAP_TMS_SEQ,
+	.execute_queue = bitbang_execute_queue,
+};
+
+static bool am335xgpio_jtag_mode_possible(void)
+{
+	if (!is_gpio_valid(tck_gpio))
+		return false;
+	if (!is_gpio_valid(tms_gpio))
+		return false;
+	if (!is_gpio_valid(tdi_gpio))
+		return false;
+	if (!is_gpio_valid(tdo_gpio))
+		return false;
+	return true;
+}
+
+static bool am335xgpio_swd_mode_possible(void)
+{
+	if (!is_gpio_valid(swclk_gpio))
+		return false;
+	if (!is_gpio_valid(swdio_gpio))
+		return false;
+	return true;
+}
+
+static int am335xgpio_init(void)
+{
+	bitbang_interface = &am335xgpio_bitbang;
+
+	LOG_INFO("AM335x GPIO JTAG/SWD bitbang driver");
+
+	if (transport_is_jtag() && !am335xgpio_jtag_mode_possible()) {
+		LOG_ERROR("Require tck, tms, tdi and tdo gpios for JTAG mode");
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	if (transport_is_swd() && !am335xgpio_swd_mode_possible()) {
+		LOG_ERROR("Require swclk and swdio gpio for SWD mode");
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	dev_mem_fd = open("/dev/gpiomem", O_RDWR | O_SYNC);
+	if (dev_mem_fd < 0) {
+		LOG_DEBUG("Cannot open /dev/gpiomem, fallback to /dev/mem");
+		dev_mem_fd = open("/dev/mem", O_RDWR | O_SYNC);
+	}
+	if (dev_mem_fd < 0) {
+		LOG_ERROR("open: %s", strerror(errno));
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	for (unsigned int i = 0; i < AM335XGPIO_NUM_GPIO_PORTS; ++i) {
+		am335xgpio_gpio_port_mmap_addr[i] = mmap(NULL, sysconf(_SC_PAGE_SIZE), PROT_READ | PROT_WRITE,
+				MAP_SHARED, dev_mem_fd, am335xgpio_gpio_port_hw_addr[i]);
+
+		if (am335xgpio_gpio_port_mmap_addr[i] == MAP_FAILED) {
+			LOG_ERROR("mmap: %s", strerror(errno));
+			close(dev_mem_fd);
+			return ERROR_JTAG_INIT_FAILED;
+		}
+	}
+
+	/*
+	 * Configure TDO as an input, and TDI, TCK, TMS, TRST, SRST as outputs.
+	 * Drive TDI and TCK low, and TMS high.
+	 */
+	if (transport_is_jtag()) {
+		tdo_gpio_mode = get_gpio_mode(tdo_gpio);
+		tdi_gpio_mode = get_gpio_mode(tdi_gpio);
+		tck_gpio_mode = get_gpio_mode(tck_gpio);
+		tms_gpio_mode = get_gpio_mode(tms_gpio);
+		LOG_DEBUG("saved GPIO mode for tdo (GPIO #%d): %s", tdo_gpio, get_gpio_mode_name(tdo_gpio_mode));
+		LOG_DEBUG("saved GPIO mode for tdi (GPIO #%d): %s", tdi_gpio, get_gpio_mode_name(tdi_gpio_mode));
+		LOG_DEBUG("saved GPIO mode for tck (GPIO #%d): %s", tck_gpio, get_gpio_mode_name(tck_gpio_mode));
+		LOG_DEBUG("saved GPIO mode for tms (GPIO #%d): %s", tms_gpio, get_gpio_mode_name(tms_gpio_mode));
+
+		set_gpio_mode(tdo_gpio, AM335XGPIO_GPIO_MODE_INPUT);
+		set_gpio_mode(tdi_gpio, AM335XGPIO_GPIO_MODE_OUTPUT_LOW);
+		set_gpio_mode(tms_gpio, AM335XGPIO_GPIO_MODE_OUTPUT_HIGH);
+		set_gpio_mode(tck_gpio, AM335XGPIO_GPIO_MODE_OUTPUT_LOW);
+
+		if (is_gpio_valid(trst_gpio)) {
+			trst_gpio_mode = get_gpio_mode(trst_gpio);
+			LOG_DEBUG("saved GPIO mode for trst (GPIO #%d): %s", trst_gpio, get_gpio_mode_name(trst_gpio_mode));
+		}
+	}
+
+	if (transport_is_swd()) {
+		swclk_gpio_mode = get_gpio_mode(swclk_gpio);
+		swdio_gpio_mode = get_gpio_mode(swdio_gpio);
+		LOG_DEBUG("saved GPIO mode for swclk (GPIO #%d): %s", swclk_gpio, get_gpio_mode_name(swclk_gpio_mode));
+		LOG_DEBUG("saved GPIO mode for swdio (GPIO #%d): %s", swdio_gpio, get_gpio_mode_name(swdio_gpio_mode));
+		if (is_gpio_valid(swdio_dir_gpio)) {
+			swdio_dir_gpio_mode = get_gpio_mode(swdio_dir_gpio);
+			LOG_DEBUG("saved GPIO mode for swdio_dir (GPIO #%d): %s",
+					swdio_dir_gpio, get_gpio_mode_name(swdio_dir_gpio_mode));
+			set_gpio_mode(swdio_dir_gpio,
+					swdio_dir_is_active_high ? AM335XGPIO_GPIO_MODE_OUTPUT_HIGH : AM335XGPIO_GPIO_MODE_OUTPUT_LOW);
+
+		}
+		set_gpio_mode(swdio_gpio, AM335XGPIO_GPIO_MODE_OUTPUT_LOW);
+		set_gpio_mode(swclk_gpio, AM335XGPIO_GPIO_MODE_OUTPUT_LOW);
+	}
+
+	if (is_gpio_valid(srst_gpio)) {
+		srst_gpio_mode = get_gpio_mode(srst_gpio);
+		LOG_DEBUG("saved GPIO mode for srst (GPIO #%d): %s", srst_gpio, get_gpio_mode_name(srst_gpio_mode));
+	}
+
+	if (is_gpio_valid(led_gpio)) {
+		led_gpio_mode = get_gpio_mode(led_gpio);
+		LOG_DEBUG("saved GPIO mode for led (GPIO #%d): %s", led_gpio, get_gpio_mode_name(led_gpio_mode));
+		set_gpio_mode(led_gpio,
+				led_is_active_high ? AM335XGPIO_GPIO_MODE_OUTPUT_LOW : AM335XGPIO_GPIO_MODE_OUTPUT_HIGH);
+	}
+
+	/* Set GPIO modes for TRST and SRST and make both inactive */
+	am335xgpio_reset(0, 0);
+	return ERROR_OK;
+}
+
+static int am335xgpio_quit(void)
+{
+	if (transport_is_jtag()) {
+		set_gpio_mode(tdo_gpio, tdo_gpio_mode);
+		set_gpio_mode(tdi_gpio, tdi_gpio_mode);
+		set_gpio_mode(tck_gpio, tck_gpio_mode);
+		set_gpio_mode(tms_gpio, tms_gpio_mode);
+		if (is_gpio_valid(trst_gpio))
+			set_gpio_mode(trst_gpio, trst_gpio_mode);
+	}
+
+	if (transport_is_swd()) {
+		set_gpio_mode(swclk_gpio, swclk_gpio_mode);
+		set_gpio_mode(swdio_gpio, swdio_gpio_mode);
+		if (is_gpio_valid(swdio_dir_gpio))
+			set_gpio_mode(swdio_dir_gpio, swdio_dir_gpio_mode);
+	}
+
+	if (is_gpio_valid(srst_gpio))
+		set_gpio_mode(srst_gpio, srst_gpio_mode);
+
+	if (is_gpio_valid(led_gpio))
+		set_gpio_mode(led_gpio, led_gpio_mode);
+
+	return ERROR_OK;
+}
+
+struct adapter_driver am335xgpio_adapter_driver = {
+	.name = "am335xgpio",
+	.transports = am335xgpio_transports,
+	.commands = am335xgpio_command_handlers,
+
+	.init = am335xgpio_init,
+	.quit = am335xgpio_quit,
+	.reset = am335xgpio_reset,
+	.speed = am335xgpio_speed,
+	.khz = am335xgpio_khz,
+	.speed_div = am335xgpio_speed_div,
+
+	.jtag_ops = &am335xgpio_interface,
+	.swd_ops = &bitbang_swd,
+};
diff --git a/src/jtag/drivers/bcm2835gpio.c b/src/jtag/drivers/bcm2835gpio.c
index b7a4d99..22d237f 100644
--- a/src/jtag/drivers/bcm2835gpio.c
+++ b/src/jtag/drivers/bcm2835gpio.c
@@ -94,6 +94,11 @@ static int speed_coeff = 113714;
 static int speed_offset = 28;
 static unsigned int jtag_delay;
 
+static int is_gpio_valid(int gpio)
+{
+	return gpio >= 0 && gpio <= 31;
+}
+
 static bb_value_t bcm2835gpio_read(void)
 {
 	return (GPIO_LEV & 1<<tdo_gpio) ? BB_HIGH : BB_LOW;
@@ -133,12 +138,12 @@ static int bcm2835gpio_reset(int trst, int srst)
 	uint32_t set = 0;
 	uint32_t clear = 0;
 
-	if (trst_gpio > 0) {
+	if (is_gpio_valid(trst_gpio)) {
 		set |= !trst<<trst_gpio;
 		clear |= trst<<trst_gpio;
 	}
 
-	if (srst_gpio > 0) {
+	if (is_gpio_valid(srst_gpio)) {
 		set |= !srst<<srst_gpio;
 		clear |= srst<<srst_gpio;
 	}
@@ -151,7 +156,7 @@ static int bcm2835gpio_reset(int trst, int srst)
 
 static void bcm2835_swdio_drive(bool is_output)
 {
-	if (swdio_dir_gpio > 0) {
+	if (is_gpio_valid(swdio_dir_gpio)) {
 		if (is_output) {
 			GPIO_SET = 1 << swdio_dir_gpio;
 			OUT_GPIO(swdio_gpio);
@@ -196,11 +201,6 @@ static int bcm2835gpio_speed(int speed)
 	return ERROR_OK;
 }
 
-static int is_gpio_valid(int gpio)
-{
-	return gpio >= 0 && gpio <= 31;
-}
-
 COMMAND_HANDLER(bcm2835gpio_handle_jtag_gpionums)
 {
 	if (CMD_ARGC == 4) {
@@ -557,7 +557,7 @@ static int bcm2835gpio_init(void)
 		OUT_GPIO(tck_gpio);
 		OUT_GPIO(tms_gpio);
 
-		if (trst_gpio != -1) {
+		if (is_gpio_valid(trst_gpio)) {
 			trst_gpio_mode = MODE_GPIO(trst_gpio);
 			GPIO_SET = 1 << trst_gpio;
 			OUT_GPIO(trst_gpio);
@@ -566,7 +566,7 @@ static int bcm2835gpio_init(void)
 
 	if (transport_is_swd()) {
 		/* Make buffer an output before the GPIO connected to it */
-		if (swdio_dir_gpio != -1) {
+		if (is_gpio_valid(swdio_dir_gpio)) {
 			swdio_dir_gpio_mode = MODE_GPIO(swdio_dir_gpio);
 			GPIO_SET = 1 << swdio_dir_gpio;
 			OUT_GPIO(swdio_dir_gpio);
@@ -581,7 +581,7 @@ static int bcm2835gpio_init(void)
 		OUT_GPIO(swdio_gpio);
 	}
 
-	if (srst_gpio != -1) {
+	if (is_gpio_valid(srst_gpio)) {
 		srst_gpio_mode = MODE_GPIO(srst_gpio);
 		GPIO_SET = 1 << srst_gpio;
 		OUT_GPIO(srst_gpio);
@@ -601,7 +601,7 @@ static int bcm2835gpio_quit(void)
 		SET_MODE_GPIO(tdi_gpio, tdi_gpio_mode);
 		SET_MODE_GPIO(tck_gpio, tck_gpio_mode);
 		SET_MODE_GPIO(tms_gpio, tms_gpio_mode);
-		if (trst_gpio != -1)
+		if (is_gpio_valid(trst_gpio))
 			SET_MODE_GPIO(trst_gpio, trst_gpio_mode);
 	}
 
@@ -610,10 +610,10 @@ static int bcm2835gpio_quit(void)
 		SET_MODE_GPIO(swdio_gpio, swdio_gpio_mode);
 	}
 
-	if (srst_gpio != -1)
+	if (is_gpio_valid(srst_gpio))
 		SET_MODE_GPIO(srst_gpio, srst_gpio_mode);
 
-	if (swdio_dir_gpio != -1)
+	if (is_gpio_valid(swdio_dir_gpio))
 		SET_MODE_GPIO(swdio_dir_gpio, swdio_dir_gpio_mode);
 
 	return ERROR_OK;
diff --git a/src/jtag/drivers/bitbang.c b/src/jtag/drivers/bitbang.c
index 898d6d3..78dcb29 100644
--- a/src/jtag/drivers/bitbang.c
+++ b/src/jtag/drivers/bitbang.c
@@ -532,8 +532,9 @@ static void bitbang_swd_write_reg(uint8_t cmd, uint32_t value, uint32_t ap_delay
 	/* Devices do not reply to DP_TARGETSEL write cmd, ignore received ack */
 	bool check_ack = swd_cmd_returns_ack(cmd);
 
+	/* init the array to silence scan-build */
+	uint8_t trn_ack_data_parity_trn[DIV_ROUND_UP(4 + 3 + 32 + 1 + 4, 8)] = {0};
 	for (;;) {
-		uint8_t trn_ack_data_parity_trn[DIV_ROUND_UP(4 + 3 + 32 + 1 + 4, 8)];
 		buf_set_u32(trn_ack_data_parity_trn, 1 + 3 + 1, 32, value);
 		buf_set_u32(trn_ack_data_parity_trn, 1 + 3 + 1 + 32, 1, parity_u32(value));
 
diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c
index 2785d9b..bb2c817 100644
--- a/src/jtag/drivers/stlink_usb.c
+++ b/src/jtag/drivers/stlink_usb.c
@@ -504,6 +504,8 @@ static inline int stlink_usb_xfer_noerrcheck(void *handle, const uint8_t *buf, i
 #define STLINK_TCP_SS_CMD_NOT_AVAILABLE      0x00001053
 #define STLINK_TCP_SS_TCP_ERROR              0x00002001
 #define STLINK_TCP_SS_TCP_CANT_CONNECT       0x00002002
+#define STLINK_TCP_SS_TCP_CLOSE_ERROR        0x00002003
+#define STLINK_TCP_SS_TCP_BUSY               0x00002004
 #define STLINK_TCP_SS_WIN32_ERROR            0x00010000
 
 /*
@@ -971,6 +973,11 @@ static int stlink_tcp_send_cmd(void *handle, int send_size, int recv_size, bool
 	if (check_tcp_status) {
 		uint32_t tcp_ss = le_to_h_u32(h->tcp_backend_priv.recv_buf);
 		if (tcp_ss != STLINK_TCP_SS_OK) {
+			if (tcp_ss == STLINK_TCP_SS_TCP_BUSY) {
+				LOG_DEBUG("TCP busy");
+				return ERROR_WAIT;
+			}
+
 			LOG_ERROR("TCP error status 0x%X", tcp_ss);
 			return ERROR_FAIL;
 		}
diff --git a/src/jtag/interfaces.c b/src/jtag/interfaces.c
index ddf70cc..9afd69d 100644
--- a/src/jtag/interfaces.c
+++ b/src/jtag/interfaces.c
@@ -150,6 +150,9 @@ extern struct adapter_driver stlink_dap_adapter_driver;
 #if BUILD_RSHIM == 1
 extern struct adapter_driver rshim_dap_adapter_driver;
 #endif
+#if BUILD_AM335XGPIO == 1
+extern struct adapter_driver am335xgpio_adapter_driver;
+#endif
 
 /**
  * The list of built-in JTAG interfaces, containing entries for those
@@ -264,5 +267,8 @@ struct adapter_driver *adapter_drivers[] = {
 #if BUILD_RSHIM == 1
 		&rshim_dap_adapter_driver,
 #endif
+#if BUILD_AM335XGPIO == 1
+		&am335xgpio_adapter_driver,
+#endif
 		NULL,
 	};
diff --git a/src/openocd.c b/src/openocd.c
index 3c96d32..fdc4a87 100644
--- a/src/openocd.c
+++ b/src/openocd.c
@@ -182,6 +182,9 @@ COMMAND_HANDLER(handle_init_command)
 
 	target_register_event_callback(log_target_callback_event_handler, CMD_CTX);
 
+	if (command_run_line(CMD_CTX, "_run_post_init_commands") != ERROR_OK)
+		return ERROR_FAIL;
+
 	return ERROR_OK;
 }
 
diff --git a/src/server/gdb_server.c b/src/server/gdb_server.c
index edea5f5..e5bf74f 100644
--- a/src/server/gdb_server.c
+++ b/src/server/gdb_server.c
@@ -3716,12 +3716,14 @@ static int gdb_input_inner(struct connection *connection)
 					break;
 
 				case 'j':
+					/* DEPRECATED */
 					/* packet supported only by smp target i.e cortex_a.c*/
 					/* handle smp packet replying coreid played to gbd */
 					gdb_read_smp_packet(connection, packet, packet_size);
 					break;
 
 				case 'J':
+					/* DEPRECATED */
 					/* packet supported only by smp target i.e cortex_a.c */
 					/* handle smp packet setting coreid to be played at next
 					 * resume to gdb */
diff --git a/src/server/server.c b/src/server/server.c
index 7b0004d..082febb 100644
--- a/src/server/server.c
+++ b/src/server/server.c
@@ -763,6 +763,8 @@ COMMAND_HANDLER(handle_shutdown_command)
 
 	shutdown_openocd = SHUTDOWN_REQUESTED;
 
+	command_run_line(CMD_CTX, "_run_pre_shutdown_commands");
+
 	if (CMD_ARGC == 1) {
 		if (!strcmp(CMD_ARGV[0], "error")) {
 			shutdown_openocd = SHUTDOWN_WITH_ERROR_CODE;
diff --git a/src/server/telnet_server.c b/src/server/telnet_server.c
index 791a1a5..8e79626 100644
--- a/src/server/telnet_server.c
+++ b/src/server/telnet_server.c
@@ -222,9 +222,8 @@ static int telnet_new_connection(struct connection *connection)
 {
 	struct telnet_connection *telnet_connection;
 	struct telnet_service *telnet_service = connection->service->priv;
-	int i;
 
-	telnet_connection = malloc(sizeof(struct telnet_connection));
+	telnet_connection = calloc(1, sizeof(struct telnet_connection));
 
 	if (!telnet_connection) {
 		LOG_ERROR("Failed to allocate telnet connection.");
@@ -234,9 +233,6 @@ static int telnet_new_connection(struct connection *connection)
 	connection->priv = telnet_connection;
 
 	/* initialize telnet connection information */
-	telnet_connection->closed = false;
-	telnet_connection->line_size = 0;
-	telnet_connection->line_cursor = 0;
 	telnet_connection->prompt = strdup("> ");
 	telnet_connection->prompt_visible = true;
 	telnet_connection->state = TELNET_STATE_DATA;
@@ -257,11 +253,6 @@ static int telnet_new_connection(struct connection *connection)
 	telnet_write(connection, "\r", 1);
 	telnet_prompt(connection);
 
-	/* initialize history */
-	for (i = 0; i < TELNET_LINE_HISTORY_SIZE; i++)
-		telnet_connection->history[i] = NULL;
-	telnet_connection->next_history = 0;
-	telnet_connection->current_history = 0;
 	telnet_load_history(telnet_connection);
 
 	log_add_callback(telnet_log_callback, connection);
@@ -624,7 +615,11 @@ static void telnet_auto_complete(struct connection *connection)
 	while ((usr_cmd_pos < t_con->line_cursor) && isspace(t_con->line[usr_cmd_pos]))
 		usr_cmd_pos++;
 
-	/* user command length */
+	/* check user command length */
+	if (t_con->line_cursor < usr_cmd_pos) {
+		telnet_bell(connection);
+		return;
+	}
 	size_t usr_cmd_len = t_con->line_cursor - usr_cmd_pos;
 
 	/* optimize multiple spaces in the user command,
diff --git a/src/target/Makefile.am b/src/target/Makefile.am
index 49e882f..799c3dd 100644
--- a/src/target/Makefile.am
+++ b/src/target/Makefile.am
@@ -1,5 +1,7 @@
 %C%_libtarget_la_LIBADD = %D%/openrisc/libopenrisc.la \
-	%D%/riscv/libriscv.la
+	%D%/riscv/libriscv.la \
+	%D%/xtensa/libxtensa.la \
+	%D%/espressif/libespressif.la
 
 %C%_libtarget_la_CPPFLAGS = $(AM_CPPFLAGS)
 
@@ -260,3 +262,5 @@ ARC_SRC = \
 
 include %D%/openrisc/Makefile.am
 include %D%/riscv/Makefile.am
+include %D%/xtensa/Makefile.am
+include %D%/espressif/Makefile.am
\ No newline at end of file
diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 8838da9..ecd9324 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -2026,9 +2026,13 @@ static int aarch64_deassert_reset(struct target *target)
 		if (target->state != TARGET_HALTED) {
 			LOG_WARNING("%s: ran after reset and before halt ...",
 				target_name(target));
-			retval = target_halt(target);
-			if (retval != ERROR_OK)
-				return retval;
+			if (target_was_examined(target)) {
+				retval = aarch64_halt_one(target, HALT_LAZY);
+				if (retval != ERROR_OK)
+					return retval;
+			} else {
+				target->state = TARGET_UNKNOWN;
+			}
 		}
 	}
 
@@ -2574,20 +2578,13 @@ static int aarch64_examine_first(struct target *target)
 	armv8->debug_ap->memaccess_tck = 10;
 
 	if (!target->dbgbase_set) {
-		target_addr_t dbgbase;
-		/* Get ROM Table base */
-		uint32_t apid;
-		int32_t coreidx = target->coreid;
-		retval = dap_get_debugbase(armv8->debug_ap, &dbgbase, &apid);
-		if (retval != ERROR_OK)
-			return retval;
 		/* Lookup Processor DAP */
-		retval = dap_lookup_cs_component(armv8->debug_ap, dbgbase, ARM_CS_C9_DEVTYPE_CORE_DEBUG,
-				&armv8->debug_base, &coreidx);
+		retval = dap_lookup_cs_component(armv8->debug_ap, ARM_CS_C9_DEVTYPE_CORE_DEBUG,
+				&armv8->debug_base, target->coreid);
 		if (retval != ERROR_OK)
 			return retval;
-		LOG_DEBUG("Detected core %" PRId32 " dbgbase: " TARGET_ADDR_FMT
-				" apid: %08" PRIx32, coreidx, armv8->debug_base, apid);
+		LOG_DEBUG("Detected core %" PRId32 " dbgbase: " TARGET_ADDR_FMT,
+				target->coreid, armv8->debug_base);
 	} else
 		armv8->debug_base = target->dbgbase;
 
diff --git a/src/target/adi_v5_jtag.c b/src/target/adi_v5_jtag.c
index be62580..94ee8cf 100644
--- a/src/target/adi_v5_jtag.c
+++ b/src/target/adi_v5_jtag.c
@@ -532,7 +532,7 @@ static int jtagdp_overrun_check(struct adiv5_dap *dap)
 
 	/* check for overrun condition in the last batch of transactions */
 	if (found_wait) {
-		LOG_INFO("DAP transaction stalled (WAIT) - slowing down");
+		LOG_INFO("DAP transaction stalled (WAIT) - slowing down and resending");
 		/* clear the sticky overrun condition */
 		retval = adi_jtag_scan_inout_check_u32(dap, JTAG_DP_DPACC,
 				DP_CTRL_STAT, DPAP_WRITE,
@@ -574,7 +574,7 @@ static int jtagdp_overrun_check(struct adiv5_dap *dap)
 					retval = ERROR_JTAG_DEVICE_ERROR;
 					break;
 				}
-				LOG_INFO("DAP transaction stalled during replay (WAIT) - resending");
+				LOG_DEBUG("DAP transaction stalled during replay (WAIT) - resending");
 				/* clear the sticky overrun condition */
 				retval = adi_jtag_scan_inout_check_u32(dap, JTAG_DP_DPACC,
 						DP_CTRL_STAT, DPAP_WRITE,
diff --git a/src/target/arm_adi_v5.c b/src/target/arm_adi_v5.c
index 8d6d661..4d5f02b 100644
--- a/src/target/arm_adi_v5.c
+++ b/src/target/arm_adi_v5.c
@@ -893,6 +893,55 @@ static const char *class_description[16] = {
 	[0xF] = "CoreLink, PrimeCell or System component",
 };
 
+#define ARCH_ID(architect, archid) ( \
+	(((architect) << ARM_CS_C9_DEVARCH_ARCHITECT_SHIFT) & ARM_CS_C9_DEVARCH_ARCHITECT_MASK) | \
+	(((archid) << ARM_CS_C9_DEVARCH_ARCHID_SHIFT) & ARM_CS_C9_DEVARCH_ARCHID_MASK) \
+)
+
+static const struct {
+	uint32_t arch_id;
+	const char *description;
+} class0x9_devarch[] = {
+	/* keep same unsorted order as in ARM IHI0029E */
+	{ ARCH_ID(ARM_ID, 0x0A00), "RAS architecture" },
+	{ ARCH_ID(ARM_ID, 0x1A01), "Instrumentation Trace Macrocell (ITM) architecture" },
+	{ ARCH_ID(ARM_ID, 0x1A02), "DWT architecture" },
+	{ ARCH_ID(ARM_ID, 0x1A03), "Flash Patch and Breakpoint unit (FPB) architecture" },
+	{ ARCH_ID(ARM_ID, 0x2A04), "Processor debug architecture (ARMv8-M)" },
+	{ ARCH_ID(ARM_ID, 0x6A05), "Processor debug architecture (ARMv8-R)" },
+	{ ARCH_ID(ARM_ID, 0x0A10), "PC sample-based profiling" },
+	{ ARCH_ID(ARM_ID, 0x4A13), "Embedded Trace Macrocell (ETM) architecture" },
+	{ ARCH_ID(ARM_ID, 0x1A14), "Cross Trigger Interface (CTI) architecture" },
+	{ ARCH_ID(ARM_ID, 0x6A15), "Processor debug architecture (v8.0-A)" },
+	{ ARCH_ID(ARM_ID, 0x7A15), "Processor debug architecture (v8.1-A)" },
+	{ ARCH_ID(ARM_ID, 0x8A15), "Processor debug architecture (v8.2-A)" },
+	{ ARCH_ID(ARM_ID, 0x2A16), "Processor Performance Monitor (PMU) architecture" },
+	{ ARCH_ID(ARM_ID, 0x0A17), "Memory Access Port v2 architecture" },
+	{ ARCH_ID(ARM_ID, 0x0A27), "JTAG Access Port v2 architecture" },
+	{ ARCH_ID(ARM_ID, 0x0A31), "Basic trace router" },
+	{ ARCH_ID(ARM_ID, 0x0A37), "Power requestor" },
+	{ ARCH_ID(ARM_ID, 0x0A47), "Unknown Access Port v2 architecture" },
+	{ ARCH_ID(ARM_ID, 0x0A50), "HSSTP architecture" },
+	{ ARCH_ID(ARM_ID, 0x0A63), "System Trace Macrocell (STM) architecture" },
+	{ ARCH_ID(ARM_ID, 0x0A75), "CoreSight ELA architecture" },
+	{ ARCH_ID(ARM_ID, 0x0AF7), "CoreSight ROM architecture" },
+};
+
+#define DEVARCH_ID_MASK         (ARM_CS_C9_DEVARCH_ARCHITECT_MASK | ARM_CS_C9_DEVARCH_ARCHID_MASK)
+#define DEVARCH_ROM_C_0X9       ARCH_ID(ARM_ID, 0x0AF7)
+
+static const char *class0x9_devarch_description(uint32_t devarch)
+{
+	if (!(devarch & ARM_CS_C9_DEVARCH_PRESENT))
+		return "not present";
+
+	for (unsigned int i = 0; i < ARRAY_SIZE(class0x9_devarch); i++)
+		if ((devarch & DEVARCH_ID_MASK) == class0x9_devarch[i].arch_id)
+			return class0x9_devarch[i].description;
+
+	return "unknown";
+}
+
 static const struct {
 	enum ap_type type;
 	const char *description;
@@ -953,7 +1002,7 @@ int dap_find_ap(struct adiv5_dap *dap, enum ap_type type_to_find, struct adiv5_a
 	return ERROR_FAIL;
 }
 
-int dap_get_debugbase(struct adiv5_ap *ap,
+static int dap_get_debugbase(struct adiv5_ap *ap,
 			target_addr_t *dbgbase, uint32_t *apid)
 {
 	struct adiv5_dap *dap = ap->dap;
@@ -989,109 +1038,89 @@ int dap_get_debugbase(struct adiv5_ap *ap,
 	return ERROR_OK;
 }
 
-int dap_lookup_cs_component(struct adiv5_ap *ap,
-			target_addr_t dbgbase, uint8_t type, target_addr_t *addr, int32_t *idx)
-{
-	uint32_t romentry, entry_offset = 0, devtype;
+/** Holds registers and coordinates of a CoreSight component */
+struct cs_component_vals {
+	struct adiv5_ap *ap;
 	target_addr_t component_base;
-	int retval;
+	uint64_t pid;
+	uint32_t cid;
+	uint32_t devarch;
+	uint32_t devid;
+	uint32_t devtype_memtype;
+};
 
-	dbgbase &= 0xFFFFFFFFFFFFF000ull;
-	*addr = 0;
+/**
+ * Read the CoreSight registers needed during ROM Table Parsing (RTP).
+ *
+ * @param ap             Pointer to AP containing the component.
+ * @param component_base On MEM-AP access method, base address of the component.
+ * @param v              Pointer to the struct holding the value of registers.
+ *
+ * @return ERROR_OK on success, else a fault code.
+ */
+static int rtp_read_cs_regs(struct adiv5_ap *ap, target_addr_t component_base,
+		struct cs_component_vals *v)
+{
+	assert(IS_ALIGNED(component_base, ARM_CS_ALIGN));
+	assert(ap && v);
 
-	do {
-		retval = mem_ap_read_atomic_u32(ap, dbgbase |
-						entry_offset, &romentry);
-		if (retval != ERROR_OK)
-			return retval;
+	uint32_t cid0, cid1, cid2, cid3;
+	uint32_t pid0, pid1, pid2, pid3, pid4;
+	int retval = ERROR_OK;
 
-		component_base = dbgbase + (target_addr_t)(romentry & ARM_CS_ROMENTRY_OFFSET_MASK);
+	v->ap = ap;
+	v->component_base = component_base;
 
-		if (romentry & ARM_CS_ROMENTRY_PRESENT) {
-			uint32_t c_cid1;
-			retval = mem_ap_read_atomic_u32(ap, component_base + ARM_CS_CIDR1, &c_cid1);
-			if (retval != ERROR_OK) {
-				LOG_ERROR("Can't read component with base address " TARGET_ADDR_FMT
-					  ", the corresponding core might be turned off", component_base);
-				return retval;
-			}
-			unsigned int class = (c_cid1 & ARM_CS_CIDR1_CLASS_MASK) >> ARM_CS_CIDR1_CLASS_SHIFT;
-			if (class == ARM_CS_CLASS_0X1_ROM_TABLE) {
-				retval = dap_lookup_cs_component(ap, component_base,
-							type, addr, idx);
-				if (retval == ERROR_OK)
-					break;
-				if (retval != ERROR_TARGET_RESOURCE_NOT_AVAILABLE)
-					return retval;
-			}
+	/* sort by offset to gain speed */
 
-			retval = mem_ap_read_atomic_u32(ap, component_base + ARM_CS_C9_DEVTYPE, &devtype);
-			if (retval != ERROR_OK)
-				return retval;
-			if ((devtype & ARM_CS_C9_DEVTYPE_MASK) == type) {
-				if (!*idx) {
-					*addr = component_base;
-					break;
-				} else
-					(*idx)--;
-			}
-		}
-		entry_offset += 4;
-	} while ((romentry > 0) && (entry_offset < 0xf00));
+	/*
+	 * Registers DEVARCH, DEVID and DEVTYPE are valid on Class 0x9 devices
+	 * only, but are at offset above 0xf00, so can be read on any device
+	 * without triggering error. Read them for eventual use on Class 0x9.
+	 */
+	if (retval == ERROR_OK)
+		retval = mem_ap_read_u32(ap, component_base + ARM_CS_C9_DEVARCH, &v->devarch);
 
-	if (!*addr)
-		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	if (retval == ERROR_OK)
+		retval = mem_ap_read_u32(ap, component_base + ARM_CS_C9_DEVID, &v->devid);
 
-	return ERROR_OK;
-}
+	/* Same address as ARM_CS_C1_MEMTYPE */
+	if (retval == ERROR_OK)
+		retval = mem_ap_read_u32(ap, component_base + ARM_CS_C9_DEVTYPE, &v->devtype_memtype);
 
-static int dap_read_part_id(struct adiv5_ap *ap, target_addr_t component_base, uint32_t *cid, uint64_t *pid)
-{
-	assert(IS_ALIGNED(component_base, ARM_CS_ALIGN));
-	assert(ap && cid && pid);
+	if (retval == ERROR_OK)
+		retval = mem_ap_read_u32(ap, component_base + ARM_CS_PIDR4, &pid4);
 
-	uint32_t cid0, cid1, cid2, cid3;
-	uint32_t pid0, pid1, pid2, pid3, pid4;
-	int retval;
+	if (retval == ERROR_OK)
+		retval = mem_ap_read_u32(ap, component_base + ARM_CS_PIDR0, &pid0);
+	if (retval == ERROR_OK)
+		retval = mem_ap_read_u32(ap, component_base + ARM_CS_PIDR1, &pid1);
+	if (retval == ERROR_OK)
+		retval = mem_ap_read_u32(ap, component_base + ARM_CS_PIDR2, &pid2);
+	if (retval == ERROR_OK)
+		retval = mem_ap_read_u32(ap, component_base + ARM_CS_PIDR3, &pid3);
 
-	/* IDs are in last 4K section */
-	retval = mem_ap_read_u32(ap, component_base + ARM_CS_PIDR0, &pid0);
-	if (retval != ERROR_OK)
-		return retval;
-	retval = mem_ap_read_u32(ap, component_base + ARM_CS_PIDR1, &pid1);
-	if (retval != ERROR_OK)
-		return retval;
-	retval = mem_ap_read_u32(ap, component_base + ARM_CS_PIDR2, &pid2);
-	if (retval != ERROR_OK)
-		return retval;
-	retval = mem_ap_read_u32(ap, component_base + ARM_CS_PIDR3, &pid3);
-	if (retval != ERROR_OK)
-		return retval;
-	retval = mem_ap_read_u32(ap, component_base + ARM_CS_PIDR4, &pid4);
-	if (retval != ERROR_OK)
-		return retval;
-	retval = mem_ap_read_u32(ap, component_base + ARM_CS_CIDR0, &cid0);
-	if (retval != ERROR_OK)
-		return retval;
-	retval = mem_ap_read_u32(ap, component_base + ARM_CS_CIDR1, &cid1);
-	if (retval != ERROR_OK)
-		return retval;
-	retval = mem_ap_read_u32(ap, component_base + ARM_CS_CIDR2, &cid2);
-	if (retval != ERROR_OK)
-		return retval;
-	retval = mem_ap_read_u32(ap, component_base + ARM_CS_CIDR3, &cid3);
-	if (retval != ERROR_OK)
-		return retval;
+	if (retval == ERROR_OK)
+		retval = mem_ap_read_u32(ap, component_base + ARM_CS_CIDR0, &cid0);
+	if (retval == ERROR_OK)
+		retval = mem_ap_read_u32(ap, component_base + ARM_CS_CIDR1, &cid1);
+	if (retval == ERROR_OK)
+		retval = mem_ap_read_u32(ap, component_base + ARM_CS_CIDR2, &cid2);
+	if (retval == ERROR_OK)
+		retval = mem_ap_read_u32(ap, component_base + ARM_CS_CIDR3, &cid3);
 
-	retval = dap_run(ap->dap);
-	if (retval != ERROR_OK)
+	if (retval == ERROR_OK)
+		retval = dap_run(ap->dap);
+	if (retval != ERROR_OK) {
+		LOG_DEBUG("Failed read CoreSight registers");
 		return retval;
+	}
 
-	*cid = (cid3 & 0xff) << 24
+	v->cid = (cid3 & 0xff) << 24
 			| (cid2 & 0xff) << 16
 			| (cid1 & 0xff) << 8
 			| (cid0 & 0xff);
-	*pid = (uint64_t)(pid4 & 0xff) << 32
+	v->pid = (uint64_t)(pid4 & 0xff) << 32
 			| (pid3 & 0xff) << 24
 			| (pid2 & 0xff) << 16
 			| (pid1 & 0xff) << 8
@@ -1397,122 +1426,269 @@ static int dap_devtype_display(struct command_invocation *cmd, uint32_t devtype)
 	return ERROR_OK;
 }
 
-static int dap_rom_display(struct command_invocation *cmd,
-				struct adiv5_ap *ap, target_addr_t dbgbase, int depth)
-{
-	int retval;
-	uint64_t pid;
-	uint32_t cid;
-	char tabs[16] = "";
+/**
+ * Actions/operations to be executed while parsing ROM tables.
+ */
+struct rtp_ops {
+	/**
+	 * Executed at the start of a new MEM-AP, typically to print the MEM-AP header.
+	 * @param retval    Error encountered while reading AP.
+	 * @param ap        Pointer to AP.
+	 * @param dbgbase   Value of MEM-AP Debug Base Address register.
+	 * @param apid      Value of MEM-AP IDR Identification Register.
+	 * @param priv      Pointer to private data.
+	 * @return          ERROR_OK on success, else a fault code.
+	 */
+	int (*mem_ap_header)(int retval, struct adiv5_ap *ap, uint64_t dbgbase,
+			uint32_t apid, void *priv);
+	/**
+	 * Executed when a CoreSight component is parsed, typically to print
+	 * information on the component.
+	 * @param retval    Error encountered while reading component's registers.
+	 * @param v         Pointer to a container of the component's registers.
+	 * @param depth     The current depth level of ROM table.
+	 * @param priv      Pointer to private data.
+	 * @return          ERROR_OK on success, else a fault code.
+	 */
+	int (*cs_component)(int retval, struct cs_component_vals *v, int depth, void *priv);
+	/**
+	 * Executed for each entry of a ROM table, typically to print the entry
+	 * and information about validity or end-of-table mark.
+	 * @param retval    Error encountered while reading the ROM table entry.
+	 * @param depth     The current depth level of ROM table.
+	 * @param offset    The offset of the entry in the ROM table.
+	 * @param romentry  The value of the ROM table entry.
+	 * @param priv      Pointer to private data.
+	 * @return          ERROR_OK on success, else a fault code.
+	 */
+	int (*rom_table_entry)(int retval, int depth, unsigned int offset, uint64_t romentry,
+			void *priv);
+	/**
+	 * Private data
+	 */
+	void *priv;
+};
 
-	if (depth > 16) {
-		command_print(cmd, "\tTables too deep");
-		return ERROR_FAIL;
-	}
+/**
+ * Wrapper around struct rtp_ops::mem_ap_header.
+ * Input parameter @a retval is propagated.
+ */
+static int rtp_ops_mem_ap_header(const struct rtp_ops *ops,
+		int retval, struct adiv5_ap *ap, uint64_t dbgbase, uint32_t apid)
+{
+	if (!ops->mem_ap_header)
+		return retval;
 
-	if (depth)
-		snprintf(tabs, sizeof(tabs), "[L%02d] ", depth);
+	int retval1 = ops->mem_ap_header(retval, ap, dbgbase, apid, ops->priv);
+	if (retval != ERROR_OK)
+		return retval;
+	return retval1;
+}
 
-	target_addr_t base_addr = dbgbase & 0xFFFFFFFFFFFFF000ull;
-	command_print(cmd, "\t\tComponent base address " TARGET_ADDR_FMT, base_addr);
+/**
+ * Wrapper around struct rtp_ops::cs_component.
+ * Input parameter @a retval is propagated.
+ */
+static int rtp_ops_cs_component(const struct rtp_ops *ops,
+		int retval, struct cs_component_vals *v, int depth)
+{
+	if (!ops->cs_component)
+		return retval;
 
-	retval = dap_read_part_id(ap, base_addr, &cid, &pid);
-	if (retval != ERROR_OK) {
-		command_print(cmd, "\t\tCan't read component, the corresponding core might be turned off");
-		return ERROR_OK; /* Don't abort recursion */
-	}
+	int retval1 = ops->cs_component(retval, v, depth, ops->priv);
+	if (retval != ERROR_OK)
+		return retval;
+	return retval1;
+}
 
-	if (!is_valid_arm_cs_cidr(cid)) {
-		command_print(cmd, "\t\tInvalid CID 0x%08" PRIx32, cid);
-		return ERROR_OK; /* Don't abort recursion */
-	}
+/**
+ * Wrapper around struct rtp_ops::rom_table_entry.
+ * Input parameter @a retval is propagated.
+ */
+static int rtp_ops_rom_table_entry(const struct rtp_ops *ops,
+		int retval, int depth, unsigned int offset, uint64_t romentry)
+{
+	if (!ops->rom_table_entry)
+		return retval;
 
-	/* component may take multiple 4K pages */
-	uint32_t size = ARM_CS_PIDR_SIZE(pid);
-	if (size > 0)
-		command_print(cmd, "\t\tStart address " TARGET_ADDR_FMT, base_addr - 0x1000 * size);
+	int retval1 = ops->rom_table_entry(retval, depth, offset, romentry, ops->priv);
+	if (retval != ERROR_OK)
+		return retval;
+	return retval1;
+}
 
-	command_print(cmd, "\t\tPeripheral ID 0x%010" PRIx64, pid);
+/* Broken ROM tables can have circular references. Stop after a while */
+#define ROM_TABLE_MAX_DEPTH (16)
 
-	const unsigned int class = (cid & ARM_CS_CIDR_CLASS_MASK) >> ARM_CS_CIDR_CLASS_SHIFT;
-	const unsigned int part_num = ARM_CS_PIDR_PART(pid);
-	unsigned int designer_id = ARM_CS_PIDR_DESIGNER(pid);
+/**
+ * Value used only during lookup of a CoreSight component in ROM table.
+ * Return CORESIGHT_COMPONENT_FOUND when component is found.
+ * Return ERROR_OK when component is not found yet.
+ * Return any other ERROR_* in case of error.
+ */
+#define CORESIGHT_COMPONENT_FOUND (1)
 
-	if (pid & ARM_CS_PIDR_JEDEC) {
-		/* JEP106 code */
-		command_print(cmd, "\t\tDesigner is 0x%03x, %s",
-				designer_id, jep106_manufacturer(designer_id));
-	} else {
-		/* Legacy ASCII ID, clear invalid bits */
-		designer_id &= 0x7f;
-		command_print(cmd, "\t\tDesigner ASCII code 0x%02x, %s",
-				designer_id, designer_id == 0x41 ? "ARM" : "<unknown>");
-	}
+static int rtp_cs_component(const struct rtp_ops *ops,
+		struct adiv5_ap *ap, target_addr_t dbgbase, int depth);
 
-	const struct dap_part_nums *partnum = pidr_to_part_num(designer_id, part_num);
-	command_print(cmd, "\t\tPart is 0x%03x, %s %s", part_num, partnum->type, partnum->full);
-	command_print(cmd, "\t\tComponent class is 0x%x, %s", class, class_description[class]);
+static int rtp_rom_loop(const struct rtp_ops *ops,
+		struct adiv5_ap *ap, target_addr_t base_address, int depth,
+		unsigned int width, unsigned int max_entries)
+{
+	assert(IS_ALIGNED(base_address, ARM_CS_ALIGN));
+
+	unsigned int offset = 0;
+	while (max_entries--) {
+		uint64_t romentry;
+		uint32_t romentry_low, romentry_high;
+		target_addr_t component_base;
+		unsigned int saved_offset = offset;
+
+		int retval = mem_ap_read_u32(ap, base_address + offset, &romentry_low);
+		offset += 4;
+		if (retval == ERROR_OK && width == 64) {
+			retval = mem_ap_read_u32(ap, base_address + offset, &romentry_high);
+			offset += 4;
+		}
+		if (retval == ERROR_OK)
+			retval = dap_run(ap->dap);
+		if (retval != ERROR_OK) {
+			LOG_DEBUG("Failed read ROM table entry");
+			return retval;
+		}
 
-	if (class == ARM_CS_CLASS_0X1_ROM_TABLE) {
-		uint32_t memtype;
-		retval = mem_ap_read_atomic_u32(ap, base_addr + ARM_CS_C1_MEMTYPE, &memtype);
+		if (width == 64) {
+			romentry = (((uint64_t)romentry_high) << 32) | romentry_low;
+			component_base = base_address +
+				((((uint64_t)romentry_high) << 32) | (romentry_low & ARM_CS_ROMENTRY_OFFSET_MASK));
+		} else {
+			romentry = romentry_low;
+			/* "romentry" is signed */
+			component_base = base_address + (int32_t)(romentry_low & ARM_CS_ROMENTRY_OFFSET_MASK);
+			if (!is_64bit_ap(ap))
+				component_base = (uint32_t)component_base;
+		}
+		retval = rtp_ops_rom_table_entry(ops, retval, depth, saved_offset, romentry);
 		if (retval != ERROR_OK)
 			return retval;
 
-		if (memtype & ARM_CS_C1_MEMTYPE_SYSMEM_MASK)
-			command_print(cmd, "\t\tMEMTYPE system memory present on bus");
-		else
-			command_print(cmd, "\t\tMEMTYPE system memory not present: dedicated debug bus");
+		if (romentry == 0) {
+			/* End of ROM table */
+			break;
+		}
 
-		/* Read ROM table entries from base address until we get 0x00000000 or reach the reserved area */
-		for (uint16_t entry_offset = 0; entry_offset < 0xF00; entry_offset += 4) {
-			uint32_t romentry;
-			retval = mem_ap_read_atomic_u32(ap, base_addr | entry_offset, &romentry);
-			if (retval != ERROR_OK)
-				return retval;
-			command_print(cmd, "\t%sROMTABLE[0x%x] = 0x%" PRIx32 "",
-					tabs, entry_offset, romentry);
-			if (romentry & ARM_CS_ROMENTRY_PRESENT) {
-				/* Recurse. "romentry" is signed */
-				retval = dap_rom_display(cmd, ap, base_addr + (int32_t)(romentry & ARM_CS_ROMENTRY_OFFSET_MASK),
-										 depth + 1);
-				if (retval != ERROR_OK)
-					return retval;
-			} else if (romentry != 0) {
-				command_print(cmd, "\t\tComponent not present");
-			} else {
-				command_print(cmd, "\t%s\tEnd of ROM table", tabs);
-				break;
-			}
+		if (!(romentry & ARM_CS_ROMENTRY_PRESENT))
+			continue;
+
+		/* Recurse */
+		retval = rtp_cs_component(ops, ap, component_base, depth + 1);
+		if (retval == CORESIGHT_COMPONENT_FOUND)
+			return CORESIGHT_COMPONENT_FOUND;
+		if (retval != ERROR_OK) {
+			/* TODO: do we need to send an ABORT before continuing? */
+			LOG_DEBUG("Ignore error parsing CoreSight component");
+			continue;
 		}
-	} else if (class == ARM_CS_CLASS_0X9_CS_COMPONENT) {
-		uint32_t devtype;
-		retval = mem_ap_read_atomic_u32(ap, base_addr + ARM_CS_C9_DEVTYPE, &devtype);
-		if (retval != ERROR_OK)
-			return retval;
+	}
 
-		retval = dap_devtype_display(cmd, devtype);
-		if (retval != ERROR_OK)
-			return retval;
+	return ERROR_OK;
+}
 
-		/* REVISIT also show ARM_CS_C9_DEVID */
+static int rtp_cs_component(const struct rtp_ops *ops,
+		struct adiv5_ap *ap, target_addr_t base_address, int depth)
+{
+	struct cs_component_vals v;
+	int retval;
+
+	assert(IS_ALIGNED(base_address, ARM_CS_ALIGN));
+
+	if (depth > ROM_TABLE_MAX_DEPTH)
+		retval = ERROR_FAIL;
+	else
+		retval = rtp_read_cs_regs(ap, base_address, &v);
+
+	retval = rtp_ops_cs_component(ops, retval, &v, depth);
+	if (retval == CORESIGHT_COMPONENT_FOUND)
+		return CORESIGHT_COMPONENT_FOUND;
+	if (retval != ERROR_OK)
+		return ERROR_OK; /* Don't abort recursion */
+
+	if (!is_valid_arm_cs_cidr(v.cid))
+		return ERROR_OK; /* Don't abort recursion */
+
+	const unsigned int class = ARM_CS_CIDR_CLASS(v.cid);
+
+	if (class == ARM_CS_CLASS_0X1_ROM_TABLE)
+		return rtp_rom_loop(ops, ap, base_address, depth, 32, 960);
+
+	if (class == ARM_CS_CLASS_0X9_CS_COMPONENT) {
+		if ((v.devarch & ARM_CS_C9_DEVARCH_PRESENT) == 0)
+			return ERROR_OK;
+
+		/* quit if not ROM table */
+		if ((v.devarch & DEVARCH_ID_MASK) != DEVARCH_ROM_C_0X9)
+			return ERROR_OK;
+
+		if ((v.devid & ARM_CS_C9_DEVID_FORMAT_MASK) == ARM_CS_C9_DEVID_FORMAT_64BIT)
+			return rtp_rom_loop(ops, ap, base_address, depth, 64, 256);
+		else
+			return rtp_rom_loop(ops, ap, base_address, depth, 32, 512);
 	}
 
+	/* Class other than 0x1 and 0x9 */
 	return ERROR_OK;
 }
 
-int dap_info_command(struct command_invocation *cmd,
-		struct adiv5_ap *ap)
+static int rtp_ap(const struct rtp_ops *ops, struct adiv5_ap *ap)
 {
 	int retval;
 	uint32_t apid;
-	target_addr_t dbgbase;
-	target_addr_t dbgaddr;
+	target_addr_t dbgbase, invalid_entry;
 
 	/* Now we read ROM table ID registers, ref. ARM IHI 0029B sec  */
 	retval = dap_get_debugbase(ap, &dbgbase, &apid);
 	if (retval != ERROR_OK)
 		return retval;
+	retval = rtp_ops_mem_ap_header(ops, retval, ap, dbgbase, apid);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (apid == 0)
+		return ERROR_FAIL;
+
+	/* NOTE: a MEM-AP may have a single CoreSight component that's
+	 * not a ROM table ... or have no such components at all.
+	 */
+	const unsigned int class = (apid & AP_REG_IDR_CLASS_MASK) >> AP_REG_IDR_CLASS_SHIFT;
+
+	if (class == AP_REG_IDR_CLASS_MEM_AP) {
+		if (is_64bit_ap(ap))
+			invalid_entry = 0xFFFFFFFFFFFFFFFFull;
+		else
+			invalid_entry = 0xFFFFFFFFul;
+
+		if (dbgbase != invalid_entry && (dbgbase & 0x3) != 0x2) {
+			retval = rtp_cs_component(ops, ap, dbgbase & 0xFFFFFFFFFFFFF000ull, 0);
+			if (retval == CORESIGHT_COMPONENT_FOUND)
+				return CORESIGHT_COMPONENT_FOUND;
+		}
+	}
+
+	return ERROR_OK;
+}
+
+/* Actions for command "dap info" */
+
+static int dap_info_mem_ap_header(int retval, struct adiv5_ap *ap,
+		target_addr_t dbgbase, uint32_t apid, void *priv)
+{
+	struct command_invocation *cmd = priv;
+	target_addr_t invalid_entry;
+
+	if (retval != ERROR_OK) {
+		command_print(cmd, "\t\tCan't read MEM-AP, the corresponding core might be turned off");
+		return retval;
+	}
 
 	command_print(cmd, "AP ID register 0x%8.8" PRIx32, apid);
 	if (apid == 0) {
@@ -1529,27 +1705,221 @@ int dap_info_command(struct command_invocation *cmd,
 
 	if (class == AP_REG_IDR_CLASS_MEM_AP) {
 		if (is_64bit_ap(ap))
-			dbgaddr = 0xFFFFFFFFFFFFFFFFull;
+			invalid_entry = 0xFFFFFFFFFFFFFFFFull;
 		else
-			dbgaddr = 0xFFFFFFFFul;
+			invalid_entry = 0xFFFFFFFFul;
 
 		command_print(cmd, "MEM-AP BASE " TARGET_ADDR_FMT, dbgbase);
 
-		if (dbgbase == dbgaddr || (dbgbase & 0x3) == 0x2) {
+		if (dbgbase == invalid_entry || (dbgbase & 0x3) == 0x2) {
 			command_print(cmd, "\tNo ROM table present");
 		} else {
 			if (dbgbase & 0x01)
 				command_print(cmd, "\tValid ROM table present");
 			else
 				command_print(cmd, "\tROM table in legacy format");
+		}
+	}
+
+	return ERROR_OK;
+}
+
+static int dap_info_cs_component(int retval, struct cs_component_vals *v, int depth, void *priv)
+{
+	struct command_invocation *cmd = priv;
+
+	if (depth > ROM_TABLE_MAX_DEPTH) {
+		command_print(cmd, "\tTables too deep");
+		return ERROR_FAIL;
+	}
+
+	command_print(cmd, "\t\tComponent base address " TARGET_ADDR_FMT, v->component_base);
+
+	if (retval != ERROR_OK) {
+		command_print(cmd, "\t\tCan't read component, the corresponding core might be turned off");
+		return retval;
+	}
+
+	if (!is_valid_arm_cs_cidr(v->cid)) {
+		command_print(cmd, "\t\tInvalid CID 0x%08" PRIx32, v->cid);
+		return ERROR_OK; /* Don't abort recursion */
+	}
+
+	/* component may take multiple 4K pages */
+	uint32_t size = ARM_CS_PIDR_SIZE(v->pid);
+	if (size > 0)
+		command_print(cmd, "\t\tStart address " TARGET_ADDR_FMT, v->component_base - 0x1000 * size);
+
+	command_print(cmd, "\t\tPeripheral ID 0x%010" PRIx64, v->pid);
+
+	const unsigned int part_num = ARM_CS_PIDR_PART(v->pid);
+	unsigned int designer_id = ARM_CS_PIDR_DESIGNER(v->pid);
+
+	if (v->pid & ARM_CS_PIDR_JEDEC) {
+		/* JEP106 code */
+		command_print(cmd, "\t\tDesigner is 0x%03x, %s",
+				designer_id, jep106_manufacturer(designer_id));
+	} else {
+		/* Legacy ASCII ID, clear invalid bits */
+		designer_id &= 0x7f;
+		command_print(cmd, "\t\tDesigner ASCII code 0x%02x, %s",
+				designer_id, designer_id == 0x41 ? "ARM" : "<unknown>");
+	}
 
-			dap_rom_display(cmd, ap, dbgbase & 0xFFFFFFFFFFFFF000ull, 0);
+	const struct dap_part_nums *partnum = pidr_to_part_num(designer_id, part_num);
+	command_print(cmd, "\t\tPart is 0x%03x, %s %s", part_num, partnum->type, partnum->full);
+
+	const unsigned int class = ARM_CS_CIDR_CLASS(v->cid);
+	command_print(cmd, "\t\tComponent class is 0x%x, %s", class, class_description[class]);
+
+	if (class == ARM_CS_CLASS_0X1_ROM_TABLE) {
+		if (v->devtype_memtype & ARM_CS_C1_MEMTYPE_SYSMEM_MASK)
+			command_print(cmd, "\t\tMEMTYPE system memory present on bus");
+		else
+			command_print(cmd, "\t\tMEMTYPE system memory not present: dedicated debug bus");
+		return ERROR_OK;
+	}
+
+	if (class == ARM_CS_CLASS_0X9_CS_COMPONENT) {
+		dap_devtype_display(cmd, v->devtype_memtype);
+
+		/* REVISIT also show ARM_CS_C9_DEVID */
+
+		if ((v->devarch & ARM_CS_C9_DEVARCH_PRESENT) == 0)
+			return ERROR_OK;
+
+		unsigned int architect_id = ARM_CS_C9_DEVARCH_ARCHITECT(v->devarch);
+		unsigned int revision = ARM_CS_C9_DEVARCH_REVISION(v->devarch);
+		command_print(cmd, "\t\tDev Arch is 0x%08" PRIx32 ", %s \"%s\" rev.%u", v->devarch,
+				jep106_manufacturer(architect_id), class0x9_devarch_description(v->devarch),
+				revision);
+
+		if ((v->devarch & DEVARCH_ID_MASK) == DEVARCH_ROM_C_0X9) {
+			command_print(cmd, "\t\tType is ROM table");
+
+			if (v->devid & ARM_CS_C9_DEVID_SYSMEM_MASK)
+				command_print(cmd, "\t\tMEMTYPE system memory present on bus");
+			else
+				command_print(cmd, "\t\tMEMTYPE system memory not present: dedicated debug bus");
 		}
+		return ERROR_OK;
 	}
 
+	/* Class other than 0x1 and 0x9 */
 	return ERROR_OK;
 }
 
+static int dap_info_rom_table_entry(int retval, int depth,
+		unsigned int offset, uint64_t romentry, void *priv)
+{
+	struct command_invocation *cmd = priv;
+	char tabs[16] = "";
+
+	if (depth)
+		snprintf(tabs, sizeof(tabs), "[L%02d] ", depth);
+
+	if (retval != ERROR_OK) {
+		command_print(cmd, "\t%sROMTABLE[0x%x] Read error", tabs, offset);
+		command_print(cmd, "\t\tUnable to continue");
+		command_print(cmd, "\t%s\tStop parsing of ROM table", tabs);
+		return retval;
+	}
+
+	command_print(cmd, "\t%sROMTABLE[0x%x] = 0x%08" PRIx64,
+			tabs, offset, romentry);
+
+	if (romentry == 0) {
+		command_print(cmd, "\t%s\tEnd of ROM table", tabs);
+		return ERROR_OK;
+	}
+
+	if (!(romentry & ARM_CS_ROMENTRY_PRESENT)) {
+		command_print(cmd, "\t\tComponent not present");
+		return ERROR_OK;
+	}
+
+	return ERROR_OK;
+}
+
+int dap_info_command(struct command_invocation *cmd, struct adiv5_ap *ap)
+{
+	struct rtp_ops dap_info_ops = {
+		.mem_ap_header   = dap_info_mem_ap_header,
+		.cs_component    = dap_info_cs_component,
+		.rom_table_entry = dap_info_rom_table_entry,
+		.priv            = cmd,
+	};
+
+	return rtp_ap(&dap_info_ops, ap);
+}
+
+/* Actions for dap_lookup_cs_component() */
+
+struct dap_lookup_data {
+	/* input */
+	unsigned int idx;
+	unsigned int type;
+	/* output */
+	uint64_t component_base;
+};
+
+static int dap_lookup_cs_component_cs_component(int retval,
+		struct cs_component_vals *v, int depth, void *priv)
+{
+	struct dap_lookup_data *lookup = priv;
+
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (!is_valid_arm_cs_cidr(v->cid))
+		return ERROR_OK;
+
+	const unsigned int class = ARM_CS_CIDR_CLASS(v->cid);
+	if (class != ARM_CS_CLASS_0X9_CS_COMPONENT)
+		return ERROR_OK;
+
+	if ((v->devtype_memtype & ARM_CS_C9_DEVTYPE_MASK) != lookup->type)
+		return ERROR_OK;
+
+	if (lookup->idx) {
+		/* search for next one */
+		--lookup->idx;
+		return ERROR_OK;
+	}
+
+	/* Found! */
+	lookup->component_base = v->component_base;
+	return CORESIGHT_COMPONENT_FOUND;
+}
+
+int dap_lookup_cs_component(struct adiv5_ap *ap, uint8_t type,
+		target_addr_t *addr, int32_t core_id)
+{
+	struct dap_lookup_data lookup = {
+		.type = type,
+		.idx  = core_id,
+	};
+	struct rtp_ops dap_lookup_cs_component_ops = {
+		.mem_ap_header   = NULL,
+		.cs_component    = dap_lookup_cs_component_cs_component,
+		.rom_table_entry = NULL,
+		.priv            = &lookup,
+	};
+
+	int retval = rtp_ap(&dap_lookup_cs_component_ops, ap);
+	if (retval == CORESIGHT_COMPONENT_FOUND) {
+		LOG_DEBUG("CS lookup found at 0x%" PRIx64, lookup.component_base);
+		*addr = lookup.component_base;
+		return ERROR_OK;
+	}
+	if (retval != ERROR_OK) {
+		LOG_DEBUG("CS lookup error %d", retval);
+		return retval;
+	}
+	LOG_DEBUG("CS lookup not found");
+	return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+}
+
 enum adiv5_cfg_param {
 	CFG_DAP,
 	CFG_AP_NUM,
@@ -1568,6 +1938,8 @@ static const struct jim_nvp nvp_config_opts[] = {
 static int adiv5_jim_spot_configure(struct jim_getopt_info *goi,
 		struct adiv5_dap **dap_p, int *ap_num_p, uint32_t *base_p)
 {
+	assert(dap_p && ap_num_p);
+
 	if (!goi->argc)
 		return JIM_OK;
 
@@ -1672,6 +2044,10 @@ int adiv5_jim_configure(struct target *target, struct jim_getopt_info *goi)
 	pc = (struct adiv5_private_config *)target->private_config;
 	if (!pc) {
 		pc = calloc(1, sizeof(struct adiv5_private_config));
+		if (!pc) {
+			LOG_ERROR("Out of memory");
+			return JIM_ERR;
+		}
 		pc->ap_num = DP_APSEL_INVALID;
 		target->private_config = pc;
 	}
diff --git a/src/target/arm_adi_v5.h b/src/target/arm_adi_v5.h
index 5c598f1..8c9a60f 100644
--- a/src/target/arm_adi_v5.h
+++ b/src/target/arm_adi_v5.h
@@ -619,10 +619,6 @@ int mem_ap_init(struct adiv5_ap *ap);
 /* Invalidate cached DP select and cached TAR and CSW of all APs */
 void dap_invalidate_cache(struct adiv5_dap *dap);
 
-/* Probe the AP for ROM Table location */
-int dap_get_debugbase(struct adiv5_ap *ap,
-			target_addr_t *dbgbase, uint32_t *apid);
-
 /* Probe Access Ports to find a particular type */
 int dap_find_ap(struct adiv5_dap *dap,
 			enum ap_type type_to_find,
@@ -641,7 +637,7 @@ static inline bool dap_is_multidrop(struct adiv5_dap *dap)
 
 /* Lookup CoreSight component */
 int dap_lookup_cs_component(struct adiv5_ap *ap,
-			target_addr_t dbgbase, uint8_t type, target_addr_t *addr, int32_t *idx);
+			uint8_t type, target_addr_t *addr, int32_t idx);
 
 struct target;
 
diff --git a/src/target/arm_coresight.h b/src/target/arm_coresight.h
index a08f4fb..58139dc 100644
--- a/src/target/arm_coresight.h
+++ b/src/target/arm_coresight.h
@@ -44,13 +44,10 @@
 #define ARM_CS_CIDR3                            (0xFFC)
 
 #define ARM_CS_CIDR_CLASS_MASK                  (0x0000F000)
-#define ARM_CS_CIDR_CLASS_SHIFT                 (12)
+#define ARM_CS_CIDR_CLASS(cidr)                 (((cidr) >> 12) & 0x000F)
 #define ARM_CS_CLASS_0X1_ROM_TABLE              (0x1)
 #define ARM_CS_CLASS_0X9_CS_COMPONENT           (0x9)
 
-#define ARM_CS_CIDR1_CLASS_MASK                 (0x000000F0)
-#define ARM_CS_CIDR1_CLASS_SHIFT                (4)
-
 static inline bool is_valid_arm_cs_cidr(uint32_t cidr)
 {
 	return (cidr & ~ARM_CS_CIDR_CLASS_MASK) == 0xB105000D;
@@ -66,6 +63,10 @@ static inline bool is_valid_arm_cs_cidr(uint32_t cidr)
 #define ARM_CS_C9_DEVARCH_PRESENT               BIT(20)
 #define ARM_CS_C9_DEVARCH_ARCHITECT_MASK        (0xFFE00000)
 #define ARM_CS_C9_DEVARCH_ARCHITECT_SHIFT       (21)
+#define ARM_CS_C9_DEVARCH_REVISION(devarch)     \
+	(((devarch) & ARM_CS_C9_DEVARCH_REVISION_MASK) >> ARM_CS_C9_DEVARCH_REVISION_SHIFT)
+#define ARM_CS_C9_DEVARCH_ARCHITECT(devarch)    \
+	(((devarch) & ARM_CS_C9_DEVARCH_ARCHITECT_MASK) >> ARM_CS_C9_DEVARCH_ARCHITECT_SHIFT)
 
 #define ARM_CS_C9_DEVID                         (0xFC8)
 
diff --git a/src/target/armv7a.c b/src/target/armv7a.c
index d564f19..1e425aa 100644
--- a/src/target/armv7a.c
+++ b/src/target/armv7a.c
@@ -112,7 +112,7 @@ static int armv7a_read_midr(struct target *target)
 	armv7a->arch = (midr >> 16) & 0xf;
 	armv7a->variant = (midr >> 20) & 0xf;
 	armv7a->implementor = (midr >> 24) & 0xff;
-	LOG_INFO("%s rev %" PRIx32 ", partnum %" PRIx32 ", arch %" PRIx32
+	LOG_DEBUG("%s rev %" PRIx32 ", partnum %" PRIx32 ", arch %" PRIx32
 			 ", variant %" PRIx32 ", implementor %" PRIx32,
 		 target->cmd_name,
 		 armv7a->rev,
diff --git a/src/target/cortex_a.c b/src/target/cortex_a.c
index 2dc1091..20b2e51 100644
--- a/src/target/cortex_a.c
+++ b/src/target/cortex_a.c
@@ -2905,18 +2905,11 @@ static int cortex_a_examine_first(struct target *target)
 	armv7a->debug_ap->memaccess_tck = 80;
 
 	if (!target->dbgbase_set) {
-		target_addr_t dbgbase;
-		/* Get ROM Table base */
-		uint32_t apid;
-		int32_t coreidx = target->coreid;
 		LOG_DEBUG("%s's dbgbase is not set, trying to detect using the ROM table",
 			  target->cmd_name);
-		retval = dap_get_debugbase(armv7a->debug_ap, &dbgbase, &apid);
-		if (retval != ERROR_OK)
-			return retval;
 		/* Lookup Processor DAP */
-		retval = dap_lookup_cs_component(armv7a->debug_ap, dbgbase, ARM_CS_C9_DEVTYPE_CORE_DEBUG,
-				&armv7a->debug_base, &coreidx);
+		retval = dap_lookup_cs_component(armv7a->debug_ap, ARM_CS_C9_DEVTYPE_CORE_DEBUG,
+				&armv7a->debug_base, target->coreid);
 		if (retval != ERROR_OK) {
 			LOG_ERROR("Can't detect %s's dbgbase from the ROM table; you need to specify it explicitly.",
 				  target->cmd_name);
diff --git a/src/target/espressif/Makefile.am b/src/target/espressif/Makefile.am
new file mode 100644
index 0000000..c681e09
--- /dev/null
+++ b/src/target/espressif/Makefile.am
@@ -0,0 +1,6 @@
+noinst_LTLIBRARIES += %D%/libespressif.la
+%C%_libespressif_la_SOURCES = \
+       %D%/esp_xtensa.c \
+       %D%/esp_xtensa.h \
+       %D%/esp32s2.c \
+       %D%/esp32s2.h
diff --git a/src/target/espressif/esp32s2.c b/src/target/espressif/esp32s2.c
new file mode 100644
index 0000000..212533f
--- /dev/null
+++ b/src/target/espressif/esp32s2.c
@@ -0,0 +1,715 @@
+/***************************************************************************
+ *   ESP32-S2 target for OpenOCD                                           *
+ *   Copyright (C) 2019 Espressif Systems Ltd.                             *
+ *   Author: Alexey Gerenkov <alexey@espressif.com>                        *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "assert.h"
+#include <target/target.h>
+#include <target/target_type.h>
+#include "esp_xtensa.h"
+#include "esp32s2.h"
+
+/* Overall memory map
+ * TODO: read memory configuration from target registers */
+#define ESP32_S2_IROM_MASK_LOW          0x40000000
+#define ESP32_S2_IROM_MASK_HIGH         0x40020000
+#define ESP32_S2_IRAM_LOW               0x40020000
+#define ESP32_S2_IRAM_HIGH              0x40070000
+#define ESP32_S2_DRAM_LOW               0x3ffb0000
+#define ESP32_S2_DRAM_HIGH              0x40000000
+#define ESP32_S2_RTC_IRAM_LOW           0x40070000
+#define ESP32_S2_RTC_IRAM_HIGH          0x40072000
+#define ESP32_S2_RTC_DRAM_LOW           0x3ff9e000
+#define ESP32_S2_RTC_DRAM_HIGH          0x3ffa0000
+#define ESP32_S2_RTC_DATA_LOW           0x50000000
+#define ESP32_S2_RTC_DATA_HIGH          0x50002000
+#define ESP32_S2_EXTRAM_DATA_LOW        0x3f500000
+#define ESP32_S2_EXTRAM_DATA_HIGH       0x3ff80000
+#define ESP32_S2_DR_REG_LOW             0x3f400000
+#define ESP32_S2_DR_REG_HIGH            0x3f4d3FFC
+#define ESP32_S2_SYS_RAM_LOW            0x60000000UL
+#define ESP32_S2_SYS_RAM_HIGH           (ESP32_S2_SYS_RAM_LOW + 0x20000000UL)
+/* ESP32-S2 DROM mapping is not contiguous. */
+/* IDF declares this as 0x3F000000..0x3FF80000, but there are peripheral registers mapped to
+ * 0x3f400000..0x3f4d3FFC. */
+#define ESP32_S2_DROM0_LOW              ESP32_S2_DROM_LOW
+#define ESP32_S2_DROM0_HIGH             ESP32_S2_DR_REG_LOW
+#define ESP32_S2_DROM1_LOW              ESP32_S2_DR_REG_HIGH
+#define ESP32_S2_DROM1_HIGH             ESP32_S2_DROM_HIGH
+
+/* ESP32 WDT */
+#define ESP32_S2_WDT_WKEY_VALUE         0x50d83aa1
+#define ESP32_S2_TIMG0_BASE             0x3f41F000
+#define ESP32_S2_TIMG1_BASE             0x3f420000
+#define ESP32_S2_TIMGWDT_CFG0_OFF       0x48
+#define ESP32_S2_TIMGWDT_PROTECT_OFF    0x64
+#define ESP32_S2_TIMG0WDT_CFG0          (ESP32_S2_TIMG0_BASE + ESP32_S2_TIMGWDT_CFG0_OFF)
+#define ESP32_S2_TIMG1WDT_CFG0          (ESP32_S2_TIMG1_BASE + ESP32_S2_TIMGWDT_CFG0_OFF)
+#define ESP32_S2_TIMG0WDT_PROTECT       (ESP32_S2_TIMG0_BASE + ESP32_S2_TIMGWDT_PROTECT_OFF)
+#define ESP32_S2_TIMG1WDT_PROTECT       (ESP32_S2_TIMG1_BASE + ESP32_S2_TIMGWDT_PROTECT_OFF)
+#define ESP32_S2_RTCCNTL_BASE           0x3f408000
+#define ESP32_S2_RTCWDT_CFG_OFF         0x94
+#define ESP32_S2_RTCWDT_PROTECT_OFF     0xAC
+#define ESP32_S2_SWD_CONF_OFF           0xB0
+#define ESP32_S2_SWD_WPROTECT_OFF       0xB4
+#define ESP32_S2_RTC_CNTL_DIG_PWC_REG_OFF      0x8C
+#define ESP32_S2_RTC_CNTL_DIG_PWC_REG   (ESP32_S2_RTCCNTL_BASE + ESP32_S2_RTC_CNTL_DIG_PWC_REG_OFF)
+#define ESP32_S2_RTCWDT_CFG             (ESP32_S2_RTCCNTL_BASE + ESP32_S2_RTCWDT_CFG_OFF)
+#define ESP32_S2_RTCWDT_PROTECT         (ESP32_S2_RTCCNTL_BASE + ESP32_S2_RTCWDT_PROTECT_OFF)
+#define ESP32_S2_SWD_CONF_REG           (ESP32_S2_RTCCNTL_BASE + ESP32_S2_SWD_CONF_OFF)
+#define ESP32_S2_SWD_WPROTECT_REG       (ESP32_S2_RTCCNTL_BASE + ESP32_S2_SWD_WPROTECT_OFF)
+#define ESP32_S2_SWD_AUTO_FEED_EN_M     BIT(31)
+#define ESP32_S2_SWD_WKEY_VALUE         0x8F1D312AU
+#define ESP32_S2_OPTIONS0               (ESP32_S2_RTCCNTL_BASE + 0x0000)
+#define ESP32_S2_SW_SYS_RST_M           0x80000000
+#define ESP32_S2_SW_SYS_RST_V           0x1
+#define ESP32_S2_SW_SYS_RST_S           31
+#define ESP32_S2_SW_STALL_PROCPU_C0_M   ((ESP32_S2_SW_STALL_PROCPU_C0_V) << (ESP32_S2_SW_STALL_PROCPU_C0_S))
+#define ESP32_S2_SW_STALL_PROCPU_C0_V   0x3
+#define ESP32_S2_SW_STALL_PROCPU_C0_S   2
+#define ESP32_S2_SW_CPU_STALL           (ESP32_S2_RTCCNTL_BASE + 0x00B8)
+#define ESP32_S2_SW_STALL_PROCPU_C1_M   ((ESP32_S2_SW_STALL_PROCPU_C1_V) << (ESP32_S2_SW_STALL_PROCPU_C1_S))
+#define ESP32_S2_SW_STALL_PROCPU_C1_V   0x3FU
+#define ESP32_S2_SW_STALL_PROCPU_C1_S   26
+#define ESP32_S2_CLK_CONF               (ESP32_S2_RTCCNTL_BASE + 0x0074)
+#define ESP32_S2_CLK_CONF_DEF           0x1583218
+#define ESP32_S2_STORE4                 (ESP32_S2_RTCCNTL_BASE + 0x00BC)
+#define ESP32_S2_STORE5                 (ESP32_S2_RTCCNTL_BASE + 0x00C0)
+#define ESP32_S2_DPORT_PMS_OCCUPY_3     0x3F4C10E0
+
+#define ESP32_S2_TRACEMEM_BLOCK_SZ      0x4000
+
+#define ESP32_S2_DR_REG_UART_BASE       0x3f400000
+#define ESP32_S2_REG_UART_BASE(i)       (ESP32_S2_DR_REG_UART_BASE + (i) * 0x10000)
+#define ESP32_S2_UART_DATE_REG(i)       (ESP32_S2_REG_UART_BASE(i) + 0x74)
+
+/* this should map local reg IDs to GDB reg mapping as defined in xtensa-config.c 'rmap' in
+ * xtensa-overlay */
+static const unsigned int esp32s2_gdb_regs_mapping[ESP32_S2_NUM_REGS] = {
+	XT_REG_IDX_PC,
+	XT_REG_IDX_AR0, XT_REG_IDX_AR1, XT_REG_IDX_AR2, XT_REG_IDX_AR3,
+	XT_REG_IDX_AR4, XT_REG_IDX_AR5, XT_REG_IDX_AR6, XT_REG_IDX_AR7,
+	XT_REG_IDX_AR8, XT_REG_IDX_AR9, XT_REG_IDX_AR10, XT_REG_IDX_AR11,
+	XT_REG_IDX_AR12, XT_REG_IDX_AR13, XT_REG_IDX_AR14, XT_REG_IDX_AR15,
+	XT_REG_IDX_AR16, XT_REG_IDX_AR17, XT_REG_IDX_AR18, XT_REG_IDX_AR19,
+	XT_REG_IDX_AR20, XT_REG_IDX_AR21, XT_REG_IDX_AR22, XT_REG_IDX_AR23,
+	XT_REG_IDX_AR24, XT_REG_IDX_AR25, XT_REG_IDX_AR26, XT_REG_IDX_AR27,
+	XT_REG_IDX_AR28, XT_REG_IDX_AR29, XT_REG_IDX_AR30, XT_REG_IDX_AR31,
+	XT_REG_IDX_AR32, XT_REG_IDX_AR33, XT_REG_IDX_AR34, XT_REG_IDX_AR35,
+	XT_REG_IDX_AR36, XT_REG_IDX_AR37, XT_REG_IDX_AR38, XT_REG_IDX_AR39,
+	XT_REG_IDX_AR40, XT_REG_IDX_AR41, XT_REG_IDX_AR42, XT_REG_IDX_AR43,
+	XT_REG_IDX_AR44, XT_REG_IDX_AR45, XT_REG_IDX_AR46, XT_REG_IDX_AR47,
+	XT_REG_IDX_AR48, XT_REG_IDX_AR49, XT_REG_IDX_AR50, XT_REG_IDX_AR51,
+	XT_REG_IDX_AR52, XT_REG_IDX_AR53, XT_REG_IDX_AR54, XT_REG_IDX_AR55,
+	XT_REG_IDX_AR56, XT_REG_IDX_AR57, XT_REG_IDX_AR58, XT_REG_IDX_AR59,
+	XT_REG_IDX_AR60, XT_REG_IDX_AR61, XT_REG_IDX_AR62, XT_REG_IDX_AR63,
+	XT_REG_IDX_SAR,
+	XT_REG_IDX_WINDOWBASE, XT_REG_IDX_WINDOWSTART, XT_REG_IDX_CONFIGID0, XT_REG_IDX_CONFIGID1,
+	XT_REG_IDX_PS, XT_REG_IDX_THREADPTR,
+	ESP32_S2_REG_IDX_GPIOOUT,
+	XT_REG_IDX_MMID, XT_REG_IDX_IBREAKENABLE, XT_REG_IDX_OCD_DDR,
+	XT_REG_IDX_IBREAKA0, XT_REG_IDX_IBREAKA1, XT_REG_IDX_DBREAKA0, XT_REG_IDX_DBREAKA1,
+	XT_REG_IDX_DBREAKC0, XT_REG_IDX_DBREAKC1,
+	XT_REG_IDX_EPC1, XT_REG_IDX_EPC2, XT_REG_IDX_EPC3, XT_REG_IDX_EPC4,
+	XT_REG_IDX_EPC5, XT_REG_IDX_EPC6, XT_REG_IDX_EPC7, XT_REG_IDX_DEPC,
+	XT_REG_IDX_EPS2, XT_REG_IDX_EPS3, XT_REG_IDX_EPS4, XT_REG_IDX_EPS5,
+	XT_REG_IDX_EPS6, XT_REG_IDX_EPS7,
+	XT_REG_IDX_EXCSAVE1, XT_REG_IDX_EXCSAVE2, XT_REG_IDX_EXCSAVE3, XT_REG_IDX_EXCSAVE4,
+	XT_REG_IDX_EXCSAVE5, XT_REG_IDX_EXCSAVE6, XT_REG_IDX_EXCSAVE7, XT_REG_IDX_CPENABLE,
+	XT_REG_IDX_INTERRUPT, XT_REG_IDX_INTSET, XT_REG_IDX_INTCLEAR, XT_REG_IDX_INTENABLE,
+	XT_REG_IDX_VECBASE, XT_REG_IDX_EXCCAUSE, XT_REG_IDX_DEBUGCAUSE, XT_REG_IDX_CCOUNT,
+	XT_REG_IDX_PRID, XT_REG_IDX_ICOUNT, XT_REG_IDX_ICOUNTLEVEL, XT_REG_IDX_EXCVADDR,
+	XT_REG_IDX_CCOMPARE0, XT_REG_IDX_CCOMPARE1, XT_REG_IDX_CCOMPARE2,
+	XT_REG_IDX_MISC0, XT_REG_IDX_MISC1, XT_REG_IDX_MISC2, XT_REG_IDX_MISC3,
+	XT_REG_IDX_A0, XT_REG_IDX_A1, XT_REG_IDX_A2, XT_REG_IDX_A3,
+	XT_REG_IDX_A4, XT_REG_IDX_A5, XT_REG_IDX_A6, XT_REG_IDX_A7,
+	XT_REG_IDX_A8, XT_REG_IDX_A9, XT_REG_IDX_A10, XT_REG_IDX_A11,
+	XT_REG_IDX_A12, XT_REG_IDX_A13, XT_REG_IDX_A14, XT_REG_IDX_A15,
+	XT_REG_IDX_PWRCTL, XT_REG_IDX_PWRSTAT, XT_REG_IDX_ERISTAT,
+	XT_REG_IDX_CS_ITCTRL, XT_REG_IDX_CS_CLAIMSET, XT_REG_IDX_CS_CLAIMCLR,
+	XT_REG_IDX_CS_LOCKACCESS, XT_REG_IDX_CS_LOCKSTATUS, XT_REG_IDX_CS_AUTHSTATUS,
+	XT_REG_IDX_FAULT_INFO,
+	XT_REG_IDX_TRAX_ID, XT_REG_IDX_TRAX_CTRL, XT_REG_IDX_TRAX_STAT,
+	XT_REG_IDX_TRAX_DATA, XT_REG_IDX_TRAX_ADDR, XT_REG_IDX_TRAX_PCTRIGGER,
+	XT_REG_IDX_TRAX_PCMATCH, XT_REG_IDX_TRAX_DELAY, XT_REG_IDX_TRAX_MEMSTART,
+	XT_REG_IDX_TRAX_MEMEND,
+	XT_REG_IDX_PMG, XT_REG_IDX_PMPC, XT_REG_IDX_PM0, XT_REG_IDX_PM1,
+	XT_REG_IDX_PMCTRL0, XT_REG_IDX_PMCTRL1, XT_REG_IDX_PMSTAT0, XT_REG_IDX_PMSTAT1,
+	XT_REG_IDX_OCD_ID, XT_REG_IDX_OCD_DCRCLR, XT_REG_IDX_OCD_DCRSET, XT_REG_IDX_OCD_DSR,
+};
+
+static const struct xtensa_user_reg_desc esp32s2_user_regs[ESP32_S2_NUM_REGS - XT_NUM_REGS] = {
+	{ "gpio_out", 0x00, 0, 32, &xtensa_user_reg_u32_type },
+};
+
+static const struct xtensa_config esp32s2_xtensa_cfg = {
+	.density = true,
+	.aregs_num = XT_AREGS_NUM_MAX,
+	.windowed = true,
+	.coproc = true,
+	.miscregs_num = 4,
+	.reloc_vec = true,
+	.proc_id = true,
+	.threadptr = true,
+	.user_regs_num = ARRAY_SIZE(esp32s2_user_regs),
+	.user_regs = esp32s2_user_regs,
+	.fetch_user_regs = xtensa_fetch_user_regs_u32,
+	.queue_write_dirty_user_regs = xtensa_queue_write_dirty_user_regs_u32,
+	.gdb_general_regs_num = ESP32_S2_NUM_REGS_G_COMMAND,
+	.gdb_regs_mapping = esp32s2_gdb_regs_mapping,
+	.irom = {
+		.count = 2,
+		.regions = {
+			{
+				.base = ESP32_S2_IROM_LOW,
+				.size = ESP32_S2_IROM_HIGH - ESP32_S2_IROM_LOW,
+				.access = XT_MEM_ACCESS_READ,
+			},
+			{
+				.base = ESP32_S2_IROM_MASK_LOW,
+				.size = ESP32_S2_IROM_MASK_HIGH - ESP32_S2_IROM_MASK_LOW,
+				.access = XT_MEM_ACCESS_READ,
+			},
+		}
+	},
+	.iram = {
+		.count = 2,
+		.regions = {
+			{
+				.base = ESP32_S2_IRAM_LOW,
+				.size = ESP32_S2_IRAM_HIGH - ESP32_S2_IRAM_LOW,
+				.access = XT_MEM_ACCESS_READ | XT_MEM_ACCESS_WRITE,
+			},
+			{
+				.base = ESP32_S2_RTC_IRAM_LOW,
+				.size = ESP32_S2_RTC_IRAM_HIGH - ESP32_S2_RTC_IRAM_LOW,
+				.access = XT_MEM_ACCESS_READ | XT_MEM_ACCESS_WRITE,
+			},
+		}
+	},
+	.drom = {
+		.count = 2,
+		.regions = {
+			{
+				.base = ESP32_S2_DROM0_LOW,
+				.size = ESP32_S2_DROM0_HIGH - ESP32_S2_DROM0_LOW,
+				.access = XT_MEM_ACCESS_READ,
+			},
+			{
+				.base = ESP32_S2_DROM1_LOW,
+				.size = ESP32_S2_DROM1_HIGH - ESP32_S2_DROM1_LOW,
+				.access = XT_MEM_ACCESS_READ,
+			},
+		}
+	},
+	.dram = {
+		.count = 6,
+		.regions = {
+			{
+				.base = ESP32_S2_DRAM_LOW,
+				.size = ESP32_S2_DRAM_HIGH - ESP32_S2_DRAM_LOW,
+				.access = XT_MEM_ACCESS_READ | XT_MEM_ACCESS_WRITE,
+			},
+			{
+				.base = ESP32_S2_RTC_DRAM_LOW,
+				.size = ESP32_S2_RTC_DRAM_HIGH - ESP32_S2_RTC_DRAM_LOW,
+				.access = XT_MEM_ACCESS_READ | XT_MEM_ACCESS_WRITE,
+			},
+			{
+				.base = ESP32_S2_RTC_DATA_LOW,
+				.size = ESP32_S2_RTC_DATA_HIGH - ESP32_S2_RTC_DATA_LOW,
+				.access = XT_MEM_ACCESS_READ | XT_MEM_ACCESS_WRITE,
+			},
+			{
+				.base = ESP32_S2_EXTRAM_DATA_LOW,
+				.size = ESP32_S2_EXTRAM_DATA_HIGH - ESP32_S2_EXTRAM_DATA_LOW,
+				.access = XT_MEM_ACCESS_READ | XT_MEM_ACCESS_WRITE,
+			},
+			{
+				.base = ESP32_S2_DR_REG_LOW,
+				.size = ESP32_S2_DR_REG_HIGH - ESP32_S2_DR_REG_LOW,
+				.access = XT_MEM_ACCESS_READ | XT_MEM_ACCESS_WRITE,
+			},
+			{
+				.base = ESP32_S2_SYS_RAM_LOW,
+				.size = ESP32_S2_SYS_RAM_HIGH - ESP32_S2_SYS_RAM_LOW,
+				.access = XT_MEM_ACCESS_READ | XT_MEM_ACCESS_WRITE,
+			},
+		}
+	},
+	.exc = {
+		.enabled = true,
+	},
+	.irq = {
+		.enabled = true,
+		.irq_num = 32,
+	},
+	.high_irq = {
+		.enabled = true,
+		.excm_level = 3,
+		.nmi_num = 1,
+	},
+	.tim_irq = {
+		.enabled = true,
+		.comp_num = 3,
+	},
+	.debug = {
+		.enabled = true,
+		.irq_level = 6,
+		.ibreaks_num = 2,
+		.dbreaks_num = 2,
+		.icount_sz = 32,
+	},
+	.trace = {
+		.enabled = true,
+		.mem_sz = ESP32_S2_TRACEMEM_BLOCK_SZ,
+	},
+};
+
+struct esp32s2_common {
+	struct esp_xtensa_common esp_xtensa;
+};
+
+static int esp32s2_soc_reset(struct target *target);
+static int esp32s2_disable_wdts(struct target *target);
+
+static int esp32s2_assert_reset(struct target *target)
+{
+	return ERROR_OK;
+}
+
+static int esp32s2_deassert_reset(struct target *target)
+{
+	struct xtensa *xtensa = target_to_xtensa(target);
+
+	LOG_TARGET_DEBUG(target, "begin");
+
+	int res = xtensa_deassert_reset(target);
+	if (res != ERROR_OK)
+		return res;
+
+	/* restore configured value
+	   esp32s2_soc_reset() modified it, but can not restore just after SW reset for some reason (???) */
+	res = xtensa_smpbreak_write(xtensa, xtensa->smp_break);
+	if (res != ERROR_OK) {
+		LOG_ERROR("Failed to restore smpbreak (%d)!", res);
+		return res;
+	}
+	return ERROR_OK;
+}
+
+int esp32s2_soft_reset_halt(struct target *target)
+{
+	LOG_TARGET_DEBUG(target, "begin");
+
+	/* Reset the SoC first */
+	int res = esp32s2_soc_reset(target);
+	if (res != ERROR_OK)
+		return res;
+	return xtensa_assert_reset(target);
+}
+
+static int esp32s2_set_peri_reg_mask(struct target *target,
+	target_addr_t addr,
+	uint32_t mask,
+	uint32_t val)
+{
+	uint32_t reg_val;
+	int res = target_read_u32(target, addr, &reg_val);
+	if (res != ERROR_OK)
+		return res;
+	reg_val = (reg_val & (~mask)) | val;
+	res = target_write_u32(target, addr, reg_val);
+	if (res != ERROR_OK)
+		return res;
+
+	return ERROR_OK;
+}
+
+static int esp32s2_stall_set(struct target *target, bool stall)
+{
+	LOG_TARGET_DEBUG(target, "begin");
+
+	int res = esp32s2_set_peri_reg_mask(target,
+		ESP32_S2_SW_CPU_STALL,
+		ESP32_S2_SW_STALL_PROCPU_C1_M,
+		stall ? 0x21U << ESP32_S2_SW_STALL_PROCPU_C1_S : 0);
+	if (res != ERROR_OK) {
+		LOG_ERROR("Failed to write ESP32_S2_SW_CPU_STALL (%d)!", res);
+		return res;
+	}
+	res = esp32s2_set_peri_reg_mask(target,
+		ESP32_S2_OPTIONS0,
+		ESP32_S2_SW_STALL_PROCPU_C0_M,
+		stall ? 0x2 << ESP32_S2_SW_STALL_PROCPU_C0_S : 0);
+	if (res != ERROR_OK) {
+		LOG_ERROR("Failed to write ESP32_S2_OPTIONS0 (%d)!", res);
+		return res;
+	}
+	return ERROR_OK;
+}
+
+static inline int esp32s2_stall(struct target *target)
+{
+	return esp32s2_stall_set(target, true);
+}
+
+static inline int esp32s2_unstall(struct target *target)
+{
+	return esp32s2_stall_set(target, false);
+}
+
+/* Reset ESP32-S2's peripherals.
+Postconditions: all peripherals except RTC_CNTL are reset, CPU's PC is undefined, PRO CPU is halted, APP CPU is in reset
+How this works:
+0. make sure target is halted; if not, try to halt it; if that fails, try to reset it (via OCD) and then halt
+1. Resets clock related registers
+2. Stalls CPU
+3. trigger SoC reset using RTC_CNTL_SW_SYS_RST bit
+4. CPU is reset and stalled at the first reset vector instruction
+5. wait for the OCD to be reset
+6. halt the target
+7. Unstalls CPU
+8. Disables WDTs and trace memory mapping
+*/
+static int esp32s2_soc_reset(struct target *target)
+{
+	int res;
+	struct xtensa *xtensa = target_to_xtensa(target);
+
+	LOG_DEBUG("start");
+
+	/* In order to write to peripheral registers, target must be halted first */
+	if (target->state != TARGET_HALTED) {
+		LOG_TARGET_DEBUG(target, "Target not halted before SoC reset, trying to halt it first");
+		xtensa_halt(target);
+		res = target_wait_state(target, TARGET_HALTED, 1000);
+		if (res != ERROR_OK) {
+			LOG_TARGET_DEBUG(target, "Couldn't halt target before SoC reset, trying to do reset-halt");
+			res = xtensa_assert_reset(target);
+			if (res != ERROR_OK) {
+				LOG_TARGET_ERROR(
+					target,
+					"Couldn't halt target before SoC reset! (xtensa_assert_reset returned %d)",
+					res);
+				return res;
+			}
+			alive_sleep(10);
+			xtensa_poll(target);
+			int reset_halt_save = target->reset_halt;
+			target->reset_halt = 1;
+			res = xtensa_deassert_reset(target);
+			target->reset_halt = reset_halt_save;
+			if (res != ERROR_OK) {
+				LOG_TARGET_ERROR(
+					target,
+					"Couldn't halt target before SoC reset! (xtensa_deassert_reset returned %d)",
+					res);
+				return res;
+			}
+			alive_sleep(10);
+			xtensa_poll(target);
+			xtensa_halt(target);
+			res = target_wait_state(target, TARGET_HALTED, 1000);
+			if (res != ERROR_OK) {
+				LOG_TARGET_ERROR(target, "Couldn't halt target before SoC reset");
+				return res;
+			}
+		}
+	}
+
+	assert(target->state == TARGET_HALTED);
+
+	/* Set some clock-related RTC registers to the default values */
+	res = target_write_u32(target, ESP32_S2_STORE4, 0);
+	if (res != ERROR_OK) {
+		LOG_ERROR("Failed to write ESP32_S2_STORE4 (%d)!", res);
+		return res;
+	}
+	res = target_write_u32(target, ESP32_S2_STORE5, 0);
+	if (res != ERROR_OK) {
+		LOG_ERROR("Failed to write ESP32_S2_STORE5 (%d)!", res);
+		return res;
+	}
+	res = target_write_u32(target, ESP32_S2_RTC_CNTL_DIG_PWC_REG, 0);
+	if (res != ERROR_OK) {
+		LOG_ERROR("Failed to write ESP32_S2_RTC_CNTL_DIG_PWC_REG (%d)!", res);
+		return res;
+	}
+	res = target_write_u32(target, ESP32_S2_CLK_CONF, ESP32_S2_CLK_CONF_DEF);
+	if (res != ERROR_OK) {
+		LOG_ERROR("Failed to write ESP32_S2_CLK_CONF (%d)!", res);
+		return res;
+	}
+	/* Stall CPU */
+	res = esp32s2_stall(target);
+	if (res != ERROR_OK)
+		return res;
+	/* enable stall */
+	res = xtensa_smpbreak_write(xtensa, OCDDCR_RUNSTALLINEN);
+	if (res != ERROR_OK) {
+		LOG_ERROR("Failed to set smpbreak (%d)!", res);
+		return res;
+	}
+	/* Reset CPU */
+	xtensa->suppress_dsr_errors = true;
+	res = esp32s2_set_peri_reg_mask(target,
+		ESP32_S2_OPTIONS0,
+		ESP32_S2_SW_SYS_RST_M,
+		1U << ESP32_S2_SW_SYS_RST_S);
+	xtensa->suppress_dsr_errors = false;
+	if (res != ERROR_OK) {
+		LOG_ERROR("Failed to write ESP32_S2_OPTIONS0 (%d)!", res);
+		return res;
+	}
+	/* Wait for SoC to reset */
+	alive_sleep(100);
+	int timeout = 100;
+	while (target->state != TARGET_RESET && target->state != TARGET_RUNNING && --timeout > 0) {
+		alive_sleep(10);
+		xtensa_poll(target);
+	}
+	if (timeout == 0) {
+		LOG_ERROR("Timed out waiting for CPU to be reset, target->state=%d", target->state);
+		return ERROR_TARGET_TIMEOUT;
+	}
+	xtensa_halt(target);
+	res = target_wait_state(target, TARGET_HALTED, 1000);
+	if (res != ERROR_OK) {
+		LOG_TARGET_ERROR(target, "Couldn't halt target before SoC reset");
+		return res;
+	}
+	/* Unstall CPU */
+	res = esp32s2_unstall(target);
+	if (res != ERROR_OK)
+		return res;
+	/* Disable WDTs */
+	res = esp32s2_disable_wdts(target);
+	if (res != ERROR_OK)
+		return res;
+	/* Disable trace memory mapping */
+	res = target_write_u32(target, ESP32_S2_DPORT_PMS_OCCUPY_3, 0);
+	if (res != ERROR_OK) {
+		LOG_ERROR("Failed to write ESP32_S2_DPORT_PMS_OCCUPY_3 (%d)!", res);
+		return res;
+	}
+	return ERROR_OK;
+}
+
+static int esp32s2_disable_wdts(struct target *target)
+{
+	/* TIMG1 WDT */
+	int res = target_write_u32(target, ESP32_S2_TIMG0WDT_PROTECT, ESP32_S2_WDT_WKEY_VALUE);
+	if (res != ERROR_OK) {
+		LOG_ERROR("Failed to write ESP32_S2_TIMG0WDT_PROTECT (%d)!", res);
+		return res;
+	}
+	res = target_write_u32(target, ESP32_S2_TIMG0WDT_CFG0, 0);
+	if (res != ERROR_OK) {
+		LOG_ERROR("Failed to write ESP32_S2_TIMG0WDT_CFG0 (%d)!", res);
+		return res;
+	}
+	/* TIMG2 WDT */
+	res = target_write_u32(target, ESP32_S2_TIMG1WDT_PROTECT, ESP32_S2_WDT_WKEY_VALUE);
+	if (res != ERROR_OK) {
+		LOG_ERROR("Failed to write ESP32_S2_TIMG1WDT_PROTECT (%d)!", res);
+		return res;
+	}
+	res = target_write_u32(target, ESP32_S2_TIMG1WDT_CFG0, 0);
+	if (res != ERROR_OK) {
+		LOG_ERROR("Failed to write ESP32_S2_TIMG1WDT_CFG0 (%d)!", res);
+		return res;
+	}
+	/* RTC WDT */
+	res = target_write_u32(target, ESP32_S2_RTCWDT_PROTECT, ESP32_S2_WDT_WKEY_VALUE);
+	if (res != ERROR_OK) {
+		LOG_ERROR("Failed to write ESP32_S2_RTCWDT_PROTECT (%d)!", res);
+		return res;
+	}
+	res = target_write_u32(target, ESP32_S2_RTCWDT_CFG, 0);
+	if (res != ERROR_OK) {
+		LOG_ERROR("Failed to write ESP32_S2_RTCWDT_CFG (%d)!", res);
+		return res;
+	}
+	/* Enable SWD auto-feed */
+	res = target_write_u32(target, ESP32_S2_SWD_WPROTECT_REG, ESP32_S2_SWD_WKEY_VALUE);
+	if (res != ERROR_OK) {
+		LOG_ERROR("Failed to write ESP32_S2_SWD_WPROTECT_REG (%d)!", res);
+		return res;
+	}
+	uint32_t swd_conf_reg = 0;
+	res = target_read_u32(target, ESP32_S2_SWD_CONF_REG, &swd_conf_reg);
+	if (res != ERROR_OK) {
+		LOG_ERROR("Failed to read ESP32_S2_SWD_CONF_REG (%d)!", res);
+		return res;
+	}
+	swd_conf_reg |= ESP32_S2_SWD_AUTO_FEED_EN_M;
+	res = target_write_u32(target, ESP32_S2_SWD_CONF_REG, swd_conf_reg);
+	if (res != ERROR_OK) {
+		LOG_ERROR("Failed to write ESP32_S2_SWD_CONF_REG (%d)!", res);
+		return res;
+	}
+	return ERROR_OK;
+}
+
+static int esp32s2_arch_state(struct target *target)
+{
+	return ERROR_OK;
+}
+
+static int esp32s2_on_halt(struct target *target)
+{
+	return esp32s2_disable_wdts(target);
+}
+
+static int esp32s2_step(struct target *target, int current, target_addr_t address, int handle_breakpoints)
+{
+	int ret = xtensa_step(target, current, address, handle_breakpoints);
+	if (ret == ERROR_OK) {
+		esp32s2_on_halt(target);
+		target_call_event_callbacks(target, TARGET_EVENT_HALTED);
+	}
+	return ret;
+}
+
+static int esp32s2_poll(struct target *target)
+{
+	enum target_state old_state = target->state;
+	int ret = esp_xtensa_poll(target);
+
+	if (old_state != TARGET_HALTED && target->state == TARGET_HALTED) {
+		/* Call any event callbacks that are applicable */
+		if (old_state == TARGET_DEBUG_RUNNING) {
+			target_call_event_callbacks(target, TARGET_EVENT_DEBUG_HALTED);
+		} else {
+			esp32s2_on_halt(target);
+			target_call_event_callbacks(target, TARGET_EVENT_HALTED);
+		}
+	}
+
+	return ret;
+}
+
+static int esp32s2_virt2phys(struct target *target,
+	target_addr_t virtual, target_addr_t *physical)
+{
+	*physical = virtual;
+	return ERROR_OK;
+}
+
+static int esp32s2_target_init(struct command_context *cmd_ctx, struct target *target)
+{
+	return esp_xtensa_target_init(cmd_ctx, target);
+}
+
+static const struct xtensa_debug_ops esp32s2_dbg_ops = {
+	.queue_enable = xtensa_dm_queue_enable,
+	.queue_reg_read = xtensa_dm_queue_reg_read,
+	.queue_reg_write = xtensa_dm_queue_reg_write
+};
+
+static const struct xtensa_power_ops esp32s2_pwr_ops = {
+	.queue_reg_read = xtensa_dm_queue_pwr_reg_read,
+	.queue_reg_write = xtensa_dm_queue_pwr_reg_write
+};
+
+static int esp32s2_target_create(struct target *target, Jim_Interp *interp)
+{
+	struct xtensa_debug_module_config esp32s2_dm_cfg = {
+		.dbg_ops = &esp32s2_dbg_ops,
+		.pwr_ops = &esp32s2_pwr_ops,
+		.tap = target->tap,
+		.queue_tdi_idle = NULL,
+		.queue_tdi_idle_arg = NULL
+	};
+
+	/* creates xtensa object */
+	struct esp32s2_common *esp32 = calloc(1, sizeof(*esp32));
+	if (!esp32) {
+		LOG_ERROR("Failed to alloc memory for arch info!");
+		return ERROR_FAIL;
+	}
+
+	int ret = esp_xtensa_init_arch_info(target, &esp32->esp_xtensa, &esp32s2_xtensa_cfg, &esp32s2_dm_cfg);
+	if (ret != ERROR_OK) {
+		LOG_ERROR("Failed to init arch info!");
+		free(esp32);
+		return ret;
+	}
+
+	/* Assume running target. If different, the first poll will fix this */
+	target->state = TARGET_RUNNING;
+	target->debug_reason = DBG_REASON_NOTHALTED;
+	return ERROR_OK;
+}
+
+static const struct command_registration esp32s2_command_handlers[] = {
+	{
+		.name = "xtensa",
+		.mode = COMMAND_ANY,
+		.help = "Xtensa commands group",
+		.usage = "",
+		.chain = xtensa_command_handlers,
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+/* Holds methods for Xtensa targets. */
+struct target_type esp32s2_target = {
+	.name = "esp32s2",
+
+	.poll = esp32s2_poll,
+	.arch_state = esp32s2_arch_state,
+
+	.halt = xtensa_halt,
+	.resume = xtensa_resume,
+	.step = esp32s2_step,
+
+	.assert_reset = esp32s2_assert_reset,
+	.deassert_reset = esp32s2_deassert_reset,
+	.soft_reset_halt = esp32s2_soft_reset_halt,
+
+	.virt2phys = esp32s2_virt2phys,
+	.mmu = xtensa_mmu_is_enabled,
+	.read_memory = xtensa_read_memory,
+	.write_memory = xtensa_write_memory,
+
+	.read_buffer = xtensa_read_buffer,
+	.write_buffer = xtensa_write_buffer,
+
+	.checksum_memory = xtensa_checksum_memory,
+
+	.get_gdb_arch = xtensa_get_gdb_arch,
+	.get_gdb_reg_list = xtensa_get_gdb_reg_list,
+
+	.add_breakpoint = esp_xtensa_breakpoint_add,
+	.remove_breakpoint = esp_xtensa_breakpoint_remove,
+
+	.add_watchpoint = xtensa_watchpoint_add,
+	.remove_watchpoint = xtensa_watchpoint_remove,
+
+	.target_create = esp32s2_target_create,
+	.init_target = esp32s2_target_init,
+	.examine = xtensa_examine,
+	.deinit_target = esp_xtensa_target_deinit,
+
+	.commands = esp32s2_command_handlers,
+};
diff --git a/src/target/espressif/esp32s2.h b/src/target/espressif/esp32s2.h
new file mode 100644
index 0000000..2c43e3e
--- /dev/null
+++ b/src/target/espressif/esp32s2.h
@@ -0,0 +1,40 @@
+/***************************************************************************
+ *   ESP32-S2 target for OpenOCD                                           *
+ *   Copyright (C) 2019 Espressif Systems Ltd.                             *
+ *   Author: Alexey Gerenkov <alexey@espressif.com>                        *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+
+#ifndef OPENOCD_TARGET_ESP32S2_H
+#define OPENOCD_TARGET_ESP32S2_H
+
+#include <target/xtensa/xtensa_regs.h>
+
+#define ESP32_S2_DROM_LOW   0x3f000000
+#define ESP32_S2_DROM_HIGH  0x3ff80000
+#define ESP32_S2_IROM_LOW   0x40080000
+#define ESP32_S2_IROM_HIGH  0x40800000
+
+/* Number of registers returned directly by the G command
+ * Corresponds to the amount of regs listed in regformats/reg-xtensa.dat in the gdb source */
+#define ESP32_S2_NUM_REGS_G_COMMAND   72
+
+enum esp32s2_reg_id {
+	/* chip specific registers that extend ISA go after ISA-defined ones */
+	ESP32_S2_REG_IDX_GPIOOUT = XT_USR_REG_START,
+	ESP32_S2_NUM_REGS,
+};
+
+#endif	/* OPENOCD_TARGET_ESP32S2_H */
diff --git a/src/target/espressif/esp_xtensa.c b/src/target/espressif/esp_xtensa.c
new file mode 100644
index 0000000..89393f4
--- /dev/null
+++ b/src/target/espressif/esp_xtensa.c
@@ -0,0 +1,71 @@
+/***************************************************************************
+ *   Espressif Xtensa target API for OpenOCD                               *
+ *   Copyright (C) 2019 Espressif Systems Ltd.                             *
+ *   Author: Alexey Gerenkov <alexey@espressif.com>                        *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <target/smp.h>
+#include "esp_xtensa.h"
+#include <target/register.h>
+
+int esp_xtensa_init_arch_info(struct target *target,
+	struct esp_xtensa_common *esp_xtensa,
+	const struct xtensa_config *xtensa_cfg,
+	struct xtensa_debug_module_config *dm_cfg)
+{
+	return xtensa_init_arch_info(target, &esp_xtensa->xtensa, xtensa_cfg, dm_cfg);
+}
+
+int esp_xtensa_target_init(struct command_context *cmd_ctx, struct target *target)
+{
+	return xtensa_target_init(cmd_ctx, target);
+}
+
+void esp_xtensa_target_deinit(struct target *target)
+{
+	LOG_DEBUG("start");
+
+	xtensa_target_deinit(target);
+	free(target_to_esp_xtensa(target));	/* same as free(xtensa) */
+}
+
+int esp_xtensa_arch_state(struct target *target)
+{
+	return ERROR_OK;
+}
+
+int esp_xtensa_poll(struct target *target)
+{
+	return xtensa_poll(target);
+}
+
+int esp_xtensa_breakpoint_add(struct target *target, struct breakpoint *breakpoint)
+{
+	return xtensa_breakpoint_add(target, breakpoint);
+	/* flash breakpoints will be handled in another patch */
+}
+
+int esp_xtensa_breakpoint_remove(struct target *target, struct breakpoint *breakpoint)
+{
+	return xtensa_breakpoint_remove(target, breakpoint);
+	/* flash breakpoints will be handled in another patch */
+}
diff --git a/src/target/espressif/esp_xtensa.h b/src/target/espressif/esp_xtensa.h
new file mode 100644
index 0000000..6badb1b
--- /dev/null
+++ b/src/target/espressif/esp_xtensa.h
@@ -0,0 +1,48 @@
+/***************************************************************************
+ *   Generic ESP xtensa target implementation for OpenOCD                  *
+ *   Copyright (C) 2019 Espressif Systems Ltd.                             *
+ *   Author: Alexey Gerenkov <alexey@espressif.com>                        *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+
+#ifndef OPENOCD_TARGET_ESP_XTENSA_H
+#define OPENOCD_TARGET_ESP_XTENSA_H
+
+#include <helper/command.h>
+#include <target/target.h>
+#include <target/xtensa/xtensa.h>
+
+struct esp_xtensa_common {
+	struct xtensa xtensa;	/* must be the first element */
+};
+
+static inline struct esp_xtensa_common *target_to_esp_xtensa(struct target *target)
+{
+	return container_of(target->arch_info, struct esp_xtensa_common, xtensa);
+}
+
+int esp_xtensa_init_arch_info(struct target *target,
+	struct esp_xtensa_common *esp_xtensa,
+	const struct xtensa_config *xtensa_cfg,
+	struct xtensa_debug_module_config *dm_cfg);
+int esp_xtensa_target_init(struct command_context *cmd_ctx, struct target *target);
+void esp_xtensa_target_deinit(struct target *target);
+int esp_xtensa_arch_state(struct target *target);
+void esp_xtensa_queue_tdi_idle(struct target *target);
+int esp_xtensa_breakpoint_add(struct target *target, struct breakpoint *breakpoint);
+int esp_xtensa_breakpoint_remove(struct target *target, struct breakpoint *breakpoint);
+int esp_xtensa_poll(struct target *target);
+
+#endif	/* OPENOCD_TARGET_ESP_XTENSA_H */
diff --git a/src/target/riscv/riscv-013.c b/src/target/riscv/riscv-013.c
index 28789bd..ee3ced6 100644
--- a/src/target/riscv/riscv-013.c
+++ b/src/target/riscv/riscv-013.c
@@ -2468,10 +2468,11 @@ static int deassert_reset(struct target *target)
 	select_dmi(target);
 
 	/* Clear the reset, but make sure haltreq is still set */
-	uint32_t control = 0;
-	control = set_field(control, DM_DMCONTROL_HALTREQ, target->reset_halt ? 1 : 0);
+	uint32_t control = 0, control_haltreq;
 	control = set_field(control, DM_DMCONTROL_DMACTIVE, 1);
-	dmi_write(target, DM_DMCONTROL, set_dmcontrol_hartsel(control, info->index));
+	control_haltreq = set_field(control, DM_DMCONTROL_HALTREQ, target->reset_halt ? 1 : 0);
+	dmi_write(target, DM_DMCONTROL,
+			set_dmcontrol_hartsel(control_haltreq, info->index));
 
 	uint32_t dmstatus;
 	int dmi_busy_delay = info->dmi_busy_delay;
@@ -2483,7 +2484,7 @@ static int deassert_reset(struct target *target)
 			if (index != info->index)
 				continue;
 			dmi_write(target, DM_DMCONTROL,
-					set_dmcontrol_hartsel(control, index));
+					set_dmcontrol_hartsel(control_haltreq, index));
 		} else {
 			index = info->index;
 		}
@@ -2525,7 +2526,7 @@ static int deassert_reset(struct target *target)
 		}
 
 		if (get_field(dmstatus, DM_DMSTATUS_ALLHAVERESET)) {
-			/* Ack reset. */
+			/* Ack reset and clear DM_DMCONTROL_HALTREQ if previously set */
 			dmi_write(target, DM_DMCONTROL,
 					set_dmcontrol_hartsel(control, index) |
 					DM_DMCONTROL_ACKHAVERESET);
diff --git a/src/target/semihosting_common.c b/src/target/semihosting_common.c
index bc1f417..2df6e38 100644
--- a/src/target/semihosting_common.c
+++ b/src/target/semihosting_common.c
@@ -159,6 +159,7 @@ int semihosting_common_init(struct target *target, void *setup,
 	semihosting->result = -1;
 	semihosting->sys_errno = -1;
 	semihosting->cmdline = NULL;
+	semihosting->basedir = NULL;
 
 	/* If possible, update it in setup(). */
 	semihosting->setup_time = clock();
@@ -870,17 +871,21 @@ int semihosting_common(struct target *target)
 					semihosting->sys_errno = EINVAL;
 					break;
 				}
-				uint8_t *fn = malloc(len+1);
+				size_t basedir_len = semihosting->basedir ? strlen(semihosting->basedir) : 0;
+				uint8_t *fn = malloc(basedir_len + len + 2);
 				if (!fn) {
 					semihosting->result = -1;
 					semihosting->sys_errno = ENOMEM;
 				} else {
-					retval = target_read_memory(target, addr, 1, len, fn);
+					strncpy((char *)fn, semihosting->basedir, basedir_len);
+					if (fn[basedir_len - 1] != '/')
+						fn[basedir_len++] = '/';
+					retval = target_read_memory(target, addr, 1, len, fn + basedir_len);
 					if (retval != ERROR_OK) {
 						free(fn);
 						return retval;
 					}
-					fn[len] = 0;
+					fn[basedir_len + len] = 0;
 					/* TODO: implement the :semihosting-features special file.
 					 * */
 					if (semihosting->is_fileio) {
@@ -2025,6 +2030,44 @@ COMMAND_HANDLER(handle_common_semihosting_read_user_param_command)
 	return ERROR_OK;
 }
 
+COMMAND_HANDLER(handle_common_semihosting_basedir_command)
+{
+	struct target *target = get_current_target(CMD_CTX);
+
+	if (CMD_ARGC > 1)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	if (!target) {
+		LOG_ERROR("No target selected");
+		return ERROR_FAIL;
+	}
+
+	struct semihosting *semihosting = target->semihosting;
+	if (!semihosting) {
+		command_print(CMD, "semihosting not supported for current target");
+		return ERROR_FAIL;
+	}
+
+	if (!semihosting->is_active) {
+		command_print(CMD, "semihosting not yet enabled for current target");
+		return ERROR_FAIL;
+	}
+
+	if (CMD_ARGC > 0) {
+		free(semihosting->basedir);
+		semihosting->basedir = strdup(CMD_ARGV[0]);
+		if (!semihosting->basedir) {
+			command_print(CMD, "semihosting failed to allocate memory for basedir!");
+			return ERROR_FAIL;
+		}
+	}
+
+	command_print(CMD, "semihosting base dir: %s",
+		semihosting->basedir ? semihosting->basedir : "");
+
+	return ERROR_OK;
+}
+
 const struct command_registration semihosting_common_handlers[] = {
 	{
 		.name = "semihosting",
@@ -2068,5 +2111,12 @@ const struct command_registration semihosting_common_handlers[] = {
 		.usage = "",
 		.help = "read parameters in semihosting-user-cmd-0x10X callbacks",
 	},
+	{
+		.name = "semihosting_basedir",
+		.handler = handle_common_semihosting_basedir_command,
+		.mode = COMMAND_EXEC,
+		.usage = "[dir]",
+		.help = "set the base directory for semihosting I/O operations",
+	},
 	COMMAND_REGISTRATION_DONE
 };
diff --git a/src/target/semihosting_common.h b/src/target/semihosting_common.h
index 459faf6..404080f 100644
--- a/src/target/semihosting_common.h
+++ b/src/target/semihosting_common.h
@@ -176,6 +176,9 @@ struct semihosting {
 	/** The current time when 'execution starts' */
 	clock_t setup_time;
 
+	/** Base directory for semihosting I/O operations. */
+	char *basedir;
+
 	int (*setup)(struct target *target, int enable);
 	int (*post_result)(struct target *target);
 };
diff --git a/src/target/smp.c b/src/target/smp.c
index 3e1ded8..569abd7 100644
--- a/src/target/smp.c
+++ b/src/target/smp.c
@@ -28,6 +28,7 @@
 #include "smp.h"
 #include "helper/binarybuffer.h"
 
+/* DEPRECATED: gdb_read_smp_packet/gdb_write_smp_packet to be removed      */
 /*  implementation of new packet in gdb interface for smp feature          */
 /*                                                                         */
 /*   j : smp  status request                                               */
@@ -53,11 +54,15 @@
 /*  maint packet jc                                                        */
 
 /* packet j :smp status request */
+#define DEPRECATED_MSG "DEPRECATED: This method is deprecated in favor of the hwthread pseudo RTOS"
 int gdb_read_smp_packet(struct connection *connection,
 		char const *packet, int packet_size)
 {
 	struct target *target = get_target_from_connection(connection);
 	int retval = ERROR_OK;
+
+	LOG_WARNING(DEPRECATED_MSG);
+
 	if (target->smp) {
 		if (strncmp(packet, "jc", 2) == 0) {
 			const uint32_t len = sizeof(target->gdb_service->core[0]);
@@ -83,6 +88,8 @@ int gdb_write_smp_packet(struct connection *connection,
 	int coreid = 0;
 	int retval = ERROR_OK;
 
+	LOG_WARNING(DEPRECATED_MSG);
+
 	/* skip command character */
 	if (target->smp) {
 		if (strncmp(packet, "Jc", 2) == 0) {
diff --git a/src/target/smp.h b/src/target/smp.h
index 490a493..d373c90 100644
--- a/src/target/smp.h
+++ b/src/target/smp.h
@@ -30,8 +30,10 @@
 
 extern const struct command_registration smp_command_handlers[];
 
+/* DEPRECATED */
 int gdb_read_smp_packet(struct connection *connection,
 		char const *packet, int packet_size);
+/* DEPRECATED */
 int gdb_write_smp_packet(struct connection *connection,
 		char const *packet, int packet_size);
 
diff --git a/src/target/target.c b/src/target/target.c
index 30f2cf0..a373848 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -57,6 +57,7 @@
 #include "transport/transport.h"
 #include "arm_cti.h"
 #include "smp.h"
+#include "semihosting_common.h"
 
 /* default halt wait timeout (ms) */
 #define DEFAULT_HALT_TIMEOUT 5000
@@ -104,6 +105,7 @@ extern struct target_type hla_target;
 extern struct target_type nds32_v2_target;
 extern struct target_type nds32_v3_target;
 extern struct target_type nds32_v3m_target;
+extern struct target_type esp32s2_target;
 extern struct target_type or1k_target;
 extern struct target_type quark_x10xx_target;
 extern struct target_type quark_d20xx_target;
@@ -140,6 +142,7 @@ static struct target_type *target_types[] = {
 	&nds32_v2_target,
 	&nds32_v3_target,
 	&nds32_v3m_target,
+	&esp32s2_target,
 	&or1k_target,
 	&quark_x10xx_target,
 	&quark_d20xx_target,
@@ -2268,6 +2271,8 @@ static void target_destroy(struct target *target)
 	if (target->type->deinit_target)
 		target->type->deinit_target(target);
 
+	if (target->semihosting)
+		free(target->semihosting->basedir);
 	free(target->semihosting);
 
 	jtag_unregister_event_callback(jtag_enable_callback, target);
@@ -2597,7 +2602,7 @@ int target_blank_check_memory(struct target *target,
 	}
 
 	if (!target->type->blank_check_memory)
-		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+		return ERROR_NOT_IMPLEMENTED;
 
 	return target->type->blank_check_memory(target, blocks, num_blocks, erased_value);
 }
@@ -5224,10 +5229,18 @@ static int target_jim_set_reg(Jim_Interp *interp, int argc,
 	}
 
 	int tmp;
+#if JIM_VERSION >= 80
 	Jim_Obj **dict = Jim_DictPairs(interp, argv[1], &tmp);
 
 	if (!dict)
 		return JIM_ERR;
+#else
+	Jim_Obj **dict;
+	int ret = Jim_DictPairs(interp, argv[1], &dict, &tmp);
+
+	if (ret != JIM_OK)
+		return ret;
+#endif
 
 	const unsigned int length = tmp;
 	struct command_context *cmd_ctx = current_command_context(interp);
@@ -6473,7 +6486,7 @@ static int jim_target_smp(Jim_Interp *interp, int argc, Jim_Obj *const *argv)
 	smp_group++;
 
 	if (target && target->rtos)
-		retval = rtos_smp_init(head->target);
+		retval = rtos_smp_init(target);
 
 	return retval;
 }
diff --git a/src/target/target.h b/src/target/target.h
index 2166f7e..e67016c 100644
--- a/src/target/target.h
+++ b/src/target/target.h
@@ -206,7 +206,7 @@ struct target {
 	 * poll too quickly because we'll just overwhelm the user with error
 	 * messages. */
 	struct backoff_timer backoff;
-	int smp;							/* add some target attributes for smp support */
+	int smp;							/* Unique non-zero number for each SMP group */
 	struct list_head *smp_targets;		/* list all targets in this smp group/cluster
 										 * The head of the list is shared between the
 										 * cluster, thus here there is a pointer */
diff --git a/src/target/target_type.h b/src/target/target_type.h
index d6b6086..a26c2e7 100644
--- a/src/target/target_type.h
+++ b/src/target/target_type.h
@@ -242,6 +242,17 @@ struct target_type {
 	/**
 	 * Free all the resources allocated by the target.
 	 *
+	 * WARNING: deinit_target is called unconditionally regardless the target has
+	 * ever been examined/initialised or not.
+	 * If a problem has prevented establishing JTAG/SWD/... communication
+	 *  or
+	 * if the target was created with -defer-examine flag and has never been
+	 *  examined
+	 * then it is not possible to communicate with the target.
+	 *
+	 * If you need to talk to the target during deinit, first check if
+	 * target_was_examined()!
+	 *
 	 * @param target The target to deinit
 	 */
 	void (*deinit_target)(struct target *target);
diff --git a/src/target/xtensa/Makefile.am b/src/target/xtensa/Makefile.am
new file mode 100644
index 0000000..f6cee99
--- /dev/null
+++ b/src/target/xtensa/Makefile.am
@@ -0,0 +1,7 @@
+noinst_LTLIBRARIES += %D%/libxtensa.la
+%C%_libxtensa_la_SOURCES = \
+       %D%/xtensa.c \
+       %D%/xtensa.h \
+       %D%/xtensa_debug_module.c \
+       %D%/xtensa_debug_module.h \
+       %D%/xtensa_regs.h
diff --git a/src/target/xtensa/xtensa.c b/src/target/xtensa/xtensa.c
new file mode 100644
index 0000000..a955960
--- /dev/null
+++ b/src/target/xtensa/xtensa.c
@@ -0,0 +1,2731 @@
+/***************************************************************************
+ *   Generic Xtensa target API for OpenOCD                                 *
+ *   Copyright (C) 2016-2019 Espressif Systems Ltd.                        *
+ *   Derived from esp108.c                                                 *
+ *   Author: Angus Gratton gus@projectgus.com                              *
+ *   Author: Jeroen Domburg <jeroen@espressif.com>                         *
+ *   Author: Alexey Gerenkov <alexey@espressif.com>                        *
+ *   Author: Andrey Gramakov <andrei.gramakov@espressif.com>               *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <stdlib.h>
+#include <helper/time_support.h>
+#include <helper/align.h>
+#include <target/register.h>
+
+#include "xtensa.h"
+
+
+#define _XT_INS_FORMAT_RSR(OPCODE, SR, T) ((OPCODE)	    \
+		| (((SR) & 0xFF) << 8) \
+		| (((T) & 0x0F) << 4))
+
+#define _XT_INS_FORMAT_RRR(OPCODE, ST, R) ((OPCODE)	    \
+		| (((ST) & 0xFF) << 4) \
+		| (((R) & 0x0F) << 12))
+
+#define _XT_INS_FORMAT_RRRN(OPCODE, S, T, IMM4) ((OPCODE)	      \
+		| (((T) & 0x0F) << 4)	\
+		| (((S) & 0x0F) << 8)	\
+		| (((IMM4) & 0x0F) << 12))
+
+#define _XT_INS_FORMAT_RRI8(OPCODE, R, S, T, IMM8) ((OPCODE)	    \
+		| (((IMM8) & 0xFF) << 16) \
+		| (((R) & 0x0F) << 12)	\
+		| (((S) & 0x0F) << 8)	\
+		| (((T) & 0x0F) << 4))
+
+#define _XT_INS_FORMAT_RRI4(OPCODE, IMM4, R, S, T) ((OPCODE) \
+		| (((IMM4) & 0x0F) << 20) \
+		| (((R) & 0x0F) << 12) \
+		| (((S) & 0x0F) << 8)	\
+		| (((T) & 0x0F) << 4))
+
+/* Xtensa processor instruction opcodes
+ * "Return From Debug Operation" to Normal */
+#define XT_INS_RFDO      0xf1e000
+/* "Return From Debug and Dispatch" - allow sw debugging stuff to take over */
+#define XT_INS_RFDD      0xf1e010
+
+/* Load to DDR register, increase addr register */
+#define XT_INS_LDDR32P(S) (0x0070E0 | ((S) << 8))
+/* Store from DDR register, increase addr register */
+#define XT_INS_SDDR32P(S) (0x0070F0 | ((S) << 8))
+
+/* Load 32-bit Indirect from A(S) + 4 * IMM8 to A(T) */
+#define XT_INS_L32I(S, T, IMM8)  _XT_INS_FORMAT_RRI8(0x002002, 0, S, T, IMM8)
+/* Load 16-bit Unsigned from A(S) + 2 * IMM8 to A(T) */
+#define XT_INS_L16UI(S, T, IMM8) _XT_INS_FORMAT_RRI8(0x001002, 0, S, T, IMM8)
+/* Load 8-bit Unsigned from A(S) + IMM8 to A(T) */
+#define XT_INS_L8UI(S, T, IMM8)  _XT_INS_FORMAT_RRI8(0x000002, 0, S, T, IMM8)
+
+/* Store 32-bit Indirect to A(S) + 4 * IMM8 from A(T) */
+#define XT_INS_S32I(S, T, IMM8) _XT_INS_FORMAT_RRI8(0x006002, 0, S, T, IMM8)
+/* Store 16-bit to A(S) + 2 * IMM8 from A(T) */
+#define XT_INS_S16I(S, T, IMM8) _XT_INS_FORMAT_RRI8(0x005002, 0, S, T, IMM8)
+/* Store 8-bit to A(S) + IMM8 from A(T) */
+#define XT_INS_S8I(S, T, IMM8)  _XT_INS_FORMAT_RRI8(0x004002, 0, S, T, IMM8)
+
+/* Read Special Register */
+#define XT_INS_RSR(SR, T) _XT_INS_FORMAT_RSR(0x030000, SR, T)
+/* Write Special Register */
+#define XT_INS_WSR(SR, T) _XT_INS_FORMAT_RSR(0x130000, SR, T)
+/* Swap Special Register */
+#define XT_INS_XSR(SR, T) _XT_INS_FORMAT_RSR(0x610000, SR, T)
+
+/* Rotate Window by (-8..7) */
+#define XT_INS_ROTW(N) ((0x408000) | (((N) & 15) << 4))
+
+/* Read User Register */
+#define XT_INS_RUR(UR, T) _XT_INS_FORMAT_RRR(0xE30000, UR, T)
+/* Write User Register */
+#define XT_INS_WUR(UR, T) _XT_INS_FORMAT_RSR(0xF30000, UR, T)
+
+/* Read Floating-Point Register */
+#define XT_INS_RFR(FR, T) _XT_INS_FORMAT_RRR(0xFA0000, (((FR) << 4) | 0x4), T)
+/* Write Floating-Point Register */
+#define XT_INS_WFR(FR, T) _XT_INS_FORMAT_RRR(0xFA0000, (((FR) << 4) | 0x5), T)
+
+/* 32-bit break */
+#define XT_INS_BREAK(IMM1, IMM2)  _XT_INS_FORMAT_RRR(0x000000, \
+		(((IMM1) & 0x0F) << 4) | ((IMM2) & 0x0F), 0x4)
+/* 16-bit break */
+#define XT_INS_BREAKN(IMM4)  _XT_INS_FORMAT_RRRN(0x00000D, IMM4, 0x2, 0xF)
+
+#define XT_INS_L32E(R, S, T) _XT_INS_FORMAT_RRI4(0x90000, 0, R, S, T)
+#define XT_INS_S32E(R, S, T) _XT_INS_FORMAT_RRI4(0x490000, 0, R, S, T)
+#define XT_INS_L32E_S32E_MASK   0xFF000F
+
+#define XT_INS_RFWO 0x3400
+#define XT_INS_RFWU 0x3500
+#define XT_INS_RFWO_RFWU_MASK   0xFFFFFF
+
+#define XT_WATCHPOINTS_NUM_MAX  2
+
+/* Special register number macro for DDR register.
+* this gets used a lot so making a shortcut to it is
+* useful.
+*/
+#define XT_SR_DDR         (xtensa_regs[XT_REG_IDX_OCD_DDR].reg_num)
+
+/*Same thing for A3/A4 */
+#define XT_REG_A3         (xtensa_regs[XT_REG_IDX_AR3].reg_num)
+#define XT_REG_A4         (xtensa_regs[XT_REG_IDX_AR4].reg_num)
+
+#define XT_PC_REG_NUM_BASE          (176)
+#define XT_SW_BREAKPOINTS_MAX_NUM   32
+
+const struct xtensa_reg_desc xtensa_regs[XT_NUM_REGS] = {
+	{ "pc", XT_PC_REG_NUM_BASE /*+XT_DEBUGLEVEL*/, XT_REG_SPECIAL, 0 },		/* actually epc[debuglevel] */
+	{ "ar0", 0x00, XT_REG_GENERAL, 0 },
+	{ "ar1", 0x01, XT_REG_GENERAL, 0 },
+	{ "ar2", 0x02, XT_REG_GENERAL, 0 },
+	{ "ar3", 0x03, XT_REG_GENERAL, 0 },
+	{ "ar4", 0x04, XT_REG_GENERAL, 0 },
+	{ "ar5", 0x05, XT_REG_GENERAL, 0 },
+	{ "ar6", 0x06, XT_REG_GENERAL, 0 },
+	{ "ar7", 0x07, XT_REG_GENERAL, 0 },
+	{ "ar8", 0x08, XT_REG_GENERAL, 0 },
+	{ "ar9", 0x09, XT_REG_GENERAL, 0 },
+	{ "ar10", 0x0A, XT_REG_GENERAL, 0 },
+	{ "ar11", 0x0B, XT_REG_GENERAL, 0 },
+	{ "ar12", 0x0C, XT_REG_GENERAL, 0 },
+	{ "ar13", 0x0D, XT_REG_GENERAL, 0 },
+	{ "ar14", 0x0E, XT_REG_GENERAL, 0 },
+	{ "ar15", 0x0F, XT_REG_GENERAL, 0 },
+	{ "ar16", 0x10, XT_REG_GENERAL, 0 },
+	{ "ar17", 0x11, XT_REG_GENERAL, 0 },
+	{ "ar18", 0x12, XT_REG_GENERAL, 0 },
+	{ "ar19", 0x13, XT_REG_GENERAL, 0 },
+	{ "ar20", 0x14, XT_REG_GENERAL, 0 },
+	{ "ar21", 0x15, XT_REG_GENERAL, 0 },
+	{ "ar22", 0x16, XT_REG_GENERAL, 0 },
+	{ "ar23", 0x17, XT_REG_GENERAL, 0 },
+	{ "ar24", 0x18, XT_REG_GENERAL, 0 },
+	{ "ar25", 0x19, XT_REG_GENERAL, 0 },
+	{ "ar26", 0x1A, XT_REG_GENERAL, 0 },
+	{ "ar27", 0x1B, XT_REG_GENERAL, 0 },
+	{ "ar28", 0x1C, XT_REG_GENERAL, 0 },
+	{ "ar29", 0x1D, XT_REG_GENERAL, 0 },
+	{ "ar30", 0x1E, XT_REG_GENERAL, 0 },
+	{ "ar31", 0x1F, XT_REG_GENERAL, 0 },
+	{ "ar32", 0x20, XT_REG_GENERAL, 0 },
+	{ "ar33", 0x21, XT_REG_GENERAL, 0 },
+	{ "ar34", 0x22, XT_REG_GENERAL, 0 },
+	{ "ar35", 0x23, XT_REG_GENERAL, 0 },
+	{ "ar36", 0x24, XT_REG_GENERAL, 0 },
+	{ "ar37", 0x25, XT_REG_GENERAL, 0 },
+	{ "ar38", 0x26, XT_REG_GENERAL, 0 },
+	{ "ar39", 0x27, XT_REG_GENERAL, 0 },
+	{ "ar40", 0x28, XT_REG_GENERAL, 0 },
+	{ "ar41", 0x29, XT_REG_GENERAL, 0 },
+	{ "ar42", 0x2A, XT_REG_GENERAL, 0 },
+	{ "ar43", 0x2B, XT_REG_GENERAL, 0 },
+	{ "ar44", 0x2C, XT_REG_GENERAL, 0 },
+	{ "ar45", 0x2D, XT_REG_GENERAL, 0 },
+	{ "ar46", 0x2E, XT_REG_GENERAL, 0 },
+	{ "ar47", 0x2F, XT_REG_GENERAL, 0 },
+	{ "ar48", 0x30, XT_REG_GENERAL, 0 },
+	{ "ar49", 0x31, XT_REG_GENERAL, 0 },
+	{ "ar50", 0x32, XT_REG_GENERAL, 0 },
+	{ "ar51", 0x33, XT_REG_GENERAL, 0 },
+	{ "ar52", 0x34, XT_REG_GENERAL, 0 },
+	{ "ar53", 0x35, XT_REG_GENERAL, 0 },
+	{ "ar54", 0x36, XT_REG_GENERAL, 0 },
+	{ "ar55", 0x37, XT_REG_GENERAL, 0 },
+	{ "ar56", 0x38, XT_REG_GENERAL, 0 },
+	{ "ar57", 0x39, XT_REG_GENERAL, 0 },
+	{ "ar58", 0x3A, XT_REG_GENERAL, 0 },
+	{ "ar59", 0x3B, XT_REG_GENERAL, 0 },
+	{ "ar60", 0x3C, XT_REG_GENERAL, 0 },
+	{ "ar61", 0x3D, XT_REG_GENERAL, 0 },
+	{ "ar62", 0x3E, XT_REG_GENERAL, 0 },
+	{ "ar63", 0x3F, XT_REG_GENERAL, 0 },
+	{ "lbeg", 0x00, XT_REG_SPECIAL, 0 },
+	{ "lend", 0x01, XT_REG_SPECIAL, 0 },
+	{ "lcount", 0x02, XT_REG_SPECIAL, 0 },
+	{ "sar", 0x03, XT_REG_SPECIAL, 0 },
+	{ "windowbase", 0x48, XT_REG_SPECIAL, 0 },
+	{ "windowstart", 0x49, XT_REG_SPECIAL, 0 },
+	{ "configid0", 0xB0, XT_REG_SPECIAL, 0 },
+	{ "configid1", 0xD0, XT_REG_SPECIAL, 0 },
+	{ "ps", 0xC6, XT_REG_SPECIAL, 0 },			/* actually EPS[debuglevel] */
+	{ "threadptr", 0xE7, XT_REG_USER, 0 },
+	{ "br", 0x04, XT_REG_SPECIAL, 0 },
+	{ "scompare1", 0x0C, XT_REG_SPECIAL, 0 },
+	{ "acclo", 0x10, XT_REG_SPECIAL, 0 },
+	{ "acchi", 0x11, XT_REG_SPECIAL, 0 },
+	{ "m0", 0x20, XT_REG_SPECIAL, 0 },
+	{ "m1", 0x21, XT_REG_SPECIAL, 0 },
+	{ "m2", 0x22, XT_REG_SPECIAL, 0 },
+	{ "m3", 0x23, XT_REG_SPECIAL, 0 },
+	{ "f0", 0x00, XT_REG_FR, XT_REGF_COPROC0 },
+	{ "f1", 0x01, XT_REG_FR, XT_REGF_COPROC0 },
+	{ "f2", 0x02, XT_REG_FR, XT_REGF_COPROC0 },
+	{ "f3", 0x03, XT_REG_FR, XT_REGF_COPROC0 },
+	{ "f4", 0x04, XT_REG_FR, XT_REGF_COPROC0 },
+	{ "f5", 0x05, XT_REG_FR, XT_REGF_COPROC0 },
+	{ "f6", 0x06, XT_REG_FR, XT_REGF_COPROC0 },
+	{ "f7", 0x07, XT_REG_FR, XT_REGF_COPROC0 },
+	{ "f8", 0x08, XT_REG_FR, XT_REGF_COPROC0 },
+	{ "f9", 0x09, XT_REG_FR, XT_REGF_COPROC0 },
+	{ "f10", 0x0A, XT_REG_FR, XT_REGF_COPROC0 },
+	{ "f11", 0x0B, XT_REG_FR, XT_REGF_COPROC0 },
+	{ "f12", 0x0C, XT_REG_FR, XT_REGF_COPROC0 },
+	{ "f13", 0x0D, XT_REG_FR, XT_REGF_COPROC0 },
+	{ "f14", 0x0E, XT_REG_FR, XT_REGF_COPROC0 },
+	{ "f15", 0x0F, XT_REG_FR, XT_REGF_COPROC0 },
+	{ "fcr", 0xE8, XT_REG_USER, XT_REGF_COPROC0 },
+	{ "fsr", 0xE9, XT_REG_USER, XT_REGF_COPROC0 },
+	{ "mmid", 0x59, XT_REG_SPECIAL, XT_REGF_NOREAD },
+	{ "ibreakenable", 0x60, XT_REG_SPECIAL, 0 },
+	{ "memctl", 0x61, XT_REG_SPECIAL, 0 },
+	{ "atomctl", 0x63, XT_REG_SPECIAL, 0 },
+	{ "ibreaka0", 0x80, XT_REG_SPECIAL, 0 },
+	{ "ibreaka1", 0x81, XT_REG_SPECIAL, 0 },
+	{ "dbreaka0", 0x90, XT_REG_SPECIAL, 0 },
+	{ "dbreaka1", 0x91, XT_REG_SPECIAL, 0 },
+	{ "dbreakc0", 0xA0, XT_REG_SPECIAL, 0 },
+	{ "dbreakc1", 0xA1, XT_REG_SPECIAL, 0 },
+	{ "epc1", 0xB1, XT_REG_SPECIAL, 0 },
+	{ "epc2", 0xB2, XT_REG_SPECIAL, 0 },
+	{ "epc3", 0xB3, XT_REG_SPECIAL, 0 },
+	{ "epc4", 0xB4, XT_REG_SPECIAL, 0 },
+	{ "epc5", 0xB5, XT_REG_SPECIAL, 0 },
+	{ "epc6", 0xB6, XT_REG_SPECIAL, 0 },
+	{ "epc7", 0xB7, XT_REG_SPECIAL, 0 },
+	{ "depc", 0xC0, XT_REG_SPECIAL, 0 },
+	{ "eps2", 0xC2, XT_REG_SPECIAL, 0 },
+	{ "eps3", 0xC3, XT_REG_SPECIAL, 0 },
+	{ "eps4", 0xC4, XT_REG_SPECIAL, 0 },
+	{ "eps5", 0xC5, XT_REG_SPECIAL, 0 },
+	{ "eps6", 0xC6, XT_REG_SPECIAL, 0 },
+	{ "eps7", 0xC7, XT_REG_SPECIAL, 0 },
+	{ "excsave1", 0xD1, XT_REG_SPECIAL, 0 },
+	{ "excsave2", 0xD2, XT_REG_SPECIAL, 0 },
+	{ "excsave3", 0xD3, XT_REG_SPECIAL, 0 },
+	{ "excsave4", 0xD4, XT_REG_SPECIAL, 0 },
+	{ "excsave5", 0xD5, XT_REG_SPECIAL, 0 },
+	{ "excsave6", 0xD6, XT_REG_SPECIAL, 0 },
+	{ "excsave7", 0xD7, XT_REG_SPECIAL, 0 },
+	{ "cpenable", 0xE0, XT_REG_SPECIAL, 0 },
+	{ "interrupt", 0xE2, XT_REG_SPECIAL, 0 },
+	{ "intset", 0xE2, XT_REG_SPECIAL, XT_REGF_NOREAD },
+	{ "intclear", 0xE3, XT_REG_SPECIAL, XT_REGF_NOREAD },
+	{ "intenable", 0xE4, XT_REG_SPECIAL, 0 },
+	{ "vecbase", 0xE7, XT_REG_SPECIAL, 0 },
+	{ "exccause", 0xE8, XT_REG_SPECIAL, 0 },
+	{ "debugcause", 0xE9, XT_REG_SPECIAL, 0 },
+	{ "ccount", 0xEA, XT_REG_SPECIAL, 0 },
+	{ "prid", 0xEB, XT_REG_SPECIAL, 0 },
+	{ "icount", 0xEC, XT_REG_SPECIAL, 0 },
+	{ "icountlevel", 0xED, XT_REG_SPECIAL, 0 },
+	{ "excvaddr", 0xEE, XT_REG_SPECIAL, 0 },
+	{ "ccompare0", 0xF0, XT_REG_SPECIAL, 0 },
+	{ "ccompare1", 0xF1, XT_REG_SPECIAL, 0 },
+	{ "ccompare2", 0xF2, XT_REG_SPECIAL, 0 },
+	{ "misc0", 0xF4, XT_REG_SPECIAL, 0 },
+	{ "misc1", 0xF5, XT_REG_SPECIAL, 0 },
+	{ "misc2", 0xF6, XT_REG_SPECIAL, 0 },
+	{ "misc3", 0xF7, XT_REG_SPECIAL, 0 },
+	{ "litbase", 0x05, XT_REG_SPECIAL, 0 },
+	{ "ptevaddr", 0x53, XT_REG_SPECIAL, 0 },
+	{ "rasid", 0x5A, XT_REG_SPECIAL, 0 },
+	{ "itlbcfg", 0x5B, XT_REG_SPECIAL, 0 },
+	{ "dtlbcfg", 0x5C, XT_REG_SPECIAL, 0 },
+	{ "mepc", 0x6A, XT_REG_SPECIAL, 0 },
+	{ "meps", 0x6B, XT_REG_SPECIAL, 0 },
+	{ "mesave", 0x6C, XT_REG_SPECIAL, 0 },
+	{ "mesr", 0x6D, XT_REG_SPECIAL, 0 },
+	{ "mecr", 0x6E, XT_REG_SPECIAL, 0 },
+	{ "mevaddr", 0x6F, XT_REG_SPECIAL, 0 },
+	{ "a0", XT_REG_IDX_AR0, XT_REG_RELGEN, 0 },	/* WARNING: For these registers, regnum points to the */
+	{ "a1", XT_REG_IDX_AR1, XT_REG_RELGEN, 0 },	/* index of the corresponding ARxregisters, NOT to */
+	{ "a2", XT_REG_IDX_AR2, XT_REG_RELGEN, 0 },	/* the processor register number! */
+	{ "a3", XT_REG_IDX_AR3, XT_REG_RELGEN, 0 },
+	{ "a4", XT_REG_IDX_AR4, XT_REG_RELGEN, 0 },
+	{ "a5", XT_REG_IDX_AR5, XT_REG_RELGEN, 0 },
+	{ "a6", XT_REG_IDX_AR6, XT_REG_RELGEN, 0 },
+	{ "a7", XT_REG_IDX_AR7, XT_REG_RELGEN, 0 },
+	{ "a8", XT_REG_IDX_AR8, XT_REG_RELGEN, 0 },
+	{ "a9", XT_REG_IDX_AR9, XT_REG_RELGEN, 0 },
+	{ "a10", XT_REG_IDX_AR10, XT_REG_RELGEN, 0 },
+	{ "a11", XT_REG_IDX_AR11, XT_REG_RELGEN, 0 },
+	{ "a12", XT_REG_IDX_AR12, XT_REG_RELGEN, 0 },
+	{ "a13", XT_REG_IDX_AR13, XT_REG_RELGEN, 0 },
+	{ "a14", XT_REG_IDX_AR14, XT_REG_RELGEN, 0 },
+	{ "a15", XT_REG_IDX_AR15, XT_REG_RELGEN, 0 },
+
+	{ "pwrctl", 0x68, XT_REG_DEBUG, XT_REGF_NOREAD },
+	{ "pwrstat", 0x68, XT_REG_DEBUG, XT_REGF_NOREAD },
+	{ "eristat", 0x68, XT_REG_DEBUG, XT_REGF_NOREAD },
+	{ "cs_itctrl", 0x68, XT_REG_DEBUG, XT_REGF_NOREAD },
+	{ "cs_claimset", 0x68, XT_REG_DEBUG, XT_REGF_NOREAD },
+	{ "cs_claimclr", 0x68, XT_REG_DEBUG, XT_REGF_NOREAD },
+	{ "cs_lockaccess", 0x68, XT_REG_DEBUG, XT_REGF_NOREAD },
+	{ "cs_lockstatus", 0x68, XT_REG_DEBUG, XT_REGF_NOREAD },
+	{ "cs_authstatus", 0x68, XT_REG_DEBUG, XT_REGF_NOREAD },
+	{ "fault_info", 0x68, XT_REG_DEBUG, XT_REGF_NOREAD },
+	{ "trax_id", 0x68, XT_REG_DEBUG, XT_REGF_NOREAD },
+	{ "trax_ctrl", 0x68, XT_REG_DEBUG, XT_REGF_NOREAD },
+	{ "trax_stat", 0x68, XT_REG_DEBUG, XT_REGF_NOREAD },
+	{ "trax_data", 0x68, XT_REG_DEBUG, XT_REGF_NOREAD },
+	{ "trax_addr", 0x68, XT_REG_DEBUG, XT_REGF_NOREAD },
+	{ "trax_pctrigger", 0x68, XT_REG_DEBUG, XT_REGF_NOREAD },
+	{ "trax_pcmatch", 0x68, XT_REG_DEBUG, XT_REGF_NOREAD },
+	{ "trax_delay", 0x68, XT_REG_DEBUG, XT_REGF_NOREAD },
+	{ "trax_memstart", 0x68, XT_REG_DEBUG, XT_REGF_NOREAD },
+	{ "trax_memend", 0x68, XT_REG_DEBUG, XT_REGF_NOREAD },
+	{ "pmg", 0x68, XT_REG_DEBUG, XT_REGF_NOREAD },
+	{ "pmoc", 0x68, XT_REG_DEBUG, XT_REGF_NOREAD },
+	{ "pm0", 0x68, XT_REG_DEBUG, XT_REGF_NOREAD },
+	{ "pm1", 0x68, XT_REG_DEBUG, XT_REGF_NOREAD },
+	{ "pmctrl0", 0x68, XT_REG_DEBUG, XT_REGF_NOREAD },
+	{ "pmctrl1", 0x68, XT_REG_DEBUG, XT_REGF_NOREAD },
+	{ "pmstat0", 0x68, XT_REG_DEBUG, XT_REGF_NOREAD },
+	{ "pmstat1", 0x68, XT_REG_DEBUG, XT_REGF_NOREAD },
+	{ "ocd_id", 0x68, XT_REG_DEBUG, XT_REGF_NOREAD },
+	{ "ocd_dcrclr", 0x68, XT_REG_DEBUG, XT_REGF_NOREAD },
+	{ "ocd_dcrset", 0x68, XT_REG_DEBUG, XT_REGF_NOREAD },
+	{ "ocd_dsr", 0x68, XT_REG_DEBUG, XT_REGF_NOREAD },
+	{ "ddr", 0x68, XT_REG_DEBUG, XT_REGF_NOREAD },
+};
+
+
+/**
+ * Types of memory used at xtensa target
+ */
+enum xtensa_mem_region_type {
+	XTENSA_MEM_REG_IROM = 0x0,
+	XTENSA_MEM_REG_IRAM,
+	XTENSA_MEM_REG_DROM,
+	XTENSA_MEM_REG_DRAM,
+	XTENSA_MEM_REG_URAM,
+	XTENSA_MEM_REG_XLMI,
+	XTENSA_MEM_REGS_NUM
+};
+
+/**
+ * Gets a config for the specific mem type
+ */
+static inline const struct xtensa_local_mem_config *xtensa_get_mem_config(
+	struct xtensa *xtensa,
+	enum xtensa_mem_region_type type)
+{
+	switch (type) {
+	case XTENSA_MEM_REG_IROM:
+		return &xtensa->core_config->irom;
+	case XTENSA_MEM_REG_IRAM:
+		return &xtensa->core_config->iram;
+	case XTENSA_MEM_REG_DROM:
+		return &xtensa->core_config->drom;
+	case XTENSA_MEM_REG_DRAM:
+		return &xtensa->core_config->dram;
+	case XTENSA_MEM_REG_URAM:
+		return &xtensa->core_config->uram;
+	case XTENSA_MEM_REG_XLMI:
+		return &xtensa->core_config->xlmi;
+	default:
+		return NULL;
+	}
+}
+
+/**
+ * Extracts an exact xtensa_local_mem_region_config from xtensa_local_mem_config
+ * for a given address
+ * Returns NULL if nothing found
+ */
+static inline const struct xtensa_local_mem_region_config *xtensa_memory_region_find(
+	const struct xtensa_local_mem_config *mem,
+	target_addr_t address)
+{
+	for (unsigned int i = 0; i < mem->count; i++) {
+		const struct xtensa_local_mem_region_config *region = &mem->regions[i];
+		if (address >= region->base && address < (region->base + region->size))
+			return region;
+	}
+	return NULL;
+}
+
+/**
+ * Returns a corresponding xtensa_local_mem_region_config from the xtensa target
+ * for a given address
+ * Returns NULL if nothing found
+ */
+static inline const struct xtensa_local_mem_region_config *xtensa_target_memory_region_find(
+	struct xtensa *xtensa,
+	target_addr_t address)
+{
+	const struct xtensa_local_mem_region_config *result;
+	const struct xtensa_local_mem_config *mcgf;
+	for (unsigned int mtype = 0; mtype < XTENSA_MEM_REGS_NUM; mtype++) {
+		mcgf = xtensa_get_mem_config(xtensa, mtype);
+		result = xtensa_memory_region_find(mcgf, address);
+		if (result)
+			return result;
+	}
+	return NULL;
+}
+
+static int xtensa_core_reg_get(struct reg *reg)
+{
+	/*We don't need this because we read all registers on halt anyway. */
+	struct xtensa *xtensa = (struct xtensa *)reg->arch_info;
+	struct target *target = xtensa->target;
+
+	if (target->state != TARGET_HALTED)
+		return ERROR_TARGET_NOT_HALTED;
+	return ERROR_OK;
+}
+
+static int xtensa_core_reg_set(struct reg *reg, uint8_t *buf)
+{
+	struct xtensa *xtensa = (struct xtensa *)reg->arch_info;
+	struct target *target = xtensa->target;
+
+	assert(reg->size <= 64 && "up to 64-bit regs are supported only!");
+	if (target->state != TARGET_HALTED)
+		return ERROR_TARGET_NOT_HALTED;
+
+	buf_cpy(buf, reg->value, reg->size);
+	reg->dirty = true;
+	reg->valid = true;
+
+	return ERROR_OK;
+}
+
+static const struct reg_arch_type xtensa_reg_type = {
+	.get = xtensa_core_reg_get,
+	.set = xtensa_core_reg_set,
+};
+
+const struct reg_arch_type xtensa_user_reg_u32_type = {
+	.get = xtensa_core_reg_get,
+	.set = xtensa_core_reg_set,
+};
+
+const struct reg_arch_type xtensa_user_reg_u128_type = {
+	.get = xtensa_core_reg_get,
+	.set = xtensa_core_reg_set,
+};
+
+static inline size_t xtensa_insn_size_get(uint32_t insn)
+{
+	return insn & BIT(3) ? 2 : XT_ISNS_SZ_MAX;
+}
+
+/* Convert a register index that's indexed relative to windowbase, to the real address. */
+static enum xtensa_reg_id xtensa_windowbase_offset_to_canonical(enum xtensa_reg_id reg_idx, int windowbase)
+{
+	unsigned int idx;
+	if (reg_idx >= XT_REG_IDX_AR0 && reg_idx <= XT_REG_IDX_AR63) {
+		idx = reg_idx - XT_REG_IDX_AR0;
+	} else if (reg_idx >= XT_REG_IDX_A0 && reg_idx <= XT_REG_IDX_A15) {
+		idx = reg_idx - XT_REG_IDX_A0;
+	} else {
+		LOG_ERROR("Error: can't convert register %d to non-windowbased register!", reg_idx);
+		return -1;
+	}
+	return ((idx + windowbase * 4) & 63) + XT_REG_IDX_AR0;
+}
+
+static enum xtensa_reg_id xtensa_canonical_to_windowbase_offset(enum xtensa_reg_id reg_idx, int windowbase)
+{
+	return xtensa_windowbase_offset_to_canonical(reg_idx, -windowbase);
+}
+
+static void xtensa_mark_register_dirty(struct xtensa *xtensa, enum xtensa_reg_id reg_idx)
+{
+	struct reg *reg_list = xtensa->core_cache->reg_list;
+	reg_list[reg_idx].dirty = true;
+}
+
+static int xtensa_queue_dbg_reg_read(struct xtensa *xtensa, unsigned int reg, uint8_t *data)
+{
+	struct xtensa_debug_module *dm = &xtensa->dbg_mod;
+
+	if (!xtensa->core_config->trace.enabled &&
+		(reg <= NARADR_MEMADDREND || (reg >= NARADR_PMG && reg <= NARADR_PMSTAT7))) {
+		LOG_ERROR("Can not access %u reg when Trace Port option disabled!", reg);
+		return ERROR_FAIL;
+	}
+	return dm->dbg_ops->queue_reg_read(dm, reg, data);
+}
+
+static int xtensa_queue_dbg_reg_write(struct xtensa *xtensa, unsigned int reg, uint32_t data)
+{
+	struct xtensa_debug_module *dm = &xtensa->dbg_mod;
+
+	if (!xtensa->core_config->trace.enabled &&
+		(reg <= NARADR_MEMADDREND || (reg >= NARADR_PMG && reg <= NARADR_PMSTAT7))) {
+		LOG_ERROR("Can not access %u reg when Trace Port option disabled!", reg);
+		return ERROR_FAIL;
+	}
+	return dm->dbg_ops->queue_reg_write(dm, reg, data);
+}
+
+static void xtensa_queue_exec_ins(struct xtensa *xtensa, uint32_t ins)
+{
+	xtensa_queue_dbg_reg_write(xtensa, NARADR_DIR0EXEC, ins);
+}
+
+static bool xtensa_reg_is_readable(enum xtensa_reg_flags flags, xtensa_reg_val_t cpenable)
+{
+	if (flags & XT_REGF_NOREAD)
+		return false;
+	if ((flags & XT_REGF_COPROC0) && (cpenable & BIT(0)) == 0)
+		return false;
+	return true;
+}
+
+static int xtensa_queue_pwr_reg_write(struct xtensa *xtensa, unsigned int reg, uint32_t data)
+{
+	struct xtensa_debug_module *dm = &xtensa->dbg_mod;
+	return dm->pwr_ops->queue_reg_write(dm, reg, data);
+}
+
+static bool xtensa_special_reg_exists(struct xtensa *xtensa, enum xtensa_reg_id reg_idx)
+{
+	/* TODO: array of size XT_NUM_REGS can be used here to map special register ID to
+	 * corresponding config option 'enabled' flag */
+	if (reg_idx >= XT_REG_IDX_LBEG && reg_idx <= XT_REG_IDX_LCOUNT)
+		return xtensa->core_config->loop;
+	else if (reg_idx == XT_REG_IDX_BR)
+		return xtensa->core_config->boolean;
+	else if (reg_idx == XT_REG_IDX_LITBASE)
+		return xtensa->core_config->ext_l32r;
+	else if (reg_idx == XT_REG_IDX_SCOMPARE1 || reg_idx == XT_REG_IDX_ATOMCTL)
+		return xtensa->core_config->cond_store;
+	else if (reg_idx >= XT_REG_IDX_ACCLO && reg_idx <= XT_REG_IDX_M3)
+		return xtensa->core_config->mac16;
+	else if (reg_idx == XT_REG_IDX_WINDOWBASE || reg_idx == XT_REG_IDX_WINDOWSTART)
+		return xtensa->core_config->windowed;
+	else if (reg_idx >= XT_REG_IDX_PTEVADDR && reg_idx <= XT_REG_IDX_DTLBCFG)
+		return xtensa->core_config->mmu.enabled;
+	else if (reg_idx == XT_REG_IDX_MMID)
+		return xtensa->core_config->trace.enabled;
+	else if (reg_idx >= XT_REG_IDX_MEPC && reg_idx <= XT_REG_IDX_MEVADDR)
+		return xtensa->core_config->mem_err_check;
+	else if (reg_idx == XT_REG_IDX_CPENABLE)
+		return xtensa->core_config->coproc;
+	else if (reg_idx == XT_REG_IDX_VECBASE)
+		return xtensa->core_config->reloc_vec;
+	else if (reg_idx == XT_REG_IDX_CCOUNT)
+		return xtensa->core_config->tim_irq.enabled;
+	else if (reg_idx >= XT_REG_IDX_CCOMPARE0 && reg_idx <= XT_REG_IDX_CCOMPARE2)
+		return xtensa->core_config->tim_irq.enabled &&
+		       (reg_idx - XT_REG_IDX_CCOMPARE0 < xtensa->core_config->tim_irq.comp_num);
+	else if (reg_idx == XT_REG_IDX_PRID)
+		return xtensa->core_config->proc_id;
+	else if (reg_idx >= XT_REG_IDX_MISC0 && reg_idx <= XT_REG_IDX_MISC3)
+		return reg_idx - XT_REG_IDX_MISC0 < xtensa->core_config->miscregs_num;
+	return true;
+}
+
+static bool xtensa_user_reg_exists(struct xtensa *xtensa, enum xtensa_reg_id reg_idx)
+{
+	if (reg_idx == XT_REG_IDX_THREADPTR)
+		return xtensa->core_config->threadptr;
+	if (reg_idx == XT_REG_IDX_FCR || reg_idx == XT_REG_IDX_FSR)
+		return xtensa->core_config->fp_coproc;
+	return false;
+}
+
+static inline bool xtensa_fp_reg_exists(struct xtensa *xtensa, enum xtensa_reg_id reg_idx)
+{
+	return xtensa->core_config->fp_coproc;
+}
+
+static inline bool xtensa_regular_reg_exists(struct xtensa *xtensa, enum xtensa_reg_id reg_idx)
+{
+	if (reg_idx >= XT_REG_IDX_AR0 && reg_idx <= XT_REG_IDX_AR63)
+		return reg_idx - XT_REG_IDX_AR0 < xtensa->core_config->aregs_num;
+	return true;
+}
+
+static int xtensa_write_dirty_registers(struct target *target)
+{
+	struct xtensa *xtensa = target_to_xtensa(target);
+	int res;
+	xtensa_reg_val_t regval, windowbase = 0;
+	bool scratch_reg_dirty = false;
+	struct reg *reg_list = xtensa->core_cache->reg_list;
+
+	LOG_TARGET_DEBUG(target, "start");
+
+	/*We need to write the dirty registers in the cache list back to the processor.
+	 *Start by writing the SFR/user registers. */
+	for (unsigned int i = 0; i < XT_NUM_REGS; i++) {
+		if (reg_list[i].dirty) {
+			if (xtensa_regs[i].type == XT_REG_SPECIAL ||
+				xtensa_regs[i].type == XT_REG_USER ||
+				xtensa_regs[i].type == XT_REG_FR) {
+				scratch_reg_dirty = true;
+				regval = xtensa_reg_get(target, i);
+				LOG_TARGET_DEBUG(target, "Writing back reg %s val %08" PRIX32,
+					xtensa_regs[i].name,
+					regval);
+				xtensa_queue_dbg_reg_write(xtensa, NARADR_DDR, regval);
+				xtensa_queue_exec_ins(xtensa, XT_INS_RSR(XT_SR_DDR, XT_REG_A3));
+				if (xtensa_regs[i].type == XT_REG_USER) {
+					if (reg_list[i].exist)
+						xtensa_queue_exec_ins(xtensa,
+							XT_INS_WUR(xtensa_regs[i].reg_num,
+								XT_REG_A3));
+				} else if (xtensa_regs[i].type == XT_REG_FR) {
+					if (reg_list[i].exist)
+						xtensa_queue_exec_ins(xtensa,
+							XT_INS_WFR(xtensa_regs[i].reg_num,
+								XT_REG_A3));
+				} else {/*SFR */
+					if (reg_list[i].exist) {
+						unsigned int reg_num = xtensa_regs[i].reg_num;
+						if (reg_num == XT_PC_REG_NUM_BASE)
+							/* reg number of PC for debug interrupt
+							 * depends on NDEBUGLEVEL */
+							reg_num += xtensa->core_config->debug.irq_level;
+
+						xtensa_queue_exec_ins(xtensa,
+							XT_INS_WSR(reg_num, XT_REG_A3));
+					}
+				}
+				reg_list[i].dirty = false;
+			}
+		}
+	}
+	if (scratch_reg_dirty)
+		xtensa_mark_register_dirty(xtensa, XT_REG_IDX_A3);
+
+	if (xtensa->core_config->user_regs_num > 0 &&
+		xtensa->core_config->queue_write_dirty_user_regs)
+		xtensa->core_config->queue_write_dirty_user_regs(target);
+
+	if (xtensa->core_config->windowed) {
+		/*Grab the windowbase, we need it. */
+		windowbase = xtensa_reg_get(target, XT_REG_IDX_WINDOWBASE);
+		/*Check if there are problems with both the ARx as well as the corresponding Rx
+		 * registers set and dirty. */
+		/*Warn the user if this happens, not much else we can do... */
+		for (unsigned int i = XT_REG_IDX_A0; i <= XT_REG_IDX_A15; i++) {
+			unsigned int j = xtensa_windowbase_offset_to_canonical(i, windowbase);
+			if (reg_list[i].dirty && reg_list[j].dirty) {
+				if (memcmp(reg_list[i].value, reg_list[j].value,
+						sizeof(xtensa_reg_val_t)) != 0)
+					LOG_WARNING(
+						"Warning: Both A%d as well as the physical register it points to (AR%d) are dirty and differs in value. Results are undefined!",
+						i - XT_REG_IDX_A0,
+						j - XT_REG_IDX_AR0);
+			}
+		}
+	}
+
+	/*Write A0-A16 */
+	for (unsigned int i = 0; i < 16; i++) {
+		if (reg_list[XT_REG_IDX_A0 + i].dirty) {
+			regval = xtensa_reg_get(target, XT_REG_IDX_A0 + i);
+			LOG_TARGET_DEBUG(target, "Writing back reg %s value %08" PRIX32 ", num =%i",
+				xtensa_regs[XT_REG_IDX_A0 + i].name,
+				regval,
+				xtensa_regs[XT_REG_IDX_A0 + i].reg_num);
+			xtensa_queue_dbg_reg_write(xtensa, NARADR_DDR, regval);
+			xtensa_queue_exec_ins(xtensa, XT_INS_RSR(XT_SR_DDR, i));
+			reg_list[XT_REG_IDX_A0 + i].dirty = false;
+		}
+	}
+
+	if (xtensa->core_config->windowed) {
+		/*Now write AR0-AR63. */
+		for (unsigned int j = 0; j < 64; j += 16) {
+			/*Write the 16 registers we can see */
+			for (unsigned int i = 0; i < 16; i++) {
+				if (i + j < xtensa->core_config->aregs_num) {
+					enum xtensa_reg_id realadr =
+						xtensa_windowbase_offset_to_canonical(XT_REG_IDX_AR0 + i + j,
+						windowbase);
+					/*Write back any dirty un-windowed registers */
+					if (reg_list[realadr].dirty) {
+						regval = xtensa_reg_get(target, realadr);
+						LOG_TARGET_DEBUG(
+							target,
+							"Writing back reg %s value %08" PRIX32 ", num =%i",
+							xtensa_regs[realadr].name,
+							regval,
+							xtensa_regs[realadr].reg_num);
+						xtensa_queue_dbg_reg_write(xtensa, NARADR_DDR, regval);
+						xtensa_queue_exec_ins(xtensa,
+							XT_INS_RSR(XT_SR_DDR, xtensa_regs[XT_REG_IDX_AR0 + i].reg_num));
+						reg_list[realadr].dirty = false;
+					}
+				}
+			}
+			/*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(4));
+		}
+	}
+	res = jtag_execute_queue();
+	xtensa_core_status_check(target);
+
+	return res;
+}
+
+int xtensa_queue_write_dirty_user_regs_u32(struct target *target)
+{
+	struct xtensa *xtensa = target_to_xtensa(target);
+	struct reg *reg_list = xtensa->core_cache->reg_list;
+	xtensa_reg_val_t reg_val;
+	bool scratch_reg_dirty = false;
+
+	LOG_TARGET_DEBUG(target, "start");
+
+	/* We need to write the dirty registers in the cache list back to the processor.
+	 * Start by writing the SFR/user registers. */
+	for (unsigned int i = 0; i < xtensa->core_config->user_regs_num; i++) {
+		if (!reg_list[XT_USR_REG_START + i].dirty)
+			continue;
+		scratch_reg_dirty = true;
+		reg_val = xtensa_reg_get(target, XT_USR_REG_START + i);
+		LOG_TARGET_DEBUG(target, "Writing back reg %s val %08" PRIX32,
+			xtensa->core_config->user_regs[i].name,
+			reg_val);
+		xtensa_queue_dbg_reg_write(xtensa, NARADR_DDR, reg_val);
+		xtensa_queue_exec_ins(xtensa, XT_INS_RSR(XT_SR_DDR, XT_REG_A3));
+		xtensa_queue_exec_ins(xtensa,
+			XT_INS_WUR(xtensa->core_config->user_regs[i].reg_num,
+				XT_REG_A3));
+		reg_list[XT_USR_REG_START + i].dirty = false;
+	}
+	if (scratch_reg_dirty)
+		xtensa_mark_register_dirty(xtensa, XT_REG_IDX_A3);
+
+	return ERROR_OK;
+}
+
+static inline bool xtensa_is_stopped(struct target *target)
+{
+	struct xtensa *xtensa = target_to_xtensa(target);
+	return xtensa->dbg_mod.core_status.dsr & OCDDSR_STOPPED;
+}
+
+int xtensa_examine(struct target *target)
+{
+	struct xtensa *xtensa = target_to_xtensa(target);
+	unsigned int cmd = PWRCTL_DEBUGWAKEUP | PWRCTL_MEMWAKEUP | PWRCTL_COREWAKEUP;
+
+	LOG_DEBUG("coreid = %d", target->coreid);
+	xtensa_queue_pwr_reg_write(xtensa, DMREG_PWRCTL, cmd);
+	xtensa_queue_pwr_reg_write(xtensa, DMREG_PWRCTL, cmd | PWRCTL_JTAGDEBUGUSE);
+	xtensa_dm_queue_enable(&xtensa->dbg_mod);
+	xtensa_dm_queue_tdi_idle(&xtensa->dbg_mod);
+	int res = jtag_execute_queue();
+	if (res != ERROR_OK)
+		return res;
+	if (!xtensa_dm_is_online(&xtensa->dbg_mod)) {
+		LOG_ERROR("Unexpected OCD_ID = %08" PRIx32, xtensa->dbg_mod.device_id);
+		return ERROR_TARGET_FAILURE;
+	}
+	LOG_DEBUG("OCD_ID = %08" PRIx32, xtensa->dbg_mod.device_id);
+	if (!target_was_examined(target))
+		target_set_examined(target);
+	xtensa_smpbreak_write(xtensa, xtensa->smp_break);
+	return ERROR_OK;
+}
+
+int xtensa_wakeup(struct target *target)
+{
+	struct xtensa *xtensa = target_to_xtensa(target);
+	unsigned int cmd = PWRCTL_DEBUGWAKEUP | PWRCTL_MEMWAKEUP | PWRCTL_COREWAKEUP;
+
+	if (xtensa->reset_asserted)
+		cmd |= PWRCTL_CORERESET;
+	xtensa_queue_pwr_reg_write(xtensa, DMREG_PWRCTL, cmd);
+	/* TODO: can we join this with the write above? */
+	xtensa_queue_pwr_reg_write(xtensa, DMREG_PWRCTL, cmd | PWRCTL_JTAGDEBUGUSE);
+	xtensa_dm_queue_tdi_idle(&xtensa->dbg_mod);
+	return jtag_execute_queue();
+}
+
+int xtensa_smpbreak_write(struct xtensa *xtensa, uint32_t set)
+{
+	uint32_t dsr_data = 0x00110000;
+	uint32_t clear = (set | OCDDCR_ENABLEOCD) ^
+		(OCDDCR_BREAKINEN | OCDDCR_BREAKOUTEN | OCDDCR_RUNSTALLINEN |
+		OCDDCR_DEBUGMODEOUTEN | OCDDCR_ENABLEOCD);
+
+	LOG_TARGET_DEBUG(xtensa->target, "write smpbreak set=0x%" PRIx32 " clear=0x%" PRIx32, set, clear);
+	xtensa_queue_dbg_reg_write(xtensa, NARADR_DCRSET, set | OCDDCR_ENABLEOCD);
+	xtensa_queue_dbg_reg_write(xtensa, NARADR_DCRCLR, clear);
+	xtensa_queue_dbg_reg_write(xtensa, NARADR_DSR, dsr_data);
+	xtensa_dm_queue_tdi_idle(&xtensa->dbg_mod);
+	return jtag_execute_queue();
+}
+
+int xtensa_smpbreak_set(struct target *target, uint32_t set)
+{
+	struct xtensa *xtensa = target_to_xtensa(target);
+	int res = ERROR_OK;
+
+	xtensa->smp_break = set;
+	if (target_was_examined(target))
+		res = xtensa_smpbreak_write(xtensa, xtensa->smp_break);
+	LOG_TARGET_DEBUG(target, "set smpbreak=%" PRIx32 ", state=%i", set, target->state);
+	return res;
+}
+
+int xtensa_smpbreak_read(struct xtensa *xtensa, uint32_t *val)
+{
+	uint8_t dcr_buf[sizeof(uint32_t)];
+
+	xtensa_queue_dbg_reg_read(xtensa, NARADR_DCRSET, dcr_buf);
+	xtensa_dm_queue_tdi_idle(&xtensa->dbg_mod);
+	int res = jtag_execute_queue();
+	*val = buf_get_u32(dcr_buf, 0, 32);
+
+	return res;
+}
+
+int xtensa_smpbreak_get(struct target *target, uint32_t *val)
+{
+	struct xtensa *xtensa = target_to_xtensa(target);
+	*val = xtensa->smp_break;
+	return ERROR_OK;
+}
+
+static inline xtensa_reg_val_t xtensa_reg_get_value(struct reg *reg)
+{
+	return buf_get_u32(reg->value, 0, 32);
+}
+
+static inline void xtensa_reg_set_value(struct reg *reg, xtensa_reg_val_t value)
+{
+	buf_set_u32(reg->value, 0, 32, value);
+	reg->dirty = true;
+}
+
+int xtensa_core_status_check(struct target *target)
+{
+	struct xtensa *xtensa = target_to_xtensa(target);
+	int res, needclear = 0;
+
+	xtensa_dm_core_status_read(&xtensa->dbg_mod);
+	xtensa_dsr_t dsr = xtensa_dm_core_status_get(&xtensa->dbg_mod);
+	LOG_TARGET_DEBUG(target, "DSR (%08" PRIX32 ")", dsr);
+	if (dsr & OCDDSR_EXECBUSY) {
+		if (!xtensa->suppress_dsr_errors)
+			LOG_TARGET_ERROR(target, "DSR (%08" PRIX32 ") indicates target still busy!", dsr);
+		needclear = 1;
+	}
+	if (dsr & OCDDSR_EXECEXCEPTION) {
+		if (!xtensa->suppress_dsr_errors)
+			LOG_TARGET_ERROR(target,
+				"DSR (%08" PRIX32 ") indicates DIR instruction generated an exception!",
+				dsr);
+		needclear = 1;
+	}
+	if (dsr & OCDDSR_EXECOVERRUN) {
+		if (!xtensa->suppress_dsr_errors)
+			LOG_TARGET_ERROR(target,
+				"DSR (%08" PRIX32 ") indicates DIR instruction generated an overrun!",
+				dsr);
+		needclear = 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!");
+		return xtensa->suppress_dsr_errors ? ERROR_OK : ERROR_FAIL;
+	}
+	return ERROR_OK;
+}
+
+xtensa_reg_val_t xtensa_reg_get(struct target *target, enum xtensa_reg_id reg_id)
+{
+	struct xtensa *xtensa = target_to_xtensa(target);
+	struct reg *reg = &xtensa->core_cache->reg_list[reg_id];
+	assert(reg_id < xtensa->core_cache->num_regs && "Attempt to access non-existing reg!");
+	return xtensa_reg_get_value(reg);
+}
+
+void xtensa_reg_set(struct target *target, enum xtensa_reg_id reg_id, xtensa_reg_val_t value)
+{
+	struct xtensa *xtensa = target_to_xtensa(target);
+	struct reg *reg = &xtensa->core_cache->reg_list[reg_id];
+	assert(reg_id < xtensa->core_cache->num_regs && "Attempt to access non-existing reg!");
+	if (xtensa_reg_get_value(reg) == value)
+		return;
+	xtensa_reg_set_value(reg, value);
+}
+
+int xtensa_assert_reset(struct target *target)
+{
+	struct xtensa *xtensa = target_to_xtensa(target);
+
+	LOG_TARGET_DEBUG(target, "target_number=%i, begin", target->target_number);
+	target->state = TARGET_RESET;
+	xtensa_queue_pwr_reg_write(xtensa,
+		DMREG_PWRCTL,
+		PWRCTL_JTAGDEBUGUSE | PWRCTL_DEBUGWAKEUP | PWRCTL_MEMWAKEUP | PWRCTL_COREWAKEUP |
+		PWRCTL_CORERESET);
+	xtensa_dm_queue_tdi_idle(&xtensa->dbg_mod);
+	int res = jtag_execute_queue();
+	if (res != ERROR_OK)
+		return res;
+	xtensa->reset_asserted = true;
+	return res;
+}
+
+int xtensa_deassert_reset(struct target *target)
+{
+	struct xtensa *xtensa = target_to_xtensa(target);
+
+	LOG_TARGET_DEBUG(target, "halt=%d", target->reset_halt);
+	if (target->reset_halt)
+		xtensa_queue_dbg_reg_write(xtensa,
+			NARADR_DCRSET,
+			OCDDCR_ENABLEOCD | OCDDCR_DEBUGINTERRUPT);
+	xtensa_queue_pwr_reg_write(xtensa,
+		DMREG_PWRCTL,
+		PWRCTL_JTAGDEBUGUSE | PWRCTL_DEBUGWAKEUP | PWRCTL_MEMWAKEUP | PWRCTL_COREWAKEUP);
+	xtensa_dm_queue_tdi_idle(&xtensa->dbg_mod);
+	int res = jtag_execute_queue();
+	if (res != ERROR_OK)
+		return res;
+	target->state = TARGET_RUNNING;
+	xtensa->reset_asserted = false;
+	return res;
+}
+
+int xtensa_fetch_all_regs(struct target *target)
+{
+	struct xtensa *xtensa = target_to_xtensa(target);
+	struct reg *reg_list = xtensa->core_cache->reg_list;
+	xtensa_reg_val_t cpenable = 0, windowbase = 0;
+	uint8_t regvals[XT_NUM_REGS][sizeof(xtensa_reg_val_t)];
+	uint8_t dsrs[XT_NUM_REGS][sizeof(xtensa_dsr_t)];
+	bool debug_dsrs = !xtensa->regs_fetched || LOG_LEVEL_IS(LOG_LVL_DEBUG);
+
+	LOG_TARGET_DEBUG(target, "start");
+
+	/* Assume the CPU has just halted. We now want to fill the register cache with all the
+	 * register contents GDB needs. For speed, we pipeline all the read operations, execute them
+	 * in one go, then sort everything out from the regvals variable. */
+
+	/* Start out with AREGS; we can reach those immediately. Grab them per 16 registers. */
+	for (unsigned int j = 0; j < XT_AREGS_NUM_MAX; j += 16) {
+		/*Grab the 16 registers we can see */
+		for (unsigned int i = 0; i < 16; i++) {
+			if (i + j < xtensa->core_config->aregs_num) {
+				xtensa_queue_exec_ins(xtensa,
+					XT_INS_WSR(XT_SR_DDR, xtensa_regs[XT_REG_IDX_AR0 + i].reg_num));
+				xtensa_queue_dbg_reg_read(xtensa, NARADR_DDR, regvals[XT_REG_IDX_AR0 + i + j]);
+				if (debug_dsrs)
+					xtensa_queue_dbg_reg_read(xtensa, NARADR_DSR, dsrs[XT_REG_IDX_AR0 + i + j]);
+			}
+		}
+		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(4));
+		}
+	}
+	if (xtensa->core_config->coproc) {
+		/* As the very first thing after AREGS, go grab the CPENABLE registers. It indicates
+		 * if we can also grab the FP */
+		/* (and theoretically other coprocessor) registers, or if this is a bad thing to do.*/
+		xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa_regs[XT_REG_IDX_CPENABLE].reg_num, XT_REG_A3));
+		xtensa_queue_exec_ins(xtensa, XT_INS_WSR(XT_SR_DDR, XT_REG_A3));
+		xtensa_queue_dbg_reg_read(xtensa, NARADR_DDR, regvals[XT_REG_IDX_CPENABLE]);
+	}
+	int res = jtag_execute_queue();
+	if (res != ERROR_OK) {
+		LOG_ERROR("Failed to read ARs (%d)!", res);
+		return res;
+	}
+	xtensa_core_status_check(target);
+
+	if (xtensa->core_config->coproc)
+		cpenable = buf_get_u32(regvals[XT_REG_IDX_CPENABLE], 0, 32);
+	/* We're now free to use any of A0-A15 as scratch registers
+	 * Grab the SFRs and user registers first. We use A3 as a scratch register. */
+	for (unsigned int i = 0; i < XT_NUM_REGS; i++) {
+		if (xtensa_reg_is_readable(xtensa_regs[i].flags, cpenable) && reg_list[i].exist &&
+			(xtensa_regs[i].type == XT_REG_SPECIAL ||
+				xtensa_regs[i].type == XT_REG_USER || xtensa_regs[i].type == XT_REG_FR)) {
+			if (xtensa_regs[i].type == XT_REG_USER) {
+				xtensa_queue_exec_ins(xtensa, XT_INS_RUR(xtensa_regs[i].reg_num, XT_REG_A3));
+			} else if (xtensa_regs[i].type == XT_REG_FR) {
+				xtensa_queue_exec_ins(xtensa, XT_INS_RFR(xtensa_regs[i].reg_num, XT_REG_A3));
+			} else {	/*SFR */
+				unsigned int reg_num = xtensa_regs[i].reg_num;
+				if (reg_num == XT_PC_REG_NUM_BASE) {
+					/* reg number of PC for debug interrupt depends on NDEBUGLEVEL */
+					reg_num += xtensa->core_config->debug.irq_level;
+				}
+				xtensa_queue_exec_ins(xtensa, XT_INS_RSR(reg_num, XT_REG_A3));
+			}
+			xtensa_queue_exec_ins(xtensa, XT_INS_WSR(XT_SR_DDR, XT_REG_A3));
+			xtensa_queue_dbg_reg_read(xtensa, NARADR_DDR, regvals[i]);
+			if (debug_dsrs)
+				xtensa_queue_dbg_reg_read(xtensa, NARADR_DSR, dsrs[i]);
+		}
+	}
+	/* Ok, send the whole mess to the CPU. */
+	res = jtag_execute_queue();
+	if (res != ERROR_OK) {
+		LOG_ERROR("Failed to fetch AR regs!");
+		return res;
+	}
+	xtensa_core_status_check(target);
+
+	if (debug_dsrs) {
+		/* DSR checking: follows order in which registers are requested. */
+		for (unsigned int i = 0; i < XT_NUM_REGS; i++) {
+			if (xtensa_reg_is_readable(xtensa_regs[i].flags, cpenable) && reg_list[i].exist &&
+				(xtensa_regs[i].type == XT_REG_SPECIAL || xtensa_regs[i].type == XT_REG_USER ||
+					xtensa_regs[i].type == XT_REG_FR)) {
+				if (buf_get_u32(dsrs[i], 0, 32) & OCDDSR_EXECEXCEPTION) {
+					LOG_ERROR("Exception reading %s!", xtensa_regs[i].name);
+					return ERROR_FAIL;
+				}
+			}
+		}
+	}
+
+	if (xtensa->core_config->user_regs_num > 0 && xtensa->core_config->fetch_user_regs) {
+		res = xtensa->core_config->fetch_user_regs(target);
+		if (res != ERROR_OK)
+			return res;
+	}
+
+	if (xtensa->core_config->windowed) {
+		/* We need the windowbase to decode the general addresses. */
+		windowbase = buf_get_u32(regvals[XT_REG_IDX_WINDOWBASE], 0, 32);
+	}
+	/* Decode the result and update the cache. */
+	for (unsigned int i = 0; i < XT_NUM_REGS; i++) {
+		if (xtensa_reg_is_readable(xtensa_regs[i].flags, cpenable) && reg_list[i].exist) {
+			if (xtensa_regs[i].type == XT_REG_GENERAL) {
+				/* TODO: add support for non-windowed configs */
+				assert(
+					xtensa->core_config->windowed &&
+					"Regs fetch is not supported for non-windowed configs!");
+				/* The 64-value general register set is read from (windowbase) on down.
+				 * We need to get the real register address by subtracting windowbase and
+				 * wrapping around. */
+				int realadr = xtensa_canonical_to_windowbase_offset(i, windowbase);
+				buf_cpy(regvals[realadr], reg_list[i].value, reg_list[i].size);
+			} else if (xtensa_regs[i].type == XT_REG_RELGEN) {
+				buf_cpy(regvals[xtensa_regs[i].reg_num], reg_list[i].value, reg_list[i].size);
+			} else {
+				buf_cpy(regvals[i], reg_list[i].value, reg_list[i].size);
+			}
+			reg_list[i].valid = true;
+		} else {
+			reg_list[i].valid = false;
+		}
+	}
+	/* We have used A3 as a scratch register and we will need to write that back. */
+	xtensa_mark_register_dirty(xtensa, XT_REG_IDX_A3);
+	xtensa->regs_fetched = true;
+
+	return ERROR_OK;
+}
+
+int xtensa_fetch_user_regs_u32(struct target *target)
+{
+	struct xtensa *xtensa = target_to_xtensa(target);
+	struct reg *reg_list = xtensa->core_cache->reg_list;
+	xtensa_reg_val_t cpenable = 0;
+	uint8_t regvals[XT_USER_REGS_NUM_MAX][sizeof(xtensa_reg_val_t)];
+	uint8_t dsrs[XT_USER_REGS_NUM_MAX][sizeof(xtensa_dsr_t)];
+	bool debug_dsrs = !xtensa->regs_fetched || LOG_LEVEL_IS(LOG_LVL_DEBUG);
+
+	assert(xtensa->core_config->user_regs_num < XT_USER_REGS_NUM_MAX && "Too many user regs configured!");
+	if (xtensa->core_config->coproc)
+		cpenable = xtensa_reg_get(target, XT_REG_IDX_CPENABLE);
+
+	for (unsigned int i = 0; i < xtensa->core_config->user_regs_num; i++) {
+		if (!xtensa_reg_is_readable(xtensa->core_config->user_regs[i].flags, cpenable))
+			continue;
+		xtensa_queue_exec_ins(xtensa, XT_INS_RUR(xtensa->core_config->user_regs[i].reg_num, XT_REG_A3));
+		xtensa_queue_exec_ins(xtensa, XT_INS_WSR(XT_SR_DDR, XT_REG_A3));
+		xtensa_queue_dbg_reg_read(xtensa, NARADR_DDR, regvals[i]);
+		if (debug_dsrs)
+			xtensa_queue_dbg_reg_read(xtensa, NARADR_DSR, dsrs[i]);
+	}
+	/* Ok, send the whole mess to the CPU. */
+	int res = jtag_execute_queue();
+	if (res != ERROR_OK) {
+		LOG_ERROR("Failed to fetch AR regs!");
+		return res;
+	}
+	xtensa_core_status_check(target);
+
+	if (debug_dsrs) {
+		/* DSR checking: follows order in which registers are requested. */
+		for (unsigned int i = 0; i < xtensa->core_config->user_regs_num; i++) {
+			if (!xtensa_reg_is_readable(xtensa->core_config->user_regs[i].flags, cpenable))
+				continue;
+			if (buf_get_u32(dsrs[i], 0, 32) & OCDDSR_EXECEXCEPTION) {
+				LOG_ERROR("Exception reading %s!", xtensa->core_config->user_regs[i].name);
+				return ERROR_FAIL;
+			}
+		}
+	}
+
+	for (unsigned int i = 0; i < xtensa->core_config->user_regs_num; i++) {
+		if (xtensa_reg_is_readable(xtensa->core_config->user_regs[i].flags, cpenable)) {
+			buf_cpy(regvals[i], reg_list[XT_USR_REG_START + i].value, reg_list[XT_USR_REG_START + i].size);
+			reg_list[XT_USR_REG_START + i].valid = true;
+		} else {
+			reg_list[XT_USR_REG_START + i].valid = false;
+		}
+	}
+
+	/* We have used A3 as a scratch register and we will need to write that back. */
+	xtensa_mark_register_dirty(xtensa, XT_REG_IDX_A3);
+	return ERROR_OK;
+}
+
+int xtensa_get_gdb_reg_list(struct target *target,
+	struct reg **reg_list[],
+	int *reg_list_size,
+	enum target_register_class reg_class)
+{
+	struct xtensa *xtensa = target_to_xtensa(target);
+	unsigned int num_regs = xtensa->core_config->gdb_general_regs_num;
+
+	if (reg_class == REG_CLASS_ALL)
+		num_regs = xtensa->regs_num;
+
+	LOG_DEBUG("reg_class=%i, num_regs=%d", reg_class, num_regs);
+
+	*reg_list = malloc(num_regs * sizeof(struct reg *));
+	if (!*reg_list)
+		return ERROR_FAIL;
+
+	for (unsigned int k = 0; k < num_regs; k++) {
+		unsigned int reg_id = xtensa->core_config->gdb_regs_mapping[k];
+		(*reg_list)[k] = &xtensa->core_cache->reg_list[reg_id];
+	}
+
+	*reg_list_size = num_regs;
+
+	return ERROR_OK;
+}
+
+int xtensa_mmu_is_enabled(struct target *target, int *enabled)
+{
+	struct xtensa *xtensa = target_to_xtensa(target);
+	*enabled = xtensa->core_config->mmu.itlb_entries_count > 0 ||
+		xtensa->core_config->mmu.dtlb_entries_count > 0;
+	return ERROR_OK;
+}
+
+int xtensa_halt(struct target *target)
+{
+	struct xtensa *xtensa = target_to_xtensa(target);
+
+	LOG_TARGET_DEBUG(target, "start");
+	if (target->state == TARGET_HALTED) {
+		LOG_TARGET_DEBUG(target, "target was already halted");
+		return ERROR_OK;
+	}
+	/* First we have to read dsr and check if the target stopped */
+	int res = xtensa_dm_core_status_read(&xtensa->dbg_mod);
+	if (res != ERROR_OK) {
+		LOG_TARGET_ERROR(target, "Failed to read core status!");
+		return res;
+	}
+	LOG_TARGET_DEBUG(target, "Core status 0x%" PRIx32, xtensa_dm_core_status_get(&xtensa->dbg_mod));
+	if (!xtensa_is_stopped(target)) {
+		xtensa_queue_dbg_reg_write(xtensa, NARADR_DCRSET, OCDDCR_ENABLEOCD | OCDDCR_DEBUGINTERRUPT);
+		xtensa_dm_queue_tdi_idle(&xtensa->dbg_mod);
+		res = jtag_execute_queue();
+		if (res != ERROR_OK)
+			LOG_TARGET_ERROR(target, "Failed to set OCDDCR_DEBUGINTERRUPT. Can't halt.");
+	}
+
+	return res;
+}
+
+int xtensa_prepare_resume(struct target *target,
+	int current,
+	target_addr_t address,
+	int handle_breakpoints,
+	int debug_execution)
+{
+	struct xtensa *xtensa = target_to_xtensa(target);
+	uint32_t bpena = 0;
+
+	LOG_TARGET_DEBUG(target,
+		"current=%d address=" TARGET_ADDR_FMT ", handle_breakpoints=%i, debug_execution=%i)",
+		current,
+		address,
+		handle_breakpoints,
+		debug_execution);
+
+	if (target->state != TARGET_HALTED) {
+		LOG_TARGET_WARNING(target, "target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (address && !current) {
+		xtensa_reg_set(target, XT_REG_IDX_PC, address);
+	} else {
+		xtensa_reg_val_t cause = xtensa_reg_get(target, XT_REG_IDX_DEBUGCAUSE);
+		if (cause & DEBUGCAUSE_DB) {
+			/* We stopped due to a watchpoint. We can't just resume executing the
+			 * instruction again because */
+			/* that would trigger the watchpoint again. To fix this, we single-step,
+			 * which ignores watchpoints. */
+			xtensa_do_step(target, current, address, handle_breakpoints);
+		}
+		if (cause & (DEBUGCAUSE_BI | DEBUGCAUSE_BN)) {
+			/* We stopped due to a break instruction. We can't just resume executing the
+			 * instruction again because */
+			/* that would trigger the break again. To fix this, we single-step, which
+			 * ignores break. */
+			xtensa_do_step(target, current, address, handle_breakpoints);
+		}
+	}
+
+	/* Write back hw breakpoints. Current FreeRTOS SMP code can set a hw breakpoint on an
+	 * exception; we need to clear that and return to the breakpoints gdb has set on resume. */
+	for (unsigned int slot = 0; slot < xtensa->core_config->debug.ibreaks_num; slot++) {
+		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);
+			bpena |= BIT(slot);
+		}
+	}
+	xtensa_reg_set(target, XT_REG_IDX_IBREAKENABLE, bpena);
+
+	/* Here we write all registers to the targets */
+	int res = xtensa_write_dirty_registers(target);
+	if (res != ERROR_OK)
+		LOG_TARGET_ERROR(target, "Failed to write back register cache.");
+	return res;
+}
+
+int xtensa_do_resume(struct target *target)
+{
+	struct xtensa *xtensa = target_to_xtensa(target);
+
+	LOG_TARGET_DEBUG(target, "start");
+
+	xtensa_queue_exec_ins(xtensa, XT_INS_RFDO);
+	int res = jtag_execute_queue();
+	if (res != ERROR_OK) {
+		LOG_TARGET_ERROR(target, "Failed to exec RFDO %d!", res);
+		return res;
+	}
+	xtensa_core_status_check(target);
+	return ERROR_OK;
+}
+
+int xtensa_resume(struct target *target,
+	int current,
+	target_addr_t address,
+	int handle_breakpoints,
+	int debug_execution)
+{
+	LOG_TARGET_DEBUG(target, "start");
+	int res = xtensa_prepare_resume(target, current, address, handle_breakpoints, debug_execution);
+	if (res != ERROR_OK) {
+		LOG_TARGET_ERROR(target, "Failed to prepare for resume!");
+		return res;
+	}
+	res = xtensa_do_resume(target);
+	if (res != ERROR_OK) {
+		LOG_TARGET_ERROR(target, "Failed to resume!");
+		return res;
+	}
+
+	target->debug_reason = DBG_REASON_NOTHALTED;
+	if (!debug_execution)
+		target->state = TARGET_RUNNING;
+	else
+		target->state = TARGET_DEBUG_RUNNING;
+
+	target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
+
+	return ERROR_OK;
+}
+
+static bool xtensa_pc_in_winexc(struct target *target, target_addr_t pc)
+{
+	uint8_t insn_buf[XT_ISNS_SZ_MAX];
+	int err = xtensa_read_buffer(target, pc, sizeof(insn_buf), insn_buf);
+	if (err != ERROR_OK)
+		return false;
+
+	xtensa_insn_t insn = buf_get_u32(insn_buf, 0, 24);
+	xtensa_insn_t masked = insn & XT_INS_L32E_S32E_MASK;
+	if (masked == XT_INS_L32E(0, 0, 0) || masked == XT_INS_S32E(0, 0, 0))
+		return true;
+
+	masked = insn & XT_INS_RFWO_RFWU_MASK;
+	if (masked == XT_INS_RFWO || masked == XT_INS_RFWU)
+		return true;
+
+	return false;
+}
+
+int xtensa_do_step(struct target *target, int current, target_addr_t address, int handle_breakpoints)
+{
+	struct xtensa *xtensa = target_to_xtensa(target);
+	int res;
+	const uint32_t icount_val = -2;	/* ICOUNT value to load for 1 step */
+	xtensa_reg_val_t dbreakc[XT_WATCHPOINTS_NUM_MAX];
+	xtensa_reg_val_t icountlvl, cause;
+	xtensa_reg_val_t oldps, newps, oldpc, cur_pc;
+
+	LOG_TARGET_DEBUG(target, "current=%d, address=" TARGET_ADDR_FMT ", handle_breakpoints=%i",
+		current, address, handle_breakpoints);
+
+	if (target->state != TARGET_HALTED) {
+		LOG_TARGET_WARNING(target, "target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (xtensa->core_config->debug.icount_sz != 32) {
+		LOG_TARGET_WARNING(target, "stepping for ICOUNT less then 32 bits is not implemented!");
+		return ERROR_FAIL;
+	}
+
+	/* Save old ps/pc */
+	oldps = xtensa_reg_get(target, XT_REG_IDX_PS);
+	oldpc = xtensa_reg_get(target, XT_REG_IDX_PC);
+
+	cause = xtensa_reg_get(target, XT_REG_IDX_DEBUGCAUSE);
+	LOG_TARGET_DEBUG(target, "oldps=%" PRIx32 ", oldpc=%" PRIx32 " dbg_cause=%" PRIx32 " exc_cause=%" PRIx32,
+		oldps,
+		oldpc,
+		cause,
+		xtensa_reg_get(target, XT_REG_IDX_EXCCAUSE));
+	if (handle_breakpoints && (cause & (DEBUGCAUSE_BI | DEBUGCAUSE_BN))) {
+		/* handle hard-coded SW breakpoints (e.g. syscalls) */
+		LOG_TARGET_DEBUG(target, "Increment PC to pass break instruction...");
+		xtensa_reg_set(target, XT_REG_IDX_DEBUGCAUSE, 0);	/* so we don't recurse into the same routine */
+		xtensa->core_cache->reg_list[XT_REG_IDX_DEBUGCAUSE].dirty = false;
+		/* pretend that we have stepped */
+		if (cause & DEBUGCAUSE_BI)
+			xtensa_reg_set(target, XT_REG_IDX_PC, oldpc + 3);	/* PC = PC+3 */
+		else
+			xtensa_reg_set(target, XT_REG_IDX_PC, oldpc + 2);	/* PC = PC+2 */
+		return ERROR_OK;
+	}
+
+	/* Xtensa has an ICOUNTLEVEL register which sets the maximum interrupt level at which the
+	 * instructions are to be counted while stepping.
+	 * For example, if we need to step by 2 instructions, and an interrupt occurs inbetween,
+	 * the processor will execute the interrupt, return, and halt after the 2nd instruction.
+	 * However, sometimes we don't want the interrupt handlers to be executed at all, while
+	 * stepping through the code. In this case (XT_STEPPING_ISR_OFF), PS.INTLEVEL can be raised
+	 * to only allow Debug and NMI interrupts.
+	 */
+	if (xtensa->stepping_isr_mode == XT_STEPPING_ISR_OFF) {
+		if (!xtensa->core_config->high_irq.enabled) {
+			LOG_TARGET_WARNING(
+				target,
+				"disabling IRQs while stepping is not implemented w/o high prio IRQs option!");
+			return ERROR_FAIL;
+		}
+		/* Mask all interrupts below Debug, i.e. PS.INTLEVEL = DEBUGLEVEL - 1 */
+		xtensa_reg_val_t temp_ps = (oldps & ~0xF) | (xtensa->core_config->debug.irq_level - 1);
+		xtensa_reg_set(target, XT_REG_IDX_PS, temp_ps);
+	}
+	/* Regardless of ISRs masking mode we need to count instructions at any CINTLEVEL during step.
+	    So set `icountlvl` to DEBUGLEVEL.
+	    If ISRs are masked they are disabled in PS (see above), so having `icountlvl` set to DEBUGLEVEL
+	    will allow to step through any type of the code, e.g. 'high int level' ISR.
+	    If ISRs are not masked With `icountlvl` set to DEBUGLEVEL, we can step into any ISR
+	    which can happen (enabled in PS).
+	*/
+	icountlvl = xtensa->core_config->debug.irq_level;
+
+	if (cause & DEBUGCAUSE_DB) {
+		/* We stopped due to a watchpoint. We can't just resume executing the instruction again because
+		 * that would trigger the watchpoint again. To fix this, we remove watchpoints,single-step and
+		 * re-enable the watchpoint. */
+		LOG_TARGET_DEBUG(
+			target,
+			"Single-stepping to get past instruction that triggered the watchpoint...");
+		xtensa_reg_set(target, XT_REG_IDX_DEBUGCAUSE, 0);	/*so we don't recurse into
+									 * the same routine */
+		xtensa->core_cache->reg_list[XT_REG_IDX_DEBUGCAUSE].dirty = false;
+		/*Save all DBREAKCx registers and set to 0 to disable watchpoints */
+		for (unsigned int slot = 0; slot < xtensa->core_config->debug.dbreaks_num; slot++) {
+			dbreakc[slot] = xtensa_reg_get(target, XT_REG_IDX_DBREAKC0 + slot);
+			xtensa_reg_set(target, XT_REG_IDX_DBREAKC0 + slot, 0);
+		}
+	}
+
+	if (!handle_breakpoints && (cause & (DEBUGCAUSE_BI | DEBUGCAUSE_BN))) {
+		/* handle normal SW breakpoint */
+		xtensa_reg_set(target, XT_REG_IDX_DEBUGCAUSE, 0);	/*so we don't recurse into
+									 * the same routine */
+		xtensa->core_cache->reg_list[XT_REG_IDX_DEBUGCAUSE].dirty = false;
+	}
+	do {
+		xtensa_reg_set(target, XT_REG_IDX_ICOUNTLEVEL, icountlvl);
+		xtensa_reg_set(target, XT_REG_IDX_ICOUNT, icount_val);
+
+		/* Now ICOUNT 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");
+			return res;
+		}
+		res = xtensa_do_resume(target);
+		if (res != ERROR_OK) {
+			LOG_TARGET_ERROR(target, "Failed to resume after setting up single step");
+			return res;
+		}
+
+		/* Wait for stepping to complete */
+		long long start = timeval_ms();
+		while (timeval_ms() < start + 500) {
+			/* Do not use target_poll here, it also triggers other things... just manually read the DSR
+			 *until stepping is complete. */
+			usleep(1000);
+			res = xtensa_dm_core_status_read(&xtensa->dbg_mod);
+			if (res != ERROR_OK) {
+				LOG_TARGET_ERROR(target, "Failed to read core status!");
+				return res;
+			}
+			if (xtensa_is_stopped(target))
+				break;
+			usleep(1000);
+		}
+		LOG_TARGET_DEBUG(target, "Finish stepping. dsr=0x%08" PRIx32,
+			xtensa_dm_core_status_get(&xtensa->dbg_mod));
+		if (!xtensa_is_stopped(target)) {
+			LOG_TARGET_WARNING(
+				target,
+				"Timed out waiting for target to finish stepping. dsr=0x%08" PRIx32,
+				xtensa_dm_core_status_get(&xtensa->dbg_mod));
+			target->debug_reason = DBG_REASON_NOTHALTED;
+			target->state = TARGET_RUNNING;
+			return ERROR_FAIL;
+		}
+		target->debug_reason = DBG_REASON_SINGLESTEP;
+		target->state = TARGET_HALTED;
+
+		xtensa_fetch_all_regs(target);
+
+		cur_pc = xtensa_reg_get(target, XT_REG_IDX_PC);
+
+		LOG_TARGET_DEBUG(target,
+			"cur_ps=%" PRIx32 ", cur_pc=%" PRIx32 " dbg_cause=%" PRIx32 " exc_cause=%" PRIx32,
+			xtensa_reg_get(target, XT_REG_IDX_PS),
+			cur_pc,
+			xtensa_reg_get(target, XT_REG_IDX_DEBUGCAUSE),
+			xtensa_reg_get(target, XT_REG_IDX_EXCCAUSE));
+
+		/* Do not step into WindowOverflow if ISRs are masked.
+		   If we stop in WindowOverflow at breakpoint with masked ISRs and
+		   try to do a step it will get us out of that handler */
+		if (xtensa->core_config->windowed &&
+			xtensa->stepping_isr_mode == XT_STEPPING_ISR_OFF &&
+			xtensa_pc_in_winexc(target, cur_pc)) {
+			/* isrmask = on, need to step out of the window exception handler */
+			LOG_DEBUG("Stepping out of window exception, PC=%" PRIX32, cur_pc);
+			oldpc = cur_pc;
+			address = oldpc + 3;
+			continue;
+		}
+
+		if (oldpc == cur_pc)
+			LOG_TARGET_WARNING(target, "Stepping doesn't seem to change PC! dsr=0x%08" PRIx32,
+				xtensa_dm_core_status_get(&xtensa->dbg_mod));
+		else
+			LOG_DEBUG("Stepped from %" PRIX32 " to %" PRIX32, oldpc, cur_pc);
+		break;
+	} while (true);
+	LOG_DEBUG("Done stepping, PC=%" PRIX32, cur_pc);
+
+	if (cause & DEBUGCAUSE_DB) {
+		LOG_TARGET_DEBUG(target, "...Done, re-installing watchpoints.");
+		/* Restore the DBREAKCx registers */
+		for (unsigned int slot = 0; slot < xtensa->core_config->debug.dbreaks_num; slot++)
+			xtensa_reg_set(target, XT_REG_IDX_DBREAKC0 + slot, dbreakc[slot]);
+	}
+
+	/* Restore int level */
+	/* TODO: Theoretically, this can mess up stepping over an instruction that modifies
+	 * ps.intlevel by itself. TODO: Look into this. */
+	if (xtensa->stepping_isr_mode == XT_STEPPING_ISR_OFF) {
+		newps = xtensa_reg_get(target, XT_REG_IDX_PS);
+		newps = (newps & ~0xF) | (oldps & 0xf);
+		xtensa_reg_set(target, XT_REG_IDX_PS, newps);
+	}
+
+	/* write ICOUNTLEVEL back to zero */
+	xtensa_reg_set(target, XT_REG_IDX_ICOUNTLEVEL, 0);
+	/* TODO: can we skip writing dirty registers and re-fetching them? */
+	res = xtensa_write_dirty_registers(target);
+	xtensa_fetch_all_regs(target);
+	return res;
+}
+
+int xtensa_step(struct target *target, int current, target_addr_t address, int handle_breakpoints)
+{
+	return xtensa_do_step(target, current, address, handle_breakpoints);
+}
+
+/**
+ * Returns true if two ranges are overlapping
+ */
+static inline bool xtensa_memory_regions_overlap(target_addr_t r1_start,
+	target_addr_t r1_end,
+	target_addr_t r2_start,
+	target_addr_t r2_end)
+{
+	if ((r2_start >= r1_start) && (r2_start < r1_end))
+		return true;	/* r2_start is in r1 region */
+	if ((r2_end > r1_start) && (r2_end <= r1_end))
+		return true;	/* r2_end is in r1 region */
+	return false;
+}
+
+/**
+ * Returns a size of overlapped region of two ranges.
+ */
+static inline target_addr_t xtensa_get_overlap_size(target_addr_t r1_start,
+	target_addr_t r1_end,
+	target_addr_t r2_start,
+	target_addr_t r2_end)
+{
+	if (xtensa_memory_regions_overlap(r1_start, r1_end, r2_start, r2_end)) {
+		target_addr_t ov_start = r1_start < r2_start ? r2_start : r1_start;
+		target_addr_t ov_end = r1_end > r2_end ? r2_end : r1_end;
+		return ov_end - ov_start;
+	}
+	return 0;
+}
+
+/**
+ * Check if the address gets to memory regions, and it's access mode
+ */
+static bool xtensa_memory_op_validate_range(struct xtensa *xtensa, target_addr_t address, size_t size, int access)
+{
+	target_addr_t adr_pos = address;	/* address cursor set to the beginning start */
+	target_addr_t adr_end = address + size;	/* region end */
+	target_addr_t overlap_size;
+	const struct xtensa_local_mem_region_config *cm;	/* current mem region */
+
+	while (adr_pos < adr_end) {
+		cm = xtensa_target_memory_region_find(xtensa, adr_pos);
+		if (!cm)	/* address is not belong to anything */
+			return false;
+		if ((cm->access & access) != access)	/* access check */
+			return false;
+		overlap_size = xtensa_get_overlap_size(cm->base, (cm->base + cm->size), adr_pos, adr_end);
+		assert(overlap_size != 0);
+		adr_pos += overlap_size;
+	}
+	return true;
+}
+
+int xtensa_read_memory(struct target *target, target_addr_t address, uint32_t size, uint32_t count, uint8_t *buffer)
+{
+	struct xtensa *xtensa = target_to_xtensa(target);
+	/* We are going to read memory in 32-bit increments. This may not be what the calling
+	 * function expects, so we may need to allocate a temp buffer and read into that first. */
+	target_addr_t addrstart_al = ALIGN_DOWN(address, 4);
+	target_addr_t addrend_al = ALIGN_UP(address + size * count, 4);
+	target_addr_t adr = addrstart_al;
+	uint8_t *albuff;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_TARGET_WARNING(target, "target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (!xtensa->permissive_mode) {
+		if (!xtensa_memory_op_validate_range(xtensa, address, (size * count),
+				XT_MEM_ACCESS_READ)) {
+			LOG_DEBUG("address " TARGET_ADDR_FMT " not readable", address);
+			return ERROR_FAIL;
+		}
+	}
+
+	if (addrstart_al == address && addrend_al == address + (size * count)) {
+		albuff = buffer;
+	} else {
+		albuff = malloc(addrend_al - addrstart_al);
+		if (!albuff) {
+			LOG_TARGET_ERROR(target, "Out of memory allocating %" TARGET_PRIdADDR " bytes!",
+				addrend_al - addrstart_al);
+			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+		}
+	}
+
+	/* We're going to use A3 here */
+	xtensa_mark_register_dirty(xtensa, XT_REG_IDX_A3);
+	/* Write start address to A3 */
+	xtensa_queue_dbg_reg_write(xtensa, NARADR_DDR, addrstart_al);
+	xtensa_queue_exec_ins(xtensa, XT_INS_RSR(XT_SR_DDR, XT_REG_A3));
+	/* Now we can safely read data from addrstart_al up to addrend_al into albuff */
+	for (unsigned int i = 0; adr != addrend_al; i += sizeof(uint32_t), adr += sizeof(uint32_t)) {
+		xtensa_queue_exec_ins(xtensa, XT_INS_LDDR32P(XT_REG_A3));
+		xtensa_queue_dbg_reg_read(xtensa, NARADR_DDR, &albuff[i]);
+	}
+	int res = jtag_execute_queue();
+	if (res == ERROR_OK)
+		res = xtensa_core_status_check(target);
+	if (res != ERROR_OK)
+		LOG_TARGET_WARNING(target, "Failed reading %d bytes at address " TARGET_ADDR_FMT,
+			count * size, address);
+
+	if (albuff != buffer) {
+		memcpy(buffer, albuff + (address & 3), (size * count));
+		free(albuff);
+	}
+
+	return res;
+}
+
+int xtensa_read_buffer(struct target *target, target_addr_t address, uint32_t count, uint8_t *buffer)
+{
+	/* xtensa_read_memory can also read unaligned stuff. Just pass through to that routine. */
+	return xtensa_read_memory(target, address, 1, count, buffer);
+}
+
+int xtensa_write_memory(struct target *target,
+	target_addr_t address,
+	uint32_t size,
+	uint32_t count,
+	const uint8_t *buffer)
+{
+	/* This memory write function can get thrown nigh everything into it, from
+	 * aligned uint32 writes to unaligned uint8ths. The Xtensa memory doesn't always
+	 * accept anything but aligned uint32 writes, though. That is why we convert
+	 * everything into that. */
+	struct xtensa *xtensa = target_to_xtensa(target);
+	target_addr_t addrstart_al = ALIGN_DOWN(address, 4);
+	target_addr_t addrend_al = ALIGN_UP(address + size * count, 4);
+	target_addr_t adr = addrstart_al;
+	int res;
+	uint8_t *albuff;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_TARGET_WARNING(target, "target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (!xtensa->permissive_mode) {
+		if (!xtensa_memory_op_validate_range(xtensa, address, (size * count), XT_MEM_ACCESS_WRITE)) {
+			LOG_WARNING("address " TARGET_ADDR_FMT " not writable", address);
+			return ERROR_FAIL;
+		}
+	}
+
+	if (size == 0 || count == 0 || !buffer)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	/* Allocate a temporary buffer to put the aligned bytes in, if needed. */
+	if (addrstart_al == address && addrend_al == address + (size * count)) {
+		/* We discard the const here because albuff can also be non-const */
+		albuff = (uint8_t *)buffer;
+	} else {
+		albuff = malloc(addrend_al - addrstart_al);
+		if (!albuff) {
+			LOG_TARGET_ERROR(target, "Out of memory allocating %" TARGET_PRIdADDR " bytes!",
+				addrend_al - addrstart_al);
+			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+		}
+	}
+
+	/* We're going to use A3 here */
+	xtensa_mark_register_dirty(xtensa, XT_REG_IDX_A3);
+
+	/* If we're using a temp aligned buffer, we need to fill the head and/or tail bit of it. */
+	if (albuff != buffer) {
+		/* See if we need to read the first and/or last word. */
+		if (address & 3) {
+			xtensa_queue_dbg_reg_write(xtensa, NARADR_DDR, addrstart_al);
+			xtensa_queue_exec_ins(xtensa, XT_INS_RSR(XT_SR_DDR, XT_REG_A3));
+			xtensa_queue_exec_ins(xtensa, XT_INS_LDDR32P(XT_REG_A3));
+			xtensa_queue_dbg_reg_read(xtensa, NARADR_DDR, &albuff[0]);
+		}
+		if ((address + (size * count)) & 3) {
+			xtensa_queue_dbg_reg_write(xtensa, NARADR_DDR, addrend_al - 4);
+			xtensa_queue_exec_ins(xtensa, XT_INS_RSR(XT_SR_DDR, XT_REG_A3));
+			xtensa_queue_exec_ins(xtensa, XT_INS_LDDR32P(XT_REG_A3));
+			xtensa_queue_dbg_reg_read(xtensa, NARADR_DDR,
+				&albuff[addrend_al - addrstart_al - 4]);
+		}
+		/* Grab bytes */
+		res = jtag_execute_queue();
+		if (res != ERROR_OK) {
+			LOG_ERROR("Error issuing unaligned memory write context instruction(s): %d", res);
+			if (albuff != buffer)
+				free(albuff);
+			return res;
+		}
+		xtensa_core_status_check(target);
+		/* Copy data to be written into the aligned buffer */
+		memcpy(&albuff[address & 3], buffer, size * count);
+		/* Now we can write albuff in aligned uint32s. */
+	}
+
+	/* Write start address to A3 */
+	xtensa_queue_dbg_reg_write(xtensa, NARADR_DDR, addrstart_al);
+	xtensa_queue_exec_ins(xtensa, XT_INS_RSR(XT_SR_DDR, XT_REG_A3));
+	/* Write the aligned buffer */
+	for (unsigned int i = 0; adr != addrend_al; i += sizeof(uint32_t), adr += sizeof(uint32_t)) {
+		xtensa_queue_dbg_reg_write(xtensa, NARADR_DDR, buf_get_u32(&albuff[i], 0, 32));
+		xtensa_queue_exec_ins(xtensa, XT_INS_SDDR32P(XT_REG_A3));
+	}
+	res = jtag_execute_queue();
+	if (res == ERROR_OK)
+		res = xtensa_core_status_check(target);
+	if (res != ERROR_OK)
+		LOG_TARGET_WARNING(target, "Failed writing %d bytes at address " TARGET_ADDR_FMT, count * size, address);
+	if (albuff != buffer)
+		free(albuff);
+
+	return res;
+}
+
+int xtensa_write_buffer(struct target *target, target_addr_t address, uint32_t count, const uint8_t *buffer)
+{
+	/* xtensa_write_memory can handle everything. Just pass on to that. */
+	return xtensa_write_memory(target, address, 1, count, buffer);
+}
+
+int xtensa_checksum_memory(struct target *target, target_addr_t address, uint32_t count, uint32_t *checksum)
+{
+	LOG_WARNING("not implemented yet");
+	return ERROR_FAIL;
+}
+
+int xtensa_poll(struct target *target)
+{
+	struct xtensa *xtensa = target_to_xtensa(target);
+
+	int res = xtensa_dm_power_status_read(&xtensa->dbg_mod, PWRSTAT_DEBUGWASRESET | PWRSTAT_COREWASRESET);
+	if (res != ERROR_OK)
+		return res;
+
+	if (xtensa_dm_tap_was_reset(&xtensa->dbg_mod)) {
+		LOG_TARGET_INFO(target, "Debug controller was reset.");
+		res = xtensa_smpbreak_write(xtensa, xtensa->smp_break);
+		if (res != ERROR_OK)
+			return res;
+	}
+	if (xtensa_dm_core_was_reset(&xtensa->dbg_mod))
+		LOG_TARGET_INFO(target, "Core was reset.");
+	xtensa_dm_power_status_cache(&xtensa->dbg_mod);
+	/* Enable JTAG, set reset if needed */
+	res = xtensa_wakeup(target);
+	if (res != ERROR_OK)
+		return res;
+
+	res = xtensa_dm_core_status_read(&xtensa->dbg_mod);
+	if (res != ERROR_OK)
+		return res;
+	if (xtensa->dbg_mod.power_status.stath & PWRSTAT_COREWASRESET) {
+		/* if RESET state is persitent  */
+		target->state = TARGET_RESET;
+	} else if (!xtensa_dm_is_powered(&xtensa->dbg_mod)) {
+		LOG_TARGET_DEBUG(target, "not powered 0x%" PRIX32 "%ld",
+			xtensa->dbg_mod.core_status.dsr,
+			xtensa->dbg_mod.core_status.dsr & OCDDSR_STOPPED);
+		target->state = TARGET_UNKNOWN;
+		if (xtensa->come_online_probes_num == 0)
+			target->examined = false;
+		else
+			xtensa->come_online_probes_num--;
+	} else if (xtensa_is_stopped(target)) {
+		if (target->state != TARGET_HALTED) {
+			enum target_state oldstate = target->state;
+			target->state = TARGET_HALTED;
+			/* Examine why the target has been halted */
+			target->debug_reason = DBG_REASON_DBGRQ;
+			xtensa_fetch_all_regs(target);
+			/* When setting debug reason DEBUGCAUSE events have the following
+			 * priorities: watchpoint == breakpoint > single step > debug interrupt. */
+			/* Watchpoint and breakpoint events at the same time results in special
+			 * debug reason: DBG_REASON_WPTANDBKPT. */
+			xtensa_reg_val_t halt_cause = xtensa_reg_get(target, XT_REG_IDX_DEBUGCAUSE);
+			/* TODO: Add handling of DBG_REASON_EXC_CATCH */
+			if (halt_cause & DEBUGCAUSE_IC)
+				target->debug_reason = DBG_REASON_SINGLESTEP;
+			if (halt_cause & (DEBUGCAUSE_IB | DEBUGCAUSE_BN | DEBUGCAUSE_BI)) {
+				if (halt_cause & DEBUGCAUSE_DB)
+					target->debug_reason = DBG_REASON_WPTANDBKPT;
+				else
+					target->debug_reason = DBG_REASON_BREAKPOINT;
+			} else if (halt_cause & DEBUGCAUSE_DB) {
+				target->debug_reason = DBG_REASON_WATCHPOINT;
+			}
+			LOG_TARGET_DEBUG(target, "Target halted, pc=0x%08" PRIX32 ", debug_reason=%08x, oldstate=%08x",
+				xtensa_reg_get(target, XT_REG_IDX_PC),
+				target->debug_reason,
+				oldstate);
+			LOG_TARGET_DEBUG(target, "Halt reason=0x%08" PRIX32 ", exc_cause=%" PRId32 ", dsr=0x%08" PRIx32,
+				halt_cause,
+				xtensa_reg_get(target, XT_REG_IDX_EXCCAUSE),
+				xtensa->dbg_mod.core_status.dsr);
+			LOG_TARGET_INFO(target, "Target halted, PC=0x%08" PRIX32 ", debug_reason=%08x",
+				xtensa_reg_get(target, XT_REG_IDX_PC), target->debug_reason);
+			xtensa_dm_core_status_clear(
+				&xtensa->dbg_mod,
+				OCDDSR_DEBUGPENDBREAK | OCDDSR_DEBUGINTBREAK | OCDDSR_DEBUGPENDTRAX |
+				OCDDSR_DEBUGINTTRAX |
+				OCDDSR_DEBUGPENDHOST | OCDDSR_DEBUGINTHOST);
+		}
+	} else {
+		target->debug_reason = DBG_REASON_NOTHALTED;
+		if (target->state != TARGET_RUNNING && target->state != TARGET_DEBUG_RUNNING) {
+			target->state = TARGET_RUNNING;
+			target->debug_reason = DBG_REASON_NOTHALTED;
+		}
+	}
+	if (xtensa->trace_active) {
+		/* Detect if tracing was active but has stopped. */
+		struct xtensa_trace_status trace_status;
+		res = xtensa_dm_trace_status_read(&xtensa->dbg_mod, &trace_status);
+		if (res == ERROR_OK) {
+			if (!(trace_status.stat & TRAXSTAT_TRACT)) {
+				LOG_INFO("Detected end of trace.");
+				if (trace_status.stat & TRAXSTAT_PCMTG)
+					LOG_TARGET_INFO(target, "Trace stop triggered by PC match");
+				if (trace_status.stat & TRAXSTAT_PTITG)
+					LOG_TARGET_INFO(target, "Trace stop triggered by Processor Trigger Input");
+				if (trace_status.stat & TRAXSTAT_CTITG)
+					LOG_TARGET_INFO(target, "Trace stop triggered by Cross-trigger Input");
+				xtensa->trace_active = false;
+			}
+		}
+	}
+	return ERROR_OK;
+}
+
+static int xtensa_sw_breakpoint_add(struct target *target,
+	struct breakpoint *breakpoint,
+	struct xtensa_sw_breakpoint *sw_bp)
+{
+	int ret = target_read_buffer(target, breakpoint->address, XT_ISNS_SZ_MAX, sw_bp->insn);
+	if (ret != ERROR_OK) {
+		LOG_TARGET_ERROR(target, "Failed to read original instruction (%d)!", ret);
+		return ret;
+	}
+
+	sw_bp->insn_sz = xtensa_insn_size_get(buf_get_u32(sw_bp->insn, 0, 24));
+	sw_bp->oocd_bp = breakpoint;
+
+	uint32_t break_insn = sw_bp->insn_sz == XT_ISNS_SZ_MAX ? XT_INS_BREAK(0, 0) : XT_INS_BREAKN(0);
+	/* convert to target endianness */
+	uint8_t break_insn_buff[4];
+	target_buffer_set_u32(target, break_insn_buff, break_insn);
+
+	ret = target_write_buffer(target, breakpoint->address, sw_bp->insn_sz, break_insn_buff);
+	if (ret != ERROR_OK) {
+		LOG_TARGET_ERROR(target, "Failed to write breakpoint instruction (%d)!", ret);
+		return ret;
+	}
+
+	return ERROR_OK;
+}
+
+static int xtensa_sw_breakpoint_remove(struct target *target, struct xtensa_sw_breakpoint *sw_bp)
+{
+	int ret = target_write_buffer(target, sw_bp->oocd_bp->address, sw_bp->insn_sz, sw_bp->insn);
+	if (ret != ERROR_OK) {
+		LOG_TARGET_ERROR(target, "Failed to read insn (%d)!", ret);
+		return ret;
+	}
+	sw_bp->oocd_bp = NULL;
+	return ERROR_OK;
+}
+
+int xtensa_breakpoint_add(struct target *target, struct breakpoint *breakpoint)
+{
+	struct xtensa *xtensa = target_to_xtensa(target);
+	unsigned int slot;
+
+	if (breakpoint->type == BKPT_SOFT) {
+		for (slot = 0; slot < XT_SW_BREAKPOINTS_MAX_NUM; slot++) {
+			if (!xtensa->sw_brps[slot].oocd_bp ||
+				xtensa->sw_brps[slot].oocd_bp == breakpoint)
+				break;
+		}
+		if (slot == XT_SW_BREAKPOINTS_MAX_NUM) {
+			LOG_TARGET_WARNING(target, "No free slots to add SW breakpoint!");
+			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+		}
+		int ret = xtensa_sw_breakpoint_add(target, breakpoint, &xtensa->sw_brps[slot]);
+		if (ret != ERROR_OK) {
+			LOG_TARGET_ERROR(target, "Failed to add SW breakpoint!");
+			return ret;
+		}
+		LOG_TARGET_DEBUG(target, "placed SW breakpoint %u @ " TARGET_ADDR_FMT,
+			slot,
+			breakpoint->address);
+		return ERROR_OK;
+	}
+
+	for (slot = 0; slot < xtensa->core_config->debug.ibreaks_num; slot++) {
+		if (!xtensa->hw_brps[slot] || xtensa->hw_brps[slot] == breakpoint)
+			break;
+	}
+	if (slot == xtensa->core_config->debug.ibreaks_num) {
+		LOG_TARGET_ERROR(target, "No free slots to add HW breakpoint!");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+
+	xtensa->hw_brps[slot] = breakpoint;
+	/* We will actually write the breakpoints when we resume the target. */
+	LOG_TARGET_DEBUG(target, "placed HW breakpoint @ " TARGET_ADDR_FMT,
+		breakpoint->address);
+
+	return ERROR_OK;
+}
+
+int xtensa_breakpoint_remove(struct target *target, struct breakpoint *breakpoint)
+{
+	struct xtensa *xtensa = target_to_xtensa(target);
+	unsigned int slot;
+
+	if (breakpoint->type == BKPT_SOFT) {
+		for (slot = 0; slot < XT_SW_BREAKPOINTS_MAX_NUM; slot++) {
+			if (xtensa->sw_brps[slot].oocd_bp && xtensa->sw_brps[slot].oocd_bp == breakpoint)
+				break;
+		}
+		if (slot == XT_SW_BREAKPOINTS_MAX_NUM) {
+			LOG_TARGET_WARNING(target, "Max SW breakpoints slot reached, slot=%u!", slot);
+			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+		}
+		int ret = xtensa_sw_breakpoint_remove(target, &xtensa->sw_brps[slot]);
+		if (ret != ERROR_OK) {
+			LOG_TARGET_ERROR(target, "Failed to remove SW breakpoint (%d)!", ret);
+			return ret;
+		}
+		LOG_TARGET_DEBUG(target, "cleared SW breakpoint %u @ " TARGET_ADDR_FMT, slot, breakpoint->address);
+		return ERROR_OK;
+	}
+
+	for (slot = 0; slot < xtensa->core_config->debug.ibreaks_num; slot++) {
+		if (xtensa->hw_brps[slot] == breakpoint)
+			break;
+	}
+	if (slot == xtensa->core_config->debug.ibreaks_num) {
+		LOG_TARGET_ERROR(target, "HW breakpoint not found!");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+	xtensa->hw_brps[slot] = NULL;
+	LOG_TARGET_DEBUG(target, "cleared HW breakpoint %u @ " TARGET_ADDR_FMT, slot, breakpoint->address);
+	return ERROR_OK;
+}
+
+int xtensa_watchpoint_add(struct target *target, struct watchpoint *watchpoint)
+{
+	struct xtensa *xtensa = target_to_xtensa(target);
+	unsigned int slot;
+	xtensa_reg_val_t dbreakcval;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_TARGET_WARNING(target, "target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (watchpoint->mask != ~(uint32_t)0) {
+		LOG_TARGET_ERROR(target, "watchpoint value masks not supported");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+
+	for (slot = 0; slot < xtensa->core_config->debug.dbreaks_num; slot++) {
+		if (!xtensa->hw_wps[slot] || xtensa->hw_wps[slot] == watchpoint)
+			break;
+	}
+	if (slot == xtensa->core_config->debug.dbreaks_num) {
+		LOG_TARGET_WARNING(target, "No free slots to add HW watchpoint!");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+
+	/* Figure out value for dbreakc5..0
+	 * It's basically 0x3F with an incremental bit removed from the LSB for each extra length power of 2. */
+	if (watchpoint->length < 1 || watchpoint->length > 64 ||
+		!IS_PWR_OF_2(watchpoint->length) ||
+		!IS_ALIGNED(watchpoint->address, watchpoint->length)) {
+		LOG_TARGET_WARNING(
+			target,
+			"Watchpoint with length %d on address " TARGET_ADDR_FMT
+			" not supported by hardware.",
+			watchpoint->length,
+			watchpoint->address);
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+	dbreakcval = ALIGN_DOWN(0x3F, watchpoint->length);
+
+	if (watchpoint->rw == WPT_READ)
+		dbreakcval |= BIT(30);
+	if (watchpoint->rw == WPT_WRITE)
+		dbreakcval |= BIT(31);
+	if (watchpoint->rw == WPT_ACCESS)
+		dbreakcval |= BIT(30) | BIT(31);
+
+	/* Write DBREAKA[slot] and DBCREAKC[slot] */
+	xtensa_reg_set(target, XT_REG_IDX_DBREAKA0 + slot, watchpoint->address);
+	xtensa_reg_set(target, XT_REG_IDX_DBREAKC0 + slot, dbreakcval);
+	xtensa->hw_wps[slot] = watchpoint;
+	LOG_TARGET_DEBUG(target, "placed HW watchpoint @ " TARGET_ADDR_FMT,
+		watchpoint->address);
+	return ERROR_OK;
+}
+
+int xtensa_watchpoint_remove(struct target *target, struct watchpoint *watchpoint)
+{
+	struct xtensa *xtensa = target_to_xtensa(target);
+	unsigned int slot;
+
+	for (slot = 0; slot < xtensa->core_config->debug.dbreaks_num; slot++) {
+		if (xtensa->hw_wps[slot] == watchpoint)
+			break;
+	}
+	if (slot == xtensa->core_config->debug.dbreaks_num) {
+		LOG_TARGET_WARNING(target, "HW watchpoint " TARGET_ADDR_FMT " not found!", watchpoint->address);
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+	xtensa_reg_set(target, XT_REG_IDX_DBREAKC0 + slot, 0);
+	xtensa->hw_wps[slot] = NULL;
+	LOG_TARGET_DEBUG(target, "cleared HW watchpoint @ " TARGET_ADDR_FMT,
+		watchpoint->address);
+	return ERROR_OK;
+}
+
+static int xtensa_build_reg_cache(struct target *target)
+{
+	struct xtensa *xtensa = target_to_xtensa(target);
+	struct reg_cache **cache_p = register_get_last_cache_p(&target->reg_cache);
+	struct reg_cache *reg_cache = calloc(1, sizeof(struct reg_cache));
+
+	if (!reg_cache) {
+		LOG_ERROR("Failed to alloc reg cache!");
+		return ERROR_FAIL;
+	}
+	reg_cache->name = "Xtensa registers";
+	reg_cache->next = NULL;
+	reg_cache->num_regs = XT_NUM_REGS + xtensa->core_config->user_regs_num;
+	/* Init reglist */
+	struct reg *reg_list = calloc(reg_cache->num_regs, sizeof(struct reg));
+	if (!reg_list) {
+		LOG_ERROR("Failed to alloc reg list!");
+		goto fail;
+	}
+	xtensa->regs_num = 0;
+
+	for (unsigned int i = 0; i < XT_NUM_REGS; i++) {
+		reg_list[i].exist = false;
+		if (xtensa_regs[i].type == XT_REG_USER) {
+			if (xtensa_user_reg_exists(xtensa, i))
+				reg_list[i].exist = true;
+			else
+				LOG_DEBUG("User reg '%s' (%d) does not exist", xtensa_regs[i].name, i);
+		} else if (xtensa_regs[i].type == XT_REG_FR) {
+			if (xtensa_fp_reg_exists(xtensa, i))
+				reg_list[i].exist = true;
+			else
+				LOG_DEBUG("FP reg '%s' (%d) does not exist", xtensa_regs[i].name, i);
+		} else if (xtensa_regs[i].type == XT_REG_SPECIAL) {
+			if (xtensa_special_reg_exists(xtensa, i))
+				reg_list[i].exist = true;
+			else
+				LOG_DEBUG("Special reg '%s' (%d) does not exist", xtensa_regs[i].name, i);
+		} else {
+			if (xtensa_regular_reg_exists(xtensa, i))
+				reg_list[i].exist = true;
+			else
+				LOG_DEBUG("Regular reg '%s' (%d) does not exist", xtensa_regs[i].name, i);
+		}
+		reg_list[i].name = xtensa_regs[i].name;
+		reg_list[i].size = 32;
+		reg_list[i].value = calloc(1, 4	/*XT_REG_LEN*/);/* make Clang Static Analyzer happy */
+		if (!reg_list[i].value) {
+			LOG_ERROR("Failed to alloc reg list value!");
+			goto fail;
+		}
+		reg_list[i].dirty = false;
+		reg_list[i].valid = false;
+		reg_list[i].type = &xtensa_reg_type;
+		reg_list[i].arch_info = xtensa;
+		if (reg_list[i].exist)
+			xtensa->regs_num++;
+	}
+	for (unsigned int i = 0; i < xtensa->core_config->user_regs_num; i++) {
+		reg_list[XT_USR_REG_START + i].exist = true;
+		reg_list[XT_USR_REG_START + i].name = xtensa->core_config->user_regs[i].name;
+		reg_list[XT_USR_REG_START + i].size = xtensa->core_config->user_regs[i].size;
+		reg_list[XT_USR_REG_START + i].value = calloc(1, reg_list[XT_USR_REG_START + i].size / 8);
+		if (!reg_list[XT_USR_REG_START + i].value) {
+			LOG_ERROR("Failed to alloc user reg list value!");
+			goto fail;
+		}
+		reg_list[XT_USR_REG_START + i].dirty = false;
+		reg_list[XT_USR_REG_START + i].valid = false;
+		reg_list[XT_USR_REG_START + i].type = xtensa->core_config->user_regs[i].type;
+		reg_list[XT_USR_REG_START + i].arch_info = xtensa;
+		xtensa->regs_num++;
+	}
+	if (xtensa->core_config->gdb_general_regs_num >= xtensa->regs_num) {
+		LOG_ERROR("Regs number less then GDB general regs number!");
+		goto fail;
+	}
+
+	/* assign GDB reg numbers to registers */
+	for (unsigned int gdb_reg_id = 0; gdb_reg_id < xtensa->regs_num; gdb_reg_id++) {
+		unsigned int reg_id = xtensa->core_config->gdb_regs_mapping[gdb_reg_id];
+		if (reg_id >= reg_cache->num_regs) {
+			LOG_ERROR("Invalid GDB map!");
+			goto fail;
+		}
+		if (!reg_list[reg_id].exist) {
+			LOG_ERROR("Non-existing reg in GDB map!");
+			goto fail;
+		}
+		reg_list[reg_id].number = gdb_reg_id;
+	}
+	reg_cache->reg_list = reg_list;
+
+	xtensa->algo_context_backup = calloc(reg_cache->num_regs, sizeof(void *));
+	if (!xtensa->algo_context_backup) {
+		LOG_ERROR("Failed to alloc mem for algorithm context backup!");
+		goto fail;
+	}
+	for (unsigned int i = 0; i < reg_cache->num_regs; i++) {
+		struct reg *reg = &reg_cache->reg_list[i];
+		xtensa->algo_context_backup[i] = calloc(1, reg->size / 8);
+		if (!xtensa->algo_context_backup[i]) {
+			LOG_ERROR("Failed to alloc mem for algorithm context!");
+			goto fail;
+		}
+	}
+
+	xtensa->core_cache = reg_cache;
+	if (cache_p)
+		*cache_p = reg_cache;
+	return ERROR_OK;
+
+fail:
+	if (reg_list) {
+		for (unsigned int i = 0; i < reg_cache->num_regs; i++)
+			free(reg_list[i].value);
+		free(reg_list);
+	}
+	if (xtensa->algo_context_backup) {
+		for (unsigned int i = 0; i < reg_cache->num_regs; i++)
+			free(xtensa->algo_context_backup[i]);
+		free(xtensa->algo_context_backup);
+	}
+	free(reg_cache);
+
+	return ERROR_FAIL;
+}
+
+int xtensa_init_arch_info(struct target *target, struct xtensa *xtensa,
+	const struct xtensa_config *xtensa_config,
+	const struct xtensa_debug_module_config *dm_cfg)
+{
+	target->arch_info = xtensa;
+	xtensa->common_magic = XTENSA_COMMON_MAGIC;
+	xtensa->target = target;
+	xtensa->core_config = xtensa_config;
+	xtensa->stepping_isr_mode = XT_STEPPING_ISR_ON;
+
+	if (!xtensa->core_config->exc.enabled || !xtensa->core_config->irq.enabled ||
+		!xtensa->core_config->high_irq.enabled || !xtensa->core_config->debug.enabled) {
+		LOG_ERROR("Xtensa configuration does not support debugging!");
+		return ERROR_FAIL;
+	}
+	return xtensa_dm_init(&xtensa->dbg_mod, dm_cfg);
+}
+
+void xtensa_set_permissive_mode(struct target *target, bool state)
+{
+	target_to_xtensa(target)->permissive_mode = state;
+}
+
+int xtensa_target_init(struct command_context *cmd_ctx, struct target *target)
+{
+	struct xtensa *xtensa = target_to_xtensa(target);
+
+	xtensa->come_online_probes_num = 3;
+	xtensa->hw_brps = calloc(xtensa->core_config->debug.ibreaks_num, sizeof(struct breakpoint *));
+	if (!xtensa->hw_brps) {
+		LOG_ERROR("Failed to alloc memory for HW breakpoints!");
+		return ERROR_FAIL;
+	}
+	xtensa->hw_wps = calloc(xtensa->core_config->debug.dbreaks_num, sizeof(struct watchpoint *));
+	if (!xtensa->hw_wps) {
+		free(xtensa->hw_brps);
+		LOG_ERROR("Failed to alloc memory for HW watchpoints!");
+		return ERROR_FAIL;
+	}
+	xtensa->sw_brps = calloc(XT_SW_BREAKPOINTS_MAX_NUM, sizeof(struct xtensa_sw_breakpoint));
+	if (!xtensa->sw_brps) {
+		free(xtensa->hw_brps);
+		free(xtensa->hw_wps);
+		LOG_ERROR("Failed to alloc memory for SW breakpoints!");
+		return ERROR_FAIL;
+	}
+
+	return xtensa_build_reg_cache(target);
+}
+
+static void xtensa_free_reg_cache(struct target *target)
+{
+	struct xtensa *xtensa = target_to_xtensa(target);
+	struct reg_cache *cache = xtensa->core_cache;
+
+	if (cache) {
+		register_unlink_cache(&target->reg_cache, cache);
+		for (unsigned int i = 0; i < cache->num_regs; i++) {
+			free(xtensa->algo_context_backup[i]);
+			free(cache->reg_list[i].value);
+		}
+		free(xtensa->algo_context_backup);
+		free(cache->reg_list);
+		free(cache);
+	}
+	xtensa->core_cache = NULL;
+	xtensa->algo_context_backup = NULL;
+}
+
+void xtensa_target_deinit(struct target *target)
+{
+	struct xtensa *xtensa = target_to_xtensa(target);
+
+	LOG_DEBUG("start");
+
+	if (target_was_examined(target)) {
+		int ret = xtensa_queue_dbg_reg_write(xtensa, NARADR_DCRCLR, OCDDCR_ENABLEOCD);
+		if (ret != ERROR_OK) {
+			LOG_ERROR("Failed to queue OCDDCR_ENABLEOCD clear operation!");
+			return;
+		}
+		xtensa_dm_queue_tdi_idle(&xtensa->dbg_mod);
+		ret = jtag_execute_queue();
+		if (ret != ERROR_OK) {
+			LOG_ERROR("Failed to clear OCDDCR_ENABLEOCD!");
+			return;
+		}
+	}
+	xtensa_free_reg_cache(target);
+	free(xtensa->hw_brps);
+	free(xtensa->hw_wps);
+	free(xtensa->sw_brps);
+}
+
+const char *xtensa_get_gdb_arch(struct target *target)
+{
+	return "xtensa";
+}
+
+COMMAND_HELPER(xtensa_cmd_permissive_mode_do, struct xtensa *xtensa)
+{
+	return CALL_COMMAND_HANDLER(handle_command_parse_bool,
+		&xtensa->permissive_mode, "xtensa permissive mode");
+}
+
+COMMAND_HANDLER(xtensa_cmd_permissive_mode)
+{
+	return CALL_COMMAND_HANDLER(xtensa_cmd_permissive_mode_do,
+		target_to_xtensa(get_current_target(CMD_CTX)));
+}
+
+/* perfmon_enable <counter_id> <select> [mask] [kernelcnt] [tracelevel] */
+COMMAND_HELPER(xtensa_cmd_perfmon_enable_do, struct xtensa *xtensa)
+{
+	struct xtensa_perfmon_config config = {
+		.mask = 0xffff,
+		.kernelcnt = 0,
+		.tracelevel = -1	/* use DEBUGLEVEL by default */
+	};
+
+	if (CMD_ARGC < 2 || CMD_ARGC > 6)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	unsigned int counter_id = strtoul(CMD_ARGV[0], NULL, 0);
+	if (counter_id >= XTENSA_MAX_PERF_COUNTERS) {
+		command_print(CMD, "counter_id should be < %d", XTENSA_MAX_PERF_COUNTERS);
+		return ERROR_COMMAND_ARGUMENT_INVALID;
+	}
+
+	config.select = strtoul(CMD_ARGV[1], NULL, 0);
+	if (config.select > XTENSA_MAX_PERF_SELECT) {
+		command_print(CMD, "select should be < %d", XTENSA_MAX_PERF_SELECT);
+		return ERROR_COMMAND_ARGUMENT_INVALID;
+	}
+
+	if (CMD_ARGC >= 3) {
+		config.mask = strtoul(CMD_ARGV[2], NULL, 0);
+		if (config.mask > XTENSA_MAX_PERF_MASK) {
+			command_print(CMD, "mask should be < %d", XTENSA_MAX_PERF_MASK);
+			return ERROR_COMMAND_ARGUMENT_INVALID;
+		}
+	}
+
+	if (CMD_ARGC >= 4) {
+		config.kernelcnt = strtoul(CMD_ARGV[3], NULL, 0);
+		if (config.kernelcnt > 1) {
+			command_print(CMD, "kernelcnt should be 0 or 1");
+			return ERROR_COMMAND_ARGUMENT_INVALID;
+		}
+	}
+
+	if (CMD_ARGC >= 5) {
+		config.tracelevel = strtoul(CMD_ARGV[4], NULL, 0);
+		if (config.tracelevel > 7) {
+			command_print(CMD, "tracelevel should be <=7");
+			return ERROR_COMMAND_ARGUMENT_INVALID;
+		}
+	}
+
+	if (config.tracelevel == -1)
+		config.tracelevel = xtensa->core_config->debug.irq_level;
+
+	return xtensa_dm_perfmon_enable(&xtensa->dbg_mod, counter_id, &config);
+}
+
+COMMAND_HANDLER(xtensa_cmd_perfmon_enable)
+{
+	return CALL_COMMAND_HANDLER(xtensa_cmd_perfmon_enable_do,
+		target_to_xtensa(get_current_target(CMD_CTX)));
+}
+
+/* perfmon_dump [counter_id] */
+COMMAND_HELPER(xtensa_cmd_perfmon_dump_do, struct xtensa *xtensa)
+{
+	if (CMD_ARGC > 1)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	int counter_id = -1;
+	if (CMD_ARGC == 1) {
+		counter_id = strtol(CMD_ARGV[0], NULL, 0);
+		if (counter_id > XTENSA_MAX_PERF_COUNTERS) {
+			command_print(CMD, "counter_id should be < %d", XTENSA_MAX_PERF_COUNTERS);
+			return ERROR_COMMAND_ARGUMENT_INVALID;
+		}
+	}
+
+	unsigned int counter_start = (counter_id < 0) ? 0 : counter_id;
+	unsigned int counter_end = (counter_id < 0) ? XTENSA_MAX_PERF_COUNTERS : counter_id + 1;
+	for (unsigned int counter = counter_start; counter < counter_end; ++counter) {
+		char result_buf[128] = { 0 };
+		size_t result_pos = snprintf(result_buf, sizeof(result_buf), "Counter %d: ", counter);
+		struct xtensa_perfmon_result result;
+		int res = xtensa_dm_perfmon_dump(&xtensa->dbg_mod, counter, &result);
+		if (res != ERROR_OK)
+			return res;
+		snprintf(result_buf + result_pos, sizeof(result_buf) - result_pos,
+			"%-12" PRIu64 "%s",
+			result.value,
+			result.overflow ? " (overflow)" : "");
+		LOG_INFO("%s", result_buf);
+	}
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(xtensa_cmd_perfmon_dump)
+{
+	return CALL_COMMAND_HANDLER(xtensa_cmd_perfmon_dump_do,
+		target_to_xtensa(get_current_target(CMD_CTX)));
+}
+
+COMMAND_HELPER(xtensa_cmd_mask_interrupts_do, struct xtensa *xtensa)
+{
+	int state = -1;
+
+	if (CMD_ARGC < 1) {
+		const char *st;
+		state = xtensa->stepping_isr_mode;
+		if (state == XT_STEPPING_ISR_ON)
+			st = "OFF";
+		else if (state == XT_STEPPING_ISR_OFF)
+			st = "ON";
+		else
+			st = "UNKNOWN";
+		command_print(CMD, "Current ISR step mode: %s", st);
+		return ERROR_OK;
+	}
+	/* Masking is ON -> interrupts during stepping are OFF, and vice versa */
+	if (!strcasecmp(CMD_ARGV[0], "off"))
+		state = XT_STEPPING_ISR_ON;
+	else if (!strcasecmp(CMD_ARGV[0], "on"))
+		state = XT_STEPPING_ISR_OFF;
+
+	if (state == -1) {
+		command_print(CMD, "Argument unknown. Please pick one of ON, OFF");
+		return ERROR_FAIL;
+	}
+	xtensa->stepping_isr_mode = state;
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(xtensa_cmd_mask_interrupts)
+{
+	return CALL_COMMAND_HANDLER(xtensa_cmd_mask_interrupts_do,
+		target_to_xtensa(get_current_target(CMD_CTX)));
+}
+
+COMMAND_HELPER(xtensa_cmd_smpbreak_do, struct target *target)
+{
+	int res = ERROR_OK;
+	uint32_t val = 0;
+
+	if (CMD_ARGC >= 1) {
+		for (unsigned int i = 0; i < CMD_ARGC; i++) {
+			if (!strcasecmp(CMD_ARGV[0], "none")) {
+				val = 0;
+			} else if (!strcasecmp(CMD_ARGV[i], "BreakIn")) {
+				val |= OCDDCR_BREAKINEN;
+			} else if (!strcasecmp(CMD_ARGV[i], "BreakOut")) {
+				val |= OCDDCR_BREAKOUTEN;
+			} else if (!strcasecmp(CMD_ARGV[i], "RunStallIn")) {
+				val |= OCDDCR_RUNSTALLINEN;
+			} else if (!strcasecmp(CMD_ARGV[i], "DebugModeOut")) {
+				val |= OCDDCR_DEBUGMODEOUTEN;
+			} else if (!strcasecmp(CMD_ARGV[i], "BreakInOut")) {
+				val |= OCDDCR_BREAKINEN | OCDDCR_BREAKOUTEN;
+			} else if (!strcasecmp(CMD_ARGV[i], "RunStall")) {
+				val |= OCDDCR_RUNSTALLINEN | OCDDCR_DEBUGMODEOUTEN;
+			} else {
+				command_print(CMD, "Unknown arg %s", CMD_ARGV[i]);
+				command_print(
+					CMD,
+					"use either BreakInOut, None or RunStall as arguments, or any combination of BreakIn, BreakOut, RunStallIn and DebugModeOut.");
+				return ERROR_OK;
+			}
+		}
+		res = xtensa_smpbreak_set(target, val);
+		if (res != ERROR_OK)
+			command_print(CMD, "Failed to set smpbreak config %d", res);
+	} else {
+		struct xtensa *xtensa = target_to_xtensa(target);
+		res = xtensa_smpbreak_read(xtensa, &val);
+		if (res == ERROR_OK) {
+			command_print(CMD, "Current bits set:%s%s%s%s",
+				(val & OCDDCR_BREAKINEN) ? " BreakIn" : "",
+				(val & OCDDCR_BREAKOUTEN) ? " BreakOut" : "",
+				(val & OCDDCR_RUNSTALLINEN) ? " RunStallIn" : "",
+				(val & OCDDCR_DEBUGMODEOUTEN) ? " DebugModeOut" : ""
+				);
+		} else {
+			command_print(CMD, "Failed to get smpbreak config %d", res);
+		}
+	}
+	return res;
+}
+
+COMMAND_HANDLER(xtensa_cmd_smpbreak)
+{
+	return CALL_COMMAND_HANDLER(xtensa_cmd_smpbreak_do,
+		get_current_target(CMD_CTX));
+}
+
+COMMAND_HELPER(xtensa_cmd_tracestart_do, struct xtensa *xtensa)
+{
+	struct xtensa_trace_status trace_status;
+	struct xtensa_trace_start_config cfg = {
+		.stoppc = 0,
+		.stopmask = XTENSA_STOPMASK_DISABLED,
+		.after = 0,
+		.after_is_words = false
+	};
+
+	/* Parse arguments */
+	for (unsigned int i = 0; i < CMD_ARGC; i++) {
+		if ((!strcasecmp(CMD_ARGV[i], "pc")) && CMD_ARGC > i) {
+			char *e;
+			i++;
+			cfg.stoppc = strtol(CMD_ARGV[i], &e, 0);
+			cfg.stopmask = 0;
+			if (*e == '/')
+				cfg.stopmask = strtol(e, NULL, 0);
+		} else if ((!strcasecmp(CMD_ARGV[i], "after")) && CMD_ARGC > i) {
+			i++;
+			cfg.after = strtol(CMD_ARGV[i], NULL, 0);
+		} else if (!strcasecmp(CMD_ARGV[i], "ins")) {
+			cfg.after_is_words = 0;
+		} else if (!strcasecmp(CMD_ARGV[i], "words")) {
+			cfg.after_is_words = 1;
+		} else {
+			command_print(CMD, "Did not understand %s", CMD_ARGV[i]);
+			return ERROR_FAIL;
+		}
+	}
+
+	int res = xtensa_dm_trace_status_read(&xtensa->dbg_mod, &trace_status);
+	if (res != ERROR_OK)
+		return res;
+	if (trace_status.stat & TRAXSTAT_TRACT) {
+		LOG_WARNING("Silently stop active tracing!");
+		res = xtensa_dm_trace_stop(&xtensa->dbg_mod, false);
+		if (res != ERROR_OK)
+			return res;
+	}
+
+	res = xtensa_dm_trace_start(&xtensa->dbg_mod, &cfg);
+	if (res != ERROR_OK)
+		return res;
+
+	xtensa->trace_active = true;
+	command_print(CMD, "Trace started.");
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(xtensa_cmd_tracestart)
+{
+	return CALL_COMMAND_HANDLER(xtensa_cmd_tracestart_do,
+		target_to_xtensa(get_current_target(CMD_CTX)));
+}
+
+COMMAND_HELPER(xtensa_cmd_tracestop_do, struct xtensa *xtensa)
+{
+	struct xtensa_trace_status trace_status;
+
+	int res = xtensa_dm_trace_status_read(&xtensa->dbg_mod, &trace_status);
+	if (res != ERROR_OK)
+		return res;
+
+	if (!(trace_status.stat & TRAXSTAT_TRACT)) {
+		command_print(CMD, "No trace is currently active.");
+		return ERROR_FAIL;
+	}
+
+	res = xtensa_dm_trace_stop(&xtensa->dbg_mod, true);
+	if (res != ERROR_OK)
+		return res;
+
+	xtensa->trace_active = false;
+	command_print(CMD, "Trace stop triggered.");
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(xtensa_cmd_tracestop)
+{
+	return CALL_COMMAND_HANDLER(xtensa_cmd_tracestop_do,
+		target_to_xtensa(get_current_target(CMD_CTX)));
+}
+
+COMMAND_HELPER(xtensa_cmd_tracedump_do, struct xtensa *xtensa, const char *fname)
+{
+	struct xtensa_trace_config trace_config;
+	struct xtensa_trace_status trace_status;
+	uint32_t memsz, wmem;
+
+	int res = xtensa_dm_trace_status_read(&xtensa->dbg_mod, &trace_status);
+	if (res != ERROR_OK)
+		return res;
+
+	if (trace_status.stat & TRAXSTAT_TRACT) {
+		command_print(CMD, "Tracing is still active. Please stop it first.");
+		return ERROR_FAIL;
+	}
+
+	res = xtensa_dm_trace_config_read(&xtensa->dbg_mod, &trace_config);
+	if (res != ERROR_OK)
+		return res;
+
+	if (!(trace_config.ctrl & TRAXCTRL_TREN)) {
+		command_print(CMD, "No active trace found; nothing to dump.");
+		return ERROR_FAIL;
+	}
+
+	memsz = trace_config.memaddr_end - trace_config.memaddr_start + 1;
+	LOG_INFO("Total trace memory: %d words", memsz);
+	if ((trace_config.addr &
+			((TRAXADDR_TWRAP_MASK << TRAXADDR_TWRAP_SHIFT) | TRAXADDR_TWSAT)) == 0) {
+		/*Memory hasn't overwritten itself yet. */
+		wmem = trace_config.addr & TRAXADDR_TADDR_MASK;
+		LOG_INFO("...but trace is only %d words", wmem);
+		if (wmem < memsz)
+			memsz = wmem;
+	} else {
+		if (trace_config.addr & TRAXADDR_TWSAT) {
+			LOG_INFO("Real trace is many times longer than that (overflow)");
+		} else {
+			uint32_t trc_sz = (trace_config.addr >> TRAXADDR_TWRAP_SHIFT) & TRAXADDR_TWRAP_MASK;
+			trc_sz = (trc_sz * memsz) + (trace_config.addr & TRAXADDR_TADDR_MASK);
+			LOG_INFO("Real trace is %d words, but the start has been truncated.", trc_sz);
+		}
+	}
+
+	uint8_t *tracemem = malloc(memsz * 4);
+	if (!tracemem) {
+		command_print(CMD, "Failed to alloc memory for trace data!");
+		return ERROR_FAIL;
+	}
+	res = xtensa_dm_trace_data_read(&xtensa->dbg_mod, tracemem, memsz * 4);
+	if (res != ERROR_OK) {
+		free(tracemem);
+		return res;
+	}
+
+	int f = open(fname, O_WRONLY | O_CREAT | O_TRUNC, 0666);
+	if (f <= 0) {
+		free(tracemem);
+		command_print(CMD, "Unable to open file %s", fname);
+		return ERROR_FAIL;
+	}
+	if (write(f, tracemem, memsz * 4) != (int)memsz * 4)
+		command_print(CMD, "Unable to write to file %s", fname);
+	else
+		command_print(CMD, "Written %d bytes of trace data to %s", memsz * 4, fname);
+	close(f);
+
+	bool is_all_zeroes = true;
+	for (unsigned int i = 0; i < memsz * 4; i++) {
+		if (tracemem[i] != 0) {
+			is_all_zeroes = false;
+			break;
+		}
+	}
+	free(tracemem);
+	if (is_all_zeroes)
+		command_print(
+			CMD,
+			"WARNING: File written is all zeroes. Are you sure you enabled trace memory?");
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(xtensa_cmd_tracedump)
+{
+	if (CMD_ARGC != 1) {
+		command_print(CMD, "Command takes exactly 1 parameter.Need filename to dump to as output!");
+		return ERROR_FAIL;
+	}
+
+	return CALL_COMMAND_HANDLER(xtensa_cmd_tracedump_do,
+		target_to_xtensa(get_current_target(CMD_CTX)), CMD_ARGV[0]);
+}
+
+const struct command_registration xtensa_command_handlers[] = {
+	{
+		.name = "set_permissive",
+		.handler = xtensa_cmd_permissive_mode,
+		.mode = COMMAND_ANY,
+		.help = "When set to 1, enable Xtensa permissive mode (less client-side checks)",
+		.usage = "[0|1]",
+	},
+	{
+		.name = "maskisr",
+		.handler = xtensa_cmd_mask_interrupts,
+		.mode = COMMAND_ANY,
+		.help = "mask Xtensa interrupts at step",
+		.usage = "['on'|'off']",
+	},
+	{
+		.name = "smpbreak",
+		.handler = xtensa_cmd_smpbreak,
+		.mode = COMMAND_ANY,
+		.help = "Set the way the CPU chains OCD breaks",
+		.usage =
+			"[none|breakinout|runstall] | [BreakIn] [BreakOut] [RunStallIn] [DebugModeOut]",
+	},
+	{
+		.name = "perfmon_enable",
+		.handler = xtensa_cmd_perfmon_enable,
+		.mode = COMMAND_EXEC,
+		.help = "Enable and start performance counter",
+		.usage = "<counter_id> <select> [mask] [kernelcnt] [tracelevel]",
+	},
+	{
+		.name = "perfmon_dump",
+		.handler = xtensa_cmd_perfmon_dump,
+		.mode = COMMAND_EXEC,
+		.help =
+			"Dump performance counter value. If no argument specified, dumps all counters.",
+		.usage = "[counter_id]",
+	},
+	{
+		.name = "tracestart",
+		.handler = xtensa_cmd_tracestart,
+		.mode = COMMAND_EXEC,
+		.help =
+			"Tracing: Set up and start a trace. Optionally set stop trigger address and amount of data captured after.",
+		.usage = "[pc <pcval>/[maskbitcount]] [after <n> [ins|words]]",
+	},
+	{
+		.name = "tracestop",
+		.handler = xtensa_cmd_tracestop,
+		.mode = COMMAND_EXEC,
+		.help = "Tracing: Stop current trace as started by the tracestart command",
+		.usage = "",
+	},
+	{
+		.name = "tracedump",
+		.handler = xtensa_cmd_tracedump,
+		.mode = COMMAND_EXEC,
+		.help = "Tracing: Dump trace memory to a files. One file per core.",
+		.usage = "<outfile>",
+	},
+	COMMAND_REGISTRATION_DONE
+};
diff --git a/src/target/xtensa/xtensa.h b/src/target/xtensa/xtensa.h
new file mode 100644
index 0000000..d8b15e1
--- /dev/null
+++ b/src/target/xtensa/xtensa.h
@@ -0,0 +1,309 @@
+/***************************************************************************
+ *   Generic Xtensa target                                                 *
+ *   Copyright (C) 2019 Espressif Systems Ltd.                             *
+ *   Author: Alexey Gerenkov <alexey@espressif.com>                        *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+
+#ifndef OPENOCD_TARGET_XTENSA_H
+#define OPENOCD_TARGET_XTENSA_H
+
+#include "assert.h"
+#include <target/target.h>
+#include <target/breakpoints.h>
+#include "xtensa_regs.h"
+#include "xtensa_debug_module.h"
+
+/**
+ * @file
+ * Holds the interface to Xtensa cores.
+ */
+
+#define XT_ISNS_SZ_MAX                  3
+
+#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)
+#define XT_PS_CALLINC_MSK               (0x3 << 16)
+#define XT_PS_OWB_MSK                   (0xF << 8)
+
+#define XT_LOCAL_MEM_REGIONS_NUM_MAX    8
+
+#define XT_AREGS_NUM_MAX                64
+#define XT_USER_REGS_NUM_MAX            256
+
+#define XT_MEM_ACCESS_NONE              0x0
+#define XT_MEM_ACCESS_READ              0x1
+#define XT_MEM_ACCESS_WRITE             0x2
+
+enum xtensa_mem_err_detect {
+	XT_MEM_ERR_DETECT_NONE,
+	XT_MEM_ERR_DETECT_PARITY,
+	XT_MEM_ERR_DETECT_ECC,
+};
+
+struct xtensa_cache_config {
+	uint8_t way_count;
+	uint8_t line_size;
+	uint16_t size;
+	bool writeback;
+	enum xtensa_mem_err_detect mem_err_check;
+};
+
+struct xtensa_local_mem_region_config {
+	target_addr_t base;
+	uint32_t size;
+	enum xtensa_mem_err_detect mem_err_check;
+	int access;
+};
+
+struct xtensa_local_mem_config {
+	uint16_t count;
+	struct xtensa_local_mem_region_config regions[XT_LOCAL_MEM_REGIONS_NUM_MAX];
+};
+
+struct xtensa_mmu_config {
+	bool enabled;
+	uint8_t itlb_entries_count;
+	uint8_t dtlb_entries_count;
+	bool ivarway56;
+	bool dvarway56;
+};
+
+struct xtensa_exception_config {
+	bool enabled;
+	uint8_t depc_num;
+};
+
+struct xtensa_irq_config {
+	bool enabled;
+	uint8_t irq_num;
+};
+
+struct xtensa_high_prio_irq_config {
+	bool enabled;
+	uint8_t excm_level;
+	uint8_t nmi_num;
+};
+
+struct xtensa_debug_config {
+	bool enabled;
+	uint8_t irq_level;
+	uint8_t ibreaks_num;
+	uint8_t dbreaks_num;
+	uint8_t icount_sz;
+};
+
+struct xtensa_tracing_config {
+	bool enabled;
+	uint32_t mem_sz;
+	bool reversed_mem_access;
+};
+
+struct xtensa_timer_irq_config {
+	bool enabled;
+	uint8_t comp_num;
+};
+
+struct xtensa_config {
+	bool density;
+	uint8_t aregs_num;
+	bool windowed;
+	bool coproc;
+	bool fp_coproc;
+	bool loop;
+	uint8_t miscregs_num;
+	bool threadptr;
+	bool boolean;
+	bool cond_store;
+	bool ext_l32r;
+	bool mac16;
+	bool reloc_vec;
+	bool proc_id;
+	bool mem_err_check;
+	uint16_t user_regs_num;
+	const struct xtensa_user_reg_desc *user_regs;
+	int (*fetch_user_regs)(struct target *target);
+	int (*queue_write_dirty_user_regs)(struct target *target);
+	struct xtensa_cache_config icache;
+	struct xtensa_cache_config dcache;
+	struct xtensa_local_mem_config irom;
+	struct xtensa_local_mem_config iram;
+	struct xtensa_local_mem_config drom;
+	struct xtensa_local_mem_config dram;
+	struct xtensa_local_mem_config uram;
+	struct xtensa_local_mem_config xlmi;
+	struct xtensa_mmu_config mmu;
+	struct xtensa_exception_config exc;
+	struct xtensa_irq_config irq;
+	struct xtensa_high_prio_irq_config high_irq;
+	struct xtensa_timer_irq_config tim_irq;
+	struct xtensa_debug_config debug;
+	struct xtensa_tracing_config trace;
+	unsigned int gdb_general_regs_num;
+	const unsigned int *gdb_regs_mapping;
+};
+
+typedef uint32_t xtensa_insn_t;
+
+enum xtensa_stepping_isr_mode {
+	XT_STEPPING_ISR_OFF,	/* interrupts are disabled during stepping */
+	XT_STEPPING_ISR_ON,		/* interrupts are enabled during stepping */
+};
+
+/* Only supported in cores with in-CPU MMU. None of Espressif chips as of now. */
+enum xtensa_mode {
+	XT_MODE_RING0,
+	XT_MODE_RING1,
+	XT_MODE_RING2,
+	XT_MODE_RING3,
+	XT_MODE_ANY	/* special value to run algorithm in current core mode */
+};
+
+struct xtensa_sw_breakpoint {
+	struct breakpoint *oocd_bp;
+	/* original insn */
+	uint8_t insn[XT_ISNS_SZ_MAX];
+	/* original insn size */
+	uint8_t insn_sz;	/* 2 or 3 bytes */
+};
+
+#define XTENSA_COMMON_MAGIC 0x54E4E555U
+
+/**
+ * Represents a generic Xtensa core.
+ */
+struct xtensa {
+	unsigned int common_magic;
+	const struct xtensa_config *core_config;
+	struct xtensa_debug_module dbg_mod;
+	struct reg_cache *core_cache;
+	unsigned int regs_num;
+	/* An array of pointers to buffers to backup registers' values while algo is run on target.
+	 * Size is 'regs_num'. */
+	void **algo_context_backup;
+	struct target *target;
+	bool reset_asserted;
+	enum xtensa_stepping_isr_mode stepping_isr_mode;
+	struct breakpoint **hw_brps;
+	struct watchpoint **hw_wps;
+	struct xtensa_sw_breakpoint *sw_brps;
+	bool trace_active;
+	bool permissive_mode;	/* bypass memory checks */
+	bool suppress_dsr_errors;
+	uint32_t smp_break;
+	/* Sometimes debug module's 'powered' bit is cleared after reset, but get set after some
+	 * time.This is the number of polling periods after which core is considered to be powered
+	 * off (marked as unexamined) if the bit retains to be cleared (e.g. if core is disabled by
+	 * SW running on target).*/
+	uint8_t come_online_probes_num;
+	bool regs_fetched;	/* true after first register fetch completed successfully */
+};
+
+static inline struct xtensa *target_to_xtensa(struct target *target)
+{
+	assert(target);
+	struct xtensa *xtensa = target->arch_info;
+	assert(xtensa->common_magic == XTENSA_COMMON_MAGIC);
+	return xtensa;
+}
+
+int xtensa_init_arch_info(struct target *target,
+	struct xtensa *xtensa,
+	const struct xtensa_config *cfg,
+	const struct xtensa_debug_module_config *dm_cfg);
+int xtensa_target_init(struct command_context *cmd_ctx, struct target *target);
+void xtensa_target_deinit(struct target *target);
+
+static inline bool xtensa_addr_in_mem(const struct xtensa_local_mem_config *mem, uint32_t addr)
+{
+	for (unsigned int i = 0; i < mem->count; i++) {
+		if (addr >= mem->regions[i].base &&
+			addr < mem->regions[i].base + mem->regions[i].size)
+			return true;
+	}
+	return false;
+}
+
+static inline bool xtensa_data_addr_valid(struct target *target, uint32_t addr)
+{
+	struct xtensa *xtensa = target_to_xtensa(target);
+
+	if (xtensa_addr_in_mem(&xtensa->core_config->drom, addr))
+		return true;
+	if (xtensa_addr_in_mem(&xtensa->core_config->dram, addr))
+		return true;
+	if (xtensa_addr_in_mem(&xtensa->core_config->uram, addr))
+		return true;
+	return false;
+}
+
+int xtensa_core_status_check(struct target *target);
+
+int xtensa_examine(struct target *target);
+int xtensa_wakeup(struct target *target);
+int xtensa_smpbreak_set(struct target *target, uint32_t set);
+int xtensa_smpbreak_get(struct target *target, uint32_t *val);
+int xtensa_smpbreak_write(struct xtensa *xtensa, uint32_t set);
+int xtensa_smpbreak_read(struct xtensa *xtensa, uint32_t *val);
+xtensa_reg_val_t xtensa_reg_get(struct target *target, enum xtensa_reg_id reg_id);
+void xtensa_reg_set(struct target *target, enum xtensa_reg_id reg_id, xtensa_reg_val_t value);
+int xtensa_fetch_all_regs(struct target *target);
+int xtensa_get_gdb_reg_list(struct target *target,
+	struct reg **reg_list[],
+	int *reg_list_size,
+	enum target_register_class reg_class);
+int xtensa_poll(struct target *target);
+void xtensa_on_poll(struct target *target);
+int xtensa_halt(struct target *target);
+int xtensa_resume(struct target *target,
+	int current,
+	target_addr_t address,
+	int handle_breakpoints,
+	int debug_execution);
+int xtensa_prepare_resume(struct target *target,
+	int current,
+	target_addr_t address,
+	int handle_breakpoints,
+	int debug_execution);
+int xtensa_do_resume(struct target *target);
+int xtensa_step(struct target *target, int current, target_addr_t address, int handle_breakpoints);
+int xtensa_do_step(struct target *target, int current, target_addr_t address, int handle_breakpoints);
+int xtensa_mmu_is_enabled(struct target *target, int *enabled);
+int xtensa_read_memory(struct target *target, target_addr_t address, uint32_t size, uint32_t count, uint8_t *buffer);
+int xtensa_read_buffer(struct target *target, target_addr_t address, uint32_t count, uint8_t *buffer);
+int xtensa_write_memory(struct target *target,
+	target_addr_t address,
+	uint32_t size,
+	uint32_t count,
+	const uint8_t *buffer);
+int xtensa_write_buffer(struct target *target, target_addr_t address, uint32_t count, const uint8_t *buffer);
+int xtensa_checksum_memory(struct target *target, target_addr_t address, uint32_t count, uint32_t *checksum);
+int xtensa_assert_reset(struct target *target);
+int xtensa_deassert_reset(struct target *target);
+int xtensa_breakpoint_add(struct target *target, struct breakpoint *breakpoint);
+int xtensa_breakpoint_remove(struct target *target, struct breakpoint *breakpoint);
+int xtensa_watchpoint_add(struct target *target, struct watchpoint *watchpoint);
+int xtensa_watchpoint_remove(struct target *target, struct watchpoint *watchpoint);
+void xtensa_set_permissive_mode(struct target *target, bool state);
+int xtensa_fetch_user_regs_u32(struct target *target);
+int xtensa_queue_write_dirty_user_regs_u32(struct target *target);
+const char *xtensa_get_gdb_arch(struct target *target);
+
+extern const struct reg_arch_type xtensa_user_reg_u32_type;
+extern const struct reg_arch_type xtensa_user_reg_u128_type;
+extern const struct command_registration xtensa_command_handlers[];
+
+#endif	/* OPENOCD_TARGET_XTENSA_H */
diff --git a/src/target/xtensa/xtensa_debug_module.c b/src/target/xtensa/xtensa_debug_module.c
new file mode 100644
index 0000000..c6959ae
--- /dev/null
+++ b/src/target/xtensa/xtensa_debug_module.c
@@ -0,0 +1,359 @@
+/***************************************************************************
+ *   Generic Xtensa debug module API for OpenOCD                           *
+ *   Copyright (C) 2019 Espressif Systems Ltd.                             *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include "xtensa_debug_module.h"
+
+#define TAPINS_PWRCTL           0x08
+#define TAPINS_PWRSTAT          0x09
+#define TAPINS_NARSEL           0x1C
+#define TAPINS_IDCODE           0x1E
+#define TAPINS_BYPASS           0x1F
+
+#define TAPINS_PWRCTL_LEN       8
+#define TAPINS_PWRSTAT_LEN      8
+#define TAPINS_NARSEL_ADRLEN    8
+#define TAPINS_NARSEL_DATALEN   32
+#define TAPINS_IDCODE_LEN       32
+#define TAPINS_BYPASS_LEN       1
+
+
+static void xtensa_dm_add_set_ir(struct xtensa_debug_module *dm, uint8_t value)
+{
+	struct scan_field field;
+	uint8_t t[4] = { 0 };
+
+	memset(&field, 0, sizeof(field));
+	field.num_bits = dm->tap->ir_length;
+	field.out_value = t;
+	buf_set_u32(t, 0, field.num_bits, value);
+	jtag_add_ir_scan(dm->tap, &field, TAP_IDLE);
+}
+
+static void xtensa_dm_add_dr_scan(struct xtensa_debug_module *dm,
+	int len,
+	const uint8_t *src,
+	uint8_t *dest,
+	tap_state_t endstate)
+{
+	struct scan_field field;
+
+	memset(&field, 0, sizeof(field));
+	field.num_bits = len;
+	field.out_value = src;
+	field.in_value = dest;
+	jtag_add_dr_scan(dm->tap, 1, &field, endstate);
+}
+
+int xtensa_dm_init(struct xtensa_debug_module *dm, const struct xtensa_debug_module_config *cfg)
+{
+	if (!dm || !cfg)
+		return ERROR_FAIL;
+
+	dm->pwr_ops = cfg->pwr_ops;
+	dm->dbg_ops = cfg->dbg_ops;
+	dm->tap = cfg->tap;
+	dm->queue_tdi_idle = cfg->queue_tdi_idle;
+	dm->queue_tdi_idle_arg = cfg->queue_tdi_idle_arg;
+	return ERROR_OK;
+}
+
+int xtensa_dm_queue_enable(struct xtensa_debug_module *dm)
+{
+	return dm->dbg_ops->queue_reg_write(dm, NARADR_DCRSET, OCDDCR_ENABLEOCD);
+}
+
+int xtensa_dm_queue_reg_read(struct xtensa_debug_module *dm, unsigned int reg, uint8_t *value)
+{
+	uint8_t regdata = (reg << 1) | 0;
+	uint8_t dummy[4] = { 0, 0, 0, 0 };
+
+	if (reg > NARADR_MAX) {
+		LOG_ERROR("Invalid DBG reg ID %d!", reg);
+		return ERROR_FAIL;
+	}
+	xtensa_dm_add_set_ir(dm, TAPINS_NARSEL);
+	xtensa_dm_add_dr_scan(dm, TAPINS_NARSEL_ADRLEN, &regdata, NULL, TAP_IDLE);
+	xtensa_dm_add_dr_scan(dm, TAPINS_NARSEL_DATALEN, dummy, value, TAP_IDLE);
+	return ERROR_OK;
+}
+
+int xtensa_dm_queue_reg_write(struct xtensa_debug_module *dm, unsigned int reg, uint32_t value)
+{
+	uint8_t regdata = (reg << 1) | 1;
+	uint8_t valdata[] = { value, value >> 8, value >> 16, value >> 24 };
+
+	if (reg > NARADR_MAX) {
+		LOG_ERROR("Invalid DBG reg ID %d!", reg);
+		return ERROR_FAIL;
+	}
+	xtensa_dm_add_set_ir(dm, TAPINS_NARSEL);
+	xtensa_dm_add_dr_scan(dm, TAPINS_NARSEL_ADRLEN, &regdata, NULL, TAP_IDLE);
+	xtensa_dm_add_dr_scan(dm, TAPINS_NARSEL_DATALEN, valdata, NULL, TAP_IDLE);
+	return ERROR_OK;
+}
+
+int xtensa_dm_queue_pwr_reg_read(struct xtensa_debug_module *dm, unsigned int reg, uint8_t *data, uint8_t clear)
+{
+	uint8_t value_clr = clear;
+	uint8_t tap_insn;
+	int tap_insn_sz;
+
+	if (reg == DMREG_PWRCTL) {
+		tap_insn = TAPINS_PWRCTL;
+		tap_insn_sz = TAPINS_PWRCTL_LEN;
+	} else if (reg == DMREG_PWRSTAT) {
+		tap_insn = TAPINS_PWRSTAT;
+		tap_insn_sz = TAPINS_PWRSTAT_LEN;
+	} else {
+		LOG_ERROR("Invalid PWR reg ID %d!", reg);
+		return ERROR_FAIL;
+	}
+	xtensa_dm_add_set_ir(dm, tap_insn);
+	xtensa_dm_add_dr_scan(dm, tap_insn_sz, &value_clr, data, TAP_IDLE);
+	return ERROR_OK;
+}
+
+int xtensa_dm_queue_pwr_reg_write(struct xtensa_debug_module *dm, unsigned int reg, uint8_t data)
+{
+	uint8_t value = data;
+	uint8_t tap_insn;
+	int tap_insn_sz;
+
+	if (reg == DMREG_PWRCTL) {
+		tap_insn = TAPINS_PWRCTL;
+		tap_insn_sz = TAPINS_PWRCTL_LEN;
+	} else if (reg == DMREG_PWRSTAT) {
+		tap_insn = TAPINS_PWRSTAT;
+		tap_insn_sz = TAPINS_PWRSTAT_LEN;
+	} else {
+		LOG_ERROR("Invalid PWR reg ID %d!", reg);
+		return ERROR_FAIL;
+	}
+	xtensa_dm_add_set_ir(dm, tap_insn);
+	xtensa_dm_add_dr_scan(dm, tap_insn_sz, &value, NULL, TAP_IDLE);
+	return ERROR_OK;
+}
+
+int xtensa_dm_device_id_read(struct xtensa_debug_module *dm)
+{
+	uint8_t id_buf[sizeof(uint32_t)];
+
+	dm->dbg_ops->queue_reg_read(dm, NARADR_OCDID, id_buf);
+	xtensa_dm_queue_tdi_idle(dm);
+	int res = jtag_execute_queue();
+	if (res != ERROR_OK)
+		return res;
+	dm->device_id = buf_get_u32(id_buf, 0, 32);
+	return ERROR_OK;
+}
+
+int xtensa_dm_power_status_read(struct xtensa_debug_module *dm, uint32_t clear)
+{
+	/* uint8_t id_buf[sizeof(uint32_t)]; */
+
+	/* TODO: JTAG does not work when PWRCTL_JTAGDEBUGUSE is not set.
+	 * It is set in xtensa_examine(), need to move reading of NARADR_OCDID out of this function */
+	/* dm->dbg_ops->queue_reg_read(dm, NARADR_OCDID, id_buf);
+	 *Read reset state */
+	dm->pwr_ops->queue_reg_read(dm, DMREG_PWRSTAT, &dm->power_status.stat, clear);
+	dm->pwr_ops->queue_reg_read(dm, DMREG_PWRSTAT, &dm->power_status.stath, clear);
+	xtensa_dm_queue_tdi_idle(dm);
+	return jtag_execute_queue();
+}
+
+int xtensa_dm_core_status_read(struct xtensa_debug_module *dm)
+{
+	uint8_t dsr_buf[sizeof(uint32_t)];
+
+	xtensa_dm_queue_enable(dm);
+	dm->dbg_ops->queue_reg_read(dm, NARADR_DSR, dsr_buf);
+	xtensa_dm_queue_tdi_idle(dm);
+	int res = jtag_execute_queue();
+	if (res != ERROR_OK)
+		return res;
+	dm->core_status.dsr = buf_get_u32(dsr_buf, 0, 32);
+	return res;
+}
+
+int xtensa_dm_core_status_clear(struct xtensa_debug_module *dm, xtensa_dsr_t bits)
+{
+	dm->dbg_ops->queue_reg_write(dm, NARADR_DSR, bits);
+	xtensa_dm_queue_tdi_idle(dm);
+	return jtag_execute_queue();
+}
+
+int xtensa_dm_trace_start(struct xtensa_debug_module *dm, struct xtensa_trace_start_config *cfg)
+{
+	/*Turn off trace unit so we can start a new trace. */
+	dm->dbg_ops->queue_reg_write(dm, NARADR_TRAXCTRL, 0);
+	xtensa_dm_queue_tdi_idle(dm);
+	int res = jtag_execute_queue();
+	if (res != ERROR_OK)
+		return res;
+
+	/*Set up parameters */
+	dm->dbg_ops->queue_reg_write(dm, NARADR_TRAXADDR, 0);
+	if (cfg->stopmask != XTENSA_STOPMASK_DISABLED) {
+		dm->dbg_ops->queue_reg_write(dm, NARADR_PCMATCHCTRL,
+			(cfg->stopmask << PCMATCHCTRL_PCML_SHIFT));
+		dm->dbg_ops->queue_reg_write(dm, NARADR_TRIGGERPC, cfg->stoppc);
+	}
+	dm->dbg_ops->queue_reg_write(dm, NARADR_DELAYCNT, cfg->after);
+	/*Options are mostly hardcoded for now. ToDo: make this more configurable. */
+	dm->dbg_ops->queue_reg_write(
+		dm,
+		NARADR_TRAXCTRL,
+		TRAXCTRL_TREN |
+		((cfg->stopmask != XTENSA_STOPMASK_DISABLED) ? TRAXCTRL_PCMEN : 0) | TRAXCTRL_TMEN |
+		(cfg->after_is_words ? 0 : TRAXCTRL_CNTU) | (0 << TRAXCTRL_SMPER_SHIFT) | TRAXCTRL_PTOWS);
+	xtensa_dm_queue_tdi_idle(dm);
+	return jtag_execute_queue();
+}
+
+int xtensa_dm_trace_stop(struct xtensa_debug_module *dm, bool pto_enable)
+{
+	uint8_t traxctl_buf[sizeof(uint32_t)];
+	uint32_t traxctl;
+	struct xtensa_trace_status trace_status;
+
+	dm->dbg_ops->queue_reg_read(dm, NARADR_TRAXCTRL, traxctl_buf);
+	xtensa_dm_queue_tdi_idle(dm);
+	int res = jtag_execute_queue();
+	if (res != ERROR_OK)
+		return res;
+	traxctl = buf_get_u32(traxctl_buf, 0, 32);
+
+	if (!pto_enable)
+		traxctl &= ~(TRAXCTRL_PTOWS | TRAXCTRL_PTOWT);
+
+	dm->dbg_ops->queue_reg_write(dm, NARADR_TRAXCTRL, traxctl | TRAXCTRL_TRSTP);
+	xtensa_dm_queue_tdi_idle(dm);
+	res = jtag_execute_queue();
+	if (res != ERROR_OK)
+		return res;
+
+	/*Check current status of trace hardware */
+	res = xtensa_dm_trace_status_read(dm, &trace_status);
+	if (res != ERROR_OK)
+		return res;
+
+	if (trace_status.stat & TRAXSTAT_TRACT) {
+		LOG_ERROR("Failed to stop tracing (0x%x)!", trace_status.stat);
+		return ERROR_FAIL;
+	}
+	return ERROR_OK;
+}
+
+int xtensa_dm_trace_status_read(struct xtensa_debug_module *dm, struct xtensa_trace_status *status)
+{
+	uint8_t traxstat_buf[sizeof(uint32_t)];
+
+	dm->dbg_ops->queue_reg_read(dm, NARADR_TRAXSTAT, traxstat_buf);
+	xtensa_dm_queue_tdi_idle(dm);
+	int res = jtag_execute_queue();
+	if (res == ERROR_OK && status)
+		status->stat = buf_get_u32(traxstat_buf, 0, 32);
+	return res;
+}
+
+int xtensa_dm_trace_config_read(struct xtensa_debug_module *dm, struct xtensa_trace_config *config)
+{
+	uint8_t traxctl_buf[sizeof(uint32_t)];
+	uint8_t memadrstart_buf[sizeof(uint32_t)];
+	uint8_t memadrend_buf[sizeof(uint32_t)];
+	uint8_t adr_buf[sizeof(uint32_t)];
+
+	if (!config)
+		return ERROR_FAIL;
+
+	dm->dbg_ops->queue_reg_read(dm, NARADR_TRAXCTRL, traxctl_buf);
+	dm->dbg_ops->queue_reg_read(dm, NARADR_MEMADDRSTART, memadrstart_buf);
+	dm->dbg_ops->queue_reg_read(dm, NARADR_MEMADDREND, memadrend_buf);
+	dm->dbg_ops->queue_reg_read(dm, NARADR_TRAXADDR, adr_buf);
+	xtensa_dm_queue_tdi_idle(dm);
+	int res = jtag_execute_queue();
+	if (res == ERROR_OK) {
+		config->ctrl = buf_get_u32(traxctl_buf, 0, 32);
+		config->memaddr_start = buf_get_u32(memadrstart_buf, 0, 32);
+		config->memaddr_end = buf_get_u32(memadrend_buf, 0, 32);
+		config->addr = buf_get_u32(adr_buf, 0, 32);
+	}
+	return res;
+}
+
+int xtensa_dm_trace_data_read(struct xtensa_debug_module *dm, uint8_t *dest, uint32_t size)
+{
+	if (!dest)
+		return ERROR_FAIL;
+
+	for (unsigned int i = 0; i < size / 4; i++)
+		dm->dbg_ops->queue_reg_read(dm, NARADR_TRAXDATA, &dest[i * 4]);
+	xtensa_dm_queue_tdi_idle(dm);
+	return jtag_execute_queue();
+}
+
+int xtensa_dm_perfmon_enable(struct xtensa_debug_module *dm, int counter_id,
+	const struct xtensa_perfmon_config *config)
+{
+	if (!config)
+		return ERROR_FAIL;
+
+	uint8_t pmstat_buf[4];
+	uint32_t pmctrl = ((config->tracelevel) << 4) +
+		(config->select << 8) +
+		(config->mask << 16) +
+		(config->kernelcnt << 3);
+
+	/* enable performance monitor */
+	dm->dbg_ops->queue_reg_write(dm, NARADR_PMG, 0x1);
+	/* reset counter */
+	dm->dbg_ops->queue_reg_write(dm, NARADR_PM0 + counter_id, 0);
+	dm->dbg_ops->queue_reg_write(dm, NARADR_PMCTRL0 + counter_id, pmctrl);
+	dm->dbg_ops->queue_reg_read(dm, NARADR_PMSTAT0 + counter_id, pmstat_buf);
+	xtensa_dm_queue_tdi_idle(dm);
+	return jtag_execute_queue();
+}
+
+int xtensa_dm_perfmon_dump(struct xtensa_debug_module *dm, int counter_id,
+	struct xtensa_perfmon_result *out_result)
+{
+	uint8_t pmstat_buf[4];
+	uint8_t pmcount_buf[4];
+
+	dm->dbg_ops->queue_reg_read(dm, NARADR_PMSTAT0 + counter_id, pmstat_buf);
+	dm->dbg_ops->queue_reg_read(dm, NARADR_PM0 + counter_id, pmcount_buf);
+	xtensa_dm_queue_tdi_idle(dm);
+	int res = jtag_execute_queue();
+	if (res == ERROR_OK) {
+		uint32_t stat = buf_get_u32(pmstat_buf, 0, 32);
+		uint64_t result = buf_get_u32(pmcount_buf, 0, 32);
+
+		/* TODO: if counter # counter_id+1 has 'select' set to 1, use its value as the
+		* high 32 bits of the counter. */
+		if (out_result) {
+			out_result->overflow = ((stat & 1) != 0);
+			out_result->value = result;
+		}
+	}
+
+	return res;
+}
diff --git a/src/target/xtensa/xtensa_debug_module.h b/src/target/xtensa/xtensa_debug_module.h
new file mode 100644
index 0000000..692f0f6
--- /dev/null
+++ b/src/target/xtensa/xtensa_debug_module.h
@@ -0,0 +1,385 @@
+/***************************************************************************
+ *   Xtensa debug module API                                               *
+ *   Copyright (C) 2019 Espressif Systems Ltd.                             *
+ *   <alexey@espressif.com>                                                *
+ *                                                                         *
+ *   Derived from original ESP8266 target.                                 *
+ *   Copyright (C) 2015 by Angus Gratton                                   *
+ *   gus@projectgus.com                                                    *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+
+#ifndef OPENOCD_TARGET_XTENSA_DEBUG_MODULE_H
+#define OPENOCD_TARGET_XTENSA_DEBUG_MODULE_H
+
+#include <jtag/jtag.h>
+#include <helper/bits.h>
+#include <target/target.h>
+
+/* Virtual IDs for using with xtensa_power_ops API */
+#define DMREG_PWRCTL       0x00
+#define DMREG_PWRSTAT      0x01
+
+/*
+ From the manual:
+ To properly use Debug registers through JTAG, software must ensure that:
+ - Tap is out of reset
+ - Xtensa Debug Module is out of reset
+ - Other bits of PWRCTL are set to their desired values, and finally
+ - JtagDebugUse transitions from 0 to 1
+ The bit must continue to be 1 in order for JTAG accesses to the Debug
+ Module to happen correctly. When it is set, any write to this bit clears it.
+ Either don't access it, or re-write it to 1 so JTAG accesses continue.
+*/
+#define PWRCTL_JTAGDEBUGUSE     BIT(7)
+#define PWRCTL_DEBUGRESET       BIT(6)
+#define PWRCTL_CORERESET        BIT(4)
+#define PWRCTL_DEBUGWAKEUP      BIT(2)
+#define PWRCTL_MEMWAKEUP        BIT(1)
+#define PWRCTL_COREWAKEUP       BIT(0)
+
+#define PWRSTAT_DEBUGWASRESET   BIT(6)
+#define PWRSTAT_COREWASRESET    BIT(4)
+#define PWRSTAT_CORESTILLNEEDED BIT(3)
+#define PWRSTAT_DEBUGDOMAINON   BIT(2)
+#define PWRSTAT_MEMDOMAINON     BIT(1)
+#define PWRSTAT_COREDOMAINON    BIT(0)
+
+/* *** NAR addresses (also used as IDs for debug registers in xtensa_debug_ops API) ***
+ *TRAX registers */
+#define NARADR_TRAXID       0x00
+#define NARADR_TRAXCTRL     0x01
+#define NARADR_TRAXSTAT     0x02
+#define NARADR_TRAXDATA     0x03
+#define NARADR_TRAXADDR     0x04
+#define NARADR_TRIGGERPC    0x05
+#define NARADR_PCMATCHCTRL  0x06
+#define NARADR_DELAYCNT     0x07
+#define NARADR_MEMADDRSTART 0x08
+#define NARADR_MEMADDREND   0x09
+/*Performance monitor registers */
+#define NARADR_PMG          0x20
+#define NARADR_INTPC        0x24
+#define NARADR_PM0          0x28
+/*... */
+#define NARADR_PM7          0x2F
+#define NARADR_PMCTRL0      0x30
+/*... */
+#define NARADR_PMCTRL7      0x37
+#define NARADR_PMSTAT0      0x38
+/*... */
+#define NARADR_PMSTAT7      0x3F
+/*OCD registers */
+#define NARADR_OCDID        0x40
+#define NARADR_DCRCLR       0x42
+#define NARADR_DCRSET       0x43
+#define NARADR_DSR          0x44
+#define NARADR_DDR          0x45
+#define NARADR_DDREXEC      0x46
+#define NARADR_DIR0EXEC     0x47
+#define NARADR_DIR0         0x48
+#define NARADR_DIR1         0x49
+/*... */
+#define NARADR_DIR7         0x4F
+/*Misc registers */
+#define NARADR_PWRCTL       0x58
+#define NARADR_PWRSTAT      0x59
+#define NARADR_ERISTAT      0x5A
+/*CoreSight registers */
+#define NARADR_ITCTRL       0x60
+#define NARADR_CLAIMSET     0x68
+#define NARADR_CLAIMCLR     0x69
+#define NARADR_LOCKACCESS   0x6c
+#define NARADR_LOCKSTATUS   0x6d
+#define NARADR_AUTHSTATUS   0x6e
+#define NARADR_DEVID        0x72
+#define NARADR_DEVTYPE      0x73
+#define NARADR_PERID4       0x74
+/*... */
+#define NARADR_PERID7       0x77
+#define NARADR_PERID0       0x78
+/*... */
+#define NARADR_PERID3       0x7b
+#define NARADR_COMPID0      0x7c
+/*... */
+#define NARADR_COMPID3      0x7f
+#define NARADR_MAX          NARADR_COMPID3
+
+/*OCD registers, bit definitions */
+#define OCDDCR_ENABLEOCD            BIT(0)
+#define OCDDCR_DEBUGINTERRUPT       BIT(1)
+#define OCDDCR_INTERRUPTALLCONDS    BIT(2)
+#define OCDDCR_BREAKINEN            BIT(16)
+#define OCDDCR_BREAKOUTEN           BIT(17)
+#define OCDDCR_DEBUGSWACTIVE        BIT(20)
+#define OCDDCR_RUNSTALLINEN         BIT(21)
+#define OCDDCR_DEBUGMODEOUTEN       BIT(22)
+#define OCDDCR_BREAKOUTITO          BIT(24)
+#define OCDDCR_BREAKACKITO          BIT(25)
+
+#define OCDDSR_EXECDONE             BIT(0)
+#define OCDDSR_EXECEXCEPTION        BIT(1)
+#define OCDDSR_EXECBUSY             BIT(2)
+#define OCDDSR_EXECOVERRUN          BIT(3)
+#define OCDDSR_STOPPED              BIT(4)
+#define OCDDSR_COREWROTEDDR         BIT(10)
+#define OCDDSR_COREREADDDR          BIT(11)
+#define OCDDSR_HOSTWROTEDDR         BIT(14)
+#define OCDDSR_HOSTREADDDR          BIT(15)
+#define OCDDSR_DEBUGPENDBREAK       BIT(16)
+#define OCDDSR_DEBUGPENDHOST        BIT(17)
+#define OCDDSR_DEBUGPENDTRAX        BIT(18)
+#define OCDDSR_DEBUGINTBREAK        BIT(20)
+#define OCDDSR_DEBUGINTHOST         BIT(21)
+#define OCDDSR_DEBUGINTTRAX         BIT(22)
+#define OCDDSR_RUNSTALLTOGGLE       BIT(23)
+#define OCDDSR_RUNSTALLSAMPLE       BIT(24)
+#define OCDDSR_BREACKOUTACKITI      BIT(25)
+#define OCDDSR_BREAKINITI           BIT(26)
+#define OCDDSR_DBGMODPOWERON        BIT(31)
+
+#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 TRAXCTRL_TREN               BIT(0)	/* Trace enable. Tracing starts on 0->1 */
+#define TRAXCTRL_TRSTP              BIT(1)	/* Trace Stop. Make 1 to stop trace. */
+#define TRAXCTRL_PCMEN              BIT(2)	/* PC match enable */
+#define TRAXCTRL_PTIEN              BIT(4)	/* Processor-trigger enable */
+#define TRAXCTRL_CTIEN              BIT(5)	/* Cross-trigger enable */
+#define TRAXCTRL_TMEN               BIT(7)	/* Tracemem Enable. Always set. */
+#define TRAXCTRL_CNTU               BIT(9)	/* Post-stop-trigger countdown units; selects when DelayCount-- happens.
+						 *0 - every 32-bit word written to tracemem, 1 - every cpu instruction */
+#define TRAXCTRL_TSEN               BIT(11)	/* Undocumented/deprecated? */
+#define TRAXCTRL_SMPER_SHIFT        12		/* Send sync every 2^(9-smper) messages. 7=reserved, 0=no sync msg */
+#define TRAXCTRL_SMPER_MASK         0x07	/* Synchronization message period */
+#define TRAXCTRL_PTOWT              BIT(16)	/* Processor Trigger Out (OCD halt) enabled when stop triggered */
+#define TRAXCTRL_PTOWS              BIT(17)	/* Processor Trigger Out (OCD halt) enabled when trace stop completes */
+#define TRAXCTRL_CTOWT              BIT(20)	/* Cross-trigger Out enabled when stop triggered */
+#define TRAXCTRL_CTOWS              BIT(21)	/* Cross-trigger Out enabled when trace stop completes */
+#define TRAXCTRL_ITCTO              BIT(22)	/* Integration mode: cross-trigger output */
+#define TRAXCTRL_ITCTIA             BIT(23)	/* Integration mode: cross-trigger ack */
+#define TRAXCTRL_ITATV              BIT(24)	/* replaces ATID when in integration mode: ATVALID output */
+#define TRAXCTRL_ATID_MASK          0x7F	/* ARB source ID */
+#define TRAXCTRL_ATID_SHIFT         24
+#define TRAXCTRL_ATEN               BIT(31)	/* ATB interface enable */
+
+#define TRAXSTAT_TRACT              BIT(0)	/* Trace active flag. */
+#define TRAXSTAT_TRIG               BIT(1)	/* Trace stop trigger. Clears on TREN 1->0 */
+#define TRAXSTAT_PCMTG              BIT(2)	/* Stop trigger caused by PC match. Clears on TREN 1->0 */
+#define TRAXSTAT_PJTR               BIT(3)	/* JTAG transaction result. 1=err in preceding jtag transaction. */
+#define TRAXSTAT_PTITG              BIT(4)	/* Stop trigger caused by Processor Trigger Input.Clears on TREN 1->0 */
+#define TRAXSTAT_CTITG              BIT(5)	/* Stop trigger caused by Cross-Trigger Input. Clears on TREN 1->0 */
+#define TRAXSTAT_MEMSZ_SHIFT        8		/* Traceram size inducator. Usable trace ram is 2^MEMSZ bytes. */
+#define TRAXSTAT_MEMSZ_MASK         0x1F
+#define TRAXSTAT_PTO                BIT(16)	/* Processor Trigger Output: current value */
+#define TRAXSTAT_CTO                BIT(17)	/* Cross-Trigger Output: current value */
+#define TRAXSTAT_ITCTOA             BIT(22)	/* Cross-Trigger Out Ack: current value */
+#define TRAXSTAT_ITCTI              BIT(23)	/* Cross-Trigger Input: current value */
+#define TRAXSTAT_ITATR              BIT(24)	/* ATREADY Input: current value */
+
+#define TRAXADDR_TADDR_SHIFT        0		/* Trax memory address, in 32-bit words. */
+#define TRAXADDR_TADDR_MASK         0x1FFFFF	/* Actually is only as big as the trace buffer size max addr. */
+#define TRAXADDR_TWRAP_SHIFT        21		/* Amount of times TADDR has overflown */
+#define TRAXADDR_TWRAP_MASK         0x3FF
+#define TRAXADDR_TWSAT              BIT(31)	/* 1 if TWRAP has overflown, clear by disabling tren.*/
+
+#define PCMATCHCTRL_PCML_SHIFT      0		/* Amount of lower bits to ignore in pc trigger register */
+#define PCMATCHCTRL_PCML_MASK       0x1F
+#define PCMATCHCTRL_PCMS            BIT(31)	/* PC Match Sense, 0-match when procs PC is in-range, 1-match when
+						 *out-of-range */
+
+#define XTENSA_MAX_PERF_COUNTERS    2
+#define XTENSA_MAX_PERF_SELECT      32
+#define XTENSA_MAX_PERF_MASK        0xffff
+
+#define XTENSA_STOPMASK_DISABLED    UINT32_MAX
+
+struct xtensa_debug_module;
+
+struct xtensa_debug_ops {
+	/** enable operation */
+	int (*queue_enable)(struct xtensa_debug_module *dm);
+	/** register read. */
+	int (*queue_reg_read)(struct xtensa_debug_module *dm, unsigned int reg, uint8_t *data);
+	/** register write. */
+	int (*queue_reg_write)(struct xtensa_debug_module *dm, unsigned int reg, uint32_t data);
+};
+
+struct xtensa_power_ops {
+	/** register read. */
+	int (*queue_reg_read)(struct xtensa_debug_module *dm, unsigned int reg, uint8_t *data,
+		uint8_t clear);
+	/** register write. */
+	int (*queue_reg_write)(struct xtensa_debug_module *dm, unsigned int reg, uint8_t data);
+};
+
+typedef uint8_t xtensa_pwrstat_t;
+typedef uint32_t xtensa_ocdid_t;
+typedef uint32_t xtensa_dsr_t;
+typedef uint32_t xtensa_traxstat_t;
+
+struct xtensa_power_status {
+	xtensa_pwrstat_t stat;
+	xtensa_pwrstat_t stath;
+	/* TODO: do not need to keep previous status to detect that core or debug module has been
+	 * reset, */
+	/*       we can clear PWRSTAT_DEBUGWASRESET and PWRSTAT_COREWASRESET after reading will do
+	 * the job; */
+	/*       upon next reet those bits will be set again. So we can get rid of
+	 *       xtensa_dm_power_status_cache_reset() and xtensa_dm_power_status_cache(). */
+	xtensa_pwrstat_t prev_stat;
+};
+
+struct xtensa_core_status {
+	xtensa_dsr_t dsr;
+};
+
+struct xtensa_trace_config {
+	uint32_t ctrl;
+	uint32_t memaddr_start;
+	uint32_t memaddr_end;
+	uint32_t addr;
+};
+
+struct xtensa_trace_status {
+	xtensa_traxstat_t stat;
+};
+
+struct xtensa_trace_start_config {
+	uint32_t stoppc;
+	bool after_is_words;
+	uint32_t after;
+	uint32_t stopmask;	/* UINT32_MAX: disable PC match option */
+};
+
+struct xtensa_perfmon_config {
+	int select;
+	uint32_t mask;
+	int kernelcnt;
+	int tracelevel;
+};
+
+struct xtensa_perfmon_result {
+	uint64_t value;
+	bool overflow;
+};
+
+struct xtensa_debug_module_config {
+	const struct xtensa_power_ops *pwr_ops;
+	const struct xtensa_debug_ops *dbg_ops;
+	struct jtag_tap *tap;
+	void (*queue_tdi_idle)(struct target *target);
+	void *queue_tdi_idle_arg;
+};
+
+struct xtensa_debug_module {
+	const struct xtensa_power_ops *pwr_ops;
+	const struct xtensa_debug_ops *dbg_ops;
+	struct jtag_tap *tap;
+	void (*queue_tdi_idle)(struct target *target);
+	void *queue_tdi_idle_arg;
+
+	struct xtensa_power_status power_status;
+	struct xtensa_core_status core_status;
+	xtensa_ocdid_t device_id;
+};
+
+int xtensa_dm_init(struct xtensa_debug_module *dm, const struct xtensa_debug_module_config *cfg);
+int xtensa_dm_queue_enable(struct xtensa_debug_module *dm);
+int xtensa_dm_queue_reg_read(struct xtensa_debug_module *dm, unsigned int reg, uint8_t *value);
+int xtensa_dm_queue_reg_write(struct xtensa_debug_module *dm, unsigned int reg, uint32_t value);
+int xtensa_dm_queue_pwr_reg_read(struct xtensa_debug_module *dm, unsigned int reg, uint8_t *data, uint8_t clear);
+int xtensa_dm_queue_pwr_reg_write(struct xtensa_debug_module *dm, unsigned int reg, uint8_t data);
+
+static inline void xtensa_dm_queue_tdi_idle(struct xtensa_debug_module *dm)
+{
+	if (dm->queue_tdi_idle)
+		dm->queue_tdi_idle(dm->queue_tdi_idle_arg);
+}
+
+int xtensa_dm_power_status_read(struct xtensa_debug_module *dm, uint32_t clear);
+static inline void xtensa_dm_power_status_cache_reset(struct xtensa_debug_module *dm)
+{
+	dm->power_status.prev_stat = 0;
+}
+static inline void xtensa_dm_power_status_cache(struct xtensa_debug_module *dm)
+{
+	dm->power_status.prev_stat = dm->power_status.stath;
+}
+static inline xtensa_pwrstat_t xtensa_dm_power_status_get(struct xtensa_debug_module *dm)
+{
+	return dm->power_status.stat;
+}
+
+int xtensa_dm_core_status_read(struct xtensa_debug_module *dm);
+int xtensa_dm_core_status_clear(struct xtensa_debug_module *dm, xtensa_dsr_t bits);
+int xtensa_dm_core_status_check(struct xtensa_debug_module *dm);
+static inline xtensa_dsr_t xtensa_dm_core_status_get(struct xtensa_debug_module *dm)
+{
+	return dm->core_status.dsr;
+}
+
+int xtensa_dm_device_id_read(struct xtensa_debug_module *dm);
+static inline xtensa_ocdid_t xtensa_dm_device_id_get(struct xtensa_debug_module *dm)
+{
+	return dm->device_id;
+}
+
+int xtensa_dm_trace_start(struct xtensa_debug_module *dm, struct xtensa_trace_start_config *cfg);
+int xtensa_dm_trace_stop(struct xtensa_debug_module *dm, bool pto_enable);
+int xtensa_dm_trace_config_read(struct xtensa_debug_module *dm, struct xtensa_trace_config *config);
+int xtensa_dm_trace_status_read(struct xtensa_debug_module *dm, struct xtensa_trace_status *status);
+int xtensa_dm_trace_data_read(struct xtensa_debug_module *dm, uint8_t *dest, uint32_t size);
+
+static inline bool xtensa_dm_is_online(struct xtensa_debug_module *dm)
+{
+	int res = xtensa_dm_device_id_read(dm);
+	if (res != ERROR_OK)
+		return false;
+	return (dm->device_id != 0xffffffff && dm->device_id != 0);
+}
+
+static inline bool xtensa_dm_tap_was_reset(struct xtensa_debug_module *dm)
+{
+	return !(dm->power_status.prev_stat & PWRSTAT_DEBUGWASRESET) &&
+	       dm->power_status.stat & PWRSTAT_DEBUGWASRESET;
+}
+
+static inline bool xtensa_dm_core_was_reset(struct xtensa_debug_module *dm)
+{
+	return !(dm->power_status.prev_stat & PWRSTAT_COREWASRESET) &&
+	       dm->power_status.stat & PWRSTAT_COREWASRESET;
+}
+
+static inline bool xtensa_dm_core_is_stalled(struct xtensa_debug_module *dm)
+{
+	return dm->core_status.dsr & OCDDSR_RUNSTALLSAMPLE;
+}
+
+static inline bool xtensa_dm_is_powered(struct xtensa_debug_module *dm)
+{
+	return dm->core_status.dsr & OCDDSR_DBGMODPOWERON;
+}
+
+int xtensa_dm_perfmon_enable(struct xtensa_debug_module *dm, int counter_id,
+	const struct xtensa_perfmon_config *config);
+int xtensa_dm_perfmon_dump(struct xtensa_debug_module *dm, int counter_id,
+	struct xtensa_perfmon_result *out_result);
+
+#endif	/* OPENOCD_TARGET_XTENSA_DEBUG_MODULE_H */
diff --git a/src/target/xtensa/xtensa_regs.h b/src/target/xtensa/xtensa_regs.h
new file mode 100644
index 0000000..7602131
--- /dev/null
+++ b/src/target/xtensa/xtensa_regs.h
@@ -0,0 +1,278 @@
+/***************************************************************************
+ *   Generic Xtensa target API for OpenOCD                                 *
+ *   Copyright (C) 2016-2019 Espressif Systems Ltd.                        *
+ *   Author: Angus Gratton gus@projectgus.com                              *
+ *   Author: Jeroen Domburg <jeroen@espressif.com>                         *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+#ifndef OPENOCD_TARGET_XTENSA_REGS_H
+#define OPENOCD_TARGET_XTENSA_REGS_H
+
+struct reg_arch_type;
+
+enum xtensa_reg_id {
+	XT_REG_IDX_PC = 0,
+	XT_REG_IDX_AR0,
+	XT_REG_IDX_AR1,
+	XT_REG_IDX_AR2,
+	XT_REG_IDX_AR3,
+	XT_REG_IDX_AR4,
+	XT_REG_IDX_AR5,
+	XT_REG_IDX_AR6,
+	XT_REG_IDX_AR7,
+	XT_REG_IDX_AR8,
+	XT_REG_IDX_AR9,
+	XT_REG_IDX_AR10,
+	XT_REG_IDX_AR11,
+	XT_REG_IDX_AR12,
+	XT_REG_IDX_AR13,
+	XT_REG_IDX_AR14,
+	XT_REG_IDX_AR15,
+	XT_REG_IDX_AR16,
+	XT_REG_IDX_AR17,
+	XT_REG_IDX_AR18,
+	XT_REG_IDX_AR19,
+	XT_REG_IDX_AR20,
+	XT_REG_IDX_AR21,
+	XT_REG_IDX_AR22,
+	XT_REG_IDX_AR23,
+	XT_REG_IDX_AR24,
+	XT_REG_IDX_AR25,
+	XT_REG_IDX_AR26,
+	XT_REG_IDX_AR27,
+	XT_REG_IDX_AR28,
+	XT_REG_IDX_AR29,
+	XT_REG_IDX_AR30,
+	XT_REG_IDX_AR31,
+	XT_REG_IDX_AR32,
+	XT_REG_IDX_AR33,
+	XT_REG_IDX_AR34,
+	XT_REG_IDX_AR35,
+	XT_REG_IDX_AR36,
+	XT_REG_IDX_AR37,
+	XT_REG_IDX_AR38,
+	XT_REG_IDX_AR39,
+	XT_REG_IDX_AR40,
+	XT_REG_IDX_AR41,
+	XT_REG_IDX_AR42,
+	XT_REG_IDX_AR43,
+	XT_REG_IDX_AR44,
+	XT_REG_IDX_AR45,
+	XT_REG_IDX_AR46,
+	XT_REG_IDX_AR47,
+	XT_REG_IDX_AR48,
+	XT_REG_IDX_AR49,
+	XT_REG_IDX_AR50,
+	XT_REG_IDX_AR51,
+	XT_REG_IDX_AR52,
+	XT_REG_IDX_AR53,
+	XT_REG_IDX_AR54,
+	XT_REG_IDX_AR55,
+	XT_REG_IDX_AR56,
+	XT_REG_IDX_AR57,
+	XT_REG_IDX_AR58,
+	XT_REG_IDX_AR59,
+	XT_REG_IDX_AR60,
+	XT_REG_IDX_AR61,
+	XT_REG_IDX_AR62,
+	XT_REG_IDX_AR63,
+	XT_REG_IDX_LBEG,
+	XT_REG_IDX_LEND,
+	XT_REG_IDX_LCOUNT,
+	XT_REG_IDX_SAR,
+	XT_REG_IDX_WINDOWBASE,
+	XT_REG_IDX_WINDOWSTART,
+	XT_REG_IDX_CONFIGID0,
+	XT_REG_IDX_CONFIGID1,
+	XT_REG_IDX_PS,
+	XT_REG_IDX_THREADPTR,
+	XT_REG_IDX_BR,
+	XT_REG_IDX_SCOMPARE1,
+	XT_REG_IDX_ACCLO,
+	XT_REG_IDX_ACCHI,
+	XT_REG_IDX_M0,
+	XT_REG_IDX_M1,
+	XT_REG_IDX_M2,
+	XT_REG_IDX_M3,
+	XT_REG_IDX_F0,
+	XT_REG_IDX_F1,
+	XT_REG_IDX_F2,
+	XT_REG_IDX_F3,
+	XT_REG_IDX_F4,
+	XT_REG_IDX_F5,
+	XT_REG_IDX_F6,
+	XT_REG_IDX_F7,
+	XT_REG_IDX_F8,
+	XT_REG_IDX_F9,
+	XT_REG_IDX_F10,
+	XT_REG_IDX_F11,
+	XT_REG_IDX_F12,
+	XT_REG_IDX_F13,
+	XT_REG_IDX_F14,
+	XT_REG_IDX_F15,
+	XT_REG_IDX_FCR,
+	XT_REG_IDX_FSR,
+	XT_REG_IDX_MMID,
+	XT_REG_IDX_IBREAKENABLE,
+	XT_REG_IDX_MEMCTL,
+	XT_REG_IDX_ATOMCTL,
+	XT_REG_IDX_IBREAKA0,
+	XT_REG_IDX_IBREAKA1,
+	XT_REG_IDX_DBREAKA0,
+	XT_REG_IDX_DBREAKA1,
+	XT_REG_IDX_DBREAKC0,
+	XT_REG_IDX_DBREAKC1,
+	XT_REG_IDX_EPC1,
+	XT_REG_IDX_EPC2,
+	XT_REG_IDX_EPC3,
+	XT_REG_IDX_EPC4,
+	XT_REG_IDX_EPC5,
+	XT_REG_IDX_EPC6,
+	XT_REG_IDX_EPC7,
+	XT_REG_IDX_DEPC,
+	XT_REG_IDX_EPS2,
+	XT_REG_IDX_EPS3,
+	XT_REG_IDX_EPS4,
+	XT_REG_IDX_EPS5,
+	XT_REG_IDX_EPS6,
+	XT_REG_IDX_EPS7,
+	XT_REG_IDX_EXCSAVE1,
+	XT_REG_IDX_EXCSAVE2,
+	XT_REG_IDX_EXCSAVE3,
+	XT_REG_IDX_EXCSAVE4,
+	XT_REG_IDX_EXCSAVE5,
+	XT_REG_IDX_EXCSAVE6,
+	XT_REG_IDX_EXCSAVE7,
+	XT_REG_IDX_CPENABLE,
+	XT_REG_IDX_INTERRUPT,
+	XT_REG_IDX_INTSET,
+	XT_REG_IDX_INTCLEAR,
+	XT_REG_IDX_INTENABLE,
+	XT_REG_IDX_VECBASE,
+	XT_REG_IDX_EXCCAUSE,
+	XT_REG_IDX_DEBUGCAUSE,
+	XT_REG_IDX_CCOUNT,
+	XT_REG_IDX_PRID,
+	XT_REG_IDX_ICOUNT,
+	XT_REG_IDX_ICOUNTLEVEL,
+	XT_REG_IDX_EXCVADDR,
+	XT_REG_IDX_CCOMPARE0,
+	XT_REG_IDX_CCOMPARE1,
+	XT_REG_IDX_CCOMPARE2,
+	XT_REG_IDX_MISC0,
+	XT_REG_IDX_MISC1,
+	XT_REG_IDX_MISC2,
+	XT_REG_IDX_MISC3,
+	XT_REG_IDX_LITBASE,
+	XT_REG_IDX_PTEVADDR,
+	XT_REG_IDX_RASID,
+	XT_REG_IDX_ITLBCFG,
+	XT_REG_IDX_DTLBCFG,
+	XT_REG_IDX_MEPC,
+	XT_REG_IDX_MEPS,
+	XT_REG_IDX_MESAVE,
+	XT_REG_IDX_MESR,
+	XT_REG_IDX_MECR,
+	XT_REG_IDX_MEVADDR,
+	XT_REG_IDX_A0,
+	XT_REG_IDX_A1,
+	XT_REG_IDX_A2,
+	XT_REG_IDX_A3,
+	XT_REG_IDX_A4,
+	XT_REG_IDX_A5,
+	XT_REG_IDX_A6,
+	XT_REG_IDX_A7,
+	XT_REG_IDX_A8,
+	XT_REG_IDX_A9,
+	XT_REG_IDX_A10,
+	XT_REG_IDX_A11,
+	XT_REG_IDX_A12,
+	XT_REG_IDX_A13,
+	XT_REG_IDX_A14,
+	XT_REG_IDX_A15,
+	XT_REG_IDX_PWRCTL,
+	XT_REG_IDX_PWRSTAT,
+	XT_REG_IDX_ERISTAT,
+	XT_REG_IDX_CS_ITCTRL,
+	XT_REG_IDX_CS_CLAIMSET,
+	XT_REG_IDX_CS_CLAIMCLR,
+	XT_REG_IDX_CS_LOCKACCESS,
+	XT_REG_IDX_CS_LOCKSTATUS,
+	XT_REG_IDX_CS_AUTHSTATUS,
+	XT_REG_IDX_FAULT_INFO,
+	XT_REG_IDX_TRAX_ID,
+	XT_REG_IDX_TRAX_CTRL,
+	XT_REG_IDX_TRAX_STAT,
+	XT_REG_IDX_TRAX_DATA,
+	XT_REG_IDX_TRAX_ADDR,
+	XT_REG_IDX_TRAX_PCTRIGGER,
+	XT_REG_IDX_TRAX_PCMATCH,
+	XT_REG_IDX_TRAX_DELAY,
+	XT_REG_IDX_TRAX_MEMSTART,
+	XT_REG_IDX_TRAX_MEMEND,
+	XT_REG_IDX_PMG,
+	XT_REG_IDX_PMPC,
+	XT_REG_IDX_PM0,
+	XT_REG_IDX_PM1,
+	XT_REG_IDX_PMCTRL0,
+	XT_REG_IDX_PMCTRL1,
+	XT_REG_IDX_PMSTAT0,
+	XT_REG_IDX_PMSTAT1,
+	XT_REG_IDX_OCD_ID,
+	XT_REG_IDX_OCD_DCRCLR,
+	XT_REG_IDX_OCD_DCRSET,
+	XT_REG_IDX_OCD_DSR,
+	XT_REG_IDX_OCD_DDR,
+	XT_NUM_REGS,
+	/* chip-specific user registers go after ISA-defined ones */
+	XT_USR_REG_START = XT_NUM_REGS
+};
+
+typedef uint32_t xtensa_reg_val_t;
+
+enum xtensa_reg_type {
+	XT_REG_GENERAL = 0,		/* General-purpose register; part of the windowed register set */
+	XT_REG_USER = 1,		/* User register, needs RUR to read */
+	XT_REG_SPECIAL = 2,		/* Special register, needs RSR to read */
+	XT_REG_DEBUG = 3,		/* Register used for the debug interface. Don't mess with this. */
+	XT_REG_RELGEN = 4,		/* Relative general address. Points to the absolute addresses plus the window
+					 *index */
+	XT_REG_FR = 5,			/* Floating-point register */
+};
+
+enum xtensa_reg_flags {
+	XT_REGF_NOREAD = 0x01,	/* Register is write-only */
+	XT_REGF_COPROC0 = 0x02	/* Can't be read if coproc0 isn't enabled */
+};
+
+struct xtensa_reg_desc {
+	const char *name;
+	unsigned int reg_num;			/* ISA register num (meaning depends on register type) */
+	enum xtensa_reg_type type;
+	enum xtensa_reg_flags flags;
+};
+
+struct xtensa_user_reg_desc {
+	const char *name;
+	/* ISA register num (meaning depends on register type) */
+	unsigned int reg_num;
+	enum xtensa_reg_flags flags;
+	uint32_t size;
+	const struct reg_arch_type *type;
+};
+
+extern const struct xtensa_reg_desc xtensa_regs[XT_NUM_REGS];
+
+#endif	/* OPENOCD_TARGET_XTENSA_REGS_H */
diff --git a/tcl/board/esp32s2-kaluga-1.cfg b/tcl/board/esp32s2-kaluga-1.cfg
new file mode 100644
index 0000000..783ea21
--- /dev/null
+++ b/tcl/board/esp32s2-kaluga-1.cfg
@@ -0,0 +1,18 @@
+# SPDX-License-Identifier: GPL-2.0-or-later
+#
+# Example OpenOCD configuration file for ESP32-S2 Kaluga board.
+#
+# For example, OpenOCD can be started for ESP32-S2 debugging on
+#
+#   openocd -f board/esp32s2-kaluga-1.cfg
+#
+
+source [find interface/ftdi/esp32s2_kaluga_v1.cfg]
+source [find target/esp32s2.cfg]
+
+# The speed of the JTAG interface, in kHz. If you get DSR/DIR errors (and they
+# do not relate to OpenOCD trying to read from a memory range without physical
+# memory being present there), you can try lowering this.
+# On ESP32-S2, this can go as high as 20MHz if CPU frequency is 80MHz, or 26MHz
+# if CPU frequency is 160MHz or 240MHz.
+adapter speed 20000
diff --git a/tcl/board/kontron_sl28.cfg b/tcl/board/kontron_sl28.cfg
new file mode 100644
index 0000000..9816f38
--- /dev/null
+++ b/tcl/board/kontron_sl28.cfg
@@ -0,0 +1,14 @@
+# SPDX-License-Identifier: GPL-2.0-or-later
+# Kontron SMARC-sAL28
+
+transport select jtag
+reset_config srst_only srst_nogate
+
+jtag newtap unknown0 tap -irlen 12
+
+set _CPUS 2
+source [find target/ls1028a.cfg]
+
+source [find tcl/cpld/altera-epm240.cfg]
+
+adapter speed 2000
diff --git a/tcl/board/steval-idb012v1.cfg b/tcl/board/steval-idb012v1.cfg
new file mode 100644
index 0000000..25efc58
--- /dev/null
+++ b/tcl/board/steval-idb012v1.cfg
@@ -0,0 +1,5 @@
+# SPDX-License-Identifier: GPL-2.0-or-later.
+# This is an evaluation board with a single BlueNRG-LPS chip.
+set CHIPNAME bluenrg-lps
+source [find interface/cmsis-dap.cfg]
+source [find target/bluenrg-x.cfg]
\ No newline at end of file
diff --git a/tcl/interface/beaglebone-jtag-native.cfg b/tcl/interface/beaglebone-jtag-native.cfg
new file mode 100644
index 0000000..cd32ca4
--- /dev/null
+++ b/tcl/interface/beaglebone-jtag-native.cfg
@@ -0,0 +1,28 @@
+# SPDX-License-Identifier: GPL-2.0-or-later
+# BeagleBone native GPIO interface for JTAG
+#
+# This is best used with a fast buffer but it is also suitable for a direct
+# connection if the target voltage matches the host's IO voltage (typically
+# 3.3V) and the cable is short.
+#
+# DO NOT APPLY VOLTAGE TO THE GPIO PINS UNTIL SYS_RESETN IS HIGH.
+#
+# Do not forget the GND connection.
+
+adapter driver am335xgpio
+
+# Transition delay calculation: SPEED_COEFF/khz - SPEED_OFFSET
+# These depend on the system clock, calibrated for stock 1 GHz BeagleBoneBlack
+# am335xgpio speed SPEED_COEFF SPEED_OFFSET
+am335xgpio speed_coeffs 600000 575
+
+am335xgpio tdo_num 20
+am335xgpio tdi_num 60
+am335xgpio tms_num 4
+am335xgpio tck_num 2
+
+am335xgpio led_num 51
+am335xgpio led_on_state on
+
+am335xgpio srst_num 65
+reset_config srst_only srst_push_pull
diff --git a/tcl/interface/beaglebone-swd-native.cfg b/tcl/interface/beaglebone-swd-native.cfg
new file mode 100644
index 0000000..f7bff6e
--- /dev/null
+++ b/tcl/interface/beaglebone-swd-native.cfg
@@ -0,0 +1,29 @@
+# SPDX-License-Identifier: GPL-2.0-or-later
+# BeagleBone native GPIO interface for SWD
+#
+# This is best used with a fast buffer but it is also suitable for a direct
+# connection if the target voltage matches the host's IO voltage (typically
+# 3.3V) and the cable is short.
+#
+# DO NOT APPLY VOLTAGE TO THE GPIO PINS UNTIL SYS_RESETN IS HIGH.
+#
+# Do not forget the GND connection.
+
+adapter driver am335xgpio
+
+# Transition delay calculation: SPEED_COEFF/khz - SPEED_OFFSET
+# These depend on the system clock, calibrated for stock 1 GHz BeagleBoneBlack
+# am335xgpio speed SPEED_COEFF SPEED_OFFSET
+am335xgpio speed_coeffs 600000 575
+
+am335xgpio swclk_num 2
+am335xgpio swdio_num 4
+am335xgpio swdio_dir_num 60
+am335xgpio swdio_dir_output_state on
+
+# USR0 LED
+am335xgpio led_num 53
+am335xgpio led_on_state on
+
+am335xgpio srst_num 65
+reset_config srst_only srst_push_pull
diff --git a/tcl/interface/ftdi/esp32s2_kaluga_v1.cfg b/tcl/interface/ftdi/esp32s2_kaluga_v1.cfg
new file mode 100644
index 0000000..1880bcb
--- /dev/null
+++ b/tcl/interface/ftdi/esp32s2_kaluga_v1.cfg
@@ -0,0 +1,29 @@
+# SPDX-License-Identifier: GPL-2.0-or-later
+#
+# Driver for the FT2232H JTAG chip on the Espressif Kaluga-1 ESP32-S2 board
+# (and most other FT2232H and FT232H based boards)
+#
+# JTAG DIP switch (labelled SW5 in the schematic) should be "ON" for lines
+# labelled TCK, TDO, TDI and TWS, to connect the FT2232H to the ESP32-S2.
+#
+
+adapter driver ftdi
+ftdi vid_pid 0x0403 0x6010 0x0403 0x6014
+
+# interface 1 is the uart
+ftdi channel 0
+
+# TCK, TDI, TDO, TMS: ADBUS0-3
+# TRST/SRST: ADBUS5 (unused for now)
+# LEDs: ACBUS3-4 (inverted)
+
+ftdi layout_init 0x0008 0x180b
+ftdi layout_signal LED -ndata 0x0800
+ftdi layout_signal LED2 -ndata 0x1000
+
+# ESP32* series chips do not have a TRST input, and the SRST line is connected
+# to the EN pin.
+# The target code doesn't handle SRST reset properly yet, so this is
+# commented out:
+# ftdi layout_signal nSRST -oe 0x0020
+# reset_config srst_only
diff --git a/tcl/interface/ftdi/tigard.cfg b/tcl/interface/ftdi/tigard.cfg
new file mode 100644
index 0000000..43ce0ad
--- /dev/null
+++ b/tcl/interface/ftdi/tigard.cfg
@@ -0,0 +1,19 @@
+# SPDX-License-Identifier: GPL-2.0-or-later
+#
+# Tigard: An FTDI FT2232H-based multi-protocol tool for hardware hacking.
+# https://github.com/tigard-tools/tigard
+
+adapter driver ftdi
+
+ftdi device_desc "Tigard V1.1"
+ftdi vid_pid 0x0403 0x6010
+
+ftdi channel 1
+
+ftdi layout_init 0x0038 0x003b
+ftdi layout_signal nTRST -data 0x0010
+ftdi layout_signal nSRST -data 0x0020
+
+# This board doesn't support open-drain reset modes since its output buffer is
+# always enabled.
+reset_config srst_push_pull trst_push_pull
diff --git a/tcl/target/bluenrg-x.cfg b/tcl/target/bluenrg-x.cfg
index ea94be9..1eba376 100644
--- a/tcl/target/bluenrg-x.cfg
+++ b/tcl/target/bluenrg-x.cfg
@@ -47,11 +47,14 @@ if {![using_hla]} {
    cortex_m reset_config sysresetreq
 }
 
+set JTAG_IDCODE_B2 0x0200A041
+set JTAG_IDCODE_B1 0x0
+
 $_TARGETNAME configure -event halted {
     global WDOG_VALUE
     global WDOG_VALUE_SET
     set _JTAG_IDCODE [mrw 0x40000004]
-    if {$_JTAG_IDCODE != 0x0201E041} {
+    if {$_JTAG_IDCODE == $JTAG_IDCODE_B2 || $_JTAG_IDCODE == $JTAG_IDCODE_B1} {
         # Stop watchdog during halt, if enabled. Only Bluenrg-1/2
         set WDOG_VALUE [mrw 0x40700008]
         if [expr {$WDOG_VALUE & (1 << 1)}] {
@@ -64,7 +67,7 @@ $_TARGETNAME configure -event resumed {
     global WDOG_VALUE
     global WDOG_VALUE_SET
     set _JTAG_IDCODE [mrw 0x40000004]
-    if {$_JTAG_IDCODE != 0x0201E041} {
+    if {$_JTAG_IDCODE == $JTAG_IDCODE_B2 || $_JTAG_IDCODE == $JTAG_IDCODE_B1} {
         if {$WDOG_VALUE_SET} {
             # Restore watchdog enable value after resume. Only Bluenrg-1/2
             mww 0x40700008 $WDOG_VALUE
diff --git a/tcl/target/esp32s2.cfg b/tcl/target/esp32s2.cfg
new file mode 100644
index 0000000..ab64c31
--- /dev/null
+++ b/tcl/target/esp32s2.cfg
@@ -0,0 +1,30 @@
+# SPDX-License-Identifier: GPL-2.0-or-later
+#
+# The ESP32-S2 only supports JTAG.
+transport select jtag
+
+if { [info exists CHIPNAME] } {
+	set _CHIPNAME $CHIPNAME
+} else {
+	set _CHIPNAME esp32s2
+}
+
+if { [info exists CPUTAPID] } {
+	set _CPUTAPID $CPUTAPID
+} else {
+	set _CPUTAPID 0x120034e5
+}
+
+set _TARGETNAME $_CHIPNAME
+set _CPUNAME cpu
+set _TAPNAME $_CHIPNAME.$_CPUNAME
+
+jtag newtap $_CHIPNAME $_CPUNAME -irlen 5 -expected-id $_CPUTAPID
+
+target create $_TARGETNAME esp32s2 -endian little -chain-position $_TAPNAME
+
+xtensa maskisr on
+
+$_TARGETNAME configure -event reset-assert-post { soft_reset_halt }
+
+gdb_breakpoint_override hard
diff --git a/tcl/target/ls1028a.cfg b/tcl/target/ls1028a.cfg
new file mode 100644
index 0000000..463ec7d
--- /dev/null
+++ b/tcl/target/ls1028a.cfg
@@ -0,0 +1,18 @@
+# SPDX-License-Identifier: GPL-2.0-or-later
+# NXP LS1028A
+
+if { [info exists CHIPNAME] } {
+	set _CHIPNAME $CHIPNAME
+} else {
+	set _CHIPNAME ls1028a
+}
+
+if { [info exists DAP_TAPID] } {
+	set _DAP_TAPID $DAP_TAPID
+} else {
+	set _DAP_TAPID 0x6ba00477
+}
+
+set _CPUS 2
+
+source [find target/lsch3_common.cfg]
diff --git a/tcl/target/ls1088a.cfg b/tcl/target/ls1088a.cfg
index f9ae9a1..193d6dd 100644
--- a/tcl/target/ls1088a.cfg
+++ b/tcl/target/ls1088a.cfg
@@ -13,62 +13,9 @@ if { [info exists DAP_TAPID] } {
 	set _DAP_TAPID 0x5ba00477
 }
 
-jtag newtap $_CHIPNAME dap -irlen 4 -expected-id $_DAP_TAPID
-dap create $_CHIPNAME.dap -chain-position $_CHIPNAME.dap
+set _CPUS 8
 
-target create $_CHIPNAME.axi mem_ap -dap $_CHIPNAME.dap -ap-num 1
-
-set _CPU_BASE 0x81000000
-set _CPU_STRIDE 0x100000
-set _CPU_DBGOFF 0x10000
-set _CPU_CTIOFF 0x20000
-
-set _TARGETS {}
-for {set i 0} {$i < 8} {incr i} {
-	set _BASE [expr {$_CPU_BASE + $_CPU_STRIDE * $i}]
-	cti create $_CHIPNAME.cti$i -dap $_CHIPNAME.dap -ap-num 0 \
-		-baseaddr [expr {$_BASE + $_CPU_CTIOFF}]
-	target create $_CHIPNAME.cpu$i aarch64 -dap $_CHIPNAME.dap \
-		-cti $_CHIPNAME.cti$i -dbgbase [expr {$_BASE + $_CPU_DBGOFF}] \
-		{*}[expr {$i ? "-coreid $i" : "-rtos hwthread" }]
-	lappend _TARGETS $_CHIPNAME.cpu$i
-}
-
-target smp {*}$_TARGETS
-
-# Service processor
-target create $_CHIPNAME.sp cortex_a -dap $_CHIPNAME.dap -ap-num 0 -dbgbase 0x80138000
-
-# Normally you will not need to call this, but if you are using the hard-coded
-# Reset Configuration Word (RCW) you will need to call this manually. The CPU's
-# reset vector is 0, and the boot ROM at that location contains ARMv7-A 32-bit
-# instructions. This will cause the CPU to almost immediately execute an
-# illegal instruction.
-#
-# This code is idempotent; releasing a released CPU has no effect, although it
-# will halt/resume the service processor.
-add_help_text release_cpu "Release a cpu which is held off"
-proc release_cpu {cpu} {
-	set RST_BRRL 0x1e60060
-
-	set old [target current]
-	targets $::_CHIPNAME.sp
-	set not_halted [string compare halted [$::_CHIPNAME.sp curstate]]
-	if {$not_halted} {
-		halt
-	}
-
-	# Release the cpu; it will start executing something bogus
-	mem2array regs 32 $RST_BRRL 1
-	mww $RST_BRRL [expr {$regs(0) | 1 << $cpu}]
-
-	if {$not_halted} {
-		resume
-	}
-	targets $old
-}
-
-targets $_CHIPNAME.cpu0
+source [find target/lsch3_common.cfg]
 
 # Seems to work OK in testing
 adapter speed 10000
diff --git a/tcl/target/lsch3_common.cfg b/tcl/target/lsch3_common.cfg
new file mode 100644
index 0000000..f48d59b
--- /dev/null
+++ b/tcl/target/lsch3_common.cfg
@@ -0,0 +1,63 @@
+# SPDX-License-Identifier: GPL-2.0-or-later
+# This contains common configuration for NXP Layerscape chassis generation 3
+
+if { ![info exists _CPUS] } {
+	error "_CPUS must be set to the number of cores"
+}
+
+jtag newtap $_CHIPNAME dap -irlen 4 -expected-id $_DAP_TAPID
+dap create $_CHIPNAME.dap -chain-position $_CHIPNAME.dap
+
+target create $_CHIPNAME.axi mem_ap -dap $_CHIPNAME.dap -ap-num 1
+
+set _CPU_BASE 0x81000000
+set _CPU_STRIDE 0x100000
+set _CPU_DBGOFF 0x10000
+set _CPU_CTIOFF 0x20000
+
+set _TARGETS {}
+for {set i 0} {$i < $_CPUS} {incr i} {
+	set _BASE [expr {$_CPU_BASE + $_CPU_STRIDE * $i}]
+	cti create $_CHIPNAME.cti$i -dap $_CHIPNAME.dap -ap-num 0 \
+		-baseaddr [expr {$_BASE + $_CPU_CTIOFF}]
+	target create $_CHIPNAME.cpu$i aarch64 -dap $_CHIPNAME.dap \
+		-cti $_CHIPNAME.cti$i -dbgbase [expr {$_BASE + $_CPU_DBGOFF}] \
+		{*}[expr {$i ? "-coreid $i" : "-rtos hwthread" }]
+	lappend _TARGETS $_CHIPNAME.cpu$i
+}
+
+target smp {*}$_TARGETS
+
+# Service processor
+target create $_CHIPNAME.sp cortex_a -dap $_CHIPNAME.dap -ap-num 0 -dbgbase 0x80138000
+
+# Normally you will not need to call this, but if you are using the hard-coded
+# Reset Configuration Word (RCW) you will need to call this manually. The CPU's
+# reset vector is 0, and the boot ROM at that location contains ARMv7-A 32-bit
+# instructions. This will cause the CPU to almost immediately execute an
+# illegal instruction.
+#
+# This code is idempotent; releasing a released CPU has no effect, although it
+# will halt/resume the service processor.
+add_help_text release_cpu "Release a cpu which is held off"
+proc release_cpu {cpu} {
+	set RST_BRRL 0x1e60060
+
+	set old [target current]
+	targets $::_CHIPNAME.sp
+	set not_halted [string compare halted [$::_CHIPNAME.sp curstate]]
+	if {$not_halted} {
+		halt
+	}
+
+	# Release the cpu; it will start executing something bogus
+	mem2array regs 32 $RST_BRRL 1
+	mww $RST_BRRL [expr {$regs(0) | 1 << $cpu}]
+
+	if {$not_halted} {
+		resume
+	}
+	targets $old
+}
+
+targets $_CHIPNAME.cpu0
diff --git a/tcl/target/renesas_rz_five.cfg b/tcl/target/renesas_rz_five.cfg
new file mode 100644
index 0000000..5ab94ab
--- /dev/null
+++ b/tcl/target/renesas_rz_five.cfg
@@ -0,0 +1,22 @@
+# SPDX-License-Identifier: GPL-2.0-or-later
+
+# Renesas RZ/Five SoC
+#
+# General-purpose Microprocessors with RISC-V CPU Core (Andes AX45MP Single) (1.0 GHz)
+
+transport select jtag
+
+reset_config trst_and_srst srst_gates_jtag
+adapter speed 4000
+adapter srst delay 500
+
+if { [info exists CHIPNAME] } {
+   set _CHIPNAME $CHIPNAME
+} else {
+   set _CHIPNAME r9A07g043u
+}
+
+jtag newtap $_CHIPNAME cpu -irlen 5 -expected-id 0x1000563d
+
+set _TARGETNAME $_CHIPNAME.cpu
+target create $_TARGETNAME riscv -chain-position $_TARGETNAME
diff --git a/tcl/target/renesas_rz_g2.cfg b/tcl/target/renesas_rz_g2.cfg
index 3615aa0..a3d5f48 100644
--- a/tcl/target/renesas_rz_g2.cfg
+++ b/tcl/target/renesas_rz_g2.cfg
@@ -6,11 +6,13 @@
 # - Each SOC can boot through the Cortex-A5x cores
 
 # Supported RZ/G2 SOCs and their cores:
-# RZ/G2H: Cortex-A57 x4, Cortex-A53 x4, Cortex-R7
-# RZ/G2M: Cortex-A57 x2, Cortex-A53 x4, Cortex-R7
-# RZ/G2N: Cortex-A57 x2,                Cortex-R7
-# RZ/G2E:                Cortex-A53 x2, Cortex-R7
-# RZ/G2L:                Cortex-A55 x2, Cortex-M33
+# RZ/G2H:   Cortex-A57 x4, Cortex-A53 x4, Cortex-R7
+# RZ/G2M:   Cortex-A57 x2, Cortex-A53 x4, Cortex-R7
+# RZ/G2N:   Cortex-A57 x2,                Cortex-R7
+# RZ/G2E:                  Cortex-A53 x2, Cortex-R7
+# RZ/G2L:                  Cortex-A55 x2, Cortex-M33
+# RZ/G2LC:                 Cortex-A55 x2, Cortex-M33
+# RZ/G2UL:                 Cortex-A55 x1, Cortex-M33
 
 # Usage:
 # There are 2 configuration options:
@@ -75,6 +77,20 @@ switch $_soc {
 		set _boot_core CA55
 		set _ap_num 0
 	}
+	G2LC {
+		set _CHIPNAME r9a07g044c
+		set _num_ca55 2
+		set _num_cm33 1
+		set _boot_core CA55
+		set _ap_num 0
+	}
+	G2UL {
+		set _CHIPNAME r9a07g043u
+		set _num_ca55 1
+		set _num_cm33 1
+		set _boot_core CA55
+		set _ap_num 0
+	}
 	default {
 		error "'$_soc' is invalid!"
 	}
@@ -169,7 +185,7 @@ if { $_boot_core == "CA57" } {
 echo "SMP targets:$smp_targets"
 eval "target smp $smp_targets"
 
-if { $_soc == "G2L"} {
+if { $_soc == "G2L" || $_soc == "G2LC" || $_soc == "G2UL" } {
 	target create $_CHIPNAME.axi_ap mem_ap -dap $_DAPNAME -ap-num 1
 }