Merge commit 'adcc8ef87bc1ed47c95f1f2d23072b2b916e1555' into en-sc/from_upstream

Change-Id: I6a718561985acf398ee47cec95c6ee6e24b9c9b7

45 files changed, 2678 insertions, 489 deletions

diff --git a/doc/manual/jtag/drivers/remote_bitbang.txt b/doc/manual/jtag/drivers/remote_bitbang.txt
index 7c8eee2..94d6038 100644
--- a/doc/manual/jtag/drivers/remote_bitbang.txt
+++ b/doc/manual/jtag/drivers/remote_bitbang.txt
@@ -37,12 +37,16 @@ swdio_read
 swd_write
 	Set the value of swclk (tck) and swdio (tms).
 
+(optional) sleep
+	Instructs the remote host to sleep/idle for some period of time before
+		executing the next request
+
 An additional function, quit, is added to the remote_bitbang interface to
 indicate there will be no more requests and the connection with the remote
 driver should be closed.
 
-These eight functions are encoded in ASCII by assigning a single character to
-each possible request. The assignments are:
+The eight mandatory functions are encoded in ASCII by assigning a single
+character to each possible request. The assignments are:
 
 	B - Blink on
 	b - Blink off
@@ -70,4 +74,10 @@ each possible request. The assignments are:
 
 The read responses are encoded in ASCII as either digit 0 or 1.
 
+If the use_remote_sleep option is set to 'yes', two additional requests may
+be sent:
+
+	D - Sleep for 1 millisecond
+	d - Sleep for 1 microsecond
+
  */
diff --git a/doc/openocd.texi b/doc/openocd.texi
index 1f145b9..a43949f 100644
--- a/doc/openocd.texi
+++ b/doc/openocd.texi
@@ -2868,6 +2868,15 @@ Specifies the hostname of the remote process to connect to using TCP, or the
 name of the UNIX socket to use if remote_bitbang port is 0.
 @end deffn
 
+@deffn {Config Command} {remote_bitbang use_remote_sleep} (on|off)
+If this option is enabled, delays will not be executed locally but instead
+forwarded to the remote host. This is useful if the remote host performs its
+own request queuing rather than executing requests immediately.
+
+This is disabled by default. This option must only be enabled if the given
+remote_bitbang host supports receiving the delay information.
+@end deffn
+
 For example, to connect remotely via TCP to the host foobar you might have
 something like:
 
@@ -2877,6 +2886,15 @@ remote_bitbang port 3335
 remote_bitbang host foobar
 @end example
 
+And if you also wished to enable remote sleeping:
+
+@example
+adapter driver remote_bitbang
+remote_bitbang port 3335
+remote_bitbang host foobar
+remote_bitbang use_remote_sleep on
+@end example
+
 To connect to another process running locally via UNIX sockets with socket
 named mysocket:
 
@@ -4435,6 +4453,10 @@ there seems to be no problems with JTAG scan chain operations.
 register during initial examination and when checking the sticky error bit.
 This bit is normally checked after setting the CSYSPWRUPREQ bit, but some
 devices do not set the ack bit until sometime later.
+@item @code{-ir-bypass} @var{NUMBER}
+@*Vendor specific bypass instruction, required by some hierarchical JTAG
+routers where the normal BYPASS instruction bypasses the whole router and
+a vendor specific bypass instruction is required to access child nodes.
 @end itemize
 @end deffn
 
@@ -6831,16 +6853,23 @@ nor is Chip Erase (only Sector Erase is implemented).}
 
 @deffn {Flash Driver} {kinetis}
 @cindex kinetis
-Kx, KLx, KVx and KE1x members of the Kinetis microcontroller family
-from NXP (former Freescale) include
-internal flash and use ARM Cortex-M0+ or M4 cores. The driver automatically
+Several microcontrollers from NXP (former Freescale), including
+Kx, KLx, KVx and KE1x members of the Kinetis family,
+and S32K11x/S32K14x microcontrollers, include
+internal flash and use ARM Cortex-M0+ or M4 cores.
+Kinetis and S32K1 families use incompatible
+identification registers, so the driver assumes Kinetis and requires
+a driver option to indicate S32K1 is to be used.
+Within the familiy, the driver automatically
 recognizes flash size and a number of flash banks (1-4) using the chip
 identification register, and autoconfigures itself.
 Use kinetis_ke driver for KE0x and KEAx devices.
 
 The @var{kinetis} driver defines option:
 @itemize
-@item -sim-base @var{addr} ... base of System Integration Module where chip identification resides. Driver tries two known locations if option is omitted.
+@item -s32k select S32K11x/S32K14x microcontroller flash support.
+
+@item -sim-base @var{addr} ... base of System Integration Module where chip identification resides. Driver tries known locations if option is omitted.
 @end itemize
 
 @example
@@ -6889,6 +6918,7 @@ command completes.
 
 @deffn {Command} {kinetis nvm_partition}
 For FlexNVM devices only (KxxDX and KxxFX).
+Not supported (yet) on S32K1 devices.
 Command shows or sets data flash or EEPROM backup size in kilobytes,
 sets two EEPROM blocks sizes in bytes and enables/disables loading
 of EEPROM contents to FlexRAM during reset.
@@ -7714,12 +7744,10 @@ applied to all of them.
 @end deffn
 
 @deffn {Flash Driver} {stm32f1x}
-All members of the STM32F0, STM32F1 and STM32F3 microcontroller families
-from STMicroelectronics and all members of the GD32F1x0, GD32F3x0 and GD32E23x microcontroller
-families from GigaDevice include internal flash and use ARM Cortex-M0/M3/M4/M23 cores.
-The driver also works with GD32VF103 powered by RISC-V core.
-The driver automatically recognizes a number of these chips using
-the chip identification register, and autoconfigures itself.
+This driver supports the STM32F0, STM32F1 and STM32F3 microcontroller series from STMicroelectronics.
+The driver is also compatible with the GD32F1, GD32VF103 (RISC-V core), GD32F3 and GD32E23 microcontroller series from GigaDevice.
+The driver also supports the APM32F0 and APM32F1 series from Geehy Semiconductor.
+The driver automatically recognizes a number of these chips using the chip identification register, and autoconfigures itself.
 
 @example
 flash bank $_FLASHNAME stm32f1x 0 0 0 0 $_TARGETNAME
@@ -7780,6 +7808,7 @@ The @var{num} parameter is a value shown by @command{flash banks}.
 @deffn {Flash Driver} {stm32f2x}
 All members of the STM32F2, STM32F4 and STM32F7 microcontroller families from STMicroelectronics
 include internal flash and use ARM Cortex-M3/M4/M7 cores.
+The driver also works for the APM32F4 series from Geehy Semiconductor.
 The driver automatically recognizes a number of these chips using
 the chip identification register, and autoconfigures itself.
 
@@ -9336,7 +9365,7 @@ Loads an image stored in memory by @command{fast_load_image} to the
 current target. Must be preceded by fast_load_image.
 @end deffn
 
-@deffn {Command} {fast_load_image} filename address [@option{bin}|@option{ihex}|@option{elf}|@option{s19}]
+@deffn {Command} {fast_load_image} filename [address [@option{bin}|@option{ihex}|@option{elf}|@option{s19} [@option{min_addr} [@option{max_length}]]]]]]
 Normally you should be using @command{load_image} or GDB load. However, for
 testing purposes or when I/O overhead is significant(OpenOCD running on an embedded
 host), storing the image in memory and uploading the image to the target
@@ -9347,8 +9376,10 @@ target programming performance as I/O and target programming can easily be profi
 separately.
 @end deffn
 
-@deffn {Command} {load_image} filename address [[@option{bin}|@option{ihex}|@option{elf}|@option{s19}] @option{min_addr} @option{max_length}]
-Load image from file @var{filename} to target memory offset by @var{address} from its load address.
+@deffn {Command} {load_image} filename [address [@option{bin}|@option{ihex}|@option{elf}|@option{s19} [@option{min_addr} [@option{max_length}]]]]
+Load image from file @var{filename} to target memory.
+If an @var{address} is specified, it is used as an offset to the file format
+defined addressing (e.g. @option{bin} file is loaded at that address).
 The file format may optionally be specified
 (@option{bin}, @option{ihex}, @option{elf}, or @option{s19}).
 In addition the following arguments may be specified:
@@ -9372,15 +9403,21 @@ The file format may optionally be specified
 (@option{bin}, @option{ihex}, or @option{elf})
 @end deffn
 
-@deffn {Command} {verify_image} filename address [@option{bin}|@option{ihex}|@option{elf}]
-Verify @var{filename} against target memory starting at @var{address}.
+@deffn {Command} {verify_image} filename [address [@option{bin}|@option{ihex}|@option{elf}]]
+Verify @var{filename} against target memory.
+If an @var{address} is specified, it is used as an offset to the file format
+defined addressing (e.g. @option{bin} file is compared against memory starting
+at that address).
 The file format may optionally be specified
 (@option{bin}, @option{ihex}, or @option{elf})
 This will first attempt a comparison using a CRC checksum, if this fails it will try a binary compare.
 @end deffn
 
-@deffn {Command} {verify_image_checksum} filename address [@option{bin}|@option{ihex}|@option{elf}]
-Verify @var{filename} against target memory starting at @var{address}.
+@deffn {Command} {verify_image_checksum} filename [address [@option{bin}|@option{ihex}|@option{elf}]]
+Verify @var{filename} against target memory.
+If an @var{address} is specified, it is used as an offset to the file format
+defined addressing (e.g. @option{bin} file is compared against memory starting
+at that address).
 The file format may optionally be specified
 (@option{bin}, @option{ihex}, or @option{elf})
 This perform a comparison using a CRC checksum only
@@ -10989,6 +11026,73 @@ addreg rtest 0x1234 org.gnu.gdb.or1k.group0 system
 
 @end deffn
 
