Merge pull request #351 from riscv/from_upstream

From upstream

147 files changed, 6456 insertions, 1242 deletions

diff --git a/HACKING b/HACKING
index eebd8ed..a6bb6aa 100644
--- a/HACKING
+++ b/HACKING
@@ -148,18 +148,22 @@ to instruct git locally how to send off the changes.
   -# Add a new remote to git using Gerrit username:
 @code
 git remote add review ssh://USERNAME@openocd.zylin.com:29418/openocd.git
-git config remote.review.push HEAD:refs/publish/master
+git config remote.review.push HEAD:refs/for/master
 @endcode
   Or with http only:
 @code
 git remote add review http://USERNAME@openocd.zylin.com/p/openocd.git
-git config remote.review.push HEAD:refs/publish/master
+git config remote.review.push HEAD:refs/for/master
 @endcode
   The http password is configured from your gerrit settings - http://openocd.zylin.com/#/settings/http-password.
   \note If you want to simplify http access you can also add your http password to the url as follows:
 @code
 git remote add review http://USERNAME:PASSWORD@openocd.zylin.com/p/openocd.git
 @endcode
+  \note All contributions should be pushed to @c refs/for/master on the
+Gerrit server, even if you plan to use several local branches for different
+topics. It is possible because @c for/master is not a traditional Git
+branch.
   -# You will need to install this hook, we will look into a better solution:
 @code
 scp -p -P 29418 USERNAME@openocd.zylin.com:hooks/commit-msg .git/hooks/
diff --git a/README b/README
index dd776f3..00e83bb 100644
--- a/README
+++ b/README
@@ -123,12 +123,12 @@ EJTAG, NDS32, XScale, Intel Quark.
 Flash drivers
 -------------
 
-ADUC702x, AT91SAM, ATH79, AVR, CFI, DSP5680xx, EFM32, EM357, eSi-TSMC,
-FM3, FM4, Freedom E SPI, Kinetis, LPC8xx/LPC1xxx/LPC2xxx/LPC541xx,
-LPC2900, LPCSPIFI, Marvell QSPI, Milandr, NIIET, NuMicro, PIC32mx,
-PSoC4, PSoC5LP, SiM3x, Stellaris, STM32, STMSMI, STR7x, STR9x, nRF51;
-NAND controllers of AT91SAM9, LPC3180, LPC32xx, i.MX31, MXC, NUC910,
-Orion/Kirkwood, S3C24xx, S3C6400, XMC1xxx, XMC4xxx.
+ADUC702x, AT91SAM, ATH79, AVR, CFI, DSP5680xx, EFM32, EM357, eSi-TSMC, FM3,
+FM4, Freedom E SPI, Kinetis, LPC8xx/LPC1xxx/LPC2xxx/LPC541xx, LPC2900,
+LPCSPIFI, Marvell QSPI, Milandr, NIIET, NuMicro, PIC32mx, PSoC4, PSoC5LP,
+SiM3x, Stellaris, STM32, STMSMI, STR7x, STR9x, nRF51; NAND controllers of
+AT91SAM9, LPC3180, LPC32xx, i.MX31, MXC, NUC910, Orion/Kirkwood, S3C24xx,
+S3C6400, XMC1xxx, XMC4xxx.
 
 
 ==================
diff --git a/contrib/60-openocd.rules b/contrib/60-openocd.rules
index 692e1b1..ac574bb 100644
--- a/contrib/60-openocd.rules
+++ b/contrib/60-openocd.rules
@@ -55,14 +55,21 @@ ATTRS{idVendor}=="0403", ATTRS{idProduct}=="cff8", MODE="660", GROUP="plugdev",
 # TI ICDI
 ATTRS{idVendor}=="0451", ATTRS{idProduct}=="c32a", MODE="660", GROUP="plugdev", TAG+="uaccess"
 
-# STLink v1
+# STMicroelectronics ST-LINK V1
 ATTRS{idVendor}=="0483", ATTRS{idProduct}=="3744", MODE="660", GROUP="plugdev", TAG+="uaccess"
 
-# STLink v2
+# STMicroelectronics ST-LINK/V2
 ATTRS{idVendor}=="0483", ATTRS{idProduct}=="3748", MODE="660", GROUP="plugdev", TAG+="uaccess"
 
-# STLink v2-1
+# STMicroelectronics ST-LINK/V2.1
 ATTRS{idVendor}=="0483", ATTRS{idProduct}=="374b", MODE="660", GROUP="plugdev", TAG+="uaccess"
+ATTRS{idVendor}=="0483", ATTRS{idProduct}=="3752", MODE="660", GROUP="plugdev", TAG+="uaccess"
+
+# STMicroelectronics STLINK-V3
+ATTRS{idVendor}=="0483", ATTRS{idProduct}=="374d", MODE="660", GROUP="plugdev", TAG+="uaccess"
+ATTRS{idVendor}=="0483", ATTRS{idProduct}=="374e", MODE="660", GROUP="plugdev", TAG+="uaccess"
+ATTRS{idVendor}=="0483", ATTRS{idProduct}=="374f", MODE="660", GROUP="plugdev", TAG+="uaccess"
+ATTRS{idVendor}=="0483", ATTRS{idProduct}=="3753", MODE="660", GROUP="plugdev", TAG+="uaccess"
 
 # Cypress KitProg in KitProg mode
 ATTRS{idVendor}=="04b4", ATTRS{idProduct}=="f139", MODE="660", GROUP="plugdev", TAG+="uaccess"
diff --git a/contrib/loaders/flash/fespi/Makefile b/contrib/loaders/flash/fespi/Makefile
index 5217dde..4d2ab51 100644
--- a/contrib/loaders/flash/fespi/Makefile
+++ b/contrib/loaders/flash/fespi/Makefile
@@ -6,7 +6,7 @@ CC=$(CROSS_COMPILE)gcc
 OBJCOPY=$(CROSS_COMPILE)objcopy
 OBJDUMP=$(CROSS_COMPILE)objdump
 
-CFLAGS = -march=rv32i -mabi=ilp32 -x assembler-with-cpp - -nostdlib -nostartfiles
+CFLAGS = -march=rv32i -mabi=ilp32 -x assembler-with-cpp -nostdlib -nostartfiles
 
 all: fespi.inc
 
diff --git a/doc/openocd.texi b/doc/openocd.texi
index 7dfd977..cd68ea9 100644
--- a/doc/openocd.texi
+++ b/doc/openocd.texi
@@ -156,8 +156,8 @@ OpenOCD internally. @xref{Debug Adapter Hardware}.
 
 @b{GDB Debug:} It allows ARM7 (ARM7TDMI and ARM720t), ARM9 (ARM920T,
 ARM922T, ARM926EJ--S, ARM966E--S), XScale (PXA25x, IXP42x), Cortex-M3
-(Stellaris LM3, ST STM32 and Energy Micro EFM32) and Intel Quark (x10xx)
-based cores to be debugged via the GDB protocol.
+(Stellaris LM3, STMicroelectronics STM32 and Energy Micro EFM32) and
+Intel Quark (x10xx) based cores to be debugged via the GDB protocol.
 
 @b{Flash Programming:} Flash writing is supported for external
 CFI-compatible NOR flashes (Intel and AMD/Spansion command set) and several
@@ -477,8 +477,8 @@ SWD and not JTAG, thus not supported.
 @end itemize
 
 @section USB ST-LINK based
-ST Micro has an adapter called @b{ST-LINK}.
-They only work with ST Micro chips, notably STM32 and STM8.
+STMicroelectronics has an adapter called @b{ST-LINK}.
+They only work with STMicroelectronics chips, notably STM32 and STM8.
 
 @itemize @bullet
 @item @b{ST-LINK}
@@ -487,6 +487,9 @@ They only work with ST Micro chips, notably STM32 and STM8.
 @item @b{ST-LINK/V2}
 @* This is available standalone and as part of some kits, eg. STM32F4DISCOVERY.
 @* Link: @url{http://www.st.com/internet/evalboard/product/251168.jsp}
+@item @b{STLINK-V3}
+@* This is available standalone and as part of some kits.
+@* Link: @url{http://www.st.com/stlink-v3}
 @end itemize
 
 For info the original ST-LINK enumerates using the mass storage usb class; however,
@@ -535,6 +538,12 @@ debuggers to ARM Cortex based targets @url{http://www.keil.com/support/man/docs/
 @item @b{TI XDS110 Debug Probe}
 @* The XDS110 is included as the embedded debug probe on many Texas Instruments
 LaunchPad evaluation boards.
+@* The XDS110 is also available as a stand-alone USB debug probe. The XDS110
+stand-alone probe has the additional ability to supply voltage to the target
+board via its AUX FUNCTIONS port. Use the
+@command{xds110_supply_voltage <millivolts>} command to set the voltage. 0 turns
+off the supply. Otherwise, the supply can be set to any value in the range 1800
+to 3600 millivolts.
 @* Link: @url{http://processors.wiki.ti.com/index.php/XDS110}
 @* Link: @url{http://processors.wiki.ti.com/index.php/XDS_Emulation_Software_Package#XDS110_Support_Utilities}
 @end itemize
@@ -2359,6 +2368,16 @@ the hardware can support.
 Returns the name of the debug adapter driver being used.
 @end deffn
 
+@deffn Command {adapter usb location} <bus>:<port>[.<port>]...
+Specifies the physical USB port of the adapter to use. The path
+roots at @var{bus} and walks down the physical ports, with each
+@var{port} option specifying a deeper level in the bus topology, the last
+@var{port} denoting where the target adapter is actually plugged.
+The USB bus topology can be queried with the command @emph{lsusb -t} or @emph{dmesg}.
+
+This command is only available if your libusb1 is at least version 1.0.16.
+@end deffn
+
 @section Interface Drivers
 
 Each of the interface drivers listed here must be explicitly
@@ -2578,10 +2597,11 @@ For example adapter definitions, see the configuration files shipped in the
 @end deffn
 
 @deffn {Interface Driver} {ft232r}
-This driver is implementing synchronous bitbang mode of an FTDI FT232R
-USB UART bridge IC.
+This driver is implementing synchronous bitbang mode of an FTDI FT232R,
+FT230X, FT231X and similar USB UART bridge ICs by reusing RS232 signals as GPIO.
+It currently doesn't support using CBUS pins as GPIO.
 
-List of connections (pin numbers for SSOP):
+List of connections (default physical pin numbers for FT232R in 28-pin SSOP package):
 @itemize @minus
 @item RXD(5) - TDI
 @item TXD(1) - TCK
@@ -2591,6 +2611,27 @@ List of connections (pin numbers for SSOP):
 @item DCD(10) - SRST
 @end itemize
 
+User can change default pinout by supplying configuration
+commands with GPIO numbers or RS232 signal names.
+GPIO numbers correspond to bit numbers in FTDI GPIO register.
+They differ from physical pin numbers.
+For details see actual FTDI chip datasheets.
+Every JTAG line must be configured to unique GPIO number
+different than any other JTAG line, even those lines
+that are sometimes not used like TRST or SRST.
+
+FT232R
+@itemize @minus
+@item bit 7 - RI
+@item bit 6 - DCD
+@item bit 5 - DSR
+@item bit 4 - DTR
+@item bit 3 - CTS
+@item bit 2 - RTS
+@item bit 1 - RXD
+@item bit 0 - TXD
+@end itemize
+
 These interfaces have several commands, used to configure the driver
 before initializing the JTAG scan chain:
 
@@ -2605,6 +2646,47 @@ vendor provides unique IDs and more than one adapter is connected to
 the host. If not specified, serial numbers are not considered.
 @end deffn
 
+@deffn {Config Command} {ft232r_jtag_nums} @var{tck} @var{tms} @var{tdi} @var{tdo}
+Set four JTAG GPIO numbers at once.
+If not specified, default 0 3 1 2 or TXD CTS RXD RTS is used.
+@end deffn
+
+@deffn {Config Command} {ft232r_tck_num} @var{tck}
+Set TCK GPIO number. If not specified, default 0 or TXD is used.
+@end deffn
+
+@deffn {Config Command} {ft232r_tms_num} @var{tms}
+Set TMS GPIO number. If not specified, default 3 or CTS is used.
+@end deffn
+
+@deffn {Config Command} {ft232r_tdi_num} @var{tdi}
+Set TDI GPIO number. If not specified, default 1 or RXD is used.
+@end deffn
+
+@deffn {Config Command} {ft232r_tdo_num} @var{tdo}
+Set TDO GPIO number. If not specified, default 2 or RTS is used.
+@end deffn
+
+@deffn {Config Command} {ft232r_trst_num} @var{trst}
+Set TRST GPIO number. If not specified, default 4 or DTR is used.
+@end deffn
+
+@deffn {Config Command} {ft232r_srst_num} @var{srst}
+Set SRST GPIO number. If not specified, default 6 or DCD is used.
+@end deffn
+
+@deffn {Config Command} {ft232r_restore_serial} @var{word}
+Restore serial port after JTAG. This USB bitmode control word
+(16-bit) will be sent before quit. Lower byte should
+set GPIO direction register to a "sane" state:
+0x15 for TXD RTS DTR as outputs (1), others as inputs (0). Higher
+byte is usually 0 to disable bitbang mode.
+When kernel driver reattaches, serial port should continue to work.
+Value 0xFFFF disables sending control word and serial port,
+then kernel driver will not reattach.
+If not specified, default 0xFFFF is used.
+@end deffn
+
 @end deffn
 
 @deffn {Interface Driver} {remote_bitbang}
@@ -2981,7 +3063,7 @@ This is a driver that supports multiple High Level Adapters.
 This type of adapter does not expose some of the lower level api's
 that OpenOCD would normally use to access the target.
 
-Currently supported adapters include the ST ST-LINK and TI ICDI.
+Currently supported adapters include the STMicroelectronics ST-LINK and TI ICDI.
 ST-LINK firmware version >= V2.J21.S4 recommended due to issues with earlier
 versions of firmware where serial number is reset after first use.  Suggest
 using ST firmware update utility to upgrade ST-LINK firmware even if current
@@ -3392,6 +3474,7 @@ How long (in milliseconds) OpenOCD should wait after deasserting
 nTRST (active-low JTAG TAP reset) before starting new JTAG operations.
 @end deffn
 
+@anchor {reset_config}
 @deffn {Command} reset_config mode_flag ...
 This command displays or modifies the reset configuration
 of your combination of JTAG board and target in target
@@ -4026,13 +4109,13 @@ resources; then a @file{board.cfg} with off-chip resources, clocking,
 and so forth.
 
 @anchor{dapdeclaration}
-@section DAP declaration (ARMv7 and ARMv8 targets)
+@section DAP declaration (ARMv6-M, ARMv7 and ARMv8 targets)
 @cindex DAP declaration
 
 Since OpenOCD version 0.11.0, the Debug Access Port (DAP) is
 no longer implicitly created together with the target. It must be
-explicitly declared using the @command{dap create} command. For all
-ARMv7 and ARMv8 targets, the option "@option{-dap} @var{dap_name}" has to be used
+explicitly declared using the @command{dap create} command. For all ARMv6-M, ARMv7
+and ARMv8 targets, the option "@option{-dap} @var{dap_name}" has to be used
 instead of "@option{-chain-position} @var{dotted.name}" when the target is created.
 
 The @command{dap} command group supports the following sub-commands:
@@ -5280,12 +5363,12 @@ since the alternate function must be enabled on the GPIO pin
 CS1/CS2 is routed to on the given SoC.
 
 @example
-flash bank $_FLASHNAME ath79 0 0 0 0 $_TARGETNAME
+flash bank $_FLASHNAME ath79 0xbf000000 0 0 0 $_TARGETNAME
 
 # When using multiple chipselects the base should be different for each,
 # otherwise the write_image command is not able to distinguish the
 # banks.
-flash bank flash0 ath79 0x00000000 0 0 0 $_TARGETNAME cs0
+flash bank flash0 ath79 0xbf000000 0 0 0 $_TARGETNAME cs0
 flash bank flash1 ath79 0x10000000 0 0 0 $_TARGETNAME cs1
 flash bank flash2 ath79 0x20000000 0 0 0 $_TARGETNAME cs2
 @end example
@@ -5367,9 +5450,16 @@ the flash.
 @anchor{at91samd}
 @deffn {Flash Driver} at91samd
 @cindex at91samd
-All members of the ATSAMD, ATSAMR, ATSAML and ATSAMC microcontroller
+All members of the ATSAM D2x, D1x, D0x, ATSAMR, ATSAML and ATSAMC microcontroller
 families from Atmel include internal flash and use ARM's Cortex-M0+ core.
-This driver uses the same command names/syntax as @xref{at91sam3}.
+
+Do not use for ATSAM D51 and E5x: use @xref{atsame5} instead.
+
+The devices have one flash bank:
+
+@example
+flash bank $_FLASHNAME at91samd 0x00000000 0 1 1 $_TARGETNAME
+@end example
 
 @deffn Command {at91samd chip-erase}
 Issues a complete Flash erase via the Device Service Unit (DSU). This can be
@@ -5531,9 +5621,72 @@ Command is used internally in event event reset-deassert-post.
 @end deffn
 @end deffn
 
+@anchor{atsame5}
+@deffn {Flash Driver} atsame5
+@cindex atsame5
+All members of the SAM E54, E53, E51 and D51 microcontroller
+families from Microchip (former Atmel) include internal flash
+and use ARM's Cortex-M4 core.
+
+The devices have two ECC flash banks with a swapping feature.
+This driver handles both banks together as it were one.
+Bank swapping is not supported yet.
+
+@example
+flash bank $_FLASHNAME atsame5 0x00000000 0 1 1 $_TARGETNAME
+@end example
+
+@deffn Command {atsame5 bootloader}
+Shows or sets the bootloader size configuration, stored in the User Page of the
+Flash. This is called the BOOTPROT region. When setting, the bootloader size
+must be specified in bytes. The nearest bigger protection size is used.
+Settings are written immediately but only take effect on MCU reset.
+Setting the bootloader size to 0 disables bootloader protection.
+
+@example
+atsame5 bootloader
+atsame5 bootloader 16384
+@end example
+@end deffn
+
+@deffn Command {atsame5 chip-erase}
+Issues a complete Flash erase via the Device Service Unit (DSU). This can be
+used to erase a chip back to its factory state and does not require the
+processor to be halted.
+@end deffn
+
+@deffn Command {atsame5 dsu_reset_deassert}
+This command releases internal reset held by DSU
+and prepares reset vector catch in case of reset halt.
+Command is used internally in event event reset-deassert-post.
+@end deffn
+
+@deffn Command {atsame5 userpage}
+Writes or reads the first 64 bits of NVM User Page which is located at
+0x804000. This field includes various fuses.
+Reading is done by invoking this command without any arguments.
+Writing is possible by giving 1 or 2 hex values. The first argument
+is the value to be written and the second one is an optional bit mask
+(a zero bit in the mask means the bit stays unchanged).
+The reserved fields are always masked out and cannot be changed.
+
+@example
+# Read
+>atsame5 userpage
+USER PAGE: 0xAEECFF80FE9A9239
+# Write
+>atsame5 userpage 0xAEECFF80FE9A9239
+# Write 2 to SEESBLK and 4 to SEEPSZ fields but leave other bits unchanged
+# (setup SmartEEPROM of virtual size 8192 bytes)
+>atsame5 userpage 0x4200000000 0x7f00000000
+@end example
+@end deffn
+
+@end deffn
+
 @deffn {Flash Driver} atsamv
 @cindex atsamv
-All members of the ATSAMV, ATSAMS, and ATSAME families from
+All members of the ATSAMV7x, ATSAMS70, and ATSAME70 families from
 Atmel include internal flash and use ARM's Cortex-M7 core.
 This driver uses the same command names/syntax as @xref{at91sam3}.
 @end deffn
@@ -5618,7 +5771,7 @@ Triggering a mass erase is also useful when users want to disable readout protec
 All versions of the SimpleLink CC13xx and CC26xx microcontrollers from Texas
 Instruments include internal flash. The cc26xx flash driver supports both the
 CC13xx and CC26xx family of devices. The driver automatically recognizes the
-specific version's flash parameters and autoconfigures itself. Flash bank 0
+specific version's flash parameters and autoconfigures itself. The flash bank
 starts at address 0.
 
 @example
@@ -5668,16 +5821,17 @@ configuration register interface, @option{clock_hz} is the expected clock
 frequency, and @option{wait_states} is the number of configured read wait states.
 
 @example
-flash bank $_FLASHNAME esirisc base_address size_bytes 0 0 $_TARGETNAME cfg_address clock_hz wait_states
+flash bank $_FLASHNAME esirisc base_address size_bytes 0 0 \
+           $_TARGETNAME cfg_address clock_hz wait_states
 @end example
 
-@deffn Command {esirisc_flash mass_erase} (bank_id)
-Erases all pages in data memory for the bank identified by @option{bank_id}.
+@deffn Command {esirisc flash mass_erase} bank_id
+Erase all pages in data memory for the bank identified by @option{bank_id}.
 @end deffn
 
-@deffn Command {esirisc_flash ref_erase} (bank_id)
-Erases the reference cell for the bank identified by @option{bank_id}. This is
-an uncommon operation.
+@deffn Command {esirisc flash ref_erase} bank_id
+Erase the reference cell for the bank identified by @option{bank_id}. @emph{This
+is an uncommon operation.}
 @end deffn
 @end deffn
 
@@ -5843,8 +5997,8 @@ Command disables watchdog timer.
 @deffn {Flash Driver} lpc2000
 This is the driver to support internal flash of all members of the
 LPC11(x)00 and LPC1300 microcontroller families and most members of
-the LPC800, LPC1500, LPC1700, LPC1800, LPC2000, LPC4000 and LPC54100
-microcontroller families from NXP.
+the LPC800, LPC1500, LPC1700, LPC1800, LPC2000, LPC4000, LPC54100,
+LPC8Nxx and NHS31xx microcontroller families from NXP.
 
 @quotation Note
 There are LPC2000 devices which are not supported by the @var{lpc2000}
@@ -5853,7 +6007,7 @@ The LPC2888 is supported by the @var{lpc288x} driver.
 The LPC29xx family is supported by the @var{lpc2900} driver.
 @end quotation
 
-The @var{lpc2000} driver defines two mandatory and one optional parameters,
+The @var{lpc2000} driver defines two mandatory and two optional parameters,
 which must appear in the following order:
 
 @itemize
@@ -5869,7 +6023,7 @@ LPC43x[2357])
 @option{lpc54100} (LPC541xx)
 @option{lpc4000} (LPC40xx)
 or @option{auto} - automatically detects flash variant and size for LPC11(x)00,
-LPC8xx, LPC13xx, LPC17xx and LPC40xx
+LPC8xx, LPC13xx, LPC17xx, LPC40xx, LPC8Nxx and NHS31xx
 @item @var{clock_kHz} ... the frequency, in kiloHertz,
 at which the core is running
 @item @option{calc_checksum} ... optional (but you probably want to provide this!),
@@ -5880,6 +6034,8 @@ table, the boot ROM will almost certainly ignore your flash image.
 However, if you do provide it,
 with most tool chains @command{verify_image} will fail.
 @end quotation
+@item @option{iap_entry} ... optional telling the driver to use a different
+ROM IAP entry point.
 @end itemize
 
 LPC flashes don't require the chip and bus width to be specified.
@@ -6415,7 +6571,7 @@ applied to all of them.
 
 @deffn {Flash Driver} stm32f1x
 All members of the STM32F0, STM32F1 and STM32F3 microcontroller families
-from ST Microelectronics include internal flash and use ARM Cortex-M0/M3/M4 cores.
+from STMicroelectronics include internal flash and use ARM Cortex-M0/M3/M4 cores.
 The driver automatically recognizes a number of these chips using
 the chip identification register, and autoconfigures itself.
 
@@ -6461,9 +6617,10 @@ or upon executing the @command{stm32f1x options_load} command.
 The @var{num} parameter is a value shown by @command{flash banks}.
 @end deffn
 
-@deffn Command {stm32f1x options_write} num (@option{SWWDG}|@option{HWWDG}) (@option{RSTSTNDBY}|@option{NORSTSTNDBY}) (@option{RSTSTOP}|@option{NORSTSTOP})
+@deffn Command {stm32f1x options_write} num (@option{SWWDG}|@option{HWWDG}) (@option{RSTSTNDBY}|@option{NORSTSTNDBY}) (@option{RSTSTOP}|@option{NORSTSTOP}) (@option{USEROPT} user_data)
 Writes the stm32 option byte with the specified values.
 The @var{num} parameter is a value shown by @command{flash banks}.
+The @var{user_data} parameter is content of higher 16 bits of the option byte register (Data0 and Data1 as one 16bit number).
 @end deffn
 
 @deffn Command {stm32f1x options_load} num
@@ -6475,7 +6632,7 @@ The @var{num} parameter is a value shown by @command{flash banks}.
 @end deffn
 
 @deffn {Flash Driver} stm32f2x
-All members of the STM32F2, STM32F4 and STM32F7 microcontroller families from ST Microelectronics
+All members of the STM32F2, STM32F4 and STM32F7 microcontroller families from STMicroelectronics
 include internal flash and use ARM Cortex-M3/M4/M7 cores.
 The driver automatically recognizes a number of these chips using
 the chip identification register, and autoconfigures itself.
@@ -6529,7 +6686,7 @@ The @var{num} parameter is a value shown by @command{flash banks}, @var{optcr2}
 @end deffn
 
 @deffn {Flash Driver} stm32h7x
-All members of the STM32H7 microcontroller families from ST Microelectronics
+All members of the STM32H7 microcontroller families from STMicroelectronics
 include internal flash and use ARM Cortex-M7 core.
 The driver automatically recognizes a number of these chips using
 the chip identification register, and autoconfigures itself.
@@ -6565,7 +6722,7 @@ The @var{num} parameter is a value shown by @command{flash banks}.
 @end deffn
 
 @deffn {Flash Driver} stm32lx
-All members of the STM32L microcontroller families from ST Microelectronics
+All members of the STM32L microcontroller families from STMicroelectronics
 include internal flash and use ARM Cortex-M3 and Cortex-M0+ cores.
 The driver automatically recognizes a number of these chips using
 the chip identification register, and autoconfigures itself.
@@ -6605,7 +6762,7 @@ The @var{num} parameter is a value shown by @command{flash banks}.
 @end deffn
 
 @deffn {Flash Driver} stm32l4x
-All members of the STM32L4 microcontroller families from ST Microelectronics
+All members of the STM32L4 microcontroller families from STMicroelectronics
 include internal flash and use ARM Cortex-M4 cores.
 The driver automatically recognizes a number of these chips using
 the chip identification register, and autoconfigures itself.
@@ -6677,7 +6834,7 @@ The @var{num} parameter is a value shown by @command{flash banks}.
 @end deffn
 
 @deffn {Flash Driver} str7x
-All members of the STR7 microcontroller family from ST Microelectronics
+All members of the STR7 microcontroller family from STMicroelectronics
 include internal flash and use ARM7TDMI cores.
 The @var{str7x} driver defines one mandatory parameter, @var{variant},
 which is either @code{STR71x}, @code{STR73x} or @code{STR75x}.
@@ -6694,7 +6851,7 @@ for the specified flash bank.
 @end deffn
 
 @deffn {Flash Driver} str9x
-Most members of the STR9 microcontroller family from ST Microelectronics
+Most members of the STR9 microcontroller family from STMicroelectronics
 include internal flash and use ARM966E cores.
 The str9 needs the flash controller to be configured using
 the @command{str9x flash_config} command prior to Flash programming.
@@ -6834,6 +6991,17 @@ the flash clock.
 @end deffn
 @end deffn
 
+@deffn {Flash Driver} w600
+W60x series Wi-Fi SoC from WinnerMicro
+are designed with ARM Cortex-M3 and have 1M Byte QFLASH inside.
+The @var{w600} driver uses the @var{target} parameter to select the
+correct bank config.
+
+@example
+flash bank $_FLASHNAME w600 0x08000000 0 0 0 $_TARGETNAMEs
+@end example
+@end deffn
+
 @deffn {Flash Driver} xmc1xxx
 All members of the XMC1xxx microcontroller family from Infineon.
 This driver does not require the chip and bus width to be specified.
@@ -8938,19 +9106,23 @@ must also be explicitly enabled.
 This finishes by listing the current vector catch configuration.
 @end deffn
 
-@deffn Command {cortex_m reset_config} (@option{srst}|@option{sysresetreq}|@option{vectreset})
-Control reset handling. The default @option{srst} is to use srst if fitted,
-otherwise fallback to @option{vectreset}.
+@deffn Command {cortex_m reset_config} (@option{sysresetreq}|@option{vectreset})
+Control reset handling if hardware srst is not fitted
+@xref{reset_config,,reset_config}.
+
 @itemize @minus
-@item @option{srst} use hardware srst if fitted otherwise fallback to @option{vectreset}.
-@item @option{sysresetreq} use NVIC SYSRESETREQ to reset system.
-@item @option{vectreset} use NVIC VECTRESET to reset system.
+@item @option{sysresetreq} use AIRCR SYSRESETREQ to reset system.
+@item @option{vectreset} use AIRCR VECTRESET to reset system (default).
 @end itemize
-Using @option{vectreset} is a safe option for all current Cortex-M cores.
+
+Using @option{vectreset} is a safe option for Cortex-M3, M4 and M7 cores.
 This however has the disadvantage of only resetting the core, all peripherals
-are unaffected. A solution would be to use a @code{reset-init} event handler to manually reset
-the peripherals.
+are unaffected. A solution would be to use a @code{reset-init} event handler
+to manually reset the peripherals.
 @xref{targetevents,,Target Events}.
+
+Cortex-M0, M0+ and M1 do not support @option{vectreset}, use @option{sysresetreq}
+instead.
 @end deffn
 
 @subsection ARMv8-A specific commands
@@ -8986,17 +9158,13 @@ Selects whether interrupts will be processed when single stepping. The default c
 eSi-RISC is a highly configurable microprocessor architecture for embedded systems
 provided by EnSilica. (See: @url{http://www.ensilica.com/risc-ip/}.)
 
-@subsection esirisc specific commands
+@subsection eSi-RISC Configuration
+
 @deffn Command {esirisc cache_arch} (@option{harvard}|@option{von_neumann})
 Configure the caching architecture. Targets with the @code{UNIFIED_ADDRESS_SPACE}
 option disabled employ a Harvard architecture. By default, @option{von_neumann} is assumed.
 @end deffn
 
-@deffn Command {esirisc flush_caches}
-Flush instruction and data caches. This command requires that the target is halted
-when the command is issued and configured with an instruction or data cache.
-@end deffn
-
 @deffn Command {esirisc hwdc} (@option{all}|@option{none}|mask ...)
 Configure hardware debug control. The HWDC register controls which exceptions return
 control back to the debugger. Possible masks are @option{all}, @option{none},
@@ -9004,6 +9172,164 @@ control back to the debugger. Possible masks are @option{all}, @option{none},
 By default, @option{reset}, @option{error}, and @option{debug} are enabled.
 @end deffn
 
+@subsection eSi-RISC Operation
+
+@deffn Command {esirisc flush_caches}
+Flush instruction and data caches. This command requires that the target is halted
+when the command is issued and configured with an instruction or data cache.
+@end deffn
+
+@subsection eSi-Trace Configuration
+
+eSi-RISC targets may be configured with support for instruction tracing. Trace
+data may be written to an in-memory buffer or FIFO. If a FIFO is configured, DMA
+is typically employed to move trace data off-device using a high-speed
+peripheral (eg. SPI). Collected trace data is encoded in one of three different
+formats. At a minimum, @command{esirisc trace buffer} or @command{esirisc trace
+fifo} must be issued along with @command{esirisc trace format} before trace data
+can be collected.
+
+OpenOCD provides rudimentary analysis of collected trace data. If more detail is
+needed, collected trace data can be dumped to a file and processed by external
+tooling.
+
+@quotation Issues
+OpenOCD is unable to process trace data sent to a FIFO. A potential workaround
+for this issue is to configure DMA to copy trace data to an in-memory buffer,
+which can then be passed to the @command{esirisc trace analyze} and
+@command{esirisc trace dump} commands.
+
+It is possible to corrupt trace data when using a FIFO if the peripheral
+responsible for draining data from the FIFO is not fast enough. This can be
+managed by enabling flow control, however this can impact timing-sensitive
+software operation on the CPU.
+@end quotation
+
+@deffn Command {esirisc trace buffer} address size [@option{wrap}]
+Configure trace buffer using the provided address and size. If the @option{wrap}
+option is specified, trace collection will continue once the end of the buffer
+is reached. By default, wrap is disabled.
+@end deffn
+
+@deffn Command {esirisc trace fifo} address
+Configure trace FIFO using the provided address.
+@end deffn
+
+@deffn Command {esirisc trace flow_control} (@option{enable}|@option{disable})
+Enable or disable stalling the CPU to collect trace data. By default, flow
+control is disabled.
+@end deffn
+
+@deffn Command {esirisc trace format} (@option{full}|@option{branch}|@option{icache}) pc_bits
+Configure trace format and number of PC bits to be captured. @option{pc_bits}
+must be within 1 and 31 as the LSB is not collected. If external tooling is used
+to analyze collected trace data, these values must match.
+
+Supported trace formats:
+@itemize
+@item @option{full} capture full trace data, allowing execution history and
+timing to be determined.
+@item @option{branch} capture taken branch instructions and branch target
+addresses.
+@item @option{icache} capture instruction cache misses.
+@end itemize
+@end deffn
+
+@deffn Command {esirisc trace trigger start} (@option{condition}) [start_data start_mask]
+Configure trigger start condition using the provided start data and mask. A
+brief description of each condition is provided below; for more detail on how
+these values are used, see the eSi-RISC Architecture Manual.
+
+Supported conditions:
+@itemize
+@item @option{none} manual tracing (see @command{esirisc trace start}).
+@item @option{pc} start tracing if the PC matches start data and mask.
+@item @option{load} start tracing if the effective address of a load
+instruction matches start data and mask.
+@item @option{store} start tracing if the effective address of a store
+instruction matches start data and mask.
+@item @option{exception} start tracing if the EID of an exception matches start
+data and mask.
+@item @option{eret} start tracing when an @code{ERET} instruction is executed.
+@item @option{wait} start tracing when a @code{WAIT} instruction is executed.
+@item @option{stop} start tracing when a @code{STOP} instruction is executed.
+@item @option{high} start tracing when an external signal is a logical high.
+@item @option{low} start tracing when an external signal is a logical low.
+@end itemize
+@end deffn
+
+@deffn Command {esirisc trace trigger stop} (@option{condition}) [stop_data stop_mask]
+Configure trigger stop condition using the provided stop data and mask. A brief
+description of each condition is provided below; for more detail on how these
+values are used, see the eSi-RISC Architecture Manual.
+
+Supported conditions:
+@itemize
+@item @option{none} manual tracing (see @command{esirisc trace stop}).
+@item @option{pc} stop tracing if the PC matches stop data and mask.
+@item @option{load} stop tracing if the effective address of a load
+instruction matches stop data and mask.
+@item @option{store} stop tracing if the effective address of a store
+instruction matches stop data and mask.
+@item @option{exception} stop tracing if the EID of an exception matches stop
+data and mask.
+@item @option{eret} stop tracing when an @code{ERET} instruction is executed.
+@item @option{wait} stop tracing when a @code{WAIT} instruction is executed.
+@item @option{stop} stop tracing when a @code{STOP} instruction is executed.
+@end itemize
+@end deffn
+
+@deffn Command {esirisc trace trigger delay} (@option{trigger}) [cycles]
+Configure trigger start/stop delay in clock cycles.
+
+Supported triggers:
+@itemize
+@item @option{none} no delay to start or stop collection.
+@item @option{start} delay @option{cycles} after trigger to start collection.
+@item @option{stop} delay @option{cycles} after trigger to stop collection.
+@item @option{both} delay @option{cycles} after both triggers to start or stop
+collection.
+@end itemize
+@end deffn
+
+@subsection eSi-Trace Operation
+
+@deffn Command {esirisc trace init}
+Initialize trace collection. This command must be called any time the
+configuration changes. If an trace buffer has been configured, the contents will
+be overwritten when trace collection starts.
+@end deffn
+
+@deffn Command {esirisc trace info}
+Display trace configuration.
+@end deffn
+
+@deffn Command {esirisc trace status}
+Display trace collection status.
+@end deffn
+
+@deffn Command {esirisc trace start}
+Start manual trace collection.
+@end deffn
+
+@deffn Command {esirisc trace stop}
+Stop manual trace collection.
+@end deffn
+
+@deffn Command {esirisc trace analyze} [address size]
+Analyze collected trace data. This command may only be used if a trace buffer
+has been configured. If a trace FIFO has been configured, trace data must be
+copied to an in-memory buffer identified by the @option{address} and
+@option{size} options using DMA.
+@end deffn
+
+@deffn Command {esirisc trace dump} [address size] @file{filename}
+Dump collected trace data to file. This command may only be used if a trace
+buffer has been configured. If a trace FIFO has been configured, trace data must
+be copied to an in-memory buffer identified by the @option{address} and
+@option{size} options using DMA.
+@end deffn
+
 @section Intel Architecture
 
 Intel Quark X10xx is the first product in the Quark family of SoCs. It is an IA-32
diff --git a/src/flash/nand/mxc.c b/src/flash/nand/mxc.c
index 5e59b9a..6be4160 100644
--- a/src/flash/nand/mxc.c
+++ b/src/flash/nand/mxc.c
@@ -179,6 +179,7 @@ COMMAND_HANDLER(handle_mxc_biswap_command)
 static const struct command_registration mxc_sub_command_handlers[] = {
 	{
 		.name = "biswap",
+		.mode = COMMAND_EXEC,
 		.handler = handle_mxc_biswap_command,
 		.help = "Turns on/off bad block information swaping from main area, "
 			"without parameter query status.",
diff --git a/src/flash/nor/Makefile.am b/src/flash/nor/Makefile.am
index 6393305..135128e 100644
--- a/src/flash/nor/Makefile.am
+++ b/src/flash/nor/Makefile.am
@@ -17,6 +17,7 @@ NOR_DRIVERS = \
 	%D%/at91sam7.c \
 	%D%/ath79.c \
 	%D%/atsamv.c \
+	%D%/atsame5.c \
 	%D%/avrf.c \
 	%D%/bluenrg-x.c \
 	%D%/cc3220sf.c \
@@ -64,6 +65,7 @@ NOR_DRIVERS = \
 	%D%/str9xpec.c \
 	%D%/tms470.c \
 	%D%/virtual.c \
+	%D%/w600.c \
 	%D%/xcf.c \
 	%D%/xmc1xxx.c \
 	%D%/xmc4xxx.c
diff --git a/src/flash/nor/aduc702x.c b/src/flash/nor/aduc702x.c
index 34cc362..824112b 100644
--- a/src/flash/nor/aduc702x.c
+++ b/src/flash/nor/aduc702x.c
@@ -74,12 +74,6 @@ static int aduc702x_build_sector_list(struct flash_bank *bank)
 	return ERROR_OK;
 }
 
-static int aduc702x_protect_check(struct flash_bank *bank)
-{
-	printf("aduc702x_protect_check not implemented yet.\n");
-	return ERROR_OK;
-}
-
 static int aduc702x_erase(struct flash_bank *bank, int first, int last)
 {
 	/* int res; */
@@ -130,12 +124,6 @@ static int aduc702x_erase(struct flash_bank *bank, int first, int last)
 	return ERROR_OK;
 }
 
-static int aduc702x_protect(struct flash_bank *bank, int set, int first, int last)
-{
-	printf("aduc702x_protect not implemented yet.\n");
-	return ERROR_FLASH_OPERATION_FAILED;
-}
-
 /* If this fn returns ERROR_TARGET_RESOURCE_NOT_AVAILABLE, then the caller can fall
  * back to another mechanism that does not require onboard RAM
  *
@@ -394,11 +382,9 @@ struct flash_driver aduc702x_flash = {
 	.name = "aduc702x",
 	.flash_bank_command = aduc702x_flash_bank_command,
 	.erase = aduc702x_erase,
-	.protect = aduc702x_protect,
 	.write = aduc702x_write,
 	.read = default_flash_read,
 	.probe = aduc702x_probe,
 	.auto_probe = aduc702x_probe,
 	.erase_check = default_flash_blank_check,
-	.protect_check = aduc702x_protect_check,
 };
diff --git a/src/flash/nor/aducm360.c b/src/flash/nor/aducm360.c
index 8681a25..7663783 100644
--- a/src/flash/nor/aducm360.c
+++ b/src/flash/nor/aducm360.c
@@ -103,13 +103,6 @@ static int aducm360_build_sector_list(struct flash_bank *bank)
 }
 
 /* ----------------------------------------------------------------------- */
-static int aducm360_protect_check(struct flash_bank *bank)
-{
-	LOG_WARNING("aducm360_protect_check not implemented.");
-	return ERROR_OK;
-}
-
-/* ----------------------------------------------------------------------- */
 static int aducm360_mass_erase(struct target *target)
 {
 	uint32_t		value;
@@ -195,13 +188,6 @@ static int aducm360_erase(struct flash_bank *bank, int first, int last)
 }
 
 /* ----------------------------------------------------------------------- */
-static int aducm360_protect(struct flash_bank *bank, int set, int first, int last)
-{
-	LOG_ERROR("aducm360_protect not implemented.");
-	return ERROR_FLASH_OPERATION_FAILED;
-}
-
-/* ----------------------------------------------------------------------- */
 static int aducm360_write_block_sync(
 		struct flash_bank *bank,
 		const uint8_t *buffer,
@@ -572,11 +558,9 @@ struct flash_driver aducm360_flash = {
 	.name = "aducm360",
 	.flash_bank_command = aducm360_flash_bank_command,
 	.erase = aducm360_erase,
-	.protect = aducm360_protect,
 	.write = aducm360_write,
 	.read = default_flash_read,
 	.probe = aducm360_probe,
 	.auto_probe = aducm360_probe,
 	.erase_check = default_flash_blank_check,
-	.protect_check = aducm360_protect_check,
 };
diff --git a/src/flash/nor/at91sam3.c b/src/flash/nor/at91sam3.c
index d80b6fe..7119188 100644
--- a/src/flash/nor/at91sam3.c
+++ b/src/flash/nor/at91sam3.c
@@ -2991,28 +2991,6 @@ static int sam3_GetInfo(struct sam3_chip *pChip)
 	return ERROR_OK;
 }
 
-static int sam3_erase_check(struct flash_bank *bank)
-{
-	int x;
-
-	LOG_DEBUG("Here");
-	if (bank->target->state != TARGET_HALTED) {
-		LOG_ERROR("Target not halted");
-		return ERROR_TARGET_NOT_HALTED;
-	}
-	if (0 == bank->num_sectors) {
-		LOG_ERROR("Target: not supported/not probed");
-		return ERROR_FAIL;
-	}
-
-	LOG_INFO("sam3 - supports auto-erase, erase_check ignored");
-	for (x = 0; x < bank->num_sectors; x++)
-		bank->sectors[x].is_erased = 1;
-
-	LOG_DEBUG("Done");
-	return ERROR_OK;
-}
-
 static int sam3_protect_check(struct flash_bank *bank)
 {
 	int r;
@@ -3785,7 +3763,7 @@ struct flash_driver at91sam3_flash = {
 	.read = default_flash_read,
 	.probe = sam3_probe,
 	.auto_probe = sam3_auto_probe,
-	.erase_check = sam3_erase_check,
+	.erase_check = default_flash_blank_check,
 	.protect_check = sam3_protect_check,
 	.free_driver_priv = sam3_free_driver_priv,
 };
diff --git a/src/flash/nor/at91samd.c b/src/flash/nor/at91samd.c
index db000b5..7f51bd6 100644
--- a/src/flash/nor/at91samd.c
+++ b/src/flash/nor/at91samd.c
@@ -180,6 +180,28 @@ static const struct samd_part samd21_parts[] = {
 	{ 0x24, "SAMD21G15B", 32, 4 },
 	{ 0x26, "SAMD21E16B", 64, 8 },
 	{ 0x27, "SAMD21E15B", 32, 4 },
+
+	/* Known SAMDA1 parts.
+	   SAMD-A1 series uses the same series identifier like the SAMD21
+	   taken from http://ww1.microchip.com/downloads/en/DeviceDoc/40001895A.pdf (pages 14-17) */
+	{ 0x29, "SAMDA1J16A", 64, 8 },
+	{ 0x2A, "SAMDA1J15A", 32, 4 },
+	{ 0x2B, "SAMDA1J14A", 16, 4 },
+	{ 0x2C, "SAMDA1G16A", 64, 8 },
+	{ 0x2D, "SAMDA1G15A", 32, 4 },
+	{ 0x2E, "SAMDA1G14A", 16, 4 },
+	{ 0x2F, "SAMDA1E16A", 64, 8 },
+	{ 0x30, "SAMDA1E15A", 32, 4 },
+	{ 0x31, "SAMDA1E14A", 16, 4 },
+	{ 0x64, "SAMDA1J16B", 64, 8 },
+	{ 0x65, "SAMDA1J15B", 32, 4 },
+	{ 0x66, "SAMDA1J14B", 16, 4 },
+	{ 0x67, "SAMDA1G16B", 64, 8 },
+	{ 0x68, "SAMDA1G15B", 32, 4 },
+	{ 0x69, "SAMDA1G14B", 16, 4 },
+	{ 0x6A, "SAMDA1E16B", 64, 8 },
+	{ 0x6B, "SAMDA1E15B", 32, 4 },
+	{ 0x6C, "SAMDA1E14B", 16, 4 },
 };
 
 /* Known SAML21 parts. */
@@ -208,6 +230,9 @@ static const struct samd_part saml21_parts[] = {
     /* SAMR30 parts have integrated SAML21 with a radio */
 	{ 0x1E, "SAMR30G18A", 256, 32 },
 	{ 0x1F, "SAMR30E18A", 256, 32 },
+
+    /* SAMR34/R35 parts have integrated SAML21 with a lora radio */
+	{ 0x28, "SAMR34J18", 256, 32 },
 };
 
 /* Known SAML22 parts. */
@@ -237,6 +262,8 @@ static const struct samd_part samc20_parts[] = {
 	{ 0x0B, "SAMC20E17A", 128, 16 },
 	{ 0x0C, "SAMC20E16A", 64, 8 },
 	{ 0x0D, "SAMC20E15A", 32, 4 },
+	{ 0x20, "SAMC20N18A", 256, 32 },
+	{ 0x21, "SAMC20N17A", 128, 16 },
 };
 
 /* Known SAMC21 parts. */
@@ -253,6 +280,8 @@ static const struct samd_part samc21_parts[] = {
 	{ 0x0B, "SAMC21E17A", 128, 16 },
 	{ 0x0C, "SAMC21E16A", 64, 8 },
 	{ 0x0D, "SAMC21E15A", 32, 4 },
+	{ 0x20, "SAMC21N18A", 256, 32 },
+	{ 0x21, "SAMC21N17A", 128, 16 },
 };
 
 /* Each family of parts contains a parts table in the DEVSEL field of DID.  The
diff --git a/src/flash/nor/ath79.c b/src/flash/nor/ath79.c
index 04d183c..e34146a 100644
--- a/src/flash/nor/ath79.c
+++ b/src/flash/nor/ath79.c
@@ -522,6 +522,9 @@ static int ath79_erase(struct flash_bank *bank, int first, int last)
 		return ERROR_FLASH_BANK_NOT_PROBED;
 	}
 
+	if (ath79_info->dev->erase_cmd == 0x00)
+		return ERROR_FLASH_OPER_UNSUPPORTED;
+
 	for (sector = first; sector <= last; sector++) {
 		if (bank->sectors[sector].is_protected) {
 			LOG_ERROR("Flash sector %d protected", sector);
@@ -560,7 +563,11 @@ static int ath79_write_page(struct flash_bank *bank, const uint8_t *buffer,
 		address,
 	};
 	int retval;
-	uint32_t i;
+	uint32_t i, pagesize;
+
+	/* if no write pagesize, use reasonable default */
+	pagesize = ath79_info->dev->pagesize ?
+		ath79_info->dev->pagesize : SPIFLASH_DEF_PAGESIZE;
 
 	if (address & 0xff) {
 		LOG_ERROR("ath79_write_page: unaligned write address: %08x",
@@ -573,7 +580,7 @@ static int ath79_write_page(struct flash_bank *bank, const uint8_t *buffer,
 	}
 	if (len > ath79_info->dev->pagesize) {
 		LOG_ERROR("ath79_write_page: len bigger than page size %d: %d",
-			  ath79_info->dev->pagesize, len);
+			pagesize, len);
 		return ERROR_FAIL;
 	}
 
@@ -611,12 +618,16 @@ static int ath79_write_buffer(struct flash_bank *bank, const uint8_t *buffer,
 			      uint32_t address, uint32_t len)
 {
 	struct ath79_flash_bank *ath79_info = bank->driver_priv;
-	const uint32_t page_size = ath79_info->dev->pagesize;
+	uint32_t page_size;
 	int retval;
 
 	LOG_DEBUG("%s: address=0x%08" PRIx32 " len=0x%08" PRIx32,
 		  __func__, address, len);
 
+	/* if no valid page_size, use reasonable default */
+	page_size = ath79_info->dev->pagesize ?
+		ath79_info->dev->pagesize : SPIFLASH_DEF_PAGESIZE;
+
 	while (len > 0) {
 		int page_len = len > page_size ? page_size : len;
 
@@ -642,13 +653,6 @@ static int ath79_write(struct flash_bank *bank, const uint8_t *buffer,
 	LOG_DEBUG("%s: offset=0x%08" PRIx32 " count=0x%08" PRIx32,
 		  __func__, offset, count);
 
-	if (offset < bank->base || offset >= bank->base + bank->size) {
-		LOG_ERROR("Start address out of range");
-		return ERROR_FAIL;
-	}
-
-	offset -= bank->base;
-
 	if (target->state != TARGET_HALTED) {
 		LOG_ERROR("Target not halted");
 		return ERROR_TARGET_NOT_HALTED;
@@ -718,13 +722,6 @@ static int ath79_read(struct flash_bank *bank, uint8_t *buffer,
 	LOG_DEBUG("%s: offset=0x%08" PRIx32 " count=0x%08" PRIx32,
 		  __func__, offset, count);
 
-	if (offset < bank->base || offset >= bank->base + bank->size) {
-		LOG_ERROR("Start address out of range");
-		return ERROR_FAIL;
-	}
-
-	offset -= bank->base;
-
 	if (target->state != TARGET_HALTED) {
 		LOG_ERROR("Target not halted");
 		return ERROR_TARGET_NOT_HALTED;
@@ -775,6 +772,7 @@ static int ath79_probe(struct flash_bank *bank)
 	struct ath79_flash_bank *ath79_info = bank->driver_priv;
 	struct flash_sector *sectors;
 	uint32_t id = 0; /* silence uninitialized warning */
+	uint32_t pagesize, sectorsize;
 	const struct ath79_target *target_device;
 	int retval;
 
@@ -820,16 +818,27 @@ static int ath79_probe(struct flash_bank *bank)
 
 	/* Set correct size value */
 	bank->size = ath79_info->dev->size_in_bytes;
+	if (bank->size <= (1UL << 16))
+		LOG_WARNING("device needs 2-byte addresses - not implemented");
+	if (bank->size > (1UL << 24))
+		LOG_WARNING("device needs paging or 4-byte addresses - not implemented");
+
+	/* if no sectors, treat whole bank as single sector */
+	sectorsize = ath79_info->dev->sectorsize ?
+		ath79_info->dev->sectorsize : ath79_info->dev->size_in_bytes;
 
 	/* create and fill sectors array */
-	bank->num_sectors =
-		ath79_info->dev->size_in_bytes / ath79_info->dev->sectorsize;
+	bank->num_sectors = ath79_info->dev->size_in_bytes / sectorsize;
 	sectors = calloc(1, sizeof(struct flash_sector) * bank->num_sectors);
 	if (!sectors) {
 		LOG_ERROR("not enough memory");
 		return ERROR_FAIL;
 	}
-	ath79_info->spi.page_buf = malloc(ath79_info->dev->pagesize);
+
+	/* if no write pagesize, use reasonable default */
+	pagesize = ath79_info->dev->pagesize ? ath79_info->dev->pagesize : SPIFLASH_DEF_PAGESIZE;
+
+	ath79_info->spi.page_buf = malloc(pagesize);
 	if (!ath79_info->spi.page_buf) {
 		LOG_ERROR("not enough memory");
 		free(sectors);
@@ -837,8 +846,8 @@ static int ath79_probe(struct flash_bank *bank)
 	}
 
 	for (int sector = 0; sector < bank->num_sectors; sector++) {
-		sectors[sector].offset = sector * ath79_info->dev->sectorsize;
-		sectors[sector].size = ath79_info->dev->sectorsize;
+		sectors[sector].offset = sector * sectorsize;
+		sectors[sector].size = sectorsize;
 		sectors[sector].is_erased = 0;
 		sectors[sector].is_protected = 1;
 	}
diff --git a/src/flash/nor/atsame5.c b/src/flash/nor/atsame5.c
new file mode 100644
index 0000000..94f0b41
--- /dev/null
+++ b/src/flash/nor/atsame5.c
@@ -0,0 +1,954 @@
+/***************************************************************************
+ *   Copyright (C) 2017 by Tomas Vanek					   *
+ *   vanekt@fbl.cz							   *
+ *                                                                         *
+ *   Based on at91samd.c                                                   *
+ *   Copyright (C) 2013 by Andrey Yurovsky                                 *
+ *   Andrey Yurovsky <yurovsky@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/>. *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "imp.h"
+#include "helper/binarybuffer.h"
+
+#include <target/cortex_m.h>
+
+/* A note to prefixing.
+ * Definitions and functions ingerited from at91samd.c without
+ * any change retained the original prefix samd_ so they eventualy
+ * may go to samd_common.h and .c
+ * As currently there are olny 3 short functions identical with
+ * the original source, no common file was created. */
+
+#define SAME5_PAGES_PER_BLOCK	16
+#define SAME5_NUM_PROT_BLOCKS	32
+#define SAMD_PAGE_SIZE_MAX	1024
+
+#define SAMD_FLASH		0x00000000	/* physical Flash memory */
+#define SAMD_USER_ROW		0x00804000	/* User Row of Flash */
+
+#define SAME5_PAC		0x40000000	/* Peripheral Access Control */
+
+#define SAMD_DSU		0x41002000	/* Device Service Unit */
+#define SAMD_NVMCTRL		0x41004000	/* Non-volatile memory controller */
+
+#define SAMD_DSU_STATUSA        1               /* DSU status register */
+#define SAMD_DSU_DID		0x18		/* Device ID register */
+#define SAMD_DSU_CTRL_EXT	0x100		/* CTRL register, external access */
+
+#define SAME5_NVMCTRL_CTRLA	0x00		/* NVM control A register */
+#define SAME5_NVMCTRL_CTRLB	0x04		/* NVM control B register */
+#define SAMD_NVMCTRL_PARAM	0x08		/* NVM parameters register */
+#define SAME5_NVMCTRL_INTFLAG	0x10		/* NVM interrupt flag register */
+#define SAME5_NVMCTRL_STATUS	0x12		/* NVM status register */
+#define SAME5_NVMCTRL_ADDR	0x14		/* NVM address register */
+#define SAME5_NVMCTRL_LOCK	0x18		/* NVM Lock section register */
+
+#define SAMD_CMDEX_KEY		0xA5UL
+#define SAMD_NVM_CMD(n)		((SAMD_CMDEX_KEY << 8) | (n & 0x7F))
+
+/* NVMCTRL commands. */
+#define SAME5_NVM_CMD_EP	0x00		/* Erase Page (User Page only) */
+#define SAME5_NVM_CMD_EB	0x01		/* Erase Block */
+#define SAME5_NVM_CMD_WP	0x03		/* Write Page */
+#define SAME5_NVM_CMD_WQW	0x04		/* Write Quad Word */
+#define SAME5_NVM_CMD_LR	0x11		/* Lock Region */
+#define SAME5_NVM_CMD_UR	0x12		/* Unlock Region */
+#define SAME5_NVM_CMD_PBC	0x15		/* Page Buffer Clear */
+#define SAME5_NVM_CMD_SSB	0x16		/* Set Security Bit */
+
+/* NVMCTRL bits */
+#define SAME5_NVMCTRL_CTRLA_WMODE_MASK	0x30
+
+#define SAME5_NVMCTRL_INTFLAG_DONE	(1 << 0)
+#define SAME5_NVMCTRL_INTFLAG_ADDRE	(1 << 1)
+#define SAME5_NVMCTRL_INTFLAG_PROGE	(1 << 2)
+#define SAME5_NVMCTRL_INTFLAG_LOCKE	(1 << 3)
+#define SAME5_NVMCTRL_INTFLAG_ECCSE	(1 << 4)
+#define SAME5_NVMCTRL_INTFLAG_ECCDE	(1 << 5)
+#define SAME5_NVMCTRL_INTFLAG_NVME	(1 << 6)
+
+
+/* Known identifiers */
+#define SAMD_PROCESSOR_M0	0x01
+#define SAMD_PROCESSOR_M4	0x06
+#define SAMD_FAMILY_D		0x00
+#define SAMD_FAMILY_E		0x03
+#define SAMD_SERIES_51		0x06
+#define SAME_SERIES_51		0x01
+#define SAME_SERIES_53		0x03
+#define SAME_SERIES_54		0x04
+
+/* Device ID macros */
+#define SAMD_GET_PROCESSOR(id) (id >> 28)
+#define SAMD_GET_FAMILY(id) (((id >> 23) & 0x1F))
+#define SAMD_GET_SERIES(id) (((id >> 16) & 0x3F))
+#define SAMD_GET_DEVSEL(id) (id & 0xFF)
+
+/* Bits to mask user row */
+#define NVMUSERROW_SAM_E5_D5_MASK	((uint64_t)0x7FFF00FF3C007FFF)
+
+struct samd_part {
+	uint8_t id;
+	const char *name;
+	uint32_t flash_kb;
+	uint32_t ram_kb;
+};
+
+/* See SAM D5x/E5x Family Silicon Errata and Data Sheet Clarification
+ * DS80000748B */
+/* Known SAMD51 parts. */
+static const struct samd_part samd51_parts[] = {
+	{ 0x00, "SAMD51P20A", 1024, 256 },
+	{ 0x01, "SAMD51P19A", 512, 192 },
+	{ 0x02, "SAMD51N20A", 1024, 256 },
+	{ 0x03, "SAMD51N19A", 512, 192 },
+	{ 0x04, "SAMD51J20A", 1024, 256 },
+	{ 0x05, "SAMD51J19A", 512, 192 },
+	{ 0x06, "SAMD51J18A", 256, 128 },
+	{ 0x07, "SAMD51G19A", 512, 192 },
+	{ 0x08, "SAMD51G18A", 256, 128 },
+};
+
+/* Known SAME51 parts. */
+static const struct samd_part same51_parts[] = {
+	{ 0x00, "SAME51N20A", 1024, 256 },
+	{ 0x01, "SAME51N19A", 512, 192 },
+	{ 0x02, "SAME51J19A", 512, 192 },
+	{ 0x03, "SAME51J18A", 256, 128 },
+	{ 0x04, "SAME51J20A", 1024, 256 },
+};
+
+/* Known SAME53 parts. */
+static const struct samd_part same53_parts[] = {
+	{ 0x02, "SAME53N20A", 1024, 256 },
+	{ 0x03, "SAME53N19A", 512, 192 },
+	{ 0x04, "SAME53J20A", 1024, 256 },
+	{ 0x05, "SAME53J19A", 512, 192 },
+	{ 0x06, "SAME53J18A", 256, 128 },
+};
+
+/* Known SAME54 parts. */
+static const struct samd_part same54_parts[] = {
+	{ 0x00, "SAME54P20A", 1024, 256 },
+	{ 0x01, "SAME54P19A", 512, 192 },
+	{ 0x02, "SAME54N20A", 1024, 256 },
+	{ 0x03, "SAME54N19A", 512, 192 },
+};
+
+/* Each family of parts contains a parts table in the DEVSEL field of DID.  The
+ * processor ID, family ID, and series ID are used to determine which exact
+ * family this is and then we can use the corresponding table. */
+struct samd_family {
+	uint8_t processor;
+	uint8_t family;
+	uint8_t series;
+	const struct samd_part *parts;
+	size_t num_parts;
+};
+
+/* Known SAMD families */
+static const struct samd_family samd_families[] = {
+	{ SAMD_PROCESSOR_M4, SAMD_FAMILY_D, SAMD_SERIES_51,
+		samd51_parts, ARRAY_SIZE(samd51_parts) },
+	{ SAMD_PROCESSOR_M4, SAMD_FAMILY_E, SAME_SERIES_51,
+		same51_parts, ARRAY_SIZE(same51_parts) },
+	{ SAMD_PROCESSOR_M4, SAMD_FAMILY_E, SAME_SERIES_53,
+		same53_parts, ARRAY_SIZE(same53_parts) },
+	{ SAMD_PROCESSOR_M4, SAMD_FAMILY_E, SAME_SERIES_54,
+		same54_parts, ARRAY_SIZE(same54_parts) },
+};
+
+struct samd_info {
+	const struct samd_params *par;
+	uint32_t page_size;
+	int num_pages;
+	int sector_size;
+	int prot_block_size;
+
+	bool probed;
+	struct target *target;
+};
+
+
+/**
+ * Gives the family structure to specific device id.
+ * @param id The id of the device.
+ * @return On failure NULL, otherwise a pointer to the structure.
+ */
+static const struct samd_family *samd_find_family(uint32_t id)
+{
+	uint8_t processor = SAMD_GET_PROCESSOR(id);
+	uint8_t family = SAMD_GET_FAMILY(id);
+	uint8_t series = SAMD_GET_SERIES(id);
+
+	for (unsigned i = 0; i < ARRAY_SIZE(samd_families); i++) {
+		if (samd_families[i].processor == processor &&
+			samd_families[i].series == series &&
+			samd_families[i].family == family)
+			return &samd_families[i];
+	}
+
+	return NULL;
+}
+
+/**
+ * Gives the part structure to specific device id.
+ * @param id The id of the device.
+ * @return On failure NULL, otherwise a pointer to the structure.
+ */
+static const struct samd_part *samd_find_part(uint32_t id)
+{
+	uint8_t devsel = SAMD_GET_DEVSEL(id);
+	const struct samd_family *family = samd_find_family(id);
+	if (family == NULL)
+		return NULL;
+
+	for (unsigned i = 0; i < family->num_parts; i++) {
+		if (family->parts[i].id == devsel)
+			return &family->parts[i];
+	}
+
+	return NULL;
+}
+
+static int same5_protect_check(struct flash_bank *bank)
+{
+	int res, prot_block;
+	uint32_t lock;
+
+	res = target_read_u32(bank->target,
+			SAMD_NVMCTRL + SAME5_NVMCTRL_LOCK, &lock);
+	if (res != ERROR_OK)
+		return res;
+
+	/* Lock bits are active-low */
+	for (prot_block = 0; prot_block < bank->num_prot_blocks; prot_block++)
+		bank->prot_blocks[prot_block].is_protected = !(lock & (1u<<prot_block));
+
+	return ERROR_OK;
+}
+
+static int samd_get_flash_page_info(struct target *target,
+		uint32_t *sizep, int *nump)
+{
+	int res;
+	uint32_t param;
+
+	res = target_read_u32(target, SAMD_NVMCTRL + SAMD_NVMCTRL_PARAM, &param);
+	if (res == ERROR_OK) {
+		/* The PSZ field (bits 18:16) indicate the page size bytes as 2^(3+n)
+		 * so 0 is 8KB and 7 is 1024KB. */
+		if (sizep)
+			*sizep = (8 << ((param >> 16) & 0x7));
+		/* The NVMP field (bits 15:0) indicates the total number of pages */
+		if (nump)
+			*nump = param & 0xFFFF;
+	} else {
+		LOG_ERROR("Couldn't read NVM Parameters register");
+	}
+
+	return res;
+}
+
+static int same5_probe(struct flash_bank *bank)
+{
+	uint32_t id;
+	int res;
+	struct samd_info *chip = (struct samd_info *)bank->driver_priv;
+	const struct samd_part *part;
+
+	if (chip->probed)
+		return ERROR_OK;
+
+	res = target_read_u32(bank->target, SAMD_DSU + SAMD_DSU_DID, &id);
+	if (res != ERROR_OK) {
+		LOG_ERROR("Couldn't read Device ID register");
+		return res;
+	}
+
+	part = samd_find_part(id);
+	if (part == NULL) {
+		LOG_ERROR("Couldn't find part corresponding to DID %08" PRIx32, id);
+		return ERROR_FAIL;
+	}
+
+	bank->size = part->flash_kb * 1024;
+
+	res = samd_get_flash_page_info(bank->target, &chip->page_size,
+			&chip->num_pages);
+	if (res != ERROR_OK) {
+		LOG_ERROR("Couldn't determine Flash page size");
+		return res;
+	}
+
+	/* Sanity check: the total flash size in the DSU should match the page size
+	 * multiplied by the number of pages. */
+	if (bank->size != chip->num_pages * chip->page_size) {
+		LOG_WARNING("SAM: bank size doesn't match NVM parameters. "
+				"Identified %" PRIu32 "KB Flash but NVMCTRL reports %u %" PRIu32 "B pages",
+				part->flash_kb, chip->num_pages, chip->page_size);
+	}
+
+	/* Erase granularity = 1 block = 16 pages */
+	chip->sector_size = chip->page_size * SAME5_PAGES_PER_BLOCK;
+
+	/* Allocate the sector table */
+	bank->num_sectors = chip->num_pages / SAME5_PAGES_PER_BLOCK;
+	bank->sectors = alloc_block_array(0, chip->sector_size, bank->num_sectors);
+	if (!bank->sectors)
+		return ERROR_FAIL;
+
+	/* 16 protection blocks per device */
+	chip->prot_block_size = bank->size / SAME5_NUM_PROT_BLOCKS;
+
+	/* Allocate the table of protection blocks */
+	bank->num_prot_blocks = SAME5_NUM_PROT_BLOCKS;
+	bank->prot_blocks = alloc_block_array(0, chip->prot_block_size, bank->num_prot_blocks);
+	if (!bank->prot_blocks)
+		return ERROR_FAIL;
+
+	same5_protect_check(bank);
+
+	/* Done */
+	chip->probed = true;
+
+	LOG_INFO("SAM MCU: %s (%" PRIu32 "KB Flash, %" PRIu32 "KB RAM)", part->name,
+			part->flash_kb, part->ram_kb);
+
+	return ERROR_OK;
+}
+
+static int same5_wait_and_check_error(struct target *target)
+{
+	int ret, ret2;
+	int rep_cnt = 100;
+	uint16_t intflag;
+
+	do {
+		ret = target_read_u16(target,
+			SAMD_NVMCTRL + SAME5_NVMCTRL_INTFLAG, &intflag);
+		if (ret == ERROR_OK && intflag & SAME5_NVMCTRL_INTFLAG_DONE)
+			break;
+	} while (--rep_cnt);
+
+	if (ret != ERROR_OK) {
+		LOG_ERROR("Can't read NVM INTFLAG");
+		return ret;
+	}
+#if 0
+	if (intflag & SAME5_NVMCTRL_INTFLAG_ECCSE)
+		LOG_ERROR("SAM: ECC Single Error");
+
+	if (intflag & SAME5_NVMCTRL_INTFLAG_ECCDE) {
+		LOG_ERROR("SAM: ECC Double Error");
+		ret = ERROR_FLASH_OPERATION_FAILED;
+	}
+#endif
+	if (intflag & SAME5_NVMCTRL_INTFLAG_ADDRE) {
+		LOG_ERROR("SAM: Addr Error");
+		ret = ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	if (intflag & SAME5_NVMCTRL_INTFLAG_NVME) {
+		LOG_ERROR("SAM: NVM Error");
+		ret = ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	if (intflag & SAME5_NVMCTRL_INTFLAG_LOCKE) {
+		LOG_ERROR("SAM: NVM lock error");
+		ret = ERROR_FLASH_PROTECTED;
+	}
+
+	if (intflag & SAME5_NVMCTRL_INTFLAG_PROGE) {
+		LOG_ERROR("SAM: NVM programming error");
+		ret = ERROR_FLASH_OPER_UNSUPPORTED;
+	}
+
+	/* Clear the error conditions by writing a one to them */
+	ret2 = target_write_u16(target,
+			SAMD_NVMCTRL + SAME5_NVMCTRL_INTFLAG, intflag);
+	if (ret2 != ERROR_OK)
+		LOG_ERROR("Can't clear NVM error conditions");
+
+	return ret;
+}
+
+static int same5_issue_nvmctrl_command(struct target *target, uint16_t cmd)
+{
+	int res;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* Issue the NVM command */
+	/* 32-bit write is used to ensure atomic operation on ST-Link */
+	res = target_write_u32(target,
+			SAMD_NVMCTRL + SAME5_NVMCTRL_CTRLB, SAMD_NVM_CMD(cmd));
+	if (res != ERROR_OK)
+		return res;
+
+	/* Check to see if the NVM command resulted in an error condition. */
+	return same5_wait_and_check_error(target);
+}
+
+/**
+ * Erases a flash block or page at the given address.
+ * @param target Pointer to the target structure.
+ * @param address The address of the row.
+ * @return On success ERROR_OK, on failure an errorcode.
+ */
+static int same5_erase_block(struct target *target, uint32_t address)
+{
+	int res;
+
+	/* Set an address contained in the block to be erased */
+	res = target_write_u32(target,
+			SAMD_NVMCTRL + SAME5_NVMCTRL_ADDR, address);
+
+	/* Issue the Erase Block command. */
+	if (res == ERROR_OK)
+		res = same5_issue_nvmctrl_command(target,
+				address == SAMD_USER_ROW ? SAME5_NVM_CMD_EP : SAME5_NVM_CMD_EB);
+
+	if (res != ERROR_OK)  {
+		LOG_ERROR("Failed to erase block containing %08" PRIx32, address);
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
+
+static int same5_pre_write_check(struct target *target)
+{
+	int res;
+	uint32_t nvm_ctrla;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* Check if manual write mode is set */
+	res = target_read_u32(target, SAMD_NVMCTRL + SAME5_NVMCTRL_CTRLA, &nvm_ctrla);
+	if (res != ERROR_OK)
+		return res;
+
+	if (nvm_ctrla & SAME5_NVMCTRL_CTRLA_WMODE_MASK) {
+		LOG_ERROR("The flash controller must be in manual write mode. Issue 'reset init' and retry.");
+		return ERROR_FAIL;
+	}
+
+	return res;
+}
+
+
+/**
+ * Modify the contents of the User Row in Flash. The User Row itself
+ * has a size of one page and contains a combination of "fuses" and
+ * calibration data. Bits which have a value of zero in the mask will
+ * not be changed.
+ * @param target Pointer to the target structure.
+ * @param data Pointer to the value to write.
+ * @param mask Pointer to bitmask, 0 -> value stays untouched.
+ * @param offset Offset in user row where new data will be applied.
+ * @param count Size of buffer and mask in bytes.
+ * @return On success ERROR_OK, on failure an errorcode.
+ */
+static int same5_modify_user_row_masked(struct target *target,
+		const uint8_t *data, const uint8_t *mask,
+		uint32_t offset, uint32_t count)
+{
+	int res;
+
+	/* Retrieve the MCU's flash page size, in bytes. */
+	uint32_t page_size;
+	res = samd_get_flash_page_info(target, &page_size, NULL);
+	if (res != ERROR_OK) {
+		LOG_ERROR("Couldn't determine Flash page size");
+		return res;
+	}
+
+	/* Make sure the size is sane. */
+	assert(page_size <= SAMD_PAGE_SIZE_MAX &&
+		page_size >= offset + count);
+
+	uint8_t buf[SAMD_PAGE_SIZE_MAX];
+	/* Read the user row (comprising one page) by words. */
+	res = target_read_memory(target, SAMD_USER_ROW, 4, page_size / 4, buf);
+	if (res != ERROR_OK)
+		return res;
+
+	/* Modify buffer and check if really changed */
+	bool changed = false;
+	uint32_t i;
+	for (i = 0; i < count; i++) {
+		uint8_t old_b = buf[offset+i];
+		uint8_t new_b = (old_b & ~mask[i]) | (data[i] & mask[i]);
+		buf[offset+i] = new_b;
+		if (old_b != new_b)
+			changed = true;
+	}
+
+	if (!changed)
+		return ERROR_OK;
+
+	res = same5_pre_write_check(target);
+	if (res != ERROR_OK)
+		return res;
+
+	res = same5_erase_block(target, SAMD_USER_ROW);
+	if (res != ERROR_OK) {
+		LOG_ERROR("Couldn't erase user row");
+		return res;
+	}
+
+	/* Write the page buffer back out to the target using Write Quad Word */
+	for (i = 0; i < page_size; i += 4 * 4) {
+		res = target_write_memory(target, SAMD_USER_ROW + i, 4, 4, buf + i);
+		if (res != ERROR_OK)
+			return res;
+
+		/* Trigger flash write */
+		res = same5_issue_nvmctrl_command(target, SAME5_NVM_CMD_WQW);
+		if (res != ERROR_OK)
+			return res;
+	}
+
+	return res;
+}
+
+/**
+ * Modifies the user row register to the given value.
+ * @param target Pointer to the target structure.
+ * @param value The value to write.
+ * @param startb The bit-offset by which the given value is shifted.
+ * @param endb The bit-offset of the last bit in value to write.
+ * @return On success ERROR_OK, on failure an errorcode.
+ */
+static int same5_modify_user_row(struct target *target, uint32_t value,
+		uint8_t startb, uint8_t endb)
+{
+	uint8_t buf_val[8] = { 0 };
+	uint8_t buf_mask[8] = { 0 };
+
+	assert(startb <= endb && endb < 64);
+	buf_set_u32(buf_val, startb, endb + 1 - startb, value);
+	buf_set_u32(buf_mask, startb, endb + 1 - startb, 0xffffffff);
+
+	return same5_modify_user_row_masked(target,
+			buf_val, buf_mask, 0, 8);
+}
+
+static int same5_protect(struct flash_bank *bank, int set, int first_prot_bl, int last_prot_bl)
+{
+	int res = ERROR_OK;
+	int prot_block;
+
+	/* We can issue lock/unlock region commands with the target running but
+	 * the settings won't persist unless we're able to modify the LOCK regions
+	 * and that requires the target to be halted. */
+	if (bank->target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	for (prot_block = first_prot_bl; prot_block <= last_prot_bl; prot_block++) {
+		if (set != bank->prot_blocks[prot_block].is_protected) {
+			/* Load an address that is within this protection block (we use offset 0) */
+			res = target_write_u32(bank->target,
+					SAMD_NVMCTRL + SAME5_NVMCTRL_ADDR,
+					bank->prot_blocks[prot_block].offset);
+			if (res != ERROR_OK)
+				goto exit;
+
+			/* Tell the controller to lock that block */
+			res = same5_issue_nvmctrl_command(bank->target,
+					set ? SAME5_NVM_CMD_LR : SAME5_NVM_CMD_UR);
+			if (res != ERROR_OK)
+				goto exit;
+		}
+	}
+
+	/* We've now applied our changes, however they will be undone by the next
+	 * reset unless we also apply them to the LOCK bits in the User Page.
+	 * A '1' means unlocked and a '0' means locked. */
+	const uint8_t lock[4] = { 0, 0, 0, 0 };
+	const uint8_t unlock[4] = { 0xff, 0xff, 0xff, 0xff };
+	uint8_t mask[4] = { 0, 0, 0, 0 };
+
+	buf_set_u32(mask, first_prot_bl, last_prot_bl + 1 - first_prot_bl, 0xffffffff);
+
+	res = same5_modify_user_row_masked(bank->target,
+			set ? lock : unlock, mask, 8, 4);
+	if (res != ERROR_OK)
+		LOG_WARNING("SAM: protect settings were not made persistent!");
+
+	res = ERROR_OK;
+
+exit:
+	same5_protect_check(bank);
+
+	return res;
+}
+
+static int same5_erase(struct flash_bank *bank, int first_sect, int last_sect)
+{
+	int res, s;
+	struct samd_info *chip = (struct samd_info *)bank->driver_priv;
+
+	if (bank->target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (!chip->probed)
+		return ERROR_FLASH_BANK_NOT_PROBED;
+
+	/* For each sector to be erased */
+	for (s = first_sect; s <= last_sect; s++) {
+		res = same5_erase_block(bank->target, bank->sectors[s].offset);
+		if (res != ERROR_OK) {
+			LOG_ERROR("SAM: failed to erase sector %d at 0x%08" PRIx32, s, bank->sectors[s].offset);
+			return res;
+		}
+	}
+
+	return ERROR_OK;
+}
+
+
+static int same5_write(struct flash_bank *bank, const uint8_t *buffer,
+		uint32_t offset, uint32_t count)
+{
+	int res;
+	uint32_t address;
+	uint32_t pg_offset;
+	uint32_t nb;
+	uint32_t nw;
+	struct samd_info *chip = (struct samd_info *)bank->driver_priv;
+	uint8_t *pb = NULL;
+
+	res = same5_pre_write_check(bank->target);
+	if (res != ERROR_OK)
+		return res;
+
+	if (!chip->probed)
+		return ERROR_FLASH_BANK_NOT_PROBED;
+
+	res = same5_issue_nvmctrl_command(bank->target, SAME5_NVM_CMD_PBC);
+	if (res != ERROR_OK) {
+		LOG_ERROR("%s: %d", __func__, __LINE__);
+		return res;
+	}
+
+	while (count) {
+		nb = chip->page_size - offset % chip->page_size;
+		if (count < nb)
+			nb = count;
+
+		address = bank->base + offset;
+		pg_offset = offset % chip->page_size;
+
+		if (offset % 4 || (offset + nb) % 4) {
+			/* Either start or end of write is not word aligned */
+			if (!pb) {
+				pb = malloc(chip->page_size);
+				if (!pb)
+					return ERROR_FAIL;
+			}
+
+			/* Set temporary page buffer to 0xff and overwrite the relevant part */
+			memset(pb, 0xff, chip->page_size);
+			memcpy(pb + pg_offset, buffer, nb);
+
+			/* Align start address to a word boundary */
+			address -= offset % 4;
+			pg_offset -= offset % 4;
+			assert(pg_offset % 4 == 0);
+
+			/* Extend length to whole words */
+			nw = (nb + offset % 4 + 3) / 4;
+			assert(pg_offset + 4 * nw <= chip->page_size);
+
+			/* Now we have original data extended by 0xff bytes
+			 * to the nearest word boundary on both start and end */
+			res = target_write_memory(bank->target, address, 4, nw, pb + pg_offset);
+		} else {
+			assert(nb % 4 == 0);
+			nw = nb / 4;
+			assert(pg_offset + 4 * nw <= chip->page_size);
+
+			/* Word aligned data, use direct write from buffer */
+			res = target_write_memory(bank->target, address, 4, nw, buffer);
+		}
+		if (res != ERROR_OK) {
+			LOG_ERROR("%s: %d", __func__, __LINE__);
+			goto free_pb;
+		}
+
+		res = same5_issue_nvmctrl_command(bank->target, SAME5_NVM_CMD_WP);
+		if (res != ERROR_OK) {
+			LOG_ERROR("%s: write failed at address 0x%08" PRIx32, __func__, address);
+			goto free_pb;
+		}
+
+		/* We're done with the page contents */
+		count -= nb;
+		offset += nb;
+		buffer += nb;
+	}
+
+free_pb:
+	if (pb)
+		free(pb);
+
+	return res;
+}
+
+
+FLASH_BANK_COMMAND_HANDLER(same5_flash_bank_command)
+{
+	if (bank->base != SAMD_FLASH) {
+		LOG_ERROR("Address 0x%08" TARGET_PRIxADDR " invalid bank address (try "
+			"0x%08" PRIx32 "[same5] )", bank->base, SAMD_FLASH);
+		return ERROR_FAIL;
+	}
+
+	struct samd_info *chip;
+	chip = calloc(1, sizeof(*chip));
+	if (!chip) {
+		LOG_ERROR("No memory for flash bank chip info");
+		return ERROR_FAIL;
+	}
+
+	chip->target = bank->target;
+	chip->probed = false;
+
+	bank->driver_priv = chip;
+
+	return ERROR_OK;
+}
+
+
+COMMAND_HANDLER(same5_handle_chip_erase_command)
+{
+	struct target *target = get_current_target(CMD_CTX);
+	if (!target)
+		return ERROR_FAIL;
+
+	/* Enable access to the DSU by disabling the write protect bit */
+	target_write_u32(target, SAME5_PAC, (1<<16) | (1<<5) | (1<<1));
+	/* intentionally without error checking - not accessible on secured chip */
+
+	/* Tell the DSU to perform a full chip erase.  It takes about 240ms to
+	 * perform the erase. */
+	int res = target_write_u8(target, SAMD_DSU + SAMD_DSU_CTRL_EXT, (1<<4));
+	if (res == ERROR_OK)
+		command_print(CMD_CTX, "chip erase started");
+	else
+		command_print(CMD_CTX, "write to DSU CTRL failed");
+
+	return res;
+}
+
+
+COMMAND_HANDLER(same5_handle_userpage_command)
+{
+	int res = ERROR_OK;
+	struct target *target = get_current_target(CMD_CTX);
+	if (!target)
+		return ERROR_FAIL;
+
+	if (CMD_ARGC > 2) {
+		command_print(CMD_CTX, "Too much Arguments given.");
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	if (CMD_ARGC >= 1) {
+		uint64_t mask = NVMUSERROW_SAM_E5_D5_MASK;
+		uint64_t value = strtoull(CMD_ARGV[0], NULL, 0);
+
+		if (CMD_ARGC == 2) {
+			uint64_t mask_temp = strtoull(CMD_ARGV[1], NULL, 0);
+			mask &= mask_temp;
+		}
+
+		uint8_t val_buf[8], mask_buf[8];
+		target_buffer_set_u64(target, val_buf, value);
+		target_buffer_set_u64(target, mask_buf, mask);
+
+		res = same5_modify_user_row_masked(target,
+				val_buf, mask_buf, 0, sizeof(val_buf));
+	}
+
+	uint8_t buffer[8];
+	int res2 = target_read_memory(target, SAMD_USER_ROW, 4, 2, buffer);
+	if (res2 == ERROR_OK) {
+		uint64_t value = target_buffer_get_u64(target, buffer);
+		command_print(CMD_CTX, "USER PAGE: 0x%016"PRIX64, value);
+	} else {
+		LOG_ERROR("USER PAGE could not be read.");
+	}
+
+	if (CMD_ARGC >= 1)
+		return res;
+	else
+		return res2;
+}
+
+
+COMMAND_HANDLER(same5_handle_bootloader_command)
+{
+	int res = ERROR_OK;
+	struct target *target = get_current_target(CMD_CTX);
+	if (!target)
+		return ERROR_FAIL;
+
+	if (CMD_ARGC >= 1) {
+		unsigned long size = strtoul(CMD_ARGV[0], NULL, 0);
+		uint32_t code = (size + 8191) / 8192;
+		if (code > 15) {
+			command_print(CMD_CTX, "Invalid bootloader size.  Please "
+						"see datasheet for a list valid sizes.");
+			return ERROR_COMMAND_SYNTAX_ERROR;
+		}
+
+		res = same5_modify_user_row(target, 15 - code, 26, 29);
+	}
+
+	uint32_t val;
+	int res2 = target_read_u32(target, SAMD_USER_ROW, &val);
+	if (res2 == ERROR_OK) {
+		uint32_t code = (val >> 26) & 0xf; /* grab size code */
+		uint32_t size = (15 - code) * 8192;
+		command_print(CMD_CTX, "Bootloader protected in the first %"
+				      PRIu32 " bytes", size);
+	}
+
+	if (CMD_ARGC >= 1)
+		return res;
+	else
+		return res2;
+}
+
+
+COMMAND_HANDLER(samd_handle_reset_deassert)
+{
+	struct target *target = get_current_target(CMD_CTX);
+	int res = ERROR_OK;
+	enum reset_types jtag_reset_config = jtag_get_reset_config();
+	if (!target)
+		return ERROR_FAIL;
+
+	/* If the target has been unresponsive before, try to re-establish
+	 * communication now - CPU is held in reset by DSU, DAP is working */
+	if (!target_was_examined(target))
+		target_examine_one(target);
+	target_poll(target);
+
+	/* In case of sysresetreq, debug retains state set in cortex_m_assert_reset()
+	 * so we just release reset held by DSU
+	 *
+	 * n_RESET (srst) clears the DP, so reenable debug and set vector catch here
+	 *
+	 * After vectreset DSU release is not needed however makes no harm
+	 */
+	if (target->reset_halt && (jtag_reset_config & RESET_HAS_SRST)) {
+		res = target_write_u32(target, DCB_DHCSR, DBGKEY | C_HALT | C_DEBUGEN);
+		if (res == ERROR_OK)
+			res = target_write_u32(target, DCB_DEMCR,
+				TRCENA | VC_HARDERR | VC_BUSERR | VC_CORERESET);
+		/* do not return on error here, releasing DSU reset is more important */
+	}
+
+	/* clear CPU Reset Phase Extension bit */
+	int res2 = target_write_u8(target, SAMD_DSU + SAMD_DSU_STATUSA, (1<<1));
+	if (res2 != ERROR_OK)
+		return res2;
+
+	return res;
+}
+
+static const struct command_registration same5_exec_command_handlers[] = {
+	{
+		.name = "dsu_reset_deassert",
+		.handler = samd_handle_reset_deassert,
+		.mode = COMMAND_EXEC,
+		.help = "Deasert internal reset held by DSU."
+	},
+	{
+		.name = "chip-erase",
+		.handler = same5_handle_chip_erase_command,
+		.mode = COMMAND_EXEC,
+		.help = "Erase the entire Flash by using the Chip-"
+			"Erase feature in the Device Service Unit (DSU).",
+	},
+	{
+		.name = "bootloader",
+		.usage = "[size_in_bytes]",
+		.handler = same5_handle_bootloader_command,
+		.mode = COMMAND_EXEC,
+		.help = "Show or set the bootloader protection size, stored in the User Row. "
+			"Changes are stored immediately but take affect after the MCU is "
+			"reset.",
+	},
+	{
+		.name = "userpage",
+		.usage = "[value] [mask]",
+		.handler = same5_handle_userpage_command,
+		.mode = COMMAND_EXEC,
+		.help = "Show or set the first 64-bit part of user page "
+			"located at address 0x804000. Use the optional mask argument "
+			"to prevent changes at positions where the bitvalue is zero. "
+			"For security reasons the reserved-bits are masked out "
+			"in background and therefore cannot be changed.",
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+static const struct command_registration same5_command_handlers[] = {
+	{
+		.name = "atsame5",
+		.mode = COMMAND_ANY,
+		.help = "atsame5 flash command group",
+		.usage = "",
+		.chain = same5_exec_command_handlers,
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+struct flash_driver atsame5_flash = {
+	.name = "atsame5",
+	.commands = same5_command_handlers,
+	.flash_bank_command = same5_flash_bank_command,
+	.erase = same5_erase,
+	.protect = same5_protect,
+	.write = same5_write,
+	.read = default_flash_read,
+	.probe = same5_probe,
+	.auto_probe = same5_probe,
+	.erase_check = default_flash_blank_check,
+	.protect_check = same5_protect_check,
+	.free_driver_priv = default_flash_free_driver_priv,
+};
diff --git a/src/flash/nor/avrf.c b/src/flash/nor/avrf.c
index 65ac601..b88f6f6 100644
--- a/src/flash/nor/avrf.c
+++ b/src/flash/nor/avrf.c
@@ -233,12 +233,6 @@ static int avrf_erase(struct flash_bank *bank, int first, int last)
 	return avr_jtagprg_leaveprogmode(avr);
 }
 
-static int avrf_protect(struct flash_bank *bank, int set, int first, int last)
-{
-	LOG_INFO("%s", __func__);
-	return ERROR_OK;
-}
-
 static int avrf_write(struct flash_bank *bank, const uint8_t *buffer, uint32_t offset, uint32_t count)
 {
 	struct target *target = bank->target;
@@ -338,7 +332,7 @@ static int avrf_probe(struct flash_bank *bank)
 			bank->sectors[i].offset = i * avr_info->flash_page_size;
 			bank->sectors[i].size = avr_info->flash_page_size;
 			bank->sectors[i].is_erased = -1;
-			bank->sectors[i].is_protected = 1;
+			bank->sectors[i].is_protected = -1;
 		}
 
 		avrf_info->probed = 1;
@@ -360,12 +354,6 @@ static int avrf_auto_probe(struct flash_bank *bank)
 	return avrf_probe(bank);
 }
 
-static int avrf_protect_check(struct flash_bank *bank)
-{
-	LOG_INFO("%s", __func__);
-	return ERROR_OK;
-}
-
 static int avrf_info(struct flash_bank *bank, char *buf, int buf_size)
 {
 	struct target *target = bank->target;
@@ -479,13 +467,11 @@ struct flash_driver avr_flash = {
 	.commands = avrf_command_handlers,
 	.flash_bank_command = avrf_flash_bank_command,
 	.erase = avrf_erase,
-	.protect = avrf_protect,
 	.write = avrf_write,
 	.read = default_flash_read,
 	.probe = avrf_probe,
 	.auto_probe = avrf_auto_probe,
 	.erase_check = default_flash_blank_check,
-	.protect_check = avrf_protect_check,
 	.info = avrf_info,
 	.free_driver_priv = default_flash_free_driver_priv,
 };
diff --git a/src/flash/nor/cc26xx.c b/src/flash/nor/cc26xx.c
index e6e9e59..0320e92 100644
--- a/src/flash/nor/cc26xx.c
+++ b/src/flash/nor/cc26xx.c
@@ -312,12 +312,6 @@ static int cc26xx_erase(struct flash_bank *bank, int first, int last)
 	return retval;
 }
 
-static int cc26xx_protect(struct flash_bank *bank, int set, int first,
-	int last)
-{
-	return ERROR_OK;
-}
-
 static int cc26xx_write(struct flash_bank *bank, const uint8_t *buffer,
 	uint32_t offset, uint32_t count)
 {
@@ -497,22 +491,12 @@ static int cc26xx_auto_probe(struct flash_bank *bank)
 
 	int retval = ERROR_OK;
 
-	if (bank->bank_number != 0) {
-		/* Invalid bank number somehow */
-		return ERROR_FAIL;
-	}
-
 	if (!cc26xx_bank->probed)
 		retval = cc26xx_probe(bank);
 
 	return retval;
 }
 
-static int cc26xx_protect_check(struct flash_bank *bank)
-{
-	return ERROR_OK;
-}
-
 static int cc26xx_info(struct flash_bank *bank, char *buf, int buf_size)
 {
 	struct cc26xx_bank *cc26xx_bank = bank->driver_priv;
@@ -555,13 +539,11 @@ struct flash_driver cc26xx_flash = {
 	.name = "cc26xx",
 	.flash_bank_command = cc26xx_flash_bank_command,
 	.erase = cc26xx_erase,
-	.protect = cc26xx_protect,
 	.write = cc26xx_write,
 	.read = default_flash_read,
 	.probe = cc26xx_probe,
 	.auto_probe = cc26xx_auto_probe,
 	.erase_check = default_flash_blank_check,
-	.protect_check = cc26xx_protect_check,
 	.info = cc26xx_info,
 	.free_driver_priv = default_flash_free_driver_priv,
 };
diff --git a/src/flash/nor/cc3220sf.c b/src/flash/nor/cc3220sf.c
index af45743..2441124 100644
--- a/src/flash/nor/cc3220sf.c
+++ b/src/flash/nor/cc3220sf.c
@@ -173,12 +173,6 @@ static int cc3220sf_erase(struct flash_bank *bank, int first, int last)
 	return retval;
 }
 
-static int cc3220sf_protect(struct flash_bank *bank, int set, int first,
-	int last)
-{
-	return ERROR_OK;
-}
-
 static int cc3220sf_write(struct flash_bank *bank, const uint8_t *buffer,
 	uint32_t offset, uint32_t count)
 {
@@ -436,18 +430,10 @@ static int cc3220sf_probe(struct flash_bank *bank)
 	uint32_t base;
 	uint32_t size;
 	int num_sectors;
-	int bank_id;
 
-	bank_id = bank->bank_number;
-
-	if (0 == bank_id) {
-		base = FLASH_BASE_ADDR;
-		size = FLASH_NUM_SECTORS * FLASH_SECTOR_SIZE;
-		num_sectors = FLASH_NUM_SECTORS;
-	} else {
-		/* Invalid bank number somehow */
-		return ERROR_FAIL;
-	}
+	base = FLASH_BASE_ADDR;
+	size = FLASH_NUM_SECTORS * FLASH_SECTOR_SIZE;
+	num_sectors = FLASH_NUM_SECTORS;
 
 	if (NULL != bank->sectors) {
 		free(bank->sectors);
@@ -485,22 +471,12 @@ static int cc3220sf_auto_probe(struct flash_bank *bank)
 
 	int retval = ERROR_OK;
 
-	if (0 != bank->bank_number) {
-		/* Invalid bank number somehow */
-		return ERROR_FAIL;
-	}
-
 	if (!cc3220sf_bank->probed)
 		retval = cc3220sf_probe(bank);
 
 	return retval;
 }
 
-static int cc3220sf_protect_check(struct flash_bank *bank)
-{
-	return ERROR_OK;
-}
-
 static int cc3220sf_info(struct flash_bank *bank, char *buf, int buf_size)
 {
 	int printed;
@@ -517,13 +493,11 @@ struct flash_driver cc3220sf_flash = {
 	.name = "cc3220sf",
 	.flash_bank_command = cc3220sf_flash_bank_command,
 	.erase = cc3220sf_erase,
-	.protect = cc3220sf_protect,
 	.write = cc3220sf_write,
 	.read = default_flash_read,
 	.probe = cc3220sf_probe,
 	.auto_probe = cc3220sf_auto_probe,
 	.erase_check = default_flash_blank_check,
-	.protect_check = cc3220sf_protect_check,
 	.info = cc3220sf_info,
 	.free_driver_priv = default_flash_free_driver_priv,
 };
diff --git a/src/flash/nor/cfi.c b/src/flash/nor/cfi.c
index 8bf17ac..8257db7 100644
--- a/src/flash/nor/cfi.c
+++ b/src/flash/nor/cfi.c
@@ -1098,11 +1098,6 @@ static int cfi_protect(struct flash_bank *bank, int set, int first, int last)
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
-	if ((first < 0) || (last < first) || (last >= bank->num_sectors)) {
-		LOG_ERROR("Invalid sector range");
-		return ERROR_FLASH_SECTOR_INVALID;
-	}
-
 	if (cfi_info->qry[0] != 'Q')
 		return ERROR_FLASH_BANK_NOT_PROBED;
 
diff --git a/src/flash/nor/core.c b/src/flash/nor/core.c
index 8f8a9ce..d26894b 100644
--- a/src/flash/nor/core.c
+++ b/src/flash/nor/core.c
@@ -68,6 +68,11 @@ int flash_driver_protect(struct flash_bank *bank, int set, int first, int last)
 	/* force "set" to 0/1 */
 	set = !!set;
 
+	if (bank->driver->protect == NULL) {
+		LOG_ERROR("Flash protection is not supported.");
+		return ERROR_FLASH_OPER_UNSUPPORTED;
+	}
+
 	/* DANGER!
 	 *
 	 * We must not use any cached information about protection state!!!!
@@ -319,8 +324,8 @@ static int default_flash_mem_blank_check(struct flash_bank *bank)
 		for (j = 0; j < bank->sectors[i].size; j += buffer_size) {
 			uint32_t chunk;
 			chunk = buffer_size;
-			if (chunk > (j - bank->sectors[i].size))
-				chunk = (j - bank->sectors[i].size);
+			if (chunk > (bank->sectors[i].size - j))
+				chunk = (bank->sectors[i].size - j);
 
 			retval = target_read_memory(target,
 					bank->base + bank->sectors[i].offset + j,
diff --git a/src/flash/nor/driver.h b/src/flash/nor/driver.h
index e7b3234..ef69a0f 100644
--- a/src/flash/nor/driver.h
+++ b/src/flash/nor/driver.h
@@ -109,6 +109,8 @@ struct flash_driver {
 	/**
 	 * Bank/sector protection routine (target-specific).
 	 *
+	 * If protection is not implemented, set method to NULL
+	 *
 	 * When called, the driver should enable/disable protection
 	 * for MINIMUM the range covered by first..last sectors
 	 * inclusive. Some chips have alignment requirements will
@@ -178,6 +180,8 @@ struct flash_driver {
 	 * flash_sector_s::is_protected field for each of the flash
 	 * bank's sectors.
 	 *
+	 * If protection is not implemented, set method to NULL
+	 *
 	 * @param bank - the bank to check
 	 * @returns ERROR_OK if successful; otherwise, an error code.
 	 */
diff --git a/src/flash/nor/drivers.c b/src/flash/nor/drivers.c
index 62db3fe..31caa5a 100644
--- a/src/flash/nor/drivers.c
+++ b/src/flash/nor/drivers.c
@@ -29,6 +29,7 @@ extern struct flash_driver at91sam4l_flash;
 extern struct flash_driver at91sam7_flash;
 extern struct flash_driver at91samd_flash;
 extern struct flash_driver ath79_flash;
+extern struct flash_driver atsame5_flash;
 extern struct flash_driver atsamv_flash;
 extern struct flash_driver avr_flash;
 extern struct flash_driver bluenrgx_flash;
@@ -78,6 +79,7 @@ extern struct flash_driver str9x_flash;
 extern struct flash_driver str9xpec_flash;
 extern struct flash_driver tms470_flash;
 extern struct flash_driver virtual_flash;
+extern struct flash_driver w600_flash;
 extern struct flash_driver xcf_flash;
 extern struct flash_driver xmc1xxx_flash;
 extern struct flash_driver xmc4xxx_flash;
@@ -96,6 +98,7 @@ static struct flash_driver *flash_drivers[] = {
 	&at91sam7_flash,
 	&at91samd_flash,
 	&ath79_flash,
+	&atsame5_flash,
 	&atsamv_flash,
 	&avr_flash,
 	&bluenrgx_flash,
@@ -148,6 +151,7 @@ static struct flash_driver *flash_drivers[] = {
 	&xcf_flash,
 	&xmc1xxx_flash,
 	&xmc4xxx_flash,
+	&w600_flash,
 	NULL,
 };
 
diff --git a/src/flash/nor/efm32.c b/src/flash/nor/efm32.c
index 1d70bd5..8ff689c 100644
--- a/src/flash/nor/efm32.c
+++ b/src/flash/nor/efm32.c
@@ -429,7 +429,7 @@ static int efm32x_erase_page(struct flash_bank *bank, uint32_t addr)
 	 */
 	int ret = 0;
 	uint32_t status = 0;
-
+	addr += bank->base;
 	LOG_DEBUG("erasing flash page at 0x%08" PRIx32, addr);
 
 	ret = efm32x_write_reg_u32(bank, EFM32_MSC_REG_ADDRB, addr);
diff --git a/src/flash/nor/esirisc_flash.c b/src/flash/nor/esirisc_flash.c
index f3833df..9e11571 100644
--- a/src/flash/nor/esirisc_flash.c
+++ b/src/flash/nor/esirisc_flash.c
@@ -104,9 +104,12 @@ struct esirisc_flash_bank {
 	uint32_t wait_states;
 };
 
+static const struct command_registration esirisc_flash_command_handlers[];
+
 FLASH_BANK_COMMAND_HANDLER(esirisc_flash_bank_command)
 {
 	struct esirisc_flash_bank *esirisc_info;
+	struct command *esirisc_cmd;
 
 	if (CMD_ARGC < 9)
 		return ERROR_COMMAND_SYNTAX_ERROR;
@@ -119,6 +122,10 @@ FLASH_BANK_COMMAND_HANDLER(esirisc_flash_bank_command)
 
 	bank->driver_priv = esirisc_info;
 
+	/* register commands using existing esirisc context */
+	esirisc_cmd = command_find_in_context(CMD_CTX, "esirisc");
+	register_commands(CMD_CTX, esirisc_cmd, esirisc_flash_command_handlers);
+
 	return ERROR_OK;
 }
 
@@ -149,7 +156,7 @@ static int esirisc_flash_disable_protect(struct flash_bank *bank)
 	if (!(control & CONTROL_WP))
 		return ERROR_OK;
 
-	esirisc_flash_unlock(bank);
+	(void)esirisc_flash_unlock(bank);
 
 	control &= ~CONTROL_WP;
 
@@ -168,7 +175,7 @@ static int esirisc_flash_enable_protect(struct flash_bank *bank)
 	if (control & CONTROL_WP)
 		return ERROR_OK;
 
-	esirisc_flash_unlock(bank);
+	(void)esirisc_flash_unlock(bank);
 
 	control |= CONTROL_WP;
 
@@ -254,7 +261,7 @@ static int esirisc_flash_erase(struct flash_bank *bank, int first, int last)
 	if (target->state != TARGET_HALTED)
 		return ERROR_TARGET_NOT_HALTED;
 
-	esirisc_flash_disable_protect(bank);
+	(void)esirisc_flash_disable_protect(bank);
 
 	for (int page = first; page < last; ++page) {
 		uint32_t address = page * PAGE_SIZE;
@@ -268,7 +275,7 @@ static int esirisc_flash_erase(struct flash_bank *bank, int first, int last)
 		}
 	}
 
-	esirisc_flash_enable_protect(bank);
+	(void)esirisc_flash_enable_protect(bank);
 
 	return retval;
 }
@@ -282,7 +289,7 @@ static int esirisc_flash_mass_erase(struct flash_bank *bank)
 	if (target->state != TARGET_HALTED)
 		return ERROR_TARGET_NOT_HALTED;
 
-	esirisc_flash_disable_protect(bank);
+	(void)esirisc_flash_disable_protect(bank);
 
 	target_write_u32(target, esirisc_info->cfg + ADDRESS, 0);
 
@@ -290,7 +297,7 @@ static int esirisc_flash_mass_erase(struct flash_bank *bank)
 	if (retval != ERROR_OK)
 		LOG_ERROR("%s: failed to mass erase", bank->name);
 
-	esirisc_flash_enable_protect(bank);
+	(void)esirisc_flash_enable_protect(bank);
 
 	return retval;
 }
@@ -308,32 +315,17 @@ static int esirisc_flash_ref_erase(struct flash_bank *bank)
 	if (target->state != TARGET_HALTED)
 		return ERROR_TARGET_NOT_HALTED;
 
-	esirisc_flash_disable_protect(bank);
+	(void)esirisc_flash_disable_protect(bank);
 
 	retval = esirisc_flash_control(bank, CONTROL_ERC);
 	if (retval != ERROR_OK)
 		LOG_ERROR("%s: failed to erase reference cell", bank->name);
 
-	esirisc_flash_enable_protect(bank);
+	(void)esirisc_flash_enable_protect(bank);
 
 	return retval;
 }
 
-static int esirisc_flash_protect(struct flash_bank *bank, int set, int first, int last)
-{
-	struct target *target = bank->target;
-
-	if (target->state != TARGET_HALTED)
-		return ERROR_TARGET_NOT_HALTED;
-
-	if (set)
-		esirisc_flash_enable_protect(bank);
-	else
-		esirisc_flash_disable_protect(bank);
-
-	return ERROR_OK;
-}
-
 static int esirisc_flash_fill_pb(struct flash_bank *bank,
 		const uint8_t *buffer, uint32_t count)
 {
@@ -375,7 +367,7 @@ static int esirisc_flash_write(struct flash_bank *bank,
 	if (target->state != TARGET_HALTED)
 		return ERROR_TARGET_NOT_HALTED;
 
-	esirisc_flash_disable_protect(bank);
+	(void)esirisc_flash_disable_protect(bank);
 
 	/*
 	 * The address register is auto-incremented based on the contents of
@@ -406,7 +398,7 @@ static int esirisc_flash_write(struct flash_bank *bank,
 		count -= num_bytes;
 	}
 
-	esirisc_flash_enable_protect(bank);
+	(void)esirisc_flash_enable_protect(bank);
 
 	return retval;
 }
@@ -432,11 +424,11 @@ static int esirisc_flash_init(struct flash_bank *bank)
 	uint32_t value;
 	int retval;
 
-	esirisc_flash_disable_protect(bank);
+	(void)esirisc_flash_disable_protect(bank);
 
 	/* initialize timing registers */
-	value = TIMING0_F(esirisc_flash_num_cycles(bank, TNVH)) |
-			TIMING0_R(esirisc_info->wait_states);
+	value = TIMING0_F(esirisc_flash_num_cycles(bank, TNVH))
+			| TIMING0_R(esirisc_info->wait_states);
 
 	LOG_DEBUG("TIMING0: 0x%" PRIx32, value);
 	target_write_u32(target, esirisc_info->cfg + TIMING0, value);
@@ -446,9 +438,9 @@ static int esirisc_flash_init(struct flash_bank *bank)
 	LOG_DEBUG("TIMING1: 0x%" PRIx32, value);
 	target_write_u32(target, esirisc_info->cfg + TIMING1, value);
 
-	value = TIMING2_T(esirisc_flash_num_cycles(bank, 10))   |
-			TIMING2_H(esirisc_flash_num_cycles(bank, 100))  |
-			TIMING2_P(esirisc_flash_num_cycles(bank, TPROG));
+	value = TIMING2_T(esirisc_flash_num_cycles(bank, 10))
+			| TIMING2_H(esirisc_flash_num_cycles(bank, 100))
+			| TIMING2_P(esirisc_flash_num_cycles(bank, TPROG));
 
 	LOG_DEBUG("TIMING2: 0x%" PRIx32, value);
 	target_write_u32(target, esirisc_info->cfg + TIMING2, value);
@@ -458,7 +450,7 @@ static int esirisc_flash_init(struct flash_bank *bank)
 	if (retval != ERROR_OK)
 		LOG_ERROR("%s: failed to recall trim code", bank->name);
 
-	esirisc_flash_enable_protect(bank);
+	(void)esirisc_flash_enable_protect(bank);
 
 	return retval;
 }
@@ -475,13 +467,6 @@ static int esirisc_flash_probe(struct flash_bank *bank)
 	bank->num_sectors = bank->size / PAGE_SIZE;
 	bank->sectors = alloc_block_array(0, PAGE_SIZE, bank->num_sectors);
 
-	/*
-	 * Register write protection is enforced using a single protection
-	 * block for the entire bank. This is as good as it gets.
-	 */
-	bank->num_prot_blocks = 1;
-	bank->prot_blocks = alloc_block_array(0, bank->size, bank->num_prot_blocks);
-
 	retval = esirisc_flash_init(bank);
 	if (retval != ERROR_OK) {
 		LOG_ERROR("%s: failed to initialize bank", bank->name);
@@ -503,23 +488,6 @@ static int esirisc_flash_auto_probe(struct flash_bank *bank)
 	return esirisc_flash_probe(bank);
 }
 
-static int esirisc_flash_protect_check(struct flash_bank *bank)
-{
-	struct esirisc_flash_bank *esirisc_info = bank->driver_priv;
-	struct target *target = bank->target;
-	uint32_t control;
-
-	if (target->state != TARGET_HALTED)
-		return ERROR_TARGET_NOT_HALTED;
-
-	target_read_u32(target, esirisc_info->cfg + CONTROL, &control);
-
-	/* single protection block (also see: esirisc_flash_probe()) */
-	bank->prot_blocks[0].is_protected = !!(control & CONTROL_WP);
-
-	return ERROR_OK;
-}
-
 static int esirisc_flash_info(struct flash_bank *bank, char *buf, int buf_size)
 {
 	struct esirisc_flash_bank *esirisc_info = bank->driver_priv;
@@ -579,14 +547,14 @@ static const struct command_registration esirisc_flash_exec_command_handlers[] =
 		.name = "mass_erase",
 		.handler = handle_esirisc_flash_mass_erase_command,
 		.mode = COMMAND_EXEC,
-		.help = "erases all pages in data memory",
+		.help = "erase all pages in data memory",
 		.usage = "bank_id",
 	},
 	{
 		.name = "ref_erase",
 		.handler = handle_esirisc_flash_ref_erase_command,
 		.mode = COMMAND_EXEC,
-		.help = "erases reference cell (uncommon)",
+		.help = "erase reference cell (uncommon)",
 		.usage = "bank_id",
 	},
 	COMMAND_REGISTRATION_DONE
@@ -594,9 +562,9 @@ static const struct command_registration esirisc_flash_exec_command_handlers[] =
 
 static const struct command_registration esirisc_flash_command_handlers[] = {
 	{
-		.name = "esirisc_flash",
-		.mode = COMMAND_ANY,
-		.help = "eSi-RISC flash command group",
+		.name = "flash",
+		.mode = COMMAND_EXEC,
+		.help = "eSi-TSMC Flash command group",
 		.usage = "",
 		.chain = esirisc_flash_exec_command_handlers,
 	},
@@ -605,17 +573,15 @@ static const struct command_registration esirisc_flash_command_handlers[] = {
 
 struct flash_driver esirisc_flash = {
 	.name = "esirisc",
-	.commands = esirisc_flash_command_handlers,
 	.usage = "flash bank bank_id 'esirisc' base_address size_bytes 0 0 target "
 			"cfg_address clock_hz wait_states",
 	.flash_bank_command = esirisc_flash_bank_command,
 	.erase = esirisc_flash_erase,
-	.protect = esirisc_flash_protect,
 	.write = esirisc_flash_write,
 	.read = default_flash_read,
 	.probe = esirisc_flash_probe,
 	.auto_probe = esirisc_flash_auto_probe,
 	.erase_check = default_flash_blank_check,
-	.protect_check = esirisc_flash_protect_check,
 	.info = esirisc_flash_info,
+	.free_driver_priv = default_flash_free_driver_priv,
 };
diff --git a/src/flash/nor/faux.c b/src/flash/nor/faux.c
index 46eda72..d68940f 100644
--- a/src/flash/nor/faux.c
+++ b/src/flash/nor/faux.c
@@ -85,12 +85,6 @@ static int faux_erase(struct flash_bank *bank, int first, int last)
 	return ERROR_OK;
 }
 
-static int faux_protect(struct flash_bank *bank, int set, int first, int last)
-{
-	LOG_USER("set protection sector %d to %d to %s", first, last, set ? "on" : "off");
-	return ERROR_OK;
-}
-
 static int faux_write(struct flash_bank *bank, const uint8_t *buffer, uint32_t offset, uint32_t count)
 {
 	struct faux_flash_bank *info = bank->driver_priv;
@@ -98,11 +92,6 @@ static int faux_write(struct flash_bank *bank, const uint8_t *buffer, uint32_t o
 	return ERROR_OK;
 }
 
-static int faux_protect_check(struct flash_bank *bank)
-{
-	return ERROR_OK;
-}
-
 static int faux_info(struct flash_bank *bank, char *buf, int buf_size)
 {
 	snprintf(buf, buf_size, "faux flash driver");
@@ -129,13 +118,11 @@ struct flash_driver faux_flash = {
 	.commands = faux_command_handlers,
 	.flash_bank_command = faux_flash_bank_command,
 	.erase = faux_erase,
-	.protect = faux_protect,
 	.write = faux_write,
 	.read = default_flash_read,
 	.probe = faux_probe,
 	.auto_probe = faux_probe,
 	.erase_check = default_flash_blank_check,
-	.protect_check = faux_protect_check,
 	.info = faux_info,
 	.free_driver_priv = default_flash_free_driver_priv,
 };
diff --git a/src/flash/nor/fespi.c b/src/flash/nor/fespi.c
index 4c3aadd..1e8251c 100644
--- a/src/flash/nor/fespi.c
+++ b/src/flash/nor/fespi.c
@@ -391,6 +391,9 @@ static int fespi_erase(struct flash_bank *bank, int first, int last)
 		}
 	}
 
+	if (fespi_info->dev->erase_cmd == 0x00)
+		return ERROR_FLASH_OPER_UNSUPPORTED;
+
 	if (fespi_write_reg(bank, FESPI_REG_TXCTRL, FESPI_TXWM(1)) != ERROR_OK)
 		return ERROR_FAIL;
 	retval = fespi_txwm_wait(bank);
@@ -795,7 +798,9 @@ static int fespi_write(struct flash_bank *bank, const uint8_t *buffer,
 		data_wa_size /= 2;
 	}
 
-	page_size = fespi_info->dev->pagesize;
+	/* If no valid page_size, use reasonable default. */
+	page_size = fespi_info->dev->pagesize ?
+		fespi_info->dev->pagesize : SPIFLASH_DEF_PAGESIZE;
 
 	fespi_txwm_wait(bank);
 
@@ -911,6 +916,7 @@ static int fespi_probe(struct flash_bank *bank)
 	uint32_t id = 0; /* silence uninitialized warning */
 	const struct fespi_target *target_device;
 	int retval;
+	uint32_t sectorsize;
 
 	if (fespi_info->probed)
 		free(bank->sectors);
@@ -972,9 +978,17 @@ static int fespi_probe(struct flash_bank *bank)
 	/* Set correct size value */
 	bank->size = fespi_info->dev->size_in_bytes;
 
+	if (bank->size <= (1UL << 16))
+		LOG_WARNING("device needs 2-byte addresses - not implemented");
+	if (bank->size > (1UL << 24))
+		LOG_WARNING("device needs paging or 4-byte addresses - not implemented");
+
+	/* if no sectors, treat whole bank as single sector */
+	sectorsize = fespi_info->dev->sectorsize ?
+		fespi_info->dev->sectorsize : fespi_info->dev->size_in_bytes;
+
 	/* create and fill sectors array */
-	bank->num_sectors =
-		fespi_info->dev->size_in_bytes / fespi_info->dev->sectorsize;
+	bank->num_sectors = fespi_info->dev->size_in_bytes / sectorsize;
 	sectors = malloc(sizeof(struct flash_sector) * bank->num_sectors);
 	if (sectors == NULL) {
 		LOG_ERROR("not enough memory");
@@ -982,8 +996,8 @@ static int fespi_probe(struct flash_bank *bank)
 	}
 
 	for (int sector = 0; sector < bank->num_sectors; sector++) {
-		sectors[sector].offset = sector * fespi_info->dev->sectorsize;
-		sectors[sector].size = fespi_info->dev->sectorsize;
+		sectors[sector].offset = sector * sectorsize;
+		sectors[sector].size = sectorsize;
 		sectors[sector].is_erased = -1;
 		sectors[sector].is_protected = 0;
 	}
diff --git a/src/flash/nor/fm4.c b/src/flash/nor/fm4.c
index f5eab9c..d4d0f76 100644
--- a/src/flash/nor/fm4.c
+++ b/src/flash/nor/fm4.c
@@ -537,11 +537,6 @@ static int fm4_auto_probe(struct flash_bank *bank)
 	return fm4_probe(bank);
 }
 
-static int fm4_protect_check(struct flash_bank *bank)
-{
-	return ERROR_OK;
-}
-
 static int fm4_get_info_command(struct flash_bank *bank, char *buf, int buf_size)
 {
 	struct fm4_flash_bank *fm4_bank = bank->driver_priv;
@@ -714,7 +709,6 @@ struct flash_driver fm4_flash = {
 	.info = fm4_get_info_command,
 	.probe = fm4_probe,
 	.auto_probe = fm4_auto_probe,
-	.protect_check = fm4_protect_check,
 	.read = default_flash_read,
 	.erase = fm4_flash_erase,
 	.erase_check = default_flash_blank_check,
diff --git a/src/flash/nor/jtagspi.c b/src/flash/nor/jtagspi.c
index c28ad22..0750fdf 100644
--- a/src/flash/nor/jtagspi.c
+++ b/src/flash/nor/jtagspi.c
@@ -166,7 +166,7 @@ static int jtagspi_probe(struct flash_bank *bank)
 	struct jtagspi_flash_bank *info = bank->driver_priv;
 	struct flash_sector *sectors;
 	uint8_t in_buf[3];
-	uint32_t id;
+	uint32_t id, sectorsize;
 
 	if (info->probed)
 		free(bank->sectors);
@@ -199,10 +199,17 @@ static int jtagspi_probe(struct flash_bank *bank)
 
 	/* Set correct size value */
 	bank->size = info->dev->size_in_bytes;
+	if (bank->size <= (1UL << 16))
+		LOG_WARNING("device needs 2-byte addresses - not implemented");
+	if (bank->size > (1UL << 24))
+		LOG_WARNING("device needs paging or 4-byte addresses - not implemented");
+
+	/* if no sectors, treat whole bank as single sector */
+	sectorsize = info->dev->sectorsize ?
+		info->dev->sectorsize : info->dev->size_in_bytes;
 
 	/* create and fill sectors array */
-	bank->num_sectors =
-		info->dev->size_in_bytes / info->dev->sectorsize;
+	bank->num_sectors = info->dev->size_in_bytes / sectorsize;
 	sectors = malloc(sizeof(struct flash_sector) * bank->num_sectors);
 	if (sectors == NULL) {
 		LOG_ERROR("not enough memory");
@@ -210,8 +217,8 @@ static int jtagspi_probe(struct flash_bank *bank)
 	}
 
 	for (int sector = 0; sector < bank->num_sectors; sector++) {
-		sectors[sector].offset = sector * info->dev->sectorsize;
-		sectors[sector].size = info->dev->sectorsize;
+		sectors[sector].offset = sector * sectorsize;
+		sectors[sector].size = sectorsize;
 		sectors[sector].is_erased = -1;
 		sectors[sector].is_protected = 0;
 	}
@@ -269,6 +276,9 @@ static int jtagspi_bulk_erase(struct flash_bank *bank)
 	int retval;
 	int64_t t0 = timeval_ms();
 
+	if (info->dev->chip_erase_cmd == 0x00)
+		return ERROR_FLASH_OPER_UNSUPPORTED;
+
 	retval = jtagspi_write_enable(bank);
 	if (retval != ERROR_OK)
 		return retval;
@@ -328,6 +338,9 @@ static int jtagspi_erase(struct flash_bank *bank, int first, int last)
 			LOG_WARNING("Bulk flash erase failed. Falling back to sector erase.");
 	}
 
+	if (info->dev->erase_cmd == 0x00)
+		return ERROR_FLASH_OPER_UNSUPPORTED;
+
 	for (sector = first; sector <= last; sector++) {
 		retval = jtagspi_sector_erase(bank, sector);
 		if (retval != ERROR_OK) {
@@ -343,21 +356,11 @@ static int jtagspi_protect(struct flash_bank *bank, int set, int first, int last
 {
 	int sector;
 
-	if ((first < 0) || (last < first) || (last >= bank->num_sectors)) {
-		LOG_ERROR("Flash sector invalid");
-		return ERROR_FLASH_SECTOR_INVALID;
-	}
-
 	for (sector = first; sector <= last; sector++)
 		bank->sectors[sector].is_protected = set;
 	return ERROR_OK;
 }
 
-static int jtagspi_protect_check(struct flash_bank *bank)
-{
-	return ERROR_OK;
-}
-
 static int jtagspi_read(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, uint32_t count)
 {
 	struct jtagspi_flash_bank *info = bank->driver_priv;
@@ -386,16 +389,19 @@ static int jtagspi_write(struct flash_bank *bank, const uint8_t *buffer, uint32_
 {
 	struct jtagspi_flash_bank *info = bank->driver_priv;
 	int retval;
-	uint32_t n;
+	uint32_t n, pagesize;
 
 	if (!(info->probed)) {
 		LOG_ERROR("Flash bank not yet probed.");
 		return ERROR_FLASH_BANK_NOT_PROBED;
 	}
 
-	for (n = 0; n < count; n += info->dev->pagesize) {
+	/* if no write pagesize, use reasonable default */
+	pagesize = info->dev->pagesize ? info->dev->pagesize : SPIFLASH_DEF_PAGESIZE;
+
+	for (n = 0; n < count; n += pagesize) {
 		retval = jtagspi_page_write(bank, buffer + n, offset + n,
-				MIN(count - n, info->dev->pagesize));
+				MIN(count - n, pagesize));
 		if (retval != ERROR_OK) {
 			LOG_ERROR("page write error");
 			return retval;
@@ -431,7 +437,6 @@ struct flash_driver jtagspi_flash = {
 	.probe = jtagspi_probe,
 	.auto_probe = jtagspi_probe,
 	.erase_check = default_flash_blank_check,
-	.protect_check = jtagspi_protect_check,
 	.info = jtagspi_info,
 	.free_driver_priv = default_flash_free_driver_priv,
 };
diff --git a/src/flash/nor/kinetis.c b/src/flash/nor/kinetis.c
index 9e93570..1d53907 100644
--- a/src/flash/nor/kinetis.c
+++ b/src/flash/nor/kinetis.c
@@ -1035,7 +1035,7 @@ static int kinetis_disable_wdog_algo(struct target *target, size_t code_size, co
 		armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
 		armv7m_info.core_mode = ARM_MODE_THREAD;
 
-		init_reg_param(&reg_params[0], "r0", 32, PARAM_IN);
+		init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
 		buf_set_u32(reg_params[0].value, 0, 32, wdog_base);
 
 		retval = target_run_algorithm(target, 0, NULL, 1, reg_params,
diff --git a/src/flash/nor/lpc2000.c b/src/flash/nor/lpc2000.c
index b14e5ca..bced291 100644
--- a/src/flash/nor/lpc2000.c
+++ b/src/flash/nor/lpc2000.c
@@ -12,6 +12,9 @@
  *   by Nemui Trinomius                                                    *
  *   nemuisan_kawausogasuki@live.jp                                        *
  *                                                                         *
+ *   LPC8N04/HNS31xx support Copyright (C) 2018                            *
+ *   by Jean-Christian de Rivaz jcdr [at] innodelec [dot] ch               *
+ *                                                                         *
  *   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     *
@@ -38,7 +41,7 @@
 
 /**
  * @file
- * flash programming support for NXP LPC8xx,LPC1xxx,LPC4xxx,LP5410x and LPC2xxx devices.
+ * flash programming support for NXP LPC8xx,LPC1xxx,LPC4xxx,LP5410x,LPC2xxx and NHS31xx devices.
  *
  * @todo Provide a way to update CCLK after declaring the flash bank. The value which is correct after chip reset will
  * rarely still work right after the clocks switch to use the PLL (e.g. 4MHz --> 100 MHz).
@@ -77,6 +80,9 @@
  * lpc800:
  * - 810 | 1 | 2 (tested with LPC810/LPC811/LPC812)
  * - 822 | 4 (tested with LPC824)
+ * - 8N04
+ * - NHS31xx (tested with NHS3100)
+ * - 844 | 5 (tested with LPC845)
  *
  * lpc1100:
  * - 11xx
@@ -111,6 +117,8 @@
  * - 408x
  * - 81x
  * - 82x
+ * - 8N04
+ * - NHS31xx
  */
 
 /* Part IDs for autodetection */
@@ -257,6 +265,20 @@
 #define LPC824_201     0x00008241
 #define LPC824_201_1   0x00008242
 
+#define LPC8N04        0x00008A04
+#define NHS3100        0x4e310020
+#define NHS3152        0x4e315220
+#define NHS3153        0x4e315320 /* Only specified in Rev.1 of the datasheet */
+
+#define LPC844_201     0x00008441
+#define LPC844_201_1   0x00008442
+#define LPC844_201_2   0x00008444
+
+#define LPC845_301     0x00008451
+#define LPC845_301_1   0x00008452
+#define LPC845_301_2   0x00008453
+#define LPC845_301_3   0x00008454
+
 #define IAP_CODE_LEN 0x34
 
 #define LPC11xx_REG_SECTORS	24
@@ -282,6 +304,7 @@ struct lpc2000_flash_bank {
 	int checksum_vector;
 	uint32_t iap_max_stack;
 	uint32_t lpc4300_bank;
+	uint32_t iap_entry_alternative;
 	bool probed;
 };
 
@@ -526,10 +549,18 @@ static int lpc2000_build_sector_list(struct flash_bank *bank)
 			case 16 * 1024:
 				bank->num_sectors = 16;
 				break;
+			case 30 * 1024:
+				lpc2000_info->cmd51_max_buffer = 1024;	/* For LPC8N04 and NHS31xx, have 8kB of SRAM */
+				bank->num_sectors = 30;			/* There have only 30kB of writable Flash out of 32kB */
+				break;
 			case 32 * 1024:
 				lpc2000_info->cmd51_max_buffer = 1024; /* For LPC824, has 8kB of SRAM */
 				bank->num_sectors = 32;
 				break;
+			case 64 * 1024:
+				lpc2000_info->cmd51_max_buffer = 1024; /* For LPC844, has 8kB of SRAM */
+				bank->num_sectors = 64;
+				break;
 			default:
 				LOG_ERROR("BUG: unknown bank->size encountered");
 				exit(-1);
@@ -741,6 +772,9 @@ static int lpc2000_iap_call(struct flash_bank *bank, struct working_area *iap_wo
 			exit(-1);
 	}
 
+	if (lpc2000_info->iap_entry_alternative != 0x0)
+		iap_entry_point = lpc2000_info->iap_entry_alternative;
+
 	struct mem_param mem_params[2];
 
 	/* command parameter table */
@@ -938,6 +972,8 @@ FLASH_BANK_COMMAND_HANDLER(lpc2000_flash_bank_command)
 		if (strcmp(CMD_ARGV[8], "calc_checksum") == 0)
 			lpc2000_info->calc_checksum = 1;
 	}
+	if (CMD_ARGC >= 10 && !lpc2000_info->iap_entry_alternative)
+		COMMAND_PARSE_NUMBER(u32, CMD_ARGV[9], lpc2000_info->iap_entry_alternative);
 
 	return ERROR_OK;
 }
@@ -1018,12 +1054,6 @@ static int lpc2000_erase(struct flash_bank *bank, int first, int last)
 	return retval;
 }
 
-static int lpc2000_protect(struct flash_bank *bank, int set, int first, int last)
-{
-	/* can't protect/unprotect on the lpc2000 */
-	return ERROR_OK;
-}
-
 static int lpc2000_write(struct flash_bank *bank, const uint8_t *buffer, uint32_t offset, uint32_t count)
 {
 	struct target *target = bank->target;
@@ -1458,6 +1488,25 @@ static int lpc2000_auto_probe_flash(struct flash_bank *bank)
 			bank->size = 32 * 1024;
 			break;
 
+		case LPC8N04:
+		case NHS3100:
+		case NHS3152:
+		case NHS3153:
+			lpc2000_info->variant = lpc800;
+			bank->size = 30 * 1024;
+			break;
+
+		case LPC844_201:
+		case LPC844_201_1:
+		case LPC844_201_2:
+		case LPC845_301:
+		case LPC845_301_1:
+		case LPC845_301_2:
+		case LPC845_301_3:
+			lpc2000_info->variant = lpc800;
+			bank->size = 64 * 1024;
+			break;
+
 		default:
 			LOG_ERROR("BUG: unknown Part ID encountered: 0x%" PRIx32, part_id);
 			exit(-1);
@@ -1501,12 +1550,6 @@ static int lpc2000_erase_check(struct flash_bank *bank)
 	return lpc2000_iap_blank_check(bank, 0, bank->num_sectors - 1);
 }
 
-static int lpc2000_protect_check(struct flash_bank *bank)
-{
-	/* sectors are always protected	*/
-	return ERROR_OK;
-}
-
 static int get_lpc2000_info(struct flash_bank *bank, char *buf, int buf_size)
 {
 	struct lpc2000_flash_bank *lpc2000_info = bank->driver_priv;
@@ -1571,13 +1614,11 @@ struct flash_driver lpc2000_flash = {
 	.commands = lpc2000_command_handlers,
 	.flash_bank_command = lpc2000_flash_bank_command,
 	.erase = lpc2000_erase,
-	.protect = lpc2000_protect,
 	.write = lpc2000_write,
 	.read = default_flash_read,
 	.probe = lpc2000_probe,
 	.auto_probe = lpc2000_probe,
 	.erase_check = lpc2000_erase_check,
-	.protect_check = lpc2000_protect_check,
 	.info = get_lpc2000_info,
 	.free_driver_priv = default_flash_free_driver_priv,
 };
diff --git a/src/flash/nor/lpc288x.c b/src/flash/nor/lpc288x.c
index 2472913..afa8d79 100644
--- a/src/flash/nor/lpc288x.c
+++ b/src/flash/nor/lpc288x.c
@@ -167,6 +167,7 @@ static int lpc288x_read_part_info(struct flash_bank *bank)
 	return ERROR_OK;
 }
 
+/* TODO: Revisit! Is it impossible to read protection status? */
 static int lpc288x_protect_check(struct flash_bank *bank)
 {
 	return ERROR_OK;
@@ -231,17 +232,6 @@ static uint32_t lpc288x_system_ready(struct flash_bank *bank)
 	return ERROR_OK;
 }
 
-static int lpc288x_erase_check(struct flash_bank *bank)
-{
-	uint32_t status = lpc288x_system_ready(bank);	/* probed? halted? */
-	if (status != ERROR_OK) {
-		LOG_INFO("Processor not halted/not probed");
-		return status;
-	}
-
-	return ERROR_OK;
-}
-
 static int lpc288x_erase(struct flash_bank *bank, int first, int last)
 {
 	uint32_t status;
@@ -431,7 +421,7 @@ struct flash_driver lpc288x_flash = {
 	.read = default_flash_read,
 	.probe = lpc288x_probe,
 	.auto_probe = lpc288x_probe,
-	.erase_check = lpc288x_erase_check,
+	.erase_check = default_flash_blank_check,
 	.protect_check = lpc288x_protect_check,
 	.free_driver_priv = default_flash_free_driver_priv,
 };
diff --git a/src/flash/nor/lpc2900.c b/src/flash/nor/lpc2900.c
index 1c65933..022fb82 100644
--- a/src/flash/nor/lpc2900.c
+++ b/src/flash/nor/lpc2900.c
@@ -1035,18 +1035,13 @@ static int lpc2900_erase(struct flash_bank *bank, int first, int last)
 	return ERROR_OK;
 }
 
-static int lpc2900_protect(struct flash_bank *bank, int set, int first, int last)
-{
-	/* This command is not supported.
-	* "Protection" in LPC2900 terms is handled transparently. Sectors will
-	* automatically be unprotected as needed.
-	* Instead we use the concept of sector security. A secured sector is shown
-	* as "protected" in OpenOCD. Sector security is a permanent feature, and
-	* cannot be disabled once activated.
-	*/
-
-	return ERROR_OK;
-}
+/* lpc2900_protect command is not supported.
+* "Protection" in LPC2900 terms is handled transparently. Sectors will
+* automatically be unprotected as needed.
+* Instead we use the concept of sector security. A secured sector is shown
+* as "protected" in OpenOCD. Sector security is a permanent feature, and
+* cannot be disabled once activated.
+*/
 
 /**
  * Write data to flash.
@@ -1591,7 +1586,6 @@ struct flash_driver lpc2900_flash = {
 	.commands = lpc2900_command_handlers,
 	.flash_bank_command = lpc2900_flash_bank_command,
 	.erase = lpc2900_erase,
-	.protect = lpc2900_protect,
 	.write = lpc2900_write,
 	.read = default_flash_read,
 	.probe = lpc2900_probe,
diff --git a/src/flash/nor/lpcspifi.c b/src/flash/nor/lpcspifi.c
index 828c60c..a50584f 100644
--- a/src/flash/nor/lpcspifi.c
+++ b/src/flash/nor/lpcspifi.c
@@ -387,6 +387,9 @@ static int lpcspifi_bulk_erase(struct flash_bank *bank)
 	uint32_t value;
 	int retval = ERROR_OK;
 
+	if (lpcspifi_info->dev->chip_erase_cmd == 0x00)
+		return ERROR_FLASH_OPER_UNSUPPORTED;
+
 	retval = lpcspifi_set_sw_mode(bank);
 
 	if (retval == ERROR_OK)
@@ -460,6 +463,9 @@ static int lpcspifi_erase(struct flash_bank *bank, int first, int last)
 			LOG_WARNING("Bulk flash erase failed. Falling back to sector-by-sector erase.");
 	}
 
+	if (lpcspifi_info->dev->erase_cmd == 0x00)
+		return ERROR_FLASH_OPER_UNSUPPORTED;
+
 	retval = lpcspifi_set_hw_mode(bank);
 	if (retval != ERROR_OK)
 		return retval;
@@ -613,7 +619,9 @@ static int lpcspifi_write(struct flash_bank *bank, const uint8_t *buffer,
 		}
 	}
 
-	page_size = lpcspifi_info->dev->pagesize;
+	/* if no valid page_size, use reasonable default */
+	page_size = lpcspifi_info->dev->pagesize ?
+		lpcspifi_info->dev->pagesize : SPIFLASH_DEF_PAGESIZE;
 
 	retval = lpcspifi_set_hw_mode(bank);
 	if (retval != ERROR_OK)
@@ -839,6 +847,7 @@ static int lpcspifi_probe(struct flash_bank *bank)
 	struct flash_sector *sectors;
 	uint32_t id = 0; /* silence uninitialized warning */
 	int retval;
+	uint32_t sectorsize;
 
 	/* If we've already probed, we should be fine to skip this time. */
 	if (lpcspifi_info->probed)
@@ -876,10 +885,17 @@ static int lpcspifi_probe(struct flash_bank *bank)
 
 	/* Set correct size value */
 	bank->size = lpcspifi_info->dev->size_in_bytes;
+	if (bank->size <= (1UL << 16))
+		LOG_WARNING("device needs 2-byte addresses - not implemented");
+	if (bank->size > (1UL << 24))
+		LOG_WARNING("device needs paging or 4-byte addresses - not implemented");
+
+	/* if no sectors, treat whole bank as single sector */
+	sectorsize = lpcspifi_info->dev->sectorsize ?
+		lpcspifi_info->dev->sectorsize : lpcspifi_info->dev->size_in_bytes;
 
 	/* create and fill sectors array */
-	bank->num_sectors =
-		lpcspifi_info->dev->size_in_bytes / lpcspifi_info->dev->sectorsize;
+	bank->num_sectors = lpcspifi_info->dev->size_in_bytes / sectorsize;
 	sectors = malloc(sizeof(struct flash_sector) * bank->num_sectors);
 	if (sectors == NULL) {
 		LOG_ERROR("not enough memory");
@@ -887,8 +903,8 @@ static int lpcspifi_probe(struct flash_bank *bank)
 	}
 
 	for (int sector = 0; sector < bank->num_sectors; sector++) {
-		sectors[sector].offset = sector * lpcspifi_info->dev->sectorsize;
-		sectors[sector].size = lpcspifi_info->dev->sectorsize;
+		sectors[sector].offset = sector * sectorsize;
+		sectors[sector].size = sectorsize;
 		sectors[sector].is_erased = -1;
 		sectors[sector].is_protected = 0;
 	}
diff --git a/src/flash/nor/mdr.c b/src/flash/nor/mdr.c
index f3916de..c4a4458 100644
--- a/src/flash/nor/mdr.c
+++ b/src/flash/nor/mdr.c
@@ -86,11 +86,6 @@ FLASH_BANK_COMMAND_HANDLER(mdr_flash_bank_command)
 	return ERROR_OK;
 }
 
-static int mdr_protect_check(struct flash_bank *bank)
-{
-	return ERROR_OK;
-}
-
 static int mdr_mass_erase(struct flash_bank *bank)
 {
 	struct target *target = bank->target;
@@ -217,11 +212,6 @@ reset_pg_and_lock:
 	return retval;
 }
 
-static int mdr_protect(struct flash_bank *bank, int set, int first, int last)
-{
-	return ERROR_OK;
-}
-
 static int mdr_write_block(struct flash_bank *bank, const uint8_t *buffer,
 		uint32_t offset, uint32_t count)
 {
@@ -625,13 +615,11 @@ struct flash_driver mdr_flash = {
 	"<type>: 0 for main memory, 1 for info memory",
 	.flash_bank_command = mdr_flash_bank_command,
 	.erase = mdr_erase,
-	.protect = mdr_protect,
 	.write = mdr_write,
 	.read = mdr_read,
 	.probe = mdr_probe,
 	.auto_probe = mdr_auto_probe,
 	.erase_check = default_flash_blank_check,
-	.protect_check = mdr_protect_check,
 	.info = get_mdr_info,
 	.free_driver_priv = default_flash_free_driver_priv,
 };
diff --git a/src/flash/nor/mrvlqspi.c b/src/flash/nor/mrvlqspi.c
index eda6cc1..cb2cfdb 100644
--- a/src/flash/nor/mrvlqspi.c
+++ b/src/flash/nor/mrvlqspi.c
@@ -503,6 +503,9 @@ static int mrvlqspi_bulk_erase(struct flash_bank *bank)
 	int retval;
 	struct mrvlqspi_flash_bank *mrvlqspi_info = bank->driver_priv;
 
+	if (mrvlqspi_info->dev->chip_erase_cmd == 0x00)
+		return ERROR_FLASH_OPER_UNSUPPORTED;
+
 	/* Set flash write enable */
 	retval = mrvlqspi_set_write_status(bank, WRITE_ENABLE);
 	if (retval != ERROR_OK)
@@ -570,6 +573,9 @@ static int mrvlqspi_flash_erase(struct flash_bank *bank, int first, int last)
 				" Falling back to sector-by-sector erase.");
 	}
 
+	if (mrvlqspi_info->dev->erase_cmd == 0x00)
+		return ERROR_FLASH_OPER_UNSUPPORTED;
+
 	for (sector = first; sector <= last; sector++) {
 		retval = mrvlqspi_block_erase(bank,
 				sector * mrvlqspi_info->dev->sectorsize);
@@ -619,7 +625,9 @@ static int mrvlqspi_flash_write(struct flash_bank *bank, const uint8_t *buffer,
 		}
 	}
 
-	page_size = mrvlqspi_info->dev->pagesize;
+	/* if no valid page_size, use reasonable default */
+	page_size = mrvlqspi_info->dev->pagesize ?
+		mrvlqspi_info->dev->pagesize : SPIFLASH_DEF_PAGESIZE;
 
 	/* See contrib/loaders/flash/mrvlqspi.S for src */
 	static const uint8_t mrvlqspi_flash_write_code[] = {
@@ -826,6 +834,7 @@ static int mrvlqspi_probe(struct flash_bank *bank)
 	uint32_t id = 0;
 	int retval;
 	struct flash_sector *sectors;
+	uint32_t sectorsize;
 
 	/* If we've already probed, we should be fine to skip this time. */
 	if (mrvlqspi_info->probed)
@@ -859,12 +868,20 @@ static int mrvlqspi_probe(struct flash_bank *bank)
 	LOG_INFO("Found flash device \'%s\' ID 0x%08" PRIx32,
 		mrvlqspi_info->dev->name, mrvlqspi_info->dev->device_id);
 
+
 	/* Set correct size value */
 	bank->size = mrvlqspi_info->dev->size_in_bytes;
+	if (bank->size <= (1UL << 16))
+		LOG_WARNING("device needs 2-byte addresses - not implemented");
+	if (bank->size > (1UL << 24))
+		LOG_WARNING("device needs paging or 4-byte addresses - not implemented");
+
+	/* if no sectors, treat whole bank as single sector */
+	sectorsize = mrvlqspi_info->dev->sectorsize ?
+		mrvlqspi_info->dev->sectorsize : mrvlqspi_info->dev->size_in_bytes;
 
 	/* create and fill sectors array */
-	bank->num_sectors = mrvlqspi_info->dev->size_in_bytes /
-					mrvlqspi_info->dev->sectorsize;
+	bank->num_sectors = mrvlqspi_info->dev->size_in_bytes / sectorsize;
 	sectors = malloc(sizeof(struct flash_sector) * bank->num_sectors);
 	if (sectors == NULL) {
 		LOG_ERROR("not enough memory");
@@ -872,9 +889,8 @@ static int mrvlqspi_probe(struct flash_bank *bank)
 	}
 
 	for (int sector = 0; sector < bank->num_sectors; sector++) {
-		sectors[sector].offset =
-				sector * mrvlqspi_info->dev->sectorsize;
-		sectors[sector].size = mrvlqspi_info->dev->sectorsize;
+		sectors[sector].offset = sector * sectorsize;
+		sectors[sector].size = sectorsize;
 		sectors[sector].is_erased = -1;
 		sectors[sector].is_protected = 0;
 	}
@@ -899,12 +915,6 @@ static int mrvlqspi_flash_erase_check(struct flash_bank *bank)
 	return ERROR_OK;
 }
 
-static int mrvlqspi_protect_check(struct flash_bank *bank)
-{
-	/* Not implemented yet */
-	return ERROR_OK;
-}
-
 int mrvlqspi_get_info(struct flash_bank *bank, char *buf, int buf_size)
 {
 	struct mrvlqspi_flash_bank *mrvlqspi_info = bank->driver_priv;
@@ -947,13 +957,11 @@ struct flash_driver mrvlqspi_flash = {
 	.name = "mrvlqspi",
 	.flash_bank_command = mrvlqspi_flash_bank_command,
 	.erase = mrvlqspi_flash_erase,
-	.protect = NULL,
 	.write = mrvlqspi_flash_write,
 	.read = mrvlqspi_flash_read,
 	.probe = mrvlqspi_probe,
 	.auto_probe = mrvlqspi_auto_probe,
 	.erase_check = mrvlqspi_flash_erase_check,
-	.protect_check = mrvlqspi_protect_check,
 	.info = mrvlqspi_get_info,
 	.free_driver_priv = default_flash_free_driver_priv,
 };
diff --git a/src/flash/nor/msp432.c b/src/flash/nor/msp432.c
index 5caa052..e2e65d4 100644
--- a/src/flash/nor/msp432.c
+++ b/src/flash/nor/msp432.c
@@ -655,12 +655,6 @@ static int msp432_erase(struct flash_bank *bank, int first, int last)
 	return retval;
 }
 
-static int msp432_protect(struct flash_bank *bank, int set, int first,
-	int last)
-{
-	return ERROR_OK;
-}
-
 static int msp432_write(struct flash_bank *bank, const uint8_t *buffer,
 	uint32_t offset, uint32_t count)
 {
@@ -985,11 +979,6 @@ static int msp432_auto_probe(struct flash_bank *bank)
 	return retval;
 }
 
-static int msp432_protect_check(struct flash_bank *bank)
-{
-	return ERROR_OK;
-}
-
 static int msp432_info(struct flash_bank *bank, char *buf, int buf_size)
 {
 	struct msp432_bank *msp432_bank = bank->driver_priv;
@@ -1091,13 +1080,11 @@ struct flash_driver msp432_flash = {
 	.commands = msp432_command_handlers,
 	.flash_bank_command = msp432_flash_bank_command,
 	.erase = msp432_erase,
-	.protect = msp432_protect,
 	.write = msp432_write,
 	.read = default_flash_read,
 	.probe = msp432_probe,
 	.auto_probe = msp432_auto_probe,
 	.erase_check = default_flash_blank_check,
-	.protect_check = msp432_protect_check,
 	.info = msp432_info,
 	.free_driver_priv = msp432_flash_free_driver_priv,
 };
diff --git a/src/flash/nor/nrf5.c b/src/flash/nor/nrf5.c
index 9c48e1a..5f6204a 100644
--- a/src/flash/nor/nrf5.c
+++ b/src/flash/nor/nrf5.c
@@ -211,6 +211,7 @@ static const struct nrf5_device_spec nrf5_known_devices_table[] = {
 	/* nRF52832 Devices */
 	NRF5_DEVICE_DEF(0x00C7, "52832", "QFAA", "B0",    512),
 	NRF5_DEVICE_DEF(0x0139, "52832", "QFAA", "E0",    512),
+	NRF5_DEVICE_DEF(0x00E3, "52832", "CIAA", "B0",    512),
 
 	/* nRF52840 Devices */
 	NRF5_DEVICE_DEF(0x0150, "52840", "QIAA", "C0",    1024),
@@ -248,7 +249,7 @@ static int nrf5_wait_for_nvmc(struct nrf5_info *chip)
 {
 	uint32_t ready;
 	int res;
-	int timeout_ms = 200;
+	int timeout_ms = 340;
 	int64_t ts_start = timeval_ms();
 
 	do {
diff --git a/src/flash/nor/ocl.c b/src/flash/nor/ocl.c
index 895c4af..acc464b 100644
--- a/src/flash/nor/ocl.c
+++ b/src/flash/nor/ocl.c
@@ -30,16 +30,6 @@ struct ocl_priv {
 	unsigned int bufalign;
 };
 
-static int ocl_erase_check(struct flash_bank *bank)
-{
-	return ERROR_OK;
-}
-
-static int ocl_protect_check(struct flash_bank *bank)
-{
-	return ERROR_OK;
-}
-
 /* flash_bank ocl 0 0 0 0 <target#> */
 FLASH_BANK_COMMAND_HANDLER(ocl_flash_bank_command)
 {
@@ -111,11 +101,6 @@ static int ocl_erase(struct flash_bank *bank, int first, int last)
 	return ERROR_OK;
 }
 
-static int ocl_protect(struct flash_bank *bank, int set, int first, int last)
-{
-	return ERROR_OK;
-}
-
 static int ocl_write(struct flash_bank *bank, const uint8_t *buffer, uint32_t offset, uint32_t count)
 {
 	struct ocl_priv *ocl = bank->driver_priv;
@@ -333,12 +318,10 @@ struct flash_driver ocl_flash = {
 	.name = "ocl",
 	.flash_bank_command = ocl_flash_bank_command,
 	.erase = ocl_erase,
-	.protect = ocl_protect,
 	.write = ocl_write,
 	.read = default_flash_read,
 	.probe = ocl_probe,
-	.erase_check = ocl_erase_check,
-	.protect_check = ocl_protect_check,
+	.erase_check = default_flash_blank_check,
 	.auto_probe = ocl_auto_probe,
 	.free_driver_priv = default_flash_free_driver_priv,
 };
diff --git a/src/flash/nor/psoc5lp.c b/src/flash/nor/psoc5lp.c
index b88abbb..d8e1c15 100644
--- a/src/flash/nor/psoc5lp.c
+++ b/src/flash/nor/psoc5lp.c
@@ -753,16 +753,6 @@ static int psoc5lp_nvl_write(struct flash_bank *bank,
 	return ERROR_OK;
 }
 
-static int psoc5lp_nvl_protect_check(struct flash_bank *bank)
-{
-	int i;
-
-	for (i = 0; i < bank->num_sectors; i++)
-		bank->sectors[i].is_protected = -1;
-
-	return ERROR_OK;
-}
-
 static int psoc5lp_nvl_get_info_command(struct flash_bank *bank,
 	char *buf, int buf_size)
 {
@@ -855,7 +845,6 @@ struct flash_driver psoc5lp_nvl_flash = {
 	.info = psoc5lp_nvl_get_info_command,
 	.probe = psoc5lp_nvl_probe,
 	.auto_probe = psoc5lp_nvl_auto_probe,
-	.protect_check = psoc5lp_nvl_protect_check,
 	.read = psoc5lp_nvl_read,
 	.erase = psoc5lp_nvl_erase,
 	.erase_check = psoc5lp_nvl_erase_check,
@@ -945,16 +934,6 @@ static int psoc5lp_eeprom_write(struct flash_bank *bank,
 	return ERROR_OK;
 }
 
-static int psoc5lp_eeprom_protect_check(struct flash_bank *bank)
-{
-	int i;
-
-	for (i = 0; i < bank->num_sectors; i++)
-		bank->sectors[i].is_protected = -1;
-
-	return ERROR_OK;
-}
-
 static int psoc5lp_eeprom_get_info_command(struct flash_bank *bank, char *buf, int buf_size)
 {
 	struct psoc5lp_eeprom_flash_bank *psoc_eeprom_bank = bank->driver_priv;
@@ -1064,7 +1043,6 @@ struct flash_driver psoc5lp_eeprom_flash = {
 	.info = psoc5lp_eeprom_get_info_command,
 	.probe = psoc5lp_eeprom_probe,
 	.auto_probe = psoc5lp_eeprom_auto_probe,
-	.protect_check = psoc5lp_eeprom_protect_check,
 	.read = default_flash_read,
 	.erase = psoc5lp_eeprom_erase,
 	.erase_check = default_flash_blank_check,
diff --git a/src/flash/nor/spi.c b/src/flash/nor/spi.c
index 6ac253d..af72ffc 100644
--- a/src/flash/nor/spi.c
+++ b/src/flash/nor/spi.c
@@ -1,4 +1,7 @@
 /***************************************************************************
+ *   Copyright (C) 2018 by Andreas Bolsch                                  *
+ *   andreas.bolsch@mni.thm.de                                             *
+ *                                                                         *
  *   Copyright (C) 2012 by George Harris                                   *
  *   george@luminairecoffee.com                                            *
  *                                                                         *
@@ -30,58 +33,118 @@
  /* Shared table of known SPI flash devices for SPI-based flash drivers. Taken
   * from device datasheets and Linux SPI flash drivers. */
 const struct flash_device flash_devices[] = {
-	/* name, erase_cmd, chip_erase_cmd, device_id, pagesize, sectorsize, size_in_bytes */
-	FLASH_ID("st m25p05",           0xd8, 0xc7, 0x00102020, 0x80,  0x8000,  0x10000),
-	FLASH_ID("st m25p10",           0xd8, 0xc7, 0x00112020, 0x80,  0x8000,  0x20000),
-	FLASH_ID("st m25p20",           0xd8, 0xc7, 0x00122020, 0x100, 0x10000, 0x40000),
-	FLASH_ID("st m25p40",           0xd8, 0xc7, 0x00132020, 0x100, 0x10000, 0x80000),
-	FLASH_ID("st m25p80",           0xd8, 0xc7, 0x00142020, 0x100, 0x10000, 0x100000),
-	FLASH_ID("st m25p16",           0xd8, 0xc7, 0x00152020, 0x100, 0x10000, 0x200000),
-	FLASH_ID("st m25p32",           0xd8, 0xc7, 0x00162020, 0x100, 0x10000, 0x400000),
-	FLASH_ID("st m25p64",           0xd8, 0xc7, 0x00172020, 0x100, 0x10000, 0x800000),
-	FLASH_ID("st m25p128",          0xd8, 0xc7, 0x00182020, 0x100, 0x40000, 0x1000000),
-	FLASH_ID("st m45pe10",          0xd8, 0xd8, 0x00114020, 0x100, 0x10000, 0x20000),
-	FLASH_ID("st m45pe20",          0xd8, 0xd8, 0x00124020, 0x100, 0x10000, 0x40000),
-	FLASH_ID("st m45pe40",          0xd8, 0xd8, 0x00134020, 0x100, 0x10000, 0x80000),
-	FLASH_ID("st m45pe80",          0xd8, 0xd8, 0x00144020, 0x100, 0x10000, 0x100000),
-	FLASH_ID("sp s25fl004",         0xd8, 0xc7, 0x00120201, 0x100, 0x10000, 0x80000),
-	FLASH_ID("sp s25fl008",         0xd8, 0xc7, 0x00130201, 0x100, 0x10000, 0x100000),
-	FLASH_ID("sp s25fl016",         0xd8, 0xc7, 0x00140201, 0x100, 0x10000, 0x200000),
-	FLASH_ID("sp s25fl116k",        0xd8, 0xc7, 0x00154001, 0x100, 0x10000, 0x200000),
-	FLASH_ID("sp s25fl032",         0xd8, 0xc7, 0x00150201, 0x100, 0x10000, 0x400000),
-	FLASH_ID("sp s25fl132k",        0xd8, 0xc7, 0x00164001, 0x100, 0x10000, 0x400000),
-	FLASH_ID("sp s25fl064",         0xd8, 0xc7, 0x00160201, 0x100, 0x10000, 0x800000),
-	FLASH_ID("sp s25fl164k",        0xd8, 0xc7, 0x00174001, 0x100, 0x10000, 0x800000),
-	FLASH_ID("sp s25fl128",         0xd8, 0xc7, 0x00182001, 0x100, 0x10000, 0x1000000),
-	FLASH_ID("sp s25fl256",         0xd8, 0xc7, 0x00190201, 0x100, 0x10000, 0x2000000),
-	FLASH_ID("cy s25fl256",         0xd8, 0xc7, 0x00196001, 0x100, 0x10000, 0x2000000),
-	FLASH_ID("atmel 25f512",        0x52, 0xc7, 0x0065001f, 0x80,  0x8000,  0x10000),
-	FLASH_ID("atmel 25f1024",       0x52, 0x62, 0x0060001f, 0x100, 0x8000,  0x20000),
-	FLASH_ID("atmel 25f2048",       0x52, 0x62, 0x0063001f, 0x100, 0x10000, 0x40000),
-	FLASH_ID("atmel 25f4096",       0x52, 0x62, 0x0064001f, 0x100, 0x10000, 0x80000),
-	FLASH_ID("atmel 25fs040",       0xd7, 0xc7, 0x0004661f, 0x100, 0x10000, 0x80000),
-	FLASH_ID("mac 25l512",          0xd8, 0xc7, 0x001020c2, 0x010, 0x10000, 0x10000),
-	FLASH_ID("mac 25l1005",         0xd8, 0xc7, 0x001120c2, 0x010, 0x10000, 0x20000),
-	FLASH_ID("mac 25l2005",         0xd8, 0xc7, 0x001220c2, 0x010, 0x10000, 0x40000),
-	FLASH_ID("mac 25l4005",         0xd8, 0xc7, 0x001320c2, 0x010, 0x10000, 0x80000),
-	FLASH_ID("mac 25l8005",         0xd8, 0xc7, 0x001420c2, 0x010, 0x10000, 0x100000),
-	FLASH_ID("mac 25l1605",         0xd8, 0xc7, 0x001520c2, 0x100, 0x10000, 0x200000),
-	FLASH_ID("mac 25l3205",         0xd8, 0xc7, 0x001620c2, 0x100, 0x10000, 0x400000),
-	FLASH_ID("mac 25l6405",         0xd8, 0xc7, 0x001720c2, 0x100, 0x10000, 0x800000),
-	FLASH_ID("micron n25q064",      0xd8, 0xc7, 0x0017ba20, 0x100, 0x10000, 0x800000),
-	FLASH_ID("micron n25q128",      0xd8, 0xc7, 0x0018ba20, 0x100, 0x10000, 0x1000000),
-	FLASH_ID("micron n25q256 3v",   0xd8, 0xc7, 0x0019ba20, 0x100, 0x10000, 0x2000000),
-	FLASH_ID("micron n25q256 1.8v", 0xd8, 0xc7, 0x0019bb20, 0x100, 0x10000, 0x2000000),
-	FLASH_ID("issi is25lp128",      0xd8, 0xc7, 0x0018609d, 0x100, 0x10000, 0x1000000),
-	FLASH_ID("issi is25wp256d",     0xd8, 0xc7, 0x0019709d, 0x100, 0x10000, 0x2000000),
-	FLASH_ID("win w25q80bv",        0xd8, 0xc7, 0x001440ef, 0x100, 0x10000, 0x100000),
-	FLASH_ID("win w25q32fv",        0xd8, 0xc7, 0x001640ef, 0x100, 0x10000, 0x400000),
-	FLASH_ID("win w25q32dw",        0xd8, 0xc7, 0x001660ef, 0x100, 0x10000, 0x400000),
-	FLASH_ID("win w25q64cv",        0xd8, 0xc7, 0x001740ef, 0x100, 0x10000, 0x800000),
-	FLASH_ID("win w25q128fv",       0xd8, 0xc7, 0x001840ef, 0x100, 0x10000, 0x1000000),
-	FLASH_ID("gd gd25q20",          0x20, 0xc7, 0x00c84012, 0x100, 0x1000,  0x80000),
-	FLASH_ID("gd gd25q16c",         0xd8, 0xc7, 0x001540c8, 0x100, 0x10000, 0x200000),
-	FLASH_ID("gd gd25q32c",         0xd8, 0xc7, 0x001640c8, 0x100, 0x10000, 0x400000),
-	FLASH_ID("gd gd25q128c",        0xd8, 0xc7, 0x001840c8, 0x100, 0x10000, 0x1000000),
-	FLASH_ID(NULL,                  0,    0,    0,          0,     0,       0)
+	/* name, read_cmd, qread_cmd, pprog_cmd, erase_cmd, chip_erase_cmd, device_id,
+	 * pagesize, sectorsize, size_in_bytes */
+	FLASH_ID("st m25p05",           0x03, 0x00, 0x02, 0xd8, 0xc7, 0x00102020, 0x80,  0x8000,  0x10000),
+	FLASH_ID("st m25p10",           0x03, 0x00, 0x02, 0xd8, 0xc7, 0x00112020, 0x80,  0x8000,  0x20000),
+	FLASH_ID("st m25p20",           0x03, 0x00, 0x02, 0xd8, 0xc7, 0x00122020, 0x100, 0x10000, 0x40000),
+	FLASH_ID("st m25p40",           0x03, 0x00, 0x02, 0xd8, 0xc7, 0x00132020, 0x100, 0x10000, 0x80000),
+	FLASH_ID("st m25p80",           0x03, 0x00, 0x02, 0xd8, 0xc7, 0x00142020, 0x100, 0x10000, 0x100000),
+	FLASH_ID("st m25p16",           0x03, 0x00, 0x02, 0xd8, 0xc7, 0x00152020, 0x100, 0x10000, 0x200000),
+	FLASH_ID("st m25p32",           0x03, 0x00, 0x02, 0xd8, 0xc7, 0x00162020, 0x100, 0x10000, 0x400000),
+	FLASH_ID("st m25p64",           0x03, 0x00, 0x02, 0xd8, 0xc7, 0x00172020, 0x100, 0x10000, 0x800000),
+	FLASH_ID("st m25p128",          0x03, 0x00, 0x02, 0xd8, 0xc7, 0x00182020, 0x100, 0x40000, 0x1000000),
+	FLASH_ID("st m45pe10",          0x03, 0x00, 0x02, 0xd8, 0xd8, 0x00114020, 0x100, 0x10000, 0x20000),
+	FLASH_ID("st m45pe20",          0x03, 0x00, 0x02, 0xd8, 0xd8, 0x00124020, 0x100, 0x10000, 0x40000),
+	FLASH_ID("st m45pe40",          0x03, 0x00, 0x02, 0xd8, 0xd8, 0x00134020, 0x100, 0x10000, 0x80000),
+	FLASH_ID("st m45pe80",          0x03, 0x00, 0x02, 0xd8, 0xd8, 0x00144020, 0x100, 0x10000, 0x100000),
+	FLASH_ID("sp s25fl004",         0x03, 0x00, 0x02, 0xd8, 0xc7, 0x00120201, 0x100, 0x10000, 0x80000),
+	FLASH_ID("sp s25fl008",         0x03, 0x08, 0x02, 0xd8, 0xc7, 0x00130201, 0x100, 0x10000, 0x100000),
+	FLASH_ID("sp s25fl016",         0x03, 0x00, 0x02, 0xd8, 0xc7, 0x00140201, 0x100, 0x10000, 0x200000),
+	FLASH_ID("sp s25fl116k",        0x03, 0x00, 0x02, 0xd8, 0xc7, 0x00154001, 0x100, 0x10000, 0x200000),
+	FLASH_ID("sp s25fl032",         0x03, 0x00, 0x02, 0xd8, 0xc7, 0x00150201, 0x100, 0x10000, 0x400000),
+	FLASH_ID("sp s25fl132k",        0x03, 0x00, 0x02, 0xd8, 0xc7, 0x00164001, 0x100, 0x10000, 0x400000),
+	FLASH_ID("sp s25fl064",         0x03, 0x00, 0x02, 0xd8, 0xc7, 0x00160201, 0x100, 0x10000, 0x800000),
+	FLASH_ID("sp s25fl164k",        0x03, 0x00, 0x02, 0xd8, 0xc7, 0x00174001, 0x100, 0x10000, 0x800000),
+	FLASH_ID("sp s25fl128s",        0x03, 0x00, 0x02, 0xd8, 0xc7, 0x00182001, 0x100, 0x10000, 0x1000000),
+	FLASH_ID("sp s25fl256s",        0x13, 0x00, 0x12, 0xdc, 0xc7, 0x00190201, 0x100, 0x10000, 0x2000000),
+	FLASH_ID("sp s25fl512s",        0x13, 0x00, 0x12, 0xdc, 0xc7, 0x00200201, 0x200, 0x40000, 0x4000000),
+	FLASH_ID("cyp s25fl064l",       0x03, 0x00, 0x02, 0xd8, 0xc7, 0x00176001, 0x100, 0x10000, 0x800000),
+	FLASH_ID("cyp s25fl128l",       0x03, 0x00, 0x02, 0xd8, 0xc7, 0x00186001, 0x100, 0x10000, 0x1000000),
+	FLASH_ID("cyp s25fl256l",       0x13, 0x00, 0x12, 0xdc, 0xc7, 0x00196001, 0x100, 0x10000, 0x2000000),
+	FLASH_ID("atmel 25f512",        0x03, 0x00, 0x02, 0x52, 0xc7, 0x0065001f, 0x80,  0x8000,  0x10000),
+	FLASH_ID("atmel 25f1024",       0x03, 0x00, 0x02, 0x52, 0x62, 0x0060001f, 0x100, 0x8000,  0x20000),
+	FLASH_ID("atmel 25f2048",       0x03, 0x00, 0x02, 0x52, 0x62, 0x0063001f, 0x100, 0x10000, 0x40000),
+	FLASH_ID("atmel 25f4096",       0x03, 0x00, 0x02, 0x52, 0x62, 0x0064001f, 0x100, 0x10000, 0x80000),
+	FLASH_ID("atmel 25fs040",       0x03, 0x00, 0x02, 0xd7, 0xc7, 0x0004661f, 0x100, 0x10000, 0x80000),
+	FLASH_ID("adesto 25df081a",     0x03, 0x00, 0x02, 0xd8, 0xc7, 0x0001451f, 0x100, 0x10000, 0x100000),
+	FLASH_ID("mac 25l512",          0x03, 0x00, 0x02, 0xd8, 0xc7, 0x001020c2, 0x010, 0x10000, 0x10000),
+	FLASH_ID("mac 25l1005",         0x03, 0x00, 0x02, 0xd8, 0xc7, 0x001120c2, 0x010, 0x10000, 0x20000),
+	FLASH_ID("mac 25l2005",         0x03, 0x00, 0x02, 0xd8, 0xc7, 0x001220c2, 0x010, 0x10000, 0x40000),
+	FLASH_ID("mac 25l4005",         0x03, 0x00, 0x02, 0xd8, 0xc7, 0x001320c2, 0x010, 0x10000, 0x80000),
+	FLASH_ID("mac 25l8005",         0x03, 0x00, 0x02, 0xd8, 0xc7, 0x001420c2, 0x010, 0x10000, 0x100000),
+	FLASH_ID("mac 25l1605",         0x03, 0x00, 0x02, 0xd8, 0xc7, 0x001520c2, 0x100, 0x10000, 0x200000),
+	FLASH_ID("mac 25l3205",         0x03, 0x00, 0x02, 0xd8, 0xc7, 0x001620c2, 0x100, 0x10000, 0x400000),
+	FLASH_ID("mac 25l6405",         0x03, 0x00, 0x02, 0xd8, 0xc7, 0x001720c2, 0x100, 0x10000, 0x800000),
+	FLASH_ID("mac 25l12845",        0x03, 0xeb, 0x02, 0xd8, 0xc7, 0x001820c2, 0x100, 0x10000, 0x1000000),
+	FLASH_ID("mac 25l25645",        0x13, 0xec, 0x12, 0xdc, 0xc7, 0x001920c2, 0x100, 0x10000, 0x2000000),
+	FLASH_ID("mac 25l51245",        0x13, 0xec, 0x12, 0xdc, 0xc7, 0x001a20c2, 0x100, 0x10000, 0x4000000),
+	FLASH_ID("mac 25lm51245",       0x13, 0xec, 0x12, 0xdc, 0xc7, 0x003a85c2, 0x100, 0x10000, 0x4000000),
+	FLASH_ID("mac 25r512f",         0x03, 0x00, 0x02, 0xd8, 0xc7, 0x001028c2, 0x100, 0x10000, 0x10000),
+	FLASH_ID("mac 25r1035f",        0x03, 0x00, 0x02, 0xd8, 0xc7, 0x001128c2, 0x100, 0x10000, 0x20000),
+	FLASH_ID("mac 25r2035f",        0x03, 0x00, 0x02, 0xd8, 0xc7, 0x001228c2, 0x100, 0x10000, 0x40000),
+	FLASH_ID("mac 25r4035f",        0x03, 0x00, 0x02, 0xd8, 0xc7, 0x001328c2, 0x100, 0x10000, 0x80000),
+	FLASH_ID("mac 25r8035f",        0x03, 0x00, 0x02, 0xd8, 0xc7, 0x001428c2, 0x100, 0x10000, 0x100000),
+	FLASH_ID("mac 25r1635f",        0x03, 0x00, 0x02, 0xd8, 0xc7, 0x001528c2, 0x100, 0x10000, 0x200000),
+	FLASH_ID("mac 25r3235f",        0x03, 0x00, 0x02, 0xd8, 0xc7, 0x001628c2, 0x100, 0x10000, 0x400000),
+	FLASH_ID("mac 25r6435f",        0x03, 0x00, 0x02, 0xd8, 0xc7, 0x001728c2, 0x100, 0x10000, 0x800000),
+	FLASH_ID("micron n25q064",      0x03, 0xeb, 0x02, 0xd8, 0xc7, 0x0017ba20, 0x100, 0x10000, 0x800000),
+	FLASH_ID("micron n25q128",      0x03, 0xeb, 0x02, 0xd8, 0xc7, 0x0018ba20, 0x100, 0x10000, 0x1000000),
+	FLASH_ID("micron n25q256 3v",   0x13, 0xec, 0x12, 0xdc, 0xc7, 0x0019ba20, 0x100, 0x10000, 0x2000000),
+	FLASH_ID("micron n25q256 1.8v", 0x13, 0xec, 0x12, 0xdc, 0xc7, 0x0019bb20, 0x100, 0x10000, 0x2000000),
+	FLASH_ID("micron mt25ql512",    0x13, 0xec, 0x12, 0xdc, 0xc7, 0x0020ba20, 0x100, 0x10000, 0x4000000),
+	FLASH_ID("micron mt25ql01",     0x13, 0xec, 0x12, 0xdc, 0xc7, 0x0021ba20, 0x100, 0x10000, 0x8000000),
+	FLASH_ID("micron mt25ql02",     0x13, 0xec, 0x12, 0xdc, 0xc7, 0x0022ba20, 0x100, 0x10000, 0x10000000),
+	FLASH_ID("win w25q80bv",        0x03, 0x00, 0x02, 0xd8, 0xc7, 0x001440ef, 0x100, 0x10000, 0x100000),
+	FLASH_ID("win w25q16jv",        0x03, 0x00, 0x02, 0xd8, 0xc7, 0x001540ef, 0x100, 0x10000, 0x200000),
+	FLASH_ID("win w25q16jv",        0x03, 0x00, 0x02, 0xd8, 0xc7, 0x001570ef, 0x100, 0x10000, 0x200000), /* QPI / DTR */
+	FLASH_ID("win w25q32fv/jv",     0x03, 0xeb, 0x02, 0xd8, 0xc7, 0x001640ef, 0x100, 0x10000, 0x400000),
+	FLASH_ID("win w25q32fv",        0x03, 0xeb, 0x02, 0xd8, 0xc7, 0x001660ef, 0x100, 0x10000, 0x400000), /* QPI mode */
+	FLASH_ID("win w25q32jv",        0x03, 0x00, 0x02, 0xd8, 0xc7, 0x001670ef, 0x100, 0x10000, 0x400000),
+	FLASH_ID("win w25q64fv/jv",     0x03, 0xeb, 0x02, 0xd8, 0xc7, 0x001740ef, 0x100, 0x10000, 0x800000),
+	FLASH_ID("win w25q64fv",        0x03, 0xeb, 0x02, 0xd8, 0xc7, 0x001760ef, 0x100, 0x10000, 0x800000), /* QPI mode */
+	FLASH_ID("win w25q64jv",        0x03, 0x00, 0x02, 0xd8, 0xc7, 0x001770ef, 0x100, 0x10000, 0x800000),
+	FLASH_ID("win w25q128fv/jv",    0x03, 0xeb, 0x02, 0xd8, 0xc7, 0x001840ef, 0x100, 0x10000, 0x1000000),
+	FLASH_ID("win w25q128fv",       0x03, 0xeb, 0x02, 0xd8, 0xc7, 0x001860ef, 0x100, 0x10000, 0x1000000), /* QPI mode */
+	FLASH_ID("win w25q128jv",       0x03, 0x00, 0x02, 0xd8, 0xc7, 0x001870ef, 0x100, 0x10000, 0x1000000),
+	FLASH_ID("win w25q256fv/jv",    0x03, 0xeb, 0x02, 0xd8, 0xc7, 0x001940ef, 0x100, 0x10000, 0x2000000),
+	FLASH_ID("win w25q256fv",       0x03, 0xeb, 0x02, 0xd8, 0xc7, 0x001960ef, 0x100, 0x10000, 0x2000000), /* QPI mode */
+	FLASH_ID("win w25q256jv",       0x03, 0x00, 0x02, 0xd8, 0xc7, 0x001970ef, 0x100, 0x10000, 0x2000000),
+	FLASH_ID("gd gd25q512",         0x03, 0x00, 0x02, 0x20, 0xc7, 0x001040c8, 0x100, 0x1000,  0x10000),
+	FLASH_ID("gd gd25q10",          0x03, 0x00, 0x02, 0x20, 0xc7, 0x001140c8, 0x100, 0x1000,  0x20000),
+	FLASH_ID("gd gd25q20",          0x03, 0x00, 0x02, 0x20, 0xc7, 0x001240c8, 0x100, 0x1000,  0x40000),
+	FLASH_ID("gd gd25q40",          0x03, 0x00, 0x02, 0x20, 0xc7, 0x001340c8, 0x100, 0x1000,  0x80000),
+	FLASH_ID("gd gd25q16c",         0x03, 0x00, 0x02, 0xd8, 0xc7, 0x001540c8, 0x100, 0x10000, 0x200000),
+	FLASH_ID("gd gd25q32c",         0x03, 0x00, 0x02, 0xd8, 0xc7, 0x001640c8, 0x100, 0x10000, 0x400000),
+	FLASH_ID("gd gd25q64c",         0x03, 0x00, 0x02, 0xd8, 0xc7, 0x001740c8, 0x100, 0x10000, 0x800000),
+	FLASH_ID("gd gd25q128c",        0x03, 0xeb, 0x02, 0xd8, 0xc7, 0x001840c8, 0x100, 0x10000, 0x1000000),
+	FLASH_ID("gd gd25q256c",        0x13, 0x00, 0x12, 0xdc, 0xc7, 0x001940c8, 0x100, 0x10000, 0x2000000),
+	FLASH_ID("gd gd25q512mc",       0x13, 0x00, 0x12, 0xdc, 0xc7, 0x002040c8, 0x100, 0x10000, 0x4000000),
+	FLASH_ID("issi is25lp032",      0x03, 0x00, 0x02, 0xd8, 0xc7, 0x0016609d, 0x100, 0x10000, 0x400000),
+	FLASH_ID("issi is25lp064",      0x03, 0x00, 0x02, 0xd8, 0xc7, 0x0017609d, 0x100, 0x10000, 0x800000),
+	FLASH_ID("issi is25lp128d",     0x03, 0xeb, 0x02, 0xd8, 0xc7, 0x0018609d, 0x100, 0x10000, 0x1000000),
+	FLASH_ID("issi is25wp128d",     0x03, 0xeb, 0x02, 0xd8, 0xc7, 0x0018709d, 0x100, 0x10000, 0x1000000),
+	FLASH_ID("issi is25lp256d",     0x13, 0xec, 0x12, 0xdc, 0xc7, 0x0019609d, 0x100, 0x10000, 0x2000000),
+	FLASH_ID("issi is25wp256d",     0x13, 0xec, 0x12, 0xdc, 0xc7, 0x0019709d, 0x100, 0x10000, 0x2000000),
+	FLASH_ID("issi is25lp512m",     0x13, 0xec, 0x12, 0xdc, 0xc7, 0x001a609d, 0x100, 0x10000, 0x4000000),
+	FLASH_ID("issi is25wp512m",     0x13, 0xec, 0x12, 0xdc, 0xc7, 0x001a709d, 0x100, 0x10000, 0x4000000),
+
+	/* FRAM, no erase commands, no write page or sectors */
+	FRAM_ID("fu mb85rs16n",         0x03, 0,    0x02, 0x00010104, 0x800),
+	FRAM_ID("fu mb85rs32v",         0x03, 0,    0x02, 0x00010204, 0x1000), /* exists ? */
+	FRAM_ID("fu mb85rs64v",         0x03, 0,    0x02, 0x00020304, 0x2000),
+	FRAM_ID("fu mb85rs128b",        0x03, 0,    0x02, 0x00090404, 0x4000),
+	FRAM_ID("fu mb85rs256b",        0x03, 0,    0x02, 0x00090504, 0x8000),
+	FRAM_ID("fu mb85rs512t",        0x03, 0,    0x02, 0x00032604, 0x10000),
+	FRAM_ID("fu mb85rs1mt",         0x03, 0,    0x02, 0x00032704, 0x20000),
+	FRAM_ID("fu mb85rs2mta",        0x03, 0,    0x02, 0x00034804, 0x40000),
+	FRAM_ID("cyp fm25v01a",         0x03, 0,    0x02, 0x000821c2, 0x4000),
+	FRAM_ID("cyp fm25v02",          0x03, 0,    0x02, 0x000022c2, 0x8000),
+	FRAM_ID("cyp fm25v02a",         0x03, 0,    0x02, 0x000822c2, 0x8000),
+	FRAM_ID("cyp fm25v05",          0x03, 0,    0x02, 0x000023c2, 0x10000),
+	FRAM_ID("cyp fm25v10",          0x03, 0,    0x02, 0x000024c2, 0x20000),
+	FRAM_ID("cyp fm25v20a",         0x03, 0,    0x02, 0x000825c2, 0x40000),
+	FRAM_ID("cyp fm25v40",          0x03, 0,    0x02, 0x004026c2, 0x80000),
+
+	FLASH_ID(NULL,                  0,    0,    0,    0,    0,    0,          0,     0,       0)
 };
diff --git a/src/flash/nor/spi.h b/src/flash/nor/spi.h
index a184998..11c381f 100644
--- a/src/flash/nor/spi.h
+++ b/src/flash/nor/spi.h
@@ -1,4 +1,7 @@
 /***************************************************************************
+ *   Copyright (C) 2018 by Andreas Bolsch                                  *
+ *   andreas.bolsch@mni.thm.de                                             *
+ *                                                                         *
  *   Copyright (C) 2012 by George Harris                                   *
  *   george@luminairecoffee.com                                            *
  *                                                                         *
@@ -22,40 +25,69 @@
 #ifndef OPENOCD_FLASH_NOR_SPI_H
 #define OPENOCD_FLASH_NOR_SPI_H
 
+#ifndef __ASSEMBLER__
+
 /* data structure to maintain flash ids from different vendors */
 struct flash_device {
 	char *name;
+	uint8_t read_cmd;
+	uint8_t qread_cmd;
+	uint8_t pprog_cmd;
 	uint8_t erase_cmd;
 	uint8_t chip_erase_cmd;
 	uint32_t device_id;
 	uint32_t pagesize;
-	unsigned long sectorsize;
-	unsigned long size_in_bytes;
+	uint32_t sectorsize;
+	uint32_t size_in_bytes;
 };
 
-#define FLASH_ID(n, es, ces, id, psize, ssize, size) \
-{	                        \
-	.name = n,              \
-	.erase_cmd = es,        \
-	.chip_erase_cmd = ces,  \
-	.device_id = id,        \
-	.pagesize = psize,      \
-	.sectorsize = ssize,    \
-	.size_in_bytes = size   \
+#define FLASH_ID(n, re, qr, pp, es, ces, id, psize, ssize, size) \
+{	                                \
+	.name = n,                      \
+	.read_cmd = re,                 \
+	.qread_cmd = qr,                \
+	.pprog_cmd = pp,                \
+	.erase_cmd = es,                \
+	.chip_erase_cmd = ces,          \
+	.device_id = id,                \
+	.pagesize = psize,              \
+	.sectorsize = ssize,            \
+	.size_in_bytes = size,          \
+}
+
+#define FRAM_ID(n, re, qr, pp, id, size) \
+{	                                \
+	.name = n,                      \
+	.read_cmd = re,                 \
+	.qread_cmd = qr,                \
+	.pprog_cmd = pp,                \
+	.erase_cmd = 0x00,              \
+	.chip_erase_cmd = 0x00,         \
+	.device_id = id,                \
+	.pagesize = 0,                  \
+	.sectorsize = 0,                \
+	.size_in_bytes = size,          \
 }
 
 extern const struct flash_device flash_devices[];
 
+#endif
+
 /* fields in SPI flash status register */
-#define SPIFLASH_BSY_BIT		0x00000001 /* WIP Bit of SPI SR on SMI SR */
-#define SPIFLASH_WE_BIT			0x00000002 /* WEL Bit of SPI SR on SMI SR */
+#define	SPIFLASH_BSY		0
+#define SPIFLASH_BSY_BIT	(1 << SPIFLASH_BSY)	/* WIP Bit of SPI SR */
+#define	SPIFLASH_WE			1
+#define SPIFLASH_WE_BIT		(1 << SPIFLASH_WE)	/* WEL Bit of SPI SR */
 
 /* SPI Flash Commands */
 #define SPIFLASH_READ_ID		0x9F /* Read Flash Identification */
+#define SPIFLASH_READ_MID		0xAF /* Read Flash Identification, multi-io */
 #define SPIFLASH_READ_STATUS	0x05 /* Read Status Register */
 #define SPIFLASH_WRITE_ENABLE	0x06 /* Write Enable */
 #define SPIFLASH_PAGE_PROGRAM	0x02 /* Page Program */
 #define SPIFLASH_FAST_READ		0x0B /* Fast Read */
 #define SPIFLASH_READ			0x03 /* Normal Read */
 
+#define SPIFLASH_DEF_PAGESIZE	256  /* default for non-page-oriented devices (FRAMs) */
+
 #endif /* OPENOCD_FLASH_NOR_SPI_H */
diff --git a/src/flash/nor/stellaris.c b/src/flash/nor/stellaris.c
index 79aaf3b..8731f8b 100644
--- a/src/flash/nor/stellaris.c
+++ b/src/flash/nor/stellaris.c
@@ -1355,6 +1355,7 @@ COMMAND_HANDLER(stellaris_handle_mass_erase_command)
 COMMAND_HANDLER(stellaris_handle_recover_command)
 {
 	struct flash_bank *bank;
+	struct arm *arm;
 	int retval;
 
 	if (CMD_ARGC != 0)
@@ -1383,12 +1384,13 @@ COMMAND_HANDLER(stellaris_handle_recover_command)
 	}
 	adapter_assert_reset();
 
+	arm = target_to_arm(bank->target);
 	for (int i = 0; i < 5; i++) {
-		retval = dap_to_swd(bank->target);
+		retval = dap_to_swd(arm->dap);
 		if (retval != ERROR_OK)
 			goto done;
 
-		retval = dap_to_jtag(bank->target);
+		retval = dap_to_jtag(arm->dap);
 		if (retval != ERROR_OK)
 			goto done;
 	}
diff --git a/src/flash/nor/stm32f1x.c b/src/flash/nor/stm32f1x.c
index fbd7598..cd060da 100644
--- a/src/flash/nor/stm32f1x.c
+++ b/src/flash/nor/stm32f1x.c
@@ -131,7 +131,7 @@ struct stm32x_flash_bank {
 static int stm32x_mass_erase(struct flash_bank *bank);
 static int stm32x_get_device_id(struct flash_bank *bank, uint32_t *device_id);
 static int stm32x_write_block(struct flash_bank *bank, const uint8_t *buffer,
-		uint32_t offset, uint32_t count);
+		uint32_t address, uint32_t count);
 
 /* flash bank stm32x <base> <size> 0 0 <target#>
  */
@@ -343,8 +343,7 @@ static int stm32x_write_options(struct flash_bank *bank)
 	target_buffer_set_u16(target, opt_bytes + 12, (stm32x_info->option_bytes.protection >> 16) & 0xff);
 	target_buffer_set_u16(target, opt_bytes + 14, (stm32x_info->option_bytes.protection >> 24) & 0xff);
 
-	uint32_t offset = STM32_OB_RDP - bank->base;
-	retval = stm32x_write_block(bank, opt_bytes, offset, sizeof(opt_bytes) / 2);
+	retval = stm32x_write_block(bank, opt_bytes, STM32_OB_RDP, sizeof(opt_bytes) / 2);
 	if (retval != ERROR_OK) {
 		if (retval == ERROR_TARGET_RESOURCE_NOT_AVAILABLE)
 			LOG_ERROR("working area required to erase options bytes");
@@ -443,8 +442,10 @@ static int stm32x_protect(struct flash_bank *bank, int set, int first, int last)
 		return retval;
 
 	retval = stm32x_erase_options(bank);
-	if (retval != ERROR_OK)
+	if (retval != ERROR_OK) {
+		LOG_ERROR("stm32x failed to erase options");
 		return retval;
+	}
 
 	for (int i = first; i <= last; i++) {
 		if (set)
@@ -457,14 +458,13 @@ static int stm32x_protect(struct flash_bank *bank, int set, int first, int last)
 }
 
 static int stm32x_write_block(struct flash_bank *bank, const uint8_t *buffer,
-		uint32_t offset, uint32_t count)
+		uint32_t address, uint32_t count)
 {
 	struct stm32x_flash_bank *stm32x_info = bank->driver_priv;
 	struct target *target = bank->target;
 	uint32_t buffer_size = 16384;
 	struct working_area *write_algorithm;
 	struct working_area *source;
-	uint32_t address = bank->base + offset;
 	struct reg_param reg_params[5];
 	struct armv7m_algorithm armv7m_info;
 	int retval = ERROR_OK;
@@ -599,7 +599,7 @@ static int stm32x_write(struct flash_bank *bank, const uint8_t *buffer,
 		goto cleanup;
 
 	/* try using a block write */
-	retval = stm32x_write_block(bank, buffer, offset, words_remaining);
+	retval = stm32x_write_block(bank, buffer, bank->base + offset, words_remaining);
 
 	if (retval == ERROR_TARGET_RESOURCE_NOT_AVAILABLE) {
 		/* if block write failed (no sufficient working area),
@@ -1227,12 +1227,12 @@ COMMAND_HANDLER(stm32x_handle_unlock_command)
 		return retval;
 
 	if (stm32x_erase_options(bank) != ERROR_OK) {
-		command_print(CMD_CTX, "stm32x failed to unlock device");
+		command_print(CMD_CTX, "stm32x failed to erase options");
 		return ERROR_OK;
 	}
 
 	if (stm32x_write_options(bank) != ERROR_OK) {
-		command_print(CMD_CTX, "stm32x failed to lock device");
+		command_print(CMD_CTX, "stm32x failed to unlock device");
 		return ERROR_OK;
 	}
 
@@ -1313,7 +1313,8 @@ COMMAND_HANDLER(stm32x_handle_options_write_command)
 {
 	struct target *target = NULL;
 	struct stm32x_flash_bank *stm32x_info = NULL;
-	uint16_t optionbyte;
+	uint8_t optionbyte;
+	uint16_t useropt;
 
 	if (CMD_ARGC < 2)
 		return ERROR_COMMAND_SYNTAX_ERROR;
@@ -1342,6 +1343,7 @@ COMMAND_HANDLER(stm32x_handle_options_write_command)
 
 	/* start with current options */
 	optionbyte = stm32x_info->option_bytes.user;
+	useropt = stm32x_info->option_bytes.data;
 
 	/* skip over flash bank */
 	CMD_ARGC--;
@@ -1360,6 +1362,13 @@ COMMAND_HANDLER(stm32x_handle_options_write_command)
 			optionbyte |= (1 << 2);
 		else if (strcmp("RSTSTNDBY", CMD_ARGV[0]) == 0)
 			optionbyte &= ~(1 << 2);
+		else if (strcmp("USEROPT", CMD_ARGV[0]) == 0) {
+			if (CMD_ARGC < 2)
+				return ERROR_COMMAND_SYNTAX_ERROR;
+			COMMAND_PARSE_NUMBER(u16, CMD_ARGV[1], useropt);
+			CMD_ARGC--;
+			CMD_ARGV++;
+		}
 		else if (stm32x_info->has_dual_banks) {
 			if (strcmp("BOOT0", CMD_ARGV[0]) == 0)
 				optionbyte |= (1 << 3);
@@ -1379,6 +1388,7 @@ COMMAND_HANDLER(stm32x_handle_options_write_command)
 	}
 
 	stm32x_info->option_bytes.user = optionbyte;
+	stm32x_info->option_bytes.data = useropt;
 
 	if (stm32x_write_options(bank) != ERROR_OK) {
 		command_print(CMD_CTX, "stm32x failed to write options");
@@ -1536,7 +1546,7 @@ static const struct command_registration stm32x_exec_command_handlers[] = {
 		.mode = COMMAND_EXEC,
 		.usage = "bank_id ('SWWDG'|'HWWDG') "
 			"('RSTSTNDBY'|'NORSTSTNDBY') "
-			"('RSTSTOP'|'NORSTSTOP')",
+			"('RSTSTOP'|'NORSTSTOP') ('USEROPT' user_data)",
 		.help = "Replace bits in device option bytes.",
 	},
 	{
diff --git a/src/flash/nor/stm32l4x.c b/src/flash/nor/stm32l4x.c
index ad17921..7c5811e 100644
--- a/src/flash/nor/stm32l4x.c
+++ b/src/flash/nor/stm32l4x.c
@@ -33,6 +33,9 @@
  * RM0394 (STM32L43x/44x/45x/46x)
  * http://www.st.com/resource/en/reference_manual/dm00151940.pdf
  *
+ * RM0432 (STM32L4R/4Sxx)
+ * http://www.st.com/resource/en/reference_manual/dm00310109.pdf
+ *
  * STM32L476RG Datasheet (for erase timing)
  * http://www.st.com/resource/en/datasheet/stm32l476rg.pdf
  *
@@ -43,6 +46,14 @@
  *
  * RM0394 devices have a single bank only.
  *
+ * RM0432 devices have single and dual bank operating modes.
+ * The FLASH size is 1Mbyte or 2Mbyte.
+ * Bank page (sector) size is 4Kbyte (dual mode) or 8Kbyte (single mode).
+ *
+ * Bank mode is controlled by two different bits in option bytes register.
+ * In 2M FLASH devices bit 22 (DBANK) controls Dual Bank mode.
+ * In 1M FLASH devices bit 21 (DB1M) controls Dual Bank mode.
+ *
  */
 
 /* Erase time can be as high as 25ms, 10x this and assume it's toast... */
@@ -82,7 +93,7 @@
 #define FLASH_BSY      (1 << 16)
 /* Fast programming not used => related errors not used*/
 #define FLASH_PGSERR   (1 << 7) /* Programming sequence error */
-#define FLASH_SIZERR   (1 << 6) /* Size  error */
+#define FLASH_SIZERR   (1 << 6) /* Size error */
 #define FLASH_PGAERR   (1 << 5) /* Programming alignment error */
 #define FLASH_WRPERR   (1 << 4) /* Write protection error */
 #define FLASH_PROGERR  (1 << 3) /* Programming error */
@@ -93,7 +104,8 @@
 
 /* STM32_FLASH_OBR bit definitions (reading) */
 
-#define OPT_DUALBANK   21	/* dual flash bank only */
+#define OPT_DBANK_LE_1M (1 << 21)	/* dual bank for devices up to 1M flash */
+#define OPT_DBANK_GE_2M (1 << 22)	/* dual bank for devices with 2M flash */
 
 /* register unlock keys */
 
@@ -325,7 +337,7 @@ static int stm32l4_protect_check(struct flash_bank *bank)
 				bank->sectors[i].is_protected = 0;
 		} else {
 			uint8_t snb;
-			snb = i - stm32l4_info->bank2_start + 256;
+			snb = i - stm32l4_info->bank2_start;
 			if (((snb >= wrp2a_start) &&
 				 (snb <= wrp2a_end)) ||
 				((snb >= wrp2b_start) &&
@@ -362,7 +374,7 @@ static int stm32l4_erase(struct flash_bank *bank, int first, int last)
 	1. Check that no Flash memory operation is ongoing by
        checking the BSY bit in the FLASH_SR register
 	2. Set the PER bit and select the page and bank
-	   you wish to erase  in the FLASH_CR register
+	   you wish to erase in the FLASH_CR register
 	3. Set the STRT bit in the FLASH_CR register
 	4. Wait for the BSY bit to be cleared
 	 */
@@ -372,9 +384,9 @@ static int stm32l4_erase(struct flash_bank *bank, int first, int last)
 		uint32_t erase_flags;
 		erase_flags = FLASH_PER | FLASH_STRT;
 
-		if  (i >= stm32l4_info->bank2_start) {
+		if (i >= stm32l4_info->bank2_start) {
 			uint8_t snb;
-			snb = (i - stm32l4_info->bank2_start) + 256;
+			snb = i - stm32l4_info->bank2_start;
 			erase_flags |= snb << FLASH_PAGE_SHIFT | FLASH_CR_BKER;
 		} else
 			erase_flags |= i << FLASH_PAGE_SHIFT;
@@ -473,7 +485,7 @@ static int stm32l4_write_block(struct flash_bank *bank, const uint8_t *buffer,
 			 * buffer, free the algorithm */
 			target_free_working_area(target, write_algorithm);
 
-			LOG_WARNING("no large enough working area available, can't do block memory writes");
+			LOG_WARNING("large enough working area not available, can't do block memory writes");
 			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 		}
 	}
@@ -594,6 +606,9 @@ static int stm32l4_probe(struct flash_bank *bank)
 
 	/* set max flash size depending on family */
 	switch (device_id & 0xfff) {
+	case 0x470:
+		max_flash_size_in_kb = 2048;
+		break;
 	case 0x461:
 	case 0x415:
 		max_flash_size_in_kb = 1024;
@@ -605,7 +620,7 @@ static int stm32l4_probe(struct flash_bank *bank)
 		max_flash_size_in_kb = 256;
 		break;
 	default:
-		LOG_WARNING("Cannot identify target as a STM32L4 family.");
+		LOG_WARNING("Cannot identify target as an STM32L4 family device.");
 		return ERROR_FAIL;
 	}
 
@@ -622,45 +637,77 @@ static int stm32l4_probe(struct flash_bank *bank)
 
 	LOG_INFO("flash size = %dkbytes", flash_size_in_kb);
 
-	/* did we assign flash size? */
-	assert(flash_size_in_kb != 0xffff);
+	/* did we assign a flash size? */
+	assert((flash_size_in_kb != 0xffff) && flash_size_in_kb);
 
-	/* get options to for DUAL BANK. */
+	/* get options for DUAL BANK. */
 	retval = target_read_u32(target, STM32_FLASH_OPTR, &options);
 
 	if (retval != ERROR_OK)
 		return retval;
 
-	/* only devices with < 1024 kiB may be set to single bank dual banks */
-	if ((flash_size_in_kb == 1024) || !(options & OPT_DUALBANK))
-		stm32l4_info->bank2_start = 256;
-	else
-		stm32l4_info->bank2_start = flash_size_in_kb << 9;
-
-	/* did we assign flash size? */
-	assert((flash_size_in_kb != 0xffff) && flash_size_in_kb);
-
-	/* calculate numbers of pages */
-	int num_pages = flash_size_in_kb / 2;
+	int num_pages = 0;
+	int page_size = 0;
 
-	/* check that calculation result makes sense */
-	assert(num_pages > 0);
+	switch (device_id & 0xfff) {
+		case 0x470:
+			/* L4R/S have 1M or 2M FLASH and dual/single bank mode.
+			 * Page size is 4K or 8K.*/
+			if (flash_size_in_kb == 2048) {
+				stm32l4_info->bank2_start = 256;
+				if (options & OPT_DBANK_GE_2M) {
+					page_size = 4096;
+					num_pages = 512;
+				} else {
+					page_size = 8192;
+					num_pages = 256;
+				}
+				break;
+			}
+			if (flash_size_in_kb == 1024) {
+				stm32l4_info->bank2_start = 128;
+				if (options & OPT_DBANK_LE_1M) {
+					page_size = 4096;
+					num_pages = 256;
+				} else {
+					page_size = 8192;
+					num_pages = 128;
+				}
+				break;
+			}
+			/* Invalid FLASH size for this device. */
+			LOG_WARNING("Invalid flash size for STM32L4+ family device.");
+			return ERROR_FAIL;
+		default:
+			/* Other L4 family devices have 2K pages. */
+			page_size = 2048;
+			num_pages = flash_size_in_kb / 2;
+			/* check that calculation result makes sense */
+			assert(num_pages > 0);
+			if ((flash_size_in_kb == 1024) || !(options & OPT_DBANK_LE_1M))
+				stm32l4_info->bank2_start = 256;
+			else
+				stm32l4_info->bank2_start = num_pages / 2;
+			break;
+	}
 
+	/* Release sector table if allocated. */
 	if (bank->sectors) {
 		free(bank->sectors);
 		bank->sectors = NULL;
 	}
 
+	/* Set bank configuration and construct sector table. */
 	bank->base = base_address;
-	bank->size = num_pages * (1 << 11);
+	bank->size = num_pages * page_size;
 	bank->num_sectors = num_pages;
 	bank->sectors = malloc(sizeof(struct flash_sector) * num_pages);
 	if (!bank->sectors)
 		return ERROR_FAIL; /* Checkme: What better error to use?*/
 
 	for (i = 0; i < num_pages; i++) {
-		bank->sectors[i].offset = i << 11;
-		bank->sectors[i].size = 1 << 11;
+		bank->sectors[i].offset = i * page_size;
+		bank->sectors[i].size = page_size;
 		bank->sectors[i].is_erased = -1;
 		bank->sectors[i].is_protected = 1;
 	}
@@ -703,6 +750,10 @@ static int get_stm32l4_info(struct flash_bank *bank, char *buf, int buf_size)
 	const char *device_str;
 
 	switch (device_id) {
+	case 0x470:
+		device_str = "STM32L4R/4Sxx";
+		break;
+
 	case 0x461:
 		device_str = "STM32L496/4A6";
 		break;
diff --git a/src/flash/nor/stm32lx.c b/src/flash/nor/stm32lx.c
index 7d43842..255ca3b 100644
--- a/src/flash/nor/stm32lx.c
+++ b/src/flash/nor/stm32lx.c
@@ -424,13 +424,6 @@ static int stm32lx_erase(struct flash_bank *bank, int first, int last)
 	return ERROR_OK;
 }
 
-static int stm32lx_protect(struct flash_bank *bank, int set, int first,
-		int last)
-{
-	LOG_WARNING("protection of the STM32L flash is not implemented");
-	return ERROR_OK;
-}
-
 static int stm32lx_write_half_pages(struct flash_bank *bank, const uint8_t *buffer,
 		uint32_t offset, uint32_t count)
 {
@@ -863,7 +856,7 @@ static int stm32lx_probe(struct flash_bank *bank)
 		bank->sectors[i].offset = i * FLASH_SECTOR_SIZE;
 		bank->sectors[i].size = FLASH_SECTOR_SIZE;
 		bank->sectors[i].is_erased = -1;
-		bank->sectors[i].is_protected = 1;
+		bank->sectors[i].is_protected = -1;
 	}
 
 	stm32lx_info->probed = 1;
@@ -956,7 +949,6 @@ struct flash_driver stm32lx_flash = {
 		.commands = stm32lx_command_handlers,
 		.flash_bank_command = stm32lx_flash_bank_command,
 		.erase = stm32lx_erase,
-		.protect = stm32lx_protect,
 		.write = stm32lx_write,
 		.read = default_flash_read,
 		.probe = stm32lx_probe,
diff --git a/src/flash/nor/stmsmi.c b/src/flash/nor/stmsmi.c
index c454291..7603305 100644
--- a/src/flash/nor/stmsmi.c
+++ b/src/flash/nor/stmsmi.c
@@ -269,17 +269,14 @@ static int smi_write_enable(struct flash_bank *bank)
 static uint32_t erase_command(struct stmsmi_flash_bank *stmsmi_info,
 	uint32_t offset)
 {
-	union {
-		uint32_t command;
-		uint8_t x[4];
-	} cmd;
-
-	cmd.x[0] = stmsmi_info->dev->erase_cmd;
-	cmd.x[1] = offset >> 16;
-	cmd.x[2] = offset >> 8;
-	cmd.x[3] = offset;
-
-	return cmd.command;
+	uint8_t cmd_bytes[] = {
+		stmsmi_info->dev->erase_cmd,
+		offset >> 16,
+		offset >> 8,
+		offset
+	};
+
+	return le_to_h_u32(cmd_bytes);
 }
 
 static int smi_erase_sector(struct flash_bank *bank, int sector)
@@ -348,6 +345,9 @@ static int stmsmi_erase(struct flash_bank *bank, int first, int last)
 		}
 	}
 
+	if (stmsmi_info->dev->erase_cmd == 0x00)
+		return ERROR_FLASH_OPER_UNSUPPORTED;
+
 	for (sector = first; sector <= last; sector++) {
 		retval = smi_erase_sector(bank, sector);
 		if (retval != ERROR_OK)
@@ -431,7 +431,9 @@ static int stmsmi_write(struct flash_bank *bank, const uint8_t *buffer,
 		}
 	}
 
-	page_size = stmsmi_info->dev->pagesize;
+	/* if no valid page_size, use reasonable default */
+	page_size = stmsmi_info->dev->pagesize ?
+		stmsmi_info->dev->pagesize : SPIFLASH_DEF_PAGESIZE;
 
 	/* unaligned buffer head */
 	if (count > 0 && (offset & 3) != 0) {
@@ -524,7 +526,7 @@ static int stmsmi_probe(struct flash_bank *bank)
 {
 	struct target *target = bank->target;
 	struct stmsmi_flash_bank *stmsmi_info = bank->driver_priv;
-	uint32_t io_base;
+	uint32_t io_base, sectorsize;
 	struct flash_sector *sectors;
 	uint32_t id = 0; /* silence uninitialized warning */
 	const struct stmsmi_target *target_device;
@@ -589,10 +591,18 @@ static int stmsmi_probe(struct flash_bank *bank)
 
 	/* Set correct size value */
 	bank->size = stmsmi_info->dev->size_in_bytes;
+	if (bank->size <= (1UL << 16))
+		LOG_WARNING("device needs 2-byte addresses - not implemented");
+	if (bank->size > (1UL << 24))
+		LOG_WARNING("device needs paging or 4-byte addresses - not implemented");
+
+	/* if no sectors, treat whole bank as single sector */
+	sectorsize = stmsmi_info->dev->sectorsize ?
+		stmsmi_info->dev->sectorsize : stmsmi_info->dev->size_in_bytes;
 
 	/* create and fill sectors array */
 	bank->num_sectors =
-		stmsmi_info->dev->size_in_bytes / stmsmi_info->dev->sectorsize;
+		stmsmi_info->dev->size_in_bytes / sectorsize;
 	sectors = malloc(sizeof(struct flash_sector) * bank->num_sectors);
 	if (sectors == NULL) {
 		LOG_ERROR("not enough memory");
@@ -600,8 +610,8 @@ static int stmsmi_probe(struct flash_bank *bank)
 	}
 
 	for (int sector = 0; sector < bank->num_sectors; sector++) {
-		sectors[sector].offset = sector * stmsmi_info->dev->sectorsize;
-		sectors[sector].size = stmsmi_info->dev->sectorsize;
+		sectors[sector].offset = sector * sectorsize;
+		sectors[sector].size = sectorsize;
 		sectors[sector].is_erased = -1;
 		sectors[sector].is_protected = 1;
 	}
diff --git a/src/flash/nor/tcl.c b/src/flash/nor/tcl.c
index 9ed55ec..0e7a34e 100644
--- a/src/flash/nor/tcl.c
+++ b/src/flash/nor/tcl.c
@@ -98,10 +98,18 @@ COMMAND_HANDLER(handle_flash_info_command)
 		if (retval != ERROR_OK)
 			return retval;
 
-		/* We must query the hardware to avoid printing stale information! */
-		retval = p->driver->protect_check(p);
-		if (retval != ERROR_OK)
-			return retval;
+		/* If the driver does not implement protection, we show the default
+		 * state of is_protected array - usually protection state unknown */
+		if (p->driver->protect_check == NULL) {
+			retval = ERROR_FLASH_OPER_UNSUPPORTED;
+		} else {
+			/* We must query the hardware to avoid printing stale information! */
+			retval = p->driver->protect_check(p);
+			if (retval != ERROR_OK && retval != ERROR_FLASH_OPER_UNSUPPORTED)
+				return retval;
+		}
+		if (retval == ERROR_FLASH_OPER_UNSUPPORTED)
+			LOG_WARNING("Flash protection check is not implemented.");
 
 		command_print(CMD_CTX,
 			"#%d : %s at 0x%8.8" TARGET_PRIxADDR ", size 0x%8.8" PRIx32
diff --git a/src/flash/nor/w600.c b/src/flash/nor/w600.c
new file mode 100644
index 0000000..3d37616
--- /dev/null
+++ b/src/flash/nor/w600.c
@@ -0,0 +1,390 @@
+/***************************************************************************
+ *   Copyright (C) 2018 by Simon Qian                                      *
+ *   SimonQian@SimonQian.com                                               *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "imp.h"
+#include <helper/binarybuffer.h>
+#include <target/algorithm.h>
+#include <target/armv7m.h>
+
+#define W600_FLASH_SECSIZE		0x1000
+#define W600_FLASH_PAGESIZE		0x100
+#define W600_FLASH_BASE			0x08000000
+#define W600_FLASH_PROTECT_SIZE	0x2000
+
+/* w600 register locations */
+
+#define QFLASH_REGBASE			0X40002000
+#define QFLASH_CMD_INFO			(QFLASH_REGBASE + 0)
+#define QFLASH_CMD_START		(QFLASH_REGBASE + 4)
+#define QFLASH_BUFFER			(QFLASH_REGBASE + 0X200)
+
+#define QFLASH_CMD_READ			(1ul << 14)
+#define QFLASH_CMD_WRITE		0
+#define QFLASH_CMD_ADDR			(1ul << 31)
+#define QFLASH_CMD_DATA			(1ul << 15)
+#define QFLASH_CMD_DATALEN(len)	(((len) & 0x3FF) << 16)
+
+#define QFLASH_CMD_RDID			(QFLASH_CMD_READ | 0x9F)
+#define QFLASH_CMD_WREN			(QFLASH_CMD_WRITE | 0x06)
+#define QFLASH_CMD_WRDI			(QFLASH_CMD_WRITE | 0x04)
+#define QFLASH_CMD_SE			(QFLASH_CMD_WRITE | QFLASH_CMD_ADDR | (1ul << 11) | 0x20)
+#define QFLASH_CMD_PP			(QFLASH_CMD_WRITE | QFLASH_CMD_ADDR | (1ul << 12) | 0x02)
+
+#define QFLASH_START			(1ul << 28)
+#define QFLASH_ADDR(addr)		(((addr) & 0xFFFFF) << 8)
+#define QFLASH_CRM(crm)			(((crm) & 0xFF) << 0)
+
+struct w600_flash_param {
+	uint8_t id;
+	uint8_t se_delay;
+	uint8_t pp_delay;
+};
+static const struct w600_flash_param w600_param[] = {
+	{
+		.id = 0x85,
+		.se_delay = 8,
+		.pp_delay = 2,
+	},
+	{
+		.id = 0x1C,
+		.se_delay = 50,
+		.pp_delay = 1,
+	},
+	{
+		.id = 0xC8,
+		.se_delay = 45,
+		.pp_delay = 1,
+	},
+	{
+		.id = 0x0B,
+		.se_delay = 60,
+		.pp_delay = 1,
+	},
+	{
+		.id = 0x68,
+		.se_delay = 50,
+		.pp_delay = 1,
+	},
+};
+
+struct w600_flash_bank {
+	int probed;
+
+	uint32_t id;
+	const struct w600_flash_param *param;
+	uint32_t register_base;
+	uint32_t user_bank_size;
+};
+
+/* flash bank w600 <base> <size> 0 0 <target#>
+ */
+FLASH_BANK_COMMAND_HANDLER(w600_flash_bank_command)
+{
+	struct w600_flash_bank *w600_info;
+
+	if (CMD_ARGC < 6)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	w600_info = malloc(sizeof(struct w600_flash_bank));
+
+	bank->driver_priv = w600_info;
+	w600_info->probed = 0;
+	w600_info->register_base = QFLASH_REGBASE;
+	w600_info->user_bank_size = bank->size;
+
+	return ERROR_OK;
+}
+
+static int w600_get_delay(struct flash_bank *bank, uint32_t cmd)
+{
+	struct w600_flash_bank *w600_info = bank->driver_priv;
+
+	if (!w600_info->param)
+		return 0;
+
+	switch (cmd) {
+	case QFLASH_CMD_SE:
+		return w600_info->param->se_delay;
+	case QFLASH_CMD_PP:
+		return w600_info->param->pp_delay;
+	default:
+		return 0;
+	}
+}
+
+static int w600_start_do(struct flash_bank *bank, uint32_t cmd, uint32_t addr,
+		uint32_t len, int timeout)
+{
+	struct target *target = bank->target;
+
+	if (len > 0)
+		cmd |= QFLASH_CMD_DATALEN(len - 1) | QFLASH_CMD_DATA;
+
+	LOG_DEBUG("WRITE CMD: 0x%08" PRIx32 "", cmd);
+	int retval = target_write_u32(target, QFLASH_CMD_INFO, cmd);
+	if (retval != ERROR_OK)
+		return retval;
+
+	addr |= QFLASH_START;
+	LOG_DEBUG("WRITE START: 0x%08" PRIx32 "", addr);
+	retval = target_write_u32(target, QFLASH_CMD_START, addr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	LOG_DEBUG("DELAY %dms", timeout);
+	alive_sleep(timeout);
+
+	int retry = 100;
+	uint32_t status;
+	for (;;) {
+		LOG_DEBUG("READ START...");
+		retval = target_read_u32(target, QFLASH_CMD_START, &status);
+		if (retval == ERROR_OK)
+			LOG_DEBUG("READ START: 0x%08" PRIx32 "", status);
+		else
+			LOG_DEBUG("READ START FAILED");
+
+		if ((retval != ERROR_OK) || (status & QFLASH_START)) {
+			if (retry-- <= 0) {
+				LOG_ERROR("timed out waiting for flash");
+				return ERROR_FAIL;
+			}
+			continue;
+		}
+		break;
+	}
+
+	return retval;
+}
+
+static int w600_write_enable(struct flash_bank *bank)
+{
+	return w600_start_do(bank, QFLASH_CMD_WREN, 0, 0, 0);
+}
+
+static int w600_write_disable(struct flash_bank *bank)
+{
+	return w600_start_do(bank, QFLASH_CMD_WRDI, 0, 0, 0);
+}
+
+static int w600_start(struct flash_bank *bank, uint32_t cmd, uint32_t addr,
+		uint32_t len)
+{
+	int retval = w600_write_enable(bank);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = w600_start_do(bank, cmd, addr, len, w600_get_delay(bank, cmd));
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = w600_write_disable(bank);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return retval;
+}
+
+static int w600_erase(struct flash_bank *bank, int first, int last)
+{
+	int retval = ERROR_OK;
+
+	if (bank->target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+	if (first < W600_FLASH_PROTECT_SIZE / W600_FLASH_SECSIZE) {
+		LOG_ERROR("can not erase protected area");
+		return ERROR_FAIL;
+	}
+
+	for (int i = first; i <= last; i++) {
+		retval = w600_start(bank, QFLASH_CMD_SE,
+			QFLASH_ADDR(bank->sectors[i].offset), 0);
+		if (retval != ERROR_OK)
+			break;
+	}
+
+	return retval;
+}
+
+static int w600_write(struct flash_bank *bank, const uint8_t *buffer,
+		uint32_t offset, uint32_t count)
+{
+	struct target *target = bank->target;
+	int retval = ERROR_OK;
+
+	if (bank->target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if ((offset % W600_FLASH_PAGESIZE) != 0) {
+		LOG_WARNING("offset 0x%" PRIx32 " breaks required %" PRIu32 "-byte alignment",
+			offset, W600_FLASH_PAGESIZE);
+		return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
+	}
+
+	if ((count % W600_FLASH_PAGESIZE) != 0) {
+		LOG_WARNING("count 0x%" PRIx32 " breaks required %" PRIu32 "-byte alignment",
+			offset, W600_FLASH_PAGESIZE);
+		return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
+	}
+
+	while (count > 0) {
+		retval = target_write_buffer(target, QFLASH_BUFFER, W600_FLASH_PAGESIZE, buffer);
+		if (retval != ERROR_OK)
+			break;
+
+		retval = w600_start(bank, QFLASH_CMD_PP, QFLASH_ADDR(offset),
+				W600_FLASH_PAGESIZE);
+		if (retval != ERROR_OK)
+			break;
+
+		count -= W600_FLASH_PAGESIZE;
+		offset += W600_FLASH_PAGESIZE;
+		buffer += W600_FLASH_PAGESIZE;
+	}
+
+	return retval;
+}
+
+static int w600_get_flash_id(struct flash_bank *bank, uint32_t *flash_id)
+{
+	struct target *target = bank->target;
+
+	int retval = w600_start(bank, QFLASH_CMD_RDID, 0, 4);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return target_read_u32(target, QFLASH_BUFFER, flash_id);
+}
+
+static int w600_probe(struct flash_bank *bank)
+{
+	struct w600_flash_bank *w600_info = bank->driver_priv;
+	uint32_t flash_size;
+	uint32_t flash_id;
+	size_t i;
+
+	w600_info->probed = 0;
+
+	/* read stm32 device id register */
+	int retval = w600_get_flash_id(bank, &flash_id);
+	if (retval != ERROR_OK)
+		return retval;
+
+	LOG_INFO("flash_id id = 0x%08" PRIx32 "", flash_id);
+	w600_info->id = flash_id;
+	w600_info->param = NULL;
+	for (i = 0; i < ARRAY_SIZE(w600_param); i++) {
+		if (w600_param[i].id == (flash_id & 0xFF)) {
+			w600_info->param = &w600_param[i];
+			break;
+		}
+	}
+	if (!w600_info->param) {
+		LOG_ERROR("flash_id not supported for w600");
+		return ERROR_FAIL;
+	}
+
+	/* if the user sets the size manually then ignore the probed value
+	 * this allows us to work around devices that have a invalid flash size register value */
+	if (w600_info->user_bank_size) {
+		LOG_INFO("ignoring flash probed value, using configured bank size");
+		flash_size = w600_info->user_bank_size;
+	} else {
+		flash_size = ((flash_id & 0xFFFFFF) >> 16) & 0xFF;
+		if ((flash_size != 0x14) && (flash_size != 0x13)) {
+			LOG_ERROR("w600 flash size failed, probe inaccurate");
+			return ERROR_FAIL;
+		}
+
+		flash_size = 1 << flash_size;
+	}
+
+	LOG_INFO("flash size = %dkbytes", flash_size / 1024);
+
+	/* calculate numbers of pages */
+	size_t num_pages = flash_size / W600_FLASH_SECSIZE;
+
+	/* check that calculation result makes sense */
+	assert(num_pages > 0);
+
+	if (bank->sectors) {
+		free(bank->sectors);
+		bank->sectors = NULL;
+	}
+
+	bank->base = W600_FLASH_BASE;
+	bank->size = num_pages * W600_FLASH_SECSIZE;
+	bank->num_sectors = num_pages;
+	bank->write_start_alignment = W600_FLASH_PAGESIZE;
+	bank->write_end_alignment = W600_FLASH_PAGESIZE;
+	bank->sectors = malloc(sizeof(struct flash_sector) * num_pages);
+
+	for (i = 0; i < num_pages; i++) {
+		bank->sectors[i].offset = i * W600_FLASH_SECSIZE;
+		bank->sectors[i].size = W600_FLASH_SECSIZE;
+		bank->sectors[i].is_erased = -1;
+		/* offset 0 to W600_FLASH_PROTECT_SIZE shoule be protected */
+		bank->sectors[i].is_protected = (i < W600_FLASH_PROTECT_SIZE / W600_FLASH_SECSIZE);
+	}
+
+	w600_info->probed = 1;
+
+	return ERROR_OK;
+}
+
+static int w600_auto_probe(struct flash_bank *bank)
+{
+	struct w600_flash_bank *w600_info = bank->driver_priv;
+	if (w600_info->probed)
+		return ERROR_OK;
+	return w600_probe(bank);
+}
+
+static int get_w600_info(struct flash_bank *bank, char *buf, int buf_size)
+{
+	uint32_t flash_id;
+
+	/* read w600 device id register */
+	int retval = w600_get_flash_id(bank, &flash_id);
+	if (retval != ERROR_OK)
+		return retval;
+
+	snprintf(buf, buf_size, "w600 : 0x%08" PRIx32 "", flash_id);
+	return ERROR_OK;
+}
+
+struct flash_driver w600_flash = {
+	.name = "w600",
+	.flash_bank_command = w600_flash_bank_command,
+	.erase = w600_erase,
+	.write = w600_write,
+	.read = default_flash_read,
+	.probe = w600_probe,
+	.auto_probe = w600_auto_probe,
+	.erase_check = default_flash_blank_check,
+	.info = get_w600_info,
+	.free_driver_priv = default_flash_free_driver_priv,
+};
diff --git a/src/helper/command.c b/src/helper/command.c
index f8b731e..0afbe2c 100644
--- a/src/helper/command.c
+++ b/src/helper/command.c
@@ -557,6 +557,10 @@ static char *__command_name(struct command *c, char delim, unsigned extra)
 	if (NULL == c->parent) {
 		/* allocate enough for the name, child names, and '\0' */
 		name = malloc(len + extra + 1);
+		if (!name) {
+			LOG_ERROR("Out of memory");
+			return NULL;
+		}
 		strcpy(name, c->name);
 	} else {
 		/* parent's extra must include both the space and name */
@@ -575,31 +579,35 @@ char *command_name(struct command *c, char delim)
 
 static bool command_can_run(struct command_context *cmd_ctx, struct command *c)
 {
-	return c->mode == COMMAND_ANY || c->mode == cmd_ctx->mode;
+	if (c->mode == COMMAND_ANY || c->mode == cmd_ctx->mode)
+		return true;
+
+	/* Many commands may be run only before/after 'init' */
+	const char *when;
+	switch (c->mode) {
+		case COMMAND_CONFIG:
+			when = "before";
+			break;
+		case COMMAND_EXEC:
+			when = "after";
+			break;
+		/* handle the impossible with humor; it guarantees a bug report! */
+		default:
+			when = "if Cthulhu is summoned by";
+			break;
+	}
+	char *full_name = command_name(c, ' ');
+	LOG_ERROR("The '%s' command must be used %s 'init'.",
+			full_name ? full_name : c->name, when);
+	free(full_name);
+	return false;
 }
 
 static int run_command(struct command_context *context,
 	struct command *c, const char *words[], unsigned num_words)
 {
-	if (!command_can_run(context, c)) {
-		/* Many commands may be run only before/after 'init' */
-		const char *when;
-		switch (c->mode) {
-			case COMMAND_CONFIG:
-				when = "before";
-				break;
-			case COMMAND_EXEC:
-				when = "after";
-				break;
-			/* handle the impossible with humor; it guarantees a bug report! */
-			default:
-				when = "if Cthulhu is summoned by";
-				break;
-		}
-		LOG_ERROR("The '%s' command must be used %s 'init'.",
-			c->name, when);
+	if (!command_can_run(context, c))
 		return ERROR_FAIL;
-	}
 
 	struct command_invocation cmd = {
 		.ctx = context,
@@ -631,15 +639,17 @@ static int run_command(struct command_context *context,
 		if (NULL != full_name) {
 			command_run_linef(context, "usage %s", full_name);
 			free(full_name);
-		} else
-			retval = -ENOMEM;
+		}
 	} else if (retval == ERROR_COMMAND_CLOSE_CONNECTION) {
 		/* just fall through for a shutdown request */
 	} else if (retval != ERROR_OK) {
 		/* we do not print out an error message because the command *should*
 		 * have printed out an error
 		 */
-		LOG_DEBUG("Command failed with error code %d", retval);
+		char *full_name = command_name(c, ' ');
+		LOG_DEBUG("Command '%s' failed with error code %d",
+					full_name ? full_name : c->name, retval);
+		free(full_name);
 	}
 
 	return retval;
@@ -870,7 +880,7 @@ static COMMAND_HELPER(command_help_show, struct command *c, unsigned n,
 {
 	char *cmd_name = command_name(c, ' ');
 	if (NULL == cmd_name)
-		return -ENOMEM;
+		return ERROR_FAIL;
 
 	/* If the match string occurs anywhere, we print out
 	 * stuff for this command. */
@@ -1026,6 +1036,9 @@ static int command_unknown(Jim_Interp *interp, int argc, Jim_Obj *const *argv)
 	}
 	/* pass the command through to the intended handler */
 	if (c->jim_handler) {
+		if (!command_can_run(cmd_ctx, c))
+			return ERROR_FAIL;
+
 		interp->cmdPrivData = c->jim_handler_data;
 		return (*c->jim_handler)(interp, count, start);
 	}
diff --git a/src/helper/log.c b/src/helper/log.c
index c8a3a6c..f980774 100644
--- a/src/helper/log.c
+++ b/src/helper/log.c
@@ -61,46 +61,16 @@ static const char * const log_strings[6] = {
 
 static int count;
 
-static struct store_log_forward *log_head;
-static int log_forward_count;
-
-struct store_log_forward {
-	struct store_log_forward *next;
-	const char *file;
-	int line;
-	const char *function;
-	const char *string;
-};
-
-/* either forward the log to the listeners or store it for possible forwarding later */
+/* forward the log to the listeners */
 static void log_forward(const char *file, unsigned line, const char *function, const char *string)
 {
-	if (log_forward_count == 0) {
-		struct log_callback *cb, *next;
-		cb = log_callbacks;
-		/* DANGER!!!! the log callback can remove itself!!!! */
-		while (cb) {
-			next = cb->next;
-			cb->fn(cb->priv, file, line, function, string);
-			cb = next;
-		}
-	} else {
-		struct store_log_forward *log = malloc(sizeof(struct store_log_forward));
-		log->file = strdup(file);
-		log->line = line;
-		log->function = strdup(function);
-		log->string = strdup(string);
-		log->next = NULL;
-		if (log_head == NULL)
-			log_head = log;
-		else {
-			/* append to tail */
-			struct store_log_forward *t;
-			t = log_head;
-			while (t->next != NULL)
-				t = t->next;
-			t->next = log;
-		}
+	struct log_callback *cb, *next;
+	cb = log_callbacks;
+	/* DANGER!!!! the log callback can remove itself!!!! */
+	while (cb) {
+		next = cb->next;
+		cb->fn(cb->priv, file, line, function, string);
+		cb = next;
 	}
 }
 
diff --git a/src/helper/startup.tcl b/src/helper/startup.tcl
index 4ca2cab..2578de9 100644
--- a/src/helper/startup.tcl
+++ b/src/helper/startup.tcl
@@ -12,15 +12,18 @@ proc exit {} {
 
 # All commands are registered with an 'ocd_' prefix, while the "real"
 # command is a wrapper that calls this function.  Its primary purpose is
-# to discard 'handler' command output,
+# to discard 'handler' command output.
+# Due to the two nested proc calls, this wrapper has to explicitly run
+# the wrapped command in the stack frame two levels above.
 proc ocd_bouncer {name args} {
 	set cmd [format "ocd_%s" $name]
 	set type [eval ocd_command type $cmd $args]
 	set errcode error
+	set skiplevel [expr [eval info level] > 1 ? 2 : 1]
 	if {$type == "native"} {
-		return [eval $cmd $args]
+		return [uplevel $skiplevel $cmd $args]
 	} else {if {$type == "simple"} {
-		set errcode [catch {eval $cmd $args}]
+		set errcode [catch {uplevel $skiplevel $cmd $args}]
 		if {$errcode == 0} {
 			return ""
 		} else {
diff --git a/src/jtag/adapter.c b/src/jtag/adapter.c
index 2035788..3fb52a7 100644
--- a/src/jtag/adapter.c
+++ b/src/jtag/adapter.c
@@ -35,6 +35,7 @@
 #include "interface.h"
 #include "interfaces.h"
 #include <transport/transport.h>
+#include <jtag/drivers/jtag_usb_common.h>
 
 #ifdef HAVE_STRINGS_H
 #include <strings.h>
@@ -456,8 +457,55 @@ COMMAND_HANDLER(handle_adapter_khz_command)
 	return retval;
 }
 
+#ifndef HAVE_JTAG_MINIDRIVER_H
+#ifdef HAVE_LIBUSB_GET_PORT_NUMBERS
+COMMAND_HANDLER(handle_usb_location_command)
+{
+	if (CMD_ARGC == 1)
+		jtag_usb_set_location(CMD_ARGV[0]);
+
+	command_print(CMD_CTX, "adapter usb location: %s", jtag_usb_get_location());
+
+	return ERROR_OK;
+}
+#endif /* HAVE_LIBUSB_GET_PORT_NUMBERS */
+
+static const struct command_registration adapter_usb_command_handlers[] = {
+#ifdef HAVE_LIBUSB_GET_PORT_NUMBERS
+	{
+		.name = "location",
+		.handler = &handle_usb_location_command,
+		.mode = COMMAND_CONFIG,
+		.help = "set the USB bus location of the USB device",
+		.usage = "<bus>-port[.port]...",
+	},
+#endif /* HAVE_LIBUSB_GET_PORT_NUMBERS */
+	COMMAND_REGISTRATION_DONE
+};
+#endif /* MINIDRIVER */
+
+static const struct command_registration adapter_command_handlers[] = {
+#ifndef HAVE_JTAG_MINIDRIVER_H
+	{
+		.name = "usb",
+		.mode = COMMAND_ANY,
+		.help = "usb adapter command group",
+		.usage = "",
+		.chain = adapter_usb_command_handlers,
+	},
+#endif /* MINIDRIVER */
+	COMMAND_REGISTRATION_DONE
+};
+
 static const struct command_registration interface_command_handlers[] = {
 	{
+		.name = "adapter",
+		.mode = COMMAND_ANY,
+		.help = "adapter command group",
+		.usage = "",
+		.chain = adapter_command_handlers,
+	},
+	{
 		.name = "adapter_khz",
 		.handler = handle_adapter_khz_command,
 		.mode = COMMAND_ANY,
diff --git a/src/jtag/aice/aice_transport.c b/src/jtag/aice/aice_transport.c
index 57c93f2..0658318 100644
--- a/src/jtag/aice/aice_transport.c
+++ b/src/jtag/aice/aice_transport.c
@@ -340,7 +340,7 @@ aice_transport_jtag_subcommand_handlers[] = {
 	},
 	{
 		.name = "configure",
-		.mode = COMMAND_EXEC,
+		.mode = COMMAND_ANY,
 		.jim_handler = jim_jtag_configure,
 		.help = "Provide a Tcl handler for the specified "
 			"TAP event.",
diff --git a/src/jtag/aice/aice_usb.c b/src/jtag/aice/aice_usb.c
index d77b26b..f67ea7c 100644
--- a/src/jtag/aice/aice_usb.c
+++ b/src/jtag/aice/aice_usb.c
@@ -2139,11 +2139,8 @@ static int aice_usb_open(struct aice_port_param_s *param)
 
 	unsigned int aice_read_ep;
 	unsigned int aice_write_ep;
-#ifdef HAVE_LIBUSB1
+
 	jtag_libusb_choose_interface(devh, &aice_read_ep, &aice_write_ep, -1, -1, -1, LIBUSB_TRANSFER_TYPE_BULK);
-#else
-	jtag_libusb_choose_interface(devh, &aice_read_ep, &aice_write_ep, -1, -1, -1, USB_ENDPOINT_TYPE_BULK);
-#endif
 	LOG_DEBUG("aice_read_ep=0x%x, aice_write_ep=0x%x", aice_read_ep, aice_write_ep);
 
 	aice_handler.usb_read_ep = aice_read_ep;
diff --git a/src/jtag/drivers/Makefile.am b/src/jtag/drivers/Makefile.am
index ccef018..572cd24 100644
--- a/src/jtag/drivers/Makefile.am
+++ b/src/jtag/drivers/Makefile.am
@@ -19,6 +19,7 @@ DRIVERFILES =
 
 # Standard Driver: common files
 DRIVERFILES += %D%/driver.c
+DRIVERFILES += %D%/jtag_usb_common.c
 
 if USE_LIBUSB1
 DRIVERFILES += %D%/libusb1_common.c
@@ -166,6 +167,7 @@ endif
 DRIVERHEADERS = \
 	%D%/bitbang.h \
 	%D%/bitq.h \
+	%D%/jtag_usb_common.h \
 	%D%/libusb0_common.h \
 	%D%/libusb1_common.h \
 	%D%/libusb_common.h \
diff --git a/src/jtag/drivers/bitbang.c b/src/jtag/drivers/bitbang.c
index 722a5f2..da4fb33 100644
--- a/src/jtag/drivers/bitbang.c
+++ b/src/jtag/drivers/bitbang.c
@@ -30,9 +30,6 @@
 #include <jtag/interface.h>
 #include <jtag/commands.h>
 
-/* YUK! - but this is currently a global.... */
-extern struct jtag_interface *jtag_interface;
-
 /**
  * Function bitbang_stableclocks
  * issues a number of clock cycles while staying in a stable state.
diff --git a/src/jtag/drivers/cmsis_dap_usb.c b/src/jtag/drivers/cmsis_dap_usb.c
index 035cc44..bd8d0be 100644
--- a/src/jtag/drivers/cmsis_dap_usb.c
+++ b/src/jtag/drivers/cmsis_dap_usb.c
@@ -1096,7 +1096,7 @@ static int cmsis_dap_init(void)
 
 	if (jtag_reset_config & RESET_CNCT_UNDER_SRST) {
 		if (jtag_reset_config & RESET_SRST_NO_GATING) {
-			retval = cmsis_dap_cmd_DAP_SWJ_Pins(0, (1 << 7), 0, NULL);
+			retval = cmsis_dap_cmd_DAP_SWJ_Pins(0, SWJ_PIN_SRST, 0, NULL);
 			if (retval != ERROR_OK)
 				return ERROR_FAIL;
 			LOG_INFO("Connecting under reset");
@@ -1670,6 +1670,30 @@ COMMAND_HANDLER(cmsis_dap_handle_info_command)
 	return ERROR_OK;
 }
 
+COMMAND_HANDLER(cmsis_dap_handle_cmd_command)
+{
+	int retval;
+	unsigned i;
+	uint8_t *buffer = cmsis_dap_handle->packet_buffer;
+
+	buffer[0] = 0;	/* report number */
+
+	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);
+
+	if (retval != ERROR_OK) {
+		LOG_ERROR("CMSIS-DAP command failed.");
+		return ERROR_JTAG_DEVICE_ERROR;
+	}
+
+	LOG_INFO("Returned data %02" PRIx8 " %02" PRIx8 " %02" PRIx8 " %02" PRIx8,
+		buffer[1], buffer[2], buffer[3], buffer[4]);
+
+	return ERROR_OK;
+}
+
 COMMAND_HANDLER(cmsis_dap_handle_vid_pid_command)
 {
 	if (CMD_ARGC > MAX_USB_IDS * 2) {
@@ -1729,6 +1753,13 @@ static const struct command_registration cmsis_dap_subcommand_handlers[] = {
 		.usage = "",
 		.help = "show cmsis-dap info",
 	},
+	{
+		.name = "cmd",
+		.handler = &cmsis_dap_handle_cmd_command,
+		.mode = COMMAND_EXEC,
+		.usage = "",
+		.help = "issue cmsis-dap command",
+	},
 	COMMAND_REGISTRATION_DONE
 };
 
diff --git a/src/jtag/drivers/ft232r.c b/src/jtag/drivers/ft232r.c
index fc3d75f..e16bcdc 100644
--- a/src/jtag/drivers/ft232r.c
+++ b/src/jtag/drivers/ft232r.c
@@ -39,24 +39,9 @@
 #include <time.h>
 
 /*
- *   Bit 7 (0x80, pin 6, RI ): unused.
- *   Bit 6 (0x40, pin 10,DCD): /SYSRST output.
- *   Bit 5 (0x20, pin 9, DSR): unused.
- *   Bit 4 (0x10, pin 2, DTR): /TRST output.
- *   Bit 3 (0x08, pin 11,CTS): TMS output.
- *   Bit 2 (0x04, pin 3, RTS): TDO input.
- *   Bit 1 (0x02, pin 5, RXD): TDI output.
- *   Bit 0 (0x01, pin 1, TXD): TCK output.
- *
  * Sync bit bang mode is implemented as described in FTDI Application
  * Note AN232R-01: "Bit Bang Modes for the FT232R and FT245R".
  */
-#define TCK			(1 << 0)
-#define TDI			(1 << 1)
-#define READ_TDO		(1 << 2)
-#define TMS			(1 << 3)
-#define NTRST			(1 << 4)
-#define NSYSRST			(1 << 6)
 
 /*
  * USB endpoints.
@@ -81,7 +66,7 @@
 #define SIO_WRITE_EEPROM	0x91
 #define SIO_ERASE_EEPROM	0x92
 
-#define FT232R_BUF_SIZE		4000
+#define FT232R_BUF_SIZE_EXTRA	4096
 
 static char *ft232r_serial_desc;
 static uint16_t ft232r_vid = 0x0403; /* FTDI */
@@ -92,6 +77,33 @@ static uint8_t *ft232r_output;
 static size_t ft232r_output_len;
 
 /**
+ * FT232R GPIO bit number to RS232 name
+ */
+#define FT232R_BIT_COUNT 8
+static char *ft232r_bit_name_array[FT232R_BIT_COUNT] = {
+	"TXD", /* 0: pin 1  TCK output */
+	"RXD", /* 1: pin 5  TDI output */
+	"RTS", /* 2: pin 3  TDO input */
+	"CTS", /* 3: pin 11 TMS output */
+	"DTR", /* 4: pin 2  /TRST output */
+	"DSR", /* 5: pin 9  unused */
+	"DCD", /* 6: pin 10 /SYSRST output */
+	"RI"   /* 7: pin 6  unused */
+};
+
+static int tck_gpio; /* initialized to 0 by default */
+static int tdi_gpio = 1;
+static int tdo_gpio = 2;
+static int tms_gpio = 3;
+static int ntrst_gpio = 4;
+static int nsysrst_gpio = 6;
+static size_t ft232r_buf_size = FT232R_BUF_SIZE_EXTRA;
+/** 0xFFFF disables restore by default, after exit serial port will not work.
+ *  0x15 sets TXD RTS DTR as outputs, after exit serial port will continue to work.
+ */
+static uint16_t ft232r_restore_bitmode = 0xFFFF;
+
+/**
  * Perform sync bitbang output/input transaction.
  * Before call, an array ft232r_output[] should be filled with data to send.
  * Counter ft232r_output_len contains the number of bytes to send.
@@ -160,20 +172,35 @@ static int ft232r_send_recv(void)
 	return ERROR_OK;
 }
 
+void ft232r_increase_buf_size(size_t new_buf_size)
+{
+	uint8_t *new_buf_ptr;
+	if (new_buf_size >= ft232r_buf_size) {
+		new_buf_size += FT232R_BUF_SIZE_EXTRA;
+		new_buf_ptr = realloc(ft232r_output, new_buf_size);
+		if (new_buf_ptr != NULL) {
+			ft232r_output = new_buf_ptr;
+			ft232r_buf_size = new_buf_size;
+		}
+	}
+}
+
 /**
  * Add one TCK/TMS/TDI sample to send buffer.
  */
 static void ft232r_write(int tck, int tms, int tdi)
 {
-	unsigned out_value = NTRST | NSYSRST;
+	unsigned out_value = (1<<ntrst_gpio) | (1<<nsysrst_gpio);
 	if (tck)
-		out_value |= TCK;
+		out_value |= (1<<tck_gpio);
 	if (tms)
-		out_value |= TMS;
+		out_value |= (1<<tms_gpio);
 	if (tdi)
-		out_value |= TDI;
+		out_value |= (1<<tdi_gpio);
 
-	if (ft232r_output_len >= FT232R_BUF_SIZE) {
+	ft232r_increase_buf_size(ft232r_output_len);
+
+	if (ft232r_output_len >= ft232r_buf_size) {
 		/* FIXME: should we just execute queue here? */
 		LOG_ERROR("ft232r_write: buffer overflow");
 		return;
@@ -187,20 +214,22 @@ static void ft232r_write(int tck, int tms, int tdi)
  */
 static void ft232r_reset(int trst, int srst)
 {
-	unsigned out_value = NTRST | NSYSRST;
+	unsigned out_value = (1<<ntrst_gpio) | (1<<nsysrst_gpio);
 	LOG_DEBUG("ft232r_reset(%d,%d)", trst, srst);
 
 	if (trst == 1)
-		out_value &= ~NTRST;		/* switch /TRST low */
+		out_value &= ~(1<<ntrst_gpio);		/* switch /TRST low */
 	else if (trst == 0)
-		out_value |= NTRST;			/* switch /TRST high */
+		out_value |= (1<<ntrst_gpio);			/* switch /TRST high */
 
 	if (srst == 1)
-		out_value &= ~NSYSRST;		/* switch /SYSRST low */
+		out_value &= ~(1<<nsysrst_gpio);		/* switch /SYSRST low */
 	else if (srst == 0)
-		out_value |= NSYSRST;		/* switch /SYSRST high */
+		out_value |= (1<<nsysrst_gpio);		/* switch /SYSRST high */
+
+	ft232r_increase_buf_size(ft232r_output_len);
 
-	if (ft232r_output_len >= FT232R_BUF_SIZE) {
+	if (ft232r_output_len >= ft232r_buf_size) {
 		/* FIXME: should we just execute queue here? */
 		LOG_ERROR("ft232r_write: buffer overflow");
 		return;
@@ -236,7 +265,10 @@ static int ft232r_init(void)
 		return ERROR_JTAG_INIT_FAILED;
 	}
 
-	libusb_detach_kernel_driver(adapter, 0);
+	if (ft232r_restore_bitmode == 0xFFFF) /* serial port will not be restored after jtag: */
+		libusb_detach_kernel_driver(adapter, 0);
+	else /* serial port will be restored after jtag: */
+		libusb_set_auto_detach_kernel_driver(adapter, 1); /* 1: DONT_DETACH_SIO_MODULE */
 
 	if (jtag_libusb_claim_interface(adapter, 0)) {
 		LOG_ERROR("unable to claim interface");
@@ -254,7 +286,7 @@ static int ft232r_init(void)
 	/* Sync bit bang mode. */
 	if (jtag_libusb_control_transfer(adapter,
 		LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE | LIBUSB_ENDPOINT_OUT,
-		SIO_SET_BITMODE, TCK | TDI | TMS | NTRST | NSYSRST | 0x400,
+		SIO_SET_BITMODE, (1<<tck_gpio) | (1<<tdi_gpio) | (1<<tms_gpio) | (1<<ntrst_gpio) | (1<<nsysrst_gpio) | 0x400,
 		0, 0, 0, 1000) != 0) {
 		LOG_ERROR("cannot set sync bitbang mode");
 		return ERROR_JTAG_INIT_FAILED;
@@ -279,7 +311,7 @@ static int ft232r_init(void)
 		return ERROR_JTAG_INIT_FAILED;
 	}
 
-	ft232r_output = malloc(FT232R_BUF_SIZE);
+	ft232r_output = malloc(ft232r_buf_size);
 	if (ft232r_output == NULL) {
 		LOG_ERROR("Unable to allocate memory for the buffer");
 		return ERROR_JTAG_INIT_FAILED;
@@ -290,11 +322,24 @@ static int ft232r_init(void)
 
 static int ft232r_quit(void)
 {
+	/* to restore serial port: set TXD RTS DTR as outputs, others as inputs, disable sync bit bang mode. */
+	if (ft232r_restore_bitmode != 0xFFFF) {
+		if (jtag_libusb_control_transfer(adapter,
+			LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE | LIBUSB_ENDPOINT_OUT,
+			SIO_SET_BITMODE, ft232r_restore_bitmode,
+			0, 0, 0, 1000) != 0) {
+			LOG_ERROR("cannot set bitmode to restore serial port");
+		}
+	}
+
 	if (jtag_libusb_release_interface(adapter, 0) != 0)
 		LOG_ERROR("usb release interface failed");
 
 	jtag_libusb_close(adapter);
-	free(ft232r_output);
+
+	free(ft232r_output); /* free used memory */
+	ft232r_output = NULL; /* reset pointer to memory */
+	ft232r_buf_size = FT232R_BUF_SIZE_EXTRA; /* reset next initial buffer size */
 
 	return ERROR_OK;
 }
@@ -331,6 +376,27 @@ static int ft232r_khz(int khz, int *divisor)
 	return ERROR_OK;
 }
 
+static char *ft232r_bit_number_to_name(int bit)
+{
+	if (bit >= 0 && bit < FT232R_BIT_COUNT)
+		return ft232r_bit_name_array[bit];
+	return "?";
+}
+
+static int ft232r_bit_name_to_number(const char *name)
+{
+	int i;
+	if (name[0] >= '0' && name[0] <= '9' && name[1] == '\0') {
+		i = atoi(name);
+		if (i >= 0 && i < FT232R_BIT_COUNT)
+			return i;
+	}
+	for (i = 0; i < FT232R_BIT_COUNT; i++)
+		if (strcasecmp(name, ft232r_bit_name_array[i]) == 0)
+			return i;
+	return -1;
+}
+
 COMMAND_HANDLER(ft232r_handle_serial_desc_command)
 {
 	if (CMD_ARGC == 1)
@@ -357,6 +423,145 @@ COMMAND_HANDLER(ft232r_handle_vid_pid_command)
 	return ERROR_OK;
 }
 
+COMMAND_HANDLER(ft232r_handle_jtag_nums_command)
+{
+	if (CMD_ARGC == 4) {
+		tck_gpio = ft232r_bit_name_to_number(CMD_ARGV[0]);
+		tms_gpio = ft232r_bit_name_to_number(CMD_ARGV[1]);
+		tdi_gpio = ft232r_bit_name_to_number(CMD_ARGV[2]);
+		tdo_gpio = ft232r_bit_name_to_number(CMD_ARGV[3]);
+	} else if (CMD_ARGC != 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	if (tck_gpio < 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	if (tms_gpio < 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	if (tdi_gpio < 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	if (tdo_gpio < 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	command_print(CMD_CTX,
+			"FT232R nums: TCK = %d %s, TMS = %d %s, TDI = %d %s, TDO = %d %s",
+			tck_gpio, ft232r_bit_number_to_name(tck_gpio),
+			tms_gpio, ft232r_bit_number_to_name(tms_gpio),
+			tdi_gpio, ft232r_bit_number_to_name(tdi_gpio),
+			tdo_gpio, ft232r_bit_number_to_name(tdo_gpio));
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(ft232r_handle_tck_num_command)
+{
+	if (CMD_ARGC == 1)
+		tck_gpio = ft232r_bit_name_to_number(CMD_ARGV[0]);
+	else if (CMD_ARGC != 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	if (tck_gpio < 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	command_print(CMD_CTX,
+			"FT232R num: TCK = %d %s", tck_gpio, ft232r_bit_number_to_name(tck_gpio));
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(ft232r_handle_tms_num_command)
+{
+	if (CMD_ARGC == 1)
+		tms_gpio = ft232r_bit_name_to_number(CMD_ARGV[0]);
+	else if (CMD_ARGC != 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	if (tms_gpio < 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	command_print(CMD_CTX,
+			"FT232R num: TMS = %d %s", tms_gpio, ft232r_bit_number_to_name(tms_gpio));
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(ft232r_handle_tdo_num_command)
+{
+	if (CMD_ARGC == 1)
+		tdo_gpio = ft232r_bit_name_to_number(CMD_ARGV[0]);
+	else if (CMD_ARGC != 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	if (tdo_gpio < 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	command_print(CMD_CTX,
+			"FT232R num: TDO = %d %s", tdo_gpio, ft232r_bit_number_to_name(tdo_gpio));
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(ft232r_handle_tdi_num_command)
+{
+	if (CMD_ARGC == 1)
+		tdi_gpio = ft232r_bit_name_to_number(CMD_ARGV[0]);
+	else if (CMD_ARGC != 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	if (tdi_gpio < 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	command_print(CMD_CTX,
+			"FT232R num: TDI = %d %s", tdi_gpio, ft232r_bit_number_to_name(tdi_gpio));
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(ft232r_handle_trst_num_command)
+{
+	if (CMD_ARGC == 1)
+		ntrst_gpio = ft232r_bit_name_to_number(CMD_ARGV[0]);
+	else if (CMD_ARGC != 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	if (ntrst_gpio < 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	command_print(CMD_CTX,
+			"FT232R num: TRST = %d %s", ntrst_gpio, ft232r_bit_number_to_name(ntrst_gpio));
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(ft232r_handle_srst_num_command)
+{
+	if (CMD_ARGC == 1)
+		nsysrst_gpio = ft232r_bit_name_to_number(CMD_ARGV[0]);
+	else if (CMD_ARGC != 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	if (nsysrst_gpio < 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	command_print(CMD_CTX,
+			"FT232R num: SRST = %d %s", nsysrst_gpio, ft232r_bit_number_to_name(nsysrst_gpio));
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(ft232r_handle_restore_serial_command)
+{
+	if (CMD_ARGC == 1)
+		COMMAND_PARSE_NUMBER(u16, CMD_ARGV[0], ft232r_restore_bitmode);
+	else if (CMD_ARGC != 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	command_print(CMD_CTX,
+			"FT232R restore serial: 0x%04X (%s)",
+			ft232r_restore_bitmode, ft232r_restore_bitmode == 0xFFFF ? "disabled" : "enabled");
+
+	return ERROR_OK;
+}
+
 static const struct command_registration ft232r_command_handlers[] = {
 	{
 		.name = "ft232r_serial_desc",
@@ -372,6 +577,62 @@ static const struct command_registration ft232r_command_handlers[] = {
 		.help = "USB VID and PID of the adapter",
 		.usage = "vid pid",
 	},
+	{
+		.name = "ft232r_jtag_nums",
+		.handler = ft232r_handle_jtag_nums_command,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio numbers for tck, tms, tdi, tdo. (in that order)",
+		.usage = "<0-7|TXD-RI> <0-7|TXD-RI> <0-7|TXD-RI> <0-7|TXD-RI>",
+	},
+	{
+		.name = "ft232r_tck_num",
+		.handler = ft232r_handle_tck_num_command,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio number for tck.",
+		.usage = "<0-7|TXD|RXD|RTS|CTS|DTR|DSR|DCD|RI>",
+	},
+	{
+		.name = "ft232r_tms_num",
+		.handler = ft232r_handle_tms_num_command,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio number for tms.",
+		.usage = "<0-7|TXD|RXD|RTS|CTS|DTR|DSR|DCD|RI>",
+	},
+	{
+		.name = "ft232r_tdo_num",
+		.handler = ft232r_handle_tdo_num_command,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio number for tdo.",
+		.usage = "<0-7|TXD|RXD|RTS|CTS|DTR|DSR|DCD|RI>",
+	},
+	{
+		.name = "ft232r_tdi_num",
+		.handler = ft232r_handle_tdi_num_command,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio number for tdi.",
+		.usage = "<0-7|TXD|RXD|RTS|CTS|DTR|DSR|DCD|RI>",
+	},
+	{
+		.name = "ft232r_srst_num",
+		.handler = ft232r_handle_srst_num_command,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio number for srst.",
+		.usage = "<0-7|TXD|RXD|RTS|CTS|DTR|DSR|DCD|RI>",
+	},
+	{
+		.name = "ft232r_trst_num",
+		.handler = ft232r_handle_trst_num_command,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio number for trst.",
+		.usage = "<0-7|TXD|RXD|RTS|CTS|DTR|DSR|DCD|RI>",
+	},
+	{
+		.name = "ft232r_restore_serial",
+		.handler = ft232r_handle_restore_serial_command,
+		.mode = COMMAND_CONFIG,
+		.help = "bitmode control word that restores serial port.",
+		.usage = "bitmode_control_word",
+	},
 	COMMAND_REGISTRATION_DONE
 };
 
@@ -553,7 +814,7 @@ static void syncbb_scan(bool ir_scan, enum scan_type type, uint8_t *buffer, int
 			int bcval = 1 << (bit_cnt % 8);
 			int val = ft232r_output[bit0_index + bit_cnt*2 + 1];
 
-			if (val & READ_TDO)
+			if (val & (1<<tdo_gpio))
 				buffer[bytec] |= bcval;
 			else
 				buffer[bytec] &= ~bcval;
diff --git a/src/jtag/drivers/imx_gpio.c b/src/jtag/drivers/imx_gpio.c
index 2a822af..f42692c 100644
--- a/src/jtag/drivers/imx_gpio.c
+++ b/src/jtag/drivers/imx_gpio.c
@@ -160,10 +160,10 @@ static int imx_gpio_swd_write(int tck, int tms, int tdi)
 static int imx_gpio_reset(int trst, int srst)
 {
 	if (trst_gpio != -1)
-		trst ? gpio_set(trst_gpio) : gpio_clear(trst_gpio);
+		trst ? gpio_clear(trst_gpio) : gpio_set(trst_gpio);
 
 	if (srst_gpio != -1)
-		srst ? gpio_set(srst_gpio) : gpio_clear(srst_gpio);
+		srst ? gpio_clear(srst_gpio) : gpio_set(srst_gpio);
 
 	return ERROR_OK;
 }
diff --git a/src/jtag/drivers/jlink.c b/src/jtag/drivers/jlink.c
index e74965e..1eae827 100644
--- a/src/jtag/drivers/jlink.c
+++ b/src/jtag/drivers/jlink.c
@@ -2130,7 +2130,7 @@ skip:
 static void jlink_swd_queue_cmd(uint8_t cmd, uint32_t *dst, uint32_t data, uint32_t ap_delay_clk)
 {
 	uint8_t data_parity_trn[DIV_ROUND_UP(32 + 1, 8)];
-	if (tap_length + 46 + 8 + ap_delay_clk >= sizeof(tdi_buffer) * 8 ||
+	if (tap_length + 46 + 8 + ap_delay_clk >= swd_buffer_size * 8 ||
 	    pending_scan_results_length == MAX_PENDING_SCAN_RESULTS) {
 		/* Not enough room in the queue. Run the queue. */
 		queued_retval = jlink_swd_run_queue();
diff --git a/src/jtag/drivers/jtag_usb_common.c b/src/jtag/drivers/jtag_usb_common.c
new file mode 100644
index 0000000..637e6c7
--- /dev/null
+++ b/src/jtag/drivers/jtag_usb_common.c
@@ -0,0 +1,85 @@
+/*
+ * SPDX-License-Identifier: GPL-2.0+
+ * Copyright (c) 2018 Pengutronix, Oleksij Rempel <kernel@pengutronix.de>
+ */
+
+#include <helper/log.h>
+
+#include "jtag_usb_common.h"
+
+static char *jtag_usb_location;
+/*
+ * 1 char: bus
+ * 2 * 7 chars: max 7 ports
+ * 1 char: test for overflow
+ * ------
+ * 16 chars
+ */
+#define JTAG_USB_MAX_LOCATION_LENGHT	16
+
+void jtag_usb_set_location(const char *location)
+{
+	if (strnlen(location, JTAG_USB_MAX_LOCATION_LENGHT) ==
+	    JTAG_USB_MAX_LOCATION_LENGHT)
+		LOG_WARNING("usb location string is too long!!\n");
+
+	if (jtag_usb_location)
+		free(jtag_usb_location);
+
+	jtag_usb_location = strndup(location, JTAG_USB_MAX_LOCATION_LENGHT);
+}
+
+const char *jtag_usb_get_location(void)
+{
+	return jtag_usb_location;
+}
+
+bool jtag_usb_location_equal(uint8_t dev_bus, uint8_t *port_path,
+			     size_t path_len)
+{
+	size_t path_step, string_lengh;
+	char *ptr, *loc;
+	bool equal = false;
+
+	/* strtok need non const char */
+	loc = strndup(jtag_usb_get_location(), JTAG_USB_MAX_LOCATION_LENGHT);
+	string_lengh = strnlen(loc, JTAG_USB_MAX_LOCATION_LENGHT);
+
+	ptr = strtok(loc, "-");
+	if (ptr == NULL) {
+		LOG_WARNING("no '-' in usb path\n");
+		goto done;
+	}
+
+	string_lengh -= 1;
+	/* check bus mismatch */
+	if (atoi(ptr) != dev_bus)
+		goto done;
+
+	path_step = 0;
+	while (path_step < path_len) {
+		ptr = strtok(NULL, ".");
+
+		/* no more tokens in path */
+		if (ptr == NULL)
+			break;
+
+		/* path mismatch at some step */
+		if (path_step < path_len && atoi(ptr) != port_path[path_step])
+			break;
+
+		path_step++;
+		string_lengh -= 2;
+	};
+
+	/* walked the full path, all elements match */
+	if (path_step == path_len && !string_lengh)
+		equal = true;
+	else
+		LOG_WARNING("excluded by device path option: %s\n",
+			    jtag_usb_get_location());
+
+done:
+	free(loc);
+	return equal;
+}
diff --git a/src/jtag/drivers/jtag_usb_common.h b/src/jtag/drivers/jtag_usb_common.h
new file mode 100644
index 0000000..8c03742
--- /dev/null
+++ b/src/jtag/drivers/jtag_usb_common.h
@@ -0,0 +1,14 @@
+/*
+ * SPDX-License-Identifier: GPL-2.0+
+ * Copyright (c) 2018 Pengutronix, Oleksij Rempel <kernel@pengutronix.de>
+ */
+
+#ifndef OPENOCD_JTAG_USB_COMMON_H
+#define OPENOCD_JTAG_USB_COMMON_H
+
+void jtag_usb_set_location(const char *location);
+const char *jtag_usb_get_location(void);
+bool jtag_usb_location_equal(uint8_t dev_bus, uint8_t *port_path,
+			     size_t path_len);
+
+#endif /* OPENOCD_JTAG_USB_COMMON_H */
diff --git a/src/jtag/drivers/jtag_vpi.c b/src/jtag/drivers/jtag_vpi.c
index 1a42b3a..35c7031 100644
--- a/src/jtag/drivers/jtag_vpi.c
+++ b/src/jtag/drivers/jtag_vpi.c
@@ -29,6 +29,10 @@
 #include <arpa/inet.h>
 #endif
 
+#ifndef _WIN32
+#include <netinet/tcp.h>
+#endif
+
 #define NO_TAP_SHIFT	0
 #define TAP_SHIFT	1
 
@@ -368,6 +372,8 @@ static int jtag_vpi_execute_queue(void)
 
 static int jtag_vpi_init(void)
 {
+	int flag = 1;
+
 	sockfd = socket(AF_INET, SOCK_STREAM, 0);
 	if (sockfd < 0) {
 		LOG_ERROR("Could not create socket");
@@ -395,6 +401,13 @@ static int jtag_vpi_init(void)
 		return ERROR_COMMAND_CLOSE_CONNECTION;
 	}
 
+	if (serv_addr.sin_addr.s_addr == htonl(INADDR_LOOPBACK)) {
+		/* This increases performance drematically for local
+		 * connections, which is the most likely arrangement
+		 * for a VPI connection. */
+		setsockopt(sockfd, IPPROTO_TCP, TCP_NODELAY, (char *)&flag, sizeof(int));
+	}
+
 	LOG_INFO("Connection to %s : %u succeed", server_address, server_port);
 
 	return ERROR_OK;
diff --git a/src/jtag/drivers/libusb0_common.c b/src/jtag/drivers/libusb0_common.c
index 1825543..14a8b61 100644
--- a/src/jtag/drivers/libusb0_common.c
+++ b/src/jtag/drivers/libusb0_common.c
@@ -67,7 +67,8 @@ int jtag_libusb_open(const uint16_t vids[], const uint16_t pids[],
 		const char *serial,
 		struct jtag_libusb_device_handle **out)
 {
-	int retval = -ENODEV;
+	int retval = ERROR_FAIL;
+	bool serial_mismatch = false;
 	struct jtag_libusb_device_handle *libusb_handle;
 	usb_init();
 
@@ -83,21 +84,27 @@ int jtag_libusb_open(const uint16_t vids[], const uint16_t pids[],
 
 			libusb_handle = usb_open(dev);
 			if (NULL == libusb_handle) {
-				retval = -errno;
+				LOG_ERROR("usb_open() failed with %s", usb_strerror());
 				continue;
 			}
 
 			/* Device must be open to use libusb_get_string_descriptor_ascii. */
 			if (serial != NULL &&
 					!string_descriptor_equal(libusb_handle, dev->descriptor.iSerialNumber, serial)) {
+				serial_mismatch = true;
 				usb_close(libusb_handle);
 				continue;
 			}
 			*out = libusb_handle;
-			retval = 0;
+			retval = ERROR_OK;
+			serial_mismatch = false;
 			break;
 		}
 	}
+
+	if (serial_mismatch)
+		LOG_INFO("No device matches the serial string");
+
 	return retval;
 }
 
diff --git a/src/jtag/drivers/libusb0_common.h b/src/jtag/drivers/libusb0_common.h
index baa9e3c..676f43a 100644
--- a/src/jtag/drivers/libusb0_common.h
+++ b/src/jtag/drivers/libusb0_common.h
@@ -38,6 +38,7 @@
 #define LIBUSB_RECIPIENT_DEVICE			USB_RECIP_DEVICE
 #define LIBUSB_ENDPOINT_OUT				USB_ENDPOINT_OUT
 #define LIBUSB_ENDPOINT_IN				USB_ENDPOINT_IN
+#define LIBUSB_TRANSFER_TYPE_BULK		USB_ENDPOINT_TYPE_BULK
 
 static inline int jtag_libusb_claim_interface(jtag_libusb_device_handle *devh,
 				       int iface)
diff --git a/src/jtag/drivers/libusb1_common.c b/src/jtag/drivers/libusb1_common.c
index a1db86f..d96ac76 100644
--- a/src/jtag/drivers/libusb1_common.c
+++ b/src/jtag/drivers/libusb1_common.c
@@ -20,8 +20,15 @@
 #ifdef HAVE_CONFIG_H
 #include "config.h"
 #endif
-#include "log.h"
+#include <jtag/drivers/jtag_usb_common.h>
 #include "libusb1_common.h"
+#include "log.h"
+
+/*
+ * comment from libusb:
+ * As per the USB 3.0 specs, the current maximum limit for the depth is 7.
+ */
+#define MAX_USB_PORTS	7
 
 static struct libusb_context *jtag_libusb_context; /**< Libusb context **/
 static libusb_device **devs; /**< The usb device list **/
@@ -38,6 +45,31 @@ static bool jtag_libusb_match(struct libusb_device_descriptor *dev_desc,
 	return false;
 }
 
+#ifdef HAVE_LIBUSB_GET_PORT_NUMBERS
+static bool jtag_libusb_location_equal(libusb_device *device)
+{
+	uint8_t port_path[MAX_USB_PORTS];
+	uint8_t dev_bus;
+	int path_len;
+
+	path_len = libusb_get_port_numbers(device, port_path, MAX_USB_PORTS);
+	if (path_len == LIBUSB_ERROR_OVERFLOW) {
+		LOG_WARNING("cannot determine path to usb device! (more than %i ports in path)\n",
+			MAX_USB_PORTS);
+		return false;
+	}
+	dev_bus = libusb_get_bus_number(device);
+
+	return jtag_usb_location_equal(dev_bus, port_path, path_len);
+}
+#else /* HAVE_LIBUSB_GET_PORT_NUMBERS */
+static bool jtag_libusb_location_equal(libusb_device *device)
+{
+	return true;
+}
+#endif /* HAVE_LIBUSB_GET_PORT_NUMBERS */
+
+
 /* Returns true if the string descriptor indexed by str_index in device matches string */
 static bool string_descriptor_equal(libusb_device_handle *device, uint8_t str_index,
 									const char *string)
@@ -72,6 +104,7 @@ int jtag_libusb_open(const uint16_t vids[], const uint16_t pids[],
 {
 	int cnt, idx, errCode;
 	int retval = ERROR_FAIL;
+	bool serial_mismatch = false;
 	struct jtag_libusb_device_handle *libusb_handle = NULL;
 
 	if (libusb_init(&jtag_libusb_context) < 0)
@@ -88,6 +121,9 @@ int jtag_libusb_open(const uint16_t vids[], const uint16_t pids[],
 		if (!jtag_libusb_match(&dev_desc, vids, pids))
 			continue;
 
+		if (jtag_usb_get_location() && !jtag_libusb_location_equal(devs[idx]))
+			continue;
+
 		errCode = libusb_open(devs[idx], &libusb_handle);
 
 		if (errCode) {
@@ -99,6 +135,7 @@ int jtag_libusb_open(const uint16_t vids[], const uint16_t pids[],
 		/* Device must be open to use libusb_get_string_descriptor_ascii. */
 		if (serial != NULL &&
 				!string_descriptor_equal(libusb_handle, dev_desc.iSerialNumber, serial)) {
+			serial_mismatch = true;
 			libusb_close(libusb_handle);
 			continue;
 		}
@@ -106,10 +143,15 @@ int jtag_libusb_open(const uint16_t vids[], const uint16_t pids[],
 		/* Success. */
 		*out = libusb_handle;
 		retval = ERROR_OK;
+		serial_mismatch = false;
 		break;
 	}
 	if (cnt >= 0)
 		libusb_free_device_list(devs, 1);
+
+	if (serial_mismatch)
+		LOG_INFO("No device matches the serial string");
+
 	return retval;
 }
 
diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c
index d9ca53e..d6c84fa 100644
--- a/src/jtag/drivers/stlink_usb.c
+++ b/src/jtag/drivers/stlink_usb.c
@@ -41,6 +41,10 @@
 
 #include "libusb_common.h"
 
+#ifdef HAVE_LIBUSB1
+#define USE_LIBUSB_ASYNCIO
+#endif
+
 #define ENDPOINT_IN  0x80
 #define ENDPOINT_OUT 0x00
 
@@ -63,10 +67,19 @@
 #define STLINK_V1_PID         (0x3744)
 #define STLINK_V2_PID         (0x3748)
 #define STLINK_V2_1_PID       (0x374B)
+#define STLINK_V2_1_NO_MSD_PID  (0x3752)
+#define STLINK_V3_USBLOADER_PID (0x374D)
+#define STLINK_V3E_PID          (0x374E)
+#define STLINK_V3S_PID          (0x374F)
+#define STLINK_V3_2VCP_PID      (0x3753)
 
-/* the current implementation of the stlink limits
- * 8bit read/writes to max 64 bytes. */
+/*
+ * ST-Link/V1, ST-Link/V2 and ST-Link/V2.1 are full-speed USB devices and
+ * this limits the bulk packet size and the 8bit read/writes to max 64 bytes.
+ * STLINK-V3 is a high speed USB 2.0 and the limit is 512 bytes.
+ */
 #define STLINK_MAX_RW8		(64)
+#define STLINKV3_MAX_RW8	(512)
 
 /* "WAIT" responses will be retried (with exponential backoff) at
  * most this many times before failing to caller.
@@ -76,6 +89,7 @@
 enum stlink_jtag_api_version {
 	STLINK_JTAG_API_V1 = 1,
 	STLINK_JTAG_API_V2,
+	STLINK_JTAG_API_V3,
 };
 
 /** */
@@ -86,8 +100,10 @@ struct stlink_usb_version {
 	int jtag;
 	/** */
 	int swim;
-	/** highest supported jtag api version */
-	enum stlink_jtag_api_version jtag_api_max;
+	/** jtag api version supported */
+	enum stlink_jtag_api_version jtag_api;
+	/** one bit for each feature supported. See macros STLINK_F_* */
+	uint32_t flags;
 };
 
 /** */
@@ -120,8 +136,6 @@ struct stlink_usb_handle_s {
 	uint16_t vid;
 	/** */
 	uint16_t pid;
-	/** this is the currently used jtag api */
-	enum stlink_jtag_api_version jtag_api;
 	/** */
 	struct {
 		/** whether SWO tracing is enabled or not */
@@ -204,7 +218,6 @@ struct stlink_usb_handle_s {
 #define STLINK_SWIM_READMEM                0x0b
 #define STLINK_SWIM_READBUF                0x0c
 
-#define STLINK_DEBUG_ENTER_JTAG            0x00
 #define STLINK_DEBUG_GETSTATUS             0x01
 #define STLINK_DEBUG_FORCEDEBUG            0x02
 #define STLINK_DEBUG_APIV1_RESETSYS        0x03
@@ -222,8 +235,9 @@ struct stlink_usb_handle_s {
 #define STLINK_DEBUG_APIV1_WRITEDEBUGREG   0x0f
 #define STLINK_DEBUG_APIV1_SETWATCHPOINT   0x10
 
-#define STLINK_DEBUG_ENTER_JTAG            0x00
-#define STLINK_DEBUG_ENTER_SWD             0xa3
+#define STLINK_DEBUG_ENTER_JTAG_RESET      0x00
+#define STLINK_DEBUG_ENTER_SWD_NO_RESET    0xa3
+#define STLINK_DEBUG_ENTER_JTAG_NO_RESET   0xa4
 
 #define STLINK_DEBUG_APIV1_ENTER           0x20
 #define STLINK_DEBUG_EXIT                  0x21
@@ -241,10 +255,21 @@ struct stlink_usb_handle_s {
 #define STLINK_DEBUG_APIV2_GETLASTRWSTATUS 0x3B
 #define STLINK_DEBUG_APIV2_DRIVE_NRST      0x3C
 
+#define STLINK_DEBUG_APIV2_GETLASTRWSTATUS2 0x3E
+
 #define STLINK_DEBUG_APIV2_START_TRACE_RX  0x40
 #define STLINK_DEBUG_APIV2_STOP_TRACE_RX   0x41
 #define STLINK_DEBUG_APIV2_GET_TRACE_NB    0x42
 #define STLINK_DEBUG_APIV2_SWD_SET_FREQ    0x43
+#define STLINK_DEBUG_APIV2_JTAG_SET_FREQ   0x44
+
+#define STLINK_DEBUG_APIV2_READMEM_16BIT   0x47
+#define STLINK_DEBUG_APIV2_WRITEMEM_16BIT  0x48
+
+#define STLINK_APIV3_SET_COM_FREQ           0x61
+#define STLINK_APIV3_GET_COM_FREQ           0x62
+
+#define STLINK_APIV3_GET_VERSION_EX         0xFB
 
 #define STLINK_DEBUG_APIV2_DRIVE_NRST_LOW   0x00
 #define STLINK_DEBUG_APIV2_DRIVE_NRST_HIGH  0x01
@@ -252,7 +277,8 @@ struct stlink_usb_handle_s {
 
 #define STLINK_TRACE_SIZE               4096
 #define STLINK_TRACE_MAX_HZ             2000000
-#define STLINK_TRACE_MIN_VERSION        13
+
+#define STLINK_V3_MAX_FREQ_NB               10
 
 /** */
 enum stlink_mode {
@@ -267,10 +293,26 @@ enum stlink_mode {
 #define REQUEST_SENSE        0x03
 #define REQUEST_SENSE_LENGTH 18
 
-static const struct {
+/*
+ * Map the relevant features, quirks and workaround for specific firmware
+ * version of stlink
+ */
+#define STLINK_F_HAS_TRACE              (1UL << 0)
+#define STLINK_F_HAS_SWD_SET_FREQ       (1UL << 1)
+#define STLINK_F_HAS_JTAG_SET_FREQ      (1UL << 2)
+#define STLINK_F_HAS_MEM_16BIT          (1UL << 3)
+#define STLINK_F_HAS_GETLASTRWSTATUS2   (1UL << 4)
+
+/* aliases */
+#define STLINK_F_HAS_TARGET_VOLT        STLINK_F_HAS_TRACE
+
+struct speed_map {
 	int speed;
 	int speed_divisor;
-} stlink_khz_to_speed_map[] = {
+};
+
+/* SWD clock speed */
+static const struct speed_map stlink_khz_to_speed_map_swd[] = {
 	{4000, 0},
 	{1800, 1}, /* default */
 	{1200, 2},
@@ -285,10 +327,188 @@ static const struct {
 	{5,  798}
 };
 
+/* JTAG clock speed */
+static const struct speed_map stlink_khz_to_speed_map_jtag[] = {
+	{18000, 2},
+	{9000,  4},
+	{4500,  8},
+	{2250, 16},
+	{1125, 32}, /* default */
+	{562,  64},
+	{281, 128},
+	{140, 256}
+};
+
 static void stlink_usb_init_buffer(void *handle, uint8_t direction, uint32_t size);
 static int stlink_swim_status(void *handle);
 
 /** */
+static unsigned int stlink_usb_block(void *handle)
+{
+	struct stlink_usb_handle_s *h = handle;
+
+	assert(handle != NULL);
+
+	if (h->version.stlink == 3)
+		return STLINKV3_MAX_RW8;
+	else
+		return STLINK_MAX_RW8;
+}
+
+
+
+#ifdef USE_LIBUSB_ASYNCIO
+
+static LIBUSB_CALL void sync_transfer_cb(struct libusb_transfer *transfer)
+{
+	int *completed = transfer->user_data;
+	*completed = 1;
+	/* caller interprets result and frees transfer */
+}
+
+
+static void sync_transfer_wait_for_completion(struct libusb_transfer *transfer)
+{
+	int r, *completed = transfer->user_data;
+
+	/* Assuming a single libusb context exists.  There no existing interface into this
+	 * module to pass a libusb context.
+	 */
+	struct libusb_context *ctx = NULL;
+
+	while (!*completed) {
+		r = libusb_handle_events_completed(ctx, completed);
+		if (r < 0) {
+			if (r == LIBUSB_ERROR_INTERRUPTED)
+				continue;
+			libusb_cancel_transfer(transfer);
+			continue;
+		}
+	}
+}
+
+
+static int transfer_error_status(const struct libusb_transfer *transfer)
+{
+	int r = 0;
+
+	switch (transfer->status) {
+		case LIBUSB_TRANSFER_COMPLETED:
+			r = 0;
+			break;
+		case LIBUSB_TRANSFER_TIMED_OUT:
+			r = LIBUSB_ERROR_TIMEOUT;
+			break;
+		case LIBUSB_TRANSFER_STALL:
+			r = LIBUSB_ERROR_PIPE;
+			break;
+		case LIBUSB_TRANSFER_OVERFLOW:
+			r = LIBUSB_ERROR_OVERFLOW;
+			break;
+		case LIBUSB_TRANSFER_NO_DEVICE:
+			r = LIBUSB_ERROR_NO_DEVICE;
+			break;
+		case LIBUSB_TRANSFER_ERROR:
+		case LIBUSB_TRANSFER_CANCELLED:
+			r = LIBUSB_ERROR_IO;
+			break;
+		default:
+			r = LIBUSB_ERROR_OTHER;
+			break;
+	}
+
+	return r;
+}
+
+struct jtag_xfer {
+	int ep;
+	uint8_t *buf;
+	size_t size;
+	/* Internal */
+	int retval;
+	int completed;
+	size_t transfer_size;
+	struct libusb_transfer *transfer;
+};
+
+static int jtag_libusb_bulk_transfer_n(jtag_libusb_device_handle *dev_handle,
+		struct jtag_xfer *transfers,
+		size_t n_transfers,
+		int timeout)
+{
+	int retval = 0;
+	int returnval = ERROR_OK;
+
+
+	for (size_t i = 0; i < n_transfers; ++i) {
+		transfers[i].retval = 0;
+		transfers[i].completed = 0;
+		transfers[i].transfer_size = 0;
+		transfers[i].transfer = libusb_alloc_transfer(0);
+
+		if (transfers[i].transfer == NULL) {
+			for (size_t j = 0; j < i; ++j)
+				libusb_free_transfer(transfers[j].transfer);
+
+			LOG_DEBUG("ERROR, failed to alloc usb transfers");
+			for (size_t k = 0; k < n_transfers; ++k)
+				transfers[k].retval = LIBUSB_ERROR_NO_MEM;
+			return ERROR_FAIL;
+		}
+	}
+
+	for (size_t i = 0; i < n_transfers; ++i) {
+		libusb_fill_bulk_transfer(
+				transfers[i].transfer,
+				dev_handle,
+				transfers[i].ep, transfers[i].buf, transfers[i].size,
+				sync_transfer_cb, &transfers[i].completed, timeout);
+		transfers[i].transfer->type = LIBUSB_TRANSFER_TYPE_BULK;
+
+		retval = libusb_submit_transfer(transfers[i].transfer);
+		if (retval < 0) {
+			LOG_DEBUG("ERROR, failed to submit transfer %zu, error %d", i, retval);
+
+			/* Probably no point continuing to submit transfers once a submission fails.
+			 * As a result, tag all remaining transfers as errors.
+			 */
+			for (size_t j = i; j < n_transfers; ++j)
+				transfers[j].retval = retval;
+
+			returnval = ERROR_FAIL;
+			break;
+		}
+	}
+
+	/* Wait for every submitted USB transfer to complete.
+	*/
+	for (size_t i = 0; i < n_transfers; ++i) {
+		if (transfers[i].retval == 0) {
+			sync_transfer_wait_for_completion(transfers[i].transfer);
+
+			retval = transfer_error_status(transfers[i].transfer);
+			if (retval) {
+				returnval = ERROR_FAIL;
+				transfers[i].retval = retval;
+				LOG_DEBUG("ERROR, transfer %zu failed, error %d", i, retval);
+			} else {
+				/* Assuming actual_length is only valid if there is no transfer error.
+				 */
+				transfers[i].transfer_size = transfers[i].transfer->actual_length;
+			}
+		}
+
+		libusb_free_transfer(transfers[i].transfer);
+		transfers[i].transfer = NULL;
+	}
+
+	return returnval;
+}
+
+#endif
+
+
+/** */
 static int stlink_usb_xfer_v1_get_status(void *handle)
 {
 	struct stlink_usb_handle_s *h = handle;
@@ -321,7 +541,45 @@ static int stlink_usb_xfer_v1_get_status(void *handle)
 	return ERROR_OK;
 }
 
-/** */
+#ifdef USE_LIBUSB_ASYNCIO
+static int stlink_usb_xfer_rw(void *handle, int cmdsize, const uint8_t *buf, int size)
+{
+	struct stlink_usb_handle_s *h = handle;
+
+	assert(handle != NULL);
+
+	size_t n_transfers = 0;
+	struct jtag_xfer transfers[2];
+
+	memset(transfers, 0, sizeof(transfers));
+
+	transfers[0].ep = h->tx_ep;
+	transfers[0].buf = h->cmdbuf;
+	transfers[0].size = cmdsize;
+
+	++n_transfers;
+
+	if (h->direction == h->tx_ep && size) {
+		transfers[1].ep = h->tx_ep;
+		transfers[1].buf = (uint8_t *)buf;
+		transfers[1].size = size;
+
+		++n_transfers;
+	} else if (h->direction == h->rx_ep && size) {
+		transfers[1].ep = h->rx_ep;
+		transfers[1].buf = (uint8_t *)buf;
+		transfers[1].size = size;
+
+		++n_transfers;
+	}
+
+	return jtag_libusb_bulk_transfer_n(
+			h->fd,
+			transfers,
+			n_transfers,
+			STLINK_WRITE_TIMEOUT);
+}
+#else
 static int stlink_usb_xfer_rw(void *handle, int cmdsize, const uint8_t *buf, int size)
 {
 	struct stlink_usb_handle_s *h = handle;
@@ -349,6 +607,7 @@ static int stlink_usb_xfer_rw(void *handle, int cmdsize, const uint8_t *buf, int
 
 	return ERROR_OK;
 }
+#endif
 
 /** */
 static int stlink_usb_xfer_v1_get_sense(void *handle)
@@ -438,7 +697,7 @@ static int stlink_usb_error_check(void *handle)
 	}
 
 	/* TODO: no error checking yet on api V1 */
-	if (h->jtag_api == STLINK_JTAG_API_V1)
+	if (h->version.jtag_api == STLINK_JTAG_API_V1)
 		h->databuf[0] = STLINK_DEBUG_ERR_OK;
 
 	switch (h->databuf[0]) {
@@ -526,7 +785,9 @@ static int stlink_cmd_allow_retry(void *handle, const uint8_t *buf, int size)
 
 		res = stlink_usb_error_check(handle);
 		if (res == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
-			usleep((1<<retries++) * 1000);
+			useconds_t delay_us = (1<<retries++) * 1000;
+			LOG_DEBUG("stlink_cmd_allow_retry ERROR_WAIT, retry %d, delaying %u microseconds", retries, delay_us);
+			usleep(delay_us);
 			continue;
 		}
 		return res;
@@ -540,7 +801,7 @@ static int stlink_usb_read_trace(void *handle, const uint8_t *buf, int size)
 
 	assert(handle != NULL);
 
-	assert(h->version.stlink >= 2);
+	assert(h->version.flags & STLINK_F_HAS_TRACE);
 
 	if (jtag_libusb_bulk_read(h->fd, h->trace_ep, (char *)buf,
 			size, STLINK_READ_TIMEOUT) != size) {
@@ -602,7 +863,11 @@ static void stlink_usb_init_buffer(void *handle, uint8_t direction, uint32_t siz
 static int stlink_usb_version(void *handle)
 {
 	int res;
-	uint16_t v;
+	uint32_t flags;
+	uint16_t version;
+	uint8_t v, x, y, jtag, swim, msd, bridge = 0;
+	char v_str[5 * (1 + 3) + 1]; /* VvJjMmBbSs */
+	char *p;
 	struct stlink_usb_handle_s *h = handle;
 
 	assert(handle != NULL);
@@ -616,27 +881,131 @@ static int stlink_usb_version(void *handle)
 	if (res != ERROR_OK)
 		return res;
 
-	v = (h->databuf[0] << 8) | h->databuf[1];
+	version = be_to_h_u16(h->databuf);
+	v = (version >> 12) & 0x0f;
+	x = (version >> 6) & 0x3f;
+	y = version & 0x3f;
+
+	h->vid = le_to_h_u16(h->databuf + 2);
+	h->pid = le_to_h_u16(h->databuf + 4);
+
+	switch (h->pid) {
+	case STLINK_V2_1_PID:
+	case STLINK_V2_1_NO_MSD_PID:
+		if ((x <= 22 && y == 7) || (x >= 25 && y >= 7 && y <= 12)) {
+			/* MxSy : STM8 V2.1 - SWIM only */
+			msd = x;
+			swim = y;
+			jtag = 0;
+		} else {
+			/* JxMy : STM32 V2.1 - JTAG/SWD only */
+			jtag = x;
+			msd = y;
+			swim = 0;
+		}
+		break;
+	default:
+		jtag = x;
+		swim = y;
+		msd = 0;
+		break;
+	}
+
+	/* STLINK-V3 requires a specific command */
+	if (v == 3 && x == 0 && y == 0) {
+		stlink_usb_init_buffer(handle, h->rx_ep, 16);
 
-	h->version.stlink = (v >> 12) & 0x0f;
-	h->version.jtag = (v >> 6) & 0x3f;
-	h->version.swim = v & 0x3f;
-	h->vid = buf_get_u32(h->databuf, 16, 16);
-	h->pid = buf_get_u32(h->databuf, 32, 16);
+		h->cmdbuf[h->cmdidx++] = STLINK_APIV3_GET_VERSION_EX;
 
-	/* set the supported jtag api version
-	 * API V2 is supported since JTAG V11
-	 */
-	if (h->version.jtag >= 11)
-		h->version.jtag_api_max = STLINK_JTAG_API_V2;
-	else
-		h->version.jtag_api_max = STLINK_JTAG_API_V1;
+		res = stlink_usb_xfer(handle, h->databuf, 12);
+		if (res != ERROR_OK)
+			return res;
+
+		v = h->databuf[0];
+		swim = h->databuf[1];
+		jtag = h->databuf[2];
+		msd  = h->databuf[3];
+		bridge = h->databuf[4];
+		h->vid = le_to_h_u16(h->databuf + 8);
+		h->pid = le_to_h_u16(h->databuf + 10);
+	}
+
+	h->version.stlink = v;
+	h->version.jtag = jtag;
+	h->version.swim = swim;
+
+	flags = 0;
+	switch (h->version.stlink) {
+	case 1:
+		/* ST-LINK/V1 from J11 switch to api-v2 (and support SWD) */
+		if (h->version.jtag >= 11)
+			h->version.jtag_api = STLINK_JTAG_API_V2;
+		else
+			h->version.jtag_api = STLINK_JTAG_API_V1;
+
+		break;
+	case 2:
+		/* all ST-LINK/V2 and ST-Link/V2.1 use api-v2 */
+		h->version.jtag_api = STLINK_JTAG_API_V2;
+
+		/* API for trace from J13 */
+		/* API for target voltage from J13 */
+		if (h->version.jtag >= 13)
+			flags |= STLINK_F_HAS_TRACE;
+
+		/* preferred API to get last R/W status from J15 */
+		if (h->version.jtag >= 15)
+			flags |= STLINK_F_HAS_GETLASTRWSTATUS2;
+
+		/* API to set SWD frequency from J22 */
+		if (h->version.jtag >= 22)
+			flags |= STLINK_F_HAS_SWD_SET_FREQ;
+
+		/* API to set JTAG frequency from J24 */
+		if (h->version.jtag >= 24)
+			flags |= STLINK_F_HAS_JTAG_SET_FREQ;
+
+		/* API to read/write memory at 16 bit from J26 */
+		if (h->version.jtag >= 26)
+			flags |= STLINK_F_HAS_MEM_16BIT;
+
+		break;
+	case 3:
+		/* all STLINK-V3 use api-v3 */
+		h->version.jtag_api = STLINK_JTAG_API_V3;
+
+		/* STLINK-V3 is a superset of ST-LINK/V2 */
+
+		/* API for trace */
+		/* API for target voltage */
+		flags |= STLINK_F_HAS_TRACE;
+
+		/* preferred API to get last R/W status */
+		flags |= STLINK_F_HAS_GETLASTRWSTATUS2;
 
-	LOG_INFO("STLINK v%d JTAG v%d API v%d SWIM v%d VID 0x%04X PID 0x%04X",
-		h->version.stlink,
-		h->version.jtag,
-		(h->version.jtag_api_max == STLINK_JTAG_API_V1) ? 1 : 2,
-		h->version.swim,
+		/* API to read/write memory at 16 bit */
+		flags |= STLINK_F_HAS_MEM_16BIT;
+
+		break;
+	default:
+		break;
+	}
+	h->version.flags = flags;
+
+	p = v_str;
+	p += sprintf(p, "V%d", v);
+	if (jtag || !msd)
+		p += sprintf(p, "J%d", jtag);
+	if (msd)
+		p += sprintf(p, "M%d", msd);
+	if (bridge)
+		p += sprintf(p, "B%d", bridge);
+	if (swim || !msd)
+		p += sprintf(p, "S%d", swim);
+
+	LOG_INFO("STLINK %s (API v%d) VID:PID %04X:%04X",
+		v_str,
+		h->version.jtag_api,
 		h->vid,
 		h->pid);
 
@@ -648,8 +1017,8 @@ static int stlink_usb_check_voltage(void *handle, float *target_voltage)
 	struct stlink_usb_handle_s *h = handle;
 	uint32_t adc_results[2];
 
-	/* only supported by stlink/v2 and for firmware >= 13 */
-	if (h->version.stlink == 1 || h->version.jtag < 13)
+	/* no error message, simply quit with error */
+	if (!(h->version.flags & STLINK_F_HAS_TARGET_VOLT))
 		return ERROR_COMMAND_NOTFOUND;
 
 	stlink_usb_init_buffer(handle, h->rx_ep, 8);
@@ -681,8 +1050,7 @@ static int stlink_usb_set_swdclk(void *handle, uint16_t clk_divisor)
 
 	assert(handle != NULL);
 
-	/* only supported by stlink/v2 and for firmware >= 22 */
-	if (h->version.stlink == 1 || h->version.jtag < 22)
+	if (!(h->version.flags & STLINK_F_HAS_SWD_SET_FREQ))
 		return ERROR_COMMAND_NOTFOUND;
 
 	stlink_usb_init_buffer(handle, h->rx_ep, 2);
@@ -700,6 +1068,30 @@ static int stlink_usb_set_swdclk(void *handle, uint16_t clk_divisor)
 	return ERROR_OK;
 }
 
+static int stlink_usb_set_jtagclk(void *handle, uint16_t clk_divisor)
+{
+	struct stlink_usb_handle_s *h = handle;
+
+	assert(handle != NULL);
+
+	if (!(h->version.flags & STLINK_F_HAS_JTAG_SET_FREQ))
+		return ERROR_COMMAND_NOTFOUND;
+
+	stlink_usb_init_buffer(handle, h->rx_ep, 2);
+
+	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_JTAG_SET_FREQ;
+	h_u16_to_le(h->cmdbuf+h->cmdidx, clk_divisor);
+	h->cmdidx += 2;
+
+	int result = stlink_cmd_allow_retry(handle, h->databuf, 2);
+
+	if (result != ERROR_OK)
+		return result;
+
+	return ERROR_OK;
+}
+
 /** */
 static int stlink_usb_current_mode(void *handle, uint8_t *mode)
 {
@@ -734,7 +1126,7 @@ static int stlink_usb_mode_enter(void *handle, enum stlink_mode type)
 	 * status
 	 * TODO: we need the test on api V1 too
 	 */
-	if (h->jtag_api == STLINK_JTAG_API_V2)
+	if (h->version.jtag_api != STLINK_JTAG_API_V1)
 		rx_size = 2;
 
 	stlink_usb_init_buffer(handle, h->rx_ep, rx_size);
@@ -742,19 +1134,19 @@ static int stlink_usb_mode_enter(void *handle, enum stlink_mode type)
 	switch (type) {
 		case STLINK_MODE_DEBUG_JTAG:
 			h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
-			if (h->jtag_api == STLINK_JTAG_API_V1)
+			if (h->version.jtag_api == STLINK_JTAG_API_V1)
 				h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_ENTER;
 			else
 				h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_ENTER;
-			h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_ENTER_JTAG;
+			h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_ENTER_JTAG_NO_RESET;
 			break;
 		case STLINK_MODE_DEBUG_SWD:
 			h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
-			if (h->jtag_api == STLINK_JTAG_API_V1)
+			if (h->version.jtag_api == STLINK_JTAG_API_V1)
 				h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_ENTER;
 			else
 				h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_ENTER;
-			h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_ENTER_SWD;
+			h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_ENTER_SWD_NO_RESET;
 			break;
 		case STLINK_MODE_DEBUG_SWIM:
 			h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
@@ -1192,7 +1584,7 @@ static int stlink_usb_write_debug_reg(void *handle, uint32_t addr, uint32_t val)
 	stlink_usb_init_buffer(handle, h->rx_ep, 2);
 
 	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
-	if (h->jtag_api == STLINK_JTAG_API_V1)
+	if (h->version.jtag_api == STLINK_JTAG_API_V1)
 		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_WRITEDEBUGREG;
 	else
 		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEDEBUGREG;
@@ -1211,7 +1603,7 @@ static int stlink_usb_trace_read(void *handle, uint8_t *buf, size_t *size)
 
 	assert(handle != NULL);
 
-	if (h->trace.enabled && h->version.jtag >= STLINK_TRACE_MIN_VERSION) {
+	if (h->trace.enabled && (h->version.flags & STLINK_F_HAS_TRACE)) {
 		int res;
 
 		stlink_usb_init_buffer(handle, h->rx_ep, 10);
@@ -1284,7 +1676,7 @@ static enum target_state stlink_usb_state(void *handle)
 		h->reconnect_pending = false;
 	}
 
-	if (h->jtag_api == STLINK_JTAG_API_V2) {
+	if (h->version.jtag_api != STLINK_JTAG_API_V1) {
 		res = stlink_usb_v2_get_status(handle);
 		if (res == TARGET_UNKNOWN)
 			h->reconnect_pending = true;
@@ -1340,7 +1732,7 @@ static void stlink_usb_trace_disable(void *handle)
 
 	assert(handle != NULL);
 
-	assert(h->version.jtag >= STLINK_TRACE_MIN_VERSION);
+	assert(h->version.flags & STLINK_F_HAS_TRACE);
 
 	LOG_DEBUG("Tracing: disable");
 
@@ -1362,7 +1754,7 @@ static int stlink_usb_trace_enable(void *handle)
 
 	assert(handle != NULL);
 
-	if (h->version.jtag >= STLINK_TRACE_MIN_VERSION) {
+	if (h->version.flags & STLINK_F_HAS_TRACE) {
 		stlink_usb_init_buffer(handle, h->rx_ep, 10);
 
 		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
@@ -1401,7 +1793,7 @@ static int stlink_usb_reset(void *handle)
 
 	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 
-	if (h->jtag_api == STLINK_JTAG_API_V1)
+	if (h->version.jtag_api == STLINK_JTAG_API_V1)
 		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_RESETSYS;
 	else
 		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_RESETSYS;
@@ -1426,7 +1818,7 @@ static int stlink_usb_run(void *handle)
 
 	assert(handle != NULL);
 
-	if (h->jtag_api == STLINK_JTAG_API_V2) {
+	if (h->version.jtag_api != STLINK_JTAG_API_V1) {
 		res = stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_DEBUGEN);
 
 		return res;
@@ -1448,7 +1840,7 @@ static int stlink_usb_halt(void *handle)
 
 	assert(handle != NULL);
 
-	if (h->jtag_api == STLINK_JTAG_API_V2) {
+	if (h->version.jtag_api != STLINK_JTAG_API_V1) {
 		res = stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_DEBUGEN);
 
 		return res;
@@ -1469,7 +1861,7 @@ static int stlink_usb_step(void *handle)
 
 	assert(handle != NULL);
 
-	if (h->jtag_api == STLINK_JTAG_API_V2) {
+	if (h->version.jtag_api != STLINK_JTAG_API_V1) {
 		/* TODO: this emulates the v1 api, it should really use a similar auto mask isr
 		 * that the Cortex-M3 currently does. */
 		stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_MASKINTS|C_DEBUGEN);
@@ -1496,7 +1888,7 @@ static int stlink_usb_read_regs(void *handle)
 	stlink_usb_init_buffer(handle, h->rx_ep, 84);
 
 	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
-	if (h->jtag_api == STLINK_JTAG_API_V1)
+	if (h->version.jtag_api == STLINK_JTAG_API_V1)
 		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_READALLREGS;
 	else
 		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READALLREGS;
@@ -1517,16 +1909,16 @@ static int stlink_usb_read_reg(void *handle, int num, uint32_t *val)
 
 	assert(handle != NULL);
 
-	stlink_usb_init_buffer(handle, h->rx_ep, h->jtag_api == STLINK_JTAG_API_V1 ? 4 : 8);
+	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;
-	if (h->jtag_api == STLINK_JTAG_API_V1)
+	if (h->version.jtag_api == STLINK_JTAG_API_V1)
 		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_READREG;
 	else
 		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READREG;
 	h->cmdbuf[h->cmdidx++] = num;
 
-	if (h->jtag_api == STLINK_JTAG_API_V1) {
+	if (h->version.jtag_api == STLINK_JTAG_API_V1) {
 		res = stlink_usb_xfer(handle, h->databuf, 4);
 		if (res != ERROR_OK)
 			return res;
@@ -1551,7 +1943,7 @@ static int stlink_usb_write_reg(void *handle, int num, uint32_t val)
 	stlink_usb_init_buffer(handle, h->rx_ep, 2);
 
 	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
-	if (h->jtag_api == STLINK_JTAG_API_V1)
+	if (h->version.jtag_api == STLINK_JTAG_API_V1)
 		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_WRITEREG;
 	else
 		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEREG;
@@ -1569,15 +1961,21 @@ static int stlink_usb_get_rw_status(void *handle)
 
 	assert(handle != NULL);
 
-	if (h->jtag_api == STLINK_JTAG_API_V1)
+	if (h->version.jtag_api == STLINK_JTAG_API_V1)
 		return ERROR_OK;
 
 	stlink_usb_init_buffer(handle, h->rx_ep, 2);
 
 	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
-	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS;
+	if (h->version.flags & STLINK_F_HAS_GETLASTRWSTATUS2) {
+		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS2;
 
-	res = stlink_usb_xfer(handle, h->databuf, 2);
+		res = stlink_usb_xfer(handle, h->databuf, 12);
+	} else {
+		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS;
+
+		res = stlink_usb_xfer(handle, h->databuf, 2);
+	}
 
 	if (res != ERROR_OK)
 		return res;
@@ -1595,9 +1993,9 @@ static int stlink_usb_read_mem8(void *handle, uint32_t addr, uint16_t len,
 
 	assert(handle != NULL);
 
-	/* max 8bit read/write is 64bytes */
-	if (len > STLINK_MAX_RW8) {
-		LOG_DEBUG("max buffer length exceeded");
+	/* max 8 bit read/write is 64 bytes or 512 bytes for v3 */
+	if (len > stlink_usb_block(h)) {
+		LOG_DEBUG("max buffer (%d) length exceeded", stlink_usb_block(h));
 		return ERROR_FAIL;
 	}
 
@@ -1633,9 +2031,9 @@ static int stlink_usb_write_mem8(void *handle, uint32_t addr, uint16_t len,
 
 	assert(handle != NULL);
 
-	/* max 8bit read/write is 64bytes */
-	if (len > STLINK_MAX_RW8) {
-		LOG_DEBUG("max buffer length exceeded");
+	/* max 8 bit read/write is 64 bytes or 512 bytes for v3 */
+	if (len > stlink_usb_block(h)) {
+		LOG_DEBUG("max buffer length (%d) exceeded", stlink_usb_block(h));
 		return ERROR_FAIL;
 	}
 
@@ -1657,6 +2055,78 @@ static int stlink_usb_write_mem8(void *handle, uint32_t addr, uint16_t len,
 }
 
 /** */
+static int stlink_usb_read_mem16(void *handle, uint32_t addr, uint16_t len,
+			  uint8_t *buffer)
+{
+	int res;
+	struct stlink_usb_handle_s *h = handle;
+
+	assert(handle != NULL);
+
+	if (!(h->version.flags & STLINK_F_HAS_MEM_16BIT))
+		return ERROR_COMMAND_NOTFOUND;
+
+	/* data must be a multiple of 2 and half-word aligned */
+	if (len % 2 || addr % 2) {
+		LOG_DEBUG("Invalid data alignment");
+		return ERROR_TARGET_UNALIGNED_ACCESS;
+	}
+
+	stlink_usb_init_buffer(handle, h->rx_ep, len);
+
+	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READMEM_16BIT;
+	h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
+	h->cmdidx += 4;
+	h_u16_to_le(h->cmdbuf+h->cmdidx, len);
+	h->cmdidx += 2;
+
+	res = stlink_usb_xfer(handle, h->databuf, len);
+
+	if (res != ERROR_OK)
+		return res;
+
+	memcpy(buffer, h->databuf, len);
+
+	return stlink_usb_get_rw_status(handle);
+}
+
+/** */
+static int stlink_usb_write_mem16(void *handle, uint32_t addr, uint16_t len,
+			   const uint8_t *buffer)
+{
+	int res;
+	struct stlink_usb_handle_s *h = handle;
+
+	assert(handle != NULL);
+
+	if (!(h->version.flags & STLINK_F_HAS_MEM_16BIT))
+		return ERROR_COMMAND_NOTFOUND;
+
+	/* data must be a multiple of 2 and half-word aligned */
+	if (len % 2 || addr % 2) {
+		LOG_DEBUG("Invalid data alignment");
+		return ERROR_TARGET_UNALIGNED_ACCESS;
+	}
+
+	stlink_usb_init_buffer(handle, h->tx_ep, len);
+
+	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEMEM_16BIT;
+	h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
+	h->cmdidx += 4;
+	h_u16_to_le(h->cmdbuf+h->cmdidx, len);
+	h->cmdidx += 2;
+
+	res = stlink_usb_xfer(handle, buffer, len);
+
+	if (res != ERROR_OK)
+		return res;
+
+	return stlink_usb_get_rw_status(handle);
+}
+
+/** */
 static int stlink_usb_read_mem32(void *handle, uint32_t addr, uint16_t len,
 			  uint8_t *buffer)
 {
@@ -1741,10 +2211,14 @@ static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size,
 	/* calculate byte count */
 	count *= size;
 
+	/* switch to 8 bit if stlink does not support 16 bit memory read */
+	if (size == 2 && !(h->version.flags & STLINK_F_HAS_MEM_16BIT))
+		size = 1;
+
 	while (count) {
 
-		bytes_remaining = (size == 4) ? \
-				stlink_max_block_size(h->max_mem_packet, addr) : STLINK_MAX_RW8;
+		bytes_remaining = (size != 1) ? \
+				stlink_max_block_size(h->max_mem_packet, addr) : stlink_usb_block(h);
 
 		if (count < bytes_remaining)
 			bytes_remaining = count;
@@ -1754,22 +2228,26 @@ static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size,
 			if (retval != ERROR_OK)
 				return retval;
 		} else
-		/* the stlink only supports 8/32bit memory read/writes
-		 * honour 32bit, all others will be handled as 8bit access */
-		if (size == 4) {
+		/*
+		 * all stlink support 8/32bit memory read/writes and only from
+		 * stlink V2J26 there is support for 16 bit memory read/write.
+		 * Honour 32 bit and, if possible, 16 bit too. Otherwise, handle
+		 * as 8bit access.
+		 */
+		if (size != 1) {
 
-			/* When in jtag mode the stlink uses the auto-increment functinality.
+			/* When in jtag mode the stlink uses the auto-increment functionality.
 			 * However it expects us to pass the data correctly, this includes
 			 * alignment and any page boundaries. We already do this as part of the
 			 * adi_v5 implementation, but the stlink is a hla adapter and so this
-			 * needs implementiong manually.
+			 * needs implementing manually.
 			 * currently this only affects jtag mode, according to ST they do single
 			 * access in SWD mode - but this may change and so we do it for both modes */
 
 			/* we first need to check for any unaligned bytes */
-			if (addr % 4) {
+			if (addr & (size - 1)) {
 
-				uint32_t head_bytes = 4 - (addr % 4);
+				uint32_t head_bytes = size - (addr & (size - 1));
 				retval = stlink_usb_read_mem8(handle, addr, head_bytes, buffer);
 				if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
 					usleep((1<<retries++) * 1000);
@@ -1783,8 +2261,10 @@ static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size,
 				bytes_remaining -= head_bytes;
 			}
 
-			if (bytes_remaining % 4)
+			if (bytes_remaining & (size - 1))
 				retval = stlink_usb_read_mem(handle, addr, 1, bytes_remaining, buffer);
+			else if (size == 2)
+				retval = stlink_usb_read_mem16(handle, addr, bytes_remaining, buffer);
 			else
 				retval = stlink_usb_read_mem32(handle, addr, bytes_remaining, buffer);
 		} else
@@ -1816,10 +2296,14 @@ static int stlink_usb_write_mem(void *handle, uint32_t addr, uint32_t size,
 	/* calculate byte count */
 	count *= size;
 
+	/* switch to 8 bit if stlink does not support 16 bit memory read */
+	if (size == 2 && !(h->version.flags & STLINK_F_HAS_MEM_16BIT))
+		size = 1;
+
 	while (count) {
 
-		bytes_remaining = (size == 4) ? \
-				stlink_max_block_size(h->max_mem_packet, addr) : STLINK_MAX_RW8;
+		bytes_remaining = (size != 1) ? \
+				stlink_max_block_size(h->max_mem_packet, addr) : stlink_usb_block(h);
 
 		if (count < bytes_remaining)
 			bytes_remaining = count;
@@ -1829,22 +2313,26 @@ static int stlink_usb_write_mem(void *handle, uint32_t addr, uint32_t size,
 			if (retval != ERROR_OK)
 				return retval;
 		} else
-		/* the stlink only supports 8/32bit memory read/writes
-		 * honour 32bit, all others will be handled as 8bit access */
-		if (size == 4) {
+		/*
+		 * all stlink support 8/32bit memory read/writes and only from
+		 * stlink V2J26 there is support for 16 bit memory read/write.
+		 * Honour 32 bit and, if possible, 16 bit too. Otherwise, handle
+		 * as 8bit access.
+		 */
+		if (size != 1) {
 
-			/* When in jtag mode the stlink uses the auto-increment functinality.
+			/* When in jtag mode the stlink uses the auto-increment functionality.
 			 * However it expects us to pass the data correctly, this includes
 			 * alignment and any page boundaries. We already do this as part of the
 			 * adi_v5 implementation, but the stlink is a hla adapter and so this
-			 * needs implementiong manually.
+			 * needs implementing manually.
 			 * currently this only affects jtag mode, according to ST they do single
 			 * access in SWD mode - but this may change and so we do it for both modes */
 
 			/* we first need to check for any unaligned bytes */
-			if (addr % 4) {
+			if (addr & (size - 1)) {
 
-				uint32_t head_bytes = 4 - (addr % 4);
+				uint32_t head_bytes = size - (addr & (size - 1));
 				retval = stlink_usb_write_mem8(handle, addr, head_bytes, buffer);
 				if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
 					usleep((1<<retries++) * 1000);
@@ -1858,8 +2346,10 @@ static int stlink_usb_write_mem(void *handle, uint32_t addr, uint32_t size,
 				bytes_remaining -= head_bytes;
 			}
 
-			if (bytes_remaining % 4)
+			if (bytes_remaining & (size - 1))
 				retval = stlink_usb_write_mem(handle, addr, 1, bytes_remaining, buffer);
+			else if (size == 2)
+				retval = stlink_usb_write_mem16(handle, addr, bytes_remaining, buffer);
 			else
 				retval = stlink_usb_write_mem32(handle, addr, bytes_remaining, buffer);
 
@@ -1886,70 +2376,223 @@ static int stlink_usb_override_target(const char *targetname)
 	return !strcmp(targetname, "cortex_m");
 }
 
-static int stlink_speed(void *handle, int khz, bool query)
+static int stlink_speed_swim(void *handle, int khz, bool query)
 {
-	unsigned i;
-	int speed_index = -1;
-	int speed_diff = INT_MAX;
-	struct stlink_usb_handle_s *h = handle;
-
-	if (h && (h->transport == HL_TRANSPORT_SWIM)) {
-		/*
+	/*
 			we dont care what the khz rate is
 			we only have low and high speed...
 			before changing speed the SWIM_CSR HS bit
 			must be updated
-		 */
-		if (khz == 0)
-			stlink_swim_speed(handle, 0);
-		else
-			stlink_swim_speed(handle, 1);
-		return khz;
-	}
+	 */
+	if (khz == 0)
+		stlink_swim_speed(handle, 0);
+	else
+		stlink_swim_speed(handle, 1);
+	return khz;
+}
 
-	/* only supported by stlink/v2 and for firmware >= 22 */
-	if (h && (h->version.stlink == 1 || h->version.jtag < 22))
-		return khz;
+static int stlink_match_speed_map(const struct speed_map *map, unsigned int map_size, int khz, bool query)
+{
+	unsigned int i;
+	int speed_index = -1;
+	int speed_diff = INT_MAX;
+	int last_valid_speed = -1;
+	bool match = true;
 
-	for (i = 0; i < ARRAY_SIZE(stlink_khz_to_speed_map); i++) {
-		if (khz == stlink_khz_to_speed_map[i].speed) {
+	for (i = 0; i < map_size; i++) {
+		if (!map[i].speed)
+			continue;
+		last_valid_speed = i;
+		if (khz == map[i].speed) {
 			speed_index = i;
 			break;
 		} else {
-			int current_diff = khz - stlink_khz_to_speed_map[i].speed;
+			int current_diff = khz - map[i].speed;
 			/* get abs value for comparison */
 			current_diff = (current_diff > 0) ? current_diff : -current_diff;
-			if ((current_diff < speed_diff) && khz >= stlink_khz_to_speed_map[i].speed) {
+			if ((current_diff < speed_diff) && khz >= map[i].speed) {
 				speed_diff = current_diff;
 				speed_index = i;
 			}
 		}
 	}
 
-	bool match = true;
-
 	if (speed_index == -1) {
 		/* this will only be here if we cannot match the slow speed.
 		 * use the slowest speed we support.*/
-		speed_index = ARRAY_SIZE(stlink_khz_to_speed_map) - 1;
+		speed_index = last_valid_speed;
 		match = false;
-	} else if (i == ARRAY_SIZE(stlink_khz_to_speed_map))
+	} else if (i == map_size)
 		match = false;
 
 	if (!match && query) {
 		LOG_INFO("Unable to match requested speed %d kHz, using %d kHz", \
-				khz, stlink_khz_to_speed_map[speed_index].speed);
+				khz, map[speed_index].speed);
 	}
 
-	if (h && !query) {
-		int result = stlink_usb_set_swdclk(h, stlink_khz_to_speed_map[speed_index].speed_divisor);
+	return speed_index;
+}
+
+static int stlink_speed_swd(void *handle, int khz, bool query)
+{
+	int speed_index;
+	struct stlink_usb_handle_s *h = handle;
+
+	/* old firmware cannot change it */
+	if (!(h->version.flags & STLINK_F_HAS_SWD_SET_FREQ))
+		return khz;
+
+	speed_index = stlink_match_speed_map(stlink_khz_to_speed_map_swd,
+		ARRAY_SIZE(stlink_khz_to_speed_map_swd), khz, query);
+
+	if (!query) {
+		int result = stlink_usb_set_swdclk(h, stlink_khz_to_speed_map_swd[speed_index].speed_divisor);
 		if (result != ERROR_OK) {
 			LOG_ERROR("Unable to set adapter speed");
 			return khz;
 		}
 	}
 
-	return stlink_khz_to_speed_map[speed_index].speed;
+	return stlink_khz_to_speed_map_swd[speed_index].speed;
+}
+
+static int stlink_speed_jtag(void *handle, int khz, bool query)
+{
+	int speed_index;
+	struct stlink_usb_handle_s *h = handle;
+
+	/* old firmware cannot change it */
+	if (!(h->version.flags & STLINK_F_HAS_JTAG_SET_FREQ))
+		return khz;
+
+	speed_index = stlink_match_speed_map(stlink_khz_to_speed_map_jtag,
+		ARRAY_SIZE(stlink_khz_to_speed_map_jtag), khz, query);
+
+	if (!query) {
+		int result = stlink_usb_set_jtagclk(h, stlink_khz_to_speed_map_jtag[speed_index].speed_divisor);
+		if (result != ERROR_OK) {
+			LOG_ERROR("Unable to set adapter speed");
+			return khz;
+		}
+	}
+
+	return stlink_khz_to_speed_map_jtag[speed_index].speed;
+}
+
+void stlink_dump_speed_map(const struct speed_map *map, unsigned int map_size)
+{
+	unsigned int i;
+
+	LOG_DEBUG("Supported clock speeds are:");
+	for (i = 0; i < map_size; i++)
+		if (map[i].speed)
+			LOG_DEBUG("%d kHz", map[i].speed);
+}
+
+static int stlink_get_com_freq(void *handle, bool is_jtag, struct speed_map *map)
+{
+	struct stlink_usb_handle_s *h = handle;
+	int i;
+
+	if (h->version.jtag_api != STLINK_JTAG_API_V3) {
+		LOG_ERROR("Unknown command");
+		return 0;
+	}
+
+	stlink_usb_init_buffer(handle, h->rx_ep, 16);
+
+	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+	h->cmdbuf[h->cmdidx++] = STLINK_APIV3_GET_COM_FREQ;
+	h->cmdbuf[h->cmdidx++] = is_jtag ? 1 : 0;
+
+	int res = stlink_usb_xfer(handle, h->databuf, 52);
+
+	int size = h->databuf[8];
+
+	if (size > STLINK_V3_MAX_FREQ_NB)
+		size = STLINK_V3_MAX_FREQ_NB;
+
+	for (i = 0; i < size; i++) {
+		map[i].speed = le_to_h_u32(&h->databuf[12 + 4 * i]);
+		map[i].speed_divisor = i;
+	}
+
+	/* set to zero all the next entries */
+	for (i = size; i < STLINK_V3_MAX_FREQ_NB; i++)
+		map[i].speed = 0;
+
+	return res;
+}
+
+static int stlink_set_com_freq(void *handle, bool is_jtag, unsigned int frequency)
+{
+	struct stlink_usb_handle_s *h = handle;
+
+	if (h->version.jtag_api != STLINK_JTAG_API_V3) {
+		LOG_ERROR("Unknown command");
+		return 0;
+	}
+
+	stlink_usb_init_buffer(handle, h->rx_ep, 16);
+
+	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+	h->cmdbuf[h->cmdidx++] = STLINK_APIV3_SET_COM_FREQ;
+	h->cmdbuf[h->cmdidx++] = is_jtag ? 1 : 0;
+	h->cmdbuf[h->cmdidx++] = 0;
+
+	h_u32_to_le(&h->cmdbuf[4], frequency);
+
+	return stlink_usb_xfer(handle, h->databuf, 8);
+}
+
+static int stlink_speed_v3(void *handle, bool is_jtag, int khz, bool query)
+{
+	struct stlink_usb_handle_s *h = handle;
+	int speed_index;
+	struct speed_map map[STLINK_V3_MAX_FREQ_NB];
+
+	stlink_get_com_freq(h, is_jtag, map);
+
+	speed_index = stlink_match_speed_map(map, ARRAY_SIZE(map), khz, query);
+
+	if (!query) {
+		int result = stlink_set_com_freq(h, is_jtag, map[speed_index].speed);
+		if (result != ERROR_OK) {
+			LOG_ERROR("Unable to set adapter speed");
+			return khz;
+		}
+	}
+	return map[speed_index].speed;
+}
+
+static int stlink_speed(void *handle, int khz, bool query)
+{
+	struct stlink_usb_handle_s *h = handle;
+
+	if (!handle)
+		return khz;
+
+	switch (h->transport) {
+	case HL_TRANSPORT_SWIM:
+		return stlink_speed_swim(handle, khz, query);
+		break;
+	case HL_TRANSPORT_SWD:
+		if (h->version.jtag_api == STLINK_JTAG_API_V3)
+			return stlink_speed_v3(handle, false, khz, query);
+		else
+			return stlink_speed_swd(handle, khz, query);
+		break;
+	case HL_TRANSPORT_JTAG:
+		if (h->version.jtag_api == STLINK_JTAG_API_V3)
+			return stlink_speed_v3(handle, true, khz, query);
+		else
+			return stlink_speed_jtag(handle, khz, query);
+		break;
+	default:
+		break;
+	}
+
+	return khz;
 }
 
 /** */
@@ -2005,7 +2648,6 @@ static int stlink_usb_open(struct hl_interface_param_s *param, void **fd)
 {
 	int err, retry_count = 1;
 	struct stlink_usb_handle_s *h;
-	enum stlink_jtag_api_version api;
 
 	LOG_DEBUG("stlink_usb_open");
 
@@ -2060,9 +2702,17 @@ static int stlink_usb_open(struct hl_interface_param_s *param, void **fd)
 			case STLINK_V1_PID:
 				h->version.stlink = 1;
 				h->tx_ep = STLINK_TX_EP;
-				h->trace_ep = STLINK_TRACE_EP;
+				break;
+			case STLINK_V3_USBLOADER_PID:
+			case STLINK_V3E_PID:
+			case STLINK_V3S_PID:
+			case STLINK_V3_2VCP_PID:
+				h->version.stlink = 3;
+				h->tx_ep = STLINK_V2_1_TX_EP;
+				h->trace_ep = STLINK_V2_1_TRACE_EP;
 				break;
 			case STLINK_V2_1_PID:
+			case STLINK_V2_1_NO_MSD_PID:
 				h->version.stlink = 2;
 				h->tx_ep = STLINK_V2_1_TX_EP;
 				h->trace_ep = STLINK_V2_1_TRACE_EP;
@@ -2113,6 +2763,9 @@ static int stlink_usb_open(struct hl_interface_param_s *param, void **fd)
 
 	switch (h->transport) {
 		case HL_TRANSPORT_SWD:
+			if (h->version.jtag_api == STLINK_JTAG_API_V1)
+				err = ERROR_FAIL;
+			/* fall-through */
 		case HL_TRANSPORT_JTAG:
 			if (h->version.jtag == 0)
 				err = ERROR_FAIL;
@@ -2131,13 +2784,6 @@ static int stlink_usb_open(struct hl_interface_param_s *param, void **fd)
 		goto error_open;
 	}
 
-	api = h->version.jtag_api_max;
-
-	LOG_INFO("using stlink api v%d", api);
-
-	/* set the used jtag api, this will default to the newest supported version */
-	h->jtag_api = api;
-
 	/* initialize the debug hardware */
 	err = stlink_usb_init_mode(h, param->connect_under_reset);
 
@@ -2157,13 +2803,23 @@ static int stlink_usb_open(struct hl_interface_param_s *param, void **fd)
 		return ERROR_OK;
 	}
 
-	/* clock speed only supported by stlink/v2 and for firmware >= 22 */
-	if (h->version.stlink >= 2 && h->version.jtag >= 22) {
-		LOG_DEBUG("Supported clock speeds are:");
+	if (h->transport == HL_TRANSPORT_JTAG) {
+		if (h->version.flags & STLINK_F_HAS_JTAG_SET_FREQ) {
+			stlink_dump_speed_map(stlink_khz_to_speed_map_jtag, ARRAY_SIZE(stlink_khz_to_speed_map_jtag));
+			stlink_speed(h, param->initial_interface_speed, false);
+		}
+	} else if (h->transport == HL_TRANSPORT_SWD) {
+		if (h->version.flags & STLINK_F_HAS_SWD_SET_FREQ) {
+			stlink_dump_speed_map(stlink_khz_to_speed_map_swd, ARRAY_SIZE(stlink_khz_to_speed_map_swd));
+			stlink_speed(h, param->initial_interface_speed, false);
+		}
+	}
 
-		for (unsigned i = 0; i < ARRAY_SIZE(stlink_khz_to_speed_map); i++)
-			LOG_DEBUG("%d kHz", stlink_khz_to_speed_map[i].speed);
+	if (h->version.jtag_api == STLINK_JTAG_API_V3) {
+		struct speed_map map[STLINK_V3_MAX_FREQ_NB];
 
+		stlink_get_com_freq(h, (h->transport == HL_TRANSPORT_JTAG), map);
+		stlink_dump_speed_map(map, ARRAY_SIZE(map));
 		stlink_speed(h, param->initial_interface_speed, false);
 	}
 
@@ -2199,7 +2855,7 @@ int stlink_config_trace(void *handle, bool enabled, enum tpiu_pin_protocol pin_p
 {
 	struct stlink_usb_handle_s *h = handle;
 
-	if (enabled && (h->jtag_api < 2 ||
+	if (enabled && (!(h->version.flags & STLINK_F_HAS_TRACE) ||
 			pin_protocol != TPIU_PIN_PROTOCOL_ASYNC_UART)) {
 		LOG_ERROR("The attached ST-LINK version doesn't support this trace mode");
 		return ERROR_FAIL;
diff --git a/src/jtag/drivers/sysfsgpio.c b/src/jtag/drivers/sysfsgpio.c
index 5535c71..90e93f5 100644
--- a/src/jtag/drivers/sysfsgpio.c
+++ b/src/jtag/drivers/sysfsgpio.c
@@ -457,62 +457,70 @@ static const struct command_registration sysfsgpio_command_handlers[] = {
 		.handler = &sysfsgpio_handle_jtag_gpionums,
 		.mode = COMMAND_CONFIG,
 		.help = "gpio numbers for tck, tms, tdi, tdo. (in that order)",
-		.usage = "(tck tms tdi tdo)* ",
+		.usage = "[tck tms tdi tdo]",
 	},
 	{
 		.name = "sysfsgpio_tck_num",
 		.handler = &sysfsgpio_handle_jtag_gpionum_tck,
 		.mode = COMMAND_CONFIG,
 		.help = "gpio number for tck.",
+		.usage = "[tck]",
 	},
 	{
 		.name = "sysfsgpio_tms_num",
 		.handler = &sysfsgpio_handle_jtag_gpionum_tms,
 		.mode = COMMAND_CONFIG,
 		.help = "gpio number for tms.",
+		.usage = "[tms]",
 	},
 	{
 		.name = "sysfsgpio_tdo_num",
 		.handler = &sysfsgpio_handle_jtag_gpionum_tdo,
 		.mode = COMMAND_CONFIG,
 		.help = "gpio number for tdo.",
+		.usage = "[tdo]",
 	},
 	{
 		.name = "sysfsgpio_tdi_num",
 		.handler = &sysfsgpio_handle_jtag_gpionum_tdi,
 		.mode = COMMAND_CONFIG,
 		.help = "gpio number for tdi.",
+		.usage = "[tdi]",
 	},
 	{
 		.name = "sysfsgpio_srst_num",
 		.handler = &sysfsgpio_handle_jtag_gpionum_srst,
 		.mode = COMMAND_CONFIG,
 		.help = "gpio number for srst.",
+		.usage = "[srst]",
 	},
 	{
 		.name = "sysfsgpio_trst_num",
 		.handler = &sysfsgpio_handle_jtag_gpionum_trst,
 		.mode = COMMAND_CONFIG,
 		.help = "gpio number for trst.",
+		.usage = "[trst]",
 	},
 	{
 		.name = "sysfsgpio_swd_nums",
 		.handler = &sysfsgpio_handle_swd_gpionums,
 		.mode = COMMAND_CONFIG,
 		.help = "gpio numbers for swclk, swdio. (in that order)",
-		.usage = "(swclk swdio)* ",
+		.usage = "[swclk swdio]",
 	},
 	{
 		.name = "sysfsgpio_swclk_num",
 		.handler = &sysfsgpio_handle_swd_gpionum_swclk,
 		.mode = COMMAND_CONFIG,
 		.help = "gpio number for swclk.",
+		.usage = "[swclk]",
 	},
 	{
 		.name = "sysfsgpio_swdio_num",
 		.handler = &sysfsgpio_handle_swd_gpionum_swdio,
 		.mode = COMMAND_CONFIG,
 		.help = "gpio number for swdio.",
+		.usage = "[swdio]",
 	},
 	COMMAND_REGISTRATION_DONE
 };
@@ -561,6 +569,8 @@ static void cleanup_all_fds(void)
 	cleanup_fd(tdo_fd, tdo_gpio);
 	cleanup_fd(trst_fd, trst_gpio);
 	cleanup_fd(srst_fd, srst_gpio);
+	cleanup_fd(swclk_fd, swclk_gpio);
+	cleanup_fd(swdio_fd, swdio_gpio);
 }
 
 static bool sysfsgpio_jtag_mode_possible(void)
diff --git a/src/jtag/drivers/usb_common.c b/src/jtag/drivers/usb_common.c
index 54be6a6..1b7602d 100644
--- a/src/jtag/drivers/usb_common.c
+++ b/src/jtag/drivers/usb_common.c
@@ -19,6 +19,7 @@
 #include "config.h"
 #endif
 #include "usb_common.h"
+#include "log.h"
 
 
 static bool jtag_usb_match(struct usb_device *dev,
@@ -45,10 +46,12 @@ int jtag_usb_open(const uint16_t vids[], const uint16_t pids[],
 				continue;
 
 			*out = usb_open(dev);
-			if (NULL == *out)
-				return -errno;
-			return 0;
+			if (NULL == *out) {
+				LOG_ERROR("usb_open() failed with %s", usb_strerror());
+				return ERROR_FAIL;
+			}
+			return ERROR_OK;
 		}
 	}
-	return -ENODEV;
+	return ERROR_FAIL;
 }
diff --git a/src/jtag/drivers/xds110.c b/src/jtag/drivers/xds110.c
index b97eef2..20b8178 100644
--- a/src/jtag/drivers/xds110.c
+++ b/src/jtag/drivers/xds110.c
@@ -29,6 +29,13 @@
 /* XDS110 USB serial number length */
 #define XDS110_SERIAL_LEN 8
 
+/* XDS110 stand-alone probe voltage supply limits */
+#define XDS110_MIN_VOLTAGE 1800
+#define XDS110_MAX_VOLTAGE 3600
+
+/* XDS110 stand-alone probe hardware ID */
+#define XDS110_STAND_ALONE_ID 0x21
+
 /* Firmware version that introduced OpenOCD support via block accesses */
 #define OCD_FIRMWARE_VERSION 0x02030011
 #define OCD_FIRMWARE_UPGRADE \
@@ -162,6 +169,7 @@
 #define SWD_DISCONNECT   0x18 /* Switch from SWD to JTAG connection */
 #define CJTAG_CONNECT    0x2b /* Switch from JTAG to cJTAG connection */
 #define CJTAG_DISCONNECT 0x2c /* Switch from cJTAG to JTAG connection */
+#define XDS_SET_SUPPLY   0x32 /* Set up stand-alone probe upply voltage */
 #define OCD_DAP_REQUEST  0x3a /* Handle block of DAP requests */
 #define OCD_SCAN_REQUEST 0x3b /* Handle block of JTAG scan requests */
 #define OCD_PATHMOVE     0x3c /* Handle PATHMOVE to navigate JTAG states */
@@ -219,6 +227,8 @@ struct xds110_info {
 	uint32_t delay_count;
 	/* XDS110 serial number */
 	char serial[XDS110_SERIAL_LEN + 1];
+	/* XDS110 voltage supply setting */
+	uint32_t voltage;
 	/* XDS110 firmware and hardware version */
 	uint32_t firmware;
 	uint16_t hardware;
@@ -242,6 +252,7 @@ static struct xds110_info xds110 = {
 	.speed = XDS110_MAX_TCK_SPEED,
 	.delay_count = 0,
 	.serial = {0},
+	.voltage = 0,
 	.firmware = 0,
 	.hardware = 0,
 	.txn_request_size = 0,
@@ -601,10 +612,15 @@ static bool xds_execute(uint32_t out_length, uint32_t in_length,
 			if (bytes_read != in_length) {
 				/* Unexpected amount of data returned */
 				success = false;
+				LOG_DEBUG("XDS110: command 0x%02x return %d bytes, expected %d",
+					xds110.write_payload[0], bytes_read, in_length);
 			} else {
 				/* Extract error code from return packet */
 				error = (int)xds110_get_u32(&xds110.read_payload[0]);
 				done = true;
+				if (SC_ERR_NONE != error)
+					LOG_DEBUG("XDS110: command 0x%02x returned error %d",
+						xds110.write_payload[0], error);
 			}
 		}
 	}
@@ -952,6 +968,24 @@ static bool cjtag_disconnect(void)
 	return success;
 }
 
+static bool xds_set_supply(uint32_t voltage)
+{
+	uint8_t *volts_pntr = &xds110.write_payload[XDS_OUT_LEN + 0]; /* 32-bits */
+	uint8_t *source_pntr = &xds110.write_payload[XDS_OUT_LEN + 4]; /* 8-bits */
+
+	bool success;
+
+	xds110.write_payload[0] = XDS_SET_SUPPLY;
+
+	xds110_set_u32(volts_pntr, voltage);
+	*source_pntr = (uint8_t)(0 != voltage ? 1 : 0);
+
+	success = xds_execute(XDS_OUT_LEN + 5, XDS_IN_LEN, DEFAULT_ATTEMPTS,
+				DEFAULT_TIMEOUT);
+
+	return success;
+}
+
 static bool ocd_dap_request(uint8_t *dap_requests, uint32_t request_size,
 	uint32_t *dap_results, uint32_t result_count)
 {
@@ -1318,7 +1352,7 @@ static void xds110_show_info(void)
 		(((firmware >>  4) & 0xf) * 10) + ((firmware >>  0) & 0xf));
 	LOG_INFO("XDS110: hardware version = 0x%04x", xds110.hardware);
 	if (0 != xds110.serial[0])
-		LOG_INFO("XDS110: serial number = %s)", xds110.serial);
+		LOG_INFO("XDS110: serial number = %s", xds110.serial);
 	if (xds110.is_swd_mode) {
 		LOG_INFO("XDS110: connected to target via SWD");
 		LOG_INFO("XDS110: SWCLK set to %d kHz", xds110.speed);
@@ -1391,6 +1425,20 @@ static int xds110_init(void)
 	}
 
 	if (success) {
+		/* Set supply voltage for stand-alone probes */
+		if (XDS110_STAND_ALONE_ID == xds110.hardware) {
+			success = xds_set_supply(xds110.voltage);
+			/* Allow time for target device to power up */
+			/* (CC32xx takes up to 1300 ms before debug is enabled) */
+			alive_sleep(1500);
+		} else if (0 != xds110.voltage) {
+			/* Voltage supply not a feature of embedded probes */
+			LOG_WARNING(
+				"XDS110: ignoring supply voltage, not supported on this probe");
+		}
+	}
+
+	if (success) {
 		success = xds_set_trst(0);
 		if (success)
 			success = xds_cycle_tck(50);
@@ -1569,6 +1617,9 @@ static void xds110_execute_reset(struct jtag_command *cmd)
 			srst = 0;
 		}
 		(void)xds_set_srst(srst);
+
+		/* Toggle TCK to trigger HIB on CC13x/CC26x devices */
+		(void)xds_cycle_tck(60000);
 	}
 }
 
@@ -1918,6 +1969,31 @@ COMMAND_HANDLER(xds110_handle_serial_command)
 	return ERROR_OK;
 }
 
+COMMAND_HANDLER(xds110_handle_supply_voltage_command)
+{
+	uint32_t voltage = 0;
+
+	if (CMD_ARGC == 1) {
+		COMMAND_PARSE_NUMBER(uint, CMD_ARGV[0], voltage);
+		if (voltage == 0 || (voltage >= XDS110_MIN_VOLTAGE && voltage
+			<= XDS110_MAX_VOLTAGE)) {
+			/* Requested voltage is in range */
+			xds110.voltage = voltage;
+		} else {
+			LOG_ERROR("XDS110: voltage must be 0 or between %d and %d "
+				"millivolts", XDS110_MIN_VOLTAGE, XDS110_MAX_VOLTAGE);
+			return ERROR_FAIL;
+		}
+		xds110.voltage = voltage;
+	} else {
+		LOG_ERROR("XDS110: expected one argument to xds110_supply_voltage "
+			"<millivolts>");
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
 static const struct command_registration xds110_subcommand_handlers[] = {
 	{
 		.name = "info",
@@ -1944,6 +2020,13 @@ static const struct command_registration xds110_command_handlers[] = {
 		.help = "set the XDS110 probe serial number",
 		.usage = "serial_string",
 	},
+	{
+		.name = "xds110_supply_voltage",
+		.handler = &xds110_handle_supply_voltage_command,
+		.mode = COMMAND_CONFIG,
+		.help = "set the XDS110 probe supply voltage",
+		.usage = "supply_voltage (millivolts)",
+	},
 	COMMAND_REGISTRATION_DONE
 };
 
diff --git a/src/jtag/swd.h b/src/jtag/swd.h
index 52f41d5..3ff4de0 100644
--- a/src/jtag/swd.h
+++ b/src/jtag/swd.h
@@ -53,16 +53,25 @@ static inline uint8_t swd_cmd(bool is_read, bool is_ap, uint8_t regnum)
 
 /* SWD_ACK_* bits are defined in <target/arm_adi_v5.h> */
 
+/*
+ * The following sequences are updated to
+ * ARM(tm) Debug Interface v5 Architecture Specification    ARM IHI 0031E
+ */
+
 /**
- * Line reset.
+ * SWD Line reset.
  *
- * Line reset is at least 50 SWCLK cycles with SWDIO driven high, followed
- * by at least one idle (low) cycle.
+ * SWD Line reset is at least 50 SWCLK cycles with SWDIO driven high,
+ * followed by at least two idle (low) cycle.
+ * Bits are stored (and transmitted) LSB-first.
  */
 static const uint8_t swd_seq_line_reset[] = {
-	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x03
+	/* At least 50 SWCLK cycles with SWDIO high */
+	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+	/* At least 2 idle (low) cycles */
+	0x00,
 };
-static const unsigned swd_seq_line_reset_len = 51;
+static const unsigned swd_seq_line_reset_len = 64;
 
 /**
  * JTAG-to-SWD sequence.
@@ -71,36 +80,53 @@ static const unsigned swd_seq_line_reset_len = 51;
  * high, putting either interface logic into reset state, followed by a
  * specific 16-bit sequence and finally a line reset in case the SWJ-DP was
  * already in SWD mode.
+ * Bits are stored (and transmitted) LSB-first.
  */
 static const uint8_t swd_seq_jtag_to_swd[] = {
-	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x7b, 0x9e,
-	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x0f,
+	/* At least 50 TCK/SWCLK cycles with TMS/SWDIO high */
+	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+	/* Switching sequence from JTAG to SWD */
+	0x9e, 0xe7,
+	/* At least 50 TCK/SWCLK cycles with TMS/SWDIO high */
+	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+	/* At least 2 idle (low) cycles */
+	0x00,
 };
-static const unsigned swd_seq_jtag_to_swd_len = 118;
+static const unsigned swd_seq_jtag_to_swd_len = 136;
 
 /**
  * SWD-to-JTAG sequence.
  *
  * The SWD-to-JTAG sequence is at least 50 TCK/SWCLK cycles with TMS/SWDIO
  * high, putting either interface logic into reset state, followed by a
- * specific 16-bit sequence and finally at least 5 TCK cycles to put the
- * JTAG TAP in TLR.
+ * specific 16-bit sequence and finally at least 5 TCK/SWCLK cycles with
+ * TMS/SWDIO high to put the JTAG TAP in Test-Logic-Reset state.
+ * Bits are stored (and transmitted) LSB-first.
  */
 static const uint8_t swd_seq_swd_to_jtag[] = {
-	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf3, 0x9c, 0xff
+	/* At least 50 TCK/SWCLK cycles with TMS/SWDIO high */
+	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+	/* Switching sequence from SWD to JTAG */
+	0x3c, 0xe7,
+	/* At least 5 TCK/SWCLK cycles with TMS/SWDIO high */
+	0xff,
 };
-static const unsigned swd_seq_swd_to_jtag_len = 71;
+static const unsigned swd_seq_swd_to_jtag_len = 80;
 
 /**
  * SWD-to-dormant sequence.
  *
  * This is at least 50 SWCLK cycles with SWDIO high to put the interface
  * in reset state, followed by a specific 16-bit sequence.
+ * Bits are stored (and transmitted) LSB-first.
  */
 static const uint8_t swd_seq_swd_to_dormant[] = {
-	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf3, 0x8e, 0x03
+	/* At least 50 SWCLK cycles with SWDIO high */
+	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+	 /* Switching sequence from SWD to dormant */
+	0xbc, 0xe3,
 };
-static const unsigned swd_seq_swd_to_dormant_len = 66;
+static const unsigned swd_seq_swd_to_dormant_len = 72;
 
 /**
  * Dormant-to-SWD sequence.
@@ -110,14 +136,82 @@ static const unsigned swd_seq_swd_to_dormant_len = 66;
  * sequence, followed by 4 TCK/SWCLK cycles with TMS/SWDIO low, followed by
  * a specific protocol-dependent activation code. For SWD the activation code
  * is an 8-bit sequence. The sequence ends with a line reset.
+ * Bits are stored (and transmitted) LSB-first.
  */
 static const uint8_t swd_seq_dormant_to_swd[] = {
+	/* At least 8 SWCLK cycles with SWDIO high */
+	0xff,
+	/* Selection alert sequence */
+	0x92, 0xf3, 0x09, 0x62, 0x95, 0x2d, 0x85, 0x86,
+	0xe9, 0xaf, 0xdd, 0xe3, 0xa2, 0x0e, 0xbc, 0x19,
+	/*
+	 * 4 SWCLK cycles with SWDIO low ...
+	 * + SWD activation code 0x1a ...
+	 * + at least 8 SWCLK cycles with SWDIO high
+	 */
+	0xa0, /* ((0x00)      & GENMASK(3, 0)) | ((0x1a << 4) & GENMASK(7, 4)) */
+	0xf1, /* ((0x1a >> 4) & GENMASK(3, 0)) | ((0xff << 4) & GENMASK(7, 4)) */
+	0xff,
+	/* At least 50 SWCLK cycles with SWDIO high */
+	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+	/* At least 2 idle (low) cycles */
+	0x00,
+};
+static const unsigned swd_seq_dormant_to_swd_len = 224;
+
+/**
+ * JTAG-to-dormant sequence.
+ *
+ * This is at least 5 TCK cycles with TMS high to put the interface
+ * in test-logic-reset state, followed by a specific 31-bit sequence.
+ * Bits are stored (and transmitted) LSB-first.
+ */
+static const uint8_t swd_seq_jtag_to_dormant[] = {
+	/* At least 5 TCK cycles with TMS high */
+	0xff,
+	/*
+	 * Still one TCK cycle with TMS high followed by 31 bits JTAG-to-DS
+	 * select sequence 0xba, 0xbb, 0xbb, 0x33,
+	 */
+	0x75, /* ((0xff >> 7) & GENMASK(0, 0)) | ((0xba << 1) & GENMASK(7, 1)) */
+	0x77, /* ((0xba >> 7) & GENMASK(0, 0)) | ((0xbb << 1) & GENMASK(7, 1)) */
+	0x77, /* ((0xbb >> 7) & GENMASK(0, 0)) | ((0xbb << 1) & GENMASK(7, 1)) */
+	0x67, /* ((0xbb >> 7) & GENMASK(0, 0)) | ((0x33 << 1) & GENMASK(7, 1)) */
+};
+static const unsigned swd_seq_jtag_to_dormant_len = 40;
+
+/**
+ * Dormant-to-JTAG sequence.
+ *
+ * This is at least 8 TCK/SWCLK cycles with TMS/SWDIO high to abort any ongoing
+ * selection alert sequence, followed by a specific 128-bit selection alert
+ * sequence, followed by 4 TCK/SWCLK cycles with TMS/SWDIO low, followed by
+ * a specific protocol-dependent activation code. For JTAG there are two
+ * possible activation codes:
+ * - "JTAG-Serial": 12 bits 0x00, 0x00
+ * - "Arm CoreSight JTAG-DP": 8 bits 0x0a
+ * We use "JTAG-Serial" only, which seams more generic.
+ * Since the target TAP can be either in Run/Test Idle or in Test-Logic-Reset
+ * states, Arm recommends to put the TAP in Run/Test Idle using one TCK cycle
+ * with TMS low. To keep the sequence length multiple of 8, 8 TCK cycle with
+ * TMS low are sent (allowed by JTAG state machine).
+ * Bits are stored (and transmitted) LSB-first.
+ */
+static const uint8_t swd_seq_dormant_to_jtag[] = {
+	/* At least 8 TCK/SWCLK cycles with TMS/SWDIO high */
 	0xff,
+	/* Selection alert sequence */
 	0x92, 0xf3, 0x09, 0x62, 0x95, 0x2d, 0x85, 0x86,
 	0xe9, 0xaf, 0xdd, 0xe3, 0xa2, 0x0e, 0xbc, 0x19,
-	0x10, 0xfa, 0xff, 0xff, 0xff, 0xff, 0xff, 0x3f
+	/*
+	 * 4 TCK/SWCLK cycles with TMS/SWDIO low ...
+	 * + 12 bits JTAG-serial activation code 0x00, 0x00
+	 */
+	0x00, 0x00,
+	/* put the TAP in Run/Test Idle */
+	0x00,
 };
-static const unsigned swd_seq_dormant_to_swd_len = 199;
+static const unsigned swd_seq_dormant_to_jtag_len = 160;
 
 enum swd_special_seq {
 	LINE_RESET,
diff --git a/src/jtag/tcl.c b/src/jtag/tcl.c
index e32f0ca..7683014 100644
--- a/src/jtag/tcl.c
+++ b/src/jtag/tcl.c
@@ -894,7 +894,7 @@ static const struct command_registration jtag_subcommand_handlers[] = {
 	},
 	{
 		.name = "configure",
-		.mode = COMMAND_EXEC,
+		.mode = COMMAND_ANY,
 		.jim_handler = jim_jtag_configure,
 		.help = "Provide a Tcl handler for the specified "
 			"TAP event.",
diff --git a/src/rtos/Makefile.am b/src/rtos/Makefile.am
index 5e18854..b3e14f8 100644
--- a/src/rtos/Makefile.am
+++ b/src/rtos/Makefile.am
@@ -12,6 +12,7 @@ noinst_LTLIBRARIES += %D%/librtos.la
 	%D%/eCos.c \
 	%D%/linux.c \
 	%D%/ChibiOS.c \
+	%D%/chromium-ec.c \
 	%D%/embKernel.c \
 	%D%/mqx.c \
 	%D%/riscv_debug.c \
diff --git a/src/rtos/chromium-ec.c b/src/rtos/chromium-ec.c
new file mode 100644
index 0000000..92ed2cb
--- /dev/null
+++ b/src/rtos/chromium-ec.c
@@ -0,0 +1,387 @@
+/*
+ * SPDX-License-Identifier: GPL-2.0
+ *
+ * Copyright (c) 2018 National Instruments Corp
+ * Author: Moritz Fischer <moritz.fischer@ettus.com>
+ *
+ * Chromium-EC RTOS Task Awareness
+ */
+
+#include <rtos/rtos.h>
+#include <target/target.h>
+#include <target/target_type.h>
+
+#include "rtos_standard_stackings.h"
+
+#define CROS_EC_MAX_TASKS 32
+#define CROS_EC_MAX_NAME 200
+#define CROS_EC_IDLE_STRING "<< idle >>"
+#define BIT(x) (1 << (x))
+
+struct chromium_ec_params {
+	const char *target_name;
+	size_t ptr_size;
+	off_t task_offset_next;
+	off_t task_offset_sp;
+	off_t task_offset_events;
+	off_t task_offset_runtime;
+	const struct rtos_register_stacking *stacking;
+};
+
+static const struct chromium_ec_params chromium_ec_params_list[] = {
+	{
+		.target_name = "hla_target",
+		.ptr_size = 4,
+		.task_offset_next = 24,
+		.task_offset_sp = 0,
+		.task_offset_events = 4,
+		.task_offset_runtime = 8,
+		.stacking = &rtos_standard_Cortex_M3_stacking,
+
+	},
+	{
+		.target_name = "cortex_m",
+		.ptr_size = 4,
+		.task_offset_next = 24,
+		.task_offset_sp = 0,
+		.task_offset_events = 4,
+		.task_offset_runtime = 8,
+		.stacking = &rtos_standard_Cortex_M3_stacking,
+	},
+};
+
+static const char * const chromium_ec_symbol_list[] = {
+	"start_called",
+	"current_task",
+	"tasks",
+	"tasks_enabled",
+	"tasks_ready",
+	"task_names",
+	"build_info",
+	NULL,
+};
+
+enum chromium_ec_symbol_values {
+	CHROMIUM_EC_VAL_start_called = 0,
+	CHROMIUM_EC_VAL_current_task,
+	CHROMIUM_EC_VAL_tasks,
+	CHROMIUM_EC_VAL_tasks_enabled,
+	CHROMIUM_EC_VAL_tasks_ready,
+	CHROMIUM_EC_VAL_task_names,
+	CHROMIUM_EC_VAL_build_info,
+
+	CHROMIUM_EC_VAL_COUNT,
+};
+
+#define CROS_EC_MAX_BUILDINFO 512
+
+static bool chromium_ec_detect_rtos(struct target *target)
+{
+	char build_info_buf[CROS_EC_MAX_BUILDINFO];
+	enum chromium_ec_symbol_values sym;
+	int ret;
+
+	if (!target || !target->rtos || !target->rtos->symbols)
+		return false;
+
+	for (sym = CHROMIUM_EC_VAL_start_called;
+	     sym < CHROMIUM_EC_VAL_COUNT; sym++) {
+		if (target->rtos->symbols[sym].address) {
+			LOG_DEBUG("Chromium-EC: Symbol \"%s\" found",
+				 chromium_ec_symbol_list[sym]);
+		} else {
+			LOG_ERROR("Chromium-EC: Symbol \"%s\" missing",
+				 chromium_ec_symbol_list[sym]);
+			return false;
+		}
+	}
+
+	ret = target_read_buffer(target,
+				 target->rtos->symbols[CHROMIUM_EC_VAL_build_info].address,
+				 sizeof(build_info_buf),
+				 (uint8_t *)build_info_buf);
+
+	if (ret != ERROR_OK)
+		return false;
+
+	LOG_INFO("Chromium-EC: Buildinfo: %s", build_info_buf);
+
+	return target->rtos->symbols &&
+	       target->rtos->symbols[CHROMIUM_EC_VAL_start_called].address;
+}
+
+static int chromium_ec_create(struct target *target)
+{
+	struct chromium_ec_params *params;
+	size_t t;
+
+	for (t = 0; t < ARRAY_SIZE(chromium_ec_params_list); t++)
+		if (!strcmp(chromium_ec_params_list[t].target_name, target->type->name)) {
+			params = malloc(sizeof(*params));
+			if (!params) {
+				LOG_ERROR("Chromium-EC: out of memory");
+				return ERROR_FAIL;
+			}
+
+			memcpy(params, &chromium_ec_params_list[t], sizeof(*params));
+			target->rtos->rtos_specific_params = (void *)params;
+			target->rtos->current_thread = 0;
+			target->rtos->thread_details = NULL;
+			target->rtos->thread_count = 0;
+
+			LOG_INFO("Chromium-EC: Using target: %s", target->type->name);
+			return ERROR_OK;
+		}
+
+	LOG_ERROR("Chromium-EC: target not supported: %s", target->type->name);
+	return ERROR_FAIL;
+}
+
+static int chromium_ec_get_current_task_ptr(struct rtos *rtos, uint32_t *current_task)
+{
+	if (!rtos || !rtos->symbols)
+		return ERROR_FAIL;
+
+	return target_read_u32(rtos->target,
+			       rtos->symbols[CHROMIUM_EC_VAL_current_task].address,
+			       current_task);
+}
+
+static int chromium_ec_get_num_tasks(struct rtos *rtos, int *num_tasks)
+{
+	uint32_t tasks_enabled;
+	int ret, t, found;
+
+	ret = target_read_u32(rtos->target,
+			      rtos->symbols[CHROMIUM_EC_VAL_tasks_enabled].address,
+			      &tasks_enabled);
+	if (ret != ERROR_OK) {
+		LOG_ERROR("Failed to determine #of tasks");
+		return ret;
+	}
+
+	found = 0;
+	for (t = 0; t < CROS_EC_MAX_TASKS; t++)
+		if (tasks_enabled & BIT(t))
+			found++;
+
+	*num_tasks = found;
+
+	return ERROR_OK;
+}
+
+static int chromium_ec_update_threads(struct rtos *rtos)
+{
+	uint32_t tasks_enabled, tasks_ready, start_called;
+	uint32_t current_task, thread_ptr, name_ptr;
+	char thread_str_buf[CROS_EC_MAX_NAME];
+	int ret, t, num_tasks, tasks_found;
+	struct chromium_ec_params *params;
+	uint8_t runtime_buf[8];
+	uint64_t runtime;
+	uint32_t events;
+
+	params = rtos->rtos_specific_params;
+	if (!params)
+		return ERROR_FAIL;
+
+	if (!rtos->symbols)
+		return ERROR_FAIL;
+
+	num_tasks = 0;
+	ret = chromium_ec_get_num_tasks(rtos, &num_tasks);
+	if (ret != ERROR_OK) {
+		LOG_ERROR("Failed to get number of tasks");
+		return ret;
+	}
+
+	current_task = 0;
+	ret = chromium_ec_get_current_task_ptr(rtos, &current_task);
+	if (ret != ERROR_OK) {
+		LOG_ERROR("Failed to get current task");
+		return ret;
+	}
+	LOG_DEBUG("Current task: %lx tasks_found: %d",
+		  (unsigned long)current_task,
+		  num_tasks);
+
+	/* set current task to what we read */
+	rtos->current_thread = current_task;
+
+	/* Nuke the old tasks */
+	rtos_free_threadlist(rtos);
+
+	/* One check if task switching has started ... */
+	start_called = 0;
+	ret = target_read_u32(rtos->target, rtos->symbols[CHROMIUM_EC_VAL_start_called].address,
+			      &start_called);
+	if (ret != ERROR_OK) {
+		LOG_ERROR("Failed to load start_called");
+		return ret;
+	}
+
+	if (!rtos->current_thread || !num_tasks || !start_called) {
+		num_tasks++;
+
+		rtos->thread_details = malloc(
+				sizeof(struct thread_detail) * num_tasks);
+		rtos->thread_details->threadid = 1;
+		rtos->thread_details->exists = true;
+		rtos->thread_details->extra_info_str = NULL;
+		rtos->thread_details->thread_name_str = strdup("Current Execution");
+
+		if (!num_tasks || !start_called) {
+			rtos->thread_count = 1;
+			return ERROR_OK;
+		}
+	} else {
+		/* create space for new thread details */
+		rtos->thread_details = malloc(
+				sizeof(struct thread_detail) * num_tasks);
+	}
+
+	tasks_enabled = 0;
+	ret = target_read_u32(rtos->target, rtos->symbols[CHROMIUM_EC_VAL_tasks_enabled].address,
+			      &tasks_enabled);
+	if (ret != ERROR_OK) {
+		LOG_ERROR("Failed to load tasks_enabled");
+		return ret;
+	}
+
+	tasks_ready = 0;
+	ret = target_read_u32(rtos->target, rtos->symbols[CHROMIUM_EC_VAL_tasks_ready].address,
+			      &tasks_ready);
+	if (ret != ERROR_OK) {
+		LOG_ERROR("Failed to load tasks_ready");
+		return ret;
+	}
+
+	thread_ptr = rtos->symbols[CHROMIUM_EC_VAL_tasks].address;
+
+	tasks_found = 0;
+	for (t = 0; t < CROS_EC_MAX_TASKS; t++) {
+		if (!(tasks_enabled & BIT(t)))
+			continue;
+
+		if (thread_ptr == current_task)
+			rtos->current_thread = thread_ptr;
+
+		rtos->thread_details[tasks_found].threadid = thread_ptr;
+		ret = target_read_u32(rtos->target,
+					 rtos->symbols[CHROMIUM_EC_VAL_task_names].address +
+					 params->ptr_size * t, &name_ptr);
+		if (ret != ERROR_OK) {
+			LOG_ERROR("Failed to read name_ptr");
+			return ret;
+		}
+
+		/* read name buffer */
+		ret = target_read_buffer(rtos->target, name_ptr, CROS_EC_MAX_NAME,
+					(uint8_t *)thread_str_buf);
+		if (ret != ERROR_OK) {
+			LOG_ERROR("Failed to read task name");
+			return ret;
+		}
+
+		/* sanitize string, gdb chokes on "<< idle >>" */
+		if (thread_str_buf[CROS_EC_MAX_NAME - 1] != '\0')
+			thread_str_buf[CROS_EC_MAX_NAME - 1] = '\0';
+		if (!strncmp(thread_str_buf, CROS_EC_IDLE_STRING, CROS_EC_MAX_NAME))
+		    rtos->thread_details[tasks_found].thread_name_str = strdup("IDLE");
+		else
+		    rtos->thread_details[tasks_found].thread_name_str = strdup(thread_str_buf);
+
+		events = 0;
+		ret = target_read_u32(rtos->target,
+				      thread_ptr + params->task_offset_events,
+				      &events);
+		if (ret != ERROR_OK)
+			LOG_ERROR("Failed to get task %d's events", t);
+
+		/* this is a bit kludgy but will do for now */
+		ret = target_read_buffer(rtos->target,
+					 thread_ptr + params->task_offset_runtime,
+					 sizeof(runtime_buf), runtime_buf);
+		if (ret != ERROR_OK)
+			LOG_ERROR("Failed to get task %d's runtime", t);
+		runtime =  target_buffer_get_u64(rtos->target, runtime_buf);
+
+		/* Priority is simply the positon in the array */
+		if (thread_ptr == current_task)
+			snprintf(thread_str_buf, sizeof(thread_str_buf),
+				 "State: Running, Priority: %u, Events: %" PRIx32 ", Runtime: %" PRIu64 "\n",
+				 t, events, runtime);
+		else
+			snprintf(thread_str_buf, sizeof(thread_str_buf),
+				 "State: %s, Priority: %u, Events: %" PRIx32 ", Runtime: %" PRIu64 "\n",
+				 tasks_ready & BIT(t) ? "Ready" : "Waiting", t,
+				 events, runtime);
+
+		rtos->thread_details[tasks_found].extra_info_str = strdup(thread_str_buf);
+		rtos->thread_details[tasks_found].exists = true;
+
+		thread_ptr += params->task_offset_next;
+
+		tasks_found++;
+	}
+
+	rtos->thread_count = tasks_found;
+
+	return ERROR_OK;
+}
+
+static int chromium_ec_get_thread_reg_list(struct rtos *rtos,
+					   threadid_t threadid,
+					   struct rtos_reg **reg_list,
+					   int *num_regs)
+{
+	struct chromium_ec_params *params = rtos->rtos_specific_params;
+	uint32_t stack_ptr = 0;
+	int ret, t;
+
+	for (t = 0; t < rtos->thread_count; t++)
+		if (threadid == rtos->thread_details[t].threadid)
+			break;
+
+	/* if we didn't find threadid, bail */
+	if (t == rtos->thread_count)
+		return ERROR_FAIL;
+
+	ret = target_read_u32(rtos->target,
+			   rtos->symbols[CHROMIUM_EC_VAL_tasks].address +
+			   params->task_offset_next * t,
+			   &stack_ptr);
+	if (ret != ERROR_OK) {
+		LOG_ERROR("Failed to load TCB");
+		return ret;
+	}
+
+	return rtos_generic_stack_read(rtos->target, params->stacking,
+				       stack_ptr, reg_list, num_regs);
+}
+
+static int chromium_ec_get_symbol_list_to_lookup(symbol_table_elem_t *symbol_list[])
+{
+	size_t s;
+
+	*symbol_list = calloc(ARRAY_SIZE(chromium_ec_symbol_list),
+			      sizeof(symbol_table_elem_t));
+	if (!(*symbol_list)) {
+		LOG_ERROR("Chromium-EC: out of memory");
+		return ERROR_FAIL;
+	}
+
+	for (s = 0; s < ARRAY_SIZE(chromium_ec_symbol_list); s++)
+		(*symbol_list)[s].symbol_name = chromium_ec_symbol_list[s];
+
+	return ERROR_OK;
+}
+
+const struct rtos_type chromium_ec_rtos = {
+	.name = "Chromium-EC",
+	.detect_rtos = chromium_ec_detect_rtos,
+	.create = chromium_ec_create,
+	.update_threads = chromium_ec_update_threads,
+	.get_thread_reg_list = chromium_ec_get_thread_reg_list,
+	.get_symbol_list_to_lookup = chromium_ec_get_symbol_list_to_lookup,
+};
diff --git a/src/rtos/rtos.c b/src/rtos/rtos.c
index a7c28f9..d4eff5b 100644
--- a/src/rtos/rtos.c
+++ b/src/rtos/rtos.c
@@ -32,6 +32,7 @@ extern struct rtos_type ThreadX_rtos;
 extern struct rtos_type eCos_rtos;
 extern struct rtos_type Linux_os;
 extern struct rtos_type ChibiOS_rtos;
+extern struct rtos_type chromium_ec_rtos;
 extern struct rtos_type embKernel_rtos;
 extern struct rtos_type mqx_rtos;
 extern struct rtos_type uCOS_III_rtos;
@@ -45,6 +46,7 @@ static struct rtos_type *rtos_types[] = {
 	&eCos_rtos,
 	&Linux_os,
 	&ChibiOS_rtos,
+	&chromium_ec_rtos,
 	&embKernel_rtos,
 	&mqx_rtos,
 	&uCOS_III_rtos,
diff --git a/src/target/Makefile.am b/src/target/Makefile.am
index 8e9fcb2..afa5f49 100644
--- a/src/target/Makefile.am
+++ b/src/target/Makefile.am
@@ -143,7 +143,8 @@ INTEL_IA32_SRC = \
 
 ESIRISC_SRC = \
 	%D%/esirisc.c \
-	%D%/esirisc_jtag.c
+	%D%/esirisc_jtag.c \
+	%D%/esirisc_trace.c
 
 %C%_libtarget_la_SOURCES += \
 	%D%/algorithm.h \
@@ -228,7 +229,8 @@ ESIRISC_SRC = \
 	%D%/arm_cti.h \
 	%D%/esirisc.h \
 	%D%/esirisc_jtag.h \
-	%D%/esirisc_regs.h
+	%D%/esirisc_regs.h \
+	%D%/esirisc_trace.h
 
 include %D%/openrisc/Makefile.am
 include %D%/riscv/Makefile.am
diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 454de9e..63174c2 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -100,6 +100,7 @@ static int aarch64_restore_system_control_reg(struct target *target)
 		case ARM_MODE_ABT:
 		case ARM_MODE_FIQ:
 		case ARM_MODE_IRQ:
+		case ARM_MODE_SYS:
 			instr = ARMV4_5_MCR(15, 0, 0, 1, 0, 0);
 			break;
 
@@ -172,6 +173,7 @@ static int aarch64_mmu_modify(struct target *target, int enable)
 	case ARM_MODE_ABT:
 	case ARM_MODE_FIQ:
 	case ARM_MODE_IRQ:
+	case ARM_MODE_SYS:
 		instr = ARMV4_5_MCR(15, 0, 0, 1, 0, 0);
 		break;
 
@@ -1032,6 +1034,7 @@ static int aarch64_post_debug_entry(struct target *target)
 	case ARM_MODE_ABT:
 	case ARM_MODE_FIQ:
 	case ARM_MODE_IRQ:
+	case ARM_MODE_SYS:
 		instr = ARMV4_5_MRC(15, 0, 0, 1, 0, 0);
 		break;
 
@@ -2824,6 +2827,7 @@ struct target_type aarch64_target = {
 	.deassert_reset = aarch64_deassert_reset,
 
 	/* REVISIT allow exporting VFP3 registers ... */
+	.get_gdb_arch = armv8_get_gdb_arch,
 	.get_gdb_reg_list = armv8_get_gdb_reg_list,
 
 	.read_memory = aarch64_read_memory,
diff --git a/src/target/adi_v5_jtag.c b/src/target/adi_v5_jtag.c
index 8c20611..a3867e1 100644
--- a/src/target/adi_v5_jtag.c
+++ b/src/target/adi_v5_jtag.c
@@ -72,8 +72,8 @@ static const char *dap_reg_name(int instr, int reg_addr)
 		case DP_RDBUFF:
 			reg_name =  "RDBUFF";
 			break;
-		case DP_WCR:
-			reg_name =  "WCR";
+		case DP_DLCR:
+			reg_name =  "DLCR";
 			break;
 		default:
 			reg_name = "UNK";
@@ -726,52 +726,3 @@ const struct dap_ops jtag_dp_ops = {
 	.run                 = jtag_dp_run,
 	.sync                = jtag_dp_sync,
 };
-
-
-static const uint8_t swd2jtag_bitseq[] = {
-	/* More than 50 TCK/SWCLK cycles with TMS/SWDIO high,
-	 * putting both JTAG and SWD logic into reset state.
-	 */
-	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
-	/* Switching equence disables SWD and enables JTAG
-	 * NOTE: bits in the DP's IDCODE can expose the need for
-	 * the old/deprecated sequence (0xae 0xde).
-	 */
-	0x3c, 0xe7,
-	/* At least 50 TCK/SWCLK cycles with TMS/SWDIO high,
-	 * putting both JTAG and SWD logic into reset state.
-	 * NOTE:  some docs say "at least 5".
-	 */
-	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
-};
-
-/** Put the debug link into JTAG mode, if the target supports it.
- * The link's initial mode may be either SWD or JTAG.
- *
- * @param target Enters JTAG mode (if possible).
- *
- * Note that targets implemented with SW-DP do not support JTAG, and
- * that some targets which could otherwise support it may have been
- * configured to disable JTAG signaling
- *
- * @return ERROR_OK or else a fault code.
- */
-int dap_to_jtag(struct target *target)
-{
-	int retval;
-
-	LOG_DEBUG("Enter JTAG mode");
-
-	/* REVISIT it's nasty to need to make calls to a "jtag"
-	 * subsystem if the link isn't in JTAG mode...
-	 */
-
-	retval = jtag_add_tms_seq(8 * sizeof(swd2jtag_bitseq),
-			swd2jtag_bitseq, TAP_RESET);
-	if (retval == ERROR_OK)
-		retval = jtag_execute_queue();
-
-	/* REVISIT set up the DAP's ops vector for JTAG mode. */
-
-	return retval;
-}
diff --git a/src/target/adi_v5_swd.c b/src/target/adi_v5_swd.c
index b520223..eb181cb 100644
--- a/src/target/adi_v5_swd.c
+++ b/src/target/adi_v5_swd.c
@@ -297,71 +297,6 @@ const struct dap_ops swd_dap_ops = {
 	.quit = swd_quit,
 };
 
-/*
- * This represents the bits which must be sent out on TMS/SWDIO to
- * switch a DAP implemented using an SWJ-DP module into SWD mode.
- * These bits are stored (and transmitted) LSB-first.
- *
- * See the DAP-Lite specification, section 2.2.5 for information
- * about making the debug link select SWD or JTAG.  (Similar info
- * is in a few other ARM documents.)
- */
-static const uint8_t jtag2swd_bitseq[] = {
-	/* More than 50 TCK/SWCLK cycles with TMS/SWDIO high,
-	 * putting both JTAG and SWD logic into reset state.
-	 */
-	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
-	/* Switching sequence enables SWD and disables JTAG
-	 * NOTE: bits in the DP's IDCODE may expose the need for
-	 * an old/obsolete/deprecated sequence (0xb6 0xed).
-	 */
-	0x9e, 0xe7,
-	/* More than 50 TCK/SWCLK cycles with TMS/SWDIO high,
-	 * putting both JTAG and SWD logic into reset state.
-	 */
-	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
-};
-
-/**
- * Put the debug link into SWD mode, if the target supports it.
- * The link's initial mode may be either JTAG (for example,
- * with SWJ-DP after reset) or SWD.
- *
- * @param target Enters SWD mode (if possible).
- *
- * Note that targets using the JTAG-DP do not support SWD, and that
- * some targets which could otherwise support it may have have been
- * configured to disable SWD signaling
- *
- * @return ERROR_OK or else a fault code.
- */
-int dap_to_swd(struct target *target)
-{
-	struct arm *arm = target_to_arm(target);
-	int retval;
-
-	if (!arm->dap) {
-		LOG_ERROR("SWD mode is not available");
-		return ERROR_FAIL;
-	}
-
-	LOG_DEBUG("Enter SWD mode");
-
-	/* REVISIT it's ugly to need to make calls to a "jtag"
-	 * subsystem if the link may not be in JTAG mode...
-	 */
-
-	retval =  jtag_add_tms_seq(8 * sizeof(jtag2swd_bitseq),
-			jtag2swd_bitseq, TAP_INVALID);
-	if (retval == ERROR_OK)
-		retval = jtag_execute_queue();
-
-	/* set up the DAP's ops vector for SWD mode. */
-	arm->dap->ops = &swd_dap_ops;
-
-	return retval;
-}
-
 static const struct command_registration swd_commands[] = {
 	{
 		/*
diff --git a/src/target/arm.h b/src/target/arm.h
index 316ff9a..ea83d38 100644
--- a/src/target/arm.h
+++ b/src/target/arm.h
@@ -263,9 +263,11 @@ struct reg_cache *armv8_build_reg_cache(struct target *target);
 extern const struct command_registration arm_command_handlers[];
 
 int arm_arch_state(struct target *target);
+const char *arm_get_gdb_arch(struct target *target);
 int arm_get_gdb_reg_list(struct target *target,
 		struct reg **reg_list[], int *reg_list_size,
 		enum target_register_class reg_class);
+const char *armv8_get_gdb_arch(struct target *target);
 int armv8_get_gdb_reg_list(struct target *target,
 		struct reg **reg_list[], int *reg_list_size,
 		enum target_register_class reg_class);
diff --git a/src/target/arm11.c b/src/target/arm11.c
index 13fbd20..4438661 100644
--- a/src/target/arm11.c
+++ b/src/target/arm11.c
@@ -1362,6 +1362,7 @@ struct target_type arm11_target = {
 	.assert_reset = arm11_assert_reset,
 	.deassert_reset = arm11_deassert_reset,
 
+	.get_gdb_arch = arm_get_gdb_arch,
 	.get_gdb_reg_list = arm_get_gdb_reg_list,
 
 	.read_memory = arm11_read_memory,
diff --git a/src/target/arm720t.c b/src/target/arm720t.c
index bcbfa9d..3d12aba 100644
--- a/src/target/arm720t.c
+++ b/src/target/arm720t.c
@@ -560,6 +560,7 @@ struct target_type arm720t_target = {
 	.deassert_reset = arm7_9_deassert_reset,
 	.soft_reset_halt = arm720t_soft_reset_halt,
 
+	.get_gdb_arch = arm_get_gdb_arch,
 	.get_gdb_reg_list = arm_get_gdb_reg_list,
 
 	.read_memory = arm720t_read_memory,
diff --git a/src/target/arm7tdmi.c b/src/target/arm7tdmi.c
index 9dcb302..e1e91c3 100644
--- a/src/target/arm7tdmi.c
+++ b/src/target/arm7tdmi.c
@@ -699,6 +699,7 @@ struct target_type arm7tdmi_target = {
 	.deassert_reset = arm7_9_deassert_reset,
 	.soft_reset_halt = arm7_9_soft_reset_halt,
 
+	.get_gdb_arch = arm_get_gdb_arch,
 	.get_gdb_reg_list = arm_get_gdb_reg_list,
 
 	.read_memory = arm7_9_read_memory,
diff --git a/src/target/arm920t.c b/src/target/arm920t.c
index 7927a2b..39d7590 100644
--- a/src/target/arm920t.c
+++ b/src/target/arm920t.c
@@ -1693,6 +1693,7 @@ struct target_type arm920t_target = {
 	.deassert_reset = arm7_9_deassert_reset,
 	.soft_reset_halt = arm920t_soft_reset_halt,
 
+	.get_gdb_arch = arm_get_gdb_arch,
 	.get_gdb_reg_list = arm_get_gdb_reg_list,
 
 	.read_memory = arm920t_read_memory,
diff --git a/src/target/arm926ejs.c b/src/target/arm926ejs.c
index 58de778..07c519a 100644
--- a/src/target/arm926ejs.c
+++ b/src/target/arm926ejs.c
@@ -804,6 +804,7 @@ struct target_type arm926ejs_target = {
 	.deassert_reset = arm7_9_deassert_reset,
 	.soft_reset_halt = arm926ejs_soft_reset_halt,
 
+	.get_gdb_arch = arm_get_gdb_arch,
 	.get_gdb_reg_list = arm_get_gdb_reg_list,
 
 	.read_memory = arm7_9_read_memory,
diff --git a/src/target/arm946e.c b/src/target/arm946e.c
index 06c9fc3..5e25d71 100644
--- a/src/target/arm946e.c
+++ b/src/target/arm946e.c
@@ -756,6 +756,7 @@ struct target_type arm946e_target = {
 	.deassert_reset = arm7_9_deassert_reset,
 	.soft_reset_halt = arm7_9_soft_reset_halt,
 
+	.get_gdb_arch = arm_get_gdb_arch,
 	.get_gdb_reg_list = arm_get_gdb_reg_list,
 
 	/* .read_memory = arm7_9_read_memory, */
diff --git a/src/target/arm966e.c b/src/target/arm966e.c
index 0429c54..c9d7f01 100644
--- a/src/target/arm966e.c
+++ b/src/target/arm966e.c
@@ -259,6 +259,7 @@ struct target_type arm966e_target = {
 	.deassert_reset = arm7_9_deassert_reset,
 	.soft_reset_halt = arm7_9_soft_reset_halt,
 
+	.get_gdb_arch = arm_get_gdb_arch,
 	.get_gdb_reg_list = arm_get_gdb_reg_list,
 
 	.read_memory = arm7_9_read_memory,
diff --git a/src/target/arm9tdmi.c b/src/target/arm9tdmi.c
index 82b430f..6425027 100644
--- a/src/target/arm9tdmi.c
+++ b/src/target/arm9tdmi.c
@@ -902,6 +902,7 @@ struct target_type arm9tdmi_target = {
 	.deassert_reset = arm7_9_deassert_reset,
 	.soft_reset_halt = arm7_9_soft_reset_halt,
 
+	.get_gdb_arch = arm_get_gdb_arch,
 	.get_gdb_reg_list = arm_get_gdb_reg_list,
 
 	.read_memory = arm7_9_read_memory,
diff --git a/src/target/arm_adi_v5.c b/src/target/arm_adi_v5.c
index 03e642b..021d02a 100644
--- a/src/target/arm_adi_v5.c
+++ b/src/target/arm_adi_v5.c
@@ -59,7 +59,7 @@
 /*
  * Relevant specifications from ARM include:
  *
- * ARM(tm) Debug Interface v5 Architecture Specification    ARM IHI 0031A
+ * ARM(tm) Debug Interface v5 Architecture Specification    ARM IHI 0031E
  * CoreSight(tm) v1.0 Architecture Specification            ARM IHI 0029B
  *
  * CoreSight(tm) DAP-Lite TRM, ARM DDI 0316D
@@ -73,6 +73,8 @@
 #include "jtag/interface.h"
 #include "arm.h"
 #include "arm_adi_v5.h"
+#include "jtag/swd.h"
+#include "transport/transport.h"
 #include <helper/jep106.h>
 #include <helper/time_support.h>
 #include <helper/list.h>
@@ -788,6 +790,77 @@ int mem_ap_init(struct adiv5_ap *ap)
 	return ERROR_OK;
 }
 
+/**
+ * Put the debug link into SWD mode, if the target supports it.
+ * The link's initial mode may be either JTAG (for example,
+ * with SWJ-DP after reset) or SWD.
+ *
+ * Note that targets using the JTAG-DP do not support SWD, and that
+ * some targets which could otherwise support it may have been
+ * configured to disable SWD signaling
+ *
+ * @param dap The DAP used
+ * @return ERROR_OK or else a fault code.
+ */
+int dap_to_swd(struct adiv5_dap *dap)
+{
+	int retval;
+
+	LOG_DEBUG("Enter SWD mode");
+
+	if (transport_is_jtag()) {
+		retval =  jtag_add_tms_seq(swd_seq_jtag_to_swd_len,
+				swd_seq_jtag_to_swd, TAP_INVALID);
+		if (retval == ERROR_OK)
+			retval = jtag_execute_queue();
+		return retval;
+	}
+
+	if (transport_is_swd()) {
+		const struct swd_driver *swd = adiv5_dap_swd_driver(dap);
+
+		return swd->switch_seq(JTAG_TO_SWD);
+	}
+
+	LOG_ERROR("Nor JTAG nor SWD transport");
+	return ERROR_FAIL;
+}
+
+/**
+ * Put the debug link into JTAG mode, if the target supports it.
+ * The link's initial mode may be either SWD or JTAG.
+ *
+ * Note that targets implemented with SW-DP do not support JTAG, and
+ * that some targets which could otherwise support it may have been
+ * configured to disable JTAG signaling
+ *
+ * @param dap The DAP used
+ * @return ERROR_OK or else a fault code.
+ */
+int dap_to_jtag(struct adiv5_dap *dap)
+{
+	int retval;
+
+	LOG_DEBUG("Enter JTAG mode");
+
+	if (transport_is_jtag()) {
+		retval = jtag_add_tms_seq(swd_seq_swd_to_jtag_len,
+				swd_seq_swd_to_jtag, TAP_RESET);
+		if (retval == ERROR_OK)
+			retval = jtag_execute_queue();
+		return retval;
+	}
+
+	if (transport_is_swd()) {
+		const struct swd_driver *swd = adiv5_dap_swd_driver(dap);
+
+		return swd->switch_seq(SWD_TO_JTAG);
+	}
+
+	LOG_ERROR("Nor JTAG nor SWD transport");
+	return ERROR_FAIL;
+}
+
 /* CID interpretation -- see ARM IHI 0029B section 3
  * and ARM IHI 0031A table 13-3.
  */
diff --git a/src/target/arm_adi_v5.h b/src/target/arm_adi_v5.h
index a340b76..96291a7 100644
--- a/src/target/arm_adi_v5.h
+++ b/src/target/arm_adi_v5.h
@@ -516,10 +516,10 @@ int dap_lookup_cs_component(struct adiv5_ap *ap,
 struct target;
 
 /* Put debug link into SWD mode */
-int dap_to_swd(struct target *target);
+int dap_to_swd(struct adiv5_dap *dap);
 
 /* Put debug link into JTAG mode */
-int dap_to_jtag(struct target *target);
+int dap_to_jtag(struct adiv5_dap *dap);
 
 extern const struct command_registration dap_instance_commands[];
 
diff --git a/src/target/arm_cti.c b/src/target/arm_cti.c
index dcaf21e..f333792 100644
--- a/src/target/arm_cti.c
+++ b/src/target/arm_cti.c
@@ -173,7 +173,7 @@ int arm_cti_clear_channel(struct arm_cti *self, uint32_t channel)
 	return arm_cti_write_reg(self, CTI_APPCLEAR, CTI_CHNL(channel));
 }
 
-static uint32_t cti_regs[26];
+static uint32_t cti_regs[28];
 
 static const struct {
 	uint32_t offset;
@@ -206,6 +206,8 @@ static const struct {
 	{ CTI_CHOU_STATUS,	"CHOUT", &cti_regs[23] },
 	{ CTI_APPSET,	"APPSET",	&cti_regs[24] },
 	{ CTI_APPCLEAR,	"APPCLR",	&cti_regs[25] },
+	{ CTI_APPPULSE,	"APPPULSE",	&cti_regs[26] },
+	{ CTI_INACK,	"INACK",	&cti_regs[27] },
 };
 
 static int cti_find_reg_offset(const char *name)
@@ -216,6 +218,8 @@ static int cti_find_reg_offset(const char *name)
 		if (!strcmp(name, cti_names[i].label))
 			return cti_names[i].offset;
 	}
+
+	LOG_ERROR("unknown CTI register %s", name);
 	return -1;
 }
 
@@ -297,7 +301,7 @@ COMMAND_HANDLER(handle_cti_write)
 	uint32_t value;
 
 	if (CMD_ARGC != 2) {
-		Jim_SetResultString(interp, "Wrong numer of args", -1);
+		Jim_SetResultString(interp, "Wrong number of args", -1);
 		return ERROR_FAIL;
 	}
 
@@ -320,7 +324,7 @@ COMMAND_HANDLER(handle_cti_read)
 	uint32_t value;
 
 	if (CMD_ARGC != 1) {
-		Jim_SetResultString(interp, "Wrong numer of args", -1);
+		Jim_SetResultString(interp, "Wrong number of args", -1);
 		return ERROR_FAIL;
 	}
 
diff --git a/src/target/arm_dap.c b/src/target/arm_dap.c
index 3adb4ed..119e511 100644
--- a/src/target/arm_dap.c
+++ b/src/target/arm_dap.c
@@ -313,6 +313,11 @@ COMMAND_HANDLER(handle_dap_info_command)
 	struct adiv5_dap *dap = arm->dap;
 	uint32_t apsel;
 
+	if (dap == NULL) {
+		LOG_ERROR("DAP instance not available. Probably a HLA target...");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+
 	switch (CMD_ARGC) {
 		case 0:
 			apsel = dap->apsel;
diff --git a/src/target/arm_opcodes.h b/src/target/arm_opcodes.h
index 482abe6..e94e833 100644
--- a/src/target/arm_opcodes.h
+++ b/src/target/arm_opcodes.h
@@ -213,7 +213,7 @@
 /* Breakpoint instruction (ARMv5)
  * Im: 16-bit immediate
  */
-#define ARMV5_BKPT(Im) (0xe1200070 | ((Im & 0xfff0) << 8) | (Im & 0xf))
+#define ARMV5_BKPT(Im) (0xe1200070 | ((Im & 0xfff0) << 4) | (Im & 0xf))
 
 
 /* Thumb mode instructions
diff --git a/src/target/armv4_5.c b/src/target/armv4_5.c
index 96a63e4..30aeb43 100644
--- a/src/target/armv4_5.c
+++ b/src/target/armv4_5.c
@@ -1134,6 +1134,7 @@ static const struct command_registration arm_exec_command_handlers[] = {
 	},
 	{
 		.name = "mrc",
+		.mode = COMMAND_EXEC,
 		.jim_handler = &jim_mcrmrc,
 		.help = "read coprocessor register",
 		.usage = "cpnum op1 CRn CRm op2",
@@ -1179,6 +1180,20 @@ const struct command_registration arm_command_handlers[] = {
 	COMMAND_REGISTRATION_DONE
 };
 
+/*
+ * gdb for arm targets (e.g. arm-none-eabi-gdb) supports several variants
+ * of arm architecture. You can list them using the autocompletion of gdb
+ * command prompt by typing "set architecture " and then press TAB key.
+ * The default, selected automatically, is "arm".
+ * Let's use the default value, here, to make gdb-multiarch behave in the
+ * same way as a gdb for arm. This can be changed later on. User can still
+ * set the specific architecture variant with the gdb command.
+ */
+const char *arm_get_gdb_arch(struct target *target)
+{
+	return "arm";
+}
+
 int arm_get_gdb_reg_list(struct target *target,
 	struct reg **reg_list[], int *reg_list_size,
 	enum target_register_class reg_class)
@@ -1340,6 +1355,8 @@ int armv4_5_run_algorithm_inner(struct target *target,
 	cpsr = buf_get_u32(arm->cpsr->value, 0, 32);
 
 	for (i = 0; i < num_mem_params; i++) {
+		if (mem_params[i].direction == PARAM_IN)
+			continue;
 		retval = target_write_buffer(target, mem_params[i].address, mem_params[i].size,
 				mem_params[i].value);
 		if (retval != ERROR_OK)
@@ -1347,6 +1364,9 @@ int armv4_5_run_algorithm_inner(struct target *target,
 	}
 
 	for (i = 0; i < num_reg_params; i++) {
+		if (reg_params[i].direction == PARAM_IN)
+			continue;
+
 		struct reg *reg = register_get_by_name(arm->core_cache, reg_params[i].reg_name, 0);
 		if (!reg) {
 			LOG_ERROR("BUG: register '%s' not found", reg_params[i].reg_name);
diff --git a/src/target/armv7a_mmu.c b/src/target/armv7a_mmu.c
index eed73ee..23d201f 100644
--- a/src/target/armv7a_mmu.c
+++ b/src/target/armv7a_mmu.c
@@ -72,7 +72,8 @@ int armv7a_mmu_translate_va(struct target *target,  uint32_t va, uint32_t *val)
 	LOG_DEBUG("1st lvl desc: %8.8" PRIx32 "", first_lvl_descriptor);
 
 	if ((first_lvl_descriptor & 0x3) == 0) {
-		LOG_ERROR("Address translation failure");
+		/* Avoid LOG_ERROR, probably GDB is guessing the stack frame */
+		LOG_WARNING("Address translation failure [1]: va %8.8" PRIx32 "", va);
 		return ERROR_TARGET_TRANSLATION_FAULT;
 	}
 
@@ -103,7 +104,8 @@ int armv7a_mmu_translate_va(struct target *target,  uint32_t va, uint32_t *val)
 	LOG_DEBUG("2nd lvl desc: %8.8" PRIx32 "", second_lvl_descriptor);
 
 	if ((second_lvl_descriptor & 0x3) == 0) {
-		LOG_ERROR("Address translation failure");
+		/* Avoid LOG_ERROR, probably GDB is guessing the stack frame */
+		LOG_WARNING("Address translation failure [2]: va %8.8" PRIx32 "", va);
 		return ERROR_TARGET_TRANSLATION_FAULT;
 	}
 
diff --git a/src/target/armv7m.c b/src/target/armv7m.c
index 7d3bd73..ef00b94 100644
--- a/src/target/armv7m.c
+++ b/src/target/armv7m.c
@@ -379,7 +379,8 @@ int armv7m_start_algorithm(struct target *target,
 	}
 
 	for (int i = 0; i < num_mem_params; i++) {
-		/* TODO: Write only out params */
+		if (mem_params[i].direction == PARAM_IN)
+			continue;
 		retval = target_write_buffer(target, mem_params[i].address,
 				mem_params[i].size,
 				mem_params[i].value);
@@ -388,6 +389,9 @@ int armv7m_start_algorithm(struct target *target,
 	}
 
 	for (int i = 0; i < num_reg_params; i++) {
+		if (reg_params[i].direction == PARAM_IN)
+			continue;
+
 		struct reg *reg =
 			register_get_by_name(armv7m->arm.core_cache, reg_params[i].reg_name, 0);
 /*		uint32_t regvalue; */
@@ -407,6 +411,23 @@ int armv7m_start_algorithm(struct target *target,
 		armv7m_set_core_reg(reg, reg_params[i].value);
 	}
 
+	{
+		/*
+		 * Ensure xPSR.T is set to avoid trying to run things in arm
+		 * (non-thumb) mode, which armv7m does not support.
+		 *
+		 * We do this by setting the entirety of xPSR, which should
+		 * remove all the unknowns about xPSR state.
+		 *
+		 * Because xPSR.T is populated on reset from the vector table,
+		 * it might be 0 if the vector table has "bad" data in it.
+		 */
+		struct reg *reg = &armv7m->arm.core_cache->reg_list[ARMV7M_xPSR];
+		buf_set_u32(reg->value, 0, 32, 0x01000000);
+		reg->valid = 1;
+		reg->dirty = 1;
+	}
+
 	if (armv7m_algorithm_info->core_mode != ARM_MODE_ANY &&
 			armv7m_algorithm_info->core_mode != core_mode) {
 
diff --git a/src/target/armv8.c b/src/target/armv8.c
index 75ada89..cee837f 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -74,6 +74,10 @@ static const struct {
 		.psr = ARM_MODE_ABT,
 	},
 	{
+		.name = "SYS",
+		.psr = ARM_MODE_SYS,
+	},
+	{
 		.name = "EL0T",
 		.psr = ARMV8_64_EL0T,
 	},
@@ -1477,6 +1481,9 @@ static int armv8_get_core_reg32(struct reg *reg)
 	struct reg *reg64;
 	int retval;
 
+	if (target->state != TARGET_HALTED)
+		return ERROR_TARGET_NOT_HALTED;
+
 	/* get the corresponding Aarch64 register */
 	reg64 = cache->reg_list + armv8_reg->num;
 	if (reg64->valid) {
@@ -1500,6 +1507,9 @@ static int armv8_set_core_reg32(struct reg *reg, uint8_t *buf)
 	struct reg *reg64 = cache->reg_list + armv8_reg->num;
 	uint32_t value = buf_get_u32(buf, 0, 32);
 
+	if (target->state != TARGET_HALTED)
+		return ERROR_TARGET_NOT_HALTED;
+
 	if (reg64 == arm->cpsr) {
 		armv8_set_cpsr(arm, value);
 	} else {
@@ -1668,6 +1678,11 @@ const struct command_registration armv8_command_handlers[] = {
 	COMMAND_REGISTRATION_DONE
 };
 
+const char *armv8_get_gdb_arch(struct target *target)
+{
+	return "aarch64";
+}
+
 int armv8_get_gdb_reg_list(struct target *target,
 	struct reg **reg_list[], int *reg_list_size,
 	enum target_register_class reg_class)
diff --git a/src/target/armv8_dpm.c b/src/target/armv8_dpm.c
index 3c941fa..a5d7d11 100644
--- a/src/target/armv8_dpm.c
+++ b/src/target/armv8_dpm.c
@@ -573,6 +573,7 @@ int armv8_dpm_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
 	case ARM_MODE_ABT:
 	case ARM_MODE_IRQ:
 	case ARM_MODE_FIQ:
+	case ARM_MODE_SYS:
 		target_el = 1;
 		break;
 	/*
@@ -790,6 +791,10 @@ int armv8_dpm_read_current_registers(struct arm_dpm *dpm)
 				dpm->last_el != armv8_curel_from_core_mode(arm_reg->mode))
 			continue;
 
+		/* Special case: ARM_MODE_SYS has no SPSR at EL1 */
+		if (r->number == ARMV8_SPSR_EL1 && arm->core_mode == ARM_MODE_SYS)
+			continue;
+
 		retval = dpmv8_read_reg(dpm, r, i);
 		if (retval != ERROR_OK)
 			goto fail;
diff --git a/src/target/cortex_a.c b/src/target/cortex_a.c
index 92ba547..648317e 100644
--- a/src/target/cortex_a.c
+++ b/src/target/cortex_a.c
@@ -202,6 +202,7 @@ static int cortex_a_mmu_modify(struct target *target, int enable)
 static int cortex_a_init_debug_access(struct target *target)
 {
 	struct armv7a_common *armv7a = target_to_armv7a(target);
+	uint32_t dscr;
 	int retval;
 
 	/* lock memory-mapped access to debug registers to prevent
@@ -231,6 +232,16 @@ static int cortex_a_init_debug_access(struct target *target)
 
 	/* Resync breakpoint registers */
 
+	/* Enable halt for breakpoint, watchpoint and vector catch */
+	retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
+			armv7a->debug_base + CPUDBG_DSCR, &dscr);
+	if (retval != ERROR_OK)
+		return retval;
+	retval = mem_ap_write_atomic_u32(armv7a->debug_ap,
+			armv7a->debug_base + CPUDBG_DSCR, dscr | DSCR_HALT_DBG_MODE);
+	if (retval != ERROR_OK)
+		return retval;
+
 	/* Since this is likely called from init or reset, update target state information*/
 	return cortex_a_poll(target);
 }
@@ -714,39 +725,26 @@ static int cortex_a_poll(struct target *target)
 			/* We have a halting debug event */
 			LOG_DEBUG("Target halted");
 			target->state = TARGET_HALTED;
-			if ((prev_target_state == TARGET_RUNNING)
-				|| (prev_target_state == TARGET_UNKNOWN)
-				|| (prev_target_state == TARGET_RESET)) {
-				retval = cortex_a_debug_entry(target);
+
+			retval = cortex_a_debug_entry(target);
+			if (retval != ERROR_OK)
+				return retval;
+
+			if (target->smp) {
+				retval = update_halt_gdb(target);
 				if (retval != ERROR_OK)
 					return retval;
-				if (target->smp) {
-					retval = update_halt_gdb(target);
-					if (retval != ERROR_OK)
-						return retval;
-				}
+			}
 
+			if (prev_target_state == TARGET_DEBUG_RUNNING) {
+				target_call_event_callbacks(target, TARGET_EVENT_DEBUG_HALTED);
+			} else { /* prev_target_state is RUNNING, UNKNOWN or RESET */
 				if (arm_semihosting(target, &retval) != 0)
 					return retval;
 
 				target_call_event_callbacks(target,
 					TARGET_EVENT_HALTED);
 			}
-			if (prev_target_state == TARGET_DEBUG_RUNNING) {
-				LOG_DEBUG(" ");
-
-				retval = cortex_a_debug_entry(target);
-				if (retval != ERROR_OK)
-					return retval;
-				if (target->smp) {
-					retval = update_halt_gdb(target);
-					if (retval != ERROR_OK)
-						return retval;
-				}
-
-				target_call_event_callbacks(target,
-					TARGET_EVENT_DEBUG_HALTED);
-			}
 		}
 	} else
 		target->state = TARGET_RUNNING;
@@ -769,19 +767,6 @@ static int cortex_a_halt(struct target *target)
 	if (retval != ERROR_OK)
 		return retval;
 
-	/*
-	 * enter halting debug mode
-	 */
-	retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
-			armv7a->debug_base + CPUDBG_DSCR, &dscr);
-	if (retval != ERROR_OK)
-		return retval;
-
-	retval = mem_ap_write_atomic_u32(armv7a->debug_ap,
-			armv7a->debug_base + CPUDBG_DSCR, dscr | DSCR_HALT_DBG_MODE);
-	if (retval != ERROR_OK)
-		return retval;
-
 	int64_t then = timeval_ms();
 	for (;; ) {
 		retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
@@ -2889,7 +2874,20 @@ static int cortex_r4_target_create(struct target *target, Jim_Interp *interp)
 static void cortex_a_deinit_target(struct target *target)
 {
 	struct cortex_a_common *cortex_a = target_to_cortex_a(target);
-	struct arm_dpm *dpm = &cortex_a->armv7a_common.dpm;
+	struct armv7a_common *armv7a = &cortex_a->armv7a_common;
+	struct arm_dpm *dpm = &armv7a->dpm;
+	uint32_t dscr;
+	int retval;
+
+	if (target_was_examined(target)) {
+		/* Disable halt for breakpoint, watchpoint and vector catch */
+		retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
+				armv7a->debug_base + CPUDBG_DSCR, &dscr);
+		if (retval == ERROR_OK)
+			mem_ap_write_atomic_u32(armv7a->debug_ap,
+					armv7a->debug_base + CPUDBG_DSCR,
+					dscr & ~DSCR_HALT_DBG_MODE);
+	}
 
 	free(cortex_a->brp_list);
 	free(dpm->dbp);
@@ -3160,6 +3158,7 @@ struct target_type cortexa_target = {
 	.deassert_reset = cortex_a_deassert_reset,
 
 	/* REVISIT allow exporting VFP3 registers ... */
+	.get_gdb_arch = arm_get_gdb_arch,
 	.get_gdb_reg_list = arm_get_gdb_reg_list,
 
 	.read_memory = cortex_a_read_memory,
@@ -3239,6 +3238,7 @@ struct target_type cortexr4_target = {
 	.deassert_reset = cortex_a_deassert_reset,
 
 	/* REVISIT allow exporting VFP3 registers ... */
+	.get_gdb_arch = arm_get_gdb_arch,
 	.get_gdb_reg_list = arm_get_gdb_reg_list,
 
 	.read_memory = cortex_a_read_phys_memory,
diff --git a/src/target/cortex_m.c b/src/target/cortex_m.c
index 07fea51..62d3760 100644
--- a/src/target/cortex_m.c
+++ b/src/target/cortex_m.c
@@ -510,7 +510,10 @@ static int cortex_m_poll(struct target *target)
 	}
 
 	if (cortex_m->dcb_dhcsr & S_RESET_ST) {
-		target->state = TARGET_RESET;
+		if (target->state != TARGET_RESET) {
+			target->state = TARGET_RESET;
+			LOG_INFO("%s: external reset detected", target_name(target));
+		}
 		return ERROR_OK;
 	}
 
@@ -564,6 +567,17 @@ static int cortex_m_poll(struct target *target)
 		}
 	}
 
+	/* Check that target is truly halted, since the target could be resumed externally */
+	if ((prev_target_state == TARGET_HALTED) && !(cortex_m->dcb_dhcsr & S_HALT)) {
+		/* registers are now invalid */
+		register_cache_invalidate(armv7m->arm.core_cache);
+
+		target->state = TARGET_RUNNING;
+		LOG_WARNING("%s: external resume detected", target_name(target));
+		target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
+		retval = ERROR_OK;
+	}
+
 	/* Did we detect a failure condition that we cleared? */
 	if (detected_failure != ERROR_OK)
 		retval = detected_failure;
@@ -863,10 +877,17 @@ static int cortex_m_step(struct target *target, int current,
 			else {
 
 				/* Set a temporary break point */
-				if (breakpoint)
+				if (breakpoint) {
 					retval = cortex_m_set_breakpoint(target, breakpoint);
-				else
-					retval = breakpoint_add(target, pc_value, 2, BKPT_HARD);
+				} else {
+					enum breakpoint_type type = BKPT_HARD;
+					if (cortex_m->fp_rev == 0 && pc_value > 0x1FFFFFFF) {
+						/* FPB rev.1 cannot handle such addr, try BKPT instr */
+						type = BKPT_SOFT;
+					}
+					retval = breakpoint_add(target, pc_value, 2, type);
+				}
+
 				bool tmp_bp_set = (retval == ERROR_OK);
 
 				/* No more breakpoints left, just do a step */
@@ -1041,10 +1062,18 @@ static int cortex_m_assert_reset(struct target *target)
 		retval = ERROR_OK;
 	} else {
 		/* Use a standard Cortex-M3 software reset mechanism.
-		 * We default to using VECRESET as it is supported on all current cores.
+		 * We default to using VECRESET as it is supported on all current cores
+		 * (except Cortex-M0, M0+ and M1 which support SYSRESETREQ only!)
 		 * This has the disadvantage of not resetting the peripherals, so a
 		 * reset-init event handler is needed to perform any peripheral resets.
 		 */
+		if (!cortex_m->vectreset_supported
+				&& reset_config == CORTEX_M_RESET_VECTRESET) {
+			reset_config = CORTEX_M_RESET_SYSRESETREQ;
+			LOG_WARNING("VECTRESET is not supported on this Cortex-M core, using SYSRESETREQ instead.");
+			LOG_WARNING("Set 'cortex_m reset_config sysresetreq'.");
+		}
+
 		LOG_DEBUG("Using Cortex-M %s", (reset_config == CORTEX_M_RESET_SYSRESETREQ)
 			? "SYSRESETREQ" : "VECTRESET");
 
@@ -2027,6 +2056,9 @@ int cortex_m_examine(struct target *target)
 		}
 		LOG_DEBUG("cpuid: 0x%8.8" PRIx32 "", cpuid);
 
+		/* VECTRESET is not supported on Cortex-M0, M0+ and M1 */
+		cortex_m->vectreset_supported = i > 1;
+
 		if (i == 4) {
 			target_read_u32(target, MVFR0, &mvfr0);
 			target_read_u32(target, MVFR1, &mvfr1);
@@ -2244,14 +2276,19 @@ static int cortex_m_init_arch_info(struct target *target,
 
 static int cortex_m_target_create(struct target *target, Jim_Interp *interp)
 {
-	struct cortex_m_common *cortex_m = calloc(1, sizeof(struct cortex_m_common));
-	cortex_m->common_magic = CORTEX_M_COMMON_MAGIC;
 	struct adiv5_private_config *pc;
 
 	pc = (struct adiv5_private_config *)target->private_config;
 	if (adiv5_verify_config(pc) != ERROR_OK)
 		return ERROR_FAIL;
 
+	struct cortex_m_common *cortex_m = calloc(1, sizeof(struct cortex_m_common));
+	if (cortex_m == NULL) {
+		LOG_ERROR("No memory creating target");
+		return ERROR_FAIL;
+	}
+
+	cortex_m->common_magic = CORTEX_M_COMMON_MAGIC;
 	cortex_m->apsel = pc->ap_num;
 
 	cortex_m_init_arch_info(target, cortex_m, pc->dap);
@@ -2418,8 +2455,16 @@ COMMAND_HANDLER(handle_cortex_m_reset_config_command)
 	if (CMD_ARGC > 0) {
 		if (strcmp(*CMD_ARGV, "sysresetreq") == 0)
 			cortex_m->soft_reset_config = CORTEX_M_RESET_SYSRESETREQ;
-		else if (strcmp(*CMD_ARGV, "vectreset") == 0)
-			cortex_m->soft_reset_config = CORTEX_M_RESET_VECTRESET;
+
+		else if (strcmp(*CMD_ARGV, "vectreset") == 0) {
+			if (target_was_examined(target)
+					&& !cortex_m->vectreset_supported)
+				LOG_WARNING("VECTRESET is not supported on your Cortex-M core!");
+			else
+				cortex_m->soft_reset_config = CORTEX_M_RESET_VECTRESET;
+
+		} else
+			return ERROR_COMMAND_SYNTAX_ERROR;
 	}
 
 	switch (cortex_m->soft_reset_config) {
@@ -2461,7 +2506,7 @@ static const struct command_registration cortex_m_exec_command_handlers[] = {
 		.handler = handle_cortex_m_reset_config_command,
 		.mode = COMMAND_ANY,
 		.help = "configure software reset handling",
-		.usage = "['srst'|'sysresetreq'|'vectreset']",
+		.usage = "['sysresetreq'|'vectreset']",
 	},
 	COMMAND_REGISTRATION_DONE
 };
@@ -2499,6 +2544,7 @@ struct target_type cortexm_target = {
 	.deassert_reset = cortex_m_deassert_reset,
 	.soft_reset_halt = cortex_m_soft_reset_halt,
 
+	.get_gdb_arch = arm_get_gdb_arch,
 	.get_gdb_reg_list = armv7m_get_gdb_reg_list,
 
 	.read_memory = cortex_m_read_memory,
diff --git a/src/target/cortex_m.h b/src/target/cortex_m.h
index 2daf4cb..22d9735 100644
--- a/src/target/cortex_m.h
+++ b/src/target/cortex_m.h
@@ -184,6 +184,7 @@ struct cortex_m_common {
 	struct reg_cache *dwt_cache;
 
 	enum cortex_m_soft_reset_config soft_reset_config;
+	bool vectreset_supported;
 
 	enum cortex_m_isrmasking_mode isrmasking_mode;
 
diff --git a/src/target/dsp563xx.c b/src/target/dsp563xx.c
index 1d728df..8991616 100644
--- a/src/target/dsp563xx.c
+++ b/src/target/dsp563xx.c
@@ -1387,6 +1387,8 @@ static int dsp563xx_run_algorithm(struct target *target,
 	}
 
 	for (i = 0; i < num_mem_params; i++) {
+		if (mem_params[i].direction == PARAM_IN)
+			continue;
 		retval = target_write_buffer(target, mem_params[i].address,
 				mem_params[i].size, mem_params[i].value);
 		if (retval != ERROR_OK)
@@ -1394,6 +1396,9 @@ static int dsp563xx_run_algorithm(struct target *target,
 	}
 
 	for (i = 0; i < num_reg_params; i++) {
+		if (reg_params[i].direction == PARAM_IN)
+			continue;
+
 		struct reg *reg = register_get_by_name(dsp563xx->core_cache,
 				reg_params[i].reg_name,
 				0);
diff --git a/src/target/esirisc.c b/src/target/esirisc.c
index 38100e8..3d2954f 100644
--- a/src/target/esirisc.c
+++ b/src/target/esirisc.c
@@ -34,27 +34,60 @@
 
 #include "esirisc.h"
 
-#define RESET_TIMEOUT	5000	/* 5s */
-#define STEP_TIMEOUT	1000	/* 1s */
+#define RESET_TIMEOUT			5000	/* 5s */
+#define STEP_TIMEOUT			1000	/* 1s */
 
 /*
  * eSi-RISC targets support a configurable number of interrupts;
  * up to 32 interrupts are supported.
  */
-static const char * const esirisc_exceptions[] = {
-	"Reset", "HardwareFailure", "NMI", "InstBreakpoint", "DataBreakpoint",
-	"Unsupported", "PrivilegeViolation", "InstBusError", "DataBusError",
-	"AlignmentError", "ArithmeticError", "SystemCall", "MemoryManagement",
-	"Unrecoverable", "Reserved",
-
-	"Interrupt0", "Interrupt1", "Interrupt2", "Interrupt3",
-	"Interrupt4", "Interrupt5", "Interrupt6", "Interrupt7",
-	"Interrupt8", "Interrupt9", "Interrupt10", "Interrupt11",
-	"Interrupt12", "Interrupt13", "Interrupt14", "Interrupt15",
-	"Interrupt16", "Interrupt17", "Interrupt18", "Interrupt19",
-	"Interrupt20", "Interrupt21", "Interrupt22", "Interrupt23",
-	"Interrupt24", "Interrupt25", "Interrupt26", "Interrupt27",
-	"Interrupt28", "Interrupt29", "Interrupt30", "Interrupt31",
+static const char * const esirisc_exception_strings[] = {
+	[EID_RESET]					= "Reset",
+	[EID_HARDWARE_FAILURE]		= "HardwareFailure",
+	[EID_NMI]					= "NMI",
+	[EID_INST_BREAKPOINT]		= "InstBreakpoint",
+	[EID_DATA_BREAKPOINT]		= "DataBreakpoint",
+	[EID_UNSUPPORTED]			= "Unsupported",
+	[EID_PRIVILEGE_VIOLATION]	= "PrivilegeViolation",
+	[EID_INST_BUS_ERROR]		= "InstBusError",
+	[EID_DATA_BUS_ERROR]		= "DataBusError",
+	[EID_ALIGNMENT_ERROR]		= "AlignmentError",
+	[EID_ARITHMETIC_ERROR]		= "ArithmeticError",
+	[EID_SYSTEM_CALL]			= "SystemCall",
+	[EID_MEMORY_MANAGEMENT]		= "MemoryManagement",
+	[EID_UNRECOVERABLE]			= "Unrecoverable",
+	[EID_INTERRUPTn+0]			= "Interrupt0",
+	[EID_INTERRUPTn+1]			= "Interrupt1",
+	[EID_INTERRUPTn+2]			= "Interrupt2",
+	[EID_INTERRUPTn+3]			= "Interrupt3",
+	[EID_INTERRUPTn+4]			= "Interrupt4",
+	[EID_INTERRUPTn+5]			= "Interrupt5",
+	[EID_INTERRUPTn+6]			= "Interrupt6",
+	[EID_INTERRUPTn+7]			= "Interrupt7",
+	[EID_INTERRUPTn+8]			= "Interrupt8",
+	[EID_INTERRUPTn+9]			= "Interrupt9",
+	[EID_INTERRUPTn+10]			= "Interrupt10",
+	[EID_INTERRUPTn+11]			= "Interrupt11",
+	[EID_INTERRUPTn+12]			= "Interrupt12",
+	[EID_INTERRUPTn+13]			= "Interrupt13",
+	[EID_INTERRUPTn+14]			= "Interrupt14",
+	[EID_INTERRUPTn+15]			= "Interrupt15",
+	[EID_INTERRUPTn+16]			= "Interrupt16",
+	[EID_INTERRUPTn+17]			= "Interrupt17",
+	[EID_INTERRUPTn+18]			= "Interrupt18",
+	[EID_INTERRUPTn+19]			= "Interrupt19",
+	[EID_INTERRUPTn+20]			= "Interrupt20",
+	[EID_INTERRUPTn+21]			= "Interrupt21",
+	[EID_INTERRUPTn+22]			= "Interrupt22",
+	[EID_INTERRUPTn+23]			= "Interrupt23",
+	[EID_INTERRUPTn+24]			= "Interrupt24",
+	[EID_INTERRUPTn+25]			= "Interrupt25",
+	[EID_INTERRUPTn+26]			= "Interrupt26",
+	[EID_INTERRUPTn+27]			= "Interrupt27",
+	[EID_INTERRUPTn+28]			= "Interrupt28",
+	[EID_INTERRUPTn+29]			= "Interrupt29",
+	[EID_INTERRUPTn+30]			= "Interrupt30",
+	[EID_INTERRUPTn+31]			= "Interrupt31",
 };
 
 /*
@@ -142,7 +175,7 @@ static int esirisc_disable_interrupts(struct target *target)
 
 	retval = esirisc_jtag_read_csr(jtag_info, CSR_THREAD, CSR_THREAD_ETC, &etc);
 	if (retval != ERROR_OK) {
-		LOG_ERROR("%s: failed to read CSR: ETC", target_name(target));
+		LOG_ERROR("%s: failed to read Thread CSR: ETC", target_name(target));
 		return retval;
 	}
 
@@ -150,7 +183,7 @@ static int esirisc_disable_interrupts(struct target *target)
 
 	retval = esirisc_jtag_write_csr(jtag_info, CSR_THREAD, CSR_THREAD_ETC, etc);
 	if (retval != ERROR_OK) {
-		LOG_ERROR("%s: failed to write CSR: ETC", target_name(target));
+		LOG_ERROR("%s: failed to write Thread CSR: ETC", target_name(target));
 		return retval;
 	}
 
@@ -169,7 +202,7 @@ static int esirisc_enable_interrupts(struct target *target)
 
 	retval = esirisc_jtag_read_csr(jtag_info, CSR_THREAD, CSR_THREAD_ETC, &etc);
 	if (retval != ERROR_OK) {
-		LOG_ERROR("%s: failed to read CSR: ETC", target_name(target));
+		LOG_ERROR("%s: failed to read Thread CSR: ETC", target_name(target));
 		return retval;
 	}
 
@@ -177,7 +210,7 @@ static int esirisc_enable_interrupts(struct target *target)
 
 	retval = esirisc_jtag_write_csr(jtag_info, CSR_THREAD, CSR_THREAD_ETC, etc);
 	if (retval != ERROR_OK) {
-		LOG_ERROR("%s: failed to write CSR: ETC", target_name(target));
+		LOG_ERROR("%s: failed to write Thread CSR: ETC", target_name(target));
 		return retval;
 	}
 
@@ -195,7 +228,7 @@ static int esirisc_save_interrupts(struct target *target)
 	int retval = esirisc_jtag_read_csr(jtag_info, CSR_THREAD, CSR_THREAD_ETC,
 			&esirisc->etc_save);
 	if (retval != ERROR_OK) {
-		LOG_ERROR("%s: failed to read CSR: ETC", target_name(target));
+		LOG_ERROR("%s: failed to read Thread CSR: ETC", target_name(target));
 		return retval;
 	}
 
@@ -212,7 +245,7 @@ static int esirisc_restore_interrupts(struct target *target)
 	int retval = esirisc_jtag_write_csr(jtag_info, CSR_THREAD, CSR_THREAD_ETC,
 			esirisc->etc_save);
 	if (retval != ERROR_OK) {
-		LOG_ERROR("%s: failed to write CSR: ETC", target_name(target));
+		LOG_ERROR("%s: failed to write Thread CSR: ETC", target_name(target));
 		return retval;
 	}
 
@@ -230,7 +263,7 @@ static int esirisc_save_hwdc(struct target *target)
 	int retval = esirisc_jtag_read_csr(jtag_info, CSR_DEBUG, CSR_DEBUG_HWDC,
 			&esirisc->hwdc_save);
 	if (retval != ERROR_OK) {
-		LOG_ERROR("%s: failed to read CSR: HWDC", target_name(target));
+		LOG_ERROR("%s: failed to read Thread CSR: HWDC", target_name(target));
 		return retval;
 	}
 
@@ -248,7 +281,7 @@ static int esirisc_restore_hwdc(struct target *target)
 	int retval = esirisc_jtag_write_csr(jtag_info, CSR_DEBUG, CSR_DEBUG_HWDC,
 			esirisc->hwdc_save);
 	if (retval != ERROR_OK) {
-		LOG_ERROR("%s: failed to write CSR: HWDC", target_name(target));
+		LOG_ERROR("%s: failed to write Debug CSR: HWDC", target_name(target));
 		return retval;
 	}
 
@@ -478,14 +511,14 @@ static int esirisc_add_breakpoint(struct target *target, struct breakpoint *brea
 	retval = esirisc_jtag_write_csr(jtag_info, CSR_DEBUG, CSR_DEBUG_IBAn + bp_index,
 			breakpoint->address);
 	if (retval != ERROR_OK) {
-		LOG_ERROR("%s: failed to write CSR: IBA", target_name(target));
+		LOG_ERROR("%s: failed to write Debug CSR: IBA", target_name(target));
 		return retval;
 	}
 
 	/* enable instruction breakpoint */
 	retval = esirisc_jtag_read_csr(jtag_info, CSR_DEBUG, CSR_DEBUG_IBC, &ibc);
 	if (retval != ERROR_OK) {
-		LOG_ERROR("%s: failed to read CSR: IBC", target_name(target));
+		LOG_ERROR("%s: failed to read Debug CSR: IBC", target_name(target));
 		return retval;
 	}
 
@@ -493,7 +526,7 @@ static int esirisc_add_breakpoint(struct target *target, struct breakpoint *brea
 
 	retval = esirisc_jtag_write_csr(jtag_info, CSR_DEBUG, CSR_DEBUG_IBC, ibc);
 	if (retval != ERROR_OK) {
-		LOG_ERROR("%s: failed to write CSR: IBC", target_name(target));
+		LOG_ERROR("%s: failed to write Debug CSR: IBC", target_name(target));
 		return retval;
 	}
 
@@ -529,7 +562,7 @@ static int esirisc_remove_breakpoint(struct target *target, struct breakpoint *b
 	/* disable instruction breakpoint */
 	retval = esirisc_jtag_read_csr(jtag_info, CSR_DEBUG, CSR_DEBUG_IBC, &ibc);
 	if (retval != ERROR_OK) {
-		LOG_ERROR("%s: failed to read CSR: IBC", target_name(target));
+		LOG_ERROR("%s: failed to read Debug CSR: IBC", target_name(target));
 		return retval;
 	}
 
@@ -537,7 +570,7 @@ static int esirisc_remove_breakpoint(struct target *target, struct breakpoint *b
 
 	retval = esirisc_jtag_write_csr(jtag_info, CSR_DEBUG, CSR_DEBUG_IBC, ibc);
 	if (retval != ERROR_OK) {
-		LOG_ERROR("%s: failed to write CSR: IBC", target_name(target));
+		LOG_ERROR("%s: failed to write Debug CSR: IBC", target_name(target));
 		return retval;
 	}
 
@@ -557,7 +590,7 @@ static int esirisc_remove_breakpoints(struct target *target)
 	/* clear instruction breakpoints */
 	int retval = esirisc_jtag_write_csr(jtag_info, CSR_DEBUG, CSR_DEBUG_IBC, 0);
 	if (retval != ERROR_OK) {
-		LOG_ERROR("%s: failed to write CSR: IBC", target_name(target));
+		LOG_ERROR("%s: failed to write Debug CSR: IBC", target_name(target));
 		return retval;
 	}
 
@@ -604,14 +637,14 @@ static int esirisc_add_watchpoint(struct target *target, struct watchpoint *watc
 	retval = esirisc_jtag_write_csr(jtag_info, CSR_DEBUG, CSR_DEBUG_DBAn + wp_index,
 			watchpoint->address);
 	if (retval != ERROR_OK) {
-		LOG_ERROR("%s: failed to write CSR: DBA", target_name(target));
+		LOG_ERROR("%s: failed to write Debug CSR: DBA", target_name(target));
 		return retval;
 	}
 
 	/* specify data breakpoint size */
 	retval = esirisc_jtag_read_csr(jtag_info, CSR_DEBUG, CSR_DEBUG_DBS, &dbs);
 	if (retval != ERROR_OK) {
-		LOG_ERROR("%s: failed to read CSR: DBS", target_name(target));
+		LOG_ERROR("%s: failed to read Debug CSR: DBS", target_name(target));
 		return retval;
 	}
 
@@ -642,14 +675,14 @@ static int esirisc_add_watchpoint(struct target *target, struct watchpoint *watc
 
 	retval = esirisc_jtag_write_csr(jtag_info, CSR_DEBUG, CSR_DEBUG_DBS, dbs);
 	if (retval != ERROR_OK) {
-		LOG_ERROR("%s: failed to write CSR: DBS", target_name(target));
+		LOG_ERROR("%s: failed to write Debug CSR: DBS", target_name(target));
 		return retval;
 	}
 
 	/* enable data breakpoint */
 	retval = esirisc_jtag_read_csr(jtag_info, CSR_DEBUG, CSR_DEBUG_DBC, &dbc);
 	if (retval != ERROR_OK) {
-		LOG_ERROR("%s: failed to read CSR: DBC", target_name(target));
+		LOG_ERROR("%s: failed to read Debug CSR: DBC", target_name(target));
 		return retval;
 	}
 
@@ -677,7 +710,7 @@ static int esirisc_add_watchpoint(struct target *target, struct watchpoint *watc
 
 	retval = esirisc_jtag_write_csr(jtag_info, CSR_DEBUG, CSR_DEBUG_DBC, dbc);
 	if (retval != ERROR_OK) {
-		LOG_ERROR("%s: failed to write CSR: DBC", target_name(target));
+		LOG_ERROR("%s: failed to write Debug CSR: DBC", target_name(target));
 		return retval;
 	}
 
@@ -713,7 +746,7 @@ static int esirisc_remove_watchpoint(struct target *target, struct watchpoint *w
 	/* disable data breakpoint */
 	retval = esirisc_jtag_read_csr(jtag_info, CSR_DEBUG, CSR_DEBUG_DBC, &dbc);
 	if (retval != ERROR_OK) {
-		LOG_ERROR("%s: failed to read CSR: DBC", target_name(target));
+		LOG_ERROR("%s: failed to read Debug CSR: DBC", target_name(target));
 		return retval;
 	}
 
@@ -721,7 +754,7 @@ static int esirisc_remove_watchpoint(struct target *target, struct watchpoint *w
 
 	retval = esirisc_jtag_write_csr(jtag_info, CSR_DEBUG, CSR_DEBUG_DBC, dbc);
 	if (retval != ERROR_OK) {
-		LOG_ERROR("%s: failed to write CSR: DBC", target_name(target));
+		LOG_ERROR("%s: failed to write Debug CSR: DBC", target_name(target));
 		return retval;
 	}
 
@@ -741,7 +774,7 @@ static int esirisc_remove_watchpoints(struct target *target)
 	/* clear data breakpoints */
 	int retval = esirisc_jtag_write_csr(jtag_info, CSR_DEBUG, CSR_DEBUG_DBC, 0);
 	if (retval != ERROR_OK) {
-		LOG_ERROR("%s: failed to write CSR: DBC", target_name(target));
+		LOG_ERROR("%s: failed to write Debug CSR: DBC", target_name(target));
 		return retval;
 	}
 
@@ -782,7 +815,7 @@ static int esirisc_disable_step(struct target *target)
 
 	retval = esirisc_jtag_read_csr(jtag_info, CSR_DEBUG, CSR_DEBUG_DC, &dc);
 	if (retval != ERROR_OK) {
-		LOG_ERROR("%s: failed to read CSR: DC", target_name(target));
+		LOG_ERROR("%s: failed to read Debug CSR: DC", target_name(target));
 		return retval;
 	}
 
@@ -790,7 +823,7 @@ static int esirisc_disable_step(struct target *target)
 
 	retval = esirisc_jtag_write_csr(jtag_info, CSR_DEBUG, CSR_DEBUG_DC, dc);
 	if (retval != ERROR_OK) {
-		LOG_ERROR("%s: failed to write CSR: DC", target_name(target));
+		LOG_ERROR("%s: failed to write Debug CSR: DC", target_name(target));
 		return retval;
 	}
 
@@ -808,7 +841,7 @@ static int esirisc_enable_step(struct target *target)
 
 	retval = esirisc_jtag_read_csr(jtag_info, CSR_DEBUG, CSR_DEBUG_DC, &dc);
 	if (retval != ERROR_OK) {
-		LOG_ERROR("%s: failed to read CSR: DC", target_name(target));
+		LOG_ERROR("%s: failed to read Debug CSR: DC", target_name(target));
 		return retval;
 	}
 
@@ -816,7 +849,7 @@ static int esirisc_enable_step(struct target *target)
 
 	retval = esirisc_jtag_write_csr(jtag_info, CSR_DEBUG, CSR_DEBUG_DC, dc);
 	if (retval != ERROR_OK) {
-		LOG_ERROR("%s: failed to write CSR: DC", target_name(target));
+		LOG_ERROR("%s: failed to write Debug CSR: DC", target_name(target));
 		return retval;
 	}
 
@@ -1132,7 +1165,7 @@ static int esirisc_reset_entry(struct target *target)
 	/* read exception table address */
 	retval = esirisc_jtag_read_csr(jtag_info, CSR_THREAD, CSR_THREAD_ETA, &eta);
 	if (retval != ERROR_OK) {
-		LOG_ERROR("%s: failed to read CSR: ETA", target_name(target));
+		LOG_ERROR("%s: failed to read Thread CSR: ETA", target_name(target));
 		return retval;
 	}
 
@@ -1147,7 +1180,7 @@ static int esirisc_reset_entry(struct target *target)
 	/* write reset entry point */
 	retval = esirisc_jtag_write_csr(jtag_info, CSR_THREAD, CSR_THREAD_EPC, epc);
 	if (retval != ERROR_OK) {
-		LOG_ERROR("%s: failed to write CSR: EPC", target_name(target));
+		LOG_ERROR("%s: failed to write Thread CSR: EPC", target_name(target));
 		return retval;
 	}
 
@@ -1215,15 +1248,9 @@ static int esirisc_arch_state(struct target *target)
 	uint32_t eid = buf_get_u32(esirisc->eid->value, 0, esirisc->eid->size);
 	uint32_t ed = buf_get_u32(esirisc->ed->value, 0, esirisc->ed->size);
 
-	LOG_DEBUG("-");
-
-	const char *exception = "Unknown";
-	if (eid < ARRAY_SIZE(esirisc_exceptions))
-		exception = esirisc_exceptions[eid];
-
 	LOG_USER("target halted due to %s, exception: %s\n"
-			"EPC: 0x%" PRIx32 " ECAS: 0x%" PRIx32 " EID: 0x%" PRIx32 " ED: 0x%" PRIx32,
-			debug_reason_name(target), exception, epc, ecas, eid, ed);
+			"EPC: 0x%" PRIx32 ", ECAS: 0x%" PRIx32 ", EID: 0x%" PRIx32 ", ED: 0x%" PRIx32,
+			debug_reason_name(target), esirisc_exception_strings[eid], epc, ecas, eid, ed);
 
 	return ERROR_OK;
 }
@@ -1242,7 +1269,7 @@ static const char *esirisc_get_gdb_arch(struct target *target)
 	 */
 	if (esirisc->gdb_arch == NULL && target_was_examined(target))
 		esirisc->gdb_arch = alloc_printf("esirisc:%d_bit_%d_reg_%s",
-				esirisc->num_bits, esirisc->num_regs, esirisc_cache_arch(esirisc));
+				esirisc->num_bits, esirisc->num_regs, esirisc_cache_arch_name(esirisc));
 
 	return esirisc->gdb_arch;
 }
@@ -1477,7 +1504,7 @@ static int esirisc_identify(struct target *target)
 
 	retval = esirisc_jtag_read_csr(jtag_info, CSR_CONFIG, CSR_CONFIG_ARCH0, &csr);
 	if (retval != ERROR_OK) {
-		LOG_ERROR("%s: failed to read CSR: ARCH0", target_name(target));
+		LOG_ERROR("%s: failed to read Configuration CSR: ARCH0", target_name(target));
 		return retval;
 	}
 
@@ -1486,7 +1513,7 @@ static int esirisc_identify(struct target *target)
 
 	retval = esirisc_jtag_read_csr(jtag_info, CSR_CONFIG, CSR_CONFIG_MEM, &csr);
 	if (retval != ERROR_OK) {
-		LOG_ERROR("%s: failed to read CSR: MEM", target_name(target));
+		LOG_ERROR("%s: failed to read Configuration CSR: MEM", target_name(target));
 		return retval;
 	}
 
@@ -1495,7 +1522,7 @@ static int esirisc_identify(struct target *target)
 
 	retval = esirisc_jtag_read_csr(jtag_info, CSR_CONFIG, CSR_CONFIG_IC, &csr);
 	if (retval != ERROR_OK) {
-		LOG_ERROR("%s: failed to read CSR: IC", target_name(target));
+		LOG_ERROR("%s: failed to read Configuration CSR: IC", target_name(target));
 		return retval;
 	}
 
@@ -1503,7 +1530,7 @@ static int esirisc_identify(struct target *target)
 
 	retval = esirisc_jtag_read_csr(jtag_info, CSR_CONFIG, CSR_CONFIG_DC, &csr);
 	if (retval != ERROR_OK) {
-		LOG_ERROR("%s: failed to read CSR: DC", target_name(target));
+		LOG_ERROR("%s: failed to read Configuration CSR: DC", target_name(target));
 		return retval;
 	}
 
@@ -1511,13 +1538,21 @@ static int esirisc_identify(struct target *target)
 
 	retval = esirisc_jtag_read_csr(jtag_info, CSR_CONFIG, CSR_CONFIG_DBG, &csr);
 	if (retval != ERROR_OK) {
-		LOG_ERROR("%s: failed to read CSR: DBG", target_name(target));
+		LOG_ERROR("%s: failed to read Configuration CSR: DBG", target_name(target));
 		return retval;
 	}
 
 	esirisc->num_breakpoints = (csr >> 7) & 0xf;	/* DBG.BP */
 	esirisc->num_watchpoints = (csr >> 12) & 0xf;	/* DBG.WP */
 
+	retval = esirisc_jtag_read_csr(jtag_info, CSR_CONFIG, CSR_CONFIG_TRACE, &csr);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("%s: failed to read Configuration CSR: TRACE", target_name(target));
+		return retval;
+	}
+
+	esirisc->has_trace = !!(csr & 1<<0);			/* TRACE.T */
+
 	return ERROR_OK;
 }
 
@@ -1616,13 +1651,14 @@ static int esirisc_examine(struct target *target)
 		target_set_examined(target);
 
 		LOG_INFO("%s: %d bit, %d registers, %s%s%s", target_name(target),
-				 esirisc->num_bits, esirisc->num_regs,
-				 target_endianness(target),
-				 esirisc->has_icache ? ", icache" : "",
-				 esirisc->has_dcache ? ", dcache" : "");
+				esirisc->num_bits, esirisc->num_regs,
+				target_endianness(target),
+				esirisc->has_icache ? ", icache" : "",
+				esirisc->has_dcache ? ", dcache" : "");
 
-		LOG_INFO("%s: hardware has %d breakpoints, %d watchpoints", target_name(target),
-				 esirisc->num_breakpoints, esirisc->num_watchpoints);
+		LOG_INFO("%s: hardware has %d breakpoints, %d watchpoints%s", target_name(target),
+				esirisc->num_breakpoints, esirisc->num_watchpoints,
+				esirisc->has_trace ? ", trace" : "");
 	}
 
 	return ERROR_OK;
@@ -1644,7 +1680,7 @@ COMMAND_HANDLER(handle_esirisc_cache_arch_command)
 		}
 	}
 
-	command_print(CMD_CTX, "esirisc cache_arch %s", esirisc_cache_arch(esirisc));
+	command_print(CMD_CTX, "esirisc cache_arch %s", esirisc_cache_arch_name(esirisc));
 
 	return ERROR_OK;
 }
@@ -1720,19 +1756,23 @@ COMMAND_HANDLER(handle_esirisc_hwdc_command)
 
 static const struct command_registration esirisc_exec_command_handlers[] = {
 	{
-		.name = "cache_arch",
-		.handler = handle_esirisc_cache_arch_command,
-		.mode = COMMAND_ANY,
-		.help = "configure cache architecture",
-		.usage = "['harvard'|'von_neumann']",
-	},
-	{
 		.name = "flush_caches",
 		.handler = handle_esirisc_flush_caches_command,
 		.mode = COMMAND_EXEC,
 		.help = "flush instruction and data caches",
 		.usage = "",
 	},
+	COMMAND_REGISTRATION_DONE
+};
+
+static const struct command_registration esirisc_any_command_handlers[] = {
+	{
+		.name = "cache_arch",
+		.handler = handle_esirisc_cache_arch_command,
+		.mode = COMMAND_ANY,
+		.help = "configure cache architecture",
+		.usage = "['harvard'|'von_neumann']",
+	},
 	{
 		.name = "hwdc",
 		.handler = handle_esirisc_hwdc_command,
@@ -1740,6 +1780,12 @@ static const struct command_registration esirisc_exec_command_handlers[] = {
 		.help = "configure hardware debug control",
 		.usage = "['all'|'none'|mask ...]",
 	},
+	{
+		.chain = esirisc_exec_command_handlers
+	},
+	{
+		.chain = esirisc_trace_command_handlers
+	},
 	COMMAND_REGISTRATION_DONE
 };
 
@@ -1749,7 +1795,7 @@ static const struct command_registration esirisc_command_handlers[] = {
 		.mode = COMMAND_ANY,
 		.help = "eSi-RISC command group",
 		.usage = "",
-		.chain = esirisc_exec_command_handlers,
+		.chain = esirisc_any_command_handlers,
 	},
 	COMMAND_REGISTRATION_DONE
 };
diff --git a/src/target/esirisc.h b/src/target/esirisc.h
index bb50652..57deba6 100644
--- a/src/target/esirisc.h
+++ b/src/target/esirisc.h
@@ -20,12 +20,14 @@
 #ifndef OPENOCD_TARGET_ESIRISC_H
 #define OPENOCD_TARGET_ESIRISC_H
 
+#include <helper/types.h>
 #include <target/breakpoints.h>
 #include <target/register.h>
 #include <target/target.h>
 
 #include "esirisc_jtag.h"
 #include "esirisc_regs.h"
+#include "esirisc_trace.h"
 
 #define MAX_BREAKPOINTS			8
 #define MAX_WATCHPOINTS			8
@@ -88,11 +90,15 @@ struct esirisc_common {
 	int num_regs;
 	bool has_icache;
 	bool has_dcache;
-	int num_breakpoints;
-	int num_watchpoints;
+	bool has_trace;
 
+	int num_breakpoints;
 	struct breakpoint *breakpoints_p[MAX_BREAKPOINTS];
+
+	int num_watchpoints;
 	struct watchpoint *watchpoints_p[MAX_WATCHPOINTS];
+
+	struct esirisc_trace trace_info;
 };
 
 union esirisc_memory {
@@ -116,7 +122,7 @@ static inline struct esirisc_common *target_to_esirisc(struct target *target)
 	return (struct esirisc_common *)target->arch_info;
 }
 
-static inline char *esirisc_cache_arch(struct esirisc_common *esirisc)
+static inline char *esirisc_cache_arch_name(struct esirisc_common *esirisc)
 {
 	return esirisc->cache_arch == ESIRISC_CACHE_HARVARD ? "harvard" : "von_neumann";
 }
diff --git a/src/target/esirisc_jtag.c b/src/target/esirisc_jtag.c
index 8ab47fa..333a622 100644
--- a/src/target/esirisc_jtag.c
+++ b/src/target/esirisc_jtag.c
@@ -265,6 +265,7 @@ int esirisc_jtag_read_byte(struct esirisc_jtag *jtag_info, uint32_t address, uin
 		return retval;
 
 	*data = *d;
+	LOG_DEBUG("address: 0x%" PRIx32 ", data: 0x%" PRIx8, address, *data);
 
 	return ERROR_OK;
 }
@@ -292,6 +293,7 @@ int esirisc_jtag_read_hword(struct esirisc_jtag *jtag_info, uint32_t address, ui
 		return retval;
 
 	*data = le_to_h_u16(d);
+	LOG_DEBUG("address: 0x%" PRIx32 ", data: 0x%" PRIx16, address, *data);
 
 	return ERROR_OK;
 }
@@ -319,6 +321,7 @@ int esirisc_jtag_read_word(struct esirisc_jtag *jtag_info, uint32_t address, uin
 		return retval;
 
 	*data = le_to_h_u32(d);
+	LOG_DEBUG("address: 0x%" PRIx32 ", data: 0x%" PRIx32, address, *data);
 
 	return ERROR_OK;
 }
@@ -328,6 +331,8 @@ int esirisc_jtag_write_byte(struct esirisc_jtag *jtag_info, uint32_t address, ui
 	struct scan_field out_fields[2];
 	uint8_t a[4];
 
+	LOG_DEBUG("address: 0x%" PRIx32 ", data: 0x%" PRIx8, address, data);
+
 	out_fields[0].num_bits = 32;
 	out_fields[0].out_value = a;
 	h_u32_to_be(a, address);
@@ -346,6 +351,8 @@ int esirisc_jtag_write_hword(struct esirisc_jtag *jtag_info, uint32_t address, u
 	struct scan_field out_fields[2];
 	uint8_t a[4], d[2];
 
+	LOG_DEBUG("address: 0x%" PRIx32 ", data: 0x%" PRIx16, address, data);
+
 	out_fields[0].num_bits = 32;
 	out_fields[0].out_value = a;
 	h_u32_to_be(a, address);
@@ -365,6 +372,8 @@ int esirisc_jtag_write_word(struct esirisc_jtag *jtag_info, uint32_t address, ui
 	struct scan_field out_fields[2];
 	uint8_t a[4], d[4];
 
+	LOG_DEBUG("address: 0x%" PRIx32 ", data: 0x%" PRIx32, address, data);
+
 	out_fields[0].num_bits = 32;
 	out_fields[0].out_value = a;
 	h_u32_to_be(a, address);
@@ -400,6 +409,7 @@ int esirisc_jtag_read_reg(struct esirisc_jtag *jtag_info, uint8_t reg, uint32_t
 		return retval;
 
 	*data = le_to_h_u32(d);
+	LOG_DEBUG("register: 0x%" PRIx32 ", data: 0x%" PRIx32, reg, *data);
 
 	return ERROR_OK;
 }
@@ -409,6 +419,8 @@ int esirisc_jtag_write_reg(struct esirisc_jtag *jtag_info, uint8_t reg, uint32_t
 	struct scan_field out_fields[2];
 	uint8_t d[4];
 
+	LOG_DEBUG("register: 0x%" PRIx32 ", data: 0x%" PRIx32, reg, data);
+
 	out_fields[0].num_bits = 8;
 	out_fields[0].out_value = &reg;
 	out_fields[0].in_value = NULL;
@@ -445,6 +457,7 @@ int esirisc_jtag_read_csr(struct esirisc_jtag *jtag_info, uint8_t bank, uint8_t
 		return retval;
 
 	*data = le_to_h_u32(d);
+	LOG_DEBUG("bank: 0x%" PRIx32 ", csr: 0x%" PRIx32 ", data: 0x%" PRIx32, bank, csr, *data);
 
 	return ERROR_OK;
 }
@@ -454,6 +467,8 @@ int esirisc_jtag_write_csr(struct esirisc_jtag *jtag_info, uint8_t bank, uint8_t
 	struct scan_field out_fields[2];
 	uint8_t c[2], d[4];
 
+	LOG_DEBUG("bank: 0x%" PRIx32 ", csr: 0x%" PRIx32 ", data: 0x%" PRIx32, bank, csr, data);
+
 	out_fields[0].num_bits = 16;
 	out_fields[0].out_value = c;
 	h_u16_to_be(c, (csr << 5) | bank);
diff --git a/src/target/esirisc_jtag.h b/src/target/esirisc_jtag.h
index 8189ddc..5f8fe66 100644
--- a/src/target/esirisc_jtag.h
+++ b/src/target/esirisc_jtag.h
@@ -20,6 +20,7 @@
 #ifndef OPENOCD_TARGET_ESIRISC_JTAG_H
 #define OPENOCD_TARGET_ESIRISC_JTAG_H
 
+#include <helper/types.h>
 #include <jtag/jtag.h>
 
 /* TAP Instructions */
diff --git a/src/target/esirisc_trace.c b/src/target/esirisc_trace.c
new file mode 100644
index 0000000..4e0a155
--- /dev/null
+++ b/src/target/esirisc_trace.c
@@ -0,0 +1,1203 @@
+/***************************************************************************
+ *   Copyright (C) 2018 by Square, Inc.                                    *
+ *   Steven Stallion <stallion@squareup.com>                               *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <helper/binarybuffer.h>
+#include <helper/command.h>
+#include <helper/fileio.h>
+#include <helper/log.h>
+#include <helper/types.h>
+#include <target/target.h>
+
+#include "esirisc.h"
+
+#define BIT_MASK(x)			((1 << (x)) - 1)
+
+/* Control Fields */
+#define CONTROL_ST			(1<<0)					/* Start */
+#define CONTROL_SP			(1<<1)					/* Stop */
+#define CONTROL_W			(1<<2)					/* Wrap */
+#define CONTROL_FC			(1<<3)					/* Flow Control */
+#define CONTROL_FMT(x)		(((x) <<  4) & 0x30)	/* Format */
+#define CONTROL_PCB(x)		(((x) << 10) & 0x7c00)	/* PC Bits */
+
+/* Status Fields */
+#define STATUS_T			(1<<0)	/* Trace Started */
+#define STATUS_TD			(1<<1)	/* Trace Disabled */
+#define	STATUS_W			(1<<2)	/* Wrapped */
+#define STATUS_O			(1<<3)	/* Overflow */
+
+/* Trigger Fields */
+#define TRIGGER_TST(x)		(((x) << 0) & 0xf)		/* Trigger Start */
+#define TRIGGER_DST			(1<<7)					/* Delay Start */
+#define TRIGGER_TSP(x)		(((x) << 8) & 0xf00)	/* Trigger Stop */
+#define TRIGGER_DSP			(1<<15)					/* Delay Start */
+
+static const char * const esirisc_trace_delay_strings[] = {
+	"none", "start", "stop", "both",
+};
+
+static const char * const esirisc_trace_format_strings[] = {
+	"full", "branch", "icache",
+};
+
+static const char * const esirisc_trace_id_strings[] = {
+	"sequential instruction",
+	"pipeline stall",
+	"direct branch",
+	"extended ID",
+};
+
+static const char * const esirisc_trace_ext_id_strings[] = {
+	"",	/* unused */
+	"exception",
+	"eret",
+	"stop instruction",
+	"wait instruction",
+	"multicycle instruction",
+	"count",
+	"initial",
+	"indirect branch",
+	"end of trace",
+	"final",
+};
+
+static const char * const esirisc_trace_trigger_strings[] = {
+	"none", "pc", "load", "store", "exception", "eret", "wait", "stop",
+	"high", "low",	/* start only */
+};
+
+static int esirisc_trace_clear_status(struct target *target)
+{
+	struct esirisc_common *esirisc = target_to_esirisc(target);
+	struct esirisc_jtag *jtag_info = &esirisc->jtag_info;
+	int retval;
+
+	if (target->state != TARGET_HALTED)
+		return ERROR_TARGET_NOT_HALTED;
+
+	retval = esirisc_jtag_write_csr(jtag_info, CSR_TRACE, CSR_TRACE_STATUS, ~0);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("%s: failed to write Trace CSR: Status", target_name(target));
+		return retval;
+	}
+
+	return ERROR_OK;
+}
+
+static int esirisc_trace_get_status(struct target *target, uint32_t *status)
+{
+	struct esirisc_common *esirisc = target_to_esirisc(target);
+	struct esirisc_jtag *jtag_info = &esirisc->jtag_info;
+
+	if (target->state != TARGET_HALTED)
+		return ERROR_TARGET_NOT_HALTED;
+
+	int retval = esirisc_jtag_read_csr(jtag_info, CSR_TRACE, CSR_TRACE_STATUS, status);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("%s: failed to read Trace CSR: Status", target_name(target));
+		return retval;
+	}
+
+	return ERROR_OK;
+}
+
+static int esirisc_trace_start(struct target *target)
+{
+	struct esirisc_common *esirisc = target_to_esirisc(target);
+	struct esirisc_jtag *jtag_info = &esirisc->jtag_info;
+	uint32_t control;
+	int retval;
+
+	if (target->state != TARGET_HALTED)
+		return ERROR_TARGET_NOT_HALTED;
+
+	retval = esirisc_jtag_read_csr(jtag_info, CSR_TRACE, CSR_TRACE_CONTROL, &control);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("%s: failed to read Trace CSR: Control", target_name(target));
+		return retval;
+	}
+
+	control |= CONTROL_ST;
+
+	retval = esirisc_jtag_write_csr(jtag_info, CSR_TRACE, CSR_TRACE_CONTROL, control);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("%s: failed to write Trace CSR: Control", target_name(target));
+		return retval;
+	}
+
+	return ERROR_OK;
+}
+
+static int esirisc_trace_stop(struct target *target)
+{
+	struct esirisc_common *esirisc = target_to_esirisc(target);
+	struct esirisc_jtag *jtag_info = &esirisc->jtag_info;
+	uint32_t control;
+	int retval;
+
+	if (target->state != TARGET_HALTED)
+		return ERROR_TARGET_NOT_HALTED;
+
+	retval = esirisc_jtag_read_csr(jtag_info, CSR_TRACE, CSR_TRACE_CONTROL, &control);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("%s: failed to read Trace CSR: Control", target_name(target));
+		return retval;
+	}
+
+	control |= CONTROL_SP;
+
+	retval = esirisc_jtag_write_csr(jtag_info, CSR_TRACE, CSR_TRACE_CONTROL, control);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("%s: failed to write Trace CSR: Control", target_name(target));
+		return retval;
+	}
+
+	return ERROR_OK;
+}
+
+static int esirisc_trace_init(struct target *target)
+{
+	struct esirisc_common *esirisc = target_to_esirisc(target);
+	struct esirisc_jtag *jtag_info = &esirisc->jtag_info;
+	struct esirisc_trace *trace_info = &esirisc->trace_info;
+	uint32_t control, trigger;
+	int retval;
+
+	if (target->state != TARGET_HALTED)
+		return ERROR_TARGET_NOT_HALTED;
+
+	/* stop if running and clear status */
+	retval = esirisc_trace_stop(target);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = esirisc_trace_clear_status(target);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* initialize Control CSR */
+	control = CONTROL_FMT(trace_info->format)
+			| CONTROL_PCB(trace_info->pc_bits);
+
+	if (trace_info->buffer_wrap)
+		control |= CONTROL_W;
+
+	if (trace_info->flow_control)
+		control |= CONTROL_FC;
+
+	retval = esirisc_jtag_write_csr(jtag_info, CSR_TRACE, CSR_TRACE_CONTROL, control);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("%s: failed to write Trace CSR: Control", target_name(target));
+		return retval;
+	}
+
+	/* initialize buffer CSRs */
+	retval = esirisc_jtag_write_csr(jtag_info, CSR_TRACE, CSR_TRACE_BUFFER_START,
+			trace_info->buffer_start);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("%s: failed to write Trace CSR: BufferStart", target_name(target));
+		return retval;
+	}
+
+	retval = esirisc_jtag_write_csr(jtag_info, CSR_TRACE, CSR_TRACE_BUFFER_END,
+			trace_info->buffer_end);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("%s: failed to write Trace CSR: BufferEnd", target_name(target));
+		return retval;
+	}
+
+	/*
+	 * The BufferCurrent CSR must be initialized to the same value as
+	 * BufferStart before tracing can be enabled:
+	 */
+	retval = esirisc_jtag_write_csr(jtag_info, CSR_TRACE, CSR_TRACE_BUFFER_CUR,
+			trace_info->buffer_start);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("%s: failed to write Trace CSR: BufferCurrent", target_name(target));
+		return retval;
+	}
+
+	/* initialize Trigger CSR */
+	trigger = TRIGGER_TST(trace_info->start_trigger)
+			| TRIGGER_TSP(trace_info->stop_trigger);
+
+	if (trace_info->delay == ESIRISC_TRACE_DELAY_START
+		|| trace_info->delay == ESIRISC_TRACE_DELAY_BOTH) {
+		trigger |= TRIGGER_DST;
+	}
+
+	if (trace_info->delay == ESIRISC_TRACE_DELAY_STOP
+		|| trace_info->delay == ESIRISC_TRACE_DELAY_BOTH) {
+		trigger |= TRIGGER_DSP;
+	}
+
+	retval = esirisc_jtag_write_csr(jtag_info, CSR_TRACE, CSR_TRACE_TRIGGER, trigger);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("%s: failed to write Trace CSR: Trigger", target_name(target));
+		return retval;
+	}
+
+	/* initialize StartData/StartMask CSRs */
+	retval = esirisc_jtag_write_csr(jtag_info, CSR_TRACE, CSR_TRACE_START_DATA,
+			trace_info->start_data);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("%s: failed to write Trace CSR: StartData", target_name(target));
+		return retval;
+	}
+
+	retval = esirisc_jtag_write_csr(jtag_info, CSR_TRACE, CSR_TRACE_START_MASK,
+			trace_info->start_mask);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("%s: failed to write Trace CSR: StartMask", target_name(target));
+		return retval;
+	}
+
+	/* initialize StopData/StopMask CSRs */
+	retval = esirisc_jtag_write_csr(jtag_info, CSR_TRACE, CSR_TRACE_STOP_DATA,
+			trace_info->stop_data);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("%s: failed to write Trace CSR: StopData", target_name(target));
+		return retval;
+	}
+
+	retval = esirisc_jtag_write_csr(jtag_info, CSR_TRACE, CSR_TRACE_STOP_MASK,
+			trace_info->stop_mask);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("%s: failed to write Trace CSR: StopMask", target_name(target));
+		return retval;
+	}
+
+	/* initialize Delay CSR */
+	retval = esirisc_jtag_write_csr(jtag_info, CSR_TRACE, CSR_TRACE_DELAY,
+			trace_info->delay_cycles);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("%s: failed to write Trace CSR: Delay", target_name(target));
+		return retval;
+	}
+
+	return ERROR_OK;
+}
+
+static int esirisc_trace_buf_get_u32(uint8_t *buffer, uint32_t size,
+		unsigned *pos, unsigned count, uint32_t *value)
+{
+	const unsigned num_bits = size * 8;
+
+	if (*pos+count > num_bits)
+		return ERROR_FAIL;
+
+	*value = buf_get_u32(buffer, *pos, count);
+	*pos += count;
+
+	return ERROR_OK;
+}
+
+static int esirisc_trace_buf_get_pc(struct target *target, uint8_t *buffer, uint32_t size,
+		unsigned *pos, uint32_t *value)
+{
+	struct esirisc_common *esirisc = target_to_esirisc(target);
+	struct esirisc_trace *trace_info = &esirisc->trace_info;
+	int retval;
+
+	retval = esirisc_trace_buf_get_u32(buffer, size, pos, trace_info->pc_bits, value);
+	if (retval != ERROR_OK)
+		return retval;
+
+	*value <<= esirisc->num_bits - trace_info->pc_bits;
+
+	return ERROR_OK;
+}
+
+static int esirisc_trace_read_memory(struct target *target, target_addr_t address, uint32_t size,
+		uint8_t *buffer)
+{
+	int retval;
+
+	if (target->state != TARGET_HALTED)
+		return ERROR_TARGET_NOT_HALTED;
+
+	retval = target_read_memory(target, address, 1, size, buffer);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("%s: failed to read trace data", target_name(target));
+		return retval;
+	}
+
+	return ERROR_OK;
+}
+
+static int esirisc_trace_read_buffer(struct target *target, uint8_t *buffer)
+{
+	struct esirisc_common *esirisc = target_to_esirisc(target);
+	struct esirisc_jtag *jtag_info = &esirisc->jtag_info;
+	struct esirisc_trace *trace_info = &esirisc->trace_info;
+	uint32_t buffer_cur, status;
+	int retval;
+
+	if (target->state != TARGET_HALTED)
+		return ERROR_TARGET_NOT_HALTED;
+
+	retval = esirisc_jtag_read_csr(jtag_info, CSR_TRACE, CSR_TRACE_BUFFER_CUR, &buffer_cur);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("%s: failed to read Trace CSR: BufferCurrent", target_name(target));
+		return retval;
+	}
+
+	/*
+	 * If the buffer has wrapped, the BufferCurrent CSR indicates the
+	 * next address to be written (ie. the start address). These bytes
+	 * must be dumped first to maintain coherency when analyzing
+	 * captured data.
+	 */
+	retval = esirisc_trace_get_status(target, &status);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (status & STATUS_W) {
+		uint32_t size = trace_info->buffer_end - buffer_cur;
+
+		retval = esirisc_trace_read_memory(target, buffer_cur, size, buffer);
+		if (retval != ERROR_OK)
+			return retval;
+
+		buffer += size;
+	}
+
+	return esirisc_trace_read_memory(target, trace_info->buffer_start,
+			buffer_cur - trace_info->buffer_start, buffer);
+}
+
+static int esirisc_trace_analyze_full(struct command_context *cmd_ctx, uint8_t *buffer, uint32_t size)
+{
+	struct target *target = get_current_target(cmd_ctx);
+	const uint32_t num_bits = size * 8;
+	int retval;
+
+	unsigned pos = 0;
+	while (pos < num_bits) {
+		uint32_t id;
+
+		retval = esirisc_trace_buf_get_u32(buffer, size, &pos, 2, &id);
+		if (retval != ERROR_OK)
+			goto fail;
+
+		switch (id) {
+			case ESIRISC_TRACE_ID_EXECUTE:
+			case ESIRISC_TRACE_ID_STALL:
+			case ESIRISC_TRACE_ID_BRANCH:
+				command_print(cmd_ctx, "%s", esirisc_trace_id_strings[id]);
+				break;
+
+			case ESIRISC_TRACE_ID_EXTENDED: {
+				uint32_t ext_id;
+
+				retval = esirisc_trace_buf_get_u32(buffer, size, &pos, 4, &ext_id);
+				if (retval != ERROR_OK)
+					goto fail;
+
+				switch (ext_id) {
+					case ESIRISC_TRACE_EXT_ID_STOP:
+					case ESIRISC_TRACE_EXT_ID_WAIT:
+					case ESIRISC_TRACE_EXT_ID_MULTICYCLE:
+						command_print(cmd_ctx, "%s", esirisc_trace_ext_id_strings[ext_id]);
+						break;
+
+					case ESIRISC_TRACE_EXT_ID_ERET:
+					case ESIRISC_TRACE_EXT_ID_PC:
+					case ESIRISC_TRACE_EXT_ID_INDIRECT:
+					case ESIRISC_TRACE_EXT_ID_END_PC: {
+						uint32_t pc;
+
+						retval = esirisc_trace_buf_get_pc(target, buffer, size, &pos, &pc);
+						if (retval != ERROR_OK)
+							goto fail;
+
+						command_print(cmd_ctx, "%s PC: 0x%" PRIx32,
+								esirisc_trace_ext_id_strings[ext_id], pc);
+
+						if (ext_id == ESIRISC_TRACE_EXT_ID_END_PC) {
+							command_print(cmd_ctx, "--- end of trace ---");
+							return ERROR_OK;
+						}
+						break;
+					}
+					case ESIRISC_TRACE_EXT_ID_EXCEPTION: {
+						uint32_t eid, epc;
+
+						retval = esirisc_trace_buf_get_u32(buffer, size, &pos, 6, &eid);
+						if (retval != ERROR_OK)
+							goto fail;
+
+						retval = esirisc_trace_buf_get_pc(target, buffer, size, &pos, &epc);
+						if (retval != ERROR_OK)
+							goto fail;
+
+						command_print(cmd_ctx, "%s EID: 0x%" PRIx32 ", EPC: 0x%" PRIx32,
+								esirisc_trace_ext_id_strings[ext_id], eid, epc);
+						break;
+					}
+					case ESIRISC_TRACE_EXT_ID_COUNT: {
+						uint32_t count;
+
+						retval = esirisc_trace_buf_get_u32(buffer, size, &pos, 6, &count);
+						if (retval != ERROR_OK)
+							goto fail;
+
+						command_print(cmd_ctx, "repeats %" PRId32 " %s", count,
+								(count == 1) ? "time" : "times");
+						break;
+					}
+					case ESIRISC_TRACE_EXT_ID_END:
+						command_print(cmd_ctx, "--- end of trace ---");
+						return ERROR_OK;
+
+					default:
+						command_print(cmd_ctx, "invalid extended trace ID: %" PRId32, ext_id);
+						return ERROR_FAIL;
+				}
+				break;
+			}
+			default:
+				command_print(cmd_ctx, "invalid trace ID: %" PRId32, id);
+				return ERROR_FAIL;
+		}
+	}
+
+fail:
+	command_print(cmd_ctx, "trace buffer too small");
+	return ERROR_BUF_TOO_SMALL;
+}
+
+static int esirisc_trace_analyze_simple(struct command_context *cmd_ctx, uint8_t *buffer, uint32_t size)
+{
+	struct target *target = get_current_target(cmd_ctx);
+	struct esirisc_common *esirisc = target_to_esirisc(target);
+	struct esirisc_trace *trace_info = &esirisc->trace_info;
+	const uint32_t end_of_trace = BIT_MASK(trace_info->pc_bits) << 1;
+	const uint32_t num_bits = size * 8;
+	int retval;
+
+	unsigned pos = 0;
+	while (pos < num_bits) {
+		uint32_t pc;
+
+		retval = esirisc_trace_buf_get_pc(target, buffer, size, &pos, &pc);
+		if (retval != ERROR_OK)
+			break;
+
+		if (pc == end_of_trace) {
+			command_print(cmd_ctx, "--- end of trace ---");
+			return ERROR_OK;
+		}
+
+		command_print(cmd_ctx, "PC: 0x%" PRIx32, pc);
+	}
+
+	command_print(cmd_ctx, "trace buffer too small");
+	return ERROR_BUF_TOO_SMALL;
+}
+
+static int esirisc_trace_analyze(struct command_context *cmd_ctx, uint8_t *buffer, uint32_t size)
+{
+	struct target *target = get_current_target(cmd_ctx);
+	struct esirisc_common *esirisc = target_to_esirisc(target);
+	struct esirisc_trace *trace_info = &esirisc->trace_info;
+
+	switch (trace_info->format) {
+		case ESIRISC_TRACE_FORMAT_FULL:
+			command_print(cmd_ctx, "--- full pipeline ---");
+			return esirisc_trace_analyze_full(cmd_ctx, buffer, size);
+
+		case ESIRISC_TRACE_FORMAT_BRANCH:
+			command_print(cmd_ctx, "--- branches taken ---");
+			return esirisc_trace_analyze_full(cmd_ctx, buffer, size);
+
+		case ESIRISC_TRACE_FORMAT_ICACHE:
+			command_print(cmd_ctx, "--- icache misses ---");
+			return esirisc_trace_analyze_simple(cmd_ctx, buffer, size);
+
+		default:
+			command_print(cmd_ctx, "invalid trace format: %i", trace_info->format);
+			return ERROR_FAIL;
+	}
+}
+
+static int esirisc_trace_analyze_buffer(struct command_context *cmd_ctx)
+{
+	struct target *target = get_current_target(cmd_ctx);
+	struct esirisc_common *esirisc = target_to_esirisc(target);
+	struct esirisc_trace *trace_info = &esirisc->trace_info;
+	uint8_t *buffer;
+	uint32_t size;
+	int retval;
+
+	size = esirisc_trace_buffer_size(trace_info);
+	buffer = calloc(1, size);
+	if (buffer == NULL) {
+		command_print(cmd_ctx, "out of memory");
+		return ERROR_FAIL;
+	}
+
+	retval = esirisc_trace_read_buffer(target, buffer);
+	if (retval != ERROR_OK)
+		goto done;
+
+	retval = esirisc_trace_analyze(cmd_ctx, buffer, size);
+
+done:
+	free(buffer);
+
+	return retval;
+}
+
+static int esirisc_trace_analyze_memory(struct command_context *cmd_ctx,
+		target_addr_t address, uint32_t size)
+{
+	struct target *target = get_current_target(cmd_ctx);
+	uint8_t *buffer;
+	int retval;
+
+	buffer = calloc(1, size);
+	if (buffer == NULL) {
+		command_print(cmd_ctx, "out of memory");
+		return ERROR_FAIL;
+	}
+
+	retval = esirisc_trace_read_memory(target, address, size, buffer);
+	if (retval != ERROR_OK)
+		goto done;
+
+	retval = esirisc_trace_analyze(cmd_ctx, buffer, size);
+
+done:
+	free(buffer);
+
+	return retval;
+}
+
+static int esirisc_trace_dump(struct command_context *cmd_ctx, const char *filename,
+		uint8_t *buffer, uint32_t size)
+{
+	struct fileio *fileio;
+	size_t size_written;
+	int retval;
+
+	retval = fileio_open(&fileio, filename, FILEIO_WRITE, FILEIO_BINARY);
+	if (retval != ERROR_OK) {
+		command_print(cmd_ctx, "could not open dump file: %s", filename);
+		return retval;
+	}
+
+	retval = fileio_write(fileio, size, buffer, &size_written);
+	if (retval == ERROR_OK)
+		command_print(cmd_ctx, "trace data dumped to: %s", filename);
+	else
+		command_print(cmd_ctx, "could not write dump file: %s", filename);
+
+	fileio_close(fileio);
+
+	return retval;
+}
+
+static int esirisc_trace_dump_buffer(struct command_context *cmd_ctx, const char *filename)
+{
+	struct target *target = get_current_target(cmd_ctx);
+	struct esirisc_common *esirisc = target_to_esirisc(target);
+	struct esirisc_trace *trace_info = &esirisc->trace_info;
+	uint8_t *buffer;
+	uint32_t size;
+	int retval;
+
+	size = esirisc_trace_buffer_size(trace_info);
+	buffer = calloc(1, size);
+	if (buffer == NULL) {
+		command_print(cmd_ctx, "out of memory");
+		return ERROR_FAIL;
+	}
+
+	retval = esirisc_trace_read_buffer(target, buffer);
+	if (retval != ERROR_OK)
+		goto done;
+
+	retval = esirisc_trace_dump(cmd_ctx, filename, buffer, size);
+
+done:
+	free(buffer);
+
+	return retval;
+}
+
+static int esirisc_trace_dump_memory(struct command_context *cmd_ctx, const char *filename,
+		target_addr_t address, uint32_t size)
+{
+	struct target *target = get_current_target(cmd_ctx);
+	uint8_t *buffer;
+	int retval;
+
+	buffer = calloc(1, size);
+	if (buffer == NULL) {
+		command_print(cmd_ctx, "out of memory");
+		return ERROR_FAIL;
+	}
+
+	retval = esirisc_trace_read_memory(target, address, size, buffer);
+	if (retval != ERROR_OK)
+		goto done;
+
+	retval = esirisc_trace_dump(cmd_ctx, filename, buffer, size);
+
+done:
+	free(buffer);
+
+	return retval;
+}
+
+COMMAND_HANDLER(handle_esirisc_trace_init_command)
+{
+	struct target *target = get_current_target(CMD_CTX);
+	struct esirisc_common *esirisc = target_to_esirisc(target);
+
+	if (!esirisc->has_trace) {
+		command_print(CMD_CTX, "target does not support trace");
+		return ERROR_FAIL;
+	}
+
+	int retval = esirisc_trace_init(target);
+	if (retval == ERROR_OK)
+		command_print(CMD_CTX, "trace initialized");
+
+	return retval;
+}
+
+COMMAND_HANDLER(handle_esirisc_trace_info_command)
+{
+	struct target *target = get_current_target(CMD_CTX);
+	struct esirisc_common *esirisc = target_to_esirisc(target);
+	struct esirisc_trace *trace_info = &esirisc->trace_info;
+
+	if (!esirisc->has_trace) {
+		command_print(CMD_CTX, "target does not support trace");
+		return ERROR_FAIL;
+	}
+
+	if (esirisc_trace_is_fifo(trace_info))
+		command_print(CMD_CTX, "trace FIFO address: 0x%" TARGET_PRIxADDR,
+				trace_info->buffer_start);
+	else {
+		command_print(CMD_CTX, "trace buffer start: 0x%" TARGET_PRIxADDR,
+				trace_info->buffer_start);
+		command_print(CMD_CTX, "trace buffer end: 0x%" TARGET_PRIxADDR,
+				trace_info->buffer_end);
+		command_print(CMD_CTX, "trace buffer will %swrap",
+				trace_info->buffer_wrap ? "" : "not ");
+	}
+
+	command_print(CMD_CTX, "flow control: %s",
+			trace_info->flow_control ? "enabled" : "disabled");
+
+	command_print(CMD_CTX, "trace format: %s",
+			esirisc_trace_format_strings[trace_info->format]);
+	command_print(CMD_CTX, "number of PC bits: %i", trace_info->pc_bits);
+
+	command_print(CMD_CTX, "start trigger: %s",
+			esirisc_trace_trigger_strings[trace_info->start_trigger]);
+	command_print(CMD_CTX, "start data: 0x%" PRIx32, trace_info->start_data);
+	command_print(CMD_CTX, "start mask: 0x%" PRIx32, trace_info->start_mask);
+
+	command_print(CMD_CTX, "stop trigger: %s",
+			esirisc_trace_trigger_strings[trace_info->stop_trigger]);
+	command_print(CMD_CTX, "stop data: 0x%" PRIx32, trace_info->stop_data);
+	command_print(CMD_CTX, "stop mask: 0x%" PRIx32, trace_info->stop_mask);
+
+	command_print(CMD_CTX, "trigger delay: %s",
+			esirisc_trace_delay_strings[trace_info->delay]);
+	command_print(CMD_CTX, "trigger delay cycles: %i", trace_info->delay_cycles);
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(handle_esirisc_trace_status_command)
+{
+	struct target *target = get_current_target(CMD_CTX);
+	struct esirisc_common *esirisc = target_to_esirisc(target);
+	uint32_t status;
+
+	if (!esirisc->has_trace) {
+		command_print(CMD_CTX, "target does not support trace");
+		return ERROR_FAIL;
+	}
+
+	int retval = esirisc_trace_get_status(target, &status);
+	if (retval != ERROR_OK)
+		return retval;
+
+	command_print(CMD_CTX, "trace is %s%s%s%s",
+			(status & STATUS_T)  ? "started" : "stopped",
+			(status & STATUS_TD) ? ", disabled"   : "",
+			(status & STATUS_W)  ? ", wrapped"    : "",
+			(status & STATUS_O)  ? ", overflowed" : "");
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(handle_esirisc_trace_start_command)
+{
+	struct target *target = get_current_target(CMD_CTX);
+	struct esirisc_common *esirisc = target_to_esirisc(target);
+
+	if (!esirisc->has_trace) {
+		command_print(CMD_CTX, "target does not support trace");
+		return ERROR_FAIL;
+	}
+
+	int retval = esirisc_trace_start(target);
+	if (retval == ERROR_OK)
+		command_print(CMD_CTX, "trace started");
+
+	return retval;
+}
+
+COMMAND_HANDLER(handle_esirisc_trace_stop_command)
+{
+	struct target *target = get_current_target(CMD_CTX);
+	struct esirisc_common *esirisc = target_to_esirisc(target);
+
+	if (!esirisc->has_trace) {
+		command_print(CMD_CTX, "target does not support trace");
+		return ERROR_FAIL;
+	}
+
+	int retval = esirisc_trace_stop(target);
+	if (retval == ERROR_OK)
+		command_print(CMD_CTX, "trace stopped");
+
+	return retval;
+}
+
+COMMAND_HANDLER(handle_esirisc_trace_analyze_command)
+{
+	struct target *target = get_current_target(CMD_CTX);
+	struct esirisc_common *esirisc = target_to_esirisc(target);
+	struct esirisc_trace *trace_info = &esirisc->trace_info;
+	target_addr_t address;
+	uint32_t size;
+
+	if (!esirisc->has_trace) {
+		command_print(CMD_CTX, "target does not support trace");
+		return ERROR_FAIL;
+	}
+
+	if (CMD_ARGC != 0 && CMD_ARGC != 2)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	if (CMD_ARGC == 0) {
+		/*
+		 * Use of the Trace FIFO typically involves DMA to a peripheral
+		 * (eg. SPI) or a separately managed buffer in memory, neither
+		 * of which may be under our control. If the destination address
+		 * and size are known in the latter case, they may be specified
+		 * as arguments as a workaround.
+		 */
+		if (esirisc_trace_is_fifo(trace_info)) {
+			command_print(CMD_CTX, "analyze from FIFO not supported");
+			return ERROR_FAIL;
+		}
+
+		return esirisc_trace_analyze_buffer(CMD_CTX);
+	} else {
+		COMMAND_PARSE_ADDRESS(CMD_ARGV[0], address);
+		COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], size);
+
+		return esirisc_trace_analyze_memory(CMD_CTX, address, size);
+	}
+}
+
+COMMAND_HANDLER(handle_esirisc_trace_dump_command)
+{
+	struct target *target = get_current_target(CMD_CTX);
+	struct esirisc_common *esirisc = target_to_esirisc(target);
+	struct esirisc_trace *trace_info = &esirisc->trace_info;
+	target_addr_t address;
+	uint32_t size;
+
+	if (!esirisc->has_trace) {
+		command_print(CMD_CTX, "target does not support trace");
+		return ERROR_FAIL;
+	}
+
+	if (CMD_ARGC != 1 && CMD_ARGC != 3)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	if (CMD_ARGC == 1) {
+		/* also see: handle_esirisc_trace_analyze_command() */
+		if (esirisc_trace_is_fifo(trace_info)) {
+			command_print(CMD_CTX, "dump from FIFO not supported");
+			return ERROR_FAIL;
+		}
+
+		return esirisc_trace_dump_buffer(CMD_CTX, CMD_ARGV[0]);
+	} else {
+		COMMAND_PARSE_ADDRESS(CMD_ARGV[0], address);
+		COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], size);
+
+		return esirisc_trace_dump_memory(CMD_CTX, CMD_ARGV[2], address, size);
+	}
+}
+
+COMMAND_HANDLER(handle_esirisc_trace_buffer_command)
+{
+	struct target *target = get_current_target(CMD_CTX);
+	struct esirisc_common *esirisc = target_to_esirisc(target);
+	struct esirisc_trace *trace_info = &esirisc->trace_info;
+	uint32_t size;
+
+	if (CMD_ARGC < 2 || CMD_ARGC > 3)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	COMMAND_PARSE_ADDRESS(CMD_ARGV[0], trace_info->buffer_start);
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], size);
+
+	trace_info->buffer_end = trace_info->buffer_start + size;
+
+	if (CMD_ARGC == 3) {
+		if (strcmp("wrap", CMD_ARGV[2]) == 0)
+			trace_info->buffer_wrap = true;
+		else
+			return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(handle_esirisc_trace_fifo_command)
+{
+	struct target *target = get_current_target(CMD_CTX);
+	struct esirisc_common *esirisc = target_to_esirisc(target);
+	struct esirisc_trace *trace_info = &esirisc->trace_info;
+
+	if (CMD_ARGC != 1)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	COMMAND_PARSE_ADDRESS(CMD_ARGV[0], trace_info->buffer_start);
+
+	/* FIFOs have the same start and end address */
+	trace_info->buffer_end = trace_info->buffer_start;
+	trace_info->buffer_wrap = true;
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(handle_esirisc_trace_flow_control_command)
+{
+	struct target *target = get_current_target(CMD_CTX);
+	struct esirisc_common *esirisc = target_to_esirisc(target);
+	struct esirisc_trace *trace_info = &esirisc->trace_info;
+
+	if (CMD_ARGC != 1)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	if (strcmp(CMD_ARGV[0], "enable") == 0)
+		trace_info->flow_control = true;
+	else if (strcmp(CMD_ARGV[0], "disable") == 0)
+		trace_info->flow_control = false;
+	else
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(handle_esirisc_trace_format_command)
+{
+	struct target *target = get_current_target(CMD_CTX);
+	struct esirisc_common *esirisc = target_to_esirisc(target);
+	struct esirisc_trace *trace_info = &esirisc->trace_info;
+	int pc_bits;
+
+	if (CMD_ARGC != 2)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	if (strcmp(CMD_ARGV[0], "full") == 0)
+		trace_info->format = ESIRISC_TRACE_FORMAT_FULL;
+	else if (strcmp(CMD_ARGV[0], "branch") == 0)
+		trace_info->format = ESIRISC_TRACE_FORMAT_BRANCH;
+	else if (strcmp(CMD_ARGV[0], "icache") == 0)
+		trace_info->format = ESIRISC_TRACE_FORMAT_ICACHE;
+	else
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	COMMAND_PARSE_NUMBER(int, CMD_ARGV[1], pc_bits);
+
+	if (pc_bits < 1 || pc_bits > 31) {
+		command_print(CMD_CTX, "invalid pc_bits: %i; must be 1..31", pc_bits);
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	trace_info->pc_bits = pc_bits;
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(handle_esirisc_trace_trigger_start_command)
+{
+	struct target *target = get_current_target(CMD_CTX);
+	struct esirisc_common *esirisc = target_to_esirisc(target);
+	struct esirisc_trace *trace_info = &esirisc->trace_info;
+
+	if (CMD_ARGC != 1 && CMD_ARGC != 3)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	if (strcmp(CMD_ARGV[0], "none") == 0)
+		trace_info->start_trigger = ESIRISC_TRACE_TRIGGER_NONE;
+	else if (strcmp(CMD_ARGV[0], "pc") == 0)
+		trace_info->start_trigger = ESIRISC_TRACE_TRIGGER_PC;
+	else if (strcmp(CMD_ARGV[0], "load") == 0)
+		trace_info->start_trigger = ESIRISC_TRACE_TRIGGER_LOAD;
+	else if (strcmp(CMD_ARGV[0], "store") == 0)
+		trace_info->start_trigger = ESIRISC_TRACE_TRIGGER_STORE;
+	else if (strcmp(CMD_ARGV[0], "exception") == 0)
+		trace_info->start_trigger = ESIRISC_TRACE_TRIGGER_EXCEPTION;
+	else if (strcmp(CMD_ARGV[0], "eret") == 0)
+		trace_info->start_trigger = ESIRISC_TRACE_TRIGGER_ERET;
+	else if (strcmp(CMD_ARGV[0], "wait") == 0)
+		trace_info->start_trigger = ESIRISC_TRACE_TRIGGER_WAIT;
+	else if (strcmp(CMD_ARGV[0], "stop") == 0)
+		trace_info->start_trigger = ESIRISC_TRACE_TRIGGER_STOP;
+	else if (strcmp(CMD_ARGV[0], "high") == 0)
+		trace_info->start_trigger = ESIRISC_TRACE_TRIGGER_HIGH;
+	else if (strcmp(CMD_ARGV[0], "low") == 0)
+		trace_info->start_trigger = ESIRISC_TRACE_TRIGGER_LOW;
+	else
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	if (CMD_ARGC == 3) {
+		COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], trace_info->start_data);
+		COMMAND_PARSE_NUMBER(u32, CMD_ARGV[2], trace_info->start_mask);
+	} else {
+		trace_info->start_data = 0;
+		trace_info->start_mask = 0;
+	}
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(handle_esirisc_trace_trigger_stop_command)
+{
+	struct target *target = get_current_target(CMD_CTX);
+	struct esirisc_common *esirisc = target_to_esirisc(target);
+	struct esirisc_trace *trace_info = &esirisc->trace_info;
+
+	if (CMD_ARGC != 1 && CMD_ARGC != 3)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	if (strcmp(CMD_ARGV[0], "none") == 0)
+		trace_info->stop_trigger = ESIRISC_TRACE_TRIGGER_NONE;
+	else if (strcmp(CMD_ARGV[0], "pc") == 0)
+		trace_info->stop_trigger = ESIRISC_TRACE_TRIGGER_PC;
+	else if (strcmp(CMD_ARGV[0], "load") == 0)
+		trace_info->stop_trigger = ESIRISC_TRACE_TRIGGER_LOAD;
+	else if (strcmp(CMD_ARGV[0], "store") == 0)
+		trace_info->stop_trigger = ESIRISC_TRACE_TRIGGER_STORE;
+	else if (strcmp(CMD_ARGV[0], "exception") == 0)
+		trace_info->stop_trigger = ESIRISC_TRACE_TRIGGER_EXCEPTION;
+	else if (strcmp(CMD_ARGV[0], "eret") == 0)
+		trace_info->stop_trigger = ESIRISC_TRACE_TRIGGER_ERET;
+	else if (strcmp(CMD_ARGV[0], "wait") == 0)
+		trace_info->stop_trigger = ESIRISC_TRACE_TRIGGER_WAIT;
+	else if (strcmp(CMD_ARGV[0], "stop") == 0)
+		trace_info->stop_trigger = ESIRISC_TRACE_TRIGGER_STOP;
+	else
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	if (CMD_ARGC == 3) {
+		COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], trace_info->stop_data);
+		COMMAND_PARSE_NUMBER(u32, CMD_ARGV[2], trace_info->stop_mask);
+	} else {
+		trace_info->stop_data = 0;
+		trace_info->stop_mask = 0;
+	}
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(handle_esirisc_trace_trigger_delay_command)
+{
+	struct target *target = get_current_target(CMD_CTX);
+	struct esirisc_common *esirisc = target_to_esirisc(target);
+	struct esirisc_trace *trace_info = &esirisc->trace_info;
+
+	if (CMD_ARGC < 1 || CMD_ARGC > 2)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	if (strcmp(CMD_ARGV[0], "none") == 0)
+		trace_info->delay = ESIRISC_TRACE_DELAY_NONE;
+	else if (strcmp(CMD_ARGV[0], "start") == 0)
+		trace_info->delay = ESIRISC_TRACE_DELAY_START;
+	else if (strcmp(CMD_ARGV[0], "stop") == 0)
+		trace_info->delay = ESIRISC_TRACE_DELAY_STOP;
+	else if (strcmp(CMD_ARGV[0], "both") == 0)
+		trace_info->delay = ESIRISC_TRACE_DELAY_BOTH;
+	else
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	if (trace_info->delay == ESIRISC_TRACE_DELAY_NONE)
+		trace_info->delay_cycles = 0;
+	else {
+		if (CMD_ARGC != 2)
+			return ERROR_COMMAND_SYNTAX_ERROR;
+
+		COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], trace_info->delay_cycles);
+	}
+
+	return ERROR_OK;
+}
+
+static const struct command_registration esirisc_trace_exec_command_handlers[] = {
+	{
+		.name = "init",
+		.handler = handle_esirisc_trace_init_command,
+		.mode = COMMAND_EXEC,
+		.help = "initialize trace collection",
+		.usage = "",
+	},
+	{
+		.name = "info",
+		.handler = handle_esirisc_trace_info_command,
+		.mode = COMMAND_EXEC,
+		.help = "display trace configuration",
+		.usage = "",
+	},
+	{
+		.name = "status",
+		.handler = handle_esirisc_trace_status_command,
+		.mode = COMMAND_EXEC,
+		.help = "display trace collection status",
+		.usage = "",
+	},
+	{
+		.name = "start",
+		.handler = handle_esirisc_trace_start_command,
+		.mode = COMMAND_EXEC,
+		.help = "start trace collection",
+		.usage = "",
+	},
+	{
+		.name = "stop",
+		.handler = handle_esirisc_trace_stop_command,
+		.mode = COMMAND_EXEC,
+		.help = "stop trace collection",
+		.usage = "",
+	},
+	{
+		.name = "analyze",
+		.handler = handle_esirisc_trace_analyze_command,
+		.mode = COMMAND_EXEC,
+		.usage = "[address size]",
+		.help = "analyze collected trace data",
+	},
+	{
+		.name = "dump",
+		.handler = handle_esirisc_trace_dump_command,
+		.mode = COMMAND_EXEC,
+		.help = "dump collected trace data to file",
+		.usage = "[address size] filename",
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+static const struct command_registration esirisc_trace_trigger_any_command_handlers[] = {
+	{
+		.name = "start",
+		.handler = handle_esirisc_trace_trigger_start_command,
+		.mode = COMMAND_ANY,
+		.help = "configure trigger start condition",
+		.usage = "('none'|'pc'|'load'|'store'|'exception'|'eret'|'wait'|'stop'|'high'|'low')"
+				 " [start_data start_mask]",
+	},
+	{
+		.name = "stop",
+		.handler = handle_esirisc_trace_trigger_stop_command,
+		.mode = COMMAND_ANY,
+		.help = "configure trigger stop condition",
+		.usage = "('none'|'pc'|'load'|'store'|'exception'|'eret'|'wait'|'stop')"
+				 " [stop_data stop_mask]",
+	},
+	{
+		.name = "delay",
+		.handler = handle_esirisc_trace_trigger_delay_command,
+		.mode = COMMAND_ANY,
+		.help = "configure trigger start/stop delay in clock cycles",
+		.usage = "('none'|'start'|'stop'|'both') [cycles]",
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+static const struct command_registration esirisc_trace_any_command_handlers[] = {
+	{
+		.name = "buffer",
+		.handler = handle_esirisc_trace_buffer_command,
+		.mode = COMMAND_ANY,
+		.help = "configure trace buffer",
+		.usage = "address size ['wrap']",
+	},
+	{
+		.name = "fifo",
+		.handler = handle_esirisc_trace_fifo_command,
+		.mode = COMMAND_ANY,
+		.help = "configure trace FIFO",
+		.usage = "address",
+	},
+	{
+		.name = "flow_control",
+		.handler = handle_esirisc_trace_flow_control_command,
+		.mode = COMMAND_ANY,
+		.help = "enable or disable stalling CPU to collect trace data",
+		.usage = "('enable'|'disable')",
+	},
+	{
+		.name = "format",
+		.handler = handle_esirisc_trace_format_command,
+		.mode = COMMAND_ANY,
+		.help = "configure trace format",
+		.usage = "('full'|'branch'|'icache') pc_bits",
+	},
+	{
+		.name = "trigger",
+		.mode = COMMAND_ANY,
+		.help = "eSi-Trace trigger command group",
+		.usage = "",
+		.chain = esirisc_trace_trigger_any_command_handlers,
+	},
+	{
+		.chain = esirisc_trace_exec_command_handlers
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+const struct command_registration esirisc_trace_command_handlers[] = {
+	{
+		.name = "trace",
+		.mode = COMMAND_ANY,
+		.help = "eSi-Trace command group",
+		.usage = "",
+		.chain = esirisc_trace_any_command_handlers,
+	},
+	COMMAND_REGISTRATION_DONE
+};
diff --git a/src/target/esirisc_trace.h b/src/target/esirisc_trace.h
new file mode 100644
index 0000000..c3cc6e9
--- /dev/null
+++ b/src/target/esirisc_trace.h
@@ -0,0 +1,105 @@
+/***************************************************************************
+ *   Copyright (C) 2018 by Square, Inc.                                    *
+ *   Steven Stallion <stallion@squareup.com>                               *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+
+#ifndef OPENOCD_TARGET_ESIRISC_TRACE_H
+#define OPENOCD_TARGET_ESIRISC_TRACE_H
+
+#include <helper/command.h>
+#include <helper/types.h>
+#include <target/target.h>
+
+enum esirisc_trace_delay {
+	ESIRISC_TRACE_DELAY_NONE,
+	ESIRISC_TRACE_DELAY_START,
+	ESIRISC_TRACE_DELAY_STOP,
+	ESIRISC_TRACE_DELAY_BOTH,
+};
+
+enum esirisc_trace_format {
+	ESIRISC_TRACE_FORMAT_FULL,
+	ESIRISC_TRACE_FORMAT_BRANCH,
+	ESIRISC_TRACE_FORMAT_ICACHE,
+};
+
+enum esirisc_trace_id {
+	ESIRISC_TRACE_ID_EXECUTE,
+	ESIRISC_TRACE_ID_STALL,
+	ESIRISC_TRACE_ID_BRANCH,
+	ESIRISC_TRACE_ID_EXTENDED,
+};
+
+enum esirisc_trace_ext_id {
+	ESIRISC_TRACE_EXT_ID_EXCEPTION = 1,
+	ESIRISC_TRACE_EXT_ID_ERET,
+	ESIRISC_TRACE_EXT_ID_STOP,
+	ESIRISC_TRACE_EXT_ID_WAIT,
+	ESIRISC_TRACE_EXT_ID_MULTICYCLE,
+	ESIRISC_TRACE_EXT_ID_COUNT,
+	ESIRISC_TRACE_EXT_ID_PC,
+	ESIRISC_TRACE_EXT_ID_INDIRECT,
+	ESIRISC_TRACE_EXT_ID_END,
+	ESIRISC_TRACE_EXT_ID_END_PC,
+};
+
+enum esirisc_trace_trigger {
+	ESIRISC_TRACE_TRIGGER_NONE,
+	ESIRISC_TRACE_TRIGGER_PC,
+	ESIRISC_TRACE_TRIGGER_LOAD,
+	ESIRISC_TRACE_TRIGGER_STORE,
+	ESIRISC_TRACE_TRIGGER_EXCEPTION,
+	ESIRISC_TRACE_TRIGGER_ERET,
+	ESIRISC_TRACE_TRIGGER_WAIT,
+	ESIRISC_TRACE_TRIGGER_STOP,
+	ESIRISC_TRACE_TRIGGER_HIGH,
+	ESIRISC_TRACE_TRIGGER_LOW,
+};
+
+struct esirisc_trace {
+	target_addr_t buffer_start;
+	target_addr_t buffer_end;
+	bool buffer_wrap;
+	bool flow_control;
+
+	enum esirisc_trace_format format;
+	int pc_bits;
+
+	enum esirisc_trace_trigger start_trigger;
+	uint32_t start_data;
+	uint32_t start_mask;
+
+	enum esirisc_trace_trigger stop_trigger;
+	uint32_t stop_data;
+	uint32_t stop_mask;
+
+	enum esirisc_trace_delay delay;
+	uint32_t delay_cycles;
+};
+
+extern const struct command_registration esirisc_trace_command_handlers[];
+
+static inline uint32_t esirisc_trace_buffer_size(struct esirisc_trace *trace_info)
+{
+	return trace_info->buffer_end - trace_info->buffer_start;
+}
+
+static inline bool esirisc_trace_is_fifo(struct esirisc_trace *trace_info)
+{
+	return trace_info->buffer_start == trace_info->buffer_end;
+}
+
+#endif /* OPENOCD_TARGET_ESIRISC_TRACE_H */
diff --git a/src/target/fa526.c b/src/target/fa526.c
index 9f6b805..bb9f735 100644
--- a/src/target/fa526.c
+++ b/src/target/fa526.c
@@ -364,6 +364,7 @@ struct target_type fa526_target = {
 	.deassert_reset = arm7_9_deassert_reset,
 	.soft_reset_halt = arm920t_soft_reset_halt,
 
+	.get_gdb_arch = arm_get_gdb_arch,
 	.get_gdb_reg_list = arm_get_gdb_reg_list,
 
 	.read_memory = arm920t_read_memory,
diff --git a/src/target/feroceon.c b/src/target/feroceon.c
index 6b14ab6..21963e5 100644
--- a/src/target/feroceon.c
+++ b/src/target/feroceon.c
@@ -710,6 +710,7 @@ struct target_type feroceon_target = {
 	.deassert_reset = arm7_9_deassert_reset,
 	.soft_reset_halt = arm926ejs_soft_reset_halt,
 
+	.get_gdb_arch = arm_get_gdb_arch,
 	.get_gdb_reg_list = arm_get_gdb_reg_list,
 
 	.read_memory = arm7_9_read_memory,
@@ -747,6 +748,7 @@ struct target_type dragonite_target = {
 	.deassert_reset = arm7_9_deassert_reset,
 	.soft_reset_halt = arm7_9_soft_reset_halt,
 
+	.get_gdb_arch = arm_get_gdb_arch,
 	.get_gdb_reg_list = arm_get_gdb_reg_list,
 
 	.read_memory = arm7_9_read_memory,
diff --git a/src/target/hla_target.c b/src/target/hla_target.c
index 9ebf241..4f97eb9 100644
--- a/src/target/hla_target.c
+++ b/src/target/hla_target.c
@@ -366,12 +366,14 @@ static int adapter_target_create(struct target *target,
 {
 	LOG_DEBUG("%s", __func__);
 	struct adiv5_private_config *pc = target->private_config;
-	struct cortex_m_common *cortex_m = calloc(1, sizeof(struct cortex_m_common));
-	if (!cortex_m)
-		return ERROR_COMMAND_SYNTAX_ERROR;
-
 	if (pc != NULL && pc->ap_num > 0) {
 		LOG_ERROR("hla_target: invalid parameter -ap-num (> 0)");
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	struct cortex_m_common *cortex_m = calloc(1, sizeof(struct cortex_m_common));
+	if (cortex_m == NULL) {
+		LOG_ERROR("No memory creating target");
 		return ERROR_FAIL;
 	}
 
@@ -470,6 +472,9 @@ static int adapter_poll(struct target *target)
 	if (prev_target_state == state)
 		return ERROR_OK;
 
+	if (prev_target_state == TARGET_DEBUG_RUNNING && state == TARGET_RUNNING)
+		return ERROR_OK;
+
 	target->state = state;
 
 	if (state == TARGET_HALTED) {
@@ -820,6 +825,7 @@ struct target_type hla_target = {
 	.resume = adapter_resume,
 	.step = adapter_step,
 
+	.get_gdb_arch = arm_get_gdb_arch,
 	.get_gdb_reg_list = armv7m_get_gdb_reg_list,
 
 	.read_memory = adapter_read_memory,
diff --git a/src/target/image.c b/src/target/image.c
index 0d98c57..9bd8f6b 100644
--- a/src/target/image.c
+++ b/src/target/image.c
@@ -643,8 +643,8 @@ static int image_mot_buffer_complete_inner(struct image *image,
 					section[image->num_sections].size += 1;
 					full_address++;
 				}
-			} else if (record_type == 5) {
-				/* S5 is the data count record, we ignore it */
+			} else if (record_type == 5 || record_type == 6) {
+				/* S5 and S6 are the data count records, we ignore them */
 				uint32_t dummy;
 
 				while (count-- > 0) {
diff --git a/src/target/mips32.c b/src/target/mips32.c
index b5dbea3..9ac2507 100644
--- a/src/target/mips32.c
+++ b/src/target/mips32.c
@@ -461,6 +461,8 @@ int mips32_run_algorithm(struct target *target, int num_mem_params,
 	}
 
 	for (int i = 0; i < num_mem_params; i++) {
+		if (mem_params[i].direction == PARAM_IN)
+			continue;
 		retval = target_write_buffer(target, mem_params[i].address,
 				mem_params[i].size, mem_params[i].value);
 		if (retval != ERROR_OK)
@@ -468,6 +470,9 @@ int mips32_run_algorithm(struct target *target, int num_mem_params,
 	}
 
 	for (int i = 0; i < num_reg_params; i++) {
+		if (reg_params[i].direction == PARAM_IN)
+			continue;
+
 		struct reg *reg = register_get_by_name(mips32->core_cache, reg_params[i].reg_name, 0);
 
 		if (!reg) {
diff --git a/src/target/riscv/riscv.c b/src/target/riscv/riscv.c
index b3464c4..e027d00 100644
--- a/src/target/riscv/riscv.c
+++ b/src/target/riscv/riscv.c
@@ -1013,6 +1013,9 @@ static int riscv_run_algorithm(struct target *target, int num_mem_params,
 
 	uint64_t saved_regs[32];
 	for (int i = 0; i < num_reg_params; i++) {
+		if (reg_params[i].direction == PARAM_IN)
+			continue;
+
 		LOG_DEBUG("save %s", reg_params[i].reg_name);
 		struct reg *r = register_get_by_name(target->reg_cache, reg_params[i].reg_name, 0);
 		if (!r) {
diff --git a/src/target/stm8.c b/src/target/stm8.c
index 5a3438a..b62ff13 100644
--- a/src/target/stm8.c
+++ b/src/target/stm8.c
@@ -1189,6 +1189,11 @@ static int stm8_write_core_reg(struct target *target, unsigned int num)
 	return ERROR_OK;
 }
 
+static const char *stm8_get_gdb_arch(struct target *target)
+{
+	return "stm8";
+}
+
 static int stm8_get_gdb_reg_list(struct target *target, struct reg **reg_list[],
 		int *reg_list_size, enum target_register_class reg_class)
 {
@@ -1800,6 +1805,7 @@ static int stm8_blank_check_memory(struct target *target,
 	destroy_mem_param(&mem_params[0]);
 	destroy_mem_param(&mem_params[1]);
 	destroy_reg_param(&reg_params[0]);
+	destroy_reg_param(&reg_params[1]);
 
 	target_free_working_area(target, erase_check_algorithm);
 
@@ -1884,6 +1890,8 @@ static int stm8_run_algorithm(struct target *target, int num_mem_params,
 	}
 
 	for (int i = 0; i < num_mem_params; i++) {
+		if (mem_params[i].direction == PARAM_IN)
+			continue;
 		retval = target_write_buffer(target, mem_params[i].address,
 				mem_params[i].size, mem_params[i].value);
 		if (retval != ERROR_OK)
@@ -1891,6 +1899,9 @@ static int stm8_run_algorithm(struct target *target, int num_mem_params,
 	}
 
 	for (int i = 0; i < num_reg_params; i++) {
+		if (reg_params[i].direction == PARAM_IN)
+			continue;
+
 		struct reg *reg = register_get_by_name(stm8->core_cache,
 				reg_params[i].reg_name, 0);
 
@@ -2199,6 +2210,7 @@ struct target_type stm8_target = {
 	.assert_reset = stm8_reset_assert,
 	.deassert_reset = stm8_reset_deassert,
 
+	.get_gdb_arch = stm8_get_gdb_arch,
 	.get_gdb_reg_list = stm8_get_gdb_reg_list,
 
 	.read_memory = stm8_read_memory,
diff --git a/src/target/target.c b/src/target/target.c
index bc7cba6..8857dc4 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -1931,6 +1931,65 @@ int target_free_working_area(struct target *target, struct working_area *area)
 	return target_free_working_area_restore(target, area, 1);
 }
 
+/* free resources and restore memory, if restoring memory fails,
+ * free up resources anyway
+ */
+static void target_free_all_working_areas_restore(struct target *target, int restore)
+{
+	struct working_area *c = target->working_areas;
+
+	LOG_DEBUG("freeing all working areas");
+
+	/* Loop through all areas, restoring the allocated ones and marking them as free */
+	while (c) {
+		if (!c->free) {
+			if (restore)
+				target_restore_working_area(target, c);
+			c->free = true;
+			*c->user = NULL; /* Same as above */
+			c->user = NULL;
+		}
+		c = c->next;
+	}
+
+	/* Run a merge pass to combine all areas into one */
+	target_merge_working_areas(target);
+
+	print_wa_layout(target);
+}
+
+void target_free_all_working_areas(struct target *target)
+{
+	target_free_all_working_areas_restore(target, 1);
+
+	/* Now we have none or only one working area marked as free */
+	if (target->working_areas) {
+		/* Free the last one to allow on-the-fly moving and resizing */
+		free(target->working_areas->backup);
+		free(target->working_areas);
+		target->working_areas = NULL;
+	}
+}
+
+/* Find the largest number of bytes that can be allocated */
+uint32_t target_get_working_area_avail(struct target *target)
+{
+	struct working_area *c = target->working_areas;
+	uint32_t max_size = 0;
+
+	if (c == NULL)
+		return target->working_area_size;
+
+	while (c) {
+		if (c->free && max_size < c->size)
+			max_size = c->size;
+
+		c = c->next;
+	}
+
+	return max_size;
+}
+
 static void target_destroy(struct target *target)
 {
 	if (target->type->deinit_target)
@@ -1950,11 +2009,6 @@ static void target_destroy(struct target *target)
 	}
 
 	target_free_all_working_areas(target);
-	/* Now we have none or only one working area marked as free */
-	if (target->working_areas) {
-		free(target->working_areas->backup);
-		free(target->working_areas);
-	}
 
 	/* release the targets SMP list */
 	if (target->smp) {
@@ -2005,57 +2059,6 @@ void target_quit(void)
 	all_targets = NULL;
 }
 
-/* free resources and restore memory, if restoring memory fails,
- * free up resources anyway
- */
-static void target_free_all_working_areas_restore(struct target *target, int restore)
-{
-	struct working_area *c = target->working_areas;
-
-	LOG_DEBUG("freeing all working areas");
-
-	/* Loop through all areas, restoring the allocated ones and marking them as free */
-	while (c) {
-		if (!c->free) {
-			if (restore)
-				target_restore_working_area(target, c);
-			c->free = true;
-			*c->user = NULL; /* Same as above */
-			c->user = NULL;
-		}
-		c = c->next;
-	}
-
-	/* Run a merge pass to combine all areas into one */
-	target_merge_working_areas(target);
-
-	print_wa_layout(target);
-}
-
-void target_free_all_working_areas(struct target *target)
-{
-	target_free_all_working_areas_restore(target, 1);
-}
-
-/* Find the largest number of bytes that can be allocated */
-uint32_t target_get_working_area_avail(struct target *target)
-{
-	struct working_area *c = target->working_areas;
-	uint32_t max_size = 0;
-
-	if (c == NULL)
-		return target->working_area_size;
-
-	while (c) {
-		if (c->free && max_size < c->size)
-			max_size = c->size;
-
-		c = c->next;
-	}
-
-	return max_size;
-}
-
 int target_arch_state(struct target *target)
 {
 	int retval;
@@ -5904,8 +5907,7 @@ static const struct command_registration target_subcommand_handlers[] = {
 	},
 	{
 		.name = "create",
-		/* REVISIT this should be COMMAND_CONFIG ... */
-		.mode = COMMAND_ANY,
+		.mode = COMMAND_CONFIG,
 		.jim_handler = jim_target_create,
 		.usage = "name type '-chain-position' name [options ...]",
 		.help = "Creates and selects a new target",
diff --git a/src/target/xscale.c b/src/target/xscale.c
index 87a3d0f..3ac4553 100644
--- a/src/target/xscale.c
+++ b/src/target/xscale.c
@@ -3703,6 +3703,7 @@ struct target_type xscale_target = {
 	.deassert_reset = xscale_deassert_reset,
 
 	/* REVISIT on some cores, allow exporting iwmmxt registers ... */
+	.get_gdb_arch = arm_get_gdb_arch,
 	.get_gdb_reg_list = arm_get_gdb_reg_list,
 
 	.read_memory = xscale_read_memory,
diff --git a/tcl/board/8devices-lima.cfg b/tcl/board/8devices-lima.cfg
index 136f861..0d35cfb 100644
--- a/tcl/board/8devices-lima.cfg
+++ b/tcl/board/8devices-lima.cfg
@@ -27,4 +27,4 @@ $_TARGETNAME configure -event reset-init {
 set ram_boot_address 0xa0000000
 $_TARGETNAME configure -work-area-phys 0xa1FFE000 -work-area-size 0x1000
 
-flash bank flash0 ath79 0 0 0 0 $_TARGETNAME cs0
+flash bank flash0 ath79 0xbf000000 0 0 0 $_TARGETNAME cs0
diff --git a/tcl/board/adsp-sc584-ezbrd.cfg b/tcl/board/adsp-sc584-ezbrd.cfg
new file mode 100644
index 0000000..1054a94
--- /dev/null
+++ b/tcl/board/adsp-sc584-ezbrd.cfg
@@ -0,0 +1,31 @@
+#
+# Analog Devices ADSP-SC584-EZBRD evaluation board
+#
+# Evaluation boards by Analog Devices (and designs derived from them) use a
+# non-standard 10-pin 0.05" ARM Cortex Debug Connector.  In this bastardized
+# implementation, pin 9 (GND or GNDDetect) has been usurped with JTAG /TRST.
+#
+# As a result, a standards-compliant debug pod will force /TRST active,
+# putting the processor's debug interface into reset and preventing usage.
+#
+# A connector adapter must be employed on these boards to isolate or remap
+# /TRST so that it is only asserted when intended.
+
+# Analog expects users to use their proprietary ICE-1000 / ICE-2000 with all
+# ADSP-SC58x designs, but this is an ARM target (and subject to the
+# qualifications above) many ARM debug pods should be compatible.
+
+#source [find interface/cmsis-dap.cfg]
+source [find interface/jlink.cfg]
+
+# Analog's silicon supports SWD and JTAG, but their proprietary ICE is limited
+# to JTAG.  (This is presumably why their connector pinout was modified.)
+# SWD is chosen here, as it is more efficient and doesn't require /TRST.
+
+transport select swd
+
+# chosen speed is 'safe' choice, but your adapter may be capable of more
+adapter_khz 400
+
+source [find target/adsp-sc58x.cfg]
+
diff --git a/tcl/board/atmel_sam3s_ek.cfg b/tcl/board/atmel_sam3s_ek.cfg
index 38b54b7..6e8ffe4 100644
--- a/tcl/board/atmel_sam3s_ek.cfg
+++ b/tcl/board/atmel_sam3s_ek.cfg
@@ -1,3 +1 @@
 source [find target/at91sam3sXX.cfg]
-
-$_TARGETNAME configure -event gdb-attach { reset init }
diff --git a/tcl/board/atmel_sam4s_ek.cfg b/tcl/board/atmel_sam4s_ek.cfg
index dcfa900..ca11e54 100644
--- a/tcl/board/atmel_sam4s_ek.cfg
+++ b/tcl/board/atmel_sam4s_ek.cfg
@@ -1,3 +1 @@
 source [find target/at91sam4sXX.cfg]
-
-$_TARGETNAME configure -event gdb-attach { reset init }
diff --git a/tcl/board/avnet_ultrazed-eg.cfg b/tcl/board/avnet_ultrazed-eg.cfg
index a0ac5c6..9879bfc 100644
--- a/tcl/board/avnet_ultrazed-eg.cfg
+++ b/tcl/board/avnet_ultrazed-eg.cfg
@@ -1,6 +1,6 @@
 #
 # AVNET UltraZED EG StarterKit
-# UlraScale-EG plus IO Carrier with on-board digilent smt2
+# ZynqMP UlraScale-EG plus IO Carrier with on-board digilent smt2
 #
 source [find interface/ftdi/digilent_jtag_smt2_nc.cfg]
 # jtag transport only
@@ -13,4 +13,4 @@ adapter_khz 1000
 
 set CHIPNAME uscale
 
-source [find target/xilinx_ultrascale.cfg]
+source [find target/xilinx_zynqmp.cfg]
diff --git a/tcl/board/dptechnics_dpt-board-v1.cfg b/tcl/board/dptechnics_dpt-board-v1.cfg
index de31c7c..21470b0 100644
--- a/tcl/board/dptechnics_dpt-board-v1.cfg
+++ b/tcl/board/dptechnics_dpt-board-v1.cfg
@@ -29,4 +29,4 @@ $_TARGETNAME configure -event reset-init {
 set ram_boot_address 0xa0000000
 $_TARGETNAME configure -work-area-phys 0xa1FFE000 -work-area-size 0x1000
 
-flash bank flash0 ath79 0 0 0 0 $_TARGETNAME cs0
+flash bank flash0 ath79 0xbf000000 0 0 0 $_TARGETNAME cs0
diff --git a/tcl/board/fsl_imx6q_sabresd.cfg b/tcl/board/fsl_imx6q_sabresd.cfg
index 797e2de..e1f0892 100644
--- a/tcl/board/fsl_imx6q_sabresd.cfg
+++ b/tcl/board/fsl_imx6q_sabresd.cfg
@@ -143,7 +143,5 @@ proc imx6q_sabresd_init { } {
 $_TARGETNAME.0 configure -event reset-assert { }
 # hook the init function into the reset-init event
 $_TARGETNAME.0 configure -event reset-init { imx6q_sabresd_init }
-# make sure target is halted when gdb attaches
-$_TARGETNAME.0 configure -event gdb-attach { halt }
 # set a slow default JTAG clock, can be overridden later
 adapter_khz 1000
diff --git a/tcl/board/lemaker_hikey.cfg b/tcl/board/lemaker_hikey.cfg
index d724440..ee677c3 100644
--- a/tcl/board/lemaker_hikey.cfg
+++ b/tcl/board/lemaker_hikey.cfg
@@ -10,9 +10,6 @@ reset_config srst_only srst_push_pull
 
 source [find target/hi6220.cfg]
 
-# halt the cores when gdb attaches
-${_TARGETNAME}0 configure -event gdb-attach "halt"
-
 # make sure the default target is the boot core
 targets ${_TARGETNAME}0
 
diff --git a/tcl/board/microchip_same54_xplained_pro.cfg b/tcl/board/microchip_same54_xplained_pro.cfg
new file mode 100644
index 0000000..db8a856
--- /dev/null
+++ b/tcl/board/microchip_same54_xplained_pro.cfg
@@ -0,0 +1,13 @@
+#
+# Microchip (former Atmel) SAM E54 Xplained Pro evaluation kit.
+# http://www.microchip.com/developmenttools/productdetails.aspx?partno=atsame54-xpro
+#
+
+source [find interface/cmsis-dap.cfg]
+
+set CHIPNAME same54
+
+source [find target/atsame5x.cfg]
+
+reset_config srst_only
+
diff --git a/tcl/board/ti-cc3200-launchxl.cfg b/tcl/board/ti_cc3200_launchxl.cfg
index fd80c53..b78b09b 100644
--- a/tcl/board/ti-cc3200-launchxl.cfg
+++ b/tcl/board/ti_cc3200_launchxl.cfg
@@ -12,7 +12,9 @@ if { [info exists TRANSPORT] } {
    transport select jtag
 }
 
+adapter_khz 2500
+
 set WORKAREASIZE 0x40000
-source [find target/cc32xx.cfg]
+source [find target/ti_cc32xx.cfg]
 
 reset_config srst_only
diff --git a/tcl/board/tocoding_poplar.cfg b/tcl/board/tocoding_poplar.cfg
index fd66156..d8b8330 100644
--- a/tcl/board/tocoding_poplar.cfg
+++ b/tcl/board/tocoding_poplar.cfg
@@ -12,9 +12,6 @@ reset_config srst_only srst_push_pull
 
 source [find tcl/target/hi3798.cfg]
 
-# halt the cores when gdb attaches
-${_TARGETNAME}0 configure -event gdb-attach "halt"
-
 # make sure the default target is the boot core
 targets ${_TARGETNAME}0
 
diff --git a/tcl/board/tp-link_tl-mr3020.cfg b/tcl/board/tp-link_tl-mr3020.cfg
index 48fb698..366bec8 100644
--- a/tcl/board/tp-link_tl-mr3020.cfg
+++ b/tcl/board/tp-link_tl-mr3020.cfg
@@ -9,4 +9,4 @@ $_TARGETNAME configure -event reset-init {
 set ram_boot_address 0xa0000000
 $_TARGETNAME configure -work-area-phys 0xa1FFE000 -work-area-size 0x1000
 
-flash bank flash0 ath79 0 0 0 0 $_TARGETNAME cs0
+flash bank flash0 ath79 0xbf000000 0 0 0 $_TARGETNAME cs0
diff --git a/tcl/board/tp-link_wdr4300.cfg b/tcl/board/tp-link_wdr4300.cfg
index c317916..7aa79ab 100644
--- a/tcl/board/tp-link_wdr4300.cfg
+++ b/tcl/board/tp-link_wdr4300.cfg
@@ -157,4 +157,4 @@ $_TARGETNAME configure -event reset-init {
 set ram_boot_address 0xa0000000
 $_TARGETNAME configure -work-area-phys 0x1d000000 -work-area-size 0x1000
 
-flash bank flash0 ath79 0 0 0 0 $_TARGETNAME cs0
+flash bank flash0 ath79 0xbf000000 0 0 0 $_TARGETNAME cs0
diff --git a/tcl/interface/stlink.cfg b/tcl/interface/stlink.cfg
index d747d85..735ad5a 100644
--- a/tcl/interface/stlink.cfg
+++ b/tcl/interface/stlink.cfg
@@ -1,12 +1,12 @@
 #
-# STMicroelectronics ST-LINK/V1, ST-LINK/V2, ST-LINK/V2-1 in-circuit
+# STMicroelectronics ST-LINK/V1, ST-LINK/V2, ST-LINK/V2-1, STLINK-V3 in-circuit
 # debugger/programmer
 #
 
 interface hla
 hla_layout stlink
 hla_device_desc "ST-LINK"
-hla_vid_pid 0x0483 0x3744 0x0483 0x3748 0x0483 0x374b
+hla_vid_pid 0x0483 0x3744 0x0483 0x3748 0x0483 0x374b 0x0483 0x374d 0x0483 0x374e 0x0483 0x374f 0x0483 0x3752 0x0483 0x3753
 
 # Optionally specify the serial number of ST-LINK/V2 usb device.  ST-LINK/V2
 # devices seem to have serial numbers with unreadable characters.  ST-LINK/V2
diff --git a/tcl/mem_helper.tcl b/tcl/mem_helper.tcl
index 86ad00f..5955793 100644
--- a/tcl/mem_helper.tcl
+++ b/tcl/mem_helper.tcl
@@ -10,6 +10,17 @@ proc mrw {reg} {
 add_usage_text mrw "address"
 add_help_text mrw "Returns value of word in memory."
 
+# mrh: "memory read halfword", returns value of $reg
+proc mrh {reg} {
+	set value ""
+	mem2array value 16 $reg 1
+	return $value(0)
+}
+
+add_usage_text mrh "address"
+add_help_text mrh "Returns value of halfword in memory."
+
+# mrb: "memory read byte", returns value of $reg
 proc mrb {reg} {
 	set value ""
 	mem2array value 8 $reg 1
diff --git a/tcl/target/adsp-sc58x.cfg b/tcl/target/adsp-sc58x.cfg
index e2b6952..8c9ef12 100644
--- a/tcl/target/adsp-sc58x.cfg
+++ b/tcl/target/adsp-sc58x.cfg
@@ -1,11 +1,17 @@
+#
 # Analog Devices ADSP-SC58x (ARM Cortex-A5 plus one or two SHARC+ DSPs)
+#
+
+# Evaluation boards by Analog Devices (and designs derived from them) use a
+# non-standard 10-pin 0.05" ARM Cortex Debug Connector.  In this bastardized
+# implementation, pin 9 (GND or GNDDetect) has been usurped with JTAG /TRST.
+#
+# As a result, a standards-compliant debug pod will force /TRST active,
+# putting the processor's debug interface into reset and preventing usage.
+#
+# A connector adapter must be employed on these boards to isolate or remap
+# /TRST so that it is only asserted when intended.
 
-# evaluation boards by Analog Devices (and designs derived from them) use a non-standard 10-pin 0.05" ARM Cortex Debug Connector
-# pin 9 (GND or GNDDetect) has been usurped with JTAG /TRST
-# as a result, a standards-compliant debug pod will only force the processor's debug interface into reset, preventing usage
-# so, a connector adapter must be employed on these boards to isolate or otherwise prevent /TRST from being asserted
-
-transport select swd
 source [find target/swj-dp.tcl]
 
 if { [info exists CHIPNAME] } {
@@ -26,21 +32,22 @@ if { [info exists CPUTAPID] } {
 	set _CPUTAPID 0x3BA02477
 }
 
-swj_newdap $_CHIPNAME cpu -expected-id $_CPUTAPID
+swj_newdap $_CHIPNAME cpu -irlen 4 -expected-id $_CPUTAPID
 dap create $_CHIPNAME.dap -chain-position $_CHIPNAME.cpu
 
+target create ap0.mem mem_ap -dap $_CHIPNAME.dap -ap-num 0
+
 set _TARGETNAME $_CHIPNAME.cpu
 target create $_TARGETNAME cortex_a -endian $_ENDIAN -dap $_CHIPNAME.dap
 
 $_TARGETNAME configure -event examine-end {
    global _TARGETNAME
-   sc58x_enabledebug $_TARGETNAME
+   sc58x_enabledebug
 }
 
-proc sc58x_enabledebug {target} {
-   # Enable debugging functionality by setting relevant bits in the TAPC_DBGCTL register
-   # the "phys" option is critical; the OpenOCD Cortex-A target code prevents normal mww when the target is not halted
-   # however, it is not possible to halt the target unless these register bits have been set
-   $target mww phys 0x31131000 0xFFFF
+proc sc58x_enabledebug {} {
+   # Enable debugging functionality by setting bits in the TAPC_DBGCTL register
+   # it is not possible to halt the target unless these bits have been set
+   ap0.mem mww 0x31131000 0xFFFF
 }
 
diff --git a/tcl/target/altera_fpgasoc.cfg b/tcl/target/altera_fpgasoc.cfg
index 1fbc5a3..9a83b5c 100644
--- a/tcl/target/altera_fpgasoc.cfg
+++ b/tcl/target/altera_fpgasoc.cfg
@@ -48,7 +48,6 @@ target create $_TARGETNAME1 cortex_a -dap $_CHIPNAME.dap \
 
 $_TARGETNAME1 configure -event reset-start { adapter_khz 1000 }
 $_TARGETNAME1 configure -event reset-assert-post "cycv_dbginit $_TARGETNAME1"
-$_TARGETNAME1 configure -event gdb-attach { halt }
 
 
 # A9 core 1
@@ -57,7 +56,6 @@ $_TARGETNAME1 configure -event gdb-attach { halt }
 
 #$_TARGETNAME2 configure -event reset-start { adapter_khz 1000 }
 #$_TARGETNAME2 configure -event reset-assert-post "cycv_dbginit $_TARGETNAME2"
-#$_TARGETNAME2 configure -event gdb-attach { halt }
 
 proc cycv_dbginit {target} {
         # General Cortex-A8/A9 debug initialisation
diff --git a/tcl/target/atsame5x.cfg b/tcl/target/atsame5x.cfg
new file mode 100644
index 0000000..61949cf
--- /dev/null
+++ b/tcl/target/atsame5x.cfg
@@ -0,0 +1,75 @@
+#
+# Microchip (former Atmel) SAM E54, E53, E51 and D51 devices
+# with a Cortex-M4 core
+#
+
+#
+# Devices only support SWD transports.
+#
+source [find target/swj-dp.tcl]
+
+if { [info exists CHIPNAME] } {
+   set _CHIPNAME $CHIPNAME
+} else {
+   set _CHIPNAME atsame5
+}
+
+if { [info exists ENDIAN] } {
+   set _ENDIAN $ENDIAN
+} else {
+   set _ENDIAN little
+}
+
+# Work-area is a space in RAM used for flash programming
+# By default use 32kB (the smallest RAM size is 128kB)
+if { [info exists WORKAREASIZE] } {
+   set _WORKAREASIZE $WORKAREASIZE
+} else {
+   set _WORKAREASIZE 0x8000
+}
+
+if { [info exists CPUTAPID] } {
+   set _CPUTAPID $CPUTAPID
+} else {
+   set _CPUTAPID 0x4ba00477
+}
+
+swj_newdap $_CHIPNAME cpu -irlen 4 -expected-id $_CPUTAPID
+dap create $_CHIPNAME.dap -chain-position $_CHIPNAME.cpu
+
+set _TARGETNAME $_CHIPNAME.cpu
+target create $_TARGETNAME cortex_m -endian $_ENDIAN -dap $_CHIPNAME.dap
+
+$_TARGETNAME configure -work-area-phys 0x20000000 -work-area-size $_WORKAREASIZE -work-area-backup 0
+
+# SAM DSU will hold the CPU in reset if TCK is low when RESET_N
+# deasserts
+#
+# dsu_reset_deassert configures whether we want to run or halt out of reset,
+# then instruct the DSU to let us out of reset.
+$_TARGETNAME configure -event reset-deassert-post {
+        atsame5 dsu_reset_deassert
+}
+
+# SRST (wired to RESET_N) resets debug circuitry
+# srst_pulls_trst is not configured here to avoid an error raised in reset halt
+reset_config srst_gates_jtag
+
+# Do not use a reset button with other SWD adapter than Atmel's EDBG.
+# DSU usually locks MCU in reset state until you issue a reset command
+# in OpenOCD.
+
+# SAM E5x/D51 runs at SYSCLK = 48 MHz from RC oscillator after reset.
+# Atmel's EDBG (on-board cmsis-dap adapter of Xplained kits) works
+# without problem at clock speed over 5000 khz. Atmel recommends
+# adapter speed less than 10 * CPU clock.
+adapter_khz 2000
+
+if {![using_hla]} {
+   # if srst is not fitted use SYSRESETREQ to
+   # perform a soft reset
+   cortex_m reset_config sysresetreq
+}
+
+set _FLASHNAME $_CHIPNAME.flash
+flash bank $_FLASHNAME atsame5 0x00000000 0 1 1 $_TARGETNAME
diff --git a/tcl/target/bcm281xx.cfg b/tcl/target/bcm281xx.cfg
index 6432a20..0715d82 100644
--- a/tcl/target/bcm281xx.cfg
+++ b/tcl/target/bcm281xx.cfg
@@ -26,10 +26,3 @@ dap create $_CHIPNAME.dap -chain-position $_CHIPNAME.cpu
 target create $_TARGETNAME0 cortex_a -dap $_CHIPNAME.dap -coreid 0 -dbgbase 0x3fe10000
 target create $_TARGETNAME1 cortex_a -dap $_CHIPNAME.dap -coreid 1 -dbgbase 0x3fe12000
 target smp $_TARGETNAME0 $_TARGETNAME1
-
-$_TARGETNAME0 configure -event gdb-attach {
-	cortex_a dbginit
-}
-$_TARGETNAME1 configure -event gdb-attach {
-	cortex_a dbginit
-}
diff --git a/tcl/target/cc2538.cfg b/tcl/target/cc2538.cfg
index 81593c1..63fd9c2 100755
--- a/tcl/target/cc2538.cfg
+++ b/tcl/target/cc2538.cfg
@@ -1,16 +1,45 @@
 # Config for Texas Instruments low power RF SoC CC2538
 # http://www.ti.com/lit/pdf/swru319
 
+adapter_khz 100
+
+source [find target/icepick.cfg]
+source [find target/ti-cjtag.cfg]
+
 if { [info exists CHIPNAME] } {
-   set CHIPNAME $CHIPNAME
+	set _CHIPNAME $CHIPNAME
+} else {
+	set _CHIPNAME cc2538
+}
+
+#
+# Main DAP
+#
+if { [info exists DAP_TAPID] } {
+	set _DAP_TAPID $DAP_TAPID
 } else {
-   set CHIPNAME cc2538
+	set _DAP_TAPID 0x8B96402F
 }
+jtag newtap $_CHIPNAME cpu -irlen 4 -ircapture 0x1 -irmask 0xf -expected-id $_DAP_TAPID -disable
+jtag configure $_CHIPNAME.cpu -event tap-enable "icepick_c_tapenable $_CHIPNAME.jrc 0"
 
+#
+# ICEpick-C (JTAG route controller)
+#
 if { [info exists JRC_TAPID] } {
-   set JRC_TAPID $JRC_TAPID
+	set _JRC_TAPID $JRC_TAPID
 } else {
-   set JRC_TAPID 0x8B96402F
+	set _JRC_TAPID 0x8B96402F
 }
+jtag newtap $_CHIPNAME jrc -irlen 6 -ircapture 0x1 -irmask 0x3f -expected-id $_JRC_TAPID -ignore-version
+# A start sequence is needed to change from cJTAG (Compact JTAG) to
+# 4-pin JTAG before talking via JTAG commands
+jtag configure $_CHIPNAME.jrc -event setup "jtag tapenable $_CHIPNAME.cpu"
+jtag configure $_CHIPNAME.jrc -event post-reset "ti_cjtag_to_4pin_jtag $_CHIPNAME.jrc"
 
-source [find target/cc26xx.cfg]
+#
+# Cortex-M3 target
+#
+set _TARGETNAME $_CHIPNAME.cpu
+dap create $_CHIPNAME.dap -chain-position $_CHIPNAME.cpu
+target create $_TARGETNAME cortex_m -dap $_CHIPNAME.dap
diff --git a/tcl/target/icepick.cfg b/tcl/target/icepick.cfg
index 0f160bb..a945bea 100644
--- a/tcl/target/icepick.cfg
+++ b/tcl/target/icepick.cfg
@@ -90,18 +90,18 @@ proc icepick_c_tapenable {jrc port} {
 	# And never to enter RESET, which will disable the TAPs.
 
 	# first enable power and clock for TAP
-	icepick_c_router $jrc 1 0x2 $port 0x100048
+	icepick_c_router $jrc 1 0x2 $port 0x110048
 
 	# TRM states that the register should be read back here, skipped for now
 
 	# enable debug "default" mode
-	icepick_c_router $jrc 1 0x2 $port 0x102048
+	icepick_c_router $jrc 1 0x2 $port 0x112048
 
 	# TRM states that debug enable and debug mode should be read back and
 	# confirmed - skipped for now
 
 	# Finally select the tap
-	icepick_c_router $jrc 1 0x2 $port 0x102148
+	icepick_c_router $jrc 1 0x2 $port 0x112148
 
 	# Enter the bypass state
 	irscan $jrc [CONST IR_BYPASS] -endstate RUN/IDLE
diff --git a/tcl/target/imx6.cfg b/tcl/target/imx6.cfg
index 5b59ecf..8a32517 100644
--- a/tcl/target/imx6.cfg
+++ b/tcl/target/imx6.cfg
@@ -57,4 +57,3 @@ adapter_khz 1000
 $_TARGETNAME configure -event reset-start { adapter_khz 1000 }
 
 $_TARGETNAME configure -event reset-assert-post "imx6_dbginit $_TARGETNAME"
-$_TARGETNAME configure -event gdb-attach { halt }
diff --git a/tcl/target/lpc1xxx.cfg b/tcl/target/lpc1xxx.cfg
index 701adf2..1969e46 100644
--- a/tcl/target/lpc1xxx.cfg
+++ b/tcl/target/lpc1xxx.cfg
@@ -99,10 +99,14 @@ $_TARGETNAME configure -work-area-phys 0x10000000 -work-area-size $_WORKAREASIZE
 # (same cmd51 destination boundary alignment, and all three support 256 byte
 # transfers).
 #
-# flash bank <name> lpc2000 <base> <size> 0 0 <target#> <variant> <clock> [calc checksum]
+# flash bank <name> lpc2000 <base> <size> 0 0 <target#> <variant> <clock> [calc checksum] [iap entry]
+set _IAP_ENTRY 0
+if { [info exists IAP_ENTRY] } {
+	set _IAP_ENTRY $IAP_ENTRY
+}
 set _FLASHNAME $_CHIPNAME.flash
 flash bank $_FLASHNAME lpc2000 0x0 0 0 0 $_TARGETNAME \
-	auto $_CCLK calc_checksum
+	auto $_CCLK calc_checksum $_IAP_ENTRY
 
 if { $_CHIPSERIES == "lpc800" || $_CHIPSERIES == "lpc1100" || $_CHIPSERIES == "lpc1200" || $_CHIPSERIES == "lpc1300" } {
 	# Do not remap 0x0000-0x0200 to anything but the flash (i.e. select
diff --git a/tcl/target/lpc84x.cfg b/tcl/target/lpc84x.cfg
new file mode 100644
index 0000000..cb36698
--- /dev/null
+++ b/tcl/target/lpc84x.cfg
@@ -0,0 +1,11 @@
+# NXP LPC84x Cortex-M0+ with at least 8kB SRAM
+if { ![info exists CHIPNAME] } {
+	set CHIPNAME lpc84x
+}
+set CHIPSERIES lpc800
+if { ![info exists WORKAREASIZE] } {
+	set WORKAREASIZE 0x1fe0
+}
+
+set IAP_ENTRY 0x0F001FF1
+source [find target/lpc1xxx.cfg]
diff --git a/tcl/target/lpc8nxx.cfg b/tcl/target/lpc8nxx.cfg
new file mode 100644
index 0000000..b933290
--- /dev/null
+++ b/tcl/target/lpc8nxx.cfg
@@ -0,0 +1,81 @@
+# NXP LPC8Nxx NHS31xx Cortex-M0+ with 8kB SRAM
+# Copyright (C) 2018 by Jean-Christian de Rivaz
+# Based on NXP proposal https://community.nxp.com/message/1011149
+# Many thanks to Dries Moors from NXP support.
+# SWD only transport
+
+source [find target/swj-dp.tcl]
+source [find mem_helper.tcl]
+
+if { [info exists CHIPNAME] } {
+	set  _CHIPNAME $CHIPNAME
+} else {
+	set  _CHIPNAME lpc8nxx
+}
+
+swj_newdap $_CHIPNAME cpu -irlen 4 -expected-id 0
+dap create $_CHIPNAME.dap -chain-position $_CHIPNAME.cpu
+
+set _TARGETNAME $_CHIPNAME.cpu
+target create $_TARGETNAME cortex_m -endian little -dap $_CHIPNAME.dap
+if {![using_hla]} {
+	# If srst is not fitted use SYSRESETREQ to  perform a soft reset
+	cortex_m reset_config sysresetreq
+}
+adapter_nsrst_delay 100
+
+$_TARGETNAME configure -work-area-phys 0x10000000 -work-area-size 0x1ff0 -work-area-backup 0
+
+flash bank $_CHIPNAME.flash lpc2000 0x0 0x7800 0 0 $_TARGETNAME lpc800 500
+
+echo "*********************************************************************************"
+echo "*         !!!!! IMPORTANT NOTICE FOR LPC8Nxx and NHS31xx CHIPS !!!!!"
+echo "* When this IC is in power-off or peep power down mode, the SWD HW block is also"
+echo "* unpowered. These modes can be entered by firmware. The default firmware image"
+echo "* (flashed in production) makes use of this. Best is to avoid these power modes"
+echo "* during development, and only later add them when the functionality is complete."
+echo "* Hardware reset or NFC field are the only ways to connect in case the SWD is"
+echo "* powered off. OpenOCD can do a hardware reset if you wire the adapter SRST"
+echo "* signal to the chip RESETN pin and add the following in your configuration:"
+echo "*     reset_config srst_only; flash init; catch init; reset"
+echo "* But if the actual firmware immediately set the power down mode after reset,"
+echo "* OpenOCD might be not fast enough to halt the CPU before the SWD lost power. In"
+echo "* that case the only solution is to apply a NFC field to keep the SWD powered."
+echo "*********************************************************************************"
+
+# Using soft-reset 'reset_config none' is strongly discouraged.
+# RESETN sets the system clock to 500 kHz. Unlike soft-reset does not.
+# Set the system clock to 500 kHz before reset to simulate the functionality of hw reset.
+#
+proc set_sysclk_500khz {} {
+	set SYSCLKCTRL 0x40048020
+	set SYSCLKUEN 0x40048024
+	mww $SYSCLKUEN 0
+	mmw $SYSCLKCTRL 0x8 0xe
+	mww $SYSCLKUEN 1
+	echo "Notice: sysclock set to 500kHz."
+}
+
+# Do not remap the ARM interrupt vectors to anything but the beginning ot the flash.
+# Table System memory remap register (SYSMEMREMAP, address 0x4004 8000) bit description
+# Bit Symbol  Value   Description
+# 0   map         -   interrupt vector remap. 0 after boot.
+#                 0   interrupt vector reside in Flash
+#                 1   interrupt vector reside in SRAM
+# 5:1 offset      -   system memory remap offset. 00000b after boot.
+#            00000b   interrupt vectors in flash or remapped to SRAM but no offset
+#            00001b -
+#            00111b   interrupt vectors offset in flash or SRAM to 1K word segment
+#            01000b -
+#            11111b   interrupt vectors offset in flash to 1K word segment 8 to 31
+# 31:6                reserved
+#
+proc set_no_remap {} {
+	mww 0x40048000 0x00
+	echo "Notice: interrupt vector set to no remap."
+}
+
+$_TARGETNAME configure -event reset-init {
+	set_sysclk_500khz
+	set_no_remap
+}
diff --git a/tcl/target/nhs31xx.cfg b/tcl/target/nhs31xx.cfg
new file mode 100644
index 0000000..964be7b
--- /dev/null
+++ b/tcl/target/nhs31xx.cfg
@@ -0,0 +1,4 @@
+# NXP NHS31xx Cortex-M0+ with 8kB SRAM
+
+set CHIPNAME nhs31xx
+source [find target/lpc8nxx.cfg]
diff --git a/tcl/target/quark_d20xx.cfg b/tcl/target/quark_d20xx.cfg
index 419f9dc..7d718c2 100644
--- a/tcl/target/quark_d20xx.cfg
+++ b/tcl/target/quark_d20xx.cfg
@@ -33,8 +33,6 @@ jtag configure quark_d20xx.quark -event tap-disable \
 
 target create quark_d20xx.quark quark_d20xx -endian little -chain-position quark_d20xx.quark
 
-quark_d20xx.quark configure -event gdb-attach { halt }
-
 quark_d20xx.quark configure -event reset-start {
 	# need to halt the target to write to memory
 	if {[quark_d20xx.quark curstate] ne "halted"} { halt }
diff --git a/tcl/target/stm32f7x.cfg b/tcl/target/stm32f7x.cfg
index b0468e2..f6a44cc 100755
--- a/tcl/target/stm32f7x.cfg
+++ b/tcl/target/stm32f7x.cfg
@@ -58,7 +58,10 @@ if {[using_jtag]} {
  jtag_ntrst_delay 100
 }
 
-# use hardware reset, connect under reset
+# Use hardware reset.
+#
+# This target is compatible with connect_assert_srst, which may be set in a
+# board file.
 reset_config srst_only srst_nogate
 
 if {![using_hla]} {
diff --git a/tcl/target/ti_tms570.cfg b/tcl/target/ti_tms570.cfg
index 21da6c0..ce3a176 100644
--- a/tcl/target/ti_tms570.cfg
+++ b/tcl/target/ti_tms570.cfg
@@ -20,8 +20,8 @@ source [find target/icepick.cfg]
 if { [info exists DAP_TAPID] } {
 	set _DAP_TAPID $DAP_TAPID
 }
-jtag newtap $_CHIPNAME dap -irlen 4 -ircapture 0x1 -irmask 0xf -expected-id $_DAP_TAPID -disable
-jtag configure $_CHIPNAME.dap -event tap-enable "icepick_c_tapenable $_CHIPNAME.jrc 0"
+jtag newtap $_CHIPNAME cpu -irlen 4 -ircapture 0x1 -irmask 0xf -expected-id $_DAP_TAPID -disable
+jtag configure $_CHIPNAME.cpu -event tap-enable "icepick_c_tapenable $_CHIPNAME.jrc 0"
 
 # ICEpick-C (JTAG route controller)
 # JRC_TAPID should be set before source-ing this file
@@ -50,21 +50,18 @@ jtag newtap $_CHIPNAME jrc -irlen 6 -ircapture 0x1 -irmask 0x3f \
 	-expected-id $_JRC_TAPID8 \
 	-expected-id $_JRC_TAPID9 \
 	-ignore-version
-jtag configure $_CHIPNAME.jrc -event setup "jtag tapenable $_CHIPNAME.dap"
+jtag configure $_CHIPNAME.jrc -event setup "jtag tapenable $_CHIPNAME.cpu"
 jtag configure $_CHIPNAME.jrc -event post-reset "runtest 100"
 
+dap create $_CHIPNAME.dap -chain-position $_CHIPNAME.cpu
+
 # Cortex-R4 target
 set _TARGETNAME $_CHIPNAME.cpu
 target create $_TARGETNAME cortex_r4 -endian $_ENDIAN \
-	-chain-position $_CHIPNAME.dap -coreid 0 -dbgbase 0x00001003
+	-dap $_CHIPNAME.dap -coreid 0 -dbgbase 0x00001003
 
 # TMS570 uses quirky BE-32 mode
-$_TARGETNAME dap ti_be_32_quirks 1
-
-$_TARGETNAME configure -event gdb-attach {
-	cortex_r4 dbginit
-	halt
-}
+$_CHIPNAME.dap ti_be_32_quirks 1
 
 $_TARGETNAME configure -event "reset-assert" {
 	global _CHIPNAME
diff --git a/tcl/target/u8500.cfg b/tcl/target/u8500.cfg
index 7d8bfe3..1313efd 100644
--- a/tcl/target/u8500.cfg
+++ b/tcl/target/u8500.cfg
@@ -206,10 +206,6 @@ dap create $_CHIPNAME.dap -chain-position $_CHIPNAME.cpu
 
 target create $_TARGETNAME_1 cortex_a -dap $_CHIPNAME.dap -dbgbase $_DAP_DBG1 -coreid 0 -rtos linux
 
-$_TARGETNAME_1 configure -event gdb-attach {
-	halt
-}
-
 
 if { ![info exists TARGETNAME_2] } {
 global _TARGETNAME_2
@@ -221,10 +217,6 @@ set _TARGETNAME_2 $TARGETNAME_2
 
 target create $_TARGETNAME_2 cortex_a -dap $_CHIPNAME.dap -dbgbase $_DAP_DBG2 -coreid 1 -rtos linux
 
-$_TARGETNAME_2 configure -event gdb-attach {
-	halt
-}
-
 
 if {![info exists SMP]} {
 global _SMP
diff --git a/tcl/target/xilinx_ultrascale.cfg b/tcl/target/xilinx_zynqmp.cfg
index 9056c97..9be781c 100644
--- a/tcl/target/xilinx_ultrascale.cfg
+++ b/tcl/target/xilinx_zynqmp.cfg
@@ -1,6 +1,6 @@
 #
 # target configuration for
-# Xilinx UltraScale+
+# Xilinx ZynqMP (UltraScale+ / A53)
 #
 if { [info exists CHIPNAME] } {
     set _CHIPNAME $CHIPNAME
@@ -9,7 +9,7 @@ if { [info exists CHIPNAME] } {
 }
 
 #
-# DAP tap
+# DAP tap (Quard core A53)
 #
 if { [info exists DAP_TAPID] } {
     set _DAP_TAPID $DAP_TAPID
@@ -21,18 +21,29 @@ jtag newtap $_CHIPNAME tap -irlen 4 -ircapture 0x1 -irmask 0xf -expected-id $_DA
 dap create $_CHIPNAME.dap -chain-position $_CHIPNAME.tap
 
 #
-# PS tap
+# PS tap (UltraScale+)
 #
 if { [info exists PS_TAPID] } {
     set _PS_TAPID $PS_TAPID
+    jtag newtap $_CHIPNAME ps -irlen 12 -ircapture 0x1 -irmask 0x03 -expected-id $_PS_TAPID
 } else {
-    set _PS_TAPID 0x04710093
+    # FPGA Programmable logic. Values take from Table 39-1 in UG1085:
+    jtag newtap $_CHIPNAME ps -irlen 12 -ircapture 0x1 -irmask 0x03 -ignore-version \
+        -expected-id 0x04711093 \
+        -expected-id 0x04710093 \
+        -expected-id 0x04721093 \
+        -expected-id 0x04720093 \
+        -expected-id 0x04739093 \
+        -expected-id 0x04730093 \
+        -expected-id 0x04738093 \
+        -expected-id 0x04740093 \
+        -expected-id 0x04750093 \
+        -expected-id 0x04759093 \
+        -expected-id 0x04758093
 }
 
 set jtag_configured 0
 
-jtag newtap $_CHIPNAME ps -irlen 12 -ircapture 0x1 -irmask 0x03 -expected-id $_PS_TAPID
-
 jtag configure $_CHIPNAME.ps -event setup {
     global _CHIPNAME
     global jtag_configured
diff --git a/tools/initial.sh b/tools/initial.sh
index 9580c9a..446b98b 100755
--- a/tools/initial.sh
+++ b/tools/initial.sh
@@ -12,7 +12,7 @@ add_remote()
 	remote_exist=`grep remote .git/config | grep review	| wc -l`
 	if [ "x$remote_exist" = "x0" ] ; then
 		git remote add review ssh://$USERNAME@openocd.zylin.com:29418/openocd.git
-		git config remote.review.push HEAD:refs/publish/master
+		git config remote.review.push HEAD:refs/for/master
 	else
 		echo "Remote review exists"
 	fi