Merge branch 'master' into from_upstream

Conflicts:
	.github/workflows/snapshot.yml
	.gitmodules
	src/flash/nor/drivers.c
	src/helper/jep106.inc
	src/rtos/hwthread.c
	src/target/riscv/riscv.c
	src/target/target.c

Change-Id: I62f65e10d15dcda4c405d4042cce1d96f8e1680a

212 files changed, 11810 insertions, 1833 deletions

diff --git a/.gitmodules b/.gitmodules
index b99c87a..410a2c2 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -1,3 +1,3 @@
 [submodule "jimtcl"]
 	path = jimtcl
-	url = https://github.com/msteveb/jimtcl
+	url = https://github.com/msteveb/jimtcl.git
diff --git a/Makefile.am b/Makefile.am
index 36f6c4f..2d276eb 100644
--- a/Makefile.am
+++ b/Makefile.am
@@ -43,13 +43,14 @@ if INTERNAL_JIMTCL
 AM_CPPFLAGS += -I$(top_srcdir)/jimtcl \
 			   -I$(top_builddir)/jimtcl
 endif
+EXTRA_DIST_NEWS != ls $(srcdir)/NEWS-*
 EXTRA_DIST += \
 	BUGS \
 	HACKING \
 	NEWTAPS \
 	README.Windows \
 	README.OSX \
-	$(wildcard $(srcdir)/NEWS*) \
+	$(EXTRA_DIST_NEWS) \
 	Doxyfile.in \
 	tools/logger.pl \
 	tools/rlink_make_speed_table \
diff --git a/NEWS b/NEWS
index 9df1650..5c04e34 100644
--- a/NEWS
+++ b/NEWS
@@ -2,27 +2,232 @@ This file includes highlights of the changes made in the OpenOCD
 source archive release.
 
 JTAG Layer:
+	* add debug level 4 for verbose I/O debug
+	* bitbang, add read buffer to improve performance
+	* Cadence SystemVerilog Direct Programming Interface (DPI) adapter driver
+	* CMSIS-DAP v2 (USB bulk based) adapter driver
+	* Cypress KitProg adapter driver
+	* FTDI FT232R sync bitbang adapter driver
+	* Linux GPIOD bitbang adapter driver through libgpiod
+	* Mellanox rshim USB or PCIe adapter driver
+	* Nuvoton Nu-Link and Nu-Link2 adapter drivers
+	* NXP IMX GPIO mmap based adapter driver
+	* ST-Link consolidate all versions in single config
+	* ST-Link read properly old USB serial numbers
+	* STLink/V3 support (for ST devices only !)
+	* STM8 SWIM transport
+	* TI XDS110 adapter driver
+	* Xilinx XVC/PCIe adapter driver
 
 Boundary Scan:
 
 Target Layer:
+	* 64 bit address support
+	* ARCv2 target support
+	* ARM Cortex-A hypervisor mode support
+	* ARM Cortex-M fast PC sampling support for profiling
+	* ARM generic CTI support
+	* ARM generic mem-ap target support
+	* ARMv7-A MMU tools
+	* ARMv7m traces add TCP stream server
+	* ARMv8 AARCH64 target support and semihosting support
+	* ARMv8 AARCH64 disassembler support through capstone library
+	* ARMv8-M target support
+	* EnSilica eSi-RISC target support, including instruction tracing
+          eSi-Trace support
+	* MIPS64 target support
+	* Motorola SREC S6 record image file support
+	* RISC-V target support
+	* SEGGER Real Time Transfer (RTT) initial support (for single target,
+	  Cortex-M only)
+	* ST STM8 target support
+	* Various MIPS32 target improvements
 
 Flash Layer:
+	* Atheros (ath79) SPI interface support
+	* Atmel atmega128rfa1 support
+	* Atmel SAM D21, D51, DA1, E51, E53, E54, G55, R30 support
+	* Atmel SAMC2?N* support
+	* Cypress PSoC5LP, PSoC6 support
+	* EnSilica eSi-RISC support
+	* Foshan Synwit Tech SWM050 support
+	* Maxim Integrated MAX32XXX support
+	* Nordic Semiconductor nRF51822, nRF52810, nRF52832 support
+	* NXP Kinetis K27, K28, KE1x, KEAx, KL28, KL8x, KV5x, KWx support
+	* Renesas RPC HF support
+	* SH QSPI support
+	* SiFive Freedom E support
+	* Silicon Labs EFR-family, EZR32HG support
+	* ST BlueNRG support
+	* ST STM32 QUAD/OCTO-SPI interface support for Flash, FRAM and EEPROM
+	* ST STM32F72x, STM32F4x3, STM32H7xx support
+	* ST STM32G0xx, STM32G4xx, STM32L4x, STM32WB, STM32WL support
+	* ST STM32L5x support (non secure mode)
+	* TI CC13xx, CC26xx, CC32xx support
+	* TI MSP432 support
+	* Winner Micro w600 support
+	* Xilinx XCF platform support
+	* Various discrete SPI NOR flashes support
 
 Board, Target, and Interface Configuration Scripts:
+	* 8devices LIMA board config
+	* Achilles Instant-Development Kit Arria 10 board config
+	* Amazon Kindle 2 and DX board config
+	* Analog Devices ADSP-SC58x, ADSP-SC584-EZBRD board config
+	* Andes Technology ADP-XC7KFF676 board config
+	* Andes Technology Corvette-F1 board config
+	* ARM Musca A board config
+	* Arty Spartan 7 FPGA board config
+	* Atmel SAMD10 Xplained mini board config
+	* Atmel SAMD11 Xplained Pro board config
+	* Atmel SAM G55 Xplained Pro board config
+	* AVNET UltraZED EG StarterKit board config
+	* Blue Pill STM32F103C8 board config
+	* DP Busblaster v4.1a board config
+	* DPTechnics DPT-Board-v1 board config
+	* Emcraft imx8 SOM BSB board config
+	* Globalscale ESPRESSObin board config
+	* Kasli board config
+	* Kintex Ultrascale XCKU040 board config
+	* Knovative KC-100 board config
+	* LeMaker HiKey board config
+	* Microchip (Atmel) SAME54 Xplained Pro board config
+	* Microchip (Atmel) SAML11 Xplained Pro board config
+	* Nordic module NRF52 board config
+	* Numato Lab Mimas A7 board config
+	* NXP Freedom FRDM-LS1012A board config
+	* NXP IMX7SABRE board config
+	* NXP IMX8MP-EVK board config
+	* NXP MC-IMX8M-EVK board config
+	* QuickLogic QuickFeather board config
+	* Renesas R-Car E2, H2, M2 board config
+	* Renesas R-Car Salvator-X(S) board config
+	* Renesas RZ/A1H GR-Peach board config
+	* Rigado BMD-300 board config
+	* Sayma AMC board config
+	* Sifive e31arty, e51arty, hifive1 board config
+	* ST B-L475E-IOT01A board config
+	* ST BlueNRG idb007v1, idb008v1, idb011v1 board config
+	* ST STM32F412g discovery board config
+	* ST STM32F413h discovery board config
+	* ST STM32F469i discovery board config
+	* ST STM32F7 Nucleo board config
+	* ST STM32F723e discovery board config
+	* ST STM32F746g discovery board config
+	* ST STM32F769i discovery board config
+	* ST STM32H735g discovery board config
+	* ST STM32H743zi Nucleo board config
+	* ST STM32H745i discovery board config
+	* ST STM32H747i discovery board config
+	* ST STM32H750b discovery board config
+	* ST STM32H7b3i discovery board config
+	* ST STM32H7x_dual_qspi board config
+	* ST STM32H7x3i Eval boards config
+	* ST STM32L073 Nucleo board config
+	* ST STM32L476g discovery board config
+	* ST STM32L496g discovery board config
+	* ST STM32L4p5g discovery board config
+	* ST STM32L4r9i discovery board config
+	* ST STM32L5 Nucleo board config
+	* ST STM32MP15x DK2 board config
+	* ST STM32WB Nucleo board config
+	* ST STM8L152R8 Nucleo board config
+	* Synopsys DesignWare ARC EM board config
+	* Synopsys DesignWare ARC HSDK board config
+	* TI BeagleBone family boards config
+	* TI CC13xx, CC26xx, CC32xx LaunchPad board config
+	* TI MSP432 LaunchPad board config
+	* Tocoding Poplar board config
+	* TP-Link WDR4300 board config
+	* Allwinner V3s target config
+	* Andes Technology NDS V5 target config
+	* Atmel atmega128rfa1 target config
+	* ARM corelink SSE-200 target config
+	* Atheros_ar9344 target config
+	* Cypress PSoC5LP, PSoC6 target config
+	* EnSilica eSi-RISC target config
+	* Foshan Synwit Tech SWM050 target config
+	* GigaDevice GD32VF103 target config
+	* Hisilicon Hi3798 target config
+	* Hisilicon Hi6220 target config
+	* Infineon TLE987x target config
+	* Marvell Armada 3700 target config
+	* Maxim Integrated MAX32XXX target config
+	* Mellanox BlueField target config
+	* Microchip (Atmel) SAME5x, SAML1x target config
+	* NXP IMX6SX, IMX6UL, IMX7, IMX7ULP, IMX8 target config
+	* NXP Kinetis KE1xZ, KE1xF target config
+	* NXP LPC84x, LPC8Nxx, LS1012A, NHS31xx target config
+	* Qualcomm QCA4531 target config
+	* QuickLogic EOS S3 target config
+	* Renesas R-Car E2, H2, M2 target config
+	* Renesas R-Car Gen3 target config
+	* Renesas RZ/A1H target config
+	* Rockchip RK3308 target config
+	* ST BlueNRG target config
+	* ST STM32G0, STM32G4, STM32H7, STM32L0, STM32L5 target config
+	* ST STM32MP15x target config
+	* ST STM32WBx, STM32WLEx target config
+	* ST STM8L152, S003, S103, S105 target config
+	* Synopsys DesignWare ARC EM target config
+	* Synopsys DesignWare ARC HS Development Kit SoC target config
+	* TI CC13xx, CC26xx, CC32xx target config
+	* TI TNETC4401 target config
+	* Xilinx UltraScale+ target config
+	* Altera 5M570Z (MAXV family) CPLD config
+	* Xilinx Ultrascale, XCF CPLD config
+	* Intel (Altera) Arria10 FPGA config
+	* Cadence SystemVerilog Direct Programming Interface (DPI) interface config
+	* Cypress KitProg interface config
+	* Digilent SMT2 NC interface config
+	* DLN-2 example of Linux GPIOD interface config
+	* FTDI C232HM interface config
+	* HIE JTAG Debugger interface config
+	* In-Circuit's ICprog interface config
+	* isodebug isolated JTAG/SWD+UART interface config
+	* Mellanox rshim USB or PCIe interface config
+	* Nuvoton Nu-Link interface config
+	* NXP IMX GPIO mmap based interface config
+	* Steppenprobe open hardware interface config
+	* TI XDS110 interface config
 
 Server Layer:
+	* 64 bit address support
+	* default bind to IPv4 localhost
+	* gdb: allow multiple connections
+	* gdb: architecture element support
+	* gdb: vCont, vRun support
+	* telnet: handle Ctrl+A, Ctrl+E and Ctrl+K
+
+RTOS:
+	* Chromium-EC rtos support
+	* hwthread pseudo rtos support
+	* NuttX rtos support
+	* RIOT rtos support
 
 Documentation:
+	* Improve STM32 flash driver
+	* Various typo fix and improvements
 
 Build and Release:
+	* Add libutil to support jimtcl version 0.80
+	* Clang warning fixes
+	* GitHub workflow for Win32 snapshot binaries
+	* Handle Tcl return values consistently
+	* Mitigation for CVE-2018-5704: Prevent some forms of Cross
+          Protocol Scripting attacks
+	* Support for libftdi 1.5
+	* Travis-CI basic support
+	* Update libjaylink to version 0.2.0
+	* Update jimtcl to version 0.79
+	* Use external (optional) library capstone for ARM and AARCH64 disassembly
 
 
 This release also contains a number of other important functional and
 cosmetic bugfixes. For more details about what has changed since the
 last release, see the git repository history:
 
-http://sourceforge.net/p/openocd/code/ci/v0.x.0/log/?path=
+http://sourceforge.net/p/openocd/code/ci/v0.11.0-rc1/log/?path=
 
 
 For older NEWS, see the NEWS files associated with each release
diff --git a/configure.ac b/configure.ac
index 97c573a..48cd279 100644
--- a/configure.ac
+++ b/configure.ac
@@ -1,7 +1,8 @@
 AC_PREREQ(2.64)
-AC_INIT([openocd], [0.10.0+dev],
+AC_INIT([openocd], [0.11.0-rc1+dev],
   [OpenOCD Mailing List <openocd-devel@lists.sourceforge.net>])
 AC_CONFIG_SRCDIR([src/openocd.c])
+AC_CONFIG_AUX_DIR([.])
 
 m4_include([config_subdir.m4])dnl
 
@@ -43,6 +44,7 @@ AC_TYPE_LONG_LONG_INT
 
 AC_SEARCH_LIBS([ioperm], [ioperm])
 AC_SEARCH_LIBS([dlopen], [dl])
+AC_SEARCH_LIBS([openpty], [util])
 
 AC_CHECK_HEADERS([sys/socket.h])
 AC_CHECK_HEADERS([elf.h])
@@ -118,6 +120,7 @@ m4_define([USB1_ADAPTERS],
 	[[ft232r], [Bitbang mode of FT232R based devices], [FT232R]],
 	[[vsllink], [Versaloon-Link JTAG Programmer], [VSLLINK]],
 	[[xds110], [TI XDS110 Debug Probe], [XDS110]],
+	[[cmsis_dap_v2], [CMSIS-DAP v2 Compliant Debugger], [CMSIS_DAP_USB]],
 	[[osbdm], [OSBDM (JTAG only) Programmer], [OSBDM]],
 	[[opendous], [eStick/opendous JTAG Programmer], [OPENDOUS]],
 	[[aice], [Andes JTAG Programmer], [AICE]]])
@@ -128,7 +131,7 @@ m4_define([USB0_ADAPTERS],
 	[[armjtagew], [Olimex ARM-JTAG-EW Programmer], [ARMJTAGEW]]])
 
 m4_define([HIDAPI_ADAPTERS],
-	[[[cmsis_dap], [CMSIS-DAP Compliant Debugger], [CMSIS_DAP]],
+	[[[cmsis_dap], [CMSIS-DAP Compliant Debugger], [CMSIS_DAP_HID]],
 	[[nulink], [Nu-Link Programmer], [HLADAPTER_NULINK]]])
 
 m4_define([HIDAPI_USB1_ADAPTERS],
@@ -669,7 +672,11 @@ for hidapi_lib in hidapi hidapi-hidraw hidapi-libusb; do
 	])
 done
 
-PKG_CHECK_MODULES([LIBFTDI], [libftdi1], [use_libftdi=yes], [
+PKG_CHECK_MODULES([LIBFTDI], [libftdi1], [
+	use_libftdi=yes
+	PKG_CHECK_EXISTS([libftdi1 >= 1.5],
+		[AC_DEFINE([HAVE_LIBFTDI_TCIOFLUSH], [1], [Define if your libftdi has ftdi_tcioflush()])])
+  ], [
 	PKG_CHECK_MODULES([LIBFTDI], [libftdi], [use_libftdi=yes], [use_libftdi=no])
 ])
 
@@ -827,6 +834,7 @@ AS_IF([test "x${gcc_wextra}" = "xyes"], [
   GCC_WARNINGS="${GCC_WARNINGS} -Wbad-function-cast"
   GCC_WARNINGS="${GCC_WARNINGS} -Wcast-align"
   GCC_WARNINGS="${GCC_WARNINGS} -Wredundant-decls"
+  GCC_WARNINGS="${GCC_WARNINGS} -Wpointer-arith"
 ])
 AS_IF([test "x${gcc_werror}" = "xyes"], [
   GCC_WARNINGS="${GCC_WARNINGS} -Werror"
diff --git a/contrib/60-openocd.rules b/contrib/60-openocd.rules
index 53f97dd..fe57364 100644
--- a/contrib/60-openocd.rules
+++ b/contrib/60-openocd.rules
@@ -167,6 +167,7 @@ ATTRS{idVendor}=="9e88", ATTRS{idProduct}=="9e8f", MODE="660", GROUP="plugdev",
 
 # Keil Software, Inc. ULink
 ATTRS{idVendor}=="c251", ATTRS{idProduct}=="2710", MODE="660", GROUP="plugdev", TAG+="uaccess"
+ATTRS{idVendor}=="c251", ATTRS{idProduct}=="2750", MODE="660", GROUP="plugdev", TAG+="uaccess"
 
 # CMSIS-DAP compatible adapters
 ATTRS{product}=="*CMSIS-DAP*", MODE="660", GROUP="plugdev", TAG+="uaccess"
diff --git a/contrib/cross-build.sh b/contrib/cross-build.sh
index 0ef0a2d..610607e 100755
--- a/contrib/cross-build.sh
+++ b/contrib/cross-build.sh
@@ -14,8 +14,8 @@
 #    paths refer to the build file system.
 #
 # This script is probably more useful as a reference than as a complete build
-# tool but for some configurations it may be usable as-is. It only cross-
-# builds libusb-1.0, hidapi and libftdi from source, but the script can be
+# tool but for some configurations it may be usable as-is. It only cross-builds
+# libusb-1.0, hidapi, libftdi and capstone from source, but the script can be
 # extended to build other prerequisites in a similar manner.
 #
 # Usage:
@@ -39,17 +39,20 @@ WORK_DIR=$PWD
 : ${LIBUSB1_SRC:=/path/to/libusb1}
 : ${HIDAPI_SRC:=/path/to/hidapi}
 : ${LIBFTDI_SRC:=/path/to/libftdi}
+: ${CAPSTONE_SRC:=/path/to/capstone}
 
 OPENOCD_SRC=`readlink -m $OPENOCD_SRC`
 LIBUSB1_SRC=`readlink -m $LIBUSB1_SRC`
 HIDAPI_SRC=`readlink -m $HIDAPI_SRC`
 LIBFTDI_SRC=`readlink -m $LIBFTDI_SRC`
+CAPSTONE_SRC=`readlink -m $CAPSTONE_SRC`
 
 HOST_TRIPLET=$1
 BUILD_DIR=$WORK_DIR/$HOST_TRIPLET-build
 LIBUSB1_BUILD_DIR=$BUILD_DIR/libusb1
 HIDAPI_BUILD_DIR=$BUILD_DIR/hidapi
 LIBFTDI_BUILD_DIR=$BUILD_DIR/libftdi
+CAPSTONE_BUILD_DIR=$BUILD_DIR/capstone
 OPENOCD_BUILD_DIR=$BUILD_DIR/openocd
 
 ## Root of host file tree
@@ -129,6 +132,26 @@ if [ -d $LIBFTDI_SRC ] ; then
   make install DESTDIR=$SYSROOT
 fi
 
+# capstone build & install into sysroot
+if [ -d $CAPSTONE_SRC ] ; then
+  mkdir -p $CAPSTONE_BUILD_DIR
+  cd $CAPSTONE_BUILD_DIR
+  cp -r $CAPSTONE_SRC/* .
+  make install DESTDIR=$SYSROOT PREFIX=$PREFIX \
+    CROSS="${HOST_TRIPLET}-" \
+    $CAPSTONE_CONFIG
+  # fix the generated capstone.pc
+  CAPSTONE_PC_FILE=${SYSROOT}${PREFIX}/lib/pkgconfig/capstone.pc
+  sed -i '/^libdir=/d' $CAPSTONE_PC_FILE
+  sed -i '/^includedir=/d' $CAPSTONE_PC_FILE
+  sed -i '/^archive=/d' $CAPSTONE_PC_FILE
+  sed -i '1s;^;prefix=/usr \
+exec_prefix=${prefix} \
+libdir=${exec_prefix}/lib \
+includedir=${prefix}/include\n\n;' $CAPSTONE_PC_FILE
+fi
+
+
 # OpenOCD build & install into sysroot
 mkdir -p $OPENOCD_BUILD_DIR
 cd $OPENOCD_BUILD_DIR
diff --git a/contrib/loaders/flash/stmqspi/Makefile b/contrib/loaders/flash/stmqspi/Makefile
new file mode 100644
index 0000000..810c7e8
--- /dev/null
+++ b/contrib/loaders/flash/stmqspi/Makefile
@@ -0,0 +1,34 @@
+BIN2C = ../../../../src/helper/bin2char.sh
+
+SRCS=stmqspi_erase_check.S stmqspi_crc32.S stmqspi_read.S stmqspi_write.S \
+     stmoctospi_erase_check.S stmoctospi_crc32.S stmoctospi_read.S stmoctospi_write.S
+OBJS=$(patsubst %.S,%.inc,$(SRCS))
+
+CROSS_COMPILE ?= arm-none-eabi-
+
+CC=$(CROSS_COMPILE)gcc
+OBJCOPY=$(CROSS_COMPILE)objcopy
+OBJDUMP=$(CROSS_COMPILE)objdump
+LD=$(CROSS_COMPILE)ld
+
+all: $(OBJS)
+
+%.o: %.S Makefile
+	$(CC) -Wall -Werror -Wa,-adhlmn -o $@ -c $< > $(@:.o=.lst)
+	@enscript -Easm -T 4 -G -o - $(@:.o=.lst) | ps2pdf - $(@:.o=.pdf) || true
+
+%.elf: %.o
+	$(LD) -s -defsym=_start=0 -o $@ $<
+
+%.bin: %.elf
+	$(OBJCOPY) -S -O binary $< $@
+
+%.inc: %.bin
+	$(BIN2C) < $< > $@
+
+clean:
+	-rm -f *.o *.elf *.lst *.pdf *.bin *.inc
+
+.PHONY:	all clean
+
+.INTERMEDIATE: $(patsubst %.S,%.o,$(SRCS)) $(patsubst %.S,%.elf,$(SRCS)) $(patsubst %.S,%.bin,$(SRCS))
diff --git a/contrib/loaders/flash/stmqspi/gpio_conf_stm32.pl b/contrib/loaders/flash/stmqspi/gpio_conf_stm32.pl
new file mode 100755
index 0000000..b753864
--- /dev/null
+++ b/contrib/loaders/flash/stmqspi/gpio_conf_stm32.pl
@@ -0,0 +1,679 @@
+#!/usr/bin/perl
+#
+# Helper for generating GPIO setup for STM32F0, F4, F7, H7, L0, L1, L4, L4+
+# and F1 (for 'stmqspi' and 'cmspi' drivers).
+#
+# Each pin is configured by "PortAndBit:Conf:Speed"
+#  'PortAndBit' specifies Port and bit number
+#  'Conf' is one of 'AFx' (alternate), 'P' (output), 'IN' (input),
+#    (each optionally by 'P' (push-pull) or 'O' (open-drain)),
+#    (all optionally followed by 'UP' (pull-up), or 'DO' (pull-down))
+#  'Speed' is one of 'L' (low), 'M' (medium), 'H' (high), 'V' (very high)
+#
+# Port configuration can be given on command line as a single string (pins separated by commas)
+# or via CubeMX generated file. The latter must consist of the quadspi.c / octospi.c and the
+# corresponding header. The precise spelling in these files doesn't seem to be consistent, though ...
+#
+# Pins have to be ordered this way:
+#  - I2C: SDA, SCL
+#  - SPI (1 line): NCS, CLK, IO1/MISO, IO0/MOSI
+#  - DPI (2 lines): NCS, CLK, IO1/MISO, IO0/MOSI
+#  - QPI (4 lines): NCS, CLK, IO3/NHOLD, IO2/NWP, IO1/MISO, IO0/MOSI
+# For dual flash: BK_1 first, then BK_2. If single NCS for both, omit NCS in BK_2
+# For octal flash: NCS, CLK, DQS, IO7 down to IO0
+
+use strict;
+use Getopt::Std;
+
+my $GPIO_BASE;
+my $Conf;
+my $STM32F1 = 0;
+
+# "Blue-Pill stm32f103cbt6 board w/ cmspi
+#$STM32F1 = 1;
+#$GPIO_BASE = 0x40010800;
+#$Conf = "PB12:PP:M, PB13:PP:V, PB14:INUP:V, PB15:INUP:V";
+#$Conf = "PB12:PP:M, PB13:PP:V, PB14:INUP:V, PB01:INUP:V";
+
+#$STM32F1 = 1;
+#$GPIO_BASE = 0x40010800;
+#$Conf = "PB07:INUP:V, PB06:INUP:V";
+
+# mini-stm32f030f4p6 board w/ cmspi
+#$GPIO_BASE = 0x48000000;
+#$Conf = "PB01:PP:V, PA05:PP:V, PA06:INUP:V, PA07:INUP:V";
+
+# stm32f407vet6 board w/ cmspi
+#$GPIO_BASE = 0x40020000;
+#$Conf = "PB00:PP:M, PB03:PP:V, PB04:INUP:V, PB05:INUP:V";
+
+# stm32f412g-disco quad
+#$GPIO_BASE = 0x40020000;
+#$Conf = "PB02:AF09:V, PF09:AF10:V, PF08:AF10:V, PF07:AF09:V, PF06:AF09:V, PG06:AF10:V";
+
+# stm32f413h-disco
+#$GPIO_BASE = 0x40020000;
+#$Conf = "PB02:AF09:V, PD13:AF09:V, PE02:AF09:V, PF09:AF10:V, PF08:AF10:V, PG06:AF10:V";
+
+# stm32f469i-disco quad
+#$GPIO_BASE = 0x40020000;
+#$Conf = "PB06:AF10:V, PF10:AF09:V, PF09:AF10:V, PF08:AF10:V, PF07:AF09:V, PF06:AF09:V";
+# w/ cmspi
+#$Conf = "PB06:PP:M, PF10:PP:V, PF06:INUP:V, PF07:INUP:V, PF09:INUP:V, PF08:INUP:V";
+
+# stm32f723e-disco quad
+#$GPIO_BASE = 0x40020000;
+#$Conf = "PB06:AF10:V, PB02:AF09:V, PC10:AF09:V, PC09:AF09:V, PD13:AF09:V, PE02:AF09:V";
+
+# stm32f746g-disco quad
+#$GPIO_BASE = 0x40020000;
+#Conf = "PB06:AF10:V, PB02:AF09:V, PD13:AF09:V, PE02:AF09:V, PD12:AF09:V, PD11:AF09:V";
+# w/ cmspi
+#$Conf = "PB06:PP:M, PB02:PP:V, PD13:INUP:V, PE02:INUP:V, PD12:INUP:V, PD11:INUP:V";
+
+# stm32f769i-disco quad
+#$GPIO_BASE = 0x40020000;
+#$Conf = "PB06:AF10:V, PB02:AF09:V, PC10:AF09:V, PC09:AF09:V, PD13:AF09:V, PE02:AF09:V";
+# w/ cmspi
+#$Conf = "PB06:PP:M, PB02:PP:V, PD13:INUP:V, PE02:INUP:V, PC10:INUP:V, PC09:INUP:V, ";
+
+# b-l475e-iot01a quad
+#$GPIO_BASE = 0x48000000;
+#$Conf = "PE15:AF10:V, PE14:AF10:V, PE13:AF10:V, PE12:AF10:V, PE11:AF10:V, PE10:AF10:V";
+
+# stm32l476g-disco quad
+#$GPIO_BASE = 0x48000000;
+#$Conf = "PE15:AF10:V, PE14:AF10:V, PE13:AF10:V, PE12:AF10:V, PE11:AF10:V, PE10:AF10:V";
+
+# stm32l496g-disco quad
+#$GPIO_BASE = 0x48000000;
+#$Conf = "PA07:AF10:V, PA06:AF10:V, PA03:AF10:V, PB11:AF10:V, PB01:AF10:V, PB00:AF10:V";
+
+# stm32l4r9i-disco octal
+#$GPIO_BASE = 0x48000000;
+#$Conf = "PG15:AF05:V, PG12:AF05:V, PG10:AF05:V, PG09:AF05:V, PH10:AF05:V, PH09:AF05:V, "
+#      . "PH08:AF05:V, PI11:AF05:V, PI10:AF05:V, PI09:AF05:V, PI06:AF05:V";
+
+# stm32l4p5g-disco octal/octal
+#$GPIO_BASE = 0x48000000;
+#$Conf = "PA07:AF10:V, PA06:AF10:V, PC03:AF10:V, PD07:AF10:V, PD05:AF10:V, PD04:AF10:V, "
+#      . "PE13:AF10:V, PE12:AF10:V, PE11:AF10:V, PE10:AF10:V, PG06:AF03:V";
+#$Conf = "PF12:AF05:V, PF04:AF05:V, PF03:AF05:V, PF02:AF05:V, PF01:AF05:V, PF00:AF05:V, "
+#      . "PG12:AF05:V, PG10:AF05:V, PG09:AF05:V, PG01:AF05:V, PG00:AF05:V";
+
+# nucleo-f767zi dual quad
+#$GPIO_BASE = 0x40020000;
+#$Conf = "PB06:AF10:V, PB02:AF09:V, PC11:AF09:V, PD13:AF09:V, PE02:AF09:V, PD12:AF09:V, "
+#      . "PD11:AF09:V, PE10:AF10:V, PE09:AF10:V, PE08:AF10:V, PE07:AF10:V";
+# w/ cmspi
+#$Conf = "PB10:PPUP:M, PB02:PPUP:V, PD13:INPUP:V, PE02:INPUP:V, PD12:INPUP:V, PD11:INPUP:V";
+#$Conf = "PC11:PPUP:M, PB02:PPUP:V, PE10:INPUP:V, PE09:INPUP:V, PE08:INPUP:V, PE07:INPUP:V";
+
+# nucleo-h743zi dual quad
+#$GPIO_BASE = 0x58020000;
+#$Conf = "PB10:AF09:V, PB02:AF09:V, PC11:AF09:V, PD13:AF09:V, PE02:AF09:V, PD12:AF09:V, "
+#      . "PD11:AF09:V, PE10:AF10:V, PE09:AF10:V, PE08:AF10:V, PE07:AF10:V";
+# w/ cmspi
+#$Conf = "PB10:PPUP:M, PB02:PPUP:V, PD13:INPUP:V, PE02:INPUP:V, PD12:INPUP:V, PD11:INPUP:V";
+#$Conf = "PC11:PPUP:M, PB02:PPUP:V, PE10:INPUP:V, PE09:INPUP:V, PE08:INPUP:V, PE07:INPUP:V";
+
+# nucleo-h7a3zi dual quad
+#$GPIO_BASE = 0x58020000;
+#$Conf = "PB10:AF09:V, PB02:AF09:V, PD13:AF09:V, PE02:AF09:V, PD12:AF09:V, PD11:AF09:V, "
+#      . "PC11:AF09:V, PE10:AF10:V, PD06:AF10:V, PE08:AF10:V, PE07:AF10:V";
+# w/ cmspi
+#$Conf = "PB10:PPUP:M, PB02:PPUP:V, PD13:INPUP:V, PE02:INPUP:V, PD12:INPUP:V, PD11:INPUP:V";
+#$Conf = "PC11:PPUP:M, PB02:PPUP:V, PE10:INPUP:V, PD06:INPUP:V, PE08:INPUP:V, PE07:INPUP:V";
+
+# nucleo-l4r5zi one dual quad single NCS
+#$GPIO_BASE = 0x48000000;
+#$Conf = "PA02:AF10:V, PE10:AF10:V, PD07:AF10:V, PD06:AF10:V, PD05:AF10:V, PD04:AF10:V, "
+#      . "PE15:AF10:V, PE14:AF10:V, PE13:AF10:V, PE12:AF10:V";
+# w/ cmspi
+#$Conf = "PA02:PPUP:M,  PE10:PPUP:V, PD07:INPDO:V, PD06:INPDO:V, PD05:INPDO:V, PD04:INPDO:V";
+#$Conf = "PA02:PPUP:M,  PE10:PPUP:V, PE15:INPDO:V, PE14:INPDO:V, PE13:INPDO:V, PE12:INPDO:V";
+
+# nucleo-l552ze-q dual quad with single NCS
+#$GPIO_BASE = 0x42020000;
+#$Conf = "PA02:AF10:V, PE10:AF10:V, PD07:AF10:V, PD06:AF10:V, PD05:AF10:V, PD04:AF10:V, "
+#      . "PE15:AF10:V, PE14:AF10:V, PE13:AF10:V, PE12:AF10:V";
+# w/ cmspi
+#$Conf = "PA02:PPUP:M,  PE10:PPUP:V, PD07:INPDO:V, PD06:INPDO:V, PD05:INPDO:V, PD04:INPDO:V";
+#$Conf = "PA02:PPUP:M,  PE10:PPUP:V, PE15:INPDO:V, PE14:INPDO:V, PE13:INPDO:V, PE12:INPDO:V";
+
+# nucleo-g071rb dual quad
+#$GPIO_BASE = 0x50000000;
+#$Conf = "PA00:PPUP:H, PA04:PPUP:V, PB03:INPUP:V, PA10:INPUP:V, PB11:INPUP:H, PB01:INPUP:H";
+#$Conf = "PA01:PPUP:H, PA04:PPUP:V, PA08:INPUP:V, PB14:INPUP:V, PB04:INPUP:V, PB05:INPUP:V";
+
+# nucleo-g474re dual quad with single NCS
+#$GPIO_BASE = 0x48000000;
+#$Conf = "PB11:AF10:H, PB10:AF10:V, PA06:AF10:V, PA07:AF10:V, PB00:AF10:V, PB01:AF10:V, "
+#      . "PC04:AF10:V, PC03:AF10:V, PC02:AF10:V, PC01:AF10:V";
+# w/ cmspi
+#$Conf = "PB11:PPUP:H, PB10:PPUP:V, PA06:INPUP:V, PA07:INPUP:V, PB00:INPUP:V, PB01:INPUP:V";
+#$Conf = "PB11:PPUP:H, PB10:PPUP:V, PC04:INPUP:V, PC03:INPUP:V, PC02:INPUP:V, PC01:INPUP:V";
+
+# stm32h745i-disco dual quad with single NCS
+#$GPIO_BASE = 0x58020000;
+#$Conf = "PG06:AF10:H, PF10:AF09:V, PF06:AF09:V, PF07:AF09:V, PF09:AF10:V, PD11:AF09:V, "
+#      . "PG14:AF09:H, PG09:AF09:V, PH03:AF09:V, PH02:AF09:V";
+
+# stm32h747i-disco dual quad with single NCS
+#GPIO_BASE = 0x58020000;
+#$Conf = "PG06:AF10:H, PB02:AF09:V, PF06:AF09:V, PF07:AF09:V, PF09:AF10:V, PD11:AF09:V, "
+#      . "PG14:AF09:H, PG09:AF09:V, PH03:AF09:V, PH02:AF09:V";
+
+# stm32h7b3i-disco octal
+#$GPIO_BASE = 0x58020000;
+#$Conf = "PG06:AF10:V, PB02:AF09:V, PC05:AF10:V, PD07:AF10:V, PG09:AF09:V, PH03:AF09:V, PC01:AF10:V, "
+#      . "PF06:AF10:V, PF07:AF10:V, PF09:AF10:V, PD11:AF09:V";
+
+# stm32h735g-disco octal
+#$GPIO_BASE = 0x58020000;
+#$Conf = "PG06:AF10:V, PF10:AF09:V, PB02:AF10:V, PD07:AF10:V, PG09:AF09:V, PD05:AF10:V, PD04:AF10:V, "
+#      . "PD13:AF09:V, PE02:AF09:V, PD12:AF09:V, PD11:AF09:V";
+
+# stm32l562e-disco octal
+#$GPIO_BASE = 0x42020000;
+#$Conf = "PA02:AF10:V, PA03:AF10:V, PB02:AF10:V, PC00:AF03:V, PC03:AF10:V, PC02:AF10:V, PC01:AF10:V, "
+#      . "PA06:AF10:V, PA07:AF10:V, PB00:AF10:V, PB01:AF10:V";
+
+&getopts('b:c:f:t');
+if ($Getopt::Std::opt_b eq '')
+{
+	if ($GPIO_BASE eq '')
+	{
+		die("usage: $0 [ -1 ] -b io_base [ -c port_configuration ] [ -f conf_file ]");
+	}
+}
+else
+{
+	$GPIO_BASE = eval $Getopt::Std::opt_b;
+}
+
+if ($Getopt::Std::opt_c eq '')
+{
+	if (($Conf eq '') && ($Getopt::Std::opt_f eq ''))
+	{
+		die("usage: $0 [ -b io_base ] ( -c port_configuration | -f conf_file )");
+	}
+}#
+else
+{
+	$Conf = $Getopt::Std::opt_c . ',';
+}
+
+$STM32F1 = $Getopt::Std::opt_t;
+
+my $Sep = "\t";
+my $Form = "${Sep}mmw 0x%08X 0x%08X 0x%08X\t;# ";
+
+my $GPIO_OFFS;
+my $GPIO_CRL;
+my $GPIO_CRH;
+my $GPIO_MODER;
+my $GPIO_OTYPER;
+my $GPIO_OSPEEDR;
+my $GPIO_PUPDR;
+my $GPIO_IDR;
+my $GPIO_ODR;
+my $GPIO_AFRL;
+my $GPIO_AFRH;
+
+if ($STM32F1)
+{
+	# offsets for F1 devices
+	$GPIO_OFFS = 0x400;
+	$GPIO_CRL = 0x00;
+	$GPIO_CRH = 0x04;
+	$GPIO_IDR = 0x08;
+	$GPIO_ODR = 0x0C;
+}
+else
+{
+	# these offsets are identical on all F0, F4, F7, H7, L4, L4+ devices up to now
+	$GPIO_OFFS = 0x400;
+	$GPIO_MODER = 0x00;
+	$GPIO_OTYPER = 0x04;
+	$GPIO_OSPEEDR = 0x08;
+	$GPIO_PUPDR = 0x0C;
+	$GPIO_IDR = 0x10;
+	$GPIO_ODR = 0x14;
+	$GPIO_AFRL = 0x20;
+	$GPIO_AFRH = 0x24;
+}
+
+my @Out = ( { }, { }, { }, { }, { }, { }, { }, { }, { }, { }, { } );
+my @Port = ( );
+my $Exor;
+my %Conf;
+my $Pins = "${Sep}#";
+
+my $pins;
+my $altn;
+my %defs;
+
+if ($Getopt::Std::opt_f ne '')
+{
+	open(CONF_FILE, '<', $Getopt::Std::opt_f) || die("can't open $Getopt::Std::opt_f");
+	while (my $line = <CONF_FILE>)
+	{
+		if ($line =~ /^\s*#define\s+.?(QSPI|QUAD_?SPI|OCTOSPI[^_]*)\w+_(Port|Pin)\s/)
+		{
+			if ($line =~ /#define\s+(\w+)\s+(\w+)/)
+			{
+				$defs{$1} = $2;
+			}
+			else
+			{
+				die($line);
+			}
+		}
+		elsif ($line =~ /^\s*(P[A-Z])([0-9]+)\s*-+>\s+.?(QSPI|QUAD_?SPI|OCTO_?SPI[^_]*)_(\w+)/)
+		{
+			$Conf{$4} = sprintf("%s%02d", $1, $2);
+		}
+		elsif ($line =~ /^\s*GPIO_InitStruct.Pin\s*=\s*([^;]+\w)/)
+		{
+			$pins = $1;
+			while ($line !~ /;/)
+			{
+				$line = <CONF_FILE>;
+				$line =~ /^\s*([^;]+\w)/;
+				$pins .= $1;
+			}
+		}
+		elsif ($line =~ /^\s*GPIO_InitStruct.Alternate\s*=\s*GPIO_AF([0-9]+)/)
+		{
+			$altn = $1;
+		}
+		elsif ($line =~ /^\s*HAL_GPIO_Init\s*\(\s*(\w+)\s*,/)
+		{
+			my $port = $1;
+			if ($port =~ /GPIO([A-Z])/)
+			{
+				$port = $1;
+			}
+			elsif (exists($defs{$port}))
+			{
+				$defs{$port} =~ /GPIO([A-Z])/;
+				$port = $1;
+			}
+			else
+			{
+				printf("\n");
+				next;
+			}
+			my @pin = split(/\s*\|\s*/, $pins);
+			foreach my $pin (@pin)
+			{
+				my $bit;
+				if (exists($defs{$pin}))
+				{
+					$defs{$pin} =~ /GPIO_PIN_([0-9]+)/;
+					$bit = $1;
+				}
+				else
+				{
+					$pin =~ /GPIO_PIN_([0-9]+)/;
+					$bit = $1;
+				}
+				$Conf .= sprintf("P%s%02d:AF%02d:V, ", $port, $bit, $altn);
+			}
+			$pins = '';
+			$altn = 0;
+		}
+	}
+	close(CONF_FILE);
+}
+else
+{
+	my @names = ( );
+	my @conf = split(/\s*,\s*/, $Conf);
+
+	if (@conf == 2)
+	{
+		push(@names, 'SDA', 'SCL');
+	} else {
+		if (@conf == 3)
+		{
+			push(@names, 'NCS', 'CLK', 'IO0/DIO');
+		}
+		elsif (@conf == 4)
+		{
+			push(@names, 'NCS', 'CLK','IO1/MISO', 'IO0/MOSI');
+		}
+		elsif (@conf == 6)
+		{
+			push(@names, 'NCS', 'CLK', 'IO3/NHOLD', 'IO2/NWP', 'IO1/MISO', 'IO0/MOSI');
+		}
+		elsif (@conf == 10)
+		{
+			push(@names, 'NCS', 'CLK', 'BK_1_IO3/NHOLD', 'BK1_IO2/NWP', 'BK1_IO1/MISO', 'BK1_IO0/MOSI');
+			push(@names, 'BK_2_IO3/NHOLD', 'BK2_IO2/NWP', 'BK2_IO1/MISO', 'BK2_IO0/MOSI');
+		}
+		elsif (@conf == 11)
+		{
+			push(@names, 'BK_1_NCS', 'CLK', 'BK_1_IO3/NHOLD', 'BK1_IO2/NWP', 'BK1_IO1/MISO', 'BK1_IO0/MOSI');
+			push(@names, 'BK_2_NCS', 'BK_2_IO3/NHOLD', 'BK2_IO2/NWP', 'BK2_IO1/MISO', 'BK2_IO0/MOSI');
+		}
+		else
+		{
+			die("invalid config");
+		}
+	}
+
+	for (my $index = 0; $index < @conf; $index++)
+	{
+		uc($conf[$index]) =~ /^P([A-K])([0-9]+):\s*([A-Z0-9]+):(L|M|H|V)$/;
+		$Pins .= sprintf(" %s: P%s%02d,", $names[$index], $1, $2);
+	}
+	chop($Pins);
+}
+
+if (exists $Conf{'BK1_IO0'})
+{
+	# QuadSPI on F4, F7, H7
+	my $line;
+	for my $i ('NCS', 'BK1_NCS', 'CLK', 'BK1_IO3', 'BK1_IO2', 'BK1_IO1', 'BK1_IO0')
+	{
+		(exists $Conf{$i}) && ($Pins .= sprintf(" %s: %s,", $Conf{$i}, $i));
+	}
+}
+
+if (exists $Conf{'BK2_IO0'})
+{
+	# QuadSPI on F4, F7, H7
+	my $line;
+	for my $i ('NCS', 'BK2_NCS', 'CLK', 'BK2_IO3', 'BK2_IO2', 'BK2_IO1', 'BK2_IO0')
+	{
+		(exists $Conf{$i}) && ($Pins .= sprintf(" %s: %s,", $Conf{$i}, $i));
+	}
+}
+
+if (exists $Conf{'P1_IO0'})
+{
+	# OctoSPI on L4+, L5, H7
+	my $line;
+	for my $i ('P1_NCS', 'P1_CLK', 'P1_DQS', 'P1_IO7', 'P1_IO6', 'P1_IO5', 'P1_IO4',
+			   'P1_IO3', 'P1_IO2', 'P1_IO1', 'P1_IO0')
+	{
+		(exists $Conf{$i}) && ($Pins .= sprintf(" %s: %s,", $Conf{$i}, $i));
+	}
+}
+
+if (exists $Conf{'P2_IO0'})
+{
+	# OctoSPI on L4+, H7
+	my $line;
+	for my $i ('P2_NCS', 'P2_CLK', 'P2_DQS', 'P2_IO7', 'P2_IO6', 'P2_IO5', 'P2_IO4',
+			   'P2_IO3', 'P2_IO2', 'P2_IO1', 'P2_IO0')
+	{
+		(exists $Conf{$i}) && ($Pins .= sprintf(" %s: %s,", $Conf{$i}, $i));
+	}
+}
+
+my @Col = ( );
+my @conf = split(/\s*,\s*/, $Conf);
+
+if (@conf == 3)
+{
+	splice(@conf, 2, 0, 'NONE', 'NONE', 'NONE');
+}
+elsif (@conf == 4)
+{
+	splice(@conf, 2, 0, 'NONE', 'NONE');
+}
+
+foreach my $line (@conf)
+{
+	$line = uc($line);
+	$line =~ /^P([A-K])([0-9]+):\s*([A-Z0-9]+):(L|M|H|V)$/;
+	my $port = $1;
+	my $pin = $2;
+	my $conf = $3;
+	my $speed = $4;
+
+	my $MODER = 0x0;
+	my $OTYPER = 0x0;
+	my $OSPEEDR = 0x0;
+	my $PUPDR = 0x0;
+	my $AFR = 0x0;
+	my $num = ord(${port}) - ord('A');
+	my $out = $Out[$num];
+
+	(exists $$out{'DEF'}) || ($$out{'DEF'} = 0);
+
+	if ($conf eq '')
+	{
+		if ($line ne 'NONE')
+		{
+			printf(STDERR "invalid conf %s\n", $line);
+		}
+		next;
+	}
+	elsif ($conf =~ /^AF([0-9]+)(|P|O)(|UP|DO)$/)
+	{
+		if ($STM32F1)
+		{
+			printf(STDERR "no alternate %s for F1 family\n", $line);
+			next;
+		}
+		if (($1 < 0) || ($1 > 15))
+		{
+			printf(STDERR "invalid alternate %s\n", $line);
+			next;
+		}
+		$MODER = 0x2;
+		$AFR = $1;
+		if ($pin <= 7)
+		{
+			$$out{'AFRL_H'} |= ($AFR << (${pin} << 2));
+			$$out{'AFRL_L'} |= (($AFR ^ 0xF) << (${pin} << 2));
+		}
+		else
+		{
+			$$out{'AFRH_H'} |= ($AFR << ((${pin} - 8) << 2));
+			$$out{'AFRH_L'} |= (($AFR ^ 0xF) << ((${pin} - 8) << 2));
+		}
+		if ($2 ne '') {
+			$OTYPER = ($1 eq 'O') ? 0x1 : 0x0;
+			$$out{'OTYPER_H'} |= ($OTYPER << $pin);
+			$$out{'OTYPER_L'} |= (($OTYPER ^ 0x1) << $pin);
+		}
+		$PUPDR = ($3 eq 'UP') ? 0x1 : (($3 eq 'DO') ? 0x2 : 0x0);
+		$$out{'PUPDR_H'} |= ($PUPDR << (${pin} << 1));
+		$$out{'PUPDR_L'} |= (($PUPDR ^0x3) << (${pin} << 1));
+		$conf = sprintf("AF%02d%s%s", $AFR, $2, $3);
+	}
+	elsif ($conf =~ /^IN(|P|O)(|UP|DO)$/)
+	{
+		if ($STM32F1)
+		{
+			$MODER = ($1 eq '') ? 0x4 : 0x8;
+			($2 eq 'UP') && ($$out{'PUPDR_H'} |= (1 << ${pin}));
+			($2 eq 'DO') && ($$out{'PUPDR_L'} |= (1 << ${pin}));
+		}
+		else
+		{
+			$MODER = 0x0;
+			if ($1 ne '')
+			{
+				$OTYPER = ($1 eq 'O') ? 0x1 : 0x0;
+				$$out{'OTYPER_H'} |= ($OTYPER << $pin);
+				$$out{'OTYPER_L'} |= (($OTYPER ^ 0x1) << $pin);
+			}
+			$PUPDR = ($2 eq 'UP') ? 0x1 : (($2 eq 'DO') ? 0x2 : 0x0);
+			$$out{'PUPDR_H'} |= ($PUPDR << (${pin} << 1));
+			$$out{'PUPDR_L'} |= (($PUPDR ^0x3) << (${pin} << 1));
+		}
+		($2 eq 'UP') && ($$out{'ODR_H'} |= (1 << ${pin}));
+		($2 eq 'DO') && ($$out{'ODR_L'} |= (1 << ${pin}));
+	}
+	elsif ($conf =~ /^P(P|O)(|UP|DO)$/)
+	{
+		if ($STM32F1)
+		{
+			$MODER = ($1 eq 'O') ? 0x4 : 0x0;
+			$MODER |= (($speed eq 'V') ? 0x03 : (($speed eq 'L') ? 0x2 : 0x1));
+			if ($2 ne '')
+			{
+				printf(STDERR "WARNING: no output w/ pull-up/pull-down for F1 family %s\n", $line);
+			}
+		}
+		else
+		{
+			$MODER = 0x1;
+			$OTYPER = ($1 eq 'O') ? 0x1 : 0x0;
+			$$out{'OTYPER_H'} |= ($OTYPER << $pin);
+			$$out{'OTYPER_L'} |= (($OTYPER ^ 0x1) << $pin);
+			$PUPDR = ($2 eq 'UP') ? 0x1 : (($2 eq 'DO') ? 0x2 : 0x0);
+			$$out{'PUPDR_H'} |= ($PUPDR << ($pin << 1));
+			$$out{'PUPDR_L'} |= (($PUPDR ^ 0x3) << ($pin << 1));
+		}
+		($2 eq 'UP') && ($$out{'ODR_H'} |= (1 << ${pin}));
+		($2 eq 'DO') && ($$out{'ODR_L'} |= (1 << ${pin}));
+	}
+	else
+	{
+		printf(STDERR "invalid conf %s\n", $line);
+		next;
+	}
+
+	if ($$out{'DEF'} & (1<< ${pin}))
+	{
+		printf(STDERR "redefinition: %s\n", $line);
+	}
+
+	if ($STM32F1)
+	{
+		if ($pin >= 8)
+		{
+			$$out{'CRH_H'} |= ($MODER << (($pin & 0x7) << 2));
+			$$out{'CRH_L'} |= (($MODER ^ 0xF) << (($pin & 0x7) << 2));
+		}
+		else
+		{
+			$$out{'CRL_H'} |= ($MODER << (($pin & 0x7) << 2));
+			$$out{'CRL_L'} |= (($MODER ^ 0xF) << (($pin & 0x7) << 2));
+		}
+
+		$Exor = sprintf("0x%08X %2d", ${GPIO_BASE} + (ord($port)  - ord('A')) * ${GPIO_OFFS} + ${GPIO_ODR}, $pin);
+		my $exor = 0xB << (($pin & 0x7) << 2);
+		(($MODER & 0x3) == 0x0) && ($Exor .= sprintf(" 0x%03X 0x%03X 0x%08X",
+			((($pin >= 8) ? ${GPIO_CRH} : ${GPIO_CRL})-${GPIO_ODR}) & 0x3FF,
+			((($pin >= 8) ? ${GPIO_CRH} : ${GPIO_CRL})-${GPIO_ODR}) & 0x3FF, $exor));
+	}
+	else
+	{
+		$$out{'DEF'} |= (1 << ${pin});
+		$$out{'MODER_H'} |= ($MODER << (${pin} << 1));
+		$$out{'MODER_L'} |= (($MODER ^ 0x3) << (${pin} << 1));
+
+		$OSPEEDR = (($speed eq 'V') ? 0x3 : (($speed eq 'H') ? 0x2 : (($speed eq 'M') ? 0x1 : 0x0)));
+		$$out{'OSPEEDR_H'} |= ($OSPEEDR << (${pin} << 1));
+		$$out{'OSPEEDR_L'} |= (($OSPEEDR ^ 0x3) << (${pin} << 1));
+
+		$Exor = sprintf("0x%08X %2d", ${GPIO_BASE} + (ord($port)  - ord('A')) * ${GPIO_OFFS} + ${GPIO_ODR}, $pin);
+		my $exor = (0x1 << ($pin << 1));
+		($MODER == 0x0) && ($Exor .= sprintf(" 0x%03X 0x%03X 0x%08X", (${GPIO_MODER}-${GPIO_ODR}) & 0x3FF,
+			(${GPIO_MODER}-${GPIO_ODR}) & 0x3FF, $exor));
+	}
+
+	push(@{$Port[$num]}, sprintf("P%s%02d:%s:%s", $port, $pin, $conf, $speed));
+	push(@Col, $Exor);
+}
+
+my $Col = sprintf("${Sep}0x%03X ", (${GPIO_IDR}-${GPIO_ODR}) & 0x3FF);
+for (my $i = 0; $i < @Col; $i++)
+{
+	if (($i != 0) && (($i % 2) == 0))
+	{
+		(($i + 1) < @Col) && ($Col .= "\\\n${Sep}");
+	}
+	$Col .= sprintf("%s ", $Col[$i]);
+}
+printf("%s\n", $Col);
+
+my @Col = ( );
+my $Set;
+for (my $i = 0; $i < @Out; $i++)
+{
+	my $out = $Out[$i];
+	my $addr = ${GPIO_BASE} + $i * ${GPIO_OFFS};
+	my $count = 0;
+
+	if ($STM32F1)
+	{
+		if (($$out{'CRH_H'} | $$out{'CRH_L'} | $$out{'CRL_H'} | $$out{'CRL_L'} |
+				$$out{'PUPDR_H'} | $$out{'PUPDR_L'}) != 0)
+		{
+			push(@Col, sort({ $b cmp $a } @{$Port[$i]}));
+
+			$Set .= sprintf("\n%s# Port %s: %s\n", ${Sep}, chr($i + ord('A')),
+				join(", ", sort({ $b cmp $a } @{$Port[$i]})));
+
+			(($$out{'CRL_H'} | $$out{'CRL_L'}) != 0) &&
+				($Set .= sprintf("${Form}CRL\n", $addr + ${GPIO_CRL}, $$out{'CRL_H'}, $$out{'CRL_L'}));
+
+			(($$out{'CRH_H'} | $$out{'CRH_L'}) != 0) &&
+				($Set .= sprintf("${Form}CRH\n", $addr + ${GPIO_CRH}, $$out{'CRH_H'}, $$out{'CRH_L'}));
+
+			(($$out{'ODR_H'} | $$out{'ODR_L'}) != 0) &&
+				($Set .= sprintf("${Form}ODR/PUPDR\n", $addr + ${GPIO_ODR}, $$out{'ODR_H'}, $$out{'ODR_L'}));
+		}
+	}
+	else
+	{
+		if (($$out{'MODER_H'} | $$out{'MODER_L'} |
+			$$out{'OTYPER_H'} | $$out{'OTYPER_L'} |
+			$$out{'OSPEEDR_H'} | $$out{'OSPEEDR_L'} |
+			$$out{'PUPDR_H'} | $$out{'PUPDR_L'} |
+			$$out{'ODR_H'} | $$out{'ODR_L'} |
+			$$out{'AFRL_H'} | $$out{'AFRL_L'} |
+			$$out{'AFRH_H'} | $$out{'AFRH_L'}) != 0)
+		{
+			push(@Col, sort({ $b cmp $a } @{$Port[$i]}));
+
+			$Set .= sprintf("%s# Port %s: %s\n", ${Sep}, chr($i + ord('A')),
+				join(", ", sort({ $b cmp $a } @{$Port[$i]})));
+
+			(($$out{'MODER_H'} | $$out{'MODER_L'}) != 0) &&
+				($Set .= sprintf("${Form}MODER\n", $addr + ${GPIO_MODER}, $$out{'MODER_H'}, $$out{'MODER_L'}));
+
+			(($$out{'OTYPER_H'} | $$out{'OTYPER_L'}) != 0) &&
+				($Set .= sprintf("${Form}OTYPER\n", $addr + ${GPIO_OTYPER}, $$out{'OTYPER_H'}, $$out{'OTYPER_L'}));
+
+			(($$out{'OSPEEDR_H'} | $$out{'OSPEEDR_L'}) != 0) &&
+				($Set .= sprintf("${Form}OSPEEDR\n", $addr + ${GPIO_OSPEEDR}, $$out{'OSPEEDR_H'}, $$out{'OSPEEDR_L'}));
+
+			(($$out{'PUPDR_H'} | $$out{'PUPDR_L'}) != 0) &&
+				($Set .= sprintf("${Form}PUPDR\n", $addr + ${GPIO_PUPDR}, $$out{'PUPDR_H'}, $$out{'PUPDR_L'}));
+
+			(($$out{'ODR_H'} | $$out{'ODR_L'}) != 0) &&
+				($Set .= sprintf("${Form}ODR\n", $addr + ${GPIO_ODR}, $$out{'ODR_H'}, $$out{'ODR_L'}));
+
+			(($$out{'AFRL_H'} | $$out{'AFRL_L'}) != 0) &&
+				($Set .= sprintf("${Form}AFRL\n", $addr + ${GPIO_AFRL}, $$out{'AFRL_H'}, $$out{'AFRL_L'}));
+
+			(($$out{'AFRH_H'} | $$out{'AFRH_L'}) != 0) &&
+				($Set .= sprintf("${Form}AFRH\n", $addr + ${GPIO_AFRH}, $$out{'AFRH_H'}, $$out{'AFRH_L'}));
+		}
+	}
+}
+
+my $Col = '';
+for (my $i = 0; $i < @Col; $i++)
+{
+	if (($i % 6) == 0)
+	{
+		chop($Col);
+		(($i + 1) < @Col) && ($Col .= "\n${Sep}#");
+	}
+	$Col .= sprintf(" %s,", $Col[$i]);
+}
+chop($Col);
+#printf("\n%s\n", $Pins);
+printf("%s\n", $Col);
+printf("%s\n", $Set);
diff --git a/contrib/loaders/flash/stmqspi/stmoctospi_crc32.S b/contrib/loaders/flash/stmqspi/stmoctospi_crc32.S
new file mode 100644
index 0000000..941ea42
--- /dev/null
+++ b/contrib/loaders/flash/stmqspi/stmoctospi_crc32.S
@@ -0,0 +1,123 @@
+/***************************************************************************
+ *   Copyright (C) 2019 by Andreas Bolsch                                  *
+ *   andreas.bolsch@mni.thm.de                                             *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+
+	.text
+	.syntax unified
+	.cpu cortex-m0
+	.thumb
+	.thumb_func
+
+/* Params:
+ * r0 - total count (bytes), crc32 (out)
+ * r1 - flash page size
+ * r2 - address offset into flash
+ * r3 - OCTOSPI io_base
+
+ * Clobbered:
+ * r4 - tmp
+ * r5 - address of OCTOSPI_DR
+ * r6 - address of OCTOSPI_CCR
+ * r7 - tmp
+ */
+
+#include "../../../../src/flash/nor/stmqspi.h"
+
+#define OCTOSPI_CCR_CCR					(OCTOSPI_CCR - OCTOSPI_CCR)
+#define OCTOSPI_TCR_CCR					(OCTOSPI_TCR - OCTOSPI_CCR)
+#define OCTOSPI_IR_CCR					(OCTOSPI_IR - OCTOSPI_CCR)
+
+	.macro	octospi_abort
+	movs	r5, #(1<<SPI_ABORT)			/* abort bit mask */
+	ldr		r7, [r3, #OCTOSPI_CR]		/* get OCTOSPI CR register */
+	orrs	r7, r7, r5					/* set abort bit */
+	str		r7, [r3, #OCTOSPI_CR]		/* store new CR register */
+	.endm
+
+	.macro	wait_busy
+0:
+	ldr		r7, [r3, #OCTOSPI_SR]		/* load status */
+	lsrs	r7, r7, #(SPI_BUSY+1)		/* shift BUSY into C */
+	bcs		0b							/* loop until BUSY cleared */
+	movs	r7, #(1<<SPI_TCF)			/* TCF bitmask */
+	str		r7, [r3, #OCTOSPI_FCR]		/* clear TCF flag */
+	.endm
+
+start:
+	subs	r0, r0, #1					/* decrement count for DLR */
+	subs	r1, r1, #1					/* page size mask and for DLR */
+	movs	r4, #0x00					/* initialize crc */
+	mvns	r4, r4						/* to 0xFFFFFFFF */
+start_read:
+	octospi_abort						/* start in clean state */
+	movs	r5, #OCTOSPI_DR				/* load OCTOSPI_DR address offset */
+	adds	r5, r5, r3					/* address of OCTOSPI_DR */
+	movs	r6, #OCTOSPI_CCR-OCTOSPI_DR	/* load OCTOSPI_CCR address offset */
+	adds	r6, r6, r5					/* address of OCTOSPI_CCR */
+	wait_busy
+	ldr		r7, cr_page_read			/* indirect read mode */
+	str		r7, [r3, #OCTOSPI_CR]		/* set mode */
+	mov		r7, r2						/* get current start address */
+	orrs	r7, r7, r1					/* end of current page */
+	subs	r7, r7, r2					/* count-1 to end of page */
+	cmp		r7, r0						/* if this count <= remaining */
+	bls		write_dlr					/* then read to end of page */
+	mov		r7, r0						/* else read all remaining */
+write_dlr:
+	str		r7, [r3, #OCTOSPI_DLR]		/* size-1 in DLR register */
+	ldr		r7, ccr_page_read			/* CCR for read */
+	str		r7, [r6, #OCTOSPI_CCR_CCR]	/* initiate transfer */
+	ldr		r7, tcr_page_read			/* TCR for read */
+	str		r7, [r6, #OCTOSPI_TCR_CCR]	/* instruction */
+	ldr		r7, ir_page_read			/* IR for read */
+	str		r7, [r6, #OCTOSPI_IR_CCR]	/* instruction */
+	str		r2, [r3, #OCTOSPI_AR]		/* store SPI start address */
+	ldr		r6, =0x04C11DB7				/* CRC32 polynomial */
+read_loop:
+	ldrb	r7, [r5]					/* read next byte from DR */
+	lsls	r7, r7, #24					/* shift into msb */
+	eors	r4, r4, r7
+	.rept	8							/* unrolled bit loop */
+	asrs	r7, r4, #31					/* copy bit 31 into bits 0 to 31 */
+	ands	r7, r7, r6					/* r7 neg. -> CRC32XOR, pos. -> 0x0 */
+	lsls	r4, r4, #1					/* shift result */
+	eors	r4, r4, r7					/* eor by CRC32XOR or 0x0 */
+	.endr
+	adds	r2, r2, #1					/* increment address */
+	subs	r0, r0, #1					/* decrement (count-1) */
+	bmi		exit						/* stop if no data left */
+	tst		r2, r1						/* page end ? */
+	bne		read_loop					/* if not, then next byte */
+page_end:
+	bal		start_read					/* then next page */
+	.pool
+
+exit:
+	mvns	r0, r4						/* invert to get final result */
+	octospi_abort						/* to idle state */
+	.align	2							/* align to word, bkpt is 4 words */
+	bkpt	#0							/* before code end for exit_point */
+	.align	2							/* align to word */
+
+cr_page_read:
+	.space	4							/* OCTOSPI_CR value for read command */
+ccr_page_read:
+	.space	4							/* OCTOSPI_CCR value for read command */
+tcr_page_read:
+	.space	4							/* OCTOSPI_TCR value for read command */
+ir_page_read:
+	.space	4							/* OCTOSPI_IR value for read command */
diff --git a/contrib/loaders/flash/stmqspi/stmoctospi_crc32.inc b/contrib/loaders/flash/stmqspi/stmoctospi_crc32.inc
new file mode 100644
index 0000000..afc6168
--- /dev/null
+++ b/contrib/loaders/flash/stmqspi/stmoctospi_crc32.inc
@@ -0,0 +1,13 @@
+/* Autogenerated with ../../../../src/helper/bin2char.sh */
+0x01,0x38,0x01,0x39,0x00,0x24,0xe4,0x43,0x02,0x25,0x1f,0x68,0x2f,0x43,0x1f,0x60,
+0x50,0x25,0xed,0x18,0xb0,0x26,0x76,0x19,0x1f,0x6a,0xbf,0x09,0xfc,0xd2,0x02,0x27,
+0x5f,0x62,0x22,0x4f,0x1f,0x60,0x17,0x46,0x0f,0x43,0xbf,0x1a,0x87,0x42,0x00,0xd9,
+0x07,0x46,0x1f,0x64,0x1e,0x4f,0x37,0x60,0x1e,0x4f,0xb7,0x60,0x1e,0x4f,0x37,0x61,
+0x9a,0x64,0x15,0x4e,0x2f,0x78,0x3f,0x06,0x7c,0x40,0xe7,0x17,0x37,0x40,0x64,0x00,
+0x7c,0x40,0xe7,0x17,0x37,0x40,0x64,0x00,0x7c,0x40,0xe7,0x17,0x37,0x40,0x64,0x00,
+0x7c,0x40,0xe7,0x17,0x37,0x40,0x64,0x00,0x7c,0x40,0xe7,0x17,0x37,0x40,0x64,0x00,
+0x7c,0x40,0xe7,0x17,0x37,0x40,0x64,0x00,0x7c,0x40,0xe7,0x17,0x37,0x40,0x64,0x00,
+0x7c,0x40,0xe7,0x17,0x37,0x40,0x64,0x00,0x7c,0x40,0x01,0x32,0x01,0x38,0x05,0xd4,
+0x0a,0x42,0xd7,0xd1,0xb8,0xe7,0x00,0x00,0xb7,0x1d,0xc1,0x04,0xe0,0x43,0x02,0x25,
+0x1f,0x68,0x2f,0x43,0x1f,0x60,0xc0,0x46,0x00,0xbe,0xc0,0x46,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
diff --git a/contrib/loaders/flash/stmqspi/stmoctospi_erase_check.S b/contrib/loaders/flash/stmqspi/stmoctospi_erase_check.S
new file mode 100644
index 0000000..3af82d4
--- /dev/null
+++ b/contrib/loaders/flash/stmqspi/stmoctospi_erase_check.S
@@ -0,0 +1,108 @@
+/***************************************************************************
+ *   Copyright (C) 2019 by Andreas Bolsch                                  *
+ *   andreas.bolsch@mni.thm.de                                             *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+
+	.text
+	.syntax unified
+	.cpu cortex-m0
+	.thumb
+	.thumb_func
+
+/* Params:
+ * r0 - sector count
+ * r1 - QSPI io_base
+
+ * Clobbered:
+ * r2 - r7 tmp */
+
+#include "../../../../src/flash/nor/stmqspi.h"
+
+#define OCTOSPI_CCR_CCR					(OCTOSPI_CCR - OCTOSPI_CCR)
+#define OCTOSPI_TCR_CCR					(OCTOSPI_TCR - OCTOSPI_CCR)
+#define OCTOSPI_IR_CCR					(OCTOSPI_IR - OCTOSPI_CCR)
+
+	.macro	octospi_abort
+	movs	r5, #(1<<SPI_ABORT)			/* abort bit mask */
+	ldr		r7, [r1, #OCTOSPI_CR]		/* get OCTOSPI_CR register */
+	orrs	r7, r7, r5					/* set abort bit */
+	str		r7, [r1, #OCTOSPI_CR]		/* store new CR register */
+	.endm
+
+	.macro	wait_busy
+0:
+	ldr		r7, [r1, #OCTOSPI_SR]		/* load status */
+	lsrs	r7, r7, #(SPI_BUSY+1)		/* shift BUSY into C */
+	bcs		0b							/* loop until BUSY cleared */
+	movs	r7, #(1<<SPI_TCF)			/* TCF bitmask */
+	str		r7, [r1, #OCTOSPI_FCR]		/* clear TCF flag */
+	.endm
+
+start:
+	adr		r2, buffer					/* pointer to start of buffer */
+	movs	r3, #OCTOSPI_DR				/* load OCTOSPI_DR address offset */
+	adds	r3, r3, r1					/* address of OCTOSPI_DR */
+sector_start:
+	octospi_abort						/* start in clean state */
+	movs	r6, #OCTOSPI_CCR-OCTOSPI_DR	/* load OCTOSPI_CCR address offset */
+	adds	r6, r6, r3					/* address of OCTOSPI_CCR */
+	wait_busy
+	ldr		r7, cr_page_read			/* indirect read mode */
+	str		r7, [r1, #OCTOSPI_CR]		/* set mode */
+	ldmia	r2!, {r4, r5}				/* load address offset, length */
+	subs	r2, r2, #4					/* point to length */
+	subs	r5, r5, #1					/* decrement sector length for DLR */
+	str		r5, [r1, #OCTOSPI_DLR]		/* size-1 in DLR register */
+	ldr		r7, ccr_page_read			/* CCR for read */
+	str		r7, [r6, #OCTOSPI_CCR_CCR]	/* initiate transfer */
+	ldr		r7, tcr_page_read			/* TCR for read */
+	str		r7, [r6, #OCTOSPI_TCR_CCR]	/* instruction */
+	ldr		r7, ir_page_read			/* IR for read */
+	str		r7, [r6, #OCTOSPI_IR_CCR]	/* instruction */
+	str		r4, [r1, #OCTOSPI_AR]		/* store SPI start address */
+	ldr		r6, [r2, #4]				/* load initial value */
+read_loop:
+	ldrb	r4, [r3, #0]				/* read next byte from DR */
+	movs	r7, #0xFF					/* fill bits 8-15 */
+	lsls	r7, r7, #8					/* with ones */
+	orrs	r4, r4, r7					/* copy ones to left of read byte */
+	ands	r6, r6, r4					/* and read byte to result */
+	lsls	r4, r4, #8					/* shift result into higher byte */
+	orrs	r6, r6, r4					/* or read byte to result */
+	subs	r5, r5, #1					/* decrement byte (count-1) */
+	bpl		read_loop					/* again if sector not completed */
+	adds	r5, r5, #1					/* increment count due to the -1 */
+	stmia	r2!, {r5, r6}				/* save final count and result for sector */
+	subs	r0, r0, #1					/* decrement sector count */
+	bne		sector_start				/* next sector? */
+	octospi_abort						/* to idle state */
+
+exit:
+	.align	2							/* align to word, bkpt is 4 words */
+	bkpt	#0							/* before code end for exit_point */
+	.align	2							/* align to word */
+
+cr_page_read:
+	.space	4							/* OCTOSPI_CR value for read command */
+ccr_page_read:
+	.space	4							/* OCTOSPI_CCR value for read command */
+tcr_page_read:
+	.space	4							/* OCTOSPI_TCR value for read command */
+ir_page_read:
+	.space	4							/* OCTOSPI_IR value for read command */
+
+	.equ buffer, .
+
diff --git a/contrib/loaders/flash/stmqspi/stmoctospi_erase_check.inc b/contrib/loaders/flash/stmqspi/stmoctospi_erase_check.inc
new file mode 100644
index 0000000..c0e124a
--- /dev/null
+++ b/contrib/loaders/flash/stmqspi/stmoctospi_erase_check.inc
@@ -0,0 +1,8 @@
+/* Autogenerated with ../../../../src/helper/bin2char.sh */
+0x1b,0xa2,0x50,0x23,0x5b,0x18,0x02,0x25,0x0f,0x68,0x2f,0x43,0x0f,0x60,0xb0,0x26,
+0xf6,0x18,0x0f,0x6a,0xbf,0x09,0xfc,0xd2,0x02,0x27,0x4f,0x62,0x10,0x4f,0x0f,0x60,
+0x30,0xca,0x04,0x3a,0x01,0x3d,0x0d,0x64,0x0e,0x4f,0x37,0x60,0x0e,0x4f,0xb7,0x60,
+0x0e,0x4f,0x37,0x61,0x8c,0x64,0x56,0x68,0x1c,0x78,0xff,0x27,0x3f,0x02,0x3c,0x43,
+0x26,0x40,0x24,0x02,0x26,0x43,0x01,0x3d,0xf6,0xd5,0x01,0x35,0x60,0xc2,0x01,0x38,
+0xd9,0xd1,0x02,0x25,0x0f,0x68,0x2f,0x43,0x0f,0x60,0xc0,0x46,0x00,0xbe,0xc0,0x46,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
diff --git a/contrib/loaders/flash/stmqspi/stmoctospi_read.S b/contrib/loaders/flash/stmqspi/stmoctospi_read.S
new file mode 100644
index 0000000..fb5ff1f
--- /dev/null
+++ b/contrib/loaders/flash/stmqspi/stmoctospi_read.S
@@ -0,0 +1,142 @@
+/***************************************************************************
+ *   Copyright (C) 2019 by Andreas Bolsch                                  *
+ *   andreas.bolsch@mni.thm.de                                             *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+
+	.text
+	.syntax unified
+	.cpu cortex-m0
+	.thumb
+	.thumb_func
+
+/* Params:
+ * r0 - total count (bytes), remaining bytes (out, 0 means successful)
+ * r1 - flash page size
+ * r2 - address offset into flash
+ * r3 - OCTOSPI io_base
+ * r8 - fifo start
+ * r9 - fifo end + 1
+
+ * Clobbered:
+ * r4 - wp
+ * r5 - address of OCTOSPI_DR
+ * r6 - address of OCTOSPI_CCR
+ * r7 - tmp
+ */
+
+#include "../../../../src/flash/nor/stmqspi.h"
+
+#define OCTOSPI_CCR_CCR					(OCTOSPI_CCR - OCTOSPI_CCR)
+#define OCTOSPI_TCR_CCR					(OCTOSPI_TCR - OCTOSPI_CCR)
+#define OCTOSPI_IR_CCR					(OCTOSPI_IR - OCTOSPI_CCR)
+
+	.macro	octospi_abort
+	movs	r5, #(1<<SPI_ABORT)			/* abort bit mask */
+	ldr		r7, [r3, #OCTOSPI_CR]		/* get OCTOSPI CR register */
+	orrs	r7, r7, r5					/* set abort bit */
+	str		r7, [r3, #OCTOSPI_CR]		/* store new CR register */
+	.endm
+
+	.macro	wait_busy
+0:
+	ldr		r7, [r3, #OCTOSPI_SR]		/* load status */
+	lsrs	r7, r7, #(SPI_BUSY+1)		/* shift BUSY into C */
+	bcs		0b							/* loop until BUSY cleared */
+	movs	r7, #(1<<SPI_TCF)			/* TCF bitmask */
+	str		r7, [r3, #OCTOSPI_FCR]		/* clear TCF flag */
+	.endm
+
+start:
+	subs	r0, r0, #1					/* decrement count for DLR */
+	subs	r1, r1, #1					/* page size mask and for DLR */
+	ldr		r4, wp						/* load wp */
+start_read:
+	octospi_abort						/* start in clean state */
+	movs	r5, #OCTOSPI_DR				/* load OCTOSPI_DR address offset */
+	adds	r5, r5, r3					/* address of OCTOSPI_DR */
+	movs	r6, #OCTOSPI_CCR-OCTOSPI_DR	/* load OCTOSPI_CCR address offset */
+	adds	r6, r6, r5					/* address of OCTOSPI_CCR */
+	wait_busy
+	ldr		r7, cr_page_read			/* indirect read mode */
+	str		r7, [r3, #OCTOSPI_CR]		/* set mode */
+	mov		r7, r2						/* get current start address */
+	orrs	r7, r7, r1					/* end of current page */
+	subs	r7, r7, r2					/* count-1 to end of page */
+	cmp		r7, r0						/* if this count <= remaining */
+	bls		write_dlr					/* then write to end of page */
+	mov		r7, r0						/* else write all remaining */
+write_dlr:
+	str		r7, [r3, #OCTOSPI_DLR]		/* size-1 in DLR register */
+	ldr		r7, ccr_page_read			/* CCR for read */
+	str		r7, [r6, #OCTOSPI_CCR_CCR]	/* initiate transfer */
+	ldr		r7, tcr_page_read			/* TCR for read */
+	str		r7, [r6, #OCTOSPI_TCR_CCR]	/* instruction */
+	ldr		r7, ir_page_read			/* IR for read */
+	str		r7, [r6, #OCTOSPI_IR_CCR]	/* instruction */
+	str		r2, [r3, #OCTOSPI_AR]		/* store SPI start address */
+read_loop:
+	ldrb	r7, [r5]					/* read next byte from DR */
+	strb	r7, [r4, #0]				/* write next byte */
+	adds	r4, r4, #1					/* increment internal wp */
+	cmp		r4, r9						/* internal wp beyond end? */
+	blo		wait_fifo					/* if no, then ok */
+	mov		r4, r8						/* else wrap around */
+wait_fifo:
+	ldr		r7, rp						/* get rp */
+	cmp		r7, #0						/* if rp equals 0 */
+	beq		exit						/* then abort */
+	cmp		r4, r7						/* check if fifo full */
+	beq		wait_fifo					/* wait until not full */
+	adr		r7, wp						/* get address of wp */
+	str		r4, [r7]					/* store updated wp */
+	adds	r2, r2, #1					/* increment address */
+	subs	r0, r0, #1					/* decrement (count-1) */
+	bmi		exit						/* stop if no data left */
+	tst		r2, r1						/* page end ? */
+	bne		read_loop					/* if not, then next byte */
+page_end:
+	bal		start_read					/* then next page */
+
+exit:
+	adds	r0, r0, #1					/* increment count due to the -1 */
+	octospi_abort						/* to idle state */
+
+	.align	2							/* align to word, bkpt is 4 words */
+	bkpt	#0							/* before code end for exit_point */
+	.align	2							/* align to word */
+
+	.space	4							/* not used */
+	.space	4							/* not used */
+	.space	4							/* not used */
+	.space	4							/* not used */
+
+	.space	4							/* not used */
+	.space	4							/* not used */
+	.space	4							/* not used */
+	.space	4							/* not used */
+
+cr_page_read:
+	.space	4							/* OCTOSPI_CR value for read command */
+ccr_page_read:
+	.space	4							/* OCTOSPI_CCR value for read command */
+tcr_page_read:
+	.space	4							/* OCTOSPI_TCR value for read command */
+ir_page_read:
+	.space	4							/* OCTOSPI_IR value for read command */
+
+	.equ wp, .							/* wp, uint32_t */
+	.equ rp, wp + 4						/* rp, uint32_t */
+	.equ buffer, rp + 4					/* buffer follows right away */
diff --git a/contrib/loaders/flash/stmqspi/stmoctospi_read.inc b/contrib/loaders/flash/stmqspi/stmoctospi_read.inc
new file mode 100644
index 0000000..583f316
--- /dev/null
+++ b/contrib/loaders/flash/stmqspi/stmoctospi_read.inc
@@ -0,0 +1,12 @@
+/* Autogenerated with ../../../../src/helper/bin2char.sh */
+0x01,0x38,0x01,0x39,0x27,0x4c,0x02,0x25,0x1f,0x68,0x2f,0x43,0x1f,0x60,0x50,0x25,
+0xed,0x18,0xb0,0x26,0x76,0x19,0x1f,0x6a,0xbf,0x09,0xfc,0xd2,0x02,0x27,0x5f,0x62,
+0x1c,0x4f,0x1f,0x60,0x17,0x46,0x0f,0x43,0xbf,0x1a,0x87,0x42,0x00,0xd9,0x07,0x46,
+0x1f,0x64,0x19,0x4f,0x37,0x60,0x19,0x4f,0xb7,0x60,0x19,0x4f,0x37,0x61,0x9a,0x64,
+0x2f,0x78,0x27,0x70,0x01,0x34,0x4c,0x45,0x00,0xd3,0x44,0x46,0x16,0x4f,0x00,0x2f,
+0x09,0xd0,0xbc,0x42,0xfa,0xd0,0x13,0xa7,0x3c,0x60,0x01,0x32,0x01,0x38,0x02,0xd4,
+0x0a,0x42,0xed,0xd1,0xcf,0xe7,0x01,0x30,0x02,0x25,0x1f,0x68,0x2f,0x43,0x1f,0x60,
+0x00,0xbe,0xc0,0x46,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,
diff --git a/contrib/loaders/flash/stmqspi/stmoctospi_write.S b/contrib/loaders/flash/stmqspi/stmoctospi_write.S
new file mode 100644
index 0000000..867a024
--- /dev/null
+++ b/contrib/loaders/flash/stmqspi/stmoctospi_write.S
@@ -0,0 +1,219 @@
+/***************************************************************************
+ *   Copyright (C) 2018 by Andreas Bolsch                                  *
+ *   andreas.bolsch@mni.thm.de                                             *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+
+	.text
+	.syntax unified
+	.cpu cortex-m0
+	.thumb
+	.thumb_func
+
+/* Params:
+ * r0 - total count (bytes), remaining bytes (out, 0 means successful)
+ * r1 - flash page size
+ * r2 - address offset into flash
+ * r3 - OCTOSPI io_base
+ * r8 - fifo start
+ * r9 - fifo end + 1
+
+ * Clobbered:
+ * r4 - rp
+ * r5 - address of OCTOSPI_DR
+ * r6 - address of OCTOSPI_CCR
+ * r7 - tmp
+ * r10 - single 0x0 / dual 0x1
+ */
+
+#include "../../../../src/flash/nor/stmqspi.h"
+
+#define OCTOSPI_CCR_CCR					(OCTOSPI_CCR - OCTOSPI_CCR)
+#define OCTOSPI_TCR_CCR					(OCTOSPI_TCR - OCTOSPI_CCR)
+#define OCTOSPI_IR_CCR					(OCTOSPI_IR - OCTOSPI_CCR)
+
+	.macro	octospi_abort
+	movs	r5, #(1<<SPI_ABORT)			/* abort bit mask */
+	ldr		r7, [r3, #OCTOSPI_CR]		/* get OCTOSPI CR register */
+	orrs	r7, r7, r5					/* set abort bit */
+	str		r7, [r3, #OCTOSPI_CR]		/* store new CR register */
+	.endm
+
+	.macro	wait_busy
+0:
+	ldr		r7, [r3, #OCTOSPI_SR]		/* load status */
+	lsrs	r7, r7, #(SPI_BUSY+1)		/* shift BUSY into C */
+	bcs		0b							/* loop until BUSY cleared */
+	movs	r7, #(1<<SPI_TCF)			/* TCF bitmask */
+	str		r7, [r3, #OCTOSPI_FCR]		/* clear TCF flag */
+	.endm
+
+start:
+	subs	r0, r0, #1					/* decrement count for DLR */
+	subs	r1, r1, #1					/* page size mask and for DLR */
+	ldr		r4, rp						/* load rp */
+	ldr		r7, [r3, #OCTOSPI_CR]		/* get OCTOSPI_CR register */
+	lsls	r7, r7, #(31-SPI_DUAL_FLASH)	/* clear higher order bits */
+	lsrs	r7, r7, #31					/* DUAL_FLASH bit into bit 0 */
+	mov		r10, r7						/* save in r10 */
+wip_loop:
+	octospi_abort						/* start in clean state */
+	movs	r5, #OCTOSPI_DR				/* load OCTOSPI_DR address offset */
+	adds	r5, r5, r3					/* address of OCTOSPI_DR */
+	movs	r6, #OCTOSPI_CCR-OCTOSPI_DR	/* load OCTOSPI_CCR address offset */
+	adds	r6, r6, r5					/* address of OCTOSPI_CCR */
+	wait_busy
+	ldr		r7, cr_read_status			/* indirect read mode */
+	str		r7, [r3, #OCTOSPI_CR]		/* set mode */
+	mov		r7, r10						/* get dual bit */
+	str		r7, [r3, #OCTOSPI_DLR]		/* one or two (for dual) bytes */
+	ldr		r7, ccr_read_status			/* CCR for status read */
+	str		r7, [r6, #OCTOSPI_CCR_CCR]	/* initiate status read */
+	ldr		r7, tcr_read_status			/* TCR for status read */
+	str		r7, [r6, #OCTOSPI_TCR_CCR]	/* instruction */
+	ldr		r7, ir_read_status			/* IR for status read */
+	str		r7, [r6, #OCTOSPI_IR_CCR]	/* instruction */
+	movs	r7, #0						/* dummy address */
+	str		r7, [r3, #OCTOSPI_AR]		/* into AR (for 8-line mode) */
+	ldrb	r7, [r5]					/* get first status register */
+	lsrs	r7, r7, #(SPIFLASH_BSY+1)	/* if first flash busy, */
+	bcs		wip_loop					/* then poll again */
+	mov		r7, r10						/* get dual bit */
+	tst		r7, r7						/* dual mode ? */
+	beq		write_enable				/* not dual, then ok */
+	ldrb	r7, [r5]					/* get second status register */
+	lsrs	r7, r7, #(SPIFLASH_BSY+1)	/* if second flash busy, */
+	bcs		wip_loop					/* then poll again */
+write_enable:
+	tst		r0, r0						/* test residual count */
+	bmi		exit						/* if negative, then finished */
+	wait_busy
+	ldr		r7, cr_write_enable			/* indirect write mode */
+	str		r7, [r3, #OCTOSPI_CR]		/* set mode */
+	ldr		r7, ccr_write_enable		/* CCR for write enable */
+	str		r7, [r6, #OCTOSPI_CCR_CCR]	/* initiate write enable */
+	ldr		r7, tcr_write_enable		/* TCR for write enable */
+	str		r7, [r6, #OCTOSPI_TCR_CCR]	/* write enable instruction */
+	ldr		r7, ir_write_enable			/* IR for write enable */
+	str		r7, [r6, #OCTOSPI_IR_CCR]	/* instruction */
+	movs	r7, #0						/* silicon bug in L5? dummy write */
+	str		r7, [r3, #OCTOSPI_AR]		/* into AR resolves issue */
+	wait_busy
+	ldr		r7, cr_read_status			/* indirect read mode */
+	str		r7, [r3, #OCTOSPI_CR]		/* set mode */
+	mov		r7, r10						/* get dual count */
+	str		r7, [r3, #OCTOSPI_DLR]		/* one or two (for dual) bytes */
+	ldr		r7, ccr_read_status			/* CCR for status read */
+	str		r7, [r6, #OCTOSPI_CCR_CCR]	/* initiate status read */
+	ldr		r7, tcr_read_status			/* TCR for status read */
+	str		r7, [r6, #OCTOSPI_TCR_CCR]	/* instruction */
+	ldr		r7, ir_read_status			/* IR for status read */
+	str		r7, [r6, #OCTOSPI_IR_CCR]	/* instruction */
+	movs	r7, #0						/* dummy address */
+	str		r7, [r3, #OCTOSPI_AR]		/* into AR (for 8-line mode) */
+	ldrb	r7, [r5]					/* get first status register */
+	lsrs	r7, r7, #(SPIFLASH_WE+1)	/* if first flash not */
+	bcc		error						/* write enabled, then error */
+	mov		r7, r10						/* get dual bit */
+	tst		r7, r7						/* dual mode ? */
+	beq		start_write					/* not dual, then ok */
+	ldrb	r7, [r5]					/* get second status register */
+	lsrs	r7, r7, #(SPIFLASH_WE+1)	/* if second flash not */
+	bcc		error						/* write enabled, then error */
+start_write:
+	wait_busy
+	ldr		r7, cr_page_write			/* indirect write mode */
+	str		r7, [r3, #OCTOSPI_CR]		/* set mode */
+	mov		r7, r2						/* get current start address */
+	orrs	r7, r7, r1					/* end of current page */
+	subs	r7, r7, r2					/* count-1 to end of page */
+	cmp		r7, r0						/* if this count <= remaining */
+	bls		write_dlr					/* then write to end of page */
+	mov		r7, r0						/* else write all remaining */
+write_dlr:
+	str		r7, [r3, #OCTOSPI_DLR]		/* size-1 in DLR register */
+	ldr		r7, ccr_page_write			/* CCR for page write */
+	str		r7, [r6, #OCTOSPI_CCR_CCR]	/* initiate transfer */
+	ldr		r7, tcr_page_write			/* TCR for page write */
+	str		r7, [r6, #OCTOSPI_TCR_CCR]	/* instruction */
+	ldr		r7, ir_page_write			/* IR for page write */
+	str		r7, [r6, #OCTOSPI_IR_CCR]	/* instruction */
+	str		r2, [r3, #OCTOSPI_AR]		/* store SPI start address */
+write_loop:
+	ldr		r7, wp						/* get wp */
+	cmp		r7, #0						/* if wp equals 0 */
+	beq		exit						/* then abort */
+	cmp		r4, r7						/* check if fifo empty */
+	beq		write_loop					/* wait until not empty */
+	ldrb	r7, [r4, #0]				/* read next byte */
+	strb	r7, [r5]					/* write next byte to DR */
+	adds	r4, r4, #1					/* increment internal rp */
+	cmp		r4, r9						/* internal rp beyond end? */
+	blo		upd_write					/* if no, then ok */
+	mov		r4, r8						/* else wrap around */
+upd_write:
+	adr		r7, rp						/* get address of rp */
+	str		r4, [r7]					/* store updated rp */
+	adds	r2, r2, #1					/* increment address */
+	subs	r0, r0, #1					/* decrement (count-1) */
+	bmi		page_end					/* stop if no data left */
+	tst		r2, r1						/* page end ? */
+	bne		write_loop					/* if not, then next byte */
+page_end:
+	ldr		r7, [r3, #OCTOSPI_SR]		/* load status */
+	lsrs	r7, r7, #(SPI_TCF+1)		/* shift TCF into C */
+	bcc		page_end					/* loop until TCF set */
+	bal		wip_loop					/* then next page */
+
+error:
+	movs	r0, #0						/* return 0xFFFFFFFF */
+	subs	r0, r0, #2					/* for error */
+exit:
+	adds	r0, r0, #1					/* increment count due to the -1 */
+	octospi_abort						/* to idle state */
+	.align	2							/* align to word, bkpt is 4 words */
+	bkpt	#0							/* before code end for exit_point */
+	.align	2							/* align to word */
+
+cr_read_status:
+	.space	4							/* OCTOSPI_CR value for READ_STATUS command */
+ccr_read_status:
+	.space	4							/* OCTOSPI_CCR value for READ_STATUS command */
+tcr_read_status:
+	.space	4							/* OCTOSPI_TCR value for READ_STATUS command */
+ir_read_status:
+	.space	4							/* OCTOSPI_IR value for READ_STATUS command */
+
+cr_write_enable:
+	.space	4							/* OCTOSPI_CR value for WRITE_ENABLE command */
+ccr_write_enable:
+	.space	4							/* OCTOSPI_CCR value for WRITE_ENABLE command */
+tcr_write_enable:
+	.space	4							/* OCTOSPI_TCR value for WRITE_ENABLE command */
+ir_write_enable:
+	.space	4							/* OCTOSPI_IR value for WRITE_ENABLE command */
+
+cr_page_write:
+	.space	4							/* OCTOSPI_CR value for PAGE_PROG command */
+ccr_page_write:
+	.space	4							/* OCTOSPI_CCR value for PAGE_PROG command */
+tcr_page_write:
+	.space	4							/* OCTOSPI_TCR value for PAGE_PROG command */
+ir_page_write:
+	.space	4							/* OCTOSPI_IR value for PAGE_PROG command */
+
+	.equ wp, .							/* wp, uint32_t */
+	.equ rp, wp + 4						/* rp, uint32_t */
+	.equ buffer, rp + 4					/* buffer follows right away */
diff --git a/contrib/loaders/flash/stmqspi/stmoctospi_write.inc b/contrib/loaders/flash/stmqspi/stmoctospi_write.inc
new file mode 100644
index 0000000..81781a2
--- /dev/null
+++ b/contrib/loaders/flash/stmqspi/stmoctospi_write.inc
@@ -0,0 +1,21 @@
+/* Autogenerated with ../../../../src/helper/bin2char.sh */
+0x01,0x38,0x01,0x39,0x4f,0x4c,0x1f,0x68,0x7f,0x06,0xff,0x0f,0xba,0x46,0x02,0x25,
+0x1f,0x68,0x2f,0x43,0x1f,0x60,0x50,0x25,0xed,0x18,0xb0,0x26,0x76,0x19,0x1f,0x6a,
+0xbf,0x09,0xfc,0xd2,0x02,0x27,0x5f,0x62,0x39,0x4f,0x1f,0x60,0x57,0x46,0x1f,0x64,
+0x38,0x4f,0x37,0x60,0x38,0x4f,0xb7,0x60,0x38,0x4f,0x37,0x61,0x00,0x27,0x9f,0x64,
+0x2f,0x78,0x7f,0x08,0xe3,0xd2,0x57,0x46,0x3f,0x42,0x02,0xd0,0x2f,0x78,0x7f,0x08,
+0xdd,0xd2,0x00,0x42,0x55,0xd4,0x1f,0x6a,0xbf,0x09,0xfc,0xd2,0x02,0x27,0x5f,0x62,
+0x2f,0x4f,0x1f,0x60,0x2f,0x4f,0x37,0x60,0x2f,0x4f,0xb7,0x60,0x2f,0x4f,0x37,0x61,
+0x00,0x27,0x9f,0x64,0x1f,0x6a,0xbf,0x09,0xfc,0xd2,0x02,0x27,0x5f,0x62,0x24,0x4f,
+0x1f,0x60,0x57,0x46,0x1f,0x64,0x23,0x4f,0x37,0x60,0x23,0x4f,0xb7,0x60,0x23,0x4f,
+0x37,0x61,0x00,0x27,0x9f,0x64,0x2f,0x78,0xbf,0x08,0x30,0xd3,0x57,0x46,0x3f,0x42,
+0x02,0xd0,0x2f,0x78,0xbf,0x08,0x2a,0xd3,0x1f,0x6a,0xbf,0x09,0xfc,0xd2,0x02,0x27,
+0x5f,0x62,0x1f,0x4f,0x1f,0x60,0x17,0x46,0x0f,0x43,0xbf,0x1a,0x87,0x42,0x00,0xd9,
+0x07,0x46,0x1f,0x64,0x1b,0x4f,0x37,0x60,0x1b,0x4f,0xb7,0x60,0x1b,0x4f,0x37,0x61,
+0x9a,0x64,0x1b,0x4f,0x00,0x2f,0x14,0xd0,0xbc,0x42,0xfa,0xd0,0x27,0x78,0x2f,0x70,
+0x01,0x34,0x4c,0x45,0x00,0xd3,0x44,0x46,0x16,0xa7,0x3c,0x60,0x01,0x32,0x01,0x38,
+0x01,0xd4,0x0a,0x42,0xed,0xd1,0x1f,0x6a,0xbf,0x08,0xfc,0xd3,0x87,0xe7,0x00,0x20,
+0x02,0x38,0x01,0x30,0x02,0x25,0x1f,0x68,0x2f,0x43,0x1f,0x60,0x00,0xbe,0xc0,0x46,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
diff --git a/contrib/loaders/flash/stmqspi/stmqspi_crc32.S b/contrib/loaders/flash/stmqspi/stmqspi_crc32.S
new file mode 100644
index 0000000..bfb2662
--- /dev/null
+++ b/contrib/loaders/flash/stmqspi/stmqspi_crc32.S
@@ -0,0 +1,108 @@
+/***************************************************************************
+ *   Copyright (C) 2019 by Andreas Bolsch                                  *
+ *   andreas.bolsch@mni.thm.de                                             *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+
+	.text
+	.syntax unified
+	.cpu cortex-m0
+	.thumb
+	.thumb_func
+
+/* Params:
+ * r0 - total count (bytes), crc32 (out)
+ * r1 - flash page size
+ * r2 - address offset into flash
+ * r3 - QSPI io_base
+
+ * Clobbered:
+ * r4 - rp
+ * r5 - address of QSPI_DR
+ * r7 - tmp
+ */
+
+#include "../../../../src/flash/nor/stmqspi.h"
+
+	.macro	qspi_abort
+	movs	r5, #(1<<SPI_ABORT)			/* abort bit mask */
+	ldr		r7, [r3, #QSPI_CR]			/* get QSPI_CR register */
+	orrs	r7, r7, r5					/* set abort bit */
+	str		r7, [r3, #QSPI_CR]			/* store new CR register */
+	.endm
+
+	.macro	wait_busy
+0:
+	ldr		r7, [r3, #QSPI_SR]			/* load status */
+	lsrs	r7, r7, #(SPI_BUSY+1)		/* shift BUSY into C */
+	bcs		0b							/* loop until BUSY cleared */
+	movs	r7, #(1<<SPI_TCF)			/* TCF bitmask */
+	str		r7, [r3, #QSPI_FCR]			/* clear TCF flag */
+	.endm
+
+start:
+	subs	r0, r0, #1					/* decrement count for DLR */
+	subs	r1, r1, #1					/* page size mask and for DLR */
+	movs	r4, #0x00					/* initialize crc */
+	mvns	r4, r4						/* to 0xFFFFFFFF */
+start_read:
+	qspi_abort							/* start in clean state */
+	movs	r5, #QSPI_DR				/* load QSPI_DR address offset */
+	adds	r5, r5, r3					/* address of QSPI_DR */
+	wait_busy
+	mov		r7, r2						/* get current start address */
+	orrs	r7, r7, r1					/* end of current page */
+	subs	r7, r7, r2					/* count-1 to end of page */
+	cmp		r7, r0						/* if this count <= remaining */
+	bls		write_dlr					/* then read to end of page */
+	mov		r7, r0						/* else read all remaining */
+write_dlr:
+	str		r7, [r3, #QSPI_DLR]			/* size-1 in DLR register */
+	ldr		r7, ccr_page_read			/* CCR for page read */
+	str		r7, [r3, #QSPI_CCR]			/* initiate transfer */
+	str		r2, [r3, #QSPI_AR]			/* store SPI start address */
+	ldr		r7, [r3, #QSPI_SR]			/* wait for command startup */
+	ldr		r6, =0x04C11DB7				/* CRC32 polynomial */
+read_loop:
+	ldrb	r7, [r5]					/* read next byte from DR */
+	lsls	r7, r7, #24					/* shift into msb */
+	eors	r4, r4, r7
+	.rept	8							/* unrolled bit loop */
+	asrs	r7, r4, #31					/* copy bit 31 into bits 0 to 31 */
+	ands	r7, r7, r6					/* r7 neg. -> CRC32XOR, pos. -> 0x0 */
+	lsls	r4, r4, #1					/* shift result */
+	eors	r4, r4, r7					/* eor by CRC32XOR or 0x0 */
+	.endr
+	adds	r2, r2, #1					/* increment address */
+	subs	r0, r0, #1					/* decrement (count-1) */
+	bmi		exit						/* stop if no data left */
+	tst		r2, r1						/* page end ? */
+	bne		read_loop					/* if not, then next byte */
+page_end:
+	bal		start_read					/* then next page */
+	.pool
+
+exit:
+	mvns	r0, r4						/* invert to get final result */
+	qspi_abort							/* to idle state */
+	.align	2							/* align to word, bkpt is 4 words */
+	bkpt	#0							/* before code end for exit_point */
+	.align	2							/* align to word */
+
+	.space	4							/* not used */
+ccr_page_read:
+	.space	4							/* QSPI_CCR value for read command */
+	.space	4							/* not used */
+	.space	4							/* not used */
diff --git a/contrib/loaders/flash/stmqspi/stmqspi_crc32.inc b/contrib/loaders/flash/stmqspi/stmqspi_crc32.inc
new file mode 100644
index 0000000..b532a48
--- /dev/null
+++ b/contrib/loaders/flash/stmqspi/stmqspi_crc32.inc
@@ -0,0 +1,12 @@
+/* Autogenerated with ../../../../src/helper/bin2char.sh */
+0x01,0x38,0x01,0x39,0x00,0x24,0xe4,0x43,0x02,0x25,0x1f,0x68,0x2f,0x43,0x1f,0x60,
+0x20,0x25,0xed,0x18,0x9f,0x68,0xbf,0x09,0xfc,0xd2,0x02,0x27,0xdf,0x60,0x17,0x46,
+0x0f,0x43,0xbf,0x1a,0x87,0x42,0x00,0xd9,0x07,0x46,0x1f,0x61,0x1c,0x4f,0x5f,0x61,
+0x9a,0x61,0x9f,0x68,0x14,0x4e,0x2f,0x78,0x3f,0x06,0x7c,0x40,0xe7,0x17,0x37,0x40,
+0x64,0x00,0x7c,0x40,0xe7,0x17,0x37,0x40,0x64,0x00,0x7c,0x40,0xe7,0x17,0x37,0x40,
+0x64,0x00,0x7c,0x40,0xe7,0x17,0x37,0x40,0x64,0x00,0x7c,0x40,0xe7,0x17,0x37,0x40,
+0x64,0x00,0x7c,0x40,0xe7,0x17,0x37,0x40,0x64,0x00,0x7c,0x40,0xe7,0x17,0x37,0x40,
+0x64,0x00,0x7c,0x40,0xe7,0x17,0x37,0x40,0x64,0x00,0x7c,0x40,0x01,0x32,0x01,0x38,
+0x04,0xd4,0x0a,0x42,0xd7,0xd1,0xbf,0xe7,0xb7,0x1d,0xc1,0x04,0xe0,0x43,0x02,0x25,
+0x1f,0x68,0x2f,0x43,0x1f,0x60,0xc0,0x46,0x00,0xbe,0xc0,0x46,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
diff --git a/contrib/loaders/flash/stmqspi/stmqspi_erase_check.S b/contrib/loaders/flash/stmqspi/stmqspi_erase_check.S
new file mode 100644
index 0000000..d011103
--- /dev/null
+++ b/contrib/loaders/flash/stmqspi/stmqspi_erase_check.S
@@ -0,0 +1,91 @@
+/***************************************************************************
+ *   Copyright (C) 2019 by Andreas Bolsch                                  *
+ *   andreas.bolsch@mni.thm.de                                             *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+
+	.text
+	.syntax unified
+	.cpu cortex-m0
+	.thumb
+	.thumb_func
+
+/* Params:
+ * r0 - sector count
+ * r1 - QSPI io_base
+
+ * Clobbered:
+ * r2 - r7 tmp */
+
+#include "../../../../src/flash/nor/stmqspi.h"
+
+	.macro	qspi_abort
+	movs	r4, #(1<<SPI_ABORT)			/* abort bit mask */
+	ldr		r7, [r1, #QSPI_CR]			/* get QSPI_CR register */
+	orrs	r7, r7, r4					/* set abort bit */
+	str		r7, [r1, #QSPI_CR]			/* store new CR register */
+	.endm
+
+	.macro	wait_busy
+0:
+	ldr		r7, [r1, #QSPI_SR]			/* load status */
+	lsrs	r7, r7, #(SPI_BUSY+1)		/* shift BUSY into C */
+	bcs		0b							/* loop until BUSY cleared */
+	movs	r7, #(1<<SPI_TCF)			/* TCF bitmask */
+	str		r7, [r1, #QSPI_FCR]			/* clear TCF flag */
+	.endm
+
+start:
+	adr		r2, buffer					/* pointer to start of buffer */
+	movs	r3, #QSPI_DR				/* load QSPI_DR address offset */
+	add		r3, r3, r1					/* address of QSPI_DR */
+sector_start:
+	qspi_abort							/* start in clean state */
+	ldmia	r2!, {r4, r5, r6}			/* load address offset, length, initial value */
+	subs	r2, r2, #8					/* point to length */
+	subs	r5, r5, #1					/* decrement sector length for DLR */
+	wait_busy
+	str		r5, [r1, #QSPI_DLR]			/* size-1 in DLR register */
+	ldr		r7, ccr_page_read			/* CCR for page read */
+	str		r7, [r1, #QSPI_CCR]			/* initiate transfer */
+	str		r4, [r1, #QSPI_AR]			/* store SPI start address */
+	ldr		r7, [r1, #QSPI_SR]			/* wait for command startup */
+read_loop:
+	ldrb	r4, [r3]					/* read next byte from DR */
+	movs	r7, #0xFF					/* fill bits 8-15 */
+	lsls	r7, r7, #8					/* with ones */
+	orrs	r4, r4, r7					/* copy ones to left of read byte */
+	ands	r6, r6, r4					/* and read byte to result */
+	lsls	r4, r4, #8					/* shift result into higher byte */
+	orrs	r6, r6, r4					/* or read byte to result */
+	subs	r5, r5, #1					/* decrement byte (count-1) */
+	bpl		read_loop					/* again if sector not completed */
+	adds	r5, r5, #1					/* increment count due to the -1 */
+	stmia	r2!, {r5, r6}				/* save final count and result for sector */
+	subs	r0, r0, #1					/* decrement sector count */
+	bne		sector_start				/* next sector? */
+	qspi_abort							/* to idle state */
+	.align	2							/* align to word, bkpt is 4 words */
+	bkpt	#0							/* before code end for exit_point */
+	.align	2							/* align to word */
+
+	.space	4							/* not used */
+ccr_page_read:
+	.space	4							/* QSPI_CCR value for read command */
+	.space	4							/* not used */
+	.space	4							/* not used */
+
+	.equ buffer, .
+
diff --git a/contrib/loaders/flash/stmqspi/stmqspi_erase_check.inc b/contrib/loaders/flash/stmqspi/stmqspi_erase_check.inc
new file mode 100644
index 0000000..3bf7898
--- /dev/null
+++ b/contrib/loaders/flash/stmqspi/stmqspi_erase_check.inc
@@ -0,0 +1,7 @@
+/* Autogenerated with ../../../../src/helper/bin2char.sh */
+0x17,0xa2,0x20,0x23,0x0b,0x44,0x02,0x24,0x0f,0x68,0x27,0x43,0x0f,0x60,0x70,0xca,
+0x08,0x3a,0x01,0x3d,0x8f,0x68,0xbf,0x09,0xfc,0xd2,0x02,0x27,0xcf,0x60,0x0d,0x61,
+0x0c,0x4f,0x4f,0x61,0x8c,0x61,0x8f,0x68,0x1c,0x78,0xff,0x27,0x3f,0x02,0x3c,0x43,
+0x26,0x40,0x24,0x02,0x26,0x43,0x01,0x3d,0xf6,0xd5,0x01,0x35,0x60,0xc2,0x01,0x38,
+0xe1,0xd1,0x02,0x24,0x0f,0x68,0x27,0x43,0x0f,0x60,0xc0,0x46,0x00,0xbe,0xc0,0x46,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
diff --git a/contrib/loaders/flash/stmqspi/stmqspi_read.S b/contrib/loaders/flash/stmqspi/stmqspi_read.S
new file mode 100644
index 0000000..b84d4eb
--- /dev/null
+++ b/contrib/loaders/flash/stmqspi/stmqspi_read.S
@@ -0,0 +1,127 @@
+/***************************************************************************
+ *   Copyright (C) 2019 by Andreas Bolsch                                  *
+ *   andreas.bolsch@mni.thm.de                                             *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+
+	.text
+	.syntax unified
+	.cpu cortex-m0
+	.thumb
+	.thumb_func
+
+/* Params:
+ * r0 - total count (bytes), remaining bytes (out, 0 means successful)
+ * r1 - flash page size
+ * r2 - address offset into flash
+ * r3 - QSPI io_base
+ * r8 - fifo start
+ * r9 - fifo end + 1
+
+ * Clobbered:
+ * r4 - wp
+ * r5 - address of QSPI_DR
+ * r7 - tmp
+ */
+
+#include "../../../../src/flash/nor/stmqspi.h"
+
+	.macro	qspi_abort
+	movs	r5, #(1<<SPI_ABORT)			/* abort bit mask */
+	ldr		r7, [r3, #QSPI_CR]			/* get QSPI_CR register */
+	orrs	r7, r7, r5					/* set abort bit */
+	str		r7, [r3, #QSPI_CR]			/* store new CR register */
+	.endm
+
+	.macro	wait_busy
+0:
+	ldr		r7, [r3, #QSPI_SR]			/* load status */
+	lsrs	r7, r7, #(SPI_BUSY+1)		/* shift BUSY into C */
+	bcs		0b							/* loop until BUSY cleared */
+	movs	r7, #(1<<SPI_TCF)			/* TCF bitmask */
+	str		r7, [r3, #QSPI_FCR]			/* clear TCF flag */
+	.endm
+
+start:
+	subs	r0, r0, #1					/* decrement count for DLR */
+	subs	r1, r1, #1					/* page size mask and for DLR */
+	ldr		r4, wp						/* load wp */
+start_read:
+	qspi_abort							/* start in clean state */
+	movs	r5, #QSPI_DR				/* load QSPI_DR address offset */
+	adds	r5, r5, r3					/* address of QSPI_DR */
+	wait_busy
+	mov		r7, r2						/* get current start address */
+	orrs	r7, r7, r1					/* end of current page */
+	subs	r7, r7, r2					/* count-1 to end of page */
+	cmp		r7, r0						/* if this count <= remaining */
+	bls		write_dlr					/* then read to end of page */
+	mov		r7, r0						/* else read all remaining */
+write_dlr:
+	str		r7, [r3, #QSPI_DLR]			/* size-1 in DLR register */
+	ldr		r7, ccr_page_read			/* CCR for page read */
+	str		r7, [r3, #QSPI_CCR]			/* initiate transfer */
+	str		r2, [r3, #QSPI_AR]			/* store SPI start address */
+	ldr		r7, [r3, #QSPI_SR]			/* wait for command startup */
+read_loop:
+	ldrb	r7, [r5]					/* read next byte from DR */
+	strb	r7, [r4, #0]				/* write next byte */
+	adds	r4, r4, #1					/* increment internal wp */
+	cmp		r4, r9						/* internal wp beyond end? */
+	blo		wait_fifo					/* if no, then ok */
+	mov		r4, r8						/* else wrap around */
+wait_fifo:
+	ldr		r7, rp						/* get rp */
+	cmp		r7, #0						/* if rp equals 0 */
+	beq		exit						/* then abort */
+	cmp		r4, r7						/* check if fifo full */
+	beq		wait_fifo					/* wait until not full */
+	adr		r7, wp						/* get address of wp */
+	str		r4, [r7]					/* store updated wp */
+	adds	r2, r2, #1					/* increment address */
+	subs	r0, r0, #1					/* decrement (count-1) */
+	bmi		exit						/* stop if no data left */
+	tst		r2, r1						/* page end ? */
+	bne		read_loop					/* if not, then next byte */
+page_end:
+	bal		start_read					/* then next page */
+
+exit:
+	adds	r0, r0, #1					/* increment count due to the -1 */
+	qspi_abort							/* to idle state */
+
+	.align	2							/* align to word, bkpt is 4 words */
+	bkpt	#0							/* before code end for exit_point */
+	.align	2							/* align to word */
+
+	.space	4							/* not used */
+	.space	4							/* not used */
+	.space	4							/* not used */
+	.space	4							/* not used */
+
+	.space	4							/* not used */
+	.space	4							/* not used */
+	.space	4							/* not used */
+	.space	4							/* not used */
+
+	.space	4							/* not used */
+ccr_page_read:
+	.space	4							/* QSPI_CCR value for read command */
+	.space	4							/* not used */
+	.space	4							/* not used */
+
+	.equ wp, .							/* wp, uint32_t */
+	.equ rp, wp + 4						/* rp, uint32_t */
+	.equ buffer, rp + 4					/* buffer follows right away */
diff --git a/contrib/loaders/flash/stmqspi/stmqspi_read.inc b/contrib/loaders/flash/stmqspi/stmqspi_read.inc
new file mode 100644
index 0000000..934b9b1
--- /dev/null
+++ b/contrib/loaders/flash/stmqspi/stmqspi_read.inc
@@ -0,0 +1,11 @@
+/* Autogenerated with ../../../../src/helper/bin2char.sh */
+0x01,0x38,0x01,0x39,0x24,0x4c,0x02,0x25,0x1f,0x68,0x2f,0x43,0x1f,0x60,0x20,0x25,
+0xed,0x18,0x9f,0x68,0xbf,0x09,0xfc,0xd2,0x02,0x27,0xdf,0x60,0x17,0x46,0x0f,0x43,
+0xbf,0x1a,0x87,0x42,0x00,0xd9,0x07,0x46,0x1f,0x61,0x18,0x4f,0x5f,0x61,0x9a,0x61,
+0x9f,0x68,0x2f,0x78,0x27,0x70,0x01,0x34,0x4c,0x45,0x00,0xd3,0x44,0x46,0x17,0x4f,
+0x00,0x2f,0x09,0xd0,0xbc,0x42,0xfa,0xd0,0x13,0xa7,0x3c,0x60,0x01,0x32,0x01,0x38,
+0x02,0xd4,0x0a,0x42,0xed,0xd1,0xd6,0xe7,0x01,0x30,0x02,0x25,0x1f,0x68,0x2f,0x43,
+0x1f,0x60,0xc0,0x46,0x00,0xbe,0xc0,0x46,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
diff --git a/contrib/loaders/flash/stmqspi/stmqspi_write.S b/contrib/loaders/flash/stmqspi/stmqspi_write.S
new file mode 100644
index 0000000..40953ac
--- /dev/null
+++ b/contrib/loaders/flash/stmqspi/stmqspi_write.S
@@ -0,0 +1,177 @@
+/***************************************************************************
+ *   Copyright (C) 2016 - 2018 by Andreas Bolsch                           *
+ *   andreas.bolsch@mni.thm.de                                             *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+
+	.text
+	.syntax unified
+	.cpu cortex-m0
+	.thumb
+	.thumb_func
+
+/* Params:
+ * r0 - total count (bytes), remaining bytes (out, 0 means successful)
+ * r1 - flash page size
+ * r2 - address offset into flash
+ * r3 - QSPI io_base
+ * r8 - fifo start
+ * r9 - fifo end + 1
+
+ * Clobbered:
+ * r4 - rp
+ * r5 - address of QSPI_DR
+ * r7 - tmp
+ * r10 - single 0x0 / dual 0x1
+ */
+
+#include "../../../../src/flash/nor/stmqspi.h"
+
+	.macro	qspi_abort
+	movs	r5, #(1<<SPI_ABORT)			/* abort bit mask */
+	ldr		r7, [r3, #QSPI_CR]			/* get QSPI_CR register */
+	orrs	r7, r7, r5					/* set abort bit */
+	str		r7, [r3, #QSPI_CR]			/* store new CR register */
+	.endm
+
+	.macro	wait_busy
+0:
+	ldr		r7, [r3, #QSPI_SR]			/* load status */
+	lsrs	r7, r7, #(SPI_BUSY+1)		/* shift BUSY into C */
+	bcs		0b							/* loop until BUSY cleared */
+	movs	r7, #(1<<SPI_TCF)			/* TCF bitmask */
+	str		r7, [r3, #QSPI_FCR]			/* clear TCF flag */
+	.endm
+
+start:
+	subs	r0, r0, #1					/* decrement count for DLR */
+	subs	r1, r1, #1					/* page size mask and for DLR */
+	ldr		r4, rp						/* load rp */
+	ldr		r7, [r3, #QSPI_CR]			/* get QSPI_CR register */
+	lsls	r7, r7, #(31-SPI_DUAL_FLASH)	/* clear higher order bits */
+	lsrs	r7, r7, #31					/* DUAL_FLASH bit into bit 0 */
+	mov		r10, r7						/* save in r10 */
+wip_loop:
+	qspi_abort							/* start in clean state */
+	movs	r5, #QSPI_DR				/* load QSPI_DR address offset */
+	adds	r5, r5, r3					/* address of QSPI_DR */
+	wait_busy
+	mov		r7, r10						/* get dual bit */
+	str		r7, [r3, #QSPI_DLR]			/* one or two (for dual) bytes */
+	ldr		r7, ccr_read_status			/* CCR for status read */
+	str		r7, [r3, #QSPI_CCR]			/* initiate status read */
+	ldr		r7, [r3, #QSPI_SR]			/* wait for command startup */
+	ldrb	r7, [r5]					/* get first status register */
+	lsrs	r7, r7, #(SPIFLASH_BSY+1)	/* if first flash busy, */
+	bcs		wip_loop					/* then poll again */
+	mov		r7, r10						/* get dual bit */
+	tst		r7, r7						/* dual mode ? */
+	beq		write_enable				/* not dual, then ok */
+	ldrb	r7, [r5]					/* get second status register */
+	lsrs	r7, r7, #(SPIFLASH_BSY+1)	/* if second flash busy, */
+	bcs		wip_loop					/* then poll again */
+write_enable:
+	tst		r0, r0						/* test residual count */
+	bmi		exit						/* if negative, then finished */
+	wait_busy
+	ldr		r7, ccr_write_enable		/* CCR for write enable */
+	str		r7, [r3, #QSPI_CCR]			/* initiate write enable */
+	wait_busy
+	mov		r7, r10						/* get dual bit */
+	str		r7, [r3, #QSPI_DLR]			/* one or two (for dual) bytes */
+	ldr		r7, ccr_read_status			/* CCR for status read */
+	str		r7, [r3, #QSPI_CCR]			/* initiate status read */
+	ldr		r7, [r3, #QSPI_SR]			/* wait for command startup */
+	ldrb	r7, [r5]					/* get first status register */
+	lsrs	r7, r7, #(SPIFLASH_WE+1)	/* if first flash not */
+	bcc		error						/* write enabled, then error */
+	mov		r7, r10						/* get dual bit */
+	tst		r7, r7						/* dual mode ? */
+	beq		start_write					/* not dual, then ok */
+	ldrb	r7, [r5]					/* get second status register */
+	lsrs	r7, r7, #(SPIFLASH_WE+1)	/* if second flash not */
+	bcc		error						/* write enabled, then error */
+start_write:
+	wait_busy
+	mov		r7, r2						/* get current start address */
+	orrs	r7, r7, r1					/* end of current page */
+	subs	r7, r7, r2					/* count-1 to end of page */
+	cmp		r7, r0						/* if this count <= remaining */
+	bls		write_dlr					/* then write to end of page */
+	mov		r7, r0						/* else write all remaining */
+write_dlr:
+	str		r7, [r3, #QSPI_DLR]			/* size-1 in DLR register */
+	ldr		r7, ccr_page_write			/* CCR for page write */
+	str		r7, [r3, #QSPI_CCR]			/* initiate transfer */
+	str		r2, [r3, #QSPI_AR]			/* store SPI start address */
+	ldr		r7, [r3, #QSPI_SR]			/* wait for command startup */
+write_loop:
+	ldr		r7, wp						/* get wp */
+	cmp		r7, #0						/* if wp equals 0 */
+	beq		exit						/* then abort */
+	cmp		r4, r7						/* check if fifo empty */
+	beq		write_loop					/* wait until not empty */
+	ldrb	r7, [r4, #0]				/* read next byte */
+	strb	r7, [r5]					/* write next byte to DR */
+	adds	r4, r4, #1					/* increment internal rp */
+	cmp		r4, r9						/* internal rp beyond end? */
+	blo		upd_write					/* if no, then ok */
+	mov		r4, r8						/* else wrap around */
+upd_write:
+	adr		r7, rp						/* get address of rp */
+	str		r4, [r7]					/* store updated rp */
+	adds	r2, r2, #1					/* increment address */
+	subs	r0, r0, #1					/* decrement (count-1) */
+	bmi		page_end					/* stop if no data left */
+	tst		r2, r1						/* page end ? */
+	bne		write_loop					/* if not, then next byte */
+page_end:
+	ldr		r7, [r3, #QSPI_SR]			/* load status */
+	lsrs	r7, r7, #(SPI_TCF+1)		/* shift TCF into C */
+	bcc		page_end					/* loop until TCF set */
+	bal		wip_loop					/* then next page */
+
+error:
+	movs	r0, #0						/* return 0xFFFFFFFF */
+	subs	r0, r0, #2					/* for error */
+exit:
+	adds	r0, r0, #1					/* increment count due to the -1 */
+	qspi_abort							/* to idle state */
+
+	.align	2							/* align to word, bkpt is 4 words */
+	bkpt	#0							/* before code end for exit_point */
+	.align	2							/* align to word */
+
+	.space	4							/* not used */
+ccr_read_status:
+	.space	4							/* QSPI_CCR value for READ_STATUS command */
+	.space	4							/* not used */
+	.space	4							/* not used */
+
+	.space	4							/* not used */
+ccr_write_enable:
+	.space	4							/* QSPI_CCR value for WRITE_ENABLE command */
+	.space	4							/* not used */
+	.space	4							/* not used */
+
+	.space	4							/* not used */
+ccr_page_write:
+	.space	4							/* QSPI_CCR value for PAGE_PROG command */
+	.space	4							/* not used */
+	.space	4							/* not used */
+
+	.equ wp, .							/* wp, uint32_t */
+	.equ rp, wp + 4						/* rp, uint32_t */
+	.equ buffer, rp + 4					/* buffer follows right away */
diff --git a/contrib/loaders/flash/stmqspi/stmqspi_write.inc b/contrib/loaders/flash/stmqspi/stmqspi_write.inc
new file mode 100644
index 0000000..6c063c8
--- /dev/null
+++ b/contrib/loaders/flash/stmqspi/stmqspi_write.inc
@@ -0,0 +1,18 @@
+/* Autogenerated with ../../../../src/helper/bin2char.sh */
+0x01,0x38,0x01,0x39,0x41,0x4c,0x1f,0x68,0x7f,0x06,0xff,0x0f,0xba,0x46,0x02,0x25,
+0x1f,0x68,0x2f,0x43,0x1f,0x60,0x20,0x25,0xed,0x18,0x9f,0x68,0xbf,0x09,0xfc,0xd2,
+0x02,0x27,0xdf,0x60,0x57,0x46,0x1f,0x61,0x2c,0x4f,0x5f,0x61,0x9f,0x68,0x2f,0x78,
+0x7f,0x08,0xec,0xd2,0x57,0x46,0x3f,0x42,0x02,0xd0,0x2f,0x78,0x7f,0x08,0xe6,0xd2,
+0x00,0x42,0x41,0xd4,0x9f,0x68,0xbf,0x09,0xfc,0xd2,0x02,0x27,0xdf,0x60,0x27,0x4f,
+0x5f,0x61,0x9f,0x68,0xbf,0x09,0xfc,0xd2,0x02,0x27,0xdf,0x60,0x57,0x46,0x1f,0x61,
+0x1e,0x4f,0x5f,0x61,0x9f,0x68,0x2f,0x78,0xbf,0x08,0x2b,0xd3,0x57,0x46,0x3f,0x42,
+0x02,0xd0,0x2f,0x78,0xbf,0x08,0x25,0xd3,0x9f,0x68,0xbf,0x09,0xfc,0xd2,0x02,0x27,
+0xdf,0x60,0x17,0x46,0x0f,0x43,0xbf,0x1a,0x87,0x42,0x00,0xd9,0x07,0x46,0x1f,0x61,
+0x1a,0x4f,0x5f,0x61,0x9a,0x61,0x9f,0x68,0x1b,0x4f,0x00,0x2f,0x14,0xd0,0xbc,0x42,
+0xfa,0xd0,0x27,0x78,0x2f,0x70,0x01,0x34,0x4c,0x45,0x00,0xd3,0x44,0x46,0x17,0xa7,
+0x3c,0x60,0x01,0x32,0x01,0x38,0x01,0xd4,0x0a,0x42,0xed,0xd1,0x9f,0x68,0xbf,0x08,
+0xfc,0xd3,0xa4,0xe7,0x00,0x20,0x02,0x38,0x01,0x30,0x02,0x25,0x1f,0x68,0x2f,0x43,
+0x1f,0x60,0xc0,0x46,0x00,0xbe,0xc0,0x46,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
diff --git a/doc/openocd.texi b/doc/openocd.texi
index 3a8804d..c30c5bc 100644
--- a/doc/openocd.texi
+++ b/doc/openocd.texi
@@ -30,9 +30,9 @@ of the Open On-Chip Debugger (OpenOCD).
 Permission is granted to copy, distribute and/or modify this document
 under the terms of the GNU Free Documentation License, Version 1.2 or
 any later version published by the Free Software Foundation; with no
-Invariant Sections, with no Front-Cover Texts, and with no Back-Cover
-Texts. A copy of the license is included in the section entitled ``GNU
-Free Documentation License''.
+Invariant Sections, no Front-Cover Texts, and no Back-Cover Texts. A
+copy of the license is included in the section entitled ``GNU Free
+Documentation License''.
 @end quotation
 @end copying
 
@@ -79,7 +79,6 @@ Free Documentation License''.
 * JTAG Commands::                    JTAG Commands
 * Boundary Scan Commands::           Boundary Scan Commands
 * Utility Commands::                 Utility Commands
-* TFTP::                             TFTP
 * GDB and OpenOCD::                  Using GDB and OpenOCD
 * Tcl Scripting API::                Tcl Scripting API
 * FAQ::                              Frequently Asked Questions
@@ -624,6 +623,13 @@ emulation model of target hardware.
 @item @b{xlnx_pcie_xvc}
 @* A JTAG driver exposing Xilinx Virtual Cable over PCI Express to OpenOCD as JTAG/SWD interface.
 
+@item @b{linuxgpiod}
+@* A bitbang JTAG driver using Linux GPIO through library libgpiod.
+
+@item @b{sysfsgpio}
+@* A bitbang JTAG driver using Linux legacy sysfs GPIO.
+This is deprecated from Linux v5.3; prefer using @b{linuxgpiod}.
+
 @end itemize
 
 @node About Jim-Tcl
@@ -2430,7 +2436,8 @@ and a specific set of GPIOs is used.
 @end deffn
 
 @deffn {Interface Driver} {cmsis-dap}
-ARM CMSIS-DAP compliant based adapter.
+ARM CMSIS-DAP compliant based adapter v1 (USB HID based)
+or v2 (USB bulk).
 
 @deffn {Config Command} {cmsis_dap_vid_pid} [vid pid]+
 The vendor ID and product ID of the CMSIS-DAP device. If not specified
@@ -2446,6 +2453,23 @@ Specifies the @var{serial} of the CMSIS-DAP device to use.
 If not specified, serial numbers are not considered.
 @end deffn
 
+@deffn {Config Command} {cmsis_dap_backend} [@option{auto}|@option{usb_bulk}|@option{hid}]
+Specifies how to communicate with the adapter:
+
+@itemize @minus
+@item @option{hid} Use HID generic reports - CMSIS-DAP v1
+@item @option{usb_bulk} Use USB bulk - CMSIS-DAP v2
+@item @option{auto} First try USB bulk CMSIS-DAP v2, if not found try HID CMSIS-DAP v1.
+This is the default if @command{cmsis_dap_backend} is not specified.
+@end itemize
+@end deffn
+
+@deffn {Config Command} {cmsis_dap_usb interface} [number]
+Specifies the @var{number} of the USB interface to use in v2 mode (USB bulk).
+In most cases need not to be specified and interfaces are searched by
+interface string or for user class interface.
+@end deffn
+
 @deffn {Command} {cmsis-dap info}
 Display various device information, like hardware version, firmware version, current bus status.
 @end deffn
@@ -2881,8 +2905,8 @@ The following example shows how to read 4 bytes from the EMUCOM channel 0x0:
 @deffn {Config} {jlink usb} <@option{0} to @option{3}>
 Set the USB address of the interface, in case more than one adapter is connected
 to the host. If not specified, USB addresses are not considered. Device
-selection via USB address is deprecated and the serial number should be used
-instead.
+selection via USB address is not always unambiguous. It is recommended to use
+the serial number instead, if possible.
 
 As a configuration command, it can be used only before 'init'.
 @end deffn
@@ -3223,6 +3247,22 @@ pinout.
 @end deffn
 
 
+@deffn {Interface Driver} {linuxgpiod}
+Linux provides userspace access to GPIO through libgpiod since Linux kernel version v4.6.
+The driver emulates either JTAG and SWD transport through bitbanging.
+
+See @file{interface/dln-2-gpiod.cfg} for a sample config.
+@end deffn
+
+
+@deffn {Interface Driver} {sysfsgpio}
+Linux legacy userspace access to GPIO through sysfs is deprecated from Linux kernel version v5.3.
+Prefer using @b{linuxgpiod}, instead.
+
+See @file{interface/sysfsgpio-raspberrypi.cfg} for a sample config.
+@end deffn
+
+
 @deffn {Interface Driver} {openjtag}
 OpenJTAG compatible USB adapter.
 This defines some driver-specific commands:
@@ -4694,6 +4734,11 @@ possible values of the parameter @var{number}, which are not only numeric values
 Use this option to override, for this target only, the global parameter set with
 command @command{gdb_port}.
 @xref{gdb_port,,command gdb_port}.
+
+@item @code{-gdb-max-connections} @var{number} -- EXPERIMENTAL: set the maximum
+number of GDB connections that are allowed for the target. Default is 1.
+A negative value for @var{number} means unlimited connections.
+See @xref{gdbmeminspect,,Using GDB as a non-intrusive memory inspector}.
 @end itemize
 @end deffn
 
@@ -5249,6 +5294,18 @@ it has been removed by the @option{unlock} flag.
 
 @end deffn
 
+@deffn Command {flash verify_image} filename [offset] [type]
+Verify the image @file{filename} to the current target's flash bank(s).
+Parameters follow the description of 'flash write_image'.
+In contrast to the 'verify_image' command, for banks with specific
+verify method, that one is used instead of the usual target's read
+memory methods. This is necessary for flash banks not readable by
+ordinary memory reads.
+This command gives only an overall good/bad result for each bank, not
+addresses of individual failed bytes as it's intended only as quick
+check for successful programming.
+@end deffn
+
 @section Other Flash commands
 @cindex flash protection
 
@@ -5507,6 +5564,117 @@ flash bank $_FLASHNAME stmsmi 0xf8000000 0 0 0 $_TARGETNAME
 
 @end deffn
 
+@deffn {Flash Driver} stmqspi
+@cindex STMicroelectronics QuadSPI/OctoSPI Interface
+@cindex QuadSPI
+@cindex OctoSPI
+@cindex stmqspi
+Some devices from STMicroelectronics include a proprietary ``QuadSPI Interface''
+(e.g. STM32F4, STM32F7, STM32L4) or ``OctoSPI Interface'' (e.g. STM32L4+)
+controller able to drive one or even two (dual mode) external SPI flash devices.
+The OctoSPI is a superset of QuadSPI, its presence is detected automatically.
+Currently only the regular command mode is supported, whereas the HyperFlash
+mode is not.
+
+QuadSPI/OctoSPI makes the flash contents directly accessible in the CPU address
+space; in case of dual mode both devices must be of the same type and are
+mapped in the same memory bank (even and odd addresses interleaved).
+CPU can directly read data, execute code (but not boot) from QuadSPI bank.
+
+The 'flash bank' command only requires the @var{base} parameter and the extra
+parameter @var{io_base} in order to identify the memory bank. Both are fixed
+by hardware, see datasheet or RM. All other parameters are ignored.
+
+The controller must be initialized after each reset and properly configured
+for memory-mapped read operation for the particular flash chip(s), for the full
+list of available register settings cf. the controller's RM. This setup is quite
+board specific (that's why booting from this memory is not possible). The
+flash driver infers all parameters from current controller register values when
+'flash probe @var{bank_id}' is executed.
+
+Normal OpenOCD commands like @command{mdw} can be used to display the flash content,
+but only after proper controller initialization as decribed above. However,
+due to a silicon bug in some devices, attempting to access the very last word
+should be avoided.
+
+It is possible to use two (even different) flash chips alternatingly, if individual
+bank chip selects are available. For some package variants, this is not the case
+due to limited pin count. To switch from one to another, adjust FSEL bit accordingly
+and re-issue 'flash probe bank_id'. Note that the bank base address will @emph{not}
+change, so the address spaces of both devices will overlap. In dual flash mode
+both chips must be identical regarding size and most other properties.
+
+Block or sector protection internal to the flash chip is not handled by this
+driver at all, but can be dealt with manually by the 'cmd' command, see below.
+The sector protection via 'flash protect' command etc. is completely internal to
+openocd, intended only to prevent accidental erase or overwrite and it does not
+persist across openocd invocations.
+
+OpenOCD contains a hardcoded list of flash devices with their properties,
+these are auto-detected. If a device is not included in this list, SFDP discovery
+is attempted. If this fails or gives inappropriate results, manual setting is
+required (see 'set' command).
+
+@example
+flash bank $_FLASHNAME stmqspi 0x90000000 0 0 0 $_TARGETNAME 0xA0001000
+flash bank $_FLASHNAME stmqspi 0x70000000 0 0 0 $_TARGETNAME 0xA0001400
+@end example
+
+There are three specific commands
+@deffn Command {stmqspi mass_erase} bank_id
+Clears sector protections and performs a mass erase. Works only if there is no
+chip specific write protection engaged.
+@end deffn
+
+@deffn Command {stmqspi set} bank_id name total_size page_size read_cmd fread_cmd pprg_cmd mass_erase_cmd sector_size sector_erase_cmd
+Set flash parameters: @var{name} human readable string, @var{total_size} size
+in bytes, @var{page_size} is write page size. @var{read_cmd}, @var{fread_cmd} and @var{pprg_cmd}
+are commands for reading and page programming. @var{fread_cmd} is used in DPI and QPI modes,
+@var{read_cmd} in normal SPI (single line) mode. @var{mass_erase_cmd}, @var{sector_size}
+and @var{sector_erase_cmd} are optional.
+
+This command is required if chip id is not hardcoded yet and e.g. for EEPROMs or FRAMs
+which don't support an id command.
+
+In dual mode parameters of both chips are set identically. The parameters refer to
+a single chip, so the whole bank gets twice the specified capacity etc.
+@end deffn
+
+@deffn Command {stmqspi cmd} bank_id resp_num cmd_byte ...
+If @var{resp_num} is zero, sends command @var{cmd_byte} and following data
+bytes. In dual mode command byte is sent to @emph{both} chips but data bytes are
+sent @emph{alternatingly} to chip 1 and 2, first to flash 1, second to flash 2, etc.,
+i.e. the total number of bytes (including cmd_byte) must be odd.
+
+If @var{resp_num} is not zero, cmd and at most four following data bytes are
+sent, in dual mode @emph{simultaneously} to both chips. Then @var{resp_num} bytes
+are read interleaved from both chips starting with chip 1. In this case
+@var{resp_num} must be even.
+
+Note the hardware dictated subtle difference of those two cases in dual-flash mode.
+
+To check basic communication settings, issue
+@example
+stmqspi cmd bank_id 0 0x04; stmqspi cmd bank_id 1 0x05; stmqspi cmd bank_id 0 0x06; stmqspi cmd bank_id 1 0x05
+@end example
+for single flash mode or
+@example
+stmqspi cmd bank_id 0 0x04; stmqspi cmd bank_id 2 0x05; stmqspi cmd bank_id 0 0x06; stmqspi cmd bank_id 2 0x05
+@end example
+for dual flash mode. This should return the status register contents.
+
+In 8-line mode, @var{cmd_byte} is sent twice - first time as given, second time
+complemented. Additionally, in 8-line mode only, some commands (e.g. Read Status)
+need a dummy address, e.g.
+@example
+stmqspi cmd bank_id 1 0x05 0x00 0x00 0x00 0x00
+@end example
+should return the status register contents.
+
+@end deffn
+
+@end deffn
+
 @deffn {Flash Driver} mrvlqspi
 This driver supports QSPI flash controller of Marvell's Wireless
 Microcontroller platform.
@@ -6989,10 +7157,9 @@ The @var{num} parameter is a value shown by @command{flash banks}.
 @end deffn
 
 @deffn {Flash Driver} stm32l4x
-All members of the STM32L4, STM32L4+, STM32WB, STM32WL and STM32G4
+All members of the STM32 G0, G4, L4, L4+, L5, WB and WL
 microcontroller families from STMicroelectronics include internal flash
-and use ARM Cortex-M4 cores.
-Additionally this driver supports STM32G0 family with ARM Cortex-M0+ core.
+and use ARM Cortex-M0+, M4 and M33 cores.
 The driver automatically recognizes a number of these chips using
 the chip identification register, and autoconfigures itself.
 
@@ -8279,6 +8446,94 @@ the watchpoint should trigger. The value may be first be masked
 using @var{mask} to mark ``don't care'' fields.
 @end deffn
 
+
+@section Real Time Transfer (RTT)
+
+Real Time Transfer (RTT) is an interface specified by SEGGER based on basic
+memory reads and writes to transfer data bidirectionally between target and host.
+The specification is independent of the target architecture.
+Every target that supports so called "background memory access", which means
+that the target memory can be accessed by the debugger while the target is
+running, can be used.
+This interface is especially of interest for targets without
+Serial Wire Output (SWO), such as ARM Cortex-M0, or where semihosting is not
+applicable because of real-time constraints.
+
+@quotation Note
+The current implementation supports only single target devices.
+@end quotation
+
+The data transfer between host and target device is organized through
+unidirectional up/down-channels for target-to-host and host-to-target
+communication, respectively.
+
+@quotation Note
+The current implementation does not respect channel buffer flags.
+They are used to determine what happens when writing to a full buffer, for
+example.
+@end quotation
+
+Channels are exposed via raw TCP/IP connections. One or more RTT servers can be
+assigned to each channel to make them accessible to an unlimited number
+of TCP/IP connections.
+
+@deffn Command {rtt setup} address size ID
+Configure RTT for the currently selected target.
+Once RTT is started, OpenOCD searches for a control block with the
+identifier @var{ID} starting at the memory address @var{address} within the next
+@var{size} bytes.
+@end deffn
+
+@deffn Command {rtt start}
+Start RTT.
+If the control block location is not known, OpenOCD starts searching for it.
+@end deffn
+
+@deffn Command {rtt stop}
+Stop RTT.
+@end deffn
+
+@deffn Command {rtt polling_interval [interval]}
+Display the polling interval.
+If @var{interval} is provided, set the polling interval.
+The polling interval determines (in milliseconds) how often the up-channels are
+checked for new data.
+@end deffn
+
+@deffn Command {rtt channels}
+Display a list of all channels and their properties.
+@end deffn
+
+@deffn Command {rtt channellist}
+Return a list of all channels and their properties as Tcl list.
+The list can be manipulated easily from within scripts.
+@end deffn
+
+@deffn Command {rtt server start} port channel
+Start a TCP server on @var{port} for the channel @var{channel}.
+@end deffn
+
+@deffn Command {rtt server stop} port
+Stop the TCP sever with port @var{port}.
+@end deffn
+
+The following example shows how to setup RTT using the SEGGER RTT implementation
+on the target device.
+
+@example
+resume
+
+rtt setup 0x20000000 2048 "SEGGER RTT"
+rtt start
+
+rtt server start 9090 0
+@end example
+
+In this example, OpenOCD searches the control block with the ID "SEGGER RTT"
+starting at 0x20000000 for 2048 bytes. The RTT channel 0 is exposed through the
+TCP/IP port 9090.
+
+
 @section Misc Commands
 
 @cindex profiling
@@ -8593,7 +8848,7 @@ CTI is mandatory for core run control and each core has an individual
 CTI instance attached to it. OpenOCD has limited support for CTI using
 the @emph{cti} group of commands.
 
-@deffn Command {cti create} cti_name @option{-dap} dap_name @option{-ap-num} apn @option{-ctibase} base_address
+@deffn Command {cti create} cti_name @option{-dap} dap_name @option{-ap-num} apn @option{-baseaddr} base_address
 Creates a CTI instance @var{cti_name} on the DAP instance @var{dap_name} on MEM-AP
 @var{apn}. The @var{base_address} must match the base address of the CTI
 on the respective MEM-AP. All arguments are mandatory. This creates a
@@ -9201,7 +9456,7 @@ Selects whether interrupts will be processed when single stepping
 @cindex ITM
 @cindex ETM
 
-@deffn Command {tpiu config} (@option{disable} | ((@option{external} | @option{internal (@var{filename} | -)}) @
+@deffn Command {tpiu config} (@option{disable} | ((@option{external} | @option{internal (@var{filename} | @var{:port} | -)}) @
                (@option{sync @var{port_width}} | ((@option{manchester} | @option{uart}) @var{formatter_enable})) @
                @var{TRACECLKIN_freq} [@var{trace_freq}]))
 
@@ -9221,23 +9476,28 @@ Command options:
 @itemize @minus
 @item @option{disable} disable TPIU handling;
 @item @option{external} configure TPIU to let user capture trace
-output externally (with an additional UART or logic analyzer hardware);
-@item @option{internal @var{filename}} configure TPIU and debug adapter to
-gather trace data and append it to @var{filename} (which can be
-either a regular file or a named pipe);
-@item @option{internal -} configure TPIU and debug adapter to
-gather trace data, but not write to any file. Useful in conjunction with the @command{tcl_trace} command;
+output externally (with an additional UART or logic analyzer hardware).
+@item @option{internal (@var{filename} | @var{:port} | -)} configure TPIU and debug adapter to
+gather trace data then:
+
+@itemize @minus
+@item append it to a regular file or a named pipe if @var{filename} is specified.
+@item listen to a TCP/IP port if @var{:port} is specified, then broadcast the trace data over this port.
+@item if '-' is specified, OpenOCD will forward trace data to @command{tcl_trace} command.
+@*@b{Note:} while broadcasting to file or TCP, the forwarding to @command{tcl_trace} will remain active.
+@end itemize
+
 @item @option{sync @var{port_width}} use synchronous parallel trace output
-mode, and set port width to @var{port_width};
+mode, and set port width to @var{port_width}.
 @item @option{manchester} use asynchronous SWO mode with Manchester
-coding;
+coding.
 @item @option{uart} use asynchronous SWO mode with NRZ (same as
-regular UART 8N1) coding;
+regular UART 8N1) coding.
 @item @var{formatter_enable} is @option{on} or @option{off} to enable
 or disable TPIU formatter which needs to be used when both ITM and ETM
-data is to be output via SWO;
+data is to be output via SWO.
 @item @var{TRACECLKIN_freq} this should be specified to match target's
-current TRACECLKIN frequency (usually the same as HCLK);
+current TRACECLKIN frequency (usually the same as HCLK).
 @item @var{trace_freq} trace port frequency. Can be omitted in
 internal mode to let the adapter driver select the maximum supported
 rate automatically.
@@ -10411,28 +10671,6 @@ For quickstart instructions run:
 openocd -f tools/firmware-recovery.tcl -c firmware_help
 @end example
 
-@node TFTP
-@chapter TFTP
-@cindex TFTP
-If OpenOCD runs on an embedded host (as ZY1000 does), then TFTP can
-be used to access files on PCs (either the developer's PC or some other PC).
-
-The way this works on the ZY1000 is to prefix a filename by
-"/tftp/ip/" and append the TFTP path on the TFTP
-server (tftpd). For example,
-
-@example
-load_image /tftp/10.0.0.96/c:\temp\abc.elf
-@end example
-
-will load c:\temp\abc.elf from the developer pc (10.0.0.96) into memory as
-if the file was hosted on the embedded host.
-
-In order to achieve decent performance, you must choose a TFTP server
-that supports a packet size bigger than the default packet size (512 bytes). There
-are numerous TFTP servers out there (free and commercial) and you will have to do
-a bit of googling to find something that fits your requirements.
-
 @node GDB and OpenOCD
 @chapter GDB and OpenOCD
 @cindex GDB
@@ -10656,7 +10894,13 @@ of a running target. Do not use GDB commands @command{continue},
 and GDB would require stopping the target to get the prompt back.
 
 Do not use this mode under an IDE like Eclipse as it caches values of
-previously shown varibles.
+previously shown variables.
+
+It's also possible to connect more than one GDB to the same target by the
+target's configuration option @code{-gdb-max-connections}. This allows, for
+example, one GDB to run a script that continuously polls a set of variables
+while other GDB can be used interactively. Be extremely careful in this case,
+because the two GDB can easily get out-of-sync.
 
 @section RTOS Support
 @cindex RTOS Support
diff --git a/src/Makefile.am b/src/Makefile.am
index 07981aa..781c1e7 100644
--- a/src/Makefile.am
+++ b/src/Makefile.am
@@ -53,7 +53,8 @@ endif
 	%D%/target/libtarget.la \
 	%D%/server/libserver.la \
 	%D%/rtos/librtos.la \
-	%D%/helper/libhelper.la
+	%D%/helper/libhelper.la \
+	%D%/rtt/librtt.la
 
 BIN2C = $(srcdir)/%D%/helper/bin2char.sh
 
@@ -83,3 +84,4 @@ include %D%/rtos/Makefile.am
 include %D%/server/Makefile.am
 include %D%/flash/Makefile.am
 include %D%/pld/Makefile.am
+include %D%/rtt/Makefile.am
diff --git a/src/flash/nand/mxc.c b/src/flash/nand/mxc.c
index 2c5de03..9002707 100644
--- a/src/flash/nand/mxc.c
+++ b/src/flash/nand/mxc.c
@@ -860,7 +860,7 @@ static int validate_target_state(struct nand_device *nand)
 	return ERROR_OK;
 }
 
-int ecc_status_v1(struct nand_device *nand)
+static int ecc_status_v1(struct nand_device *nand)
 {
 	struct mxc_nf_controller *mxc_nf_info = nand->controller_priv;
 	struct target *target = nand->target;
@@ -886,7 +886,7 @@ int ecc_status_v1(struct nand_device *nand)
 	return ERROR_OK;
 }
 
-int ecc_status_v2(struct nand_device *nand)
+static int ecc_status_v2(struct nand_device *nand)
 {
 	struct mxc_nf_controller *mxc_nf_info = nand->controller_priv;
 	struct target *target = nand->target;
diff --git a/src/flash/nor/Makefile.am b/src/flash/nor/Makefile.am
index 16e43a0..771f21f 100644
--- a/src/flash/nor/Makefile.am
+++ b/src/flash/nor/Makefile.am
@@ -52,10 +52,12 @@ NOR_DRIVERS = \
 	%D%/psoc5lp.c \
 	%D%/psoc6.c \
 	%D%/renesas_rpchf.c \
+	%D%/sfdp.c \
 	%D%/sh_qspi.c \
 	%D%/sim3x.c \
 	%D%/spi.c \
 	%D%/stmsmi.c \
+	%D%/stmqspi.c \
 	%D%/stellaris.c \
 	%D%/stm32f1x.c \
 	%D%/stm32f2x.c \
@@ -84,6 +86,8 @@ NORHEADERS = \
 	%D%/imp.h \
 	%D%/non_cfi.h \
 	%D%/ocl.h \
+	%D%/sfdp.h \
 	%D%/spi.h \
 	%D%/stm32l4x.h \
+	%D%/stmqspi.h \
 	%D%/msp432.h
diff --git a/src/flash/nor/at91sam3.c b/src/flash/nor/at91sam3.c
index c9baffc..9c4afd4 100644
--- a/src/flash/nor/at91sam3.c
+++ b/src/flash/nor/at91sam3.c
@@ -3099,7 +3099,7 @@ FLASH_BANK_COMMAND_HANDLER(sam3_flash_bank_command)
  * is owned by this bank. This simplification works only for one shot
  * deallocation like current flash_free_all_banks()
  */
-void sam3_free_driver_priv(struct flash_bank *bank)
+static void sam3_free_driver_priv(struct flash_bank *bank)
 {
 	struct sam3_chip *chip = all_sam3_chips;
 	while (chip) {
diff --git a/src/flash/nor/atsamv.c b/src/flash/nor/atsamv.c
index af48398..8f1450b 100644
--- a/src/flash/nor/atsamv.c
+++ b/src/flash/nor/atsamv.c
@@ -676,6 +676,9 @@ showall:
 		}
 		if ((who >= 0) && (((unsigned)who) < SAMV_NUM_GPNVM_BITS)) {
 			r = samv_get_gpnvm(target, who, &v);
+			if (r != ERROR_OK)
+				return r;
+
 			command_print(CMD, "samv-gpnvm%u: %u", who, v);
 			return r;
 		} else {
diff --git a/src/flash/nor/bluenrg-x.c b/src/flash/nor/bluenrg-x.c
index 57ea739..57aebc5 100644
--- a/src/flash/nor/bluenrg-x.c
+++ b/src/flash/nor/bluenrg-x.c
@@ -43,7 +43,7 @@ struct flash_ctrl_priv_data {
 	char *part_name;
 };
 
-const struct flash_ctrl_priv_data flash_priv_data_1 = {
+static const struct flash_ctrl_priv_data flash_priv_data_1 = {
 	.die_id_reg = 0x4090001C,
 	.jtag_idcode_reg = 0x40900028,
 	.flash_base = 0x10040000,
@@ -53,7 +53,7 @@ const struct flash_ctrl_priv_data flash_priv_data_1 = {
 	.part_name = "BLUENRG-1",
 };
 
-const struct flash_ctrl_priv_data flash_priv_data_2 = {
+static const struct flash_ctrl_priv_data flash_priv_data_2 = {
 	.die_id_reg = 0x4090001C,
 	.jtag_idcode_reg = 0x40900028,
 	.flash_base = 0x10040000,
@@ -63,7 +63,7 @@ const struct flash_ctrl_priv_data flash_priv_data_2 = {
 	.part_name = "BLUENRG-2",
 };
 
-const struct flash_ctrl_priv_data flash_priv_data_lp = {
+static const struct flash_ctrl_priv_data flash_priv_data_lp = {
 	.die_id_reg = 0x40000000,
 	.jtag_idcode_reg = 0x40000004,
 	.flash_base = 0x10040000,
@@ -79,7 +79,11 @@ struct bluenrgx_flash_bank {
 	const struct flash_ctrl_priv_data *flash_ptr;
 };
 
-const struct flash_ctrl_priv_data *flash_ctrl[] = {&flash_priv_data_1, &flash_priv_data_2, &flash_priv_data_lp};
+static const struct flash_ctrl_priv_data *flash_ctrl[] = {
+	&flash_priv_data_1,
+	&flash_priv_data_2,
+	&flash_priv_data_lp
+};
 
 /* flash_bank bluenrg-x 0 0 0 0 <target#> */
 FLASH_BANK_COMMAND_HANDLER(bluenrgx_flash_bank_command)
diff --git a/src/flash/nor/core.c b/src/flash/nor/core.c
index d563013..c162c70 100644
--- a/src/flash/nor/core.c
+++ b/src/flash/nor/core.c
@@ -94,7 +94,7 @@ int flash_driver_protect(struct flash_bank *bank, int set, unsigned int first,
 }
 
 int flash_driver_write(struct flash_bank *bank,
-	uint8_t *buffer, uint32_t offset, uint32_t count)
+	const uint8_t *buffer, uint32_t offset, uint32_t count)
 {
 	int retval;
 
@@ -135,6 +135,43 @@ int default_flash_read(struct flash_bank *bank,
 	return target_read_buffer(bank->target, offset + bank->base, count, buffer);
 }
 
+int flash_driver_verify(struct flash_bank *bank,
+	const uint8_t *buffer, uint32_t offset, uint32_t count)
+{
+	int retval;
+
+	retval = bank->driver->verify ? bank->driver->verify(bank, buffer, offset, count) :
+		default_flash_verify(bank, buffer, offset, count);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("verify failed in bank at " TARGET_ADDR_FMT " starting at 0x%8.8" PRIx32,
+			bank->base, offset);
+	}
+
+	return retval;
+}
+
+int default_flash_verify(struct flash_bank *bank,
+	const uint8_t *buffer, uint32_t offset, uint32_t count)
+{
+	uint32_t target_crc, image_crc;
+	int retval;
+
+	retval = image_calculate_checksum(buffer, count, &image_crc);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = target_checksum_memory(bank->target, offset + bank->base, count, &target_crc);
+	if (retval != ERROR_OK)
+		return retval;
+
+	LOG_DEBUG("addr " TARGET_ADDR_FMT ", len 0x%08" PRIx32 ", crc 0x%08" PRIx32 " 0x%08" PRIx32,
+		offset + bank->base, count, ~image_crc, ~target_crc);
+	if (target_crc == image_crc)
+		return ERROR_OK;
+	else
+		return ERROR_FAIL;
+}
+
 void flash_bank_add(struct flash_bank *bank)
 {
 	/* put flash bank in linked list */
@@ -697,12 +734,12 @@ static bool flash_write_check_gap(struct flash_bank *bank,
 }
 
 
-int flash_write_unlock(struct target *target, struct image *image,
-	uint32_t *written, bool erase, bool unlock)
+int flash_write_unlock_verify(struct target *target, struct image *image,
+	uint32_t *written, bool erase, bool unlock, bool write, bool verify)
 {
 	int retval = ERROR_OK;
 
-	int section;
+	unsigned int section;
 	uint32_t section_offset;
 	struct flash_bank *c;
 	int *padding;
@@ -727,8 +764,8 @@ int flash_write_unlock(struct target *target, struct image *image,
 	 * whereas an image can have sections out of order. */
 	struct imagesection **sections = malloc(sizeof(struct imagesection *) *
 			image->num_sections);
-	int i;
-	for (i = 0; i < image->num_sections; i++)
+
+	for (unsigned int i = 0; i < image->num_sections; i++)
 		sections[i] = &image->sections[i];
 
 	qsort(sections, image->num_sections, sizeof(struct imagesection *),
@@ -738,7 +775,7 @@ int flash_write_unlock(struct target *target, struct image *image,
 	while (section < image->num_sections) {
 		uint32_t buffer_idx;
 		uint8_t *buffer;
-		int section_last;
+		unsigned int section_last;
 		target_addr_t run_address = sections[section]->base_address + section_offset;
 		uint32_t run_size = sections[section]->size - section_offset;
 		int pad_bytes = 0;
@@ -932,8 +969,17 @@ int flash_write_unlock(struct target *target, struct image *image,
 		}
 
 		if (retval == ERROR_OK) {
-			/* write flash sectors */
-			retval = flash_driver_write(c, buffer, run_address - c->base, run_size);
+			if (write) {
+				/* write flash sectors */
+				retval = flash_driver_write(c, buffer, run_address - c->base, run_size);
+			}
+		}
+
+		if (retval == ERROR_OK) {
+			if (verify) {
+				/* verify flash sectors */
+				retval = flash_driver_verify(c, buffer, run_address - c->base, run_size);
+			}
 		}
 
 		free(buffer);
@@ -957,7 +1003,7 @@ done:
 int flash_write(struct target *target, struct image *image,
 	uint32_t *written, bool erase)
 {
-	return flash_write_unlock(target, image, written, erase, false);
+	return flash_write_unlock_verify(target, image, written, erase, false, true, false);
 }
 
 struct flash_sector *alloc_block_array(uint32_t offset, uint32_t size,
diff --git a/src/flash/nor/core.h b/src/flash/nor/core.h
index 163e578..107a1c5 100644
--- a/src/flash/nor/core.h
+++ b/src/flash/nor/core.h
@@ -200,6 +200,7 @@ void default_flash_free_driver_priv(struct flash_bank *bank);
 
 /** Deallocates all flash banks */
 void flash_free_all_banks(void);
+
 /**
  * Provides default read implementation for flash memory.
  * @param bank The bank to read.
@@ -210,6 +211,18 @@ void flash_free_all_banks(void);
  */
 int default_flash_read(struct flash_bank *bank,
 		uint8_t *buffer, uint32_t offset, uint32_t count);
+
+/**
+ * Provides default verify implementation for flash memory.
+ * @param bank The bank to verify.
+ * @param buffer The data bytes to verify.
+ * @param offset The offset into the chip to verify.
+ * @param count The number of bytes to verify.
+ * @returns ERROR_OK if successful; otherwise, an error code.
+ */
+int default_flash_verify(struct flash_bank *bank,
+		const uint8_t *buffer, uint32_t offset, uint32_t count);
+
 /**
  * Provides default erased-bank check handling. Checks to see if
  * the flash driver knows they are erased; if things look uncertain,
@@ -217,7 +230,6 @@ int default_flash_read(struct flash_bank *bank,
  * @returns ERROR_OK if successful; otherwise, an error code.
  */
 int default_flash_blank_check(struct flash_bank *bank);
-
 /**
  * Returns the flash bank specified by @a name, which matches the
  * driver name and a suffix (option) specify the driver-specific
diff --git a/src/flash/nor/driver.h b/src/flash/nor/driver.h
index 7f66047..e29d4f5 100644
--- a/src/flash/nor/driver.h
+++ b/src/flash/nor/driver.h
@@ -156,6 +156,20 @@ struct flash_driver {
 			uint8_t *buffer, uint32_t offset, uint32_t count);
 
 	/**
+	 * Verify data in flash.  Note CPU address will be
+	 * "bank->base + offset", while the physical address is
+	 * dependent upon current target MMU mappings.
+	 *
+	 * @param bank The bank to verify
+	 * @param buffer The data bytes to verify against.
+	 * @param offset The offset into the chip to verify.
+	 * @param count The number of bytes to verify.
+	 * @returns ERROR_OK if successful; otherwise, an error code.
+	 */
+	int (*verify)(struct flash_bank *bank,
+			const uint8_t *buffer, uint32_t offset, uint32_t count);
+
+	/**
 	 * Probe to determine what kind of flash is present.
 	 * This is invoked by the "probe" script command.
 	 *
diff --git a/src/flash/nor/drivers.c b/src/flash/nor/drivers.c
index 8c8f13e..047b5f7 100644
--- a/src/flash/nor/drivers.c
+++ b/src/flash/nor/drivers.c
@@ -44,6 +44,7 @@ extern const struct flash_driver faux_flash;
 extern const struct flash_driver fm3_flash;
 extern const struct flash_driver fm4_flash;
 extern const struct flash_driver fespi_flash;
+extern const struct flash_driver gd32vf103_flash;
 extern const struct flash_driver jtagspi_flash;
 extern const struct flash_driver kinetis_flash;
 extern const struct flash_driver kinetis_ke_flash;
@@ -75,7 +76,7 @@ extern const struct flash_driver stm32f2x_flash;
 extern const struct flash_driver stm32lx_flash;
 extern const struct flash_driver stm32l4x_flash;
 extern const struct flash_driver stm32h7x_flash;
-extern const struct flash_driver gd32vf103_flash;
+extern const struct flash_driver stmqspi_flash;
 extern const struct flash_driver stmsmi_flash;
 extern const struct flash_driver str7x_flash;
 extern const struct flash_driver str9x_flash;
@@ -150,6 +151,7 @@ static const struct flash_driver * const flash_drivers[] = {
 	&stm32h7x_flash,
 	&gd32vf103_flash,
 	&stmsmi_flash,
+	&stmqspi_flash,
 	&str7x_flash,
 	&str9x_flash,
 	&str9xpec_flash,
diff --git a/src/flash/nor/imp.h b/src/flash/nor/imp.h
index 06fb2a2..f66cf03 100644
--- a/src/flash/nor/imp.h
+++ b/src/flash/nor/imp.h
@@ -42,12 +42,14 @@ int flash_driver_erase(struct flash_bank *bank, unsigned int first,
 int flash_driver_protect(struct flash_bank *bank, int set, unsigned int first,
 		unsigned int last);
 int flash_driver_write(struct flash_bank *bank,
-		uint8_t *buffer, uint32_t offset, uint32_t count);
+		const uint8_t *buffer, uint32_t offset, uint32_t count);
 int flash_driver_read(struct flash_bank *bank,
 		uint8_t *buffer, uint32_t offset, uint32_t count);
+int flash_driver_verify(struct flash_bank *bank,
+		const uint8_t *buffer, uint32_t offset, uint32_t count);
 
 /* write (optional verify) an image to flash memory of the given target */
-int flash_write_unlock(struct target *target, struct image *image,
-		uint32_t *written, bool erase, bool unlock);
+int flash_write_unlock_verify(struct target *target, struct image *image,
+		uint32_t *written, bool erase, bool unlock, bool write, bool verify);
 
 #endif /* OPENOCD_FLASH_NOR_IMP_H */
diff --git a/src/flash/nor/kinetis_ke.c b/src/flash/nor/kinetis_ke.c
index 349b256..5aba7fc 100644
--- a/src/flash/nor/kinetis_ke.c
+++ b/src/flash/nor/kinetis_ke.c
@@ -434,7 +434,7 @@ static int kinetis_ke_prepare_flash(struct flash_bank *bank)
 	return ERROR_OK;
 }
 
-int kinetis_ke_stop_watchdog(struct target *target)
+static int kinetis_ke_stop_watchdog(struct target *target)
 {
 	struct working_area *watchdog_algorithm;
 	struct armv7m_algorithm armv7m_info;
diff --git a/src/flash/nor/lpc2000.c b/src/flash/nor/lpc2000.c
index 942ef55..3ad62d6 100644
--- a/src/flash/nor/lpc2000.c
+++ b/src/flash/nor/lpc2000.c
@@ -1103,9 +1103,9 @@ static int lpc2000_write(struct flash_bank *bank, const uint8_t *buffer, uint32_
 
 		uint32_t original_value = buf_get_u32(buffer + (lpc2000_info->checksum_vector * 4), 0, 32);
 		if (original_value != checksum) {
-			LOG_WARNING("Verification will fail since checksum in image (0x%8.8" PRIx32 ") to be written to flash is "
+			LOG_WARNING("Boot verification checksum in image (0x%8.8" PRIx32 ") to be written to flash is "
 					"different from calculated vector checksum (0x%8.8" PRIx32 ").", original_value, checksum);
-			LOG_WARNING("To remove this warning modify build tools on developer PC to inject correct LPC vector "
+			LOG_WARNING("OpenOCD will write the correct checksum. To remove this warning modify build tools on developer PC to inject correct LPC vector "
 					"checksum.");
 		}
 
diff --git a/src/flash/nor/lpc2900.c b/src/flash/nor/lpc2900.c
index c8e885a..6596cde 100644
--- a/src/flash/nor/lpc2900.c
+++ b/src/flash/nor/lpc2900.c
@@ -635,9 +635,9 @@ COMMAND_HANDLER(lpc2900_handle_write_custom_command)
 
 	/* The image will always start at offset 0 */
 	struct image image;
-	image.base_address_set = 1;
+	image.base_address_set = true;
 	image.base_address = 0;
-	image.start_address_set = 0;
+	image.start_address_set = false;
 
 	const char *filename = CMD_ARGV[1];
 	const char *type = (CMD_ARGC >= 3) ? CMD_ARGV[2] : NULL;
diff --git a/src/flash/nor/mrvlqspi.c b/src/flash/nor/mrvlqspi.c
index 3293e61..3f5ce2c 100644
--- a/src/flash/nor/mrvlqspi.c
+++ b/src/flash/nor/mrvlqspi.c
@@ -761,7 +761,7 @@ static int mrvlqspi_flash_write(struct flash_bank *bank, const uint8_t *buffer,
 	return retval;
 }
 
-int mrvlqspi_flash_read(struct flash_bank *bank, uint8_t *buffer,
+static int mrvlqspi_flash_read(struct flash_bank *bank, uint8_t *buffer,
 				uint32_t offset, uint32_t count)
 {
 	struct target *target = bank->target;
@@ -914,7 +914,7 @@ static int mrvlqspi_flash_erase_check(struct flash_bank *bank)
 	return ERROR_OK;
 }
 
-int mrvlqspi_get_info(struct flash_bank *bank, char *buf, int buf_size)
+static int mrvlqspi_get_info(struct flash_bank *bank, char *buf, int buf_size)
 {
 	struct mrvlqspi_flash_bank *mrvlqspi_info = bank->driver_priv;
 
diff --git a/src/flash/nor/nrf5.c b/src/flash/nor/nrf5.c
index 0b65367..45d3d09 100644
--- a/src/flash/nor/nrf5.c
+++ b/src/flash/nor/nrf5.c
@@ -158,7 +158,7 @@ struct nrf5_info {
 	bool ficr_info_valid;
 	struct nrf52_ficr_info ficr_info;
 	const struct nrf5_device_spec *spec;
-	uint32_t hwid;
+	uint16_t hwid;
 	enum nrf5_features features;
 	unsigned int flash_size_kb;
 	unsigned int ram_size_kb;
@@ -328,7 +328,7 @@ static int nrf5_wait_for_nvmc(struct nrf5_info *chip)
 	do {
 		res = target_read_u32(chip->target, NRF5_NVMC_READY, &ready);
 		if (res != ERROR_OK) {
-			LOG_ERROR("Couldn't read NVMC_READY register");
+			LOG_ERROR("Error waiting NVMC_READY: generic flash write/erase error (check protection etc...)");
 			return res;
 		}
 
@@ -440,6 +440,38 @@ error:
 	return ERROR_FAIL;
 }
 
+static int nrf5_protect_check_clenr0(struct flash_bank *bank)
+{
+	int res;
+	uint32_t clenr0;
+	struct nrf5_bank *nbank = bank->driver_priv;
+	struct nrf5_info *chip = nbank->chip;
+
+	assert(chip != NULL);
+
+	res = target_read_u32(chip->target, NRF51_FICR_CLENR0,
+			      &clenr0);
+	if (res != ERROR_OK) {
+		LOG_ERROR("Couldn't read code region 0 size[FICR]");
+		return res;
+	}
+
+	if (clenr0 == 0xFFFFFFFF) {
+		res = target_read_u32(chip->target, NRF51_UICR_CLENR0,
+				      &clenr0);
+		if (res != ERROR_OK) {
+			LOG_ERROR("Couldn't read code region 0 size[UICR]");
+			return res;
+		}
+	}
+
+	for (unsigned int i = 0; i < bank->num_sectors; i++)
+		bank->sectors[i].is_protected =
+			clenr0 != 0xFFFFFFFF && bank->sectors[i].offset < clenr0;
+
+	return ERROR_OK;
+}
+
 static int nrf5_protect_check_bprot(struct flash_bank *bank)
 {
 	struct nrf5_bank *nbank = bank->driver_priv;
@@ -469,9 +501,6 @@ static int nrf5_protect_check_bprot(struct flash_bank *bank)
 
 static int nrf5_protect_check(struct flash_bank *bank)
 {
-	int res;
-	uint32_t clenr0;
-
 	/* UICR cannot be write protected so just return early */
 	if (bank->base == NRF5_UICR_BASE)
 		return ERROR_OK;
@@ -484,53 +513,20 @@ static int nrf5_protect_check(struct flash_bank *bank)
 	if (chip->features & NRF5_FEATURE_BPROT)
 		return nrf5_protect_check_bprot(bank);
 
-	if (!(chip->features & NRF5_FEATURE_SERIES_51)) {
-		LOG_WARNING("Flash protection of this nRF device is not supported");
-		return ERROR_FLASH_OPER_UNSUPPORTED;
-	}
-
-	res = target_read_u32(chip->target, NRF51_FICR_CLENR0,
-			      &clenr0);
-	if (res != ERROR_OK) {
-		LOG_ERROR("Couldn't read code region 0 size[FICR]");
-		return res;
-	}
+	if (chip->features & NRF5_FEATURE_SERIES_51)
+		return nrf5_protect_check_clenr0(bank);
 
-	if (clenr0 == 0xFFFFFFFF) {
-		res = target_read_u32(chip->target, NRF51_UICR_CLENR0,
-				      &clenr0);
-		if (res != ERROR_OK) {
-			LOG_ERROR("Couldn't read code region 0 size[UICR]");
-			return res;
-		}
-	}
-
-	for (unsigned int i = 0; i < bank->num_sectors; i++)
-		bank->sectors[i].is_protected =
-			clenr0 != 0xFFFFFFFF && bank->sectors[i].offset < clenr0;
-
-	return ERROR_OK;
+	LOG_WARNING("Flash protection of this nRF device is not supported");
+	return ERROR_FLASH_OPER_UNSUPPORTED;
 }
 
-static int nrf5_protect(struct flash_bank *bank, int set, unsigned int first,
+static int nrf5_protect_clenr0(struct flash_bank *bank, int set, unsigned int first,
 		unsigned int last)
 {
 	int res;
 	uint32_t clenr0, ppfc;
-	struct nrf5_info *chip;
-
-	/* UICR cannot be write protected so just bail out early */
-	if (bank->base == NRF5_UICR_BASE)
-		return ERROR_FAIL;
-
-	res = nrf5_get_probed_chip_if_halted(bank, &chip);
-	if (res != ERROR_OK)
-		return res;
-
-	if (!(chip->features & NRF5_FEATURE_SERIES_51)) {
-		LOG_ERROR("Flash protection setting of this nRF device is not supported");
-		return ERROR_FLASH_OPER_UNSUPPORTED;
-	}
+	struct nrf5_bank *nbank = bank->driver_priv;
+	struct nrf5_info *chip = nbank->chip;
 
 	if (first != 0) {
 		LOG_ERROR("Code region 0 must start at the beginning of the bank");
@@ -552,25 +548,59 @@ static int nrf5_protect(struct flash_bank *bank, int set, unsigned int first,
 	res = target_read_u32(chip->target, NRF51_UICR_CLENR0,
 			      &clenr0);
 	if (res != ERROR_OK) {
-		LOG_ERROR("Couldn't read code region 0 size[UICR]");
+		LOG_ERROR("Couldn't read code region 0 size from UICR");
 		return res;
 	}
 
-	if (clenr0 == 0xFFFFFFFF) {
-		res = target_write_u32(chip->target, NRF51_UICR_CLENR0,
-				       clenr0);
-		if (res != ERROR_OK) {
-			LOG_ERROR("Couldn't write code region 0 size[UICR]");
-			return res;
-		}
-
-	} else {
+	if (!set || clenr0 != 0xFFFFFFFF) {
 		LOG_ERROR("You need to perform chip erase before changing the protection settings");
+		return ERROR_FAIL;
 	}
 
-	nrf5_protect_check(bank);
+	res = nrf5_nvmc_write_enable(chip);
+	if (res != ERROR_OK)
+		goto error;
 
-	return ERROR_OK;
+	clenr0 = bank->sectors[last].offset + bank->sectors[last].size;
+	res = target_write_u32(chip->target, NRF51_UICR_CLENR0, clenr0);
+
+	int res2 = nrf5_wait_for_nvmc(chip);
+
+	if (res == ERROR_OK)
+		res = res2;
+
+	if (res == ERROR_OK)
+		LOG_INFO("A reset or power cycle is required for the new protection settings to take effect.");
+	else
+		LOG_ERROR("Couldn't write code region 0 size to UICR");
+
+error:
+	nrf5_nvmc_read_only(chip);
+
+	return res;
+}
+
+static int nrf5_protect(struct flash_bank *bank, int set, unsigned int first,
+		unsigned int last)
+{
+	int res;
+	struct nrf5_info *chip;
+
+	/* UICR cannot be write protected so just bail out early */
+	if (bank->base == NRF5_UICR_BASE) {
+		LOG_ERROR("UICR page does not support protection");
+		return ERROR_FLASH_OPER_UNSUPPORTED;
+	}
+
+	res = nrf5_get_probed_chip_if_halted(bank, &chip);
+	if (res != ERROR_OK)
+		return res;
+
+	if (chip->features & NRF5_FEATURE_SERIES_51)
+		return nrf5_protect_clenr0(bank, set, first, last);
+
+	LOG_ERROR("Flash protection setting is not supported on this nRF5 device");
+	return ERROR_FLASH_OPER_UNSUPPORTED;
 }
 
 static bool nrf5_info_variant_to_str(uint32_t variant, char *bf)
@@ -618,7 +648,7 @@ static int nrf5_info(struct flash_bank *bank, char *buf, int buf_size)
 				variant, &variant[2]);
 
 	} else {
-		res = snprintf(buf, buf_size, "nRF51xxx (HWID 0x%08" PRIx32 ")",
+		res = snprintf(buf, buf_size, "nRF51xxx (HWID 0x%04" PRIx16 ")",
 				chip->hwid);
 	}
 	if (res <= 0)
@@ -735,14 +765,15 @@ static int nrf5_probe(struct flash_bank *bank)
 	struct nrf5_info *chip = nbank->chip;
 	struct target *target = chip->target;
 
-	res = target_read_u32(target, NRF5_FICR_CONFIGID, &chip->hwid);
+	uint32_t configid;
+	res = target_read_u32(target, NRF5_FICR_CONFIGID, &configid);
 	if (res != ERROR_OK) {
 		LOG_ERROR("Couldn't read CONFIGID register");
 		return res;
 	}
 
-	chip->hwid &= 0xFFFF;	/* HWID is stored in the lower two
-			 * bytes of the CONFIGID register */
+	/* HWID is stored in the lower two bytes of the CONFIGID register */
+	chip->hwid = configid & 0xFFFF;
 
 	/* guess a nRF51 series if the device has no FICR INFO and we don't know HWID */
 	chip->features = NRF5_FEATURE_SERIES_51;
@@ -820,8 +851,6 @@ static int nrf5_probe(struct flash_bank *bank)
 		if (!bank->sectors)
 			return ERROR_FAIL;
 
-		nrf5_protect_check(bank);
-
 		chip->bank[0].probed = true;
 
 	} else {
@@ -983,7 +1012,7 @@ static int nrf5_ll_flash_write(struct nrf5_info *chip, uint32_t address, const u
 			0, NULL,
 			ARRAY_SIZE(reg_params), reg_params,
 			source->address, source->size,
-			write_algorithm->address, 0,
+			write_algorithm->address, write_algorithm->address + sizeof(nrf5_flash_write_code) - 2,
 			&armv7m_info);
 
 	target_free_working_area(target, source);
@@ -1011,6 +1040,34 @@ static int nrf5_write(struct flash_bank *bank, const uint8_t *buffer,
 	assert(offset % 4 == 0);
 	assert(count % 4 == 0);
 
+	/* UICR CLENR0 based protection used on nRF51 is somewhat clumsy:
+	 * RM reads: Code running from code region 1 will not be able to write
+	 * to code region 0.
+	 * Unfortunately the flash loader running from RAM can write to both
+	 * code regions whithout any hint the protection is violated.
+	 *
+	 * Update protection state and check if any flash sector to be written
+	 * is protected. */
+	if (chip->features & NRF5_FEATURE_SERIES_51) {
+
+		res = nrf5_protect_check_clenr0(bank);
+		if (res != ERROR_OK)
+			return res;
+
+		for (unsigned int sector = 0; sector < bank->num_sectors; sector++) {
+			struct flash_sector *bs = &bank->sectors[sector];
+
+			/* Start offset in or before this sector? */
+			/* End offset in or behind this sector? */
+			if ((offset < (bs->offset + bs->size))
+					&& ((offset + count - 1) >= bs->offset)
+					&& bs->is_protected == 1) {
+				LOG_ERROR("Write refused, sector %d is protected", sector);
+				return ERROR_FLASH_PROTECTED;
+			}
+		}
+	}
+
 	res = nrf5_nvmc_write_enable(chip);
 	if (res != ERROR_OK)
 		goto error;
@@ -1037,11 +1094,36 @@ static int nrf5_erase(struct flash_bank *bank, unsigned int first,
 	if (res != ERROR_OK)
 		return res;
 
+	/* UICR CLENR0 based protection used on nRF51 prevents erase
+	 * absolutely silently. NVMC has no flag to indicate the protection
+	 * was violated.
+	 *
+	 * Update protection state and check if any flash sector to be erased
+	 * is protected. */
+	if (chip->features & NRF5_FEATURE_SERIES_51) {
+
+		res = nrf5_protect_check_clenr0(bank);
+		if (res != ERROR_OK)
+			return res;
+	}
+
 	/* For each sector to be erased */
-	for (unsigned int s = first; s <= last && res == ERROR_OK; s++)
+	for (unsigned int s = first; s <= last && res == ERROR_OK; s++) {
+
+		if (chip->features & NRF5_FEATURE_SERIES_51
+				&& bank->sectors[s].is_protected == 1) {
+			LOG_ERROR("Flash sector %d is protected", s);
+			return ERROR_FLASH_PROTECTED;
+		}
+
 		res = nrf5_erase_page(bank, chip, &bank->sectors[s]);
+		if (res != ERROR_OK) {
+			LOG_ERROR("Error erasing sector %d", s);
+			return res;
+		}
+	}
 
-	return res;
+	return ERROR_OK;
 }
 
 static void nrf5_free_driver_priv(struct flash_bank *bank)
@@ -1158,23 +1240,16 @@ COMMAND_HANDLER(nrf5_handle_mass_erase_command)
 	}
 
 	res = nrf5_erase_all(chip);
-	if (res != ERROR_OK) {
-		LOG_ERROR("Failed to erase the chip");
-		nrf5_protect_check(bank);
-		return res;
-	}
+	if (res == ERROR_OK) {
+		LOG_INFO("Mass erase completed.");
+		if (chip->features & NRF5_FEATURE_SERIES_51)
+			LOG_INFO("A reset or power cycle is required if the flash was protected before.");
 
-	res = nrf5_protect_check(bank);
-	if (res != ERROR_OK) {
-		LOG_ERROR("Failed to check chip's write protection");
-		return res;
+	} else {
+		LOG_ERROR("Failed to erase the chip");
 	}
 
-	res = get_flash_bank_by_addr(target, NRF5_UICR_BASE, true, &bank);
-	if (res != ERROR_OK)
-		return res;
-
-	return ERROR_OK;
+	return res;
 }
 
 COMMAND_HANDLER(nrf5_handle_info_command)
diff --git a/src/flash/nor/psoc4.c b/src/flash/nor/psoc4.c
index 9c2fdf7..b606e18 100644
--- a/src/flash/nor/psoc4.c
+++ b/src/flash/nor/psoc4.c
@@ -139,7 +139,7 @@ struct psoc4_chip_family {
 	uint32_t flags;
 };
 
-const struct psoc4_chip_family psoc4_families[] = {
+static const struct psoc4_chip_family psoc4_families[] = {
 	{ 0x93, "PSoC4100/4200",           .flags = PSOC4_FAMILY_FLAG_LEGACY },
 	{ 0x9A, "PSoC4000",                .flags = 0 },
 	{ 0x9E, "PSoC/PRoC BLE (119E)",    .flags = 0 },
diff --git a/src/flash/nor/psoc6.c b/src/flash/nor/psoc6.c
index df151c1..931404e 100644
--- a/src/flash/nor/psoc6.c
+++ b/src/flash/nor/psoc6.c
@@ -151,12 +151,6 @@ static int sromalgo_prepare(struct target *target)
 	if (hr != ERROR_OK)
 		return hr;
 
-	/* Restore THUMB bit in xPSR register */
-	const struct armv7m_common *cm = target_to_armv7m(target);
-	hr = cm->store_core_reg_u32(target, ARMV7M_xPSR, 0x01000000);
-	if (hr != ERROR_OK)
-		return hr;
-
 	/* Allocate Working Area for Stack and Flash algorithm */
 	hr = target_alloc_working_area(target, RAM_STACK_WA_SIZE, &g_stack_area);
 	if (hr != ERROR_OK)
@@ -908,7 +902,7 @@ COMMAND_HANDLER(psoc6_handle_mass_erase_command)
  * @param target current target
  * @return ERROR_OK in case of success, ERROR_XXX code otherwise
  *************************************************************************************************/
-int handle_reset_halt(struct target *target)
+static int handle_reset_halt(struct target *target)
 {
 	int hr;
 	uint32_t reset_addr;
diff --git a/src/flash/nor/sfdp.c b/src/flash/nor/sfdp.c
new file mode 100644
index 0000000..2183ac1
--- /dev/null
+++ b/src/flash/nor/sfdp.c
@@ -0,0 +1,263 @@
+/***************************************************************************
+ *   Copyright (C) 2019 by Andreas Bolsch <andreas.bolsch@mni.thm.de	   *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or	   *
+ *   (at your option) any later version.								   *
+ *																		   *
+ *   This program is distributed in the hope that it will be useful,	   *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of		   *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the		   *
+ *   GNU General Public License for more details.						   *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License	   *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "imp.h"
+#include "spi.h"
+#include "sfdp.h"
+
+#define SFDP_MAGIC			0x50444653
+#define SFDP_ACCESS_PROT	0xFF
+#define SFDP_BASIC_FLASH	0xFF00
+#define SFDP_4BYTE_ADDR		0xFF84
+
+static const char *sfdp_name = "sfdp";
+
+struct sfdp_hdr {
+	uint32_t			signature;
+	uint32_t			revision;
+};
+
+struct sfdp_phdr {
+	uint32_t			revision;
+	uint32_t			ptr;
+};
+
+struct sfdp_basic_flash_param {
+	uint32_t			fast_addr;		/* 01: fast read and 3/4 address bytes */
+	uint32_t			density;		/* 02: memory density */
+	uint32_t			fast_1x4;		/* 03: 1-1-4 and 1-4-4 fast read */
+	uint32_t			fast_1x2;		/* 04: 1-2-2 and 1-1-2 fast read */
+	uint32_t			fast_444;		/* 05: 4-4-4 and 2-2-2 fast read */
+	uint32_t			read_222;		/* 06: 2-2-2 fast read instr and dummy */
+	uint32_t			read_444;		/* 07: 4-4-4 fast read instr and dummy */
+	uint32_t			erase_t12;		/* 08: erase types 1, 2 */
+	uint32_t			erase_t34;		/* 09: erase types 3, 4 */
+	uint32_t			erase_time;		/* 10: erase times for types 1 - 4 */
+	uint32_t			chip_byte;		/* 11: chip erase time, byte prog time, page prog */
+	uint32_t			susp_time;		/* 12: suspend and resume times */
+	uint32_t			susp_instr;		/* 13: suspend and resume instr */
+	uint32_t			pwrd_instr;		/* 14: powerdown instr */
+	uint32_t			quad_req;		/* 15: quad enable requirements */
+	uint32_t			addr_reset;		/* 16: 3-/4-byte addressing and reset */
+	uint32_t			read_1x8;		/* 17: 1-1-8 and 1-8-8 fast read instr and dummy */
+	uint32_t			dtr_drive;		/* 18: dtr modes and drive strength */
+	uint32_t			octal_req;		/* 19: octal enable requirements */
+	uint32_t			speed_888;		/* 20: speed in 8-8-8 modes */
+};
+
+struct sfdp_4byte_addr_param {
+	uint32_t			flags;			/* 01: various flags */
+	uint32_t			erase_t1234;	/* 02: erase commands */
+};
+
+/* Try to get parameters from flash via SFDP */
+int spi_sfdp(struct flash_bank *bank, struct flash_device *dev,
+	read_sfdp_block_t read_sfdp_block)
+{
+	struct sfdp_hdr header;
+	struct sfdp_phdr *pheaders = NULL;
+	uint32_t *ptable = NULL;
+	unsigned int j, k, nph;
+	int retval, erase_type = 0;
+
+	memset(dev, 0, sizeof(struct flash_device));
+
+	/* retrieve SFDP header */
+	memset(&header, 0, sizeof(header));
+	retval = read_sfdp_block(bank, 0x0, sizeof(header) >> 2, (uint32_t *)&header);
+	if (retval != ERROR_OK)
+		return retval;
+	LOG_DEBUG("header 0x%08" PRIx32 " 0x%08" PRIx32, header.signature, header.revision);
+	if (header.signature != SFDP_MAGIC) {
+		LOG_INFO("no SDFP found");
+		return ERROR_FLASH_BANK_NOT_PROBED;
+	}
+	if (((header.revision >> 24) & 0xFF) != SFDP_ACCESS_PROT) {
+		LOG_ERROR("access protocol 0x%02x not implemented",
+			(header.revision >> 24) & 0xFFU);
+		return ERROR_FLASH_BANK_NOT_PROBED;
+	}
+
+	/* retrieve table of parameter headers */
+	nph = ((header.revision >> 16) & 0xFF) + 1;
+	LOG_DEBUG("parameter headers: %d", nph);
+	pheaders = malloc(sizeof(struct sfdp_phdr) * nph);
+	if (pheaders == NULL) {
+		LOG_ERROR("not enough memory");
+		return ERROR_FAIL;
+	}
+	memset(pheaders, 0, sizeof(struct sfdp_phdr) * nph);
+	retval = read_sfdp_block(bank, sizeof(header),
+		(sizeof(struct sfdp_phdr) >> 2) * nph, (uint32_t *)pheaders);
+	if (retval != ERROR_OK)
+		goto err;
+
+	for (k = 0; k < nph; k++) {
+		uint8_t words = (pheaders[k].revision >> 24) & 0xFF;
+		uint16_t id = (((pheaders[k].ptr) >> 16) & 0xFF00) | (pheaders[k].revision & 0xFF);
+		uint32_t ptr = pheaders[k].ptr & 0xFFFFFF;
+
+		LOG_DEBUG("pheader %d len=0x%02" PRIx8 " id=0x%04" PRIx16
+			" ptr=0x%06" PRIx32, k, words, id, ptr);
+
+		/* retrieve parameter table */
+		ptable = malloc(words << 2);
+		if (ptable == NULL) {
+			LOG_ERROR("not enough memory");
+			retval = ERROR_FAIL;
+			goto err;
+		}
+		retval = read_sfdp_block(bank, ptr, words, ptable);
+		if (retval != ERROR_OK)
+			goto err;
+
+		for (j = 0; j < words; j++)
+			LOG_DEBUG("word %02d 0x%08X", j + 1, ptable[j]);
+
+		if (id == SFDP_BASIC_FLASH) {
+			struct sfdp_basic_flash_param *table = (struct sfdp_basic_flash_param *)ptable;
+			uint16_t erase;
+
+			if (words < 9) {
+				LOG_ERROR("id=0x%04" PRIx16 " invalid length %d", id, words);
+				retval = ERROR_FLASH_BANK_NOT_PROBED;
+				goto err;
+			}
+
+			LOG_DEBUG("basic flash parameter table");
+			/* dummy device name */
+			dev->name = sfdp_name;
+
+			/* default instructions */
+			dev->read_cmd = SPIFLASH_READ;
+			dev->pprog_cmd = SPIFLASH_PAGE_PROGRAM;
+			dev->chip_erase_cmd = SPIFLASH_MASS_ERASE;
+
+			/* get device size */
+			if (table->density & (1UL << 31))
+				dev->size_in_bytes = 1UL << ((table->density & ~(1UL << 31)) - 3);
+			else
+				dev->size_in_bytes = (table->density + 1) >> 3;
+
+			/* 2-2-2 read instruction, not used */
+			if (table->fast_444 & (1UL << 0))
+				dev->qread_cmd = (table->read_222 >> 24) & 0xFF;
+
+			/* 4-4-4 read instruction */
+			if (table->fast_444 & (1UL << 4))
+				dev->qread_cmd = (table->read_444 >> 24) & 0xFF;
+
+			/* find the largest erase block size and instruction */
+			erase = (table->erase_t12 >> 0) & 0xFFFF;
+			erase_type = 1;
+			if (((table->erase_t12 >> 16) & 0xFF) > (erase & 0xFF)) {
+				erase = (table->erase_t12 >> 16) & 0xFFFF;
+				erase_type = 2;
+			}
+			if (((table->erase_t34 >> 0) & 0xFF) > (erase & 0xFF)) {
+				erase = (table->erase_t34 >> 0) & 0xFFFF;
+				erase_type = 3;
+			}
+			if (((table->erase_t34 >> 16) & 0xFF) > (erase & 0xFF)) {
+				erase = (table->erase_t34 >> 16) & 0xFFFF;
+				erase_type = 4;
+			}
+			dev->erase_cmd = (erase >> 8) & 0xFF;
+			dev->sectorsize = 1UL << (erase & 0xFF);
+
+			if ((offsetof(struct sfdp_basic_flash_param, chip_byte) >> 2) < words) {
+				/* get Program Page Size, if chip_byte present, that's optional */
+				dev->pagesize = 1UL << ((table->chip_byte >> 4) & 0x0F);
+			} else {
+				/* no explicit page size specified ... */
+				if (table->fast_addr & (1UL << 2)) {
+					/* Write Granularity = 1, use 64 bytes */
+					dev->pagesize = 1UL << 6;
+				} else {
+					/* Write Granularity = 0, use 16 bytes */
+					dev->pagesize = 1UL << 4;
+				}
+			}
+
+			if (dev->size_in_bytes > (1UL << 24)) {
+				if (((table->fast_addr >> 17) & 0x3) == 0x0)
+					LOG_ERROR("device needs paging - not implemented");
+
+				/* 4-byte addresses needed if more than 16 MBytes */
+				if (((offsetof(struct sfdp_basic_flash_param, addr_reset) >> 2) < words) &&
+					(table->addr_reset & (1UL << 29))) {
+					/* dedicated 4-byte-address instructions, hopefully these ...
+					 * this entry is unfortunately optional as well
+					 * a subsequent 4-byte address table may overwrite this */
+					dev->read_cmd = 0x13;
+					dev->pprog_cmd = 0x12;
+					dev->erase_cmd = 0xDC;
+					if (dev->qread_cmd != 0)
+						dev->qread_cmd = 0xEC;
+				} else if (((table->fast_addr >> 17) & 0x3) == 0x1)
+					LOG_INFO("device has to be switched to 4-byte addresses");
+			}
+		} else if (id == SFDP_4BYTE_ADDR) {
+			struct sfdp_4byte_addr_param *table = (struct sfdp_4byte_addr_param *)ptable;
+
+			if (words >= (offsetof(struct sfdp_4byte_addr_param, erase_t1234)
+				+ sizeof(table->erase_t1234)) >> 2) {
+				LOG_INFO("4-byte address parameter table");
+
+				/* read and page program instructions */
+				if (table->flags & (1UL << 0))
+					dev->read_cmd = 0x13;
+				if (table->flags & (1UL << 5))
+					dev->qread_cmd = 0xEC;
+				if (table->flags & (1UL << 6))
+					dev->pprog_cmd = 0x12;
+
+				/* erase instructions */
+				if ((erase_type == 1) && (table->flags & (1UL << 9)))
+					dev->erase_cmd = (table->erase_t1234 >> 0) & 0xFF;
+				else if ((erase_type == 2) && (table->flags & (1UL << 10)))
+					dev->erase_cmd = (table->erase_t1234 >> 8) & 0xFF;
+				else if ((erase_type == 3) && (table->flags & (1UL << 11)))
+					dev->erase_cmd = (table->erase_t1234 >> 16) & 0xFF;
+				else if ((erase_type == 4) && (table->flags & (1UL << 12)))
+					dev->erase_cmd = (table->erase_t1234 >> 24) & 0xFF;
+			} else
+				LOG_ERROR("parameter table id=0x%04" PRIx16 " invalid length %d", id, words);
+		} else
+			LOG_DEBUG("unimplemented parameter table id=0x%04" PRIx16, id);
+
+		free(ptable);
+		ptable = NULL;
+	}
+
+	if (erase_type != 0) {
+		LOG_INFO("valid SFDP detected");
+		retval = ERROR_OK;
+	} else {
+		LOG_ERROR("incomplete/invalid SFDP");
+		retval = ERROR_FLASH_BANK_NOT_PROBED;
+	}
+
+err:
+	free(pheaders);
+	free(ptable);
+
+	return retval;
+}
diff --git a/src/flash/nor/sfdp.h b/src/flash/nor/sfdp.h
new file mode 100644
index 0000000..f924a4e
--- /dev/null
+++ b/src/flash/nor/sfdp.h
@@ -0,0 +1,34 @@
+/***************************************************************************
+ *   Copyright (C) 2019 by Andreas Bolsch <andreas.bolsch@mni.thm.de	   *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or	   *
+ *   (at your option) any later version.								   *
+ *																		   *
+ *   This program is distributed in the hope that it will be useful,	   *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of		   *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the		   *
+ *   GNU General Public License for more details.						   *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License	   *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+
+#ifndef OPENOCD_FLASH_NOR_SFDP_H
+#define OPENOCD_FLASH_NOR_SFDP_H
+
+/* per JESD216D 'addr' is *byte* based but must be word aligned,
+ * 'buffer' is word based, word aligned and always little-endian encoded,
+ * in the flash, 'addr_len' is 3 or 4, 'dummy' ***usually*** 8
+ *
+ * the actual number of dummy clocks should be worked out by this function
+ * dynamically, i.e. by scanning the first few bytes for the SFDP signature
+ *
+ * buffer contents is supposed to be returned in ***host*** endianness */
+typedef int (*read_sfdp_block_t)(struct flash_bank *bank, uint32_t addr,
+	uint32_t words, uint32_t *buffer);
+
+extern int spi_sfdp(struct flash_bank *bank, struct flash_device *dev,
+	read_sfdp_block_t read_sfdp_block);
+
+#endif /* OPENOCD_FLASH_NOR_SFDP_H */
diff --git a/src/flash/nor/spi.h b/src/flash/nor/spi.h
index 11c381f..f8a0a65 100644
--- a/src/flash/nor/spi.h
+++ b/src/flash/nor/spi.h
@@ -1,5 +1,5 @@
 /***************************************************************************
- *   Copyright (C) 2018 by Andreas Bolsch                                  *
+ *   Copyright (C) 2018-2019 by Andreas Bolsch                             *
  *   andreas.bolsch@mni.thm.de                                             *
  *                                                                         *
  *   Copyright (C) 2012 by George Harris                                   *
@@ -29,7 +29,7 @@
 
 /* data structure to maintain flash ids from different vendors */
 struct flash_device {
-	char *name;
+	const char *name;
 	uint8_t read_cmd;
 	uint8_t qread_cmd;
 	uint8_t pprog_cmd;
@@ -87,6 +87,8 @@ extern const struct flash_device flash_devices[];
 #define SPIFLASH_PAGE_PROGRAM	0x02 /* Page Program */
 #define SPIFLASH_FAST_READ		0x0B /* Fast Read */
 #define SPIFLASH_READ			0x03 /* Normal Read */
+#define SPIFLASH_MASS_ERASE		0xC7 /* Mass Erase */
+#define SPIFLASH_READ_SFDP		0x5A /* Read Serial Flash Discoverable Parameters */
 
 #define SPIFLASH_DEF_PAGESIZE	256  /* default for non-page-oriented devices (FRAMs) */
 
diff --git a/src/flash/nor/stm32f1x.c b/src/flash/nor/stm32f1x.c
index 78efc8b..9cd282d 100644
--- a/src/flash/nor/stm32f1x.c
+++ b/src/flash/nor/stm32f1x.c
@@ -215,7 +215,7 @@ static int stm32x_check_operation_supported(struct flash_bank *bank)
 	/* if we have a dual flash bank device then
 	 * we need to perform option byte stuff on bank0 only */
 	if (stm32x_info->register_base != FLASH_REG_BASE_B0) {
-		LOG_ERROR("Option Byte Operation's must use bank0");
+		LOG_ERROR("Option byte operations must use bank 0");
 		return ERROR_FLASH_OPERATION_FAILED;
 	}
 
diff --git a/src/flash/nor/stm32l4x.c b/src/flash/nor/stm32l4x.c
index 379f1b4..55a8d8f 100644
--- a/src/flash/nor/stm32l4x.c
+++ b/src/flash/nor/stm32l4x.c
@@ -81,8 +81,7 @@
  * http://www.st.com/resource/en/reference_manual/dm00530369.pdf
  */
 
-/*
- * STM32G0xxx series for reference.
+/* STM32G0xxx series for reference.
  *
  * RM0444 (STM32G0x1)
  * http://www.st.com/resource/en/reference_manual/dm00371828.pdf
@@ -91,10 +90,9 @@
  * http://www.st.com/resource/en/reference_manual/dm00463896.pdf
  */
 
-/*
- * STM32G4xxx series for reference.
+/* STM32G4xxx series for reference.
  *
- * RM0440 (STM32G43x/44x/47x/48x)
+ * RM0440 (STM32G43x/44x/47x/48x/49x/4Ax)
  * http://www.st.com/resource/en/reference_manual/dm00355726.pdf
  *
  * Cat. 2 devices have single bank only, page size is 2kByte.
@@ -104,12 +102,60 @@
  *
  * Bank mode is controlled by bit 22 (DBANK) in option bytes register.
  * Both banks are treated as a single OpenOCD bank.
+ *
+ * Cat. 4 devices have single bank only, page size is 2kByte.
+ */
+
+/* STM32L5xxx series for reference.
+ *
+ * RM0428 (STM32L552xx/STM32L562xx)
+ * http://www.st.com/resource/en/reference_manual/dm00346336.pdf
  */
 
 /* Erase time can be as high as 25ms, 10x this and assume it's toast... */
 
 #define FLASH_ERASE_TIMEOUT 250
 
+enum stm32l4_flash_reg_index {
+	STM32_FLASH_ACR_INDEX,
+	STM32_FLASH_KEYR_INDEX,
+	STM32_FLASH_OPTKEYR_INDEX,
+	STM32_FLASH_SR_INDEX,
+	STM32_FLASH_CR_INDEX,
+	STM32_FLASH_OPTR_INDEX,
+	STM32_FLASH_WRP1AR_INDEX,
+	STM32_FLASH_WRP1BR_INDEX,
+	STM32_FLASH_WRP2AR_INDEX,
+	STM32_FLASH_WRP2BR_INDEX,
+	STM32_FLASH_REG_INDEX_NUM,
+};
+
+static const uint32_t stm32l4_flash_regs[STM32_FLASH_REG_INDEX_NUM] = {
+	[STM32_FLASH_ACR_INDEX]      = 0x000,
+	[STM32_FLASH_KEYR_INDEX]     = 0x008,
+	[STM32_FLASH_OPTKEYR_INDEX]  = 0x00C,
+	[STM32_FLASH_SR_INDEX]       = 0x010,
+	[STM32_FLASH_CR_INDEX]       = 0x014,
+	[STM32_FLASH_OPTR_INDEX]     = 0x020,
+	[STM32_FLASH_WRP1AR_INDEX]   = 0x02C,
+	[STM32_FLASH_WRP1BR_INDEX]   = 0x030,
+	[STM32_FLASH_WRP2AR_INDEX]   = 0x04C,
+	[STM32_FLASH_WRP2BR_INDEX]   = 0x050,
+};
+
+static const uint32_t stm32l5_ns_flash_regs[STM32_FLASH_REG_INDEX_NUM] = {
+	[STM32_FLASH_ACR_INDEX]      = 0x000,
+	[STM32_FLASH_KEYR_INDEX]     = 0x008,
+	[STM32_FLASH_OPTKEYR_INDEX]  = 0x010,
+	[STM32_FLASH_SR_INDEX]       = 0x020,
+	[STM32_FLASH_CR_INDEX]       = 0x028,
+	[STM32_FLASH_OPTR_INDEX]     = 0x040,
+	[STM32_FLASH_WRP1AR_INDEX]   = 0x058,
+	[STM32_FLASH_WRP1BR_INDEX]   = 0x05C,
+	[STM32_FLASH_WRP2AR_INDEX]   = 0x068,
+	[STM32_FLASH_WRP2BR_INDEX]   = 0x06C,
+};
+
 struct stm32l4_rev {
 	const uint16_t rev;
 	const char *str;
@@ -123,6 +169,7 @@ struct stm32l4_part_info {
 	const uint16_t max_flash_size_kb;
 	const bool has_dual_bank;
 	const uint32_t flash_regs_base;
+	const uint32_t *default_flash_regs;
 	const uint32_t fsize_addr;
 };
 
@@ -135,10 +182,11 @@ struct stm32l4_flash_bank {
 	uint32_t user_bank_size;
 	uint32_t wrpxxr_mask;
 	const struct stm32l4_part_info *part_info;
+	const uint32_t *flash_regs;
 };
 
-/* human readable list of families this drivers supports */
-static const char *device_families = "STM32L4/L4+/WB/WL/G4/G0";
+/* human readable list of families this drivers supports (sorted alphabetically) */
+static const char *device_families = "STM32G0/G4/L4/L4+/L5/WB/WL";
 
 static const struct stm32l4_rev stm32_415_revs[] = {
 	{ 0x1000, "1" }, { 0x1001, "2" }, { 0x1003, "3" }, { 0x1007, "4" }
@@ -184,6 +232,14 @@ static const struct stm32l4_rev stm32_471_revs[] = {
 	{ 0x1001, "Z" },
 };
 
+static const struct stm32l4_rev stm32_472_revs[] = {
+	{ 0x1000, "A" }, { 0x2000, "B" },
+};
+
+static const struct stm32l4_rev stm32_479_revs[] = {
+	{ 0x1000, "A" },
+};
+
 static const struct stm32l4_rev stm32_495_revs[] = {
 	{ 0x2001, "2.1" },
 };
@@ -205,6 +261,7 @@ static const struct stm32l4_part_info stm32l4_parts[] = {
 	  .max_flash_size_kb     = 1024,
 	  .has_dual_bank         = true,
 	  .flash_regs_base       = 0x40022000,
+	  .default_flash_regs    = stm32l4_flash_regs,
 	  .fsize_addr            = 0x1FFF75E0,
 	},
 	{
@@ -215,6 +272,7 @@ static const struct stm32l4_part_info stm32l4_parts[] = {
 	  .max_flash_size_kb     = 256,
 	  .has_dual_bank         = false,
 	  .flash_regs_base       = 0x40022000,
+	  .default_flash_regs    = stm32l4_flash_regs,
 	  .fsize_addr            = 0x1FFF75E0,
 	},
 	{
@@ -225,6 +283,7 @@ static const struct stm32l4_part_info stm32l4_parts[] = {
 	  .max_flash_size_kb     = 128,
 	  .has_dual_bank         = false,
 	  .flash_regs_base       = 0x40022000,
+	  .default_flash_regs    = stm32l4_flash_regs,
 	  .fsize_addr            = 0x1FFF75E0,
 	},
 	{
@@ -235,6 +294,7 @@ static const struct stm32l4_part_info stm32l4_parts[] = {
 	  .max_flash_size_kb     = 1024,
 	  .has_dual_bank         = true,
 	  .flash_regs_base       = 0x40022000,
+	  .default_flash_regs    = stm32l4_flash_regs,
 	  .fsize_addr            = 0x1FFF75E0,
 	},
 	{
@@ -245,6 +305,7 @@ static const struct stm32l4_part_info stm32l4_parts[] = {
 	  .max_flash_size_kb     = 512,
 	  .has_dual_bank         = false,
 	  .flash_regs_base       = 0x40022000,
+	  .default_flash_regs    = stm32l4_flash_regs,
 	  .fsize_addr            = 0x1FFF75E0,
 	},
 	{
@@ -255,6 +316,7 @@ static const struct stm32l4_part_info stm32l4_parts[] = {
 	  .max_flash_size_kb     = 128,
 	  .has_dual_bank         = false,
 	  .flash_regs_base       = 0x40022000,
+	  .default_flash_regs    = stm32l4_flash_regs,
 	  .fsize_addr            = 0x1FFF75E0,
 	},
 	{
@@ -265,6 +327,7 @@ static const struct stm32l4_part_info stm32l4_parts[] = {
 	  .max_flash_size_kb     = 64,
 	  .has_dual_bank         = false,
 	  .flash_regs_base       = 0x40022000,
+	  .default_flash_regs    = stm32l4_flash_regs,
 	  .fsize_addr            = 0x1FFF75E0,
 	},
 	{
@@ -275,6 +338,7 @@ static const struct stm32l4_part_info stm32l4_parts[] = {
 	  .max_flash_size_kb     = 128,
 	  .has_dual_bank         = false,
 	  .flash_regs_base       = 0x40022000,
+	  .default_flash_regs    = stm32l4_flash_regs,
 	  .fsize_addr            = 0x1FFF75E0,
 	},
 	{
@@ -285,6 +349,7 @@ static const struct stm32l4_part_info stm32l4_parts[] = {
 	  .max_flash_size_kb     = 512,
 	  .has_dual_bank         = true,
 	  .flash_regs_base       = 0x40022000,
+	  .default_flash_regs    = stm32l4_flash_regs,
 	  .fsize_addr            = 0x1FFF75E0,
 	},
 	{
@@ -295,6 +360,7 @@ static const struct stm32l4_part_info stm32l4_parts[] = {
 	  .max_flash_size_kb     = 2048,
 	  .has_dual_bank         = true,
 	  .flash_regs_base       = 0x40022000,
+	  .default_flash_regs    = stm32l4_flash_regs,
 	  .fsize_addr            = 0x1FFF75E0,
 	},
 	{
@@ -305,6 +371,29 @@ static const struct stm32l4_part_info stm32l4_parts[] = {
 	  .max_flash_size_kb     = 1024,
 	  .has_dual_bank         = true,
 	  .flash_regs_base       = 0x40022000,
+	  .default_flash_regs    = stm32l4_flash_regs,
+	  .fsize_addr            = 0x1FFF75E0,
+	},
+	{
+	  .id                    = 0x472,
+	  .revs                  = stm32_472_revs,
+	  .num_revs              = ARRAY_SIZE(stm32_472_revs),
+	  .device_str            = "STM32L55/L56xx",
+	  .max_flash_size_kb     = 512,
+	  .has_dual_bank         = true,
+	  .flash_regs_base       = 0x40022000,
+	  .default_flash_regs    = stm32l5_ns_flash_regs,
+	  .fsize_addr            = 0x0BFA05E0,
+	},
+	{
+	  .id                    = 0x479,
+	  .revs                  = stm32_479_revs,
+	  .num_revs              = ARRAY_SIZE(stm32_479_revs),
+	  .device_str            = "STM32G49/G4Axx",
+	  .max_flash_size_kb     = 512,
+	  .has_dual_bank         = false,
+	  .flash_regs_base       = 0x40022000,
+	  .default_flash_regs    = stm32l4_flash_regs,
 	  .fsize_addr            = 0x1FFF75E0,
 	},
 	{
@@ -315,6 +404,7 @@ static const struct stm32l4_part_info stm32l4_parts[] = {
 	  .max_flash_size_kb     = 1024,
 	  .has_dual_bank         = false,
 	  .flash_regs_base       = 0x58004000,
+	  .default_flash_regs    = stm32l4_flash_regs,
 	  .fsize_addr            = 0x1FFF75E0,
 	},
 	{
@@ -325,6 +415,7 @@ static const struct stm32l4_part_info stm32l4_parts[] = {
 	  .max_flash_size_kb     = 512,
 	  .has_dual_bank         = false,
 	  .flash_regs_base       = 0x58004000,
+	  .default_flash_regs    = stm32l4_flash_regs,
 	  .fsize_addr            = 0x1FFF75E0,
 	},
 	{
@@ -335,6 +426,7 @@ static const struct stm32l4_part_info stm32l4_parts[] = {
 	  .max_flash_size_kb     = 256,
 	  .has_dual_bank         = false,
 	  .flash_regs_base       = 0x58004000,
+	  .default_flash_regs    = stm32l4_flash_regs,
 	  .fsize_addr            = 0x1FFF75E0,
 	},
 };
@@ -368,16 +460,37 @@ static inline uint32_t stm32l4_get_flash_reg(struct flash_bank *bank, uint32_t r
 	return stm32l4_info->part_info->flash_regs_base + reg_offset;
 }
 
+static inline uint32_t stm32l4_get_flash_reg_by_index(struct flash_bank *bank,
+	enum stm32l4_flash_reg_index reg_index)
+{
+	struct stm32l4_flash_bank *stm32l4_info = bank->driver_priv;
+	return stm32l4_get_flash_reg(bank, stm32l4_info->flash_regs[reg_index]);
+}
+
 static inline int stm32l4_read_flash_reg(struct flash_bank *bank, uint32_t reg_offset, uint32_t *value)
 {
 	return target_read_u32(bank->target, stm32l4_get_flash_reg(bank, reg_offset), value);
 }
 
+static inline int stm32l4_read_flash_reg_by_index(struct flash_bank *bank,
+	enum stm32l4_flash_reg_index reg_index, uint32_t *value)
+{
+	struct stm32l4_flash_bank *stm32l4_info = bank->driver_priv;
+	return stm32l4_read_flash_reg(bank, stm32l4_info->flash_regs[reg_index], value);
+}
+
 static inline int stm32l4_write_flash_reg(struct flash_bank *bank, uint32_t reg_offset, uint32_t value)
 {
 	return target_write_u32(bank->target, stm32l4_get_flash_reg(bank, reg_offset), value);
 }
 
+static inline int stm32l4_write_flash_reg_by_index(struct flash_bank *bank,
+	enum stm32l4_flash_reg_index reg_index, uint32_t value)
+{
+	struct stm32l4_flash_bank *stm32l4_info = bank->driver_priv;
+	return stm32l4_write_flash_reg(bank, stm32l4_info->flash_regs[reg_index], value);
+}
+
 static int stm32l4_wait_status_busy(struct flash_bank *bank, int timeout)
 {
 	uint32_t status;
@@ -385,7 +498,7 @@ static int stm32l4_wait_status_busy(struct flash_bank *bank, int timeout)
 
 	/* wait for busy to clear */
 	for (;;) {
-		retval = stm32l4_read_flash_reg(bank, STM32_FLASH_SR, &status);
+		retval = stm32l4_read_flash_reg_by_index(bank, STM32_FLASH_SR_INDEX, &status);
 		if (retval != ERROR_OK)
 			return retval;
 		LOG_DEBUG("status: 0x%" PRIx32 "", status);
@@ -398,7 +511,6 @@ static int stm32l4_wait_status_busy(struct flash_bank *bank, int timeout)
 		alive_sleep(1);
 	}
 
-
 	if (status & FLASH_WRPERR) {
 		LOG_ERROR("stm32x device protected");
 		retval = ERROR_FAIL;
@@ -411,7 +523,7 @@ static int stm32l4_wait_status_busy(struct flash_bank *bank, int timeout)
 		/* If this operation fails, we ignore it and report the original
 		 * retval
 		 */
-		stm32l4_write_flash_reg(bank, STM32_FLASH_SR, status & FLASH_ERROR);
+		stm32l4_write_flash_reg_by_index(bank, STM32_FLASH_SR_INDEX, status & FLASH_ERROR);
 	}
 
 	return retval;
@@ -424,7 +536,7 @@ static int stm32l4_unlock_reg(struct flash_bank *bank)
 	/* first check if not already unlocked
 	 * otherwise writing on STM32_FLASH_KEYR will fail
 	 */
-	int retval = stm32l4_read_flash_reg(bank, STM32_FLASH_CR, &ctrl);
+	int retval = stm32l4_read_flash_reg_by_index(bank, STM32_FLASH_CR_INDEX, &ctrl);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -432,15 +544,15 @@ static int stm32l4_unlock_reg(struct flash_bank *bank)
 		return ERROR_OK;
 
 	/* unlock flash registers */
-	retval = stm32l4_write_flash_reg(bank, STM32_FLASH_KEYR, KEY1);
+	retval = stm32l4_write_flash_reg_by_index(bank, STM32_FLASH_KEYR_INDEX, KEY1);
 	if (retval != ERROR_OK)
 		return retval;
 
-	retval = stm32l4_write_flash_reg(bank, STM32_FLASH_KEYR, KEY2);
+	retval = stm32l4_write_flash_reg_by_index(bank, STM32_FLASH_KEYR_INDEX, KEY2);
 	if (retval != ERROR_OK)
 		return retval;
 
-	retval = stm32l4_read_flash_reg(bank, STM32_FLASH_CR, &ctrl);
+	retval = stm32l4_read_flash_reg_by_index(bank, STM32_FLASH_CR_INDEX, &ctrl);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -456,7 +568,7 @@ static int stm32l4_unlock_option_reg(struct flash_bank *bank)
 {
 	uint32_t ctrl;
 
-	int retval = stm32l4_read_flash_reg(bank, STM32_FLASH_CR, &ctrl);
+	int retval = stm32l4_read_flash_reg_by_index(bank, STM32_FLASH_CR_INDEX, &ctrl);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -464,15 +576,15 @@ static int stm32l4_unlock_option_reg(struct flash_bank *bank)
 		return ERROR_OK;
 
 	/* unlock option registers */
-	retval = stm32l4_write_flash_reg(bank, STM32_FLASH_OPTKEYR, OPTKEY1);
+	retval = stm32l4_write_flash_reg_by_index(bank, STM32_FLASH_OPTKEYR_INDEX, OPTKEY1);
 	if (retval != ERROR_OK)
 		return retval;
 
-	retval = stm32l4_write_flash_reg(bank, STM32_FLASH_OPTKEYR, OPTKEY2);
+	retval = stm32l4_write_flash_reg_by_index(bank, STM32_FLASH_OPTKEYR_INDEX, OPTKEY2);
 	if (retval != ERROR_OK)
 		return retval;
 
-	retval = stm32l4_read_flash_reg(bank, STM32_FLASH_CR, &ctrl);
+	retval = stm32l4_read_flash_reg_by_index(bank, STM32_FLASH_CR_INDEX, &ctrl);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -508,14 +620,14 @@ static int stm32l4_write_option(struct flash_bank *bank, uint32_t reg_offset,
 	if (retval != ERROR_OK)
 		goto err_lock;
 
-	retval = stm32l4_write_flash_reg(bank, STM32_FLASH_CR, FLASH_OPTSTRT);
+	retval = stm32l4_write_flash_reg_by_index(bank, STM32_FLASH_CR_INDEX, FLASH_OPTSTRT);
 	if (retval != ERROR_OK)
 		goto err_lock;
 
 	retval = stm32l4_wait_status_busy(bank, FLASH_ERASE_TIMEOUT);
 
 err_lock:
-	retval2 = stm32l4_write_flash_reg(bank, STM32_FLASH_CR, FLASH_LOCK | FLASH_OPTLOCK);
+	retval2 = stm32l4_write_flash_reg_by_index(bank, STM32_FLASH_CR_INDEX, FLASH_LOCK | FLASH_OPTLOCK);
 
 	if (retval != ERROR_OK)
 		return retval;
@@ -528,11 +640,11 @@ static int stm32l4_protect_check(struct flash_bank *bank)
 	struct stm32l4_flash_bank *stm32l4_info = bank->driver_priv;
 
 	uint32_t wrp1ar, wrp1br, wrp2ar, wrp2br;
-	stm32l4_read_flash_reg(bank, STM32_FLASH_WRP1AR, &wrp1ar);
-	stm32l4_read_flash_reg(bank, STM32_FLASH_WRP1BR, &wrp1br);
+	stm32l4_read_flash_reg_by_index(bank, STM32_FLASH_WRP1AR_INDEX, &wrp1ar);
+	stm32l4_read_flash_reg_by_index(bank, STM32_FLASH_WRP1BR_INDEX, &wrp1br);
 	if (stm32l4_info->part_info->has_dual_bank) {
-		stm32l4_read_flash_reg(bank, STM32_FLASH_WRP2AR, &wrp2ar);
-		stm32l4_read_flash_reg(bank, STM32_FLASH_WRP2BR, &wrp2br);
+		stm32l4_read_flash_reg_by_index(bank, STM32_FLASH_WRP2AR_INDEX, &wrp2ar);
+		stm32l4_read_flash_reg_by_index(bank, STM32_FLASH_WRP2BR_INDEX, &wrp2br);
 	} else {
 		/* prevent uninitialized errors */
 		wrp2ar = 0;
@@ -611,7 +723,7 @@ static int stm32l4_erase(struct flash_bank *bank, unsigned int first,
 			erase_flags |= snb << FLASH_PAGE_SHIFT | FLASH_CR_BKER;
 		} else
 			erase_flags |= i << FLASH_PAGE_SHIFT;
-		retval = stm32l4_write_flash_reg(bank, STM32_FLASH_CR, erase_flags);
+		retval = stm32l4_write_flash_reg_by_index(bank, STM32_FLASH_CR_INDEX, erase_flags);
 		if (retval != ERROR_OK)
 			break;
 
@@ -623,7 +735,7 @@ static int stm32l4_erase(struct flash_bank *bank, unsigned int first,
 	}
 
 err_lock:
-	retval2 = stm32l4_write_flash_reg(bank, STM32_FLASH_CR, FLASH_LOCK);
+	retval2 = stm32l4_write_flash_reg_by_index(bank, STM32_FLASH_CR_INDEX, FLASH_LOCK);
 
 	if (retval != ERROR_OK)
 		return retval;
@@ -651,7 +763,7 @@ static int stm32l4_protect(struct flash_bank *bank, int set, unsigned int first,
 			reg_value = ((last & 0xFF) << 16) | begin;
 		}
 
-		ret = stm32l4_write_option(bank, STM32_FLASH_WRP2AR, reg_value, 0xffffffff);
+		ret = stm32l4_write_option(bank, stm32l4_info->flash_regs[STM32_FLASH_WRP2AR_INDEX], reg_value, 0xffffffff);
 	}
 	/* Bank 1 */
 	reg_value = 0xFF; /* Default to bank un-protected */
@@ -661,7 +773,7 @@ static int stm32l4_protect(struct flash_bank *bank, int set, unsigned int first,
 			reg_value = (end << 16) | (first & 0xFF);
 		}
 
-		ret = stm32l4_write_option(bank, STM32_FLASH_WRP1AR, reg_value, 0xffffffff);
+		ret = stm32l4_write_option(bank, stm32l4_info->flash_regs[STM32_FLASH_WRP1AR_INDEX], reg_value, 0xffffffff);
 	}
 
 	return ret;
@@ -727,8 +839,8 @@ static int stm32l4_write_block(struct flash_bank *bank, const uint8_t *buffer,
 	buf_set_u32(reg_params[1].value, 0, 32, source->address + source->size);
 	buf_set_u32(reg_params[2].value, 0, 32, address);
 	buf_set_u32(reg_params[3].value, 0, 32, count);
-	buf_set_u32(reg_params[4].value, 0, 32, stm32l4_get_flash_reg(bank, STM32_FLASH_SR));
-	buf_set_u32(reg_params[5].value, 0, 32, stm32l4_get_flash_reg(bank, STM32_FLASH_CR));
+	buf_set_u32(reg_params[4].value, 0, 32, stm32l4_get_flash_reg_by_index(bank, STM32_FLASH_SR_INDEX));
+	buf_set_u32(reg_params[5].value, 0, 32, stm32l4_get_flash_reg_by_index(bank, STM32_FLASH_CR_INDEX));
 
 	retval = target_run_flash_async_algorithm(target, buffer, count, 8,
 			0, NULL,
@@ -748,7 +860,7 @@ static int stm32l4_write_block(struct flash_bank *bank, const uint8_t *buffer,
 		if (error != 0) {
 			LOG_ERROR("flash write failed = %08" PRIx32, error);
 			/* Clear but report errors */
-			stm32l4_write_flash_reg(bank, STM32_FLASH_SR, error);
+			stm32l4_write_flash_reg_by_index(bank, STM32_FLASH_SR_INDEX, error);
 			retval = ERROR_FAIL;
 		}
 	}
@@ -825,7 +937,7 @@ static int stm32l4_write(struct flash_bank *bank, const uint8_t *buffer,
 	retval = stm32l4_write_block(bank, buffer, offset, count / 8);
 
 err_lock:
-	retval2 = stm32l4_write_flash_reg(bank, STM32_FLASH_CR, FLASH_LOCK);
+	retval2 = stm32l4_write_flash_reg_by_index(bank, STM32_FLASH_CR_INDEX, FLASH_LOCK);
 
 	if (retval != ERROR_OK) {
 		LOG_ERROR("block write failed");
@@ -838,17 +950,17 @@ static int stm32l4_read_idcode(struct flash_bank *bank, uint32_t *id)
 {
 	int retval;
 
-	/* try stm32l4/l4+/wb/g4 id register first, then stm32g0 id register */
-	retval = target_read_u32(bank->target, DBGMCU_IDCODE_L4_G4, id);
-	if ((retval != ERROR_OK) || ((*id & 0xfff) == 0) || ((*id & 0xfff) == 0xfff)) {
-		retval = target_read_u32(bank->target, DBGMCU_IDCODE_G0, id);
-		if ((retval != ERROR_OK) || ((*id & 0xfff) == 0) || ((*id & 0xfff) == 0xfff)) {
-			LOG_ERROR("can't get device id");
-			return (retval == ERROR_OK) ? ERROR_FAIL : retval;
-		}
+	/* try reading possible IDCODE registers, in the following order */
+	uint32_t DBGMCU_IDCODE[] = {DBGMCU_IDCODE_L4_G4, DBGMCU_IDCODE_G0, DBGMCU_IDCODE_L5};
+
+	for (unsigned int i = 0; i < ARRAY_SIZE(DBGMCU_IDCODE); i++) {
+		retval = target_read_u32(bank->target, DBGMCU_IDCODE[i], id);
+		if ((retval == ERROR_OK) && ((*id & 0xfff) != 0) && ((*id & 0xfff) != 0xfff))
+			return ERROR_OK;
 	}
 
-	return retval;
+	LOG_ERROR("can't get the device id");
+	return (retval == ERROR_OK) ? ERROR_FAIL : retval;
 }
 
 static int stm32l4_probe(struct flash_bank *bank)
@@ -880,6 +992,7 @@ static int stm32l4_probe(struct flash_bank *bank)
 	}
 
 	part_info = stm32l4_info->part_info;
+	stm32l4_info->flash_regs = stm32l4_info->part_info->default_flash_regs;
 
 	char device_info[1024];
 	retval = bank->driver->info(bank, device_info, sizeof(device_info));
@@ -913,7 +1026,7 @@ static int stm32l4_probe(struct flash_bank *bank)
 	assert((flash_size_kb != 0xffff) && flash_size_kb);
 
 	/* read flash option register */
-	retval = stm32l4_read_flash_reg(bank, STM32_FLASH_OPTR, &options);
+	retval = stm32l4_read_flash_reg_by_index(bank, STM32_FLASH_OPTR_INDEX, &options);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -924,6 +1037,7 @@ static int stm32l4_probe(struct flash_bank *bank)
 	int page_size_kb = 0;
 
 	stm32l4_info->dual_bank_mode = false;
+	bool use_dbank_bit = false;
 
 	switch (device_id) {
 	case 0x415: /* STM32L47/L48xx */
@@ -952,6 +1066,7 @@ static int stm32l4_probe(struct flash_bank *bank)
 	case 0x464: /* STM32L41/L42xx */
 	case 0x466: /* STM32G03/G04xx */
 	case 0x468: /* STM32G43/G44xx */
+	case 0x479: /* STM32G49/G4Axx */
 	case 0x497: /* STM32WLEx */
 		/* single bank flash */
 		page_size_kb = 2;
@@ -989,7 +1104,7 @@ static int stm32l4_probe(struct flash_bank *bank)
 		page_size_kb = 8;
 		num_pages = flash_size_kb / page_size_kb;
 		stm32l4_info->bank1_sectors = num_pages;
-		const bool use_dbank_bit = flash_size_kb == part_info->max_flash_size_kb;
+		use_dbank_bit = flash_size_kb == part_info->max_flash_size_kb;
 		if ((use_dbank_bit && (options & BIT(22))) ||
 			(!use_dbank_bit && (options & BIT(21)))) {
 			stm32l4_info->dual_bank_mode = true;
@@ -998,6 +1113,23 @@ static int stm32l4_probe(struct flash_bank *bank)
 			stm32l4_info->bank1_sectors = num_pages / 2;
 		}
 		break;
+	case 0x472: /* STM32L55/L56xx */
+		/* STM32L55/L56xx can be single/dual bank:
+		 *   if size = 512K check DBANK bit(22)
+		 *   if size = 256K check DB256K bit(21)
+		 */
+		page_size_kb = 4;
+		num_pages = flash_size_kb / page_size_kb;
+		stm32l4_info->bank1_sectors = num_pages;
+		use_dbank_bit = flash_size_kb == part_info->max_flash_size_kb;
+		if ((use_dbank_bit && (options & BIT(22))) ||
+			(!use_dbank_bit && (options & BIT(21)))) {
+			stm32l4_info->dual_bank_mode = true;
+			page_size_kb = 2;
+			num_pages = flash_size_kb / page_size_kb;
+			stm32l4_info->bank1_sectors = num_pages / 2;
+		}
+		break;
 	case 0x495: /* STM32WB5x */
 	case 0x496: /* STM32WB3x */
 		/* single bank flash */
@@ -1086,19 +1218,17 @@ static int get_stm32l4_info(struct flash_bank *bank, char *buf, int buf_size)
 		for (unsigned int i = 0; i < part_info->num_revs; i++) {
 			if (rev_id == part_info->revs[i].rev) {
 				rev_str = part_info->revs[i].str;
-
-				if (rev_str != NULL) {
-					snprintf(buf, buf_size, "%s - Rev: %s%s",
-						part_info->device_str, rev_str, stm32l4_info->probed ?
-							(stm32l4_info->dual_bank_mode ? " dual-bank" : " single-bank") : "");
-					return ERROR_OK;
-				}
+				break;
 			}
 		}
 
-		snprintf(buf, buf_size, "%s - Rev: unknown (0x%04x)%s",
-			part_info->device_str, rev_id, stm32l4_info->probed ?
-				(stm32l4_info->dual_bank_mode ? " dual-bank" : " single-bank") : "");
+		int buf_len = snprintf(buf, buf_size, "%s - Rev %s : 0x%04x",
+				part_info->device_str, rev_str ? rev_str : "'unknown'", rev_id);
+
+		if (stm32l4_info->probed)
+			snprintf(buf + buf_len, buf_size - buf_len, " - %s-bank",
+					stm32l4_info->dual_bank_mode ? "Flash dual" : "Flash single");
+
 		return ERROR_OK;
 	} else {
 		snprintf(buf, buf_size, "Cannot identify target as an %s device", device_families);
@@ -1133,18 +1263,18 @@ static int stm32l4_mass_erase(struct flash_bank *bank)
 	if (retval != ERROR_OK)
 		goto err_lock;
 
-	retval = stm32l4_write_flash_reg(bank, STM32_FLASH_CR, action);
+	retval = stm32l4_write_flash_reg_by_index(bank, STM32_FLASH_CR_INDEX, action);
 	if (retval != ERROR_OK)
 		goto err_lock;
 
-	retval = stm32l4_write_flash_reg(bank, STM32_FLASH_CR, action | FLASH_STRT);
+	retval = stm32l4_write_flash_reg_by_index(bank, STM32_FLASH_CR_INDEX, action | FLASH_STRT);
 	if (retval != ERROR_OK)
 		goto err_lock;
 
 	retval = stm32l4_wait_status_busy(bank, FLASH_ERASE_TIMEOUT);
 
 err_lock:
-	retval2 = stm32l4_write_flash_reg(bank, STM32_FLASH_CR, FLASH_LOCK);
+	retval2 = stm32l4_write_flash_reg_by_index(bank, STM32_FLASH_CR_INDEX, FLASH_LOCK);
 
 	if (retval != ERROR_OK)
 		return retval;
@@ -1257,7 +1387,7 @@ COMMAND_HANDLER(stm32l4_handle_option_load_command)
 	 * "Note: If the read protection is set while the debugger is still
 	 * connected through JTAG/SWD, apply a POR (power-on reset) instead of a system reset."
 	 */
-	retval = stm32l4_write_flash_reg(bank, STM32_FLASH_CR, FLASH_OBL_LAUNCH);
+	retval = stm32l4_write_flash_reg_by_index(bank, STM32_FLASH_CR_INDEX, FLASH_OBL_LAUNCH);
 
 	command_print(CMD, "stm32l4x option load completed. Power-on reset might be required");
 
@@ -1288,7 +1418,8 @@ COMMAND_HANDLER(stm32l4_handle_lock_command)
 	}
 
 	/* set readout protection level 1 by erasing the RDP option byte */
-	if (stm32l4_write_option(bank, STM32_FLASH_OPTR, 0, 0x000000FF) != ERROR_OK) {
+	struct stm32l4_flash_bank *stm32l4_info = bank->driver_priv;
+	if (stm32l4_write_option(bank, stm32l4_info->flash_regs[STM32_FLASH_OPTR_INDEX], 0, 0x000000FF) != ERROR_OK) {
 		command_print(CMD, "%s failed to lock device", bank->driver->name);
 		return ERROR_OK;
 	}
@@ -1315,7 +1446,9 @@ COMMAND_HANDLER(stm32l4_handle_unlock_command)
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
-	if (stm32l4_write_option(bank, STM32_FLASH_OPTR, RDP_LEVEL_0, 0x000000FF) != ERROR_OK) {
+	struct stm32l4_flash_bank *stm32l4_info = bank->driver_priv;
+	if (stm32l4_write_option(bank, stm32l4_info->flash_regs[STM32_FLASH_OPTR_INDEX],
+			RDP_LEVEL_0, 0x000000FF) != ERROR_OK) {
 		command_print(CMD, "%s failed to unlock device", bank->driver->name);
 		return ERROR_OK;
 	}
diff --git a/src/flash/nor/stm32l4x.h b/src/flash/nor/stm32l4x.h
index abd8010..3e810a0 100644
--- a/src/flash/nor/stm32l4x.h
+++ b/src/flash/nor/stm32l4x.h
@@ -19,18 +19,6 @@
 #ifndef OPENOCD_FLASH_NOR_STM32L4X
 #define OPENOCD_FLASH_NOR_STM32L4X
 
-/* Flash registers offsets */
-#define STM32_FLASH_ACR			0x00
-#define STM32_FLASH_KEYR		0x08
-#define STM32_FLASH_OPTKEYR		0x0c
-#define STM32_FLASH_SR			0x10
-#define STM32_FLASH_CR			0x14
-#define STM32_FLASH_OPTR		0x20
-#define STM32_FLASH_WRP1AR		0x2c
-#define STM32_FLASH_WRP1BR		0x30
-#define STM32_FLASH_WRP2AR		0x4c
-#define STM32_FLASH_WRP2BR		0x50
-
 /* FLASH_CR register bits */
 #define FLASH_PG				(1 << 0)
 #define FLASH_PER				(1 << 1)
diff --git a/src/flash/nor/stmqspi.c b/src/flash/nor/stmqspi.c
new file mode 100644
index 0000000..f54e497
--- /dev/null
+++ b/src/flash/nor/stmqspi.c
@@ -0,0 +1,2449 @@
+/***************************************************************************
+ *   Copyright (C) 2016 - 2019 by Andreas Bolsch                           *
+ *   andreas.bolsch@mni.thm.de                                             *
+ *                                                                         *
+ *   Copyright (C) 2010 by Antonio Borneo                                  *
+ *   borneo.antonio@gmail.com                                              *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or	   *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+
+/* STM QuadSPI (QSPI) and OctoSPI (OCTOSPI) controller are SPI bus controllers
+ * specifically designed for SPI memories.
+ * Two working modes are available:
+ * - indirect mode: the SPI is controlled by SW. Any custom commands can be sent
+ *   on the bus.
+ * - memory mapped mode: the SPI is under QSPI/OCTOSPI control. Memory content
+ *   is directly accessible in CPU memory space. CPU can read and execute from
+ *   memory (but not write to) */
+
+/* ATTENTION:
+ * To have flash mapped in CPU memory space, the QSPI/OCTOSPI controller
+ * has to be in "memory mapped mode". This requires following constraints:
+ * 1) The command "reset init" has to initialize QSPI/OCTOSPI controller and put
+ *    it in memory mapped mode;
+ * 2) every command in this file has to return to prompt in memory mapped mode. */
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "imp.h"
+#include <helper/bits.h>
+#include <helper/time_support.h>
+#include <target/algorithm.h>
+#include <target/armv7m.h>
+#include <target/image.h>
+#include "stmqspi.h"
+#include "sfdp.h"
+
+/* deprecated */
+#undef SPIFLASH_READ
+#undef SPIFLASH_PAGE_PROGRAM
+
+#define READ_REG(a)												\
+({																\
+	uint32_t _result;											\
+																\
+	retval = target_read_u32(target, io_base + (a), &_result);	\
+	(retval == ERROR_OK) ? _result : 0x0;						\
+})
+
+/* saved mode settings */
+#define QSPI_MODE (stmqspi_info->saved_ccr & \
+	(0xF0000000U | QSPI_DCYC_MASK | QSPI_4LINE_MODE | QSPI_ALTB_MODE | QSPI_ADDR4))
+
+/* saved read mode settings but indirect read instead of memory mapped
+ * in particular, use the dummy cycle setting from this saved setting */
+#define	QSPI_CCR_READ (QSPI_READ_MODE | (stmqspi_info->saved_ccr & \
+	(0xF0000000U | QSPI_DCYC_MASK | QSPI_4LINE_MODE | QSPI_ALTB_MODE | QSPI_ADDR4 | 0xFF)))
+
+/* QSPI_CCR for various other commands, these never use dummy cycles nor alternate bytes */
+#define	QSPI_CCR_READ_STATUS \
+	((QSPI_MODE & ~QSPI_DCYC_MASK & QSPI_NO_ADDR & QSPI_NO_ALTB) | \
+	(QSPI_READ_MODE | SPIFLASH_READ_STATUS))
+
+#define	QSPI_CCR_READ_ID \
+	((QSPI_MODE & ~QSPI_DCYC_MASK & QSPI_NO_ADDR & QSPI_NO_ALTB) | \
+	(QSPI_READ_MODE | SPIFLASH_READ_ID))
+
+#define	QSPI_CCR_READ_MID \
+	((QSPI_MODE & ~QSPI_DCYC_MASK & QSPI_NO_ADDR & QSPI_NO_ALTB) | \
+	(QSPI_READ_MODE | SPIFLASH_READ_MID))
+
+/* always use 3-byte addresses for read SFDP */
+#define	QSPI_CCR_READ_SFDP \
+	((QSPI_MODE & ~QSPI_DCYC_MASK & ~QSPI_ADDR4 & QSPI_NO_ALTB) | \
+	(QSPI_READ_MODE | QSPI_ADDR3 | SPIFLASH_READ_SFDP))
+
+#define QSPI_CCR_WRITE_ENABLE \
+	((QSPI_MODE & ~QSPI_DCYC_MASK & QSPI_NO_ADDR & QSPI_NO_ALTB & QSPI_NO_DATA) | \
+	(QSPI_WRITE_MODE | SPIFLASH_WRITE_ENABLE))
+
+#define QSPI_CCR_SECTOR_ERASE \
+	((QSPI_MODE & ~QSPI_DCYC_MASK & QSPI_NO_ALTB & QSPI_NO_DATA) | \
+	(QSPI_WRITE_MODE | stmqspi_info->dev.erase_cmd))
+
+#define QSPI_CCR_MASS_ERASE \
+	((QSPI_MODE & ~QSPI_DCYC_MASK & QSPI_NO_ADDR & QSPI_NO_ALTB & QSPI_NO_DATA) | \
+	(QSPI_WRITE_MODE | stmqspi_info->dev.chip_erase_cmd))
+
+#define QSPI_CCR_PAGE_PROG \
+	((QSPI_MODE & ~QSPI_DCYC_MASK & QSPI_NO_ALTB) | \
+	(QSPI_WRITE_MODE | stmqspi_info->dev.pprog_cmd))
+
+/* saved mode settings */
+#define OCTOSPI_MODE (stmqspi_info->saved_cr & 0xCFFFFFFF)
+
+#define OPI_MODE ((stmqspi_info->saved_ccr & OCTOSPI_ISIZE_MASK) != 0)
+
+#define OCTOSPI_MODE_CCR (stmqspi_info->saved_ccr & \
+	(0xF0000000U | OCTOSPI_8LINE_MODE | OCTOSPI_ALTB_MODE | OCTOSPI_ADDR4))
+
+/* use saved ccr for read */
+#define OCTOSPI_CCR_READ OCTOSPI_MODE_CCR
+
+/* OCTOSPI_CCR for various other commands, these never use alternate bytes	*
+ * for READ_STATUS and READ_ID, 4-byte address 0							*
+ * 4 dummy cycles must sent in OPI mode when DQS is disabled. However, when	*
+ * DQS is enabled, some STM32 devices need at least 6 dummy cycles for		*
+ * proper operation, but otherwise the actual number has no effect!			*
+ * E.g. RM0432 Rev. 7 is incorrect regarding this: L4R9 works well with 4	*
+ * dummy clocks whereas L4P5 not at all.									*
+ */
+#define OPI_DUMMY \
+	((stmqspi_info->saved_ccr & OCTOSPI_DQSEN) ? 6U : 4U)
+
+#define	OCTOSPI_CCR_READ_STATUS \
+	((OCTOSPI_MODE_CCR & OCTOSPI_NO_DDTR & \
+	(OPI_MODE ? ~0U : OCTOSPI_NO_ADDR) & OCTOSPI_NO_ALTB))
+
+#define	OCTOSPI_CCR_READ_ID \
+	((OCTOSPI_MODE_CCR & OCTOSPI_NO_DDTR & \
+	(OPI_MODE ? ~0U : OCTOSPI_NO_ADDR) & OCTOSPI_NO_ALTB))
+
+#define	OCTOSPI_CCR_READ_MID OCTOSPI_CCR_READ_ID
+
+/* 4-byte address in octo mode, else 3-byte address for read SFDP */
+#define	OCTOSPI_CCR_READ_SFDP(len) \
+	((OCTOSPI_MODE_CCR & OCTOSPI_NO_DDTR & ~OCTOSPI_ADDR4 & OCTOSPI_NO_ALTB) | \
+	(((len) < 4) ? OCTOSPI_ADDR3 : OCTOSPI_ADDR4))
+
+#define OCTOSPI_CCR_WRITE_ENABLE \
+	((OCTOSPI_MODE_CCR & OCTOSPI_NO_ADDR & OCTOSPI_NO_ALTB & OCTOSPI_NO_DATA))
+
+#define OCTOSPI_CCR_SECTOR_ERASE \
+	((OCTOSPI_MODE_CCR & OCTOSPI_NO_ALTB & OCTOSPI_NO_DATA))
+
+#define OCTOSPI_CCR_MASS_ERASE \
+	((OCTOSPI_MODE_CCR & OCTOSPI_NO_ADDR & OCTOSPI_NO_ALTB & OCTOSPI_NO_DATA))
+
+#define OCTOSPI_CCR_PAGE_PROG \
+	((OCTOSPI_MODE_CCR & QSPI_NO_ALTB))
+
+#define SPI_ADSIZE (((stmqspi_info->saved_ccr >> SPI_ADSIZE_POS) & 0x3) + 1)
+
+#define OPI_CMD(cmd) ((OPI_MODE ? ((((uint16_t)(cmd)) << 8) | (~(cmd) & 0xFFU)) : (cmd)))
+
+#define OCTOSPI_CMD(mode, ccr, ir)										\
+({																		\
+	retval = target_write_u32(target, io_base + OCTOSPI_CR,				\
+		OCTOSPI_MODE | (mode));											\
+	if (retval == ERROR_OK)												\
+		retval = target_write_u32(target, io_base + OCTOSPI_TCR,		\
+			(stmqspi_info->saved_tcr & ~OCTOSPI_DCYC_MASK) |			\
+			((OPI_MODE && ((mode) == OCTOSPI_READ_MODE)) ?				\
+			(OPI_DUMMY << OCTOSPI_DCYC_POS) : 0));						\
+	if (retval == ERROR_OK)												\
+		retval = target_write_u32(target, io_base + OCTOSPI_CCR, ccr);	\
+	if (retval == ERROR_OK)												\
+		retval = target_write_u32(target, io_base + OCTOSPI_IR,			\
+			OPI_CMD(ir));												\
+	retval;																\
+})
+
+/* convert uint32_t into 4 uint8_t in little endian byte order */
+static inline uint32_t h_to_le_32(uint32_t val)
+{
+	uint32_t result;
+
+	h_u32_to_le((uint8_t *)&result, val);
+	return result;
+}
+
+/* Timeout in ms */
+#define SPI_CMD_TIMEOUT			(100)
+#define SPI_PROBE_TIMEOUT		(100)
+#define SPI_MAX_TIMEOUT			(2000)
+#define SPI_MASS_ERASE_TIMEOUT	(400000)
+
+struct sector_info {
+	uint32_t offset;
+	uint32_t size;
+	uint32_t result;
+};
+
+struct stmqspi_flash_bank {
+	bool probed;
+	char devname[32];
+	bool octo;
+	struct flash_device dev;
+	uint32_t io_base;
+	uint32_t saved_cr;	/* in particalar FSEL, DFM bit mask in QUADSPI_CR *AND* OCTOSPI_CR */
+	uint32_t saved_ccr; /* different meaning for QUADSPI and OCTOSPI */
+	uint32_t saved_tcr;	/* only for OCTOSPI */
+	uint32_t saved_ir;	/* only for OCTOSPI */
+	unsigned int sfdp_dummy1;	/* number of dummy bytes for SFDP read for flash1 and octo */
+	unsigned int sfdp_dummy2;	/* number of dummy bytes for SFDP read for flash2 */
+};
+
+FLASH_BANK_COMMAND_HANDLER(stmqspi_flash_bank_command)
+{
+	struct stmqspi_flash_bank *stmqspi_info;
+	uint32_t io_base;
+
+	LOG_DEBUG("%s", __func__);
+
+	if (CMD_ARGC < 7)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[6], io_base);
+
+	stmqspi_info = malloc(sizeof(struct stmqspi_flash_bank));
+	if (stmqspi_info == NULL) {
+		LOG_ERROR("not enough memory");
+		return ERROR_FAIL;
+	}
+
+	bank->driver_priv = stmqspi_info;
+	stmqspi_info->sfdp_dummy1 = 0;
+	stmqspi_info->sfdp_dummy2 = 0;
+	stmqspi_info->probed = false;
+	stmqspi_info->io_base = io_base;
+
+	return ERROR_OK;
+}
+
+/* Poll busy flag */
+/* timeout in ms */
+static int poll_busy(struct flash_bank *bank, int timeout)
+{
+	struct target *target = bank->target;
+	struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
+	uint32_t io_base = stmqspi_info->io_base;
+	uint32_t spi_sr;
+	int retval;
+	long long endtime;
+
+	endtime = timeval_ms() + timeout;
+	do {
+		spi_sr = READ_REG(SPI_SR);
+		if ((spi_sr & BIT(SPI_BUSY)) == 0) {
+			if (retval == ERROR_OK) {
+				/* Clear transmit finished flag */
+				retval = target_write_u32(target, io_base + SPI_FCR, BIT(SPI_TCF));
+			}
+			return retval;
+		} else
+			LOG_DEBUG("busy: 0x%08X", spi_sr);
+		alive_sleep(1);
+	} while (timeval_ms() < endtime);
+
+	LOG_ERROR("Timeout while polling BUSY");
+	return ERROR_FLASH_OPERATION_FAILED;
+}
+
+/* Set to memory-mapped mode, e.g. after an error */
+static int set_mm_mode(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
+	uint32_t io_base = stmqspi_info->io_base;
+	int retval;
+
+	/* Reset Address register bits 0 and 1, see various errata sheets */
+	retval = target_write_u32(target, io_base + SPI_AR, 0x0);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Abort any previous operation */
+	retval = target_write_u32(target, io_base + SPI_CR,
+		READ_REG(SPI_CR) | BIT(SPI_ABORT));
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Wait for busy to be cleared */
+	retval = poll_busy(bank, SPI_PROBE_TIMEOUT);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Finally switch to memory mapped mode */
+	if (IS_OCTOSPI) {
+		retval = target_write_u32(target, io_base + OCTOSPI_CR,
+			OCTOSPI_MODE | OCTOSPI_MM_MODE);
+		if (retval == ERROR_OK)
+			retval = target_write_u32(target, io_base + OCTOSPI_CCR,
+				stmqspi_info->saved_ccr);
+		if (retval == ERROR_OK)
+			retval = target_write_u32(target, io_base + OCTOSPI_TCR,
+				stmqspi_info->saved_tcr);
+		if (retval == ERROR_OK)
+			retval = target_write_u32(target, io_base + OCTOSPI_IR,
+				stmqspi_info->saved_ir);
+	} else {
+		retval = target_write_u32(target, io_base + QSPI_CR,
+			stmqspi_info->saved_cr);
+		if (retval == ERROR_OK)
+			retval = target_write_u32(target, io_base + QSPI_CCR,
+				stmqspi_info->saved_ccr);
+	}
+	return retval;
+}
+
+/* Read the status register of the external SPI flash chip(s). */
+static int read_status_reg(struct flash_bank *bank, uint16_t *status)
+{
+	struct target *target = bank->target;
+	struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
+	uint32_t io_base = stmqspi_info->io_base;
+	uint8_t data;
+	int count, retval;
+
+	/* Abort any previous operation */
+	retval = target_write_u32(target, io_base + SPI_CR,
+		READ_REG(SPI_CR) | BIT(SPI_ABORT));
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Wait for busy to be cleared */
+	retval = poll_busy(bank, SPI_PROBE_TIMEOUT);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Read always two (for DTR mode) bytes per chip */
+	count = 2;
+	retval = target_write_u32(target, io_base + SPI_DLR,
+		((stmqspi_info->saved_cr & BIT(SPI_DUAL_FLASH)) ? 2 * count : count) - 1);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Read status */
+	if (IS_OCTOSPI) {
+		retval = OCTOSPI_CMD(OCTOSPI_READ_MODE, OCTOSPI_CCR_READ_STATUS, SPIFLASH_READ_STATUS);
+		if (OPI_MODE) {
+			/* Dummy address 0, only required for 8-line mode */
+			retval = target_write_u32(target, io_base + SPI_AR, 0);
+			if (retval != ERROR_OK)
+				goto err;
+		}
+	} else
+		retval = target_write_u32(target, io_base + QSPI_CCR, QSPI_CCR_READ_STATUS);
+	if (retval != ERROR_OK)
+		goto err;
+
+	*status = 0;
+
+	/* for debugging only */
+	(void)READ_REG(SPI_SR);
+
+	for ( ; count > 0; --count) {
+		if ((stmqspi_info->saved_cr & (BIT(SPI_DUAL_FLASH) | BIT(SPI_FSEL_FLASH)))
+			!= BIT(SPI_FSEL_FLASH)) {
+			/* get status of flash 1 in dual mode or flash 1 only mode */
+			retval = target_read_u8(target, io_base + SPI_DR, &data);
+			if (retval != ERROR_OK)
+				goto err;
+			*status |= data;
+		}
+
+		if ((stmqspi_info->saved_cr & (BIT(SPI_DUAL_FLASH) | BIT(SPI_FSEL_FLASH))) != 0) {
+			/* get status of flash 2 in dual mode or flash 2 only mode */
+			retval = target_read_u8(target, io_base + SPI_DR, &data);
+			if (retval != ERROR_OK)
+				goto err;
+			*status |= ((uint16_t)data) << 8;
+		}
+	}
+
+	LOG_DEBUG("flash status regs: 0x%04" PRIx16, *status);
+
+err:
+	return retval;
+}
+
+/* check for WIP (write in progress) bit(s) in status register(s) */
+/* timeout in ms */
+static int wait_till_ready(struct flash_bank *bank, int timeout)
+{
+	uint16_t status;
+	int retval;
+	long long endtime;
+
+	endtime = timeval_ms() + timeout;
+	do {
+		/* Read flash status register(s) */
+		retval = read_status_reg(bank, &status);
+		if (retval != ERROR_OK)
+			return retval;
+
+		if ((status & ((SPIFLASH_BSY_BIT << 8) | SPIFLASH_BSY_BIT)) == 0)
+			return retval;
+		alive_sleep(25);
+	} while (timeval_ms() < endtime);
+
+	LOG_ERROR("timeout");
+	return ERROR_FLASH_OPERATION_FAILED;
+}
+
+/* Send "write enable" command to SPI flash chip(s). */
+static int qspi_write_enable(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
+	uint32_t io_base = stmqspi_info->io_base;
+	uint16_t status;
+	int retval;
+
+	/* Abort any previous operation */
+	retval = target_write_u32(target, io_base + SPI_CR,
+		READ_REG(SPI_CR) | BIT(SPI_ABORT));
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Wait for busy to be cleared */
+	retval = poll_busy(bank, SPI_PROBE_TIMEOUT);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Send write enable command */
+	if (IS_OCTOSPI) {
+		retval = OCTOSPI_CMD(OCTOSPI_WRITE_MODE, OCTOSPI_CCR_WRITE_ENABLE, SPIFLASH_WRITE_ENABLE);
+		if (OPI_MODE) {
+			/* Dummy address 0, only required for 8-line mode */
+			retval = target_write_u32(target, io_base + SPI_AR, 0);
+			if (retval != ERROR_OK)
+				goto err;
+		}
+	} else
+		retval = target_write_u32(target, io_base + QSPI_CCR, QSPI_CCR_WRITE_ENABLE);
+	if (retval != ERROR_OK)
+		goto err;
+
+
+	/* Wait for transmit of command completed */
+	poll_busy(bank, SPI_CMD_TIMEOUT);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Read flash status register */
+	retval = read_status_reg(bank, &status);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Check write enabled for flash 1 */
+	if ((stmqspi_info->saved_cr & (BIT(SPI_DUAL_FLASH) | BIT(SPI_FSEL_FLASH)))
+		!= BIT(SPI_FSEL_FLASH))
+		if ((status & (SPIFLASH_WE_BIT | SPIFLASH_BSY_BIT)) != SPIFLASH_WE_BIT) {
+			LOG_ERROR("Cannot write enable flash1. Status=0x%02x",
+				status & 0xFFU);
+			return ERROR_FLASH_OPERATION_FAILED;
+		}
+
+	/* Check write enabled for flash 2 */
+	status >>= 8;
+	if ((stmqspi_info->saved_cr & (BIT(SPI_DUAL_FLASH) | BIT(SPI_FSEL_FLASH))) != 0)
+		if ((status & (SPIFLASH_WE_BIT | SPIFLASH_BSY_BIT)) != SPIFLASH_WE_BIT) {
+			LOG_ERROR("Cannot write enable flash2. Status=0x%02x",
+				status & 0xFFU);
+			return ERROR_FLASH_OPERATION_FAILED;
+		}
+
+err:
+	return retval;
+}
+
+COMMAND_HANDLER(stmqspi_handle_mass_erase_command)
+{
+	struct target *target = NULL;
+	struct flash_bank *bank;
+	struct stmqspi_flash_bank *stmqspi_info;
+	struct duration bench;
+	uint32_t io_base;
+	uint16_t status;
+	unsigned int sector;
+	int retval;
+
+	LOG_DEBUG("%s", __func__);
+
+	if (CMD_ARGC != 1)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	stmqspi_info = bank->driver_priv;
+	target = bank->target;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (!(stmqspi_info->probed)) {
+		LOG_ERROR("Flash bank not probed");
+		return ERROR_FLASH_BANK_NOT_PROBED;
+	}
+
+	if (stmqspi_info->dev.chip_erase_cmd == 0x00) {
+		LOG_ERROR("Mass erase not available for this device");
+		return ERROR_FLASH_OPER_UNSUPPORTED;
+	}
+
+	for (sector = 0; sector < bank->num_sectors; sector++) {
+		if (bank->sectors[sector].is_protected) {
+			LOG_ERROR("Flash sector %u protected", sector);
+			return ERROR_FLASH_PROTECTED;
+		}
+	}
+
+	io_base = stmqspi_info->io_base;
+	duration_start(&bench);
+
+	retval = qspi_write_enable(bank);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Send Mass Erase command */
+	if (IS_OCTOSPI)
+		retval = OCTOSPI_CMD(OCTOSPI_WRITE_MODE, OCTOSPI_CCR_MASS_ERASE,
+			stmqspi_info->dev.chip_erase_cmd);
+	else
+		retval = target_write_u32(target, io_base + QSPI_CCR, QSPI_CCR_MASS_ERASE);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Wait for transmit of command completed */
+	poll_busy(bank, SPI_CMD_TIMEOUT);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Read flash status register(s) */
+	retval = read_status_reg(bank, &status);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Check for command in progress for flash 1 */
+	if (((stmqspi_info->saved_cr & (BIT(SPI_DUAL_FLASH) | BIT(SPI_FSEL_FLASH)))
+		!= BIT(SPI_FSEL_FLASH)) && ((status & SPIFLASH_BSY_BIT) == 0) &&
+		((status & SPIFLASH_WE_BIT) != 0)) {
+		LOG_ERROR("Mass erase command not accepted by flash1. Status=0x%02x",
+			status & 0xFFU);
+		retval = ERROR_FLASH_OPERATION_FAILED;
+		goto err;
+	}
+
+	/* Check for command in progress for flash 2 */
+	status >>= 8;
+	if (((stmqspi_info->saved_cr & (BIT(SPI_DUAL_FLASH) | BIT(SPI_FSEL_FLASH))) != 0) &&
+		((status & SPIFLASH_BSY_BIT) == 0) &&
+		((status & SPIFLASH_WE_BIT) != 0)) {
+		LOG_ERROR("Mass erase command not accepted by flash2. Status=0x%02x",
+			status & 0xFFU);
+		retval = ERROR_FLASH_OPERATION_FAILED;
+		goto err;
+	}
+
+	/* Poll WIP for end of self timed Sector Erase cycle */
+	retval = wait_till_ready(bank, SPI_MASS_ERASE_TIMEOUT);
+
+	duration_measure(&bench);
+	if (retval == ERROR_OK) {
+		/* set all sectors as erased */
+		for (sector = 0; sector < bank->num_sectors; sector++)
+			bank->sectors[sector].is_erased = 1;
+
+		command_print(CMD, "stmqspi mass erase completed in %fs (%0.3f KiB/s)",
+			duration_elapsed(&bench),
+			duration_kbps(&bench, bank->size));
+	} else {
+		command_print(CMD, "stmqspi mass erase not completed even after %fs",
+			duration_elapsed(&bench));
+	}
+
+err:
+	/* Switch to memory mapped mode before return to prompt */
+	set_mm_mode(bank);
+
+	return retval;
+}
+
+static int log2u(uint32_t word)
+{
+	int result;
+
+	for (result = 0; (unsigned int) result < sizeof(uint32_t) * CHAR_BIT; result++)
+		if (word == BIT(result))
+			return result;
+
+	return -1;
+}
+
+COMMAND_HANDLER(stmqspi_handle_set)
+{
+	struct flash_bank *bank = NULL;
+	struct target *target = NULL;
+	struct stmqspi_flash_bank *stmqspi_info = NULL;
+	struct flash_sector *sectors = NULL;
+	uint32_t io_base;
+	unsigned int index = 0, dual, fsize;
+	int retval;
+
+	LOG_DEBUG("%s", __func__);
+
+	dual = (stmqspi_info->saved_cr & BIT(SPI_DUAL_FLASH)) ? 1 : 0;
+
+	/* chip_erase_cmd, sectorsize and erase_cmd are optional */
+	if ((CMD_ARGC < 7) || (CMD_ARGC > 10))
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	retval = CALL_COMMAND_HANDLER(flash_command_get_bank, index++, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	target = bank->target;
+	stmqspi_info = bank->driver_priv;
+
+	/* invalidate all old info */
+	if (stmqspi_info->probed)
+		free(bank->sectors);
+	bank->size = 0;
+	bank->num_sectors = 0;
+	bank->sectors = NULL;
+	stmqspi_info->sfdp_dummy1 = 0;
+	stmqspi_info->sfdp_dummy2 = 0;
+	stmqspi_info->probed = false;
+	memset(&stmqspi_info->dev, 0, sizeof(stmqspi_info->dev));
+	stmqspi_info->dev.name = "unknown";
+
+	strncpy(stmqspi_info->devname, CMD_ARGV[index++], sizeof(stmqspi_info->devname) - 1);
+	stmqspi_info->devname[sizeof(stmqspi_info->devname) - 1] = '\0';
+
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[index++], stmqspi_info->dev.size_in_bytes);
+	if (log2u(stmqspi_info->dev.size_in_bytes) < 8) {
+		command_print(CMD, "stmqspi: device size must be 2^n with n >= 8");
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[index++], stmqspi_info->dev.pagesize);
+	if (stmqspi_info->dev.pagesize > stmqspi_info->dev.size_in_bytes ||
+		(log2u(stmqspi_info->dev.pagesize) < 0)) {
+		command_print(CMD, "stmqspi: page size must be 2^n and <= device size");
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	COMMAND_PARSE_NUMBER(u8, CMD_ARGV[index++], stmqspi_info->dev.read_cmd);
+	if ((stmqspi_info->dev.read_cmd != 0x03) &&
+		(stmqspi_info->dev.read_cmd != 0x13)) {
+		command_print(CMD, "stmqspi: only 0x03/0x13 READ cmd allowed");
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	COMMAND_PARSE_NUMBER(u8, CMD_ARGV[index++], stmqspi_info->dev.qread_cmd);
+	if ((stmqspi_info->dev.qread_cmd != 0x00) &&
+		(stmqspi_info->dev.qread_cmd != 0x0B) &&
+		(stmqspi_info->dev.qread_cmd != 0x0C) &&
+		(stmqspi_info->dev.qread_cmd != 0x3B) &&
+		(stmqspi_info->dev.qread_cmd != 0x3C) &&
+		(stmqspi_info->dev.qread_cmd != 0x6B) &&
+		(stmqspi_info->dev.qread_cmd != 0x6C) &&
+		(stmqspi_info->dev.qread_cmd != 0xBB) &&
+		(stmqspi_info->dev.qread_cmd != 0xBC) &&
+		(stmqspi_info->dev.qread_cmd != 0xEB) &&
+		(stmqspi_info->dev.qread_cmd != 0xEC) &&
+		(stmqspi_info->dev.qread_cmd != 0xEE)) {
+		command_print(CMD, "stmqspi: only 0x0B/0x0C/0x3B/0x3C/"
+			"0x6B/0x6C/0xBB/0xBC/0xEB/0xEC/0xEE QREAD allowed");
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	COMMAND_PARSE_NUMBER(u8, CMD_ARGV[index++], stmqspi_info->dev.pprog_cmd);
+	if ((stmqspi_info->dev.pprog_cmd != 0x02) &&
+		(stmqspi_info->dev.pprog_cmd != 0x12) &&
+		(stmqspi_info->dev.pprog_cmd != 0x32)) {
+		command_print(CMD, "stmqspi: only 0x02/0x12/0x32 PPRG cmd allowed");
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	if (index < CMD_ARGC)
+		COMMAND_PARSE_NUMBER(u8, CMD_ARGV[index++], stmqspi_info->dev.chip_erase_cmd);
+	else
+		stmqspi_info->dev.chip_erase_cmd = 0x00;
+
+	if (index < CMD_ARGC) {
+		COMMAND_PARSE_NUMBER(u32, CMD_ARGV[index++], stmqspi_info->dev.sectorsize);
+		if ((stmqspi_info->dev.sectorsize > stmqspi_info->dev.size_in_bytes) ||
+			(stmqspi_info->dev.sectorsize < stmqspi_info->dev.pagesize) ||
+			(log2u(stmqspi_info->dev.sectorsize) < 0)) {
+			command_print(CMD, "stmqspi: sector size must be 2^n and <= device size");
+			return ERROR_COMMAND_SYNTAX_ERROR;
+		}
+
+		if (index < CMD_ARGC)
+			COMMAND_PARSE_NUMBER(u8, CMD_ARGV[index++], stmqspi_info->dev.erase_cmd);
+		else
+			return ERROR_COMMAND_SYNTAX_ERROR;
+	} else {
+		/* no sector size / sector erase cmd given, treat whole bank as a single sector */
+		stmqspi_info->dev.erase_cmd = 0x00;
+		stmqspi_info->dev.sectorsize = stmqspi_info->dev.size_in_bytes;
+	}
+
+	/* set correct size value */
+	bank->size = stmqspi_info->dev.size_in_bytes << dual;
+
+	io_base = stmqspi_info->io_base;
+	fsize = (READ_REG(SPI_DCR) >> SPI_FSIZE_POS) & (BIT(SPI_FSIZE_LEN) - 1);
+	if (retval != ERROR_OK)
+		return retval;
+
+	LOG_DEBUG("FSIZE = 0x%04x", fsize);
+	if (bank->size == BIT(fsize + 1))
+		LOG_DEBUG("FSIZE in DCR(1) matches actual capacity. Beware of silicon bug in H7, L4+, MP1.");
+	else if (bank->size == BIT(fsize + 0))
+		LOG_DEBUG("FSIZE in DCR(1) is off by one regarding actual capacity. Fix for silicon bug?");
+	else
+		LOG_ERROR("FSIZE in DCR(1) doesn't match actual capacity.");
+
+	/* create and fill sectors array */
+	bank->num_sectors =
+		stmqspi_info->dev.size_in_bytes / stmqspi_info->dev.sectorsize;
+	sectors = malloc(sizeof(struct flash_sector) * bank->num_sectors);
+	if (sectors == NULL) {
+		LOG_ERROR("not enough memory");
+		return ERROR_FAIL;
+	}
+
+	for (unsigned int sector = 0; sector < bank->num_sectors; sector++) {
+		sectors[sector].offset = sector * (stmqspi_info->dev.sectorsize << dual);
+		sectors[sector].size = (stmqspi_info->dev.sectorsize << dual);
+		sectors[sector].is_erased = -1;
+		sectors[sector].is_protected = 0;
+	}
+
+	bank->sectors = sectors;
+	stmqspi_info->dev.name = stmqspi_info->devname;
+	if (stmqspi_info->dev.size_in_bytes / 4096)
+		LOG_INFO("flash \'%s\' id = unknown\nchip size = %" PRIu32 "kbytes,"
+			" bank size = %" PRIu32 "kbytes", stmqspi_info->dev.name,
+			stmqspi_info->dev.size_in_bytes / 1024,
+			(stmqspi_info->dev.size_in_bytes / 1024) << dual);
+	else
+		LOG_INFO("flash \'%s\' id = unknown\nchip size = %" PRIu32 "bytes,"
+			" bank size = %" PRIu32 "bytes", stmqspi_info->dev.name,
+			stmqspi_info->dev.size_in_bytes,
+			stmqspi_info->dev.size_in_bytes << dual);
+
+	stmqspi_info->probed = true;
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(stmqspi_handle_cmd)
+{
+	struct target *target = NULL;
+	struct flash_bank *bank;
+	struct stmqspi_flash_bank *stmqspi_info = NULL;
+	uint32_t io_base, addr;
+	uint8_t num_write, num_read, cmd_byte, data;
+	unsigned int count;
+	const int max = 21;
+	char temp[4], output[(2 + max + 256) * 3 + 8];
+	int retval;
+
+	LOG_DEBUG("%s", __func__);
+
+	if (CMD_ARGC < 3)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	num_write = CMD_ARGC - 2;
+	if (num_write > max) {
+		LOG_ERROR("at most %d bytes may be sent", max);
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	target = bank->target;
+	stmqspi_info = bank->driver_priv;
+	io_base = stmqspi_info->io_base;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	COMMAND_PARSE_NUMBER(u8, CMD_ARGV[1], num_read);
+	COMMAND_PARSE_NUMBER(u8, CMD_ARGV[2], cmd_byte);
+
+	if (num_read == 0) {
+		/* nothing to read, then one command byte and for dual flash
+		 * an *even* number of data bytes to follow */
+		if (stmqspi_info->saved_cr & BIT(SPI_DUAL_FLASH)) {
+			if ((num_write & 1) == 0) {
+				LOG_ERROR("number of data bytes to write must be even in dual mode");
+				return ERROR_COMMAND_SYNTAX_ERROR;
+			}
+		}
+	} else {
+		/* read mode, one command byte and up to four following address bytes */
+		if (stmqspi_info->saved_cr & BIT(SPI_DUAL_FLASH)) {
+			if ((num_read & 1) != 0) {
+				LOG_ERROR("number of bytes to read must be even in dual mode");
+				return ERROR_COMMAND_SYNTAX_ERROR;
+			}
+		}
+		if ((num_write < 1) || (num_write > 5)) {
+			LOG_ERROR("one cmd and up to four addr bytes must be send when reading");
+			return ERROR_COMMAND_SYNTAX_ERROR;
+		}
+	}
+
+	/* Abort any previous operation */
+	retval = target_write_u32(target, io_base + SPI_CR,
+		READ_REG(SPI_CR) | BIT(SPI_ABORT));
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Wait for busy to be cleared */
+	retval = poll_busy(bank, SPI_PROBE_TIMEOUT);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* send command byte */
+	snprintf(output, sizeof(output), "spi: %02x ", cmd_byte);
+	if (num_read == 0) {
+		/* write, send cmd byte */
+		retval = target_write_u32(target, io_base + SPI_DLR, ((uint32_t)num_write) - 2);
+		if (retval != ERROR_OK)
+			goto err;
+
+		if (IS_OCTOSPI)
+			retval = OCTOSPI_CMD(OCTOSPI_WRITE_MODE,
+				(OCTOSPI_MODE_CCR & OCTOSPI_NO_ALTB & OCTOSPI_NO_ADDR &
+				((num_write == 1) ? OCTOSPI_NO_DATA : ~0U)), cmd_byte);
+		else
+			retval = target_write_u32(target, io_base + QSPI_CCR,
+				(QSPI_MODE & ~QSPI_DCYC_MASK & QSPI_NO_ALTB & QSPI_NO_ADDR &
+				((num_write == 1) ? QSPI_NO_DATA : ~0U)) |
+				(QSPI_WRITE_MODE | cmd_byte));
+		if (retval != ERROR_OK)
+			goto err;
+
+		/* send additional data bytes */
+		for (count = 3; count < CMD_ARGC; count++) {
+			COMMAND_PARSE_NUMBER(u8, CMD_ARGV[count], data);
+			snprintf(temp, sizeof(temp), "%02" PRIx8 " ", data);
+			retval = target_write_u8(target, io_base + SPI_DR, data);
+			if (retval != ERROR_OK)
+				goto err;
+			strncat(output, temp, sizeof(output) - strlen(output) - 1);
+		}
+		strncat(output, "-> ", sizeof(output) - strlen(output) - 1);
+	} else {
+		/* read, pack additional bytes into address */
+		addr = 0;
+		for (count = 3; count < CMD_ARGC; count++) {
+			COMMAND_PARSE_NUMBER(u8, CMD_ARGV[count], data);
+			snprintf(temp, sizeof(temp), "%02" PRIx8 " ", data);
+			addr = (addr << 8) | data;
+			strncat(output, temp, sizeof(output) - strlen(output) - 1);
+		}
+		strncat(output, "-> ", sizeof(output) - strlen(output) - 1);
+
+		/* send cmd byte, if ADMODE indicates no address, this already triggers command */
+		retval = target_write_u32(target, io_base + SPI_DLR, ((uint32_t)num_read) - 1);
+		if (retval != ERROR_OK)
+			goto err;
+		if (IS_OCTOSPI)
+			retval = OCTOSPI_CMD(OCTOSPI_READ_MODE,
+				(OCTOSPI_MODE_CCR & OCTOSPI_NO_DDTR & OCTOSPI_NO_ALTB & ~OCTOSPI_ADDR4 &
+					((num_write == 1) ? OCTOSPI_NO_ADDR : ~0U)) |
+				(((num_write - 2) & 0x3U) << SPI_ADSIZE_POS), cmd_byte);
+		else
+			retval = target_write_u32(target, io_base + QSPI_CCR,
+				(QSPI_MODE & ~QSPI_DCYC_MASK & QSPI_NO_ALTB & ~QSPI_ADDR4 &
+					((num_write == 1) ? QSPI_NO_ADDR : ~0U)) |
+				((QSPI_READ_MODE | (((num_write - 2) & 0x3U) << SPI_ADSIZE_POS) | cmd_byte)));
+		if (retval != ERROR_OK)
+			goto err;
+
+		if (num_write > 1) {
+			/* if ADMODE indicates address required, only the write to AR triggers command */
+			retval = target_write_u32(target, io_base + SPI_AR, addr);
+			if (retval != ERROR_OK)
+				goto err;
+		}
+
+		/* read response bytes */
+		for ( ; num_read > 0; num_read--) {
+			retval = target_read_u8(target, io_base + SPI_DR, &data);
+			if (retval != ERROR_OK)
+				goto err;
+			snprintf(temp, sizeof(temp), "%02" PRIx8 " ", data);
+			strncat(output, temp, sizeof(output) - strlen(output) - 1);
+		}
+	}
+	command_print(CMD, "%s", output);
+
+err:
+	/* Switch to memory mapped mode before return to prompt */
+	set_mm_mode(bank);
+
+	return retval;
+}
+
+static int qspi_erase_sector(struct flash_bank *bank, unsigned int sector)
+{
+	struct target *target = bank->target;
+	struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
+	uint32_t io_base = stmqspi_info->io_base;
+	uint16_t status;
+	int retval;
+
+	retval = qspi_write_enable(bank);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Send Sector Erase command */
+	if (IS_OCTOSPI)
+		retval = OCTOSPI_CMD(OCTOSPI_WRITE_MODE, OCTOSPI_CCR_SECTOR_ERASE,
+			stmqspi_info->dev.erase_cmd);
+	else
+		retval = target_write_u32(target, io_base + QSPI_CCR, QSPI_CCR_SECTOR_ERASE);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Address is sector offset, this write initiates command transmission */
+	retval = target_write_u32(target, io_base + SPI_AR, bank->sectors[sector].offset);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Wait for transmit of command completed */
+	poll_busy(bank, SPI_CMD_TIMEOUT);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Read flash status register(s) */
+	retval = read_status_reg(bank, &status);
+	if (retval != ERROR_OK)
+		goto err;
+
+	LOG_DEBUG("erase status regs: 0x%04" PRIx16, status);
+
+	/* Check for command in progress for flash 1 */
+	/* If BSY and WE are already cleared the erase did probably complete already */
+	if (((stmqspi_info->saved_cr & (BIT(SPI_DUAL_FLASH) | BIT(SPI_FSEL_FLASH)))
+		!= BIT(SPI_FSEL_FLASH)) && ((status & SPIFLASH_BSY_BIT) == 0) &&
+		((status & SPIFLASH_WE_BIT) != 0)) {
+		LOG_ERROR("Sector erase command not accepted by flash1. Status=0x%02x",
+			status & 0xFFU);
+		retval = ERROR_FLASH_OPERATION_FAILED;
+		goto err;
+	}
+
+	/* Check for command in progress for flash 2 */
+	/* If BSY and WE are already cleared the erase did probably complete already */
+	status >>= 8;
+	if (((stmqspi_info->saved_cr & (BIT(SPI_DUAL_FLASH) | BIT(SPI_FSEL_FLASH))) != 0) &&
+		((status & SPIFLASH_BSY_BIT) == 0) &&
+		((status & SPIFLASH_WE_BIT) != 0)) {
+		LOG_ERROR("Sector erase command not accepted by flash2. Status=0x%02x",
+			status & 0xFFU);
+		retval = ERROR_FLASH_OPERATION_FAILED;
+		goto err;
+	}
+
+	/* Erase takes a long time, so some sort of progress message is a good idea */
+	LOG_DEBUG("erasing sector %4u", sector);
+
+	/* Poll WIP for end of self timed Sector Erase cycle */
+	retval = wait_till_ready(bank, SPI_MAX_TIMEOUT);
+
+err:
+	return retval;
+}
+
+static int stmqspi_erase(struct flash_bank *bank, unsigned int first, unsigned int last)
+{
+	struct target *target = bank->target;
+	struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
+	unsigned int sector;
+	int retval = ERROR_OK;
+
+	LOG_DEBUG("%s: from sector %u to sector %u", __func__, first, last);
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (!(stmqspi_info->probed)) {
+		LOG_ERROR("Flash bank not probed");
+		return ERROR_FLASH_BANK_NOT_PROBED;
+	}
+
+	if (stmqspi_info->dev.erase_cmd == 0x00) {
+		LOG_ERROR("Sector erase not available for this device");
+		return ERROR_FLASH_OPER_UNSUPPORTED;
+	}
+
+	if ((last < first) || (last >= bank->num_sectors)) {
+		LOG_ERROR("Flash sector invalid");
+		return ERROR_FLASH_SECTOR_INVALID;
+	}
+
+	for (sector = first; sector <= last; sector++) {
+		if (bank->sectors[sector].is_protected) {
+			LOG_ERROR("Flash sector %u protected", sector);
+			return ERROR_FLASH_PROTECTED;
+		}
+	}
+
+	for (sector = first; sector <= last; sector++) {
+		retval = qspi_erase_sector(bank, sector);
+		if (retval != ERROR_OK)
+			break;
+		alive_sleep(10);
+		keep_alive();
+	}
+
+	if (retval != ERROR_OK)
+		LOG_ERROR("Flash sector_erase failed on sector %u", sector);
+
+	/* Switch to memory mapped mode before return to prompt */
+	set_mm_mode(bank);
+
+	return retval;
+}
+
+static int stmqspi_protect(struct flash_bank *bank, int set,
+	unsigned int first, unsigned int last)
+{
+	unsigned int sector;
+
+	for (sector = first; sector <= last; sector++)
+		bank->sectors[sector].is_protected = set;
+
+	if (set)
+		LOG_WARNING("setting soft protection only, not related to flash's hardware write protection");
+
+	return ERROR_OK;
+}
+
+/* Check whether flash is blank */
+static int stmqspi_blank_check(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
+	uint32_t io_base = stmqspi_info->io_base;
+	struct duration bench;
+	struct reg_param reg_params[2];
+	struct armv7m_algorithm armv7m_info;
+	struct working_area *algorithm;
+	const uint8_t *code;
+	struct sector_info erase_check_info;
+	uint32_t codesize, maxsize, result, exit_point;
+	unsigned int count, index, num_sectors, sector;
+	int retval;
+	const uint32_t erased = 0x00FF;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (!(stmqspi_info->probed)) {
+		LOG_ERROR("Flash bank not probed");
+		return ERROR_FLASH_BANK_NOT_PROBED;
+	}
+
+	/* Abort any previous operation */
+	retval = target_write_u32(target, io_base + SPI_CR,
+		READ_REG(SPI_CR) | BIT(SPI_ABORT));
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Wait for busy to be cleared */
+	retval = poll_busy(bank, SPI_PROBE_TIMEOUT);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* see contrib/loaders/flash/stmqspi/stmqspi_erase_check.S for src */
+	static const uint8_t stmqspi_erase_check_code[] = {
+		#include "../../../contrib/loaders/flash/stmqspi/stmqspi_erase_check.inc"
+	};
+
+	/* see contrib/loaders/flash/stmqspi/stmoctospi_erase_check.S for src */
+	static const uint8_t stmoctospi_erase_check_code[] = {
+		#include "../../../contrib/loaders/flash/stmqspi/stmoctospi_erase_check.inc"
+	};
+
+	if (IS_OCTOSPI) {
+		code = stmoctospi_erase_check_code;
+		codesize = sizeof(stmoctospi_erase_check_code);
+	} else {
+		code = stmqspi_erase_check_code;
+		codesize = sizeof(stmqspi_erase_check_code);
+	}
+
+	/* This will overlay the last 4 words of stmqspi/stmoctospi_erase_check_code in target */
+	/* for read use the saved settings (memory mapped mode) but indirect read mode */
+	uint32_t ccr_buffer[][4] = {
+		/* cr  (not used for QSPI)			*
+		 * ccr (for both QSPI and OCTOSPI)	*
+		 * tcr (not used for QSPI)			*
+		 * ir  (not used for QSPI)			*/
+		{
+			h_to_le_32(OCTOSPI_MODE | OCTOSPI_READ_MODE),
+			h_to_le_32(IS_OCTOSPI ? OCTOSPI_CCR_READ : QSPI_CCR_READ),
+			h_to_le_32(stmqspi_info->saved_tcr),
+			h_to_le_32(stmqspi_info->saved_ir),
+		},
+	};
+
+	maxsize = target_get_working_area_avail(target);
+	if (maxsize < codesize + sizeof(erase_check_info)) {
+		LOG_ERROR("Not enough working area, can't do QSPI blank check");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+
+	num_sectors = (maxsize - codesize) / sizeof(erase_check_info);
+	num_sectors = (bank->num_sectors < num_sectors) ? bank->num_sectors : num_sectors;
+
+	if (target_alloc_working_area_try(target,
+			codesize + num_sectors * sizeof(erase_check_info), &algorithm) != ERROR_OK) {
+		LOG_ERROR("allocating working area failed");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	};
+
+	/* prepare blank check code, excluding ccr_buffer */
+	retval = target_write_buffer(target, algorithm->address,
+		codesize - sizeof(ccr_buffer), code);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* prepare QSPI/OCTOSPI_CCR register values */
+	retval = target_write_buffer(target, algorithm->address
+		+ codesize - sizeof(ccr_buffer),
+		sizeof(ccr_buffer), (uint8_t *)ccr_buffer);
+	if (retval != ERROR_OK)
+		goto err;
+
+	duration_start(&bench);
+
+	/* after breakpoint instruction (halfword), one nop (halfword) and
+	 * port_buffer till end of code */
+	exit_point = algorithm->address + codesize - sizeof(uint32_t) - sizeof(ccr_buffer);
+
+	init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);	/* sector count */
+	init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);	/* QSPI/OCTOSPI io_base */
+
+	sector = 0;
+	while (sector < bank->num_sectors) {
+		/* at most num_sectors sectors to handle in one run */
+		count = bank->num_sectors - sector;
+		if (count > num_sectors)
+			count = num_sectors;
+
+		for (index = 0; index < count; index++) {
+			erase_check_info.offset = h_to_le_32(bank->sectors[sector + index].offset);
+			erase_check_info.size = h_to_le_32(bank->sectors[sector + index].size);
+			erase_check_info.result = h_to_le_32(erased);
+
+			retval = target_write_buffer(target, algorithm->address
+				+ codesize + index * sizeof(erase_check_info),
+					sizeof(erase_check_info), (uint8_t *)&erase_check_info);
+			if (retval != ERROR_OK)
+				goto err;
+		}
+
+		buf_set_u32(reg_params[0].value, 0, 32, count);
+		buf_set_u32(reg_params[1].value, 0, 32, stmqspi_info->io_base);
+
+		armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
+		armv7m_info.core_mode = ARM_MODE_THREAD;
+
+		LOG_DEBUG("checking sectors %u to %u", sector, sector + count - 1);
+		/* check a block of sectors */
+		retval = target_run_algorithm(target,
+			0, NULL,
+			ARRAY_SIZE(reg_params), reg_params,
+			algorithm->address, exit_point,
+			count * ((bank->sectors[sector].size >> 6) + 1) + 1000,
+			&armv7m_info);
+		if (retval != ERROR_OK)
+			break;
+
+		for (index = 0; index < count; index++) {
+			retval = target_read_buffer(target, algorithm->address
+				+ codesize + index * sizeof(erase_check_info),
+					sizeof(erase_check_info), (uint8_t *)&erase_check_info);
+			if (retval != ERROR_OK)
+				goto err;
+
+			if ((erase_check_info.offset != h_to_le_32(bank->sectors[sector + index].offset)) ||
+				(erase_check_info.size != 0)) {
+				LOG_ERROR("corrupted blank check info");
+				goto err;
+			}
+
+			/* we need le_32_to_h, but that's the same as h_to_le_32 */
+			result = h_to_le_32(erase_check_info.result);
+			bank->sectors[sector + index].is_erased = ((result & 0xFF) == 0xFF);
+			LOG_DEBUG("Flash sector %u checked: 0x%04x", sector + index, result & 0xFFFFU);
+		}
+		keep_alive();
+		sector += count;
+	}
+
+	destroy_reg_param(&reg_params[0]);
+	destroy_reg_param(&reg_params[1]);
+
+	duration_measure(&bench);
+	LOG_INFO("stmqspi blank checked in %fs (%0.3f KiB/s)", duration_elapsed(&bench),
+		duration_kbps(&bench, bank->size));
+
+err:
+	target_free_working_area(target, algorithm);
+
+	/* Switch to memory mapped mode before return to prompt */
+	set_mm_mode(bank);
+
+	return retval;
+}
+
+/* Verify checksum */
+static int qspi_verify(struct flash_bank *bank, uint8_t *buffer,
+	uint32_t offset, uint32_t count)
+{
+	struct target *target = bank->target;
+	struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
+	struct reg_param reg_params[4];
+	struct armv7m_algorithm armv7m_info;
+	struct working_area *algorithm;
+	const uint8_t *code;
+	uint32_t pagesize, codesize, crc32, result, exit_point;
+	int retval;
+
+	/* see contrib/loaders/flash/stmqspi/stmqspi_crc32.S for src */
+	static const uint8_t stmqspi_crc32_code[] = {
+		#include "../../../contrib/loaders/flash/stmqspi/stmqspi_crc32.inc"
+	};
+
+	/* see contrib/loaders/flash/stmqspi/stmoctospi_crc32.S for src */
+	static const uint8_t stmoctospi_crc32_code[] = {
+		#include "../../../contrib/loaders/flash/stmqspi/stmoctospi_crc32.inc"
+	};
+
+	if (IS_OCTOSPI) {
+		code = stmoctospi_crc32_code;
+		codesize = sizeof(stmoctospi_crc32_code);
+	} else {
+		code = stmqspi_crc32_code;
+		codesize = sizeof(stmqspi_crc32_code);
+	}
+
+	/* block size doesn't matter that much here */
+	pagesize = stmqspi_info->dev.sectorsize;
+	if (pagesize == 0)
+		pagesize = stmqspi_info->dev.pagesize;
+	if (pagesize == 0)
+		pagesize = SPIFLASH_DEF_PAGESIZE;
+
+	/* adjust size according to dual flash mode */
+	pagesize = (stmqspi_info->saved_cr & BIT(SPI_DUAL_FLASH)) ? pagesize << 1 : pagesize;
+
+	/* This will overlay the last 4 words of stmqspi/stmoctospi_crc32_code in target */
+	/* for read use the saved settings (memory mapped mode) but indirect read mode */
+	uint32_t ccr_buffer[][4] = {
+		/* cr  (not used for QSPI)			*
+		 * ccr (for both QSPI and OCTOSPI)	*
+		 * tcr (not used for QSPI)			*
+		 * ir  (not used for QSPI)			*/
+		{
+			h_to_le_32(OCTOSPI_MODE | OCTOSPI_READ_MODE),
+			h_to_le_32(IS_OCTOSPI ? OCTOSPI_CCR_READ : QSPI_CCR_READ),
+			h_to_le_32(stmqspi_info->saved_tcr),
+			h_to_le_32(stmqspi_info->saved_ir),
+		},
+	};
+
+	if (target_alloc_working_area_try(target, codesize, &algorithm) != ERROR_OK) {
+		LOG_ERROR("Not enough working area, can't do QSPI verify");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	};
+
+	/* prepare verify code, excluding ccr_buffer */
+	retval = target_write_buffer(target, algorithm->address,
+		codesize - sizeof(ccr_buffer), code);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* prepare QSPI/OCTOSPI_CCR register values */
+	retval = target_write_buffer(target, algorithm->address
+		+ codesize - sizeof(ccr_buffer),
+		sizeof(ccr_buffer), (uint8_t *)ccr_buffer);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* after breakpoint instruction (halfword), one nop (halfword) and
+	 * port_buffer till end of code */
+	exit_point = algorithm->address + codesize - sizeof(uint32_t) - sizeof(ccr_buffer);
+
+	init_reg_param(&reg_params[0], "r0", 32, PARAM_IN_OUT); /* count (in), crc32 (out) */
+	init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);	/* pagesize */
+	init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);	/* offset into flash address */
+	init_reg_param(&reg_params[3], "r3", 32, PARAM_OUT);	/* QSPI/OCTOSPI io_base */
+
+	buf_set_u32(reg_params[0].value, 0, 32, count);
+	buf_set_u32(reg_params[1].value, 0, 32, pagesize);
+	buf_set_u32(reg_params[2].value, 0, 32, offset);
+	buf_set_u32(reg_params[3].value, 0, 32, stmqspi_info->io_base);
+
+
+	armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
+	armv7m_info.core_mode = ARM_MODE_THREAD;
+
+	retval = target_run_algorithm(target,
+		0, NULL,
+		ARRAY_SIZE(reg_params), reg_params,
+		algorithm->address, exit_point,
+		(count >> 5) + 1000,
+		&armv7m_info);
+	keep_alive();
+
+	image_calculate_checksum(buffer, count, &crc32);
+
+	if (retval == ERROR_OK) {
+		result = buf_get_u32(reg_params[0].value, 0, 32);
+		LOG_DEBUG("addr " TARGET_ADDR_FMT ", len 0x%08" PRIx32 ", crc 0x%08" PRIx32 " 0x%08" PRIx32,
+			offset + bank->base, count, ~crc32, result);
+		if (~crc32 != result)
+			retval = ERROR_FAIL;
+	}
+
+	destroy_reg_param(&reg_params[0]);
+	destroy_reg_param(&reg_params[1]);
+	destroy_reg_param(&reg_params[2]);
+	destroy_reg_param(&reg_params[3]);
+
+err:
+	target_free_working_area(target, algorithm);
+
+	/* Switch to memory mapped mode before return to prompt */
+	set_mm_mode(bank);
+
+	return retval;
+}
+
+static int qspi_read_write_block(struct flash_bank *bank, uint8_t *buffer,
+	uint32_t offset, uint32_t count, bool write)
+{
+	struct target *target = bank->target;
+	struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
+	uint32_t io_base = stmqspi_info->io_base;
+	struct reg_param reg_params[6];
+	struct armv7m_algorithm armv7m_info;
+	struct working_area *algorithm;
+	uint32_t pagesize, fifo_start, fifosize, remaining;
+	uint32_t maxsize, codesize, exit_point;
+	const uint8_t *code = NULL;
+	unsigned int dual;
+	int retval;
+
+	LOG_DEBUG("%s: offset=0x%08" PRIx32 " len=0x%08" PRIx32,
+		__func__, offset, count);
+
+	dual = (stmqspi_info->saved_cr & BIT(SPI_DUAL_FLASH)) ? 1 : 0;
+
+	/* see contrib/loaders/flash/stmqspi/stmqspi_read.S for src */
+	static const uint8_t stmqspi_read_code[] = {
+#include "../../../contrib/loaders/flash/stmqspi/stmqspi_read.inc"
+	};
+
+	/* see contrib/loaders/flash/stmqspi/stmoctospi_read.S for src */
+	static const uint8_t stmoctospi_read_code[] = {
+#include "../../../contrib/loaders/flash/stmqspi/stmoctospi_read.inc"
+	};
+
+	/* see contrib/loaders/flash/stmqspi/stmqspi_write.S for src */
+	static const uint8_t stmqspi_write_code[] = {
+#include "../../../contrib/loaders/flash/stmqspi/stmqspi_write.inc"
+	};
+
+	/* see contrib/loaders/flash/stmqspi/stmoctospi_write.S for src */
+	static const uint8_t stmoctospi_write_code[] = {
+#include "../../../contrib/loaders/flash/stmqspi/stmoctospi_write.inc"
+	};
+
+	/* This will overlay the last 12 words of stmqspi/stmoctospi_read/write_code in target */
+	/* for read use the saved settings (memory mapped mode) but indirect read mode */
+	uint32_t ccr_buffer[][4] = {
+		/* cr  (not used for QSPI)			*
+		 * ccr (for both QSPI and OCTOSPI)	*
+		 * tcr (not used for QSPI)			*
+		 * ir  (not used for QSPI)			*/
+		{
+			h_to_le_32(OCTOSPI_MODE | OCTOSPI_READ_MODE),
+			h_to_le_32(IS_OCTOSPI ? OCTOSPI_CCR_READ_STATUS : QSPI_CCR_READ_STATUS),
+			h_to_le_32((stmqspi_info->saved_tcr & ~OCTOSPI_DCYC_MASK) |
+						(OPI_MODE ? (OPI_DUMMY << OCTOSPI_DCYC_POS) : 0)),
+			h_to_le_32(OPI_CMD(SPIFLASH_READ_STATUS)),
+		},
+		{
+			h_to_le_32(OCTOSPI_MODE | OCTOSPI_WRITE_MODE),
+			h_to_le_32(IS_OCTOSPI ? OCTOSPI_CCR_WRITE_ENABLE : QSPI_CCR_WRITE_ENABLE),
+			h_to_le_32(stmqspi_info->saved_tcr & ~OCTOSPI_DCYC_MASK),
+			h_to_le_32(OPI_CMD(SPIFLASH_WRITE_ENABLE)),
+		},
+		{
+			h_to_le_32(OCTOSPI_MODE | (write ? OCTOSPI_WRITE_MODE : OCTOSPI_READ_MODE)),
+			h_to_le_32(write ? (IS_OCTOSPI ? OCTOSPI_CCR_PAGE_PROG : QSPI_CCR_PAGE_PROG) :
+				(IS_OCTOSPI ? OCTOSPI_CCR_READ : QSPI_CCR_READ)),
+			h_to_le_32(write ? (stmqspi_info->saved_tcr & ~OCTOSPI_DCYC_MASK) :
+				stmqspi_info->saved_tcr),
+			h_to_le_32(write ? OPI_CMD(stmqspi_info->dev.pprog_cmd) : stmqspi_info->saved_ir),
+		},
+	};
+
+	/* force reasonable defaults */
+	fifosize = stmqspi_info->dev.sectorsize ?
+		stmqspi_info->dev.sectorsize : stmqspi_info->dev.size_in_bytes;
+
+	if (write) {
+		if (IS_OCTOSPI) {
+			code = stmoctospi_write_code;
+			codesize = sizeof(stmoctospi_write_code);
+		} else {
+			code = stmqspi_write_code;
+			codesize = sizeof(stmqspi_write_code);
+		}
+	} else {
+		if (IS_OCTOSPI) {
+			code = stmoctospi_read_code;
+			codesize = sizeof(stmoctospi_read_code);
+		} else {
+			code = stmqspi_read_code;
+			codesize = sizeof(stmqspi_read_code);
+		}
+	}
+
+	/* for write, pagesize must be taken into account */
+	/* for read, the page size doesn't matter that much */
+	pagesize = stmqspi_info->dev.pagesize;
+	if (pagesize == 0)
+		pagesize = (fifosize <= SPIFLASH_DEF_PAGESIZE) ?
+			fifosize : SPIFLASH_DEF_PAGESIZE;
+
+	/* adjust sizes according to dual flash mode */
+	pagesize <<= dual;
+	fifosize <<= dual;
+
+	/* memory buffer, we assume sectorsize to be a power of 2 times pagesize */
+	maxsize = target_get_working_area_avail(target);
+	if (maxsize < codesize + 2 * sizeof(uint32_t) + pagesize) {
+		LOG_ERROR("not enough working area, can't do QSPI page reads/writes");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+
+	/* fifo size at most sector size, and multiple of page size */
+	maxsize -= (codesize + 2 * sizeof(uint32_t));
+	fifosize = ((maxsize < fifosize) ? maxsize : fifosize) & ~(pagesize - 1);
+
+	if (target_alloc_working_area_try(target,
+		codesize + 2 * sizeof(uint32_t) + fifosize, &algorithm) != ERROR_OK) {
+		LOG_ERROR("allocating working area failed");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	};
+
+	/* prepare flash write code, excluding ccr_buffer */
+	retval = target_write_buffer(target, algorithm->address,
+		codesize - sizeof(ccr_buffer), code);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* prepare QSPI/OCTOSPI_CCR register values */
+	retval = target_write_buffer(target, algorithm->address
+		+ codesize - sizeof(ccr_buffer),
+		sizeof(ccr_buffer), (uint8_t *)ccr_buffer);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* target buffer starts right after flash_write_code, i.e.
+	 * wp and rp are implicitly included in buffer!!! */
+	fifo_start = algorithm->address + codesize + 2 * sizeof(uint32_t);
+
+	init_reg_param(&reg_params[0], "r0", 32, PARAM_IN_OUT); /* count (in), status (out) */
+	init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);	/* pagesize */
+	init_reg_param(&reg_params[2], "r2", 32, PARAM_IN_OUT);	/* offset into flash address */
+	init_reg_param(&reg_params[3], "r3", 32, PARAM_OUT);	/* QSPI/OCTOSPI io_base */
+	init_reg_param(&reg_params[4], "r8", 32, PARAM_OUT);	/* fifo start */
+	init_reg_param(&reg_params[5], "r9", 32, PARAM_OUT);	/* fifo end + 1 */
+
+	buf_set_u32(reg_params[0].value, 0, 32, count);
+	buf_set_u32(reg_params[1].value, 0, 32, pagesize);
+	buf_set_u32(reg_params[2].value, 0, 32, offset);
+	buf_set_u32(reg_params[3].value, 0, 32, io_base);
+	buf_set_u32(reg_params[4].value, 0, 32, fifo_start);
+	buf_set_u32(reg_params[5].value, 0, 32, fifo_start + fifosize);
+
+	armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
+	armv7m_info.core_mode = ARM_MODE_THREAD;
+
+	/* after breakpoint instruction (halfword), one nop (halfword) and
+	 * ccr_buffer follow till end of code */
+	exit_point = algorithm->address + codesize
+		- (sizeof(ccr_buffer) + sizeof(uint32_t));
+
+	if (write) {
+		retval = target_run_flash_async_algorithm(target, buffer, count, 1,
+				0, NULL,
+				ARRAY_SIZE(reg_params), reg_params,
+				algorithm->address + codesize,
+				fifosize + 2 * sizeof(uint32_t),
+				algorithm->address, exit_point,
+				&armv7m_info);
+	} else {
+		retval = target_run_read_async_algorithm(target, buffer, count, 1,
+				0, NULL,
+				ARRAY_SIZE(reg_params), reg_params,
+				algorithm->address + codesize,
+				fifosize + 2 * sizeof(uint32_t),
+				algorithm->address, exit_point,
+				&armv7m_info);
+	}
+
+	remaining = buf_get_u32(reg_params[0].value, 0, 32);
+	if ((retval == ERROR_OK) && remaining)
+		retval = ERROR_FLASH_OPERATION_FAILED;
+
+	if (retval != ERROR_OK) {
+		offset = buf_get_u32(reg_params[2].value, 0, 32);
+		LOG_ERROR("flash %s failed at address 0x%" PRIx32 ", remaining 0x%" PRIx32,
+			write ? "write" : "read", offset, remaining);
+	}
+
+	destroy_reg_param(&reg_params[0]);
+	destroy_reg_param(&reg_params[1]);
+	destroy_reg_param(&reg_params[2]);
+	destroy_reg_param(&reg_params[3]);
+	destroy_reg_param(&reg_params[4]);
+	destroy_reg_param(&reg_params[5]);
+
+err:
+	target_free_working_area(target, algorithm);
+
+	/* Switch to memory mapped mode before return to prompt */
+	set_mm_mode(bank);
+
+	return retval;
+}
+
+static int stmqspi_read(struct flash_bank *bank, uint8_t *buffer,
+	uint32_t offset, uint32_t count)
+{
+	struct target *target = bank->target;
+	struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
+	uint32_t io_base = stmqspi_info->io_base;
+	int retval;
+
+	LOG_DEBUG("%s: offset=0x%08" PRIx32 " count=0x%08" PRIx32,
+		__func__, offset, count);
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (!(stmqspi_info->probed)) {
+		LOG_ERROR("Flash bank not probed");
+		return ERROR_FLASH_BANK_NOT_PROBED;
+	}
+
+	if (offset + count > bank->size) {
+		LOG_WARNING("Read beyond end of flash. Extra data to be ignored.");
+		count = bank->size - offset;
+	}
+
+	/* Abort any previous operation */
+	retval = target_write_u32(target, io_base + SPI_CR,
+		READ_REG(SPI_CR) | BIT(SPI_ABORT));
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Wait for busy to be cleared */
+	retval = poll_busy(bank, SPI_PROBE_TIMEOUT);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return qspi_read_write_block(bank, buffer, offset, count, false);
+}
+
+static int stmqspi_write(struct flash_bank *bank, const uint8_t *buffer,
+	uint32_t offset, uint32_t count)
+{
+	struct target *target = bank->target;
+	struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
+	uint32_t io_base = stmqspi_info->io_base;
+	unsigned int dual, sector;
+	bool octal_dtr;
+	int retval;
+
+	LOG_DEBUG("%s: offset=0x%08" PRIx32 " count=0x%08" PRIx32,
+		__func__, offset, count);
+
+	dual = (stmqspi_info->saved_cr & BIT(SPI_DUAL_FLASH)) ? 1 : 0;
+	octal_dtr = IS_OCTOSPI && (stmqspi_info->saved_ccr & BIT(OCTOSPI_DDTR));
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (!(stmqspi_info->probed)) {
+		LOG_ERROR("Flash bank not probed");
+		return ERROR_FLASH_BANK_NOT_PROBED;
+	}
+
+	if (offset + count > bank->size) {
+		LOG_WARNING("Write beyond end of flash. Extra data discarded.");
+		count = bank->size - offset;
+	}
+
+	/* Check sector protection */
+	for (sector = 0; sector < bank->num_sectors; sector++) {
+		/* Start offset in or before this sector? */
+		/* End offset in or behind this sector? */
+		if ((offset < (bank->sectors[sector].offset + bank->sectors[sector].size)) &&
+			((offset + count - 1) >= bank->sectors[sector].offset) &&
+			bank->sectors[sector].is_protected) {
+			LOG_ERROR("Flash sector %u protected", sector);
+			return ERROR_FLASH_PROTECTED;
+		}
+	}
+
+	if ((dual || octal_dtr) && ((offset & 1) != 0 || (count & 1) != 0)) {
+		LOG_ERROR("In dual-QSPI and octal-DTR modes writes must be two byte aligned: "
+			"%s: address=0x%08" PRIx32 " len=0x%08" PRIx32, __func__, offset, count);
+		return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
+	}
+
+	/* Abort any previous operation */
+	retval = target_write_u32(target, io_base + SPI_CR,
+		READ_REG(SPI_CR) | BIT(SPI_ABORT));
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Wait for busy to be cleared */
+	retval = poll_busy(bank, SPI_PROBE_TIMEOUT);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return qspi_read_write_block(bank, (uint8_t *)buffer, offset, count, true);
+}
+
+static int stmqspi_verify(struct flash_bank *bank, const uint8_t *buffer,
+	uint32_t offset, uint32_t count)
+{
+	struct target *target = bank->target;
+	struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
+	uint32_t io_base = stmqspi_info->io_base;
+	unsigned int dual;
+	bool octal_dtr;
+	int retval;
+
+	LOG_DEBUG("%s: offset=0x%08" PRIx32 " count=0x%08" PRIx32,
+		__func__, offset, count);
+
+	dual = (stmqspi_info->saved_cr & BIT(SPI_DUAL_FLASH)) ? 1 : 0;
+	octal_dtr = IS_OCTOSPI && (stmqspi_info->saved_ccr & BIT(OCTOSPI_DDTR));
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (!(stmqspi_info->probed)) {
+		LOG_ERROR("Flash bank not probed");
+		return ERROR_FLASH_BANK_NOT_PROBED;
+	}
+
+	if (offset + count > bank->size) {
+		LOG_WARNING("Verify beyond end of flash. Extra data ignored.");
+		count = bank->size - offset;
+	}
+
+	if ((dual || octal_dtr) && ((offset & 1) != 0 || (count & 1) != 0)) {
+		LOG_ERROR("In dual-QSPI and octal-DTR modes reads must be two byte aligned: "
+			"%s: address=0x%08" PRIx32 " len=0x%08" PRIx32, __func__, offset, count);
+		return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
+	}
+
+	/* Abort any previous operation */
+	retval = target_write_u32(target, io_base + SPI_CR,
+		READ_REG(SPI_CR) | BIT(SPI_ABORT));
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Wait for busy to be cleared */
+	retval = poll_busy(bank, SPI_PROBE_TIMEOUT);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return qspi_verify(bank, (uint8_t *)buffer, offset, count);
+}
+
+/* Find appropriate dummy setting, in particular octo mode */
+static int find_sfdp_dummy(struct flash_bank *bank, int len)
+{
+	struct target *target = bank->target;
+	struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
+	uint32_t io_base = stmqspi_info->io_base;
+	uint8_t data;
+	unsigned int dual, count;
+	bool flash1 = !(stmqspi_info->saved_cr & BIT(SPI_FSEL_FLASH));
+	int retval;
+	const unsigned int max_bytes = 64;
+
+	dual = (stmqspi_info->saved_cr & BIT(SPI_DUAL_FLASH)) ? 1 : 0;
+
+	LOG_DEBUG("%s: len=%d, dual=%u, flash1=%d",
+		__func__, len, dual, flash1);
+
+	/* Abort any previous operation */
+	retval = target_write_u32(target, io_base + SPI_CR,
+		stmqspi_info->saved_cr | BIT(SPI_ABORT));
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Wait for busy to be cleared */
+	retval = poll_busy(bank, SPI_PROBE_TIMEOUT);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Switch to saved_cr (had to be set accordingly before this call) */
+	retval = target_write_u32(target, io_base + SPI_CR, stmqspi_info->saved_cr);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Read at most that many bytes */
+	retval = target_write_u32(target, io_base + SPI_DLR, (max_bytes << dual) - 1);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Read SFDP block */
+	if (IS_OCTOSPI)
+		retval = OCTOSPI_CMD(OCTOSPI_READ_MODE, OCTOSPI_CCR_READ_SFDP(len),
+			SPIFLASH_READ_SFDP);
+	else
+		retval = target_write_u32(target, io_base + QSPI_CCR, QSPI_CCR_READ_SFDP);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Read from start of sfdp block */
+	retval = target_write_u32(target, io_base + SPI_AR, 0);
+	if (retval != ERROR_OK)
+		goto err;
+
+	for (count = 0 ; count < max_bytes; count++) {
+		if ((dual != 0) && !flash1) {
+			/* discard even byte in dual flash-mode if flash2 */
+			retval = target_read_u8(target, io_base + SPI_DR, &data);
+			if (retval != ERROR_OK)
+				goto err;
+		}
+
+		retval = target_read_u8(target, io_base + SPI_DR, &data);
+		if (retval != ERROR_OK)
+			goto err;
+
+		if (data == 0x53) {
+			LOG_DEBUG("start of SFDP header for flash%c after %u dummy bytes",
+				flash1 ? '1' : '2', count);
+			if (flash1)
+				stmqspi_info->sfdp_dummy1 = count;
+			else
+				stmqspi_info->sfdp_dummy2 = count;
+			return ERROR_OK;
+		}
+
+		if ((dual != 0) && flash1) {
+			/* discard odd byte in dual flash-mode if flash1 */
+			retval = target_read_u8(target, io_base + SPI_DR, &data);
+			if (retval != ERROR_OK)
+				goto err;
+		}
+	}
+
+	retval = ERROR_FAIL;
+	LOG_DEBUG("no start of SFDP header even after %u dummy bytes", count);
+
+err:
+	/* Abort operation */
+	retval = target_write_u32(target, io_base + SPI_CR,
+		READ_REG(SPI_CR) | BIT(SPI_ABORT));
+
+	return retval;
+}
+
+/* Read SFDP parameter block */
+static int read_sfdp_block(struct flash_bank *bank, uint32_t addr,
+	uint32_t words, uint32_t *buffer)
+{
+	struct target *target = bank->target;
+	struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
+	uint32_t io_base = stmqspi_info->io_base;
+	bool flash1 = !(stmqspi_info->saved_cr & BIT(SPI_FSEL_FLASH));
+	unsigned int dual, count, len, *dummy;
+	int retval;
+
+	dual = (stmqspi_info->saved_cr & BIT(SPI_DUAL_FLASH)) ? 1 : 0;
+
+	if (IS_OCTOSPI && (((stmqspi_info->saved_ccr >> SPI_DMODE_POS) & 0x7) > 3)) {
+		/* in OCTO mode 4-byte address and (yet) unknown number of dummy clocks */
+		len = 4;
+
+		/* in octo mode, use sfdp_dummy1 only */
+		dummy = &stmqspi_info->sfdp_dummy1;
+		if (*dummy == 0) {
+			retval = find_sfdp_dummy(bank, len);
+			if (retval != ERROR_OK)
+				return retval;
+		}
+	} else {
+		/* in all other modes 3-byte-address and 8(?) dummy clocks */
+		len = 3;
+
+		/* use sfdp_dummy1/2 according to currently selected flash */
+		dummy = (stmqspi_info->saved_cr & BIT(SPI_FSEL_FLASH)) ?
+			&stmqspi_info->sfdp_dummy2 : &stmqspi_info->sfdp_dummy1;
+
+		/* according to SFDP standard, there should always be 8 dummy *CLOCKS*
+		 * giving 1, 2 or 4 dummy *BYTES*, however, this is apparently not
+		 * always implemented correctly, so determine the number of dummy bytes
+		 * dynamically */
+		if (*dummy == 0) {
+			retval = find_sfdp_dummy(bank, len);
+			if (retval != ERROR_OK)
+				return retval;
+		}
+	}
+
+	LOG_DEBUG("%s: addr=0x%08" PRIx32 " words=0x%08" PRIx32 " dummy=%u",
+		__func__, addr, words, *dummy);
+
+	/* Abort any previous operation */
+	retval = target_write_u32(target, io_base + SPI_CR,
+		stmqspi_info->saved_cr | BIT(SPI_ABORT));
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Wait for busy to be cleared */
+	retval = poll_busy(bank, SPI_PROBE_TIMEOUT);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Switch to one flash only */
+	retval = target_write_u32(target, io_base + SPI_CR, stmqspi_info->saved_cr);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Read that many words plus dummy bytes */
+	retval = target_write_u32(target, io_base + SPI_DLR,
+		((*dummy + words * sizeof(uint32_t)) << dual) - 1);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Read SFDP block */
+	if (IS_OCTOSPI)
+		retval = OCTOSPI_CMD(OCTOSPI_READ_MODE, OCTOSPI_CCR_READ_SFDP(len),
+			SPIFLASH_READ_SFDP);
+	else
+		retval = target_write_u32(target, io_base + QSPI_CCR, QSPI_CCR_READ_SFDP);
+	if (retval != ERROR_OK)
+		goto err;
+
+	retval = target_write_u32(target, io_base + SPI_AR, addr << dual);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* dummy clocks */
+	for (count = *dummy << dual; count > 0; --count) {
+		retval = target_read_u8(target, io_base + SPI_DR, (uint8_t *)buffer);
+		if (retval != ERROR_OK)
+			goto err;
+	}
+
+	for ( ; words > 0; words--) {
+		if (dual != 0) {
+			uint32_t word1, word2;
+
+			retval = target_read_u32(target, io_base + SPI_DR, &word1);
+			if (retval != ERROR_OK)
+				goto err;
+			retval = target_read_u32(target, io_base + SPI_DR, &word2);
+			if (retval != ERROR_OK)
+				goto err;
+
+			if (!flash1) {
+				/* shift odd numbered bytes into even numbered ones */
+				word1 >>= 8;
+				word2 >>= 8;
+			}
+
+			/* pack even numbered bytes into one word */
+			*buffer = (word1 & 0xFFU) | ((word1 & 0xFF0000U) >> 8) |
+				((word2 & 0xFFU) << 16) | ((word2 & 0xFF0000U) << 8);
+
+
+		} else {
+			retval = target_read_u32(target, io_base + SPI_DR, buffer);
+			if (retval != ERROR_OK)
+				goto err;
+		}
+		LOG_DEBUG("raw SFDP data 0x%08" PRIx32, *buffer);
+
+		/* endian correction, sfdp data is always le uint32_t based */
+		*buffer = le_to_h_u32((uint8_t *)buffer);
+		buffer++;
+	}
+
+err:
+	return retval;
+}
+
+/* Return ID of flash device(s) */
+/* On exit, indirect mode is kept */
+static int read_flash_id(struct flash_bank *bank, uint32_t *id1, uint32_t *id2)
+{
+	struct target *target = bank->target;
+	struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
+	uint32_t io_base = stmqspi_info->io_base;
+	uint8_t byte;
+	unsigned int type, count, len1, len2;
+	int retval;
+
+	/* invalidate both ids */
+	*id1 = 0;
+	*id2 = 0;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* SPIFLASH_READ_MID causes device in octal mode to go berserk, so don't use in this case */
+	for (type = (IS_OCTOSPI && OPI_MODE) ? 1 : 0; type < 2 ; type++) {
+		/* Abort any previous operation */
+		retval = target_write_u32(target, io_base + SPI_CR,
+			READ_REG(SPI_CR) | BIT(SPI_ABORT));
+		if (retval != ERROR_OK)
+			goto err;
+
+		/* Poll WIP */
+		retval = wait_till_ready(bank, SPI_PROBE_TIMEOUT);
+		if (retval != ERROR_OK)
+			goto err;
+
+		/* Wait for busy to be cleared */
+		retval = poll_busy(bank, SPI_PROBE_TIMEOUT);
+		if (retval != ERROR_OK)
+			goto err;
+
+		/* Read at most 16 bytes per chip */
+		count = 16;
+		retval = target_write_u32(target, io_base + SPI_DLR,
+			(stmqspi_info->saved_cr & BIT(SPI_DUAL_FLASH) ? count * 2 : count) - 1);
+		if (retval != ERROR_OK)
+			goto err;
+
+		/* Read id: one particular flash chip (N25Q128) switches back to SPI mode when receiving
+		 * SPI_FLASH_READ_ID in QPI mode, hence try SPIFLASH_READ_MID first */
+		switch (type) {
+			case 0:
+				if (IS_OCTOSPI)
+					retval = OCTOSPI_CMD(OCTOSPI_READ_MODE, OCTOSPI_CCR_READ_MID, SPIFLASH_READ_MID);
+				else
+					retval = target_write_u32(target, io_base + QSPI_CCR, QSPI_CCR_READ_MID);
+				break;
+
+			case 1:
+				if (IS_OCTOSPI)
+					retval = OCTOSPI_CMD(OCTOSPI_READ_MODE, OCTOSPI_CCR_READ_ID, SPIFLASH_READ_ID);
+				else
+					retval = target_write_u32(target, io_base + QSPI_CCR, QSPI_CCR_READ_ID);
+				break;
+
+			default:
+				return ERROR_FAIL;
+		}
+
+		if (retval != ERROR_OK)
+			goto err;
+
+		/* Dummy address 0, only required for 8-line mode */
+		if (IS_OCTOSPI && OPI_MODE) {
+			retval = target_write_u32(target, io_base + SPI_AR, 0);
+			if (retval != ERROR_OK)
+				goto err;
+		}
+
+		/* for debugging only */
+		(void)READ_REG(SPI_SR);
+
+		/* Read ID from Data Register */
+		for (len1 = 0, len2 = 0; count > 0; --count) {
+			if ((stmqspi_info->saved_cr & (BIT(SPI_DUAL_FLASH) |
+				BIT(SPI_FSEL_FLASH))) != BIT(SPI_FSEL_FLASH)) {
+				retval = target_read_u8(target, io_base + SPI_DR, &byte);
+				if (retval != ERROR_OK)
+					goto err;
+				/* collect 3 bytes without continuation codes */
+				if ((byte != 0x7F) && (len1 < 3)) {
+					*id1 = (*id1 >> 8) | ((uint32_t)byte) << 16;
+					len1++;
+				}
+			}
+			if ((stmqspi_info->saved_cr & (BIT(SPI_DUAL_FLASH) |
+				BIT(SPI_FSEL_FLASH))) != 0) {
+				retval = target_read_u8(target, io_base + SPI_DR, &byte);
+				if (retval != ERROR_OK)
+					goto err;
+				/* collect 3 bytes without continuation codes */
+				if ((byte != 0x7F) && (len2 < 3)) {
+					*id2 = (*id2 >> 8) | ((uint32_t)byte) << 16;
+					len2++;
+				}
+			}
+		}
+
+		if (((*id1 != 0x000000) && (*id1 != 0xFFFFFF)) ||
+			((*id2 != 0x000000) && (*id2 != 0xFFFFFF)))
+			break;
+	}
+
+	if ((stmqspi_info->saved_cr & (BIT(SPI_DUAL_FLASH) |
+		BIT(SPI_FSEL_FLASH))) != BIT(SPI_FSEL_FLASH)) {
+		if ((*id1 == 0x000000) || (*id1 == 0xFFFFFF)) {
+			/* no id retrieved, so id must be set manually */
+			LOG_INFO("No id from flash1");
+			retval = ERROR_FLASH_BANK_NOT_PROBED;
+		}
+	}
+
+	if ((stmqspi_info->saved_cr & (BIT(SPI_DUAL_FLASH) | BIT(SPI_FSEL_FLASH))) != 0) {
+		if ((*id2 == 0x000000) || (*id2 == 0xFFFFFF)) {
+			/* no id retrieved, so id must be set manually */
+			LOG_INFO("No id from flash2");
+			retval = ERROR_FLASH_BANK_NOT_PROBED;
+		}
+	}
+
+err:
+	return retval;
+}
+
+static int stmqspi_probe(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
+	struct flash_sector *sectors = NULL;
+	uint32_t io_base = stmqspi_info->io_base;
+	uint32_t id1 = 0, id2 = 0, data = 0;
+	const struct flash_device *p;
+	const uint32_t magic = 0xAEF1510E;
+	unsigned int dual, fsize;
+	bool octal_dtr;
+	int retval;
+
+	if (stmqspi_info->probed) {
+		bank->size = 0;
+		bank->num_sectors = 0;
+		free(bank->sectors);
+		bank->sectors = NULL;
+		memset(&stmqspi_info->dev, 0, sizeof(stmqspi_info->dev));
+		stmqspi_info->sfdp_dummy1 = 0;
+		stmqspi_info->sfdp_dummy2 = 0;
+		stmqspi_info->probed = false;
+	}
+
+	/* Abort any previous operation */
+	retval = target_write_u32(target, io_base + SPI_CR,
+		READ_REG(SPI_CR) | BIT(SPI_ABORT));
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Wait for busy to be cleared */
+	retval = poll_busy(bank, SPI_PROBE_TIMEOUT);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* check whether QSPI_ABR is writeable and readback returns the value written */
+	retval = target_write_u32(target, io_base + QSPI_ABR, magic);
+	if (retval == ERROR_OK) {
+		retval = target_read_u32(target, io_base + QSPI_ABR, &data);
+		retval = target_write_u32(target, io_base + QSPI_ABR, 0);
+	}
+
+	if (data == magic) {
+		LOG_DEBUG("QSPI_ABR register present");
+		stmqspi_info->octo = false;
+	} else if (READ_REG(OCTOSPI_MAGIC) == OCTO_MAGIC_ID) {
+		LOG_DEBUG("OCTOSPI_MAGIC present");
+		stmqspi_info->octo = true;
+	} else {
+		LOG_ERROR("No QSPI, no OCTOSPI at 0x%08" PRIx32, io_base);
+		stmqspi_info->probed = false;
+		stmqspi_info->dev.name = "none";
+		return ERROR_FAIL;
+	}
+
+	/* save current FSEL and DFM bits in QSPI/OCTOSPI_CR, current QSPI/OCTOSPI_CCR value */
+	stmqspi_info->saved_cr = READ_REG(SPI_CR);
+	if (retval == ERROR_OK)
+		stmqspi_info->saved_ccr = READ_REG(SPI_CCR);
+
+	if (IS_OCTOSPI) {
+		uint32_t mtyp;
+
+		mtyp = ((READ_REG(OCTOSPI_DCR1) & OCTOSPI_MTYP_MASK)) >> OCTOSPI_MTYP_POS;
+		if (retval == ERROR_OK)
+			stmqspi_info->saved_tcr = READ_REG(OCTOSPI_TCR);
+		if (retval == ERROR_OK)
+			stmqspi_info->saved_ir = READ_REG(OCTOSPI_IR);
+		if ((mtyp != 0x0) && (mtyp != 0x1)) {
+			retval = ERROR_FAIL;
+			LOG_ERROR("Only regular SPI protocol supported in OCTOSPI");
+		}
+		if (retval == ERROR_OK) {
+			LOG_DEBUG("OCTOSPI at 0x%08" PRIx64 ", io_base at 0x%08" PRIx32 ", OCTOSPI_CR 0x%08"
+				PRIx32 ", OCTOSPI_CCR 0x%08" PRIx32 ", %d-byte addr", bank->base, io_base,
+				stmqspi_info->saved_cr, stmqspi_info->saved_ccr, SPI_ADSIZE);
+		} else {
+			LOG_ERROR("No OCTOSPI at io_base 0x%08" PRIx32, io_base);
+			stmqspi_info->probed = false;
+			stmqspi_info->dev.name = "none";
+			return ERROR_FAIL;
+		}
+	} else {
+		if (retval == ERROR_OK) {
+			LOG_DEBUG("QSPI at 0x%08" PRIx64 ", io_base at 0x%08" PRIx32 ", QSPI_CR 0x%08"
+				PRIx32 ", QSPI_CCR 0x%08" PRIx32 ", %d-byte addr", bank->base, io_base,
+				stmqspi_info->saved_cr, stmqspi_info->saved_ccr, SPI_ADSIZE);
+				if (stmqspi_info->saved_ccr & (1U << QSPI_DDRM))
+					LOG_WARNING("DDR mode is untested and suffers from some silicon bugs");
+		} else {
+			LOG_ERROR("No QSPI at io_base 0x%08" PRIx32, io_base);
+			stmqspi_info->probed = false;
+			stmqspi_info->dev.name = "none";
+			return ERROR_FAIL;
+		}
+	}
+
+	dual = (stmqspi_info->saved_cr & BIT(SPI_DUAL_FLASH)) ? 1 : 0;
+	octal_dtr = IS_OCTOSPI && (stmqspi_info->saved_ccr & BIT(OCTOSPI_DDTR));
+	if (dual || octal_dtr)
+		bank->write_start_alignment = bank->write_end_alignment = 2;
+	else
+		bank->write_start_alignment = bank->write_end_alignment = 1;
+
+	/* read and decode flash ID; returns in indirect mode */
+	retval = read_flash_id(bank, &id1, &id2);
+	LOG_DEBUG("id1 0x%06" PRIx32 ", id2 0x%06" PRIx32, id1, id2);
+	if (retval == ERROR_FLASH_BANK_NOT_PROBED) {
+		/* no id retrieved, so id must be set manually */
+		LOG_INFO("No id - set flash parameters manually");
+		retval = ERROR_OK;
+		goto err;
+	}
+
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* identify flash1 */
+	for (p = flash_devices; id1 && p->name ; p++) {
+		if (p->device_id == id1) {
+			memcpy(&stmqspi_info->dev, p, sizeof(stmqspi_info->dev));
+			if (p->size_in_bytes / 4096)
+				LOG_INFO("flash1 \'%s\' id = 0x%06" PRIx32 " size = %" PRIu32
+					"kbytes", p->name, id1, p->size_in_bytes / 1024);
+			else
+				LOG_INFO("flash1 \'%s\' id = 0x%06" PRIx32 " size = %" PRIu32
+					"bytes", p->name, id1, p->size_in_bytes);
+			break;
+		}
+	}
+
+	if (id1 && !p->name) {
+		/* chip not been identified by id, then try SFDP */
+		struct flash_device temp;
+		uint32_t saved_cr = stmqspi_info->saved_cr;
+
+		/* select flash1 */
+		stmqspi_info->saved_cr = stmqspi_info->saved_cr & ~BIT(SPI_FSEL_FLASH);
+		retval = spi_sfdp(bank, &temp, &read_sfdp_block);
+
+		/* restore saved_cr */
+		stmqspi_info->saved_cr = saved_cr;
+
+		if (retval == ERROR_OK) {
+			LOG_INFO("flash1 \'%s\' id = 0x%06" PRIx32 " size = %" PRIu32
+				"kbytes", temp.name, id1, temp.size_in_bytes / 1024);
+			/* save info and retrieved *good* id as spi_sfdp clears all info */
+			memcpy(&stmqspi_info->dev, &temp, sizeof(stmqspi_info->dev));
+			stmqspi_info->dev.device_id = id1;
+		} else {
+			/* even not identified by SFDP, then give up */
+			LOG_WARNING("Unknown flash1 device id = 0x%06" PRIx32
+				" - set flash parameters manually", id1);
+			retval = ERROR_OK;
+			goto err;
+		}
+	}
+
+	/* identify flash2 */
+	for (p = flash_devices; id2 && p->name ; p++) {
+		if (p->device_id == id2) {
+			if (p->size_in_bytes / 4096)
+				LOG_INFO("flash2 \'%s\' id = 0x%06" PRIx32 " size = %" PRIu32
+					"kbytes", p->name, id2, p->size_in_bytes / 1024);
+			else
+				LOG_INFO("flash2 \'%s\' id = 0x%06" PRIx32 " size = %" PRIu32
+					"bytes", p->name, id2, p->size_in_bytes);
+
+			if (!id1)
+				memcpy(&stmqspi_info->dev, p, sizeof(stmqspi_info->dev));
+			else {
+				if ((stmqspi_info->dev.read_cmd != p->read_cmd) ||
+					(stmqspi_info->dev.qread_cmd != p->qread_cmd) ||
+					(stmqspi_info->dev.pprog_cmd != p->pprog_cmd) ||
+					(stmqspi_info->dev.erase_cmd != p->erase_cmd) ||
+					(stmqspi_info->dev.chip_erase_cmd != p->chip_erase_cmd) ||
+					(stmqspi_info->dev.sectorsize != p->sectorsize) ||
+					(stmqspi_info->dev.size_in_bytes != p->size_in_bytes)) {
+					LOG_ERROR("Incompatible flash1/flash2 devices");
+					goto err;
+				}
+				/* page size is optional in SFDP, so accept smallest value */
+				if (p->pagesize < stmqspi_info->dev.pagesize)
+					stmqspi_info->dev.pagesize = p->pagesize;
+			}
+			break;
+		}
+	}
+
+	if (id2 && !p->name) {
+		/* chip not been identified by id, then try SFDP */
+		struct flash_device temp;
+		uint32_t saved_cr = stmqspi_info->saved_cr;
+
+		/* select flash2 */
+		stmqspi_info->saved_cr = stmqspi_info->saved_cr | BIT(SPI_FSEL_FLASH);
+		retval = spi_sfdp(bank, &temp, &read_sfdp_block);
+
+		/* restore saved_cr */
+		stmqspi_info->saved_cr = saved_cr;
+
+		if (retval == ERROR_OK)
+			LOG_INFO("flash2 \'%s\' id = 0x%06" PRIx32 " size = %" PRIu32
+				"kbytes", temp.name, id2, temp.size_in_bytes / 1024);
+		else {
+			/* even not identified by SFDP, then give up */
+			LOG_WARNING("Unknown flash2 device id = 0x%06" PRIx32
+				" - set flash parameters manually", id2);
+			retval = ERROR_OK;
+			goto err;
+		}
+
+		if (!id1)
+			memcpy(&stmqspi_info->dev, &temp, sizeof(stmqspi_info->dev));
+		else {
+			if ((stmqspi_info->dev.read_cmd != temp.read_cmd) ||
+				(stmqspi_info->dev.qread_cmd != temp.qread_cmd) ||
+				(stmqspi_info->dev.pprog_cmd != temp.pprog_cmd) ||
+				(stmqspi_info->dev.erase_cmd != temp.erase_cmd) ||
+				(stmqspi_info->dev.chip_erase_cmd != temp.chip_erase_cmd) ||
+				(stmqspi_info->dev.sectorsize != temp.sectorsize) ||
+				(stmqspi_info->dev.size_in_bytes != temp.size_in_bytes)) {
+				LOG_ERROR("Incompatible flash1/flash2 devices");
+				goto err;
+			}
+			/* page size is optional in SFDP, so accept smallest value */
+			if (temp.pagesize < stmqspi_info->dev.pagesize)
+				stmqspi_info->dev.pagesize = temp.pagesize;
+		}
+	}
+
+	/* Set correct size value */
+	bank->size = stmqspi_info->dev.size_in_bytes << dual;
+
+	fsize = ((READ_REG(SPI_DCR) >> SPI_FSIZE_POS) & (BIT(SPI_FSIZE_LEN) - 1));
+	if (retval != ERROR_OK)
+		goto err;
+
+	LOG_DEBUG("FSIZE = 0x%04x", fsize);
+	if (bank->size == BIT((fsize + 1)))
+		LOG_DEBUG("FSIZE in DCR(1) matches actual capacity. Beware of silicon bug in H7, L4+, MP1.");
+	else if (bank->size == BIT((fsize + 0)))
+		LOG_DEBUG("FSIZE in DCR(1) is off by one regarding actual capacity. Fix for silicon bug?");
+	else
+		LOG_ERROR("FSIZE in DCR(1) doesn't match actual capacity.");
+
+	/* if no sectors, then treat whole flash as single sector */
+	if (stmqspi_info->dev.sectorsize == 0)
+		stmqspi_info->dev.sectorsize = stmqspi_info->dev.size_in_bytes;
+	/* if no page_size, then use sectorsize as page_size */
+	if (stmqspi_info->dev.pagesize == 0)
+		stmqspi_info->dev.pagesize = stmqspi_info->dev.sectorsize;
+
+	/* create and fill sectors array */
+	bank->num_sectors = stmqspi_info->dev.size_in_bytes / stmqspi_info->dev.sectorsize;
+	sectors = malloc(sizeof(struct flash_sector) * bank->num_sectors);
+	if (sectors == NULL) {
+		LOG_ERROR("not enough memory");
+		retval = ERROR_FAIL;
+		goto err;
+	}
+
+	for (unsigned int sector = 0; sector < bank->num_sectors; sector++) {
+		sectors[sector].offset = sector * (stmqspi_info->dev.sectorsize << dual);
+		sectors[sector].size = (stmqspi_info->dev.sectorsize << dual);
+		sectors[sector].is_erased = -1;
+		sectors[sector].is_protected = 0;
+	}
+
+	bank->sectors = sectors;
+	stmqspi_info->probed = true;
+
+err:
+	/* Switch to memory mapped mode before return to prompt */
+	set_mm_mode(bank);
+
+	return retval;
+}
+
+static int stmqspi_auto_probe(struct flash_bank *bank)
+{
+	struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
+
+	if (stmqspi_info->probed)
+		return ERROR_OK;
+	stmqspi_probe(bank);
+	return ERROR_OK;
+}
+
+static int stmqspi_protect_check(struct flash_bank *bank)
+{
+	/* Nothing to do. Protection is only handled in SW. */
+	return ERROR_OK;
+}
+
+static int get_stmqspi_info(struct flash_bank *bank, char *buf, int buf_size)
+{
+	struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
+
+	if (!(stmqspi_info->probed)) {
+		snprintf(buf, buf_size,
+			"\nQSPI flash bank not probed yet\n");
+		return ERROR_FLASH_BANK_NOT_PROBED;
+	}
+
+	snprintf(buf, buf_size, "flash%s%s \'%s\', device id = 0x%06" PRIx32
+			", flash size = %" PRIu32 "%sbytes\n(page size = %" PRIu32
+			", read = 0x%02" PRIx8 ", qread = 0x%02" PRIx8
+			", pprog = 0x%02" PRIx8 ", mass_erase = 0x%02" PRIx8
+			", sector size = %" PRIu32 "%sbytes, sector_erase = 0x%02" PRIx8 ")",
+			((stmqspi_info->saved_cr & (BIT(SPI_DUAL_FLASH) |
+			BIT(SPI_FSEL_FLASH))) != BIT(SPI_FSEL_FLASH)) ? "1" : "",
+			((stmqspi_info->saved_cr & (BIT(SPI_DUAL_FLASH) |
+			BIT(SPI_FSEL_FLASH))) != 0) ? "2" : "",
+			stmqspi_info->dev.name, stmqspi_info->dev.device_id,
+			bank->size / 4096 ? bank->size / 1024 : bank->size,
+			bank->size / 4096 ? "k" : "", stmqspi_info->dev.pagesize,
+			stmqspi_info->dev.read_cmd, stmqspi_info->dev.qread_cmd,
+			stmqspi_info->dev.pprog_cmd, stmqspi_info->dev.chip_erase_cmd,
+			stmqspi_info->dev.sectorsize / 4096 ?
+				stmqspi_info->dev.sectorsize / 1024 : stmqspi_info->dev.sectorsize,
+			stmqspi_info->dev.sectorsize / 4096 ? "k" : "",
+			stmqspi_info->dev.erase_cmd);
+
+	return ERROR_OK;
+}
+
+static const struct command_registration stmqspi_exec_command_handlers[] = {
+	{
+		.name = "mass_erase",
+		.handler = stmqspi_handle_mass_erase_command,
+		.mode = COMMAND_EXEC,
+		.usage = "bank_id",
+		.help = "Mass erase entire flash device.",
+	},
+	{
+		.name = "set",
+		.handler = stmqspi_handle_set,
+		.mode = COMMAND_EXEC,
+		.usage = "bank_id name chip_size page_size read_cmd qread_cmd pprg_cmd "
+			"[ mass_erase_cmd ] [ sector_size sector_erase_cmd ]",
+		.help = "Set params of single flash chip",
+	},
+	{
+		.name = "cmd",
+		.handler = stmqspi_handle_cmd,
+		.mode = COMMAND_EXEC,
+		.usage = "bank_id num_resp cmd_byte ...",
+		.help = "Send low-level command cmd_byte and following bytes or read num_resp.",
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+static const struct command_registration stmqspi_command_handlers[] = {
+	{
+		.name = "stmqspi",
+		.mode = COMMAND_ANY,
+		.help = "stmqspi flash command group",
+		.usage = "",
+		.chain = stmqspi_exec_command_handlers,
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+struct flash_driver stmqspi_flash = {
+	.name = "stmqspi",
+	.commands = stmqspi_command_handlers,
+	.flash_bank_command = stmqspi_flash_bank_command,
+	.erase = stmqspi_erase,
+	.protect = stmqspi_protect,
+	.write = stmqspi_write,
+	.read = stmqspi_read,
+	.verify = stmqspi_verify,
+	.probe = stmqspi_probe,
+	.auto_probe = stmqspi_auto_probe,
+	.erase_check = stmqspi_blank_check,
+	.protect_check = stmqspi_protect_check,
+	.info = get_stmqspi_info,
+	.free_driver_priv = default_flash_free_driver_priv,
+};
diff --git a/src/flash/nor/stmqspi.h b/src/flash/nor/stmqspi.h
new file mode 100644
index 0000000..85da25f
--- /dev/null
+++ b/src/flash/nor/stmqspi.h
@@ -0,0 +1,125 @@
+/***************************************************************************
+ *   Copyright (C) 2016 - 2018 by Andreas Bolsch                           *
+ *   andreas.bolsch@mni.thm.de                                             *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+
+#ifndef OPENOCD_FLASH_NOR_STMQSPI_H
+#define OPENOCD_FLASH_NOR_STMQSPI_H
+
+#include "spi.h"
+
+/* QSPI register offsets */
+#define QSPI_CR			(0x00)	/* Control register */
+#define QSPI_DCR		(0x04)	/* Device configuration register */
+#define QSPI_SR			(0x08)	/* Status register */
+#define QSPI_FCR		(0x0C)	/* Flag clear register */
+#define QSPI_DLR		(0x10)	/* Data length register */
+#define QSPI_CCR		(0x14)	/* Communication configuration register */
+#define QSPI_AR			(0x18)	/* Address register */
+#define QSPI_ABR		(0x1C)	/* Alternate bytes register */
+#define QSPI_DR			(0x20)	/* Data register */
+
+/* common bits in QSPI_CR and OCTOSPI_CR */
+#define SPI_FSEL_FLASH	7		/* Select flash 2 */
+#define SPI_DUAL_FLASH	6		/* Dual flash mode */
+#define SPI_ABORT		1		/* Abort bit */
+
+/* common bits in QSPI_DCR and OCTOSPI_DCR1 */
+#define SPI_FSIZE_POS	16		/* bit position of FSIZE */
+#define SPI_FSIZE_LEN	5		/* width of FSIZE field */
+
+/* common bits in QSPI_SR/FCR and OCTOSPI_SR/FCR */
+#define SPI_BUSY		5		/* Busy flag */
+#define SPI_FTF			2		/* FIFO threshold flag */
+#define SPI_TCF			1		/* Transfer complete flag */
+
+/* fields in QSPI_CCR */
+#define QSPI_DDRM			31					/* position of DDRM bit */
+#define SPI_DMODE_POS		24					/* bit position of DMODE */
+#define QSPI_DCYC_POS		18					/* bit position of DCYC */
+#define QSPI_DCYC_LEN		5					/* width of DCYC field */
+#define QSPI_DCYC_MASK		((BIT(QSPI_DCYC_LEN) - 1) << QSPI_DCYC_POS)
+#define SPI_ADSIZE_POS		12					/* bit position of ADSIZE */
+
+#define QSPI_WRITE_MODE		0x00000000U			/* indirect write mode */
+#define QSPI_READ_MODE		0x04000000U			/* indirect read mode */
+#define QSPI_MM_MODE		0x0C000000U			/* memory mapped mode */
+#define QSPI_ALTB_MODE		0x0003C000U			/* alternate byte mode */
+#define QSPI_4LINE_MODE		0x03000F00U			/* 4 lines for data, addr, instr */
+#define QSPI_NO_DATA		(~0x03000000U)		/* no data */
+#define QSPI_NO_ALTB		(~QSPI_ALTB_MODE)	/* no alternate */
+#define QSPI_NO_ADDR		(~0x00000C00U)		/* no address */
+#define QSPI_ADDR3			(0x2U << SPI_ADSIZE_POS)	/* 3 byte address */
+#define QSPI_ADDR4			(0x3U << SPI_ADSIZE_POS)	/* 4 byte address */
+
+/* OCTOSPI register offsets */
+#define OCTOSPI_CR		(0x000)	/* Control register */
+#define OCTOSPI_DCR1	(0x008)	/* Device configuration register 1 */
+#define OCTOSPI_DCR2	(0x00C)	/* Device configuration register 2 */
+#define OCTOSPI_DCR3	(0x010)	/* Device configuration register 3 */
+#define	OCTOSPI_SR		(0x020)	/* Status register */
+#define OCTOSPI_FCR		(0x024)	/* Flag clear register */
+#define OCTOSPI_DLR		(0x040)	/* Data length register */
+#define OCTOSPI_AR		(0x048)	/* Address register */
+#define OCTOSPI_DR		(0x050)	/* Data register */
+#define OCTOSPI_CCR		(0x100)	/* Communication configuration register */
+#define OCTOSPI_TCR		(0x108)	/* Timing configuration register */
+#define OCTOSPI_IR		(0x110)	/* Instruction register */
+#define OCTOSPI_WCCR	(0x180)	/* Write communication configuration register */
+#define OCTOSPI_WIR		(0x190)	/* Write instruction register */
+#define OCTOSPI_MAGIC	(0x3FC)	/* Magic ID register, deleted from RM, why? */
+
+#define OCTO_MAGIC_ID	0xA3C5DD01	/* Magic ID, deleted from RM, why? */
+
+/* additional bits in OCTOSPI_CR */
+#define OCTOSPI_WRITE_MODE	0x00000000U			/* indirect write mode */
+#define OCTOSPI_READ_MODE	0x10000000U			/* indirect read mode */
+#define OCTOSPI_MM_MODE		0x30000000U			/* memory mapped mode */
+
+/* additional fields in OCTOSPI_DCR1 */
+#define OCTOSPI_MTYP_POS	(24)				/* bit position of MTYP */
+#define OCTOSPI_MTYP_LEN	(3)					/* width of MTYP field */
+#define OCTOSPI_MTYP_MASK	((BIT(OCTOSPI_MTYP_LEN) - 1) << OCTOSPI_MTYP_POS)
+
+/* fields in OCTOSPI_CCR */
+#define OCTOSPI_ALTB_MODE	0x001F0000U				/* alternate byte mode */
+#define OCTOSPI_8LINE_MODE	0x0F003F3FU				/* 8 lines DTR for data, addr, instr */
+#define OCTOSPI_NO_DATA		(~0x0F000000U)			/* no data */
+#define OCTOSPI_NO_ALTB		(~OCTOSPI_ALTB_MODE)	/* no alternate */
+#define OCTOSPI_NO_ADDR		(~0x00000F00U)			/* no address */
+#define OCTOSPI_ADDR3		(0x2U << SPI_ADSIZE_POS)	/* 3 byte address */
+#define OCTOSPI_ADDR4		(0x3U << SPI_ADSIZE_POS)	/* 4 byte address */
+#define OCTOSPI_DQSEN		29						/* DQS enable */
+#define OCTOSPI_DDTR		27						/* DTR for data */
+#define OCTOSPI_NO_DDTR		(~BIT(OCTOSPI_DDTR))	/* no DTR for data, but maybe still DQS */
+#define OCTOSPI_ISIZE_MASK	(0x30)					/* ISIZE field */
+
+/* fields in OCTOSPI_TCR */
+#define OCTOSPI_DCYC_POS	0					/* bit position of DCYC */
+#define OCTOSPI_DCYC_LEN	5					/* width of DCYC field */
+#define OCTOSPI_DCYC_MASK	((BIT(OCTOSPI_DCYC_LEN) - 1) << OCTOSPI_DCYC_POS)
+
+#define IS_OCTOSPI			(stmqspi_info->octo)
+#define SPI_CR				(IS_OCTOSPI ? OCTOSPI_CR : QSPI_CR)
+#define SPI_DCR				(IS_OCTOSPI ? OCTOSPI_DCR1 : QSPI_DCR)
+#define	SPI_SR				(IS_OCTOSPI ? OCTOSPI_SR : QSPI_SR)
+#define SPI_FCR				(IS_OCTOSPI ? OCTOSPI_FCR : QSPI_FCR)
+#define SPI_DLR				(IS_OCTOSPI ? OCTOSPI_DLR : QSPI_DLR)
+#define SPI_AR				(IS_OCTOSPI ? OCTOSPI_AR : QSPI_AR)
+#define SPI_DR				(IS_OCTOSPI ? OCTOSPI_DR : QSPI_DR)
+#define SPI_CCR				(IS_OCTOSPI ? OCTOSPI_CCR : QSPI_CCR)
+
+#endif /* OPENOCD_FLASH_NOR_STMQSPI_H */
diff --git a/src/flash/nor/stmsmi.c b/src/flash/nor/stmsmi.c
index 278c73e..f633e36 100644
--- a/src/flash/nor/stmsmi.c
+++ b/src/flash/nor/stmsmi.c
@@ -41,34 +41,33 @@
 #include <jtag/jtag.h>
 #include <helper/time_support.h>
 
-#define SMI_READ_REG(a) (_SMI_READ_REG(a))
-#define _SMI_READ_REG(a)			\
-{									\
-	int __a;						\
-	uint32_t __v;					\
+#define SMI_READ_REG(a)				\
+({									\
+	int _ret;						\
+	uint32_t _value;				\
 									\
-	__a = target_read_u32(target, io_base + (a), &__v); \
-	if (__a != ERROR_OK)			\
-		return __a;					\
-	__v;							\
-}
+	_ret = target_read_u32(target, io_base + (a), &_value); \
+	if (_ret != ERROR_OK)			\
+		return _ret;				\
+	_value;							\
+})
 
 #define SMI_WRITE_REG(a, v)			\
 {									\
-	int __r;						\
+	int _retval;					\
 									\
-	__r = target_write_u32(target, io_base + (a), (v)); \
-	if (__r != ERROR_OK)			\
-		return __r;					\
+	_retval = target_write_u32(target, io_base + (a), (v)); \
+	if (_retval != ERROR_OK)		\
+		return _retval;				\
 }
 
 #define SMI_POLL_TFF(timeout)		\
 {									\
-	int __r;						\
+	int _retval;					\
 									\
-	__r = poll_tff(target, io_base, timeout); \
-	if (__r != ERROR_OK)			\
-		return __r;					\
+	_retval = poll_tff(target, io_base, timeout); \
+	if (_retval != ERROR_OK)		\
+		return _retval;				\
 }
 
 #define SMI_SET_SW_MODE()	SMI_WRITE_REG(SMI_CR1, \
diff --git a/src/flash/nor/tcl.c b/src/flash/nor/tcl.c
index fbb602b..0e0a924 100644
--- a/src/flash/nor/tcl.c
+++ b/src/flash/nor/tcl.c
@@ -30,7 +30,7 @@
  * Implements Tcl commands used to access NOR flash facilities.
  */
 
-COMMAND_HELPER(flash_command_get_bank_maybe_probe, unsigned name_index,
+static COMMAND_HELPER(flash_command_get_bank_maybe_probe, unsigned name_index,
 	       struct flash_bank **bank, bool do_probe)
 {
 	const char *name = CMD_ARGV[name_index];
@@ -441,20 +441,21 @@ COMMAND_HANDLER(handle_flash_write_image_command)
 	duration_start(&bench);
 
 	if (CMD_ARGC >= 2) {
-		image.base_address_set = 1;
+		image.base_address_set = true;
 		COMMAND_PARSE_NUMBER(llong, CMD_ARGV[1], image.base_address);
 	} else {
-		image.base_address_set = 0;
+		image.base_address_set = false;
 		image.base_address = 0x0;
 	}
 
-	image.start_address_set = 0;
+	image.start_address_set = false;
 
 	retval = image_open(&image, CMD_ARGV[0], (CMD_ARGC == 3) ? CMD_ARGV[2] : NULL);
 	if (retval != ERROR_OK)
 		return retval;
 
-	retval = flash_write_unlock(target, &image, &written, auto_erase, auto_unlock);
+	retval = flash_write_unlock_verify(target, &image, &written, auto_erase,
+		auto_unlock, true, false);
 	if (retval != ERROR_OK) {
 		image_close(&image);
 		return retval;
@@ -471,6 +472,58 @@ COMMAND_HANDLER(handle_flash_write_image_command)
 	return retval;
 }
 
+COMMAND_HANDLER(handle_flash_verify_image_command)
+{
+	struct target *target = get_current_target(CMD_CTX);
+
+	struct image image;
+	uint32_t verified;
+
+	int retval;
+
+	if (CMD_ARGC < 1)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	if (!target) {
+		LOG_ERROR("no target selected");
+		return ERROR_FAIL;
+	}
+
+	struct duration bench;
+	duration_start(&bench);
+
+	if (CMD_ARGC >= 2) {
+		image.base_address_set = 1;
+		COMMAND_PARSE_NUMBER(llong, CMD_ARGV[1], image.base_address);
+	} else {
+		image.base_address_set = 0;
+		image.base_address = 0x0;
+	}
+
+	image.start_address_set = 0;
+
+	retval = image_open(&image, CMD_ARGV[0], (CMD_ARGC == 3) ? CMD_ARGV[2] : NULL);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = flash_write_unlock_verify(target, &image, &verified, false,
+		false, false, true);
+	if (retval != ERROR_OK) {
+		image_close(&image);
+		return retval;
+	}
+
+	if ((ERROR_OK == retval) && (duration_measure(&bench) == ERROR_OK)) {
+		command_print(CMD, "verified %" PRIu32 " bytes from file %s "
+			"in %fs (%0.3f KiB/s)", verified, CMD_ARGV[0],
+			duration_elapsed(&bench), duration_kbps(&bench, verified));
+	}
+
+	image_close(&image);
+
+	return retval;
+}
+
 COMMAND_HANDLER(handle_flash_fill_command)
 {
 	target_addr_t address;
@@ -1143,7 +1196,15 @@ static const struct command_registration flash_exec_command_handlers[] = {
 		.mode = COMMAND_EXEC,
 		.usage = "[erase] [unlock] filename [offset [file_type]]",
 		.help = "Write an image to flash.  Optionally first unprotect "
-			"and/or erase the region to be used.  Allow optional "
+			"and/or erase the region to be used. Allow optional "
+			"offset from beginning of bank (defaults to zero)",
+	},
+	{
+		.name = "verify_image",
+		.handler = handle_flash_verify_image_command,
+		.mode = COMMAND_EXEC,
+		.usage = "filename [offset [file_type]]",
+		.help = "Verify an image against flash. Allow optional "
 			"offset from beginning of bank (defaults to zero)",
 	},
 	{
diff --git a/src/flash/nor/xcf.c b/src/flash/nor/xcf.c
index 29eef2d..0cef43b 100644
--- a/src/flash/nor/xcf.c
+++ b/src/flash/nor/xcf.c
@@ -45,7 +45,7 @@
 #define ID_XCF32P                       0x05059093
 #define ID_MEANINGFUL_MASK              0x0FFFFFFF
 
-const char *xcf_name_list[] = {
+static const char * const xcf_name_list[] = {
 	"XCF08P",
 	"XCF16P",
 	"XCF32P",
@@ -399,7 +399,7 @@ static void flip_u8(uint8_t *out, const uint8_t *in, int len)
  * Function presumes need of bit reversing if it can not exactly detects
  * the opposite.
  */
-bool need_bit_reverse(const uint8_t *buffer)
+static bool need_bit_reverse(const uint8_t *buffer)
 {
 	const size_t L = 20;
 	uint8_t reference[L];
diff --git a/src/flash/startup.tcl b/src/flash/startup.tcl
index aafb939..280a059 100644
--- a/src/flash/startup.tcl
+++ b/src/flash/startup.tcl
@@ -116,6 +116,7 @@ proc stm32l1x args { eval stm32lx $args }
 # stm32[g0|g4|wb|wl] uses the same flash driver as the stm32l4x
 proc stm32g0x args { eval stm32l4x $args }
 proc stm32g4x args { eval stm32l4x $args }
+proc stm32l5x args { eval stm32l4x $args }
 proc stm32wbx args { eval stm32l4x $args }
 proc stm32wlx args { eval stm32l4x $args }
 
diff --git a/src/helper/command.c b/src/helper/command.c
index cfaa1f7..96b1244 100644
--- a/src/helper/command.c
+++ b/src/helper/command.c
@@ -349,7 +349,9 @@ static int register_command_handler(struct command_context *cmd_ctx,
 {
 	Jim_Interp *interp = cmd_ctx->interp;
 
+#if 0
 	LOG_DEBUG("registering '%s'...", c->name);
+#endif
 
 	Jim_CmdProc *func = c->handler ? &script_command : &command_unknown;
 	int retval = Jim_CreateCommand(interp, c->name, func, c, NULL);
@@ -920,39 +922,29 @@ COMMAND_HANDLER(handle_help_command)
 	bool full = strcmp(CMD_NAME, "help") == 0;
 	int retval;
 	struct command *c = CMD_CTX->commands;
-	char *cmd_match = NULL;
-
-	if (CMD_ARGC == 0)
-		cmd_match = "";
-	else if (CMD_ARGC >= 1) {
-		unsigned i;
-
-		for (i = 0; i < CMD_ARGC; ++i) {
-			if (NULL != cmd_match) {
-				char *prev = cmd_match;
-
-				cmd_match = alloc_printf("%s %s", cmd_match, CMD_ARGV[i]);
-				free(prev);
-				if (NULL == cmd_match) {
-					LOG_ERROR("unable to build search string");
-					return -ENOMEM;
-				}
-			} else {
-				cmd_match = alloc_printf("%s", CMD_ARGV[i]);
-				if (NULL == cmd_match) {
-					LOG_ERROR("unable to build search string");
-					return -ENOMEM;
-				}
-			}
+	char *cmd_match;
+
+	if (CMD_ARGC <= 0)
+		cmd_match = strdup("");
+
+	else {
+		cmd_match = strdup(CMD_ARGV[0]);
+
+		for (unsigned int i = 1; i < CMD_ARGC && cmd_match; ++i) {
+			char *prev = cmd_match;
+			cmd_match = alloc_printf("%s %s", prev, CMD_ARGV[i]);
+			free(prev);
 		}
-	} else
-		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
 
+	if (cmd_match == NULL) {
+		LOG_ERROR("unable to build search string");
+		return -ENOMEM;
+	}
 	retval = CALL_COMMAND_HANDLER(command_help_show_list,
 			c, 0, full, cmd_match);
 
-	if (CMD_ARGC >= 1)
-		free(cmd_match);
+	free(cmd_match);
 	return retval;
 }
 
diff --git a/src/helper/jep106.inc b/src/helper/jep106.inc
index b0c0468..df93a5c 100644
--- a/src/helper/jep106.inc
+++ b/src/helper/jep106.inc
@@ -1,4 +1,9 @@
-/* Autogenerated with update_jep106.pl*/
+/*
+ * Should be autogenerated with update_jep106.pl but latest
+ * file from JEDEC is only available in PDF form. The PDF
+ * also breaks the pdftotext flow due to embedded watermarks.
+ * Created with a mix of scripts and manual editing.
+ */
 [0][0x01 - 1] = "AMD",
 [0][0x02 - 1] = "AMI",
 [0][0x03 - 1] = "Fairchild",
@@ -194,7 +199,7 @@
 [1][0x43 - 1] = "Suwa Electronics",
 [1][0x44 - 1] = "Transmeta",
 [1][0x45 - 1] = "Micron CMS",
-[1][0x46 - 1] = "American Computer & Digital",
+[1][0x46 - 1] = "American Computer & Digital Components Inc",
 [1][0x47 - 1] = "Enhance 3000 Inc",
 [1][0x48 - 1] = "Tower Semiconductor",
 [1][0x49 - 1] = "CPU Design",
@@ -438,7 +443,7 @@
 [3][0x3b - 1] = "Concept Computer",
 [3][0x3c - 1] = "SILCOM",
 [3][0x3d - 1] = "3Dlabs",
-[3][0x3e - 1] = "c’t Magazine",
+[3][0x3e - 1] = "c't Magazine",
 [3][0x3f - 1] = "Sanera Systems",
 [3][0x40 - 1] = "Silicon Packets",
 [3][0x41 - 1] = "Viasystems Group",
@@ -532,7 +537,7 @@
 [4][0x1b - 1] = "Phonex Broadband",
 [4][0x1c - 1] = "Skyworks Solutions",
 [4][0x1d - 1] = "Entropic Communications",
-[4][0x1e - 1] = "I’M Intelligent Memory Ltd",
+[4][0x1e - 1] = "I'M Intelligent Memory Ltd",
 [4][0x1f - 1] = "Zensys A/S",
 [4][0x20 - 1] = "Legend Silicon Corp",
 [4][0x21 - 1] = "Sci-worx GmbH",
@@ -610,7 +615,7 @@
 [4][0x69 - 1] = "Century Micro Inc",
 [4][0x6a - 1] = "Icera Semiconductor",
 [4][0x6b - 1] = "Mediaworks Integrated Systems",
-[4][0x6c - 1] = "O’Neil Product Development",
+[4][0x6c - 1] = "O'Neil Product Development",
 [4][0x6d - 1] = "Supreme Top Technology Ltd",
 [4][0x6e - 1] = "MicroDisplay Corporation",
 [4][0x6f - 1] = "Team Group Inc",
@@ -837,7 +842,7 @@
 [6][0x50 - 1] = "Silego Technology Inc",
 [6][0x51 - 1] = "Kinglife",
 [6][0x52 - 1] = "Ability Industries Ltd",
-[6][0x53 - 1] = "Silicon Power Computer &",
+[6][0x53 - 1] = "Silicon Power Computer & Communications",
 [6][0x54 - 1] = "Augusta Technology Inc",
 [6][0x55 - 1] = "Nantronics Semiconductors",
 [6][0x56 - 1] = "Hilscher Gesellschaft",
@@ -961,7 +966,7 @@
 [7][0x4e - 1] = "Mustang",
 [7][0x4f - 1] = "Orca Systems",
 [7][0x50 - 1] = "Passif Semiconductor",
-[7][0x51 - 1] = "GigaDevice Semiconductor (Beijing)",
+[7][0x51 - 1] = "GigaDevice Semiconductor (Beijing) Inc",
 [7][0x52 - 1] = "Memphis Electronic",
 [7][0x53 - 1] = "Beckhoff Automation GmbH",
 [7][0x54 - 1] = "Harmony Semiconductor Corp",
@@ -985,7 +990,7 @@
 [7][0x66 - 1] = "Avalanche Technology",
 [7][0x67 - 1] = "R&D Center ELVEES OJSC",
 [7][0x68 - 1] = "KingboMars Technology Co Ltd",
-[7][0x69 - 1] = "High Bridge Solutions Industria",
+[7][0x69 - 1] = "High Bridge Solutions Industria Eletronica",
 [7][0x6a - 1] = "Transcend Technology Co Ltd",
 [7][0x6b - 1] = "Everspin Technologies",
 [7][0x6c - 1] = "Hon-Hai Precision",
@@ -1031,7 +1036,7 @@
 [8][0x17 - 1] = "Axell Corporation",
 [8][0x18 - 1] = "Essencore Limited",
 [8][0x19 - 1] = "Phytium",
-[8][0x1a - 1] = "Xi’an UniIC Semiconductors Co Ltd",
+[8][0x1a - 1] = "Xi'an UniIC Semiconductors Co Ltd",
 [8][0x1b - 1] = "Ambiq Micro",
 [8][0x1c - 1] = "eveRAM Technology Inc",
 [8][0x1d - 1] = "Infomax",
@@ -1083,7 +1088,7 @@
 [8][0x4b - 1] = "in2H2 inc",
 [8][0x4c - 1] = "Shenzhen Pango Microsystems Co Ltd",
 [8][0x4d - 1] = "Vasekey",
-[8][0x4e - 1] = "Cal-Comp Industria de",
+[8][0x4e - 1] = "Cal-Comp Industria de Semicondutores",
 [8][0x4f - 1] = "Eyenix Co Ltd",
 [8][0x50 - 1] = "Heoriady",
 [8][0x51 - 1] = "Accelerated Memory Production Inc",
@@ -1114,7 +1119,7 @@
 [8][0x6a - 1] = "Axign",
 [8][0x6b - 1] = "Kingred Electronic Technology Ltd",
 [8][0x6c - 1] = "Chao Yue Zhuo Computer Business Dept.",
-[8][0x6d - 1] = "Guangzhou Si Nuo Electronic",
+[8][0x6d - 1] = "Guangzhou Si Nuo Electronic Technology.",
 [8][0x6e - 1] = "Crocus Technology Inc",
 [8][0x6f - 1] = "Creative Chips GmbH",
 [8][0x70 - 1] = "GE Aviation Systems LLC.",
@@ -1130,6 +1135,8 @@
 [8][0x7a - 1] = "AltoBeam",
 [8][0x7b - 1] = "Wave Computing",
 [8][0x7c - 1] = "Beijing TrustNet Technology Co Ltd",
+[8][0x7d - 1] = "Innovium Inc",
+[8][0x7e - 1] = "Starsway Technology Limited",
 [9][0x01 - 1] = "Weltronics Co LTD",
 [9][0x02 - 1] = "VMware Inc",
 [9][0x03 - 1] = "Hewlett Packard Enterprise",
@@ -1160,6 +1167,7 @@
 [9][0x1c - 1] = "MemxPro Inc",
 [9][0x1d - 1] = "Tammuz Co Ltd",
 [9][0x1e - 1] = "Allwinner Technology",
+[9][0x1f - 1] = "Shenzhen City Futian District Qing Xuan Tong Computer Trading Firm",
 [9][0x20 - 1] = "XMC",
 [9][0x21 - 1] = "Teclast",
 [9][0x22 - 1] = "Maxsun",
@@ -1272,7 +1280,7 @@
 [10][0x0f - 1] = "UNIC Memory Technology Co Ltd",
 [10][0x10 - 1] = "Shenzhen Huafeng Science Technology",
 [10][0x11 - 1] = "ChangXin Memory Technologies Inc",
-[10][0x12 - 1] = "Guangzhou Xinyi Heng Computer",
+[10][0x12 - 1] = "Guangzhou Xinyi Heng Computer Trading Firm",
 [10][0x13 - 1] = "SambaNova Systems",
 [10][0x14 - 1] = "V-GEN",
 [10][0x15 - 1] = "Jump Trading",
@@ -1291,7 +1299,7 @@
 [10][0x22 - 1] = "Shenzhen Chuangshifeida Technology",
 [10][0x23 - 1] = "Guangzhou MiaoYuanJi Technology",
 [10][0x24 - 1] = "ADVAN Inc",
-[10][0x25 - 1] = "Shenzhen Qianhai Weishengda",
+[10][0x25 - 1] = "Shenzhen Qianhai Weishengda Electronic Commerce Company Ltd",
 [10][0x26 - 1] = "Guangzhou Guang Xie Cheng Trading",
 [10][0x27 - 1] = "StarRam International Co Ltd",
 [10][0x28 - 1] = "Shen Zhen XinShenHua Tech Co Ltd",
@@ -1321,7 +1329,7 @@
 [10][0x40 - 1] = "Shenzhen Zhongguang Yunhe Trading",
 [10][0x41 - 1] = "Smart Shine(QingDao) Microelectronics",
 [10][0x42 - 1] = "Thermaltake Technology Co Ltd",
-[10][0x43 - 1] = "Shenzhen O’Yang Maile Technology Ltd",
+[10][0x43 - 1] = "Shenzhen O'Yang Maile Technology Ltd",
 [10][0x44 - 1] = "UPMEM",
 [10][0x45 - 1] = "Chun Well Technology Holding Limited",
 [10][0x46 - 1] = "Astera Labs Inc",
@@ -1397,7 +1405,7 @@
 [11][0x0e - 1] = "MLOONG",
 [11][0x0f - 1] = "Quadratica LLC",
 [11][0x10 - 1] = "Anpec Electronics",
-[11][0x11 - 1] = "Xi’an Morebeck Semiconductor Tech Co",
+[11][0x11 - 1] = "Xi'an Morebeck Semiconductor Tech Co",
 [11][0x12 - 1] = "Kingbank Technology Co Ltd",
 [11][0x13 - 1] = "ITRenew Inc",
 [11][0x14 - 1] = "Shenzhen Eaget Innovation Tech Ltd",
diff --git a/src/jtag/aice/aice_pipe.c b/src/jtag/aice/aice_pipe.c
index bdc8c09..c0e532c 100644
--- a/src/jtag/aice/aice_pipe.c
+++ b/src/jtag/aice/aice_pipe.c
@@ -35,8 +35,8 @@
 #ifdef _WIN32
 PROCESS_INFORMATION proc_info;
 
-HANDLE aice_pipe_output[2];
-HANDLE aice_pipe_input[2];
+static HANDLE aice_pipe_output[2];
+static HANDLE aice_pipe_input[2];
 
 static int aice_pipe_write(const void *buffer, int count)
 {
@@ -158,8 +158,8 @@ static int aice_pipe_open(struct aice_port_param_s *param)
 
 #else
 
-int aice_pipe_output[2];
-int aice_pipe_input[2];
+static int aice_pipe_output[2];
+static int aice_pipe_input[2];
 
 static int aice_pipe_write(const void *buffer, int count)
 {
diff --git a/src/jtag/aice/aice_usb.c b/src/jtag/aice/aice_usb.c
index 7144632..7688aea 100644
--- a/src/jtag/aice/aice_usb.c
+++ b/src/jtag/aice/aice_usb.c
@@ -683,7 +683,7 @@ int aice_write_ctrl(uint32_t address, uint32_t data)
 	return ERROR_OK;
 }
 
-int aice_read_dtr(uint8_t target_id, uint32_t *data)
+static int aice_read_dtr(uint8_t target_id, uint32_t *data)
 {
 	int retry_times = 0;
 
@@ -733,7 +733,7 @@ int aice_read_dtr(uint8_t target_id, uint32_t *data)
 	return ERROR_OK;
 }
 
-int aice_read_dtr_to_buffer(uint8_t target_id, uint32_t buffer_idx)
+static int aice_read_dtr_to_buffer(uint8_t target_id, uint32_t buffer_idx)
 {
 	int retry_times = 0;
 
@@ -784,7 +784,7 @@ int aice_read_dtr_to_buffer(uint8_t target_id, uint32_t buffer_idx)
 	return ERROR_OK;
 }
 
-int aice_write_dtr(uint8_t target_id, uint32_t data)
+static int aice_write_dtr(uint8_t target_id, uint32_t data)
 {
 	int retry_times = 0;
 
@@ -836,7 +836,7 @@ int aice_write_dtr(uint8_t target_id, uint32_t data)
 	return ERROR_OK;
 }
 
-int aice_write_dtr_from_buffer(uint8_t target_id, uint32_t buffer_idx)
+static int aice_write_dtr_from_buffer(uint8_t target_id, uint32_t buffer_idx)
 {
 	int retry_times = 0;
 
@@ -887,7 +887,7 @@ int aice_write_dtr_from_buffer(uint8_t target_id, uint32_t buffer_idx)
 	return ERROR_OK;
 }
 
-int aice_read_misc(uint8_t target_id, uint32_t address, uint32_t *data)
+static int aice_read_misc(uint8_t target_id, uint32_t address, uint32_t *data)
 {
 	int retry_times = 0;
 
@@ -936,7 +936,7 @@ int aice_read_misc(uint8_t target_id, uint32_t address, uint32_t *data)
 	return ERROR_OK;
 }
 
-int aice_write_misc(uint8_t target_id, uint32_t address, uint32_t data)
+static int aice_write_misc(uint8_t target_id, uint32_t address, uint32_t data)
 {
 	int retry_times = 0;
 
@@ -992,7 +992,7 @@ int aice_write_misc(uint8_t target_id, uint32_t address, uint32_t data)
 	return ERROR_OK;
 }
 
-int aice_read_edmsr(uint8_t target_id, uint32_t address, uint32_t *data)
+static int aice_read_edmsr(uint8_t target_id, uint32_t address, uint32_t *data)
 {
 	int retry_times = 0;
 
@@ -1042,7 +1042,7 @@ int aice_read_edmsr(uint8_t target_id, uint32_t address, uint32_t *data)
 	return ERROR_OK;
 }
 
-int aice_write_edmsr(uint8_t target_id, uint32_t address, uint32_t data)
+static int aice_write_edmsr(uint8_t target_id, uint32_t address, uint32_t data)
 {
 	int retry_times = 0;
 
@@ -1236,7 +1236,7 @@ static int aice_do_execute(uint8_t target_id)
 	return ERROR_OK;
 }
 
-int aice_write_mem_b(uint8_t target_id, uint32_t address, uint32_t data)
+static int aice_write_mem_b(uint8_t target_id, uint32_t address, uint32_t data)
 {
 	int retry_times = 0;
 
@@ -1290,7 +1290,7 @@ int aice_write_mem_b(uint8_t target_id, uint32_t address, uint32_t data)
 	return ERROR_OK;
 }
 
-int aice_write_mem_h(uint8_t target_id, uint32_t address, uint32_t data)
+static int aice_write_mem_h(uint8_t target_id, uint32_t address, uint32_t data)
 {
 	int retry_times = 0;
 
@@ -1345,7 +1345,7 @@ int aice_write_mem_h(uint8_t target_id, uint32_t address, uint32_t data)
 	return ERROR_OK;
 }
 
-int aice_write_mem(uint8_t target_id, uint32_t address, uint32_t data)
+static int aice_write_mem(uint8_t target_id, uint32_t address, uint32_t data)
 {
 	int retry_times = 0;
 
@@ -1400,7 +1400,7 @@ int aice_write_mem(uint8_t target_id, uint32_t address, uint32_t data)
 	return ERROR_OK;
 }
 
-int aice_fastread_mem(uint8_t target_id, uint8_t *word, uint32_t num_of_words)
+static int aice_fastread_mem(uint8_t target_id, uint8_t *word, uint32_t num_of_words)
 {
 	int retry_times = 0;
 
@@ -1450,7 +1450,7 @@ int aice_fastread_mem(uint8_t target_id, uint8_t *word, uint32_t num_of_words)
 	return ERROR_OK;
 }
 
-int aice_fastwrite_mem(uint8_t target_id, const uint8_t *word, uint32_t num_of_words)
+static int aice_fastwrite_mem(uint8_t target_id, const uint8_t *word, uint32_t num_of_words)
 {
 	int retry_times = 0;
 
@@ -1506,7 +1506,7 @@ int aice_fastwrite_mem(uint8_t target_id, const uint8_t *word, uint32_t num_of_w
 	return ERROR_OK;
 }
 
-int aice_read_mem_b(uint8_t target_id, uint32_t address, uint32_t *data)
+static int aice_read_mem_b(uint8_t target_id, uint32_t address, uint32_t *data)
 {
 	int retry_times = 0;
 
@@ -1556,7 +1556,7 @@ int aice_read_mem_b(uint8_t target_id, uint32_t address, uint32_t *data)
 	return ERROR_OK;
 }
 
-int aice_read_mem_h(uint8_t target_id, uint32_t address, uint32_t *data)
+static int aice_read_mem_h(uint8_t target_id, uint32_t address, uint32_t *data)
 {
 	int retry_times = 0;
 
@@ -1606,7 +1606,7 @@ int aice_read_mem_h(uint8_t target_id, uint32_t address, uint32_t *data)
 	return ERROR_OK;
 }
 
-int aice_read_mem(uint8_t target_id, uint32_t address, uint32_t *data)
+static int aice_read_mem(uint8_t target_id, uint32_t address, uint32_t *data)
 {
 	int retry_times = 0;
 
@@ -1657,7 +1657,7 @@ int aice_read_mem(uint8_t target_id, uint32_t address, uint32_t *data)
 	return ERROR_OK;
 }
 
-int aice_batch_buffer_read(uint8_t buf_index, uint32_t *word, uint32_t num_of_words)
+static int aice_batch_buffer_read(uint8_t buf_index, uint32_t *word, uint32_t num_of_words)
 {
 	int retry_times = 0;
 
@@ -1760,7 +1760,7 @@ int aice_batch_buffer_write(uint8_t buf_index, const uint8_t *word, uint32_t num
 typedef int (*read_mem_func_t)(uint32_t coreid, uint32_t address, uint32_t *data);
 typedef int (*write_mem_func_t)(uint32_t coreid, uint32_t address, uint32_t data);
 
-struct aice_nds32_info core_info[AICE_MAX_NUM_CORE];
+static struct aice_nds32_info core_info[AICE_MAX_NUM_CORE];
 static uint8_t total_num_of_core;
 
 static char *custom_srst_script;
diff --git a/src/jtag/core.c b/src/jtag/core.c
index 03a26be..5abf832 100644
--- a/src/jtag/core.c
+++ b/src/jtag/core.c
@@ -209,7 +209,7 @@ unsigned jtag_tap_count_enabled(void)
 }
 
 /** Append a new TAP to the chain of all taps. */
-void jtag_tap_add(struct jtag_tap *t)
+static void jtag_tap_add(struct jtag_tap *t)
 {
 	unsigned jtag_num_taps = 0;
 
@@ -953,12 +953,12 @@ int default_interface_jtag_execute_queue(void)
 
 	int result = jtag->jtag_ops->execute_queue();
 
-#if !BUILD_ZY1000
+#if !HAVE_JTAG_MINIDRIVER_H
 	/* Only build this if we use a regular driver with a command queue.
 	 * Otherwise jtag_command_queue won't be found at compile/link time. Its
 	 * definition is in jtag/commands.c, which is only built/linked by
 	 * jtag/Makefile.am if MINIDRIVER_DUMMY || !MINIDRIVER, but those variables
-	 * aren't accessible here. */
+	 * aren't accessible here. Use HAVE_JTAG_MINIDRIVER_H */
 	struct jtag_command *cmd = jtag_command_queue;
 	while (debug_level >= LOG_LVL_DEBUG_IO && cmd) {
 		switch (cmd->type) {
diff --git a/src/jtag/drivers/Makefile.am b/src/jtag/drivers/Makefile.am
index e8d20cc..f7a54b0 100644
--- a/src/jtag/drivers/Makefile.am
+++ b/src/jtag/drivers/Makefile.am
@@ -170,8 +170,15 @@ endif
 if OPENJTAG
 DRIVERFILES += %D%/openjtag.c
 endif
-if CMSIS_DAP
-DRIVERFILES += %D%/cmsis_dap_usb.c
+if CMSIS_DAP_HID
+DRIVERFILES += %D%/cmsis_dap_usb_hid.c
+DRIVERFILES += %D%/cmsis_dap.c
+endif
+if CMSIS_DAP_USB
+DRIVERFILES += %D%/cmsis_dap_usb_bulk.c
+if !CMSIS_DAP_HID
+DRIVERFILES += %D%/cmsis_dap.c
+endif
 endif
 if IMX_GPIO
 DRIVERFILES += %D%/imx_gpio.c
@@ -187,7 +194,9 @@ DRIVERHEADERS = \
 	%D%/bitbang.h \
 	%D%/bitq.h \
 	%D%/jtag_usb_common.h \
+	%D%/libftdi_helper.h \
 	%D%/libusb_helper.h \
+	%D%/cmsis_dap.h \
 	%D%/minidriver_imp.h \
 	%D%/mpsse.h \
 	%D%/rlink.h \
diff --git a/src/jtag/drivers/cmsis_dap_usb.c b/src/jtag/drivers/cmsis_dap.c
index 6d55392..16480ae 100644
--- a/src/jtag/drivers/cmsis_dap_usb.c
+++ b/src/jtag/drivers/cmsis_dap.c
@@ -1,4 +1,7 @@
 /***************************************************************************
+ *   Copyright (C) 2018 by Mickaël Thomas                                  *
+ *   mickael9@gmail.com                                                    *
+ *                                                                         *
  *   Copyright (C) 2016 by Maksym Hilliaka                                 *
  *   oter@frozen-team.com                                                  *
  *                                                                         *
@@ -38,12 +41,17 @@
 #include <jtag/commands.h>
 #include <jtag/tcl.h>
 
-#include <hidapi.h>
+#include "cmsis_dap.h"
 
-/*
- * See CMSIS-DAP documentation:
- * Version 0.01 - Beta.
- */
+static const struct cmsis_dap_backend *const cmsis_dap_backends[] = {
+#if BUILD_CMSIS_DAP_USB == 1
+	&cmsis_dap_usb_backend,
+#endif
+
+#if BUILD_CMSIS_DAP_HID == 1
+	&cmsis_dap_hid_backend,
+#endif
+};
 
 /* USB Config */
 
@@ -52,6 +60,7 @@
  * PID 0xf001: LPC-Link-II CMSIS_DAP
  * PID 0xf002: OPEN-SDA CMSIS_DAP (Freedom Board)
  * PID 0x2722: Keil ULINK2 CMSIS-DAP
+ * PID 0x2750: Keil ULINKplus CMSIS-DAP
  *
  * VID 0x0d28: mbed Software
  * PID 0x0204: MBED CMSIS-DAP
@@ -61,10 +70,10 @@
 /* vid = pid = 0 marks the end of the list */
 static uint16_t cmsis_dap_vid[MAX_USB_IDS + 1] = { 0 };
 static uint16_t cmsis_dap_pid[MAX_USB_IDS + 1] = { 0 };
-static wchar_t *cmsis_dap_serial;
+static char *cmsis_dap_serial;
+static int cmsis_dap_backend = -1;
 static bool swd_mode;
 
-#define PACKET_SIZE       (64 + 1)	/* 64 bytes plus report id */
 #define USB_TIMEOUT       1000
 
 /* CMSIS-DAP General Commands */
@@ -163,14 +172,6 @@ static const char * const info_caps_str[] = {
 /* max clock speed (kHz) */
 #define DAP_MAX_CLOCK             5000
 
-struct cmsis_dap {
-	hid_device *dev_handle;
-	uint16_t packet_size;
-	int packet_count;
-	uint8_t *packet_buffer;
-	uint8_t caps;
-	uint8_t mode;
-};
 
 struct pending_transfer_result {
 	uint8_t cmd;
@@ -223,134 +224,66 @@ static uint8_t output_pins = SWJ_PIN_SRST | SWJ_PIN_TRST;
 
 static struct cmsis_dap *cmsis_dap_handle;
 
-static int cmsis_dap_usb_open(void)
-{
-	hid_device *dev = NULL;
-	int i;
-	struct hid_device_info *devs, *cur_dev;
-	unsigned short target_vid, target_pid;
-	bool found = false;
 
-	target_vid = 0;
-	target_pid = 0;
+static int cmsis_dap_open(void)
+{
+	const struct cmsis_dap_backend *backend = NULL;
 
-	if (hid_init() != 0) {
-		LOG_ERROR("unable to open HIDAPI");
+	struct cmsis_dap *dap = malloc(sizeof(struct cmsis_dap));
+	if (dap == NULL) {
+		LOG_ERROR("unable to allocate memory");
 		return ERROR_FAIL;
 	}
 
-	/*
-	 * The CMSIS-DAP specification stipulates:
-	 * "The Product String must contain "CMSIS-DAP" somewhere in the string. This is used by the
-	 * debuggers to identify a CMSIS-DAP compliant Debug Unit that is connected to a host computer."
-	 */
-	devs = hid_enumerate(0x0, 0x0);
-	cur_dev = devs;
-	while (NULL != cur_dev) {
-		if (0 == cmsis_dap_vid[0]) {
-			if (NULL == cur_dev->product_string) {
-				LOG_DEBUG("Cannot read product string of device 0x%x:0x%x",
-					  cur_dev->vendor_id, cur_dev->product_id);
-			} else {
-				if (wcsstr(cur_dev->product_string, L"CMSIS-DAP")) {
-					/* if the user hasn't specified VID:PID *and*
-					 * product string contains "CMSIS-DAP", pick it
-					 */
-					found = true;
-				}
-			}
-		} else {
-			/* otherwise, exhaustively compare against all VID:PID in list */
-			for (i = 0; cmsis_dap_vid[i] || cmsis_dap_pid[i]; i++) {
-				if ((cmsis_dap_vid[i] == cur_dev->vendor_id) && (cmsis_dap_pid[i] == cur_dev->product_id))
-					found = true;
-			}
-
-			if (cmsis_dap_vid[i] || cmsis_dap_pid[i])
-				found = true;
-		}
-
-		/* LPC-LINK2 has cmsis-dap on interface 0 and other HID functions on other interfaces */
-		if (cur_dev->vendor_id == 0x1fc9 && cur_dev->product_id == 0x0090 && cur_dev->interface_number != 0)
-			found = false;
+	dap->caps = 0;
+	dap->mode = 0;
+	dap->packet_size = 0; /* initialized by backend */
 
-		if (found) {
-			/* we have found an adapter, so exit further checks */
-			/* check serial number matches if given */
-			if (cmsis_dap_serial != NULL) {
-				if ((cur_dev->serial_number != NULL) && wcscmp(cmsis_dap_serial, cur_dev->serial_number) == 0) {
-					break;
-				}
-			} else
+	if (cmsis_dap_backend >= 0) {
+		/* Use forced backend */
+		backend = cmsis_dap_backends[cmsis_dap_backend];
+		if (backend->open(dap, cmsis_dap_vid, cmsis_dap_pid, cmsis_dap_serial) != ERROR_OK)
+			backend = NULL;
+	} else {
+		/* Try all backends */
+		for (unsigned int i = 0; i < ARRAY_SIZE(cmsis_dap_backends); i++) {
+			backend = cmsis_dap_backends[i];
+			if (backend->open(dap, cmsis_dap_vid, cmsis_dap_pid, cmsis_dap_serial) == ERROR_OK)
 				break;
-
-			found = false;
+			else
+				backend = NULL;
 		}
-
-		cur_dev = cur_dev->next;
-	}
-
-	if (NULL != cur_dev) {
-		target_vid = cur_dev->vendor_id;
-		target_pid = cur_dev->product_id;
 	}
 
-	if (target_vid == 0 && target_pid == 0) {
-		LOG_ERROR("unable to find CMSIS-DAP device");
-		hid_free_enumeration(devs);
+	if (backend == NULL) {
+		LOG_ERROR("unable to find a matching CMSIS-DAP device");
+		free(dap);
 		return ERROR_FAIL;
 	}
 
-	dev = hid_open_path(cur_dev->path);
-	hid_free_enumeration(devs);
+	assert(dap->packet_size > 0);
 
-	if (dev == NULL) {
-		LOG_ERROR("unable to open CMSIS-DAP device 0x%x:0x%x", target_vid, target_pid);
-		return ERROR_FAIL;
-	}
+	dap->backend = backend;
+	dap->packet_buffer = malloc(dap->packet_size);
 
-	struct cmsis_dap *dap = malloc(sizeof(struct cmsis_dap));
-	if (dap == NULL) {
+	if (dap->packet_buffer == NULL) {
 		LOG_ERROR("unable to allocate memory");
+		dap->backend->close(dap);
+		free(dap);
 		return ERROR_FAIL;
 	}
 
-	dap->dev_handle = dev;
-	dap->caps = 0;
-	dap->mode = 0;
-
 	cmsis_dap_handle = dap;
 
-	/* allocate default packet buffer, may be changed later.
-	 * currently with HIDAPI we have no way of getting the output report length
-	 * without this info we cannot communicate with the adapter.
-	 * For the moment we ahve to hard code the packet size */
-
-	int packet_size = PACKET_SIZE;
-
-	/* atmel cmsis-dap uses 512 byte reports */
-	/* except when it doesn't e.g. with mEDBG on SAMD10 Xplained
-	 * board */
-	/* TODO: HID report descriptor should be parsed instead of
-	 * hardcoding a match by VID */
-	if (target_vid == 0x03eb && target_pid != 0x2145 && target_pid != 0x2175)
-		packet_size = 512 + 1;
-
-	cmsis_dap_handle->packet_buffer = malloc(packet_size);
-	cmsis_dap_handle->packet_size = packet_size;
-
-	if (cmsis_dap_handle->packet_buffer == NULL) {
-		LOG_ERROR("unable to allocate memory");
-		return ERROR_FAIL;
-	}
-
 	return ERROR_OK;
 }
 
-static void cmsis_dap_usb_close(struct cmsis_dap *dap)
+static void cmsis_dap_close(struct cmsis_dap *dap)
 {
-	hid_close(dap->dev_handle);
-	hid_exit();
+	if (dap->backend) {
+		dap->backend->close(dap);
+		dap->backend = NULL;
+	}
 
 	free(cmsis_dap_handle->packet_buffer);
 	free(cmsis_dap_handle);
@@ -364,47 +297,27 @@ static void cmsis_dap_usb_close(struct cmsis_dap *dap)
 	}
 }
 
-static int cmsis_dap_usb_write(struct cmsis_dap *dap, int txlen)
-{
-#ifdef CMSIS_DAP_JTAG_DEBUG
-	LOG_DEBUG("cmsis-dap usb xfer cmd=%02X", dap->packet_buffer[1]);
-#endif
-	/* Pad the rest of the TX buffer with 0's */
-	memset(dap->packet_buffer + txlen, 0, dap->packet_size - txlen);
-
-	/* write data to device */
-	int retval = hid_write(dap->dev_handle, dap->packet_buffer, dap->packet_size);
-	if (retval == -1) {
-		LOG_ERROR("error writing data: %ls", hid_error(dap->dev_handle));
-		return ERROR_FAIL;
-	}
-
-	return ERROR_OK;
-}
-
 /* Send a message and receive the reply */
-static int cmsis_dap_usb_xfer(struct cmsis_dap *dap, int txlen)
+static int cmsis_dap_xfer(struct cmsis_dap *dap, int txlen)
 {
 	if (pending_fifo_block_count) {
 		LOG_ERROR("pending %d blocks, flushing", pending_fifo_block_count);
 		while (pending_fifo_block_count) {
-			hid_read_timeout(dap->dev_handle, dap->packet_buffer, dap->packet_size, 10);
+			dap->backend->read(dap, 10);
 			pending_fifo_block_count--;
 		}
 		pending_fifo_put_idx = 0;
 		pending_fifo_get_idx = 0;
 	}
 
-	int retval = cmsis_dap_usb_write(dap, txlen);
-	if (retval != ERROR_OK)
+	int retval = dap->backend->write(dap, txlen, USB_TIMEOUT);
+	if (retval < 0)
 		return retval;
 
 	/* get reply */
-	retval = hid_read_timeout(dap->dev_handle, dap->packet_buffer, dap->packet_size, USB_TIMEOUT);
-	if (retval == -1 || retval == 0) {
-		LOG_DEBUG("error reading data: %ls", hid_error(dap->dev_handle));
-		return ERROR_FAIL;
-	}
+	retval = dap->backend->read(dap, USB_TIMEOUT);
+	if (retval < 0)
+		return retval;
 
 	return ERROR_OK;
 }
@@ -422,7 +335,7 @@ static int cmsis_dap_cmd_DAP_SWJ_Pins(uint8_t pins, uint8_t mask, uint32_t delay
 	buffer[5] = (delay >> 8) & 0xff;
 	buffer[6] = (delay >> 16) & 0xff;
 	buffer[7] = (delay >> 24) & 0xff;
-	retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 8);
+	retval = cmsis_dap_xfer(cmsis_dap_handle, 8);
 
 	if (retval != ERROR_OK) {
 		LOG_ERROR("CMSIS-DAP command CMD_DAP_SWJ_PINS failed.");
@@ -448,7 +361,7 @@ static int cmsis_dap_cmd_DAP_SWJ_Clock(uint32_t swj_clock)
 	buffer[3] = (swj_clock >> 8) & 0xff;
 	buffer[4] = (swj_clock >> 16) & 0xff;
 	buffer[5] = (swj_clock >> 24) & 0xff;
-	retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 6);
+	retval = cmsis_dap_xfer(cmsis_dap_handle, 6);
 
 	if (retval != ERROR_OK || buffer[1] != DAP_OK) {
 		LOG_ERROR("CMSIS-DAP command CMD_DAP_SWJ_CLOCK failed.");
@@ -477,7 +390,7 @@ static int cmsis_dap_cmd_DAP_SWJ_Sequence(uint8_t s_len, const uint8_t *sequence
 	buffer[2] = s_len;
 	bit_copy(&buffer[3], 0, sequence, 0, s_len);
 
-	retval = cmsis_dap_usb_xfer(cmsis_dap_handle, DIV_ROUND_UP(s_len, 8) + 3);
+	retval = cmsis_dap_xfer(cmsis_dap_handle, DIV_ROUND_UP(s_len, 8) + 3);
 
 	if (retval != ERROR_OK || buffer[1] != DAP_OK)
 		return ERROR_FAIL;
@@ -493,7 +406,7 @@ static int cmsis_dap_cmd_DAP_Info(uint8_t info, uint8_t **data)
 	buffer[0] = 0;	/* report number */
 	buffer[1] = CMD_DAP_INFO;
 	buffer[2] = info;
-	retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 3);
+	retval = cmsis_dap_xfer(cmsis_dap_handle, 3);
 
 	if (retval != ERROR_OK) {
 		LOG_ERROR("CMSIS-DAP command CMD_INFO failed.");
@@ -514,7 +427,7 @@ static int cmsis_dap_cmd_DAP_LED(uint8_t led, uint8_t state)
 	buffer[1] = CMD_DAP_LED;
 	buffer[2] = led;
 	buffer[3] = state;
-	retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 4);
+	retval = cmsis_dap_xfer(cmsis_dap_handle, 4);
 
 	if (retval != ERROR_OK || buffer[1] != 0x00) {
 		LOG_ERROR("CMSIS-DAP command CMD_LED failed.");
@@ -532,7 +445,7 @@ static int cmsis_dap_cmd_DAP_Connect(uint8_t mode)
 	buffer[0] = 0;	/* report number */
 	buffer[1] = CMD_DAP_CONNECT;
 	buffer[2] = mode;
-	retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 3);
+	retval = cmsis_dap_xfer(cmsis_dap_handle, 3);
 
 	if (retval != ERROR_OK) {
 		LOG_ERROR("CMSIS-DAP command CMD_CONNECT failed.");
@@ -554,7 +467,7 @@ static int cmsis_dap_cmd_DAP_Disconnect(void)
 
 	buffer[0] = 0;	/* report number */
 	buffer[1] = CMD_DAP_DISCONNECT;
-	retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 2);
+	retval = cmsis_dap_xfer(cmsis_dap_handle, 2);
 
 	if (retval != ERROR_OK || buffer[1] != DAP_OK) {
 		LOG_ERROR("CMSIS-DAP command CMD_DISCONNECT failed.");
@@ -576,7 +489,7 @@ static int cmsis_dap_cmd_DAP_TFER_Configure(uint8_t idle, uint16_t retry_count,
 	buffer[4] = (retry_count >> 8) & 0xff;
 	buffer[5] = match_retry & 0xff;
 	buffer[6] = (match_retry >> 8) & 0xff;
-	retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 7);
+	retval = cmsis_dap_xfer(cmsis_dap_handle, 7);
 
 	if (retval != ERROR_OK || buffer[1] != DAP_OK) {
 		LOG_ERROR("CMSIS-DAP command CMD_TFER_Configure failed.");
@@ -594,7 +507,7 @@ static int cmsis_dap_cmd_DAP_SWD_Configure(uint8_t cfg)
 	buffer[0] = 0;	/* report number */
 	buffer[1] = CMD_DAP_SWD_CONFIGURE;
 	buffer[2] = cfg;
-	retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 3);
+	retval = cmsis_dap_xfer(cmsis_dap_handle, 3);
 
 	if (retval != ERROR_OK || buffer[1] != DAP_OK) {
 		LOG_ERROR("CMSIS-DAP command CMD_SWD_Configure failed.");
@@ -614,7 +527,7 @@ static int cmsis_dap_cmd_DAP_Delay(uint16_t delay_us)
 	buffer[1] = CMD_DAP_DELAY;
 	buffer[2] = delay_us & 0xff;
 	buffer[3] = (delay_us >> 8) & 0xff;
-	retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 4);
+	retval = cmsis_dap_xfer(cmsis_dap_handle, 4);
 
 	if (retval != ERROR_OK || buffer[1] != DAP_OK) {
 		LOG_ERROR("CMSIS-DAP command CMD_Delay failed.");
@@ -684,9 +597,14 @@ static void cmsis_dap_swd_write_from_queue(struct cmsis_dap *dap)
 		}
 	}
 
-	queued_retval = cmsis_dap_usb_write(dap, idx);
-	if (queued_retval != ERROR_OK)
+	int retval = dap->backend->write(dap, idx, USB_TIMEOUT);
+
+	if (retval < 0) {
+		queued_retval = retval;
 		goto skip;
+	} else {
+		queued_retval = ERROR_OK;
+	}
 
 	pending_fifo_put_idx = (pending_fifo_put_idx + 1) % dap->packet_count;
 	pending_fifo_block_count++;
@@ -708,12 +626,12 @@ static void cmsis_dap_swd_read_process(struct cmsis_dap *dap, int timeout_ms)
 		LOG_ERROR("no pending write");
 
 	/* get reply */
-	int retval = hid_read_timeout(dap->dev_handle, dap->packet_buffer, dap->packet_size, timeout_ms);
-	if (retval == 0 && timeout_ms < USB_TIMEOUT)
+	int retval = dap->backend->read(dap, timeout_ms);
+	if (retval == ERROR_TIMEOUT_REACHED && timeout_ms < USB_TIMEOUT)
 		return;
 
-	if (retval == -1 || retval == 0) {
-		LOG_DEBUG("error reading data: %ls", hid_error(dap->dev_handle));
+	if (retval <= 0) {
+		LOG_DEBUG("error reading data");
 		queued_retval = ERROR_FAIL;
 		goto skip;
 	}
@@ -969,7 +887,7 @@ static int cmsis_dap_init(void)
 	int retval;
 	uint8_t *data;
 
-	retval = cmsis_dap_usb_open();
+	retval = cmsis_dap_open();
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -1048,7 +966,7 @@ static int cmsis_dap_init(void)
 		LOG_DEBUG("CMSIS-DAP: Packet Count = %d", pkt_cnt);
 	}
 
-	LOG_DEBUG("Allocating FIFO for %d pending HID requests", cmsis_dap_handle->packet_count);
+	LOG_DEBUG("Allocating FIFO for %d pending packets", cmsis_dap_handle->packet_count);
 	for (int i = 0; i < cmsis_dap_handle->packet_count; i++) {
 		pending_fifo[i].transfers = malloc(pending_queue_len * sizeof(struct pending_transfer_result));
 		if (!pending_fifo[i].transfers) {
@@ -1120,7 +1038,7 @@ static int cmsis_dap_quit(void)
 	cmsis_dap_cmd_DAP_LED(LED_ID_RUN, LED_OFF);
 	cmsis_dap_cmd_DAP_LED(LED_ID_CONNECT, LED_OFF);
 
-	cmsis_dap_usb_close(cmsis_dap_handle);
+	cmsis_dap_close(cmsis_dap_handle);
 
 	return ERROR_OK;
 }
@@ -1261,7 +1179,7 @@ static void cmsis_dap_flush(void)
 #endif
 
 	/* send command to USB device */
-	int retval = cmsis_dap_usb_xfer(cmsis_dap_handle, queued_seq_buf_end + 3);
+	int retval = cmsis_dap_xfer(cmsis_dap_handle, queued_seq_buf_end + 3);
 	if (retval != ERROR_OK || buffer[1] != DAP_OK) {
 		LOG_ERROR("CMSIS-DAP command CMD_DAP_JTAG_SEQ failed.");
 		exit(-1);
@@ -1668,7 +1586,7 @@ COMMAND_HANDLER(cmsis_dap_handle_cmd_command)
 	for (i = 0; i < CMD_ARGC; i++)
 		buffer[i + 1] = strtoul(CMD_ARGV[i], NULL, 16);
 
-	retval = cmsis_dap_usb_xfer(cmsis_dap_handle, CMD_ARGC + 1);
+	retval = cmsis_dap_xfer(cmsis_dap_handle, CMD_ARGC + 1);
 
 	if (retval != ERROR_OK) {
 		LOG_ERROR("CMSIS-DAP command failed.");
@@ -1713,20 +1631,31 @@ COMMAND_HANDLER(cmsis_dap_handle_vid_pid_command)
 
 COMMAND_HANDLER(cmsis_dap_handle_serial_command)
 {
+	if (CMD_ARGC == 1)
+		cmsis_dap_serial = strdup(CMD_ARGV[0]);
+	else
+		LOG_ERROR("expected exactly one argument to cmsis_dap_serial <serial-number>");
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(cmsis_dap_handle_backend_command)
+{
 	if (CMD_ARGC == 1) {
-		size_t len = mbstowcs(NULL, CMD_ARGV[0], 0);
-		cmsis_dap_serial = calloc(len + 1, sizeof(wchar_t));
-		if (cmsis_dap_serial == NULL) {
-			LOG_ERROR("unable to allocate memory");
-			return ERROR_OK;
-		}
-		if (mbstowcs(cmsis_dap_serial, CMD_ARGV[0], len + 1) == (size_t)-1) {
-			free(cmsis_dap_serial);
-			cmsis_dap_serial = NULL;
-			LOG_ERROR("unable to convert serial");
+		if (strcmp(CMD_ARGV[0], "auto") == 0) {
+			cmsis_dap_backend = -1; /* autoselect */
+		} else {
+			for (unsigned int i = 0; i < ARRAY_SIZE(cmsis_dap_backends); i++) {
+				if (strcasecmp(cmsis_dap_backends[i]->name, CMD_ARGV[0]) == 0) {
+					cmsis_dap_backend = i;
+					return ERROR_OK;
+				}
+			}
+
+			LOG_ERROR("invalid backend argument to cmsis_dap_backend <backend>");
 		}
 	} else {
-		LOG_ERROR("expected exactly one argument to cmsis_dap_serial <serial-number>");
+		LOG_ERROR("expected exactly one argument to cmsis_dap_backend <backend>");
 	}
 
 	return ERROR_OK;
@@ -1750,6 +1679,7 @@ static const struct command_registration cmsis_dap_subcommand_handlers[] = {
 	COMMAND_REGISTRATION_DONE
 };
 
+
 static const struct command_registration cmsis_dap_command_handlers[] = {
 	{
 		.name = "cmsis-dap",
@@ -1772,6 +1702,22 @@ static const struct command_registration cmsis_dap_command_handlers[] = {
 		.help = "set the serial number of the adapter",
 		.usage = "serial_string",
 	},
+	{
+		.name = "cmsis_dap_backend",
+		.handler = &cmsis_dap_handle_backend_command,
+		.mode = COMMAND_CONFIG,
+		.help = "set the communication backend to use (USB bulk or HID).",
+		.usage = "(auto | usb_bulk | hid)",
+	},
+#if BUILD_CMSIS_DAP_USB
+	{
+		.name = "cmsis_dap_usb",
+		.chain = cmsis_dap_usb_subcommand_handlers,
+		.mode = COMMAND_ANY,
+		.help = "USB bulk backend-specific commands",
+		.usage = "<cmd>",
+	},
+#endif
 	COMMAND_REGISTRATION_DONE
 };
 
diff --git a/src/jtag/drivers/cmsis_dap.h b/src/jtag/drivers/cmsis_dap.h
new file mode 100644
index 0000000..054621c
--- /dev/null
+++ b/src/jtag/drivers/cmsis_dap.h
@@ -0,0 +1,32 @@
+#ifndef OPENOCD_JTAG_DRIVERS_CMSIS_DAP_H
+#define OPENOCD_JTAG_DRIVERS_CMSIS_DAP_H
+
+#include <stdint.h>
+
+struct cmsis_dap_backend;
+struct cmsis_dap_backend_data;
+struct command_registration;
+
+struct cmsis_dap {
+	struct cmsis_dap_backend_data *bdata;
+	const struct cmsis_dap_backend *backend;
+	uint16_t packet_size;
+	int packet_count;
+	uint8_t *packet_buffer;
+	uint8_t caps;
+	uint8_t mode;
+};
+
+struct cmsis_dap_backend {
+	const char *name;
+	int (*open)(struct cmsis_dap *dap, uint16_t vids[], uint16_t pids[], char *serial);
+	void (*close)(struct cmsis_dap *dap);
+	int (*read)(struct cmsis_dap *dap, int timeout_ms);
+	int (*write)(struct cmsis_dap *dap, int len, int timeout_ms);
+};
+
+extern const struct cmsis_dap_backend cmsis_dap_hid_backend;
+extern const struct cmsis_dap_backend cmsis_dap_usb_backend;
+extern const struct command_registration cmsis_dap_usb_subcommand_handlers[];
+
+#endif
diff --git a/src/jtag/drivers/cmsis_dap_usb_bulk.c b/src/jtag/drivers/cmsis_dap_usb_bulk.c
new file mode 100644
index 0000000..0134c03
--- /dev/null
+++ b/src/jtag/drivers/cmsis_dap_usb_bulk.c
@@ -0,0 +1,454 @@
+/***************************************************************************
+ *   Copyright (C) 2018 by Mickaël Thomas                                  *
+ *   mickael9@gmail.com                                                    *
+ *                                                                         *
+ *   Copyright (C) 2016 by Maksym Hilliaka                                 *
+ *   oter@frozen-team.com                                                  *
+ *                                                                         *
+ *   Copyright (C) 2016 by Phillip Pearson                                 *
+ *   pp@myelin.co.nz                                                       *
+ *                                                                         *
+ *   Copyright (C) 2014 by Paul Fertser                                    *
+ *   fercerpav@gmail.com                                                   *
+ *                                                                         *
+ *   Copyright (C) 2013 by mike brown                                      *
+ *   mike@theshedworks.org.uk                                              *
+ *                                                                         *
+ *   Copyright (C) 2013 by Spencer Oliver                                  *
+ *   spen@spen-soft.co.uk                                                  *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <libusb.h>
+#include <helper/log.h>
+
+#include "cmsis_dap.h"
+
+struct cmsis_dap_backend_data {
+	libusb_context *usb_ctx;
+	libusb_device_handle *dev_handle;
+	unsigned int ep_out;
+	unsigned int ep_in;
+	int interface;
+};
+
+static int cmsis_dap_usb_interface = -1;
+
+static int cmsis_dap_usb_open(struct cmsis_dap *dap, uint16_t vids[], uint16_t pids[], char *serial)
+{
+	int err;
+	libusb_context *ctx;
+	libusb_device **device_list;
+
+	err = libusb_init(&ctx);
+	if (err) {
+		LOG_ERROR("libusb initialization failed: %s", libusb_strerror(err));
+		return ERROR_FAIL;
+	}
+
+	int num_devices = libusb_get_device_list(ctx, &device_list);
+	if (num_devices < 0) {
+		LOG_ERROR("could not enumerate USB devices: %s", libusb_strerror(num_devices));
+		libusb_exit(ctx);
+		return ERROR_FAIL;
+	}
+
+	for (int i = 0; i < num_devices; i++) {
+		libusb_device *dev = device_list[i];
+		struct libusb_device_descriptor dev_desc;
+
+		err = libusb_get_device_descriptor(dev, &dev_desc);
+		if (err) {
+			LOG_ERROR("could not get device descriptor for device %d: %s", i, libusb_strerror(err));
+			continue;
+		}
+
+		/* Match VID/PID */
+
+		bool id_match = false;
+		bool id_filter = vids[0] || pids[0];
+		for (int id = 0; vids[id] || pids[id]; id++) {
+			id_match = !vids[id] || dev_desc.idVendor == vids[id];
+			id_match &= !pids[id] || dev_desc.idProduct == pids[id];
+
+			if (id_match)
+				break;
+		}
+
+		if (id_filter && !id_match)
+			continue;
+
+		/* Don't continue if we asked for a serial number and the device doesn't have one */
+		if (dev_desc.iSerialNumber == 0 && serial && serial[0])
+			continue;
+
+		libusb_device_handle *dev_handle = NULL;
+		err = libusb_open(dev, &dev_handle);
+		if (err) {
+			/* It's to be expected that most USB devices can't be opened
+			 * so only report an error if it was explicitly selected
+			 */
+			if (id_filter) {
+				LOG_ERROR("could not open device 0x%04x:0x%04x: %s",
+						dev_desc.idVendor, dev_desc.idProduct, libusb_strerror(err));
+			} else {
+				LOG_DEBUG("could not open device 0x%04x:0x%04x: %s",
+						dev_desc.idVendor, dev_desc.idProduct, libusb_strerror(err));
+			}
+			continue;
+		}
+
+		/* Match serial number */
+
+		bool serial_match = false;
+		char dev_serial[256] = {0};
+		if (dev_desc.iSerialNumber > 0) {
+			err = libusb_get_string_descriptor_ascii(
+					dev_handle, dev_desc.iSerialNumber,
+					(uint8_t *)dev_serial, sizeof(dev_serial));
+
+			if (err < 0) {
+				const char *msg = "could not read serial number for device 0x%04x:0x%04x: %s";
+				if (serial)
+					LOG_WARNING(msg, dev_desc.idVendor, dev_desc.idProduct,
+								libusb_strerror(err));
+				else
+					LOG_DEBUG(msg, dev_desc.idVendor, dev_desc.idProduct,
+								libusb_strerror(err));
+			} else if (serial && strncmp(dev_serial, serial, sizeof(dev_serial)) == 0) {
+				serial_match = true;
+			}
+		}
+
+		if (serial && !serial_match) {
+			libusb_close(dev_handle);
+			continue;
+		}
+
+		/* Find the CMSIS-DAP string in product string */
+
+		bool cmsis_dap_in_product_str = false;
+		char product_string[256] = {0};
+		if (dev_desc.iProduct > 0) {
+			err = libusb_get_string_descriptor_ascii(
+					dev_handle, dev_desc.iProduct,
+					(uint8_t *)product_string, sizeof(product_string));
+			if (err < 0) {
+				LOG_WARNING("could not read product string for device 0x%04x:0x%04x: %s",
+						dev_desc.idVendor, dev_desc.idProduct, libusb_strerror(err));
+			} else if (strstr(product_string, "CMSIS-DAP")) {
+				LOG_DEBUG("found product string of 0x%04x:0x%04x '%s'",
+						  dev_desc.idVendor, dev_desc.idProduct, product_string);
+				cmsis_dap_in_product_str = true;
+			}
+		}
+
+		bool device_identified_reliably = cmsis_dap_in_product_str
+											|| serial_match || id_match;
+
+		/* Find the CMSIS-DAP interface */
+
+		for (int config = 0; config < dev_desc.bNumConfigurations; config++) {
+			struct libusb_config_descriptor *config_desc;
+			err = libusb_get_config_descriptor(dev, config, &config_desc);
+			if (err) {
+				LOG_ERROR("could not get configuration descriptor %d for device 0x%04x:0x%04x: %s",
+						config, dev_desc.idVendor, dev_desc.idProduct, libusb_strerror(err));
+				continue;
+			}
+
+			LOG_DEBUG("enumerating interfaces of 0x%04x:0x%04x",
+					  dev_desc.idVendor, dev_desc.idProduct);
+			int config_num = config_desc->bConfigurationValue;
+			const struct libusb_interface_descriptor *intf_desc_candidate = NULL;
+			const struct libusb_interface_descriptor *intf_desc_found = NULL;
+
+			for (int interface = 0; interface < config_desc->bNumInterfaces; interface++) {
+				const struct libusb_interface_descriptor *intf_desc = &config_desc->interface[interface].altsetting[0];
+				int interface_num = intf_desc->bInterfaceNumber;
+
+				/* Skip this interface if another one was requested explicitly */
+				if (cmsis_dap_usb_interface != -1 && cmsis_dap_usb_interface != interface_num)
+					continue;
+
+				/* CMSIS-DAP v2 spec says:
+				 *
+				 * CMSIS-DAP with default V2 configuration uses WinUSB and is therefore faster.
+				 * Optionally support for streaming SWO trace is provided via an additional USB endpoint.
+				 *
+				 * The WinUSB configuration requires custom class support with the interface setting
+				 *     Class Code: 0xFF (Vendor specific)
+				 *     Subclass: 0x00
+				 *     Protocol code: 0x00
+				 *
+				 * Depending on the configuration it uses the following USB endpoints which should be configured
+				 * in the interface descriptor in this order:
+				 *  - Endpoint 1: Bulk Out – used for commands received from host PC.
+				 *  - Endpoint 2: Bulk In – used for responses send to host PC.
+				 *  - Endpoint 3: Bulk In (optional) – used for streaming SWO trace (if enabled with SWO_STREAM).
+				 */
+
+				/* Search for "CMSIS-DAP" in the interface string */
+				bool cmsis_dap_in_interface_str = false;
+				if (intf_desc->iInterface != 0) {
+
+					char interface_str[256] = {0};
+
+					err = libusb_get_string_descriptor_ascii(
+							dev_handle, intf_desc->iInterface,
+							(uint8_t *)interface_str, sizeof(interface_str));
+					if (err < 0) {
+						LOG_DEBUG("could not read interface string %d for device 0x%04x:0x%04x: %s",
+								  intf_desc->iInterface,
+								  dev_desc.idVendor, dev_desc.idProduct,
+								  libusb_strerror(err));
+					} else if (strstr(interface_str, "CMSIS-DAP")) {
+						cmsis_dap_in_interface_str = true;
+						LOG_DEBUG("found interface %d string '%s'",
+								  interface_num, interface_str);
+					}
+				}
+
+				/* Bypass the following check if this interface was explicitly requested. */
+				if (cmsis_dap_usb_interface == -1) {
+					if (!cmsis_dap_in_product_str && !cmsis_dap_in_interface_str)
+						continue;
+				}
+
+				/* check endpoints */
+				if (intf_desc->bNumEndpoints < 2) {
+					LOG_DEBUG("skipping interface %d, has only %d endpoints",
+							  interface_num, intf_desc->bNumEndpoints);
+					continue;
+				}
+
+				if ((intf_desc->endpoint[0].bmAttributes & 3) != LIBUSB_TRANSFER_TYPE_BULK ||
+						(intf_desc->endpoint[0].bEndpointAddress & 0x80) != LIBUSB_ENDPOINT_OUT) {
+					LOG_DEBUG("skipping interface %d, endpoint[0] is not bulk out",
+							  interface_num);
+					continue;
+				}
+
+				if ((intf_desc->endpoint[1].bmAttributes & 3) != LIBUSB_TRANSFER_TYPE_BULK ||
+						(intf_desc->endpoint[1].bEndpointAddress & 0x80) != LIBUSB_ENDPOINT_IN) {
+					LOG_DEBUG("skipping interface %d, endpoint[1] is not bulk in",
+							  interface_num);
+					continue;
+				}
+
+				/* We can rely on the interface is really CMSIS-DAP if
+				 * - we've seen CMSIS-DAP in the interface string
+				 * - config asked explicitly for an interface number
+				 * - the device has only one interface
+				 * The later two cases should be honored only if we know
+				 * we are on the rigt device */
+				bool intf_identified_reliably = cmsis_dap_in_interface_str
+							|| (device_identified_reliably &&
+									(cmsis_dap_usb_interface != -1
+									 || config_desc->bNumInterfaces == 1));
+
+				if (intf_desc->bInterfaceClass != LIBUSB_CLASS_VENDOR_SPEC ||
+						intf_desc->bInterfaceSubClass != 0 || intf_desc->bInterfaceProtocol != 0) {
+					/* If the interface is reliably identified
+					 * then we need not insist on setting USB class, subclass and protocol
+					 * exactly as the specification requires.
+					 * At least KitProg3 uses class 0 contrary to the specification */
+					if (intf_identified_reliably) {
+						LOG_WARNING("Using CMSIS-DAPv2 interface %d with wrong class %" PRId8
+								  " subclass %" PRId8 " or protocol %" PRId8,
+								  interface_num,
+								  intf_desc->bInterfaceClass,
+								  intf_desc->bInterfaceSubClass,
+								  intf_desc->bInterfaceProtocol);
+					} else {
+						LOG_DEBUG("skipping interface %d, class %" PRId8
+								  " subclass %" PRId8 " protocol %" PRId8,
+								  interface_num,
+								  intf_desc->bInterfaceClass,
+								  intf_desc->bInterfaceSubClass,
+								  intf_desc->bInterfaceProtocol);
+						continue;
+
+					}
+				}
+
+				if (intf_identified_reliably) {
+					/* That's the one! */
+					intf_desc_found = intf_desc;
+					break;
+				}
+
+				if (!intf_desc_candidate && device_identified_reliably) {
+					/* This interface looks suitable for CMSIS-DAP. Store the pointer to it
+					 * and keep searching for another one with CMSIS-DAP in interface string */
+					intf_desc_candidate = intf_desc;
+				}
+			}
+
+			if (!intf_desc_found) {
+				/* We were not able to identify reliably which interface is CMSIS-DAP.
+				 * Let's use the first suitable if we found one */
+				intf_desc_found = intf_desc_candidate;
+			}
+
+			if (!intf_desc_found) {
+				libusb_free_config_descriptor(config_desc);
+				continue;
+			}
+
+			/* We've chosen an interface, connect to it */
+			int interface_num = intf_desc_found->bInterfaceNumber;
+			int packet_size = intf_desc_found->endpoint[0].wMaxPacketSize;
+			int ep_out = intf_desc_found->endpoint[0].bEndpointAddress;
+			int ep_in = intf_desc_found->endpoint[1].bEndpointAddress;
+
+			libusb_free_config_descriptor(config_desc);
+			libusb_free_device_list(device_list, true);
+
+			LOG_INFO("Using CMSIS-DAPv2 interface with VID:PID=0x%04x:0x%04x, serial=%s",
+					dev_desc.idVendor, dev_desc.idProduct, dev_serial);
+
+			int current_config;
+			err = libusb_get_configuration(dev_handle, &current_config);
+			if (err) {
+				LOG_ERROR("could not find current configuration: %s", libusb_strerror(err));
+				libusb_close(dev_handle);
+				libusb_exit(ctx);
+				return ERROR_FAIL;
+			}
+
+			if (config_num != current_config) {
+				err = libusb_set_configuration(dev_handle, config_num);
+				if (err) {
+					LOG_ERROR("could not set configuration: %s", libusb_strerror(err));
+					libusb_close(dev_handle);
+					libusb_exit(ctx);
+					return ERROR_FAIL;
+				}
+			}
+
+			err = libusb_claim_interface(dev_handle, interface_num);
+			if (err)
+				LOG_WARNING("could not claim interface: %s", libusb_strerror(err));
+
+			dap->bdata = malloc(sizeof(struct cmsis_dap_backend_data));
+			if (dap->bdata == NULL) {
+				LOG_ERROR("unable to allocate memory");
+				libusb_release_interface(dev_handle, interface_num);
+				libusb_close(dev_handle);
+				libusb_exit(ctx);
+				return ERROR_FAIL;
+			}
+
+			dap->packet_size = packet_size + 1; /* "+ 1" for compatibility with the HID backend */
+			dap->bdata->usb_ctx = ctx;
+			dap->bdata->dev_handle = dev_handle;
+			dap->bdata->ep_out = ep_out;
+			dap->bdata->ep_in = ep_in;
+			dap->bdata->interface = interface_num;
+			return ERROR_OK;
+		}
+
+		libusb_close(dev_handle);
+	}
+
+	libusb_free_device_list(device_list, true);
+
+	libusb_exit(ctx);
+	return ERROR_FAIL;
+}
+
+static void cmsis_dap_usb_close(struct cmsis_dap *dap)
+{
+	libusb_release_interface(dap->bdata->dev_handle, dap->bdata->interface);
+	libusb_close(dap->bdata->dev_handle);
+	libusb_exit(dap->bdata->usb_ctx);
+	free(dap->bdata);
+	dap->bdata = NULL;
+}
+
+static int cmsis_dap_usb_read(struct cmsis_dap *dap, int timeout_ms)
+{
+	int transferred = 0;
+	int err;
+
+	err = libusb_bulk_transfer(dap->bdata->dev_handle, dap->bdata->ep_in,
+							dap->packet_buffer, dap->packet_size, &transferred, timeout_ms);
+	if (err) {
+		if (err == LIBUSB_ERROR_TIMEOUT) {
+			return ERROR_TIMEOUT_REACHED;
+		} else {
+			LOG_ERROR("error reading data: %s", libusb_strerror(err));
+			return ERROR_FAIL;
+		}
+	}
+
+	memset(&dap->packet_buffer[transferred], 0, dap->packet_size - transferred);
+
+	return transferred;
+}
+
+static int cmsis_dap_usb_write(struct cmsis_dap *dap, int txlen, int timeout_ms)
+{
+	int transferred = 0;
+	int err;
+
+	/* skip the first byte that is only used by the HID backend */
+	err = libusb_bulk_transfer(dap->bdata->dev_handle, dap->bdata->ep_out,
+							dap->packet_buffer + 1, txlen - 1, &transferred, timeout_ms);
+	if (err) {
+		if (err == LIBUSB_ERROR_TIMEOUT) {
+			return ERROR_TIMEOUT_REACHED;
+		} else {
+			LOG_ERROR("error writing data: %s", libusb_strerror(err));
+			return ERROR_FAIL;
+		}
+	}
+
+	return transferred;
+}
+
+COMMAND_HANDLER(cmsis_dap_handle_usb_interface_command)
+{
+	if (CMD_ARGC == 1)
+		cmsis_dap_usb_interface = strtoul(CMD_ARGV[0], NULL, 10);
+	else
+		LOG_ERROR("expected exactly one argument to cmsis_dap_usb_interface <interface_number>");
+
+	return ERROR_OK;
+}
+
+const struct command_registration cmsis_dap_usb_subcommand_handlers[] = {
+	{
+		.name = "interface",
+		.handler = &cmsis_dap_handle_usb_interface_command,
+		.mode = COMMAND_CONFIG,
+		.help = "set the USB interface number to use (for USB bulk backend only)",
+		.usage = "<interface_number>",
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+const struct cmsis_dap_backend cmsis_dap_usb_backend = {
+	.name = "usb_bulk",
+	.open = cmsis_dap_usb_open,
+	.close = cmsis_dap_usb_close,
+	.read = cmsis_dap_usb_read,
+	.write = cmsis_dap_usb_write,
+};
diff --git a/src/jtag/drivers/cmsis_dap_usb_hid.c b/src/jtag/drivers/cmsis_dap_usb_hid.c
new file mode 100644
index 0000000..681aef1
--- /dev/null
+++ b/src/jtag/drivers/cmsis_dap_usb_hid.c
@@ -0,0 +1,208 @@
+/***************************************************************************
+ *   Copyright (C) 2018 by Mickaël Thomas                                  *
+ *   mickael9@gmail.com                                                    *
+ *                                                                         *
+ *   Copyright (C) 2016 by Maksym Hilliaka                                 *
+ *   oter@frozen-team.com                                                  *
+ *                                                                         *
+ *   Copyright (C) 2016 by Phillip Pearson                                 *
+ *   pp@myelin.co.nz                                                       *
+ *                                                                         *
+ *   Copyright (C) 2014 by Paul Fertser                                    *
+ *   fercerpav@gmail.com                                                   *
+ *                                                                         *
+ *   Copyright (C) 2013 by mike brown                                      *
+ *   mike@theshedworks.org.uk                                              *
+ *                                                                         *
+ *   Copyright (C) 2013 by Spencer Oliver                                  *
+ *   spen@spen-soft.co.uk                                                  *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <hidapi.h>
+#include <helper/log.h>
+
+#include "cmsis_dap.h"
+
+#define PACKET_SIZE       (64 + 1)	/* 64 bytes plus report id */
+
+struct cmsis_dap_backend_data {
+	hid_device *dev_handle;
+};
+
+static int cmsis_dap_hid_open(struct cmsis_dap *dap, uint16_t vids[], uint16_t pids[], char *serial)
+{
+	hid_device *dev = NULL;
+	int i;
+	struct hid_device_info *devs, *cur_dev;
+	unsigned short target_vid, target_pid;
+
+	target_vid = 0;
+	target_pid = 0;
+
+	if (hid_init() != 0) {
+		LOG_ERROR("unable to open HIDAPI");
+		return ERROR_FAIL;
+	}
+
+	/*
+	 * The CMSIS-DAP specification stipulates:
+	 * "The Product String must contain "CMSIS-DAP" somewhere in the string. This is used by the
+	 * debuggers to identify a CMSIS-DAP compliant Debug Unit that is connected to a host computer."
+	 */
+	devs = hid_enumerate(0x0, 0x0);
+	cur_dev = devs;
+	while (NULL != cur_dev) {
+		bool found = false;
+
+		if (0 == vids[0]) {
+			if (NULL == cur_dev->product_string) {
+				LOG_DEBUG("Cannot read product string of device 0x%x:0x%x",
+					  cur_dev->vendor_id, cur_dev->product_id);
+			} else if (wcsstr(cur_dev->product_string, L"CMSIS-DAP")) {
+				/* if the user hasn't specified VID:PID *and*
+				 * product string contains "CMSIS-DAP", pick it
+				 */
+				found = true;
+			}
+		} else {
+			/* otherwise, exhaustively compare against all VID:PID in list */
+			for (i = 0; vids[i] || pids[i]; i++) {
+				if ((vids[i] == cur_dev->vendor_id) && (pids[i] == cur_dev->product_id))
+					found = true;
+			}
+		}
+
+		/* LPC-LINK2 has cmsis-dap on interface 0 and other HID functions on other interfaces */
+		if (cur_dev->vendor_id == 0x1fc9 && cur_dev->product_id == 0x0090 && cur_dev->interface_number != 0)
+			found = false;
+
+		if (found) {
+			/* check serial number matches if given */
+			if (serial == NULL)
+				break;
+
+			if (cur_dev->serial_number != NULL) {
+				size_t len = (strlen(serial) + 1) * sizeof(wchar_t);
+				wchar_t *wserial = malloc(len);
+				mbstowcs(wserial, serial, len);
+
+				if (wcscmp(wserial, cur_dev->serial_number) == 0) {
+					free(wserial);
+					break;
+				} else {
+					free(wserial);
+					wserial = NULL;
+				}
+			}
+		}
+
+		cur_dev = cur_dev->next;
+	}
+
+	if (NULL != cur_dev) {
+		target_vid = cur_dev->vendor_id;
+		target_pid = cur_dev->product_id;
+	}
+
+	if (target_vid == 0 && target_pid == 0) {
+		hid_free_enumeration(devs);
+		return ERROR_FAIL;
+	}
+
+	dap->bdata = malloc(sizeof(struct cmsis_dap_backend_data));
+	if (dap->bdata == NULL) {
+		LOG_ERROR("unable to allocate memory");
+		return ERROR_FAIL;
+	}
+
+	dev = hid_open_path(cur_dev->path);
+	hid_free_enumeration(devs);
+
+	if (dev == NULL) {
+		LOG_ERROR("unable to open CMSIS-DAP device 0x%x:0x%x", target_vid, target_pid);
+		return ERROR_FAIL;
+	}
+
+	/* allocate default packet buffer, may be changed later.
+	 * currently with HIDAPI we have no way of getting the output report length
+	 * without this info we cannot communicate with the adapter.
+	 * For the moment we have to hard code the packet size */
+
+	dap->packet_size = PACKET_SIZE;
+
+	/* atmel cmsis-dap uses 512 byte reports */
+	/* except when it doesn't e.g. with mEDBG on SAMD10 Xplained
+	 * board */
+	/* TODO: HID report descriptor should be parsed instead of
+	 * hardcoding a match by VID */
+	if (target_vid == 0x03eb && target_pid != 0x2145 && target_pid != 0x2175)
+		dap->packet_size = 512 + 1;
+
+	dap->bdata->dev_handle = dev;
+
+	return ERROR_OK;
+}
+
+static void cmsis_dap_hid_close(struct cmsis_dap *dap)
+{
+	hid_close(dap->bdata->dev_handle);
+	hid_exit();
+	free(dap->bdata);
+	dap->bdata = NULL;
+}
+
+static int cmsis_dap_hid_read(struct cmsis_dap *dap, int timeout_ms)
+{
+	int retval = hid_read_timeout(dap->bdata->dev_handle, dap->packet_buffer, dap->packet_size, timeout_ms);
+
+	if (retval == 0) {
+		return ERROR_TIMEOUT_REACHED;
+	} else if (retval == -1) {
+		LOG_ERROR("error reading data: %ls", hid_error(dap->bdata->dev_handle));
+		return ERROR_FAIL;
+	}
+
+	return retval;
+}
+
+static int cmsis_dap_hid_write(struct cmsis_dap *dap, int txlen, int timeout_ms)
+{
+	(void) timeout_ms;
+
+	/* Pad the rest of the TX buffer with 0's */
+	memset(dap->packet_buffer + txlen, 0, dap->packet_size - txlen);
+
+	/* write data to device */
+	int retval = hid_write(dap->bdata->dev_handle, dap->packet_buffer, dap->packet_size);
+	if (retval == -1) {
+		LOG_ERROR("error writing data: %ls", hid_error(dap->bdata->dev_handle));
+		return ERROR_FAIL;
+	}
+
+	return retval;
+}
+
+const struct cmsis_dap_backend cmsis_dap_hid_backend = {
+	.name = "hid",
+	.open = cmsis_dap_hid_open,
+	.close = cmsis_dap_hid_close,
+	.read = cmsis_dap_hid_read,
+	.write = cmsis_dap_hid_write,
+};
diff --git a/src/jtag/drivers/ft232r.c b/src/jtag/drivers/ft232r.c
index d97db56..a63480c 100644
--- a/src/jtag/drivers/ft232r.c
+++ b/src/jtag/drivers/ft232r.c
@@ -170,7 +170,7 @@ static int ft232r_send_recv(void)
 	return ERROR_OK;
 }
 
-void ft232r_increase_buf_size(size_t new_buf_size)
+static void ft232r_increase_buf_size(size_t new_buf_size)
 {
 	uint8_t *new_buf_ptr;
 	if (new_buf_size >= ft232r_buf_size) {
diff --git a/src/jtag/drivers/gw16012.c b/src/jtag/drivers/gw16012.c
index ef4b5d9..db0a677 100644
--- a/src/jtag/drivers/gw16012.c
+++ b/src/jtag/drivers/gw16012.c
@@ -63,7 +63,7 @@
 #endif
 
 /* configuration */
-uint16_t gw16012_port;
+static uint16_t gw16012_port;
 
 /* interface variables
  */
diff --git a/src/jtag/drivers/jlink.c b/src/jtag/drivers/jlink.c
index ae8ce49..b915707 100644
--- a/src/jtag/drivers/jlink.c
+++ b/src/jtag/drivers/jlink.c
@@ -1270,7 +1270,7 @@ static bool calculate_swo_prescaler(unsigned int traceclkin_freq,
 		uint32_t trace_freq, uint16_t *prescaler)
 {
 	unsigned int presc = (traceclkin_freq + trace_freq / 2) / trace_freq;
-	if (presc > TPIU_ACPR_MAX_SWOSCALER)
+	if (presc == 0 || presc > TPIU_ACPR_MAX_SWOSCALER + 1)
 		return false;
 
 	/* Probe's UART speed must be within 3% of the TPIU's SWO baud rate. */
@@ -1296,7 +1296,7 @@ static bool detect_swo_freq_and_prescaler(struct jaylink_swo_speed speed,
 		*trace_freq = speed.freq / divider;
 		presc = ((1.0 - SWO_MAX_FREQ_DEV) * traceclkin_freq) / *trace_freq + 1;
 
-		if (presc > TPIU_ACPR_MAX_SWOSCALER)
+		if (presc > TPIU_ACPR_MAX_SWOSCALER + 1)
 			break;
 
 		deviation = fabs(1.0 - ((double)*trace_freq * presc / traceclkin_freq));
diff --git a/src/jtag/drivers/libftdi_helper.h b/src/jtag/drivers/libftdi_helper.h
new file mode 100644
index 0000000..e187b57
--- /dev/null
+++ b/src/jtag/drivers/libftdi_helper.h
@@ -0,0 +1,27 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+
+#ifndef OPENOCD_JTAG_DRIVERS_LIBFTDI_HELPER_H
+#define OPENOCD_JTAG_DRIVERS_LIBFTDI_HELPER_H
+
+#include <ftdi.h>
+
+#ifndef HAVE_LIBFTDI_TCIOFLUSH
+/* Backward compatibility with libftdi pre 1.5 */
+
+static inline int ftdi_tciflush(struct ftdi_context *ftdi)
+{
+	return ftdi_usb_purge_rx_buffer(ftdi);
+}
+
+static inline int ftdi_tcoflush(struct ftdi_context *ftdi)
+{
+	return ftdi_usb_purge_tx_buffer(ftdi);
+}
+
+static inline int ftdi_tcioflush(struct ftdi_context *ftdi)
+{
+	return ftdi_usb_purge_buffers(ftdi);
+}
+#endif
+
+#endif /* OPENOCD_JTAG_DRIVERS_LIBFTDI_HELPER_H */
diff --git a/src/jtag/drivers/nulink_usb.c b/src/jtag/drivers/nulink_usb.c
index 5fdbed3..00738ee 100644
--- a/src/jtag/drivers/nulink_usb.c
+++ b/src/jtag/drivers/nulink_usb.c
@@ -411,7 +411,7 @@ static int nulink_usb_step(void *handle)
 	return res;
 }
 
-static int nulink_usb_read_reg(void *handle, int num, uint32_t *val)
+static int nulink_usb_read_reg(void *handle, unsigned int regsel, uint32_t *val)
 {
 	struct nulink_usb_handle_s *h = handle;
 
@@ -434,7 +434,7 @@ static int nulink_usb_read_reg(void *handle, int num, uint32_t *val)
 	h->cmdbuf[h->cmdidx] = 0;
 	h->cmdidx += 1;
 	/* u32Addr */
-	h_u32_to_le(h->cmdbuf + h->cmdidx, num);
+	h_u32_to_le(h->cmdbuf + h->cmdidx, regsel);
 	h->cmdidx += 4;
 	/* u32Data */
 	h_u32_to_le(h->cmdbuf + h->cmdidx, 0);
@@ -450,7 +450,7 @@ static int nulink_usb_read_reg(void *handle, int num, uint32_t *val)
 	return res;
 }
 
-static int nulink_usb_write_reg(void *handle, int num, uint32_t val)
+static int nulink_usb_write_reg(void *handle, unsigned int regsel, uint32_t val)
 {
 	struct nulink_usb_handle_s *h = handle;
 
@@ -473,7 +473,7 @@ static int nulink_usb_write_reg(void *handle, int num, uint32_t val)
 	h->cmdbuf[h->cmdidx] = 0;
 	h->cmdidx += 1;
 	/* u32Addr */
-	h_u32_to_le(h->cmdbuf + h->cmdidx, num);
+	h_u32_to_le(h->cmdbuf + h->cmdidx, regsel);
 	h->cmdidx += 4;
 	/* u32Data */
 	h_u32_to_le(h->cmdbuf + h->cmdidx, val);
diff --git a/src/jtag/drivers/opendous.c b/src/jtag/drivers/opendous.c
index f0e4f56..82fcbc1 100644
--- a/src/jtag/drivers/opendous.c
+++ b/src/jtag/drivers/opendous.c
@@ -144,7 +144,7 @@ static int opendous_usb_write(struct opendous_jtag *opendous_jtag, int out_lengt
 static int opendous_usb_read(struct opendous_jtag *opendous_jtag);
 
 /* helper functions */
-int opendous_get_version_info(void);
+static int opendous_get_version_info(void);
 
 #ifdef _DEBUG_USB_COMMS_
 static void opendous_debug_buffer(uint8_t *buffer, int length);
@@ -544,7 +544,7 @@ int opendous_get_status(void)
 	return ERROR_OK;
 }
 
-int opendous_get_version_info(void)
+static int opendous_get_version_info(void)
 {
 	return ERROR_OK;
 }
diff --git a/src/jtag/drivers/openjtag.c b/src/jtag/drivers/openjtag.c
index 2cf5751..6940c88 100644
--- a/src/jtag/drivers/openjtag.c
+++ b/src/jtag/drivers/openjtag.c
@@ -82,7 +82,7 @@ typedef enum openjtag_tap_state {
 } openjtag_tap_state_t;
 
 /* OPENJTAG access library includes */
-#include <ftdi.h>
+#include "libftdi_helper.h"
 
 /* OpenJTAG vid/pid */
 static uint16_t openjtag_vid = 0x0403;
@@ -436,8 +436,8 @@ static int openjtag_init_standard(void)
 		return ERROR_JTAG_DEVICE_ERROR;
 	}
 
-	if (ftdi_usb_purge_buffers(&ftdic) < 0) {
-		LOG_ERROR("ftdi_purge_buffers: %s", ftdic.error_str);
+	if (ftdi_tcioflush(&ftdic) < 0) {
+		LOG_ERROR("ftdi flush: %s", ftdic.error_str);
 		return ERROR_JTAG_INIT_FAILED;
 	}
 
diff --git a/src/jtag/drivers/presto.c b/src/jtag/drivers/presto.c
index 6c3a187..43d669e 100644
--- a/src/jtag/drivers/presto.c
+++ b/src/jtag/drivers/presto.c
@@ -34,7 +34,7 @@
 #include "bitq.h"
 
 /* PRESTO access library includes */
-#include <ftdi.h>
+#include "libftdi_helper.h"
 
 /* -------------------------------------------------------------------------- */
 
@@ -160,8 +160,8 @@ static int presto_open_libftdi(char *req_serial)
 		return ERROR_JTAG_DEVICE_ERROR;
 	}
 
-	if (ftdi_usb_purge_buffers(&presto->ftdic) < 0) {
-		LOG_ERROR("unable to purge PRESTO buffers");
+	if (ftdi_tcioflush(&presto->ftdic) < 0) {
+		LOG_ERROR("unable to flush PRESTO buffers");
 		return ERROR_JTAG_DEVICE_ERROR;
 	}
 
@@ -174,7 +174,7 @@ static int presto_open_libftdi(char *req_serial)
 	if (presto_read(&presto_data, 1) != ERROR_OK) {
 		LOG_DEBUG("no response from PRESTO, retrying");
 
-		if (ftdi_usb_purge_buffers(&presto->ftdic) < 0)
+		if (ftdi_tcioflush(&presto->ftdic) < 0)
 			return ERROR_JTAG_DEVICE_ERROR;
 
 		presto_data = 0xD0;
diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c
index ce55b94..4545bcb 100644
--- a/src/jtag/drivers/stlink_usb.c
+++ b/src/jtag/drivers/stlink_usb.c
@@ -299,6 +299,7 @@ struct stlink_usb_handle_s {
 
 #define STLINK_TRACE_SIZE               4096
 #define STLINK_TRACE_MAX_HZ             2000000
+#define STLINK_V3_TRACE_MAX_HZ          24000000
 
 #define STLINK_V3_MAX_FREQ_NB               10
 
@@ -323,6 +324,9 @@ struct stlink_usb_handle_s {
 
 /* aliases */
 #define STLINK_F_HAS_TARGET_VOLT        STLINK_F_HAS_TRACE
+#define STLINK_F_HAS_FPU_REG            STLINK_F_HAS_GETLASTRWSTATUS2
+
+#define STLINK_REGSEL_IS_FPU(x)         ((x) > 0x1F)
 
 struct speed_map {
 	int speed;
@@ -358,7 +362,7 @@ static const struct speed_map stlink_khz_to_speed_map_jtag[] = {
 
 static void stlink_usb_init_buffer(void *handle, uint8_t direction, uint32_t size);
 static int stlink_swim_status(void *handle);
-void stlink_dump_speed_map(const struct speed_map *map, unsigned int map_size);
+static void stlink_dump_speed_map(const struct speed_map *map, unsigned int map_size);
 static int stlink_get_com_freq(void *handle, bool is_jtag, struct speed_map *map);
 static int stlink_speed(void *handle, int khz, bool query);
 static int stlink_usb_open_ap(void *handle, unsigned short apsel);
@@ -2015,13 +2019,22 @@ static int stlink_usb_read_regs(void *handle)
 }
 
 /** */
-static int stlink_usb_read_reg(void *handle, int num, uint32_t *val)
+static int stlink_usb_read_reg(void *handle, unsigned int regsel, uint32_t *val)
 {
 	int res;
 	struct stlink_usb_handle_s *h = handle;
 
 	assert(handle != NULL);
 
+	if (STLINK_REGSEL_IS_FPU(regsel) && !(h->version.flags & STLINK_F_HAS_FPU_REG)) {
+		res = stlink_usb_write_debug_reg(h, DCB_DCRSR, regsel & 0x7f);
+		if (res != ERROR_OK)
+			return res;
+
+		/* FIXME: poll DHCSR.S_REGRDY before read DCRDR */
+		return stlink_usb_v2_read_debug_reg(h, DCB_DCRDR, val);
+	}
+
 	stlink_usb_init_buffer(handle, h->rx_ep, h->version.jtag_api == STLINK_JTAG_API_V1 ? 4 : 8);
 
 	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
@@ -2029,7 +2042,7 @@ static int stlink_usb_read_reg(void *handle, int num, uint32_t *val)
 		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_READREG;
 	else
 		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READREG;
-	h->cmdbuf[h->cmdidx++] = num;
+	h->cmdbuf[h->cmdidx++] = regsel;
 
 	if (h->version.jtag_api == STLINK_JTAG_API_V1) {
 		res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 4);
@@ -2047,12 +2060,21 @@ static int stlink_usb_read_reg(void *handle, int num, uint32_t *val)
 }
 
 /** */
-static int stlink_usb_write_reg(void *handle, int num, uint32_t val)
+static int stlink_usb_write_reg(void *handle, unsigned int regsel, uint32_t val)
 {
 	struct stlink_usb_handle_s *h = handle;
 
 	assert(handle != NULL);
 
+	if (STLINK_REGSEL_IS_FPU(regsel) && !(h->version.flags & STLINK_F_HAS_FPU_REG)) {
+		int res = stlink_usb_write_debug_reg(h, DCB_DCRDR, val);
+		if (res != ERROR_OK)
+			return res;
+
+		return stlink_usb_write_debug_reg(h, DCB_DCRSR, DCRSR_WnR | (regsel & 0x7f));
+		/* FIXME: poll DHCSR.S_REGRDY after write DCRSR */
+	}
+
 	stlink_usb_init_buffer(handle, h->rx_ep, 2);
 
 	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
@@ -2060,7 +2082,7 @@ static int stlink_usb_write_reg(void *handle, int num, uint32_t val)
 		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_WRITEREG;
 	else
 		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEREG;
-	h->cmdbuf[h->cmdidx++] = num;
+	h->cmdbuf[h->cmdidx++] = regsel;
 	h_u32_to_le(h->cmdbuf+h->cmdidx, val);
 	h->cmdidx += 4;
 
@@ -2577,7 +2599,7 @@ static int stlink_speed_jtag(void *handle, int khz, bool query)
 	return stlink_khz_to_speed_map_jtag[speed_index].speed;
 }
 
-void stlink_dump_speed_map(const struct speed_map *map, unsigned int map_size)
+static void stlink_dump_speed_map(const struct speed_map *map, unsigned int map_size)
 {
 	unsigned int i;
 
@@ -2725,7 +2747,7 @@ static int stlink_usb_close(void *handle)
  *    based on the length (0x1a = 26) we could easily decide if we have to fixup the serial
  *    and then we have just to convert the raw data into printable characters using sprintf
  */
-char *stlink_usb_get_alternate_serial(libusb_device_handle *device,
+static char *stlink_usb_get_alternate_serial(libusb_device_handle *device,
 		struct libusb_device_descriptor *dev_desc)
 {
 	int usb_retval;
@@ -2982,13 +3004,12 @@ static int stlink_usb_hl_open(struct hl_interface_param_s *param, void **fd)
 	return stlink_usb_open(param, stlink_get_mode(param->transport), fd);
 }
 
-int stlink_config_trace(void *handle, bool enabled,
+static int stlink_config_trace(void *handle, bool enabled,
 		enum tpiu_pin_protocol pin_protocol, uint32_t port_size,
 		unsigned int *trace_freq, unsigned int traceclkin_freq,
 		uint16_t *prescaler)
 {
 	struct stlink_usb_handle_s *h = handle;
-	uint16_t presc;
 
 	if (enabled && (!(h->version.flags & STLINK_F_HAS_TRACE) ||
 			pin_protocol != TPIU_PIN_PROTOCOL_ASYNC_UART)) {
@@ -2996,34 +3017,42 @@ int stlink_config_trace(void *handle, bool enabled,
 		return ERROR_FAIL;
 	}
 
-	if (!enabled) {
-		stlink_usb_trace_disable(h);
-		return ERROR_OK;
-	}
+	unsigned int max_trace_freq = (h->version.stlink == 3) ?
+			STLINK_V3_TRACE_MAX_HZ : STLINK_TRACE_MAX_HZ;
 
-	if (*trace_freq > STLINK_TRACE_MAX_HZ) {
+	/* Only concern ourselves with the frequency if the STlink is processing it. */
+	if (enabled && *trace_freq > max_trace_freq) {
 		LOG_ERROR("ST-LINK doesn't support SWO frequency higher than %u",
-			  STLINK_TRACE_MAX_HZ);
+			  max_trace_freq);
 		return ERROR_FAIL;
 	}
 
 	stlink_usb_trace_disable(h);
 
 	if (!*trace_freq)
-		*trace_freq = STLINK_TRACE_MAX_HZ;
+		*trace_freq = max_trace_freq;
 
-	presc = traceclkin_freq / *trace_freq;
-
-	if (traceclkin_freq % *trace_freq > 0)
-		presc++;
+	unsigned int presc = (traceclkin_freq + *trace_freq / 2) / *trace_freq;
+	if (presc == 0 || presc > TPIU_ACPR_MAX_SWOSCALER + 1) {
+		LOG_ERROR("SWO frequency is not suitable. Please choose a different "
+			"frequency.");
+		return ERROR_FAIL;
+	}
 
-	if (presc > TPIU_ACPR_MAX_SWOSCALER) {
+	/* Probe's UART speed must be within 3% of the TPIU's SWO baud rate. */
+	unsigned int max_deviation = (traceclkin_freq * 3) / 100;
+	if (presc * *trace_freq < traceclkin_freq - max_deviation ||
+			presc * *trace_freq > traceclkin_freq + max_deviation) {
 		LOG_ERROR("SWO frequency is not suitable. Please choose a different "
 			"frequency.");
 		return ERROR_FAIL;
 	}
 
 	*prescaler = presc;
+
+	if (!enabled)
+		return ERROR_OK;
+
 	h->trace.source_hz = *trace_freq;
 
 	return stlink_usb_trace_enable(h);
diff --git a/src/jtag/drivers/ti_icdi_usb.c b/src/jtag/drivers/ti_icdi_usb.c
index d276e58..00b2f96 100644
--- a/src/jtag/drivers/ti_icdi_usb.c
+++ b/src/jtag/drivers/ti_icdi_usb.c
@@ -31,7 +31,7 @@
 
 #include <target/cortex_m.h>
 
-#include <libusb.h>
+#include "libusb_helper.h"
 
 #define ICDI_WRITE_ENDPOINT 0x02
 #define ICDI_READ_ENDPOINT 0x83
@@ -44,8 +44,7 @@
 #define PACKET_END "#"
 
 struct icdi_usb_handle_s {
-	libusb_context *usb_ctx;
-	libusb_device_handle *usb_dev;
+	struct libusb_device_handle *usb_dev;
 
 	char *read_buffer;
 	char *write_buffer;
@@ -476,13 +475,13 @@ static int icdi_usb_read_regs(void *handle)
 	return ERROR_OK;
 }
 
-static int icdi_usb_read_reg(void *handle, int num, uint32_t *val)
+static int icdi_usb_read_reg(void *handle, unsigned int regsel, uint32_t *val)
 {
 	int result;
 	struct icdi_usb_handle_s *h = handle;
 	char cmd[10];
 
-	snprintf(cmd, sizeof(cmd), "p%x", num);
+	snprintf(cmd, sizeof(cmd), "p%x", regsel);
 	result = icdi_send_cmd(handle, cmd);
 	if (result != ERROR_OK)
 		return result;
@@ -505,14 +504,14 @@ static int icdi_usb_read_reg(void *handle, int num, uint32_t *val)
 	return result;
 }
 
-static int icdi_usb_write_reg(void *handle, int num, uint32_t val)
+static int icdi_usb_write_reg(void *handle, unsigned int regsel, uint32_t val)
 {
 	int result;
 	char cmd[20];
 	uint8_t buf[4];
 	h_u32_to_le(buf, val);
 
-	int cmd_len = snprintf(cmd, sizeof(cmd), "P%x=", num);
+	int cmd_len = snprintf(cmd, sizeof(cmd), "P%x=", regsel);
 	hexify(cmd + cmd_len, buf, 4, sizeof(cmd));
 
 	result = icdi_send_cmd(handle, cmd);
@@ -657,10 +656,7 @@ static int icdi_usb_close(void *handle)
 		return ERROR_OK;
 
 	if (h->usb_dev)
-		libusb_close(h->usb_dev);
-
-	if (h->usb_ctx)
-		libusb_exit(h->usb_ctx);
+		jtag_libusb_close(h->usb_dev);
 
 	free(h->read_buffer);
 	free(h->write_buffer);
@@ -670,6 +666,7 @@ static int icdi_usb_close(void *handle)
 
 static int icdi_usb_open(struct hl_interface_param_s *param, void **fd)
 {
+	/* TODO: Convert remaining libusb_ calls to jtag_libusb_ */
 	int retval;
 	struct icdi_usb_handle_s *h;
 
@@ -682,19 +679,14 @@ static int icdi_usb_open(struct hl_interface_param_s *param, void **fd)
 		return ERROR_FAIL;
 	}
 
-	LOG_DEBUG("transport: %d vid: 0x%04x pid: 0x%04x", param->transport,
-		  param->vid[0], param->pid[0]);
-
-	/* TODO: convert libusb_ calls to jtag_libusb_ */
-	if (param->vid[1])
-		LOG_WARNING("Bad configuration: 'hla_vid_pid' command does not accept more than one VID PID pair on ti-icdi!");
+	for (uint8_t i = 0; param->vid[i] && param->pid[i]; ++i)
+		LOG_DEBUG("transport: %d vid: 0x%04x pid: 0x%04x serial: %s", param->transport,
+			param->vid[i], param->pid[i], param->serial ? param->serial : "");
 
-	if (libusb_init(&h->usb_ctx) != 0) {
-		LOG_ERROR("libusb init failed");
-		goto error_open;
-	}
+	/* TI (Stellaris) ICDI provides its serial number in the USB descriptor;
+	   no need to provide a callback here. */
+	jtag_libusb_open(param->vid, param->pid, param->serial, &h->usb_dev, NULL);
 
-	h->usb_dev = libusb_open_device_with_vid_pid(h->usb_ctx, param->vid[0], param->pid[0]);
 	if (!h->usb_dev) {
 		LOG_ERROR("open failed");
 		goto error_open;
diff --git a/src/jtag/drivers/ulink.c b/src/jtag/drivers/ulink.c
index 68249dc..f473ce3 100644
--- a/src/jtag/drivers/ulink.c
+++ b/src/jtag/drivers/ulink.c
@@ -25,6 +25,7 @@
 #include <jtag/commands.h>
 #include <target/image.h>
 #include <libusb.h>
+#include "libusb_helper.h"
 #include "OpenULINK/include/msgtypes.h"
 
 /** USB Vendor ID of ULINK device in unconfigured state (no firmware loaded
@@ -148,6 +149,9 @@ struct ulink {
 	struct libusb_device_handle *usb_device_handle;
 	enum ulink_type type;
 
+	unsigned int ep_in;		/**< IN endpoint number */
+	unsigned int ep_out;		/**< OUT endpoint number */
+
 	int delay_scan_in;	/**< Delay value for SCAN_IN commands */
 	int delay_scan_out;	/**< Delay value for SCAN_OUT commands */
 	int delay_scan_io;	/**< Delay value for SCAN_IO commands */
@@ -162,34 +166,34 @@ struct ulink {
 /**************************** Function Prototypes *****************************/
 
 /* USB helper functions */
-int ulink_usb_open(struct ulink **device);
-int ulink_usb_close(struct ulink **device);
+static int ulink_usb_open(struct ulink **device);
+static int ulink_usb_close(struct ulink **device);
 
 /* ULINK MCU (Cypress EZ-USB) specific functions */
-int ulink_cpu_reset(struct ulink *device, unsigned char reset_bit);
-int ulink_load_firmware_and_renumerate(struct ulink **device, const char *filename,
+static int ulink_cpu_reset(struct ulink *device, unsigned char reset_bit);
+static int ulink_load_firmware_and_renumerate(struct ulink **device, const char *filename,
 		uint32_t delay);
-int ulink_load_firmware(struct ulink *device, const char *filename);
-int ulink_write_firmware_section(struct ulink *device,
+static int ulink_load_firmware(struct ulink *device, const char *filename);
+static int ulink_write_firmware_section(struct ulink *device,
 		struct image *firmware_image, int section_index);
 
 /* Generic helper functions */
-void ulink_print_signal_states(uint8_t input_signals, uint8_t output_signals);
+static void ulink_print_signal_states(uint8_t input_signals, uint8_t output_signals);
 
 /* OpenULINK command generation helper functions */
-int ulink_allocate_payload(struct ulink_cmd *ulink_cmd, int size,
+static int ulink_allocate_payload(struct ulink_cmd *ulink_cmd, int size,
 		enum ulink_payload_direction direction);
 
 /* OpenULINK command queue helper functions */
-int ulink_get_queue_size(struct ulink *device,
+static int ulink_get_queue_size(struct ulink *device,
 		enum ulink_payload_direction direction);
-void ulink_clear_queue(struct ulink *device);
-int ulink_append_queue(struct ulink *device, struct ulink_cmd *ulink_cmd);
-int ulink_execute_queued_commands(struct ulink *device, int timeout);
+static void ulink_clear_queue(struct ulink *device);
+static int ulink_append_queue(struct ulink *device, struct ulink_cmd *ulink_cmd);
+static int ulink_execute_queued_commands(struct ulink *device, int timeout);
 
 static void ulink_print_queue(struct ulink *device);
 
-int ulink_append_scan_cmd(struct ulink *device,
+static int ulink_append_scan_cmd(struct ulink *device,
 		enum scan_type scan_type,
 		int scan_size_bits,
 		uint8_t *tdi,
@@ -201,39 +205,39 @@ int ulink_append_scan_cmd(struct ulink *device,
 		uint8_t tms_sequence_end,
 		struct jtag_command *origin,
 		bool postprocess);
-int ulink_append_clock_tms_cmd(struct ulink *device, uint8_t count,
+static int ulink_append_clock_tms_cmd(struct ulink *device, uint8_t count,
 		uint8_t sequence);
-int ulink_append_clock_tck_cmd(struct ulink *device, uint16_t count);
-int ulink_append_get_signals_cmd(struct ulink *device);
-int ulink_append_set_signals_cmd(struct ulink *device, uint8_t low,
+static int ulink_append_clock_tck_cmd(struct ulink *device, uint16_t count);
+static int ulink_append_get_signals_cmd(struct ulink *device);
+static int ulink_append_set_signals_cmd(struct ulink *device, uint8_t low,
 		uint8_t high);
-int ulink_append_sleep_cmd(struct ulink *device, uint32_t us);
-int ulink_append_configure_tck_cmd(struct ulink *device,
+static int ulink_append_sleep_cmd(struct ulink *device, uint32_t us);
+static int ulink_append_configure_tck_cmd(struct ulink *device,
 		int delay_scan_in,
 		int delay_scan_out,
 		int delay_scan_io,
 		int delay_tck,
 		int delay_tms);
-int ulink_append_led_cmd(struct ulink *device, uint8_t led_state);
-int ulink_append_test_cmd(struct ulink *device);
+static int __attribute__((unused)) ulink_append_led_cmd(struct ulink *device, uint8_t led_state);
+static int ulink_append_test_cmd(struct ulink *device);
 
 /* OpenULINK TCK frequency helper functions */
-int ulink_calculate_delay(enum ulink_delay_type type, long f, int *delay);
+static int ulink_calculate_delay(enum ulink_delay_type type, long f, int *delay);
 
 /* Interface between OpenULINK and OpenOCD */
 static void ulink_set_end_state(tap_state_t endstate);
-int ulink_queue_statemove(struct ulink *device);
+static int ulink_queue_statemove(struct ulink *device);
 
-int ulink_queue_scan(struct ulink *device, struct jtag_command *cmd);
-int ulink_queue_tlr_reset(struct ulink *device, struct jtag_command *cmd);
-int ulink_queue_runtest(struct ulink *device, struct jtag_command *cmd);
-int ulink_queue_reset(struct ulink *device, struct jtag_command *cmd);
-int ulink_queue_pathmove(struct ulink *device, struct jtag_command *cmd);
-int ulink_queue_sleep(struct ulink *device, struct jtag_command *cmd);
-int ulink_queue_stableclocks(struct ulink *device, struct jtag_command *cmd);
+static int ulink_queue_scan(struct ulink *device, struct jtag_command *cmd);
+static int ulink_queue_tlr_reset(struct ulink *device, struct jtag_command *cmd);
+static int ulink_queue_runtest(struct ulink *device, struct jtag_command *cmd);
+static int ulink_queue_reset(struct ulink *device, struct jtag_command *cmd);
+static int ulink_queue_pathmove(struct ulink *device, struct jtag_command *cmd);
+static int ulink_queue_sleep(struct ulink *device, struct jtag_command *cmd);
+static int ulink_queue_stableclocks(struct ulink *device, struct jtag_command *cmd);
 
-int ulink_post_process_scan(struct ulink_cmd *ulink_cmd);
-int ulink_post_process_queue(struct ulink *device);
+static int ulink_post_process_scan(struct ulink_cmd *ulink_cmd);
+static int ulink_post_process_queue(struct ulink *device);
 
 /* adapter driver functions */
 static int ulink_execute_queue(void);
@@ -245,12 +249,12 @@ static int ulink_quit(void);
 
 /****************************** Global Variables ******************************/
 
-struct ulink *ulink_handle;
+static struct ulink *ulink_handle;
 
 /**************************** USB helper functions ****************************/
 
 /**
- * Opens the ULINK device and claims its USB interface.
+ * Opens the ULINK device
  *
  * Currently, only the original ULINK is supported
  *
@@ -258,7 +262,7 @@ struct ulink *ulink_handle;
  * @return on success: ERROR_OK
  * @return on failure: ERROR_FAIL
  */
-int ulink_usb_open(struct ulink **device)
+static int ulink_usb_open(struct ulink **device)
 {
 	ssize_t num_devices, i;
 	bool found;
@@ -288,9 +292,6 @@ int ulink_usb_open(struct ulink **device)
 		return ERROR_FAIL;
 	libusb_free_device_list(usb_devices, 1);
 
-	if (libusb_claim_interface(usb_device_handle, 0) != 0)
-		return ERROR_FAIL;
-
 	(*device)->usb_device_handle = usb_device_handle;
 	(*device)->type = ULINK_1;
 
@@ -304,7 +305,7 @@ int ulink_usb_open(struct ulink **device)
  * @return on success: ERROR_OK
  * @return on failure: ERROR_FAIL
  */
-int ulink_usb_close(struct ulink **device)
+static int ulink_usb_close(struct ulink **device)
 {
 	if (libusb_release_interface((*device)->usb_device_handle, 0) != 0)
 		return ERROR_FAIL;
@@ -327,7 +328,7 @@ int ulink_usb_close(struct ulink **device)
  * @return on success: ERROR_OK
  * @return on failure: ERROR_FAIL
  */
-int ulink_cpu_reset(struct ulink *device, unsigned char reset_bit)
+static int ulink_cpu_reset(struct ulink *device, unsigned char reset_bit)
 {
 	int ret;
 
@@ -354,7 +355,7 @@ int ulink_cpu_reset(struct ulink *device, unsigned char reset_bit)
  * @return on success: ERROR_OK
  * @return on failure: ERROR_FAIL
  */
-int ulink_load_firmware_and_renumerate(struct ulink **device,
+static int ulink_load_firmware_and_renumerate(struct ulink **device,
 	const char *filename, uint32_t delay)
 {
 	int ret;
@@ -390,10 +391,10 @@ int ulink_load_firmware_and_renumerate(struct ulink **device,
  * @return on success: ERROR_OK
  * @return on failure: ERROR_FAIL
  */
-int ulink_load_firmware(struct ulink *device, const char *filename)
+static int ulink_load_firmware(struct ulink *device, const char *filename)
 {
 	struct image ulink_firmware_image;
-	int ret, i;
+	int ret;
 
 	ret = ulink_cpu_reset(device, CPU_RESET);
 	if (ret != ERROR_OK) {
@@ -402,7 +403,7 @@ int ulink_load_firmware(struct ulink *device, const char *filename)
 	}
 
 	ulink_firmware_image.base_address = 0;
-	ulink_firmware_image.base_address_set = 0;
+	ulink_firmware_image.base_address_set = false;
 
 	ret = image_open(&ulink_firmware_image, filename, "ihex");
 	if (ret != ERROR_OK) {
@@ -411,7 +412,7 @@ int ulink_load_firmware(struct ulink *device, const char *filename)
 	}
 
 	/* Download all sections in the image to ULINK */
-	for (i = 0; i < ulink_firmware_image.num_sections; i++) {
+	for (unsigned int i = 0; i < ulink_firmware_image.num_sections; i++) {
 		ret = ulink_write_firmware_section(device, &ulink_firmware_image, i);
 		if (ret != ERROR_OK)
 			return ret;
@@ -439,7 +440,7 @@ int ulink_load_firmware(struct ulink *device, const char *filename)
  * @return on success: ERROR_OK
  * @return on failure: ERROR_FAIL
  */
-int ulink_write_firmware_section(struct ulink *device,
+static int ulink_write_firmware_section(struct ulink *device,
 	struct image *firmware_image, int section_index)
 {
 	uint16_t addr, size, bytes_remaining, chunk_size;
@@ -499,7 +500,7 @@ int ulink_write_firmware_section(struct ulink *device,
  * @param input_signals input signal states as returned by CMD_GET_SIGNALS
  * @param output_signals output signal states as returned by CMD_GET_SIGNALS
  */
-void ulink_print_signal_states(uint8_t input_signals, uint8_t output_signals)
+static void ulink_print_signal_states(uint8_t input_signals, uint8_t output_signals)
 {
 	LOG_INFO("ULINK signal states: TDI: %i, TDO: %i, TMS: %i, TCK: %i, TRST: %i,"
 		" SRST: %i",
@@ -522,7 +523,7 @@ void ulink_print_signal_states(uint8_t input_signals, uint8_t output_signals)
  * @return on success: ERROR_OK
  * @return on failure: ERROR_FAIL
  */
-int ulink_allocate_payload(struct ulink_cmd *ulink_cmd, int size,
+static int ulink_allocate_payload(struct ulink_cmd *ulink_cmd, int size,
 	enum ulink_payload_direction direction)
 {
 	uint8_t *payload;
@@ -576,7 +577,7 @@ int ulink_allocate_payload(struct ulink_cmd *ulink_cmd, int size,
  * @return the number of bytes currently stored in the queue for the specified
  *  direction.
  */
-int ulink_get_queue_size(struct ulink *device,
+static int ulink_get_queue_size(struct ulink *device,
 	enum ulink_payload_direction direction)
 {
 	struct ulink_cmd *current = device->queue_start;
@@ -605,7 +606,7 @@ int ulink_get_queue_size(struct ulink *device,
  * @return on success: ERROR_OK
  * @return on failure: ERROR_FAIL
  */
-void ulink_clear_queue(struct ulink *device)
+static void ulink_clear_queue(struct ulink *device)
 {
 	struct ulink_cmd *current = device->queue_start;
 	struct ulink_cmd *next = NULL;
@@ -647,7 +648,7 @@ void ulink_clear_queue(struct ulink *device)
  * @return on success: ERROR_OK
  * @return on failure: ERROR_FAIL
  */
-int ulink_append_queue(struct ulink *device, struct ulink_cmd *ulink_cmd)
+static int ulink_append_queue(struct ulink *device, struct ulink_cmd *ulink_cmd)
 {
 	int newsize_out, newsize_in;
 	int ret = ERROR_OK;
@@ -698,7 +699,7 @@ int ulink_append_queue(struct ulink *device, struct ulink_cmd *ulink_cmd)
  * @return on success: ERROR_OK
  * @return on failure: ERROR_FAIL
  */
-int ulink_execute_queued_commands(struct ulink *device, int timeout)
+static int ulink_execute_queued_commands(struct ulink *device, int timeout)
 {
 	struct ulink_cmd *current;
 	int ret, i, index_out, index_in, count_out, count_in, transferred;
@@ -725,7 +726,7 @@ int ulink_execute_queued_commands(struct ulink *device, int timeout)
 	}
 
 	/* Send packet to ULINK */
-	ret = libusb_bulk_transfer(device->usb_device_handle, (2 | LIBUSB_ENDPOINT_OUT),
+	ret = libusb_bulk_transfer(device->usb_device_handle, device->ep_out,
 			(unsigned char *)buffer, count_out, &transferred, timeout);
 	if (ret != 0)
 		return ERROR_FAIL;
@@ -734,7 +735,7 @@ int ulink_execute_queued_commands(struct ulink *device, int timeout)
 
 	/* Wait for response if commands contain IN payload data */
 	if (count_in > 0) {
-		ret = libusb_bulk_transfer(device->usb_device_handle, (2 | LIBUSB_ENDPOINT_IN),
+		ret = libusb_bulk_transfer(device->usb_device_handle, device->ep_in,
 				(unsigned char *)buffer, 64, &transferred, timeout);
 		if (ret != 0)
 			return ERROR_FAIL;
@@ -865,7 +866,7 @@ static void ulink_print_queue(struct ulink *device)
  * @return on success: ERROR_OK
  * @return on failure: ERROR_FAIL
  */
-int ulink_append_scan_cmd(struct ulink *device, enum scan_type scan_type,
+static int ulink_append_scan_cmd(struct ulink *device, enum scan_type scan_type,
 	int scan_size_bits, uint8_t *tdi, uint8_t *tdo_start, uint8_t *tdo,
 	uint8_t tms_count_start, uint8_t tms_sequence_start, uint8_t tms_count_end,
 	uint8_t tms_sequence_end, struct jtag_command *origin, bool postprocess)
@@ -966,7 +967,7 @@ int ulink_append_scan_cmd(struct ulink *device, enum scan_type scan_type,
  * @return on success: ERROR_OK
  * @return on failure: ERROR_FAIL
  */
-int ulink_append_clock_tms_cmd(struct ulink *device, uint8_t count,
+static int ulink_append_clock_tms_cmd(struct ulink *device, uint8_t count,
 	uint8_t sequence)
 {
 	struct ulink_cmd *cmd = calloc(1, sizeof(struct ulink_cmd));
@@ -1003,7 +1004,7 @@ int ulink_append_clock_tms_cmd(struct ulink *device, uint8_t count,
  * @return on success: ERROR_OK
  * @return on failure: ERROR_FAIL
  */
-int ulink_append_clock_tck_cmd(struct ulink *device, uint16_t count)
+static int ulink_append_clock_tck_cmd(struct ulink *device, uint16_t count)
 {
 	struct ulink_cmd *cmd = calloc(1, sizeof(struct ulink_cmd));
 	int ret;
@@ -1036,7 +1037,7 @@ int ulink_append_clock_tck_cmd(struct ulink *device, uint16_t count)
  * @return on success: ERROR_OK
  * @return on failure: ERROR_FAIL
  */
-int ulink_append_get_signals_cmd(struct ulink *device)
+static int ulink_append_get_signals_cmd(struct ulink *device)
 {
 	struct ulink_cmd *cmd = calloc(1, sizeof(struct ulink_cmd));
 	int ret;
@@ -1075,7 +1076,7 @@ int ulink_append_get_signals_cmd(struct ulink *device)
  * @return on success: ERROR_OK
  * @return on failure: ERROR_FAIL
  */
-int ulink_append_set_signals_cmd(struct ulink *device, uint8_t low,
+static int ulink_append_set_signals_cmd(struct ulink *device, uint8_t low,
 	uint8_t high)
 {
 	struct ulink_cmd *cmd = calloc(1, sizeof(struct ulink_cmd));
@@ -1108,7 +1109,7 @@ int ulink_append_set_signals_cmd(struct ulink *device, uint8_t low,
  * @return on success: ERROR_OK
  * @return on failure: ERROR_FAIL
  */
-int ulink_append_sleep_cmd(struct ulink *device, uint32_t us)
+static int ulink_append_sleep_cmd(struct ulink *device, uint32_t us)
 {
 	struct ulink_cmd *cmd = calloc(1, sizeof(struct ulink_cmd));
 	int ret;
@@ -1144,7 +1145,7 @@ int ulink_append_sleep_cmd(struct ulink *device, uint32_t us)
  * @return on success: ERROR_OK
  * @return on failure: ERROR_FAIL
  */
-int ulink_append_configure_tck_cmd(struct ulink *device, int delay_scan_in,
+static int ulink_append_configure_tck_cmd(struct ulink *device, int delay_scan_in,
 	int delay_scan_out, int delay_scan_io, int delay_tck, int delay_tms)
 {
 	struct ulink_cmd *cmd = calloc(1, sizeof(struct ulink_cmd));
@@ -1206,7 +1207,7 @@ int ulink_append_configure_tck_cmd(struct ulink *device, int delay_scan_in,
  * @return on success: ERROR_OK
  * @return on failure: ERROR_FAIL
  */
-int ulink_append_led_cmd(struct ulink *device, uint8_t led_state)
+static int ulink_append_led_cmd(struct ulink *device, uint8_t led_state)
 {
 	struct ulink_cmd *cmd = calloc(1, sizeof(struct ulink_cmd));
 	int ret;
@@ -1236,7 +1237,7 @@ int ulink_append_led_cmd(struct ulink *device, uint8_t led_state)
  * @return on success: ERROR_OK
  * @return on failure: ERROR_FAIL
  */
-int ulink_append_test_cmd(struct ulink *device)
+static int ulink_append_test_cmd(struct ulink *device)
 {
 	struct ulink_cmd *cmd = calloc(1, sizeof(struct ulink_cmd));
 	int ret;
@@ -1292,7 +1293,7 @@ int ulink_append_test_cmd(struct ulink *device)
  * @return on success: ERROR_OK
  * @return on failure: ERROR_FAIL
  */
-int ulink_calculate_delay(enum ulink_delay_type type, long f, int *delay)
+static int ulink_calculate_delay(enum ulink_delay_type type, long f, int *delay)
 {
 	float t, x, x_ceil;
 
@@ -1423,7 +1424,7 @@ static void ulink_set_end_state(tap_state_t endstate)
  * @return on success: ERROR_OK
  * @return on failure: ERROR_FAIL
  */
-int ulink_queue_statemove(struct ulink *device)
+static int ulink_queue_statemove(struct ulink *device)
 {
 	uint8_t tms_sequence, tms_count;
 	int ret;
@@ -1452,7 +1453,7 @@ int ulink_queue_statemove(struct ulink *device)
  * @return on success: ERROR_OK
  * @return on failure: ERROR_FAIL
  */
-int ulink_queue_scan(struct ulink *device, struct jtag_command *cmd)
+static int ulink_queue_scan(struct ulink *device, struct jtag_command *cmd)
 {
 	uint32_t scan_size_bits, scan_size_bytes, bits_last_scan;
 	uint32_t scans_max_payload, bytecount;
@@ -1631,7 +1632,7 @@ int ulink_queue_scan(struct ulink *device, struct jtag_command *cmd)
  * @return on success: ERROR_OK
  * @return on failure: ERROR_FAIL
  */
-int ulink_queue_tlr_reset(struct ulink *device, struct jtag_command *cmd)
+static int ulink_queue_tlr_reset(struct ulink *device, struct jtag_command *cmd)
 {
 	int ret;
 
@@ -1654,7 +1655,7 @@ int ulink_queue_tlr_reset(struct ulink *device, struct jtag_command *cmd)
  * @return on success: ERROR_OK
  * @return on failure: ERROR_FAIL
  */
-int ulink_queue_runtest(struct ulink *device, struct jtag_command *cmd)
+static int ulink_queue_runtest(struct ulink *device, struct jtag_command *cmd)
 {
 	int ret;
 
@@ -1685,7 +1686,7 @@ int ulink_queue_runtest(struct ulink *device, struct jtag_command *cmd)
  * @return on success: ERROR_OK
  * @return on failure: ERROR_FAIL
  */
-int ulink_queue_reset(struct ulink *device, struct jtag_command *cmd)
+static int ulink_queue_reset(struct ulink *device, struct jtag_command *cmd)
 {
 	uint8_t low = 0, high = 0;
 
@@ -1711,7 +1712,7 @@ int ulink_queue_reset(struct ulink *device, struct jtag_command *cmd)
  * @return on success: ERROR_OK
  * @return on failure: ERROR_FAIL
  */
-int ulink_queue_pathmove(struct ulink *device, struct jtag_command *cmd)
+static int ulink_queue_pathmove(struct ulink *device, struct jtag_command *cmd)
 {
 	int ret, i, num_states, batch_size, state_count;
 	tap_state_t *path;
@@ -1770,7 +1771,7 @@ int ulink_queue_pathmove(struct ulink *device, struct jtag_command *cmd)
  * @return on success: ERROR_OK
  * @return on failure: ERROR_FAIL
  */
-int ulink_queue_sleep(struct ulink *device, struct jtag_command *cmd)
+static int ulink_queue_sleep(struct ulink *device, struct jtag_command *cmd)
 {
 	/* IMPORTANT! Due to the time offset in command execution introduced by
 	 * command queueing, this needs to be implemented in the ULINK device */
@@ -1783,7 +1784,7 @@ int ulink_queue_sleep(struct ulink *device, struct jtag_command *cmd)
  * @param device pointer to struct ulink identifying ULINK driver instance.
  * @param cmd pointer to the command that shall be executed.
  */
-int ulink_queue_stableclocks(struct ulink *device, struct jtag_command *cmd)
+static int ulink_queue_stableclocks(struct ulink *device, struct jtag_command *cmd)
 {
 	int ret;
 	unsigned num_cycles;
@@ -1828,7 +1829,7 @@ int ulink_queue_stableclocks(struct ulink *device, struct jtag_command *cmd)
  * @return on success: ERROR_OK
  * @return on failure: ERROR_FAIL
  */
-int ulink_post_process_scan(struct ulink_cmd *ulink_cmd)
+static int ulink_post_process_scan(struct ulink_cmd *ulink_cmd)
 {
 	struct jtag_command *cmd = ulink_cmd->cmd_origin;
 	int ret;
@@ -1859,7 +1860,7 @@ int ulink_post_process_scan(struct ulink_cmd *ulink_cmd)
  * @return on success: ERROR_OK
  * @return on failure: ERROR_FAIL
  */
-int ulink_post_process_queue(struct ulink *device)
+static int ulink_post_process_queue(struct ulink *device)
 {
 	struct ulink_cmd *current;
 	struct jtag_command *openocd_cmd;
@@ -2156,6 +2157,12 @@ static int ulink_init(void)
 	} else
 		LOG_INFO("ULINK device is already running OpenULINK firmware");
 
+	/* Get OpenULINK USB IN/OUT endpoints and claim the interface */
+	ret = jtag_libusb_choose_interface(ulink_handle->usb_device_handle,
+		&ulink_handle->ep_in, &ulink_handle->ep_out, -1, -1, -1, -1);
+	if (ret != ERROR_OK)
+		return ret;
+
 	/* Initialize OpenULINK command queue */
 	ulink_clear_queue(ulink_handle);
 
@@ -2171,7 +2178,7 @@ static int ulink_init(void)
 		 * shut down by the user via Ctrl-C. Try to retrieve this Bulk IN packet. */
 		dummy = calloc(64, sizeof(uint8_t));
 
-		ret = libusb_bulk_transfer(ulink_handle->usb_device_handle, (2 | LIBUSB_ENDPOINT_IN),
+		ret = libusb_bulk_transfer(ulink_handle->usb_device_handle, ulink_handle->ep_in,
 				dummy, 64, &transferred, 200);
 
 		free(dummy);
diff --git a/src/jtag/drivers/usb_blaster/ublast2_access_libusb.c b/src/jtag/drivers/usb_blaster/ublast2_access_libusb.c
index f8ff66e..6f15fa7 100644
--- a/src/jtag/drivers/usb_blaster/ublast2_access_libusb.c
+++ b/src/jtag/drivers/usb_blaster/ublast2_access_libusb.c
@@ -134,7 +134,7 @@ static int load_usb_blaster_firmware(struct libusb_device_handle *libusb_dev,
 	}
 
 	ublast2_firmware_image.base_address = 0;
-	ublast2_firmware_image.base_address_set = 0;
+	ublast2_firmware_image.base_address_set = false;
 
 	int ret = image_open(&ublast2_firmware_image, low->firmware_path, "ihex");
 	if (ret != ERROR_OK) {
@@ -162,7 +162,7 @@ static int load_usb_blaster_firmware(struct libusb_device_handle *libusb_dev,
 				     100);
 
 	/* Download all sections in the image to ULINK */
-	for (int i = 0; i < ublast2_firmware_image.num_sections; i++) {
+	for (unsigned int i = 0; i < ublast2_firmware_image.num_sections; i++) {
 		ret = ublast2_write_firmware_section(libusb_dev,
 						     &ublast2_firmware_image, i);
 		if (ret != ERROR_OK) {
diff --git a/src/jtag/drivers/usb_blaster/usb_blaster.c b/src/jtag/drivers/usb_blaster/usb_blaster.c
index 9648ba2..de3b5d5 100644
--- a/src/jtag/drivers/usb_blaster/usb_blaster.c
+++ b/src/jtag/drivers/usb_blaster/usb_blaster.c
@@ -275,7 +275,7 @@ static void ublast_queue_byte(uint8_t abyte)
  *
  * Returns pin value (1 means driven high, 0 mean driven low)
  */
-bool ublast_compute_pin(enum gpio_steer steer)
+static bool ublast_compute_pin(enum gpio_steer steer)
 {
 	switch (steer) {
 	case FIXED_0:
diff --git a/src/jtag/drivers/versaloon/usbtoxxx/usbtoswd.c b/src/jtag/drivers/versaloon/usbtoxxx/usbtoswd.c
index ef1b675..cb4862f 100644
--- a/src/jtag/drivers/versaloon/usbtoxxx/usbtoswd.c
+++ b/src/jtag/drivers/versaloon/usbtoxxx/usbtoswd.c
@@ -28,7 +28,7 @@
 #include "usbtoxxx.h"
 #include "usbtoxxx_internal.h"
 
-RESULT usbtoswd_read_callback(void *p, uint8_t *src, uint8_t *processed)
+static RESULT usbtoswd_read_callback(void *p, uint8_t *src, uint8_t *processed)
 {
 	struct versaloon_pending_t *pending = (struct versaloon_pending_t *)p;
 
@@ -38,7 +38,7 @@ RESULT usbtoswd_read_callback(void *p, uint8_t *src, uint8_t *processed)
 	return ERROR_OK;
 }
 
-RESULT usbtoswd_write_callback(void *p, uint8_t *src, uint8_t *processed)
+static RESULT usbtoswd_write_callback(void *p, uint8_t *src, uint8_t *processed)
 {
 	struct versaloon_pending_t *pending = (struct versaloon_pending_t *)p;
 
diff --git a/src/jtag/drivers/versaloon/usbtoxxx/usbtoxxx.c b/src/jtag/drivers/versaloon/usbtoxxx/usbtoxxx.c
index 678b097..b46bbe0 100644
--- a/src/jtag/drivers/versaloon/usbtoxxx/usbtoxxx.c
+++ b/src/jtag/drivers/versaloon/usbtoxxx/usbtoxxx.c
@@ -29,7 +29,7 @@
 
 #define N_A "n/a"
 
-const char *types_name[96] = {
+static const char *types_name[96] = {
 	"usbtousart", "usbtospi", "usbtoi2c", "usbtogpio", "usbtocan", "usbtopwm",
 	"usbtoadc", "usbtodac",
 	"usbtomicrowire", "usbtoswim", "usbtodusi", N_A, N_A, N_A, "usbtopower", "usbtodelay",
@@ -55,8 +55,8 @@ static uint16_t usbtoxxx_buffer_index;
 static uint16_t usbtoxxx_current_cmd_index;
 static uint8_t *usbtoxxx_buffer;
 
-uint16_t collect_index;
-uint8_t collect_cmd;
+static uint16_t collect_index;
+static uint8_t collect_cmd;
 static uint8_t poll_nesting;
 
 struct usbtoxxx_context_t {
@@ -86,7 +86,7 @@ static void usbtoxxx_pop_context(struct usbtoxxx_context_t *c)
 	versaloon_pending_idx = c->versaloon_pending_idx;
 }
 
-RESULT usbtoxxx_validate_current_command_type(void)
+static RESULT usbtoxxx_validate_current_command_type(void)
 {
 	if (type_pre > 0) {
 		/* not the first command */
@@ -272,7 +272,7 @@ bool usbtoxxx_interface_supported(uint8_t cmd)
 	return (usbtoxxx_abilities[cmd  / 8] & (1 << (cmd % 8))) > 0;
 }
 
-RESULT usbtoxxx_ensure_buffer_size(uint16_t cmdlen)
+static RESULT usbtoxxx_ensure_buffer_size(uint16_t cmdlen)
 {
 	/* check free space, commit if not enough */
 	if (((usbtoxxx_buffer_index + usbtoxxx_current_cmd_index + cmdlen)
diff --git a/src/jtag/drivers/versaloon/versaloon.c b/src/jtag/drivers/versaloon/versaloon.c
index 8efe443..b517795 100644
--- a/src/jtag/drivers/versaloon/versaloon.c
+++ b/src/jtag/drivers/versaloon/versaloon.c
@@ -38,12 +38,12 @@ uint16_t versaloon_pending_idx;
 libusb_device_handle *versaloon_usb_device_handle;
 static uint32_t versaloon_usb_to = VERSALOON_TIMEOUT;
 
-RESULT versaloon_init(void);
-RESULT versaloon_fini(void);
-RESULT versaloon_get_target_voltage(uint16_t *voltage);
-RESULT versaloon_set_target_voltage(uint16_t voltage);
-RESULT versaloon_delay_ms(uint16_t ms);
-RESULT versaloon_delay_us(uint16_t us);
+static RESULT versaloon_init(void);
+static RESULT versaloon_fini(void);
+static RESULT versaloon_get_target_voltage(uint16_t *voltage);
+static RESULT versaloon_set_target_voltage(uint16_t voltage);
+static RESULT versaloon_delay_ms(uint16_t ms);
+static RESULT versaloon_delay_us(uint16_t us);
 
 struct versaloon_interface_t versaloon_interface = {
 	.init				= versaloon_init,
@@ -233,7 +233,7 @@ RESULT versaloon_send_command(uint16_t out_len, uint16_t *inlen)
 }
 
 #define VERSALOON_RETRY_CNT 10
-RESULT versaloon_init(void)
+static RESULT versaloon_init(void)
 {
 	uint16_t ret = 0;
 	uint8_t retry;
@@ -291,7 +291,7 @@ RESULT versaloon_init(void)
 	return versaloon_get_target_voltage(&ret);
 }
 
-RESULT versaloon_fini(void)
+static RESULT versaloon_fini(void)
 {
 	if (versaloon_usb_device_handle != NULL) {
 		usbtoxxx_fini();
@@ -309,7 +309,7 @@ RESULT versaloon_fini(void)
 	return ERROR_OK;
 }
 
-RESULT versaloon_set_target_voltage(uint16_t voltage)
+static RESULT versaloon_set_target_voltage(uint16_t voltage)
 {
 	usbtopwr_init(0);
 	usbtopwr_config(0);
@@ -319,7 +319,7 @@ RESULT versaloon_set_target_voltage(uint16_t voltage)
 	return usbtoxxx_execute_command();
 }
 
-RESULT versaloon_get_target_voltage(uint16_t *voltage)
+static RESULT versaloon_get_target_voltage(uint16_t *voltage)
 {
 	uint16_t inlen;
 
@@ -345,12 +345,12 @@ RESULT versaloon_get_target_voltage(uint16_t *voltage)
 	}
 }
 
-RESULT versaloon_delay_ms(uint16_t ms)
+static RESULT versaloon_delay_ms(uint16_t ms)
 {
 	return usbtodelay_delay(ms | 0x8000);
 }
 
-RESULT versaloon_delay_us(uint16_t us)
+static RESULT versaloon_delay_us(uint16_t us)
 {
 	return usbtodelay_delay(us & 0x7FFF);
 }
diff --git a/src/jtag/drivers/xds110.c b/src/jtag/drivers/xds110.c
index fb04d51..df1ab65 100644
--- a/src/jtag/drivers/xds110.c
+++ b/src/jtag/drivers/xds110.c
@@ -186,7 +186,7 @@
 #define CMD_STABLECLOCKS 4
 
 /* Array to convert from OpenOCD tap_state_t to XDS JTAG state */
-const uint32_t xds_jtag_state[] = {
+static const uint32_t xds_jtag_state[] = {
 	XDS_JTAG_STATE_EXIT2_DR,   /* TAP_DREXIT2   = 0x0 */
 	XDS_JTAG_STATE_EXIT1_DR,   /* TAP_DREXIT1   = 0x1 */
 	XDS_JTAG_STATE_SHIFT_DR,   /* TAP_DRSHIFT   = 0x2 */
diff --git a/src/jtag/drivers/xlnx-pcie-xvc.c b/src/jtag/drivers/xlnx-pcie-xvc.c
index 2423a9f..22f256f 100644
--- a/src/jtag/drivers/xlnx-pcie-xvc.c
+++ b/src/jtag/drivers/xlnx-pcie-xvc.c
@@ -120,7 +120,7 @@ static int xlnx_pcie_xvc_transact(size_t num_bits, uint32_t tms, uint32_t tdi,
 	return ERROR_OK;
 }
 
-int xlnx_pcie_xvc_execute_stableclocks(struct jtag_command *cmd)
+static int xlnx_pcie_xvc_execute_stableclocks(struct jtag_command *cmd)
 {
 	int tms = tap_get_state() == TAP_RESET ? 1 : 0;
 	size_t left = cmd->cmd.stableclocks->num_cycles;
diff --git a/src/jtag/hla/hla_interface.c b/src/jtag/hla/hla_interface.c
index 490eb9f..6691a9a 100644
--- a/src/jtag/hla/hla_interface.c
+++ b/src/jtag/hla/hla_interface.c
@@ -188,7 +188,7 @@ int hl_interface_override_target(const char **targetname)
 	return ERROR_FAIL;
 }
 
-int hl_interface_config_trace(bool enabled, enum tpiu_pin_protocol pin_protocol,
+static int hl_interface_config_trace(bool enabled, enum tpiu_pin_protocol pin_protocol,
 		uint32_t port_size, unsigned int *trace_freq,
 		unsigned int traceclkin_freq, uint16_t *prescaler)
 {
@@ -203,7 +203,7 @@ int hl_interface_config_trace(bool enabled, enum tpiu_pin_protocol pin_protocol,
 	return ERROR_OK;
 }
 
-int hl_interface_poll_trace(uint8_t *buf, size_t *size)
+static int hl_interface_poll_trace(uint8_t *buf, size_t *size)
 {
 	if (hl_if.layout->api->poll_trace)
 		return hl_if.layout->api->poll_trace(hl_if.handle, buf, size);
diff --git a/src/jtag/hla/hla_layout.h b/src/jtag/hla/hla_layout.h
index e0bbd0f..a8088fe 100644
--- a/src/jtag/hla/hla_layout.h
+++ b/src/jtag/hla/hla_layout.h
@@ -51,10 +51,25 @@ struct hl_layout_api_s {
 	int (*step)(void *handle);
 	/** */
 	int (*read_regs)(void *handle);
-	/** */
-	int (*read_reg)(void *handle, int num, uint32_t *val);
-	/** */
-	int (*write_reg)(void *handle, int num, uint32_t val);
+	/**
+	 * Read one register from the target
+	 *
+	 * @param handle A pointer to the device-specific handle
+	 * @param regsel Register selection index compatible with all the
+	 * values allowed by armv7m DCRSR.REGSEL
+	 * @param val A pointer to retrieve the register value
+	 * @returns ERROR_OK on success, or an error code on failure.
+	 */
+	int (*read_reg)(void *handle, unsigned int regsel, uint32_t *val);
+	/**
+	 * Write one register to the target
+	 * @param handle A pointer to the device-specific handle
+	 * @param regsel Register selection index compatible with all the
+	 * values allowed by armv7m DCRSR.REGSEL
+	 * @param val The value to be written in the register
+	 * @returns ERROR_OK on success, or an error code on failure.
+	 */
+	int (*write_reg)(void *handle, unsigned int regsel, uint32_t val);
 	/** */
 	int (*read_mem)(void *handle, uint32_t addr, uint32_t size,
 			uint32_t count, uint8_t *buffer);
diff --git a/src/jtag/interfaces.c b/src/jtag/interfaces.c
index 2fa53be..061a78f 100644
--- a/src/jtag/interfaces.c
+++ b/src/jtag/interfaces.c
@@ -134,7 +134,7 @@ extern struct adapter_driver aice_adapter_driver;
 #if BUILD_BCM2835GPIO == 1
 extern struct adapter_driver bcm2835gpio_adapter_driver;
 #endif
-#if BUILD_CMSIS_DAP == 1
+#if BUILD_CMSIS_DAP_USB == 1 || BUILD_CMSIS_DAP_HID == 1
 extern struct adapter_driver cmsis_dap_adapter_driver;
 #endif
 #if BUILD_KITPROG == 1
@@ -254,7 +254,7 @@ struct adapter_driver *adapter_drivers[] = {
 #if BUILD_BCM2835GPIO == 1
 		&bcm2835gpio_adapter_driver,
 #endif
-#if BUILD_CMSIS_DAP == 1
+#if BUILD_CMSIS_DAP_USB == 1 || BUILD_CMSIS_DAP_HID == 1
 		&cmsis_dap_adapter_driver,
 #endif
 #if BUILD_KITPROG == 1
diff --git a/src/jtag/tcl.c b/src/jtag/tcl.c
index 153a98e..2fa162e 100644
--- a/src/jtag/tcl.c
+++ b/src/jtag/tcl.c
@@ -194,6 +194,11 @@ static int Jim_Command_drscan(Jim_Interp *interp, int argc, Jim_Obj *const *args
 	retval = jtag_execute_queue();
 	if (retval != ERROR_OK) {
 		Jim_SetResultString(interp, "drscan: jtag execute failed", -1);
+
+		for (i = 0; i < field_count; i++)
+			free(fields[i].in_value);
+		free(fields);
+
 		return JIM_ERR;
 	}
 
diff --git a/src/openocd.c b/src/openocd.c
index 8862284..83c3545 100644
--- a/src/openocd.c
+++ b/src/openocd.c
@@ -38,9 +38,11 @@
 #include <pld/pld.h>
 #include <target/arm_cti.h>
 #include <target/arm_adi_v5.h>
+#include <rtt/rtt.h>
 
 #include <server/server.h>
 #include <server/gdb_server.h>
+#include <server/rtt_server.h>
 
 #ifdef HAVE_STRINGS_H
 #include <strings.h>
@@ -230,10 +232,7 @@ static int openocd_register_commands(struct command_context *cmd_ctx)
 
 struct command_context *global_cmd_ctx;
 
-/* NB! this fn can be invoked outside this file for non PC hosted builds
- * NB! do not change to 'static'!!!!
- */
-struct command_context *setup_command_handler(Jim_Interp *interp)
+static struct command_context *setup_command_handler(Jim_Interp *interp)
 {
 	log_init();
 	LOG_DEBUG("log_init: complete");
@@ -247,6 +246,7 @@ struct command_context *setup_command_handler(Jim_Interp *interp)
 		&server_register_commands,
 		&gdb_register_commands,
 		&log_register_commands,
+		&rtt_server_register_commands,
 		&transport_register_commands,
 		&interface_register_commands,
 		&target_register_commands,
@@ -338,6 +338,9 @@ int openocd_main(int argc, char *argv[])
 	if (ioutil_init(cmd_ctx) != ERROR_OK)
 		return EXIT_FAILURE;
 
+	if (rtt_init() != ERROR_OK)
+		return EXIT_FAILURE;
+
 	LOG_OUTPUT("For bug reports, read\n\t"
 		"http://openocd.org/doc/doxygen/bugs.html"
 		"\n");
@@ -367,6 +370,7 @@ int openocd_main(int argc, char *argv[])
 	/* Shutdown commandline interface */
 	command_exit(cmd_ctx);
 
+	rtt_exit();
 	free_config();
 
 	if (ERROR_FAIL == ret)
diff --git a/src/rtos/ThreadX.c b/src/rtos/ThreadX.c
index 302641b..7d58725 100644
--- a/src/rtos/ThreadX.c
+++ b/src/rtos/ThreadX.c
@@ -110,7 +110,7 @@ static const struct stack_register_offset rtos_threadx_arm926ejs_stack_offsets_i
 	{ 16, 0x04, 32 },		/* xPSR      */
 };
 
-const struct rtos_register_stacking rtos_threadx_arm926ejs_stacking[] = {
+static const struct rtos_register_stacking rtos_threadx_arm926ejs_stacking[] = {
 {
 	ARM926EJS_REGISTERS_SIZE_SOLICITED,	/* stack_registers_size */
 	-1,									/* stack_growth_direction */
diff --git a/src/rtos/hwthread.c b/src/rtos/hwthread.c
index 773a87d..4051a5d 100644
--- a/src/rtos/hwthread.c
+++ b/src/rtos/hwthread.c
@@ -37,11 +37,11 @@ static int hwthread_get_thread_reg_list(struct rtos *rtos, int64_t thread_id,
 		struct rtos_reg **reg_list, int *num_regs);
 static int hwthread_get_symbol_list_to_lookup(symbol_table_elem_t *symbol_list[]);
 static int hwthread_smp_init(struct target *target);
-int hwthread_set_reg(struct rtos *rtos, uint32_t reg_num, uint8_t *reg_value);
-bool hwthread_needs_fake_step(struct target *target, int64_t thread_id);
-int hwthread_read_buffer(struct rtos *rtos, target_addr_t address,
+static int hwthread_set_reg(struct rtos *rtos, uint32_t reg_num, uint8_t *reg_value);
+static bool hwthread_needs_fake_step(struct target *target, int64_t thread_id);
+static int hwthread_read_buffer(struct rtos *rtos, target_addr_t address,
 		uint32_t size, uint8_t *buffer);
-int hwthread_write_buffer(struct rtos *rtos, target_addr_t address,
+static int hwthread_write_buffer(struct rtos *rtos, target_addr_t address,
 		uint32_t size, const uint8_t *buffer);
 
 #define HW_THREAD_NAME_STR_SIZE (32)
@@ -247,23 +247,35 @@ static int hwthread_get_thread_reg_list(struct rtos *rtos, int64_t thread_id,
 	if (!target_was_examined(curr))
 		return ERROR_FAIL;
 
+	int reg_list_size;
 	struct reg **reg_list;
-	int retval = target_get_gdb_reg_list(curr, &reg_list, rtos_reg_list_size,
+	int retval = target_get_gdb_reg_list(curr, &reg_list, &reg_list_size,
 			REG_CLASS_GENERAL);
 	if (retval != ERROR_OK)
 		return retval;
 
+	int j = 0;
+	for (int i = 0; i < reg_list_size; i++) {
+		if (reg_list[i] == NULL || reg_list[i]->exist == false || reg_list[i]->hidden)
+			continue;
+		j++;
+	}
+	*rtos_reg_list_size = j;
 	*rtos_reg_list = calloc(*rtos_reg_list_size, sizeof(struct rtos_reg));
 	if (*rtos_reg_list == NULL) {
 		free(reg_list);
 		return ERROR_FAIL;
 	}
 
-	for (int i = 0; i < *rtos_reg_list_size; i++) {
-		(*rtos_reg_list)[i].number = (*reg_list)[i].number;
-		(*rtos_reg_list)[i].size = (*reg_list)[i].size;
-		memcpy((*rtos_reg_list)[i].value, (*reg_list)[i].value,
+	j = 0;
+	for (int i = 0; i < reg_list_size; i++) {
+		if (reg_list[i] == NULL || reg_list[i]->exist == false || reg_list[i]->hidden)
+			continue;
+		(*rtos_reg_list)[j].number = (*reg_list)[i].number;
+		(*rtos_reg_list)[j].size = (*reg_list)[i].size;
+		memcpy((*rtos_reg_list)[j].value, (*reg_list)[i].value,
 				((*reg_list)[i].size + 7) / 8);
+		j++;
 	}
 	free(reg_list);
 
@@ -308,7 +320,7 @@ static int hwthread_get_thread_reg(struct rtos *rtos, int64_t thread_id,
 	return ERROR_OK;
 }
 
-int hwthread_set_reg(struct rtos *rtos, uint32_t reg_num, uint8_t *reg_value)
+static int hwthread_set_reg(struct rtos *rtos, uint32_t reg_num, uint8_t *reg_value)
 {
 	if (rtos == NULL)
 		return ERROR_FAIL;
@@ -395,12 +407,12 @@ static int hwthread_create(struct target *target)
 	return 0;
 }
 
-bool hwthread_needs_fake_step(struct target *target, int64_t thread_id)
+static bool hwthread_needs_fake_step(struct target *target, int64_t thread_id)
 {
 	return false;
 }
 
-int hwthread_read_buffer(struct rtos *rtos, target_addr_t address,
+static int hwthread_read_buffer(struct rtos *rtos, target_addr_t address,
 		uint32_t size, uint8_t *buffer)
 {
 	if (rtos == NULL)
@@ -415,7 +427,7 @@ int hwthread_read_buffer(struct rtos *rtos, target_addr_t address,
 	return target_read_buffer(curr, address, size, buffer);
 }
 
-int hwthread_write_buffer(struct rtos *rtos, target_addr_t address,
+static int hwthread_write_buffer(struct rtos *rtos, target_addr_t address,
 		uint32_t size, const uint8_t *buffer)
 {
 	if (rtos == NULL)
diff --git a/src/rtos/linux.c b/src/rtos/linux.c
index 44e132d..0cb4b54 100644
--- a/src/rtos/linux.c
+++ b/src/rtos/linux.c
@@ -92,7 +92,7 @@ struct cpu_context {
 	uint32_t PC;
 	uint32_t preempt_count;
 };
-struct cpu_context *cpu_context_read(struct target *target, uint32_t base_addr,
+static struct cpu_context *cpu_context_read(struct target *target, uint32_t base_addr,
 				     uint32_t *info_addr);
 static int insert_into_threadlist(struct target *target, struct threads *t);
 
@@ -144,7 +144,7 @@ static int linux_read_memory(struct target *target,
 	return ERROR_OK;
 }
 
-int fill_buffer(struct target *target, uint32_t addr, uint8_t *buffer)
+static int fill_buffer(struct target *target, uint32_t addr, uint8_t *buffer)
 {
 
 	if ((addr & 0xfffffffc) != addr)
@@ -155,7 +155,7 @@ int fill_buffer(struct target *target, uint32_t addr, uint8_t *buffer)
 
 }
 
-uint32_t get_buffer(struct target *target, const uint8_t *buffer)
+static uint32_t get_buffer(struct target *target, const uint8_t *buffer)
 {
 	uint32_t value = 0;
 	const uint8_t *value_ptr = buffer;
@@ -293,7 +293,7 @@ int fill_task_pid(struct target *target, struct threads *t)
 }
 #endif
 
-int fill_task(struct target *target, struct threads *t)
+static int fill_task(struct target *target, struct threads *t)
 {
 	int retval;
 	uint32_t pid_addr = t->base_addr + PID;
@@ -349,7 +349,7 @@ int fill_task(struct target *target, struct threads *t)
 	return retval;
 }
 
-int get_name(struct target *target, struct threads *t)
+static int get_name(struct target *target, struct threads *t)
 {
 	int retval;
 	uint32_t full_name[4];
@@ -395,7 +395,7 @@ int get_name(struct target *target, struct threads *t)
 
 }
 
-int get_current(struct target *target, int create)
+static int get_current(struct target *target, int create)
 {
 	struct target_list *head;
 	head = target->head;
@@ -483,7 +483,7 @@ int get_current(struct target *target, int create)
 	return ERROR_OK;
 }
 
-struct cpu_context *cpu_context_read(struct target *target, uint32_t base_addr,
+static struct cpu_context *cpu_context_read(struct target *target, uint32_t base_addr,
 	uint32_t *thread_info_addr_old)
 {
 	struct cpu_context *context = calloc(1, sizeof(struct cpu_context));
@@ -579,7 +579,7 @@ retry:
 	return context;
 }
 
-uint32_t next_task(struct target *target, struct threads *t)
+static uint32_t next_task(struct target *target, struct threads *t)
 {
 	uint8_t *buffer = calloc(1, 4);
 	uint32_t next_addr = t->base_addr + NEXT;
@@ -598,7 +598,7 @@ uint32_t next_task(struct target *target, struct threads *t)
 	return 0;
 }
 
-struct current_thread *add_current_thread(struct current_thread *currents,
+static struct current_thread *add_current_thread(struct current_thread *currents,
 	struct current_thread *ct)
 {
 	ct->next = NULL;
@@ -617,7 +617,7 @@ struct current_thread *add_current_thread(struct current_thread *currents,
 	}
 }
 
-struct threads *liste_del_task(struct threads *task_list, struct threads **t,
+static struct threads *liste_del_task(struct threads *task_list, struct threads **t,
 	struct threads *prev)
 {
 	LOG_INFO("del task %" PRId64, (*t)->threadid);
@@ -634,7 +634,7 @@ struct threads *liste_del_task(struct threads *task_list, struct threads **t,
 	return task_list;
 }
 
-struct threads *liste_add_task(struct threads *task_list, struct threads *t,
+static struct threads *liste_add_task(struct threads *task_list, struct threads *t,
 	struct threads **last)
 {
 	t->next = NULL;
@@ -683,7 +683,7 @@ static int current_base_addr(struct linux_os *linux_os, uint32_t base_addr)
 	return 0;
 }
 
-int linux_get_tasks(struct target *target, int context)
+static int linux_get_tasks(struct target *target, int context)
 {
 	int loop = 0;
 	int retval = 0;
@@ -1033,7 +1033,7 @@ static int linux_task_update(struct target *target, int context)
 	return ERROR_OK;
 }
 
-int linux_gdb_thread_packet(struct target *target,
+static int linux_gdb_thread_packet(struct target *target,
 	struct connection *connection, char const *packet,
 	int packet_size)
 {
@@ -1070,7 +1070,7 @@ int linux_gdb_thread_packet(struct target *target,
 	return ERROR_OK;
 }
 
-int linux_gdb_thread_update(struct target *target,
+static int linux_gdb_thread_update(struct target *target,
 	struct connection *connection, char const *packet,
 	int packet_size)
 {
@@ -1117,7 +1117,7 @@ int linux_gdb_thread_update(struct target *target,
 	return ERROR_OK;
 }
 
-int linux_thread_extra_info(struct target *target,
+static int linux_thread_extra_info(struct target *target,
 	struct connection *connection, char const *packet,
 	int packet_size)
 {
@@ -1163,7 +1163,7 @@ int linux_thread_extra_info(struct target *target,
 	return ERROR_OK;
 }
 
-int linux_gdb_T_packet(struct connection *connection,
+static int linux_gdb_T_packet(struct connection *connection,
 	struct target *target, char const *packet, int packet_size)
 {
 	int64_t threadid;
@@ -1223,7 +1223,7 @@ int linux_gdb_T_packet(struct connection *connection,
 	return retval;
 }
 
-int linux_gdb_h_packet(struct connection *connection,
+static int linux_gdb_h_packet(struct connection *connection,
 	struct target *target, char const *packet, int packet_size)
 {
 	struct linux_os *linux_os = (struct linux_os *)
diff --git a/src/rtos/nuttx.c b/src/rtos/nuttx.c
index 3c90625..19b93ba 100644
--- a/src/rtos/nuttx.c
+++ b/src/rtos/nuttx.c
@@ -72,7 +72,7 @@ struct tcb {
 	uint8_t  dat[512];
 };
 
-struct {
+static struct {
 	uint32_t addr;
 	uint32_t prio;
 } g_tasklist[TASK_QUEUE_NUM];
diff --git a/src/rtos/rtos.c b/src/rtos/rtos.c
index 4798f04..b391b2c 100644
--- a/src/rtos/rtos.c
+++ b/src/rtos/rtos.c
@@ -59,6 +59,8 @@ static struct rtos_type *rtos_types[] = {
 	NULL
 };
 
+static int rtos_try_next(struct target *target);
+
 int rtos_thread_packet(struct connection *connection, const char *packet, int packet_size);
 
 int rtos_smp_init(struct target *target)
@@ -632,7 +634,7 @@ int rtos_generic_stack_read(struct target *target,
 	return ERROR_OK;
 }
 
-int rtos_try_next(struct target *target)
+static int rtos_try_next(struct target *target)
 {
 	struct rtos *os = target->rtos;
 	struct rtos_type **type = rtos_types;
diff --git a/src/rtos/rtos.h b/src/rtos/rtos.h
index 9b43afb..1bda730 100644
--- a/src/rtos/rtos.h
+++ b/src/rtos/rtos.h
@@ -134,7 +134,6 @@ int rtos_generic_stack_read(struct target *target,
 		int64_t stack_ptr,
 		struct rtos_reg **reg_list,
 		int *num_regs);
-int rtos_try_next(struct target *target);
 int gdb_thread_packet(struct connection *connection, char const *packet, int packet_size);
 int rtos_get_gdb_reg(struct connection *connection, int reg_num);
 int rtos_get_gdb_reg_list(struct connection *connection);
diff --git a/src/rtt/Makefile.am b/src/rtt/Makefile.am
new file mode 100644
index 0000000..e3fcefd
--- /dev/null
+++ b/src/rtt/Makefile.am
@@ -0,0 +1,2 @@
+noinst_LTLIBRARIES += %D%/librtt.la
+%C%_librtt_la_SOURCES = %D%/rtt.c %D%/rtt.h %D%/tcl.c
diff --git a/src/rtt/rtt.c b/src/rtt/rtt.c
new file mode 100644
index 0000000..bf3cca5
--- /dev/null
+++ b/src/rtt/rtt.c
@@ -0,0 +1,332 @@
+/*
+ * Copyright (C) 2016-2020 by Marc Schink <dev@zapb.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <string.h>
+
+#include <helper/log.h>
+#include <helper/list.h>
+#include <target/target.h>
+#include <target/rtt.h>
+
+#include "rtt.h"
+
+static struct {
+	struct rtt_source source;
+	/** Control block. */
+	struct rtt_control ctrl;
+	struct target *target;
+	/** Start address to search for the control block. */
+	target_addr_t addr;
+	/** Size of the control block search area. */
+	size_t size;
+	/** Control block identifier. */
+	char id[RTT_CB_MAX_ID_LENGTH];
+	/** Whether RTT is configured. */
+	bool configured;
+	/** Whether RTT is started. */
+	bool started;
+	/** Whether configuration changed. */
+	bool changed;
+	/** Whether the control block was found. */
+	bool found_cb;
+
+	struct rtt_sink_list **sink_list;
+	size_t sink_list_length;
+
+	unsigned int polling_interval;
+} rtt;
+
+int rtt_init(void)
+{
+	rtt.sink_list_length = 1;
+	rtt.sink_list = calloc(rtt.sink_list_length,
+		sizeof(struct rtt_sink_list *));
+
+	if (!rtt.sink_list)
+		return ERROR_FAIL;
+
+	rtt.sink_list[0] = NULL;
+	rtt.started = false;
+
+	rtt.polling_interval = 100;
+
+	return ERROR_OK;
+}
+
+int rtt_exit(void)
+{
+	free(rtt.sink_list);
+
+	return ERROR_OK;
+}
+
+static int read_channel_callback(void *user_data)
+{
+	int ret;
+
+	ret = rtt.source.read(rtt.target, &rtt.ctrl, rtt.sink_list,
+		rtt.sink_list_length, NULL);
+
+	if (ret != ERROR_OK) {
+		target_unregister_timer_callback(&read_channel_callback, NULL);
+		rtt.source.stop(rtt.target, NULL);
+		return ret;
+	}
+
+	return ERROR_OK;
+}
+
+int rtt_setup(target_addr_t address, size_t size, const char *id)
+{
+	size_t id_length = strlen(id);
+
+	if (!id_length || id_length >= RTT_CB_MAX_ID_LENGTH) {
+		LOG_ERROR("rtt: Invalid control block ID");
+		return ERROR_COMMAND_ARGUMENT_INVALID;
+	}
+
+	rtt.addr = address;
+	rtt.size = size;
+	strncpy(rtt.id, id, id_length + 1);
+	rtt.changed = true;
+	rtt.configured = true;
+
+	return ERROR_OK;
+}
+
+int rtt_register_source(const struct rtt_source source,
+		struct target *target)
+{
+	if (!source.find_cb || !source.read_cb || !source.read_channel_info)
+		return ERROR_FAIL;
+
+	if (!source.start || !source.stop)
+		return ERROR_FAIL;
+
+	if (!source.read || !source.write)
+		return ERROR_FAIL;
+
+	rtt.source = source;
+	rtt.target = target;
+
+	return ERROR_OK;
+}
+
+int rtt_start(void)
+{
+	int ret;
+	target_addr_t addr = rtt.addr;
+
+	if (rtt.started)
+		return ERROR_OK;
+
+	if (!rtt.found_cb || rtt.changed) {
+		rtt.source.find_cb(rtt.target, &addr, rtt.size, rtt.id,
+			&rtt.found_cb, NULL);
+
+		rtt.changed = false;
+
+		if (rtt.found_cb) {
+			LOG_INFO("rtt: Control block found at 0x%" TARGET_PRIxADDR,
+				addr);
+			rtt.ctrl.address = addr;
+		} else {
+			LOG_INFO("rtt: No control block found");
+			return ERROR_OK;
+		}
+	}
+
+	ret = rtt.source.read_cb(rtt.target, rtt.ctrl.address, &rtt.ctrl, NULL);
+
+	if (ret != ERROR_OK)
+		return ret;
+
+	ret = rtt.source.start(rtt.target, &rtt.ctrl, NULL);
+
+	if (ret != ERROR_OK)
+		return ret;
+
+	target_register_timer_callback(&read_channel_callback,
+		rtt.polling_interval, 1, NULL);
+	rtt.started = true;
+
+	return ERROR_OK;
+}
+
+int rtt_stop(void)
+{
+	int ret;
+
+	if (!rtt.configured) {
+		LOG_ERROR("rtt: Not configured");
+		return ERROR_FAIL;
+	}
+
+	target_unregister_timer_callback(&read_channel_callback, NULL);
+	rtt.started = false;
+
+	ret = rtt.source.stop(rtt.target, NULL);
+
+	if (ret != ERROR_OK)
+		return ret;
+
+	return ERROR_OK;
+}
+
+static int adjust_sink_list(size_t length)
+{
+	struct rtt_sink_list **tmp;
+
+	if (length <= rtt.sink_list_length)
+		return ERROR_OK;
+
+	tmp = realloc(rtt.sink_list, sizeof(struct rtt_sink_list *) * length);
+
+	if (!tmp)
+		return ERROR_FAIL;
+
+	for (size_t i = rtt.sink_list_length; i < length; i++)
+		tmp[i] = NULL;
+
+	rtt.sink_list = tmp;
+	rtt.sink_list_length = length;
+
+	return ERROR_OK;
+}
+
+int rtt_register_sink(unsigned int channel_index, rtt_sink_read read,
+		void *user_data)
+{
+	struct rtt_sink_list *tmp;
+
+	if (channel_index >= rtt.sink_list_length) {
+		if (adjust_sink_list(channel_index + 1) != ERROR_OK)
+			return ERROR_FAIL;
+	}
+
+	LOG_DEBUG("rtt: Registering sink for channel %u", channel_index);
+
+	tmp = malloc(sizeof(struct rtt_sink_list));
+
+	if (!tmp)
+		return ERROR_FAIL;
+
+	tmp->read = read;
+	tmp->user_data = user_data;
+	tmp->next = rtt.sink_list[channel_index];
+
+	rtt.sink_list[channel_index] = tmp;
+
+	return ERROR_OK;
+}
+
+int rtt_unregister_sink(unsigned int channel_index, rtt_sink_read read,
+		void *user_data)
+{
+	struct rtt_sink_list *prev_sink;
+
+	LOG_DEBUG("rtt: Unregistering sink for channel %u", channel_index);
+
+	if (channel_index >= rtt.sink_list_length)
+		return ERROR_FAIL;
+
+	prev_sink = rtt.sink_list[channel_index];
+
+	for (struct rtt_sink_list *sink = rtt.sink_list[channel_index]; sink;
+			prev_sink = sink, sink = sink->next) {
+		if (sink->read == read && sink->user_data == user_data) {
+
+			if (sink == rtt.sink_list[channel_index])
+				rtt.sink_list[channel_index] = sink->next;
+			else
+				prev_sink->next = sink->next;
+
+			free(sink);
+
+			return ERROR_OK;
+		}
+	}
+
+	return ERROR_OK;
+}
+
+int rtt_get_polling_interval(unsigned int *interval)
+{
+	if (!interval)
+		return ERROR_FAIL;
+
+	*interval = rtt.polling_interval;
+
+	return ERROR_OK;
+}
+
+int rtt_set_polling_interval(unsigned int interval)
+{
+	if (!interval)
+		return ERROR_FAIL;
+
+	if (rtt.polling_interval != interval) {
+		target_unregister_timer_callback(&read_channel_callback, NULL);
+		target_register_timer_callback(&read_channel_callback, interval, 1,
+			NULL);
+	}
+
+	rtt.polling_interval = interval;
+
+	return ERROR_OK;
+}
+
+int rtt_write_channel(unsigned int channel_index, const uint8_t *buffer,
+		size_t *length)
+{
+	if (channel_index >= rtt.ctrl.num_up_channels) {
+		LOG_WARNING("rtt: Down-channel %u is not available", channel_index);
+		return ERROR_OK;
+	}
+
+	return rtt.source.write(rtt.target, &rtt.ctrl, channel_index, buffer,
+		length, NULL);
+}
+
+bool rtt_started(void)
+{
+	return rtt.started;
+}
+
+bool rtt_configured(void)
+{
+	return rtt.configured;
+}
+
+bool rtt_found_cb(void)
+{
+	return rtt.found_cb;
+}
+
+const struct rtt_control *rtt_get_control(void)
+{
+	return &rtt.ctrl;
+}
+
+int rtt_read_channel_info(unsigned int channel_index,
+	enum rtt_channel_type type, struct rtt_channel_info *info)
+{
+	return rtt.source.read_channel_info(rtt.target, &rtt.ctrl,
+		channel_index, type, info, NULL);
+}
diff --git a/src/rtt/rtt.h b/src/rtt/rtt.h
new file mode 100644
index 0000000..597c838
--- /dev/null
+++ b/src/rtt/rtt.h
@@ -0,0 +1,287 @@
+/*
+ * Copyright (C) 2016-2020 by Marc Schink <dev@zapb.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef OPENOCD_RTT_RTT_H
+#define OPENOCD_RTT_RTT_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#include <helper/command.h>
+#include <target/target.h>
+
+/**
+ * Control block ID length in bytes, including the trailing null-terminator.
+ */
+#define RTT_CB_MAX_ID_LENGTH	16
+
+/* Control block size in bytes. */
+#define RTT_CB_SIZE		(RTT_CB_MAX_ID_LENGTH + 2 * sizeof(uint32_t))
+
+/* Channel structure size in bytes. */
+#define RTT_CHANNEL_SIZE	24
+
+/* Minimal channel buffer size in bytes. */
+#define RTT_CHANNEL_BUFFER_MIN_SIZE	2
+
+/** RTT control block. */
+struct rtt_control {
+	/** Control block address on the target. */
+	target_addr_t address;
+	/** Control block identifier, including trailing null-terminator. */
+	char id[RTT_CB_MAX_ID_LENGTH];
+	/** Maximum number of up-channels. */
+	uint32_t num_up_channels;
+	/** Maximum number of down-channels. */
+	uint32_t num_down_channels;
+};
+
+/** RTT channel. */
+struct rtt_channel {
+	/** Channel structure address on the target. */
+	target_addr_t address;
+	/** Channel name address on the target. */
+	uint32_t name_addr;
+	/** Buffer address on the target. */
+	uint32_t buffer_addr;
+	/** Channel buffer size in bytes. */
+	uint32_t size;
+	/**  Write position within the buffer in bytes. */
+	uint32_t write_pos;
+	/** Read position within the buffer in bytes. */
+	uint32_t read_pos;
+	/**
+	 * Buffer flags.
+	 *
+	 * @note: Not used at the moment.
+	 */
+	uint32_t flags;
+};
+
+/** RTT channel information. */
+struct rtt_channel_info {
+	/** Channel name. */
+	char *name;
+	/** Length of the name in bytes, including the trailing null-terminator. */
+	size_t name_length;
+	/** Buffer size in bytes. */
+	uint32_t size;
+	/**
+	 * Buffer flags.
+	 *
+	 * @note: Not used at the moment.
+	 */
+	uint32_t flags;
+};
+
+typedef int (*rtt_sink_read)(unsigned int channel, const uint8_t *buffer,
+		size_t length, void *user_data);
+
+struct rtt_sink_list {
+	rtt_sink_read read;
+	void *user_data;
+
+	struct rtt_sink_list *next;
+};
+
+/** Channel type. */
+enum rtt_channel_type {
+	/** Up channel (target to host). */
+	RTT_CHANNEL_TYPE_UP,
+	/** Down channel (host to target). */
+	RTT_CHANNEL_TYPE_DOWN
+};
+
+typedef int (*rtt_source_find_ctrl_block)(struct target *target,
+		target_addr_t *address, size_t size, const char *id, bool *found,
+		void *user_data);
+typedef int (*rtt_source_read_ctrl_block)(struct target *target,
+		target_addr_t address, struct rtt_control *ctrl_block,
+		void *user_data);
+typedef int (*rtt_source_read_channel_info)(struct target *target,
+		const struct rtt_control *ctrl, unsigned int channel,
+		enum rtt_channel_type type, struct rtt_channel_info *info,
+		void *user_data);
+typedef int (*rtt_source_start)(struct target *target,
+		const struct rtt_control *ctrl, void *user_data);
+typedef int (*rtt_source_stop)(struct target *target, void *user_data);
+typedef int (*rtt_source_read)(struct target *target,
+		const struct rtt_control *ctrl, struct rtt_sink_list **sinks,
+		size_t num_channels, void *user_data);
+typedef int (*rtt_source_write)(struct target *target,
+		struct rtt_control *ctrl, unsigned int channel,
+		const uint8_t *buffer, size_t *length, void *user_data);
+
+/** RTT source. */
+struct rtt_source {
+	rtt_source_find_ctrl_block find_cb;
+	rtt_source_read_ctrl_block read_cb;
+	rtt_source_read_channel_info read_channel_info;
+	rtt_source_start start;
+	rtt_source_stop stop;
+	rtt_source_read read;
+	rtt_source_write write;
+};
+
+/**
+ * Initialize Real-Time Transfer (RTT).
+ *
+ * @returns ERROR_OK on success, an error code on failure.
+ */
+int rtt_init(void);
+
+/**
+ * Shutdown Real-Time Transfer (RTT).
+ *
+ * @returns ERROR_OK on success, an error code on failure.
+ */
+int rtt_exit(void);
+
+/**
+ * Register an RTT source for a target.
+ *
+ * @param[in] source RTT source.
+ * @param[in,out] target Target.
+ *
+ * @returns ERROR_OK on success, an error code on failure.
+ */
+int rtt_register_source(const struct rtt_source source,
+		struct target *target);
+
+/**
+ * Setup RTT.
+ *
+ * @param[in] address Start address to search for the control block.
+ * @param[in] size Size of the control block search area.
+ * @param[in] id Identifier of the control block. Must be null-terminated.
+ *
+ * @returns ERROR_OK on success, an error code on failure.
+ */
+int rtt_setup(target_addr_t address, size_t size, const char *id);
+
+/**
+ * Start Real-Time Transfer (RTT).
+ *
+ * @returns ERROR_OK on success, an error code on failure.
+ */
+int rtt_start(void);
+
+/**
+ * Stop Real-Time Transfer (RTT).
+ *
+ * @returns ERROR_OK on success, an error code on failure.
+ */
+int rtt_stop(void);
+
+/**
+ * Get the polling interval.
+ *
+ * @param[out] interval Polling interval in milliseconds.
+ *
+ * @returns ERROR_OK on success, an error code on failure.
+ */
+int rtt_get_polling_interval(unsigned int *interval);
+
+/**
+ * Set the polling interval.
+ *
+ * @param[in] interval Polling interval in milliseconds.
+ *
+ * @returns ERROR_OK on success, an error code on failure.
+ */
+int rtt_set_polling_interval(unsigned int interval);
+
+/**
+ * Get whether RTT is started.
+ *
+ * @returns Whether RTT is started.
+ */
+bool rtt_started(void);
+
+/**
+ * Get whether RTT is configured.
+ *
+ * @returns Whether RTT is configured.
+ */
+bool rtt_configured(void);
+
+/**
+ * Get whether RTT control block was found.
+ *
+ * @returns Whether RTT was found.
+ */
+bool rtt_found_cb(void);
+
+/**
+ * Get the RTT control block.
+ *
+ * @returns The RTT control block.
+ */
+const struct rtt_control *rtt_get_control(void);
+
+/**
+ * Read channel information.
+ *
+ * @param[in] channel_index Channel index.
+ * @param[in] channel_type Channel type.
+ * @param[out] info Channel information.
+ *
+ * @returns ERROR_OK on success, an error code on failure.
+ */
+int rtt_read_channel_info(unsigned int channel_index,
+	enum rtt_channel_type type, struct rtt_channel_info *info);
+
+/**
+ * Register an RTT sink.
+ *
+ * @param[in] channel_index Channel index.
+ * @param[in] read Read callback function.
+ * @param[in,out] user_data User data to be passed to the callback function.
+ *
+ * @returns ERROR_OK on success, an error code on failure.
+ */
+int rtt_register_sink(unsigned int channel_index, rtt_sink_read read,
+		void *user_data);
+
+/**
+ * Unregister an RTT sink.
+ *
+ * @param[in] channel_index Channel index.
+ * @param[in] read Read callback function.
+ * @param[in,out] user_data User data to be passed to the callback function.
+ *
+ * @returns ERROR_OK on success, an error code on failure.
+ */
+int rtt_unregister_sink(unsigned int channel_index, rtt_sink_read read,
+		void *user_data);
+
+/**
+ * Write to an RTT channel.
+ *
+ * @param[in] channel_index Channel index.
+ * @param[in] buffer Buffer with data that should be written to the channel.
+ * @param[in,out] length Number of bytes to write. On success, the argument gets
+ *                       updated with the actual number of written bytes.
+ *
+ * @returns ERROR_OK on success, an error code on failure.
+ */
+int rtt_write_channel(unsigned int channel_index, const uint8_t *buffer,
+		size_t *length);
+
+extern const struct command_registration rtt_target_command_handlers[];
+
+#endif /* OPENOCD_RTT_RTT_H */
diff --git a/src/rtt/tcl.c b/src/rtt/tcl.c
new file mode 100644
index 0000000..f5abf2e
--- /dev/null
+++ b/src/rtt/tcl.c
@@ -0,0 +1,311 @@
+/*
+ * Copyright (C) 2019-2020 by Marc Schink <dev@zapb.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <helper/log.h>
+#include <target/rtt.h>
+
+#include "rtt.h"
+
+#define CHANNEL_NAME_SIZE	128
+
+COMMAND_HANDLER(handle_rtt_setup_command)
+{
+struct rtt_source source;
+
+	if (CMD_ARGC != 3)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	source.find_cb = &target_rtt_find_control_block;
+	source.read_cb = &target_rtt_read_control_block;
+	source.start = &target_rtt_start;
+	source.stop = &target_rtt_stop;
+	source.read = &target_rtt_read_callback;
+	source.write = &target_rtt_write_callback;
+	source.read_channel_info = &target_rtt_read_channel_info;
+
+	target_addr_t address;
+	uint32_t size;
+
+	COMMAND_PARSE_NUMBER(target_addr, CMD_ARGV[0], address);
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], size);
+
+	rtt_register_source(source, get_current_target(CMD_CTX));
+
+	if (rtt_setup(address, size, CMD_ARGV[2]) != ERROR_OK)
+		return ERROR_FAIL;
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(handle_rtt_start_command)
+{
+	if (CMD_ARGC > 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	if (!rtt_configured()) {
+		command_print(CMD, "RTT is not configured");
+		return ERROR_FAIL;
+	}
+
+	return rtt_start();
+}
+
+COMMAND_HANDLER(handle_rtt_stop_command)
+{
+	if (CMD_ARGC > 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	return rtt_stop();
+}
+
+COMMAND_HANDLER(handle_rtt_polling_interval_command)
+{
+	if (CMD_ARGC == 0) {
+		int ret;
+		unsigned int interval;
+
+		ret = rtt_get_polling_interval(&interval);
+
+		if (ret != ERROR_OK) {
+			command_print(CMD, "Failed to get polling interval");
+			return ret;
+		}
+
+		command_print(CMD, "%u ms", interval);
+	} else if (CMD_ARGC == 1) {
+		int ret;
+		unsigned int interval;
+
+		COMMAND_PARSE_NUMBER(uint, CMD_ARGV[0], interval);
+		ret = rtt_set_polling_interval(interval);
+
+		if (ret != ERROR_OK) {
+			command_print(CMD, "Failed to set polling interval");
+			return ret;
+		}
+	} else {
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(handle_rtt_channels_command)
+{
+	int ret;
+	char channel_name[CHANNEL_NAME_SIZE];
+	const struct rtt_control *ctrl;
+	struct rtt_channel_info info;
+
+	if (!rtt_found_cb()) {
+		command_print(CMD, "rtt: Control block not available");
+		return ERROR_FAIL;
+	}
+
+	ctrl = rtt_get_control();
+
+	command_print(CMD, "Channels: up=%u, down=%u", ctrl->num_up_channels,
+		ctrl->num_down_channels);
+
+	command_print(CMD, "Up-channels:");
+
+	info.name = channel_name;
+	info.name_length = sizeof(channel_name);
+
+	for (unsigned int i = 0; i < ctrl->num_up_channels; i++) {
+		ret = rtt_read_channel_info(i, RTT_CHANNEL_TYPE_UP, &info);
+
+		if (ret != ERROR_OK)
+			return ret;
+
+		if (!info.size)
+			continue;
+
+		command_print(CMD, "%u: %s %u %u", i, info.name, info.size,
+			info.flags);
+	}
+
+	command_print(CMD, "Down-channels:");
+
+	for (unsigned int i = 0; i < ctrl->num_down_channels; i++) {
+		ret = rtt_read_channel_info(i, RTT_CHANNEL_TYPE_DOWN, &info);
+
+		if (ret != ERROR_OK)
+			return ret;
+
+		if (!info.size)
+			continue;
+
+		command_print(CMD, "%u: %s %u %u", i, info.name, info.size,
+			info.flags);
+	}
+
+	return ERROR_OK;
+}
+
+static int jim_channel_list(Jim_Interp *interp, int argc,
+	Jim_Obj * const *argv)
+{
+	Jim_Obj *list;
+	Jim_Obj *channel_list;
+	char channel_name[CHANNEL_NAME_SIZE];
+	const struct rtt_control *ctrl;
+	struct rtt_channel_info info;
+
+	if (!rtt_found_cb()) {
+		Jim_SetResultFormatted(interp, "rtt: Control block not available");
+		return ERROR_FAIL;
+	}
+
+	ctrl = rtt_get_control();
+
+	info.name = channel_name;
+	info.name_length = sizeof(channel_name);
+
+	list = Jim_NewListObj(interp, NULL, 0);
+	channel_list = Jim_NewListObj(interp, NULL, 0);
+
+	for (unsigned int i = 0; i < ctrl->num_up_channels; i++) {
+		int ret;
+		Jim_Obj *tmp;
+
+		ret = rtt_read_channel_info(i, RTT_CHANNEL_TYPE_UP, &info);
+
+		if (ret != ERROR_OK)
+			return ret;
+
+		if (!info.size)
+			continue;
+
+		tmp = Jim_NewListObj(interp, NULL, 0);
+
+		Jim_ListAppendElement(interp, tmp, Jim_NewStringObj(interp,
+			"name", -1));
+		Jim_ListAppendElement(interp, tmp, Jim_NewStringObj(interp,
+			info.name, -1));
+
+		Jim_ListAppendElement(interp, tmp, Jim_NewStringObj(interp,
+			"size", -1));
+		Jim_ListAppendElement(interp, tmp, Jim_NewIntObj(interp,
+			info.size));
+
+		Jim_ListAppendElement(interp, tmp, Jim_NewStringObj(interp,
+			"flags", -1));
+		Jim_ListAppendElement(interp, tmp, Jim_NewIntObj(interp,
+			info.flags));
+
+		Jim_ListAppendElement(interp, channel_list, tmp);
+	}
+
+	Jim_ListAppendElement(interp, list, channel_list);
+
+	channel_list = Jim_NewListObj(interp, NULL, 0);
+
+	for (unsigned int i = 0; i < ctrl->num_down_channels; i++) {
+		int ret;
+		Jim_Obj *tmp;
+
+		ret = rtt_read_channel_info(i, RTT_CHANNEL_TYPE_DOWN, &info);
+
+		if (ret != ERROR_OK)
+			return ret;
+
+		if (!info.size)
+			continue;
+
+		tmp = Jim_NewListObj(interp, NULL, 0);
+
+		Jim_ListAppendElement(interp, tmp, Jim_NewStringObj(interp,
+			"name", -1));
+		Jim_ListAppendElement(interp, tmp, Jim_NewStringObj(interp,
+			info.name, -1));
+
+		Jim_ListAppendElement(interp, tmp, Jim_NewStringObj(interp,
+			"size", -1));
+		Jim_ListAppendElement(interp, tmp, Jim_NewIntObj(interp,
+			info.size));
+
+		Jim_ListAppendElement(interp, tmp, Jim_NewStringObj(interp,
+			"flags", -1));
+		Jim_ListAppendElement(interp, tmp, Jim_NewIntObj(interp,
+			info.flags));
+
+		Jim_ListAppendElement(interp, channel_list, tmp);
+	}
+
+	Jim_ListAppendElement(interp, list, channel_list);
+	Jim_SetResult(interp, list);
+
+	return JIM_OK;
+}
+
+static const struct command_registration rtt_subcommand_handlers[] = {
+	{
+		.name = "setup",
+		.handler = handle_rtt_setup_command,
+		.mode = COMMAND_ANY,
+		.help = "setup RTT",
+		.usage = "<address> <size> <ID>"
+	},
+	{
+		.name = "start",
+		.handler = handle_rtt_start_command,
+		.mode = COMMAND_EXEC,
+		.help = "start RTT",
+		.usage = ""
+	},
+	{
+		.name = "stop",
+		.handler = handle_rtt_stop_command,
+		.mode = COMMAND_EXEC,
+		.help = "stop RTT",
+		.usage = ""
+	},
+	{
+		.name = "polling_interval",
+		.handler = handle_rtt_polling_interval_command,
+		.mode = COMMAND_EXEC,
+		.help = "show or set polling interval in ms",
+		.usage = "[interval]"
+	},
+	{
+		.name = "channels",
+		.handler = handle_rtt_channels_command,
+		.mode = COMMAND_EXEC,
+		.help = "list available channels",
+		.usage = ""
+	},
+	{
+		.name = "channellist",
+		.jim_handler = jim_channel_list,
+		.mode = COMMAND_EXEC,
+		.help = "list available channels",
+		.usage = ""
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+const struct command_registration rtt_target_command_handlers[] = {
+	{
+		.name = "rtt",
+		.mode = COMMAND_EXEC,
+		.help = "RTT target commands",
+		.usage = "",
+		.chain = rtt_subcommand_handlers
+	},
+	COMMAND_REGISTRATION_DONE
+};
diff --git a/src/server/Makefile.am b/src/server/Makefile.am
index 804efac..d270ee2 100644
--- a/src/server/Makefile.am
+++ b/src/server/Makefile.am
@@ -8,7 +8,9 @@ noinst_LTLIBRARIES += %D%/libserver.la
 	%D%/gdb_server.h \
 	%D%/server_stubs.c \
 	%D%/tcl_server.c \
-	%D%/tcl_server.h
+	%D%/tcl_server.h \
+	%D%/rtt_server.c \
+	%D%/rtt_server.h
 
 %C%_libserver_la_CFLAGS = $(AM_CFLAGS)
 if IS_MINGW
diff --git a/src/server/gdb_server.c b/src/server/gdb_server.c
index c6c74c2..a5f4acb 100644
--- a/src/server/gdb_server.c
+++ b/src/server/gdb_server.c
@@ -967,15 +967,6 @@ static int gdb_new_connection(struct connection *connection)
 	breakpoint_clear_target(target);
 	watchpoint_clear_target(target);
 
-	if (target->rtos) {
-		/* clean previous rtos session if supported*/
-		if (target->rtos->type->clean)
-			target->rtos->type->clean(target);
-
-		/* update threads */
-		rtos_update_threads(target);
-	}
-
 	/* remove the initial ACK from the incoming buffer */
 	retval = gdb_get_char(connection, &initial_ack);
 	if (retval != ERROR_OK)
@@ -988,6 +979,15 @@ static int gdb_new_connection(struct connection *connection)
 		gdb_putback_char(connection, initial_ack);
 	target_call_event_callbacks(target, TARGET_EVENT_GDB_ATTACH);
 
+	if (target->rtos) {
+		/* clean previous rtos session if supported*/
+		if (target->rtos->type->clean)
+			target->rtos->type->clean(target);
+
+		/* update threads */
+		rtos_update_threads(target);
+	}
+
 	if (gdb_use_memory_map) {
 		/* Connect must fail if the memory map can't be set up correctly.
 		 *
@@ -1187,7 +1187,7 @@ static int gdb_get_registers_packet(struct connection *connection,
 		return gdb_error(connection, retval);
 
 	for (i = 0; i < reg_list_size; i++) {
-		if (reg_list[i] == NULL || reg_list[i]->exist == false)
+		if (reg_list[i] == NULL || reg_list[i]->exist == false || reg_list[i]->hidden)
 			continue;
 		reg_packet_size += DIV_ROUND_UP(reg_list[i]->size, 8) * 2;
 	}
@@ -1201,7 +1201,7 @@ static int gdb_get_registers_packet(struct connection *connection,
 	reg_packet_p = reg_packet;
 
 	for (i = 0; i < reg_list_size; i++) {
-		if (reg_list[i] == NULL || reg_list[i]->exist == false)
+		if (reg_list[i] == NULL || reg_list[i]->exist == false || reg_list[i]->hidden)
 			continue;
 		if (!reg_list[i]->valid) {
 			retval = reg_list[i]->type->get(reg_list[i]);
@@ -1330,7 +1330,7 @@ static int gdb_get_register_packet(struct connection *connection,
 		}
 	}
 
-	reg_packet = malloc(DIV_ROUND_UP(reg_list[reg_num]->size, 8) * 2 + 1); /* plus one for string termination null */
+	reg_packet = calloc(DIV_ROUND_UP(reg_list[reg_num]->size, 8) * 2 + 1, 1); /* plus one for string termination null */
 
 	gdb_str_to_target(target, reg_packet, reg_list[reg_num]);
 
@@ -2187,7 +2187,7 @@ static int get_reg_features_list(struct target *target, char const **feature_lis
 	*feature_list = calloc(1, sizeof(char *));
 
 	for (int i = 0; i < reg_list_size; i++) {
-		if (reg_list[i]->exist == false)
+		if (reg_list[i]->exist == false || reg_list[i]->hidden)
 			continue;
 
 		if (reg_list[i]->feature != NULL
@@ -2353,7 +2353,7 @@ static int gdb_generate_target_description(struct target *target, char **tdesc_o
 			int i;
 			for (i = 0; i < reg_list_size; i++) {
 
-				if (reg_list[i]->exist == false)
+				if (reg_list[i]->exist == false || reg_list[i]->hidden)
 					continue;
 
 				if (strcmp(reg_list[i]->feature->name, features[current_feature]))
@@ -2600,7 +2600,7 @@ static int gdb_get_thread_list_chunk(struct target *target, char **thread_list,
 		transfer_type = 'l';
 
 	*chunk = malloc(length + 2 + 3);
-    /* Allocating extra 3 bytes prevents false positive valgrind report
+	/* Allocating extra 3 bytes prevents false positive valgrind report
 	 * of strlen(chunk) word access:
 	 * Invalid read of size 4
 	 * Address 0x4479934 is 44 bytes inside a block of size 45 alloc'd */
@@ -3614,8 +3614,8 @@ static int gdb_target_start(struct target *target, const char *port)
 	target->gdb_service = gdb_service;
 
 	ret = add_service("gdb",
-			port, 1, &gdb_new_connection, &gdb_input,
-			&gdb_connection_closed, gdb_service);
+			port, target->gdb_max_connections, &gdb_new_connection, &gdb_input,
+			&gdb_connection_closed, gdb_service, NULL);
 	/* initialize all targets gdb service with the same pointer */
 	{
 		struct target_list *head;
diff --git a/src/server/rtt_server.c b/src/server/rtt_server.c
new file mode 100644
index 0000000..3c885cc
--- /dev/null
+++ b/src/server/rtt_server.c
@@ -0,0 +1,192 @@
+/*
+ * Copyright (C) 2016-2017 by Marc Schink <dev@zapb.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdint.h>
+#include <rtt/rtt.h>
+
+#include "server.h"
+#include "rtt_server.h"
+
+/**
+ * @file
+ *
+ * RTT server.
+ *
+ * This server allows access to Real Time Transfer (RTT) channels via TCP
+ * connections.
+ */
+
+struct rtt_service {
+	unsigned int channel;
+};
+
+static int read_callback(unsigned int channel, const uint8_t *buffer,
+		size_t length, void *user_data)
+{
+	int ret;
+	struct connection *connection;
+	size_t offset;
+
+	connection = (struct connection *)user_data;
+	offset = 0;
+
+	while (offset < length) {
+		ret = connection_write(connection, buffer + offset, length - offset);
+
+		if (ret < 0) {
+			LOG_ERROR("Failed to write data to socket.");
+			return ERROR_FAIL;
+		}
+
+		offset += ret;
+	}
+
+	return ERROR_OK;
+}
+
+static int rtt_new_connection(struct connection *connection)
+{
+	int ret;
+	struct rtt_service *service;
+
+	service = connection->service->priv;
+
+	LOG_DEBUG("rtt: New connection for channel %u", service->channel);
+
+	ret = rtt_register_sink(service->channel, &read_callback, connection);
+
+	if (ret != ERROR_OK)
+		return ret;
+
+	return ERROR_OK;
+}
+
+static int rtt_connection_closed(struct connection *connection)
+{
+	struct rtt_service *service;
+
+	service = (struct rtt_service *)connection->service->priv;
+	rtt_unregister_sink(service->channel, &read_callback, connection);
+
+	LOG_DEBUG("rtt: Connection for channel %u closed", service->channel);
+
+	return ERROR_OK;
+}
+
+static int rtt_input(struct connection *connection)
+{
+	int bytes_read;
+	unsigned char buffer[1024];
+	struct rtt_service *service;
+	size_t length;
+
+	service = (struct rtt_service *)connection->service->priv;
+	bytes_read = connection_read(connection, buffer, sizeof(buffer));
+
+	if (!bytes_read)
+		return ERROR_SERVER_REMOTE_CLOSED;
+	else if (bytes_read < 0) {
+		LOG_ERROR("error during read: %s", strerror(errno));
+		return ERROR_SERVER_REMOTE_CLOSED;
+	}
+
+	length = bytes_read;
+	rtt_write_channel(service->channel, buffer, &length);
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(handle_rtt_start_command)
+{
+	int ret;
+	struct rtt_service *service;
+
+	if (CMD_ARGC != 2)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	service = malloc(sizeof(struct rtt_service));
+
+	if (!service)
+		return ERROR_FAIL;
+
+	COMMAND_PARSE_NUMBER(uint, CMD_ARGV[1], service->channel);
+
+	ret = add_service("rtt", CMD_ARGV[0], CONNECTION_LIMIT_UNLIMITED,
+		rtt_new_connection, rtt_input, rtt_connection_closed, service, NULL);
+
+	if (ret != ERROR_OK) {
+		free(service);
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(handle_rtt_stop_command)
+{
+	if (CMD_ARGC != 1)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	remove_service("rtt", CMD_ARGV[0]);
+
+	return ERROR_OK;
+}
+
+static const struct command_registration rtt_server_subcommand_handlers[] = {
+	{
+		.name = "start",
+		.handler = handle_rtt_start_command,
+		.mode = COMMAND_ANY,
+		.help = "Start a RTT server",
+		.usage = "<port> <channel>"
+	},
+	{
+		.name = "stop",
+		.handler = handle_rtt_stop_command,
+		.mode = COMMAND_ANY,
+		.help = "Stop a RTT server",
+		.usage = "<port>"
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+static const struct command_registration rtt_server_command_handlers[] = {
+	{
+		.name = "server",
+		.mode = COMMAND_ANY,
+		.help = "RTT server",
+		.usage = "",
+		.chain = rtt_server_subcommand_handlers
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+static const struct command_registration rtt_command_handlers[] = {
+	{
+		.name = "rtt",
+		.mode = COMMAND_ANY,
+		.help = "RTT",
+		.usage = "",
+		.chain = rtt_server_command_handlers
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+int rtt_server_register_commands(struct command_context *ctx)
+{
+	return register_commands(ctx, NULL, rtt_command_handlers);
+}
diff --git a/src/server/rtt_server.h b/src/server/rtt_server.h
new file mode 100644
index 0000000..aec6f22
--- /dev/null
+++ b/src/server/rtt_server.h
@@ -0,0 +1,25 @@
+/*
+ * Copyright (C) 2016-2017 by Marc Schink <dev@zapb.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef OPENOCD_SERVER_RTT_SERVER_H
+#define OPENOCD_SERVER_RTT_SERVER_H
+
+#include <helper/command.h>
+
+int rtt_server_register_commands(struct command_context *ctx);
+
+#endif /* OPENOCD_SERVER_RTT_SERVER_H */
diff --git a/src/server/server.c b/src/server/server.c
index 4e970fa..4de9889 100644
--- a/src/server/server.c
+++ b/src/server/server.c
@@ -211,7 +211,8 @@ int add_service(char *name,
 	new_connection_handler_t new_connection_handler,
 	input_handler_t input_handler,
 	connection_closed_handler_t connection_closed_handler,
-	void *priv)
+	void *priv,
+	struct service **new_service)
 {
 	struct service *c, **p;
 	struct hostent *hp;
@@ -347,6 +348,10 @@ int add_service(char *name,
 		;
 	*p = c;
 
+	/* if new_service is not NULL, return the created service into it */
+	if (new_service)
+		*new_service = c;
+
 	return ERROR_OK;
 }
 
@@ -603,7 +608,7 @@ int server_loop(struct command_context *command_context)
 	return shutdown_openocd == SHUTDOWN_WITH_ERROR_CODE ? ERROR_FAIL : ERROR_OK;
 }
 
-void sig_handler(int sig)
+static void sig_handler(int sig)
 {
 	/* store only first signal that hits us */
 	if (shutdown_openocd == CONTINUE_MAIN_LOOP) {
diff --git a/src/server/server.h b/src/server/server.h
index ff2ada9..99f5fe2 100644
--- a/src/server/server.h
+++ b/src/server/server.h
@@ -77,7 +77,7 @@ struct service {
 int add_service(char *name, const char *port,
 		int max_connections, new_connection_handler_t new_connection_handler,
 		input_handler_t in_handler, connection_closed_handler_t close_handler,
-		void *priv);
+		void *priv, struct service **new_service);
 int remove_service(const char *name, const char *port);
 
 int server_host_os_entry(void);
diff --git a/src/server/tcl_server.c b/src/server/tcl_server.c
index 1ecb827..07213ae 100644
--- a/src/server/tcl_server.c
+++ b/src/server/tcl_server.c
@@ -285,7 +285,7 @@ int tcl_init(void)
 
 	return add_service("tcl", tcl_port, CONNECTION_LIMIT_UNLIMITED,
 		&tcl_new_connection, &tcl_input,
-		&tcl_closed, NULL);
+		&tcl_closed, NULL, NULL);
 }
 
 COMMAND_HANDLER(handle_tcl_port_command)
diff --git a/src/server/telnet_server.c b/src/server/telnet_server.c
index 0243c63..4f88d3a 100644
--- a/src/server/telnet_server.c
+++ b/src/server/telnet_server.c
@@ -538,7 +538,17 @@ static int telnet_input(struct connection *connection)
 							telnet_move_cursor(connection, 0);
 						else if (*buf_p == CTRL('E'))
 							telnet_move_cursor(connection, t_con->line_size);
-						else
+						else if (*buf_p == CTRL('K')) {         /* kill line to end */
+							if (t_con->line_cursor < t_con->line_size) {
+								/* overwrite with space, until end of line, move back */
+								for (size_t i = t_con->line_cursor; i < t_con->line_size; i++)
+									telnet_write(connection, " ", 1);
+								for (size_t i = t_con->line_cursor; i < t_con->line_size; i++)
+									telnet_write(connection, "\b", 1);
+								t_con->line[t_con->line_cursor] = '\0';
+								t_con->line_size = t_con->line_cursor;
+							}
+						} else
 							LOG_DEBUG("unhandled nonprintable: %2.2x", *buf_p);
 					}
 				}
@@ -684,7 +694,7 @@ int telnet_init(char *banner)
 
 	int ret = add_service("telnet", telnet_port, CONNECTION_LIMIT_UNLIMITED,
 		telnet_new_connection, telnet_input, telnet_connection_closed,
-		telnet_service);
+		telnet_service, NULL);
 
 	if (ret != ERROR_OK) {
 		free(telnet_service);
diff --git a/src/target/Makefile.am b/src/target/Makefile.am
index 19ba771..1d30747 100644
--- a/src/target/Makefile.am
+++ b/src/target/Makefile.am
@@ -48,7 +48,8 @@ TARGET_CORE_SRC = \
 	%D%/target_request.c \
 	%D%/testee.c \
 	%D%/semihosting_common.c \
-	%D%/smp.c
+	%D%/smp.c \
+	%D%/rtt.c
 
 ARMV4_5_SRC = \
 	%D%/armv4_5.c \
@@ -259,7 +260,8 @@ ARC_SRC = \
 	%D%/arc.h \
 	%D%/arc_cmd.h \
 	%D%/arc_jtag.h \
-	%D%/arc_mem.h
+	%D%/arc_mem.h \
+	%D%/rtt.h
 
 include %D%/openrisc/Makefile.am
 include %D%/riscv/Makefile.am
diff --git a/src/target/adi_v5_jtag.c b/src/target/adi_v5_jtag.c
index c2100eb..6dede97 100644
--- a/src/target/adi_v5_jtag.c
+++ b/src/target/adi_v5_jtag.c
@@ -145,7 +145,7 @@ struct dap_cmd {
 struct dap_cmd_pool {
 	struct list_head lh;
 	struct dap_cmd cmd;
-} dap_cmd_pool;
+};
 
 static void log_dap_cmd(const char *header, struct dap_cmd *el)
 {
diff --git a/src/target/adi_v5_swd.c b/src/target/adi_v5_swd.c
index ee30ff7..b25181e 100644
--- a/src/target/adi_v5_swd.c
+++ b/src/target/adi_v5_swd.c
@@ -118,26 +118,69 @@ static int swd_connect(struct adiv5_dap *dap)
 		}
 	}
 
-	/* Note, debugport_init() does setup too */
-	swd->switch_seq(JTAG_TO_SWD);
 
-	/* Clear link state, including the SELECT cache. */
-	dap->do_reconnect = false;
-	dap_invalidate_cache(dap);
+	int64_t timeout = timeval_ms() + 500;
 
-	swd_queue_dp_read(dap, DP_DPIDR, &dpidr);
+	do {
+		/* Note, debugport_init() does setup too */
+		swd->switch_seq(JTAG_TO_SWD);
 
-	/* force clear all sticky faults */
-	swd_clear_sticky_errors(dap);
+		/* Clear link state, including the SELECT cache. */
+		dap->do_reconnect = false;
+		dap_invalidate_cache(dap);
+
+		status = swd_queue_dp_read(dap, DP_DPIDR, &dpidr);
+		if (status == ERROR_OK) {
+			status = swd_run_inner(dap);
+			if (status == ERROR_OK)
+				break;
+		}
+
+		alive_sleep(1);
+
+	} while (timeval_ms() < timeout);
+
+	if (status != ERROR_OK) {
+		LOG_ERROR("Error connecting DP: cannot read IDR");
+		return status;
+	}
+
+	LOG_INFO("SWD DPIDR %#8.8" PRIx32, dpidr);
+
+	do {
+		dap->do_reconnect = false;
 
-	status = swd_run_inner(dap);
+		/* force clear all sticky faults */
+		swd_clear_sticky_errors(dap);
+
+		status = swd_run_inner(dap);
+		if (status != ERROR_WAIT)
+			break;
+
+		alive_sleep(10);
+
+	} while (timeval_ms() < timeout);
+
+	/* IHI 0031E B4.3.2:
+	 * "A WAIT response must not be issued to the ...
+	 * ... writes to the ABORT register"
+	 * swd_clear_sticky_errors() writes to the ABORT register only.
+	 *
+	 * Unfortunately at least Microchip SAMD51/E53/E54 returns WAIT
+	 * in a corner case. Just try if ABORT resolves the problem.
+	 */
+	if (status == ERROR_WAIT) {
+		LOG_WARNING("Connecting DP: stalled AP operation, issuing ABORT");
 
-	if (status == ERROR_OK) {
-		LOG_INFO("SWD DPIDR %#8.8" PRIx32, dpidr);
 		dap->do_reconnect = false;
+
+		swd->write_reg(swd_cmd(false,  false, DP_ABORT),
+			DAPABORT | STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR, 0);
+		status = swd_run_inner(dap);
+	}
+
+	if (status == ERROR_OK)
 		status = dap_dp_init(dap);
-	} else
-		dap->do_reconnect = true;
 
 	return status;
 }
diff --git a/src/target/arc.c b/src/target/arc.c
index e1b5764..ffe9745 100644
--- a/src/target/arc.c
+++ b/src/target/arc.c
@@ -48,6 +48,8 @@
  */
 
 
+static int arc_remove_watchpoint(struct target *target,
+	struct watchpoint *watchpoint);
 
 void arc_reg_data_type_add(struct target *target,
 		struct arc_reg_data_type *data_type)
@@ -303,7 +305,7 @@ static int arc_init_reg(struct target *target, struct reg *reg,
 	/* Initialize struct reg */
 	reg->name = reg_desc->name;
 	reg->size = 32; /* All register in ARC are 32-bit */
-	reg->value = &reg_desc->reg_value;
+	reg->value = reg_desc->reg_value;
 	reg->type = &arc_reg_type;
 	reg->arch_info = reg_desc;
 	reg->caller_save = true; /* @todo should be configurable. */
@@ -1696,6 +1698,7 @@ void arc_reset_actionpoints(struct target *target)
 	struct arc_common *arc = target_to_arc(target);
 	struct arc_actionpoint *ap_list = arc->actionpoints_list;
 	struct breakpoint *next_b;
+	struct watchpoint *next_w;
 
 	while (target->breakpoints) {
 		next_b = target->breakpoints->next;
@@ -1704,6 +1707,12 @@ void arc_reset_actionpoints(struct target *target)
 		free(target->breakpoints);
 		target->breakpoints = next_b;
 	}
+	while (target->watchpoints) {
+		next_w = target->watchpoints->next;
+		arc_remove_watchpoint(target, target->watchpoints);
+		free(target->watchpoints);
+		target->watchpoints = next_w;
+	}
 	for (unsigned int i = 0; i < arc->actionpoints_num; i++) {
 		if ((ap_list[i].used) && (ap_list[i].reg_address))
 			arc_remove_auxreg_actionpoint(target, ap_list[i].reg_address);
@@ -1800,6 +1809,159 @@ int arc_remove_auxreg_actionpoint(struct target *target, uint32_t auxreg_addr)
 	return retval;
 }
 
+
+static int arc_set_watchpoint(struct target *target,
+		struct watchpoint *watchpoint)
+{
+	unsigned int wp_num;
+	struct arc_common *arc = target_to_arc(target);
+	struct arc_actionpoint *ap_list = arc->actionpoints_list;
+
+	if (watchpoint->set) {
+		LOG_WARNING("watchpoint already set");
+		return ERROR_OK;
+	}
+
+	for (wp_num = 0; wp_num < arc->actionpoints_num; wp_num++) {
+		if (!ap_list[wp_num].used)
+			break;
+	}
+
+	if (wp_num >= arc->actionpoints_num) {
+		LOG_ERROR("No free actionpoints, maximum amount is %u",
+				arc->actionpoints_num);
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+
+	if (watchpoint->length != 4) {
+		LOG_ERROR("Only watchpoints of length 4 are supported");
+		return ERROR_TARGET_UNALIGNED_ACCESS;
+	}
+
+	int enable = AP_AC_TT_DISABLE;
+	switch (watchpoint->rw) {
+		case WPT_READ:
+			enable = AP_AC_TT_READ;
+			break;
+		case WPT_WRITE:
+			enable = AP_AC_TT_WRITE;
+			break;
+		case WPT_ACCESS:
+			enable = AP_AC_TT_READWRITE;
+			break;
+		default:
+			LOG_ERROR("BUG: watchpoint->rw neither read, write nor access");
+			return ERROR_FAIL;
+	}
+
+	int retval =  arc_configure_actionpoint(target, wp_num,
+					watchpoint->address, enable, AP_AC_AT_MEMORY_ADDR);
+
+	if (retval == ERROR_OK) {
+		watchpoint->set = wp_num + 1;
+		ap_list[wp_num].used = 1;
+		ap_list[wp_num].bp_value = watchpoint->address;
+		ap_list[wp_num].type = ARC_AP_WATCHPOINT;
+
+		LOG_DEBUG("wpid: %" PRIu32 ", wp_num %u wp_value 0x%" PRIx32,
+				watchpoint->unique_id, wp_num, ap_list[wp_num].bp_value);
+	}
+
+	return retval;
+}
+
+static int arc_unset_watchpoint(struct target *target,
+		struct watchpoint *watchpoint)
+{
+	/* get pointers to arch-specific information */
+	struct arc_common *arc = target_to_arc(target);
+	struct arc_actionpoint *ap_list = arc->actionpoints_list;
+
+	if (!watchpoint->set) {
+		LOG_WARNING("watchpoint not set");
+		return ERROR_OK;
+	}
+
+	unsigned int wp_num = watchpoint->set - 1;
+	if ((watchpoint->set == 0) || (wp_num >= arc->actionpoints_num)) {
+		LOG_DEBUG("Invalid actionpoint ID: %u in watchpoint: %" PRIu32,
+				wp_num, watchpoint->unique_id);
+		return ERROR_OK;
+	}
+
+	int retval =  arc_configure_actionpoint(target, wp_num,
+				watchpoint->address, AP_AC_TT_DISABLE, AP_AC_AT_MEMORY_ADDR);
+
+	if (retval == ERROR_OK) {
+		watchpoint->set = 0;
+		ap_list[wp_num].used = 0;
+		ap_list[wp_num].bp_value = 0;
+
+		LOG_DEBUG("wpid: %" PRIu32 " - releasing actionpoint ID: %u",
+				watchpoint->unique_id, wp_num);
+	}
+
+	return retval;
+}
+
+static int arc_add_watchpoint(struct target *target,
+	struct watchpoint *watchpoint)
+{
+	if (target->state != TARGET_HALTED) {
+		LOG_WARNING("target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	CHECK_RETVAL(arc_set_watchpoint(target, watchpoint));
+
+	return ERROR_OK;
+}
+
+static int arc_remove_watchpoint(struct target *target,
+	struct watchpoint *watchpoint)
+{
+	if (target->state != TARGET_HALTED) {
+		LOG_WARNING("target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (watchpoint->set)
+		CHECK_RETVAL(arc_unset_watchpoint(target, watchpoint));
+
+	return ERROR_OK;
+}
+
+static int arc_hit_watchpoint(struct target *target, struct watchpoint **hit_watchpoint)
+{
+	assert(target);
+	assert(hit_watchpoint);
+
+	struct arc_actionpoint *actionpoint = NULL;
+	CHECK_RETVAL(get_current_actionpoint(target, &actionpoint));
+
+	if (actionpoint != NULL) {
+		if (!actionpoint->used)
+			LOG_WARNING("Target halted by unused actionpoint.");
+
+		/* If this check fails - that is some sort of an error in OpenOCD. */
+		if (actionpoint->type != ARC_AP_WATCHPOINT)
+			LOG_WARNING("Target halted by breakpoint, but is treated as a watchpoint.");
+
+		for (struct watchpoint *watchpoint = target->watchpoints;
+				watchpoint != NULL;
+				watchpoint = watchpoint->next) {
+			if (actionpoint->bp_value == watchpoint->address) {
+				*hit_watchpoint = watchpoint;
+				LOG_DEBUG("Hit watchpoint, wpid: %" PRIu32 ", watchpoint num: %i",
+							watchpoint->unique_id, watchpoint->set - 1);
+				return ERROR_OK;
+			}
+		}
+	}
+
+	return ERROR_FAIL;
+}
+
 /* Helper function which switches core to single_step mode by
  * doing aux r/w operations.  */
 int arc_config_step(struct target *target, int enable_step)
@@ -2106,9 +2268,9 @@ struct target_type arcv2_target = {
 	.add_context_breakpoint = NULL,
 	.add_hybrid_breakpoint = NULL,
 	.remove_breakpoint = arc_remove_breakpoint,
-	.add_watchpoint = NULL,
-	.remove_watchpoint = NULL,
-	.hit_watchpoint = NULL,
+	.add_watchpoint = arc_add_watchpoint,
+	.remove_watchpoint = arc_remove_watchpoint,
+	.hit_watchpoint = arc_hit_watchpoint,
 
 	.run_algorithm = NULL,
 	.start_algorithm = NULL,
diff --git a/src/target/arm7_9_common.c b/src/target/arm7_9_common.c
index d70d273..797f61c 100644
--- a/src/target/arm7_9_common.c
+++ b/src/target/arm7_9_common.c
@@ -1391,6 +1391,11 @@ static int arm7_9_full_context(struct target *target)
 	int retval;
 	struct arm7_9_common *arm7_9 = target_to_arm7_9(target);
 	struct arm *arm = &arm7_9->arm;
+	struct {
+		uint32_t value;
+		uint8_t *reg_p;
+	} read_cache[6 * (16 + 1)];
+	int read_cache_idx = 0;
 
 	LOG_DEBUG("-");
 
@@ -1433,10 +1438,12 @@ static int arm7_9_full_context(struct target *target)
 			for (j = 0; j < 15; j++) {
 				if (!ARMV4_5_CORE_REG_MODE(arm->core_cache,
 						armv4_5_number_to_mode(i), j).valid) {
-					reg_p[j] = (uint32_t *)ARMV4_5_CORE_REG_MODE(
+					read_cache[read_cache_idx].reg_p = ARMV4_5_CORE_REG_MODE(
 							arm->core_cache,
 							armv4_5_number_to_mode(i),
 							j).value;
+					reg_p[j] = &read_cache[read_cache_idx].value;
+					read_cache_idx++;
 					mask |= 1 << j;
 					ARMV4_5_CORE_REG_MODE(arm->core_cache,
 						armv4_5_number_to_mode(i),
@@ -1454,9 +1461,10 @@ static int arm7_9_full_context(struct target *target)
 			/* check if the PSR has to be read */
 			if (!ARMV4_5_CORE_REG_MODE(arm->core_cache, armv4_5_number_to_mode(i),
 					16).valid) {
-				arm7_9->read_xpsr(target,
-					(uint32_t *)ARMV4_5_CORE_REG_MODE(arm->core_cache,
-						armv4_5_number_to_mode(i), 16).value, 1);
+				read_cache[read_cache_idx].reg_p = ARMV4_5_CORE_REG_MODE(arm->core_cache,
+					armv4_5_number_to_mode(i), 16).value;
+				arm7_9->read_xpsr(target, &read_cache[read_cache_idx].value, 1);
+				read_cache_idx++;
 				ARMV4_5_CORE_REG_MODE(arm->core_cache, armv4_5_number_to_mode(i),
 					16).valid = true;
 				ARMV4_5_CORE_REG_MODE(arm->core_cache, armv4_5_number_to_mode(i),
@@ -1472,6 +1480,14 @@ static int arm7_9_full_context(struct target *target)
 	retval = jtag_execute_queue();
 	if (retval != ERROR_OK)
 		return retval;
+	/*
+	 * FIXME: regs in cache should be tagged as 'valid' only now,
+	 * not before the jtag_execute_queue()
+	 */
+	while (read_cache_idx) {
+		read_cache_idx--;
+		buf_set_u32(read_cache[read_cache_idx].reg_p, 0, 32, read_cache[read_cache_idx].value);
+	}
 	return ERROR_OK;
 }
 
diff --git a/src/target/arm7tdmi.c b/src/target/arm7tdmi.c
index 01685ab..10263f4 100644
--- a/src/target/arm7tdmi.c
+++ b/src/target/arm7tdmi.c
@@ -115,11 +115,9 @@ static inline int arm7tdmi_clock_out_inner(struct arm_jtag *jtag_info, uint32_t
 
 /* put an instruction in the ARM7TDMI pipeline or write the data bus,
  * and optionally read data
- *
- * FIXME remove the unused "deprecated" parameter
  */
 static inline int arm7tdmi_clock_out(struct arm_jtag *jtag_info,
-		uint32_t out, uint32_t *deprecated, int breakpoint)
+		uint32_t out, int breakpoint)
 {
 	int retval;
 	retval = arm_jtag_scann(jtag_info, 0x1, TAP_DRPAUSE);
@@ -246,35 +244,35 @@ static void arm7tdmi_change_to_arm(struct target *target,
 	 * to allow common handling of ARM and THUMB debugging */
 
 	/* fetch STR r0, [r0] */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_STR(0, 0), NULL, 0);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_STR(0, 0), 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0);
 	/* nothing fetched, STR r0, [r0] in Execute (2) */
 	arm7tdmi_clock_data_in(jtag_info, r0);
 
 	/* MOV r0, r15 fetched, STR in Decode */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_MOV(0, 15), NULL, 0);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_STR(0, 0), NULL, 0);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_MOV(0, 15), 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_STR(0, 0), 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0);
 	/* nothing fetched, STR r0, [r0] in Execute (2) */
 	arm7tdmi_clock_data_in(jtag_info, pc);
 
 	/* use pc-relative LDR to clear r0[1:0] (for switch to ARM mode) */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_LDR_PCREL(0), NULL, 0);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_LDR_PCREL(0), 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0);
 	/* nothing fetched, data for LDR r0, [PC, #0] */
-	arm7tdmi_clock_out(jtag_info, 0x0, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, 0x0, 0);
 	/* nothing fetched, data from previous cycle is written to register */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0);
 
 	/* fetch BX */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_BX(0), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_BX(0), 0);
 	/* NOP fetched, BX in Decode, MOV in Execute */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0);
 	/* NOP fetched, BX in Execute (1) */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0);
 
 	jtag_execute_queue();
 
@@ -301,12 +299,12 @@ static void arm7tdmi_read_core_regs(struct target *target,
 	/* STMIA r0-15, [r0] at debug speed
 	 * register values will start to appear on 4th DCLK
 	 */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_STMIA(0, mask & 0xffff, 0, 0), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_STMIA(0, mask & 0xffff, 0, 0), 0);
 
 	/* fetch NOP, STM in DECODE stage */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 	/* fetch NOP, STM in EXECUTE stage (1st cycle) */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 
 	for (i = 0; i <= 15; i++) {
 		if (mask & (1 << i))
@@ -329,12 +327,12 @@ static void arm7tdmi_read_core_regs_target_buffer(struct target *target,
 	/* STMIA r0-15, [r0] at debug speed
 	 * register values will start to appear on 4th DCLK
 	 */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_STMIA(0, mask & 0xffff, 0, 0), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_STMIA(0, mask & 0xffff, 0, 0), 0);
 
 	/* fetch NOP, STM in DECODE stage */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 	/* fetch NOP, STM in EXECUTE stage (1st cycle) */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 
 	for (i = 0; i <= 15; i++) {
 		/* nothing fetched, STM still in EXECUTE (1 + i cycle), read databus */
@@ -360,14 +358,14 @@ static void arm7tdmi_read_xpsr(struct target *target, uint32_t *xpsr, int spsr)
 	struct arm_jtag *jtag_info = &arm7_9->jtag_info;
 
 	/* MRS r0, cpsr */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_MRS(0, spsr & 1), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_MRS(0, spsr & 1), 0);
 
 	/* STR r0, [r15] */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_STR(0, 15), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_STR(0, 15), 0);
 	/* fetch NOP, STR in DECODE stage */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 	/* fetch NOP, STR in EXECUTE stage (1st cycle) */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 	/* nothing fetched, STR still in EXECUTE (2nd cycle) */
 	arm7tdmi_clock_data_in(jtag_info, xpsr);
 }
@@ -380,25 +378,25 @@ static void arm7tdmi_write_xpsr(struct target *target, uint32_t xpsr, int spsr)
 	LOG_DEBUG("xpsr: %8.8" PRIx32 ", spsr: %i", xpsr, spsr);
 
 	/* MSR1 fetched */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM(xpsr & 0xff, 0, 1, spsr), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM(xpsr & 0xff, 0, 1, spsr), 0);
 	/* MSR2 fetched, MSR1 in DECODE */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM((xpsr & 0xff00) >> 8, 0xc, 2, spsr), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM((xpsr & 0xff00) >> 8, 0xc, 2, spsr), 0);
 	/* MSR3 fetched, MSR1 in EXECUTE (1), MSR2 in DECODE */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM((xpsr & 0xff0000) >> 16, 0x8, 4, spsr), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM((xpsr & 0xff0000) >> 16, 0x8, 4, spsr), 0);
 	/* nothing fetched, MSR1 in EXECUTE (2) */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 	/* MSR4 fetched, MSR2 in EXECUTE (1), MSR3 in DECODE */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM((xpsr & 0xff000000) >> 24, 0x4, 8, spsr), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM((xpsr & 0xff000000) >> 24, 0x4, 8, spsr), 0);
 	/* nothing fetched, MSR2 in EXECUTE (2) */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 	/* NOP fetched, MSR3 in EXECUTE (1), MSR4 in DECODE */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 	/* nothing fetched, MSR3 in EXECUTE (2) */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 	/* NOP fetched, MSR4 in EXECUTE (1) */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 	/* nothing fetched, MSR4 in EXECUTE (2) */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 }
 
 static void arm7tdmi_write_xpsr_im8(struct target *target,
@@ -410,13 +408,13 @@ static void arm7tdmi_write_xpsr_im8(struct target *target,
 	LOG_DEBUG("xpsr_im: %2.2x, rot: %i, spsr: %i", xpsr_im, rot, spsr);
 
 	/* MSR fetched */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM(xpsr_im, rot, 1, spsr), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM(xpsr_im, rot, 1, spsr), 0);
 	/* NOP fetched, MSR in DECODE */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 	/* NOP fetched, MSR in EXECUTE (1) */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 	/* nothing fetched, MSR in EXECUTE (2) */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 }
 
 static void arm7tdmi_write_core_regs(struct target *target,
@@ -429,7 +427,7 @@ static void arm7tdmi_write_core_regs(struct target *target,
 	/* LDMIA r0-15, [r0] at debug speed
 	* register values will start to appear on 4th DCLK
 	*/
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, mask & 0xffff, 0, 0), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, mask & 0xffff, 0, 0), 0);
 
 	/* fetch NOP, LDM in DECODE stage */
 	arm7tdmi_clock_out_inner(jtag_info, ARMV4_5_NOP, 0);
@@ -450,9 +448,9 @@ static void arm7tdmi_load_word_regs(struct target *target, uint32_t mask)
 	struct arm_jtag *jtag_info = &arm7_9->jtag_info;
 
 	/* put system-speed load-multiple into the pipeline */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 1);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, mask & 0xffff, 0, 1), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 1);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, mask & 0xffff, 0, 1), 0);
 }
 
 static void arm7tdmi_load_hword_reg(struct target *target, int num)
@@ -461,9 +459,9 @@ static void arm7tdmi_load_hword_reg(struct target *target, int num)
 	struct arm_jtag *jtag_info = &arm7_9->jtag_info;
 
 	/* put system-speed load half-word into the pipeline */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 1);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_LDRH_IP(num, 0), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 1);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_LDRH_IP(num, 0), 0);
 }
 
 static void arm7tdmi_load_byte_reg(struct target *target, int num)
@@ -472,9 +470,9 @@ static void arm7tdmi_load_byte_reg(struct target *target, int num)
 	struct arm_jtag *jtag_info = &arm7_9->jtag_info;
 
 	/* put system-speed load byte into the pipeline */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 1);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_LDRB_IP(num, 0), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 1);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_LDRB_IP(num, 0), 0);
 }
 
 static void arm7tdmi_store_word_regs(struct target *target, uint32_t mask)
@@ -483,9 +481,9 @@ static void arm7tdmi_store_word_regs(struct target *target, uint32_t mask)
 	struct arm_jtag *jtag_info = &arm7_9->jtag_info;
 
 	/* put system-speed store-multiple into the pipeline */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 1);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_STMIA(0, mask, 0, 1), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 1);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_STMIA(0, mask, 0, 1), 0);
 }
 
 static void arm7tdmi_store_hword_reg(struct target *target, int num)
@@ -494,9 +492,9 @@ static void arm7tdmi_store_hword_reg(struct target *target, int num)
 	struct arm_jtag *jtag_info = &arm7_9->jtag_info;
 
 	/* put system-speed store half-word into the pipeline */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 1);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_STRH_IP(num, 0), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 1);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_STRH_IP(num, 0), 0);
 }
 
 static void arm7tdmi_store_byte_reg(struct target *target, int num)
@@ -505,9 +503,9 @@ static void arm7tdmi_store_byte_reg(struct target *target, int num)
 	struct arm_jtag *jtag_info = &arm7_9->jtag_info;
 
 	/* put system-speed store byte into the pipeline */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 1);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_STRB_IP(num, 0), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 1);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_STRB_IP(num, 0), 0);
 }
 
 static void arm7tdmi_write_pc(struct target *target, uint32_t pc)
@@ -518,7 +516,7 @@ static void arm7tdmi_write_pc(struct target *target, uint32_t pc)
 	/* LDMIA r0-15, [r0] at debug speed
 	 * register values will start to appear on 4th DCLK
 	 */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, 0x8000, 0, 0), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, 0x8000, 0, 0), 0);
 	/* fetch NOP, LDM in DECODE stage */
 	arm7tdmi_clock_out_inner(jtag_info, ARMV4_5_NOP, 0);
 	/* fetch NOP, LDM in EXECUTE stage (1st cycle) */
@@ -540,7 +538,7 @@ static void arm7tdmi_branch_resume(struct target *target)
 	struct arm7_9_common *arm7_9 = target_to_arm7_9(target);
 	struct arm_jtag *jtag_info = &arm7_9->jtag_info;
 
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 1);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 1);
 	arm7tdmi_clock_out_inner(jtag_info, ARMV4_5_B(0xfffffa, 0), 0);
 }
 
@@ -556,53 +554,52 @@ static void arm7tdmi_branch_resume_thumb(struct target *target)
 	/* LDMIA r0, [r0] at debug speed
 	 * register values will start to appear on 4th DCLK
 	 */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, 0x1, 0, 0), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, 0x1, 0, 0), 0);
 
 	/* fetch NOP, LDM in DECODE stage */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 	/* fetch NOP, LDM in EXECUTE stage (1st cycle) */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 	/* nothing fetched, LDM in EXECUTE stage (2nd cycle) */
-	arm7tdmi_clock_out(jtag_info,
-			buf_get_u32(arm->pc->value, 0, 32) | 1, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, buf_get_u32(arm->pc->value, 0, 32) | 1, 0);
 	/* nothing fetched, LDM in EXECUTE stage (3rd cycle) */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 
 	/* Branch and eXchange */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_BX(0), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_BX(0), 0);
 
 	embeddedice_read_reg(dbg_stat);
 
 	/* fetch NOP, BX in DECODE stage */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 
 	/* target is now in Thumb state */
 	embeddedice_read_reg(dbg_stat);
 
 	/* fetch NOP, BX in EXECUTE stage (1st cycle) */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0);
 
 	/* target is now in Thumb state */
 	embeddedice_read_reg(dbg_stat);
 
 	/* load r0 value */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_LDR_PCREL(0), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_LDR_PCREL(0), 0);
 	/* fetch NOP, LDR in Decode */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0);
 	/* fetch NOP, LDR in Execute */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0);
 	/* nothing fetched, LDR in EXECUTE stage (2nd cycle) */
-	arm7tdmi_clock_out(jtag_info, buf_get_u32(arm->core_cache->reg_list[0].value, 0, 32), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, buf_get_u32(arm->core_cache->reg_list[0].value, 0, 32), 0);
 	/* nothing fetched, LDR in EXECUTE stage (3rd cycle) */
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0);
 
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0);
 
 	embeddedice_read_reg(dbg_stat);
 
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 1);
-	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_B(0x7f8), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 1);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_B(0x7f8), 0);
 }
 
 static void arm7tdmi_build_reg_cache(struct target *target)
diff --git a/src/target/arm926ejs.c b/src/target/arm926ejs.c
index 95a4f7c..21fd689 100644
--- a/src/target/arm926ejs.c
+++ b/src/target/arm926ejs.c
@@ -723,7 +723,7 @@ static int arm926ejs_target_create(struct target *target, Jim_Interp *interp)
 	return arm926ejs_init_arch_info(target, arm926ejs, target->tap);
 }
 
-void arm926ejs_deinit_target(struct target *target)
+static void arm926ejs_deinit_target(struct target *target)
 {
 	struct arm *arm = target_to_arm(target);
 	struct arm926ejs_common *arm926ejs = target_to_arm926(target);
diff --git a/src/target/arm946e.c b/src/target/arm946e.c
index 8754c86..036e8ba 100644
--- a/src/target/arm946e.c
+++ b/src/target/arm946e.c
@@ -51,8 +51,8 @@
  */
 static uint8_t arm946e_preserve_cache;
 
-int arm946e_post_debug_entry(struct target *target);
-void arm946e_pre_restore_context(struct target *target);
+static int arm946e_post_debug_entry(struct target *target);
+static void arm946e_pre_restore_context(struct target *target);
 static int arm946e_read_cp15(struct target *target, int reg_addr, uint32_t *value);
 
 int arm946e_init_arch_info(struct target *target,
@@ -250,7 +250,7 @@ static uint32_t arm946e_cp15_get_csize(struct target *target, int idsel)
 	return csize ? 1 << (12 + (csize-3)) : 0;
 }
 
-uint32_t arm946e_invalidate_whole_dcache(struct target *target)
+static uint32_t arm946e_invalidate_whole_dcache(struct target *target)
 {
 	uint32_t csize = arm946e_cp15_get_csize(target, GET_DCACHE_SIZE);
 	if (csize == 0)
@@ -306,7 +306,7 @@ uint32_t arm946e_invalidate_whole_dcache(struct target *target)
 	return ERROR_OK;
 }
 
-uint32_t arm946e_invalidate_whole_icache(struct target *target)
+static uint32_t arm946e_invalidate_whole_icache(struct target *target)
 {
 	/* Check cache presence before flushing - avoid undefined behavior */
 	uint32_t csize = arm946e_cp15_get_csize(target, GET_ICACHE_SIZE);
@@ -327,7 +327,7 @@ uint32_t arm946e_invalidate_whole_icache(struct target *target)
 	return ERROR_OK;
 }
 
-int arm946e_post_debug_entry(struct target *target)
+static int arm946e_post_debug_entry(struct target *target)
 {
 	uint32_t ctr_reg = 0x0;
 	uint32_t retval = ERROR_OK;
@@ -368,7 +368,7 @@ int arm946e_post_debug_entry(struct target *target)
 	return ERROR_OK;
 }
 
-void arm946e_pre_restore_context(struct target *target)
+static void arm946e_pre_restore_context(struct target *target)
 {
 	uint32_t ctr_reg = 0x0;
 	uint32_t retval;
@@ -393,7 +393,7 @@ void arm946e_pre_restore_context(struct target *target)
 	}	/* if preserve_cache */
 }
 
-uint32_t arm946e_invalidate_dcache(struct target *target, uint32_t address,
+static uint32_t arm946e_invalidate_dcache(struct target *target, uint32_t address,
 	uint32_t size, uint32_t count)
 {
 	uint32_t cur_addr = 0x0;
@@ -458,7 +458,7 @@ uint32_t arm946e_invalidate_dcache(struct target *target, uint32_t address,
 	return ERROR_OK;
 }
 
-uint32_t arm946e_invalidate_icache(struct target *target, uint32_t address,
+static uint32_t arm946e_invalidate_icache(struct target *target, uint32_t address,
 	uint32_t size, uint32_t count)
 {
 	uint32_t cur_addr = 0x0;
@@ -509,7 +509,7 @@ uint32_t arm946e_invalidate_icache(struct target *target, uint32_t address,
 }
 
 /** Writes a buffer, in the specified word size, with current MMU settings. */
-int arm946e_write_memory(struct target *target, target_addr_t address,
+static int arm946e_write_memory(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	int retval;
@@ -557,7 +557,7 @@ int arm946e_write_memory(struct target *target, target_addr_t address,
 
 }
 
-int arm946e_read_memory(struct target *target, target_addr_t address,
+static int arm946e_read_memory(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	int retval;
diff --git a/src/target/arm_adi_v5.c b/src/target/arm_adi_v5.c
index a09e269..59bb186 100644
--- a/src/target/arm_adi_v5.c
+++ b/src/target/arm_adi_v5.c
@@ -652,35 +652,22 @@ int dap_dp_init(struct adiv5_dap *dap)
 
 	LOG_DEBUG("%s", adiv5_dap_name(dap));
 
+	dap->do_reconnect = false;
 	dap_invalidate_cache(dap);
 
 	/*
 	 * Early initialize dap->dp_ctrl_stat.
-	 * In jtag mode only, if the following atomic reads fail and set the
-	 * sticky error, it will trigger the clearing of the sticky. Without this
-	 * initialization system and debug power would be disabled while clearing
-	 * the sticky error bit.
+	 * In jtag mode only, if the following queue run (in dap_dp_poll_register)
+	 * fails and sets the sticky error, it will trigger the clearing
+	 * of the sticky. Without this initialization system and debug power
+	 * would be disabled while clearing the sticky error bit.
 	 */
 	dap->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ;
 
-	for (size_t i = 0; i < 30; i++) {
-		/* DP initialization */
-
-		retval = dap_dp_read_atomic(dap, DP_CTRL_STAT, NULL);
-		if (retval == ERROR_OK)
-			break;
-	}
-
 	/*
 	 * This write operation clears the sticky error bit in jtag mode only and
 	 * is ignored in swd mode. It also powers-up system and debug domains in
 	 * both jtag and swd modes, if not done before.
-	 * Actually we do not need to clear the sticky error here because it has
-	 * been already cleared (if it was set) in the previous atomic read. This
-	 * write could be removed, but this initial part of dap_dp_init() is the
-	 * result of years of fine tuning and there are strong concerns about any
-	 * unnecessary code change. It doesn't harm, so let's keep it here and
-	 * preserve the historical sequence of read/write operations!
 	 */
 	retval = dap_queue_dp_write(dap, DP_CTRL_STAT, dap->dp_ctrl_stat | SSTICKYERR);
 	if (retval != ERROR_OK)
@@ -732,6 +719,35 @@ int dap_dp_init(struct adiv5_dap *dap)
 }
 
 /**
+ * Initialize a DAP or do reconnect if DAP is not accessible.
+ *
+ * @param dap The DAP being initialized.
+ */
+int dap_dp_init_or_reconnect(struct adiv5_dap *dap)
+{
+	LOG_DEBUG("%s", adiv5_dap_name(dap));
+
+	/*
+	 * Early initialize dap->dp_ctrl_stat.
+	 * In jtag mode only, if the following atomic reads fail and set the
+	 * sticky error, it will trigger the clearing of the sticky. Without this
+	 * initialization system and debug power would be disabled while clearing
+	 * the sticky error bit.
+	 */
+	dap->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ;
+
+	dap->do_reconnect = false;
+
+	dap_dp_read_atomic(dap, DP_CTRL_STAT, NULL);
+	if (dap->do_reconnect) {
+		/* dap connect calls dap_dp_init() after transport dependent initialization */
+		return dap->ops->connect(dap);
+	} else {
+		return dap_dp_init(dap);
+	}
+}
+
+/**
  * Initialize a DAP.  This sets up the power domains, prepares the DP
  * for further use, and arranges to use AP #0 for all AP operations
  * until dap_ap-select() changes that policy.
@@ -1479,15 +1495,118 @@ int dap_info_command(struct command_invocation *cmd,
 
 enum adiv5_cfg_param {
 	CFG_DAP,
-	CFG_AP_NUM
+	CFG_AP_NUM,
+	CFG_BASEADDR,
+	CFG_CTIBASE, /* DEPRECATED */
 };
 
 static const Jim_Nvp nvp_config_opts[] = {
-	{ .name = "-dap",    .value = CFG_DAP },
-	{ .name = "-ap-num", .value = CFG_AP_NUM },
+	{ .name = "-dap",       .value = CFG_DAP },
+	{ .name = "-ap-num",    .value = CFG_AP_NUM },
+	{ .name = "-baseaddr",  .value = CFG_BASEADDR },
+	{ .name = "-ctibase",   .value = CFG_CTIBASE }, /* DEPRECATED */
 	{ .name = NULL, .value = -1 }
 };
 
+static int adiv5_jim_spot_configure(Jim_GetOptInfo *goi,
+		struct adiv5_dap **dap_p, int *ap_num_p, uint32_t *base_p)
+{
+	if (!goi->argc)
+		return JIM_OK;
+
+	Jim_SetEmptyResult(goi->interp);
+
+	Jim_Nvp *n;
+	int e = Jim_Nvp_name2value_obj(goi->interp, nvp_config_opts,
+				goi->argv[0], &n);
+	if (e != JIM_OK)
+		return JIM_CONTINUE;
+
+	/* base_p can be NULL, then '-baseaddr' option is treated as unknown */
+	if (!base_p && (n->value == CFG_BASEADDR || n->value == CFG_CTIBASE))
+		return JIM_CONTINUE;
+
+	e = Jim_GetOpt_Obj(goi, NULL);
+	if (e != JIM_OK)
+		return e;
+
+	switch (n->value) {
+	case CFG_DAP:
+		if (goi->isconfigure) {
+			Jim_Obj *o_t;
+			struct adiv5_dap *dap;
+			e = Jim_GetOpt_Obj(goi, &o_t);
+			if (e != JIM_OK)
+				return e;
+			dap = dap_instance_by_jim_obj(goi->interp, o_t);
+			if (!dap) {
+				Jim_SetResultString(goi->interp, "DAP name invalid!", -1);
+				return JIM_ERR;
+			}
+			if (*dap_p && *dap_p != dap) {
+				Jim_SetResultString(goi->interp,
+					"DAP assignment cannot be changed!", -1);
+				return JIM_ERR;
+			}
+			*dap_p = dap;
+		} else {
+			if (goi->argc)
+				goto err_no_param;
+			if (!*dap_p) {
+				Jim_SetResultString(goi->interp, "DAP not configured", -1);
+				return JIM_ERR;
+			}
+			Jim_SetResultString(goi->interp, adiv5_dap_name(*dap_p), -1);
+		}
+		break;
+
+	case CFG_AP_NUM:
+		if (goi->isconfigure) {
+			jim_wide ap_num;
+			e = Jim_GetOpt_Wide(goi, &ap_num);
+			if (e != JIM_OK)
+				return e;
+			if (ap_num < 0 || ap_num > DP_APSEL_MAX) {
+				Jim_SetResultString(goi->interp, "Invalid AP number!", -1);
+				return JIM_ERR;
+			}
+			*ap_num_p = ap_num;
+		} else {
+			if (goi->argc)
+				goto err_no_param;
+			if (*ap_num_p == DP_APSEL_INVALID) {
+				Jim_SetResultString(goi->interp, "AP number not configured", -1);
+				return JIM_ERR;
+			}
+			Jim_SetResult(goi->interp, Jim_NewIntObj(goi->interp, *ap_num_p));
+		}
+		break;
+
+	case CFG_CTIBASE:
+		LOG_WARNING("DEPRECATED! use \'-baseaddr' not \'-ctibase\'");
+		/* fall through */
+	case CFG_BASEADDR:
+		if (goi->isconfigure) {
+			jim_wide base;
+			e = Jim_GetOpt_Wide(goi, &base);
+			if (e != JIM_OK)
+				return e;
+			*base_p = (uint32_t)base;
+		} else {
+			if (goi->argc)
+				goto err_no_param;
+			Jim_SetResult(goi->interp, Jim_NewIntObj(goi->interp, *base_p));
+		}
+		break;
+	};
+
+	return JIM_OK;
+
+err_no_param:
+	Jim_WrongNumArgs(goi->interp, goi->argc, goi->argv, "NO PARAMS");
+	return JIM_ERR;
+}
+
 int adiv5_jim_configure(struct target *target, Jim_GetOptInfo *goi)
 {
 	struct adiv5_private_config *pc;
@@ -1502,90 +1621,19 @@ int adiv5_jim_configure(struct target *target, Jim_GetOptInfo *goi)
 
 	target->has_dap = true;
 
-	if (goi->argc > 0) {
-		Jim_Nvp *n;
-
-		Jim_SetEmptyResult(goi->interp);
-
-		/* check first if topmost item is for us */
-		e = Jim_Nvp_name2value_obj(goi->interp, nvp_config_opts,
-								   goi->argv[0], &n);
-		if (e != JIM_OK)
-			return JIM_CONTINUE;
-
-		e = Jim_GetOpt_Obj(goi, NULL);
-		if (e != JIM_OK)
-			return e;
-
-		switch (n->value) {
-		case CFG_DAP:
-			if (goi->isconfigure) {
-				Jim_Obj *o_t;
-				struct adiv5_dap *dap;
-				e = Jim_GetOpt_Obj(goi, &o_t);
-				if (e != JIM_OK)
-					return e;
-				dap = dap_instance_by_jim_obj(goi->interp, o_t);
-				if (dap == NULL) {
-					Jim_SetResultString(goi->interp, "DAP name invalid!", -1);
-					return JIM_ERR;
-				}
-				if (pc->dap != NULL && pc->dap != dap) {
-					Jim_SetResultString(goi->interp,
-						"DAP assignment cannot be changed after target was created!", -1);
-					return JIM_ERR;
-				}
-				if (target->tap_configured) {
-					Jim_SetResultString(goi->interp,
-						"-chain-position and -dap configparams are mutually exclusive!", -1);
-					return JIM_ERR;
-				}
-				pc->dap = dap;
-				target->tap = dap->tap;
-				target->dap_configured = true;
-			} else {
-				if (goi->argc != 0) {
-					Jim_WrongNumArgs(goi->interp,
-										goi->argc, goi->argv,
-					"NO PARAMS");
-					return JIM_ERR;
-				}
-
-				if (pc->dap == NULL) {
-					Jim_SetResultString(goi->interp, "DAP not configured", -1);
-					return JIM_ERR;
-				}
-				Jim_SetResultString(goi->interp, adiv5_dap_name(pc->dap), -1);
-			}
-			break;
+	e = adiv5_jim_spot_configure(goi, &pc->dap, &pc->ap_num, NULL);
+	if (e != JIM_OK)
+		return e;
 
-		case CFG_AP_NUM:
-			if (goi->isconfigure) {
-				jim_wide ap_num;
-				e = Jim_GetOpt_Wide(goi, &ap_num);
-				if (e != JIM_OK)
-					return e;
-				if (ap_num < 0 || ap_num > DP_APSEL_MAX) {
-					Jim_SetResultString(goi->interp, "Invalid AP number!", -1);
-					return JIM_ERR;
-				}
-				pc->ap_num = ap_num;
-			} else {
-				if (goi->argc != 0) {
-					Jim_WrongNumArgs(goi->interp,
-									 goi->argc, goi->argv,
-					  "NO PARAMS");
-					return JIM_ERR;
-				}
-
-				if (pc->ap_num == DP_APSEL_INVALID) {
-					Jim_SetResultString(goi->interp, "AP number not configured", -1);
-					return JIM_ERR;
-				}
-				Jim_SetResult(goi->interp, Jim_NewIntObj(goi->interp, pc->ap_num));
-			}
-			break;
+	if (pc->dap && !target->dap_configured) {
+		if (target->tap_configured) {
+			pc->dap = NULL;
+			Jim_SetResultString(goi->interp,
+				"-chain-position and -dap configparams are mutually exclusive!", -1);
+			return JIM_ERR;
 		}
+		target->tap = pc->dap->tap;
+		target->dap_configured = true;
 	}
 
 	return JIM_OK;
@@ -1602,6 +1650,19 @@ int adiv5_verify_config(struct adiv5_private_config *pc)
 	return ERROR_OK;
 }
 
+int adiv5_jim_mem_ap_spot_configure(struct adiv5_mem_ap_spot *cfg,
+		Jim_GetOptInfo *goi)
+{
+	return adiv5_jim_spot_configure(goi, &cfg->dap, &cfg->ap_num, &cfg->base);
+}
+
+int adiv5_mem_ap_spot_init(struct adiv5_mem_ap_spot *p)
+{
+	p->dap = NULL;
+	p->ap_num = DP_APSEL_INVALID;
+	p->base = 0;
+	return ERROR_OK;
+}
 
 COMMAND_HANDLER(handle_dap_info_command)
 {
diff --git a/src/target/arm_adi_v5.h b/src/target/arm_adi_v5.h
index ea71551..8edfaa8 100644
--- a/src/target/arm_adi_v5.h
+++ b/src/target/arm_adi_v5.h
@@ -550,6 +550,7 @@ int mem_ap_write_buf_noincr(struct adiv5_ap *ap,
 
 /* Initialisation of the debug system, power domains and registers */
 int dap_dp_init(struct adiv5_dap *dap);
+int dap_dp_init_or_reconnect(struct adiv5_dap *dap);
 int mem_ap_init(struct adiv5_ap *ap);
 
 /* Invalidate cached DP select and cached TAR and CSW of all APs */
@@ -601,4 +602,14 @@ struct adiv5_private_config {
 extern int adiv5_verify_config(struct adiv5_private_config *pc);
 extern int adiv5_jim_configure(struct target *target, Jim_GetOptInfo *goi);
 
+struct adiv5_mem_ap_spot {
+	struct adiv5_dap *dap;
+	int ap_num;
+	uint32_t base;
+};
+
+extern int adiv5_mem_ap_spot_init(struct adiv5_mem_ap_spot *p);
+extern int adiv5_jim_mem_ap_spot_configure(struct adiv5_mem_ap_spot *cfg,
+		Jim_GetOptInfo *goi);
+
 #endif /* OPENOCD_TARGET_ARM_ADI_V5_H */
diff --git a/src/target/arm_cti.c b/src/target/arm_cti.c
index 579bacb..689e9df 100644
--- a/src/target/arm_cti.c
+++ b/src/target/arm_cti.c
@@ -31,28 +31,21 @@
 #include "helper/command.h"
 
 struct arm_cti {
-	target_addr_t base;
-	struct adiv5_ap *ap;
-};
-
-struct arm_cti_object {
 	struct list_head lh;
-	struct arm_cti cti;
-	int ap_num;
 	char *name;
+	struct adiv5_mem_ap_spot spot;
 };
 
 static LIST_HEAD(all_cti);
 
 const char *arm_cti_name(struct arm_cti *self)
 {
-	struct arm_cti_object *obj = container_of(self, struct arm_cti_object, cti);
-	return obj->name;
+	return self->name;
 }
 
 struct arm_cti *cti_instance_by_jim_obj(Jim_Interp *interp, Jim_Obj *o)
 {
-	struct arm_cti_object *obj = NULL;
+	struct arm_cti *obj = NULL;
 	const char *name;
 	bool found = false;
 
@@ -66,16 +59,17 @@ struct arm_cti *cti_instance_by_jim_obj(Jim_Interp *interp, Jim_Obj *o)
 	}
 
 	if (found)
-		return &obj->cti;
+		return obj;
 	return NULL;
 }
 
 static int arm_cti_mod_reg_bits(struct arm_cti *self, unsigned int reg, uint32_t mask, uint32_t value)
 {
+	struct adiv5_ap *ap = dap_ap(self->spot.dap, self->spot.ap_num);
 	uint32_t tmp;
 
 	/* Read register */
-	int retval = mem_ap_read_atomic_u32(self->ap, self->base + reg, &tmp);
+	int retval = mem_ap_read_atomic_u32(ap, self->spot.base + reg, &tmp);
 	if (ERROR_OK != retval)
 		return retval;
 
@@ -85,26 +79,28 @@ static int arm_cti_mod_reg_bits(struct arm_cti *self, unsigned int reg, uint32_t
 	tmp |= value & mask;
 
 	/* write new value */
-	return mem_ap_write_atomic_u32(self->ap, self->base + reg, tmp);
+	return mem_ap_write_atomic_u32(ap, self->spot.base + reg, tmp);
 }
 
 int arm_cti_enable(struct arm_cti *self, bool enable)
 {
+	struct adiv5_ap *ap = dap_ap(self->spot.dap, self->spot.ap_num);
 	uint32_t val = enable ? 1 : 0;
 
-	return mem_ap_write_atomic_u32(self->ap, self->base + CTI_CTR, val);
+	return mem_ap_write_atomic_u32(ap, self->spot.base + CTI_CTR, val);
 }
 
 int arm_cti_ack_events(struct arm_cti *self, uint32_t event)
 {
+	struct adiv5_ap *ap = dap_ap(self->spot.dap, self->spot.ap_num);
 	int retval;
 	uint32_t tmp;
 
-	retval = mem_ap_write_atomic_u32(self->ap, self->base + CTI_INACK, event);
+	retval = mem_ap_write_atomic_u32(ap, self->spot.base + CTI_INACK, event);
 	if (retval == ERROR_OK) {
 		int64_t then = timeval_ms();
 		for (;;) {
-			retval = mem_ap_read_atomic_u32(self->ap, self->base + CTI_TROUT_STATUS, &tmp);
+			retval = mem_ap_read_atomic_u32(ap, self->spot.base + CTI_TROUT_STATUS, &tmp);
 			if (retval != ERROR_OK)
 				break;
 			if ((tmp & event) == 0)
@@ -138,15 +134,19 @@ int arm_cti_ungate_channel(struct arm_cti *self, uint32_t channel)
 
 int arm_cti_write_reg(struct arm_cti *self, unsigned int reg, uint32_t value)
 {
-	return mem_ap_write_atomic_u32(self->ap, self->base + reg, value);
+	struct adiv5_ap *ap = dap_ap(self->spot.dap, self->spot.ap_num);
+
+	return mem_ap_write_atomic_u32(ap, self->spot.base + reg, value);
 }
 
 int arm_cti_read_reg(struct arm_cti *self, unsigned int reg, uint32_t *p_value)
 {
+	struct adiv5_ap *ap = dap_ap(self->spot.dap, self->spot.ap_num);
+
 	if (p_value == NULL)
 		return ERROR_COMMAND_ARGUMENT_INVALID;
 
-	return mem_ap_read_atomic_u32(self->ap, self->base + reg, p_value);
+	return mem_ap_read_atomic_u32(ap, self->spot.base + reg, p_value);
 }
 
 int arm_cti_pulse_channel(struct arm_cti *self, uint32_t channel)
@@ -225,7 +225,7 @@ static int cti_find_reg_offset(const char *name)
 
 int arm_cti_cleanup_all(void)
 {
-	struct arm_cti_object *obj, *tmp;
+	struct arm_cti *obj, *tmp;
 
 	list_for_each_entry_safe(obj, tmp, &all_cti, lh) {
 		free(obj->name);
@@ -237,16 +237,16 @@ int arm_cti_cleanup_all(void)
 
 COMMAND_HANDLER(handle_cti_dump)
 {
-	struct arm_cti_object *obj = CMD_DATA;
-	struct arm_cti *cti = &obj->cti;
+	struct arm_cti *cti = CMD_DATA;
+	struct adiv5_ap *ap = dap_ap(cti->spot.dap, cti->spot.ap_num);
 	int retval = ERROR_OK;
 
 	for (int i = 0; (retval == ERROR_OK) && (i < (int)ARRAY_SIZE(cti_names)); i++)
-		retval = mem_ap_read_u32(cti->ap,
-				cti->base + cti_names[i].offset, cti_names[i].p_val);
+		retval = mem_ap_read_u32(ap,
+				cti->spot.base + cti_names[i].offset, cti_names[i].p_val);
 
 	if (retval == ERROR_OK)
-		retval = dap_run(cti->ap->dap);
+		retval = dap_run(ap->dap);
 
 	if (retval != ERROR_OK)
 		return JIM_ERR;
@@ -260,8 +260,7 @@ COMMAND_HANDLER(handle_cti_dump)
 
 COMMAND_HANDLER(handle_cti_enable)
 {
-	struct arm_cti_object *obj = CMD_DATA;
-	struct arm_cti *cti = &obj->cti;
+	struct arm_cti *cti = CMD_DATA;
 	bool on_off;
 
 	if (CMD_ARGC != 1)
@@ -274,8 +273,7 @@ COMMAND_HANDLER(handle_cti_enable)
 
 COMMAND_HANDLER(handle_cti_testmode)
 {
-	struct arm_cti_object *obj = CMD_DATA;
-	struct arm_cti *cti = &obj->cti;
+	struct arm_cti *cti = CMD_DATA;
 	bool on_off;
 
 	if (CMD_ARGC != 1)
@@ -288,8 +286,7 @@ COMMAND_HANDLER(handle_cti_testmode)
 
 COMMAND_HANDLER(handle_cti_write)
 {
-	struct arm_cti_object *obj = CMD_DATA;
-	struct arm_cti *cti = &obj->cti;
+	struct arm_cti *cti = CMD_DATA;
 	int offset;
 	uint32_t value;
 
@@ -307,8 +304,7 @@ COMMAND_HANDLER(handle_cti_write)
 
 COMMAND_HANDLER(handle_cti_read)
 {
-	struct arm_cti_object *obj = CMD_DATA;
-	struct arm_cti *cti = &obj->cti;
+	struct arm_cti *cti = CMD_DATA;
 	int offset;
 	int retval;
 	uint32_t value;
@@ -331,8 +327,7 @@ COMMAND_HANDLER(handle_cti_read)
 
 COMMAND_HANDLER(handle_cti_ack)
 {
-	struct arm_cti_object *obj = CMD_DATA;
-	struct arm_cti *cti = &obj->cti;
+	struct arm_cti *cti = CMD_DATA;
 	uint32_t event;
 
 	if (CMD_ARGC != 1)
@@ -351,8 +346,7 @@ COMMAND_HANDLER(handle_cti_ack)
 
 COMMAND_HANDLER(handle_cti_channel)
 {
-	struct arm_cti_object *obj = CMD_DATA;
-	struct arm_cti *cti = &obj->cti;
+	struct arm_cti *cti = CMD_DATA;
 	int retval = ERROR_OK;
 	uint32_t ch_num;
 
@@ -436,83 +430,26 @@ static const struct command_registration cti_instance_command_handlers[] = {
 	COMMAND_REGISTRATION_DONE
 };
 
-enum cti_cfg_param {
-	CFG_DAP,
-	CFG_AP_NUM,
-	CFG_CTIBASE
-};
-
-static const Jim_Nvp nvp_config_opts[] = {
-	{ .name = "-dap",     .value = CFG_DAP },
-	{ .name = "-ctibase", .value = CFG_CTIBASE },
-	{ .name = "-ap-num",  .value = CFG_AP_NUM },
-	{ .name = NULL, .value = -1 }
-};
-
-static int cti_configure(Jim_GetOptInfo *goi, struct arm_cti_object *cti)
+static int cti_configure(Jim_GetOptInfo *goi, struct arm_cti *cti)
 {
-	struct adiv5_dap *dap = NULL;
-	Jim_Nvp *n;
-	jim_wide w;
-	int e;
-
 	/* parse config or cget options ... */
 	while (goi->argc > 0) {
-		Jim_SetEmptyResult(goi->interp);
-
-		e = Jim_GetOpt_Nvp(goi, nvp_config_opts, &n);
-		if (e != JIM_OK) {
-			Jim_GetOpt_NvpUnknown(goi, nvp_config_opts, 0);
+		int e = adiv5_jim_mem_ap_spot_configure(&cti->spot, goi);
+		if (e != JIM_OK)
 			return e;
-		}
-		switch (n->value) {
-		case CFG_DAP: {
-			Jim_Obj *o_t;
-			e = Jim_GetOpt_Obj(goi, &o_t);
-			if (e != JIM_OK)
-				return e;
-			dap = dap_instance_by_jim_obj(goi->interp, o_t);
-			if (dap == NULL) {
-				Jim_SetResultString(goi->interp, "-dap is invalid", -1);
-				return JIM_ERR;
-			}
-			/* loop for more */
-			break;
-		}
-		case CFG_CTIBASE:
-			e = Jim_GetOpt_Wide(goi, &w);
-			if (e != JIM_OK)
-				return e;
-			cti->cti.base = (uint32_t)w;
-			/* loop for more */
-			break;
-
-		case CFG_AP_NUM:
-			e = Jim_GetOpt_Wide(goi, &w);
-			if (e != JIM_OK)
-				return e;
-			if (w < 0 || w > DP_APSEL_MAX) {
-				Jim_SetResultString(goi->interp, "-ap-num is invalid", -1);
-				return JIM_ERR;
-			}
-			cti->ap_num = (uint32_t)w;
-		}
 	}
 
-	if (dap == NULL) {
+	if (!cti->spot.dap) {
 		Jim_SetResultString(goi->interp, "-dap required when creating CTI", -1);
 		return JIM_ERR;
 	}
 
-	cti->cti.ap = dap_ap(dap, cti->ap_num);
-
 	return JIM_OK;
 }
-
 static int cti_create(Jim_GetOptInfo *goi)
 {
 	struct command_context *cmd_ctx;
-	static struct arm_cti_object *cti;
+	static struct arm_cti *cti;
 	Jim_Obj *new_cmd;
 	Jim_Cmd *cmd;
 	const char *cp;
@@ -536,10 +473,14 @@ static int cti_create(Jim_GetOptInfo *goi)
 	}
 
 	/* Create it */
-	cti = calloc(1, sizeof(struct arm_cti_object));
+	cti = calloc(1, sizeof(*cti));
 	if (cti == NULL)
 		return JIM_ERR;
 
+	adiv5_mem_ap_spot_init(&cti->spot);
+
+	/* Do the rest as "configure" options */
+	goi->isconfigure = 1;
 	e = cti_configure(goi, cti);
 	if (e != JIM_OK) {
 		free(cti);
@@ -593,7 +534,7 @@ static int jim_cti_create(Jim_Interp *interp, int argc, Jim_Obj *const *argv)
 
 static int jim_cti_names(Jim_Interp *interp, int argc, Jim_Obj *const *argv)
 {
-	struct arm_cti_object *obj;
+	struct arm_cti *obj;
 
 	if (argc != 1) {
 		Jim_WrongNumArgs(interp, 1, argv, "Too many parameters");
diff --git a/src/target/arm_semihosting.c b/src/target/arm_semihosting.c
index 61f1e78..723be57 100644
--- a/src/target/arm_semihosting.c
+++ b/src/target/arm_semihosting.c
@@ -315,7 +315,7 @@ int arm_semihosting(struct target *target, int *retval)
 		if (0 <= semihosting->op && semihosting->op <= 0x31) {
 			*retval = semihosting_common(target);
 			if (*retval != ERROR_OK) {
-				LOG_ERROR("Failed semihosting operation");
+				LOG_ERROR("Failed semihosting operation (0x%02X)", semihosting->op);
 				return 0;
 			}
 		} else {
diff --git a/src/target/armv4_5.c b/src/target/armv4_5.c
index 7da28e3..b725853 100644
--- a/src/target/armv4_5.c
+++ b/src/target/armv4_5.c
@@ -856,6 +856,9 @@ COMMAND_HANDLER(handle_armv4_5_reg_command)
 		char *sep = "\n";
 		char *shadow = "";
 
+		if (!arm_mode_data[mode].n_indices)
+			continue;
+
 		/* label this bank of registers (or shadows) */
 		switch (arm_mode_data[mode].psr) {
 			case ARM_MODE_SYS:
@@ -869,6 +872,7 @@ COMMAND_HANDLER(handle_armv4_5_reg_command)
 					continue;
 			/* FALLTHROUGH */
 			case ARM_MODE_MON:
+			case ARM_MODE_1176_MON:
 				if (arm->core_type != ARM_CORE_TYPE_SEC_EXT
 					&& arm->core_type != ARM_CORE_TYPE_VIRT_EXT)
 					continue;
diff --git a/src/target/armv7a.c b/src/target/armv7a.c
index c36744d..abca335 100644
--- a/src/target/armv7a.c
+++ b/src/target/armv7a.c
@@ -589,7 +589,7 @@ static const struct command_registration l2_cache_commands[] = {
 
 };
 
-const struct command_registration l2x_cache_command_handlers[] = {
+static const struct command_registration l2x_cache_command_handlers[] = {
 	{
 		.name = "cache_config",
 		.mode = COMMAND_EXEC,
diff --git a/src/target/armv7a_cache.c b/src/target/armv7a_cache.c
index e5f1fb0..fa6df2a 100644
--- a/src/target/armv7a_cache.c
+++ b/src/target/armv7a_cache.c
@@ -572,7 +572,7 @@ static const struct command_registration arm7a_l1_i_cache_commands[] = {
 	COMMAND_REGISTRATION_DONE
 };
 
-const struct command_registration arm7a_l1_di_cache_group_handlers[] = {
+static const struct command_registration arm7a_l1_di_cache_group_handlers[] = {
 	{
 		.name = "info",
 		.handler = arm7a_l1_cache_info_cmd,
@@ -597,7 +597,7 @@ const struct command_registration arm7a_l1_di_cache_group_handlers[] = {
 	COMMAND_REGISTRATION_DONE
 };
 
-const struct command_registration arm7a_cache_group_handlers[] = {
+static const struct command_registration arm7a_cache_group_handlers[] = {
 	{
 		.name = "auto",
 		.handler = arm7a_cache_disable_auto_cmd,
diff --git a/src/target/armv7m.c b/src/target/armv7m.c
index ea6ee61..f14ce0d 100644
--- a/src/target/armv7m.c
+++ b/src/target/armv7m.c
@@ -14,6 +14,9 @@
  *   Copyright (C) 2018 by Liviu Ionescu                                   *
  *   <ilg@livius.net>                                                      *
  *                                                                         *
+ *   Copyright (C) 2019 by Tomas Vanek                                     *
+ *   vanekt@fbl.cz                                                         *
+ *                                                                         *
  *   This program is free software; you can redistribute it and/or modify  *
  *   it under the terms of the GNU General Public License as published by  *
  *   the Free Software Foundation; either version 2 of the License, or     *
@@ -108,10 +111,19 @@ static const struct {
 	{ ARMV7M_MSP, "msp", 32, REG_TYPE_DATA_PTR, "system", "org.gnu.gdb.arm.m-system" },
 	{ ARMV7M_PSP, "psp", 32, REG_TYPE_DATA_PTR, "system", "org.gnu.gdb.arm.m-system" },
 
+	/* A working register for packing/unpacking special regs, hidden from gdb */
+	{ ARMV7M_PMSK_BPRI_FLTMSK_CTRL, "pmsk_bpri_fltmsk_ctrl", 32, REG_TYPE_INT, NULL, NULL },
+
+	/* WARNING: If you use armv7m_write_core_reg() on one of 4 following
+	 * special registers, the new data go to ARMV7M_PMSK_BPRI_FLTMSK_CTRL
+	 * cache only and are not flushed to CPU HW register.
+	 * To trigger write to CPU HW register, add
+	 *		armv7m_write_core_reg(,,ARMV7M_PMSK_BPRI_FLTMSK_CTRL,);
+	 */
 	{ ARMV7M_PRIMASK, "primask", 1, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.m-system" },
 	{ ARMV7M_BASEPRI, "basepri", 8, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.m-system" },
 	{ ARMV7M_FAULTMASK, "faultmask", 1, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.m-system" },
-	{ ARMV7M_CONTROL, "control", 2, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.m-system" },
+	{ ARMV7M_CONTROL, "control", 3, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.m-system" },
 
 	{ ARMV7M_D0, "d0", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" },
 	{ ARMV7M_D1, "d1", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" },
@@ -150,6 +162,9 @@ int armv7m_restore_context(struct target *target)
 	if (armv7m->pre_restore_context)
 		armv7m->pre_restore_context(target);
 
+	/* The descending order of register writes is crucial for correct
+	 * packing of ARMV7M_PMSK_BPRI_FLTMSK_CTRL!
+	 * See also comments in the register table above */
 	for (i = cache->num_regs - 1; i >= 0; i--) {
 		if (cache->reg_list[i].dirty) {
 			armv7m->arm.write_core_reg(target, &cache->reg_list[i], i,
@@ -211,87 +226,195 @@ static int armv7m_set_core_reg(struct reg *reg, uint8_t *buf)
 	return ERROR_OK;
 }
 
+static uint32_t armv7m_map_id_to_regsel(unsigned int arm_reg_id)
+{
+	switch (arm_reg_id) {
+	case ARMV7M_R0 ... ARMV7M_R14:
+	case ARMV7M_PC:
+	case ARMV7M_xPSR:
+	case ARMV7M_MSP:
+	case ARMV7M_PSP:
+		/* NOTE:  we "know" here that the register identifiers
+		 * match the Cortex-M DCRSR.REGSEL selectors values
+		 * for R0..R14, PC, xPSR, MSP, and PSP.
+		 */
+		return arm_reg_id;
+
+	case ARMV7M_PMSK_BPRI_FLTMSK_CTRL:
+		return ARMV7M_REGSEL_PMSK_BPRI_FLTMSK_CTRL;
+
+	case ARMV7M_FPSCR:
+		return ARMV7M_REGSEL_FPSCR;
+
+	case ARMV7M_D0 ... ARMV7M_D15:
+		return ARMV7M_REGSEL_S0 + 2 * (arm_reg_id - ARMV7M_D0);
+
+	default:
+		LOG_ERROR("Bad register ID %u", arm_reg_id);
+		return arm_reg_id;
+	}
+}
+
+static bool armv7m_map_reg_packing(unsigned int arm_reg_id,
+					unsigned int *reg32_id, uint32_t *offset)
+{
+	switch (arm_reg_id) {
+
+	case ARMV7M_PRIMASK:
+		*reg32_id = ARMV7M_PMSK_BPRI_FLTMSK_CTRL;
+		*offset = 0;
+		return true;
+	case ARMV7M_BASEPRI:
+		*reg32_id = ARMV7M_PMSK_BPRI_FLTMSK_CTRL;
+		*offset = 1;
+		return true;
+	case ARMV7M_FAULTMASK:
+		*reg32_id = ARMV7M_PMSK_BPRI_FLTMSK_CTRL;
+		*offset = 2;
+		return true;
+	case ARMV7M_CONTROL:
+		*reg32_id = ARMV7M_PMSK_BPRI_FLTMSK_CTRL;
+		*offset = 3;
+		return true;
+
+	default:
+		return false;
+	}
+}
+
 static int armv7m_read_core_reg(struct target *target, struct reg *r,
 	int num, enum arm_mode mode)
 {
 	uint32_t reg_value;
 	int retval;
-	struct arm_reg *armv7m_core_reg;
 	struct armv7m_common *armv7m = target_to_armv7m(target);
 
 	assert(num < (int)armv7m->arm.core_cache->num_regs);
+	assert(num == (int)r->number);
 
-	armv7m_core_reg = armv7m->arm.core_cache->reg_list[num].arch_info;
+	/* If a code calls read_reg, it expects the cache is no more dirty.
+	 * Clear the dirty flag regardless of the later read succeeds or not
+	 * to prevent unwanted cache flush after a read error */
+	r->dirty = false;
+
+	if (r->size <= 8) {
+		/* any 8-bit or shorter register is packed */
+		uint32_t offset = 0;	/* silence false gcc warning */
+		unsigned int reg32_id;
+
+		bool is_packed = armv7m_map_reg_packing(num, &reg32_id, &offset);
+		assert(is_packed);
+		struct reg *r32 = &armv7m->arm.core_cache->reg_list[reg32_id];
+
+		/* Read 32-bit container register if not cached */
+		if (!r32->valid) {
+			retval = armv7m_read_core_reg(target, r32, reg32_id, mode);
+			if (retval != ERROR_OK)
+				return retval;
+		}
+
+		/* Copy required bits of 32-bit container register */
+		buf_cpy(r32->value + offset, r->value, r->size);
 
-	if ((armv7m_core_reg->num >= ARMV7M_D0) && (armv7m_core_reg->num <= ARMV7M_D15)) {
-		/* map D0..D15 to S0..S31 */
-		size_t regidx = ARMV7M_S0 + 2 * (armv7m_core_reg->num - ARMV7M_D0);
-		retval = armv7m->load_core_reg_u32(target, regidx, &reg_value);
-		if (retval != ERROR_OK)
-			return retval;
-		buf_set_u32(armv7m->arm.core_cache->reg_list[num].value,
-			    0, 32, reg_value);
-		retval = armv7m->load_core_reg_u32(target, regidx + 1, &reg_value);
-		if (retval != ERROR_OK)
-			return retval;
-		buf_set_u32(armv7m->arm.core_cache->reg_list[num].value + 4,
-			    0, 32, reg_value);
 	} else {
-		retval = armv7m->load_core_reg_u32(target,
-						   armv7m_core_reg->num, &reg_value);
+		assert(r->size == 32 || r->size == 64);
+
+		struct arm_reg *armv7m_core_reg = r->arch_info;
+		uint32_t regsel = armv7m_map_id_to_regsel(armv7m_core_reg->num);
+
+		retval = armv7m->load_core_reg_u32(target, regsel, &reg_value);
 		if (retval != ERROR_OK)
 			return retval;
-		buf_set_u32(armv7m->arm.core_cache->reg_list[num].value, 0, 32, reg_value);
+		buf_set_u32(r->value, 0, 32, reg_value);
+
+		if (r->size == 64) {
+			retval = armv7m->load_core_reg_u32(target, regsel + 1, &reg_value);
+			if (retval != ERROR_OK) {
+				r->valid = false;
+				return retval;
+			}
+			buf_set_u32(r->value + 4, 0, 32, reg_value);
+
+			uint64_t q = buf_get_u64(r->value, 0, 64);
+			LOG_DEBUG("read %s value 0x%016" PRIx64, r->name, q);
+		} else {
+			LOG_DEBUG("read %s value 0x%08" PRIx32, r->name, reg_value);
+		}
 	}
 
-	armv7m->arm.core_cache->reg_list[num].valid = true;
-	armv7m->arm.core_cache->reg_list[num].dirty = false;
+	r->valid = true;
 
-	return retval;
+	return ERROR_OK;
 }
 
 static int armv7m_write_core_reg(struct target *target, struct reg *r,
 	int num, enum arm_mode mode, uint8_t *value)
 {
 	int retval;
-	struct arm_reg *armv7m_core_reg;
+	uint32_t t;
 	struct armv7m_common *armv7m = target_to_armv7m(target);
 
 	assert(num < (int)armv7m->arm.core_cache->num_regs);
+	assert(num == (int)r->number);
 
-	armv7m_core_reg = armv7m->arm.core_cache->reg_list[num].arch_info;
+	if (value != r->value) {
+		/* If we are not flushing the cache, store the new value to the cache */
+		buf_cpy(value, r->value, r->size);
+	}
 
-	if ((armv7m_core_reg->num >= ARMV7M_D0) && (armv7m_core_reg->num <= ARMV7M_D15)) {
-		/* map D0..D15 to S0..S31 */
-		size_t regidx = ARMV7M_S0 + 2 * (armv7m_core_reg->num - ARMV7M_D0);
+	if (r->size <= 8) {
+		/* any 8-bit or shorter register is packed */
+		uint32_t offset = 0;	/* silence false gcc warning */
+		unsigned int reg32_id;
 
-		uint32_t t = buf_get_u32(value, 0, 32);
-		retval = armv7m->store_core_reg_u32(target, regidx, t);
-		if (retval != ERROR_OK)
-			goto out_error;
+		bool is_packed = armv7m_map_reg_packing(num, &reg32_id, &offset);
+		assert(is_packed);
+		struct reg *r32 = &armv7m->arm.core_cache->reg_list[reg32_id];
+
+		if (!r32->valid) {
+			/* Before merging with other parts ensure the 32-bit register is valid */
+			retval = armv7m_read_core_reg(target, r32, reg32_id, mode);
+			if (retval != ERROR_OK)
+				return retval;
+		}
+
+		/* Write a part to the 32-bit container register */
+		buf_cpy(value, r32->value + offset, r->size);
+		r32->dirty = true;
 
-		t = buf_get_u32(value + 4, 0, 32);
-		retval = armv7m->store_core_reg_u32(target, regidx + 1, t);
-		if (retval != ERROR_OK)
-			goto out_error;
 	} else {
-		uint32_t t = buf_get_u32(value, 0, 32);
+		assert(r->size == 32 || r->size == 64);
+
+		struct arm_reg *armv7m_core_reg = r->arch_info;
+		uint32_t regsel = armv7m_map_id_to_regsel(armv7m_core_reg->num);
 
-		LOG_DEBUG("write core reg %i value 0x%" PRIx32 "", num, t);
-		retval = armv7m->store_core_reg_u32(target, armv7m_core_reg->num, t);
+		t = buf_get_u32(value, 0, 32);
+		retval = armv7m->store_core_reg_u32(target, regsel, t);
 		if (retval != ERROR_OK)
 			goto out_error;
+
+		if (r->size == 64) {
+			t = buf_get_u32(value + 4, 0, 32);
+			retval = armv7m->store_core_reg_u32(target, regsel + 1, t);
+			if (retval != ERROR_OK)
+				goto out_error;
+
+			uint64_t q = buf_get_u64(value, 0, 64);
+			LOG_DEBUG("write %s value 0x%016" PRIx64, r->name, q);
+		} else {
+			LOG_DEBUG("write %s value 0x%08" PRIx32, r->name, t);
+		}
 	}
 
-	armv7m->arm.core_cache->reg_list[num].valid = true;
-	armv7m->arm.core_cache->reg_list[num].dirty = false;
+	r->valid = true;
+	r->dirty = false;
 
 	return ERROR_OK;
 
 out_error:
-	LOG_ERROR("Error setting register");
-	armv7m->arm.core_cache->reg_list[num].dirty = armv7m->arm.core_cache->reg_list[num].valid;
-	return ERROR_JTAG_DEVICE_ERROR;
+	r->dirty = true;
+	LOG_ERROR("Error setting register %s", r->name);
+	return retval;
 }
 
 /**
@@ -370,8 +493,7 @@ int armv7m_start_algorithm(struct target *target,
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
-	/* refresh core register cache
-	 * Not needed if core register cache is always consistent with target process state */
+	/* Store all non-debug execution registers to armv7m_algorithm_info context */
 	for (unsigned i = 0; i < armv7m->arm.core_cache->num_regs; i++) {
 
 		armv7m_algorithm_info->context[i] = buf_get_u32(
@@ -618,12 +740,10 @@ struct reg_cache *armv7m_build_reg_cache(struct target *target)
 
 		reg_list[i].name = armv7m_regs[i].name;
 		reg_list[i].size = armv7m_regs[i].bits;
-		size_t storage_size = DIV_ROUND_UP(armv7m_regs[i].bits, 8);
-		if (storage_size < 4)
-			storage_size = 4;
-		reg_list[i].value = calloc(1, storage_size);
+		reg_list[i].value = arch_info[i].value;
 		reg_list[i].dirty = false;
 		reg_list[i].valid = false;
+		reg_list[i].hidden = i == ARMV7M_PMSK_BPRI_FLTMSK_CTRL;
 		reg_list[i].type = &armv7m_reg_type;
 		reg_list[i].arch_info = &arch_info[i];
 
@@ -632,6 +752,9 @@ struct reg_cache *armv7m_build_reg_cache(struct target *target)
 		reg_list[i].exist = true;
 		reg_list[i].caller_save = true;	/* gdb defaults to true */
 
+		if (reg_list[i].hidden)
+			continue;
+
 		feature = calloc(1, sizeof(struct reg_feature));
 		if (feature) {
 			feature->name = armv7m_regs[i].feature;
@@ -671,7 +794,6 @@ void armv7m_free_reg_cache(struct target *target)
 
 		free(reg->feature);
 		free(reg->reg_data_type);
-		free(reg->value);
 	}
 
 	free(cache->reg_list[0].arch_info);
diff --git a/src/target/armv7m.h b/src/target/armv7m.h
index 01bf19e..db6f8bc 100644
--- a/src/target/armv7m.h
+++ b/src/target/armv7m.h
@@ -34,75 +34,115 @@ extern const int armv7m_msp_reg_map[];
 
 const char *armv7m_exception_string(int number);
 
+/* Cortex-M DCRSR.REGSEL selectors */
+enum {
+	ARMV7M_REGSEL_R0,
+	ARMV7M_REGSEL_R1,
+	ARMV7M_REGSEL_R2,
+	ARMV7M_REGSEL_R3,
+
+	ARMV7M_REGSEL_R4,
+	ARMV7M_REGSEL_R5,
+	ARMV7M_REGSEL_R6,
+	ARMV7M_REGSEL_R7,
+
+	ARMV7M_REGSEL_R8,
+	ARMV7M_REGSEL_R9,
+	ARMV7M_REGSEL_R10,
+	ARMV7M_REGSEL_R11,
+
+	ARMV7M_REGSEL_R12,
+	ARMV7M_REGSEL_R13,
+	ARMV7M_REGSEL_R14,
+	ARMV7M_REGSEL_PC = 15,
+
+	ARMV7M_REGSEL_xPSR = 16,
+	ARMV7M_REGSEL_MSP,
+	ARMV7M_REGSEL_PSP,
+
+	ARMV7M_REGSEL_PMSK_BPRI_FLTMSK_CTRL = 0x14,
+	ARMV7M_REGSEL_FPSCR = 0x21,
+
+	/* 32bit Floating-point registers */
+	ARMV7M_REGSEL_S0 = 0x40,
+	ARMV7M_REGSEL_S1,
+	ARMV7M_REGSEL_S2,
+	ARMV7M_REGSEL_S3,
+	ARMV7M_REGSEL_S4,
+	ARMV7M_REGSEL_S5,
+	ARMV7M_REGSEL_S6,
+	ARMV7M_REGSEL_S7,
+	ARMV7M_REGSEL_S8,
+	ARMV7M_REGSEL_S9,
+	ARMV7M_REGSEL_S10,
+	ARMV7M_REGSEL_S11,
+	ARMV7M_REGSEL_S12,
+	ARMV7M_REGSEL_S13,
+	ARMV7M_REGSEL_S14,
+	ARMV7M_REGSEL_S15,
+	ARMV7M_REGSEL_S16,
+	ARMV7M_REGSEL_S17,
+	ARMV7M_REGSEL_S18,
+	ARMV7M_REGSEL_S19,
+	ARMV7M_REGSEL_S20,
+	ARMV7M_REGSEL_S21,
+	ARMV7M_REGSEL_S22,
+	ARMV7M_REGSEL_S23,
+	ARMV7M_REGSEL_S24,
+	ARMV7M_REGSEL_S25,
+	ARMV7M_REGSEL_S26,
+	ARMV7M_REGSEL_S27,
+	ARMV7M_REGSEL_S28,
+	ARMV7M_REGSEL_S29,
+	ARMV7M_REGSEL_S30,
+	ARMV7M_REGSEL_S31,
+};
+
 /* offsets into armv7m core register cache */
 enum {
 	/* for convenience, the first set of indices match
-	 * the Cortex-M3/-M4 DCRSR selectors
+	 * the Cortex-M DCRSR.REGSEL selectors
+	 */
+	ARMV7M_R0 = ARMV7M_REGSEL_R0,
+	ARMV7M_R1 = ARMV7M_REGSEL_R1,
+	ARMV7M_R2 = ARMV7M_REGSEL_R2,
+	ARMV7M_R3 = ARMV7M_REGSEL_R3,
+
+	ARMV7M_R4 = ARMV7M_REGSEL_R4,
+	ARMV7M_R5 = ARMV7M_REGSEL_R5,
+	ARMV7M_R6 = ARMV7M_REGSEL_R6,
+	ARMV7M_R7 = ARMV7M_REGSEL_R7,
+
+	ARMV7M_R8 = ARMV7M_REGSEL_R8,
+	ARMV7M_R9 = ARMV7M_REGSEL_R9,
+	ARMV7M_R10 = ARMV7M_REGSEL_R10,
+	ARMV7M_R11 = ARMV7M_REGSEL_R11,
+
+	ARMV7M_R12 = ARMV7M_REGSEL_R12,
+	ARMV7M_R13 = ARMV7M_REGSEL_R13,
+	ARMV7M_R14 = ARMV7M_REGSEL_R14,
+	ARMV7M_PC = ARMV7M_REGSEL_PC,
+
+	ARMV7M_xPSR = ARMV7M_REGSEL_xPSR,
+	ARMV7M_MSP = ARMV7M_REGSEL_MSP,
+	ARMV7M_PSP = ARMV7M_REGSEL_PSP,
+
+	/* following indices are arbitrary, do not match DCRSR.REGSEL selectors */
+
+	/* working register for packing/unpacking special regs, hidden from gdb */
+	ARMV7M_PMSK_BPRI_FLTMSK_CTRL,
+
+	/* WARNING: If you use armv7m_write_core_reg() on one of 4 following
+	 * special registers, the new data go to ARMV7M_PMSK_BPRI_FLTMSK_CTRL
+	 * cache only and are not flushed to CPU HW register.
+	 * To trigger write to CPU HW register, add
+	 *		armv7m_write_core_reg(,,ARMV7M_PMSK_BPRI_FLTMSK_CTRL,);
 	 */
-	ARMV7M_R0,
-	ARMV7M_R1,
-	ARMV7M_R2,
-	ARMV7M_R3,
-
-	ARMV7M_R4,
-	ARMV7M_R5,
-	ARMV7M_R6,
-	ARMV7M_R7,
-
-	ARMV7M_R8,
-	ARMV7M_R9,
-	ARMV7M_R10,
-	ARMV7M_R11,
-
-	ARMV7M_R12,
-	ARMV7M_R13,
-	ARMV7M_R14,
-	ARMV7M_PC = 15,
-
-	ARMV7M_xPSR = 16,
-	ARMV7M_MSP,
-	ARMV7M_PSP,
-
-	/* this next set of indices is arbitrary */
 	ARMV7M_PRIMASK,
 	ARMV7M_BASEPRI,
 	ARMV7M_FAULTMASK,
 	ARMV7M_CONTROL,
 
-	/* 32bit Floating-point registers */
-	ARMV7M_S0,
-	ARMV7M_S1,
-	ARMV7M_S2,
-	ARMV7M_S3,
-	ARMV7M_S4,
-	ARMV7M_S5,
-	ARMV7M_S6,
-	ARMV7M_S7,
-	ARMV7M_S8,
-	ARMV7M_S9,
-	ARMV7M_S10,
-	ARMV7M_S11,
-	ARMV7M_S12,
-	ARMV7M_S13,
-	ARMV7M_S14,
-	ARMV7M_S15,
-	ARMV7M_S16,
-	ARMV7M_S17,
-	ARMV7M_S18,
-	ARMV7M_S19,
-	ARMV7M_S20,
-	ARMV7M_S21,
-	ARMV7M_S22,
-	ARMV7M_S23,
-	ARMV7M_S24,
-	ARMV7M_S25,
-	ARMV7M_S26,
-	ARMV7M_S27,
-	ARMV7M_S28,
-	ARMV7M_S29,
-	ARMV7M_S30,
-	ARMV7M_S31,
-
 	/* 64bit Floating-point registers */
 	ARMV7M_D0,
 	ARMV7M_D1,
@@ -121,10 +161,8 @@ enum {
 	ARMV7M_D14,
 	ARMV7M_D15,
 
-	/* Floating-point status registers */
-	ARMV7M_FPSID,
+	/* Floating-point status register */
 	ARMV7M_FPSCR,
-	ARMV7M_FPEXC,
 
 	ARMV7M_LAST_REG,
 };
@@ -137,7 +175,7 @@ enum {
 };
 
 #define ARMV7M_NUM_CORE_REGS (ARMV7M_xPSR + 1)
-#define ARMV7M_NUM_CORE_REGS_NOFP (ARMV7M_NUM_CORE_REGS + 6)
+#define ARMV7M_NUM_CORE_REGS_NOFP (ARMV7M_CONTROL + 1)
 
 #define ARMV7M_COMMON_MAGIC 0x2A452A45
 
@@ -159,8 +197,8 @@ struct armv7m_common {
 	struct armv7m_trace_config trace_config;
 
 	/* Direct processor core register read and writes */
-	int (*load_core_reg_u32)(struct target *target, uint32_t num, uint32_t *value);
-	int (*store_core_reg_u32)(struct target *target, uint32_t num, uint32_t value);
+	int (*load_core_reg_u32)(struct target *target, uint32_t regsel, uint32_t *value);
+	int (*store_core_reg_u32)(struct target *target, uint32_t regsel, uint32_t value);
 
 	int (*examine_debug_reason)(struct target *target);
 	int (*post_debug_entry)(struct target *target);
diff --git a/src/target/armv7m_trace.c b/src/target/armv7m_trace.c
index 6b368f7..10f1422 100644
--- a/src/target/armv7m_trace.c
+++ b/src/target/armv7m_trace.c
@@ -40,13 +40,43 @@ static int armv7m_poll_trace(void *target)
 
 	target_call_trace_callbacks(target, size, buf);
 
-	if (armv7m->trace_config.trace_file != NULL) {
-		if (fwrite(buf, 1, size, armv7m->trace_config.trace_file) == size)
-			fflush(armv7m->trace_config.trace_file);
-		else {
-			LOG_ERROR("Error writing to the trace destination file");
-			return ERROR_FAIL;
+	switch (armv7m->trace_config.internal_channel) {
+	case TRACE_INTERNAL_CHANNEL_FILE:
+		if (armv7m->trace_config.trace_file != NULL) {
+			if (fwrite(buf, 1, size, armv7m->trace_config.trace_file) == size)
+				fflush(armv7m->trace_config.trace_file);
+			else {
+				LOG_ERROR("Error writing to the trace destination file");
+				return ERROR_FAIL;
+			}
+		}
+		break;
+	case TRACE_INTERNAL_CHANNEL_TCP:
+		if (armv7m->trace_config.trace_service != NULL) {
+			/* broadcast to all service connections */
+			struct connection *connection = armv7m->trace_config.trace_service->connections;
+			retval = ERROR_OK;
+			while (connection) {
+				if (connection_write(connection, buf, size) != (int) size)
+					retval = ERROR_FAIL;
+
+				connection = connection->next;
+			}
+
+			if (retval != ERROR_OK) {
+				LOG_ERROR("Error streaming the trace to TCP/IP port");
+				return ERROR_FAIL;
+			}
 		}
+		break;
+	case TRACE_INTERNAL_CHANNEL_TCL_ONLY:
+		/* nothing to do :
+		 * the trace data is sent to TCL by calling the target_call_trace_callbacks
+		 **/
+		break;
+	default:
+		LOG_ERROR("unsupported trace internal channel");
+		return ERROR_FAIL;
 	}
 
 	return ERROR_OK;
@@ -152,11 +182,71 @@ int armv7m_trace_itm_config(struct target *target)
 	return ERROR_OK;
 }
 
-static void close_trace_file(struct armv7m_common *armv7m)
+static void close_trace_channel(struct armv7m_common *armv7m)
 {
-	if (armv7m->trace_config.trace_file)
-		fclose(armv7m->trace_config.trace_file);
-	armv7m->trace_config.trace_file = NULL;
+	switch (armv7m->trace_config.internal_channel) {
+	case TRACE_INTERNAL_CHANNEL_FILE:
+		if (armv7m->trace_config.trace_file)
+			fclose(armv7m->trace_config.trace_file);
+		armv7m->trace_config.trace_file = NULL;
+		break;
+	case TRACE_INTERNAL_CHANNEL_TCP:
+		if (armv7m->trace_config.trace_service)
+			remove_service(armv7m->trace_config.trace_service->name, armv7m->trace_config.trace_service->port);
+		armv7m->trace_config.trace_service = NULL;
+		break;
+	case TRACE_INTERNAL_CHANNEL_TCL_ONLY:
+		/* nothing to do:
+		 * the trace polling is disabled in the beginning of armv7m_trace_tpiu_config
+		 **/
+		break;
+	default:
+		LOG_ERROR("unsupported trace internal channel");
+	}
+}
+
+static int trace_new_connection(struct connection *connection)
+{
+	/* nothing to do */
+	return ERROR_OK;
+}
+
+static int trace_input(struct connection *connection)
+{
+	/* create a dummy buffer to check if the connection is still active */
+	const int buf_len = 100;
+	unsigned char buf[buf_len];
+	int bytes_read = connection_read(connection, buf, buf_len);
+
+	if (bytes_read == 0)
+		return ERROR_SERVER_REMOTE_CLOSED;
+	else if (bytes_read == -1) {
+		LOG_ERROR("error during read: %s", strerror(errno));
+		return ERROR_SERVER_REMOTE_CLOSED;
+	}
+
+	return ERROR_OK;
+}
+
+static int trace_connection_closed(struct connection *connection)
+{
+	/* nothing to do, no connection->priv to free */
+	return ERROR_OK;
+}
+
+extern struct command_context *global_cmd_ctx;
+
+int armv7m_trace_tpiu_exit(struct target *target)
+{
+	struct armv7m_common *armv7m = target_to_armv7m(target);
+
+	if (global_cmd_ctx->mode == COMMAND_CONFIG ||
+		armv7m->trace_config.config_type == TRACE_CONFIG_TYPE_DISABLED)
+		return ERROR_OK;
+
+	close_trace_channel(armv7m);
+	armv7m->trace_config.config_type = TRACE_CONFIG_TYPE_DISABLED;
+	return armv7m_trace_tpiu_config(target);
 }
 
 COMMAND_HANDLER(handle_tpiu_config_command)
@@ -170,7 +260,7 @@ COMMAND_HANDLER(handle_tpiu_config_command)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 	if (!strcmp(CMD_ARGV[cmd_idx], "disable")) {
 		if (CMD_ARGC == cmd_idx + 1) {
-			close_trace_file(armv7m);
+			close_trace_channel(armv7m);
 
 			armv7m->trace_config.config_type = TRACE_CONFIG_TYPE_DISABLED;
 			if (CMD_CTX->mode == COMMAND_EXEC)
@@ -180,7 +270,7 @@ COMMAND_HANDLER(handle_tpiu_config_command)
 		}
 	} else if (!strcmp(CMD_ARGV[cmd_idx], "external") ||
 		   !strcmp(CMD_ARGV[cmd_idx], "internal")) {
-		close_trace_file(armv7m);
+		close_trace_channel(armv7m);
 
 		armv7m->trace_config.config_type = TRACE_CONFIG_TYPE_EXTERNAL;
 		if (!strcmp(CMD_ARGV[cmd_idx], "internal")) {
@@ -189,12 +279,26 @@ COMMAND_HANDLER(handle_tpiu_config_command)
 				return ERROR_COMMAND_SYNTAX_ERROR;
 
 			armv7m->trace_config.config_type = TRACE_CONFIG_TYPE_INTERNAL;
+			armv7m->trace_config.internal_channel = TRACE_INTERNAL_CHANNEL_TCL_ONLY;
 
 			if (strcmp(CMD_ARGV[cmd_idx], "-") != 0) {
-				armv7m->trace_config.trace_file = fopen(CMD_ARGV[cmd_idx], "ab");
-				if (!armv7m->trace_config.trace_file) {
-					LOG_ERROR("Can't open trace destination file");
-					return ERROR_FAIL;
+				if (CMD_ARGV[cmd_idx][0] == ':') {
+					armv7m->trace_config.internal_channel = TRACE_INTERNAL_CHANNEL_TCP;
+
+					int ret = add_service("armv7m_trace", &(CMD_ARGV[cmd_idx][1]),
+							CONNECTION_LIMIT_UNLIMITED, trace_new_connection, trace_input,
+							trace_connection_closed, NULL, &armv7m->trace_config.trace_service);
+					if (ret != ERROR_OK) {
+						LOG_ERROR("Can't configure trace TCP port");
+						return ERROR_FAIL;
+					}
+				} else {
+					armv7m->trace_config.internal_channel = TRACE_INTERNAL_CHANNEL_FILE;
+					armv7m->trace_config.trace_file = fopen(CMD_ARGV[cmd_idx], "ab");
+					if (!armv7m->trace_config.trace_file) {
+						LOG_ERROR("Can't open trace destination file");
+						return ERROR_FAIL;
+					}
 				}
 			}
 		}
@@ -306,7 +410,7 @@ static const struct command_registration tpiu_command_handlers[] = {
 		.mode = COMMAND_ANY,
 		.help = "Configure TPIU features",
 		.usage = "(disable | "
-		"((external | internal <filename>) "
+		"((external | internal (<filename> | <:port> | -)) "
 		"(sync <port width> | ((manchester | uart) <formatter enable>)) "
 		"<TRACECLKIN freq> [<trace freq>]))",
 	},
diff --git a/src/target/armv7m_trace.h b/src/target/armv7m_trace.h
index e5879fb..cdf79e7 100644
--- a/src/target/armv7m_trace.h
+++ b/src/target/armv7m_trace.h
@@ -18,6 +18,7 @@
 #ifndef OPENOCD_TARGET_ARMV7M_TRACE_H
 #define OPENOCD_TARGET_ARMV7M_TRACE_H
 
+#include <server/server.h>
 #include <target/target.h>
 #include <command.h>
 
@@ -32,8 +33,14 @@ enum trace_config_type {
 	TRACE_CONFIG_TYPE_INTERNAL	/**< trace output is handled by OpenOCD adapter driver */
 };
 
+enum trace_internal_channel {
+	TRACE_INTERNAL_CHANNEL_TCL_ONLY,	/** trace data is sent only to 'tcl_trace'  */
+	TRACE_INTERNAL_CHANNEL_FILE,		/** trace data is appended to a file */
+	TRACE_INTERNAL_CHANNEL_TCP			/** trace data is appended to a TCP/IP port*/
+};
+
 enum tpiu_pin_protocol {
-	TPIU_PIN_PROTOCOL_SYNC,			/**< synchronous trace output */
+	TPIU_PIN_PROTOCOL_SYNC,				/**< synchronous trace output */
 	TPIU_PIN_PROTOCOL_ASYNC_MANCHESTER,	/**< asynchronous output with Manchester coding */
 	TPIU_PIN_PROTOCOL_ASYNC_UART		/**< asynchronous output with NRZ coding */
 };
@@ -49,6 +56,9 @@ struct armv7m_trace_config {
 	/** Currently active trace capture mode */
 	enum trace_config_type config_type;
 
+	/** The used channel when internal mode is selected */
+	enum trace_internal_channel internal_channel;
+
 	/** Currently active trace output mode */
 	enum tpiu_pin_protocol pin_protocol;
 	/** TPIU formatter enable/disable (in async mode) */
@@ -73,8 +83,10 @@ struct armv7m_trace_config {
 	unsigned int traceclkin_freq;
 	/** Current frequency of trace port */
 	unsigned int trace_freq;
-	/** Handle to output trace data in INTERNAL capture mode */
+	/** Handle to output trace data in INTERNAL capture mode via file */
 	FILE *trace_file;
+	/** Handle to output trace data in INTERNAL capture mode via tcp */
+	struct service *trace_service;
 };
 
 extern const struct command_registration armv7m_trace_command_handlers[];
@@ -84,6 +96,10 @@ extern const struct command_registration armv7m_trace_command_handlers[];
  */
 int armv7m_trace_tpiu_config(struct target *target);
 /**
+ * Disable TPIU data gathering at exit
+ */
+int armv7m_trace_tpiu_exit(struct target *target);
+/**
  * Configure hardware accordingly to the current ITM target settings
  */
 int armv7m_trace_itm_config(struct target *target);
diff --git a/src/target/armv8.c b/src/target/armv8.c
index ab60cd3..95efdc9 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -1126,7 +1126,7 @@ int armv8_init_arch_info(struct target *target, struct armv8_common *armv8)
 	return ERROR_OK;
 }
 
-int armv8_aarch64_state(struct target *target)
+static int armv8_aarch64_state(struct target *target)
 {
 	struct arm *arm = target_to_arm(target);
 
diff --git a/src/target/avr32_ap7k.c b/src/target/avr32_ap7k.c
index 6221059..b0c0875 100644
--- a/src/target/avr32_ap7k.c
+++ b/src/target/avr32_ap7k.c
@@ -63,7 +63,7 @@ static const struct avr32_core_reg
 static int avr32_read_core_reg(struct target *target, int num);
 static int avr32_write_core_reg(struct target *target, int num);
 
-int avr32_ap7k_save_context(struct target *target)
+static int avr32_ap7k_save_context(struct target *target)
 {
 	int retval, i;
 	struct avr32_ap7k_common *ap7k = target_to_ap7k(target);
@@ -80,7 +80,7 @@ int avr32_ap7k_save_context(struct target *target)
 	return ERROR_OK;
 }
 
-int avr32_ap7k_restore_context(struct target *target)
+static int avr32_ap7k_restore_context(struct target *target)
 {
 	int i;
 
@@ -555,7 +555,7 @@ static int avr32_ap7k_examine(struct target *target)
 	return ERROR_OK;
 }
 
-int avr32_ap7k_arch_state(struct target *target)
+static int avr32_ap7k_arch_state(struct target *target)
 {
 	struct avr32_ap7k_common *ap7k = target_to_ap7k(target);
 
@@ -565,7 +565,7 @@ int avr32_ap7k_arch_state(struct target *target)
 	return ERROR_OK;
 }
 
-int avr32_ap7k_get_gdb_reg_list(struct target *target, struct reg **reg_list[],
+static int avr32_ap7k_get_gdb_reg_list(struct target *target, struct reg **reg_list[],
 		int *reg_list_size, enum target_register_class reg_class)
 {
 #if 0
diff --git a/src/target/avr32_jtag.c b/src/target/avr32_jtag.c
index 64ebf12..62c8f98 100644
--- a/src/target/avr32_jtag.c
+++ b/src/target/avr32_jtag.c
@@ -55,7 +55,7 @@ static int avr32_jtag_set_instr(struct avr32_jtag *jtag_info, int new_instr)
 	return ERROR_OK;
 }
 
-int avr32_jtag_nexus_set_address(struct avr32_jtag *jtag_info,
+static int avr32_jtag_nexus_set_address(struct avr32_jtag *jtag_info,
 		uint32_t addr, int mode)
 {
 	struct scan_field fields[2];
@@ -92,7 +92,7 @@ int avr32_jtag_nexus_set_address(struct avr32_jtag *jtag_info,
 }
 
 
-int avr32_jtag_nexus_read_data(struct avr32_jtag *jtag_info,
+static int avr32_jtag_nexus_read_data(struct avr32_jtag *jtag_info,
 	uint32_t *pdata)
 {
 
@@ -129,7 +129,7 @@ int avr32_jtag_nexus_read_data(struct avr32_jtag *jtag_info,
 	return ERROR_OK;
 }
 
-int avr32_jtag_nexus_write_data(struct avr32_jtag *jtag_info,
+static int avr32_jtag_nexus_write_data(struct avr32_jtag *jtag_info,
 		uint32_t data)
 {
 
@@ -184,7 +184,7 @@ int avr32_jtag_nexus_write(struct avr32_jtag *jtag_info,
 	return avr32_jtag_nexus_write_data(jtag_info, value);
 }
 
-int avr32_jtag_mwa_set_address(struct avr32_jtag *jtag_info, int slave,
+static int avr32_jtag_mwa_set_address(struct avr32_jtag *jtag_info, int slave,
 		uint32_t addr, int mode)
 {
 	struct scan_field fields[2];
@@ -223,7 +223,7 @@ int avr32_jtag_mwa_set_address(struct avr32_jtag *jtag_info, int slave,
 	return ERROR_OK;
 }
 
-int avr32_jtag_mwa_read_data(struct avr32_jtag *jtag_info,
+static int avr32_jtag_mwa_read_data(struct avr32_jtag *jtag_info,
 	uint32_t *pdata)
 {
 
@@ -260,7 +260,7 @@ int avr32_jtag_mwa_read_data(struct avr32_jtag *jtag_info,
 	return ERROR_OK;
 }
 
-int avr32_jtag_mwa_write_data(struct avr32_jtag *jtag_info,
+static int avr32_jtag_mwa_write_data(struct avr32_jtag *jtag_info,
 	uint32_t data)
 {
 
diff --git a/src/target/cortex_a.c b/src/target/cortex_a.c
index bd8e49f..d27c298 100644
--- a/src/target/cortex_a.c
+++ b/src/target/cortex_a.c
@@ -1115,7 +1115,8 @@ static int cortex_a_post_debug_entry(struct target *target)
 	return ERROR_OK;
 }
 
-int cortex_a_set_dscr_bits(struct target *target, unsigned long bit_mask, unsigned long value)
+static int cortex_a_set_dscr_bits(struct target *target,
+		unsigned long bit_mask, unsigned long value)
 {
 	struct armv7a_common *armv7a = target_to_armv7a(target);
 	uint32_t dscr;
@@ -1680,10 +1681,10 @@ static int cortex_a_assert_reset(struct target *target)
 		 */
 
 		/*
-		 * FIXME: fix reset when transport is SWD. This is a temporary
+		 * FIXME: fix reset when transport is not JTAG. This is a temporary
 		 * work-around for release v0.10 that is not intended to stay!
 		 */
-		if (transport_is_swd() ||
+		if (!transport_is_jtag() ||
 				(target->reset_halt && (jtag_get_reset_config() & RESET_SRST_NO_GATING)))
 			adapter_assert_reset();
 
diff --git a/src/target/cortex_m.c b/src/target/cortex_m.c
index 55664a7..ac308b4 100644
--- a/src/target/cortex_m.c
+++ b/src/target/cortex_m.c
@@ -39,6 +39,7 @@
 #include "arm_opcodes.h"
 #include "arm_semihosting.h"
 #include <helper/time_support.h>
+#include <rtt/rtt.h>
 
 /* NOTE:  most of this should work fine for the Cortex-M1 and
  * Cortex-M0 cores too, although they're ARMv6-M not ARMv7-M.
@@ -56,8 +57,8 @@ static int cortex_m_store_core_reg_u32(struct target *target,
 		uint32_t num, uint32_t value);
 static void cortex_m_dwt_free(struct target *target);
 
-static int cortexm_dap_read_coreregister_u32(struct target *target,
-	uint32_t *value, int regnum)
+static int cortex_m_load_core_reg_u32(struct target *target,
+		uint32_t regsel, uint32_t *value)
 {
 	struct armv7m_common *armv7m = target_to_armv7m(target);
 	int retval;
@@ -71,7 +72,7 @@ static int cortexm_dap_read_coreregister_u32(struct target *target,
 			return retval;
 	}
 
-	retval = mem_ap_write_u32(armv7m->debug_ap, DCB_DCRSR, regnum);
+	retval = mem_ap_write_u32(armv7m->debug_ap, DCB_DCRSR, regsel);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -89,8 +90,8 @@ static int cortexm_dap_read_coreregister_u32(struct target *target,
 	return retval;
 }
 
-static int cortexm_dap_write_coreregister_u32(struct target *target,
-	uint32_t value, int regnum)
+static int cortex_m_store_core_reg_u32(struct target *target,
+		uint32_t regsel, uint32_t value)
 {
 	struct armv7m_common *armv7m = target_to_armv7m(target);
 	int retval;
@@ -108,7 +109,7 @@ static int cortexm_dap_write_coreregister_u32(struct target *target,
 	if (retval != ERROR_OK)
 		return retval;
 
-	retval = mem_ap_write_atomic_u32(armv7m->debug_ap, DCB_DCRSR, regnum | DCRSR_WnR);
+	retval = mem_ap_write_atomic_u32(armv7m->debug_ap, DCB_DCRSR, regsel | DCRSR_WnR);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -527,12 +528,6 @@ static int cortex_m_debug_entry(struct target *target)
 	r = arm->cpsr;
 	xPSR = buf_get_u32(r->value, 0, 32);
 
-	/* For IT instructions xPSR must be reloaded on resume and clear on debug exec */
-	if (xPSR & 0xf00) {
-		r->dirty = r->valid;
-		cortex_m_store_core_reg_u32(target, 16, xPSR & ~0xff);
-	}
-
 	/* Are we in an exception handler */
 	if (xPSR & 0x1FF) {
 		armv7m->exception_number = (xPSR & 0x1FF);
@@ -541,7 +536,7 @@ static int cortex_m_debug_entry(struct target *target)
 		arm->map = armv7m_msp_reg_map;
 	} else {
 		unsigned control = buf_get_u32(arm->core_cache
-				->reg_list[ARMV7M_CONTROL].value, 0, 2);
+				->reg_list[ARMV7M_CONTROL].value, 0, 3);
 
 		/* is this thread privileged? */
 		arm->core_mode = control & 1
@@ -656,13 +651,9 @@ static int cortex_m_poll(struct target *target)
 		}
 	}
 
-	/* REVISIT when S_SLEEP is set, it's in a Sleep or DeepSleep state.
-	 * How best to model low power modes?
-	 */
-
 	if (target->state == TARGET_UNKNOWN) {
-		/* check if processor is retiring instructions */
-		if (cortex_m->dcb_dhcsr & S_RETIRE_ST) {
+		/* check if processor is retiring instructions or sleeping */
+		if (cortex_m->dcb_dhcsr & S_RETIRE_ST || cortex_m->dcb_dhcsr & S_SLEEP) {
 			target->state = TARGET_RUNNING;
 			retval = ERROR_OK;
 		}
@@ -827,15 +818,19 @@ static int cortex_m_resume(struct target *target, int current,
 		 * in parallel with disabled interrupts can cause local faults
 		 * to not be taken.
 		 *
-		 * REVISIT this clearly breaks non-debug execution, since the
-		 * PRIMASK register state isn't saved/restored...  workaround
-		 * by never resuming app code after debug execution.
+		 * This breaks non-debug (application) execution if not
+		 * called from armv7m_start_algorithm() which saves registers.
 		 */
 		buf_set_u32(r->value, 0, 1, 1);
 		r->dirty = true;
 		r->valid = true;
 
-		/* Make sure we are in Thumb mode */
+		/* Make sure we are in Thumb mode, set xPSR.T bit */
+		/* armv7m_start_algorithm() initializes entire xPSR register.
+		 * This duplicity handles the case when cortex_m_resume()
+		 * is used with the debug_execution flag directly,
+		 * not called through armv7m_start_algorithm().
+		 */
 		r = armv7m->arm.cpsr;
 		buf_set_u32(r->value, 24, 1, 1);
 		r->dirty = true;
@@ -1208,11 +1203,13 @@ static int cortex_m_assert_reset(struct target *target)
 		if (retval3 != ERROR_OK)
 			LOG_DEBUG("Ignoring AP write error right after reset");
 
-		retval3 = dap_dp_init(armv7m->debug_ap->dap);
-		if (retval3 != ERROR_OK)
+		retval3 = dap_dp_init_or_reconnect(armv7m->debug_ap->dap);
+		if (retval3 != ERROR_OK) {
 			LOG_ERROR("DP initialisation failed");
-
-		else {
+			/* The error return value must not be propagated in this case.
+			 * SYSRESETREQ or VECTRESET have been possibly triggered
+			 * so reset processing should continue */
+		} else {
 			/* I do not know why this is necessary, but it
 			 * fixes strange effects (step/resume cause NMI
 			 * after reset) on LM3S6918 -- Michael Schwingen
@@ -1255,7 +1252,8 @@ static int cortex_m_deassert_reset(struct target *target)
 	if ((jtag_reset_config & RESET_HAS_SRST) &&
 	    !(jtag_reset_config & RESET_SRST_NO_GATING) &&
 		target_was_examined(target)) {
-		int retval = dap_dp_init(armv7m->debug_ap->dap);
+
+		int retval = dap_dp_init_or_reconnect(armv7m->debug_ap->dap);
 		if (retval != ERROR_OK) {
 			LOG_ERROR("DP initialisation failed");
 			return retval;
@@ -1413,7 +1411,7 @@ int cortex_m_remove_breakpoint(struct target *target, struct breakpoint *breakpo
 	return cortex_m_unset_breakpoint(target, breakpoint);
 }
 
-int cortex_m_set_watchpoint(struct target *target, struct watchpoint *watchpoint)
+static int cortex_m_set_watchpoint(struct target *target, struct watchpoint *watchpoint)
 {
 	int dwt_num = 0;
 	struct cortex_m_common *cortex_m = target_to_cm(target);
@@ -1496,7 +1494,7 @@ int cortex_m_set_watchpoint(struct target *target, struct watchpoint *watchpoint
 	return ERROR_OK;
 }
 
-int cortex_m_unset_watchpoint(struct target *target, struct watchpoint *watchpoint)
+static int cortex_m_unset_watchpoint(struct target *target, struct watchpoint *watchpoint)
 {
 	struct cortex_m_common *cortex_m = target_to_cm(target);
 	struct cortex_m_dwt_comparator *comparator;
@@ -1610,176 +1608,6 @@ void cortex_m_enable_watchpoints(struct target *target)
 	}
 }
 
-static int cortex_m_load_core_reg_u32(struct target *target,
-		uint32_t num, uint32_t *value)
-{
-	int retval;
-
-	/* NOTE:  we "know" here that the register identifiers used
-	 * in the v7m header match the Cortex-M3 Debug Core Register
-	 * Selector values for R0..R15, xPSR, MSP, and PSP.
-	 */
-	switch (num) {
-		case 0 ... 18:
-			/* read a normal core register */
-			retval = cortexm_dap_read_coreregister_u32(target, value, num);
-
-			if (retval != ERROR_OK) {
-				LOG_ERROR("JTAG failure %i", retval);
-				return ERROR_JTAG_DEVICE_ERROR;
-			}
-			LOG_DEBUG("load from core reg %i  value 0x%" PRIx32 "", (int)num, *value);
-			break;
-
-		case ARMV7M_FPSCR:
-			/* Floating-point Status and Registers */
-			retval = target_write_u32(target, DCB_DCRSR, 0x21);
-			if (retval != ERROR_OK)
-				return retval;
-			retval = target_read_u32(target, DCB_DCRDR, value);
-			if (retval != ERROR_OK)
-				return retval;
-			LOG_DEBUG("load from FPSCR  value 0x%" PRIx32, *value);
-			break;
-
-		case ARMV7M_S0 ... ARMV7M_S31:
-			/* Floating-point Status and Registers */
-			retval = target_write_u32(target, DCB_DCRSR, num - ARMV7M_S0 + 0x40);
-			if (retval != ERROR_OK)
-				return retval;
-			retval = target_read_u32(target, DCB_DCRDR, value);
-			if (retval != ERROR_OK)
-				return retval;
-			LOG_DEBUG("load from FPU reg S%d  value 0x%" PRIx32,
-				  (int)(num - ARMV7M_S0), *value);
-			break;
-
-		case ARMV7M_PRIMASK:
-		case ARMV7M_BASEPRI:
-		case ARMV7M_FAULTMASK:
-		case ARMV7M_CONTROL:
-			/* Cortex-M3 packages these four registers as bitfields
-			 * in one Debug Core register.  So say r0 and r2 docs;
-			 * it was removed from r1 docs, but still works.
-			 */
-			cortexm_dap_read_coreregister_u32(target, value, 20);
-
-			switch (num) {
-				case ARMV7M_PRIMASK:
-					*value = buf_get_u32((uint8_t *)value, 0, 1);
-					break;
-
-				case ARMV7M_BASEPRI:
-					*value = buf_get_u32((uint8_t *)value, 8, 8);
-					break;
-
-				case ARMV7M_FAULTMASK:
-					*value = buf_get_u32((uint8_t *)value, 16, 1);
-					break;
-
-				case ARMV7M_CONTROL:
-					*value = buf_get_u32((uint8_t *)value, 24, 2);
-					break;
-			}
-
-			LOG_DEBUG("load from special reg %i value 0x%" PRIx32 "", (int)num, *value);
-			break;
-
-		default:
-			return ERROR_COMMAND_SYNTAX_ERROR;
-	}
-
-	return ERROR_OK;
-}
-
-static int cortex_m_store_core_reg_u32(struct target *target,
-		uint32_t num, uint32_t value)
-{
-	int retval;
-	uint32_t reg;
-	struct armv7m_common *armv7m = target_to_armv7m(target);
-
-	/* NOTE:  we "know" here that the register identifiers used
-	 * in the v7m header match the Cortex-M3 Debug Core Register
-	 * Selector values for R0..R15, xPSR, MSP, and PSP.
-	 */
-	switch (num) {
-		case 0 ... 18:
-			retval = cortexm_dap_write_coreregister_u32(target, value, num);
-			if (retval != ERROR_OK) {
-				struct reg *r;
-
-				LOG_ERROR("JTAG failure");
-				r = armv7m->arm.core_cache->reg_list + num;
-				r->dirty = r->valid;
-				return ERROR_JTAG_DEVICE_ERROR;
-			}
-			LOG_DEBUG("write core reg %i value 0x%" PRIx32 "", (int)num, value);
-			break;
-
-		case ARMV7M_FPSCR:
-			/* Floating-point Status and Registers */
-			retval = target_write_u32(target, DCB_DCRDR, value);
-			if (retval != ERROR_OK)
-				return retval;
-			retval = target_write_u32(target, DCB_DCRSR, 0x21 | (1<<16));
-			if (retval != ERROR_OK)
-				return retval;
-			LOG_DEBUG("write FPSCR value 0x%" PRIx32, value);
-			break;
-
-		case ARMV7M_S0 ... ARMV7M_S31:
-			/* Floating-point Status and Registers */
-			retval = target_write_u32(target, DCB_DCRDR, value);
-			if (retval != ERROR_OK)
-				return retval;
-			retval = target_write_u32(target, DCB_DCRSR, (num - ARMV7M_S0 + 0x40) | (1<<16));
-			if (retval != ERROR_OK)
-				return retval;
-			LOG_DEBUG("write FPU reg S%d  value 0x%" PRIx32,
-				  (int)(num - ARMV7M_S0), value);
-			break;
-
-		case ARMV7M_PRIMASK:
-		case ARMV7M_BASEPRI:
-		case ARMV7M_FAULTMASK:
-		case ARMV7M_CONTROL:
-			/* Cortex-M3 packages these four registers as bitfields
-			 * in one Debug Core register.  So say r0 and r2 docs;
-			 * it was removed from r1 docs, but still works.
-			 */
-			cortexm_dap_read_coreregister_u32(target, &reg, 20);
-
-			switch (num) {
-				case ARMV7M_PRIMASK:
-					buf_set_u32((uint8_t *)&reg, 0, 1, value);
-					break;
-
-				case ARMV7M_BASEPRI:
-					buf_set_u32((uint8_t *)&reg, 8, 8, value);
-					break;
-
-				case ARMV7M_FAULTMASK:
-					buf_set_u32((uint8_t *)&reg, 16, 1, value);
-					break;
-
-				case ARMV7M_CONTROL:
-					buf_set_u32((uint8_t *)&reg, 24, 2, value);
-					break;
-			}
-
-			cortexm_dap_write_coreregister_u32(target, reg, 20);
-
-			LOG_DEBUG("write special reg %i value 0x%" PRIx32 " ", (int)num, value);
-			break;
-
-		default:
-			return ERROR_COMMAND_SYNTAX_ERROR;
-	}
-
-	return ERROR_OK;
-}
-
 static int cortex_m_read_memory(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, uint8_t *buffer)
 {
@@ -1820,6 +1648,8 @@ void cortex_m_deinit_target(struct target *target)
 {
 	struct cortex_m_common *cortex_m = target_to_cm(target);
 
+	armv7m_trace_tpiu_exit(target);
+
 	free(cortex_m->fp_comparator_list);
 
 	cortex_m_dwt_free(target);
@@ -1835,28 +1665,23 @@ int cortex_m_profiling(struct target *target, uint32_t *samples,
 	struct timeval timeout, now;
 	struct armv7m_common *armv7m = target_to_armv7m(target);
 	uint32_t reg_value;
-	bool use_pcsr = false;
-	int retval = ERROR_OK;
-	struct reg *reg;
-
-	gettimeofday(&timeout, NULL);
-	timeval_add_time(&timeout, seconds, 0);
+	int retval;
 
 	retval = target_read_u32(target, DWT_PCSR, &reg_value);
 	if (retval != ERROR_OK) {
 		LOG_ERROR("Error while reading PCSR");
 		return retval;
 	}
-
-	if (reg_value != 0) {
-		use_pcsr = true;
-		LOG_INFO("Starting Cortex-M profiling. Sampling DWT_PCSR as fast as we can...");
-	} else {
-		LOG_INFO("Starting profiling. Halting and resuming the"
-			 " target as often as we can...");
-		reg = register_get_by_name(target->reg_cache, "pc", 1);
+	if (reg_value == 0) {
+		LOG_INFO("PCSR sampling not supported on this processor.");
+		return target_profiling_default(target, samples, max_num_samples, num_samples, seconds);
 	}
 
+	gettimeofday(&timeout, NULL);
+	timeval_add_time(&timeout, seconds, 0);
+
+	LOG_INFO("Starting Cortex-M profiling. Sampling DWT_PCSR as fast as we can...");
+
 	/* Make sure the target is running */
 	target_poll(target);
 	if (target->state == TARGET_HALTED)
@@ -1870,40 +1695,21 @@ int cortex_m_profiling(struct target *target, uint32_t *samples,
 	uint32_t sample_count = 0;
 
 	for (;;) {
-		if (use_pcsr) {
-			if (armv7m && armv7m->debug_ap) {
-				uint32_t read_count = max_num_samples - sample_count;
-				if (read_count > 1024)
-					read_count = 1024;
-
-				retval = mem_ap_read_buf_noincr(armv7m->debug_ap,
-							(void *)&samples[sample_count],
-							4, read_count, DWT_PCSR);
-				sample_count += read_count;
-			} else {
-				target_read_u32(target, DWT_PCSR, &samples[sample_count++]);
-			}
+		if (armv7m && armv7m->debug_ap) {
+			uint32_t read_count = max_num_samples - sample_count;
+			if (read_count > 1024)
+				read_count = 1024;
+
+			retval = mem_ap_read_buf_noincr(armv7m->debug_ap,
+						(void *)&samples[sample_count],
+						4, read_count, DWT_PCSR);
+			sample_count += read_count;
 		} else {
-			target_poll(target);
-			if (target->state == TARGET_HALTED) {
-				reg_value = buf_get_u32(reg->value, 0, 32);
-				/* current pc, addr = 0, do not handle breakpoints, not debugging */
-				retval = target_resume(target, 1, 0, 0, 0);
-				samples[sample_count++] = reg_value;
-				target_poll(target);
-				alive_sleep(10); /* sleep 10ms, i.e. <100 samples/second. */
-			} else if (target->state == TARGET_RUNNING) {
-				/* We want to quickly sample the PC. */
-				retval = target_halt(target);
-			} else {
-				LOG_INFO("Target not halted or running");
-				retval = ERROR_OK;
-				break;
-			}
+			target_read_u32(target, DWT_PCSR, &samples[sample_count++]);
 		}
 
 		if (retval != ERROR_OK) {
-			LOG_ERROR("Error while reading %s", use_pcsr ? "PCSR" : "target pc");
+			LOG_ERROR("Error while reading PCSR");
 			return retval;
 		}
 
@@ -2013,7 +1819,7 @@ static void cortex_m_dwt_addreg(struct target *t, struct reg *r, const struct dw
 	r->type = &dwt_reg_type;
 }
 
-void cortex_m_dwt_setup(struct cortex_m_common *cm, struct target *target)
+static void cortex_m_dwt_setup(struct cortex_m_common *cm, struct target *target)
 {
 	uint32_t dwtcr;
 	struct reg_cache *cache;
@@ -2242,7 +2048,6 @@ int cortex_m_examine(struct target *target)
 			for (idx = ARMV7M_NUM_CORE_REGS_NOFP;
 			     idx < armv7m->arm.core_cache->num_regs;
 			     idx++) {
-				free(armv7m->arm.core_cache->reg_list[idx].value);
 				free(armv7m->arm.core_cache->reg_list[idx].feature);
 				free(armv7m->arm.core_cache->reg_list[idx].reg_data_type);
 			}
@@ -2687,6 +2492,9 @@ static const struct command_registration cortex_m_command_handlers[] = {
 		.usage = "",
 		.chain = cortex_m_exec_command_handlers,
 	},
+	{
+		.chain = rtt_target_command_handlers,
+	},
 	COMMAND_REGISTRATION_DONE
 };
 
diff --git a/src/target/cortex_m.h b/src/target/cortex_m.h
index 415a6c2..b470fbd 100644
--- a/src/target/cortex_m.h
+++ b/src/target/cortex_m.h
@@ -233,13 +233,10 @@ int cortex_m_set_breakpoint(struct target *target, struct breakpoint *breakpoint
 int cortex_m_unset_breakpoint(struct target *target, struct breakpoint *breakpoint);
 int cortex_m_add_breakpoint(struct target *target, struct breakpoint *breakpoint);
 int cortex_m_remove_breakpoint(struct target *target, struct breakpoint *breakpoint);
-int cortex_m_set_watchpoint(struct target *target, struct watchpoint *watchpoint);
-int cortex_m_unset_watchpoint(struct target *target, struct watchpoint *watchpoint);
 int cortex_m_add_watchpoint(struct target *target, struct watchpoint *watchpoint);
 int cortex_m_remove_watchpoint(struct target *target, struct watchpoint *watchpoint);
 void cortex_m_enable_breakpoints(struct target *target);
 void cortex_m_enable_watchpoints(struct target *target);
-void cortex_m_dwt_setup(struct cortex_m_common *cm, struct target *target);
 void cortex_m_deinit_target(struct target *target);
 int cortex_m_profiling(struct target *target, uint32_t *samples,
 	uint32_t max_num_samples, uint32_t *num_samples, uint32_t seconds);
diff --git a/src/target/dsp5680xx.c b/src/target/dsp5680xx.c
index d6107ab..ee26d24 100644
--- a/src/target/dsp5680xx.c
+++ b/src/target/dsp5680xx.c
@@ -40,7 +40,7 @@ struct dsp5680xx_common dsp5680xx_context;
 #define CHECK_HALT(target) if (target->state != TARGET_HALTED) HALT_FAIL
 #define check_halt_and_debug(target) { CHECK_HALT(target); CHECK_DBG; }
 
-int dsp5680xx_execute_queue(void)
+static int dsp5680xx_execute_queue(void)
 {
 	int retval;
 
@@ -892,12 +892,6 @@ static int dsp5680xx_arch_state(struct target *target)
 	return ERROR_OK;
 }
 
-int dsp5680xx_target_status(struct target *target, uint8_t *jtag_st,
-			    uint16_t *eonce_st)
-{
-	return target->state;
-}
-
 static int dsp5680xx_assert_reset(struct target *target)
 {
 	target->state = TARGET_RESET;
@@ -1555,7 +1549,7 @@ static int perl_crc(const uint8_t *buff8, uint32_t word_count)
  *
  * @return
  */
-int dsp5680xx_f_SIM_reset(struct target *target)
+static int dsp5680xx_f_SIM_reset(struct target *target)
 {
 	int retval = ERROR_OK;
 
@@ -1978,7 +1972,8 @@ int dsp5680xx_f_erase(struct target *target, int first, int last)
  * 0x0000001E  0xA961		 bra	 *-30
  */
 
-const uint16_t pgm_write_pflash[] = { 0x8A46, 0x0013, 0x807D, 0xE700,
+static const uint16_t pgm_write_pflash[] = {
+		0x8A46, 0x0013, 0x807D, 0xE700,
 		0xE700, 0x8A44, 0xFFFE, 0x017B,
 		0xE700, 0xF514, 0x8563, 0x8646,
 		0x0020, 0x0014, 0x8646, 0x0080,
@@ -1988,7 +1983,7 @@ const uint16_t pgm_write_pflash[] = { 0x8A46, 0x0013, 0x807D, 0xE700,
 		0x0013, 0x0010, 0xA961
 };
 
-const uint32_t pgm_write_pflash_length = 31;
+static const uint32_t pgm_write_pflash_length = 31;
 
 int dsp5680xx_f_wr(struct target *t, const uint8_t *b, uint32_t a, uint32_t count,
 		   int is_flash_lock)
diff --git a/src/target/etm.c b/src/target/etm.c
index 5d079ff..faa941f 100644
--- a/src/target/etm.c
+++ b/src/target/etm.c
@@ -279,7 +279,7 @@ static void etm_reg_add(unsigned bcd_vers, struct arm_jtag *jtag_info,
 
 		reg->name = r->name;
 		reg->size = r->size;
-		reg->value = &ereg->value;
+		reg->value = ereg->value;
 		reg->arch_info = ereg;
 		reg->type = &etm_scan6_type;
 		reg++;
@@ -650,7 +650,6 @@ static struct etm_capture_driver *etm_capture_drivers[] = {
 
 static int etm_read_instruction(struct etm_context *ctx, struct arm_instruction *instruction)
 {
-	int i;
 	int section = -1;
 	size_t size_read;
 	uint32_t opcode;
@@ -660,7 +659,7 @@ static int etm_read_instruction(struct etm_context *ctx, struct arm_instruction
 		return ERROR_TRACE_IMAGE_UNAVAILABLE;
 
 	/* search for the section the current instruction belongs to */
-	for (i = 0; i < ctx->image->num_sections; i++) {
+	for (unsigned int i = 0; i < ctx->image->num_sections; i++) {
 		if ((ctx->image->sections[i].base_address <= ctx->current_pc) &&
 			(ctx->image->sections[i].base_address + ctx->image->sections[i].size >
 			ctx->current_pc)) {
@@ -1683,15 +1682,15 @@ COMMAND_HANDLER(handle_etm_image_command)
 	}
 
 	etm_ctx->image = malloc(sizeof(struct image));
-	etm_ctx->image->base_address_set = 0;
-	etm_ctx->image->start_address_set = 0;
+	etm_ctx->image->base_address_set = false;
+	etm_ctx->image->start_address_set = false;
 
 	/* a base address isn't always necessary, default to 0x0 (i.e. don't relocate) */
 	if (CMD_ARGC >= 2) {
-		etm_ctx->image->base_address_set = 1;
+		etm_ctx->image->base_address_set = true;
 		COMMAND_PARSE_NUMBER(llong, CMD_ARGV[1], etm_ctx->image->base_address);
 	} else
-		etm_ctx->image->base_address_set = 0;
+		etm_ctx->image->base_address_set = false;
 
 	if (image_open(etm_ctx->image, CMD_ARGV[0],
 		(CMD_ARGC >= 3) ? CMD_ARGV[2] : NULL) != ERROR_OK) {
diff --git a/src/target/hla_target.c b/src/target/hla_target.c
index f0dc572..3d41387 100644
--- a/src/target/hla_target.c
+++ b/src/target/hla_target.c
@@ -39,6 +39,7 @@
 #include "cortex_m.h"
 #include "arm_semihosting.h"
 #include "target_request.h"
+#include <rtt/rtt.h>
 
 #define savedDCRDR  dbgbase  /* FIXME: using target->dbgbase to preserve DCRDR */
 
@@ -51,184 +52,17 @@ static inline struct hl_interface_s *target_to_adapter(struct target *target)
 }
 
 static int adapter_load_core_reg_u32(struct target *target,
-		uint32_t num, uint32_t *value)
+		uint32_t regsel, uint32_t *value)
 {
-	int retval;
 	struct hl_interface_s *adapter = target_to_adapter(target);
-
-	LOG_DEBUG("%s", __func__);
-
-	/* NOTE:  we "know" here that the register identifiers used
-	 * in the v7m header match the Cortex-M3 Debug Core Register
-	 * Selector values for R0..R15, xPSR, MSP, and PSP.
-	 */
-	switch (num) {
-	case 0 ... 18:
-		/* read a normal core register */
-		retval = adapter->layout->api->read_reg(adapter->handle, num, value);
-
-		if (retval != ERROR_OK) {
-			LOG_ERROR("JTAG failure %i", retval);
-			return ERROR_JTAG_DEVICE_ERROR;
-		}
-		LOG_DEBUG("load from core reg %i  value 0x%" PRIx32 "", (int)num, *value);
-		break;
-
-	case ARMV7M_FPSCR:
-		/* Floating-point Status and Registers */
-		retval = target_write_u32(target, ARMV7M_SCS_DCRSR, 33);
-		if (retval != ERROR_OK)
-			return retval;
-		retval = target_read_u32(target, ARMV7M_SCS_DCRDR, value);
-		if (retval != ERROR_OK)
-			return retval;
-		LOG_DEBUG("load from FPSCR  value 0x%" PRIx32, *value);
-		break;
-
-	case ARMV7M_S0 ... ARMV7M_S31:
-		/* Floating-point Status and Registers */
-		retval = target_write_u32(target, ARMV7M_SCS_DCRSR, num-ARMV7M_S0+64);
-		if (retval != ERROR_OK)
-			return retval;
-		retval = target_read_u32(target, ARMV7M_SCS_DCRDR, value);
-		if (retval != ERROR_OK)
-			return retval;
-		LOG_DEBUG("load from FPU reg S%d  value 0x%" PRIx32,
-			  (int)(num - ARMV7M_S0), *value);
-		break;
-
-	case ARMV7M_PRIMASK:
-	case ARMV7M_BASEPRI:
-	case ARMV7M_FAULTMASK:
-	case ARMV7M_CONTROL:
-		/* Cortex-M3 packages these four registers as bitfields
-		 * in one Debug Core register.  So say r0 and r2 docs;
-		 * it was removed from r1 docs, but still works.
-		 */
-		retval = adapter->layout->api->read_reg(adapter->handle, 20, value);
-		if (retval != ERROR_OK)
-			return retval;
-
-		switch (num) {
-		case ARMV7M_PRIMASK:
-			*value = buf_get_u32((uint8_t *) value, 0, 1);
-			break;
-
-		case ARMV7M_BASEPRI:
-			*value = buf_get_u32((uint8_t *) value, 8, 8);
-			break;
-
-		case ARMV7M_FAULTMASK:
-			*value = buf_get_u32((uint8_t *) value, 16, 1);
-			break;
-
-		case ARMV7M_CONTROL:
-			*value = buf_get_u32((uint8_t *) value, 24, 2);
-			break;
-		}
-
-		LOG_DEBUG("load from special reg %i value 0x%" PRIx32 "",
-			  (int)num, *value);
-		break;
-
-	default:
-		return ERROR_COMMAND_SYNTAX_ERROR;
-	}
-
-	return ERROR_OK;
+	return adapter->layout->api->read_reg(adapter->handle, regsel, value);
 }
 
 static int adapter_store_core_reg_u32(struct target *target,
-		uint32_t num, uint32_t value)
+		uint32_t regsel, uint32_t value)
 {
-	int retval;
-	uint32_t reg;
-	struct armv7m_common *armv7m = target_to_armv7m(target);
 	struct hl_interface_s *adapter = target_to_adapter(target);
-
-	LOG_DEBUG("%s", __func__);
-
-	/* NOTE:  we "know" here that the register identifiers used
-	 * in the v7m header match the Cortex-M3 Debug Core Register
-	 * Selector values for R0..R15, xPSR, MSP, and PSP.
-	 */
-	switch (num) {
-	case 0 ... 18:
-		retval = adapter->layout->api->write_reg(adapter->handle, num, value);
-
-		if (retval != ERROR_OK) {
-			struct reg *r;
-
-			LOG_ERROR("JTAG failure");
-			r = armv7m->arm.core_cache->reg_list + num;
-			r->dirty = r->valid;
-			return ERROR_JTAG_DEVICE_ERROR;
-		}
-		LOG_DEBUG("write core reg %i value 0x%" PRIx32 "", (int)num, value);
-		break;
-
-	case ARMV7M_FPSCR:
-		/* Floating-point Status and Registers */
-		retval = target_write_u32(target, ARMV7M_SCS_DCRDR, value);
-		if (retval != ERROR_OK)
-			return retval;
-		retval = target_write_u32(target, ARMV7M_SCS_DCRSR, 33 | (1<<16));
-		if (retval != ERROR_OK)
-			return retval;
-		LOG_DEBUG("write FPSCR value 0x%" PRIx32, value);
-		break;
-
-	case ARMV7M_S0 ... ARMV7M_S31:
-		/* Floating-point Status and Registers */
-		retval = target_write_u32(target, ARMV7M_SCS_DCRDR, value);
-		if (retval != ERROR_OK)
-			return retval;
-		retval = target_write_u32(target, ARMV7M_SCS_DCRSR, (num-ARMV7M_S0+64) | (1<<16));
-		if (retval != ERROR_OK)
-			return retval;
-		LOG_DEBUG("write FPU reg S%d  value 0x%" PRIx32,
-			  (int)(num - ARMV7M_S0), value);
-		break;
-
-	case ARMV7M_PRIMASK:
-	case ARMV7M_BASEPRI:
-	case ARMV7M_FAULTMASK:
-	case ARMV7M_CONTROL:
-		/* Cortex-M3 packages these four registers as bitfields
-		 * in one Debug Core register.  So say r0 and r2 docs;
-		 * it was removed from r1 docs, but still works.
-		 */
-
-		adapter->layout->api->read_reg(adapter->handle, 20, &reg);
-
-		switch (num) {
-		case ARMV7M_PRIMASK:
-			buf_set_u32((uint8_t *) &reg, 0, 1, value);
-			break;
-
-		case ARMV7M_BASEPRI:
-			buf_set_u32((uint8_t *) &reg, 8, 8, value);
-			break;
-
-		case ARMV7M_FAULTMASK:
-			buf_set_u32((uint8_t *) &reg, 16, 1, value);
-			break;
-
-		case ARMV7M_CONTROL:
-			buf_set_u32((uint8_t *) &reg, 24, 2, value);
-			break;
-		}
-
-		adapter->layout->api->write_reg(adapter->handle, 20, reg);
-
-		LOG_DEBUG("write special reg %i value 0x%" PRIx32 " ", (int)num, value);
-		break;
-
-	default:
-		return ERROR_COMMAND_SYNTAX_ERROR;
-	}
-
-	return ERROR_OK;
+	return adapter->layout->api->write_reg(adapter->handle, regsel, value);
 }
 
 static int adapter_examine_debug_reason(struct target *target)
@@ -433,7 +267,7 @@ static int adapter_debug_entry(struct target *target)
 		arm->map = armv7m_msp_reg_map;
 	} else {
 		unsigned control = buf_get_u32(arm->core_cache
-				->reg_list[ARMV7M_CONTROL].value, 0, 2);
+				->reg_list[ARMV7M_CONTROL].value, 0, 3);
 
 		/* is this thread privileged? */
 		arm->core_mode = control & 1
@@ -793,6 +627,9 @@ static const struct command_registration adapter_command_handlers[] = {
 	{
 		.chain = armv7m_trace_command_handlers,
 	},
+	{
+		.chain = rtt_target_command_handlers,
+	},
 	COMMAND_REGISTRATION_DONE
 };
 
diff --git a/src/target/image.c b/src/target/image.c
index 68262e9..fd5eff8 100644
--- a/src/target/image.c
+++ b/src/target/image.c
@@ -124,7 +124,6 @@ static int image_ihex_buffer_complete_inner(struct image *image,
 	uint32_t full_address;
 	uint32_t cooked_bytes;
 	bool end_rec = false;
-	int i;
 
 	/* we can't determine the number of sections that we'll have to create ahead of time,
 	 * so we locally hold them until parsing is finished */
@@ -207,7 +206,7 @@ static int image_ihex_buffer_complete_inner(struct image *image,
 
 				/* copy section information */
 				image->sections = malloc(sizeof(struct imagesection) * image->num_sections);
-				for (i = 0; i < image->num_sections; i++) {
+				for (unsigned int i = 0; i < image->num_sections; i++) {
 					image->sections[i].private = section[i].private;
 					image->sections[i].base_address = section[i].base_address;
 					image->sections[i].size = section[i].size;
@@ -294,7 +293,7 @@ static int image_ihex_buffer_complete_inner(struct image *image,
 				cal_checksum += (uint8_t)start_address;
 				bytes_read += 8;
 
-				image->start_address_set = 1;
+				image->start_address_set = true;
 				image->start_address = be_to_h_u32((uint8_t *)&start_address);
 			} else {
 				LOG_ERROR("unhandled IHEX record type: %i", (int)record_type);
@@ -471,7 +470,7 @@ static int image_elf_read_headers(struct image *image)
 		}
 	}
 
-	image->start_address_set = 1;
+	image->start_address_set = true;
 	image->start_address = field32(elf, elf->header->e_entry);
 
 	return ERROR_OK;
@@ -529,7 +528,6 @@ static int image_mot_buffer_complete_inner(struct image *image,
 	uint32_t full_address;
 	uint32_t cooked_bytes;
 	bool end_rec = false;
-	int i;
 
 	/* we can't determine the number of sections that we'll have to create ahead of time,
 	 * so we locally hold them until parsing is finished */
@@ -658,7 +656,7 @@ static int image_mot_buffer_complete_inner(struct image *image,
 
 				/* copy section information */
 				image->sections = malloc(sizeof(struct imagesection) * image->num_sections);
-				for (i = 0; i < image->num_sections; i++) {
+				for (unsigned int i = 0; i < image->num_sections; i++) {
 					image->sections[i].private = section[i].private;
 					image->sections[i].base_address = section[i].base_address;
 					image->sections[i].size = section[i].size;
@@ -821,21 +819,20 @@ int image_open(struct image *image, const char *url, const char *type_string)
 		}
 	} else if (image->type == IMAGE_BUILDER) {
 		image->num_sections = 0;
-		image->base_address_set = 0;
+		image->base_address_set = false;
 		image->sections = NULL;
 		image->type_private = NULL;
 	}
 
 	if (image->base_address_set) {
 		/* relocate */
-		int section;
-		for (section = 0; section < image->num_sections; section++)
+		for (unsigned int section = 0; section < image->num_sections; section++)
 			image->sections[section].base_address += image->base_address;
 											/* we're done relocating. The two statements below are mainly
 											* for documentation purposes: stop anyone from empirically
 											* thinking they should use these values henceforth. */
 		image->base_address = 0;
-		image->base_address_set = 0;
+		image->base_address_set = false;
 	}
 
 	return retval;
@@ -1009,9 +1006,7 @@ void image_close(struct image *image)
 		free(image_mot->buffer);
 		image_mot->buffer = NULL;
 	} else if (image->type == IMAGE_BUILDER) {
-		int i;
-
-		for (i = 0; i < image->num_sections; i++) {
+		for (unsigned int i = 0; i < image->num_sections; i++) {
 			free(image->sections[i].private);
 			image->sections[i].private = NULL;
 		}
@@ -1024,7 +1019,7 @@ void image_close(struct image *image)
 	image->sections = NULL;
 }
 
-int image_calculate_checksum(uint8_t *buffer, uint32_t nbytes, uint32_t *checksum)
+int image_calculate_checksum(const uint8_t *buffer, uint32_t nbytes, uint32_t *checksum)
 {
 	uint32_t crc = 0xffffffff;
 	LOG_DEBUG("Calculating checksum");
diff --git a/src/target/image.h b/src/target/image.h
index 9907a5f..53c27d8 100644
--- a/src/target/image.h
+++ b/src/target/image.h
@@ -55,11 +55,11 @@ struct imagesection {
 struct image {
 	enum image_type type;		/* image type (plain, ihex, ...) */
 	void *type_private;		/* type private data */
-	int num_sections;		/* number of sections contained in the image */
+	unsigned int num_sections;		/* number of sections contained in the image */
 	struct imagesection *sections;	/* array of sections */
-	int base_address_set;	/* whether the image has a base address set (for relocation purposes) */
+	bool base_address_set;	/* whether the image has a base address set (for relocation purposes) */
 	long long base_address;		/* base address, if one is set */
-	int start_address_set;	/* whether the image has a start address (entry point) associated */
+	bool start_address_set;	/* whether the image has a start address (entry point) associated */
 	uint32_t start_address;		/* start address, if one is set */
 };
 
@@ -99,7 +99,7 @@ void image_close(struct image *image);
 int image_add_section(struct image *image, uint32_t base, uint32_t size,
 		int flags, uint8_t const *data);
 
-int image_calculate_checksum(uint8_t *buffer, uint32_t nbytes,
+int image_calculate_checksum(const uint8_t *buffer, uint32_t nbytes,
 		uint32_t *checksum);
 
 #define ERROR_IMAGE_FORMAT_ERROR	(-1400)
diff --git a/src/target/mips32_pracc.c b/src/target/mips32_pracc.c
index 9bac40e..d6bd1c5 100644
--- a/src/target/mips32_pracc.c
+++ b/src/target/mips32_pracc.c
@@ -120,7 +120,7 @@ static void mips32_pracc_finish(struct mips_ejtag *ejtag_info)
 	mips_ejtag_drscan_32_out(ejtag_info, ctrl);
 }
 
-int mips32_pracc_clean_text_jump(struct mips_ejtag *ejtag_info)
+static int mips32_pracc_clean_text_jump(struct mips_ejtag *ejtag_info)
 {
 	uint32_t jt_code = MIPS32_J(ejtag_info->isa, MIPS32_PRACC_TEXT);
 	pracc_swap16_array(ejtag_info, &jt_code, 1);
@@ -453,7 +453,7 @@ exit:
 	return retval;
 }
 
-int mips32_pracc_read_u32(struct mips_ejtag *ejtag_info, uint32_t addr, uint32_t *buf)
+static int mips32_pracc_read_u32(struct mips_ejtag *ejtag_info, uint32_t addr, uint32_t *buf)
 {
 	struct pracc_queue_info ctx = {.ejtag_info = ejtag_info};
 	pracc_queue_init(&ctx);
diff --git a/src/target/mips_ejtag.c b/src/target/mips_ejtag.c
index 7544afe..4b049fb 100644
--- a/src/target/mips_ejtag.c
+++ b/src/target/mips_ejtag.c
@@ -58,7 +58,7 @@ int mips_ejtag_get_idcode(struct mips_ejtag *ejtag_info)
 	return mips_ejtag_drscan_32(ejtag_info, &ejtag_info->idcode);
 }
 
-int mips_ejtag_get_impcode(struct mips_ejtag *ejtag_info)
+static int mips_ejtag_get_impcode(struct mips_ejtag *ejtag_info)
 {
 	mips_ejtag_set_instr(ejtag_info, EJTAG_INST_IMPCODE);
 
@@ -119,7 +119,8 @@ int mips_ejtag_drscan_64(struct mips_ejtag *ejtag_info, uint64_t *data)
 	return ERROR_OK;
 }
 
-void mips_ejtag_drscan_32_queued(struct mips_ejtag *ejtag_info, uint32_t data_out, uint8_t *data_in)
+static void mips_ejtag_drscan_32_queued(struct mips_ejtag *ejtag_info,
+		uint32_t data_out, uint8_t *data_in)
 {
 	assert(ejtag_info->tap != NULL);
 	struct jtag_tap *tap = ejtag_info->tap;
diff --git a/src/target/mips_mips64.c b/src/target/mips_mips64.c
index f941af5..0fc0897 100644
--- a/src/target/mips_mips64.c
+++ b/src/target/mips_mips64.c
@@ -62,7 +62,7 @@ static int mips_mips64_debug_entry(struct target *target)
 	mips_mips64_examine_debug_reason(target);
 
 	LOG_DEBUG("entered debug state at PC 0x%" PRIx64 ", target->state: %s",
-		  *(uint64_t *)pc->value, target_state_name(target));
+		  buf_get_u64(pc->value, 0, 64), target_state_name(target));
 
 	return ERROR_OK;
 }
diff --git a/src/target/nds32.c b/src/target/nds32.c
index 487e19c..add66b2 100644
--- a/src/target/nds32.c
+++ b/src/target/nds32.c
@@ -2496,6 +2496,12 @@ int nds32_profiling(struct target *target, uint32_t *samples,
 	struct aice_port_s *aice = target_to_aice(target);
 	struct nds32 *nds32 = target_to_nds32(target);
 
+	/* REVISIT: can nds32 profile without halting? */
+	if (target->state != TARGET_HALTED) {
+		LOG_WARNING("target %s is not halted (profiling)", target->cmd_name);
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
 	if (max_num_samples < iteration)
 		iteration = max_num_samples;
 
diff --git a/src/target/nds32_tlb.c b/src/target/nds32_tlb.c
index c4bce1a..93a9241 100644
--- a/src/target/nds32_tlb.c
+++ b/src/target/nds32_tlb.c
@@ -31,7 +31,7 @@ int nds32_probe_tlb(struct nds32 *nds32, const target_addr_t virtual_address,
 	return aice_read_tlb(aice, virtual_address, physical_address);
 }
 
-struct page_table_walker_info_s page_table_info[PAGE_SIZE_NUM] = {
+static struct page_table_walker_info_s page_table_info[PAGE_SIZE_NUM] = {
 	/* 4K page */
 	{0xFFC00000, 20, 0x003FF000, 10, 0x00000FFF, 0xFFFFF000, 0xFFFFF000, 0xFFFFF000},
 	/* 8K page */
diff --git a/src/target/nds32_v3.c b/src/target/nds32_v3.c
index e5d146b..f9cd47a 100644
--- a/src/target/nds32_v3.c
+++ b/src/target/nds32_v3.c
@@ -404,7 +404,7 @@ static int nds32_v3_remove_watchpoint(struct target *target,
 	return ERROR_OK;
 }
 
-struct nds32_v3_common_callback nds32_v3_common_callback = {
+static struct nds32_v3_common_callback nds32_v3_common_callback = {
 	.check_interrupt_stack = nds32_v3_check_interrupt_stack,
 	.restore_interrupt_stack = nds32_v3_restore_interrupt_stack,
 	.activate_hardware_breakpoint = nds32_v3_activate_hardware_breakpoint,
diff --git a/src/target/nds32_v3m.c b/src/target/nds32_v3m.c
index 86903a5..952d0eb 100644
--- a/src/target/nds32_v3m.c
+++ b/src/target/nds32_v3m.c
@@ -379,7 +379,7 @@ static int nds32_v3m_remove_watchpoint(struct target *target,
 	return ERROR_OK;
 }
 
-struct nds32_v3_common_callback nds32_v3m_common_callback = {
+static struct nds32_v3_common_callback nds32_v3m_common_callback = {
 	.check_interrupt_stack = nds32_v3m_check_interrupt_stack,
 	.restore_interrupt_stack = nds32_v3m_restore_interrupt_stack,
 	.activate_hardware_breakpoint = nds32_v3m_activate_hardware_breakpoint,
diff --git a/src/target/openrisc/jsp_server.c b/src/target/openrisc/jsp_server.c
index 1d05944..b4b2566 100644
--- a/src/target/openrisc/jsp_server.c
+++ b/src/target/openrisc/jsp_server.c
@@ -57,7 +57,7 @@ static int telnet_write(struct connection *connection, const void *data, int len
 	return ERROR_SERVER_REMOTE_CLOSED;
 }
 
-int jsp_poll_read(void *priv)
+static int jsp_poll_read(void *priv)
 {
 	struct jsp_service *jsp_service = (struct jsp_service *)priv;
 	unsigned char out_buffer[10];
@@ -207,7 +207,8 @@ int jsp_init(struct or1k_jtag *jtag_info, char *banner)
 		jsp_new_connection,
 		jsp_input,
 		jsp_connection_closed,
-		jsp_service);
+		jsp_service,
+		NULL);
 }
 
 COMMAND_HANDLER(handle_jsp_port_command)
diff --git a/src/target/openrisc/or1k.c b/src/target/openrisc/or1k.c
index d685359..5b8d7de 100644
--- a/src/target/openrisc/or1k.c
+++ b/src/target/openrisc/or1k.c
@@ -1200,7 +1200,7 @@ static int or1k_get_gdb_reg_list(struct target *target, struct reg **reg_list[],
 
 }
 
-int or1k_get_gdb_fileio_info(struct target *target, struct gdb_fileio_info *fileio_info)
+static int or1k_get_gdb_fileio_info(struct target *target, struct gdb_fileio_info *fileio_info)
 {
 	return ERROR_FAIL;
 }
diff --git a/src/target/quark_d20xx.c b/src/target/quark_d20xx.c
index 42d3b8c..9169379 100644
--- a/src/target/quark_d20xx.c
+++ b/src/target/quark_d20xx.c
@@ -43,7 +43,7 @@
 #include "lakemont.h"
 #include "x86_32_common.h"
 
-int quark_d20xx_target_create(struct target *t, Jim_Interp *interp)
+static int quark_d20xx_target_create(struct target *t, Jim_Interp *interp)
 {
 	struct x86_32_common *x86_32 = calloc(1, sizeof(struct x86_32_common));
 	if (x86_32 == NULL) {
@@ -56,7 +56,7 @@ int quark_d20xx_target_create(struct target *t, Jim_Interp *interp)
 	return ERROR_OK;
 }
 
-int quark_d20xx_init_target(struct command_context *cmd_ctx, struct target *t)
+static int quark_d20xx_init_target(struct command_context *cmd_ctx, struct target *t)
 {
 	return lakemont_init_target(cmd_ctx, t);
 }
diff --git a/src/target/register.h b/src/target/register.h
index 7c53d6e..5f1c25f 100644
--- a/src/target/register.h
+++ b/src/target/register.h
@@ -127,13 +127,15 @@ struct reg {
 	bool caller_save;
 	/* Pointer to place where the value is stored, in the format understood by
 	 * the binarybuffer.h functions. */
-	void *value;
+	uint8_t *value;
 	/* The stored value needs to be written to the target. */
 	bool dirty;
 	/* When true, value is valid. */
 	bool valid;
 	/* When false, the register doesn't actually exist in the target. */
 	bool exist;
+	/* Hide the register from gdb and omit it in 'reg' cmd output */
+	bool hidden;
 	/* Size of the register in bits. */
 	uint32_t size;
 	/* Used for generating XML description of registers. Can be set to NULL for
diff --git a/src/target/riscv/riscv.c b/src/target/riscv/riscv.c
index 2c34795..9978931 100644
--- a/src/target/riscv/riscv.c
+++ b/src/target/riscv/riscv.c
@@ -4835,7 +4835,7 @@ int riscv_init_registers(struct target *target)
 			assert(reg_name < info->reg_names + target->reg_cache->num_regs *
 					max_reg_name_len);
 		}
-		r->value = &info->reg_cache_values[number];
+		r->value = info->reg_cache_values[number];
 	}
 
 	return ERROR_OK;
diff --git a/src/target/riscv/riscv.h b/src/target/riscv/riscv.h
index 8e098c0..aa9614a 100644
--- a/src/target/riscv/riscv.h
+++ b/src/target/riscv/riscv.h
@@ -101,8 +101,8 @@ typedef struct {
 
 	/* OpenOCD's register cache points into here. This is not per-hart because
 	 * we just invalidate the entire cache when we change which hart is
-	 * selected. */
-	uint64_t reg_cache_values[RISCV_MAX_REGISTERS];
+	 * selected. Use an array of 8 uint8_t per register. */
+	uint8_t reg_cache_values[RISCV_MAX_REGISTERS][8];
 
 	/* Single buffer that contains all register names, instead of calling
 	 * malloc for each register. Needs to be freed when reg_list is freed. */
diff --git a/src/target/riscv/riscv_semihosting.c b/src/target/riscv/riscv_semihosting.c
index 3048709..c39f030 100644
--- a/src/target/riscv/riscv_semihosting.c
+++ b/src/target/riscv/riscv_semihosting.c
@@ -143,7 +143,7 @@ semihosting_result_t riscv_semihosting(struct target *target, int *retval)
 		if (0 <= semihosting->op && semihosting->op <= 0x31) {
 			*retval = semihosting_common(target);
 			if (*retval != ERROR_OK) {
-				LOG_ERROR("Failed semihosting operation");
+				LOG_ERROR("Failed semihosting operation (0x%02X)", semihosting->op);
 				return SEMI_ERROR;
 			}
 		} else {
diff --git a/src/target/rtt.c b/src/target/rtt.c
new file mode 100644
index 0000000..7e556e1
--- /dev/null
+++ b/src/target/rtt.c
@@ -0,0 +1,424 @@
+/*
+ * Copyright (C) 2016-2020 by Marc Schink <dev@zapb.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stddef.h>
+#include <stdint.h>
+#include <helper/log.h>
+#include <helper/binarybuffer.h>
+#include <helper/command.h>
+#include <rtt/rtt.h>
+
+#include "target.h"
+
+static int read_rtt_channel(struct target *target,
+		const struct rtt_control *ctrl, unsigned int channel_index,
+		enum rtt_channel_type type, struct rtt_channel *channel)
+{
+	int ret;
+	uint8_t buf[RTT_CHANNEL_SIZE];
+	target_addr_t address;
+
+	address = ctrl->address + RTT_CB_SIZE + (channel_index * RTT_CHANNEL_SIZE);
+
+	if (type == RTT_CHANNEL_TYPE_DOWN)
+		address += ctrl->num_up_channels * RTT_CHANNEL_SIZE;
+
+	ret = target_read_buffer(target, address, RTT_CHANNEL_SIZE, buf);
+
+	if (ret != ERROR_OK)
+		return ret;
+
+	channel->address = address;
+	channel->name_addr = buf_get_u32(buf + 0, 0, 32);
+	channel->buffer_addr = buf_get_u32(buf + 4, 0, 32);
+	channel->size = buf_get_u32(buf + 8, 0, 32);
+	channel->write_pos = buf_get_u32(buf + 12, 0, 32);
+	channel->read_pos = buf_get_u32(buf + 16, 0, 32);
+	channel->flags = buf_get_u32(buf + 20, 0, 32);
+
+	return ERROR_OK;
+}
+
+int target_rtt_start(struct target *target, const struct rtt_control *ctrl,
+		void *user_data)
+{
+	return ERROR_OK;
+}
+
+int target_rtt_stop(struct target *target, void *user_data)
+{
+	return ERROR_OK;
+}
+
+static int read_channel_name(struct target *target, target_addr_t address,
+		char *name, size_t length)
+{
+	size_t offset;
+
+	offset = 0;
+
+	while (offset < length) {
+		int ret;
+		size_t read_length;
+
+		read_length = MIN(32, length - offset);
+		ret = target_read_buffer(target, address + offset, read_length,
+			(uint8_t *)name + offset);
+
+		if (ret != ERROR_OK)
+			return ret;
+
+		if (memchr(name + offset, '\0', read_length))
+			return ERROR_OK;
+
+		offset += read_length;
+	}
+
+	name[length - 1] = '\0';
+
+	return ERROR_OK;
+}
+
+static int write_to_channel(struct target *target,
+		const struct rtt_channel *channel, const uint8_t *buffer,
+		size_t *length)
+{
+	int ret;
+	uint32_t len;
+
+	if (!*length)
+		return ERROR_OK;
+
+	if (channel->write_pos == channel->read_pos) {
+		uint32_t first_length;
+
+		len = MIN(*length, channel->size - 1);
+		first_length = MIN(len, channel->size - channel->write_pos);
+
+		ret = target_write_buffer(target,
+			channel->buffer_addr + channel->write_pos, first_length,
+			buffer);
+
+		if (ret != ERROR_OK)
+			return ret;
+
+		ret = target_write_buffer(target, channel->buffer_addr,
+			len - first_length, buffer + first_length);
+
+		if (ret != ERROR_OK)
+			return ret;
+	} else if (channel->write_pos < channel->read_pos) {
+		len = MIN(*length, channel->read_pos - channel->write_pos - 1);
+
+		if (!len) {
+			*length = 0;
+			return ERROR_OK;
+		}
+
+		ret = target_write_buffer(target,
+			channel->buffer_addr + channel->write_pos, len, buffer);
+
+		if (ret != ERROR_OK)
+			return ret;
+	} else {
+		uint32_t first_length;
+
+		len = MIN(*length,
+			channel->size - channel->write_pos + channel->read_pos - 1);
+
+		if (!len) {
+			*length = 0;
+			return ERROR_OK;
+		}
+
+		first_length = MIN(len, channel->size - channel->write_pos);
+
+		ret = target_write_buffer(target,
+			channel->buffer_addr + channel->write_pos, first_length,
+			buffer);
+
+		if (ret != ERROR_OK)
+			return ret;
+
+		buffer = buffer + first_length;
+
+		ret = target_write_buffer(target, channel->buffer_addr,
+			len - first_length, buffer);
+
+		if (ret != ERROR_OK)
+			return ret;
+	}
+
+	ret = target_write_u32(target, channel->address + 12,
+		(channel->write_pos + len) % channel->size);
+
+	if (ret != ERROR_OK)
+		return ret;
+
+	*length = len;
+
+	return ERROR_OK;
+}
+
+static bool channel_is_active(const struct rtt_channel *channel)
+{
+	if (!channel)
+		return false;
+
+	if (!channel->size)
+		return false;
+
+	return true;
+}
+
+int target_rtt_write_callback(struct target *target, struct rtt_control *ctrl,
+		unsigned int channel_index, const uint8_t *buffer, size_t *length,
+		void *user_data)
+{
+	int ret;
+	struct rtt_channel channel;
+
+	ret = read_rtt_channel(target, ctrl, channel_index,
+		RTT_CHANNEL_TYPE_DOWN, &channel);
+
+	if (ret != ERROR_OK) {
+		LOG_ERROR("rtt: Failed to read down-channel %u description",
+			channel_index);
+		return ret;
+	}
+
+	if (!channel_is_active(&channel)) {
+		LOG_WARNING("rtt: Down-channel %u is not active", channel_index);
+		return ERROR_OK;
+	}
+
+	if (channel.size < RTT_CHANNEL_BUFFER_MIN_SIZE) {
+		LOG_WARNING("rtt: Down-channel %u is not large enough",
+			channel_index);
+		return ERROR_OK;
+	}
+
+	ret = write_to_channel(target, &channel, buffer, length);
+
+	if (ret != ERROR_OK)
+		return ret;
+
+	LOG_DEBUG("rtt: Wrote %zu bytes into down-channel %u", *length,
+		channel_index);
+
+	return ERROR_OK;
+}
+
+int target_rtt_read_control_block(struct target *target,
+		target_addr_t address, struct rtt_control *ctrl, void *user_data)
+{
+	int ret;
+	uint8_t buf[RTT_CB_SIZE];
+
+	ret = target_read_buffer(target, address, RTT_CB_SIZE, buf);
+
+	if (ret != ERROR_OK)
+		return ret;
+
+	memcpy(ctrl->id, buf, RTT_CB_MAX_ID_LENGTH);
+	ctrl->id[RTT_CB_MAX_ID_LENGTH - 1] = '\0';
+	ctrl->num_up_channels = buf_get_u32(buf + RTT_CB_MAX_ID_LENGTH + 0,
+		0, 32);
+	ctrl->num_down_channels = buf_get_u32(buf + RTT_CB_MAX_ID_LENGTH + 4,
+		0, 32);
+
+	return ERROR_OK;
+}
+
+int target_rtt_find_control_block(struct target *target,
+		target_addr_t *address, size_t size, const char *id, bool *found,
+		void *user_data)
+{
+	uint8_t buf[1024];
+
+	*found = false;
+
+	size_t j = 0;
+	size_t cb_offset = 0;
+	const size_t id_length = strlen(id);
+
+	LOG_INFO("rtt: Searching for control block '%s'", id);
+
+	for (target_addr_t addr = 0; addr < size; addr = addr + sizeof(buf)) {
+		int ret;
+
+		const size_t buf_size = MIN(sizeof(buf), size - addr);
+		ret = target_read_buffer(target, *address + addr, buf_size, buf);
+
+		if (ret != ERROR_OK)
+			return ret;
+
+		size_t start = 0;
+		size_t i = 0;
+
+		while (i < buf_size) {
+			if (buf[i] != id[j]) {
+				start++;
+				cb_offset++;
+				i = start;
+				j = 0;
+
+				continue;
+			}
+
+			i++;
+			j++;
+
+			if (j == id_length) {
+				*address = *address + cb_offset;
+				*found = true;
+				return ERROR_OK;
+			}
+		}
+	}
+
+	return ERROR_OK;
+}
+
+int target_rtt_read_channel_info(struct target *target,
+		const struct rtt_control *ctrl, unsigned int channel_index,
+		enum rtt_channel_type type, struct rtt_channel_info *info,
+		void *user_data)
+{
+	int ret;
+	struct rtt_channel channel;
+
+	ret = read_rtt_channel(target, ctrl, channel_index, type, &channel);
+
+	if (ret != ERROR_OK) {
+		LOG_ERROR("rtt: Failed to read channel %u description",
+			channel_index);
+		return ret;
+	}
+
+	ret = read_channel_name(target, channel.name_addr, info->name,
+		info->name_length);
+
+	if (ret != ERROR_OK)
+		return ret;
+
+	info->size = channel.size;
+	info->flags = channel.flags;
+
+	return ERROR_OK;
+}
+
+static int read_from_channel(struct target *target,
+		const struct rtt_channel *channel, uint8_t *buffer,
+		size_t *length)
+{
+	int ret;
+	uint32_t len;
+
+	if (!*length)
+		return ERROR_OK;
+
+	if (channel->read_pos == channel->write_pos) {
+		len = 0;
+	} else if (channel->read_pos < channel->write_pos) {
+		len = MIN(*length, channel->write_pos - channel->read_pos);
+
+		ret = target_read_buffer(target,
+			channel->buffer_addr + channel->read_pos, len, buffer);
+
+		if (ret != ERROR_OK)
+			return ret;
+	} else {
+		uint32_t first_length;
+
+		len = MIN(*length,
+			channel->size - channel->read_pos + channel->write_pos);
+		first_length = MIN(len, channel->size - channel->read_pos);
+
+		ret = target_read_buffer(target,
+			channel->buffer_addr + channel->read_pos, first_length, buffer);
+
+		if (ret != ERROR_OK)
+			return ret;
+
+		ret = target_read_buffer(target, channel->buffer_addr,
+			len - first_length, buffer + first_length);
+
+		if (ret != ERROR_OK)
+			return ret;
+	}
+
+	if (len > 0) {
+		ret = target_write_u32(target, channel->address + 16,
+			(channel->read_pos + len) % channel->size);
+
+		if (ret != ERROR_OK)
+			return ret;
+	}
+
+	*length = len;
+
+	return ERROR_OK;
+}
+
+int target_rtt_read_callback(struct target *target,
+		const struct rtt_control *ctrl, struct rtt_sink_list **sinks,
+		size_t num_channels, void *user_data)
+{
+	num_channels = MIN(num_channels, ctrl->num_up_channels);
+
+	for (size_t i = 0; i < num_channels; i++) {
+		int ret;
+		struct rtt_channel channel;
+		uint8_t buffer[1024];
+		size_t length;
+
+		if (!sinks[i])
+			continue;
+
+		ret = read_rtt_channel(target, ctrl, i, RTT_CHANNEL_TYPE_UP,
+			&channel);
+
+		if (ret != ERROR_OK) {
+			LOG_ERROR("rtt: Failed to read up-channel %zu description", i);
+			return ret;
+		}
+
+		if (!channel_is_active(&channel)) {
+			LOG_WARNING("rtt: Up-channel %zu is not active", i);
+			continue;
+		}
+
+		if (channel.size < RTT_CHANNEL_BUFFER_MIN_SIZE) {
+			LOG_WARNING("rtt: Up-channel %zu is not large enough", i);
+			continue;
+		}
+
+		length = sizeof(buffer);
+		ret = read_from_channel(target, &channel, buffer, &length);
+
+		if (ret != ERROR_OK) {
+			LOG_ERROR("rtt: Failed to read from up-channel %zu", i);
+			return ret;
+		}
+
+		for (struct rtt_sink_list *sink = sinks[i]; sink; sink = sink->next)
+			sink->read(i, buffer, length, sink->user_data);
+	}
+
+	return ERROR_OK;
+}
diff --git a/src/target/rtt.h b/src/target/rtt.h
new file mode 100644
index 0000000..0122475
--- /dev/null
+++ b/src/target/rtt.h
@@ -0,0 +1,46 @@
+/*
+ * Copyright (C) 2016-2020 by Marc Schink <dev@zapb.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef OPENOCD_TARGET_RTT_H
+#define OPENOCD_TARGET_RTT_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#include <target/target.h>
+#include <rtt/rtt.h>
+
+int target_rtt_start(struct target *target, const struct rtt_control *ctrl,
+		void *user_data);
+int target_rtt_stop(struct target *target, void *user_data);
+int target_rtt_find_control_block(struct target *target,
+		target_addr_t *address, size_t size, const char *id, bool *found,
+		void *user_data);
+int target_rtt_read_control_block(struct target *target,
+		target_addr_t address, struct rtt_control *ctrl, void *user_data);
+int target_rtt_write_callback(struct target *target,
+		struct rtt_control *ctrl, unsigned int channel_index,
+		const uint8_t *buffer, size_t *length, void *user_data);
+int target_rtt_read_callback(struct target *target,
+		const struct rtt_control *ctrl, struct rtt_sink_list **sinks,
+		size_t length, void *user_data);
+int target_rtt_read_channel_info(struct target *target,
+		const struct rtt_control *ctrl, unsigned int channel_index,
+		enum rtt_channel_type type, struct rtt_channel_info *info,
+		void *user_data);
+
+#endif /* OPENOCD_TARGET_RTT_H */
diff --git a/src/target/stm8.c b/src/target/stm8.c
index 78bf6a2..e99b3c2 100644
--- a/src/target/stm8.c
+++ b/src/target/stm8.c
@@ -1945,7 +1945,7 @@ static int stm8_run_algorithm(struct target *target, int num_mem_params,
 	return ERROR_OK;
 }
 
-int stm8_jim_configure(struct target *target, Jim_GetOptInfo *goi)
+static int stm8_jim_configure(struct target *target, Jim_GetOptInfo *goi)
 {
 	struct stm8_common *stm8 = target_to_stm8(target);
 	jim_wide w;
diff --git a/src/target/target.c b/src/target/target.c
index 9e27f33..4ea6aca 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -72,8 +72,6 @@ static int target_get_gdb_fileio_info_default(struct target *target,
 		struct gdb_fileio_info *fileio_info);
 static int target_gdb_fileio_end_default(struct target *target, int retcode,
 		int fileio_errno, bool ctrl_c);
-static int target_profiling_default(struct target *target, uint32_t *samples,
-		uint32_t max_num_samples, uint32_t *num_samples, uint32_t seconds);
 
 /* targets */
 extern struct target_type arm7tdmi_target;
@@ -156,8 +154,8 @@ static struct target_type *target_types[] = {
 struct target *all_targets;
 static struct target_event_callback *target_event_callbacks;
 static struct target_timer_callback *target_timer_callbacks;
-LIST_HEAD(target_reset_callback_list);
-LIST_HEAD(target_trace_callback_list);
+static LIST_HEAD(target_reset_callback_list);
+static LIST_HEAD(target_trace_callback_list);
 static const int polling_interval = TARGET_DEFAULT_POLLING_INTERVAL;
 
 static const Jim_Nvp nvp_assert[] = {
@@ -768,9 +766,11 @@ int target_examine(void)
 		if (target->defer_examine)
 			continue;
 
-		retval = target_examine_one(target);
-		if (retval != ERROR_OK)
-			return retval;
+		int retval2 = target_examine_one(target);
+		if (retval2 != ERROR_OK) {
+			LOG_WARNING("target %s examination failed", target_name(target));
+			retval = retval2;
+		}
 	}
 	return retval;
 }
@@ -1031,11 +1031,11 @@ int target_run_flash_async_algorithm(struct target *target,
 			 * programming. The exact delay shouldn't matter as long as it's
 			 * less than buffer size / flash speed. This is very unlikely to
 			 * run when using high latency connections such as USB. */
-			alive_sleep(10);
+			alive_sleep(2);
 
 			/* to stop an infinite loop on some targets check and increment a timeout
 			 * this issue was observed on a stellaris using the new ICDI interface */
-			if (timeout++ >= 500) {
+			if (timeout++ >= 2500) {
 				LOG_ERROR("timeout waiting for algorithm, a target reset is recommended");
 				return ERROR_FLASH_OPERATION_FAILED;
 			}
@@ -1049,6 +1049,10 @@ int target_run_flash_async_algorithm(struct target *target,
 		if (thisrun_bytes > count * block_size)
 			thisrun_bytes = count * block_size;
 
+		/* Force end of large blocks to be word aligned */
+		if (thisrun_bytes >= 16)
+			thisrun_bytes -= (rp + thisrun_bytes) & 0x03;
+
 		/* Write data to fifo */
 		retval = target_write_buffer(target, wp, thisrun_bytes, buffer);
 		if (retval != ERROR_OK)
@@ -1098,6 +1102,156 @@ int target_run_flash_async_algorithm(struct target *target,
 	return retval;
 }
 
+int target_run_read_async_algorithm(struct target *target,
+		uint8_t *buffer, uint32_t count, int block_size,
+		int num_mem_params, struct mem_param *mem_params,
+		int num_reg_params, struct reg_param *reg_params,
+		uint32_t buffer_start, uint32_t buffer_size,
+		uint32_t entry_point, uint32_t exit_point, void *arch_info)
+{
+	int retval;
+	int timeout = 0;
+
+	const uint8_t *buffer_orig = buffer;
+
+	/* Set up working area. First word is write pointer, second word is read pointer,
+	 * rest is fifo data area. */
+	uint32_t wp_addr = buffer_start;
+	uint32_t rp_addr = buffer_start + 4;
+	uint32_t fifo_start_addr = buffer_start + 8;
+	uint32_t fifo_end_addr = buffer_start + buffer_size;
+
+	uint32_t wp = fifo_start_addr;
+	uint32_t rp = fifo_start_addr;
+
+	/* validate block_size is 2^n */
+	assert(!block_size || !(block_size & (block_size - 1)));
+
+	retval = target_write_u32(target, wp_addr, wp);
+	if (retval != ERROR_OK)
+		return retval;
+	retval = target_write_u32(target, rp_addr, rp);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Start up algorithm on target */
+	retval = target_start_algorithm(target, num_mem_params, mem_params,
+			num_reg_params, reg_params,
+			entry_point,
+			exit_point,
+			arch_info);
+
+	if (retval != ERROR_OK) {
+		LOG_ERROR("error starting target flash read algorithm");
+		return retval;
+	}
+
+	while (count > 0) {
+		retval = target_read_u32(target, wp_addr, &wp);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("failed to get write pointer");
+			break;
+		}
+
+		LOG_DEBUG("offs 0x%zx count 0x%" PRIx32 " wp 0x%" PRIx32 " rp 0x%" PRIx32,
+			(size_t)(buffer - buffer_orig), count, wp, rp);
+
+		if (wp == 0) {
+			LOG_ERROR("flash read algorithm aborted by target");
+			retval = ERROR_FLASH_OPERATION_FAILED;
+			break;
+		}
+
+		if (((wp - fifo_start_addr) & (block_size - 1)) || wp < fifo_start_addr || wp >= fifo_end_addr) {
+			LOG_ERROR("corrupted fifo write pointer 0x%" PRIx32, wp);
+			break;
+		}
+
+		/* Count the number of bytes available in the fifo without
+		 * crossing the wrap around. */
+		uint32_t thisrun_bytes;
+		if (wp >= rp)
+			thisrun_bytes = wp - rp;
+		else
+			thisrun_bytes = fifo_end_addr - rp;
+
+		if (thisrun_bytes == 0) {
+			/* Throttle polling a bit if transfer is (much) faster than flash
+			 * reading. The exact delay shouldn't matter as long as it's
+			 * less than buffer size / flash speed. This is very unlikely to
+			 * run when using high latency connections such as USB. */
+			alive_sleep(2);
+
+			/* to stop an infinite loop on some targets check and increment a timeout
+			 * this issue was observed on a stellaris using the new ICDI interface */
+			if (timeout++ >= 2500) {
+				LOG_ERROR("timeout waiting for algorithm, a target reset is recommended");
+				return ERROR_FLASH_OPERATION_FAILED;
+			}
+			continue;
+		}
+
+		/* Reset our timeout */
+		timeout = 0;
+
+		/* Limit to the amount of data we actually want to read */
+		if (thisrun_bytes > count * block_size)
+			thisrun_bytes = count * block_size;
+
+		/* Force end of large blocks to be word aligned */
+		if (thisrun_bytes >= 16)
+			thisrun_bytes -= (rp + thisrun_bytes) & 0x03;
+
+		/* Read data from fifo */
+		retval = target_read_buffer(target, rp, thisrun_bytes, buffer);
+		if (retval != ERROR_OK)
+			break;
+
+		/* Update counters and wrap write pointer */
+		buffer += thisrun_bytes;
+		count -= thisrun_bytes / block_size;
+		rp += thisrun_bytes;
+		if (rp >= fifo_end_addr)
+			rp = fifo_start_addr;
+
+		/* Store updated write pointer to target */
+		retval = target_write_u32(target, rp_addr, rp);
+		if (retval != ERROR_OK)
+			break;
+
+		/* Avoid GDB timeouts */
+		keep_alive();
+
+	}
+
+	if (retval != ERROR_OK) {
+		/* abort flash write algorithm on target */
+		target_write_u32(target, rp_addr, 0);
+	}
+
+	int retval2 = target_wait_algorithm(target, num_mem_params, mem_params,
+			num_reg_params, reg_params,
+			exit_point,
+			10000,
+			arch_info);
+
+	if (retval2 != ERROR_OK) {
+		LOG_ERROR("error waiting for target flash write algorithm");
+		retval = retval2;
+	}
+
+	if (retval == ERROR_OK) {
+		/* check if algorithm set wp = 0 after fifo writer loop finished */
+		retval = target_read_u32(target, wp_addr, &wp);
+		if (retval == ERROR_OK && wp == 0) {
+			LOG_ERROR("flash read algorithm aborted by target");
+			retval = ERROR_FLASH_OPERATION_FAILED;
+		}
+	}
+
+	return retval;
+}
+
 int target_read_memory(struct target *target,
 		target_addr_t address, uint32_t size, uint32_t count, uint8_t *buffer)
 {
@@ -1265,10 +1419,10 @@ int target_get_gdb_reg_list_noread(struct target *target,
 bool target_supports_gdb_connection(struct target *target)
 {
 	/*
-	 * based on current code, we can simply exclude all the targets that
-	 * don't provide get_gdb_reg_list; this could change with new targets.
+	 * exclude all the targets that don't provide get_gdb_reg_list
+	 * or that have explicit gdb_max_connection == 0
 	 */
-	return !!target->type->get_gdb_reg_list;
+	return !!target->type->get_gdb_reg_list && !!target->gdb_max_connections;
 }
 
 int target_step(struct target *target,
@@ -1328,13 +1482,9 @@ unsigned target_data_bits(struct target *target)
 	return 32;
 }
 
-int target_profiling(struct target *target, uint32_t *samples,
+static int target_profiling(struct target *target, uint32_t *samples,
 			uint32_t max_num_samples, uint32_t *num_samples, uint32_t seconds)
 {
-	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target %s is not halted (profiling)", target->cmd_name);
-		return ERROR_TARGET_NOT_HALTED;
-	}
 	return target->type->profiling(target, samples, max_num_samples,
 			num_samples, seconds);
 }
@@ -2154,7 +2304,7 @@ static int target_gdb_fileio_end_default(struct target *target,
 	return ERROR_OK;
 }
 
-static int target_profiling_default(struct target *target, uint32_t *samples,
+int target_profiling_default(struct target *target, uint32_t *samples,
 		uint32_t max_num_samples, uint32_t *num_samples, uint32_t seconds)
 {
 	struct timeval timeout, now;
@@ -2905,7 +3055,7 @@ COMMAND_HANDLER(handle_reg_command)
 			for (i = 0, reg = cache->reg_list;
 					i < cache->num_regs;
 					i++, reg++, count++) {
-				if (reg->exist == false)
+				if (reg->exist == false || reg->hidden)
 					continue;
 				/* only print cached values if they are valid */
 				if (reg->exist) {
@@ -3441,11 +3591,11 @@ static COMMAND_HELPER(parse_load_image_command_CMD_ARGV, struct image *image,
 		target_addr_t addr;
 		COMMAND_PARSE_ADDRESS(CMD_ARGV[1], addr);
 		image->base_address = addr;
-		image->base_address_set = 1;
+		image->base_address_set = true;
 	} else
-		image->base_address_set = 0;
+		image->base_address_set = false;
 
-	image->start_address_set = 0;
+	image->start_address_set = false;
 
 	if (CMD_ARGC >= 4)
 		COMMAND_PARSE_ADDRESS(CMD_ARGV[3], *min_address);
@@ -3468,7 +3618,6 @@ COMMAND_HANDLER(handle_load_image_command)
 	uint32_t image_size;
 	target_addr_t min_address = 0;
 	target_addr_t max_address = -1;
-	int i;
 	struct image image;
 
 	int retval = CALL_COMMAND_HANDLER(parse_load_image_command_CMD_ARGV,
@@ -3486,7 +3635,7 @@ COMMAND_HANDLER(handle_load_image_command)
 
 	image_size = 0x0;
 	retval = ERROR_OK;
-	for (i = 0; i < image.num_sections; i++) {
+	for (unsigned int i = 0; i < image.num_sections; i++) {
 		buffer = malloc(image.sections[i].size);
 		if (buffer == NULL) {
 			command_print(CMD,
@@ -3619,7 +3768,6 @@ static COMMAND_HELPER(handle_verify_image_command_internal, enum verify_mode ver
 	uint8_t *buffer;
 	size_t buf_cnt;
 	uint32_t image_size;
-	int i;
 	int retval;
 	uint32_t checksum = 0;
 	uint32_t mem_checksum = 0;
@@ -3643,13 +3791,13 @@ static COMMAND_HELPER(handle_verify_image_command_internal, enum verify_mode ver
 		target_addr_t addr;
 		COMMAND_PARSE_ADDRESS(CMD_ARGV[1], addr);
 		image.base_address = addr;
-		image.base_address_set = 1;
+		image.base_address_set = true;
 	} else {
-		image.base_address_set = 0;
+		image.base_address_set = false;
 		image.base_address = 0x0;
 	}
 
-	image.start_address_set = 0;
+	image.start_address_set = false;
 
 	retval = image_open(&image, CMD_ARGV[0], (CMD_ARGC == 3) ? CMD_ARGV[2] : NULL);
 	if (retval != ERROR_OK)
@@ -3658,12 +3806,12 @@ static COMMAND_HELPER(handle_verify_image_command_internal, enum verify_mode ver
 	image_size = 0x0;
 	int diffs = 0;
 	retval = ERROR_OK;
-	for (i = 0; i < image.num_sections; i++) {
+	for (unsigned int i = 0; i < image.num_sections; i++) {
 		buffer = malloc(image.sections[i].size);
 		if (buffer == NULL) {
 			command_print(CMD,
-					"error allocating buffer for section (%d bytes)",
-					(int)(image.sections[i].size));
+					"error allocating buffer for section (%" PRIu32 " bytes)",
+					image.sections[i].size);
 			break;
 		}
 		retval = image_read_section(&image, i, 0x0, image.sections[i].size, buffer, &buf_cnt);
@@ -4134,6 +4282,7 @@ COMMAND_HANDLER(handle_profile_command)
 	uint32_t offset;
 	uint32_t num_of_samples;
 	int retval = ERROR_OK;
+	bool halted_before_profiling = target->state == TARGET_HALTED;
 
 	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], offset);
 
@@ -4164,12 +4313,23 @@ COMMAND_HANDLER(handle_profile_command)
 		free(samples);
 		return retval;
 	}
-	if (target->state == TARGET_RUNNING) {
+
+	if (target->state == TARGET_RUNNING && halted_before_profiling) {
+		/* The target was halted before we started and is running now. Halt it,
+		 * for consistency. */
 		retval = target_halt(target);
 		if (retval != ERROR_OK) {
 			free(samples);
 			return retval;
 		}
+	} else if (target->state == TARGET_HALTED && !halted_before_profiling) {
+		/* The target was running before we started and is halted now. Resume
+		 * it, for consistency. */
+		retval = target_resume(target, 1, 0, 0, 0);
+		if (retval != ERROR_OK) {
+			free(samples);
+			return retval;
+		}
 	}
 
 	retval = target_poll(target);
@@ -4678,6 +4838,7 @@ enum target_cfg_param {
 	TCFG_RTOS,
 	TCFG_DEFER_EXAMINE,
 	TCFG_GDB_PORT,
+	TCFG_GDB_MAX_CONNECTIONS,
 };
 
 static Jim_Nvp nvp_config_opts[] = {
@@ -4694,6 +4855,7 @@ static Jim_Nvp nvp_config_opts[] = {
 	{ .name = "-rtos",             .value = TCFG_RTOS },
 	{ .name = "-defer-examine",    .value = TCFG_DEFER_EXAMINE },
 	{ .name = "-gdb-port",         .value = TCFG_GDB_PORT },
+	{ .name = "-gdb-max-connections",   .value = TCFG_GDB_MAX_CONNECTIONS },
 	{ .name = NULL, .value = -1 }
 };
 
@@ -5001,6 +5163,25 @@ no_params:
 			Jim_SetResultString(goi->interp, target->gdb_port_override ? : "undefined", -1);
 			/* loop for more */
 			break;
+
+		case TCFG_GDB_MAX_CONNECTIONS:
+			if (goi->isconfigure) {
+				struct command_context *cmd_ctx = current_command_context(goi->interp);
+				if (cmd_ctx->mode != COMMAND_CONFIG) {
+					Jim_SetResultString(goi->interp, "-gdb-max-conenctions must be configured before 'init'", -1);
+					return JIM_ERR;
+				}
+
+				e = Jim_GetOpt_Wide(goi, &w);
+				if (e != JIM_OK)
+					return e;
+				target->gdb_max_connections = (w < 0) ? CONNECTION_LIMIT_UNLIMITED : (int)w;
+			} else {
+				if (goi->argc != 0)
+					goto no_params;
+			}
+			Jim_SetResult(goi->interp, Jim_NewIntObj(goi->interp, target->gdb_max_connections));
+			break;
 		}
 	} /* while (goi->argc) */
 
@@ -5581,6 +5762,7 @@ static int target_create(Jim_GetOptInfo *goi)
 	target->rtos_auto_detect = false;
 
 	target->gdb_port_override = NULL;
+	target->gdb_max_connections = 1;
 
 	/* Do the rest as "configure" options */
 	goi->isconfigure = 1;
@@ -5703,7 +5885,9 @@ static int jim_target_current(Jim_Interp *interp, int argc, Jim_Obj *const *argv
 	struct command_context *cmd_ctx = current_command_context(interp);
 	assert(cmd_ctx != NULL);
 
-	Jim_SetResultString(interp, target_name(get_current_target(cmd_ctx)), -1);
+	struct target *target = get_current_target_or_null(cmd_ctx);
+	if (target)
+		Jim_SetResultString(interp, target_name(target), -1);
 	return JIM_OK;
 }
 
@@ -5875,7 +6059,6 @@ COMMAND_HANDLER(handle_fast_load_image_command)
 	uint32_t image_size;
 	target_addr_t min_address = 0;
 	target_addr_t max_address = -1;
-	int i;
 
 	struct image image;
 
@@ -5901,7 +6084,7 @@ COMMAND_HANDLER(handle_fast_load_image_command)
 		return ERROR_FAIL;
 	}
 	memset(fastload, 0, sizeof(struct FastLoad)*image.num_sections);
-	for (i = 0; i < image.num_sections; i++) {
+	for (unsigned int i = 0; i < image.num_sections; i++) {
 		buffer = malloc(image.sections[i].size);
 		if (buffer == NULL) {
 			command_print(CMD, "error allocating buffer for section (%d bytes)",
diff --git a/src/target/target.h b/src/target/target.h
index 4e8897c..f6044c5 100644
--- a/src/target/target.h
+++ b/src/target/target.h
@@ -47,7 +47,7 @@ struct gdb_fileio_info;
 
 /*
  * TARGET_UNKNOWN = 0: we don't know anything about the target yet
- * TARGET_RUNNING = 1: the target is executing user code
+ * TARGET_RUNNING = 1: the target is executing or ready to execute user code
  * TARGET_HALTED  = 2: the target is not executing code, and ready to talk to the
  * debugger. on an xscale it means that the debug handler is executing
  * TARGET_RESET   = 3: the target is being held in reset (only a temporary state,
@@ -211,6 +211,8 @@ struct target {
 
 	char *gdb_port_override;			/* target-specific override for gdb_port */
 
+	int gdb_max_connections;			/* max number of simultaneous gdb connections */
+
 	/* The semihosting information, extracted from the target. */
 	struct semihosting *semihosting;
 };
@@ -576,6 +578,18 @@ int target_run_flash_async_algorithm(struct target *target,
 		void *arch_info);
 
 /**
+ * This routine is a wrapper for asynchronous algorithms.
+ *
+ */
+int target_run_read_async_algorithm(struct target *target,
+		uint8_t *buffer, uint32_t count, int block_size,
+		int num_mem_params, struct mem_param *mem_params,
+		int num_reg_params, struct reg_param *reg_params,
+		uint32_t buffer_start, uint32_t buffer_size,
+		uint32_t entry_point, uint32_t exit_point,
+		void *arch_info);
+
+/**
  * Read @a count items of @a size bytes from the memory of @a target at
  * the @a address given.
  *
@@ -755,6 +769,9 @@ void target_handle_md_output(struct command_invocation *cmd,
 	struct target *target, target_addr_t address, unsigned size,
 	unsigned count, const uint8_t *buffer, bool include_address);
 
+int target_profiling_default(struct target *target, uint32_t *samples, uint32_t
+		max_num_samples, uint32_t *num_samples, uint32_t seconds);
+
 #define ERROR_TARGET_INVALID	(-300)
 #define ERROR_TARGET_INIT_FAILED (-301)
 #define ERROR_TARGET_TIMEOUT	(-302)
diff --git a/src/target/xscale.c b/src/target/xscale.c
index 6d1d426..b25999d 100644
--- a/src/target/xscale.c
+++ b/src/target/xscale.c
@@ -2582,7 +2582,6 @@ static int xscale_read_instruction(struct target *target, uint32_t pc,
 	struct arm_instruction *instruction)
 {
 	struct xscale_common *const xscale = target_to_xscale(target);
-	int i;
 	int section = -1;
 	size_t size_read;
 	uint32_t opcode;
@@ -2592,7 +2591,7 @@ static int xscale_read_instruction(struct target *target, uint32_t pc,
 		return ERROR_TRACE_IMAGE_UNAVAILABLE;
 
 	/* search for the section the current instruction belongs to */
-	for (i = 0; i < xscale->trace.image->num_sections; i++) {
+	for (unsigned int i = 0; i < xscale->trace.image->num_sections; i++) {
 		if ((xscale->trace.image->sections[i].base_address <= pc) &&
 			(xscale->trace.image->sections[i].base_address +
 			xscale->trace.image->sections[i].size > pc)) {
@@ -2883,7 +2882,7 @@ static void xscale_build_reg_cache(struct target *target)
 	/* fill in values for the xscale reg cache */
 	(*cache_p)->name = "XScale registers";
 	(*cache_p)->next = NULL;
-	(*cache_p)->reg_list = malloc(num_regs * sizeof(struct reg));
+	(*cache_p)->reg_list = calloc(num_regs, sizeof(struct reg));
 	(*cache_p)->num_regs = num_regs;
 
 	for (i = 0; i < num_regs; i++) {
@@ -3428,15 +3427,15 @@ COMMAND_HANDLER(xscale_handle_trace_image_command)
 	}
 
 	xscale->trace.image = malloc(sizeof(struct image));
-	xscale->trace.image->base_address_set = 0;
-	xscale->trace.image->start_address_set = 0;
+	xscale->trace.image->base_address_set = false;
+	xscale->trace.image->start_address_set = false;
 
 	/* a base address isn't always necessary, default to 0x0 (i.e. don't relocate) */
 	if (CMD_ARGC >= 2) {
-		xscale->trace.image->base_address_set = 1;
+		xscale->trace.image->base_address_set = true;
 		COMMAND_PARSE_NUMBER(llong, CMD_ARGV[1], xscale->trace.image->base_address);
 	} else
-		xscale->trace.image->base_address_set = 0;
+		xscale->trace.image->base_address_set = false;
 
 	if (image_open(xscale->trace.image, CMD_ARGV[0],
 		(CMD_ARGC >= 3) ? CMD_ARGV[2] : NULL) != ERROR_OK) {
diff --git a/src/transport/transport.c b/src/transport/transport.c
index 9214dcd..cb000ab 100644
--- a/src/transport/transport.c
+++ b/src/transport/transport.c
@@ -122,16 +122,6 @@ int allow_transports(struct command_context *ctx, const char * const *vector)
 }
 
 /**
- * Used to verify correct adapter driver initialization.
- *
- * @returns true if the adapter declares one or more transports.
- */
-bool transports_are_declared(void)
-{
-	return allowed_transports != NULL;
-}
-
-/**
  * Registers a transport.  There are general purpose transports
  * (such as JTAG), as well as relatively proprietary ones which are
  * specific to a given chip (or chip family).
diff --git a/src/transport/transport.h b/src/transport/transport.h
index 809564e..6bf6aac 100644
--- a/src/transport/transport.h
+++ b/src/transport/transport.h
@@ -92,15 +92,13 @@ COMMAND_HELPER(transport_list_parse, char ***vector);
 
 int allow_transports(struct command_context *ctx, const char * const *vector);
 
-bool transports_are_declared(void);
-
 bool transport_is_jtag(void);
 bool transport_is_swd(void);
 bool transport_is_dapdirect_jtag(void);
 bool transport_is_dapdirect_swd(void);
 bool transport_is_swim(void);
 
-#if BUILD_HLADAPTER
+#if BUILD_HLADAPTER && !HAVE_JTAG_MINIDRIVER_H
 bool transport_is_hla(void);
 #else
 static inline bool transport_is_hla(void)
diff --git a/tcl/board/eir.cfg b/tcl/board/eir.cfg
index 422db0d..67758b8 100644
--- a/tcl/board/eir.cfg
+++ b/tcl/board/eir.cfg
@@ -1,7 +1,7 @@
 # Elector Internet Radio board
 # http://www.ethernut.de/en/hardware/eir/index.html
 
-source [find target/sam7se512.cfg]
+source [find target/at91sam7se512.cfg]
 
 $_TARGETNAME configure -event reset-init {
 	# WDT_MR, disable watchdog
diff --git a/tcl/board/olimex_sam7_ex256.cfg b/tcl/board/olimex_sam7_ex256.cfg
index 426ead6..08ed4c1 100644
--- a/tcl/board/olimex_sam7_ex256.cfg
+++ b/tcl/board/olimex_sam7_ex256.cfg
@@ -1,3 +1,3 @@
 # Olimex SAM7-EX256 has a single Atmel at91sam7ex256 on it.
 
-source [find target/sam7x256.cfg]
+source [find target/at91sam7x256.cfg]
diff --git a/tcl/board/open-bldc.cfg b/tcl/board/open-bldc.cfg
deleted file mode 100644
index da8654c..0000000
--- a/tcl/board/open-bldc.cfg
+++ /dev/null
@@ -1,7 +0,0 @@
-# Open Source Brush Less DC Motor Controller
-# http://open-bldc.org
-
-# Work-area size (RAM size) = 20kB for STM32F103RB device
-set WORKAREASIZE 0x5000
-
-source [find target/stm32.cfg]
diff --git a/tcl/board/st_b-l475e-iot01a.cfg b/tcl/board/st_b-l475e-iot01a.cfg
new file mode 100644
index 0000000..e75c99d
--- /dev/null
+++ b/tcl/board/st_b-l475e-iot01a.cfg
@@ -0,0 +1,58 @@
+# SPDX-License-Identifier: GPL-2.0-or-later
+
+# This is an B-L475E-IOT01A Discovery kit for IoT node with a single STM32L475VGT6 chip.
+# http://www.st.com/en/evaluation-tools/b-l475e-iot01a.html
+
+# This is for using the onboard STLINK
+source [find interface/stlink.cfg]
+
+transport select hla_swd
+
+# increase working area to 96KB
+set WORKAREASIZE 0x18000
+
+# enable stmqspi
+set QUADSPI 1
+
+source [find target/stm32l4x.cfg]
+
+# QUADSPI initialization
+proc qspi_init { } {
+	global a
+	mmw 0x4002104C 0x000001FF 0				;# RCC_AHB2ENR |= GPIOAEN-GPIOIEN (enable clocks)
+	mmw 0x40021050 0x00000100 0				;# RCC_AHB3ENR |= QSPIEN (enable clock)
+	sleep 1									;# Wait for clock startup
+
+	# PE11: NCS, PE10: CLK, PE15: BK1_IO3, PE14: BK1_IO2, PE13: BK1_IO1, PE12: BK1_IO0
+
+	# PE15:AF10:V, PE14:AF10:V, PE13:AF10:V, PE12:AF10:V, PE11:AF10:V, PE10:AF10:V
+
+	# Port E: PE15:AF10:V, PE14:AF10:V, PE13:AF10:V, PE12:AF10:V, PE11:AF10:V, PE10:AF10:V
+	mmw 0x48001000 0xAAA00000 0x55500000	;# MODER
+	mmw 0x48001008 0xFFF00000 0x00000000	;# OSPEEDR
+	mmw 0x48001024 0xAAAAAA00 0x55555500	;# AFRH
+
+	mww 0xA0001030 0x00001000				;# QUADSPI_LPTR: deactivate CS after 4096 clocks when FIFO is full
+	mww 0xA0001000 0x01500008				;# QUADSPI_CR: PRESCALER=1, APMS=1, FTHRES=0, FSEL=0, DFM=0, SSHIFT=0, TCEN=1
+	mww 0xA0001004 0x00160100				;# QUADSPI_DCR: FSIZE=0x16, CSHT=0x01, CKMODE=0
+	mmw 0xA0001000 0x00000001 0				;# QUADSPI_CR: EN=1
+
+	# memory-mapped read mode with 3-byte addresses
+	mww 0xA0001014 0x0D002503				;# QUADSPI_CCR: FMODE=0x3, DMODE=0x1, DCYC=0x0, ADSIZE=0x2, ADMODE=0x1, IMODE=0x1, INSTR=READ
+}
+
+$_TARGETNAME configure -event reset-init {
+	mmw 0x40022000 0x00000004 0x00000003	;# 4 WS for 72 MHz HCLK
+	sleep 1
+	mmw 0x40021000 0x00000100 0x00000000	;# HSI on
+	mww 0x4002100C 0x01002432				;# 72 MHz: PLLREN=1, PLLM=4, PLLN=36, PLLR=2, HSI
+	mww 0x40021008 0x00008001				;# always HSI, APB1: /1, APB2: /1
+	mmw 0x40021000 0x01000000 0x00000000	;# PLL on
+	sleep 1
+	mmw 0x40021008 0x00000003 0x00000000	;# switch to PLL
+	sleep 1
+
+	adapter speed 4000
+
+	qspi_init
+}
diff --git a/tcl/board/st_nucleo_l5.cfg b/tcl/board/st_nucleo_l5.cfg
new file mode 100644
index 0000000..6450f08
--- /dev/null
+++ b/tcl/board/st_nucleo_l5.cfg
@@ -0,0 +1,13 @@
+# SPDX-License-Identifier: GPL-2.0-or-later
+
+# This is for STM32L5 Nucleo Dev Boards.
+# http://www.st.com/en/evaluation-tools/stm32-mcu-nucleo.html
+
+source [find interface/stlink-dap.cfg]
+
+transport select dapdirect_swd
+
+source [find target/stm32l5x.cfg]
+
+# use hardware reset
+reset_config srst_only srst_nogate
diff --git a/tcl/board/stm32f412g-disco.cfg b/tcl/board/stm32f412g-disco.cfg
new file mode 100644
index 0000000..b6bdb64
--- /dev/null
+++ b/tcl/board/stm32f412g-disco.cfg
@@ -0,0 +1,70 @@
+# This is an STM32F412G discovery board with a single STM32F412ZGT6 chip.
+# http://www.st.com/en/evaluation-tools/32f412gdiscovery.html
+
+# This is for using the onboard STLINK
+source [find interface/stlink.cfg]
+
+transport select hla_swd
+
+# increase working area to 128KB
+set WORKAREASIZE 0x20000
+
+# enable stmqspi
+set QUADSPI 1
+
+source [find target/stm32f4x.cfg]
+
+# QUADSPI initialization
+proc qspi_init { } {
+	global a
+	mmw 0x40023830 0x000000FF 0				;# RCC_AHB1ENR |= GPIOAEN-GPIOHEN (enable clocks)
+	mmw 0x40023838 0x00000002 0				;# RCC_AHB3ENR |= QSPIEN (enable clock)
+	sleep 1									;# Wait for clock startup
+
+	# PB02: CLK, PG06: BK1_NCS, PF06: BK1_IO3, PF07: BK1_IO2, PF09: BK1_IO1, PF08: BK1_IO0
+
+	# PB02:AF09:V, PF09:AF10:V, PF08:AF10:V, PF07:AF09:V, PF06:AF09:V, PG06:AF10:V
+
+	# Port B: PB02:AF09:V
+	mmw 0x40020400 0x00000020 0x00000010	;# MODER
+	mmw 0x40020408 0x00000030 0x00000000	;# OSPEEDR
+	mmw 0x40020420 0x00000900 0x00000600	;# AFRL
+
+	# Port F: PF09:AF10:V, PF08:AF10:V, PF07:AF09:V, PF06:AF09:V
+	mmw 0x40021400 0x000AA000 0x00055000	;# MODER
+	mmw 0x40021408 0x000FF000 0x00000000	;# OSPEEDR
+	mmw 0x40021420 0x99000000 0x66000000	;# AFRL
+	mmw 0x40021424 0x000000AA 0x00000055	;# AFRH
+
+	# Port G: PG06:AF10:V
+	mmw 0x40021800 0x00002000 0x00001000	;# MODER
+	mmw 0x40021808 0x00003000 0x00000000	;# OSPEEDR
+	mmw 0x40021820 0x0A000000 0x05000000	;# AFRL
+
+	mww 0xA0001030 0x00001000				;# QUADSPI_LPTR: deactivate CS after 4096 clocks when FIFO is full
+	mww 0xA0001000 0x03500008				;# QUADSPI_CR: PRESCALER=3, APMS=1, FTHRES=0, FSEL=0, DFM=0, SSHIFT=0, TCEN=1
+	mww 0xA0001004 0x00170100				;# QUADSPI_DCR: FSIZE=0x17, CSHT=0x01, CKMODE=0
+	mmw 0xA0001000 0x00000001 0				;# QUADSPI_CR: EN=1
+
+	# 1-line spi mode
+	mww 0xA0001014 0x000003F5				;# QUADSPI_CCR: FMODE=0x0, DMODE=0x0, DCYC=0x0, ADSIZE=0x0, ADMODE=0x0, IMODE=0x3, INSTR=RSTQIO
+	sleep 1
+
+	# memory-mapped read mode with 3-byte addresses
+	mww 0xA0001014 0x0D002503				;# QUADSPI_CCR: FMODE=0x3, DMODE=0x1, DCYC=0x0, ADSIZE=0x2, ADMODE=0x1, IMODE=0x1, INSTR=READ
+}
+
+$_TARGETNAME configure -event reset-init {
+	mww 0x40023C00 0x00000003				;# 3 WS for 96 MHz HCLK
+	sleep 1
+	mww 0x40023804 0x24001808				;# 96 MHz: HSI, PLLM=8, PLLN=96, PLLP=2
+	mww 0x40023808 0x00001000				;# APB1: /2, APB2: /1
+	mmw 0x40023800 0x01000000 0x00000000	;# PLL on
+	sleep 1
+	mmw 0x40023808 0x00000002 0x00000000	;# switch to PLL
+	sleep 1
+
+	adapter speed 4000
+
+	qspi_init
+}
diff --git a/tcl/board/stm32f413h-disco.cfg b/tcl/board/stm32f413h-disco.cfg
new file mode 100644
index 0000000..99f2a49
--- /dev/null
+++ b/tcl/board/stm32f413h-disco.cfg
@@ -0,0 +1,83 @@
+# This is an STM32F413H discovery board with a single STM32F413ZHT6 chip.
+# http://www.st.com/en/evaluation-tools/32f413hdiscovery.html
+
+#
+# Untested!!!
+#
+
+# This is for using the onboard STLINK
+source [find interface/stlink.cfg]
+
+transport select hla_swd
+
+# increase working area to 128KB
+set WORKAREASIZE 0x20000
+
+# enable stmqspi
+set QUADSPI 1
+
+source [find target/stm32f4x.cfg]
+
+# QUADSPI initialization
+proc qspi_init { } {
+	global a
+	mmw 0x40023830 0x000000FF 0				;# RCC_AHB1ENR |= GPIOAEN-GPIOHEN (enable clocks)
+	mmw 0x40023838 0x00000002 0				;# RCC_AHB3ENR |= QSPIEN (enable clock)
+	sleep 1									;# Wait for clock startup
+
+	# PG06: BK1_NCS, PB02: CLK, PD13: BK1_IO3, PE02: BK1_IO2, PF09: BK1_IO1, PF08: BK1_IO0
+
+	# PB02:AF09:V, PD13:AF09:V, PE02:AF09:V, PF09:AF10:V, PF08:AF10:V, PG06:AF10:V
+
+	# Port B: PB02:AF09:V
+	mmw 0x40020400 0x00000020 0x00000010	;# MODER
+	mmw 0x40020408 0x00000030 0x00000000	;# OSPEEDR
+	mmw 0x40020420 0x00000900 0x00000600	;# AFRL
+
+	# Port D: PD13:AF09:V
+	mmw 0x40020C00 0x08000000 0x04000000	;# MODER
+	mmw 0x40020C08 0x0C000000 0x00000000	;# OSPEEDR
+	mmw 0x40020C24 0x00900000 0x00600000	;# AFRH
+
+	# Port E: PE02:AF09:V
+	mmw 0x40021000 0x00000020 0x00000010	;# MODER
+	mmw 0x40021008 0x00000030 0x00000000	;# OSPEEDR
+	mmw 0x40021020 0x00000900 0x00000600	;# AFRL
+
+	# Port F: PF09:AF10:V, PF08:AF10:V
+	mmw 0x40021400 0x000A0000 0x00050000	;# MODER
+	mmw 0x40021408 0x000F0000 0x00000000	;# OSPEEDR
+	mmw 0x40021424 0x000000AA 0x00000055	;# AFRH
+
+	# Port G: PG06:AF10:V
+	mmw 0x40021800 0x00002000 0x00001000	;# MODER
+	mmw 0x40021808 0x00003000 0x00000000	;# OSPEEDR
+	mmw 0x40021820 0x0A000000 0x05000000	;# AFRL
+
+	mww 0xA0001030 0x00001000				;# QUADSPI_LPTR: deactivate CS after 4096 clocks when FIFO is full
+	mww 0xA0001000 0x03500008				;# QUADSPI_CR: PRESCALER=3, APMS=1, FTHRES=0, FSEL=0, DFM=0, SSHIFT=0, TCEN=1
+	mww 0xA0001004 0x00170100				;# QUADSPI_DCR: FSIZE=0x17, CSHT=0x01, CKMODE=0
+	mmw 0xA0001000 0x00000001 0				;# QUADSPI_CR: EN=1
+
+	# 1-line spi mode
+	mww 0xA0001014 0x000003F5				;# QUADSPI_CCR: FMODE=0x0, DMODE=0x0, DCYC=0x0, ADSIZE=0x0, ADMODE=0x0, IMODE=0x3, INSTR=RSTQIO
+	sleep 1
+
+	# memory-mapped read mode with 3-byte addresses
+	mww 0xA0001014 0x0D002503				;# QUADSPI_CCR: FMODE=0x3, DMODE=0x1, DCYC=0x0, ADSIZE=0x2, ADMODE=0x1, IMODE=0x1, INSTR=READ
+}
+
+$_TARGETNAME configure -event reset-init {
+	mww 0x40023C00 0x00000003				;# 3 WS for 96 MHz HCLK
+	sleep 1
+	mww 0x40023804 0x24001808				;# 96 MHz: HSI, PLLM=8, PLLN=96, PLLP=2
+	mww 0x40023808 0x00001000				;# APB1: /2, APB2: /1
+	mmw 0x40023800 0x01000000 0x00000000	;# PLL on
+	sleep 1
+	mmw 0x40023808 0x00000002 0x00000000	;# switch to PLL
+	sleep 1
+
+	adapter speed 4000
+
+	qspi_init
+}
diff --git a/tcl/board/stm32f469i-disco.cfg b/tcl/board/stm32f469i-disco.cfg
new file mode 100644
index 0000000..ab67512
--- /dev/null
+++ b/tcl/board/stm32f469i-disco.cfg
@@ -0,0 +1,65 @@
+# This is an STM32F469I discovery board with a single STM32F469NIH6 chip.
+# http://www.st.com/en/evaluation-tools/32f469idiscovery.html
+
+# This is for using the onboard STLINK
+source [find interface/stlink.cfg]
+
+transport select hla_swd
+
+# increase working area to 128KB
+set WORKAREASIZE 0x20000
+
+# enable stmqspi
+set QUADSPI 1
+
+source [find target/stm32f4x.cfg]
+
+# QUADSPI initialization
+proc qspi_init { } {
+	global a
+	mmw 0x40023830 0x000007FF 0				;# RCC_AHB1ENR |= GPIOAEN-GPIOKEN (enable clocks)
+	mmw 0x40023838 0x00000002 0				;# RCC_AHB3ENR |= QSPIEN (enable clock)
+	sleep 1									;# Wait for clock startup
+
+	# PF10: CLK, PB06: BK1_NCS, PF06: BK1_IO3, PF07: BK1_IO2, PF09: BK1_IO1, PF08: BK1_IO0
+
+	# PB06:AF10:V, PF10:AF09:V, PF09:AF10:V, PF08:AF10:V, PF07:AF09:V, PF06:AF09:V
+
+	# Port B: PB06:AF10:V
+	mmw 0x40020400 0x00002000 0x00001000	;# MODER
+	mmw 0x40020408 0x00003000 0x00000000	;# OSPEEDR
+	mmw 0x40020420 0x0A000000 0x05000000	;# AFRL
+
+	# Port F: PF10:AF09:V, PF09:AF10:V, PF08:AF10:V, PF07:AF09:V, PF06:AF09:V
+	mmw 0x40021400 0x002AA000 0x00155000	;# MODER
+	mmw 0x40021408 0x003FF000 0x00000000	;# OSPEEDR
+	mmw 0x40021420 0x99000000 0x66000000	;# AFRL
+	mmw 0x40021424 0x000009AA 0x00000655	;# AFRH
+
+	mww 0xA0001030 0x00001000				;# QUADSPI_LPTR: deactivate CS after 4096 clocks when FIFO is full
+	mww 0xA0001000 0x03500008				;# QUADSPI_CR: PRESCALER=3, APMS=1, FTHRES=0, FSEL=0, DFM=0, SSHIFT=0, TCEN=1
+	mww 0xA0001004 0x00170100				;# QUADSPI_DCR: FSIZE=0x17, CSHT=0x01, CKMODE=0
+	mmw 0xA0001000 0x00000001 0				;# QUADSPI_CR: EN=1
+
+	# 1-line spi mode
+	mww 0xA0001014 0x000003F5				;# QUADSPI_CCR: FMODE=0x0, DMODE=0x0, DCYC=0x0, ADSIZE=0x0, ADMODE=0x0, IMODE=0x3, INSTR=RSTQIO
+	sleep 1
+
+	# memory-mapped read mode with 3-byte addresses
+	mww 0xA0001014 0x0D002503				;# QUADSPI_CCR: FMODE=0x3, DMODE=0x1, DCYC=0x0, ADSIZE=0x2, ADMODE=0x1, IMODE=0x1, INSTR=READ
+}
+
+$_TARGETNAME configure -event reset-init {
+	mww 0x40023C00 0x00000005				;# 5 WS for 160 MHz HCLK
+	sleep 1
+	mww 0x40023804 0x24002808				;# 160 MHz: HSI, PLLM=8, PLLN=160, PLLP=2
+	mww 0x40023808 0x00009400				;# APB1: /4, APB2: /2
+	mmw 0x40023800 0x01000000 0x00000000	;# PLL on
+	sleep 1
+	mmw 0x40023808 0x00000002 0x00000000	;# switch to PLL
+	sleep 1
+
+	adapter speed 4000
+
+	qspi_init
+}
diff --git a/tcl/board/stm32f723e-disco.cfg b/tcl/board/stm32f723e-disco.cfg
new file mode 100644
index 0000000..3c04d86
--- /dev/null
+++ b/tcl/board/stm32f723e-disco.cfg
@@ -0,0 +1,74 @@
+# This is an STM32F723E discovery board with a single STM32F723IEK6 chip.
+# http://www.st.com/en/evaluation-tools/32f723ediscovery.html
+
+# This is for using the onboard STLINK
+source [find interface/stlink.cfg]
+
+transport select hla_swd
+
+# increase working area to 128KB
+set WORKAREASIZE 0x20000
+
+# enable stmqspi
+set QUADSPI 1
+
+source [find target/stm32f7x.cfg]
+
+# QUADSPI initialization
+proc qspi_init { } {
+	global a
+	mmw 0x40023830 0x000007FF 0				;# RCC_AHB1ENR |= GPIOAEN-GPIOKEN (enable clocks)
+	mmw 0x40023838 0x00000002 0				;# RCC_AHB3ENR |= QSPIEN (enable clock)
+	sleep 1									;# Wait for clock startup
+
+	# PB02: CLK, PB06: BK1_NCS, PD13: BK1_IO3, PE02: BK1_IO2, PC10: BK1_IO1, PC09: BK1_IO0
+
+	# PB06:AF10:V, PB02:AF09:V, PC10:AF09:V, PC09:AF09:V, PD13:AF09:V, PE02:AF09:V
+
+	# Port B: PB06:AF10:V, PB02:AF09:V
+	mmw 0x40020400 0x00002020 0x00001010	;# MODER
+	mmw 0x40020408 0x00003030 0x00000000	;# OSPEEDR
+	mmw 0x40020420 0x0A000900 0x05000600	;# AFRL
+
+	# Port C: PC10:AF09:V, PC09:AF09:V
+	mmw 0x40020800 0x00280000 0x00140000	;# MODER
+	mmw 0x40020808 0x003C0000 0x00000000	;# OSPEEDR
+	mmw 0x40020824 0x00000990 0x00000660	;# AFRH
+
+	# Port D: PD13:AF09:V
+	mmw 0x40020C00 0x08000000 0x04000000	;# MODER
+	mmw 0x40020C08 0x0C000000 0x00000000	;# OSPEEDR
+	mmw 0x40020C24 0x00900000 0x00600000	;# AFRH
+
+	# Port E: PE02:AF09:V
+	mmw 0x40021000 0x00000020 0x00000010	;# MODER
+	mmw 0x40021008 0x00000030 0x00000000	;# OSPEEDR
+	mmw 0x40021020 0x00000900 0x00000600	;# AFRL
+
+	mww 0xA0001030 0x00001000				;# QUADSPI_LPTR: deactivate CS after 4096 clocks when FIFO is full
+	mww 0xA0001000 0x03500008				;# QUADSPI_CR: PRESCALER=3, APMS=1, FTHRES=0, FSEL=0, DFM=0, SSHIFT=0, TCEN=1
+	mww 0xA0001004 0x00190100				;# QUADSPI_DCR: FSIZE=0x19, CSHT=0x01, CKMODE=0
+	mmw 0xA0001000 0x00000001 0				;# QUADSPI_CR: EN=1
+
+	# 1-line spi mode
+	mww 0xA0001014 0x000003F5				;# QUADSPI_CCR: FMODE=0x0, DMODE=0x0, DCYC=0x0, ADSIZE=0x0, ADMODE=0x0, IMODE=0x3, INSTR=RSTQIO
+	sleep 1
+
+	# memory-mapped read mode with 4-byte addresses
+	mww 0xA0001014 0x0D003513				;# QUADSPI_CCR: FMODE=0x3, DMODE=0x1, DCYC=0x0, ADSIZE=0x3, ADMODE=0x1, IMODE=0x1, INSTR=READ
+}
+
+$_TARGETNAME configure -event reset-init {
+	mww 0x40023C00 0x00000006				;# 6 WS for 192 MHz HCLK
+	sleep 1
+	mww 0x40023804 0x24003008				;# 192 MHz: PLLM=8, PLLN=192, PLLP=2
+	mww 0x40023808 0x00009400				;# APB1: /4, APB2: /2
+	mmw 0x40023800 0x01000000 0x00000000	;# PLL on
+	sleep 1
+	mmw 0x40023808 0x00000002 0x00000000	;# switch to PLL
+	sleep 1
+
+	adapter speed 4000
+
+	qspi_init
+}
diff --git a/tcl/board/stm32f746g-disco.cfg b/tcl/board/stm32f746g-disco.cfg
new file mode 100644
index 0000000..14e89e1
--- /dev/null
+++ b/tcl/board/stm32f746g-disco.cfg
@@ -0,0 +1,69 @@
+# This is an STM32F746G discovery board with a single STM32F746NGH6 chip.
+# http://www.st.com/en/evaluation-tools/32f746gdiscovery.html
+
+# This is for using the onboard STLINK
+source [find interface/stlink.cfg]
+
+transport select hla_swd
+
+# increase working area to 256KB
+set WORKAREASIZE 0x40000
+
+# enable stmqspi
+set QUADSPI 1
+
+source [find target/stm32f7x.cfg]
+
+# QUADSPI initialization
+proc qspi_init { } {
+	global a
+	mmw 0x40023830 0x000007FF 0				;# RCC_AHB1ENR |= GPIOAEN-GPIOKEN (enable clocks)
+	mmw 0x40023838 0x00000002 0				;# RCC_AHB3ENR |= QSPIEN (enable clock)
+	sleep 1									;# Wait for clock startup
+
+	# PB02: CLK, PB06: BK1_NCS, PD13: BK1_IO3, PE02: BK1_IO2, PD12: BK1_IO1, PD11: BK1_IO0
+
+	# PB06:AF10:V, PB02:AF09:V, PD13:AF09:V, PD12:AF09:V, PD11:AF09:V, PE02:AF09:V
+
+	# Port B: PB06:AF10:V, PB02:AF09:V
+	mmw 0x40020400 0x00002020 0x00001010	;# MODER
+	mmw 0x40020408 0x00003030 0x00000000	;# OSPEEDR
+	mmw 0x40020420 0x0A000900 0x05000600	;# AFRL
+
+	# Port D: PD13:AF09:V, PD12:AF09:V, PD11:AF09:V
+	mmw 0x40020C00 0x0A800000 0x05400000	;# MODER
+	mmw 0x40020C08 0x0FC00000 0x00000000	;# OSPEEDR
+	mmw 0x40020C24 0x00999000 0x00666000	;# AFRH
+
+	# Port E: PE02:AF09:V
+	mmw 0x40021000 0x00000020 0x00000010	;# MODER
+	mmw 0x40021008 0x00000030 0x00000000	;# OSPEEDR
+	mmw 0x40021020 0x00000900 0x00000600	;# AFRL
+
+	mww 0xA0001030 0x00001000				;# QUADSPI_LPTR: deactivate CS after 4096 clocks when FIFO is full
+	mww 0xA0001000 0x03500008				;# QUADSPI_CR: PRESCALER=3, APMS=1, FTHRES=0, FSEL=0, DFM=0, SSHIFT=0, TCEN=1
+	mww 0xA0001004 0x00170100				;# QUADSPI_DCR: FSIZE=0x17, CSHT=0x01, CKMODE=0
+	mmw 0xA0001000 0x00000001 0				;# QUADSPI_CR: EN=1
+
+	# 1-line spi mode
+	mww 0xA0001014 0x000003F5				;# QUADSPI_CCR: FMODE=0x0, DMODE=0x0, DCYC=0x0, ADSIZE=0x0, ADMODE=0x0, IMODE=0x3, INSTR=RSTQIO
+	sleep 1
+
+	# memory-mapped read mode with 3-byte addresses
+	mww 0xA0001014 0x0D002503				;# QUADSPI_CCR: FMODE=0x3, DMODE=0x1, DCYC=0x0, ADSIZE=0x2, ADMODE=0x1, IMODE=0x1, INSTR=READ
+}
+
+$_TARGETNAME configure -event reset-init {
+	mww 0x40023C00 0x00000006				;# 6 WS for 192 MHz HCLK
+	sleep 1
+	mww 0x40023804 0x24003008				;# 192 MHz: PLLM=8, PLLN=192, PLLP=2
+	mww 0x40023808 0x00009400				;# APB1: /4, APB2: /2
+	mmw 0x40023800 0x01000000 0x00000000	;# PLL on
+	sleep 1
+	mmw 0x40023808 0x00000002 0x00000000	;# switch to PLL
+	sleep 1
+
+	adapter speed 4000
+
+	qspi_init
+}
diff --git a/tcl/board/stm32f769i-disco.cfg b/tcl/board/stm32f769i-disco.cfg
new file mode 100644
index 0000000..cc4334b
--- /dev/null
+++ b/tcl/board/stm32f769i-disco.cfg
@@ -0,0 +1,79 @@
+# This is an STM32F769I discovery board with a single STM32F769NIH6 chip.
+# http://www.st.com/en/evaluation-tools/32f769idiscovery.html
+
+# This is for using the onboard STLINK
+source [find interface/stlink.cfg]
+
+transport select hla_swd
+
+# increase working area to 256KB
+set WORKAREASIZE 0x40000
+
+# enable stmqspi
+set QUADSPI 1
+
+source [find target/stm32f7x.cfg]
+
+# QUADSPI initialization
+proc qspi_init { } {
+	global a
+	mmw 0x40023830 0x000007FF 0				;# RCC_AHB1ENR |= GPIOAEN-GPIOKEN (enable clocks)
+	mmw 0x40023838 0x00000002 0				;# RCC_AHB3ENR |= QSPIEN (enable clock)
+	sleep 1									;# Wait for clock startup
+
+	# PB02: CLK, PB06: BK1_NCS, PD13: BK1_IO3, PE02: BK1_IO2, PC10: BK1_IO1, PC09: BK1_IO0
+
+	# PB06:AF10:V, PB02:AF09:V, PC10:AF09:V, PC09:AF09:V, PD13:AF09:V, PE02:AF09:V
+
+	# Port B: PB06:AF10:V, PB02:AF09:V
+	mmw 0x40020400 0x00002020 0x00001010	;# MODER
+	mmw 0x40020408 0x00003030 0x00000000	;# OSPEEDR
+	mmw 0x40020420 0x0A000900 0x05000600	;# AFRL
+
+	# Port C: PC10:AF09:V, PC09:AF09:V
+	mmw 0x40020800 0x00280000 0x00140000	;# MODER
+	mmw 0x40020808 0x003C0000 0x00000000	;# OSPEEDR
+	mmw 0x40020824 0x00000990 0x00000660	;# AFRH
+
+	# Port D: PD13:AF09:V
+	mmw 0x40020C00 0x08000000 0x04000000	;# MODER
+	mmw 0x40020C08 0x0C000000 0x00000000	;# OSPEEDR
+	mmw 0x40020C24 0x00900000 0x00600000	;# AFRH
+
+	# Port E: PE02:AF09:V
+	mmw 0x40021000 0x00000020 0x00000010	;# MODER
+	mmw 0x40021008 0x00000030 0x00000000	;# OSPEEDR
+	mmw 0x40021020 0x00000900 0x00000600	;# AFRL
+
+	mww 0xA0001030 0x00001000				;# QUADSPI_LPTR: deactivate CS after 4096 clocks when FIFO is full
+	mww 0xA0001000 0x03500008				;# QUADSPI_CR: PRESCALER=3, APMS=1, FTHRES=0, FSEL=0, DFM=0, SSHIFT=0, TCEN=1
+	mww 0xA0001004 0x00190100				;# QUADSPI_DCR: FSIZE=0x19, CSHT=0x01, CKMODE=0
+	mmw 0xA0001000 0x00000001 0				;# QUADSPI_CR: EN=1
+
+	# exit qpi mode
+	mww 0xA0001014 0x000033f5				;# QUADSPI_CCR: FMODE=0x0, DMODE=0x0, DCYC=0x0, ADSIZE=0x0, ADMODE=0x0, IMODE=0x3, INSTR=RSTQIO
+
+	# 1-line memory-mapped read mode with 4-byte addresses
+	mww 0xA0001014 0x0D003513				;# QUADSPI_CCR: FMODE=0x3, DMODE=0x1, DCYC=0x0, ADSIZE=0x3, ADMODE=0x1, IMODE=0x1, INSTR=READ
+
+	# 4-line qpi mode
+	mww 0xA0001014 0x00003135				;# QUADSPI_CCR: FMODE=0x0, DMODE=0x0, DCYC=0x0, ADSIZE=0x3, ADMODE=0x1, IMODE=0x1, INSTR=EQIO
+
+	# 4-line memory-mapped read mode with 4-byte addresses
+	mww 0xA0001014 0x0F283FEC				;# QUADSPI_CCR: FMODE=0x3, DMODE=0x1, DCYC=0xA, ADSIZE=0x3, ADMODE=0x1, IMODE=0x1, INSTR=4READ4B
+}
+
+$_TARGETNAME configure -event reset-init {
+	mww 0x40023C00 0x00000006				;# 6 WS for 192 MHz HCLK
+	sleep 1
+	mww 0x40023804 0x24003008				;# 192 MHz: PLLM=8, PLLN=192, PLLP=2
+	mww 0x40023808 0x00009400				;# APB1: /4, APB2: /2
+	mmw 0x40023800 0x01000000 0x00000000	;# PLL on
+	sleep 1
+	mmw 0x40023808 0x00000002 0x00000000	;# switch to PLL
+	sleep 1
+
+	adapter speed 4000
+
+	qspi_init
+}
diff --git a/tcl/board/stm32h735g-disco.cfg b/tcl/board/stm32h735g-disco.cfg
new file mode 100644
index 0000000..405e470
--- /dev/null
+++ b/tcl/board/stm32h735g-disco.cfg
@@ -0,0 +1,122 @@
+# This is a stm32h735g-dk with a single STM32H735IGK6 chip.
+# https://www.st.com/en/evaluation-tools/stm32h735g-dk.html
+#
+
+# This is for using the onboard STLINK
+source [find interface/stlink.cfg]
+
+transport select hla_swd
+
+set CHIPNAME stm32h735igk6
+
+# enable stmqspi
+if {![info exists OCTOSPI1]} {
+	set OCTOSPI1 1
+	set OCTOSPI2 0
+}
+
+source [find target/stm32h7x.cfg]
+
+# OCTOSPI initialization
+# octo: 8-line mode
+proc octospi_init { octo } {
+	global a b
+	mmw 0x58024540 0x000006FF 0				;# RCC_AHB4ENR |= GPIOAEN-GPIOKEN (enable clocks)
+	mmw 0x58024534 0x00284000 0				;# RCC_AHB3ENR |= IOMNGREN, OSPI2EN, OSPI1EN (enable clocks)
+	sleep 1									;# Wait for clock startup
+
+	mww 0x5200B404 0x03010111				;# OCTOSPIM_P1CR: assign Port 1 to OCTOSPI1
+	mww 0x5200B408 0x00000000				;# OCTOSPIM_P2CR: disable Port 2
+
+	# PG06: OCSPI1_NCS, PF10: OCSPI1_CLK, PB02: OCSPI1_DQS, PD07: OCSPI1_IO7, PG09: OCSPI1_IO6, PD05: OCSPI1_IO5,
+	# PD04: OCSPI1_IO4, PD13: OCSPI1_IO3, PE02: OCSPI1_IO2, PD12: OCSPI1_IO1, PD11: OCSPI1_IO0
+
+	# PB02:AF10:V, PD13:AF09:V, PD12:AF09:V, PD11:AF09:V, PD07:AF10:V, PD05:AF10:V
+	# PD04:AF10:V, PE02:AF09:V, PF10:AF09:V, PG09:AF09:V, PG06:AF10:V
+	# Port B: PB02:AF10:V
+	mmw 0x58020400 0x00000020 0x00000010	;# MODER
+	mmw 0x58020408 0x00000030 0x00000000	;# OSPEEDR
+	mmw 0x5802040C 0x00000000 0x00000030	;# PUPDR
+	mmw 0x58020420 0x00000A00 0x00000500	;# AFRL
+	# Port D: PD13:AF09:V, PD12:AF09:V, PD11:AF09:V, PD07:AF10:V, PD05:AF10:V, PD04:AF10:V
+	mmw 0x58020C00 0x0A808A00 0x05404500	;# MODER
+	mmw 0x58020C08 0x0FC0CF00 0x00000000	;# OSPEEDR
+	mmw 0x58020C0C 0x00000000 0x0FC0CF00	;# PUPDR
+	mmw 0x58020C20 0xA0AA0000 0x50550000	;# AFRL
+	mmw 0x58020C24 0x00999000 0x00666000	;# AFRH
+	# Port E: PE02:AF09:V
+	mmw 0x58021000 0x00000020 0x00000010	;# MODER
+	mmw 0x58021008 0x00000030 0x00000000	;# OSPEEDR
+	mmw 0x5802100C 0x00000000 0x00000030	;# PUPDR
+	mmw 0x58021020 0x00000900 0x00000600	;# AFRL
+	# Port F: PF10:AF09:V
+	mmw 0x58021400 0x00200000 0x00100000	;# MODER
+	mmw 0x58021408 0x00300000 0x00000000	;# OSPEEDR
+	mmw 0x5802140C 0x00000000 0x00300000	;# PUPDR
+	mmw 0x58021424 0x00000900 0x00000600	;# AFRH
+	# Port G: PG09:AF09:V, PG06:AF10:V
+	mmw 0x58021800 0x00082000 0x00041000	;# MODER
+	mmw 0x58021808 0x000C3000 0x00000000	;# OSPEEDR
+	mmw 0x5802180C 0x00000000 0x000C3000	;# PUPDR
+	mmw 0x58021820 0x0A000000 0x05000000	;# AFRL
+	mmw 0x58021824 0x00000090 0x00000060	;# AFRH
+
+	# OCTOSPI1: memory-mapped 1-line read mode with 4-byte addresses
+	mww 0x52005130 0x00001000				;# OCTOSPI_LPTR: deactivate CS after 4096 clocks when FIFO is full
+	mww 0x52005000 0x3040000B				;# OCTOSPI_CR: FMODE=0x1, APMS=1, FTHRES=0, FSEL=0, DQM=0, TCEN=0
+	mww 0x52005008 0x01190100				;# OCTOSPI_DCR1: MTYP=0x1, FSIZE=0x19, CSHT=0x01, CKMODE=0, DLYBYP=0
+	mww 0x5200500C 0x00000005				;# OCTOSPI_DCR2: PRESCALER=5
+
+	mww 0x52005108 0x00000000				;# OCTOSPI_TCR: SSHIFT=0, DHQC=0, DCYC=0x0
+	mww 0x52005100 0x01003101				;# OCTOSPI_CCR: DMODE=0x1, ABMODE=0x0, ADSIZE=0x3, ADMODE=0x1, ISIZE=0x0, IMODE=0x1
+	mww 0x52005110 0x00000013				;# OCTOSPI_IR: INSTR=READ4B
+
+	flash probe $a							;# load configuration from CR, TCR, CCR, IR register values
+
+	if { $octo == 1 } {
+		stmqspi cmd $a 1 0x71 0x00 0x00 0x00 0x00			;# Read Conf. Reg. 2, addr 0x00000000: DOPI, SOPI bits
+		stmqspi cmd $a 0 0x06								;# Write Enable
+		stmqspi cmd $a 1 0x05								;# Read Status Register
+		stmqspi cmd $a 0 0x72 0x00 0x00 0x00 0x00 0x02		;# Write Conf. Reg. 2, addr 0x00000000: DTR OPI enable
+
+		# OCTOSPI1: memory-mapped 8-line read mode with 4-byte addresses
+		mww 0x52005000 0x3040000B				;# OCTOSPI_CR: FMODE=0x3, APMS=1, FTHRES=0, FSEL=0, DQM=0, TCEN=1, EN=1
+		mww 0x52005108 0x10000006				;# OCTOSPI_TCR: SSHIFT=0, DHQC=1, DCYC=0x6
+		mww 0x52005100 0x2C003C1C				;# OCTOSPI_CCR: DTR, DMODE=0x4, ABMODE=0x0, ADSIZE=0x3, ADMODE=0x4, ISIZE=0x1, IMODE=0x4
+		mww 0x52005110 0x0000EE11				;# OCTOSPI_IR: INSTR=OCTA DTR Read
+
+		flash probe $a							;# reload configuration from CR, TCR, CCR, IR register values
+
+		stmqspi cmd $a 0 0x06								;# Write Enable
+		stmqspi cmd $a 1 0x05 0x00 0x00 0x00 0x00			;# Read Status Register (note dummy address in 8-line mode)
+		stmqspi cmd $a 0 0x04								;# Write Disable
+		stmqspi cmd $a 1 0x05 0x00 0x00 0x00 0x00			;# Read Status Register (note dummy address in 8-line mode)
+		stmqspi cmd $a 1 0x71 0x00 0x00 0x00 0x00			;# Read Conf. Reg. 2, addr 0x00000000: DOPI, SOPI bits
+	}
+}
+
+$_CHIPNAME.cpu0 configure -event reset-init {
+	global OCTOSPI1
+	global OCTOSPI2
+
+	mmw 0x52002000 0x00000004 0x0000000B	;# FLASH_ACR: 4 WS for 192 MHZ HCLK
+
+	mmw 0x58024400 0x00000001 0x00000018	;# RCC_CR: HSIDIV=1, HSI on
+	mmw 0x58024410 0x10000000 0xEE000007	;# RCC_CFGR: MCO2=system, MCO2PRE=8, HSI as system clock
+	mww 0x58024418 0x00000040				;# RCC_D1CFGR: D1CPRE=1, D1PPRE=2, HPRE=1
+	mww 0x5802441C 0x00000440				;# RCC_D2CFGR: D2PPRE2=2, D2PPRE1=2
+	mww 0x58024420 0x00000040				;# RCC_D3CFGR: D3PPRE=2
+	mww 0x58024428 0x00000040				;# RCC_PPLCKSELR: DIVM3=0, DIVM2=0, DIVM1=4, PLLSRC=HSI
+	mmw 0x5802442C 0x0001000C 0x00000002	;# RCC_PLLCFGR: PLL1RGE=8MHz to 16MHz, PLL1VCOSEL=wide
+	mww 0x58024430 0x01070217				;# RCC_PLL1DIVR: 192 MHz: DIVR1=2, DIVQ=8, DIVP1=2, DIVN1=24
+	mmw 0x58024400 0x01000000 0				;# RCC_CR: PLL1ON=1
+	sleep 1
+	mmw 0x58024410 0x00000003 0				;# RCC_CFGR: PLL1 as system clock
+	sleep 1
+
+	adapter speed 24000
+
+	if { $OCTOSPI1 } {
+		octospi_init 1
+	}
+}
diff --git a/tcl/board/stm32h745i-disco.cfg b/tcl/board/stm32h745i-disco.cfg
new file mode 100644
index 0000000..5adcfea
--- /dev/null
+++ b/tcl/board/stm32h745i-disco.cfg
@@ -0,0 +1,45 @@
+# This is a stm32h745i-disco with a single STM32H745XIH6 chip.
+# www.st.com/en/product/stm32h745i-disco.html
+#
+
+# This is for using the onboard STLINK
+source [find interface/stlink.cfg]
+
+transport select hla_swd
+
+set CHIPNAME stm32h745xih6
+
+# enable stmqspi
+if {![info exists QUADSPI]} {
+	set QUADSPI 1
+}
+
+source [find target/stm32h7x_dual_bank.cfg]
+
+source [find board/stm32h7x_dual_qspi.cfg]
+
+$_CHIPNAME.cpu0 configure -event reset-init {
+	global QUADSPI
+
+	mmw 0x52002000 0x00000004 0x0000000B	;# FLASH_ACR: 4 WS for 192 MHZ HCLK
+
+	mmw 0x58024400 0x00000001 0x00000018	;# RCC_CR: HSIDIV=1, HSI on
+	mmw 0x58024410 0x10000000 0xEE000007	;# RCC_CFGR: MCO2=system, MCO2PRE=8, HSI as system clock
+	mww 0x58024418 0x00000040				;# RCC_D1CFGR: D1CPRE=1, D1PPRE=2, HPRE=1
+	mww 0x5802441C 0x00000440				;# RCC_D2CFGR: D2PPRE2=2, D2PPRE1=2
+	mww 0x58024420 0x00000040				;# RCC_D3CFGR: D3PPRE=2
+	mww 0x58024428 0x00000040				;# RCC_PPLCKSELR: DIVM3=0, DIVM2=0, DIVM1=4, PLLSRC=HSI
+	mmw 0x5802442C 0x0001000C 0x00000002	;# RCC_PLLCFGR: PLL1RGE=8MHz to 16MHz, PLL1VCOSEL=wide
+	mww 0x58024430 0x01070217				;# RCC_PLL1DIVR: 192 MHz: DIVR1=2, DIVQ=8, DIVP1=2, DIVN1=24
+	mmw 0x58024400 0x01000000 0				;# RCC_CR: PLL1ON=1
+	sleep 1
+	mmw 0x58024410 0x00000003 0				;# RCC_CFGR: PLL1 as system clock
+	sleep 1
+
+	adapter speed 24000
+
+	if { $QUADSPI } {
+		qspi_init 1
+	}
+}
+
diff --git a/tcl/board/stm32h747i-disco.cfg b/tcl/board/stm32h747i-disco.cfg
new file mode 100644
index 0000000..22fd74a
--- /dev/null
+++ b/tcl/board/stm32h747i-disco.cfg
@@ -0,0 +1,136 @@
+# This is a stm32h747i-disco with a single STM32H747XIH6 chip.
+# www.st.com/en/product/stm32h747i-disco.html
+#
+
+# This is for using the onboard STLINK
+source [find interface/stlink.cfg]
+
+transport select hla_swd
+
+set CHIPNAME stm32h747xih6
+
+# enable stmqspi
+if {![info exists QUADSPI]} {
+	set QUADSPI 1
+}
+
+source [find target/stm32h7x_dual_bank.cfg]
+
+# QUADSPI initialization
+# qpi: 4-line mode
+proc qspi_init { qpi } {
+	global a
+	mmw 0x580244E0 0x000007FF 0				;# RCC_AHB4ENR |= GPIOAEN-GPIOKEN (enable clocks)
+	mmw 0x580244D4 0x00004000 0				;# RCC_AHB3ENR |= QSPIEN (enable clock)
+	sleep 1									;# Wait for clock startup
+
+	# PG06: BK1_NCS, PB02: CLK, PF06: BK1_IO3, PF07: BK1_IO2, PF09: BK1_IO1, PD11: BK1_IO0,
+	# PG14: BK2_IO3, PG09: BK2_IO2, PH03: BK2_IO1, PH02: BK2_IO0
+
+	# PB02:AF09:V, PD11:AF09:V, PF09:AF10:V, PF07:AF09:V, PF06:AF09:V, PG14:AF09:H
+	# PG09:AF09:V, PG06:AF10:H, PH03:AF09:V, PH02:AF09:V
+
+	# Port B: PB02:AF09:V
+	mmw 0x58020400 0x00000020 0x00000010	;# MODER
+	mmw 0x58020408 0x00000030 0x00000000	;# OSPEEDR
+	mmw 0x58020420 0x00000900 0x00000600	;# AFRL
+	# Port D: PD11:AF09:V
+	mmw 0x58020C00 0x00800000 0x00400000	;# MODER
+	mmw 0x58020C08 0x00C00000 0x00000000	;# OSPEEDR
+	mmw 0x58020C24 0x00009000 0x00006000	;# AFRH
+	# Port F: PF09:AF10:V, PF07:AF09:V, PF06:AF09:V
+	mmw 0x58021400 0x0008A000 0x00045000	;# MODER
+	mmw 0x58021408 0x000CF000 0x00000000	;# OSPEEDR
+	mmw 0x58021420 0x99000000 0x66000000	;# AFRL
+	mmw 0x58021424 0x000000A0 0x00000050	;# AFRH
+	# Port G: PG14:AF09:H, PG09:AF09:V, PG06:AF10:H
+	mmw 0x58021800 0x20082000 0x10041000	;# MODER
+	mmw 0x58021808 0x200C2000 0x10001000	;# OSPEEDR
+	mmw 0x58021820 0x0A000000 0x05000000	;# AFRL
+	mmw 0x58021824 0x09000090 0x06000060	;# AFRH
+	# Port H: PH03:AF09:V, PH02:AF09:V
+	mmw 0x58021C00 0x000000A0 0x00000050	;# MODER
+	mmw 0x58021C08 0x000000F0 0x00000000	;# OSPEEDR
+	mmw 0x58021C20 0x00009900 0x00006600	;# AFRL
+
+	# correct FSIZE is 0x1A, however, this causes trouble when
+	# reading the last bytes at end of bank in *memory mapped* mode
+
+	# for dual flash mode 2 * mt25ql512
+	mww 0x52005000 0x05500058				;# QUADSPI_CR: PRESCALER=5, APMS=1, FTHRES=0, FSEL=0, DFM=1, SSHIFT=1, TCEN=1
+	mww 0x52005004 0x001A0200				;# QUADSPI_DCR: FSIZE=0x1A, CSHT=0x02, CKMODE=0
+
+	mww 0x52005030 0x00001000				;# QUADSPI_LPTR: deactivate CS after 4096 clocks when FIFO is full
+	mww 0x52005014 0x0D002503				;# QUADSPI_CCR: FMODE=0x3, DMODE=0x1, DCYC=0x0, ADSIZE=0x3, ADMODE=0x1, IMODE=0x1
+	mmw 0x52005000 0x00000001 0				;# QUADSPI_CR: EN=1
+
+	# Exit QPI mode
+	mmw 0x52005000 0x00000002 0				;# QUADSPI_CR: ABORT=1
+	mww 0x52005014 0x000003F5				;# QUADSPI_CCR: FMODE=0x0, DMODE=0x0, DCYC=0x0, ADSIZE=0x0, ADMODE=0x0, IMODE=0x3, INSTR=Exit QPI
+	sleep 1
+
+	if { $qpi == 1 } {
+		# Write Enable
+		mmw 0x52005000 0x00000002 0			;# QUADSPI_CR: ABORT=1
+		mww 0x52005014 0x00000106			;# QUADSPI_CCR: FMODE=0x0, DMODE=0x0, DCYC=0x0, ADSIZE=0x0, ADMODE=0x0, IMODE=0x1, INSTR=Write Enable
+		sleep 1
+
+		# Configure dummy clocks via volatile configuration register
+		mmw 0x52005000 0x00000002 0			;# QUADSPI_CR: ABORT=1
+		mww 0x52005010 0x00000001			;# QUADSPI_DLR: 2 data bytes
+		mww 0x52005014 0x01000181			;# QUADSPI_CCR: FMODE=0x0, DMODE=0x1, DCYC=0x0, ADSIZE=0x0, ADMODE=0x0, IMODE=0x1, INSTR=Write Volatile Conf. Reg.
+		mwh 0x52005020 0xABAB				;# QUADSPI_DR: 0xAB 0xAB for 10 dummy clocks
+		sleep 1
+
+		# Write Enable
+		mmw 0x52005000 0x00000002 0			;# QUADSPI_CR: ABORT=1
+		mww 0x52005014 0x00000106			;# QUADSPI_CCR: FMODE=0x0, DMODE=0x0, DCYC=0x0, ADSIZE=0x0, ADMODE=0x0, IMODE=0x1, INSTR=Write Enable
+		sleep 1
+
+		# Enable QPI mode via enhanced volatile configuration register
+		mmw 0x52005000 0x00000002 0			;# QUADSPI_CR: ABORT=1
+		mww 0x52005010 0x00000001			;# QUADSPI_DLR: 2 data bytes
+		mww 0x52005014 0x01000161			;# QUADSPI_CCR: FMODE=0x0, DMODE=0x1, DCYC=0x0, ADSIZE=0x0, ADMODE=0x0, IMODE=0x1, INSTR=Write Enhanced Conf. Reg.
+		mwh 0x52005020 0x3F3F				;# QUADSPI_DR: 0x3F 0x3F to enable QPI and DPI mode
+		sleep 1
+
+		# Enter QPI mode
+		mmw 0x52005000 0x00000002 0			;# QUADSPI_CR: ABORT=1
+		mww 0x52005014 0x00000135			;# QUADSPI_CCR: FMODE=0x0, DMODE=0x0, DCYC=0x0, ADSIZE=0x0, ADMODE=0x0, IMODE=0x1, INSTR=Enter QPI
+		sleep 1
+
+		# memory-mapped fast read mode with 4-byte addresses and 10 dummy cycles (for read only)
+		mmw 0x52005000 0x00000002 0			;# QUADSPI_CR: ABORT=1
+		mww 0x52005014 0x0F283FEC			;# QUADSPI_CCR: FMODE=0x3, DMODE=0x3, DCYC=0xA, ADSIZE=0x3, ADMODE=0x3, IMODE=0x3, INSTR=Fast READ
+	} else {
+		# memory-mapped read mode with 4-byte addresses
+		mmw 0x52005000 0x00000002 0			;# QUADSPI_CR: ABORT=1
+		mww 0x52005014 0x0D003513			;# QUADSPI_CCR: FMODE=0x3, DMODE=0x1, DCYC=0x0, ADSIZE=0x3, ADMODE=0x1, IMODE=0x1, INSTR=READ
+	}
+}
+
+$_CHIPNAME.cpu0 configure -event reset-init {
+	global QUADSPI
+
+	mmw 0x52002000 0x00000004 0x0000000B	;# FLASH_ACR: 4 WS for 192 MHZ HCLK
+
+	mmw 0x58024400 0x00000001 0x00000018	;# RCC_CR: HSIDIV=1, HSI on
+	mmw 0x58024410 0x10000000 0xEE000007	;# RCC_CFGR: MCO2=system, MCO2PRE=8, HSI as system clock
+	mww 0x58024418 0x00000040				;# RCC_D1CFGR: D1CPRE=1, D1PPRE=2, HPRE=1
+	mww 0x5802441C 0x00000440				;# RCC_D2CFGR: D2PPRE2=2, D2PPRE1=2
+	mww 0x58024420 0x00000040				;# RCC_D3CFGR: D3PPRE=2
+	mww 0x58024428 0x00000040				;# RCC_PPLCKSELR: DIVM3=0, DIVM2=0, DIVM1=4, PLLSRC=HSI
+	mmw 0x5802442C 0x0001000C 0x00000002	;# RCC_PLLCFGR: PLL1RGE=8MHz to 16MHz, PLL1VCOSEL=wide
+	mww 0x58024430 0x01070217				;# RCC_PLL1DIVR: 192 MHz: DIVR1=2, DIVQ=8, DIVP1=2, DIVN1=24
+	mmw 0x58024400 0x01000000 0				;# RCC_CR: PLL1ON=1
+	sleep 1
+	mmw 0x58024410 0x00000003 0				;# RCC_CFGR: PLL1 as system clock
+	sleep 1
+
+	adapter speed 24000
+
+	if { $QUADSPI } {
+		qspi_init 1
+	}
+}
+
diff --git a/tcl/board/stm32h750b-disco.cfg b/tcl/board/stm32h750b-disco.cfg
new file mode 100644
index 0000000..e606203
--- /dev/null
+++ b/tcl/board/stm32h750b-disco.cfg
@@ -0,0 +1,45 @@
+# This is a stm32h750b-dk with a single STM32H750XBH6 chip.
+# www.st.com/en/product/stm32h750b-dk.html
+#
+
+# This is for using the onboard STLINK
+source [find interface/stlink.cfg]
+
+transport select hla_swd
+
+set CHIPNAME stm32h750xbh6
+
+# enable stmqspi
+if {![info exists QUADSPI]} {
+	set QUADSPI 1
+}
+
+source [find target/stm32h7x.cfg]
+
+source [find board/stm32h7x_dual_qspi.cfg]
+
+$_CHIPNAME.cpu0 configure -event reset-init {
+	global QUADSPI
+
+	mmw 0x52002000 0x00000004 0x0000000B	;# FLASH_ACR: 4 WS for 192 MHZ HCLK
+
+	mmw 0x58024400 0x00000001 0x00000018	;# RCC_CR: HSIDIV=1, HSI on
+	mmw 0x58024410 0x10000000 0xEE000007	;# RCC_CFGR: MCO2=system, MCO2PRE=8, HSI as system clock
+	mww 0x58024418 0x00000040				;# RCC_D1CFGR: D1CPRE=1, D1PPRE=2, HPRE=1
+	mww 0x5802441C 0x00000440				;# RCC_D2CFGR: D2PPRE2=2, D2PPRE1=2
+	mww 0x58024420 0x00000040				;# RCC_D3CFGR: D3PPRE=2
+	mww 0x58024428 0x00000040				;# RCC_PPLCKSELR: DIVM3=0, DIVM2=0, DIVM1=4, PLLSRC=HSI
+	mmw 0x5802442C 0x0001000C 0x00000002	;# RCC_PLLCFGR: PLL1RGE=8MHz to 16MHz, PLL1VCOSEL=wide
+	mww 0x58024430 0x01070217				;# RCC_PLL1DIVR: 192 MHz: DIVR1=2, DIVQ=8, DIVP1=2, DIVN1=24
+	mmw 0x58024400 0x01000000 0				;# RCC_CR: PLL1ON=1
+	sleep 1
+	mmw 0x58024410 0x00000003 0				;# RCC_CFGR: PLL1 as system clock
+	sleep 1
+
+	adapter speed 24000
+
+	if { $QUADSPI } {
+		qspi_init 1
+	}
+}
+
diff --git a/tcl/board/stm32h7b3i-disco.cfg b/tcl/board/stm32h7b3i-disco.cfg
new file mode 100644
index 0000000..e5512ea
--- /dev/null
+++ b/tcl/board/stm32h7b3i-disco.cfg
@@ -0,0 +1,128 @@
+# This is a stm32h7b3i-dk with a single STM32H7B3LIH6Q chip.
+# https://www.st.com/en/evaluation-tools/stm32h7b3i-dk.html
+#
+
+# This is for using the onboard STLINK
+source [find interface/stlink.cfg]
+
+transport select hla_swd
+
+set CHIPNAME stm32h7b3lih6q
+
+# enable stmqspi
+if {![info exists OCTOSPI1]} {
+	set OCTOSPI1 1
+	set OCTOSPI2 0
+}
+
+source [find target/stm32h7x_dual_bank.cfg]
+
+# OCTOSPI initialization
+# octo: 8-line mode
+proc octospi_init { octo } {
+	global a b
+	mmw 0x58024540 0x000007FF 0				;# RCC_AHB4ENR |= GPIOAEN-GPIOKEN (enable clocks)
+	mmw 0x58024534 0x00284000 0				;# RCC_AHB3ENR |= IOMNGREN, OSPI2EN, OSPI1EN (enable clocks)
+	sleep 1									;# Wait for clock startup
+
+	mww 0x5200B404 0x03010111				;# OCTOSPIM_P1CR: assign Port 1 to OCTOSPI1
+	mww 0x5200B408 0x00000000				;# OCTOSPIM_P2CR: disable Port 2
+
+	# PG06: OCSPI1_NCS, PB02: OCSPI1_CLK, PC05: OCSPI1_DQS, PD07: OCSPI1_IO7, PG09: OCSPI1_IO6, PH03: OCSPI1_IO5,
+	# PC01: OCSPI1_IO4, PF06: OCSPI1_IO3, PF07: OCSPI1_IO2, PF09: OCSPI1_IO1, PD11: OCSPI1_IO0
+
+	# PB02:AF09:V, PC05:AF10:V, PC01:AF10:V, PD11:AF09:V, PD07:AF10:V, PF09:AF10:V
+	# PF07:AF10:V, PF06:AF10:V, PG09:AF09:V, PG06:AF10:V, PH03:AF09:V
+	# Port B: PB02:AF09:V
+	mmw 0x58020400 0x00000020 0x00000010	;# MODER
+	mmw 0x58020408 0x00000030 0x00000000	;# OSPEEDR
+	mmw 0x5802040C 0x00000000 0x00000030	;# PUPDR
+	mmw 0x58020420 0x00000900 0x00000600	;# AFRL
+	# Port C: PC05:AF10:V, PC01:AF10:V
+	mmw 0x58020800 0x00000808 0x00000404	;# MODER
+	mmw 0x58020808 0x00000C0C 0x00000000	;# OSPEEDR
+	mmw 0x5802080C 0x00000000 0x00000C0C	;# PUPDR
+	mmw 0x58020820 0x00A000A0 0x00500050	;# AFRL
+	# Port D: PD11:AF09:V, PD07:AF10:V
+	mmw 0x58020C00 0x00808000 0x00404000	;# MODER
+	mmw 0x58020C08 0x00C0C000 0x00000000	;# OSPEEDR
+	mmw 0x58020C0C 0x00000000 0x00C0C000	;# PUPDR
+	mmw 0x58020C20 0xA0000000 0x50000000	;# AFRL
+	mmw 0x58020C24 0x00009000 0x00006000	;# AFRH
+	# Port F: PF09:AF10:V, PF07:AF10:V, PF06:AF10:V
+	mmw 0x58021400 0x0008A000 0x00045000	;# MODER
+	mmw 0x58021408 0x000CF000 0x00000000	;# OSPEEDR
+	mmw 0x5802140C 0x00000000 0x000CF000	;# PUPDR
+	mmw 0x58021420 0xAA000000 0x55000000	;# AFRL
+	mmw 0x58021424 0x000000A0 0x00000050	;# AFRH
+	# Port G: PG09:AF09:V, PG06:AF10:V
+	mmw 0x58021800 0x00082000 0x00041000	;# MODER
+	mmw 0x58021808 0x000C3000 0x00000000	;# OSPEEDR
+	mmw 0x5802180C 0x00000000 0x000C3000	;# PUPDR
+	mmw 0x58021820 0x0A000000 0x05000000	;# AFRL
+	mmw 0x58021824 0x00000090 0x00000060	;# AFRH
+	# Port H: PH03:AF09:V
+	mmw 0x58021C00 0x00000080 0x00000040	;# MODER
+	mmw 0x58021C08 0x000000C0 0x00000000	;# OSPEEDR
+	mmw 0x58021C0C 0x00000000 0x000000C0	;# PUPDR
+	mmw 0x58021C20 0x00009000 0x00006000	;# AFRL
+
+	# OCTOSPI1: memory-mapped 1-line read mode with 4-byte addresses
+	mww 0x52005130 0x00001000				;# OCTOSPI_LPTR: deactivate CS after 4096 clocks when FIFO is full
+	mww 0x52005000 0x3040000B				;# OCTOSPI_CR: FMODE=0x1, APMS=1, FTHRES=0, FSEL=0, DQM=0, TCEN=0
+	mww 0x52005008 0x01190100				;# OCTOSPI_DCR1: MTYP=0x1, FSIZE=0x19, CSHT=0x01, CKMODE=0, DLYBYP=0
+	mww 0x5200500C 0x00000005				;# OCTOSPI_DCR2: PRESCALER=5
+
+	mww 0x52005108 0x00000000				;# OCTOSPI_TCR: SSHIFT=0, DHQC=0, DCYC=0x0
+	mww 0x52005100 0x01003101				;# OCTOSPI_CCR: DMODE=0x1, ABMODE=0x0, ADSIZE=0x3, ADMODE=0x1, ISIZE=0x0, IMODE=0x1
+	mww 0x52005110 0x00000013				;# OCTOSPI_IR: INSTR=READ4B
+
+	flash probe $a							;# load configuration from CR, TCR, CCR, IR register values
+
+	if { $octo == 1 } {
+		stmqspi cmd $a 1 0x71 0x00 0x00 0x00 0x00			;# Read Conf. Reg. 2, addr 0x00000000: DOPI, SOPI bits
+		stmqspi cmd $a 0 0x06								;# Write Enable
+		stmqspi cmd $a 1 0x05								;# Read Status Register
+		stmqspi cmd $a 0 0x72 0x00 0x00 0x00 0x00 0x02		;# Write Conf. Reg. 2, addr 0x00000000: DTR OPI enable
+
+		# OCTOSPI1: memory-mapped 8-line read mode with 4-byte addresses
+		mww 0x52005000 0x3040000B				;# OCTOSPI_CR: FMODE=0x3, APMS=1, FTHRES=0, FSEL=0, DQM=0, TCEN=1, EN=1
+		mww 0x52005108 0x10000006				;# OCTOSPI_TCR: SSHIFT=0, DHQC=1, DCYC=0x6
+		mww 0x52005100 0x2C003C1C				;# OCTOSPI_CCR: DTR, DMODE=0x4, ABMODE=0x0, ADSIZE=0x3, ADMODE=0x4, ISIZE=0x1, IMODE=0x4
+		mww 0x52005110 0x0000EE11				;# OCTOSPI_IR: INSTR=OCTA DTR Read
+
+		flash probe $a							;# reload configuration from CR, TCR, CCR, IR register values
+
+		stmqspi cmd $a 0 0x06								;# Write Enable
+		stmqspi cmd $a 1 0x05 0x00 0x00 0x00 0x00			;# Read Status Register (note dummy address in 8-line mode)
+		stmqspi cmd $a 0 0x04								;# Write Disable
+		stmqspi cmd $a 1 0x05 0x00 0x00 0x00 0x00			;# Read Status Register (note dummy address in 8-line mode)
+		stmqspi cmd $a 1 0x71 0x00 0x00 0x00 0x00			;# Read Conf. Reg. 2, addr 0x00000000: DOPI, SOPI bits
+	}
+}
+
+$_CHIPNAME.cpu0 configure -event reset-init {
+	global OCTOSPI1
+	global OCTOSPI2
+
+	mmw 0x52002000 0x00000004 0x0000000B	;# FLASH_ACR: 4 WS for 192 MHZ HCLK
+
+	mmw 0x58024400 0x00000001 0x00000018	;# RCC_CR: HSIDIV=1, HSI on
+	mmw 0x58024410 0x10000000 0xEE000007	;# RCC_CFGR: MCO2=system, MCO2PRE=8, HSI as system clock
+	mww 0x58024418 0x00000040				;# RCC_D1CFGR: D1CPRE=1, D1PPRE=2, HPRE=1
+	mww 0x5802441C 0x00000440				;# RCC_D2CFGR: D2PPRE2=2, D2PPRE1=2
+	mww 0x58024420 0x00000040				;# RCC_D3CFGR: D3PPRE=2
+	mww 0x58024428 0x00000040				;# RCC_PPLCKSELR: DIVM3=0, DIVM2=0, DIVM1=4, PLLSRC=HSI
+	mmw 0x5802442C 0x0001000C 0x00000002	;# RCC_PLLCFGR: PLL1RGE=8MHz to 16MHz, PLL1VCOSEL=wide
+	mww 0x58024430 0x01070217				;# RCC_PLL1DIVR: 192 MHz: DIVR1=2, DIVQ=8, DIVP1=2, DIVN1=24
+	mmw 0x58024400 0x01000000 0				;# RCC_CR: PLL1ON=1
+	sleep 1
+	mmw 0x58024410 0x00000003 0				;# RCC_CFGR: PLL1 as system clock
+	sleep 1
+
+	adapter speed 24000
+
+	if { $OCTOSPI1 } {
+		octospi_init 1
+	}
+}
diff --git a/tcl/board/stm32h7x_dual_qspi.cfg b/tcl/board/stm32h7x_dual_qspi.cfg
new file mode 100644
index 0000000..bdff9c1
--- /dev/null
+++ b/tcl/board/stm32h7x_dual_qspi.cfg
@@ -0,0 +1,90 @@
+# stm32h754i-disco and stm32h750b-dk dual quad qspi.
+
+# QUADSPI initialization
+# qpi: 4-line mode
+proc qspi_init { qpi } {
+	global a
+	mmw 0x580244E0 0x000007FF 0				;# RCC_AHB4ENR |= GPIOAEN-GPIOKEN (enable clocks)
+	mmw 0x580244D4 0x00004000 0				;# RCC_AHB3ENR |= QSPIEN (enable clock)
+	sleep 1									;# Wait for clock startup
+
+	# PG06: BK1_NCS, PF10: CLK, PF06: BK1_IO3, PF07: BK1_IO2, PF09: BK1_IO1, PD11: BK1_IO0,
+	# PG14: BK2_IO3, PG09: BK2_IO2, PH03: BK2_IO1, PH02: BK2_IO0
+
+	# PD11:AF09:V, PF10:AF09:V, PF09:AF10:V, PF07:AF09:V, PF06:AF09:V, PG14:AF09:H
+	# PG09:AF09:V, PG06:AF10:H, PH03:AF09:V, PH02:AF09:V
+
+	# Port D: PD11:AF09:V
+	mmw 0x58020C00 0x00800000 0x00400000	;# MODER
+	mmw 0x58020C08 0x00C00000 0x00000000	;# OSPEEDR
+	mmw 0x58020C24 0x00009000 0x00006000	;# AFRH
+	# Port F: PF10:AF09:V, PF09:AF10:V, PF07:AF09:V, PF06:AF09:V
+	mmw 0x58021400 0x0028A000 0x00145000	;# MODER
+	mmw 0x58021408 0x003CF000 0x00000000	;# OSPEEDR
+	mmw 0x58021420 0x99000000 0x66000000	;# AFRL
+	mmw 0x58021424 0x000009A0 0x00000650	;# AFRH
+	# Port G: PG14:AF09:H, PG09:AF09:V, PG06:AF10:H
+	mmw 0x58021800 0x20082000 0x10041000	;# MODER
+	mmw 0x58021808 0x200C2000 0x10001000	;# OSPEEDR
+	mmw 0x58021820 0x0A000000 0x05000000	;# AFRL
+	mmw 0x58021824 0x09000090 0x06000060	;# AFRH
+	# Port H: PH03:AF09:V, PH02:AF09:V
+	mmw 0x58021C00 0x000000A0 0x00000050	;# MODER
+	mmw 0x58021C08 0x000000F0 0x00000000	;# OSPEEDR
+	mmw 0x58021C20 0x00009900 0x00006600	;# AFRL
+
+	# correct FSIZE is 0x1A, however, this causes trouble when
+	# reading the last bytes at end of bank in *memory mapped* mode
+
+	# for dual flash mode 2 * mt25ql512
+	mww 0x52005000 0x05500058				;# QUADSPI_CR: PRESCALER=5, APMS=1, FTHRES=0, FSEL=0, DFM=1, SSHIFT=1, TCEN=1
+	mww 0x52005004 0x001A0200				;# QUADSPI_DCR: FSIZE=0x1A, CSHT=0x02, CKMODE=0
+
+	mww 0x52005030 0x00001000				;# QUADSPI_LPTR: deactivate CS after 4096 clocks when FIFO is full
+	mww 0x52005014 0x0D002503				;# QUADSPI_CCR: FMODE=0x3, DMODE=0x1, DCYC=0x0, ADSIZE=0x3, ADMODE=0x1, IMODE=0x1
+	mmw 0x52005000 0x00000001 0				;# QUADSPI_CR: EN=1
+
+	# Exit QPI mode
+	mmw 0x52005000 0x00000002 0				;# QUADSPI_CR: ABORT=1
+	mww 0x52005014 0x000003F5				;# QUADSPI_CCR: FMODE=0x0, DMODE=0x0, DCYC=0x0, ADSIZE=0x0, ADMODE=0x0, IMODE=0x3, INSTR=Exit QPI
+	sleep 1
+
+	if { $qpi == 1 } {
+		# Write Enable
+		mmw 0x52005000 0x00000002 0			;# QUADSPI_CR: ABORT=1
+		mww 0x52005014 0x00000106			;# QUADSPI_CCR: FMODE=0x0, DMODE=0x0, DCYC=0x0, ADSIZE=0x0, ADMODE=0x0, IMODE=0x1, INSTR=Write Enable
+		sleep 1
+
+		# Configure dummy clocks via volatile configuration register
+		mmw 0x52005000 0x00000002 0			;# QUADSPI_CR: ABORT=1
+		mww 0x52005010 0x00000001			;# QUADSPI_DLR: 2 data bytes
+		mww 0x52005014 0x01000181			;# QUADSPI_CCR: FMODE=0x0, DMODE=0x1, DCYC=0x0, ADSIZE=0x0, ADMODE=0x0, IMODE=0x1, INSTR=Write Volatile Conf. Reg.
+		mwh 0x52005020 0xABAB				;# QUADSPI_DR: 0xAB 0xAB for 10 dummy clocks
+		sleep 1
+
+		# Write Enable
+		mmw 0x52005000 0x00000002 0			;# QUADSPI_CR: ABORT=1
+		mww 0x52005014 0x00000106			;# QUADSPI_CCR: FMODE=0x0, DMODE=0x0, DCYC=0x0, ADSIZE=0x0, ADMODE=0x0, IMODE=0x1, INSTR=Write Enable
+		sleep 1
+
+		# Enable QPI mode via enhanced volatile configuration register
+		mmw 0x52005000 0x00000002 0			;# QUADSPI_CR: ABORT=1
+		mww 0x52005010 0x00000001			;# QUADSPI_DLR: 2 data bytes
+		mww 0x52005014 0x01000161			;# QUADSPI_CCR: FMODE=0x0, DMODE=0x1, DCYC=0x0, ADSIZE=0x0, ADMODE=0x0, IMODE=0x1, INSTR=Write Enhanced Conf. Reg.
+		mwh 0x52005020 0x3F3F				;# QUADSPI_DR: 0x3F 0x3F to enable QPI and DPI mode
+		sleep 1
+
+		# Enter QPI mode
+		mmw 0x52005000 0x00000002 0			;# QUADSPI_CR: ABORT=1
+		mww 0x52005014 0x00000135			;# QUADSPI_CCR: FMODE=0x0, DMODE=0x0, DCYC=0x0, ADSIZE=0x0, ADMODE=0x0, IMODE=0x1, INSTR=Enter QPI
+		sleep 1
+
+		# memory-mapped fast read mode with 4-byte addresses and 10 dummy cycles (for read only)
+		mmw 0x52005000 0x00000002 0			;# QUADSPI_CR: ABORT=1
+		mww 0x52005014 0x0F283FEC			;# QUADSPI_CCR: FMODE=0x3, DMODE=0x3, DCYC=0xA, ADSIZE=0x3, ADMODE=0x3, IMODE=0x3, INSTR=Fast READ
+	} else {
+		# memory-mapped read mode with 4-byte addresses
+		mmw 0x52005000 0x00000002 0			;# QUADSPI_CR: ABORT=1
+		mww 0x52005014 0x0D003513			;# QUADSPI_CCR: FMODE=0x3, DMODE=0x1, DCYC=0x0, ADSIZE=0x3, ADMODE=0x1, IMODE=0x1, INSTR=READ
+	}
+}
diff --git a/tcl/board/stm32l476g-disco.cfg b/tcl/board/stm32l476g-disco.cfg
new file mode 100644
index 0000000..dab2fe1
--- /dev/null
+++ b/tcl/board/stm32l476g-disco.cfg
@@ -0,0 +1,56 @@
+# This is an STM32L476G discovery board with a single STM32L476VGT6 chip.
+# http://www.st.com/en/evaluation-tools/32l476gdiscovery.html
+
+# This is for using the onboard STLINK
+source [find interface/stlink.cfg]
+
+transport select hla_swd
+
+# increase working area to 96KB
+set WORKAREASIZE 0x18000
+
+# enable stmqspi
+set QUADSPI 1
+
+source [find target/stm32l4x.cfg]
+
+# QUADSPI initialization
+proc qspi_init { } {
+	global a
+	mmw 0x4002104C 0x000001FF 0				;# RCC_AHB2ENR |= GPIOAEN-GPIOIEN (enable clocks)
+	mmw 0x40021050 0x00000100 0				;# RCC_AHB3ENR |= QSPIEN (enable clock)
+	sleep 1									;# Wait for clock startup
+
+	# PE11: NCS, PE10: CLK, PE15: BK1_IO3, PE14: BK1_IO2, PE13: BK1_IO1, PE12: BK1_IO0
+
+	# PE15:AF10:V, PE14:AF10:V, PE13:AF10:V, PE12:AF10:V, PE11:AF10:V, PE10:AF10:V
+
+	# Port E: PE15:AF10:V, PE14:AF10:V, PE13:AF10:V, PE12:AF10:V, PE11:AF10:V, PE10:AF10:V
+	mmw 0x48001000 0xAAA00000 0x55500000    ;# MODER
+	mmw 0x48001008 0xFFF00000 0x00000000    ;# OSPEEDR
+	mmw 0x48001024 0xAAAAAA00 0x55555500    ;# AFRH
+
+	mww 0xA0001030 0x00001000				;# QUADSPI_LPTR: deactivate CS after 4096 clocks when FIFO is full
+	mww 0xA0001000 0x01500008				;# QUADSPI_CR: PRESCALER=1, APMS=1, FTHRES=0, FSEL=0, DFM=0, SSHIFT=0, TCEN=1
+	mww 0xA0001004 0x00170100				;# QUADSPI_DCR: FSIZE=0x17, CSHT=0x01, CKMODE=0
+	mmw 0xA0001000 0x00000001 0				;# QUADSPI_CR: EN=1
+
+	# memory-mapped read mode with 3-byte addresses
+	mww 0xA0001014 0x0D002503				;# QUADSPI_CCR: FMODE=0x3, DMODE=0x1, DCYC=0x0, ADSIZE=0x2, ADMODE=0x1, IMODE=0x1, INSTR=READ
+}
+
+$_TARGETNAME configure -event reset-init {
+	mmw 0x40022000 0x00000004 0x00000003	;# 4 WS for 72 MHz HCLK
+	sleep 1
+	mmw 0x40021000 0x00000100 0x00000000	;# HSI on
+	mww 0x4002100C 0x01002432				;# 72 MHz: PLLREN=1, PLLM=4, PLLN=36, PLLR=2, HSI
+	mww 0x40021008 0x00008001				;# always HSI, APB1: /1, APB2: /1
+	mmw 0x40021000 0x01000000 0x00000000	;# PLL on
+	sleep 1
+	mmw 0x40021008 0x00000003 0x00000000	;# switch to PLL
+	sleep 1
+
+	adapter speed 4000
+
+	qspi_init
+}
diff --git a/tcl/board/stm32l496g-disco.cfg b/tcl/board/stm32l496g-disco.cfg
new file mode 100644
index 0000000..a93b07c
--- /dev/null
+++ b/tcl/board/stm32l496g-disco.cfg
@@ -0,0 +1,66 @@
+# This is an STM32L496G discovery board with a single STM32L496AGI6 chip.
+# http://www.st.com/en/evaluation-tools/32l496gdiscovery.html
+
+# This is for using the onboard STLINK
+source [find interface/stlink.cfg]
+
+transport select hla_swd
+
+# increase working area to 96KB
+set WORKAREASIZE 0x18000
+
+# enable stmqspi
+set QUADSPI 1
+
+source [find target/stm32l4x.cfg]
+
+# QUADSPI initialization
+proc qspi_init { } {
+	global a
+	mmw 0x4002104C 0x000001FF 0				;# RCC_AHB2ENR |= GPIOAEN-GPIOIEN (enable clocks)
+	mmw 0x40021050 0x00000100 0				;# RCC_AHB3ENR |= QSPIEN (enable clock)
+	sleep 1									;# Wait for clock startup
+
+	# PB11: BK1_NCS, PA03: CLK, PA06: BK1_IO3, PA07: BK1_IO2, PB00: BK1_IO1, PB01: BK1_IO0
+
+	# PA07:AF10:V, PA06:AF10:V, PA03:AF10:V, PB11:AF10:V, PB01:AF10:V, PB00:AF10:V
+
+	# Port A: PA07:AF10:V, PA06:AF10:V, PA03:AF10:V
+	mmw 0x48000000 0x0000A080 0x00005040    ;# MODER
+	mmw 0x48000008 0x0000F0C0 0x00000000    ;# OSPEEDR
+	mmw 0x48000020 0xAA00A000 0x55005000    ;# AFRL
+
+	# Port B: PB11:AF10:V, PB01:AF10:V, PB00:AF10:V
+	mmw 0x48000400 0x0080000A 0x00400005    ;# MODER
+	mmw 0x48000408 0x00C0000F 0x00000000    ;# OSPEEDR
+	mmw 0x48000420 0x000000AA 0x00000055    ;# AFRL
+	mmw 0x48000424 0x0000A000 0x00005000    ;# AFRH
+
+	mww 0xA0001030 0x00001000				;# QUADSPI_LPTR: deactivate CS after 4096 clocks when FIFO is full
+	mww 0xA0001000 0x01500008				;# QUADSPI_CR: PRESCALER=1, APMS=1, FTHRES=0, FSEL=0, DFM=0, SSHIFT=0, TCEN=1
+	mww 0xA0001004 0x00160100				;# QUADSPI_DCR: FSIZE=0x16, CSHT=0x01, CKMODE=0
+	mmw 0xA0001000 0x00000001 0				;# QUADSPI_CR: EN=1
+
+	# 1-line spi mode
+	mww 0xA0001014 0x000003F5				;# QUADSPI_CCR: FMODE=0x0, DMODE=0x0, DCYC=0x0, ADSIZE=0x0, ADMODE=0x0, IMODE=0x3, INSTR=RSTQIO
+	sleep 1
+
+	# memory-mapped read mode with 3-byte addresses
+	mww 0xA0001014 0x0D002503				;# QUADSPI_CCR: FMODE=0x3, DMODE=0x1, DCYC=0x0, ADSIZE=0x2, ADMODE=0x1, IMODE=0x1, INSTR=READ
+}
+
+$_TARGETNAME configure -event reset-init {
+	mmw 0x40022000 0x00000004 0x00000003	;# 4 WS for 72 MHz HCLK
+	sleep 1
+	mmw 0x40021000 0x00000100 0x00000000	;# HSI on
+	mww 0x4002100C 0x01002432				;# 72 MHz: PLLREN=1, PLLM=4, PLLN=36, PLLR=2, HSI
+	mww 0x40021008 0x00008001				;# always HSI, APB1: /1, APB2: /1
+	mmw 0x40021000 0x01000000 0x00000000	;# PLL on
+	sleep 1
+	mmw 0x40021008 0x00000003 0x00000000	;# switch to PLL
+	sleep 1
+
+	adapter speed 4000
+
+	qspi_init
+}
diff --git a/tcl/board/stm32l4p5g-disco.cfg b/tcl/board/stm32l4p5g-disco.cfg
new file mode 100644
index 0000000..d7420ed
--- /dev/null
+++ b/tcl/board/stm32l4p5g-disco.cfg
@@ -0,0 +1,130 @@
+# This is a STM32L4P5G discovery board with a single STM32L4R9AGI6 chip.
+# http://www.st.com/en/evaluation-tools/stm32l4p5g-dk.html
+
+# This is for using the onboard STLINK
+source [find interface/stlink.cfg]
+
+transport select hla_swd
+
+# increase working area to 96KB
+set WORKAREASIZE 0x18000
+
+# enable stmqspi
+set OCTOSPI1 1
+set OCTOSPI2 0
+
+source [find target/stm32l4x.cfg]
+
+# OCTOSPI initialization
+# octo: 8-line mode
+proc octospi_init { octo } {
+	global a b
+	mmw 0x4002104C 0x001001FF 0				;# RCC_AHB2ENR |= OSPIMEN, GPIOAEN-GPIOIEN (enable clocks)
+	mmw 0x40021050 0x00000300 0				;# RCC_AHB3ENR |= OSPI2EN, OSPI1EN (enable clocks)
+	mmw 0x40021058 0x10000000 0				;# RCC_APB1ENR1 |= PWREN (enable clock)
+	sleep 1									;# Wait for clock startup
+
+	mmw 0x40007004 0x00000200 0				;# PWR_CR2 |= IOSV (required for use of GPOIG, cf. RM0432)
+
+	mww 0x50061C04 0x07050333				;# OCTOSPIM_P1CR: assign Port 1 to OCTOSPI2
+	mww 0x50061C08 0x03010111				;# OCTOSPIM_P2CR: assign Port 2 to OCTOSPI1
+
+	# PE11: P1_NCS, PE10: P1_CLK, PG06: P1_DQS, PD07: P1_IO7, PC03: P1_IO6, PD05: P1_IO5
+	# PD04: P1_IO4, PA06: P1_IO3, PA07: P1_IO2, PE13: P1_IO1, PE11: P1_IO0
+
+	# PA07:AF10:V, PA06:AF10:V, PC03:AF10:V, PD07:AF10:V, PD05:AF10:V, PD04:AF10:V
+	# PE13:AF10:V, PE12:AF10:V, PE11:AF10:V, PE10:AF10:V, PG06:AF03:V
+
+	# Port A: PA07:AF10:V, PA06:AF10:V
+	mmw 0x48000000 0x0000A000 0x00005000	;# MODER
+	mmw 0x48000008 0x0000F000 0x00000000	;# OSPEEDR
+	mmw 0x4800000C 0x00000000 0x0000F000	;# PUPDR
+	mmw 0x48000020 0xAA000000 0x55000000	;# AFRL
+	# Port C: PC03:AF10:V
+	mmw 0x48000800 0x00000080 0x00000040	;# MODER
+	mmw 0x48000808 0x000000C0 0x00000000	;# OSPEEDR
+	mmw 0x4800080C 0x00000000 0x000000C0	;# PUPDR
+	mmw 0x48000820 0x0000A000 0x00005000	;# AFRL
+	# Port D: PD07:AF10:V, PD05:AF10:V, PD04:AF10:V
+	mmw 0x48000C00 0x00008A00 0x00004500	;# MODER
+	mmw 0x48000C08 0x0000CF00 0x00000000	;# OSPEEDR
+	mmw 0x48000C0C 0x00000000 0x0000CF00	;# PUPDR
+	mmw 0x48000C20 0xA0AA0000 0x50550000	;# AFRL
+	# Port E: PE13:AF10:V, PE12:AF10:V, PE11:AF10:V, PE10:AF10:V
+	mmw 0x48001000 0x0AA00000 0x05500000	;# MODER
+	mmw 0x48001008 0x0FF00000 0x00000000	;# OSPEEDR
+	mmw 0x4800100C 0x00000000 0x0FF00000	;# PUPDR
+	mmw 0x48001024 0x00AAAA00 0x00555500	;# AFRH
+	# Port G: PG06:AF03:V
+	mmw 0x48001800 0x00002000 0x00001000	;# MODER
+	mmw 0x48001808 0x00003000 0x00000000	;# OSPEEDR
+	mmw 0x4800180C 0x00000000 0x00003000	;# PUPDR
+	mmw 0x48001820 0x03000000 0x0C000000	;# AFRL
+
+	# PG12: P2_NCS, PF04: P2_CLK, PF12: P2_DQS, PG10: P2_IO7, PG09: P2_IO6, PG01: P2_IO5
+	# PG00: P2_IO4, PF03: P2_IO3, PF02: P2_IO2, PF01: P2_IO1, PF00: P2_IO0
+
+	# PF12:AF05:V, PF04:AF05:V, PF03:AF05:V, PF02:AF05:V, PF01:AF05:V, PF00:AF05:V
+	# PG12:AF05:V, PG10:AF05:V, PG09:AF05:V, PG01:AF05:V, PG00:AF05:V
+
+	# Port F: PF12:AF05:V, PF04:AF05:V, PF03:AF05:V, PF02:AF05:V, PF01:AF05:V, PF00:AF05:V
+	mmw 0x48001400 0x020002AA 0x01000155	;# MODER
+	mmw 0x48001408 0x030003FF 0x00000000	;# OSPEEDR
+	mmw 0x4800140C 0x00000000 0x030003FF	;# PUPDR
+	mmw 0x48001420 0x00055555 0x000AAAAA	;# AFRL
+	mmw 0x48001424 0x00050000 0x000A0000	;# AFRH
+	# Port G: PG12:AF05:V, PG10:AF05:V, PG09:AF05:V, PG01:AF05:V, PG00:AF05:V
+	mmw 0x48001800 0x0228000A 0x01140005	;# MODER
+	mmw 0x48001808 0x033C000F 0x00000000	;# OSPEEDR
+	mmw 0x4800180C 0x00000000 0x033C000F	;# PUPDR
+	mmw 0x48001820 0x00000055 0x000000AA	;# AFRL
+	mmw 0x48001824 0x00050550 0x000A0AA0	;# AFRH
+
+	# OCTOSPI1: memory-mapped 1-line read mode with 4-byte addresses
+	mww 0xA0001130 0x00001000				;# OCTOSPI_LPTR: deactivate CS after 4096 clocks when FIFO is full
+	mww 0xA0001000 0x3040000B				;# OCTOSPI_CR: FMODE=0x1, APMS=1, FTHRES=0, FSEL=0, DQM=0, TCEN=0
+	mww 0xA0001008 0x01190100				;# OCTOSPI_DCR1: MTYP=0x1, FSIZE=0x19, CSHT=0x01, CKMODE=0, DLYBYP=0
+	mww 0xA000100C 0x00000001				;# OCTOSPI_DCR2: PRESCALER=1
+
+	mww 0xA0001108 0x00000000				;# OCTOSPI_TCR: SSHIFT=0, DHQC=0, DCYC=0x0
+	mww 0xA0001100 0x01003101				;# OCTOSPI_CCR: DMODE=0x1, ABMODE=0x0, ADSIZE=0x3, ADMODE=0x1, ISIZE=0x0, IMODE=0x1
+	mww 0xA0001110 0x00000013				;# OCTOSPI_IR: INSTR=READ4B
+
+	if { $octo == 1 } {
+		stmqspi cmd $a 1 0x71 0x00 0x00 0x00 0x00			;# Read Conf. Reg. 2, addr 0x00000000: DOPI, SOPI bits
+		stmqspi cmd $a 0 0x06								;# Write Enable
+		stmqspi cmd $a 1 0x05								;# Read Status Register
+		stmqspi cmd $a 0 0x72 0x00 0x00 0x00 0x00 0x02		;# Write Conf. Reg. 2, addr 0x00000000: DTR OPI enable
+
+		# OCTOSPI1: memory-mapped 8-line read mode with 4-byte addresses
+		mww 0xA0001000 0x3040000B				;# OCTOSPI_CR: FMODE=0x3, APMS=1, FTHRES=0, FSEL=0, DQM=0, TCEN=1, EN=1
+		mww 0xA0001108 0x10000006				;# OCTOSPI_TCR: SSHIFT=0, DHQC=1, DCYC=0x6
+		mww 0xA0001100 0x2C003C1C				;# OCTOSPI_CCR: DTR, DMODE=0x4, ABMODE=0x0, ADSIZE=0x3, ADMODE=0x4, ISIZE=0x1, IMODE=0x4
+		mww 0xA0001110 0x0000EE11				;# OCTOSPI_IR: INSTR=OCTA DTR Read
+
+		flash probe $a							;# reload configuration from CR, TCR, CCR, IR register values
+
+		stmqspi cmd $a 0 0x06								;# Write Enable
+		stmqspi cmd $a 1 0x05 0x00 0x00 0x00 0x00			;# Read Status Register (note dummy address in 8-line mode)
+		stmqspi cmd $a 0 0x04								;# Write Disable
+		stmqspi cmd $a 1 0x05 0x00 0x00 0x00 0x00			;# Read Status Register (note dummy address in 8-line mode)
+		stmqspi cmd $a 1 0x71 0x00 0x00 0x00 0x00			;# Read Conf. Reg. 2, addr 0x00000000: DOPI, SOPI bits
+	}
+}
+
+$_TARGETNAME configure -event reset-init {
+	mmw 0x40022000 0x00000003 0x0000000C	;# 3 WS for 72 MHz HCLK
+	sleep 1
+	mmw 0x40021000 0x00000100 0x00000000	;# HSI on
+	mww 0x4002100C 0x01002432				;# RCC_PLLCFGR 72 MHz: PLLREN=1, PLLM=4, PLLN=36, PLLR=2, HSI
+	mww 0x40021008 0x00008001				;# always HSI, APB1: /1, APB2: /1
+	mmw 0x40021000 0x01000000 0x00000000	;# PLL on
+	sleep 1
+	mmw 0x40021008 0x00000003 0x00000000	;# switch to PLL
+	sleep 1
+
+	adapter speed 24000
+
+	octospi_init 1
+}
+
diff --git a/tcl/board/stm32l4r9i-disco.cfg b/tcl/board/stm32l4r9i-disco.cfg
new file mode 100644
index 0000000..70ed199
--- /dev/null
+++ b/tcl/board/stm32l4r9i-disco.cfg
@@ -0,0 +1,100 @@
+# This is a STM32L4R9I discovery board with a single STM32L4R9AII6 chip.
+# http://www.st.com/en/evaluation-tools/32l4r9idiscovery.html
+
+# This is for using the onboard STLINK
+source [find interface/stlink.cfg]
+
+transport select hla_swd
+
+# increase working area to 96KB
+set WORKAREASIZE 0x18000
+
+# enable stmqspi
+set OCTOSPI1 1
+set OCTOSPI2 0
+
+source [find target/stm32l4x.cfg]
+
+# OCTOSPI initialization
+# octo: 8-line mode
+proc octospi_init { octo } {
+	global a b
+	mmw 0x4002104C 0x001001FF 0				;# RCC_AHB2ENR |= OSPIMEN, GPIOAEN-GPIOIEN (enable clocks)
+	mmw 0x40021050 0x00000300 0				;# RCC_AHB3ENR |= OSPI2EN, OSPI1EN (enable clocks)
+	mmw 0x40021058 0x10000000 0				;# RCC_APB1ENR1 |= PWREN (enable clock)
+	sleep 1									;# Wait for clock startup
+
+	mmw 0x40007004 0x00000200 0				;# PWR_CR2 |= IOSV (required for use of GPOIG, cf. RM0432)
+
+	mww 0x50061C04 0x00000000				;# OCTOSPIM_P1CR: disable Port 1
+	mww 0x50061C08 0x03010111				;# OCTOSPIM_P2CR: assign Port 2 to OCTOSPI1
+
+	# PG12: P2_NCS, PI06: P2_CLK, PG15: P2_DQS, PG10: P2_IO7, PG09: P2_IO6, PH10: P2_IO5,
+	# PH09: P2_IO4, PH08: P2_IO3, PI09: P2_IO2, PI10: P2_IO1, PI11: P2_IO0
+
+	# PG15:AF05:V, PG12:AF05:V, PG10:AF05:V, PG09:AF05:V, PH10:AF05:V, PH09:AF05:V
+	# PH08:AF05:V, PI11:AF05:V, PI10:AF05:V, PI09:AF05:V, PI06:AF05:V
+
+	# Port G: PG15:AF05:V, PG12:AF05:V, PG10:AF05:V, PG09:AF05:V
+	mmw 0x48001800 0x82280000 0x41140000	;# MODER
+	mmw 0x48001808 0xC33C0000 0x00000000	;# OSPEEDR
+	mmw 0x48001824 0x50050550 0xA00A0AA0	;# AFRH
+
+	# Port H: PH10:AF05:V, PH09:AF05:V, PH08:AF05:V
+	mmw 0x48001C00 0x002A0000 0x00150000	;# MODER
+	mmw 0x48001C08 0x003F0000 0x00000000	;# OSPEEDR
+	mmw 0x48001C24 0x00000555 0x00000AAA	;# AFRH
+
+	# Port I: PI11:AF05:V, PI10:AF05:V, PI09:AF05:V, PI06:AF05:V
+	mmw 0x48002000 0x00A82000 0x00541000	;# MODER
+	mmw 0x48002008 0x00FC3000 0x00000000	;# OSPEEDR
+	mmw 0x48002020 0x05000000 0x0A000000	;# AFRL
+	mmw 0x48002024 0x00005550 0x0000AAA0	;# AFRH
+
+	# OCTOSPI1: memory-mapped 1-line read mode with 4-byte addresses
+	mww 0xA0001130 0x00001000				;# OCTOSPI_LPTR: deactivate CS after 4096 clocks when FIFO is full
+	mww 0xA0001000 0x3040000B				;# OCTOSPI_CR: FMODE=0x1, APMS=1, FTHRES=0, FSEL=0, DQM=0, TCEN=0
+	mww 0xA0001008 0x01190100				;# OCTOSPI_DCR1: MTYP=0x1, FSIZE=0x19, CSHT=0x01, CKMODE=0, DLYBYP=0
+	mww 0xA000100C 0x00000001				;# OCTOSPI_DCR2: PRESCALER=1
+
+	mww 0xA0001108 0x00000000				;# OCTOSPI_TCR: SSHIFT=0, DHQC=0, DCYC=0x0
+	mww 0xA0001100 0x01003101				;# OCTOSPI_CCR: DMODE=0x1, ABMODE=0x0, ADSIZE=0x3, ADMODE=0x1, ISIZE=0x0, IMODE=0x1
+	mww 0xA0001110 0x00000013				;# OCTOSPI_IR: INSTR=READ4B
+
+	if { $octo == 1 } {
+		stmqspi cmd $a 1 0x71 0x00 0x00 0x00 0x00			;# Read Conf. Reg. 2, addr 0x00000000: DOPI, SOPI bits
+		stmqspi cmd $a 0 0x06								;# Write Enable
+		stmqspi cmd $a 1 0x05								;# Read Status Register
+		stmqspi cmd $a 0 0x72 0x00 0x00 0x00 0x00 0x02		;# Write Conf. Reg. 2, addr 0x00000000: DTR OPI enable
+
+		# OCTOSPI1: memory-mapped 8-line read mode with 4-byte addresses
+		mww 0xA0001000 0x3040000B				;# OCTOSPI_CR: FMODE=0x3, APMS=1, FTHRES=0, FSEL=0, DQM=0, TCEN=1, EN=1
+		mww 0xA0001108 0x10000006				;# OCTOSPI_TCR: SSHIFT=0, DHQC=1, DCYC=0x6
+		mww 0xA0001100 0x2C003C1C				;# OCTOSPI_CCR: DTR, DMODE=0x4, ABMODE=0x0, ADSIZE=0x3, ADMODE=0x4, ISIZE=0x1, IMODE=0x4
+		mww 0xA0001110 0x0000EE11				;# OCTOSPI_IR: INSTR=OCTA DTR Read
+
+		flash probe $a							;# reload configuration from CR, TCR, CCR, IR register values
+
+		stmqspi cmd $a 0 0x06								;# Write Enable
+		stmqspi cmd $a 1 0x05 0x00 0x00 0x00 0x00			;# Read Status Register (note dummy address in 8-line mode)
+		stmqspi cmd $a 0 0x04								;# Write Disable
+		stmqspi cmd $a 1 0x05 0x00 0x00 0x00 0x00			;# Read Status Register (note dummy address in 8-line mode)
+		stmqspi cmd $a 1 0x71 0x00 0x00 0x00 0x00			;# Read Conf. Reg. 2, addr 0x00000000: DOPI, SOPI bits
+	}
+}
+
+$_TARGETNAME configure -event reset-init {
+	mmw 0x40022000 0x00000003 0x0000000C	;# 3 WS for 72 MHz HCLK
+	sleep 1
+	mmw 0x40021000 0x00000100 0x00000000	;# HSI on
+	mww 0x4002100C 0x01002432				;# RCC_PLLCFGR 72 MHz: PLLREN=1, PLLM=4, PLLN=36, PLLR=2, HSI
+	mww 0x40021008 0x00008001				;# always HSI, APB1: /1, APB2: /1
+	mmw 0x40021000 0x01000000 0x00000000	;# PLL on
+	sleep 1
+	mmw 0x40021008 0x00000003 0x00000000	;# switch to PLL
+	sleep 1
+
+	adapter speed 4000
+
+	octospi_init 1
+}
diff --git a/tcl/board/tocoding_poplar.cfg b/tcl/board/tocoding_poplar.cfg
index d0951ce..6d2e635 100644
--- a/tcl/board/tocoding_poplar.cfg
+++ b/tcl/board/tocoding_poplar.cfg
@@ -10,7 +10,7 @@ adapter speed 10000
 # SRST-only reset configuration
 reset_config srst_only srst_push_pull
 
-source [find tcl/target/hi3798.cfg]
+source [find target/hi3798.cfg]
 
 # make sure the default target is the boot core
 targets ${_TARGETNAME}0
diff --git a/tcl/board/tx25_stk5.cfg b/tcl/board/tx25_stk5.cfg
index 846bf58..9d77afd 100644
--- a/tcl/board/tx25_stk5.cfg
+++ b/tcl/board/tx25_stk5.cfg
@@ -4,7 +4,7 @@
 # -------------------------------------------------------------------------
 
 
-source [find tcl/target/imx25.cfg]
+source [find target/imx25.cfg]
 
 	#-------------------------------------------------------------------------
 	# Declare Nand
diff --git a/tcl/interface/ftdi/hie-jtag.cfg b/tcl/interface/ftdi/hie-jtag.cfg
new file mode 100644
index 0000000..39af87d
--- /dev/null
+++ b/tcl/interface/ftdi/hie-jtag.cfg
@@ -0,0 +1,20 @@
+# SPDX-License-Identifier: GPL-2.0-or-later
+#
+# Hofstädtler Industrie-Electronic (HIE) JTAG Debugger
+#
+# https://www.hofstaedtler.com/jtag
+#
+
+adapter driver ftdi
+ftdi_channel 0
+ftdi_vid_pid 0x0403 0x6014
+ftdi_device_desc "HIE JTAG Debugger"
+
+ftdi_layout_init 0x0c08 0x4f1b
+
+# define both Reset signals
+ftdi_layout_signal nTRST -data 0x0100 -noe 0x0400
+ftdi_layout_signal nSRST -data 0x0200 -noe 0x0800
+
+# Toggle USB LED
+ftdi_layout_signal LED -ndata 0x4000
diff --git a/tcl/interface/ftdi/steppenprobe.cfg b/tcl/interface/ftdi/steppenprobe.cfg
new file mode 100755
index 0000000..7b5b9a0
--- /dev/null
+++ b/tcl/interface/ftdi/steppenprobe.cfg
@@ -0,0 +1,41 @@
+# SPDX-License-Identifier: GPL-2.0-or-later
+
+#
+# Steppenprobe
+# https://github.com/diegoherranz/steppenprobe
+#
+
+adapter driver ftdi
+ftdi_vid_pid 0x0403 0x6010
+
+# Initial Layout
+ftdi_layout_init 0x0058 0x99fb
+# Signal        Data    Direction       Notes
+# TCK           0       1 (out)
+# TDI           0       1 (out)
+# TDO           0       0 (in)
+# TMS           1       1 (out)         JTAG IEEE std recommendation
+# LED           1       1 (out)         LED off
+# SWD_EN        0       1 (out)         OpenOCD sets this high for SWD
+# SWDIO_OE      1       1 (out)         Ext. buffer tristated
+# SRST          0       1 (out)         Translates to nSRST=Z
+
+# Unused        0       1 (out)
+# GPIO_A        0       0 (in)
+# GPIO_B        0       0 (in)
+# Unused        0       1 (out)
+# Unused        0       1 (out)
+# GPIO_C        0       0 (in)
+# GPIO_D        0       0 (in)
+# Unused        0       1 (out)
+
+# Signals definition
+ftdi_layout_signal LED -ndata 0x0010
+ftdi_layout_signal SWD_EN -data 0x0020
+ftdi_layout_signal SWDIO_OE -ndata 0x0040
+ftdi_layout_signal nSRST -oe 0x0080
+
+ftdi_layout_signal GPIO_A -data 0x0200 -oe 0x0200 -input 0x0200
+ftdi_layout_signal GPIO_B -data 0x0400 -oe 0x0400 -input 0x0400
+ftdi_layout_signal GPIO_C -data 0x2000 -oe 0x2000 -input 0x2000
+ftdi_layout_signal GPIO_D -data 0x4000 -oe 0x4000 -input 0x4000
diff --git a/tcl/interface/ti-icdi.cfg b/tcl/interface/ti-icdi.cfg
index 9b46b43..8561a31 100644
--- a/tcl/interface/ti-icdi.cfg
+++ b/tcl/interface/ti-icdi.cfg
@@ -11,3 +11,7 @@ adapter driver hla
 hla_layout ti-icdi
 hla_vid_pid 0x1cbe 0x00fd
 
+# Optionally specify the serial number of TI-ICDI devices, for when using
+# multiple devices. Serial numbers can be obtained using lsusb -v
+# Ex.
+#hla_serial "0F003065"
diff --git a/tcl/target/bluefield.cfg b/tcl/target/bluefield.cfg
index b31dfe8..62b1e31 100644
--- a/tcl/target/bluefield.cfg
+++ b/tcl/target/bluefield.cfg
@@ -46,7 +46,7 @@ set _cores 16
 
 # Create each core
 for { set _core $_core_start } { $_core < $_core_start + $_cores } { incr _core 1 } {
-    cti create cti$_core -dap $_CHIPNAME.dap -ctibase [set $_TARGETNAME.cti($_core)] -ap-num 0
+    cti create cti$_core -dap $_CHIPNAME.dap -baseaddr [set $_TARGETNAME.cti($_core)] -ap-num 0
 
     set _command "target create ${_TARGETNAME}$_core aarch64 \
                          -dap $_CHIPNAME.dap -coreid $_core -cti cti$_core"
diff --git a/tcl/target/efm32_stlink.cfg b/tcl/target/efm32_stlink.cfg
deleted file mode 100644
index 230155e..0000000
--- a/tcl/target/efm32_stlink.cfg
+++ /dev/null
@@ -1,2 +0,0 @@
-echo "WARNING: target/efm32_stlink.cfg is deprecated, please switch to target/efm32.cfg"
-source [find target/efm32.cfg]
diff --git a/tcl/target/hi3798.cfg b/tcl/target/hi3798.cfg
index aa811d4..7b19218 100644
--- a/tcl/target/hi3798.cfg
+++ b/tcl/target/hi3798.cfg
@@ -30,7 +30,7 @@ set $_TARGETNAME.cti(3) 0x80320000
 set _cores 4
 for { set _core 0 } { $_core < $_cores } { incr _core 1 } {
 
-    cti create cti$_core -dap $_CHIPNAME.dap -ctibase [set $_TARGETNAME.cti($_core)] -ap-num 0
+    cti create cti$_core -dap $_CHIPNAME.dap -baseaddr [set $_TARGETNAME.cti($_core)] -ap-num 0
 
     set _command "target create ${_TARGETNAME}$_core aarch64 \
                          -dap $_CHIPNAME.dap -coreid $_core -cti cti$_core"
diff --git a/tcl/target/hi6220.cfg b/tcl/target/hi6220.cfg
index c2feb0b..ddeeaad 100644
--- a/tcl/target/hi6220.cfg
+++ b/tcl/target/hi6220.cfg
@@ -37,7 +37,7 @@ set $_TARGETNAME.cti(7) 0x801DB000
 set _cores 8
 for { set _core 0 } { $_core < $_cores } { incr _core 1 } {
 
-    cti create cti$_core -dap $_CHIPNAME.dap -ctibase [set $_TARGETNAME.cti($_core)] -ap-num 0
+    cti create cti$_core -dap $_CHIPNAME.dap -baseaddr [set $_TARGETNAME.cti($_core)] -ap-num 0
 
     set _command "target create ${_TARGETNAME}$_core aarch64 \
                          -dap $_CHIPNAME.dap -coreid $_core -cti cti$_core"
@@ -57,7 +57,7 @@ for { set _core 0 } { $_core < $_cores } { incr _core 1 } {
 
 eval $_smp_command
 
-cti create cti.sys -dap $_CHIPNAME.dap -ap-num 0 -ctibase 0x80003000
+cti create cti.sys -dap $_CHIPNAME.dap -ap-num 0 -baseaddr 0x80003000
 
 # declare the auxiliary Cortex-M3 core on AP #2 (runs mcuimage.bin)
 target create ${_TARGETNAME}.m3 cortex_m -dap $_CHIPNAME.dap -ap-num 2 -defer-examine
diff --git a/tcl/target/imx8m.cfg b/tcl/target/imx8m.cfg
index e3b7d24..9a8bfec 100644
--- a/tcl/target/imx8m.cfg
+++ b/tcl/target/imx8m.cfg
@@ -35,7 +35,7 @@ set CTIBASE {0x80420000 0x80520000 0x80620000 0x80720000}
 for { set _core 0 } { $_core < $_cores } { incr _core } {
 
     cti create $_CTINAME.$_core -dap $_CHIPNAME.dap -ap-num 1 \
-        -ctibase [lindex $CTIBASE $_core]
+        -baseaddr [lindex $CTIBASE $_core]
 
     set _command "target create $_TARGETNAME.$_core aarch64 -dap $_CHIPNAME.dap \
         -dbgbase [lindex $DBGBASE $_core] -cti $_CTINAME.$_core"
diff --git a/tcl/target/kl25z_hla.cfg b/tcl/target/kl25z_hla.cfg
deleted file mode 100644
index e4deac6..0000000
--- a/tcl/target/kl25z_hla.cfg
+++ /dev/null
@@ -1,2 +0,0 @@
-echo "WARNING: target/kl25z_hla.cfg is deprecated, please switch to target/kl25.cfg"
-source [find target/kl25.cfg]
diff --git a/tcl/target/ls1012a.cfg b/tcl/target/ls1012a.cfg
index 19d3e58..e1bd168 100644
--- a/tcl/target/ls1012a.cfg
+++ b/tcl/target/ls1012a.cfg
@@ -25,7 +25,7 @@ jtag newtap $_CHIPNAME sap -irlen 8 -expected-id $_SAP_TAPID
 
 dap create $_CHIPNAME.dap -chain-position $_CHIPNAME.dap
 
-cti create $_CHIPNAME.cti -dap $_CHIPNAME.dap -ap-num 1 -ctibase 0x80420000
+cti create $_CHIPNAME.cti -dap $_CHIPNAME.dap -ap-num 1 -baseaddr 0x80420000
 
 set _TARGETNAME $_CHIPNAME.cpu
 target create $_TARGETNAME aarch64 -dap $_CHIPNAME.dap -dbgbase 0x80410000 -cti $_CHIPNAME.cti
diff --git a/tcl/target/marvell/88f37x0.cfg b/tcl/target/marvell/88f37x0.cfg
index 5e75135..5c3dd73 100644
--- a/tcl/target/marvell/88f37x0.cfg
+++ b/tcl/target/marvell/88f37x0.cfg
@@ -44,7 +44,7 @@ set _smp_command ""
 
 for { set _core 0 } { $_core < $_cores } { incr _core 1 } {
 
-    cti create cti$_core -dap $_CHIPNAME.dap -ctibase [lindex $_ctis $_core] -ap-num 0
+    cti create cti$_core -dap $_CHIPNAME.dap -baseaddr [lindex $_ctis $_core] -ap-num 0
 
     set _command "target create ${_TARGETNAME}$_core aarch64 \
                          -dap $_CHIPNAME.dap -coreid $_core \
diff --git a/tcl/target/nrf51_stlink.tcl b/tcl/target/nrf51_stlink.tcl
deleted file mode 100644
index 7e23c5a..0000000
--- a/tcl/target/nrf51_stlink.tcl
+++ /dev/null
@@ -1,2 +0,0 @@
-echo "WARNING: target/nrf51_stlink.cfg is deprecated, please switch to target/nrf51.cfg"
-source [find target/nrf51.cfg]
diff --git a/tcl/target/nrf52.cfg b/tcl/target/nrf52.cfg
index 88f2c69..d0c52fd 100644
--- a/tcl/target/nrf52.cfg
+++ b/tcl/target/nrf52.cfg
@@ -53,7 +53,7 @@ flash bank $_CHIPNAME.uicr nrf5 0x10001000 0 1 1 $_TARGETNAME
 # Test if MEM-AP is locked by UICR APPROTECT
 proc nrf52_check_ap_lock {} {
 	set dap [[target current] cget -dap]
-	set err [catch {set APPROTECTSTATUS [ocd_$dap apreg 1 0xc]}]
+	set err [catch {set APPROTECTSTATUS [$dap apreg 1 0xc]}]
 	if {$err == 0 && $APPROTECTSTATUS != 1} {
 		echo "****** WARNING ******"
 		echo "nRF52 device has AP lock engaged (see UICR APPROTECT register)."
@@ -71,7 +71,7 @@ proc nrf52_recover {} {
 	set target [target current]
 	set dap [$target cget -dap]
 
-	set IDR [ocd_$dap apreg 1 0xfc]
+	set IDR [$dap apreg 1 0xfc]
 	if {$IDR != 0x02880000} {
 		echo "Error: Cannot access nRF52 CTRL-AP!"
 		return
@@ -79,37 +79,38 @@ proc nrf52_recover {} {
 
 	poll off
 
-	# Assert reset
-	$dap apreg 1 0 1
-
-	# Reset ERASEALLSTATUS event
-	$dap apreg 1 8 0
-
-	# Trigger ERASEALL task
+	# Reset and trigger ERASEALL task
 	$dap apreg 1 4 0
 	$dap apreg 1 4 1
 
 	for {set i 0} {1} {incr i} {
-		set ERASEALLSTATUS [ocd_$dap apreg 1 8]
-		if {$ERASEALLSTATUS == 1} {
+		set ERASEALLSTATUS [$dap apreg 1 8]
+		if {$ERASEALLSTATUS == 0} {
 			echo "$target device has been successfully erased and unlocked."
 			break
 		}
-		if {$i >= 5} {
+		if {$i == 0} {
+			echo "Waiting for chip erase..."
+		}
+		if {$i >= 150} {
 			echo "Error: $target recovery failed."
 			break
 		}
 		sleep 100
 	}
 
+	# Assert reset
+	$dap apreg 1 0 1
+
 	# Deassert reset
 	$dap apreg 1 0 0
 
-	if {$ERASEALLSTATUS == 1} {
-		sleep 100
-		$target arp_examine
-		poll on
-	}
+	# Reset ERASEALL task
+	$dap apreg 1 4 0
+
+	sleep 100
+	$target arp_examine
+	poll on
 }
 
 add_help_text nrf52_recover "Mass erase and unlock nRF52 device"
diff --git a/tcl/target/renesas_rcar_gen3.cfg b/tcl/target/renesas_rcar_gen3.cfg
index 72f185d..5738d37 100644
--- a/tcl/target/renesas_rcar_gen3.cfg
+++ b/tcl/target/renesas_rcar_gen3.cfg
@@ -122,7 +122,7 @@ proc setup_a5x {core_name dbgbase ctibase num boot} {
 		set _TARGETNAME $_CHIPNAME.$core_name.$_core
 		set _CTINAME $_TARGETNAME.cti
 		cti create $_CTINAME -dap $_DAPNAME -ap-num 1 \
-			-ctibase [lindex $ctibase $_core]
+			-baseaddr [lindex $ctibase $_core]
 		set _command "target create $_TARGETNAME aarch64 -dap $_DAPNAME \
 			-ap-num 1 -dbgbase [lindex $dbgbase $_core] -cti $_CTINAME"
 		if { $_core == 0  && $boot == 1 } {
@@ -140,7 +140,7 @@ proc setup_cr7 {dbgbase ctibase boot} {
 	global _DAPNAME
 	set _TARGETNAME $_CHIPNAME.r7
 	set _CTINAME $_TARGETNAME.cti
-	cti create $_CTINAME -dap $_DAPNAME -ap-num 1 -ctibase $ctibase
+	cti create $_CTINAME -dap $_DAPNAME -ap-num 1 -baseaddr $ctibase
 	set _command "target create $_TARGETNAME cortex_r4 -dap $_DAPNAME \
 		-ap-num 1 -dbgbase $dbgbase"
 	if { $boot == 1 } {
diff --git a/tcl/target/rk3308.cfg b/tcl/target/rk3308.cfg
index d3d409e..7f957da 100644
--- a/tcl/target/rk3308.cfg
+++ b/tcl/target/rk3308.cfg
@@ -28,7 +28,7 @@ swd newdap $_CHIPNAME cpu -expected-id $_DAP_TAPID -ignore-version
 dap create $_CHIPNAME.dap -chain-position $_CHIPNAME.cpu
 target create $_CHIPNAME.ahb mem_ap -dap $_CHIPNAME.dap -ap-num 0
 
-# declare the 8 main application cores
+# declare the 4 main application cores
 set _TARGETNAME $_CHIPNAME.core
 set _smp_command ""
 
@@ -45,7 +45,7 @@ set $_TARGETNAME.cti(3) 0x8101b000
 set _cores 4
 for { set _core 0 } { $_core < $_cores } { incr _core 1 } {
 
-    cti create cti$_core -dap $_CHIPNAME.dap -ctibase [set $_TARGETNAME.cti($_core)] -ap-num 0
+    cti create cti$_core -dap $_CHIPNAME.dap -baseaddr [set $_TARGETNAME.cti($_core)] -ap-num 0
 
     set _command "target create ${_TARGETNAME}$_core aarch64 \
                          -dap $_CHIPNAME.dap -coreid $_core -cti cti$_core \
@@ -53,6 +53,7 @@ for { set _core 0 } { $_core < $_cores } { incr _core 1 } {
 
     if { $_core != 0 } {
         set _smp_command "$_smp_command ${_TARGETNAME}$_core"
+        set _command "$_command -defer-examine"
     } else {
         # uncomment to use hardware threads pseudo rtos
         # set _command "$_command -rtos hwthread"
diff --git a/tcl/target/stellaris_icdi.cfg b/tcl/target/stellaris_icdi.cfg
deleted file mode 100644
index f856a7a..0000000
--- a/tcl/target/stellaris_icdi.cfg
+++ /dev/null
@@ -1,2 +0,0 @@
-echo "WARNING: target/stellaris_icdi.cfg is deprecated, please switch to target/stellaris.cfg"
-source [find target/stellaris.cfg]
diff --git a/tcl/target/stm32_stlink.cfg b/tcl/target/stm32_stlink.cfg
deleted file mode 100644
index 295292e..0000000
--- a/tcl/target/stm32_stlink.cfg
+++ /dev/null
@@ -1 +0,0 @@
-echo "WARNING: stm32_stlink.cfg is deprecated (and does nothing, you can safely remove it.)"
diff --git a/tcl/target/stm32f0x_stlink.cfg b/tcl/target/stm32f0x_stlink.cfg
deleted file mode 100644
index cecfb7a..0000000
--- a/tcl/target/stm32f0x_stlink.cfg
+++ /dev/null
@@ -1,2 +0,0 @@
-echo "WARNING: target/stm32f0x_stlink.cfg is deprecated, please switch to target/stm32f0x.cfg"
-source [find target/stm32f0x.cfg]
diff --git a/tcl/target/stm32f1x_stlink.cfg b/tcl/target/stm32f1x_stlink.cfg
deleted file mode 100644
index 0a3e643..0000000
--- a/tcl/target/stm32f1x_stlink.cfg
+++ /dev/null
@@ -1,2 +0,0 @@
-echo "WARNING: target/stm32f1x_stlink.cfg is deprecated, please switch to target/stm32f1x.cfg"
-source [find target/stm32f1x.cfg]
diff --git a/tcl/target/stm32f2x_stlink.cfg b/tcl/target/stm32f2x_stlink.cfg
deleted file mode 100644
index 451b2b5..0000000
--- a/tcl/target/stm32f2x_stlink.cfg
+++ /dev/null
@@ -1,2 +0,0 @@
-echo "WARNING: target/stm32f2x_stlink.cfg is deprecated, please switch to target/stm32f2x.cfg"
-source [find target/stm32f2x.cfg]
diff --git a/tcl/target/stm32f3x_stlink.cfg b/tcl/target/stm32f3x_stlink.cfg
deleted file mode 100644
index 8769358..0000000
--- a/tcl/target/stm32f3x_stlink.cfg
+++ /dev/null
@@ -1,2 +0,0 @@
-echo "WARNING: target/stm32f3x_stlink.cfg is deprecated, please switch to target/stm32f3x.cfg"
-source [find target/stm32f3x.cfg]
diff --git a/tcl/target/stm32f4x.cfg b/tcl/target/stm32f4x.cfg
index b95e783..1587533 100644
--- a/tcl/target/stm32f4x.cfg
+++ b/tcl/target/stm32f4x.cfg
@@ -52,6 +52,12 @@ flash bank $_FLASHNAME stm32f2x 0 0 0 0 $_TARGETNAME
 
 flash bank $_CHIPNAME.otp stm32f2x 0x1fff7800 0 0 0 $_TARGETNAME
 
+if { [info exists QUADSPI] && $QUADSPI } {
+   set a [llength [flash list]]
+   set _QSPINAME $_CHIPNAME.qspi
+   flash bank $_QSPINAME stmqspi 0x90000000 0 0 0 $_TARGETNAME 0xA0001000
+}
+
 # JTAG speed should be <= F_CPU/6. F_CPU after reset is 16MHz, so use F_JTAG = 2MHz
 #
 # Since we may be running of an RC oscilator, we crank down the speed a
diff --git a/tcl/target/stm32f4x_stlink.cfg b/tcl/target/stm32f4x_stlink.cfg
deleted file mode 100644
index af3e8a0..0000000
--- a/tcl/target/stm32f4x_stlink.cfg
+++ /dev/null
@@ -1,2 +0,0 @@
-echo "WARNING: target/stm32f4x_stlink.cfg is deprecated, please switch to target/stm32f4x.cfg"
-source [find target/stm32f4x.cfg]
diff --git a/tcl/target/stm32f7x.cfg b/tcl/target/stm32f7x.cfg
index 6ad4b65..3c7679d 100644
--- a/tcl/target/stm32f7x.cfg
+++ b/tcl/target/stm32f7x.cfg
@@ -12,7 +12,7 @@ if { [info exists CHIPNAME] } {
    set _CHIPNAME stm32f7x
 }
 
-   set _ENDIAN little
+set _ENDIAN little
 
 # Work-area is a space in RAM used for flash programming
 # By default use 128kB
@@ -64,6 +64,12 @@ flash bank $_CHIPNAME.otp stm32f2x 0x1ff0f000 0 0 0 $_TARGETNAME
 #     the Flash via ITCM alias as virtual
 flash bank $_CHIPNAME.itcm-flash.alias virtual 0x00200000 0 0 0 $_TARGETNAME $_FLASHNAME
 
+if { [info exists QUADSPI] && $QUADSPI } {
+   set a [llength [flash list]]
+   set _QSPINAME $_CHIPNAME.qspi
+   flash bank $_QSPINAME stmqspi 0x90000000 0 0 0 $_TARGETNAME 0xA0001000
+}
+
 # adapter speed should be <= F_CPU/6. F_CPU after reset is 16MHz, so use F_JTAG = 2MHz
 adapter speed 2000
 
diff --git a/tcl/target/stm32h7x.cfg b/tcl/target/stm32h7x.cfg
index 43a8b02..8258e50 100644
--- a/tcl/target/stm32h7x.cfg
+++ b/tcl/target/stm32h7x.cfg
@@ -104,6 +104,23 @@ if {[set $_CHIPNAME.DUAL_CORE]} {
 # Make sure that cpu0 is selected
 targets $_CHIPNAME.cpu0
 
+if { [info exists QUADSPI] && $QUADSPI } {
+   set a [llength [flash list]]
+   set _QSPINAME $_CHIPNAME.qspi
+   flash bank $_QSPINAME stmqspi 0x90000000 0 0 0 $_CHIPNAME.cpu0 0x52005000
+} else {
+   if { [info exists OCTOSPI1] && $OCTOSPI1 } {
+      set a [llength [flash list]]
+      set _OCTOSPINAME1 $_CHIPNAME.octospi1
+      flash bank $_OCTOSPINAME1 stmqspi 0x90000000 0 0 0 $_CHIPNAME.cpu0 0x52005000
+   }
+   if { [info exists OCTOSPI2] && $OCTOSPI2 } {
+      set b [llength [flash list]]
+      set _OCTOSPINAME2 $_CHIPNAME.octospi2
+      flash bank $_OCTOSPINAME2 stmqspi 0x70000000 0 0 0 $_CHIPNAME.cpu0 0x5200A000
+   }
+}
+
 # Clock after reset is HSI at 64 MHz, no need of PLL
 adapter speed 1800
 
@@ -149,8 +166,10 @@ $_CHIPNAME.cpu0 configure -event examine-end {
 	stm32h7x_dbgmcu_mmw 0x004 0x00600000 0
 
 	# Enable debug during low power modes (uses more power)
-	# DBGMCU_CR |= DBG_STANDBY | DBG_STOP | DBG_SLEEP in D3, D2 & D1 Domains
-	stm32h7x_dbgmcu_mmw 0x004 0x000001BF 0
+	# DBGMCU_CR |= DBG_STANDBY | DBG_STOP | DBG_SLEEP D1 Domain
+	stm32h7x_dbgmcu_mmw 0x004 0x00000007 0
+	# DBGMCU_CR |= DBG_STANDBY | DBG_STOP | DBG_SLEEP D2 Domain
+	stm32h7x_dbgmcu_mmw 0x004 0x00000038 0
 
 	# Stop watchdog counters during halt
 	# DBGMCU_APB3FZ1 |= WWDG1
@@ -173,10 +192,19 @@ $_CHIPNAME.cpu0 configure -event reset-init {
 	adapter speed 4000
 }
 
+# get _CHIPNAME from current target
+proc stm32h7x_get_chipname {} {
+	set t [target current]
+	set sep [string last "." $t]
+	if {$sep == -1} {
+		return $t
+	}
+	return [string range $t 0 [expr $sep - 1]]
+}
+
 if {[set $_CHIPNAME.DUAL_CORE]} {
 	$_CHIPNAME.cpu1 configure -event examine-end {
-		# get _CHIPNAME from the current target
-		set _CHIPNAME [regsub ".cpu\\d$" [target current] ""]
+		set _CHIPNAME [stm32h7x_get_chipname]
 		global $_CHIPNAME.USE_CTI
 
 		# Stop watchdog counters during halt
@@ -212,8 +240,7 @@ proc stm32h7x_mmw {used_target reg setbits clearbits} {
 proc stm32h7x_dbgmcu_mmw {reg_offset setbits clearbits} {
 	# use $_CHIPNAME.ap2 if possible, and use the proper dbgmcu base address
 	if {![using_hla]} {
-		# get _CHIPNAME from the current target
-		set _CHIPNAME [regsub ".(cpu|ap)\\d*$" [target current] ""]
+		set _CHIPNAME [stm32h7x_get_chipname]
 		set used_target $_CHIPNAME.ap2
 		set reg_addr [expr 0xE00E1000 + $reg_offset]
 	} {
@@ -226,8 +253,8 @@ proc stm32h7x_dbgmcu_mmw {reg_offset setbits clearbits} {
 
 if {[set $_CHIPNAME.USE_CTI]} {
 	# create CTI instances for both cores
-	cti create $_CHIPNAME.cti0 -dap $_CHIPNAME.dap -ap-num 0 -ctibase 0xE0043000
-	cti create $_CHIPNAME.cti1 -dap $_CHIPNAME.dap -ap-num 3 -ctibase 0xE0043000
+	cti create $_CHIPNAME.cti0 -dap $_CHIPNAME.dap -ap-num 0 -baseaddr 0xE0043000
+	cti create $_CHIPNAME.cti1 -dap $_CHIPNAME.dap -ap-num 3 -baseaddr 0xE0043000
 
 	$_CHIPNAME.cpu0 configure -event halted { stm32h7x_cti_prepare_restart_all }
 	$_CHIPNAME.cpu1 configure -event halted { stm32h7x_cti_prepare_restart_all }
@@ -236,8 +263,7 @@ if {[set $_CHIPNAME.USE_CTI]} {
 	$_CHIPNAME.cpu1 configure -event debug-halted { stm32h7x_cti_prepare_restart_all }
 
 	proc stm32h7x_cti_start {} {
-		# get _CHIPNAME from the current target
-		set _CHIPNAME [regsub ".cpu\\d$" [target current] ""]
+		set _CHIPNAME [stm32h7x_get_chipname]
 
 		# Configure Cores' CTIs to halt each other
 		# TRIGIN0 (DBGTRIGGER) and TRIGOUT0 (EDBGRQ) at CTM_CHANNEL_0
@@ -252,8 +278,7 @@ if {[set $_CHIPNAME.USE_CTI]} {
 	}
 
 	proc stm32h7x_cti_stop {} {
-		# get _CHIPNAME from the current target
-		set _CHIPNAME [regsub ".cpu\\d$" [target current] ""]
+		set _CHIPNAME [stm32h7x_get_chipname]
 
 		$_CHIPNAME.cti0 enable off
 		$_CHIPNAME.cti1 enable off
@@ -265,8 +290,7 @@ if {[set $_CHIPNAME.USE_CTI]} {
 	}
 
 	proc stm32h7x_cti_prepare_restart {cti} {
-		# get _CHIPNAME from the current target
-		set _CHIPNAME [regsub ".cpu\\d$" [target current] ""]
+		set _CHIPNAME [stm32h7x_get_chipname]
 
 		# Acknowlodge EDBGRQ at TRIGOUT0
 		$_CHIPNAME.$cti write INACK 0x01
diff --git a/tcl/target/stm32l4x.cfg b/tcl/target/stm32l4x.cfg
index 46e6f7e..7f08f3c 100644
--- a/tcl/target/stm32l4x.cfg
+++ b/tcl/target/stm32l4x.cfg
@@ -50,6 +50,23 @@ $_TARGETNAME configure -work-area-phys 0x20000000 -work-area-size $_WORKAREASIZE
 set _FLASHNAME $_CHIPNAME.flash
 flash bank $_FLASHNAME stm32l4x 0 0 0 0 $_TARGETNAME
 
+if { [info exists QUADSPI] && $QUADSPI } {
+   set a [llength [flash list]]
+   set _QSPINAME $_CHIPNAME.qspi
+   flash bank $_QSPINAME stmqspi 0x90000000 0 0 0 $_TARGETNAME 0xA0001000
+} else {
+   if { [info exists OCTOSPI1] && $OCTOSPI1 } {
+      set a [llength [flash list]]
+      set _OCTOSPINAME1 $_CHIPNAME.octospi1
+      flash bank $_OCTOSPINAME1 stmqspi 0x90000000 0 0 0 $_TARGETNAME 0xA0001000
+   }
+   if { [info exists OCTOSPI2] && $OCTOSPI2 } {
+      set b [llength [flash list]]
+      set _OCTOSPINAME2 $_CHIPNAME.octospi2
+      flash bank $_OCTOSPINAME2 stmqspi 0x70000000 0 0 0 $_TARGETNAME 0xA0001400
+   }
+}
+
 # Common knowledges tells JTAG speed should be <= F_CPU/6.
 # F_CPU after reset is MSI 4MHz, so use F_JTAG = 500 kHz to stay on
 # the safe side.
diff --git a/tcl/target/stm32l5x.cfg b/tcl/target/stm32l5x.cfg
new file mode 100644
index 0000000..bf56360
--- /dev/null
+++ b/tcl/target/stm32l5x.cfg
@@ -0,0 +1,130 @@
+# SPDX-License-Identifier: GPL-2.0-or-later
+
+# script for stm32l5x family
+
+#
+# stm32l5 devices support both JTAG and SWD transports.
+#
+source [find target/swj-dp.tcl]
+source [find mem_helper.tcl]
+
+if { [info exists CHIPNAME] } {
+	set _CHIPNAME $CHIPNAME
+} else {
+	set _CHIPNAME stm32l5x
+}
+
+set _ENDIAN little
+
+# Work-area is a space in RAM used for flash programming
+# By default use 64kB
+if { [info exists WORKAREASIZE] } {
+	set _WORKAREASIZE $WORKAREASIZE
+} else {
+	set _WORKAREASIZE 0x10000
+}
+
+#jtag scan chain
+if { [info exists CPUTAPID] } {
+	set _CPUTAPID $CPUTAPID
+} else {
+	if { [using_jtag] } {
+		# See STM Document RM0438
+		# RM0438 Rev5, Section 52.2.8 JTAG debug port - Table 425. JTAG-DP data registers
+		# Corresponds to Cortex®-M33 JTAG debug port ID code
+		set _CPUTAPID 0x0ba04477
+	} {
+		# SWD IDCODE (single drop, arm)
+		set _CPUTAPID 0x0be12477
+	}
+}
+
+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 $_ENDIAN -dap $_CHIPNAME.dap
+
+# use non-secure RAM by default
+$_TARGETNAME configure -work-area-phys 0x20000000 -work-area-size $_WORKAREASIZE -work-area-backup 0
+
+# declare non-secure flash
+flash bank $_CHIPNAME.flash_ns stm32l4x 0 0 0 0 $_TARGETNAME
+
+# Common knowledges tells JTAG speed should be <= F_CPU/6.
+# F_CPU after reset is MSI 4MHz, so use F_JTAG = 500 kHz to stay on
+# the safe side.
+#
+# Note that there is a pretty wide band where things are
+# more or less stable, see http://openocd.zylin.com/#/c/3366/
+adapter speed 500
+
+adapter srst delay 100
+if {[using_jtag]} {
+	jtag_ntrst_delay 100
+}
+
+reset_config srst_nogate
+
+if {![using_hla]} {
+	# if srst is not fitted use SYSRESETREQ to
+	# perform a soft reset
+	cortex_m reset_config sysresetreq
+}
+
+proc clock_config_110_mhz {} {
+	# MCU clock is MSI (4MHz) after reset, set MCU freq at 110 MHz with PLL
+	# RCC_APB1ENR1 = PWREN
+	mww 0x40021058 0x10000000
+	# delay for register clock enable (read back reg)
+	mrw 0x40021058
+	# PWR_CR1 : VOS Range 0
+	mww 0x40007000 0
+	# while (PWR_SR2 & VOSF)
+	while {([mrw 0x40007014] & 0x0400)} {}
+	# FLASH_ACR : 5 WS for 110 MHz HCLK
+	mww 0x40022000 0x00000005
+	# RCC_PLLCFGR = PLLP=PLLQ=0, PLLR=00=2, PLLREN=1, PLLN=55, PLLM=0000=1, PLLSRC=MSI 4MHz
+	# fVCO = 4 x 55 /1 = 220
+	# SYSCLOCK = fVCO/PLLR = 220/2 = 110 MHz
+	mww 0x4002100C 0x01003711
+	# RCC_CR |= PLLON
+	mmw 0x40021000 0x01000000 0
+	# while !(RCC_CR & PLLRDY)
+	while {!([mrw 0x40021000] & 0x02000000)} {}
+	# RCC_CFGR |= SW_PLL
+	mmw 0x40021008 0x00000003 0
+	# while ((RCC_CFGR & SWS) != PLL)
+	while {([mrw 0x40021008] & 0x0C) != 0x0C} {}
+}
+
+$_TARGETNAME configure -event reset-init {
+	clock_config_110_mhz
+	# Boost JTAG frequency
+	adapter speed 4000
+}
+
+$_TARGETNAME configure -event reset-start {
+	# Reset clock is MSI (4 MHz)
+	adapter speed 480
+}
+
+$_TARGETNAME configure -event examine-end {
+	# DBGMCU_CR |= DBG_STANDBY | DBG_STOP
+	mmw 0xE0044004 0x00000006 0
+
+	# Stop watchdog counters during halt
+	# DBGMCU_APB1_FZ |= DBG_IWDG_STOP | DBG_WWDG_STOP
+	mmw 0xE0044008 0x00001800 0
+}
+
+$_TARGETNAME configure -event trace-config {
+	# Set TRACE_IOEN; TRACE_MODE is set to async; when using sync
+	# change this value accordingly to configure trace pins
+	# assignment
+	mmw 0xE0044004 0x00000020 0
+}
diff --git a/tcl/target/stm32lx_stlink.cfg b/tcl/target/stm32lx_stlink.cfg
deleted file mode 100644
index 5f694b5..0000000
--- a/tcl/target/stm32lx_stlink.cfg
+++ /dev/null
@@ -1,2 +0,0 @@
-echo "WARNING: target/stm32lx_stlink.cfg is deprecated, please switch to target/stm32l1.cfg"
-source [find target/stm32l1.cfg]
diff --git a/tcl/target/stm32mp15x.cfg b/tcl/target/stm32mp15x.cfg
index f2ba94e..4a8bc86 100644
--- a/tcl/target/stm32mp15x.cfg
+++ b/tcl/target/stm32mp15x.cfg
@@ -59,10 +59,10 @@ $_CHIPNAME.cpu1 cortex_a maskisr on
 $_CHIPNAME.cpu0 cortex_a dacrfixup on
 $_CHIPNAME.cpu1 cortex_a dacrfixup on
 
-cti create $_CHIPNAME.cti.sys  -dap $_CHIPNAME.dap -ap-num 1 -ctibase 0xE0094000
-cti create $_CHIPNAME.cti.cpu0 -dap $_CHIPNAME.dap -ap-num 1 -ctibase 0xE00D8000
-cti create $_CHIPNAME.cti.cpu1 -dap $_CHIPNAME.dap -ap-num 1 -ctibase 0xE00D9000
-cti create $_CHIPNAME.cti.cm4  -dap $_CHIPNAME.dap -ap-num 2 -ctibase 0xE0043000
+cti create $_CHIPNAME.cti.sys  -dap $_CHIPNAME.dap -ap-num 1 -baseaddr 0xE0094000
+cti create $_CHIPNAME.cti.cpu0 -dap $_CHIPNAME.dap -ap-num 1 -baseaddr 0xE00D8000
+cti create $_CHIPNAME.cti.cpu1 -dap $_CHIPNAME.dap -ap-num 1 -baseaddr 0xE00D9000
+cti create $_CHIPNAME.cti.cm4  -dap $_CHIPNAME.dap -ap-num 2 -baseaddr 0xE0043000
 
 # interface does not work while srst is asserted
 # this is target specific, valid for every board
diff --git a/tcl/target/stm32w108_stlink.cfg b/tcl/target/stm32w108_stlink.cfg
deleted file mode 100644
index 120feea..0000000
--- a/tcl/target/stm32w108_stlink.cfg
+++ /dev/null
@@ -1,2 +0,0 @@
-echo "WARNING: target/stm32w108xx_stlink.cfg is deprecated, please switch to target/stm32w108xx.cfg"
-source [find target/stm32w108xx.cfg]
diff --git a/tcl/target/xilinx_zynqmp.cfg b/tcl/target/xilinx_zynqmp.cfg
index 9be781c..b21603f 100644
--- a/tcl/target/xilinx_zynqmp.cfg
+++ b/tcl/target/xilinx_zynqmp.cfg
@@ -74,7 +74,7 @@ set _cores 4
 for { set _core 0 } { $_core < $_cores } { incr _core } {
 
     cti create $_CTINAME.$_core -dap $_CHIPNAME.dap -ap-num 1 \
-        -ctibase [lindex $CTIBASE $_core]
+        -baseaddr [lindex $CTIBASE $_core]
 
     set _command "target create $_TARGETNAME.$_core aarch64 -dap $_CHIPNAME.dap \
         -dbgbase [lindex $DBGBASE $_core] -cti $_CTINAME.$_core"