+@section MIPS Architecture
+@cindex microMIPS
+@cindex MIPS32
+@cindex MIPS64
+
+@uref{http://mips.com/, MIPS} is a simple, streamlined, highly scalable RISC
+architecture. The architecture is evolving over time, from MIPS I~V to
+MIPS release 1~6 iterations, the architecture is now able to handle various tasks
+with different ASEs, including SIMD(MSA), DSP, VZ, MT and more.
+MIPS32 supports 32-bit programs while MIPS64 can support both 32-bit and 64-bit programs.
+
+@subsection MIPS Terminology
+
+The term ASE means Application-Specific Extension, ASEs provide features that
+improve the efficiency and performance of certain workloads, such as
+digital signal processing(DSP), Virtualization(VZ), Multi-Threading(MT),
+SIMD(MSA) and more.
+
+MIPS Cores use Coprocessors(CPx) to configure their behaviour or to let software
+know the capabilities of current CPU, the main Coprocessor is CP0, containing 32
+registers with a maximum select number of 7.
+
+@subsection MIPS FPU & Vector Registers
+
+MIPS processors does not all comes with FPU co-processor, and when it does, the FPU
+appears as Coprocessor 1 whereas the Coprocessor 0 is for the main processor.
+
+Most of MIPS FPUs are 64 bits, IEEE 754 standard, and they provides both 32-bit
+single precision and 64-bit double precision calculations. Fixed point format
+calculations are also provided with both 32 and 64-bit modes.
+
+The MIPS SIMD Architecture(MSA) operates on 32 128-bit wide vector registers.
+If both MSA and the scalar floating-point unit (FPU) are present, the 128-bit MSA
+vector registers extend and share the 64-bit FPU registers. MSA and FPU can not be
+both present, unless the FPU has 64-bit floating-point register.
+
+@subsection MIPS Configuration Commands
+
+@deffn {Command} {mips32 cpuinfo}
+Displays detailed information about current CPU core. This includes core type,
+vendor, instruction set, cache size, and other relevant details.
+@end deffn
+
+@deffn {Config Command} {mips32 scan_delay} [nanoseconds]
+Display or set scan delay in nano seconds. A value below 2_000_000 will set the
+scan delay into legacy mode.
+@end deffn
+
+@deffn {Config Command} {mips32 cp0} [[reg_name|regnum select] [value]]
+Displays or sets coprocessor 0 register by register number and select or their name.
+This command shows all available cp0 register if no arguments are provided.
+
+For common MIPS Coprocessor 0 registers, you can find the definitions of them
+on MIPS Privileged Resource Architecture Documents(MIPS Document MD00090).
+
+For core specific cp0 registers, you can find the definitions of them on Core
+Specific Software User's Manual(SUM), for example, MIPS M5150 Software User Manual
+(MD00980).
+@end deffn
+
+@deffn {Command} {mips32 ejtag_reg}
+Reads EJTAG Registers for inspection.
+
+EJTAG Register Specification could be found in MIPS Document MD00047F, for
+core specific EJTAG Register definition, please check Core Specific SUM manual.
+@end deffn
+
 @section RISC-V Architecture
 
 @uref{http://riscv.org/, RISC-V} is a free and open ISA. OpenOCD supports JTAG
diff --git a/doc/usb_adapters/cmsis_dap/0d28_0204_nxp_daplink.txt b/doc/usb_adapters/cmsis_dap/0d28_0204_nxp_daplink.txt
index 2ec0d58..5141a1b 100644
--- a/doc/usb_adapters/cmsis_dap/0d28_0204_nxp_daplink.txt
+++ b/doc/usb_adapters/cmsis_dap/0d28_0204_nxp_daplink.txt
@@ -76,8 +76,35 @@ Device Descriptor:
           bNumDescriptors         1
           bDescriptorType        34 Report
           wDescriptorLength      33
-         Report Descriptors:
-           ** UNAVAILABLE **
+          Report Descriptor: (length is 33)
+            Item(Global): Usage Page, data= [ 0x00 0xff ] 65280
+                            (null)
+            Item(Local ): Usage, data= [ 0x01 ] 1
+                            (null)
+            Item(Main  ): Collection, data= [ 0x01 ] 1
+                            Application
+            Item(Global): Logical Minimum, data= [ 0x00 ] 0
+            Item(Global): Logical Maximum, data= [ 0xff 0x00 ] 255
+            Item(Global): Report Size, data= [ 0x08 ] 8
+            Item(Global): Report Count, data= [ 0x40 ] 64
+            Item(Local ): Usage, data= [ 0x01 ] 1
+                            (null)
+            Item(Main  ): Input, data= [ 0x02 ] 2
+                            Data Variable Absolute No_Wrap Linear
+                            Preferred_State No_Null_Position Non_Volatile Bitfield
+            Item(Global): Report Count, data= [ 0x40 ] 64
+            Item(Local ): Usage, data= [ 0x01 ] 1
+                            (null)
+            Item(Main  ): Output, data= [ 0x02 ] 2
+                            Data Variable Absolute No_Wrap Linear
+                            Preferred_State No_Null_Position Non_Volatile Bitfield
+            Item(Global): Report Count, data= [ 0x01 ] 1
+            Item(Local ): Usage, data= [ 0x01 ] 1
+                            (null)
+            Item(Main  ): Feature, data= [ 0x02 ] 2
+                            Data Variable Absolute No_Wrap Linear
+                            Preferred_State No_Null_Position Non_Volatile Bitfield
+            Item(Main  ): End Collection, data=none
       Endpoint Descriptor:
         bLength                 7
         bDescriptorType         5
diff --git a/doc/usb_adapters/cmsis_dap/c251_2722_keil_ulink2.txt b/doc/usb_adapters/cmsis_dap/c251_2722_keil_ulink2.txt
index 520f7c5..65903b3 100644
--- a/doc/usb_adapters/cmsis_dap/c251_2722_keil_ulink2.txt
+++ b/doc/usb_adapters/cmsis_dap/c251_2722_keil_ulink2.txt
@@ -46,8 +46,35 @@ Device Descriptor:
           bNumDescriptors         1
           bDescriptorType        34 Report
           wDescriptorLength      33
-         Report Descriptors:
-           ** UNAVAILABLE **
+          Report Descriptor: (length is 33)
+            Item(Global): Usage Page, data= [ 0x00 0xff ] 65280
+                            (null)
+            Item(Local ): Usage, data= [ 0x01 ] 1
+                            (null)
+            Item(Main  ): Collection, data= [ 0x01 ] 1
+                            Application
+            Item(Global): Logical Minimum, data= [ 0x00 ] 0
+            Item(Global): Logical Maximum, data= [ 0xff 0x00 ] 255
+            Item(Global): Report Size, data= [ 0x08 ] 8
+            Item(Global): Report Count, data= [ 0x40 ] 64
+            Item(Local ): Usage, data= [ 0x01 ] 1
+                            (null)
+            Item(Main  ): Input, data= [ 0x02 ] 2
+                            Data Variable Absolute No_Wrap Linear
+                            Preferred_State No_Null_Position Non_Volatile Bitfield
+            Item(Global): Report Count, data= [ 0x40 ] 64
+            Item(Local ): Usage, data= [ 0x01 ] 1
+                            (null)
+            Item(Main  ): Output, data= [ 0x02 ] 2
+                            Data Variable Absolute No_Wrap Linear
+                            Preferred_State No_Null_Position Non_Volatile Bitfield
+            Item(Global): Report Count, data= [ 0x01 ] 1
+            Item(Local ): Usage, data= [ 0x01 ] 1
+                            (null)
+            Item(Main  ): Feature, data= [ 0x02 ] 2
+                            Data Variable Absolute No_Wrap Linear
+                            Preferred_State No_Null_Position Non_Volatile Bitfield
+            Item(Main  ): End Collection, data=none
       Endpoint Descriptor:
         bLength                 7
         bDescriptorType         5
diff --git a/doc/usb_adapters/dump.sh b/doc/usb_adapters/dump.sh
index 1ecfb40..557ef7f 100755
--- a/doc/usb_adapters/dump.sh
+++ b/doc/usb_adapters/dump.sh
@@ -1,6 +1,22 @@
 #!/bin/sh
 # SPDX-License-Identifier: GPL-2.0-or-later
 
+hid_unavailable_report() {
+	a=$(echo $1 | tr '[:lower:]' '[:upper:]')
+	b=$(basename $(dirname $(ls -d /sys/bus/usb/drivers/usbhid/*/*:$a.*)))
+
+	echo ""
+	echo "ATTENTION!"
+	echo "Unable to read completely the USB descriptors."
+	echo "Please run the following command(s) and then run this script again"
+	for i in $b; do
+		echo "    sudo sh -c \"echo -n $i > /sys/bus/usb/drivers/usbhid/unbind\""
+	done
+	echo ""
+	echo "Please notice that the USB device will not function after the above"
+	echo "operations; you should unplug and replug it to get it working again."
+}
+
 devs=$(lsusb -d $1:$2 | wc -l)
 case "$devs" in
 	0 )
@@ -22,3 +38,5 @@ echo ''
 echo '# Optional comment'
 
 lsusb -v -d $1:$2 | sed 's/ *$//'
+
+lsusb -v -d $1:$2 2>&1 | grep -Fq '** UNAVAILABLE **' && (hid_unavailable_report $1:$2 > /dev/stderr)
diff --git a/doc/usb_adapters/nulink/0416_511d_nuvoton_nulink.txt b/doc/usb_adapters/nulink/0416_511d_nuvoton_nulink.txt
index fb43923..2ac9a44 100644
--- a/doc/usb_adapters/nulink/0416_511d_nuvoton_nulink.txt
+++ b/doc/usb_adapters/nulink/0416_511d_nuvoton_nulink.txt
@@ -47,8 +47,32 @@ Device Descriptor:
           bNumDescriptors         1
           bDescriptorType        34 Report
           wDescriptorLength      28
-         Report Descriptors:
-           ** UNAVAILABLE **
+          Report Descriptor: (length is 28)
+            Item(Global): Usage Page, data= [ 0x01 ] 1
+                            Generic Desktop Controls
+            Item(Local ): Usage, data= [ 0x00 ] 0
+                            Undefined
+            Item(Main  ): Collection, data= [ 0x01 ] 1
+                            Application
+            Item(Global): Logical Minimum, data= [ 0x00 ] 0
+            Item(Global): Logical Maximum, data= [ 0xff 0x00 ] 255
+            Item(Local ): Usage Minimum, data= [ 0x00 ] 0
+                            Undefined
+            Item(Local ): Usage Maximum, data= [ 0x00 ] 0
+                            Undefined
+            Item(Global): Report Size, data= [ 0x08 ] 8
+            Item(Global): Report Count, data= [ 0x40 ] 64
+            Item(Main  ): Input, data= [ 0x02 ] 2
+                            Data Variable Absolute No_Wrap Linear
+                            Preferred_State No_Null_Position Non_Volatile Bitfield
+            Item(Local ): Usage Minimum, data= [ 0x00 ] 0
+                            Undefined
+            Item(Local ): Usage Maximum, data= [ 0x00 ] 0
+                            Undefined
+            Item(Main  ): Output, data= [ 0x02 ] 2
+                            Data Variable Absolute No_Wrap Linear
+                            Preferred_State No_Null_Position Non_Volatile Bitfield
+            Item(Main  ): End Collection, data=none
       Endpoint Descriptor:
         bLength                 7
         bDescriptorType         5
diff --git a/doc/usb_adapters/nulink/0416_5200_nuvoton_nulink.txt b/doc/usb_adapters/nulink/0416_5200_nuvoton_nulink.txt
index 1d8661f..5735d3b 100644
--- a/doc/usb_adapters/nulink/0416_5200_nuvoton_nulink.txt
+++ b/doc/usb_adapters/nulink/0416_5200_nuvoton_nulink.txt
@@ -16,7 +16,7 @@ Device Descriptor:
   idProduct          0x5200 Nuvoton Nu-Link2-ME ICE/MSC/VCOM
   bcdDevice            0.00
   iManufacturer           1 Nuvoton
-  iProduct                2 Nu-Link2 Bulk
+  iProduct                9 Nu-Link2
   iSerial                 6 13010000AAAAAAAAAAAAAAAAAAAAAAAA
   bNumConfigurations      1
   Configuration Descriptor:
@@ -38,7 +38,7 @@ Device Descriptor:
       bInterfaceClass       255 Vendor Specific Class
       bInterfaceSubClass      0
       bInterfaceProtocol      0
-      iInterface              0
+      iInterface              2 Nu-Link2 Bulk
       Endpoint Descriptor:
         bLength                 7
         bDescriptorType         5
@@ -146,8 +146,35 @@ Device Descriptor:
           bNumDescriptors         1
           bDescriptorType        34 Report
           wDescriptorLength      35
-         Report Descriptors:
-           ** UNAVAILABLE **
+          Report Descriptor: (length is 35)
+            Item(Global): Usage Page, data= [ 0x06 0xff ] 65286
+                            (null)
+            Item(Local ): Usage, data= [ 0x01 ] 1
+                            (null)
+            Item(Main  ): Collection, data= [ 0x01 ] 1
+                            Application
+            Item(Global): Logical Minimum, data= [ 0x00 ] 0
+            Item(Global): Logical Maximum, data= [ 0xff 0x00 ] 255
+            Item(Global): Report Size, data= [ 0x08 ] 8
+            Item(Global): Report Count, data= [ 0x00 0x04 ] 1024
+            Item(Local ): Usage, data= [ 0x01 ] 1
+                            (null)
+            Item(Main  ): Input, data= [ 0x02 ] 2
+                            Data Variable Absolute No_Wrap Linear
+                            Preferred_State No_Null_Position Non_Volatile Bitfield
+            Item(Global): Report Count, data= [ 0x00 0x04 ] 1024
+            Item(Local ): Usage, data= [ 0x01 ] 1
+                            (null)
+            Item(Main  ): Output, data= [ 0x02 ] 2
+                            Data Variable Absolute No_Wrap Linear
+                            Preferred_State No_Null_Position Non_Volatile Bitfield
+            Item(Global): Report Count, data= [ 0x08 ] 8
+            Item(Local ): Usage, data= [ 0x01 ] 1
+                            (null)
+            Item(Main  ): Feature, data= [ 0x02 ] 2
+                            Data Variable Absolute No_Wrap Linear
+                            Preferred_State No_Null_Position Non_Volatile Bitfield
+            Item(Main  ): End Collection, data=none
       Endpoint Descriptor:
         bLength                 7
         bDescriptorType         5
diff --git a/doc/usb_adapters/xds110/0451_0451_ti_xds110.txt b/doc/usb_adapters/xds110/0451_bef3_ti_xds110.txt
index 9ad9b7d..628b529 100644
--- a/doc/usb_adapters/xds110/0451_0451_ti_xds110.txt
+++ b/doc/usb_adapters/xds110/0451_bef3_ti_xds110.txt
@@ -220,8 +220,28 @@ Device Descriptor:
           bNumDescriptors         1
           bDescriptorType        34 Report
           wDescriptorLength      24
-         Report Descriptors:
-           ** UNAVAILABLE **
+          Report Descriptor: (length is 24)
+            Item(Global): Usage Page, data= [ 0x00 0xff ] 65280
+                            (null)
+            Item(Local ): Usage, data= [ 0x01 ] 1
+                            (null)
+            Item(Main  ): Collection, data= [ 0x01 ] 1
+                            Application
+            Item(Local ): Usage, data= [ 0x03 ] 3
+                            (null)
+            Item(Global): Report Size, data= [ 0x08 ] 8
+            Item(Global): Report Count, data= [ 0x40 ] 64
+            Item(Main  ): Input, data= [ 0x02 ] 2
+                            Data Variable Absolute No_Wrap Linear
+                            Preferred_State No_Null_Position Non_Volatile Bitfield
+            Item(Local ): Usage, data= [ 0x04 ] 4
+                            (null)
+            Item(Global): Report Size, data= [ 0x08 ] 8
+            Item(Global): Report Count, data= [ 0x40 ] 64
+            Item(Main  ): Output, data= [ 0x02 ] 2
+                            Data Variable Absolute No_Wrap Linear
+                            Preferred_State No_Null_Position Non_Volatile Bitfield
+            Item(Main  ): End Collection, data=none
       Endpoint Descriptor:
         bLength                 7
         bDescriptorType         5
diff --git a/src/flash/nor/kinetis.c b/src/flash/nor/kinetis.c
index 7137b4a..e8074e3 100644
--- a/src/flash/nor/kinetis.c
+++ b/src/flash/nor/kinetis.c
@@ -80,6 +80,7 @@
 #define FLEXRAM		0x14000000
 
 #define MSCM_OCMDR0	0x40001400
+#define MSCM_OCMDR1	0x40001404
 #define FMC_PFB01CR	0x4001f004
 #define FTFX_FSTAT	0x40020000
 #define FTFX_FCNFG	0x40020001
@@ -230,6 +231,28 @@
 #define KINETIS_SDID_PROJECTID_KE1XF 0x00000080
 #define KINETIS_SDID_PROJECTID_KE1XZ 0x00000100
 
+/* The S32K series uses a different, incompatible SDID layout :
+ * Bit 31-28 : GENERATION
+ * Bit 27-24 : SUBSERIES
+ * Bit 23-20 : DERIVATE
+ * Bit 19-16 : RAMSIZE
+ * Bit 15-12 : REVID
+ * Bit 11-8  : PACKAGE
+ * Bit 7-0   : FEATURES
+ */
+
+#define KINETIS_SDID_S32K_SERIES_MASK	0xFF000000	/* GENERATION + SUBSERIES */
+#define KINETIS_SDID_S32K_SERIES_K11X	0x11000000
+#define KINETIS_SDID_S32K_SERIES_K14X	0x14000000
+
+#define KINETIS_SDID_S32K_DERIVATE_MASK	0x00F00000
+#define KINETIS_SDID_S32K_DERIVATE_KXX2	0x00200000
+#define KINETIS_SDID_S32K_DERIVATE_KXX3	0x00300000
+#define KINETIS_SDID_S32K_DERIVATE_KXX4	0x00400000
+#define KINETIS_SDID_S32K_DERIVATE_KXX5	0x00500000
+#define KINETIS_SDID_S32K_DERIVATE_KXX6	0x00600000
+#define KINETIS_SDID_S32K_DERIVATE_KXX8	0x00800000
+
 struct kinetis_flash_bank {
 	struct kinetis_chip *k_chip;
 	bool probed;
@@ -276,6 +299,11 @@ struct kinetis_chip {
 	uint32_t sim_base;
 
 	enum {
+	      CT_KINETIS = 0,
+	      CT_S32K,
+	} chip_type;
+
+	enum {
 		FS_PROGRAM_SECTOR = 1,
 		FS_PROGRAM_LONGWORD = 2,
 		FS_PROGRAM_PHRASE = 4,		/* Unsupported */
@@ -290,6 +318,7 @@ struct kinetis_chip {
 		KINETIS_CACHE_K,	/* invalidate using FMC->PFB0CR/PFB01CR */
 		KINETIS_CACHE_L,	/* invalidate using MCM->PLACR */
 		KINETIS_CACHE_MSCM,	/* devices like KE1xF, invalidate MSCM->OCMDR0 */
+		KINETIS_CACHE_MSCM2,	/* devices like S32K, invalidate MSCM->OCMDR0 and MSCM->OCMDR1 */
 	} cache_type;
 
 	enum {
@@ -392,6 +421,7 @@ const struct flash_driver kinetis_flash;
 static int kinetis_write_inner(struct flash_bank *bank, const uint8_t *buffer,
 			uint32_t offset, uint32_t count);
 static int kinetis_probe_chip(struct kinetis_chip *k_chip);
+static int kinetis_probe_chip_s32k(struct kinetis_chip *k_chip);
 static int kinetis_auto_probe(struct flash_bank *bank);
 
 
@@ -877,6 +907,8 @@ static int kinetis_chip_options(struct kinetis_chip *k_chip, int argc, const cha
 		if (strcmp(argv[i], "-sim-base") == 0) {
 			if (i + 1 < argc)
 				k_chip->sim_base = strtoul(argv[++i], NULL, 0);
+		} else if (strcmp(argv[i], "-s32k") == 0) {
+			k_chip->chip_type = CT_S32K;
 		} else
 			LOG_ERROR("Unsupported flash bank option %s", argv[i]);
 	}
@@ -1140,7 +1172,13 @@ static int kinetis_disable_wdog(struct kinetis_chip *k_chip)
 	int retval;
 
 	if (!k_chip->probed) {
-		retval = kinetis_probe_chip(k_chip);
+		switch (k_chip->chip_type) {
+		case CT_S32K:
+			retval = kinetis_probe_chip_s32k(k_chip);
+			break;
+		default:
+			retval = kinetis_probe_chip(k_chip);
+		}
 		if (retval != ERROR_OK)
 			return retval;
 	}
@@ -1639,6 +1677,12 @@ static void kinetis_invalidate_flash_cache(struct kinetis_chip *k_chip)
 		/* disable data prefetch and flash speculate */
 		break;
 
+	case KINETIS_CACHE_MSCM2:
+		target_write_u32(target, MSCM_OCMDR0, 0x30);
+		target_write_u32(target, MSCM_OCMDR1, 0x30);
+		/* disable data prefetch and flash speculate */
+		break;
+
 	default:
 		break;
 	}
@@ -2048,6 +2092,174 @@ static int kinetis_write(struct flash_bank *bank, const uint8_t *buffer,
 }
 
 
+static int kinetis_probe_chip_s32k(struct kinetis_chip *k_chip)
+{
+	int result;
+	uint8_t fcfg1_eesize, fcfg1_depart;
+	uint32_t ee_size = 0;
+	uint32_t pflash_size_k, nvm_size_k, dflash_size_k;
+	unsigned int generation = 0, subseries = 0, derivate = 0;
+
+	struct target *target = k_chip->target;
+	k_chip->probed = false;
+	k_chip->pflash_sector_size = 0;
+	k_chip->pflash_base = 0;
+	k_chip->nvm_base = 0x10000000;
+	k_chip->progr_accel_ram = FLEXRAM;
+	k_chip->flash_support = FS_PROGRAM_PHRASE | FS_PROGRAM_SECTOR;
+	k_chip->watchdog_type = KINETIS_WDOG32_KE1X;
+
+	if (k_chip->sim_base == 0)
+		k_chip->sim_base = SIM_BASE;
+
+	result = target_read_u32(target, k_chip->sim_base + SIM_SDID_OFFSET, &k_chip->sim_sdid);
+	if (result != ERROR_OK)
+		return result;
+
+	generation = (k_chip->sim_sdid) >> 28 & 0x0f;
+	subseries = (k_chip->sim_sdid) >> 24 & 0x0f;
+	derivate = (k_chip->sim_sdid) >> 20 & 0x0f;
+
+	switch (k_chip->sim_sdid & KINETIS_SDID_S32K_SERIES_MASK) {
+	case KINETIS_SDID_S32K_SERIES_K11X:
+		k_chip->cache_type = KINETIS_CACHE_L;
+		k_chip->num_pflash_blocks = 1;
+		k_chip->num_nvm_blocks = 1;
+		/* Non-interleaved */
+		k_chip->max_flash_prog_size = 512;
+
+		switch (k_chip->sim_sdid & KINETIS_SDID_S32K_DERIVATE_MASK) {
+			case KINETIS_SDID_S32K_DERIVATE_KXX6:
+				/* S32K116 CPU 48Mhz Flash 128KB RAM 17KB+2KB */
+				/* Non-Interleaved */
+				k_chip->pflash_size = 128 << 10;
+				k_chip->pflash_sector_size = 2 << 10;
+				/* Non-Interleaved */
+				k_chip->nvm_size = 32 << 10;
+				k_chip->nvm_sector_size = 2 << 10;
+				break;
+			case KINETIS_SDID_S32K_DERIVATE_KXX8:
+				/* S32K118 CPU 80Mhz Flash 256KB+32KB RAM 32KB+4KB */
+				/* Non-Interleaved */
+				k_chip->pflash_size = 256 << 10;
+				k_chip->pflash_sector_size = 2 << 10;
+				/* Non-Interleaved */
+				k_chip->nvm_size = 32 << 10;
+				k_chip->nvm_sector_size = 2 << 10;
+				break;
+		}
+		break;
+
+	case KINETIS_SDID_S32K_SERIES_K14X:
+		k_chip->cache_type = KINETIS_CACHE_MSCM2;
+		k_chip->num_pflash_blocks = 1;
+		k_chip->num_nvm_blocks = 1;
+		/* Non-interleaved */
+		k_chip->max_flash_prog_size = 512;
+		switch (k_chip->sim_sdid & KINETIS_SDID_S32K_DERIVATE_MASK) {
+		case KINETIS_SDID_S32K_DERIVATE_KXX2:
+		case KINETIS_SDID_S32K_DERIVATE_KXX3:
+			/* S32K142/S32K142W CPU 80Mhz Flash 256KB+64KB RAM 32KB+4KB */
+			/* Non-Interleaved */
+			k_chip->pflash_size = 256 << 10;
+			k_chip->pflash_sector_size = 2 << 10;
+			/* Non-Interleaved */
+			k_chip->nvm_size = 64 << 10;
+			k_chip->nvm_sector_size = 2 << 10;
+			break;
+		case KINETIS_SDID_S32K_DERIVATE_KXX4:
+		case KINETIS_SDID_S32K_DERIVATE_KXX5:
+			/* S32K144/S32K144W CPU 80Mhz Flash 512KB+64KB RAM 64KB+4KB */
+			/* Interleaved */
+			k_chip->pflash_size = 512 << 10;
+			k_chip->pflash_sector_size = 4 << 10;
+			/* Non-Interleaved */
+			k_chip->nvm_size = 64 << 10;
+			k_chip->nvm_sector_size = 2 << 10;
+			break;
+		case KINETIS_SDID_S32K_DERIVATE_KXX6:
+			/* S32K146 CPU 80Mhz Flash 1024KB+64KB RAM 128KB+4KB */
+			/* Interleaved */
+			k_chip->pflash_size = 1024 << 10;
+			k_chip->pflash_sector_size = 4 << 10;
+			k_chip->num_pflash_blocks = 2;
+			/* Non-Interleaved */
+			k_chip->nvm_size = 64 << 10;
+			k_chip->nvm_sector_size = 2 << 10;
+			break;
+		case KINETIS_SDID_S32K_DERIVATE_KXX8:
+			/* S32K148 CPU 80Mhz Flash 1536KB+512KB RAM 256KB+4KB */
+			/* Interleaved */
+			k_chip->pflash_size = 1536 << 10;
+			k_chip->pflash_sector_size = 4 << 10;
+			k_chip->num_pflash_blocks = 3;
+			/* Interleaved */
+			k_chip->nvm_size = 512 << 10;
+			k_chip->nvm_sector_size = 4 << 10;
+			/* Interleaved */
+			k_chip->max_flash_prog_size = 1 << 10;
+			break;
+		}
+		break;
+
+	default:
+		LOG_ERROR("Unsupported S32K1xx-series");
+	}
+
+	if (k_chip->pflash_sector_size == 0) {
+		LOG_ERROR("MCU is unsupported, SDID 0x%08" PRIx32, k_chip->sim_sdid);
+		return ERROR_FLASH_OPER_UNSUPPORTED;
+	}
+
+	result = target_read_u32(target, k_chip->sim_base + SIM_FCFG1_OFFSET, &k_chip->sim_fcfg1);
+	if (result != ERROR_OK)
+		return result;
+	k_chip->sim_fcfg2 = 0; /* S32K1xx does not implement FCFG2 register. */
+
+	fcfg1_depart = (k_chip->sim_fcfg1 >> 12) & 0x0f;
+	fcfg1_eesize = (k_chip->sim_fcfg1 >> 16) & 0x0f;
+	if (fcfg1_eesize <= 9)
+		ee_size = (16 << (10 - fcfg1_eesize));
+	if ((fcfg1_depart & 0x8) == 0) {
+		/* Binary 0xxx values encode the amount reserved for EEPROM emulation. */
+		if (fcfg1_depart)
+			k_chip->dflash_size = k_chip->nvm_size - (4096 << fcfg1_depart);
+		else
+			k_chip->dflash_size = k_chip->nvm_size;
+	} else {
+		/* Binary 1xxx valued encode the DFlash size. */
+		if (fcfg1_depart & 0x7)
+			k_chip->dflash_size = 4096 << (fcfg1_depart & 0x7);
+		else
+			k_chip->dflash_size = 0;
+	}
+
+	snprintf(k_chip->name, sizeof(k_chip->name), "S32K%u%u%u",
+		 generation, subseries, derivate);
+
+	pflash_size_k = k_chip->pflash_size / 1024;
+	dflash_size_k = k_chip->dflash_size / 1024;
+
+	LOG_INFO("%s detected: %u flash blocks", k_chip->name, k_chip->num_pflash_blocks + k_chip->num_nvm_blocks);
+	LOG_INFO("%u PFlash banks: %" PRIu32 " KiB total", k_chip->num_pflash_blocks, pflash_size_k);
+
+	nvm_size_k = k_chip->nvm_size / 1024;
+
+	if (k_chip->num_nvm_blocks) {
+		LOG_INFO("%u FlexNVM banks: %" PRIu32 " KiB total, %" PRIu32 " KiB available as data flash, %"
+			 PRIu32 " bytes FlexRAM",
+			 k_chip->num_nvm_blocks, nvm_size_k, dflash_size_k, ee_size);
+	}
+
+	k_chip->probed = true;
+
+	if (create_banks)
+		kinetis_create_missing_banks(k_chip);
+
+	return ERROR_OK;
+}
+
+
 static int kinetis_probe_chip(struct kinetis_chip *k_chip)
 {
 	int result;
@@ -2693,7 +2905,13 @@ static int kinetis_probe(struct flash_bank *bank)
 	k_bank->probed = false;
 
 	if (!k_chip->probed) {
-		result = kinetis_probe_chip(k_chip);
+		switch (k_chip->chip_type) {
+		case CT_S32K:
+			result = kinetis_probe_chip_s32k(k_chip);
+			break;
+		default:
+			result = kinetis_probe_chip(k_chip);
+		}
 		if (result != ERROR_OK)
 			return result;
 	}
@@ -2765,23 +2983,26 @@ static int kinetis_probe(struct flash_bank *bank)
 		return ERROR_FLASH_BANK_INVALID;
 	}
 
-	fcfg2_pflsh = (uint8_t)((k_chip->sim_fcfg2 >> 23) & 0x01);
-	fcfg2_maxaddr0 = (uint8_t)((k_chip->sim_fcfg2 >> 24) & 0x7f);
-	fcfg2_maxaddr1 = (uint8_t)((k_chip->sim_fcfg2 >> 16) & 0x7f);
+	/* S32K1xx does not implement FCFG2 register. Skip checks. */
+	if (k_chip->chip_type != CT_S32K) {
+		fcfg2_pflsh = (uint8_t)((k_chip->sim_fcfg2 >> 23) & 0x01);
+		fcfg2_maxaddr0 = (uint8_t)((k_chip->sim_fcfg2 >> 24) & 0x7f);
+		fcfg2_maxaddr1 = (uint8_t)((k_chip->sim_fcfg2 >> 16) & 0x7f);
 
-	if (k_bank->bank_number == 0 && k_chip->fcfg2_maxaddr0_shifted != bank->size)
-		LOG_WARNING("MAXADDR0 0x%02" PRIx8 " check failed,"
-				" please report to OpenOCD mailing list", fcfg2_maxaddr0);
+		if (k_bank->bank_number == 0 && k_chip->fcfg2_maxaddr0_shifted != bank->size)
+			LOG_WARNING("MAXADDR0 0x%02" PRIx8 " check failed,"
+				    " please report to OpenOCD mailing list", fcfg2_maxaddr0);
 
-	if (fcfg2_pflsh) {
-		if (k_bank->bank_number == 1 && k_chip->fcfg2_maxaddr1_shifted != bank->size)
-			LOG_WARNING("MAXADDR1 0x%02" PRIx8 " check failed,"
-				" please report to OpenOCD mailing list", fcfg2_maxaddr1);
-	} else {
-		if (k_bank->bank_number == first_nvm_bank
-				&& k_chip->fcfg2_maxaddr1_shifted != k_chip->dflash_size)
-			LOG_WARNING("FlexNVM MAXADDR1 0x%02" PRIx8 " check failed,"
-				" please report to OpenOCD mailing list", fcfg2_maxaddr1);
+		if (fcfg2_pflsh) {
+			if (k_bank->bank_number == 1 && k_chip->fcfg2_maxaddr1_shifted != bank->size)
+				LOG_WARNING("MAXADDR1 0x%02" PRIx8 " check failed,"
+					    " please report to OpenOCD mailing list", fcfg2_maxaddr1);
+		} else {
+			if (k_bank->bank_number == first_nvm_bank
+			    && k_chip->fcfg2_maxaddr1_shifted != k_chip->dflash_size)
+				LOG_WARNING("FlexNVM MAXADDR1 0x%02" PRIx8 " check failed,"
+					    " please report to OpenOCD mailing list", fcfg2_maxaddr1);
+		}
 	}
 
 	free(bank->sectors);
@@ -2932,6 +3153,11 @@ COMMAND_HANDLER(kinetis_nvm_partition)
 
 	k_chip = kinetis_get_chip(target);
 
+	if (k_chip->chip_type == CT_S32K) {
+		LOG_ERROR("NVM partition not supported on S32K1xx (yet).");
+		return ERROR_FAIL;
+	}
+
 	if (CMD_ARGC >= 2) {
 		if (strcmp(CMD_ARGV[0], "dataflash") == 0)
 			sz_type = DF_SIZE;
diff --git a/src/flash/nor/stm32f1x.c b/src/flash/nor/stm32f1x.c
index b3bb843..5a3c2da 100644
--- a/src/flash/nor/stm32f1x.c
+++ b/src/flash/nor/stm32f1x.c
@@ -745,6 +745,7 @@ static int stm32x_get_property_addr(struct target *target, struct stm32x_propert
 
 	switch (cortex_m_get_impl_part(target)) {
 	case CORTEX_M0_PARTNO: /* STM32F0x devices */
+	case CORTEX_M0P_PARTNO: /* APM32F0x devices */
 		addr->device_id = 0x40015800;
 		addr->flash_size = 0x1FFFF7CC;
 		return ERROR_OK;
diff --git a/src/helper/bin2char.sh b/src/helper/bin2char.sh
index b89433d..cf94bee 100755
--- a/src/helper/bin2char.sh
+++ b/src/helper/bin2char.sh
@@ -12,4 +12,4 @@
 }
 
 echo "/* Autogenerated with $0 */"
-od -v -A n -t x1 | sed 's/ *\(..\) */0x\1,/g'
+od -v -A n -t x1 | sed 's/ *\(..\) */0x\1,/g;/^$/d'
diff --git a/src/jtag/drivers/bitbang.c b/src/jtag/drivers/bitbang.c
index 665dbf3..186d209 100644
--- a/src/jtag/drivers/bitbang.c
+++ b/src/jtag/drivers/bitbang.c
@@ -278,6 +278,15 @@ static int bitbang_scan(bool ir_scan, enum scan_type type, uint8_t *buffer,
 	return ERROR_OK;
 }
 
+static void bitbang_sleep(unsigned int microseconds)
+{
+	if (bitbang_interface->sleep) {
+		bitbang_interface->sleep(microseconds);
+	} else {
+		jtag_sleep(microseconds);
+	}
+}
+
 int bitbang_execute_queue(void)
 {
 	struct jtag_command *cmd = jtag_command_queue;	/* currently processed command */
@@ -351,7 +360,9 @@ int bitbang_execute_queue(void)
 				break;
 			case JTAG_SLEEP:
 				LOG_DEBUG_IO("sleep %" PRIu32, cmd->cmd.sleep->us);
-				jtag_sleep(cmd->cmd.sleep->us);
+				if (bitbang_interface->flush && (bitbang_interface->flush() != ERROR_OK))
+					return ERROR_FAIL;
+				bitbang_sleep(cmd->cmd.sleep->us);
 				break;
 			case JTAG_TMS:
 				retval = bitbang_execute_tms(cmd);
diff --git a/src/jtag/drivers/bitbang.h b/src/jtag/drivers/bitbang.h
index 4ea1cc0..097a5c0 100644
--- a/src/jtag/drivers/bitbang.h
+++ b/src/jtag/drivers/bitbang.h
@@ -54,6 +54,12 @@ struct bitbang_interface {
 
 	/** Set SWCLK and SWDIO to the given value. */
 	int (*swd_write)(int swclk, int swdio);
+
+	/** Sleep for some number of microseconds. **/
+	int (*sleep)(unsigned int microseconds);
+
+	/** Force a flush. */
+	int (*flush)(void);
 };
 
 extern const struct swd_driver bitbang_swd;
diff --git a/src/jtag/drivers/driver.c b/src/jtag/drivers/driver.c
index 7732315..fae2aad 100644
--- a/src/jtag/drivers/driver.c
+++ b/src/jtag/drivers/driver.c
@@ -85,7 +85,13 @@ int interface_jtag_add_ir_scan(struct jtag_tap *active,
 			tap->bypass = true;
 
 			field->num_bits = tap->ir_length;
-			field->out_value = buf_set_ones(cmd_queue_alloc(DIV_ROUND_UP(tap->ir_length, 8)), tap->ir_length);
+			if (tap->ir_bypass_value) {
+				uint8_t *v = cmd_queue_alloc(DIV_ROUND_UP(tap->ir_length, 8));
+				buf_set_u64(v, 0, tap->ir_length, tap->ir_bypass_value);
+				field->out_value = v;
+			} else {
+				field->out_value = buf_set_ones(cmd_queue_alloc(DIV_ROUND_UP(tap->ir_length, 8)), tap->ir_length);
+			}
 			field->in_value = NULL; /* do not collect input for tap's in bypass */
 		}
 
diff --git a/src/jtag/drivers/remote_bitbang.c b/src/jtag/drivers/remote_bitbang.c
index 261c30d..6d0fba2 100644
--- a/src/jtag/drivers/remote_bitbang.c
+++ b/src/jtag/drivers/remote_bitbang.c
@@ -31,6 +31,8 @@ static int remote_bitbang_fd;
 static uint8_t remote_bitbang_send_buf[512];
 static unsigned int remote_bitbang_send_buf_used;
 
+static bool use_remote_sleep;
+
 /* Circular buffer. When start == end, the buffer is empty. */
 static char remote_bitbang_recv_buf[256];
 static unsigned int remote_bitbang_recv_buf_start;
@@ -216,6 +218,32 @@ static int remote_bitbang_reset(int trst, int srst)
 	return remote_bitbang_queue(c, FLUSH_SEND_BUF);
 }
 
+static int remote_bitbang_sleep(unsigned int microseconds)
+{
+	if (!use_remote_sleep) {
+		jtag_sleep(microseconds);
+		return ERROR_OK;
+	}
+
+	int tmp;
+	unsigned int ms = microseconds / 1000;
+	unsigned int us = microseconds % 1000;
+
+	for (unsigned int i = 0; i < ms; i++) {
+		tmp = remote_bitbang_queue('D', NO_FLUSH);
+		if (tmp != ERROR_OK)
+			return tmp;
+	}
+
+	for (unsigned int i = 0; i < us; i++) {
+		tmp = remote_bitbang_queue('d', NO_FLUSH);
+		if (tmp != ERROR_OK)
+			return tmp;
+	}
+
+	return remote_bitbang_flush();
+}
+
 static int remote_bitbang_blink(int on)
 {
 	char c = on ? 'B' : 'b';
@@ -252,6 +280,8 @@ static struct bitbang_interface remote_bitbang_bitbang = {
 	.swdio_drive = &remote_bitbang_swdio_drive,
 	.swd_write = &remote_bitbang_swd_write,
 	.blink = &remote_bitbang_blink,
+	.sleep = &remote_bitbang_sleep,
+	.flush = &remote_bitbang_flush,
 };
 
 static int remote_bitbang_init_tcp(void)
@@ -377,6 +407,16 @@ COMMAND_HANDLER(remote_bitbang_handle_remote_bitbang_host_command)
 
 static const char * const remote_bitbang_transports[] = { "jtag", "swd", NULL };
 
+COMMAND_HANDLER(remote_bitbang_handle_remote_bitbang_use_remote_sleep_command)
+{
+	if (CMD_ARGC != 1)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	COMMAND_PARSE_ON_OFF(CMD_ARGV[0], use_remote_sleep);
+
+	return ERROR_OK;
+}
+
 static const struct command_registration remote_bitbang_subcommand_handlers[] = {
 	{
 		.name = "port",
@@ -394,7 +434,15 @@ static const struct command_registration remote_bitbang_subcommand_handlers[] =
 			"  if port is 0 or unset, this is the name of the unix socket to use.",
 		.usage = "host_name",
 	},
-	COMMAND_REGISTRATION_DONE,
+	{
+		.name = "use_remote_sleep",
+		.handler = remote_bitbang_handle_remote_bitbang_use_remote_sleep_command,
+		.mode = COMMAND_CONFIG,
+		.help = "Rather than executing sleep locally, include delays in the "
+			"instruction stream for the remote host.",
+		.usage = "(on|off)",
+	},
+	COMMAND_REGISTRATION_DONE
 };
 
 static const struct command_registration remote_bitbang_command_handlers[] = {
diff --git a/src/jtag/hla/hla_transport.c b/src/jtag/hla/hla_transport.c
index 08ee18f..c0443d8 100644
--- a/src/jtag/hla/hla_transport.c
+++ b/src/jtag/hla/hla_transport.c
@@ -45,6 +45,7 @@ static const struct command_registration hl_swd_transport_subcommand_handlers[]
 			"['-ignore-version'] "
 			"['-ignore-bypass'] "
 			"['-ircapture' number] "
+			"['-ir-bypass' number] "
 			"['-mask' number]",
 	 },
 	COMMAND_REGISTRATION_DONE
@@ -74,6 +75,7 @@ static const struct command_registration hl_transport_jtag_subcommand_handlers[]
 			"['-ignore-version'] "
 			"['-ignore-bypass'] "
 			"['-ircapture' number] "
+			"['-ir-bypass' number] "
 			"['-mask' number]",
 	 },
 	{
diff --git a/src/jtag/jtag.h b/src/jtag/jtag.h
index 1d1c495..470ae18 100644
--- a/src/jtag/jtag.h
+++ b/src/jtag/jtag.h
@@ -133,6 +133,9 @@ struct jtag_tap {
 	/** Bypass register selected */
 	bool bypass;
 
+	/** Bypass instruction value */
+	uint64_t ir_bypass_value;
+
 	struct jtag_tap_event_action *event_action;
 
 	struct jtag_tap *next_tap;
diff --git a/src/jtag/tcl.c b/src/jtag/tcl.c
index 85a66aa..407aeb1 100644
--- a/src/jtag/tcl.c
+++ b/src/jtag/tcl.c
@@ -386,6 +386,7 @@ static int jtag_tap_configure_cmd(struct jim_getopt_info *goi, struct jtag_tap *
 #define NTAP_OPT_EXPECTED_ID 5
 #define NTAP_OPT_VERSION   6
 #define NTAP_OPT_BYPASS    7
+#define NTAP_OPT_IRBYPASS    8
 
 static const struct nvp jtag_newtap_opts[] = {
 	{ .name = "-irlen",          .value = NTAP_OPT_IRLEN },
@@ -396,6 +397,7 @@ static const struct nvp jtag_newtap_opts[] = {
 	{ .name = "-expected-id",    .value = NTAP_OPT_EXPECTED_ID },
 	{ .name = "-ignore-version", .value = NTAP_OPT_VERSION },
 	{ .name = "-ignore-bypass",  .value = NTAP_OPT_BYPASS },
+	{ .name = "-ir-bypass",      .value = NTAP_OPT_IRBYPASS },
 	{ .name = NULL,              .value = -1 },
 };
 
@@ -499,6 +501,15 @@ static COMMAND_HELPER(handle_jtag_newtap_args, struct jtag_tap *tap)
 			tap->ignore_bypass = true;
 			break;
 
+		case NTAP_OPT_IRBYPASS:
+			if (!CMD_ARGC)
+				return ERROR_COMMAND_ARGUMENT_INVALID;
+
+			COMMAND_PARSE_NUMBER(u64, CMD_ARGV[0], tap->ir_bypass_value);
+			CMD_ARGC--;
+			CMD_ARGV++;
+			break;
+
 		default:
 			nvp_unknown_command_print(CMD, jtag_newtap_opts, NULL, CMD_ARGV[-1]);
 			return ERROR_COMMAND_ARGUMENT_INVALID;
@@ -752,6 +763,7 @@ static const struct command_registration jtag_subcommand_handlers[] = {
 			"['-ignore-version'] "
 			"['-ignore-bypass'] "
 			"['-ircapture' number] "
+			"['-ir-bypass' number] "
 			"['-mask' number]",
 	},
 	{
diff --git a/src/server/server.h b/src/server/server.h
index c9d4698..ea1e94e 100644
--- a/src/server/server.h
+++ b/src/server/server.h
@@ -118,5 +118,6 @@ COMMAND_HELPER(server_port_command, unsigned short *out);
 
 #define ERROR_SERVER_REMOTE_CLOSED		(-400)
 #define ERROR_CONNECTION_REJECTED		(-401)
+#define ERROR_SERVER_INTERRUPTED		(-402)
 
 #endif /* OPENOCD_SERVER_SERVER_H */
diff --git a/src/target/adi_v5_dapdirect.c b/src/target/adi_v5_dapdirect.c
index 575092c..f3a90c0 100644
--- a/src/target/adi_v5_dapdirect.c
+++ b/src/target/adi_v5_dapdirect.c
@@ -66,6 +66,7 @@ static const struct command_registration dapdirect_jtag_subcommand_handlers[] =
 			"['-ignore-version'] "
 			"['-ignore-bypass'] "
 			"['-ircapture' number] "
+			"['-ir-bypass' number] "
 			"['-mask' number]",
 	},
 	{
@@ -156,6 +157,7 @@ static const struct command_registration dapdirect_swd_subcommand_handlers[] = {
 			"['-ignore-version'] "
 			"['-ignore-bypass'] "
 			"['-ircapture' number] "
+			"['-ir-bypass' number] "
 			"['-mask' number]",
 	},
 	COMMAND_REGISTRATION_DONE
diff --git a/src/target/adi_v5_jtag.c b/src/target/adi_v5_jtag.c
index afdc0e5..8d54a50 100644
--- a/src/target/adi_v5_jtag.c
+++ b/src/target/adi_v5_jtag.c
@@ -353,17 +353,25 @@ static int adi_jtag_dp_scan_u32(struct adiv5_dap *dap,
 	uint64_t sel = (reg_addr >> 4) & DP_SELECT_DPBANK;
 
 	/* No need to change SELECT or RDBUFF as they are not banked */
-	if (instr == JTAG_DP_DPACC && reg_addr != DP_SELECT && reg_addr != DP_RDBUFF &&
-			sel != (dap->select & 0xf)) {
-		if (dap->select != DP_SELECT_INVALID)
-			sel |= dap->select & ~0xfull;
-		dap->select = sel;
-		LOG_DEBUG("DP BANKSEL: %x", (uint32_t)sel);
+	if (instr == JTAG_DP_DPACC && reg_addr != DP_SELECT && reg_addr != DP_RDBUFF
+			&& (!dap->select_valid || sel != (dap->select & DP_SELECT_DPBANK))) {
+		/* Use the AP part of dap->select regardless of dap->select_valid:
+		 * if !dap->select_valid
+		 * dap->select contains a speculative value likely going to be used
+		 * in the following swd_queue_ap_bankselect() */
+		sel |= dap->select & SELECT_AP_MASK;
+
+		LOG_DEBUG_IO("DP BANK SELECT: %" PRIx32, (uint32_t)sel);
+
 		buf_set_u32(out_value_buf, 0, 32, (uint32_t)sel);
+
 		retval = adi_jtag_dp_scan(dap, JTAG_DP_DPACC,
 				DP_SELECT, DPAP_WRITE, out_value_buf, NULL, 0, NULL);
 		if (retval != ERROR_OK)
 			return retval;
+
+		dap->select = sel;
+		dap->select_valid = true;
 	}
 	buf_set_u32(out_value_buf, 0, 32, outvalue);
 
@@ -520,7 +528,10 @@ static int jtagdp_overrun_check(struct adiv5_dap *dap)
 					/* timeout happened */
 					if (tmp->ack == JTAG_ACK_WAIT) {
 						LOG_ERROR("Timeout during WAIT recovery");
-						dap->select = DP_SELECT_INVALID;
+						dap->select_valid = false;
+						dap->select1_valid = false;
+						/* Keep dap->select unchanged, the same AP and AP bank
+						 * is likely going to be used further */
 						jtag_ap_q_abort(dap, NULL);
 						/* clear the sticky overrun condition */
 						adi_jtag_scan_inout_check_u32(dap, JTAG_DP_DPACC,
@@ -580,7 +591,7 @@ static int jtagdp_overrun_check(struct adiv5_dap *dap)
 
 			/* TODO: ADIv6 DP SELECT1 handling */
 
-			dap->select = DP_SELECT_INVALID;
+			dap->select_valid = false;
 		}
 
 		list_for_each_entry_safe(el, tmp, &replay_list, lh) {
@@ -615,7 +626,10 @@ static int jtagdp_overrun_check(struct adiv5_dap *dap)
 			if (retval == ERROR_OK) {
 				if (el->ack == JTAG_ACK_WAIT) {
 					LOG_ERROR("Timeout during WAIT recovery");
-					dap->select = DP_SELECT_INVALID;
+					dap->select_valid = false;
+					dap->select1_valid = false;
+					/* Keep dap->select unchanged, the same AP and AP bank
+					 * is likely going to be used further */
 					jtag_ap_q_abort(dap, NULL);
 					/* clear the sticky overrun condition */
 					adi_jtag_scan_inout_check_u32(dap, JTAG_DP_DPACC,
@@ -748,41 +762,60 @@ static int jtag_dp_q_write(struct adiv5_dap *dap, unsigned reg,
 	return retval;
 }
 
-/** Select the AP register bank matching bits 7:4 of reg. */
+/** Select the AP register bank */
 static int jtag_ap_q_bankselect(struct adiv5_ap *ap, unsigned reg)
 {
 	int retval;
 	struct adiv5_dap *dap = ap->dap;
 	uint64_t sel;
 
-	if (is_adiv6(dap)) {
+	if (is_adiv6(dap))
 		sel = ap->ap_num | (reg & 0x00000FF0);
-		if (sel == (dap->select & ~0xfull))
-			return ERROR_OK;
+	else
+		sel = (ap->ap_num << 24) | (reg & ADIV5_DP_SELECT_APBANK);
 
-		if (dap->select != DP_SELECT_INVALID)
-			sel |= dap->select & 0xf;
-		dap->select = sel;
-		LOG_DEBUG("AP BANKSEL: %" PRIx64, sel);
+	uint64_t sel_diff = (sel ^ dap->select) & SELECT_AP_MASK;
+
+	bool set_select = !dap->select_valid || (sel_diff & 0xffffffffull);
+	bool set_select1 = is_adiv6(dap) && dap->asize > 32
+						&& (!dap->select1_valid
+							|| sel_diff & (0xffffffffull << 32));
+
+	if (set_select && set_select1) {
+		/* Prepare DP bank for DP_SELECT1 now to save one write */
+		sel |= (DP_SELECT1 >> 4) & DP_SELECT_DPBANK;
+	} else {
+		/* Use the DP part of dap->select regardless of dap->select_valid:
+		 * if !dap->select_valid
+		 * dap->select contains a speculative value likely going to be used
+		 * in the following swd_queue_dp_bankselect().
+		 * Moreover dap->select_valid should never be false here as a DP bank
+		 * is always selected before selecting an AP bank */
+		sel |= dap->select & DP_SELECT_DPBANK;
+	}
+
+	if (set_select) {
+		LOG_DEBUG_IO("AP BANK SELECT: %" PRIx32, (uint32_t)sel);
 
 		retval = jtag_dp_q_write(dap, DP_SELECT, (uint32_t)sel);
-		if (retval != ERROR_OK)
+		if (retval != ERROR_OK) {
+			dap->select_valid = false;
 			return retval;
-
-		if (dap->asize > 32)
-			return jtag_dp_q_write(dap, DP_SELECT1, (uint32_t)(sel >> 32));
-		return ERROR_OK;
+		}
 	}
 
-	/* ADIv5 */
-	sel = (ap->ap_num << 24) | (reg & ADIV5_DP_SELECT_APBANK);
+	if (set_select1) {
+		LOG_DEBUG_IO("AP BANK SELECT1: %" PRIx32, (uint32_t)(sel >> 32));
 
-	if (sel == dap->select)
-		return ERROR_OK;
+		retval = jtag_dp_q_write(dap, DP_SELECT1, (uint32_t)(sel >> 32));
+		if (retval != ERROR_OK) {
+			dap->select1_valid = false;
+			return retval;
+		}
+	}
 
 	dap->select = sel;
-
-	return jtag_dp_q_write(dap, DP_SELECT, (uint32_t)sel);
+	return ERROR_OK;
 }
 
 static int jtag_ap_q_read(struct adiv5_ap *ap, unsigned reg,
diff --git a/src/target/adi_v5_swd.c b/src/target/adi_v5_swd.c
index 1b74365..6d6f287 100644
--- a/src/target/adi_v5_swd.c
+++ b/src/target/adi_v5_swd.c
@@ -48,6 +48,8 @@ static bool do_sync;
 
 static struct adiv5_dap *swd_multidrop_selected_dap;
 
+static bool swd_multidrop_in_swd_state;
+
 
 static int swd_queue_dp_write_inner(struct adiv5_dap *dap, unsigned int reg,
 		uint32_t data);
@@ -99,27 +101,31 @@ static inline int check_sync(struct adiv5_dap *dap)
 	return do_sync ? swd_run_inner(dap) : ERROR_OK;
 }
 
-/** Select the DP register bank matching bits 7:4 of reg. */
+/** Select the DP register bank */
 static int swd_queue_dp_bankselect(struct adiv5_dap *dap, unsigned int reg)
 {
-	/* Only register address 0 and 4 are banked. */
+	/* Only register address 0 (ADIv6 only) and 4 are banked. */
 	if ((reg & 0xf) > 4)
 		return ERROR_OK;
 
-	uint64_t sel = (reg & 0x000000F0) >> 4;
-	if (dap->select != DP_SELECT_INVALID)
-		sel |= dap->select & ~0xfULL;
+	uint32_t sel = (reg >> 4) & DP_SELECT_DPBANK;
 
-	if (sel == dap->select)
+	/* DP register 0 is not mapped according to ADIv5
+	 * whereas ADIv6 ensures DPBANKSEL = 0 after line reset */
+	if ((dap->select_valid || ((reg & 0xf) == 0 && dap->select_dpbanksel_valid))
+			&& (sel == (dap->select & DP_SELECT_DPBANK)))
 		return ERROR_OK;
 
-	dap->select = sel;
+	/* Use the AP part of dap->select regardless of dap->select_valid:
+	 * if !dap->select_valid
+	 * dap->select contains a speculative value likely going to be used
+	 * in the following swd_queue_ap_bankselect() */
+	sel |= (uint32_t)(dap->select & SELECT_AP_MASK);
 
-	int retval = swd_queue_dp_write_inner(dap, DP_SELECT, (uint32_t)sel);
-	if (retval != ERROR_OK)
-		dap->select = DP_SELECT_INVALID;
+	LOG_DEBUG_IO("DP BANK SELECT: %" PRIx32, sel);
 
-	return retval;
+	/* dap->select cache gets updated in the following call */
+	return swd_queue_dp_write_inner(dap, DP_SELECT, sel);
 }
 
 static int swd_queue_dp_read_inner(struct adiv5_dap *dap, unsigned int reg,
@@ -147,24 +153,31 @@ static int swd_queue_dp_write_inner(struct adiv5_dap *dap, unsigned int reg,
 	swd_finish_read(dap);
 
 	if (reg == DP_SELECT) {
-		dap->select = data & (ADIV5_DP_SELECT_APSEL | ADIV5_DP_SELECT_APBANK | DP_SELECT_DPBANK);
+		dap->select = data | (dap->select & (0xffffffffull << 32));
 
 		swd->write_reg(swd_cmd(false, false, reg), data, 0);
 
 		retval = check_sync(dap);
-		if (retval != ERROR_OK)
-			dap->select = DP_SELECT_INVALID;
+		dap->select_valid = (retval == ERROR_OK);
+		dap->select_dpbanksel_valid = dap->select_valid;
 
 		return retval;
 	}
 
+	if (reg == DP_SELECT1)
+		dap->select = ((uint64_t)data << 32) | (dap->select & 0xffffffffull);
+
 	retval = swd_queue_dp_bankselect(dap, reg);
-	if (retval != ERROR_OK)
-		return retval;
+	if (retval == ERROR_OK) {
+		swd->write_reg(swd_cmd(false, false, reg), data, 0);
+
+		retval = check_sync(dap);
+	}
 
-	swd->write_reg(swd_cmd(false, false, reg), data, 0);
+	if (reg == DP_SELECT1)
+		dap->select1_valid = (retval == ERROR_OK);
 
-	return check_sync(dap);
+	return retval;
 }
 
 
@@ -176,20 +189,26 @@ static int swd_multidrop_select_inner(struct adiv5_dap *dap, uint32_t *dpidr_ptr
 
 	assert(dap_is_multidrop(dap));
 
-	swd_send_sequence(dap, LINE_RESET);
-	/* From ARM IHI 0074C ADIv6.0, chapter B4.3.3 "Connection and line reset
-	 * sequence":
-	 * - line reset sets DP_SELECT_DPBANK to zero;
-	 * - read of DP_DPIDR takes the connection out of reset;
-	 * - write of DP_TARGETSEL keeps the connection in reset;
-	 * - other accesses return protocol error (SWDIO not driven by target).
-	 *
-	 * Read DP_DPIDR to get out of reset. Initialize dap->select to zero to
-	 * skip the write to DP_SELECT, avoiding the protocol error. Set again
-	 * dap->select to DP_SELECT_INVALID because the rest of the register is
-	 * unknown after line reset.
+	/* Send JTAG_TO_DORMANT and DORMANT_TO_SWD just once
+	 * and then use shorter LINE_RESET until communication fails */
+	if (!swd_multidrop_in_swd_state) {
+		swd_send_sequence(dap, JTAG_TO_DORMANT);
+		swd_send_sequence(dap, DORMANT_TO_SWD);
+	} else {
+		swd_send_sequence(dap, LINE_RESET);
+	}
+
+	/*
+	 * Zero dap->select and set dap->select_dpbanksel_valid
+	 * to skip the write to DP_SELECT before DPIDR read, avoiding
+	 * the protocol error.
+	 * Clear the other validity flags because the rest of the DP
+	 * SELECT and SELECT1 registers is unknown after line reset.
 	 */
 	dap->select = 0;
+	dap->select_dpbanksel_valid = true;
+	dap->select_valid = false;
+	dap->select1_valid = false;
 
 	retval = swd_queue_dp_write_inner(dap, DP_TARGETSEL, dap->multidrop_targetsel);
 	if (retval != ERROR_OK)
@@ -209,8 +228,6 @@ static int swd_multidrop_select_inner(struct adiv5_dap *dap, uint32_t *dpidr_ptr
 			return retval;
 	}
 
-	dap->select = DP_SELECT_INVALID;
-
 	retval = swd_queue_dp_read_inner(dap, DP_DLPIDR, &dlpidr);
 	if (retval != ERROR_OK)
 		return retval;
@@ -238,6 +255,7 @@ static int swd_multidrop_select_inner(struct adiv5_dap *dap, uint32_t *dpidr_ptr
 
 	LOG_DEBUG_IO("Selected DP_TARGETSEL 0x%08" PRIx32, dap->multidrop_targetsel);
 	swd_multidrop_selected_dap = dap;
+	swd_multidrop_in_swd_state = true;
 
 	if (dpidr_ptr)
 		*dpidr_ptr = dpidr;
@@ -287,8 +305,9 @@ static int swd_connect_multidrop(struct adiv5_dap *dap)
 	int64_t timeout = timeval_ms() + 500;
 
 	do {
-		swd_send_sequence(dap, JTAG_TO_DORMANT);
-		swd_send_sequence(dap, DORMANT_TO_SWD);
+		/* Do not make any assumptions about SWD state in case of reconnect */
+		if (dap->do_reconnect)
+			swd_multidrop_in_swd_state = false;
 
 		/* Clear link state, including the SELECT cache. */
 		dap->do_reconnect = false;
@@ -299,6 +318,7 @@ static int swd_connect_multidrop(struct adiv5_dap *dap)
 		if (retval == ERROR_OK)
 			break;
 
+		swd_multidrop_in_swd_state = false;
 		alive_sleep(1);
 
 	} while (timeval_ms() < timeout);
@@ -309,6 +329,7 @@ static int swd_connect_multidrop(struct adiv5_dap *dap)
 		return retval;
 	}
 
+	swd_multidrop_in_swd_state = true;
 	LOG_INFO("SWD DPIDR 0x%08" PRIx32 ", DLPIDR 0x%08" PRIx32,
 			  dpidr, dlpidr);
 
@@ -335,19 +356,20 @@ static int swd_connect_single(struct adiv5_dap *dap)
 
 		/* The sequences to enter in SWD (JTAG_TO_SWD and DORMANT_TO_SWD) end
 		 * with a SWD line reset sequence (50 clk with SWDIO high).
-		 * From ARM IHI 0074C ADIv6.0, chapter B4.3.3 "Connection and line reset
-		 * sequence":
-		 * - line reset sets DP_SELECT_DPBANK to zero;
+		 * From ARM IHI 0031F ADIv5.2 and ARM IHI 0074C ADIv6.0,
+		 * chapter B4.3.3 "Connection and line reset sequence":
+		 * - DPv3 (ADIv6) only: line reset sets DP_SELECT_DPBANK to zero;
 		 * - read of DP_DPIDR takes the connection out of reset;
 		 * - write of DP_TARGETSEL keeps the connection in reset;
 		 * - other accesses return protocol error (SWDIO not driven by target).
 		 *
-		 * Read DP_DPIDR to get out of reset. Initialize dap->select to zero to
-		 * skip the write to DP_SELECT, avoiding the protocol error. Set again
-		 * dap->select to DP_SELECT_INVALID because the rest of the register is
-		 * unknown after line reset.
+		 * dap_invalidate_cache() sets dap->select to zero and all validity
+		 * flags to invalid. Set dap->select_dpbanksel_valid only
+		 * to skip the write to DP_SELECT, avoiding the protocol error.
+		 * Read DP_DPIDR to get out of reset.
 		 */
-		dap->select = 0;
+		dap->select_dpbanksel_valid = true;
+
 		retval = swd_queue_dp_read_inner(dap, DP_DPIDR, &dpidr);
 		if (retval == ERROR_OK) {
 			retval = swd_run_inner(dap);
@@ -360,8 +382,6 @@ static int swd_connect_single(struct adiv5_dap *dap)
 		dap->switch_through_dormant = !dap->switch_through_dormant;
 	} while (timeval_ms() < timeout);
 
-	dap->select = DP_SELECT_INVALID;
-
 	if (retval != ERROR_OK) {
 		LOG_ERROR("Error connecting DP: cannot read IDR");
 		return retval;
@@ -386,6 +406,13 @@ static int swd_connect_single(struct adiv5_dap *dap)
 	return retval;
 }
 
+static int swd_pre_connect(struct adiv5_dap *dap)
+{
+	swd_multidrop_in_swd_state = false;
+
+	return ERROR_OK;
+}
+
 static int swd_connect(struct adiv5_dap *dap)
 {
 	int status;
@@ -494,49 +521,55 @@ static int swd_queue_dp_write(struct adiv5_dap *dap, unsigned reg,
 	return swd_queue_dp_write_inner(dap, reg, data);
 }
 
-/** Select the AP register bank matching bits 7:4 of reg. */
+/** Select the AP register bank */
 static int swd_queue_ap_bankselect(struct adiv5_ap *ap, unsigned reg)
 {
 	int retval;
 	struct adiv5_dap *dap = ap->dap;
 	uint64_t sel;
 
-	if (is_adiv6(dap)) {
+	if (is_adiv6(dap))
 		sel = ap->ap_num | (reg & 0x00000FF0);
-		if (sel == (dap->select & ~0xfULL))
-			return ERROR_OK;
-
-		if (dap->select != DP_SELECT_INVALID)
-			sel |= dap->select & 0xf;
-		dap->select = sel;
-		LOG_DEBUG("AP BANKSEL: %" PRIx64, sel);
-
-		retval = swd_queue_dp_write(dap, DP_SELECT, (uint32_t)sel);
+	else
+		sel = (ap->ap_num << 24) | (reg & ADIV5_DP_SELECT_APBANK);
 
-		if (retval == ERROR_OK && dap->asize > 32)
-			retval = swd_queue_dp_write(dap, DP_SELECT1, (uint32_t)(sel >> 32));
+	uint64_t sel_diff = (sel ^ dap->select) & SELECT_AP_MASK;
 
-		if (retval != ERROR_OK)
-			dap->select = DP_SELECT_INVALID;
+	bool set_select = !dap->select_valid || (sel_diff & 0xffffffffull);
+	bool set_select1 = is_adiv6(dap) && dap->asize > 32
+						&& (!dap->select1_valid
+							|| sel_diff & (0xffffffffull << 32));
 
-		return retval;
+	if (set_select && set_select1) {
+		/* Prepare DP bank for DP_SELECT1 now to save one write */
+		sel |= (DP_SELECT1 & 0x000000f0) >> 4;
+	} else {
+		/* Use the DP part of dap->select regardless of dap->select_valid:
+		 * if !dap->select_valid
+		 * dap->select contains a speculative value likely going to be used
+		 * in the following swd_queue_dp_bankselect().
+		 * Moreover dap->select_valid should never be false here as a DP bank
+		 * is always selected before selecting an AP bank */
+		sel |= dap->select & DP_SELECT_DPBANK;
 	}
 
-	/* ADIv5 */
-	sel = (ap->ap_num << 24) | (reg & ADIV5_DP_SELECT_APBANK);
-	if (dap->select != DP_SELECT_INVALID)
-		sel |= dap->select & DP_SELECT_DPBANK;
+	if (set_select) {
+		LOG_DEBUG_IO("AP BANK SELECT: %" PRIx32, (uint32_t)sel);
 
-	if (sel == dap->select)
-		return ERROR_OK;
+		retval = swd_queue_dp_write(dap, DP_SELECT, (uint32_t)sel);
+		if (retval != ERROR_OK)
+			return retval;
+	}
 
-	dap->select = sel;
+	if (set_select1) {
+		LOG_DEBUG_IO("AP BANK SELECT1: %" PRIx32, (uint32_t)(sel >> 32));
 
-	retval = swd_queue_dp_write_inner(dap, DP_SELECT, sel);
-	if (retval != ERROR_OK)
-		dap->select = DP_SELECT_INVALID;
+		retval = swd_queue_dp_write(dap, DP_SELECT1, (uint32_t)(sel >> 32));
+		if (retval != ERROR_OK)
+			return retval;
+	}
 
-	return retval;
+	return ERROR_OK;
 }
 
 static int swd_queue_ap_read(struct adiv5_ap *ap, unsigned reg,
@@ -615,7 +648,12 @@ static void swd_quit(struct adiv5_dap *dap)
 
 	done = true;
 	if (dap_is_multidrop(dap)) {
+		/* Emit the switch seq to dormant state regardless the state mirrored
+		 * in swd_multidrop_in_swd_state. Doing so ensures robust operation
+		 * in the case the variable is out of sync.
+		 * Sending SWD_TO_DORMANT makes no change if the DP is already dormant. */
 		swd->switch_seq(SWD_TO_DORMANT);
+		swd_multidrop_in_swd_state = false;
 		/* Revisit!
 		 * Leaving DPs in dormant state was tested and offers some safety
 		 * against DPs mismatch in case of unintentional use of non-multidrop SWD.
@@ -636,6 +674,7 @@ static void swd_quit(struct adiv5_dap *dap)
 }
 
 const struct dap_ops swd_dap_ops = {
+	.pre_connect_init = swd_pre_connect,
 	.connect = swd_connect,
 	.send_sequence = swd_send_sequence,
 	.queue_dp_read = swd_queue_dp_read,
@@ -666,6 +705,7 @@ static const struct command_registration swd_commands[] = {
 			"['-ignore-version'] "
 			"['-ignore-bypass'] "
 			"['-ircapture' number] "
+			"['-ir-bypass' number] "
 			"['-mask' number]",
 	},
 	COMMAND_REGISTRATION_DONE
diff --git a/src/target/arm_adi_v5.c b/src/target/arm_adi_v5.c
index da5da31..ff12658 100644
--- a/src/target/arm_adi_v5.c
+++ b/src/target/arm_adi_v5.c
@@ -50,7 +50,8 @@
 /*
  * Relevant specifications from ARM include:
  *
- * ARM(tm) Debug Interface v5 Architecture Specification    ARM IHI 0031E
+ * ARM(tm) Debug Interface v5 Architecture Specification    ARM IHI 0031F
+ * ARM(tm) Debug Interface v6 Architecture Specification    ARM IHI 0074C
  * CoreSight(tm) v1.0 Architecture Specification            ARM IHI 0029B
  *
  * CoreSight(tm) DAP-Lite TRM, ARM DDI 0316D
@@ -164,6 +165,12 @@ static uint32_t mem_ap_get_tar_increment(struct adiv5_ap *ap)
 			return 2;
 		case CSW_32BIT:
 			return 4;
+		case CSW_64BIT:
+			return 8;
+		case CSW_128BIT:
+			return 16;
+		case CSW_256BIT:
+			return 32;
 		default:
 			return 0;
 		}
@@ -320,11 +327,144 @@ int mem_ap_write_atomic_u32(struct adiv5_ap *ap, target_addr_t address,
 }
 
 /**
+ * Queue transactions setting up transfer parameters for the
+ * currently selected MEM-AP. If transfer size or packing
+ * has not been probed, run the queue, read back CSW and check if the requested
+ * transfer mode is supported.
+ *
+ * @param ap The MEM-AP.
+ * @param size Transfer width in bytes. Corresponding CSW.Size will be set.
+ * @param address Transfer address, MEM-AP TAR will be set to this value.
+ * @param addrinc TAR will be autoincremented.
+ * @param pack Try to setup packed transfer.
+ * @param this_size Points to a variable set to the size of single transfer
+ *		or to 4 when transferring packed bytes or halfwords
+ *
+ * @return ERROR_OK if the transaction was properly queued, else a fault code.
+ */
+static int mem_ap_setup_transfer_verify_size_packing(struct adiv5_ap *ap,
+	unsigned int size, target_addr_t address,
+	bool addrinc, bool pack, unsigned int *this_size)
+{
+	int retval;
+	uint32_t csw_size;
+
+	switch (size) {
+	case 1:
+		csw_size = CSW_8BIT;
+		break;
+	case 2:
+		csw_size = CSW_16BIT;
+		break;
+	case 4:
+		csw_size = CSW_32BIT;
+		break;
+	case 8:
+		csw_size = CSW_64BIT;
+		break;
+	case 16:
+		csw_size = CSW_128BIT;
+		break;
+	case 32:
+		csw_size = CSW_256BIT;
+		break;
+	default:
+		LOG_ERROR("Size %u not supported", size);
+		return ERROR_TARGET_SIZE_NOT_SUPPORTED;
+	}
+
+	if (!addrinc || size >= 4
+			|| (ap->packed_transfers_probed && !ap->packed_transfers_supported)
+			|| max_tar_block_size(ap->tar_autoincr_block, address) < 4)
+		pack = false;
+
+	uint32_t csw_addrinc = pack ? CSW_ADDRINC_PACKED :
+		 addrinc ? CSW_ADDRINC_SINGLE : CSW_ADDRINC_OFF;
+	retval = mem_ap_setup_csw(ap, csw_size | csw_addrinc);
+	if (retval != ERROR_OK)
+		return retval;
+
+	bool do_probe = !(ap->csw_size_probed_mask & size)
+		 || (pack && !ap->packed_transfers_probed);
+	if (do_probe) {
+		uint32_t csw_readback;
+		retval = dap_queue_ap_read(ap, MEM_AP_REG_CSW(ap->dap), &csw_readback);
+		if (retval != ERROR_OK)
+			return retval;
+
+		retval = dap_run(ap->dap);
+		if (retval != ERROR_OK)
+			return retval;
+
+		bool size_supported = ((csw_readback & CSW_SIZE_MASK) == csw_size);
+		LOG_DEBUG("AP#0x%" PRIx64 " probed size %u: %s", ap->ap_num, size,
+			size_supported ? "supported" : "not supported");
+		ap->csw_size_probed_mask |= size;
+		if (size_supported) {
+			ap->csw_size_supported_mask |= size;
+			if (pack && !ap->packed_transfers_probed) {
+				ap->packed_transfers_probed = true;
+				ap->packed_transfers_supported =
+					((csw_readback & CSW_ADDRINC_MASK) == csw_addrinc);
+				LOG_DEBUG("probed packing: %s",
+					ap->packed_transfers_supported ? "supported" : "not supported");
+			}
+		}
+	}
+
+	if (!(ap->csw_size_supported_mask & size)) {
+		LOG_ERROR("Size %u not supported", size);
+		return ERROR_TARGET_SIZE_NOT_SUPPORTED;
+	}
+
+	if (pack && !ap->packed_transfers_supported)
+		return ERROR_TARGET_PACKING_NOT_SUPPORTED;
+
+	*this_size = pack ? 4 : size;
+
+	return mem_ap_setup_tar(ap, address);
+}
+
+/**
+ * Queue transactions setting up transfer parameters for the
+ * currently selected MEM-AP. If transfer size or packing
+ * has not been probed, run the queue, read back CSW and check if the requested
+ * transfer mode is supported.
+ * If packing is not supported fallback and prepare CSW for unpacked transfer.
+ *
+ * @param ap The MEM-AP.
+ * @param size Transfer width in bytes. Corresponding CSW.Size will be set.
+ * @param address Transfer address, MEM-AP TAR will be set to this value.
+ * @param addrinc TAR will be autoincremented.
+ * @param pack Try to setup packed transfer.
+ * @param this_size Points to a variable set to the size of single transfer
+ *		or to 4 when transferring packed bytes or halfwords
+ *
+ * @return ERROR_OK if the transaction was properly queued, else a fault code.
+ */
+static int mem_ap_setup_transfer_verify_size_packing_fallback(struct adiv5_ap *ap,
+	unsigned int size, target_addr_t address,
+	bool addrinc, bool pack, unsigned int *this_size)
+{
+	int retval = mem_ap_setup_transfer_verify_size_packing(ap,
+					size, address,
+					addrinc, pack, this_size);
+	if (retval == ERROR_TARGET_PACKING_NOT_SUPPORTED) {
+		/* Retry without packing */
+		retval = mem_ap_setup_transfer_verify_size_packing(ap,
+					size, address,
+					addrinc, false, this_size);
+	}
+	return retval;
+}
+
+/**
  * Synchronous write of a block of memory, using a specific access size.
  *
  * @param ap The MEM-AP to access.
  * @param buffer The data buffer to write. No particular alignment is assumed.
- * @param size Which access size to use, in bytes. 1, 2 or 4.
+ * @param size Which access size to use, in bytes. 1, 2, or 4.
+ *	If large data extension is available also accepts sizes 8, 16, 32.
  * @param count The number of writes to do (in size units, not bytes).
  * @param address Address to be written; it must be writable by the currently selected MEM-AP.
  * @param addrinc Whether the target address should be increased for each write or not. This
@@ -336,9 +476,6 @@ static int mem_ap_write(struct adiv5_ap *ap, const uint8_t *buffer, uint32_t siz
 {
 	struct adiv5_dap *dap = ap->dap;
 	size_t nbytes = size * count;
-	const uint32_t csw_addrincr = addrinc ? CSW_ADDRINC_SINGLE : CSW_ADDRINC_OFF;
-	uint32_t csw_size;
-	target_addr_t addr_xor;
 	int retval = ERROR_OK;
 
 	/* TI BE-32 Quirks mode:
@@ -353,103 +490,85 @@ static int mem_ap_write(struct adiv5_ap *ap, const uint8_t *buffer, uint32_t siz
 	 * To make writes of size < 4 work as expected, we xor a value with the address before
 	 * setting the TAP, and we set the TAP after every transfer rather then relying on
 	 * address increment. */
-
-	if (size == 4) {
-		csw_size = CSW_32BIT;
-		addr_xor = 0;
-	} else if (size == 2) {
-		csw_size = CSW_16BIT;
-		addr_xor = dap->ti_be_32_quirks ? 2 : 0;
-	} else if (size == 1) {
-		csw_size = CSW_8BIT;
-		addr_xor = dap->ti_be_32_quirks ? 3 : 0;
-	} else {
-		return ERROR_TARGET_UNALIGNED_ACCESS;
+	target_addr_t ti_be_addr_xor = 0;
+	target_addr_t ti_be_lane_xor = 0;
+	if (dap->ti_be_32_quirks) {
+		ti_be_lane_xor = 3;
+		switch (size) {
+		case 1:
+			ti_be_addr_xor = 3;
+			break;
+		case 2:
+			ti_be_addr_xor = 2;
+			break;
+		case 4:
+			break;
+		default:
+			LOG_ERROR("Write more than 32 bits not supported with ti_be_32_quirks");
+			return ERROR_TARGET_SIZE_NOT_SUPPORTED;
+		}
 	}
 
 	if (ap->unaligned_access_bad && (address % size != 0))
 		return ERROR_TARGET_UNALIGNED_ACCESS;
 
-	while (nbytes > 0) {
-		uint32_t this_size = size;
+	/* Nuvoton NPCX quirks prevent packed writes */
+	bool pack = !dap->nu_npcx_quirks;
 
-		/* Select packed transfer if possible */
-		if (addrinc && ap->packed_transfers && nbytes >= 4
-				&& max_tar_block_size(ap->tar_autoincr_block, address) >= 4) {
-			this_size = 4;
-			retval = mem_ap_setup_csw(ap, csw_size | CSW_ADDRINC_PACKED);
-		} else {
-			retval = mem_ap_setup_csw(ap, csw_size | csw_addrincr);
-		}
-
-		if (retval != ERROR_OK)
-			break;
-
-		retval = mem_ap_setup_tar(ap, address ^ addr_xor);
+	while (nbytes > 0) {
+		unsigned int this_size;
+		retval = mem_ap_setup_transfer_verify_size_packing_fallback(ap,
+					size, address ^ ti_be_addr_xor,
+					addrinc, pack && nbytes >= 4, &this_size);
 		if (retval != ERROR_OK)
 			return retval;
 
 		/* How many source bytes each transfer will consume, and their location in the DRW,
 		 * depends on the type of transfer and alignment. See ARM document IHI0031C. */
-		uint32_t outvalue = 0;
 		uint32_t drw_byte_idx = address;
-		if (dap->ti_be_32_quirks) {
-			switch (this_size) {
-			case 4:
-				outvalue |= (uint32_t)*buffer++ << 8 * (3 ^ (drw_byte_idx++ & 3) ^ addr_xor);
-				outvalue |= (uint32_t)*buffer++ << 8 * (3 ^ (drw_byte_idx++ & 3) ^ addr_xor);
-				outvalue |= (uint32_t)*buffer++ << 8 * (3 ^ (drw_byte_idx++ & 3) ^ addr_xor);
-				outvalue |= (uint32_t)*buffer++ << 8 * (3 ^ (drw_byte_idx & 3) ^ addr_xor);
-				break;
-			case 2:
-				outvalue |= (uint32_t)*buffer++ << 8 * (1 ^ (drw_byte_idx++ & 3) ^ addr_xor);
-				outvalue |= (uint32_t)*buffer++ << 8 * (1 ^ (drw_byte_idx & 3) ^ addr_xor);
-				break;
-			case 1:
-				outvalue |= (uint32_t)*buffer++ << 8 * (0 ^ (drw_byte_idx & 3) ^ addr_xor);
-				break;
+		unsigned int drw_ops = DIV_ROUND_UP(this_size, 4);
+
+		while (drw_ops--) {
+			uint32_t outvalue = 0;
+			if (dap->nu_npcx_quirks && this_size <= 2) {
+				switch (this_size) {
+				case 2:
+					{
+						/* Alternate low and high byte to all byte lanes */
+						uint32_t low = *buffer++;
+						uint32_t high = *buffer++;
+						outvalue |= low << 8 * (drw_byte_idx++ & 3);
+						outvalue |= high << 8 * (drw_byte_idx++ & 3);
+						outvalue |= low << 8 * (drw_byte_idx++ & 3);
+						outvalue |= high << 8 * (drw_byte_idx & 3);
+					}
+					break;
+				case 1:
+					{
+						/* Mirror output byte to all byte lanes */
+						uint32_t data = *buffer++;
+						outvalue |= data;
+						outvalue |= data << 8;
+						outvalue |= data << 16;
+						outvalue |= data << 24;
+					}
+				}
+			} else {
+				unsigned int drw_bytes = MIN(this_size, 4);
+				while (drw_bytes--)
+					outvalue |= (uint32_t)*buffer++ <<
+								8 * ((drw_byte_idx++ & 3) ^ ti_be_lane_xor);
 			}
-		} else if (dap->nu_npcx_quirks) {
-			switch (this_size) {
-			case 4:
-				outvalue |= (uint32_t)*buffer++ << 8 * (drw_byte_idx++ & 3);
-				outvalue |= (uint32_t)*buffer++ << 8 * (drw_byte_idx++ & 3);
-				outvalue |= (uint32_t)*buffer++ << 8 * (drw_byte_idx++ & 3);
-				outvalue |= (uint32_t)*buffer++ << 8 * (drw_byte_idx & 3);
-				break;
-			case 2:
-				outvalue |= (uint32_t)*buffer << 8 * (drw_byte_idx++ & 3);
-				outvalue |= (uint32_t)*(buffer+1) << 8 * (drw_byte_idx++ & 3);
-				outvalue |= (uint32_t)*buffer++ << 8 * (drw_byte_idx++ & 3);
-				outvalue |= (uint32_t)*buffer++ << 8 * (drw_byte_idx & 3);
+
+			retval = dap_queue_ap_write(ap, MEM_AP_REG_DRW(dap), outvalue);
+			if (retval != ERROR_OK)
 				break;
-			case 1:
-				outvalue |= (uint32_t)*buffer << 8 * (drw_byte_idx++ & 3);
-				outvalue |= (uint32_t)*buffer << 8 * (drw_byte_idx++ & 3);
-				outvalue |= (uint32_t)*buffer << 8 * (drw_byte_idx++ & 3);
-				outvalue |= (uint32_t)*buffer++ << 8 * (drw_byte_idx & 3);
-			}
-		} else {
-			switch (this_size) {
-			case 4:
-				outvalue |= (uint32_t)*buffer++ << 8 * (drw_byte_idx++ & 3);
-				outvalue |= (uint32_t)*buffer++ << 8 * (drw_byte_idx++ & 3);
-				/* fallthrough */
-			case 2:
-				outvalue |= (uint32_t)*buffer++ << 8 * (drw_byte_idx++ & 3);
-				/* fallthrough */
-			case 1:
-				outvalue |= (uint32_t)*buffer++ << 8 * (drw_byte_idx & 3);
-			}
 		}
-
-		nbytes -= this_size;
-
-		retval = dap_queue_ap_write(ap, MEM_AP_REG_DRW(dap), outvalue);
 		if (retval != ERROR_OK)
 			break;
 
 		mem_ap_update_tar_cache(ap);
+		nbytes -= this_size;
 		if (addrinc)
 			address += this_size;
 	}
@@ -474,7 +593,8 @@ static int mem_ap_write(struct adiv5_ap *ap, const uint8_t *buffer, uint32_t siz
  *
  * @param ap The MEM-AP to access.
  * @param buffer The data buffer to receive the data. No particular alignment is assumed.
- * @param size Which access size to use, in bytes. 1, 2 or 4.
+ * @param size Which access size to use, in bytes. 1, 2, or 4.
+ *	If large data extension is available also accepts sizes 8, 16, 32.
  * @param count The number of reads to do (in size units, not bytes).
  * @param adr Address to be read; it must be readable by the currently selected MEM-AP.
  * @param addrinc Whether the target address should be increased after each read or not. This
@@ -486,8 +606,6 @@ static int mem_ap_read(struct adiv5_ap *ap, uint8_t *buffer, uint32_t size, uint
 {
 	struct adiv5_dap *dap = ap->dap;
 	size_t nbytes = size * count;
-	const uint32_t csw_addrincr = addrinc ? CSW_ADDRINC_SINGLE : CSW_ADDRINC_OFF;
-	uint32_t csw_size;
 	target_addr_t address = adr;
 	int retval = ERROR_OK;
 
@@ -496,16 +614,12 @@ static int mem_ap_read(struct adiv5_ap *ap, uint8_t *buffer, uint32_t size, uint
 	 * They read from the physical address requested, but with DRW byte-reversed.
 	 * For example, a byte read from address 0 will place the result in the high bytes of DRW.
 	 * Also, packed 8-bit and 16-bit transfers seem to sometimes return garbage in some bytes,
-	 * so avoid them. */
+	 * so avoid them (ap->packed_transfers is forced to false in mem_ap_init). */
 
-	if (size == 4)
-		csw_size = CSW_32BIT;
-	else if (size == 2)
-		csw_size = CSW_16BIT;
-	else if (size == 1)
-		csw_size = CSW_8BIT;
-	else
-		return ERROR_TARGET_UNALIGNED_ACCESS;
+	if (dap->ti_be_32_quirks && size > 4) {
+		LOG_ERROR("Read more than 32 bits not supported with ti_be_32_quirks");
+		return ERROR_TARGET_SIZE_NOT_SUPPORTED;
+	}
 
 	if (ap->unaligned_access_bad && (adr % size != 0))
 		return ERROR_TARGET_UNALIGNED_ACCESS;
@@ -513,7 +627,8 @@ static int mem_ap_read(struct adiv5_ap *ap, uint8_t *buffer, uint32_t size, uint
 	/* Allocate buffer to hold the sequence of DRW reads that will be made. This is a significant
 	 * over-allocation if packed transfers are going to be used, but determining the real need at
 	 * this point would be messy. */
-	uint32_t *read_buf = calloc(count, sizeof(uint32_t));
+	uint32_t *read_buf = calloc(count, MAX(sizeof(uint32_t), size));
+
 	/* Multiplication count * sizeof(uint32_t) may overflow, calloc() is safe */
 	uint32_t *read_ptr = read_buf;
 	if (!read_buf) {
@@ -525,26 +640,20 @@ static int mem_ap_read(struct adiv5_ap *ap, uint8_t *buffer, uint32_t size, uint
 	 * useful bytes it contains, and their location in the word, depends on the type of transfer
 	 * and alignment. */
 	while (nbytes > 0) {
-		uint32_t this_size = size;
-
-		/* Select packed transfer if possible */
-		if (addrinc && ap->packed_transfers && nbytes >= 4
-				&& max_tar_block_size(ap->tar_autoincr_block, address) >= 4) {
-			this_size = 4;
-			retval = mem_ap_setup_csw(ap, csw_size | CSW_ADDRINC_PACKED);
-		} else {
-			retval = mem_ap_setup_csw(ap, csw_size | csw_addrincr);
-		}
+		unsigned int this_size;
+		retval = mem_ap_setup_transfer_verify_size_packing_fallback(ap,
+					size, address,
+					addrinc, nbytes >= 4, &this_size);
 		if (retval != ERROR_OK)
 			break;
 
-		retval = mem_ap_setup_tar(ap, address);
-		if (retval != ERROR_OK)
-			break;
 
-		retval = dap_queue_ap_read(ap, MEM_AP_REG_DRW(dap), read_ptr++);
-		if (retval != ERROR_OK)
-			break;
+		unsigned int drw_ops = DIV_ROUND_UP(this_size, 4);
+		while (drw_ops--) {
+			retval = dap_queue_ap_read(ap, MEM_AP_REG_DRW(dap), read_ptr++);
+			if (retval != ERROR_OK)
+				break;
+		}
 
 		nbytes -= this_size;
 		if (addrinc)
@@ -563,7 +672,9 @@ static int mem_ap_read(struct adiv5_ap *ap, uint8_t *buffer, uint32_t size, uint
 
 	/* If something failed, read TAR to find out how much data was successfully read, so we can
 	 * at least give the caller what we have. */
-	if (retval != ERROR_OK) {
+	if (retval == ERROR_TARGET_SIZE_NOT_SUPPORTED) {
+		nbytes = 0;
+	} else if (retval != ERROR_OK) {
 		target_addr_t tar;
 		if (mem_ap_read_tar(ap, &tar) == ERROR_OK) {
 			/* TAR is incremented after failed transfer on some devices (eg Cortex-M4) */
@@ -576,39 +687,28 @@ static int mem_ap_read(struct adiv5_ap *ap, uint8_t *buffer, uint32_t size, uint
 		}
 	}
 
+	target_addr_t ti_be_lane_xor = dap->ti_be_32_quirks ? 3 : 0;
+
 	/* Replay loop to populate caller's buffer from the correct word and byte lane */
 	while (nbytes > 0) {
-		uint32_t this_size = size;
+		/* Convert transfers longer than 32-bit on word-at-a-time basis */
+		unsigned int this_size = MIN(size, 4);
 
-		if (addrinc && ap->packed_transfers && nbytes >= 4
+		if (size < 4 && addrinc && ap->packed_transfers_supported && nbytes >= 4
 				&& max_tar_block_size(ap->tar_autoincr_block, address) >= 4) {
-			this_size = 4;
+			this_size = 4;	/* Packed read of 4 bytes or 2 halfwords */
 		}
 
-		if (dap->ti_be_32_quirks) {
-			switch (this_size) {
-			case 4:
-				*buffer++ = *read_ptr >> 8 * (3 - (address++ & 3));
-				*buffer++ = *read_ptr >> 8 * (3 - (address++ & 3));
-				/* fallthrough */
-			case 2:
-				*buffer++ = *read_ptr >> 8 * (3 - (address++ & 3));
-				/* fallthrough */
-			case 1:
-				*buffer++ = *read_ptr >> 8 * (3 - (address++ & 3));
-			}
-		} else {
-			switch (this_size) {
-			case 4:
-				*buffer++ = *read_ptr >> 8 * (address++ & 3);
-				*buffer++ = *read_ptr >> 8 * (address++ & 3);
-				/* fallthrough */
-			case 2:
-				*buffer++ = *read_ptr >> 8 * (address++ & 3);
-				/* fallthrough */
-			case 1:
-				*buffer++ = *read_ptr >> 8 * (address++ & 3);
-			}
+		switch (this_size) {
+		case 4:
+			*buffer++ = *read_ptr >> 8 * ((address++ & 3) ^ ti_be_lane_xor);
+			*buffer++ = *read_ptr >> 8 * ((address++ & 3) ^ ti_be_lane_xor);
+			/* fallthrough */
+		case 2:
+			*buffer++ = *read_ptr >> 8 * ((address++ & 3) ^ ti_be_lane_xor);
+			/* fallthrough */
+		case 1:
+			*buffer++ = *read_ptr >> 8 * ((address++ & 3) ^ ti_be_lane_xor);
 		}
 
 		read_ptr++;
@@ -655,7 +755,11 @@ int mem_ap_write_buf_noincr(struct adiv5_ap *ap,
  */
 void dap_invalidate_cache(struct adiv5_dap *dap)
 {
-	dap->select = DP_SELECT_INVALID;
+	dap->select = 0;	/* speculate the first AP access will select AP 0, bank 0 */
+	dap->select_valid = false;
+	dap->select1_valid = false;
+	dap->select_dpbanksel_valid = false;
+
 	dap->last_read = NULL;
 
 	int i;
@@ -783,7 +887,7 @@ int dap_dp_init_or_reconnect(struct adiv5_dap *dap)
 int mem_ap_init(struct adiv5_ap *ap)
 {
 	/* check that we support packed transfers */
-	uint32_t csw, cfg;
+	uint32_t cfg;
 	int retval;
 	struct adiv5_dap *dap = ap->dap;
 
@@ -800,30 +904,23 @@ int mem_ap_init(struct adiv5_ap *ap)
 	ap->cfg_reg = cfg;
 	ap->tar_valid = false;
 	ap->csw_value = 0;      /* force csw and tar write */
-	retval = mem_ap_setup_transfer(ap, CSW_8BIT | CSW_ADDRINC_PACKED, 0);
-	if (retval != ERROR_OK)
-		return retval;
 
-	retval = dap_queue_ap_read(ap, MEM_AP_REG_CSW(dap), &csw);
-	if (retval != ERROR_OK)
-		return retval;
-
-	retval = dap_run(dap);
-	if (retval != ERROR_OK)
-		return retval;
+	/* CSW 32-bit size must be supported (IHI 0031F and 0074D). */
+	ap->csw_size_supported_mask = BIT(CSW_32BIT);
+	ap->csw_size_probed_mask = BIT(CSW_32BIT);
 
-	if (csw & CSW_ADDRINC_PACKED)
-		ap->packed_transfers = true;
-	else
-		ap->packed_transfers = false;
+	/* Suppress probing sizes longer than 32 bit if AP has no large data extension */
+	if (!(cfg & MEM_AP_REG_CFG_LD))
+		ap->csw_size_probed_mask |= BIT(CSW_64BIT) | BIT(CSW_128BIT) | BIT(CSW_256BIT);
 
-	/* Packed transfers on TI BE-32 processors do not work correctly in
+	/* Both IHI 0031F and 0074D state: Implementations that support transfers
+	 * smaller than a word must support packed transfers. Unfortunately at least
+	 * Cortex-M0 and Cortex-M0+ do not comply with this rule.
+	 * Probe for packed transfers except we know they are broken.
+	 * Packed transfers on TI BE-32 processors do not work correctly in
 	 * many cases. */
-	if (dap->ti_be_32_quirks)
-		ap->packed_transfers = false;
-
-	LOG_DEBUG("MEM_AP Packed Transfers: %s",
-			ap->packed_transfers ? "enabled" : "disabled");
+	ap->packed_transfers_supported = false;
+	ap->packed_transfers_probed = dap->ti_be_32_quirks ? true : false;
 
 	/* The ARM ADI spec leaves implementation-defined whether unaligned
 	 * memory accesses work, only work partially, or cause a sticky error.
diff --git a/src/target/arm_adi_v5.h b/src/target/arm_adi_v5.h
index e215893..60c161f 100644
--- a/src/target/arm_adi_v5.h
+++ b/src/target/arm_adi_v5.h
@@ -100,7 +100,11 @@
 #define ADIV5_DP_SELECT_APSEL	0xFF000000
 #define ADIV5_DP_SELECT_APBANK	0x000000F0
 #define DP_SELECT_DPBANK		0x0000000F
-#define DP_SELECT_INVALID		0x00FFFF00 /* Reserved bits one */
+/*
+ * Mask of AP ADDR in select cache, concatenating DP SELECT and DP_SELECT1.
+ * In case of ADIv5, the mask contains both APSEL and APBANKSEL fields.
+ */
+#define SELECT_AP_MASK			(~(uint64_t)DP_SELECT_DPBANK)
 
 #define DP_APSEL_MAX			(255) /* Strict limit for ADIv5, number of AP buffers for ADIv6 */
 #define DP_APSEL_INVALID		0xF00 /* more than DP_APSEL_MAX and not ADIv6 aligned 4k */
@@ -161,6 +165,9 @@
 #define CSW_8BIT		0
 #define CSW_16BIT		1
 #define CSW_32BIT		2
+#define CSW_64BIT		3
+#define CSW_128BIT		4
+#define CSW_256BIT		5
 #define CSW_ADDRINC_MASK    (3UL << 4)
 #define CSW_ADDRINC_OFF     0UL
 #define CSW_ADDRINC_SINGLE  (1UL << 4)
@@ -266,6 +273,26 @@ struct adiv5_ap {
 	uint32_t csw_value;
 
 	/**
+	 * Save the supported CSW.Size data types for the MEM-AP.
+	 * Each bit corresponds to a data type.
+	 * 0b1 = Supported data size. 0b0 = Not supported.
+	 * Bit 0 = Byte (8-bits)
+	 * Bit 1 = Halfword (16-bits)
+	 * Bit 2 = Word (32-bits) - always supported by spec.
+	 * Bit 3 = Doubleword (64-bits)
+	 * Bit 4 = 128-bits
+	 * Bit 5 = 256-bits
+	 */
+	uint32_t csw_size_supported_mask;
+	/**
+	 * Probed CSW.Size data types for the MEM-AP.
+	 * Each bit corresponds to a data type.
+	 * 0b1 = Data size has been probed. 0b0 = Not yet probed.
+	 * Bits assigned to sizes same way as above.
+	 */
+	uint32_t csw_size_probed_mask;
+
+	/**
 	 * Cache for (MEM-AP) AP_REG_TAR register value This is written to
 	 * configure the address being read or written
 	 * "-1" indicates no cached value.
@@ -282,7 +309,8 @@ struct adiv5_ap {
 	uint32_t tar_autoincr_block;
 
 	/* true if packed transfers are supported by the MEM-AP */
-	bool packed_transfers;
+	bool packed_transfers_supported;
+	bool packed_transfers_probed;
 
 	/* true if unaligned memory access is not supported by the MEM-AP */
 	bool unaligned_access_bad;
@@ -338,11 +366,21 @@ struct adiv5_dap {
 	/* The current manually selected AP by the "dap apsel" command */
 	uint64_t apsel;
 
+	/** Cache for DP SELECT and SELECT1 (ADIv6) register. */
+	uint64_t select;
+	/** Validity of DP SELECT cache. false will force register rewrite */
+	bool select_valid;
+	bool select1_valid;	/* ADIv6 only */
 	/**
-	 * Cache for DP_SELECT register. A value of DP_SELECT_INVALID
-	 * indicates no cached value and forces rewrite of the register.
+	 * Partial DPBANKSEL validity for SWD only.
+	 * ADIv6 line reset sets DP SELECT DPBANKSEL to zero,
+	 * ADIv5 does not.
+	 * We can rely on it for the banked DP register 0 also on ADIv5
+	 * as ADIv5 has no mapping for DP reg 0 - it is always DPIDR.
+	 * It is important to avoid setting DP SELECT in connection
+	 * reset state before reading DPIDR.
 	 */
-	uint64_t select;
+	bool select_dpbanksel_valid;
 
 	/* information about current pending SWjDP-AHBAP transaction */
 	uint8_t  ack;
@@ -406,6 +444,9 @@ struct adiv5_dap {
  * available until run().
  */
 struct dap_ops {
+	/** Optional; called once on the first enabled dap before connecting */
+	int (*pre_connect_init)(struct adiv5_dap *dap);
+
 	/** connect operation for SWD */
 	int (*connect)(struct adiv5_dap *dap);
 
diff --git a/src/target/arm_dap.c b/src/target/arm_dap.c
index 84cc6c7..9f4afae 100644
--- a/src/target/arm_dap.c
+++ b/src/target/arm_dap.c
@@ -91,6 +91,7 @@ static int dap_init_all(void)
 {
 	struct arm_dap_object *obj;
 	int retval;
+	bool pre_connect = true;
 
 	LOG_DEBUG("Initializing all DAPs ...");
 
@@ -123,6 +124,14 @@ static int dap_init_all(void)
 				is_adiv6(dap) ? "ADIv6" : "ADIv5");
 		}
 
+		if (pre_connect && dap->ops->pre_connect_init) {
+			retval = dap->ops->pre_connect_init(dap);
+			if (retval != ERROR_OK)
+				return retval;
+
+			pre_connect = false;
+		}
+
 		retval = dap->ops->connect(dap);
 		if (retval != ERROR_OK)
 			return retval;
diff --git a/src/target/armv8.c b/src/target/armv8.c
index d197477..daf1ffc 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -943,6 +943,81 @@ int armv8_mmu_translate_va(struct target *target,  target_addr_t va, target_addr
 	return ERROR_OK;
 }
 
+static void armv8_decode_cacheability(int attr)
+{
+	if (attr == 0) {
+		LOG_USER_N("UNPREDICTABLE");
+		return;
+	}
+	if (attr == 4) {
+		LOG_USER_N("Non-cacheable");
+		return;
+	}
+	switch (attr & 0xC) {
+		case 0:
+			LOG_USER_N("Write-Through Transient");
+			break;
+		case 0x4:
+			LOG_USER_N("Write-Back Transient");
+			break;
+		case 0x8:
+			LOG_USER_N("Write-Through Non-transient");
+			break;
+		case 0xC:
+			LOG_USER_N("Write-Back Non-transient");
+			break;
+	}
+	if (attr & 2)
+		LOG_USER_N(" Read-Allocate");
+	else
+		LOG_USER_N(" No-Read Allocate");
+	if (attr & 1)
+		LOG_USER_N(" Write-Allocate");
+	else
+		LOG_USER_N(" No-Write Allocate");
+}
+
+static void armv8_decode_memory_attr(int attr)
+{
+	if (attr == 0x40) {
+		LOG_USER("Normal Memory, Inner Non-cacheable, "
+			 "Outer Non-cacheable, XS=0");
+	} else if (attr == 0xA0) {
+		LOG_USER("Normal Memory, Inner Write-through Cacheable, "
+			 "Outer Write-through Cacheable, Read-Allocate, "
+			 "No-Write Allocate, Non-transient, XS=0");
+	} else if (attr == 0xF0) {
+		LOG_USER("Tagged Normal Memory, Inner Write-Back, "
+			 "Outer Write-Back, Read-Allocate, Write-Allocate, "
+			 "Non-transient");
+	} else if ((attr & 0xF0) == 0) {
+		switch (attr & 0xC) {
+			case 0:
+				LOG_USER_N("Device-nGnRnE Memory");
+				break;
+			case 0x4:
+				LOG_USER_N("Device-nGnRE Memory");
+				break;
+			case 0x8:
+				LOG_USER_N("Device-nGRE Memory");
+				break;
+			case 0xC:
+				LOG_USER_N("Device-GRE Memory");
+				break;
+		}
+		if (attr & 1)
+			LOG_USER(", XS=0");
+		else
+			LOG_USER_N("\n");
+	} else {
+		LOG_USER_N("Normal Memory, Inner ");
+		armv8_decode_cacheability(attr & 0xF);
+		LOG_USER_N(", Outer ");
+		armv8_decode_cacheability(attr >> 4);
+		LOG_USER_N("\n");
+	}
+}
+
 /*  V8 method VA TO PA  */
 int armv8_mmu_translate_va_pa(struct target *target, target_addr_t va,
 	target_addr_t *val, int meminfo)
@@ -1025,11 +1100,9 @@ int armv8_mmu_translate_va_pa(struct target *target, target_addr_t va,
 			int NS = (par >> 9) & 1;
 			int ATTR = (par >> 56) & 0xFF;
 
-			char *memtype = (ATTR & 0xF0) == 0 ? "Device Memory" : "Normal Memory";
-
 			LOG_USER("%sshareable, %s",
 					shared_name[SH], secure_name[NS]);
-			LOG_USER("%s", memtype);
+			armv8_decode_memory_attr(ATTR);
 		}
 	}
 
diff --git a/src/target/cortex_m.c b/src/target/cortex_m.c
index 3eafee0..d9e8b53 100644
--- a/src/target/cortex_m.c
+++ b/src/target/cortex_m.c
@@ -106,6 +106,12 @@ static const struct cortex_m_part_info cortex_m_parts[] = {
 		.flags = CORTEX_M_F_HAS_FPV5,
 	},
 	{
+		.impl_part = CORTEX_M85_PARTNO,
+		.name = "Cortex-M85",
+		.arch = ARM_ARCH_V8M,
+		.flags = CORTEX_M_F_HAS_FPV5,
+	},
+	{
 		.impl_part = STAR_MC1_PARTNO,
 		.name = "STAR-MC1",
 		.arch = ARM_ARCH_V8M,
diff --git a/src/target/cortex_m.h b/src/target/cortex_m.h
index 0bc1399..a585b78 100644
--- a/src/target/cortex_m.h
+++ b/src/target/cortex_m.h
@@ -56,6 +56,7 @@ enum cortex_m_impl_part {
 	CORTEX_M33_PARTNO    = ARM_MAKE_CPUID(ARM_IMPLEMENTOR_ARM, 0xD21),
 	CORTEX_M35P_PARTNO   = ARM_MAKE_CPUID(ARM_IMPLEMENTOR_ARM, 0xD31),
 	CORTEX_M55_PARTNO    = ARM_MAKE_CPUID(ARM_IMPLEMENTOR_ARM, 0xD22),
+	CORTEX_M85_PARTNO    = ARM_MAKE_CPUID(ARM_IMPLEMENTOR_ARM, 0xD23),
 	INFINEON_SLX2_PARTNO = ARM_MAKE_CPUID(ARM_IMPLEMENTOR_INFINEON, 0xDB0),
 	REALTEK_M200_PARTNO  = ARM_MAKE_CPUID(ARM_IMPLEMENTOR_REALTEK, 0xd20),
 	REALTEK_M300_PARTNO  = ARM_MAKE_CPUID(ARM_IMPLEMENTOR_REALTEK, 0xd22),
diff --git a/src/target/image.c b/src/target/image.c
index 9175c20..440fe17 100644
--- a/src/target/image.c
+++ b/src/target/image.c
@@ -24,6 +24,7 @@
 #include "image.h"
 #include "target.h"
 #include <helper/log.h>
+#include <server/server.h>
 
 /* convert ELF header field to host endianness */
 #define field16(elf, field) \
@@ -1295,6 +1296,8 @@ int image_calculate_checksum(const uint8_t *buffer, uint32_t nbytes, uint32_t *c
 			crc = (crc << 8) ^ crc32_table[((crc >> 24) ^ *buffer++) & 255];
 		}
 		keep_alive();
+		if (openocd_is_shutdown_pending())
+			return ERROR_SERVER_INTERRUPTED;
 	}
 
 	LOG_DEBUG("Calculating checksum done; checksum=0x%" PRIx32, crc);
diff --git a/src/target/mips32.c b/src/target/mips32.c
index 5c346c4..8a3ddbf 100644
--- a/src/target/mips32.c
+++ b/src/target/mips32.c
@@ -160,6 +160,7 @@ static const struct {
 
 #define MIPS32_NUM_REGS ARRAY_SIZE(mips32_regs)
 
+
 static int mips32_get_core_reg(struct reg *reg)
 {
 	int retval;
@@ -333,9 +334,7 @@ int mips32_restore_context(struct target *target)
 	}
 
 	/* write core regs */
-	mips32_pracc_write_regs(mips32);
-
-	return ERROR_OK;
+	return mips32_pracc_write_regs(mips32);
 }
 
 int mips32_arch_state(struct target *target)
@@ -764,18 +763,94 @@ static int mips32_read_c0_prid(struct target *target)
 	return retval;
 }
 
-/*
- * Detect processor type and apply required quirks.
+/**
+ * mips32_find_cpu_by_prid - Find CPU information by processor ID.
+ * @param[in] prid: Processor ID of the CPU.
+ *
+ * @brief This function looks up the CPU entry in the mips32_cpu_entry array based on the provided
+ * processor ID. It also handles special cases like AMD/Alchemy CPUs that use Company Options
+ * instead of Processor IDs.
+ *
+ * @return Pointer to the corresponding cpu_entry struct, or the 'unknown' entry if not found.
+ */
+static const struct cpu_entry *mips32_find_cpu_by_prid(uint32_t prid)
+{
+	/* AMD/Alchemy CPU uses Company Options instead of Processor ID.
+	 * Therefore an extra transform step for prid to map it to an assigned ID,
+	 */
+	if ((prid & PRID_COMP_MASK) == PRID_COMP_ALCHEMY) {
+		/* Clears Processor ID field, then put Company Option field to its place */
+		prid = (prid & 0xFFFF00FF) | ((prid & 0xFF000000) >> 16);
+	}
+
+	/* Mask out Company Option */
+	prid &= 0x00FFFFFF;
+
+	for (unsigned int i = 0; i < MIPS32_NUM_CPU_ENTRIES; i++) {
+		const struct cpu_entry *entry = &mips32_cpu_entry[i];
+		if ((entry->prid & MIPS32_CORE_MASK) <= prid && prid <= entry->prid)
+			return entry;
+	}
+
+	/* If nothing matched, then return unknown entry */
+	return &mips32_cpu_entry[MIPS32_NUM_CPU_ENTRIES - 1];
+}
+
+static bool mips32_cpu_is_lexra(struct mips_ejtag *ejtag_info)
+{
+	return (ejtag_info->prid & PRID_COMP_MASK) == PRID_COMP_LEXRA;
+}
+
+static int mips32_cpu_get_release(struct mips_ejtag *ejtag_info)
+{
+	return (ejtag_info->config[0] & MIPS32_CONFIG0_AR_MASK) >> MIPS32_CONFIG0_AR_SHIFT;
+}
+
+/**
+ * mips32_cpu_support_sync - Checks CPU supports ordering
+ * @param[in] ejtag_info: MIPS EJTAG information structure.
+ *
+ * @brief MIPS ISA implemented on Lexra CPUs is MIPS-I, similar to R3000,
+ * which does not have the SYNC instruction alone with unaligned
+ * load/store instructions.
+ *
+ * @returns true if current CPU supports sync instruction(CPU is not Lexra)
+*/
+bool mips32_cpu_support_sync(struct mips_ejtag *ejtag_info)
+{
+	return !mips32_cpu_is_lexra(ejtag_info);
+}
+
+/**
+ * mips32_cpu_support_hazard_barrier - Checks CPU supports hazard barrier
+ * @param[in] ejtag_info: MIPS EJTAG information structure.
+ *
+ * @brief hazard barrier instructions EHB and *.HB was introduced to MIPS from release 2.
+ *
+ * @returns true if current CPU supports hazard barrier(release > 1)
+*/
+bool mips32_cpu_support_hazard_barrier(struct mips_ejtag *ejtag_info)
+{
+	return mips32_cpu_get_release(ejtag_info) > MIPS32_RELEASE_1;
+}
+
+/**
+ * mips32_cpu_probe - Detects processor type and applies necessary quirks.
+ * @param[in] target: The target CPU to probe.
+ *
+ * @brief This function probes the CPU, reads its PRID (Processor ID), and determines the CPU type.
+ * It applies any quirks necessary for specific processor types.
  *
  * NOTE: The proper detection of certain CPUs can become quite complicated.
  * Please consult the following Linux kernel code when adding new CPUs:
  *  arch/mips/include/asm/cpu.h
  *  arch/mips/kernel/cpu-probe.c
+ *
+ * @return ERROR_OK on success; error code on failure.
  */
 int mips32_cpu_probe(struct target *target)
 {
 	struct mips32_common *mips32 = target_to_mips32(target);
-	const char *cpu_name = "unknown";
 	int retval;
 
 	if (mips32->prid)
@@ -785,28 +860,51 @@ int mips32_cpu_probe(struct target *target)
 	if (retval != ERROR_OK)
 		return retval;
 
+	const struct cpu_entry *entry = mips32_find_cpu_by_prid(mips32->prid);
+
 	switch (mips32->prid & PRID_COMP_MASK) {
 	case PRID_COMP_INGENIC_E1:
 		switch (mips32->prid & PRID_IMP_MASK) {
 		case PRID_IMP_XBURST_REV1:
-			cpu_name = "Ingenic XBurst rev1";
 			mips32->cpu_quirks |= EJTAG_QUIRK_PAD_DRET;
 			break;
 		default:
 			break;
 		}
 		break;
+
+	/* Determine which CP0 registers are available in the current processor core */
+	case PRID_COMP_MTI:
+		switch (entry->prid & PRID_IMP_MASK) {
+		case PRID_IMP_MAPTIV_UC:
+			mips32->cp0_mask = MIPS_CP0_MAPTIV_UC;
+			break;
+		case PRID_IMP_MAPTIV_UP:
+		case PRID_IMP_M5150:
+			mips32->cp0_mask = MIPS_CP0_MAPTIV_UP;
+			break;
+		case PRID_IMP_IAPTIV:
+		case PRID_IMP_IAPTIV_CM:
+			mips32->cp0_mask = MIPS_CP0_IAPTIV;
+			break;
+		default:
+			/* CP0 mask should be the same as MK4 by default */
+			mips32->cp0_mask = MIPS_CP0_MK4;
+			break;
+		}
+
 	default:
 		break;
 	}
 
-	LOG_DEBUG("CPU: %s (PRId %08x)", cpu_name, mips32->prid);
+	mips32->cpu_info = entry;
+	LOG_DEBUG("CPU: %s (PRId %08x)", entry->cpu_name, mips32->prid);
 
 	return ERROR_OK;
 }
 
 /* reads dsp implementation info from CP0 Config3 register {DSPP, DSPREV}*/
-void mips32_read_config_dsp(struct mips32_common *mips32, struct mips_ejtag *ejtag_info)
+static void mips32_read_config_dsp(struct mips32_common *mips32, struct mips_ejtag *ejtag_info)
 {
 	uint32_t dsp_present = ((ejtag_info->config[3] & MIPS32_CONFIG3_DSPP_MASK) >> MIPS32_CONFIG3_DSPP_SHIFT);
 	if (dsp_present) {
@@ -818,7 +916,7 @@ void mips32_read_config_dsp(struct mips32_common *mips32, struct mips_ejtag *ejt
 }
 
 /* read fpu implementation info from CP0 Config1 register {CU1, FP}*/
-int mips32_read_config_fpu(struct mips32_common *mips32, struct mips_ejtag *ejtag_info)
+static int mips32_read_config_fpu(struct mips32_common *mips32, struct mips_ejtag *ejtag_info)
 {
 	int retval;
 	uint32_t fp_imp = (ejtag_info->config[1] & MIPS32_CONFIG1_FP_MASK) >> MIPS32_CONFIG1_FP_SHIFT;
@@ -858,8 +956,23 @@ int mips32_read_config_fpu(struct mips32_common *mips32, struct mips_ejtag *ejta
 	return ERROR_OK;
 }
 
-/* Checks if current target implements Common Device Memory Map and therefore Fast Debug Channel (MD00090) */
-void mips32_read_config_fdc(struct mips32_common *mips32, struct mips_ejtag *ejtag_info, uint32_t dcr)
+/**
+ * mips32_read_config_fdc - Read Fast Debug Channel configuration
+ * @param[in,out] mips32: MIPS32 common structure
+ * @param[in] ejtag_info: EJTAG information structure
+ * @param[in] dcr: Device Configuration Register value
+ *
+ * @brief Checks if the current target implements the Common Device Memory Map (CDMM) and Fast Debug Channel (FDC).
+ *
+ * This function examines the configuration registers and the Device Configuration Register (DCR) to determine
+ * if the current MIPS32 target supports the Common Device Memory Map (CDMM) and the Fast Debug Channel (FDC).
+ * If supported, it sets the corresponding flags in the MIPS32 common structure. \n
+ *
+ * NOTE:These are defined on MD00090, page 67 and MD00047F, page 82, respectively.
+ * MIPS Documents are pretty much all available online,
+ * it should pop up first when you search "MDxxxxx"
+ */
+static void mips32_read_config_fdc(struct mips32_common *mips32, struct mips_ejtag *ejtag_info, uint32_t dcr)
 {
 	if (((ejtag_info->config[3] & MIPS32_CONFIG3_CDMM_MASK) != 0) && ((dcr & EJTAG_DCR_FDC) != 0)) {
 		mips32->fdc = 1;
@@ -1113,12 +1226,284 @@ static int mips32_verify_pointer(struct command_invocation *cmd,
 }
 
 /**
- * MIPS32 targets expose command interface
- * to manipulate CP0 registers
+ * mips32_read_config_mmu - Reads MMU configuration and logs relevant information.
+ * @param[in] ejtag_info: EJTAG interface information.
+ *
+ * @brief Reads the MMU configuration from the CP0 register and calculates the number of TLB entries,
+ * ways, and sets. Handles different MMU types like VTLB only, root RPU/Fixed, and VTLB and FTLB.
+ *
+ * @return ERROR_OK on success; error code on failure.
+ */
+static int mips32_read_config_mmu(struct mips_ejtag *ejtag_info)
+{
+	uint32_t config4, tlb_entries = 0, ways = 0, sets = 0;
+	uint32_t config0 = ejtag_info->config[0];
+	uint32_t config1 = ejtag_info->config[1];
+	uint32_t config3 = ejtag_info->config[3];
+	uint32_t mmu_type = (config0 >> 7) & 7;
+	uint32_t vz_present = (config3 & BIT(23));
+
+	int retval = mips32_cp0_read(ejtag_info, &config4, 16, 4);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* mmu type = 1: VTLB only (Note: Does not account for Config4.ExtVTLB)
+	 * mmu type = 3: root RPU/Fixed (Note: Only valid with VZ ASE)
+	 * mmu type = 4: VTLB and FTLB
+	 */
+	if ((mmu_type == 1 || mmu_type == 4) || (mmu_type == 3 && vz_present)) {
+		tlb_entries = (uint32_t)(((config1 >> 25) & 0x3f) + 1);
+		if (mmu_type == 4) {
+			/* Release 6 definition for Config4[0:15] (MD01251, page 243) */
+			/* The FTLB ways field is defined as [2, 3, 4, 5, 6, 7, 8, ...0 (reserved)] */
+			int index = ((config4 >> 4) & 0xf);
+			ways = index > 6 ? 0 : index + 2;
+
+			/* The FTLB sets field is defined as [1, 2, 4, 8, ..., 16384, 32768] (powers of 2) */
+			index = (config4 & 0xf);
+			sets = 1 << index;
+			tlb_entries = tlb_entries + (ways * sets);
+		}
+	}
+	LOG_USER("TLB Entries: %d (%d ways, %d sets per way)", tlb_entries, ways, sets);
+
+	return ERROR_OK;
+}
+
+/**
+ * mips32_cp0_find_register_by_name - Find CP0 register by its name.
+ * @param[in] cp0_mask: Mask to filter out irrelevant registers.
+ * @param[in] reg_name: Name of the register to find.
+ *
+ * @brief This function iterates through mips32_cp0_regs to find a register
+ * matching reg_name, considering cp0_mask to filter out registers
+ * not relevant for the current core.
+ *
+ * @return Pointer to the found register, or NULL if not found.
+ */
+static const struct mips32_cp0 *mips32_cp0_find_register_by_name(uint32_t cp0_mask, const char *reg_name)
+{
+	if (reg_name)
+		for (unsigned int i = 0; i < MIPS32NUMCP0REGS; i++) {
+			if ((mips32_cp0_regs[i].core & cp0_mask) == 0)
+				continue;
+
+			if (strcmp(mips32_cp0_regs[i].name, reg_name) == 0)
+				return &mips32_cp0_regs[i];
+		}
+	return NULL;
+}
+
+/**
+ * mips32_cp0_get_all_regs - Print all CP0 registers and their values.
+ * @param[in] cmd: Command invocation context.
+ * @param[in] ejtag_info: EJTAG interface information.
+ * @param[in] cp0_mask: Mask to identify relevant registers.
+ *
+ * @brief Iterates over all CP0 registers, reads their values, and prints them.
+ * Only considers registers relevant to the current core, as defined by cp0_mask.
+ *
+ * @return ERROR_OK on success; error code on failure.
+ */
+static int mips32_cp0_get_all_regs(struct command_invocation *cmd, struct mips_ejtag *ejtag_info, uint32_t cp0_mask)
+{
+	uint32_t value;
+
+	for (unsigned int i = 0; i < MIPS32NUMCP0REGS; i++) {
+		/* Register name not valid for this core */
+		if ((mips32_cp0_regs[i].core & cp0_mask) == 0)
+			continue;
+
+		int retval = mips32_cp0_read(ejtag_info, &value, mips32_cp0_regs[i].reg, mips32_cp0_regs[i].sel);
+		if (retval != ERROR_OK) {
+			command_print(CMD, "Error: couldn't access reg %s", mips32_cp0_regs[i].name);
+			return retval;
+		}
+
+		command_print(CMD, "%*s: 0x%8.8" PRIx32, 14, mips32_cp0_regs[i].name, value);
+	}
+	return ERROR_OK;
+}
+
+/**
+ * mips32_cp0_get_reg_by_name - Read and print a CP0 register's value by name.
+ * @param[in] cmd: Command invocation context.
+ * @param[in] ejtag_info: EJTAG interface information.
+ * @param[in] cp0_mask: Mask to identify relevant registers.
+ *
+ * @brief Finds a CP0 register by name, reads its value, and prints it.
+ * Handles error scenarios like register not found or read failure.
+ *
+ * @return ERROR_OK on success; error code on failure.
+ */
+static int mips32_cp0_get_reg_by_name(struct command_invocation *cmd, struct mips_ejtag *ejtag_info, uint32_t cp0_mask)
+{
+	const struct mips32_cp0 *cp0_regs = mips32_cp0_find_register_by_name(cp0_mask, CMD_ARGV[0]);
+	if (!cp0_regs) {
+		command_print(CMD, "Error: Register '%s' not found", CMD_ARGV[0]);
+		return ERROR_COMMAND_ARGUMENT_INVALID;
+	}
+
+	uint32_t value;
+	int retval = mips32_cp0_read(ejtag_info, &value, cp0_regs->reg, cp0_regs->sel);
+	if (retval != ERROR_OK) {
+		command_print(CMD, "Error: Encounter an Error while reading cp0 reg %d sel %d",
+					cp0_regs->reg, cp0_regs->sel);
+		return retval;
+	}
+
+	command_print(CMD, "0x%8.8" PRIx32, value);
+	return ERROR_OK;
+}
+
+/**
+ * mips32_cp0_get_reg_by_number - Read and print a CP0 register's value by number.
+ * @param[in] cmd: Command invocation context.
+ * @param[in] ejtag_info: EJTAG interface information.
+ *
+ * @brief Reads a specific CP0 register (identified by number and selection) and prints its value.
+ * The register number and selection are parsed from the command arguments.
+ *
+ * @return ERROR_OK on success; error code on failure.
+ */
+static int mips32_cp0_get_reg_by_number(struct command_invocation *cmd, struct mips_ejtag *ejtag_info)
+{
+	uint32_t cp0_reg, cp0_sel, value;
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], cp0_reg);
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], cp0_sel);
+
+	int retval = mips32_cp0_read(ejtag_info, &value, cp0_reg, cp0_sel);
+	if (retval != ERROR_OK) {
+		command_print(CMD,
+				"Error: couldn't access reg %" PRIu32,
+				cp0_reg);
+		return retval;
+	}
+
+	command_print(CMD, "cp0 reg %" PRIu32 ", select %" PRIu32 ": %8.8" PRIx32,
+			cp0_reg, cp0_sel, value);
+	return ERROR_OK;
+}
+
+/**
+ * mips32_cp0_set_reg_by_name - Write to a CP0 register identified by name.
+ * @param[in] cmd: Command invocation context.
+ * @param[in] mips32: Common MIPS32 data structure.
+ * @param[in] ejtag_info: EJTAG interface information.
+ *
+ * @brief Writes a value to a CP0 register specified by name. Updates internal
+ * cache if specific registers (STATUS, CAUSE, DEPC, GUESTCTL1) are modified.
+ *
+ * @return ERROR_OK on success; error code on failure.
+ */
+static int mips32_cp0_set_reg_by_name(struct command_invocation *cmd,
+		struct mips32_common *mips32, struct mips_ejtag *ejtag_info)
+{
+	const struct mips32_cp0 *cp0_regs = mips32_cp0_find_register_by_name(mips32->cp0_mask, CMD_ARGV[0]);
+	if (!cp0_regs) {
+		command_print(CMD, "Error: Register '%s' not found", CMD_ARGV[0]);
+		return ERROR_COMMAND_ARGUMENT_INVALID;
+	}
+
+
+	uint32_t value;
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], value);
+
+	if (cp0_regs->reg == MIPS32_C0_STATUS && cp0_regs->sel == 0) {
+		/* Update cached Status register if user is writing to Status */
+		mips32->core_regs.cp0[MIPS32_REG_C0_STATUS_INDEX] = value;
+		mips32->core_cache->reg_list[MIPS32_REGLIST_C0_STATUS_INDEX].dirty = 1;
+	} else if (cp0_regs->reg == MIPS32_C0_CAUSE && cp0_regs->sel == 0) {
+		/* Update register cache with new value if its Cause */
+		mips32->core_regs.cp0[MIPS32_REG_C0_CAUSE_INDEX] = value;
+		mips32->core_cache->reg_list[MIPS32_REGLIST_C0_CAUSE_INDEX].dirty = 1;
+	} else if (cp0_regs->reg == MIPS32_C0_DEPC && cp0_regs->sel == 0) {
+		/* Update cached PC if its DEPC */
+		mips32->core_regs.cp0[MIPS32_REG_C0_PC_INDEX] = value;
+		mips32->core_cache->reg_list[MIPS32_REGLIST_C0_PC_INDEX].dirty = 1;
+	} else if (cp0_regs->reg == MIPS32_C0_GUESTCTL1 && cp0_regs->sel == 4) {
+		/* Update cached guestCtl1 */
+		mips32->core_regs.cp0[MIPS32_REG_C0_GUESTCTL1_INDEX] = value;
+		mips32->core_cache->reg_list[MIPS32_REGLIST_C0_GUESTCTL1_INDEX].dirty = 1;
+	}
+
+	int retval = mips32_cp0_write(ejtag_info, value,
+								cp0_regs->reg,
+								cp0_regs->sel);
+	if (retval != ERROR_OK) {
+		command_print(CMD, "Error: Encounter an Error while writing to cp0 reg %d, sel %d",
+					cp0_regs->reg, cp0_regs->sel);
+		return retval;
+	}
+
+	command_print(CMD, "cp0 reg %s (%u, select %u: %8.8" PRIx32 ")",
+			CMD_ARGV[0], cp0_regs->reg, cp0_regs->sel, value);
+	return ERROR_OK;
+}
+
+/**
+ * mips32_cp0_set_reg_by_number - Write to a CP0 register identified by number.
+ * @param[in] cmd: Command invocation context.
+ * @param[in] mips32: Common MIPS32 data structure.
+ * @param[in] ejtag_info: EJTAG interface information.
+ *
+ * @brief Writes a value to a CP0 register specified by number and selection.
+ * Handles special cases like updating the internal cache for certain registers.
+ *
+ * @return ERROR_OK on success; error code on failure.
+ */
+static int mips32_cp0_set_reg_by_number(struct command_invocation *cmd,
+		struct mips32_common *mips32, struct mips_ejtag *ejtag_info)
+{
+	uint32_t cp0_reg, cp0_sel, value;
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], cp0_reg);
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], cp0_sel);
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[2], value);
+
+	if (cp0_reg == MIPS32_C0_STATUS && cp0_sel == 0) {
+		/* Update cached status register if user is writing to Status register */
+		mips32->core_regs.cp0[MIPS32_REG_C0_STATUS_INDEX] = value;
+		mips32->core_cache->reg_list[MIPS32_REGLIST_C0_STATUS_INDEX].dirty = 1;
+	} else if (cp0_reg == MIPS32_C0_CAUSE && cp0_sel == 0) {
+		/* Update register cache with new value if its Cause register */
+		mips32->core_regs.cp0[MIPS32_REG_C0_CAUSE_INDEX] = value;
+		mips32->core_cache->reg_list[MIPS32_REGLIST_C0_CAUSE_INDEX].dirty = 1;
+	} else if (cp0_reg == MIPS32_C0_DEPC && cp0_sel == 0) {
+		/* Update cached PC if its DEPC */
+		mips32->core_regs.cp0[MIPS32_REG_C0_PC_INDEX] = value;
+		mips32->core_cache->reg_list[MIPS32_REGLIST_C0_PC_INDEX].dirty = 1;
+	} else if (cp0_reg == MIPS32_C0_GUESTCTL1 && cp0_sel == 4) {
+		/* Update cached guestCtl1, too */
+		mips32->core_regs.cp0[MIPS32_REG_C0_GUESTCTL1_INDEX] = value;
+		mips32->core_cache->reg_list[MIPS32_REGLIST_C0_GUESTCTL1_INDEX].dirty = 1;
+	}
+
+	int retval = mips32_cp0_write(ejtag_info, value, cp0_reg, cp0_sel);
+	if (retval != ERROR_OK) {
+		command_print(CMD,
+				"Error: couldn't access cp0 reg %" PRIu32 ", select %" PRIu32,
+				cp0_reg,  cp0_sel);
+		return retval;
+	}
+
+	command_print(CMD, "cp0 reg %" PRIu32 ", select %" PRIu32 ": %8.8" PRIx32,
+			cp0_reg, cp0_sel, value);
+	return ERROR_OK;
+}
+
+/**
+ * mips32_handle_cp0_command - Handle commands related to CP0 registers.
+ * @cmd: Command invocation context.
+ *
+ * Orchestrates different operations on CP0 registers based on the command arguments.
+ * Supports operations like reading all registers, reading/writing a specific register
+ * by name or number.
+ *
+ * Return: ERROR_OK on success; error code on failure.
  */
 COMMAND_HANDLER(mips32_handle_cp0_command)
 {
-	int retval;
+	int retval, tmp;
 	struct target *target = get_current_target(CMD_CTX);
 	struct mips32_common *mips32 = target_to_mips32(target);
 	struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
@@ -1133,45 +1518,296 @@ COMMAND_HANDLER(mips32_handle_cp0_command)
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
-	/* two or more argument, access a single register/select (write if third argument is given) */
-	if (CMD_ARGC < 2)
+	switch (CMD_ARGC) {
+		case 0: /* No arg => print out all cp0 regs */
+			retval = mips32_cp0_get_all_regs(CMD, ejtag_info, mips32->cp0_mask);
+			break;
+		case 1: /* 1 arg => get cp0 #reg/#sel value by name */
+			retval = mips32_cp0_get_reg_by_name(CMD, ejtag_info, mips32->cp0_mask);
+			break;
+		case 2: /* 2 args => get cp0 reg/sel value or set value by name */
+			tmp = *CMD_ARGV[0];
+			if (isdigit(tmp)) /* starts from number then args are #reg and #sel */
+				retval = mips32_cp0_get_reg_by_number(CMD, ejtag_info);
+			else /* or set value by register name */
+				retval = mips32_cp0_set_reg_by_name(CMD, mips32, ejtag_info);
+
+			break;
+		case 3: /* 3 args => set cp0 reg/sel value*/
+			retval = mips32_cp0_set_reg_by_number(CMD, mips32, ejtag_info);
+			break;
+		default: /* Other argc => err */
+			retval = ERROR_COMMAND_SYNTAX_ERROR;
+			break;
+	}
+
+	return retval;
+}
+
+/**
+ * mips32_handle_cpuinfo_command - Handles the 'cpuinfo' command.
+ * @param[in] cmd: Command invocation context.
+ *
+ * @brief Executes the 'cpuinfo' command which displays detailed information about the current CPU core.
+ * This includes core type, vendor, instruction set, cache size, and other relevant details.
+ *
+ * @return ERROR_OK on success; error code on failure.
+ */
+COMMAND_HANDLER(mips32_handle_cpuinfo_command)
+{
+	int retval;
+	struct target *target = get_current_target(CMD_CTX);
+	struct mips32_common *mips32 = target_to_mips32(target);
+	struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
+
+	uint32_t prid = mips32->prid; /* cp0 PRID - 15, 0 */
+	uint32_t config0 = ejtag_info->config[0]; /*	cp0 config - 16, 0 */
+	uint32_t config1 = ejtag_info->config[1]; /*	cp0 config - 16, 1 */
+	uint32_t config3 = ejtag_info->config[3]; /*	cp0 config - 16, 3 */
+
+	/* Following configs are not read during probe */
+	uint32_t config5; /*	cp0 config - 16, 5 */
+
+	/* No args for now */
+	if (CMD_ARGC != 0)
 		return ERROR_COMMAND_SYNTAX_ERROR;
-	else {
-		uint32_t cp0_reg, cp0_sel;
-		COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], cp0_reg);
-		COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], cp0_sel);
 
-		if (CMD_ARGC == 2) {
-			uint32_t value;
+	if (target->state != TARGET_HALTED) {
+		command_print(CMD, "target must be stopped for \"%s\" command", CMD_NAME);
+		return ERROR_TARGET_NOT_HALTED;
+	}
 
-			retval = mips32_cp0_read(ejtag_info, &value, cp0_reg, cp0_sel);
-			if (retval != ERROR_OK) {
-				command_print(CMD,
-						"couldn't access reg %" PRIu32,
-						cp0_reg);
-				return ERROR_OK;
-			}
-			command_print(CMD, "cp0 reg %" PRIu32 ", select %" PRIu32 ": %8.8" PRIx32,
-					cp0_reg, cp0_sel, value);
+	retval = mips32_cp0_read(ejtag_info, &config5, 16, 5);
+	if (retval != ERROR_OK)
+		return retval;
 
-		} else if (CMD_ARGC == 3) {
-			uint32_t value;
-			COMMAND_PARSE_NUMBER(u32, CMD_ARGV[2], value);
-			retval = mips32_cp0_write(ejtag_info, value, cp0_reg, cp0_sel);
-			if (retval != ERROR_OK) {
-				command_print(CMD,
-						"couldn't access cp0 reg %" PRIu32 ", select %" PRIu32,
-						cp0_reg,  cp0_sel);
-				return ERROR_OK;
-			}
-			command_print(CMD, "cp0 reg %" PRIu32 ", select %" PRIu32 ": %8.8" PRIx32,
-					cp0_reg, cp0_sel, value);
-		}
+	/* Determine Core info */
+	const struct cpu_entry *entry = mips32->cpu_info;
+	/* Display Core Type info */
+	command_print(CMD, "CPU Core: %s", entry->cpu_name);
+
+	/* Display Core Vendor ID if it's unknown */
+	if (entry == &mips32_cpu_entry[MIPS32_NUM_CPU_ENTRIES - 1])
+		command_print(CMD, "Vendor: Unknown CPU vendor code %x.", ((prid & 0x00ffff00) >> 16));
+	else
+		command_print(CMD, "Vendor: %s", entry->vendor);
+
+	/* If MIPS release 2 or above, then get exception base info */
+	enum mips32_isa_rel ar = mips32->isa_rel;
+	if (ar > MIPS32_RELEASE_1) {	/* release 2 and above */
+		uint32_t ebase;
+		retval = mips32_cp0_read(ejtag_info, &ebase, 15, 1);
+		if (retval != ERROR_OK)
+			return retval;
+
+		command_print(CMD, "Current CPU ID: %d", (ebase & 0x1ff));
+	} else {
+		command_print(CMD, "Current CPU ID: 0");
+	}
+
+	char *instr;
+	switch ((config3 & MIPS32_CONFIG3_ISA_MASK) >> MIPS32_CONFIG3_ISA_SHIFT) {
+		case 0:
+			instr = "MIPS32";
+		break;
+		case 1:
+			instr = "microMIPS";
+		break;
+		case 2:
+			instr = "MIPS32 (at reset) and microMIPS";
+		break;
+		case 3:
+			instr = "microMIPS (at reset) and MIPS32";
+		break;
+	}
+
+	/* Display Instruction Set Info */
+	command_print(CMD, "Instr set: %s", instr);
+	command_print(CMD, "Instr rel: %s",
+			ar == MIPS32_RELEASE_1 ? "1"
+			: ar == MIPS32_RELEASE_2 ? "2"
+			: ar == MIPS32_RELEASE_6 ? "6"
+			: "unknown");
+	command_print(CMD, "PRId: %x", prid);
+	/* Some of MIPS CPU Revisions(for M74K) can be seen on MD00541, page 26 */
+	uint32_t rev = prid & 0x000000ff;
+	command_print(CMD, "RTL Rev: %d.%d.%d", (rev & 0xE0), (rev & 0x1C), (rev & 0x3));
+
+	command_print(CMD, "Max Number of Instr Breakpoints: %d", mips32->num_inst_bpoints);
+	command_print(CMD, "Max Number of  Data Breakpoints: %d", mips32->num_data_bpoints);
+
+	/* MMU Support */
+	uint32_t mmu_type = (config0 >> 7) & 7; /* MMU Type Info */
+	char *mmu;
+	switch (mmu_type) {
+		case MIPS32_MMU_TLB:
+			mmu = "TLB";
+		break;
+		case MIPS32_MMU_BAT:
+			mmu = "BAT";
+		break;
+		case MIPS32_MMU_FIXED:
+			mmu = "FIXED";
+		break;
+		case MIPS32_MMU_DUAL_VTLB_FTLB:
+			mmu = "DUAL VAR/FIXED";
+		break;
+		default:
+			mmu = "Unknown";
+	}
+	command_print(CMD, "MMU Type: %s", mmu);
+
+	retval = mips32_read_config_mmu(ejtag_info);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Definitions of I/D Cache Sizes are available on MD01251, page 224~226 */
+	int index;
+	uint32_t ways, sets, bpl;
+
+	/* Determine Instr Cache Size */
+	/* Ways mapping = [1, 2, 3, 4, 5, 6, 7, 8] */
+	ways = ((config1 >> MIPS32_CFG1_IASHIFT) & 7);
+
+	/* Sets per way = [64, 128, 256, 512, 1024, 2048, 4096, 32] */
+	index = ((config1 >> MIPS32_CFG1_ISSHIFT) & 7);
+	sets = index == 7 ? 32 : 32 << (index + 1);
+
+	/* Bytes per line = [0, 4, 8, 16, 32, 64, 128, Reserved] */
+	index = ((config1 >> MIPS32_CFG1_ILSHIFT) & 7);
+	bpl = index == 0 ? 0 : 4 << (index - 1);
+	command_print(CMD, "Instr Cache: %d (%d ways, %d lines, %d byte per line)", ways * sets * bpl, ways, sets, bpl);
+
+	/* Determine data cache size, same as above */
+	ways = ((config1 >>  MIPS32_CFG1_DASHIFT) & 7);
+
+	index = ((config1 >> MIPS32_CFG1_DSSHIFT) & 7);
+	sets = index == 7 ? 32 : 32 << (index + 1);
+
+	index = ((config1 >> MIPS32_CFG1_DLSHIFT) & 7);
+	bpl = index == 0 ? 0 : 4 << (index - 1);
+	command_print(CMD, " Data Cache: %d (%d ways, %d lines, %d byte per line)", ways * sets * bpl, ways, sets, bpl);
+
+	/* does the core hava FPU*/
+	mips32_read_config_fpu(mips32, ejtag_info);
+
+	/* does the core support a DSP */
+	mips32_read_config_dsp(mips32, ejtag_info);
+
+	/* VZ module */
+	uint32_t vzase = (config3 & BIT(23));
+	if (vzase)
+		command_print(CMD, "VZ implemented: yes");
+	else
+		command_print(CMD, "VZ implemented: no");
+
+	/* multithreading */
+	uint32_t mtase  = (config3 & BIT(2));
+	if (mtase) {
+		command_print(CMD, "MT  implemented: yes");
+
+		/* Get VPE and Thread info */
+		uint32_t tcbind;
+		uint32_t mvpconf0;
+
+		/* Read tcbind register */
+		retval = mips32_cp0_read(ejtag_info, &tcbind, 2, 2);
+		if (retval != ERROR_OK)
+			return retval;
+
+		command_print(CMD, " | Current VPE: %d", (tcbind & 0xf));
+		command_print(CMD, " | Current  TC: %d", ((tcbind >> 21) & 0xff));
+
+		/* Read mvpconf0 register */
+		retval = mips32_cp0_read(ejtag_info, &mvpconf0, 0, 2);
+		if (retval != ERROR_OK)
+			return retval;
+
+		command_print(CMD, " | Total  TC: %d", (mvpconf0 & 0xf) + 1);
+		command_print(CMD, " | Total VPE: %d", ((mvpconf0 >> 10) & 0xf) + 1);
+	} else {
+		command_print(CMD, "MT  implemented: no");
 	}
 
+	/* MIPS SIMD Architecture (MSA) */
+	uint32_t msa = (config3 & BIT(28));
+	command_print(CMD, "MSA implemented: %s", msa ? "yes" : "no");
+
+	/* Move To/From High COP0 (MTHC0/MFHC0) instructions are implemented.
+	 * Implicates current ISA release >= 5.*/
+	uint32_t mvh = (config5 & BIT(5));
+	command_print(CMD, "MVH implemented: %s", mvh ? "yes" : "no");
+
+	/* Common Device Memory Map implemented? */
+	uint32_t cdmm = (config3 & BIT(3));
+	command_print(CMD, "CDMM implemented: %s", cdmm ? "yes" : "no");
+
 	return ERROR_OK;
 }
 
+/**
+ * mips32_handle_ejtag_reg_command - Handler commands related to EJTAG
+ * @param[in] cmd: Command invocation context
+ *
+ * @brief Prints all EJTAG Registers including DCR features.
+ *
+ * @return ERROR_OK on success; error code on failure.
+ */
+COMMAND_HANDLER(mips32_handle_ejtag_reg_command)
+{
+	struct target *target = get_current_target(CMD_CTX);
+	struct mips32_common *mips32 = target_to_mips32(target);
+	struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
+
+	uint32_t ejtag_ctrl;
+	uint32_t dcr;
+	int retval;
+
+	retval = mips_ejtag_get_idcode(ejtag_info);
+	if (retval != ERROR_OK)
+		command_print(CMD, "Error: Encounter an Error while getting idcode");
+	else
+		command_print(CMD, "       idcode: 0x%8.8" PRIx32, ejtag_info->idcode);
+
+	retval = mips_ejtag_get_impcode(ejtag_info);
+	if (retval != ERROR_OK)
+		command_print(CMD, "Error: Encounter an Error while getting impcode");
+	else
+		command_print(CMD, "      impcode: 0x%8.8" PRIx32, ejtag_info->impcode);
+
+	mips_ejtag_set_instr(ejtag_info, EJTAG_INST_CONTROL);
+	ejtag_ctrl = ejtag_info->ejtag_ctrl;
+	retval = mips_ejtag_drscan_32(ejtag_info, &ejtag_ctrl);
+	if (retval != ERROR_OK)
+		command_print(CMD, "Error: Encounter an Error while executing drscan reading EJTAG Control register");
+	else
+		command_print(CMD, "ejtag control: 0x%8.8" PRIx32, ejtag_ctrl);
+
+	ejtag_main_print_imp(ejtag_info);
+
+	/* Display current DCR */
+	retval = target_read_u32(target, EJTAG_DCR, &dcr);
+	if (retval != ERROR_OK)
+		command_print(CMD, "Error: Encounter an Error while reading Debug Control Register");
+	else
+		command_print(CMD, "          DCR: 0x%8.8" PRIx32, dcr);
+
+	for (unsigned int i = 0; i < EJTAG_DCR_ENTRIES; i++) {
+		if (dcr & BIT(dcr_features[i].bit))
+			command_print(CMD, "%s supported", dcr_features[i].name);
+	}
+
+	return ERROR_OK;
+}
+
+/**
+ * mips32_handle_scan_delay_command - Handler command for changing scan delay
+ * @param[in] cmd: Command invocation context
+ *
+ * @brief Changes current scan mode between legacy and fast queued mode.
+ *
+ * @return ERROR_OK on success; error code on failure.
+ */
 COMMAND_HANDLER(mips32_handle_scan_delay_command)
 {
 	struct target *target = get_current_target(CMD_CTX);
@@ -1200,16 +1836,30 @@ static const struct command_registration mips32_exec_command_handlers[] = {
 		.name = "cp0",
 		.handler = mips32_handle_cp0_command,
 		.mode = COMMAND_EXEC,
-		.usage = "regnum select [value]",
+		.usage = "[[reg_name|regnum select] [value]]",
 		.help = "display/modify cp0 register",
 	},
-		{
+	{
+		.name = "cpuinfo",
+		.handler = mips32_handle_cpuinfo_command,
+		.mode = COMMAND_EXEC,
+		.help = "display CPU information",
+		.usage = "",
+	},
+	{
 		.name = "scan_delay",
 		.handler = mips32_handle_scan_delay_command,
 		.mode = COMMAND_ANY,
 		.help = "display/set scan delay in nano seconds",
 		.usage = "[value]",
 	},
+	{
+		.name = "ejtag_reg",
+		.handler = mips32_handle_ejtag_reg_command,
+		.mode = COMMAND_ANY,
+		.help = "read ejtag registers",
+		.usage = "",
+	},
 	COMMAND_REGISTRATION_DONE
 };
 
diff --git a/src/target/mips32.h b/src/target/mips32.h
index fc89624..208c9da 100644
--- a/src/target/mips32.h
+++ b/src/target/mips32.h
@@ -84,7 +84,7 @@
 /* CP1 FIR register fields */
 #define MIPS32_CP1_FIR_F64_SHIFT	22
 
-static const struct {
+static const struct mips32_cp0 {
 	unsigned int reg;
 	unsigned int sel;
 	const char *name;
@@ -202,7 +202,7 @@ static const struct {
 	{31, 3, "kscratch2", MIPS_CP0_MAPTIV_UC | MIPS_CP0_MAPTIV_UP},
 };
 
-#define MIPS32NUMCP0REGS ((int)ARRAY_SIZE(mips32_cp0_regs))
+#define MIPS32NUMCP0REGS (ARRAY_SIZE(mips32_cp0_regs))
 
 /* Insert extra NOPs after the DRET instruction on exit from debug. */
 #define	EJTAG_QUIRK_PAD_DRET		BIT(0)
@@ -263,6 +263,96 @@ enum mips32_isa_rel {
 	MIPS32_RELEASE_UNKNOWN,
 };
 
+enum mips32_isa_supported {
+	MIPS16,
+	MIPS32,
+	MIPS64,
+	MICROMIPS_ONLY,
+	MIPS32_AT_RESET_AND_MICROMIPS,
+	MICROMIPS_AT_RESET_AND_MIPS32,
+};
+#define MIPS32_CORE_MASK	0xFFFFFF00
+#define MIPS32_VARIANT_MASK	0x00FF
+
+/* This struct contains mips cpu types with their name respectively.
+ * The PrID register format is as following:
+ *  - Company Optionsp[31:24]
+ *  - Company ID[23:16]
+ *  - Processor ID[15:8]
+ *  - Revision[7:0]
+ * Here the revision field represents the maximum value of revision.
+ */
+static const struct cpu_entry {
+	uint32_t prid;
+	enum mips32_isa_supported isa;
+	const char *vendor;
+	const char *cpu_name;
+} mips32_cpu_entry[] = {
+	/* MIPS Technologies cores */
+	{0x000180FF, MIPS32, "MIPS", "4Kc"},
+	{0x000181FF, MIPS64, "MIPS", "5Kc"},
+	{0x000182FF, MIPS64, "MIPS", "20Kc"},
+	{0x000183FF, MIPS32, "MIPS", "4KM"},
+
+	{0x000184FF, MIPS32, "MIPS", "4KEc"},
+	{0x000190FF, MIPS32, "MIPS", "4KEc"},
+
+	{0x000185FF, MIPS32, "MIPS", "4KEm"},
+	{0x000191FF, MIPS32, "MIPS", "4KEm"},
+
+	{0x000186FF, MIPS32, "MIPS", "4KSc"},
+	{0x000187FF, MIPS32, "MIPS", "M4K"},
+	{0x000188FF, MIPS64, "MIPS", "25Kf"},
+	{0x000189FF, MIPS64, "MIPS", "5KEc"},
+	{0x000192FF, MIPS32, "MIPS", "4KSD"},
+	{0x000193FF, MIPS32, "MIPS", "24Kc"},
+	{0x000195FF, MIPS32, "MIPS", "34Kc"},
+	{0x000196FF, MIPS32, "MIPS", "24KEc"},
+	{0x000197FF, MIPS32, "MIPS", "74Kc"},
+	{0x000199FF, MIPS32, "MIPS", "1004Kc"},
+	{0x00019AFF, MIPS32, "MIPS", "1074Kc"},
+	{0x00019BFF, MIPS32, "MIPS", "M14K"},
+	{0x00019CFF, MIPS32, "MIPS", "M14Kc"},
+	{0x00019DFF, MIPS32, "MIPS", "microAptiv_UC(M14KE)"},
+	{0x00019EFF, MIPS32, "MIPS", "microAptiv_UP(M14KEc)"},
+	{0x0001A0FF, MIPS32, "MIPS", "interAptiv"},
+	{0x0001A1FF, MIPS32, "MIPS", "interAptiv_CM"},
+	{0x0001A2FF, MIPS32, "MIPS", "proAptiv"},
+	{0x0001A3FF, MIPS32, "MIPS", "proAptiv_CM"},
+	{0x0001A6FF, MIPS32, "MIPS", "M5100"},
+	{0x0001A7FF, MIPS32, "MIPS", "M5150"},
+	{0x0001A8FF, MIPS32, "MIPS", "P5600"},
+	{0x0001A9FF, MIPS32, "MIPS", "I5500"},
+
+	/* Broadcom */
+	{0x000200FF, MIPS32, "Broadcom", "Broadcom"},
+
+	/* AMD Alchemy Series*/
+	/* NOTE: AMD/Alchemy series uses Company Option instead of
+	 * Processor ID, to match the find function, Processor ID field
+	 * is the copy of Company Option field */
+	{0x000300FF, MIPS32, "AMD Alchemy", "AU1000"},
+	{0x010301FF, MIPS32, "AMD Alchemy", "AU1500"},
+	{0x020302FF, MIPS32, "AMD Alchemy", "AU1100"},
+	{0x030303FF, MIPS32, "AMD Alchemy", "AU1550"},
+	{0x04030401, MIPS32, "AMD Alchemy", "AU1200"},
+	{0x040304FF, MIPS32, "AMD Alchemy", "AU1250"},
+	{0x050305FF, MIPS32, "AMD Alchemy", "AU1210"},
+
+	/* Altera */
+	{0x001000FF, MIPS32, "Altera", "Altera"},
+
+	/* Lexra */
+	{0x000B00FF, MIPS32, "Lexra", "Lexra"},
+
+	/* Ingenic */
+	{0x00e102FF, MIPS32, "Ingenic", "Ingenic XBurst rev1"},
+
+	{0xFFFFFFFF, MIPS32, "Unknown", "Unknown"}
+};
+
+#define MIPS32_NUM_CPU_ENTRIES (ARRAY_SIZE(mips32_cpu_entry))
+
 enum mips32_fp_imp {
 	MIPS32_FP_IMP_NONE = 0,
 	MIPS32_FP_IMP_32 = 1,
@@ -306,6 +396,11 @@ struct mips32_common {
 
 	int fdc;
 	int semihosting;
+
+	/* The cp0 registers implemented on different processor cores could be different, too.
+	 * Here you can see most of the registers are implemented on interAptiv, which is
+	 * a 2c4t SMP processor, it has more features than M-class processors, like vpe
+	 * and other config registers for multhreading. */
 	uint32_t cp0_mask;
 
 	/* FPU enabled (cp0.status.cu1) */
@@ -315,6 +410,8 @@ struct mips32_common {
 
 	/* processor identification register */
 	uint32_t prid;
+	/* detected CPU type */
+	const struct cpu_entry *cpu_info;
 	/* CPU specific quirks */
 	uint32_t cpu_quirks;
 
@@ -357,6 +454,7 @@ struct mips32_algorithm {
 #define MIPS32_OP_BEQ	0x04u
 #define MIPS32_OP_BGTZ	0x07u
 #define MIPS32_OP_BNE	0x05u
+#define MIPS32_OP_ADD	0x20u
 #define MIPS32_OP_ADDI	0x08u
 #define MIPS32_OP_AND	0x24u
 #define MIPS32_OP_CACHE	0x2Fu
@@ -384,6 +482,7 @@ struct mips32_algorithm {
 #define MIPS32_OP_SRA	0x03u
 #define MIPS32_OP_SYNCI	0x1Fu
 #define MIPS32_OP_SLL	0x00u
+#define MIPS32_OP_SLLV	0x04u
 #define MIPS32_OP_SLTI	0x0Au
 #define MIPS32_OP_MOVN	0x0Bu
 
@@ -393,8 +492,11 @@ struct mips32_algorithm {
 #define MIPS32_OP_SPECIAL2	0x07u
 #define MIPS32_OP_SPECIAL3	0x1Fu
 
-#define MIPS32_COP0_MF	0x00u
-#define MIPS32_COP0_MT	0x04u
+#define MIPS32_COP_MF	0x00u
+#define MIPS32_COP_CF	0x02u
+#define MIPS32_COP_MFH	0x03u
+#define MIPS32_COP_MT	0x04u
+#define MIPS32_COP_MTH	0x07u
 
 #define MIPS32_R_INST(opcode, rs, rt, rd, shamt, funct) \
 	(((opcode) << 26) | ((rs) << 21) | ((rt) << 16) | ((rd) << 11) | ((shamt) << 6) | (funct))
@@ -403,6 +505,7 @@ struct mips32_algorithm {
 #define MIPS32_J_INST(opcode, addr)	(((opcode) << 26) | (addr))
 
 #define MIPS32_ISA_NOP				0
+#define MIPS32_ISA_ADD(dst, src, tar)		MIPS32_R_INST(MIPS32_OP_SPECIAL, src, tar, dst, 0, MIPS32_OP_ADD)
 #define MIPS32_ISA_ADDI(tar, src, val)		MIPS32_I_INST(MIPS32_OP_ADDI, src, tar, val)
 #define MIPS32_ISA_ADDIU(tar, src, val)		MIPS32_I_INST(MIPS32_OP_ADDIU, src, tar, val)
 #define MIPS32_ISA_ADDU(dst, src, tar)		MIPS32_R_INST(MIPS32_OP_SPECIAL, src, tar, dst, 0, MIPS32_OP_ADDU)
@@ -416,6 +519,7 @@ struct mips32_algorithm {
 #define MIPS32_ISA_CACHE(op, off, base)		MIPS32_I_INST(MIPS32_OP_CACHE, base, op, off)
 #define MIPS32_ISA_J(tar)			MIPS32_J_INST(MIPS32_OP_J, (0x0FFFFFFFu & (tar)) >> 2)
 #define MIPS32_ISA_JR(reg)			MIPS32_R_INST(0, reg, 0, 0, 0, MIPS32_OP_JR)
+#define MIPS32_ISA_JRHB(reg)		MIPS32_R_INST(0, reg, 0, 0, 0x10, MIPS32_OP_JR)
 
 #define MIPS32_ISA_LB(reg, off, base)		MIPS32_I_INST(MIPS32_OP_LB, base, reg, off)
 #define MIPS32_ISA_LBU(reg, off, base)		MIPS32_I_INST(MIPS32_OP_LBU, base, reg, off)
@@ -423,14 +527,16 @@ struct mips32_algorithm {
 #define MIPS32_ISA_LUI(reg, val)		MIPS32_I_INST(MIPS32_OP_LUI, 0, reg, val)
 #define MIPS32_ISA_LW(reg, off, base)		MIPS32_I_INST(MIPS32_OP_LW, base, reg, off)
 
-#define MIPS32_ISA_MFC0(gpr, cpr, sel)		MIPS32_R_INST(MIPS32_OP_COP0, MIPS32_COP0_MF, gpr, cpr, 0, sel)
-#define MIPS32_ISA_MTC0(gpr, cpr, sel)		MIPS32_R_INST(MIPS32_OP_COP0, MIPS32_COP0_MT, gpr, cpr, 0, sel)
+#define MIPS32_ISA_MFC0(gpr, cpr, sel)		MIPS32_R_INST(MIPS32_OP_COP0, MIPS32_COP_MF, gpr, cpr, 0, sel)
+#define MIPS32_ISA_MTC0(gpr, cpr, sel)		MIPS32_R_INST(MIPS32_OP_COP0, MIPS32_COP_MT, gpr, cpr, 0, sel)
 #define MIPS32_ISA_MFLO(reg)			MIPS32_R_INST(0, 0, 0, reg, 0, MIPS32_OP_MFLO)
 #define MIPS32_ISA_MFHI(reg)			MIPS32_R_INST(0, 0, 0, reg, 0, MIPS32_OP_MFHI)
 #define MIPS32_ISA_MTLO(reg)			MIPS32_R_INST(0, reg, 0, 0, 0, MIPS32_OP_MTLO)
 #define MIPS32_ISA_MTHI(reg)			MIPS32_R_INST(0, reg, 0, 0, 0, MIPS32_OP_MTHI)
 
+#define MIPS32_ISA_MUL(dst, src, t)		MIPS32_R_INST(28, src, t, dst, 0, MIPS32_OP_MUL)
 #define MIPS32_ISA_MOVN(dst, src, tar)		MIPS32_R_INST(MIPS32_OP_SPECIAL, src, tar, dst, 0, MIPS32_OP_MOVN)
+#define MIPS32_ISA_OR(dst, src, val)		MIPS32_R_INST(0, src, val, dst, 0, 37)
 #define MIPS32_ISA_ORI(tar, src, val)		MIPS32_I_INST(MIPS32_OP_ORI, src, tar, val)
 #define MIPS32_ISA_RDHWR(tar, dst)		MIPS32_R_INST(MIPS32_OP_SPECIAL3, 0, tar, dst, 0, MIPS32_OP_RDHWR)
 #define MIPS32_ISA_SB(reg, off, base)		MIPS32_I_INST(MIPS32_OP_SB, base, reg, off)
@@ -438,6 +544,7 @@ struct mips32_algorithm {
 #define MIPS32_ISA_SW(reg, off, base)		MIPS32_I_INST(MIPS32_OP_SW, base, reg, off)
 
 #define MIPS32_ISA_SLL(dst, src, sa)		MIPS32_R_INST(MIPS32_OP_SPECIAL, 0, src, dst, sa, MIPS32_OP_SLL)
+#define MIPS32_ISA_SLLV(dst, src, sa)		MIPS32_R_INST(MIPS32_OP_SPECIAL, 0, src, dst, sa, MIPS32_OP_SLLV)
 #define MIPS32_ISA_SLTI(tar, src, val)		MIPS32_I_INST(MIPS32_OP_SLTI, src, tar, val)
 #define MIPS32_ISA_SLTU(dst, src, tar)		MIPS32_R_INST(MIPS32_OP_SPECIAL, src, tar, dst, 0, MIPS32_OP_SLTU)
 #define MIPS32_ISA_SRA(reg, src, off)		MIPS32_R_INST(MIPS32_OP_SPECIAL, 0, src, reg, off, MIPS32_OP_SRA)
@@ -466,10 +573,12 @@ struct mips32_algorithm {
 #define MIPS16_ISA_SDBBP			0xE801u
 
 /*MICRO MIPS INSTRUCTIONS, see doc MD00582 */
-#define POOL32A					0X00u
-#define POOL32AXF				0x3Cu
-#define POOL32B					0x08u
-#define POOL32I					0x10u
+#define MMIPS32_POOL32A					0x00u
+#define MMIPS32_POOL32F					0x15u
+#define MMIPS32_POOL32FXF				0x3Bu
+#define MMIPS32_POOL32AXF				0x3Cu
+#define MMIPS32_POOL32B					0x08u
+#define MMIPS32_POOL32I					0x10u
 #define MMIPS32_OP_ADDI			0x04u
 #define MMIPS32_OP_ADDIU		0x0Cu
 #define MMIPS32_OP_ADDU			0x150u
@@ -481,6 +590,7 @@ struct mips32_algorithm {
 #define MMIPS32_OP_CACHE		0x06u
 #define MMIPS32_OP_J			0x35u
 #define MMIPS32_OP_JALR			0x03Cu
+#define MMIPS32_OP_JALRHB		0x07Cu
 #define MMIPS32_OP_LB			0x07u
 #define MMIPS32_OP_LBU			0x05u
 #define MMIPS32_OP_LHU			0x0Du
@@ -508,55 +618,59 @@ struct mips32_algorithm {
 
 #define MMIPS32_ADDI(tar, src, val)		MIPS32_I_INST(MMIPS32_OP_ADDI, tar, src, val)
 #define MMIPS32_ADDIU(tar, src, val)		MIPS32_I_INST(MMIPS32_OP_ADDIU, tar, src, val)
-#define MMIPS32_ADDU(dst, src, tar)		MIPS32_R_INST(POOL32A, tar, src, dst, 0, MMIPS32_OP_ADDU)
-#define MMIPS32_AND(dst, src, tar)		MIPS32_R_INST(POOL32A, tar, src, dst, 0, MMIPS32_OP_AND)
+#define MMIPS32_ADDU(dst, src, tar)		MIPS32_R_INST(MMIPS32_POOL32A, tar, src, dst, 0, MMIPS32_OP_ADDU)
+#define MMIPS32_AND(dst, src, tar)		MIPS32_R_INST(MMIPS32_POOL32A, tar, src, dst, 0, MMIPS32_OP_AND)
 #define MMIPS32_ANDI(tar, src, val)		MIPS32_I_INST(MMIPS32_OP_ANDI, tar, src, val)
 
 #define MMIPS32_B(off)				MMIPS32_BEQ(0, 0, off)
 #define MMIPS32_BEQ(src, tar, off)		MIPS32_I_INST(MMIPS32_OP_BEQ, tar, src, off)
-#define MMIPS32_BGTZ(reg, off)			MIPS32_I_INST(POOL32I, MMIPS32_OP_BGTZ, reg, off)
+#define MMIPS32_BGTZ(reg, off)			MIPS32_I_INST(MMIPS32_POOL32I, MMIPS32_OP_BGTZ, reg, off)
 #define MMIPS32_BNE(src, tar, off)		MIPS32_I_INST(MMIPS32_OP_BNE, tar, src, off)
-#define MMIPS32_CACHE(op, off, base)		MIPS32_R_INST(POOL32B, op, base, MMIPS32_OP_CACHE << 1, 0, off)
+#define MMIPS32_CACHE(op, off, base)		MIPS32_R_INST(MMIPS32_POOL32B, op, base, MMIPS32_OP_CACHE << 1, 0, off)
 
 #define MMIPS32_J(tar)				MIPS32_J_INST(MMIPS32_OP_J, ((0x07FFFFFFu & ((tar) >> 1))))
-#define MMIPS32_JR(reg)				MIPS32_R_INST(POOL32A, 0, reg, 0, MMIPS32_OP_JALR, POOL32AXF)
+#define MMIPS32_JR(reg)				MIPS32_R_INST(MMIPS32_POOL32A, 0, reg, 0, MMIPS32_OP_JALR, MMIPS32_POOL32AXF)
+#define MMIPS32_JRHB(reg)			MIPS32_R_INST(MMIPS32_POOL32A, 0, reg, 0, MMIPS32_OP_JALRHB, MMIPS32_POOL32AXF)
 #define MMIPS32_LB(reg, off, base)		MIPS32_I_INST(MMIPS32_OP_LB, reg, base, off)
 #define MMIPS32_LBU(reg, off, base)		MIPS32_I_INST(MMIPS32_OP_LBU, reg, base, off)
 #define MMIPS32_LHU(reg, off, base)		MIPS32_I_INST(MMIPS32_OP_LHU, reg, base, off)
-#define MMIPS32_LUI(reg, val)			MIPS32_I_INST(POOL32I, MMIPS32_OP_LUI, reg, val)
+#define MMIPS32_LUI(reg, val)			MIPS32_I_INST(MMIPS32_POOL32I, MMIPS32_OP_LUI, reg, val)
 #define MMIPS32_LW(reg, off, base)		MIPS32_I_INST(MMIPS32_OP_LW, reg, base, off)
 
-#define MMIPS32_MFC0(gpr, cpr, sel)		MIPS32_R_INST(POOL32A, gpr, cpr, sel, MMIPS32_OP_MFC0, POOL32AXF)
-#define MMIPS32_MFLO(reg)			MIPS32_R_INST(POOL32A, 0, reg, 0, MMIPS32_OP_MFLO, POOL32AXF)
-#define MMIPS32_MFHI(reg)			MIPS32_R_INST(POOL32A, 0, reg, 0, MMIPS32_OP_MFHI, POOL32AXF)
-#define MMIPS32_MTC0(gpr, cpr, sel)		MIPS32_R_INST(POOL32A, gpr, cpr, sel, MMIPS32_OP_MTC0, POOL32AXF)
-#define MMIPS32_MTLO(reg)			MIPS32_R_INST(POOL32A, 0, reg, 0, MMIPS32_OP_MTLO, POOL32AXF)
-#define MMIPS32_MTHI(reg)			MIPS32_R_INST(POOL32A, 0, reg, 0, MMIPS32_OP_MTHI, POOL32AXF)
+#define MMIPS32_MFC0(gpr, cpr, sel)		MIPS32_R_INST(MMIPS32_POOL32A, gpr, cpr, sel,\
+														MMIPS32_OP_MFC0, MMIPS32_POOL32AXF)
+#define MMIPS32_MFLO(reg)			MIPS32_R_INST(MMIPS32_POOL32A, 0, reg, 0, MMIPS32_OP_MFLO, MMIPS32_POOL32AXF)
+#define MMIPS32_MFHI(reg)			MIPS32_R_INST(MMIPS32_POOL32A, 0, reg, 0, MMIPS32_OP_MFHI, MMIPS32_POOL32AXF)
+#define MMIPS32_MTC0(gpr, cpr, sel)		MIPS32_R_INST(MMIPS32_POOL32A, gpr, cpr, sel,\
+														MMIPS32_OP_MTC0, MMIPS32_POOL32AXF)
+#define MMIPS32_MTLO(reg)			MIPS32_R_INST(MMIPS32_POOL32A, 0, reg, 0, MMIPS32_OP_MTLO, MMIPS32_POOL32AXF)
+#define MMIPS32_MTHI(reg)			MIPS32_R_INST(MMIPS32_POOL32A, 0, reg, 0, MMIPS32_OP_MTHI, MMIPS32_POOL32AXF)
 
-#define MMIPS32_MOVN(dst, src, tar)		MIPS32_R_INST(POOL32A, tar, src, dst, 0, MMIPS32_OP_MOVN)
+#define MMIPS32_MOVN(dst, src, tar)		MIPS32_R_INST(MMIPS32_POOL32A, tar, src, dst, 0, MMIPS32_OP_MOVN)
 #define MMIPS32_NOP				0
 #define MMIPS32_ORI(tar, src, val)		MIPS32_I_INST(MMIPS32_OP_ORI, tar, src, val)
-#define MMIPS32_RDHWR(tar, dst)			MIPS32_R_INST(POOL32A, dst, tar, 0, MMIPS32_OP_RDHWR, POOL32AXF)
+#define MMIPS32_RDHWR(tar, dst)			MIPS32_R_INST(MMIPS32_POOL32A, dst, tar, 0, MMIPS32_OP_RDHWR, MMIPS32_POOL32AXF)
 #define MMIPS32_SB(reg, off, base)		MIPS32_I_INST(MMIPS32_OP_SB, reg, base, off)
 #define MMIPS32_SH(reg, off, base)		MIPS32_I_INST(MMIPS32_OP_SH, reg, base, off)
 #define MMIPS32_SW(reg, off, base)		MIPS32_I_INST(MMIPS32_OP_SW, reg, base, off)
 
-#define MMIPS32_SRL(reg, src, off)		MIPS32_R_INST(POOL32A, reg, src, off, 0, MMIPS32_OP_SRL)
-#define MMIPS32_SLTU(dst, src, tar)		MIPS32_R_INST(POOL32A, tar, src, dst, 0, MMIPS32_OP_SLTU)
-#define MMIPS32_SYNCI(off, base)		MIPS32_I_INST(POOL32I, MMIPS32_OP_SYNCI, base, off)
-#define MMIPS32_SLL(dst, src, sa)		MIPS32_R_INST(POOL32A, dst, src, sa, 0, MMIPS32_OP_SLL)
+#define MMIPS32_SRL(reg, src, off)		MIPS32_R_INST(MMIPS32_POOL32A, reg, src, off, 0, MMIPS32_OP_SRL)
+#define MMIPS32_SLTU(dst, src, tar)		MIPS32_R_INST(MMIPS32_POOL32A, tar, src, dst, 0, MMIPS32_OP_SLTU)
+#define MMIPS32_SYNCI(off, base)		MIPS32_I_INST(MMIPS32_POOL32I, MMIPS32_OP_SYNCI, base, off)
+#define MMIPS32_SLL(dst, src, sa)		MIPS32_R_INST(MMIPS32_POOL32A, dst, src, sa, 0, MMIPS32_OP_SLL)
+#define MMIPS32_SLLV(dst, src, sa)		MIPS32_R_INST(MMIPS32_POOL32A, dst, src, sa, 0, MMIPS32_OP_SLLV)
 #define MMIPS32_SLTI(tar, src, val)		MIPS32_I_INST(MMIPS32_OP_SLTI, tar, src, val)
-#define MMIPS32_SYNC				0x00001A7Cu /* MIPS32_R_INST(POOL32A, 0, 0, 0, 0x1ADu, POOL32AXF) */
+#define MMIPS32_SYNC				0x00001A7Cu /* MIPS32_R_INST(MMIPS32_POOL32A, 0, 0, 0, 0x1ADu, MMIPS32_POOL32AXF) */
 
-#define MMIPS32_XOR(reg, val1, val2)		MIPS32_R_INST(POOL32A, val1, val2, reg, 0, MMIPS32_OP_XOR)
+#define MMIPS32_XOR(reg, val1, val2)		MIPS32_R_INST(MMIPS32_POOL32A, val1, val2, reg, 0, MMIPS32_OP_XOR)
 #define MMIPS32_XORI(tar, src, val)		MIPS32_I_INST(MMIPS32_OP_XORI, tar, src, val)
 
 #define MMIPS32_SYNCI_STEP	0x1u	/* reg num od address step size to be used with synci instruction */
 
 
 /* ejtag specific instructions */
-#define MMIPS32_DRET			0x0000E37Cu	/* MIPS32_R_INST(POOL32A, 0, 0, 0, 0x38D, POOL32AXF) */
-#define MMIPS32_SDBBP			0x0000DB7Cu	/* MIPS32_R_INST(POOL32A, 0, 0, 0, 0x1BD, POOL32AXF) */
+#define MMIPS32_DRET			0x0000E37Cu	/* MIPS32_R_INST(MMIPS32_POOL32A, 0, 0, 0, 0x38D, MMIPS32_POOL32AXF) */
+#define MMIPS32_SDBBP			0x0000DB7Cu	/* MIPS32_R_INST(MMIPS32_POOL32A, 0, 0, 0, 0x1BD, MMIPS32_POOL32AXF) */
 #define MMIPS16_SDBBP			0x46C0u		/* POOL16C instr */
 
 /* instruction code with isa selection */
@@ -575,6 +689,7 @@ struct mips32_algorithm {
 
 #define MIPS32_J(isa, tar)			(isa ? MMIPS32_J(tar) : MIPS32_ISA_J(tar))
 #define MIPS32_JR(isa, reg)			(isa ? MMIPS32_JR(reg) : MIPS32_ISA_JR(reg))
+#define MIPS32_JRHB(isa, reg)		(isa ? MMIPS32_JRHB(reg) : MIPS32_ISA_JRHB(reg))
 #define MIPS32_LB(isa, reg, off, base)		(isa ? MMIPS32_LB(reg, off, base) : MIPS32_ISA_LB(reg, off, base))
 #define MIPS32_LBU(isa, reg, off, base)		(isa ? MMIPS32_LBU(reg, off, base) : MIPS32_ISA_LBU(reg, off, base))
 #define MIPS32_LHU(isa, reg, off, base)		(isa ? MMIPS32_LHU(reg, off, base) : MIPS32_ISA_LHU(reg, off, base))
@@ -588,6 +703,7 @@ struct mips32_algorithm {
 #define MIPS32_MTLO(isa, reg)			(isa ? MMIPS32_MTLO(reg) : MIPS32_ISA_MTLO(reg))
 #define MIPS32_MTHI(isa, reg)			(isa ? MMIPS32_MTHI(reg) : MIPS32_ISA_MTHI(reg))
 
+#define MIPS32_MUL(isa, dst, src, t)		(MIPS32_ISA_MUL(dst, src, t))
 #define MIPS32_MOVN(isa, dst, src, tar)		(isa ? MMIPS32_MOVN(dst, src, tar) : MIPS32_ISA_MOVN(dst, src, tar))
 #define MIPS32_ORI(isa, tar, src, val)		(isa ? MMIPS32_ORI(tar, src, val) : MIPS32_ISA_ORI(tar, src, val))
 #define MIPS32_RDHWR(isa, tar, dst)		(isa ? MMIPS32_RDHWR(tar, dst) : MIPS32_ISA_RDHWR(tar, dst))
@@ -596,6 +712,8 @@ struct mips32_algorithm {
 #define MIPS32_SW(isa, reg, off, base)		(isa ? MMIPS32_SW(reg, off, base) : MIPS32_ISA_SW(reg, off, base))
 
 #define MIPS32_SLL(isa, dst, src, sa)		(isa ? MMIPS32_SLL(dst, src, sa) : MIPS32_ISA_SLL(dst, src, sa))
+#define MIPS32_EHB(isa)		                (isa ? MMIPS32_SLL(0, 0, 3) : MIPS32_ISA_SLL(0, 0, 3))
+#define MIPS32_SLLV(isa, dst, src, sa)		(MIPS32_ISA_SLLV(dst, src, sa))
 #define MIPS32_SLTI(isa, tar, src, val)		(isa ? MMIPS32_SLTI(tar, src, val) : MIPS32_ISA_SLTI(tar, src, val))
 #define MIPS32_SLTU(isa, dst, src, tar)		(isa ? MMIPS32_SLTU(dst, src, tar) : MIPS32_ISA_SLTU(dst, src, tar))
 #define MIPS32_SRL(isa, reg, src, off)		(isa ? MMIPS32_SRL(reg, src, off) : MIPS32_ISA_SRL(reg, src, off))
@@ -613,6 +731,9 @@ struct mips32_algorithm {
 
 #define MIPS16_SDBBP(isa)			(isa ? MMIPS16_SDBBP : MIPS16_ISA_SDBBP)
 
+/* ejtag specific instructions */
+#define MICRO_MIPS32_SDBBP			0x000046C0
+#define MICRO_MIPS_SDBBP			0x46C0
 /*
  * MIPS32 Config1 Register (CP0 Register 16, Select 1)
  */
@@ -708,78 +829,6 @@ struct mips32_algorithm {
 #define MIPS32_MMU_FIXED		3
 #define MIPS32_MMU_DUAL_VTLB_FTLB	4
 
-enum mips32_cpu_vendor {
-	MIPS32_CPU_VENDOR_MTI,
-	MIPS32_CPU_VENDOR_ALCHEMY,
-	MIPS32_CPU_VENDOR_BROADCOM,
-	MIPS32_CPU_VENDOR_ALTERA,
-	MIPS32_CPU_VENDOR_LEXRA,
-};
-
-enum mips32_isa_supported {
-	MIPS16,
-	MIPS32,
-	MIPS64,
-	MICROMIPS_ONLY,
-	MIPS32_AT_RESET_AND_MICROMIPS,
-	MICROMIPS_AT_RESET_AND_MIPS32,
-};
-
-struct mips32_cpu_features {
-	/* Type of CPU	(4Kc, 24Kf, etc.) */
-	uint32_t cpu_core;
-
-	/* Internal representation of cpu type */
-	uint32_t cpu_type;
-
-	/* Processor vendor */
-	enum mips32_cpu_vendor vendor;
-
-	/* Supported ISA and boot config */
-	enum mips32_isa_supported isa;
-
-	/* PRID */
-	uint32_t prid;
-
-	/* Processor implemented the MultiThreading ASE */
-	bool mtase;
-
-	/* Processor implemented the DSP ASE */
-	bool dspase;
-
-	/* Processor implemented the SmartMIPS ASE */
-	bool smase;
-
-	/* Processor implemented the MIPS16[e] ASE */
-	bool m16ase;
-
-	/* Processor implemented the microMIPS ASE */
-	bool micromipsase;
-
-	/* Processor implemented the Virtualization ASE */
-	uint32_t vzase;
-
-	uint32_t vz_guest_id_width;
-
-	/* ebase.cpuid number */
-	uint32_t cpuid;
-
-	uint32_t inst_cache_size;
-	uint32_t data_cache_size;
-	uint32_t mmu_type;
-	uint32_t tlb_entries;
-	uint32_t num_shadow_regs;
-
-	/* Processor implemented the MSA module */
-	bool msa;
-
-	/* Processor implemented mfhc0 and mthc0 instructions */
-	bool mvh;
-
-	bool guest_ctl1_present;
-	bool cdmm;
-};
-
 extern const struct command_registration mips32_command_handlers[];
 
 int mips32_arch_state(struct target *target);
@@ -818,4 +867,7 @@ int mips32_checksum_memory(struct target *target, target_addr_t address,
 int mips32_blank_check_memory(struct target *target,
 		struct target_memory_check_block *blocks, int num_blocks, uint8_t erased_value);
 
+bool mips32_cpu_support_sync(struct mips_ejtag *ejtag_info);
+bool mips32_cpu_support_hazard_barrier(struct mips_ejtag *ejtag_info);
+
 #endif /* OPENOCD_TARGET_MIPS32_H */
diff --git a/src/target/mips32_pracc.c b/src/target/mips32_pracc.c
index db50ef9..22edf6a 100644
--- a/src/target/mips32_pracc.c
+++ b/src/target/mips32_pracc.c
@@ -61,6 +61,7 @@
 #include <helper/time_support.h>
 #include <jtag/adapter.h>
 
+#include "mips_cpu.h"
 #include "mips32.h"
 #include "mips32_pracc.h"
 
@@ -452,6 +453,8 @@ static int mips32_pracc_read_u32(struct mips_ejtag *ejtag_info, uint32_t addr, u
 	pracc_add(&ctx, 0, MIPS32_LUI(ctx.isa, 15, PRACC_UPPER_BASE_ADDR));	/* $15 = MIPS32_PRACC_BASE_ADDR */
 	pracc_add(&ctx, 0, MIPS32_LUI(ctx.isa, 8, UPPER16((addr + 0x8000)))); /* load  $8 with modified upper addr */
 	pracc_add(&ctx, 0, MIPS32_LW(ctx.isa, 8, LOWER16(addr), 8));			/* lw $8, LOWER16(addr)($8) */
+	if (mips32_cpu_support_sync(ejtag_info))
+		pracc_add(&ctx, 0, MIPS32_SYNC(ctx.isa));
 	pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT,
 				MIPS32_SW(ctx.isa, 8, PRACC_OUT_OFFSET, 15));	/* sw $8,PRACC_OUT_OFFSET($15) */
 	pracc_add_li32(&ctx, 8, ejtag_info->reg8, 0);				/* restore $8 */
@@ -508,6 +511,8 @@ int mips32_pracc_read_mem(struct mips_ejtag *ejtag_info, uint32_t addr, int size
 			else
 				pracc_add(&ctx, 0, MIPS32_LBU(ctx.isa, 8, LOWER16(addr), 9));
 
+			if (mips32_cpu_support_sync(ejtag_info))
+				pracc_add(&ctx, 0, MIPS32_SYNC(ctx.isa));
 			pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT + i * 4,			/* store $8 at param out */
 					  MIPS32_SW(ctx.isa, 8, PRACC_OUT_OFFSET + i * 4, 15));
 			addr += size;
@@ -550,6 +555,8 @@ int mips32_cp0_read(struct mips_ejtag *ejtag_info, uint32_t *val, uint32_t cp0_r
 	pracc_queue_init(&ctx);
 
 	pracc_add(&ctx, 0, MIPS32_LUI(ctx.isa, 15, PRACC_UPPER_BASE_ADDR));	/* $15 = MIPS32_PRACC_BASE_ADDR */
+	if (mips32_cpu_support_hazard_barrier(ejtag_info))
+		pracc_add(&ctx, 0, MIPS32_EHB(ctx.isa));
 	pracc_add(&ctx, 0, MIPS32_MFC0(ctx.isa, 8, cp0_reg, cp0_sel));		/* move cp0 reg / sel to $8 */
 	pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT,
 				MIPS32_SW(ctx.isa, 8, PRACC_OUT_OFFSET, 15));	/* store $8 to pracc_out */
@@ -571,6 +578,8 @@ int mips32_cp0_write(struct mips_ejtag *ejtag_info, uint32_t val, uint32_t cp0_r
 	pracc_add_li32(&ctx, 15, val, 0);				/* Load val to $15 */
 
 	pracc_add(&ctx, 0, MIPS32_MTC0(ctx.isa, 15, cp0_reg, cp0_sel));		/* write $15 to cp0 reg / sel */
+	if (mips32_cpu_support_hazard_barrier(ejtag_info))
+		pracc_add(&ctx, 0, MIPS32_EHB(ctx.isa));
 	pracc_add(&ctx, 0, MIPS32_B(ctx.isa, NEG16((ctx.code_count + 1) << ctx.isa)));		/* jump to start */
 	pracc_add(&ctx, 0, MIPS32_MFC0(ctx.isa, 15, 31, 0));			/* restore $15 from DeSave */
 
@@ -786,6 +795,7 @@ int mips32_pracc_write_mem(struct mips_ejtag *ejtag_info, uint32_t addr, int siz
 	if ((KSEGX(addr) == KSEG1) || ((addr >= 0xff200000) && (addr <= 0xff3fffff)))
 		return retval; /*Nothing to do*/
 
+	/* Reads Config0 */
 	mips32_cp0_read(ejtag_info, &conf, 16, 0);
 
 	switch (KSEGX(addr)) {
@@ -813,11 +823,28 @@ int mips32_pracc_write_mem(struct mips_ejtag *ejtag_info, uint32_t addr, int siz
 		uint32_t start_addr = addr;
 		uint32_t end_addr = addr + count * size;
 		uint32_t rel = (conf & MIPS32_CONFIG0_AR_MASK) >> MIPS32_CONFIG0_AR_SHIFT;
-		if (rel > 1) {
-			LOG_DEBUG("Unknown release in cache code");
+		/* FIXME: In MIPS Release 6, the encoding of CACHE instr has changed */
+		if (rel > MIPS32_RELEASE_2) {
+			LOG_DEBUG("Unsupported MIPS Release ( > 5)");
 			return ERROR_FAIL;
 		}
 		retval = mips32_pracc_synchronize_cache(ejtag_info, start_addr, end_addr, cached, rel);
+	} else {
+		struct pracc_queue_info ctx = {.ejtag_info = ejtag_info};
+
+		pracc_queue_init(&ctx);
+		if (mips32_cpu_support_sync(ejtag_info))
+			pracc_add(&ctx, 0, MIPS32_SYNC(ctx.isa));
+		if (mips32_cpu_support_hazard_barrier(ejtag_info))
+			pracc_add(&ctx, 0, MIPS32_EHB(ctx.isa));
+		pracc_add(&ctx, 0, MIPS32_B(ctx.isa, NEG16((ctx.code_count + 1) << ctx.isa)));	/* jump to start */
+		pracc_add(&ctx, 0, MIPS32_NOP);
+		ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL, 1);
+		if (ctx.retval != ERROR_OK) {
+			LOG_ERROR("Unable to barrier");
+			retval = ctx.retval;
+		}
+		pracc_queue_free(&ctx);
 	}
 
 	return retval;
@@ -866,6 +893,8 @@ int mips32_pracc_write_regs(struct mips32_common *mips32)
 		pracc_add(&ctx, 0, cp0_write_code[i]);
 	}
 
+	if (mips32_cpu_support_hazard_barrier(ejtag_info))
+		pracc_add(&ctx, 0, MIPS32_EHB(ctx.isa));
 	/* load registers 2 to 31 with li32, optimize */
 	for (int i = 2; i < 32; i++)
 		pracc_add_li32(&ctx, i, gprs[i], 1);
@@ -1019,6 +1048,70 @@ int mips32_pracc_read_regs(struct mips32_common *mips32)
 	return ctx.retval;
 }
 
+/**
+ * mips32_pracc_fastdata_xfer_synchronize_cache - Synchronize cache for fast data transfer
+ * @param[in] ejtag_info: EJTAG information structure
+ * @param[in] addr: Starting address for cache synchronization
+ * @param[in] size: Size of each data element
+ * @param[in] count: Number of data elements
+ *
+ * @brief Synchronizes the cache for fast data transfer based on
+ * the specified address and cache configuration.
+ * If the region is cacheable (write-back cache or write-through cache),
+ * it synchronizes the cache for the specified range.
+ * The synchronization is performed using the MIPS32 cache synchronization function.
+ *
+ * @return ERROR_OK on success; error code on failure.
+ */
+static int mips32_pracc_fastdata_xfer_synchronize_cache(struct mips_ejtag *ejtag_info,
+													uint32_t addr, int size, int count)
+{
+	int retval = ERROR_OK;
+
+	if ((KSEGX(addr) == KSEG1) || (addr >= 0xff200000 && addr <= 0xff3fffff)) // DESEG?
+		return retval; /*Nothing to do*/
+
+	int cached = 0;
+	uint32_t conf = 0;
+
+	mips32_cp0_read(ejtag_info, &conf, 16, 0);
+
+	switch (KSEGX(addr)) {
+		case KUSEG:
+			cached = (conf & MIPS32_CONFIG0_KU_MASK) >> MIPS32_CONFIG0_KU_SHIFT;
+			break;
+		case KSEG0:
+			cached = (conf & MIPS32_CONFIG0_K0_MASK) >> MIPS32_CONFIG0_K0_SHIFT;
+			break;
+		case KSEG2:
+		case KSEG3:
+			cached = (conf & MIPS32_CONFIG0_K23_MASK) >> MIPS32_CONFIG0_K23_SHIFT;
+			break;
+		default:
+			/* what ? */
+			break;
+	}
+
+    /**
+	 * Check cacheability bits coherency algorithm
+	 * is the region cacheable or uncached.
+	 * If cacheable we have to synchronize the cache
+	 */
+	if (cached == 3 || cached == 0) {		/* Write back cache or write through cache */
+		uint32_t start_addr = addr;
+		uint32_t end_addr = addr + count * size;
+		uint32_t rel = (conf & MIPS32_CONFIG0_AR_MASK) >> MIPS32_CONFIG0_AR_SHIFT;
+		/* FIXME: In MIPS Release 6, the encoding of CACHE instr has changed */
+		if (rel > MIPS32_RELEASE_2) {
+			LOG_DEBUG("Unsupported MIPS Release ( > 5)");
+			return ERROR_FAIL;
+		}
+		retval = mips32_pracc_synchronize_cache(ejtag_info, start_addr, end_addr, cached, rel);
+	}
+
+	return retval;
+}
+
 /* fastdata upload/download requires an initialized working area
  * to load the download code; it should not be called otherwise
  * fetch order from the fastdata area
@@ -1039,13 +1132,17 @@ int mips32_pracc_fastdata_xfer(struct mips_ejtag *ejtag_info, struct working_are
 		/* start of fastdata area in t0 */
 		MIPS32_LUI(isa, 8, UPPER16(MIPS32_PRACC_FASTDATA_AREA)),
 		MIPS32_ORI(isa, 8, 8, LOWER16(MIPS32_PRACC_FASTDATA_AREA)),
-		MIPS32_LW(isa, 9, 0, 8),						/* start addr in t1 */
-		MIPS32_LW(isa, 10, 0, 8),						/* end addr to t2 */
-					/* loop: */
+		MIPS32_LW(isa, 9, 0, 8),					/* start addr in t1 */
+		mips32_cpu_support_sync(ejtag_info) ? MIPS32_SYNC(isa) : MIPS32_NOP,				/* barrier for ordering */
+		MIPS32_LW(isa, 10, 0, 8),					/* end addr to t2 */
+		mips32_cpu_support_sync(ejtag_info) ? MIPS32_SYNC(isa) : MIPS32_NOP,				/* barrier for ordering */
+		/* loop: */
 		write_t ? MIPS32_LW(isa, 11, 0, 8) : MIPS32_LW(isa, 11, 0, 9),	/* from xfer area : from memory */
 		write_t ? MIPS32_SW(isa, 11, 0, 9) : MIPS32_SW(isa, 11, 0, 8),	/* to memory      : to xfer area */
 
-		MIPS32_BNE(isa, 10, 9, NEG16(3 << isa)),			/* bne $t2,t1,loop */
+		mips32_cpu_support_sync(ejtag_info) ? MIPS32_SYNC(isa) : MIPS32_NOP,				/* barrier for ordering */
+
+		MIPS32_BNE(isa, 10, 9, NEG16(4 << isa)),			/* bne $t2,t1,loop */
 		MIPS32_ADDI(isa, 9, 9, 4),					/* addi t1,t1,4 */
 
 		MIPS32_LW(isa, 8, MIPS32_FASTDATA_HANDLER_SIZE - 4, 15),
@@ -1055,7 +1152,9 @@ int mips32_pracc_fastdata_xfer(struct mips_ejtag *ejtag_info, struct working_are
 
 		MIPS32_LUI(isa, 15, UPPER16(MIPS32_PRACC_TEXT)),
 		MIPS32_ORI(isa, 15, 15, LOWER16(MIPS32_PRACC_TEXT) | isa),	/* isa bit for JR instr */
-		MIPS32_JR(isa, 15),								/* jr start */
+		mips32_cpu_support_hazard_barrier(ejtag_info)
+			? MIPS32_JRHB(isa, 15)
+			: MIPS32_JR(isa, 15),								/* jr start */
 		MIPS32_MFC0(isa, 15, 31, 0),					/* move COP0 DeSave to $15 */
 	};
 
@@ -1075,7 +1174,9 @@ int mips32_pracc_fastdata_xfer(struct mips_ejtag *ejtag_info, struct working_are
 	uint32_t jmp_code[] = {
 		MIPS32_LUI(isa, 15, UPPER16(source->address)),			/* load addr of jump in $15 */
 		MIPS32_ORI(isa, 15, 15, LOWER16(source->address) | isa),	/* isa bit for JR instr */
-		MIPS32_JR(isa, 15),						/* jump to ram program */
+		mips32_cpu_support_hazard_barrier(ejtag_info)
+			? MIPS32_JRHB(isa, 15)
+			: MIPS32_JR(isa, 15),	/* jump to ram program */
 		isa ? MIPS32_XORI(isa, 15, 15, 1) : MIPS32_NOP,	/* drop isa bit, needed for LW/SW instructions */
 	};
 
@@ -1139,5 +1240,5 @@ int mips32_pracc_fastdata_xfer(struct mips_ejtag *ejtag_info, struct working_are
 	if (ejtag_info->pa_addr != MIPS32_PRACC_TEXT)
 		LOG_ERROR("mini program did not return to start");
 
-	return retval;
+	return mips32_pracc_fastdata_xfer_synchronize_cache(ejtag_info, addr, 4, count);
 }
diff --git a/src/target/mips32_pracc.h b/src/target/mips32_pracc.h
index 587a446..78d0872 100644
--- a/src/target/mips32_pracc.h
+++ b/src/target/mips32_pracc.h
@@ -53,6 +53,8 @@ struct pracc_queue_info {
 	struct pa_list *pracc_list;	/* Code and store addresses at dmseg */
 };
 
+struct mips32_common;
+
 void pracc_queue_init(struct pracc_queue_info *ctx);
 void pracc_add(struct pracc_queue_info *ctx, uint32_t addr, uint32_t instr);
 void pracc_queue_free(struct pracc_queue_info *ctx);
diff --git a/src/target/mips_cpu.h b/src/target/mips_cpu.h
index 8190b32..2f31dbd 100644
--- a/src/target/mips_cpu.h
+++ b/src/target/mips_cpu.h
@@ -12,7 +12,12 @@
 /* Assigned Company values for bits 23:16 of the PRId register. */
 #define	PRID_COMP_MASK		0xff0000
 
-#define	PRID_COMP_LEGACY	0x000000
+#define	PRID_COMP_LEGACY    0x000000
+#define	PRID_COMP_MTI       0x010000
+#define PRID_COMP_BROADCOM  0x020000
+#define PRID_COMP_ALCHEMY   0x030000
+#define PRID_COMP_LEXRA     0x0b0000
+#define PRID_COMP_ALTERA    0x100000
 #define	PRID_COMP_INGENIC_E1	0xe10000
 
 /*
@@ -22,6 +27,12 @@
  */
 #define PRID_IMP_MASK		0xff00
 
+#define PRID_IMP_MAPTIV_UC      0x9D00
+#define PRID_IMP_MAPTIV_UP      0x9E00
+#define PRID_IMP_IAPTIV_CM      0xA000
+#define PRID_IMP_IAPTIV         0xA100
+#define PRID_IMP_M5150          0xA700
+
 #define PRID_IMP_XBURST_REV1	0x0200 /* XBurst®1 with MXU1.0/MXU1.1 SIMD ISA */
 
 #endif /* OPENOCD_TARGET_MIPS_CPU_H */
diff --git a/src/target/mips_ejtag.c b/src/target/mips_ejtag.c
index 5c92bf9..389461c 100644
--- a/src/target/mips_ejtag.c
+++ b/src/target/mips_ejtag.c
@@ -47,7 +47,7 @@ int mips_ejtag_get_idcode(struct mips_ejtag *ejtag_info)
 	return mips_ejtag_drscan_32(ejtag_info, &ejtag_info->idcode);
 }
 
-static int mips_ejtag_get_impcode(struct mips_ejtag *ejtag_info)
+int mips_ejtag_get_impcode(struct mips_ejtag *ejtag_info)
 {
 	mips_ejtag_set_instr(ejtag_info, EJTAG_INST_IMPCODE);
 
@@ -332,7 +332,7 @@ static void ejtag_v26_print_imp(struct mips_ejtag *ejtag_info)
 		EJTAG_IMP_HAS(EJTAG_V26_IMP_DINT) ? " DINT" : "");
 }
 
-static void ejtag_main_print_imp(struct mips_ejtag *ejtag_info)
+void ejtag_main_print_imp(struct mips_ejtag *ejtag_info)
 {
 	LOG_DEBUG("EJTAG main: features:%s%s%s%s%s",
 		EJTAG_IMP_HAS(EJTAG_IMP_ASID8) ? " ASID_8" : "",
diff --git a/src/target/mips_ejtag.h b/src/target/mips_ejtag.h
index 852444a..7376e51 100644
--- a/src/target/mips_ejtag.h
+++ b/src/target/mips_ejtag.h
@@ -186,10 +186,27 @@
 #define EJTAG64_V25_IBA0		0xFFFFFFFFFF301100ull
 #define EJTAG64_V25_IBS			0xFFFFFFFFFF301000ull
 
+static const struct dcr_feature {
+	int bit;
+	const char *name;
+} dcr_features[] = {
+	{22, "DAS"},
+	{18, "FDC"},
+	{17, "DataBrk"},
+	{16, "InstBrk"},
+	{15, "Inverted Data value"},
+	{14, "Data value stored"},
+	{10, "Complex Breakpoints"},
+	{ 9, "PC Sampling"},
+};
+
+#define EJTAG_DCR_ENTRIES (ARRAY_SIZE(dcr_features))
+
 struct mips_ejtag {
 	struct jtag_tap *tap;
 	uint32_t impcode;
 	uint32_t idcode;
+	uint32_t prid;
 	uint32_t ejtag_ctrl;
 	int fast_access_save;
 	uint32_t config_regs;	/* number of config registers read */
@@ -244,6 +261,9 @@ int mips_ejtag_init(struct mips_ejtag *ejtag_info);
 int mips_ejtag_config_step(struct mips_ejtag *ejtag_info, int enable_step);
 int mips64_ejtag_config_step(struct mips_ejtag *ejtag_info, bool enable_step);
 
+void ejtag_main_print_imp(struct mips_ejtag *ejtag_info);
+int mips_ejtag_get_impcode(struct mips_ejtag *ejtag_info);
+
 static inline void mips_le_to_h_u32(jtag_callback_data_t arg)
 {
 	uint8_t *in = (uint8_t *)arg;
diff --git a/src/target/target.c b/src/target/target.c
index 216dcb2..8fddce7 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -1214,6 +1214,10 @@ int target_run_read_async_algorithm(struct target *target,
 		/* Avoid GDB timeouts */
 		keep_alive();
 
+		if (openocd_is_shutdown_pending()) {
+			retval = ERROR_SERVER_INTERRUPTED;
+			break;
+		}
 	}
 
 	if (retval != ERROR_OK) {
@@ -3225,8 +3229,11 @@ int target_wait_state(struct target *target, enum target_state state, unsigned i
 				nvp_value2name(nvp_target_state, state)->name);
 		}
 
-		if (cur-then > 500)
+		if (cur - then > 500) {
 			keep_alive();
+			if (openocd_is_shutdown_pending())
+				return ERROR_SERVER_INTERRUPTED;
+		}
 
 		if ((cur-then) > ms) {
 			LOG_ERROR("timed out while waiting for target %s",
@@ -3509,6 +3516,11 @@ static int target_fill_mem(struct target *target,
 			break;
 		/* avoid GDB timeouts */
 		keep_alive();
+
+		if (openocd_is_shutdown_pending()) {
+			retval = ERROR_SERVER_INTERRUPTED;
+			break;
+		}
 	}
 	free(target_buf);
 
@@ -3851,6 +3863,12 @@ static COMMAND_HELPER(handle_verify_image_command_internal, enum verify_mode ver
 							}
 						}
 						keep_alive();
+						if (openocd_is_shutdown_pending()) {
+							retval = ERROR_SERVER_INTERRUPTED;
+							free(data);
+							free(buffer);
+							goto done;
+						}
 					}
 				}
 				free(data);
@@ -6910,8 +6928,8 @@ static const struct command_registration target_exec_command_handlers[] = {
 		.mode = COMMAND_ANY,
 		.help = "Load image into server memory for later use by "
 			"fast_load; primarily for profiling",
-		.usage = "filename address ['bin'|'ihex'|'elf'|'s19'] "
-			"[min_address [max_length]]",
+		.usage = "filename [address ['bin'|'ihex'|'elf'|'s19' "
+			"[min_address [max_length]]]]",
 	},
 	{
 		.name = "fast_load",
@@ -7084,8 +7102,8 @@ static const struct command_registration target_exec_command_handlers[] = {
 		.name = "load_image",
 		.handler = handle_load_image_command,
 		.mode = COMMAND_EXEC,
-		.usage = "filename address ['bin'|'ihex'|'elf'|'s19'] "
-			"[min_address] [max_length]",
+		.usage = "filename [address ['bin'|'ihex'|'elf'|'s19' "
+			"[min_address [max_length]]]]",
 	},
 	{
 		.name = "dump_image",
diff --git a/src/target/target.h b/src/target/target.h
index 8b2e362..975561b 100644
--- a/src/target/target.h
+++ b/src/target/target.h
@@ -802,6 +802,8 @@ int target_profiling_default(struct target *target, uint32_t *samples, uint32_t
 #define ERROR_TARGET_NOT_EXAMINED (-311)
 #define ERROR_TARGET_DUPLICATE_BREAKPOINT (-312)
 #define ERROR_TARGET_ALGO_EXIT  (-313)
+#define ERROR_TARGET_SIZE_NOT_SUPPORTED  (-314)
+#define ERROR_TARGET_PACKING_NOT_SUPPORTED  (-315)
 
 extern bool get_target_reset_nag(void);
 
diff --git a/tcl/board/ti_j722sevm.cfg b/tcl/board/ti_j722sevm.cfg
new file mode 100644
index 0000000..6a5c2d9
--- /dev/null
+++ b/tcl/board/ti_j722sevm.cfg
@@ -0,0 +1,24 @@
+# SPDX-License-Identifier: GPL-2.0-or-later
+# Copyright (C) 2023 Texas Instruments Incorporated - https://www.ti.com/
+#
+# Texas Instruments EVM-J722S: https://www.ti.com/lit/zip/sprr495
+#
+
+# J722S EVM has an xds110 onboard.
+source [find interface/xds110.cfg]
+
+transport select jtag
+
+# default JTAG configuration has only SRST and no TRST
+reset_config srst_only srst_push_pull
+
+# delay after SRST goes inactive
+adapter srst delay 20
+
+if { ![info exists SOC] } {
+	set SOC j722s
+}
+
+source [find target/ti_k3.cfg]
+
+adapter speed 2500
diff --git a/tcl/target/geehy/apm32f0x.cfg b/tcl/target/geehy/apm32f0x.cfg
new file mode 100644
index 0000000..502c092
--- /dev/null
+++ b/tcl/target/geehy/apm32f0x.cfg
@@ -0,0 +1,49 @@
+# SPDX-License-Identifier: GPL-2.0-or-later
+
+#
+# Geehy APM32F0x target
+#
+# https://global.geehy.com/MCU
+#
+
+#
+# APM32F0x devices support SWD transport only.
+#
+source [find target/swj-dp.tcl]
+
+if { [info exists CHIPNAME] } {
+   set _CHIPNAME $CHIPNAME
+} else {
+   set _CHIPNAME apm32f0x
+}
+
+# Work-area is a space in RAM used for flash programming, by default use 1 KiB.
+if { [info exists WORKAREASIZE] } {
+   set _WORKAREASIZE $WORKAREASIZE
+} else {
+   set _WORKAREASIZE 0x400
+}
+
+if { [info exists CPUTAPID] } {
+   set _CPUTAPID $CPUTAPID
+} else {
+   set _CPUTAPID 0x0bc11477
+}
+
+swj_newdap $_CHIPNAME cpu -expected-id $_CPUTAPID
+dap create $_CHIPNAME.dap -chain-position $_CHIPNAME.cpu
+
+set _TARGETNAME $_CHIPNAME.cpu
+target create $_TARGETNAME cortex_m -endian little -dap $_CHIPNAME.dap
+
+$_TARGETNAME configure -work-area-phys 0x20000000 -work-area-size $_WORKAREASIZE -work-area-backup 0
+
+set _FLASHNAME $_CHIPNAME.flash
+flash bank $_FLASHNAME stm32f1x 0x08000000 0 0 0 $_TARGETNAME
+
+adapter speed 1000
+
+if {![using_hla]} {
+   # if srst is not fitted use SYSRESETREQ to perform a soft reset.
+   cortex_m reset_config sysresetreq
+}
diff --git a/tcl/target/geehy/apm32f1x.cfg b/tcl/target/geehy/apm32f1x.cfg
new file mode 100644
index 0000000..dc42e06
--- /dev/null
+++ b/tcl/target/geehy/apm32f1x.cfg
@@ -0,0 +1,57 @@
+# SPDX-License-Identifier: GPL-2.0-or-later
+
+#
+# Geehy APM32F1x target
+#
+# https://global.geehy.com/MCU
+#
+
+#
+# APM32F1x devices support JTAG and SWD transport.
+#
+source [find target/swj-dp.tcl]
+
+if { [info exists CHIPNAME] } {
+   set _CHIPNAME $CHIPNAME
+} else {
+   set _CHIPNAME apm32f1x
+}
+
+# Work-area is a space in RAM used for flash programming, by default use 4 KiB.
+if { [info exists WORKAREASIZE] } {
+   set _WORKAREASIZE $WORKAREASIZE
+} else {
+   set _WORKAREASIZE 0x1000
+}
+
+if { [info exists CPUTAPID] } {
+   set _CPUTAPID $CPUTAPID
+} else {
+   if { [using_jtag] } {
+      set _CPUTAPID 0x4ba00477
+   } {
+      set _CPUTAPID 0x2ba01477
+   }
+}
+
+swj_newdap $_CHIPNAME cpu -irlen 4 -expected-id $_CPUTAPID
+dap create $_CHIPNAME.dap -chain-position $_CHIPNAME.cpu
+
+if {[using_jtag]} {
+   jtag newtap $_CHIPNAME bs -irlen 5
+}
+
+set _TARGETNAME $_CHIPNAME.cpu
+target create $_TARGETNAME cortex_m -endian little -dap $_CHIPNAME.dap
+
+$_TARGETNAME configure -work-area-phys 0x20000000 -work-area-size $_WORKAREASIZE -work-area-backup 0
+
+set _FLASHNAME $_CHIPNAME.flash
+flash bank $_FLASHNAME stm32f1x 0 0 0 0 $_TARGETNAME
+
+adapter speed 1000
+
+if {![using_hla]} {
+   # if srst is not fitted use SYSRESETREQ to perform a soft reset.
+   cortex_m reset_config sysresetreq
+}
diff --git a/tcl/target/geehy/apm32f4x.cfg b/tcl/target/geehy/apm32f4x.cfg
new file mode 100644
index 0000000..3ed58d1
--- /dev/null
+++ b/tcl/target/geehy/apm32f4x.cfg
@@ -0,0 +1,57 @@
+# SPDX-License-Identifier: GPL-2.0-or-later
+
+#
+# Geehy APM32F4x target
+#
+# https://global.geehy.com/MCU
+#
+
+#
+# APM32F4x devices support JTAG and SWD transport.
+#
+source [find target/swj-dp.tcl]
+
+if { [info exists CHIPNAME] } {
+   set _CHIPNAME $CHIPNAME
+} else {
+   set _CHIPNAME apm32f4x
+}
+
+# Work-area is a space in RAM used for flash programming, by default use 4 KiB.
+if { [info exists WORKAREASIZE] } {
+   set _WORKAREASIZE $WORKAREASIZE
+} else {
+   set _WORKAREASIZE 0x1000
+}
+
+if { [info exists CPUTAPID] } {
+   set _CPUTAPID $CPUTAPID
+} else {
+   if { [using_jtag] } {
+     set _CPUTAPID 0x4ba00477
+   } else {
+     set _CPUTAPID 0x2ba01477
+   }
+}
+
+swj_newdap $_CHIPNAME cpu -irlen 4 -ircapture 0x1 -irmask 0xf -expected-id $_CPUTAPID
+dap create $_CHIPNAME.dap -chain-position $_CHIPNAME.cpu
+
+if { [using_jtag] } {
+   jtag newtap $_CHIPNAME bs -irlen 5
+}
+
+set _TARGETNAME $_CHIPNAME.cpu
+target create $_TARGETNAME cortex_m -endian little -dap $_CHIPNAME.dap
+
+$_TARGETNAME configure -work-area-phys 0x20000000 -work-area-size $_WORKAREASIZE -work-area-backup 0
+
+set _FLASHNAME $_CHIPNAME.flash
+flash bank $_FLASHNAME stm32f2x 0 0 0 0 $_TARGETNAME
+
+adapter speed 1000
+
+if {![using_hla]} {
+   # if srst is not fitted use SYSRESETREQ to perform a soft reset.
+   cortex_m reset_config sysresetreq
+}
diff --git a/tcl/target/marvell/cn9130.cfg b/tcl/target/marvell/cn9130.cfg
new file mode 100644
index 0000000..23e472f
--- /dev/null
+++ b/tcl/target/marvell/cn9130.cfg
@@ -0,0 +1,178 @@
+# SPDX-License-Identifier: GPL-2.0-or-later
+
+# cn9130 -- support for the Marvell Octeon TX2 / CN9130 CPU family
+#
+# henrik.nordstorm@addiva.se, Nov 2023
+
+if { [info exists CHIPNAME] } {
+   set _CHIPNAME $CHIPNAME
+} else {
+   set _CHIPNAME cn9130
+}
+
+if { [info exists MASTERTAPID] } {
+   set _MASTERTAPID $MASTERTAPID
+} else {
+   set _MASTERTAPID 0x07025357
+}
+
+if { [info exists APTAPID] } {
+   set _APTAPID $APTAPID
+} else {
+   set _APTAPID 0x4ba00477
+}
+
+if { [info exists SBTAPID] } {
+   set _SBTAPID $SBTAPID
+} else {
+   set _SBTAPID 0x4ba00477
+}
+
+if { [info exists CORES] } {
+   set _CORES $CORES
+} else {
+   set _CORES 4
+}
+
+# CTI base address should be possible to read from the CoreSight
+# ROM table like how the DBG base address is when not specified.
+if { [info exists CTIBASE] } {
+   set _CTIBASE $CTIBASE
+} else {
+   set _CTIBASE {0x80420000 0x80520000 0x80620000 0x80720000}
+}
+
+# CN9130 is a multi-die chip and has a multi level hierarchical
+# JTAG TAP, where all the DAPs are disabled at reset, requiring
+# both configuration to enable access to the chip DAPs, and a
+# vendor specific bypass IR instruction to access the slave TAPs
+# via the master TAP. In addition there is a number of sample
+# bits that should be ignored.
+#
+# The default BYPASS instruction in the master TAP bypasses the
+# whole chip and not only the master TAP. And similarly on
+# IDCODE the master TAP only responds with it's own ID and
+# bypasses the other TAPs on the chip, while OpenOCD expects
+# ID from all enabled TAPs in the chain.
+
+# Bootstrap with the default boundary scan oriented TAP configuration
+# where the master,ap,sb TAPs are seen as one big fat TAP, which matches
+# what OpenOCD expects from IDCODE and BYPASS.
+
+jtag newtap $_CHIPNAME bs  -irlen 19 -enable -expected-id $_MASTERTAPID
+
+# Declare the full JTAG chain, but in disabled state during setup
+
+jtag newtap $_CHIPNAME sample4 -irlen 1 -disable
+jtag newtap $_CHIPNAME sample3 -irlen 1 -disable
+jtag newtap $_CHIPNAME sample2 -irlen 1 -disable
+jtag newtap $_CHIPNAME ap.cpu  -irlen 4 -disable -expected-id $_APTAPID
+jtag newtap $_CHIPNAME ap      -irlen 5 -disable
+jtag newtap $_CHIPNAME sample1 -irlen 1 -disable
+jtag newtap $_CHIPNAME sb.cpu  -irlen 4 -disable -expected-id $_SBTAPID
+jtag newtap $_CHIPNAME sb      -irlen 5 -disable
+jtag newtap $_CHIPNAME master  -irlen 5 -disable -ir-bypass 0x11 -expected-id $_MASTERTAPID
+
+# Once the iniial IDCODE scan has completed switch to more detailed
+# scan chain giving access to the individual chip TAPs.
+
+jtag configure $_CHIPNAME.bs -event setup "cn9130_enable_full_chain $_CHIPNAME"
+
+proc cn9130_enable_full_chain { _CHIPNAME } {
+   # Switch to detailed TAP declaration
+   jtag tapdisable $_CHIPNAME.bs
+   jtag tapenable $_CHIPNAME.master
+   jtag tapenable $_CHIPNAME.sb
+   jtag tapenable $_CHIPNAME.sample1
+   jtag tapenable $_CHIPNAME.ap
+   jtag tapenable $_CHIPNAME.sample2
+   jtag tapenable $_CHIPNAME.sample3
+   jtag tapenable $_CHIPNAME.sample4
+}
+
+# AP & SB TAPs have a config register to enable/disable access to
+# the auxilary DAP TAP. Default off which hides the DAP TAP from
+# the scan chain.
+proc cn9130_dap_config { chip tap state } {
+   irscan $chip.$tap 0x12
+   drscan $chip.$tap 32 $state
+}
+
+jtag configure $_CHIPNAME.bs -event tap-disable ""
+jtag configure $_CHIPNAME.bs -event tap-enable ""
+jtag configure $_CHIPNAME.sample4 -event tap-enable ""
+jtag configure $_CHIPNAME.sample3 -event tap-enable ""
+jtag configure $_CHIPNAME.sample2 -event tap-enable ""
+jtag configure $_CHIPNAME.ap.cpu -event tap-disable "cn9130_dap_config $_CHIPNAME ap 0"
+jtag configure cn9130.ap.cpu -event tap-enable "cn9130_dap_config $_CHIPNAME ap 1"
+jtag configure $_CHIPNAME.ap -event tap-enable ""
+jtag configure $_CHIPNAME.sample1 -event tap-enable ""
+jtag configure $_CHIPNAME.sb.cpu -event tap-disable "cn9130_dap_config $_CHIPNAME sb 0"
+jtag configure cn9130.sb.cpu -event tap-enable "cn9130_dap_config $_CHIPNAME sb 1"
+jtag configure $_CHIPNAME.sb -event tap-enable ""
+jtag configure $_CHIPNAME.master -event tap-enable ""
+
+dap create $_CHIPNAME.ap.dap -chain-position $_CHIPNAME.ap.cpu
+
+# Main bus
+target create $_CHIPNAME.ap.axi mem_ap \
+   -dap $_CHIPNAME.ap.dap \
+   -ap-num 0
+
+# Periperials bus
+target create $_CHIPNAME.ap.apb mem_ap \
+   -dap $_CHIPNAME.ap.dap \
+   -ap-num 1
+
+# MSS bus
+target create $_CHIPNAME.ap.ahb mem_ap \
+   -dap $_CHIPNAME.ap.dap \
+   -ap-num 2
+
+# AP A72 CPU cores
+set _smp_command ""
+for { set _core 0 } { $_core < $_CORES } { incr _core 1 } {
+   cti create $_CHIPNAME.ap.cti.$_core \
+      -dap $_CHIPNAME.ap.dap \
+      -baseaddr [ lindex $_CTIBASE $_core ]  \
+      -ap-num 1
+
+   if { $_core == 0 } {
+      target create $_CHIPNAME.ap.a72.$_core aarch64 \
+         -dap $_CHIPNAME.ap.dap \
+         -ap-num 1 \
+         -cti $_CHIPNAME.ap.cti.$_core \
+         -coreid $_core \
+         -rtos hwthread
+      set _smp_command "target smp $_CHIPNAME.ap.a72.$_core"
+   } else {
+      # Defer non-boot cores. Held hard in reset until
+      # SMP is activated.
+      target create $_CHIPNAME.ap.a72.$_core aarch64 \
+         -dap $_CHIPNAME.ap.dap \
+         -ap-num 1 \
+         -cti $_CHIPNAME.ap.cti.$_core \
+         -coreid $_core \
+         -defer-examine
+      set _smp_command "$_smp_command $_CHIPNAME.ap.a72.$_core"
+   }
+
+}
+
+# Set up the A72 cluster as SMP
+# Note: Only the boot core is active by default. The other core DAPs can
+# be enabled by arp_examine after they have been released from hard reset.
+eval $_smp_command
+
+# AP MSS M3 CPU core. Defer as it is held in reset until firmware is loaded.
+target create $_CHIPNAME.ap.mss cortex_m -dap $_CHIPNAME.ap.dap -ap-num 2 -defer-examine
+
+# Why is this needed? reset fails with "Debug regions are unpowered" otherwise
+$_CHIPNAME.ap.axi configure -event examine-start "dap init"
+
+# Automate enabling the AP A72 DAP once the full scan chain is enabled
+proc cn9130_ap_setup { _CHIPNAME } {
+   jtag tapenable $_CHIPNAME.ap.cpu
+   targets $_CHIPNAME.ap.a72.0
+}
+jtag configure $_CHIPNAME.ap -event setup "cn9130_ap_setup $_CHIPNAME"
diff --git a/tcl/target/s32k.cfg b/tcl/target/s32k.cfg
new file mode 100644
index 0000000..3ff3239
--- /dev/null
+++ b/tcl/target/s32k.cfg
@@ -0,0 +1,79 @@
+# SPDX-License-Identifier: GPL-2.0-or-later
+
+#
+# Freescale S32K devices
+# Similar to Kinetis Kx series devices.
+#
+
+source [find target/swj-dp.tcl]
+
+if { [info exists CHIPNAME] } {
+    set _CHIPNAME $CHIPNAME
+} else {
+    set _CHIPNAME s32k
+}
+
+# Work-area is a space in RAM used for flash programming
+# By default use 4kB
+if { [info exists WORKAREASIZE] } {
+   set _WORKAREASIZE $WORKAREASIZE
+} else {
+   set _WORKAREASIZE 0x1000
+}
+
+if { [info exists CPUTAPID] } {
+    set _CPUTAPID $CPUTAPID
+} else {
+    set _CPUTAPID 0x0995001d
+}
+
+swj_newdap $_CHIPNAME cpu -irlen 4 -ircapture 0x1 -irmask 0xf -expected-id $_CPUTAPID
+dap create $_CHIPNAME.dap -chain-position $_CHIPNAME.cpu
+
+set _TARGETNAME $_CHIPNAME.cpu
+target create $_TARGETNAME cortex_m -dap $_CHIPNAME.dap
+
+$_TARGETNAME configure -work-area-phys 0x20000000 -work-area-size $_WORKAREASIZE -work-area-backup 0
+
+set _FLASHNAME $_CHIPNAME.pflash
+flash bank $_FLASHNAME kinetis 0 0 0 0 $_TARGETNAME -s32k
+kinetis create_banks
+
+adapter speed 1000
+
+reset_config srst_nogate
+
+if {[using_hla]} {
+   echo ""
+   echo "!!!!!!!!!!!!!!!!!!!!!! WARNING !!!!!!!!!!!!!!!!! WARNING !!!!!!!!!!!!!!!!!!!!!!"
+   echo " Kinetis MCUs have a MDM-AP dedicated mainly to MCU security related functions."
+   echo " A high level adapter (like a ST-Link) you are currently using cannot access"
+   echo " the MDM-AP, so commands like 'mdm mass_erase' are not available in your"
+   echo " configuration. Also security locked state of the device will not be reported."
+   echo " Expect problems connecting to a blank device without boot ROM."
+   echo ""
+   echo " Be very careful as you can lock the device though there is no way to unlock"
+   echo " it without mass erase. Don't set write protection on the first block."
+   echo "!!!!!!!!!!!!!!!!!!!!!! WARNING !!!!!!!!!!!!!!!!! WARNING !!!!!!!!!!!!!!!!!!!!!!"
+   echo ""
+} else {
+   # Detect secured MCU or boot lock-up in RESET/WDOG loop
+   $_TARGETNAME configure -event examine-fail {
+      kinetis mdm check_security
+   }
+   # During RESET/WDOG loop the target is sometimes falsely examined
+   $_TARGETNAME configure -event examine-end {
+      kinetis mdm check_security
+   }
+
+   # if srst is not fitted use SYSRESETREQ to
+   # perform a soft reset
+   cortex_m reset_config sysresetreq
+}
+
+# Disable watchdog not to disturb OpenOCD algorithms running on MCU
+# (e.g. armv7m_checksum_memory() in verify_image)
+# Flash driver also disables watchdog before FTFA flash programming.
+$_TARGETNAME configure -event reset-init {
+   kinetis disable_wdog
+}
diff --git a/tcl/target/ti_k3.cfg b/tcl/target/ti_k3.cfg
index 23825b8..ebea821 100644
--- a/tcl/target/ti_k3.cfg
+++ b/tcl/target/ti_k3.cfg
@@ -24,6 +24,8 @@
 #  Has 2 ARMV8 Cores and 6 R5 Cores and an M3
 # * J721S2: https://www.ti.com/lit/pdf/spruj28
 #  Has 2 ARMV8 Cores and 6 R5 Cores and an M4F
+# * J722S: https://www.ti.com/lit/zip/sprujb3
+#  Has 4 ARMV8 Cores and 3 R5 Cores
 # * J784S4/AM69: http://www.ti.com/lit/zip/spruj52
 #  Has 8 ARMV8 Cores and 8 R5 Cores
 #
@@ -185,6 +187,7 @@ switch $_soc {
 		set _dmem_emu_base_address_map_to 0x1d500000
 		set _dmem_emu_ap_list 1
 	}
+	j722s -
 	am62p -
 	am62a7 {
 		set _K3_DAP_TAPID 0x0bb8d02f
@@ -211,6 +214,14 @@ switch $_soc {
 			set _K3_DAP_TAPID 0x0bb9d02f
 			set R5_NAMES {wkup0_r5.0 mcu0_r5.0}
 		}
+		# Overrides for j722s
+		if { "$_soc" == "j722s" } {
+			set _K3_DAP_TAPID 0x0bba002f
+			set _r5_cores 3
+			set R5_NAMES {wkup0_r5.0 main0_r5.0 mcu0_r5.0}
+			set R5_DBGBASE {0x9d410000 0x9d510000 0x9d810000}
+			set R5_CTIBASE {0x9d418000 0x9d518000 0x9d818000}
+		}
 	}
 	j721e {
 		set _K3_DAP_TAPID 0x0bb6402f