The openocd-0.9.0 release

Signed-off-by: Paul Fertser <fercerpav@gmail.com>

.gitignore

@@ -30,8 +30,8 @@ src/jtag/drivers/OpenULINK/*.rst
 *.swp
 
 src/startup.tcl
-startup_tcl.c
-xscale_debug.h
+startup_tcl.inc
+xscale_debug.inc
 
 bin2char
 bin2char.exe

BUGS

@@ -6,11 +6,11 @@ posting a message with your report:
 
 	openocd-devel@lists.sourceforge.net
 
-Also, please check the Trac bug database to see if a ticket for
+Also, please check the bug database to see if a ticket for
 the bug has already been opened.  You might be asked to open
 such a ticket, or to update an existing ticket with more data.
 
-	https://sourceforge.net/apps/trac/openocd/
+	http://bugs.openocd.org/
 
 To minimize work for OpenOCD developers, you should try to include
 all of the information listed below.  If you feel that some of the

HACKING

@@ -172,6 +172,13 @@ not have to) be disregarded if all conditions listed below are met:
 
 @section browsing Browsing Patches
 All OpenOCD patches can be reviewed <a href="http://openocd.zylin.com/">here</a>.
+
+@section reviewing Reviewing Patches
+From the main <a href="http://openocd.zylin.com/#/q/status:open,n,z">Review
+page</a> select the patch you want to review and click on that patch. On the
+appearing page select the download method (top right). Apply the
+patch. After building and testing you can leave a note with the "Reply"
+button and mark the patch with -1, 0 and +1.
 */
 /** @file
 This file contains the @ref patchguide page.

NEWS

@@ -2,105 +2,103 @@ This file includes highlights of the changes made in the OpenOCD
 source archive release.
 
 JTAG Layer:
-	* New CMSIS-DAP driver
-	* Andes AICE debug adapter support
-	* New OpenJTAG driver
-	* New BCM2835 (RaspberryPi) driver
-	* JTAG VPI client driver (for OpenRISC Reference Platform SoC)
-	* Xilinx BSCAN_* for OpenRISC support
-	* ST-LINKv2-1 support
-	* ST-LINKv2 SWO tracing support (UART emulation)
-	* JLink-OB (onboard) support
-	* Altera USB Blaster driver rewrite, initial Blaster II
-	  support
-	* ULINK driver ported to libusb-1.0, OpenULINK build fixes
-	* Support up to 64 bit IR lengths
-	* SVF playback (FPGA programming) fixes
-	* "ftdi" interface driver got extensive testing and is now
-	  recommended over the old ft2232 implementation
+	* SWD support with FTDI, Versaloon, J-Link, sysfsgpio
+	* CMSIS-DAP massive speed and stability improvements
+	* Versaloon driver ported to libusb-1.0
+	* STLink can reestablish communication with a target that was
+          disconnected or rebooted
+	* STLink FAULT and WAIT SWD handling improved
+	* New hla_serial command to distinguish between several HLA
+          adapters attached to a single machine
+	* Serial number support for CMSIS-DAP and J-Link adapters
+	* Support for more J-Link adapters
+	* TAP autoprobing improvements
+	* Big speedup for SVF playback with USB Blaster
 
 Boundary Scan:
 
 Target Layer:
-	* New target: Andes nds32
-	* New target: OpenRISC OR1K
-	* New target: Intel Quark X10xx
-	* MIPS EJTAG 1.5/2.0 support
-	* MIPS speed improvements
-	* Cortex-M, Cortex-A (MEM-AP, APB-AP) targets working with BE
-	  hosts now
-	* XScale vector_catch support, reset fixes
-	* dsp563xx ad-hoc breakpoint/watchpoint support
-	* RTOS support for embKernel
-	* Target profiling improvements
-	* Memory access functions testbench
+	* Stability improvements for targets that get disconnected or
+          rebooted during a debug session
+	* MIPS speed and reliability improvements
+	* MIPS 1.5/2.0 fixes
+	* ARMv7-R improvements
+	* Cortex-A improvements, A7, A15 MPCores support
+	* FPU support for ARMv7-M (Cortex-M4F)
+	* TPIU/ITM support (including SWO/SWV tracing), can be
+          captured with external tools or STLink
+	* JTAG Serial Port (Advanced Debug System softcore) support
+	* Profiling support for OpenRISC
+	* ChibiOS/RT 3.0 support (with and without FPU)
+	* FreeRTOS current versions support
+	* Freescale MQX RTOS support
+	* GDB target description support for MIPS
+	* The last created target is auto-selected as the current
 
 Flash Layer:
-	* STM32 family sync with reference manuals, other bugfixes
-	* STM32F401, STM32F07x support
-	* Atmel SAM4L, SAMG5x support
-	* at91sam3sd8{a,b}, at91sam3s8{a,b,c}, at91sam4s,
-	  at91sam3n0{a,b,0a,0b} support, bugfixes
-	* Atmel SAMD support
-	* Milandr 1986ВЕ* support
-	* Kinetis KL, K21 support
-	* Nuvoton NuMicro MINI5{1,2,4} support
-	* Nuvoton NUC910 series support
-	* NXP LPC43xx, LPC2000 fixes
-	* NXP LPC800, LPC810 support
-	* More ATmega parts supported
-	* Fujitsu MB9Ax family support
-	* EFM32 Wonder Gecko family support
-	* Nordic nRF51 support
+	* nRF51 async loader to improve flashing performance and stability
+	* Cypress PSoC 41xx/42xx and CCG1 families flash driver
+	* Silabs SiM3 family flash driver
+	* Marvell Wireless Microcontroller SPI flash driver
+	* Kinetis mass erase (part unsecuring) implemented
+	* lpcspifi stability fixes
+	* STM32 family sync with reference manuals, L0 support, bugfixes
+	* LPC2000 driver automatically determines part and flash size
+	* NXP LPC11(x)xx, LPC13xx, LPC15xx, LPC8xx, LPC5410x, LPC407x support
+	* Atmel SAMD, SAMR, SAML21 devices support
+	* Atmel SAM4E16 support
+	* ZeroGecko family support
+	* TI Tiva C Blizzard and Snowflake families support
+	* Nuvoton NuMicro M051 support
 
 Board, Target, and Interface Configuration Scripts:
-	* STM32W108xx generic target config
-	* STM32F429 discovery board config
-	* STM32 Nucleo boards configs
-	* DENX M53EVK board config
-	* Altera Cyclone V SoC, SoCkit config
-	* New TI Launchpads board configs
-	* TI am43xx devices, AM437x GP EVM, AM438x ePOS EVM board
-	  configs
-	* Marvell Armada 370 family initial support
-	* TI TMDX570LS31USB (TMS570, Cortex-R4) support scripts
-	* Freescale FRDM-KL25Z, KL46Z board configs
-	* Digilent Zedboard config
-	* Asus RT-N16, Linksys WRT54GL, BT HomeHub board configs
-	* Atmel Xplained initial support
-	* Broadcom bcm28155_ap board config
-	* TUMPA, TUMPA Lite interface configs
-	* Digilent JTAG-SMT2 interface config
-	* New RAM testing functions
-	* Easy-to-use firmware recovery helpers targetting ordinary
-	  users with common equipment
+	* Normal target configs can work with HLA (STLink, ICDI) adapters
+	* STM32 discovery and Nucleo boards configs
+	* Gumstix AeroCore board config
+	* General Plus GP326XXXA target config
+	* Micrel KS869x target config
+	* ASUS RT-N66U board config
+	* Atmel SAM4E-EK board config
+	* Atmel AT91SAM4L proper reset handling implemented
+	* TI OMAP/AM 3505, 3517 target configs
+	* nRF51822-mKIT board config
+	* RC Module К1879ХБ1Я target config
+	* TI TMDX570LS20SUSB board config
+	* TI TMS570 USB Kit board config
+	* TI CC2538, CC26xx target configs
+	* TI AM437x major config improvements, DDR support
+	* TI AM437X IDK board config
+	* TI SimpleLink Wi-Fi CC3200 LaunchPad configs
+	* Silicon Labs EM357, EM358 target configs
+	* Infineon XMC1000, XMC4000 family targets and boards configs
+	* Atheros AR9331 target config
+	* TP-LINK TL-MR3020 board config
+	* Alphascale asm9260t target and eval kit configs
+	* Olimex SAM7-LA2 (AT91SAM7A2) board config
+	* EFM32 Gecko boards configs
+	* Spansion FM4 target and SK-FM4-176L-S6E2CC board configs
+	* LPC1xxx target configs were restructured
+	* IoT-LAB debug adapter config
+	* DP BusBlaster KT-Link compatible config
 
 Server Layer:
-	* Auto-generation of GDB target description for ARMv7-M,
-	  ARM4, nds32, OR1K, Quark
-	* GDB File-I/O Remote Protocol extension support
-	* Default GDB flashing events handlers to initialise and reset
-	  the target automatically when "load" is used
+	* Polling period can be configured
+	* "shutdown" command has an immediate effect
+	* The "program" command doesn't lead to a shutdown by
+          default, use optional "exit" parameter for the old behaviour
+	* Proper OS signal handling was implemented
+	* Async target notifications for the Tcl RPC
 
 Documentation:
-	* Extensive README* changes
-	* The official User's Guide was proofread
-	* Example cross-build script
-	* RTOS documentation improvements
-	* Tcl RPC documentation and examples added
 
 Build and Release:
-	* *BSD, OS X, clang, ARM, windows build fixes
-	* New pkg-config support changes the way libusb (and other
-	  dependencies) are handled. Many adapter drivers are now
-	  selected automatically during the configure stage.
 
 
 This release also contains a number of other important functional and
 cosmetic bugfixes. For more details about what has changed since the
 last release, see the git repository history:
 
-http://sourceforge.net/p/openocd/code/ci/v0.8.0/log/?path=
+http://sourceforge.net/p/openocd/code/ci/v0.9.0/log/?path=
 
 
 For older NEWS, see the NEWS files associated with each release

NEWS-0.8.0

@@ -0,0 +1,111 @@
+This file includes highlights of the changes made in the OpenOCD
+source archive release.
+
+JTAG Layer:
+	* New CMSIS-DAP driver
+	* Andes AICE debug adapter support
+	* New OpenJTAG driver
+	* New BCM2835 (RaspberryPi) driver
+	* JTAG VPI client driver (for OpenRISC Reference Platform SoC)
+	* Xilinx BSCAN_* for OpenRISC support
+	* ST-LINKv2-1 support
+	* ST-LINKv2 SWO tracing support (UART emulation)
+	* JLink-OB (onboard) support
+	* Altera USB Blaster driver rewrite, initial Blaster II
+	  support
+	* ULINK driver ported to libusb-1.0, OpenULINK build fixes
+	* Support up to 64 bit IR lengths
+	* SVF playback (FPGA programming) fixes
+	* "ftdi" interface driver got extensive testing and is now
+	  recommended over the old ft2232 implementation
+
+Boundary Scan:
+
+Target Layer:
+	* New target: Andes nds32
+	* New target: OpenRISC OR1K
+	* New target: Intel Quark X10xx
+	* MIPS EJTAG 1.5/2.0 support
+	* MIPS speed improvements
+	* Cortex-M, Cortex-A (MEM-AP, APB-AP) targets working with BE
+	  hosts now
+	* XScale vector_catch support, reset fixes
+	* dsp563xx ad-hoc breakpoint/watchpoint support
+	* RTOS support for embKernel
+	* Target profiling improvements
+	* Memory access functions testbench
+
+Flash Layer:
+	* STM32 family sync with reference manuals, other bugfixes
+	* STM32F401, STM32F07x support
+	* Atmel SAM4L, SAMG5x support
+	* at91sam3sd8{a,b}, at91sam3s8{a,b,c}, at91sam4s,
+	  at91sam3n0{a,b,0a,0b} support, bugfixes
+	* Atmel SAMD support
+	* Milandr 1986ВЕ* support
+	* Kinetis KL, K21 support
+	* Nuvoton NuMicro MINI5{1,2,4} support
+	* Nuvoton NUC910 series support
+	* NXP LPC43xx, LPC2000 fixes
+	* NXP LPC800, LPC810 support
+	* More ATmega parts supported
+	* Fujitsu MB9Ax family support
+	* EFM32 Wonder Gecko family support
+	* Nordic nRF51 support
+
+Board, Target, and Interface Configuration Scripts:
+	* STM32W108xx generic target config
+	* STM32F429 discovery board config
+	* STM32 Nucleo boards configs
+	* DENX M53EVK board config
+	* Altera Cyclone V SoC, SoCkit config
+	* New TI Launchpads board configs
+	* TI am43xx devices, AM437x GP EVM, AM438x ePOS EVM board
+	  configs
+	* Marvell Armada 370 family initial support
+	* TI TMDX570LS31USB (TMS570, Cortex-R4) support scripts
+	* Freescale FRDM-KL25Z, KL46Z board configs
+	* Digilent Zedboard config
+	* Asus RT-N16, Linksys WRT54GL, BT HomeHub board configs
+	* Atmel Xplained initial support
+	* Broadcom bcm28155_ap board config
+	* TUMPA, TUMPA Lite interface configs
+	* Digilent JTAG-SMT2 interface config
+	* New RAM testing functions
+	* Easy-to-use firmware recovery helpers targetting ordinary
+	  users with common equipment
+
+Server Layer:
+	* Auto-generation of GDB target description for ARMv7-M,
+	  ARM4, nds32, OR1K, Quark
+	* GDB File-I/O Remote Protocol extension support
+	* Default GDB flashing events handlers to initialise and reset
+	  the target automatically when "load" is used
+
+Documentation:
+	* Extensive README* changes
+	* The official User's Guide was proofread
+	* Example cross-build script
+	* RTOS documentation improvements
+	* Tcl RPC documentation and examples added
+
+Build and Release:
+	* *BSD, OS X, clang, ARM, windows build fixes
+	* New pkg-config support changes the way libusb (and other
+	  dependencies) are handled. Many adapter drivers are now
+	  selected automatically during the configure stage.
+
+
+This release also contains a number of other important functional and
+cosmetic bugfixes. For more details about what has changed since the
+last release, see the git repository history:
+
+http://sourceforge.net/p/openocd/code/ci/v0.8.0/log/?path=
+
+
+For older NEWS, see the NEWS files associated with each release
+(i.e. NEWS-<version>).
+
+For more information about contributing test reports, bug fixes, or new
+features and device support, please read the new Developer Manual (or
+the BUGS and PATCHES.txt files in the source archive).

README

@@ -39,7 +39,11 @@ If you are connecting a particular adapter with some specific target,
 you need to source both the jtag interface and the target configs,
 e.g.:
 
-  openocd -f interface/ftdi/jtagkey2.cfg -f target/ti_calypso.cfg
+  openocd -f interface/ftdi/jtagkey2.cfg -c "transport select jtag" \
+          -f target/ti_calypso.cfg
+
+  openocd -f interface/stlink-v2-1.cfg -c "transport select hla_swd" \
+          -f target/stm32l0.cfg
 
 NB: when using an FTDI-based adapter you should prefer configs in the
 ftdi directory; the old configs for the ft2232 are deprecated.
@@ -57,10 +61,10 @@ In addition to the in-tree documentation, the latest manuals may be
 viewed online at the following URLs:
 
   OpenOCD User's Guide:
-    http://openocd.sourceforge.net/doc/html/index.html
+    http://openocd.org/doc/html/index.html
 
   OpenOCD Developer's Manual:
-    http://openocd.sourceforge.net/doc/doxygen/html/index.html
+    http://openocd.org/doc/doxygen/html/index.html
 
 These reflect the latest development versions, so the following section
 introduces how to build the complete documentation from the package.
@@ -123,9 +127,10 @@ Flash drivers
 -------------
 
 ADUC702x, AT91SAM, AVR, CFI, DSP5680xx, EFM32, EM357, FM3, Kinetis,
-LPC2000, LPC2900, LPCSPIFI, Milandr, NuMicro, PIC32mx, Stellaris,
-STM32, STMSMI, STR7x, STR9x, nRF51; NAND controllers of AT91SAM9, LPC3180,
-LPC32xx, i.MX31, MXC, NUC910, Orion/Kirkwood, S3C24xx, S3C6400.
+LPC8xx/LPC1xxx/LPC2xxx/LPC541xx, LPC2900, LPCSPIFI, Marvell QSPI,
+Milandr, NuMicro, PIC32mx, PSoC4, SiM3x, Stellaris, STM32, STMSMI,
+STR7x, STR9x, nRF51; NAND controllers of AT91SAM9, LPC3180, LPC32xx,
+i.MX31, MXC, NUC910, Orion/Kirkwood, S3C24xx, S3C6400.
 
 
 ==================

README.OSX

@@ -3,23 +3,38 @@ Building OpenOCD for OSX
 
 There are a few prerequisites you will need first:
 
-- Xcode 4 (install from the AppStore)
-- Command Line Tools (install from Xcode 4 -> Preferences -> Downloads)
-- MacPorts (http://www.macports.org/install.php)
+- Xcode 5 (install from the AppStore)
+- Command Line Tools (install from Xcode 5 -> Preferences -> Downloads)
+- Gentoo Prefix (http://www.gentoo.org/proj/en/gentoo-alt/prefix/bootstrap.xml)
   or
 - Homebrew (http://mxcl.github.io/homebrew/)
+  or
+- MacPorts (http://www.macports.org/install.php)
 
-libtool, automake, autoconf, pkg-config and libusb can be easily
-installed via MacPorts:
-  sudo port install libtool automake autoconf pkgconfig libusb [libusb-compat]
-or with Homebrew:
-  brew install libtool automake libusb [libusb-compat] [hidapi]
+
+With Gentoo Prefix you can build the release version or the latest
+devel version (-9999) the usual way described in the Gentoo
+documentation. Alternatively, install the prerequisites and build
+manually from the sources.
+
+
+With Homebrew you can either run:
+  brew install [--HEAD] openocd (where optional --HEAD asks brew to
+                                 install the current git version)
+    or
+  brew install libtool automake libusb [libusb-compat] [hidapi] [libftdi]
+    (to install the needed dependencies and then proceed with the
+     manual building procedure)
+
+
+For building with MacPorts you need to run:
+  sudo port install libtool automake autoconf pkgconfig \
+    libusb [libusb-compat] [libftdi1]
 
 You should also specify LDFLAGS and CPPFLAGS to allow configure to use
 MacPorts' libraries, so run configure like this:
   LDFLAGS=-L/opt/local/lib CPPFLAGS=-I/opt/local/include ./configure [options]
 
-If you're using Homebrew, no custom flags are necessary.
 
 See README for the generic building instructions.
 

README.Windows

@@ -7,6 +7,9 @@ recommended as it doesn't provide enough C99 compatibility).
 Alternatively, one can cross-compile it using MinGW-w64 on a *nix
 host. See README for the generic instructions.
 
+Also, the MSYS2 project provides both ready-made binaries and an easy
+way to self-compile from their software repository out of the box.
+
 Native MinGW-w64/MSYS compilation
 -----------------------------
 
@@ -22,13 +25,21 @@ installation.
 USB adapters
 ------------
 
-You usually need to have WinUSB.sys (or libusbK.sys) driver installed
-for a USB-based adapter. Some vendor software (e.g. for ST-LINKv2)
-does it on its own. For the other cases the easiest way to assign
-WinUSB to a device is to use the latest Zadig installer:
+For the adapters that use a HID-based protocol, e.g. CMSIS-DAP, you do
+not need to perform any additional configuration.
+
+For all the others you usually need to have WinUSB.sys (or
+libusbK.sys) driver installed. Some vendor software (e.g. for
+ST-LINKv2) does it on its own. For the other cases the easiest way to
+assign WinUSB to a device is to use the latest Zadig installer:
 
   http://zadig.akeo.ie
 
+When using a composite USB device, it's often necessary to assign
+WinUSB.sys to the composite parent instead of the specific
+interface. To do that one needs to activate an advanced option in the
+Zadig installer.
+
 For the old drivers that use libusb-0.1 API you might need to link
 against libusb-win32 headers and install the corresponding driver with
 Zadig.

configure.ac

@@ -1,10 +1,18 @@
 AC_PREREQ(2.64)
-AC_INIT([openocd], [0.8.0],
+AC_INIT([openocd], [0.9.0],
   [OpenOCD Mailing List <openocd-devel@lists.sourceforge.net>])
 AC_CONFIG_SRCDIR([src/openocd.c])
 
 m4_include([config_subdir.m4])dnl
 
+# check for makeinfo before calling AM_INIT_AUTOMAKE
+AC_CHECK_PROG([MAKEINFO], [makeinfo], [makeinfo])
+if test "x$MAKEINFO" = "x"; then
+  MAKEINFO='echo makeinfo missing; true'
+  AC_MSG_WARN([Info documentation will not be built.])
+fi
+AC_SUBST([MAKEINFO])
+
 AM_INIT_AUTOMAKE([-Wall -Wno-portability dist-bzip2 dist-zip subdir-objects])
 
 AC_CONFIG_HEADERS([config.h])
@@ -196,7 +204,8 @@ m4_define([USB1_ADAPTERS],
 	[[stlink], [ST-Link JTAG Programmer], [HLADAPTER_STLINK]],
 	[[ti_icdi], [TI ICDI JTAG Programmer], [HLADAPTER_ICDI]],
 	[[ulink], [Keil ULINK JTAG Programmer], [ULINK]],
-	[[usb_blaster_2], [Altera USB-Blaster II Compatible], [USB_BLASTER_2]]])
+	[[usb_blaster_2], [Altera USB-Blaster II Compatible], [USB_BLASTER_2]],
+	[[vsllink], [Versaloon-Link JTAG Programmer], [VSLLINK]]])
 
 m4_define([USB_ADAPTERS],
 	[[[jlink], [Segger J-Link JTAG Programmer], [JLINK]],
@@ -205,8 +214,7 @@ m4_define([USB_ADAPTERS],
 	[[aice], [Andes JTAG Programmer], [AICE]]])
 
 m4_define([USB0_ADAPTERS],
-	[[[vsllink], [Versaloon-Link JTAG Programmer], [VSLLINK]],
-	[[usbprog], [USBProg JTAG Programmer], [USBPROG]],
+	[[[usbprog], [USBProg JTAG Programmer], [USBPROG]],
 	[[rlink], [Raisonance RLink JTAG Programmer], [RLINK]],
 	[[armjtagew], [Olimex ARM-JTAG-EW Programmer], [ARMJTAGEW]]])
 
@@ -615,6 +623,10 @@ case $host in
     ;;
 esac
 
+if test $is_win32 = yes; then
+    AC_DEFINE([WIN32_LEAN_AND_MEAN], [1], [1 to exclude old conflicting definitions when building on Windows])
+fi
+
 if test $build_parport = yes; then
   build_bitbang=yes
   AC_DEFINE([BUILD_PARPORT], [1], [1 if you want parport.])
@@ -1174,7 +1186,7 @@ AM_CONDITIONAL([FT2232_DRIVER], [test $build_ft2232_ftd2xx = yes -o $build_ft223
 AM_CONDITIONAL([USB_BLASTER_LIBFTDI], [test $build_usb_blaster_libftdi = yes])
 AM_CONDITIONAL([USB_BLASTER_FTD2XX], [test $build_usb_blaster_ftd2xx = yes])
 AM_CONDITIONAL([JTAG_VPI], [test $build_jtag_vpi = yes -o $build_jtag_vpi = yes])
-AM_CONDITIONAL([USB_BLASTER_DRIVER], [test $build_usb_blaster_ftd2xx = yes -o $build_usb_blaster_libftdi = yes -o $use_libusb1 = yes])
+AM_CONDITIONAL([USB_BLASTER_DRIVER], [test $build_usb_blaster_ftd2xx = yes -o $build_usb_blaster_libftdi = yes -o $enable_usb_blaster_2 != no])
 AM_CONDITIONAL([AMTJTAGACCEL], [test $build_amtjtagaccel = yes])
 AM_CONDITIONAL([GW16012], [test $build_gw16012 = yes])
 AM_CONDITIONAL([PRESTO_LIBFTDI], [test $build_presto_libftdi = yes])
@@ -1248,47 +1260,6 @@ if test $gcc_warnings = yes; then
   CFLAGS="$CFLAGS $GCC_WARNINGS"
 fi
 
-# Setup for compiling build tools
-AC_MSG_CHECKING([for a C compiler for build tools])
-if test $cross_compiling = yes; then
-  AC_CHECK_PROGS(CC_FOR_BUILD, gcc cc)
-  CFLAGS_FOR_BUILD="-g -O2 $GCC_WARNINGS"
-else
-  CC_FOR_BUILD=$CC
-  CFLAGS_FOR_BUILD=$CFLAGS
-fi
-
-AC_MSG_RESULT([$CC_FOR_BUILD])
-AC_SUBST([CC_FOR_BUILD])
-AC_SUBST([CFLAGS_FOR_BUILD])
-
-AC_MSG_CHECKING([for suffix of executable build tools])
-if test $cross_compiling = yes; then
-  cat >conftest.c <<\_______EOF
-int main ()
-{
-  exit (0);
-}
-_______EOF
-  for i in .exe ""; do
-    compile="$CC_FOR_BUILD conftest.c -o conftest$i"
-    if AC_TRY_EVAL(compile); then
-      if (./conftest) 2>&AC_FD_CC; then
-        EXEEXT_FOR_BUILD=$i
-        break
-      fi
-    fi
-  done
-  rm -f conftest*
-  if test "${EXEEXT_FOR_BUILD+set}" != set; then
-    AC_MSG_ERROR([Cannot determine suffix of executable build tools])
-  fi
-else
-  EXEEXT_FOR_BUILD=$EXEEXT
-fi
-AC_MSG_RESULT([$EXEEXT_FOR_BUILD])
-AC_SUBST([EXEEXT_FOR_BUILD])
-
 AC_CONFIG_FILES([
   Makefile
   src/Makefile
@@ -1318,16 +1289,16 @@ echo
 echo OpenOCD configuration summary
 echo --------------------------------------------------
 m4_foreach([adapter], [USB1_ADAPTERS, USB_ADAPTERS, USB0_ADAPTERS, HIDAPI_ADAPTERS],
-	[echo -n m4_format(["%-40s"], ADAPTER_DESC([adapter]))
+	[s=m4_format(["%-40s"], ADAPTER_DESC([adapter]))
 	case $ADAPTER_VAR([adapter]) in
 		auto)
-			echo yes '(auto)'
+			echo "$s"yes '(auto)'
 			;;
 		yes)
-			echo yes
+			echo "$s"yes
 			;;
 		no)
-			echo no
+			echo "$s"no
 			;;
 	esac
 ])

contrib/99-openocd.rules

@@ -50,6 +50,9 @@ ATTRS{idVendor}=="0403", ATTRS{idProduct}=="c141", MODE="664", GROUP="plugdev"
 # Amontec JTAGkey and JTAGkey-tiny
 ATTRS{idVendor}=="0403", ATTRS{idProduct}=="cff8", MODE="664", GROUP="plugdev"
 
+# TI ICDI
+ATTRS{idVendor}=="0451", ATTRS{idProduct}=="c32a", MODE="664", GROUP="plugdev"
+
 # STLink v1
 ATTRS{idVendor}=="0483", ATTRS{idProduct}=="3744", MODE="664", GROUP="plugdev"
 

contrib/itmdump.c

@@ -44,6 +44,7 @@
 #include <string.h>
 #include <unistd.h>
 
+unsigned int dump_swit;
 
 /* Example ITM trace word (0xWWXXYYZZ) parsing for task events, sent
  * on port 31 (Reserved for "the" RTOS in CMSIS v1.30)
@@ -59,6 +60,9 @@ static void show_task(int port, unsigned data)
 	unsigned code = data >> 16;
 	char buf[16];
 
+	if (dump_swit)
+		return;
+
 	switch (code) {
 	case 0:
 		strcpy(buf, "run");
@@ -87,6 +91,9 @@ static void show_reserved(FILE *f, char *label, int c)
 {
 	unsigned i;
 
+	if (dump_swit)
+		return;
+
 	printf("%s - %#02x", label, c);
 
 	for (i = 0; (c & 0x80) && i < 4; i++) {
@@ -105,7 +112,6 @@ static bool read_varlen(FILE *f, int c, unsigned *value)
 {
 	unsigned size;
 	unsigned char buf[4];
-	unsigned i;
 
 	*value = 0;
 
@@ -130,23 +136,25 @@ static bool read_varlen(FILE *f, int c, unsigned *value)
 
 	*value =  (buf[3] << 24)
 		+ (buf[2] << 16)
-		+ (buf[2] << 8)
+		+ (buf[1] << 8)
 		+ (buf[0] << 0);
 	return true;
 
 err:
 	printf("(ERROR %d - %s)\n", errno, strerror(errno));
-	return;
+	return false;
 }
 
 static void show_hard(FILE *f, int c)
 {
 	unsigned type = c >> 3;
 	unsigned value;
-	unsigned size;
 	char *label;
 
-	printf("DWT - ", type);
+	if (dump_swit)
+		return;
+
+	printf("DWT - ");
 
 	if (!read_varlen(f, c, &value))
 		return;
@@ -216,7 +224,7 @@ static void show_hard(FILE *f, int c)
 		}
 		break;
 	default:
-		printf("UNDEFINED");
+		printf("UNDEFINED, rawtype: %x", type);
 		break;
 	}
 
@@ -241,19 +249,28 @@ struct {
 
 static void show_swit(FILE *f, int c)
 {
-	unsigned size;
 	unsigned port = c >> 3;
-	unsigned char buf[4];
 	unsigned value = 0;
 	unsigned i;
 
-	printf("SWIT %u - ", port);
+	if (port + 1 == dump_swit) {
+		if (!read_varlen(f, c, &value))
+			return;
+		printf("%c", value);
+		return;
+	}
 
 	if (!read_varlen(f, c, &value))
 		return;
+
+	if (dump_swit)
+		return;
+
+	printf("SWIT %u - ", port);
+
 	printf("%#08x", value);
 
-	for (i = 0; i <= sizeof(format) / sizeof(format[0]); i++) {
+	for (i = 0; i < sizeof(format) / sizeof(format[0]); i++) {
 		if (format[i].port == port) {
 			printf(", ");
 			format[i].show(port, value);
@@ -263,10 +280,6 @@ static void show_swit(FILE *f, int c)
 
 	printf("\n");
 	return;
-
-err:
-	printf("(ERROR %d - %s)\n", errno, strerror(errno));
-	return;
 }
 
 static void show_timestamp(FILE *f, int c)
@@ -275,6 +288,9 @@ static void show_timestamp(FILE *f, int c)
 	char *label = "";
 	bool delayed = false;
 
+	if (dump_swit)
+		return;
+
 	printf("TIMESTAMP - ");
 
 	/* Format 2: header only */
@@ -293,7 +309,7 @@ static void show_timestamp(FILE *f, int c)
 	}
 
 	/* Format 1:  one to four bytes of data too */
-	switch (c) {
+	switch (c >> 4) {
 	default:
 		label = ", reserved control\n";
 		break;
@@ -356,7 +372,7 @@ int main(int argc, char **argv)
 	int c;
 
 	/* parse arguments */
-	while ((c = getopt(argc, argv, "f:")) != EOF) {
+	while ((c = getopt(argc, argv, "f:d:")) != EOF) {
 		switch (c) {
 		case 'f':
 			/* e.g. from UART connected to /dev/ttyUSB0 */
@@ -366,8 +382,10 @@ int main(int argc, char **argv)
 				return 1;
 			}
 			break;
+		case 'd':
+			dump_swit = atoi(optarg);
+			break;
 		default:
-usage:
 			fprintf(stderr, "usage: %s [-f input]",
 				basename(argv[0]));
 			return 1;

contrib/loaders/flash/cortex-m0.S

@@ -0,0 +1,72 @@
+/***************************************************************************
+ *   Copyright (C) 2014 by Angus Gratton                                   *
+ *   Derived from stm32f1x.S:
+ *   Copyright (C) 2011 by Andreas Fritiofson                              *
+ *   andreas.fritiofson@gmail.com                                          *
+ *   Copyright (C) 2013 by Roman Dmitrienko                                *
+ *   me@iamroman.org                                                       *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+	.text
+	.syntax unified
+	.cpu cortex-m0
+	.thumb
+	.thumb_func
+
+/* Written for NRF51822 (src/flash/nor/nrf51.c) however the NRF NVMC is
+ * very generic (CPU blocks during flash writes), so this is actually
+ * just a generic word-oriented copy routine for cortex-m0 (also
+ * suitable for cortex m0plus/m3/m4.)
+ *
+ * To assemble:
+ * arm-none-eabi-gcc -c cortex-m0.S
+ *
+ * To disassemble:
+ * arm-none-eabi-objdump -o cortex-m0.o
+ *
+ * Thanks to Jens Bauer for providing advice on some of the tweaks.
+ */
+
+	/* Params:
+	 * r0 - byte count (in)
+	 * r1 - workarea start
+	 * r2 - workarea end
+	 * r3 - target address
+	 * Clobbered:
+	 * r4 - rp
+	 * r5 - wp, tmp
+	 */
+
+wait_fifo:
+	ldr 	r5, [r1, #0]	/* read wp */
+	cmp 	r5, #0	        /* abort if wp == 0 */
+	beq 	exit
+	ldr 	r4, [r1, #4]	/* read rp */
+	cmp 	r4, r5		/* wait until rp != wp */
+	beq 	wait_fifo
+
+	ldmia	r4!, {r5}	/* "*target_address++ = *rp++" */
+        stmia   r3!, {r5}
+
+        cmp 	r4, r2		/* wrap rp at end of work area buffer */
+	bcc	no_wrap
+	mov	r4, r1
+	adds	r4, #8          /* skip rp,wp at start of work area */
+no_wrap:
+	str 	r4, [r1, #4]	/* write back rp */
+	subs	r0, #4          /* decrement byte count */
+	bne     wait_fifo	/* loop if not done */
+exit:
+	bkpt    #0

contrib/loaders/flash/lpcspifi_write.S

@@ -39,6 +39,17 @@
  * r11 - current page end address
  */
 
+/*
+ * This code is embedded within: src/flash/nor/lpcspifi.c as a "C" array.
+ *
+ * To rebuild:
+ *   arm-none-eabi-gcc -c lpcspifi_write.S
+ *   arm-none-eabi-objcopy -O binary lpcspifi_write.o lpcspifi_write.bin
+ *   xxd -c 8 -i lpcspifi_write.bin > lpcspifi_write.txt
+ *
+ * Then read and edit this result into the "C" source.
+ */
+
 #define SSP_BASE_HIGH				0x4008
 #define SSP_BASE_LOW				0x3000
 #define SSP_CR0_OFFSET				0x00
@@ -204,6 +215,7 @@ error:
 	movs	r0, #0
 	str 	r0, [r2, #4]	/* set rp = 0 on error */
 exit:
+	bl 		cs_up			/* end the command before returning */
 	mov 	r0, r6
 	bkpt 	#0x00
 

contrib/loaders/flash/mrvlqspi_write.S

@@ -0,0 +1,232 @@
+/***************************************************************************
+ *   Copyright (C) 2014 by Mahavir Jain <mjain@marvell.com>                *
+ *                                                                         *
+ *   Adapted from (contrib/loaders/flash/lpcspifi_write.S):                *
+ *   Copyright (C) 2012 by George Harris                                   *
+ *   george@luminairecoffee.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, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.           *
+ ***************************************************************************/
+
+	.text
+	.syntax unified
+	.cpu cortex-m3
+	.thumb
+	.thumb_func
+
+/*
+ * For compilation:
+ * arm-none-eabi-gcc -mcpu=cortex-m3 -mthumb -c contrib/loaders/flash/mrvlqspi_write.S
+ * arm-none-eabi-objcopy -O binary mrvlqspi_write.o code.bin
+ * Copy code.bin into mrvlqspi flash driver
+ */
+
+/*
+ * Params :
+ * r0 = workarea start, status (out)
+ * r1 = workarea end
+ * r2 = target address (offset from flash base)
+ * r3 = count (bytes)
+ * r4 = page size
+ * r5 = qspi base address
+ * Clobbered:
+ * r7 - rp
+ * r8 - wp, tmp
+ * r9 - send/receive data
+ * r10 - current page end address
+ */
+
+#define CNTL	0x0
+#define CONF	0x4
+#define DOUT	0x8
+#define DIN	0xc
+#define INSTR   0x10
+#define ADDR    0x14
+#define RDMODE  0x18
+#define HDRCNT	0x1c
+#define DINCNT  0x20
+
+#define SS_EN (1 << 0)
+#define XFER_RDY (1 << 1)
+#define RFIFO_EMPTY (1 << 4)
+#define WFIFO_EMPTY (1 << 6)
+#define WFIFO_FULL (1 << 7)
+#define FIFO_FLUSH (1 << 9)
+#define RW_EN (1 << 13)
+#define XFER_STOP (1 << 14)
+#define XFER_START (1 << 15)
+
+#define INS_WRITE_ENABLE 0x06
+#define INS_READ_STATUS 0x05
+#define INS_PAGE_PROGRAM 0x02
+
+init:
+	mov.w 	r10, #0x00
+find_next_page_boundary:
+	add 	r10, r4		/* Increment to the next page */
+	cmp 	r10, r2
+	/* If we have not reached the next page boundary after the target address, keep going */
+	bls 	find_next_page_boundary
+write_enable:
+	/* Flush read/write fifo's */
+	bl 	flush_fifo
+
+	/* Instruction byte 1 */
+	movs 	r8, #0x1
+	str 	r8, [r5, #HDRCNT]
+
+	/* Set write enable instruction */
+	movs 	r8, #INS_WRITE_ENABLE
+	str 	r8, [r5, #INSTR]
+
+	movs	r9, #0x1
+	bl	start_tx
+	bl 	stop_tx
+page_program:
+	/* Instruction byte 1, Addr byte 3 */
+	movs 	r8, #0x31
+	str 	r8, [r5, #HDRCNT]
+	/* Todo: set addr and data pin to single */
+write_address:
+	mov 	r8, r2
+	str 	r8, [r5, #ADDR]
+	/* Set page program instruction */
+	movs 	r8, #INS_PAGE_PROGRAM
+	str 	r8, [r5, #INSTR]
+	/* Start write transfer */
+	movs	r9, #0x1
+	bl	start_tx
+wait_fifo:
+	ldr 	r8, [r0]  	/* read the write pointer */
+	cmp 	r8, #0 		/* if it's zero, we're gonzo */
+	beq 	exit
+	ldr 	r7, [r0, #4] 	/* read the read pointer */
+	cmp 	r7, r8 		/* wait until they are not equal */
+	beq 	wait_fifo
+write:
+	ldrb 	r9, [r7], #0x01 /* Load one byte from the FIFO, increment the read pointer by 1 */
+	bl 	write_data 	/* send the byte to the flash chip */
+
+	cmp 	r7, r1		/* wrap the read pointer if it is at the end */
+	it  	cs
+	addcs	r7, r0, #8	/* skip loader args */
+	str 	r7, [r0, #4]	/* store the new read pointer */
+	subs	r3, r3, #1	/* decrement count */
+	cmp	r3, #0 		/* Exit if we have written everything */
+	beq	write_wait
+	add 	r2, #1 		/* Increment flash address by 1 */
+	cmp 	r10, r2   	/* See if we have reached the end of a page */
+	bne 	wait_fifo 	/* If not, keep writing bytes */
+write_wait:
+	bl 	stop_tx		/* Otherwise, end the command and keep going w/ the next page */
+	add 	r10, r4 	/* Move up the end-of-page address by the page size*/
+check_flash_busy:		/* Wait for the flash to finish the previous page write */
+	/* Flush read/write fifo's */
+	bl 	flush_fifo
+	/* Instruction byte 1 */
+	movs 	r8, #0x1
+	str 	r8, [r5, #HDRCNT]
+	/* Continuous data in of status register */
+	movs	r8, #0x0
+	str 	r8, [r5, #DINCNT]
+	/* Set write enable instruction */
+	movs 	r8, #INS_READ_STATUS
+	str 	r8, [r5, #INSTR]
+	/* Start read transfer */
+	movs	r9, #0x0
+	bl	start_tx
+wait_flash_busy:
+	bl 	read_data
+	and.w	r9, r9, #0x1
+	cmp	r9, #0x0
+	bne.n	wait_flash_busy
+	bl 	stop_tx
+	cmp	r3, #0
+	bne.n 	write_enable 	/* If it is done, start a new page write */
+	b	exit		/* All data written, exit */
+
+write_data: 			/* Send/receive 1 byte of data over QSPI */
+	ldr	r8, [r5, #CNTL]
+	lsls    r8, r8, #24
+	bmi.n	write_data
+	str 	r9, [r5, #DOUT]
+	bx	lr
+
+read_data:			/* Read 1 byte of data over QSPI */
+	ldr	r8, [r5, #CNTL]
+	lsls    r8, r8, #27
+	bmi.n	read_data
+	ldr	r9, [r5, #DIN]
+	bx	lr
+
+flush_fifo:			/* Flush read write fifos */
+	ldr	r8, [r5, #CONF]
+	orr.w   r8, r8, #FIFO_FLUSH
+	str     r8, [r5, #CONF]
+flush_reset:
+	ldr	r8, [r5, #CONF]
+	lsls    r8, r8, #22
+	bmi.n	flush_reset
+	bx	lr
+
+start_tx:
+	ldr	r8, [r5, #CNTL]
+	orr.w	r8, r8, #SS_EN
+	str	r8, [r5, #CNTL]
+xfer_rdy:
+	ldr	r8, [r5, #CNTL]
+	lsls    r8, r8, #30
+	bpl.n	xfer_rdy
+	ldr	r8, [r5, #CONF]
+	bfi	r8, r9, #13, #1
+	orr.w	r8, r8, #XFER_START
+	str	r8, [r5, #CONF]
+	bx lr
+
+stop_tx:
+	ldr	r8, [r5, #CNTL]
+	lsls    r8, r8, #30
+	bpl.n	stop_tx
+wfifo_wait:
+	ldr	r8, [r5, #CNTL]
+	lsls    r8, r8, #25
+	bpl.n	wfifo_wait
+	ldr	r8, [r5, #CONF]
+	orr.w	r8, r8, #XFER_STOP
+	str	r8, [r5, #CONF]
+xfer_start:
+	ldr	r8, [r5, #CONF]
+	lsls	r8, r8, #16
+	bmi.n 	xfer_start
+ss_disable:
+	# Disable SS_EN
+	ldr	r8, [r5, #CNTL]
+	bic.w	r8, r8, #SS_EN
+	str	r8, [r5, #CNTL]
+wait:
+	ldr	r8, [r5, #CNTL]
+	lsls    r8, r8, #30
+	bpl.n	wait
+	bx 	lr
+
+error:
+	movs	r0, #0
+	str 	r0, [r2, #4]	/* set rp = 0 on error */
+exit:
+	mov 	r0, r6
+	bkpt 	#0x00
+
+	.end

contrib/loaders/flash/sim3x.s

@@ -0,0 +1,81 @@
+/***************************************************************************
+ *   Copyright (C) 2014 by Ladislav Bábel                                  *
+ *   ladababel@seznam.cz                                                   *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (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.                          *
+ ***************************************************************************/
+
+#define INITIAL_UNLOCK    0x5A
+#define MULTIPLE_UNLOCK   0xF2
+
+#define FLASHCTRL_KEY     0x4002E0C0
+#define FLASHCTRL_CONFIG  0x4002E000
+#define FLASHCTRL_WRADDR  0x4002E0A0
+#define FLASHCTRL_WRDATA  0x4002E0B0
+#define BUSYF             0x00100000
+
+
+		/* Write the initial unlock value to KEY (0xA5) */
+		movs    r6, #INITIAL_UNLOCK
+		str     r6, [r0, #FLASHCTRL_KEY]
+
+		/* Write the multiple unlock value to KEY (0xF2) */
+		movs    r6, #MULTIPLE_UNLOCK
+		str     r6, [r0, #FLASHCTRL_KEY]
+
+wait_fifo:
+		ldr     r6, [r2, #0]
+		cmp	    r6, #0
+		beq     exit
+		ldr     r5, [r2, #4]
+		cmp     r5, r6
+		beq     wait_fifo
+
+		/* wait for BUSYF flag */
+wait_busy1:
+		ldr     r6, [r0, #FLASHCTRL_CONFIG]
+		tst     r6, #BUSYF
+		bne     wait_busy1
+
+		/* Write the destination address to WRADDR */
+		str     r4, [r0, #FLASHCTRL_WRADDR]
+
+		/* Write the data half-word to WRDATA in right-justified format */
+		ldrh    r6, [r5]
+		str     r6, [r0, #FLASHCTRL_WRDATA]
+
+		adds    r5, #2
+		adds    r4, #2
+
+		/* wrap rp at end of buffer */
+		cmp     r5, r3
+		bcc     no_wrap
+		mov     r5, r2
+		adds    r5, #8
+
+no_wrap:
+		str     r5, [r2, #4]
+		subs    r1, r1, #1
+		cmp     r1, #0
+		beq     exit
+		b       wait_fifo
+
+exit:
+		movs    r6, #MULTIPLE_LOCK
+		str     r6, [r0, #FLASHCTRL_KEY]
+
+		/* wait for BUSYF flag */
+wait_busy2:
+		ldr     r6, [r0, #FLASHCTRL_CONFIG]
+		tst     r6, #BUSYF
+		bne     wait_busy2
+
+		bkpt    #0

contrib/rtos-helpers/FreeRTOS-openocd.c

@@ -0,0 +1,20 @@
+/*
+ * Since at least FreeRTOS V7.5.3 uxTopUsedPriority is no longer
+ * present in the kernel, so it has to be supplied by other means for
+ * OpenOCD's threads awareness.
+ *
+ * Add this file to your project, and, if you're using --gc-sections,
+ * ``--undefined=uxTopUsedPriority'' (or
+ * ``-Wl,--undefined=uxTopUsedPriority'' when using gcc for final
+ * linking) to your LDFLAGS; same with all the other symbols you need.
+ */
+
+#include "FreeRTOS.h"
+
+#ifdef __GNUC__
+#define USED __attribute__((used))
+#else
+#define USED
+#endif
+
+const int USED uxTopUsedPriority = configMAX_PRIORITIES;

src/Makefile.am

@@ -32,8 +32,7 @@ endif
 
 libopenocd_la_SOURCES = \
 	hello.c \
-	openocd.c \
-	startup_tcl.c
+	openocd.c
 
 noinst_HEADERS = \
 	hello.h \
@@ -75,7 +74,7 @@ libopenocd_la_LIBADD = \
 	$(top_builddir)/src/rtos/librtos.la \
 	$(top_builddir)/src/helper/libhelper.la \
 	$(LIBFTDI_LIBS) $(MINGWLDADD) \
-	$(HIDAPI_LIBS) $(LIBUSB1_LIBS) $(LIBUSB0_LIBS)
+	$(HIDAPI_LIBS) $(LIBUSB0_LIBS) $(LIBUSB1_LIBS)
 
 STARTUP_TCL_SRCS = \
 	$(srcdir)/helper/startup.tcl \
@@ -86,23 +85,23 @@ STARTUP_TCL_SRCS = \
 
 EXTRA_DIST = $(STARTUP_TCL_SRCS)
 
-BUILT_SOURCES = startup.tcl
+BUILT_SOURCES = startup_tcl.inc
 
 startup.tcl: $(STARTUP_TCL_SRCS)
 	cat $^ > $@
 
-BIN2C = $(top_builddir)/src/helper/bin2char$(EXEEXT_FOR_BUILD)
+BIN2C = $(top_srcdir)/src/helper/bin2char.sh
 
-# Convert .tcl to cfile
-startup_tcl.c: startup.tcl $(BIN2C)
-	$(BIN2C) openocd_startup_tcl < $< > $@ || rm -f $@
+# Convert .tcl to c-array
+startup_tcl.inc: startup.tcl $(BIN2C)
+	$(BIN2C) < $< > $@ || { rm -f $@; false; }
 
-# add startup_tcl.c to make clean list
-CLEANFILES = startup.tcl startup_tcl.c
+# add generated files to make clean list
+CLEANFILES = startup.tcl startup_tcl.inc
 
 # we do not want generated file in the dist
 dist-hook:
-	rm -f $(distdir)/startup_tcl.c
+	rm -f $(distdir)/startup_tcl.inc
 
 MAINTAINERCLEANFILES = $(srcdir)/Makefile.in
 

src/flash/nor/Makefile.am

@@ -43,7 +43,10 @@ NOR_DRIVERS = \
 	kinetis.c \
 	mini51.c \
 	nuc1x.c \
-	nrf51.c
+	nrf51.c \
+	mrvlqspi.c \
+	psoc4.c \
+	sim3x.c
 
 noinst_HEADERS = \
 	core.h \

src/flash/nor/at91sam4.c

@@ -67,7 +67,7 @@
 
 #define REG_NAME_WIDTH  (12)
 
-/* at91sam4s series (has always one flash bank)*/
+/* at91sam4s/at91sam4e series (has always one flash bank)*/
 #define FLASH_BANK_BASE_S   0x00400000
 
 /* at91sam4sd series (two one flash banks), first bank address */
@@ -260,6 +260,42 @@ static struct sam4_chip *get_current_sam4(struct command_context *cmd_ctx)
 
 /* these are used to *initialize* the "pChip->details" structure. */
 static const struct sam4_chip_details all_sam4_details[] = {
+
+	/* Start at91sam4e* series */
+	/*atsam4e16e - LQFP144/LFBGA144*/
+	{
+		.chipid_cidr    = 0xA3CC0CE0,
+		.name           = "at91sam4e16e",
+		.total_flash_size     = 1024 * 1024,
+		.total_sram_size      = 128 * 1024,
+		.n_gpnvms       = 2,
+		.n_banks        = 1,
+		{
+/*		.bank[0] = {*/
+		  {
+			.probed = 0,
+			.pChip  = NULL,
+			.pBank  = NULL,
+			.bank_number = 0,
+			.base_address = FLASH_BANK_BASE_S,
+			.controller_address = 0x400e0a00,
+			.flash_wait_states = 6,	/* workaround silicon bug */
+			.present = 1,
+			.size_bytes =  1024 * 1024,
+			.nsectors   =  128,
+			.sector_size = 8192,
+			.page_size   = 512,
+		  },
+/*		.bank[1] = {*/
+		  {
+			.present = 0,
+			.probed = 0,
+			.bank_number = 1,
+
+		  },
+		},
+	},
+
 	/* Start at91sam4s* series */
 	/*atsam4s16c - LQFP100/BGA100*/
 	{
@@ -460,6 +496,40 @@ static const struct sam4_chip_details all_sam4_details[] = {
 		},
 	},
 
+	/*atsam4s4a - LQFP48/BGA48*/
+	{
+		.chipid_cidr    = 0x288b09e0,
+		.name           = "at91sam4s4a",
+		.total_flash_size     = 256 * 1024,
+		.total_sram_size      = 64 * 1024,
+		.n_gpnvms       = 2,
+		.n_banks        = 1,
+		{
+/*		.bank[0] = {*/
+		  {
+			.probed = 0,
+			.pChip  = NULL,
+			.pBank  = NULL,
+			.bank_number = 0,
+			.base_address = FLASH_BANK_BASE_S,
+			.controller_address = 0x400e0a00,
+			.flash_wait_states = 6,	/* workaround silicon bug */
+			.present = 1,
+			.size_bytes =  256 * 1024,
+			.nsectors   =  32,
+			.sector_size = 8192,
+			.page_size   = 512,
+		  },
+/*		.bank[1] = {*/
+		  {
+			.present = 0,
+			.probed = 0,
+			.bank_number = 1,
+
+		  },
+		},
+	},
+
 	/*at91sam4sd32c*/
 	{
 		.chipid_cidr    = 0x29a70ee0,
@@ -504,6 +574,94 @@ static const struct sam4_chip_details all_sam4_details[] = {
 		},
 	},
 
+	/*at91sam4sd16c*/
+	{
+		.chipid_cidr    = 0x29a70ce0,
+		.name           = "at91sam4sd16c",
+		.total_flash_size     = 1024 * 1024,
+		.total_sram_size      = 160 * 1024,
+		.n_gpnvms       = 3,
+		.n_banks        = 2,
+
+/*		.bank[0] = { */
+		{
+			{
+				.probed = 0,
+				.pChip  = NULL,
+				.pBank  = NULL,
+				.bank_number = 0,
+				.base_address = FLASH_BANK0_BASE_SD,
+				.controller_address = 0x400e0a00,
+				.flash_wait_states = 6,	/* workaround silicon bug */
+				.present = 1,
+				.size_bytes =  512 * 1024,
+				.nsectors   =  64,
+				.sector_size = 8192,
+				.page_size   = 512,
+			},
+
+/*		.bank[1] = { */
+			{
+				.probed = 0,
+				.pChip  = NULL,
+				.pBank  = NULL,
+				.bank_number = 1,
+				.base_address = FLASH_BANK1_BASE_1024K_SD,
+				.controller_address = 0x400e0c00,
+				.flash_wait_states = 6,	/* workaround silicon bug */
+				.present = 1,
+				.size_bytes =  512 * 1024,
+				.nsectors   =  64,
+				.sector_size = 8192,
+				.page_size   = 512,
+			},
+		},
+	},
+
+	/*at91sam4sa16c*/
+	{
+		.chipid_cidr    = 0x28a70ce0,
+		.name           = "at91sam4sa16c",
+		.total_flash_size     = 1024 * 1024,
+		.total_sram_size      = 160 * 1024,
+		.n_gpnvms       = 3,
+		.n_banks        = 2,
+
+/*		.bank[0] = { */
+		{
+			{
+				.probed = 0,
+				.pChip  = NULL,
+				.pBank  = NULL,
+				.bank_number = 0,
+				.base_address = FLASH_BANK0_BASE_SD,
+				.controller_address = 0x400e0a00,
+				.flash_wait_states = 6,	/* workaround silicon bug */
+				.present = 1,
+				.size_bytes =  512 * 1024,
+				.nsectors   =  64,
+				.sector_size = 8192,
+				.page_size   = 512,
+			},
+
+/*		.bank[1] = { */
+			{
+				.probed = 0,
+				.pChip  = NULL,
+				.pBank  = NULL,
+				.bank_number = 1,
+				.base_address = FLASH_BANK1_BASE_1024K_SD,
+				.controller_address = 0x400e0c00,
+				.flash_wait_states = 6,	/* workaround silicon bug */
+				.present = 1,
+				.size_bytes =  512 * 1024,
+				.nsectors   =  64,
+				.sector_size = 8192,
+				.page_size   = 512,
+			},
+		},
+	},
+
 	/* at91samg53n19 */
 	{
 		.chipid_cidr    = 0x247e0ae0,
@@ -1111,6 +1269,7 @@ static const struct archnames { unsigned value; const char *name; } archnames[]
 	{ 0x37,  "CAP7 Series"                                                          },
 	{ 0x39,  "CAP9 Series"                                                          },
 	{ 0x3B,  "CAP11 Series"                                                         },
+	{ 0x3C, "ATSAM4E"                                                               },
 	{ 0x40,  "AT91x40 Series"                                                       },
 	{ 0x42,  "AT91x42 Series"                                                       },
 	{ 0x43,  "SAMG51 Series"

src/flash/nor/at91sam4l.c

@@ -24,18 +24,20 @@
 
 #include "imp.h"
 
+#include <target/cortex_m.h>
+
 /* At this time, the SAM4L Flash is available in these capacities:
  * ATSAM4Lx4xx: 256KB (512 pages)
  * ATSAM4Lx2xx: 128KB (256 pages)
  * ATSAM4Lx8xx: 512KB (1024 pages)
  */
 
-/* There are 16 lockable regions regardless of overall capacity.	The number
+/* There are 16 lockable regions regardless of overall capacity. The number
  * of pages per sector is therefore dependant on capacity. */
 #define SAM4L_NUM_SECTORS 16
 
 /* Locations in memory map */
-#define SAM4L_FLASH			0x00000000 /* Flash region */
+#define SAM4L_FLASH			((uint32_t)0x00000000) /* Flash region */
 #define SAM4L_FLASH_USER	0x00800000 /* Flash user page region */
 #define SAM4L_FLASHCALW		0x400A0000 /* Flash controller */
 #define SAM4L_CHIPID		0x400E0740 /* Chip Identification */
@@ -75,6 +77,14 @@
 
 #define SAM4L_FMCD_CMDKEY	0xA5UL	/* 'key' to issue commands, see 14.10.2 */
 
+
+/* SMAP registers and bits */
+#define SMAP_BASE 0x400A3000
+
+#define SMAP_SCR (SMAP_BASE + 8)
+#define SMAP_SCR_HCR (1 << 1)
+
+
 struct sam4l_chip_info {
 	uint32_t id;
 	uint32_t exid;
@@ -253,7 +263,7 @@ static int sam4l_check_page_erased(struct flash_bank *bank, uint32_t pn,
 	/* Issue a quick page read to verify that we've erased this page */
 	res = sam4l_flash_command(bank->target, SAM4L_FCMD_QPR, pn);
 	if (res != ERROR_OK) {
-		LOG_ERROR("Quick page read %d failed", pn);
+		LOG_ERROR("Quick page read %" PRIu32 " failed", pn);
 		return res;
 	}
 
@@ -307,7 +317,7 @@ static int sam4l_probe(struct flash_bank *bank)
 			chip->flash_kb = 512;
 			break;
 		default:
-			LOG_ERROR("Unknown flash size (chip ID is %08X), assuming 128K", id);
+			LOG_ERROR("Unknown flash size (chip ID is %08" PRIx32 "), assuming 128K", id);
 			chip->flash_kb = 128;
 			break;
 	}
@@ -351,8 +361,8 @@ static int sam4l_probe(struct flash_bank *bank)
 	/* Done */
 	chip->probed = true;
 
-	LOG_INFO("SAM4L MCU: %s (Rev %c) (%uKB Flash with %d %dB pages, %uKB RAM)",
-			chip->details ? chip->details->name : "unknown", 'A' + (id & 0xF),
+	LOG_INFO("SAM4L MCU: %s (Rev %c) (%" PRIu32 "KB Flash with %d %" PRId32 "B pages, %" PRIu32 "KB RAM)",
+			chip->details ? chip->details->name : "unknown", (char)('A' + (id & 0xF)),
 			chip->flash_kb, chip->num_pages, chip->page_size, chip->ram_kb);
 
 	return ERROR_OK;
@@ -633,21 +643,47 @@ static int sam4l_write(struct flash_bank *bank, const uint8_t *buffer,
 	return ERROR_OK;
 }
 
-COMMAND_HANDLER(sam4l_handle_info_command)
+
+COMMAND_HANDLER(sam4l_handle_reset_deassert)
 {
-	return ERROR_OK;
+	struct target *target = get_current_target(CMD_CTX);
+	struct armv7m_common *armv7m = target_to_armv7m(target);
+	struct adiv5_dap *swjdp = armv7m->arm.dap;
+	int retval = ERROR_OK;
+	enum reset_types jtag_reset_config = jtag_get_reset_config();
+
+	/* In case of sysresetreq, debug retains state set in cortex_m_assert_reset()
+	 * so we just release reset held by SMAP
+	 *
+	 * n_RESET (srst) clears the DP, so reenable debug and set vector catch here
+	 *
+	 * After vectreset SMAP release is not needed however makes no harm
+	 */
+	if (target->reset_halt && (jtag_reset_config & RESET_HAS_SRST)) {
+		retval = mem_ap_write_u32(swjdp, DCB_DHCSR, DBGKEY | C_HALT | C_DEBUGEN);
+		if (retval == ERROR_OK)
+			retval = mem_ap_write_atomic_u32(swjdp, DCB_DEMCR,
+				TRCENA | VC_HARDERR | VC_BUSERR | VC_CORERESET);
+		/* do not return on error here, releasing SMAP reset is more important */
+	}
+
+	int retval2 = mem_ap_write_atomic_u32(swjdp, SMAP_SCR, SMAP_SCR_HCR);
+	if (retval2 != ERROR_OK)
+		return retval2;
+
+	return retval;
 }
 
 static const struct command_registration at91sam4l_exec_command_handlers[] = {
 	{
-		.name = "info",
-		.handler = sam4l_handle_info_command,
+		.name = "smap_reset_deassert",
+		.handler = sam4l_handle_reset_deassert,
 		.mode = COMMAND_EXEC,
-		.help = "Print information about the current at91sam4l chip"
-			"and its flash configuration.",
+		.help = "deasert internal reset held by SMAP"
 	},
 	COMMAND_REGISTRATION_DONE
 };
+
 static const struct command_registration at91sam4l_command_handlers[] = {
 	{
 		.name = "at91sam4l",

src/flash/nor/at91samd.c

@@ -23,10 +23,14 @@
 #endif
 
 #include "imp.h"
+#include "helper/binarybuffer.h"
 
 #define SAMD_NUM_SECTORS	16
+#define SAMD_PAGE_SIZE_MAX	1024
 
-#define SAMD_FLASH			0x00000000	/* physical Flash memory */
+#define SAMD_FLASH			((uint32_t)0x00000000)	/* physical Flash memory */
+#define SAMD_USER_ROW		((uint32_t)0x00804000)	/* User Row of Flash */
+#define SAMD_PAC1			0x41000000	/* Peripheral Access Control 1 */
 #define SAMD_DSU			0x41002000	/* Device Service Unit */
 #define SAMD_NVMCTRL		0x41004000	/* Non-volatile memory controller */
 
@@ -59,8 +63,11 @@
 /* Known identifiers */
 #define SAMD_PROCESSOR_M0	0x01
 #define SAMD_FAMILY_D		0x00
+#define SAMD_FAMILY_L		0x01
 #define SAMD_SERIES_20		0x00
 #define SAMD_SERIES_21		0x01
+#define SAMD_SERIES_10		0x02
+#define SAMD_SERIES_11		0x03
 
 struct samd_part {
 	uint8_t id;
@@ -69,6 +76,32 @@ struct samd_part {
 	uint32_t ram_kb;
 };
 
+/* Known SAMD10 parts */
+static const struct samd_part samd10_parts[] = {
+	{ 0x0, "SAMD10D14AMU", 16, 4 },
+	{ 0x1, "SAMD10D13AMU", 8, 4 },
+	{ 0x2, "SAMD10D12AMU", 4, 4 },
+	{ 0x3, "SAMD10D14ASU", 16, 4 },
+	{ 0x4, "SAMD10D13ASU", 8, 4 },
+	{ 0x5, "SAMD10D12ASU", 4, 4 },
+	{ 0x6, "SAMD10C14A", 16, 4 },
+	{ 0x7, "SAMD10C13A", 8, 4 },
+	{ 0x8, "SAMD10C12A", 4, 4 },
+};
+
+/* Known SAMD11 parts */
+static const struct samd_part samd11_parts[] = {
+	{ 0x0, "SAMD11D14AMU", 16, 4 },
+	{ 0x1, "SAMD11D13AMU", 8, 4 },
+	{ 0x2, "SAMD11D12AMU", 4, 4 },
+	{ 0x3, "SAMD11D14ASU", 16, 4 },
+	{ 0x4, "SAMD11D13ASU", 8, 4 },
+	{ 0x5, "SAMD11D12ASU", 4, 4 },
+	{ 0x6, "SAMD11C14A", 16, 4 },
+	{ 0x7, "SAMD11C13A", 8, 4 },
+	{ 0x8, "SAMD11C12A", 4, 4 },
+};
+
 /* Known SAMD20 parts. See Table 12-8 in 42129F–SAM–10/2013 */
 static const struct samd_part samd20_parts[] = {
 	{ 0x0, "SAMD20J18A", 256, 32 },
@@ -81,6 +114,7 @@ static const struct samd_part samd20_parts[] = {
 	{ 0x7, "SAMD20G16A", 64, 8 },
 	{ 0x8, "SAMD20G15A", 32, 4 },
 	{ 0x9, "SAMD20G14A", 16, 2 },
+	{ 0xA, "SAMD20E18A", 256, 32 },
 	{ 0xB, "SAMD20E17A", 128, 16 },
 	{ 0xC, "SAMD20E16A", 64, 8 },
 	{ 0xD, "SAMD20E15A", 32, 4 },
@@ -106,6 +140,30 @@ static const struct samd_part samd21_parts[] = {
 	{ 0xE, "SAMD21E14A", 16, 2 },
 };
 
+/* Known SAMR21 parts. */
+static const struct samd_part samr21_parts[] = {
+	{ 0x19, "SAMR21G18A", 256, 32 },
+	{ 0x1A, "SAMR21G17A", 128, 32 },
+	{ 0x1B, "SAMR21G16A",  64, 32 },
+	{ 0x1C, "SAMR21E18A", 256, 32 },
+	{ 0x1D, "SAMR21E17A", 128, 32 },
+	{ 0x1E, "SAMR21E16A",  64, 32 },
+};
+
+/* Known SAML21 parts. */
+static const struct samd_part saml21_parts[] = {
+	{ 0x00, "SAML21J18A", 256, 32 },
+	{ 0x01, "SAML21J17A", 128, 16 },
+	{ 0x02, "SAML21J16A", 64, 8 },
+	{ 0x05, "SAML21G18A", 256, 32 },
+	{ 0x06, "SAML21G17A", 128, 16 },
+	{ 0x07, "SAML21G16A", 64, 8 },
+	{ 0x0A, "SAML21E18A", 256, 32 },
+	{ 0x0B, "SAML21E17A", 128, 16 },
+	{ 0x0C, "SAML21E16A", 64, 8 },
+	{ 0x0D, "SAML21E15A", 32, 4 },
+};
+
 /* 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. */
@@ -123,6 +181,14 @@ static const struct samd_family samd_families[] = {
 		samd20_parts, ARRAY_SIZE(samd20_parts) },
 	{ SAMD_PROCESSOR_M0, SAMD_FAMILY_D, SAMD_SERIES_21,
 		samd21_parts, ARRAY_SIZE(samd21_parts) },
+	{ SAMD_PROCESSOR_M0, SAMD_FAMILY_D, SAMD_SERIES_21,
+		samr21_parts, ARRAY_SIZE(samr21_parts) },
+	{ SAMD_PROCESSOR_M0, SAMD_FAMILY_D, SAMD_SERIES_10,
+		samd10_parts, ARRAY_SIZE(samd10_parts) },
+	{ SAMD_PROCESSOR_M0, SAMD_FAMILY_D, SAMD_SERIES_11,
+		samd11_parts, ARRAY_SIZE(samd11_parts) },
+	{ SAMD_PROCESSOR_M0, SAMD_FAMILY_L, SAMD_SERIES_21,
+		saml21_parts, ARRAY_SIZE(saml21_parts) },
 };
 
 struct samd_info {
@@ -140,8 +206,8 @@ static struct samd_info *samd_chips;
 static const struct samd_part *samd_find_part(uint32_t id)
 {
 	uint8_t processor = (id >> 28);
-	uint8_t family = (id >> 24) & 0x0F;
-	uint8_t series = (id >> 16) & 0xFF;
+	uint8_t family = (id >> 23) & 0x1F;
+	uint8_t series = (id >> 16) & 0x3F;
 	uint8_t devsel = id & 0xFF;
 
 	for (unsigned i = 0; i < ARRAY_SIZE(samd_families); i++) {
@@ -175,9 +241,31 @@ static int samd_protect_check(struct flash_bank *bank)
 	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 samd_probe(struct flash_bank *bank)
 {
-	uint32_t id, param;
+	uint32_t id;
 	int res;
 	struct samd_info *chip = (struct samd_info *)bank->driver_priv;
 	const struct samd_part *part;
@@ -197,28 +285,22 @@ static int samd_probe(struct flash_bank *bank)
 		return ERROR_FAIL;
 	}
 
-	res = target_read_u32(bank->target,
-			SAMD_NVMCTRL + SAMD_NVMCTRL_PARAM, &param);
-	if (res != ERROR_OK) {
-		LOG_ERROR("Couldn't read NVM Parameters register");
-		return res;
-	}
-
 	bank->size = part->flash_kb * 1024;
 
 	chip->sector_size = bank->size / SAMD_NUM_SECTORS;
 
-	/* The PSZ field (bits 18:16) indicate the page size bytes as 2^(3+n) so
-	 * 0 is 8KB and 7 is 1024KB. */
-	chip->page_size = (8 << ((param >> 16) & 0x7));
-	/* The NVMP field (bits 15:0) indicates the total number of pages */
-	chip->num_pages = param & 0xFFFF;
+	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("SAMD: bank size doesn't match NVM parameters. "
-				"Identified %uKB Flash but NVMCTRL reports %u %uB pages",
+				"Identified %" PRIu32 "KB Flash but NVMCTRL reports %u %" PRIu32 "B pages",
 				part->flash_kb, chip->num_pages, chip->page_size);
 	}
 
@@ -243,53 +325,19 @@ static int samd_probe(struct flash_bank *bank)
 	/* Done */
 	chip->probed = true;
 
-	LOG_INFO("SAMD MCU: %s (%uKB Flash, %uKB RAM)", part->name,
+	LOG_INFO("SAMD MCU: %s (%" PRIu32 "KB Flash, %" PRIu32 "KB RAM)", part->name,
 			part->flash_kb, part->ram_kb);
 
 	return ERROR_OK;
 }
 
-static int samd_protect(struct flash_bank *bank, int set, int first, int last)
-{
-	int res;
-	struct samd_info *chip = (struct samd_info *)bank->driver_priv;
-
-	res = ERROR_OK;
-
-	for (int s = first; s <= last; s++) {
-		if (set != bank->sectors[s].is_protected) {
-			/* Load an address that is within this sector (we use offset 0) */
-			res = target_write_u32(bank->target, SAMD_NVMCTRL + SAMD_NVMCTRL_ADDR,
-					       s * chip->sector_size);
-			if (res != ERROR_OK)
-				goto exit;
-
-			/* Tell the controller to lock that sector */
-
-			uint16_t cmd = (set) ?
-				SAMD_NVM_CMD(SAMD_NVM_CMD_LR) :
-				SAMD_NVM_CMD(SAMD_NVM_CMD_UR);
-
-			res = target_write_u16(bank->target,
-					       SAMD_NVMCTRL + SAMD_NVMCTRL_CTRLA,
-					       cmd);
-			if (res != ERROR_OK)
-				goto exit;
-		}
-	}
-exit:
-	samd_protect_check(bank);
-
-	return res;
-}
-
-static bool samd_check_error(struct flash_bank *bank)
+static bool samd_check_error(struct target *target)
 {
 	int ret;
 	bool error;
 	uint16_t status;
 
-	ret = target_read_u16(bank->target,
+	ret = target_read_u16(target,
 			SAMD_NVMCTRL + SAMD_NVMCTRL_STATUS, &status);
 	if (ret != ERROR_OK) {
 		LOG_ERROR("Can't read NVM status");
@@ -310,7 +358,7 @@ static bool samd_check_error(struct flash_bank *bank)
 	}
 
 	/* Clear the error conditions by writing a one to them */
-	ret = target_write_u16(bank->target,
+	ret = target_write_u16(target,
 			SAMD_NVMCTRL + SAMD_NVMCTRL_STATUS, status);
 	if (ret != ERROR_OK)
 		LOG_ERROR("Can't clear NVM error conditions");
@@ -318,32 +366,216 @@ static bool samd_check_error(struct flash_bank *bank)
 	return error;
 }
 
-static int samd_erase_row(struct flash_bank *bank, uint32_t address)
+static int samd_issue_nvmctrl_command(struct target *target, uint16_t cmd)
 {
-	int res;
-	bool error = false;
-
-	/* Set an address contained in the row to be erased */
-	res = target_write_u32(bank->target,
-			SAMD_NVMCTRL + SAMD_NVMCTRL_ADDR, address >> 1);
-	if (res == ERROR_OK) {
-		/* Issue the Erase Row command to erase that row */
-		res = target_write_u16(bank->target,
-				SAMD_NVMCTRL + SAMD_NVMCTRL_CTRLA,
-				SAMD_NVM_CMD(SAMD_NVM_CMD_ER));
-
-		/* Check (and clear) error conditions */
-		error = samd_check_error(bank);
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
 	}
 
-	if (res != ERROR_OK || error)  {
-		LOG_ERROR("Failed to erase row containing %08X" PRIx32, address);
+	/* Read current configuration. */
+	uint16_t tmp = 0;
+	int res = target_read_u16(target, SAMD_NVMCTRL + SAMD_NVMCTRL_CTRLB,
+			&tmp);
+	if (res != ERROR_OK)
+		return res;
+
+	/* Set cache disable. */
+	res = target_write_u16(target, SAMD_NVMCTRL + SAMD_NVMCTRL_CTRLB,
+			tmp | (1<<18));
+	if (res != ERROR_OK)
+		return res;
+
+	/* Issue the NVM command */
+	int res_cmd = target_write_u16(target,
+			SAMD_NVMCTRL + SAMD_NVMCTRL_CTRLA, SAMD_NVM_CMD(cmd));
+
+	/* Try to restore configuration, regardless of NVM command write
+	 * status. */
+	res = target_write_u16(target, SAMD_NVMCTRL + SAMD_NVMCTRL_CTRLB, tmp);
+
+	if (res_cmd != ERROR_OK)
+		return res_cmd;
+
+	if (res != ERROR_OK)
+		return res;
+
+	/* Check to see if the NVM command resulted in an error condition. */
+	if (samd_check_error(target))
+		return ERROR_FAIL;
+
+	return ERROR_OK;
+}
+
+static int samd_erase_row(struct target *target, uint32_t address)
+{
+	int res;
+
+	/* Set an address contained in the row to be erased */
+	res = target_write_u32(target,
+			SAMD_NVMCTRL + SAMD_NVMCTRL_ADDR, address >> 1);
+
+	/* Issue the Erase Row command to erase that row. */
+	if (res == ERROR_OK)
+		res = samd_issue_nvmctrl_command(target,
+				address == SAMD_USER_ROW ? SAMD_NVM_CMD_EAR : SAMD_NVM_CMD_ER);
+
+	if (res != ERROR_OK)  {
+		LOG_ERROR("Failed to erase row containing %08" PRIx32, address);
 		return ERROR_FAIL;
 	}
 
 	return ERROR_OK;
 }
 
+static bool is_user_row_reserved_bit(uint8_t bit)
+{
+	/* See Table 9-3 in the SAMD20 datasheet for more information. */
+	switch (bit) {
+		/* Reserved bits */
+		case 3:
+		case 7:
+		/* Voltage regulator internal configuration with default value of 0x70,
+		 * may not be changed. */
+		case 17 ... 24:
+		/* 41 is voltage regulator internal configuration and must not be
+		 * changed.  42 through 47 are reserved. */
+		case 41 ... 47:
+			return true;
+		default:
+			break;
+	}
+
+	return false;
+}
+
+/* Modify the contents of the User Row in Flash.  These are described in Table
+ * 9-3 of the SAMD20 datasheet.  The User Row itself has a size of one page
+ * and contains a combination of "fuses" and calibration data in bits 24:17.
+ * We therefore try not to erase the row's contents unless we absolutely have
+ * to and we don't permit modifying reserved bits. */
+static int samd_modify_user_row(struct target *target, uint32_t value,
+		uint8_t startb, uint8_t endb)
+{
+	int res;
+
+	if (is_user_row_reserved_bit(startb) || is_user_row_reserved_bit(endb)) {
+		LOG_ERROR("Can't modify bits in the requested range");
+		return ERROR_FAIL;
+	}
+
+	/* Retrieve the MCU's page size, in bytes. This is also the size of the
+	 * entire User Row. */
+	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 before we allocate. */
+	assert(page_size > 0 && page_size <= SAMD_PAGE_SIZE_MAX);
+
+	/* Make sure we're within the single page that comprises the User Row. */
+	if (startb >= (page_size * 8) || endb >= (page_size * 8)) {
+		LOG_ERROR("Can't modify bits outside the User Row page range");
+		return ERROR_FAIL;
+	}
+
+	uint8_t *buf = malloc(page_size);
+	if (!buf)
+		return ERROR_FAIL;
+
+	/* Read the user row (comprising one page) by half-words. */
+	res = target_read_memory(target, SAMD_USER_ROW, 2, page_size / 2, buf);
+	if (res != ERROR_OK)
+		goto out_user_row;
+
+	/* We will need to erase before writing if the new value needs a '1' in any
+	 * position for which the current value had a '0'.  Otherwise we can avoid
+	 * erasing. */
+	uint32_t cur = buf_get_u32(buf, startb, endb - startb + 1);
+	if ((~cur) & value) {
+		res = samd_erase_row(target, SAMD_USER_ROW);
+		if (res != ERROR_OK) {
+			LOG_ERROR("Couldn't erase user row");
+			goto out_user_row;
+		}
+	}
+
+	/* Modify */
+	buf_set_u32(buf, startb, endb - startb + 1, value);
+
+	/* Write the page buffer back out to the target.  A Flash write will be
+	 * triggered automatically. */
+	res = target_write_memory(target, SAMD_USER_ROW, 4, page_size / 4, buf);
+	if (res != ERROR_OK)
+		goto out_user_row;
+
+	if (samd_check_error(target)) {
+		res = ERROR_FAIL;
+		goto out_user_row;
+	}
+
+	/* Success */
+	res = ERROR_OK;
+
+out_user_row:
+	free(buf);
+
+	return res;
+}
+
+static int samd_protect(struct flash_bank *bank, int set, int first, int last)
+{
+	struct samd_info *chip = (struct samd_info *)bank->driver_priv;
+
+	/* 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;
+	}
+
+	int res = ERROR_OK;
+
+	for (int s = first; s <= last; s++) {
+		if (set != bank->sectors[s].is_protected) {
+			/* Load an address that is within this sector (we use offset 0) */
+			res = target_write_u32(bank->target,
+							SAMD_NVMCTRL + SAMD_NVMCTRL_ADDR,
+							((s * chip->sector_size) >> 1));
+			if (res != ERROR_OK)
+				goto exit;
+
+			/* Tell the controller to lock that sector */
+			res = samd_issue_nvmctrl_command(bank->target,
+					set ? SAMD_NVM_CMD_LR : SAMD_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.  The
+	 * LOCK bits start at bit 48, correspoding to Sector 0 and end with bit 63,
+	 * corresponding to Sector 15.  A '1' means unlocked and a '0' means
+	 * locked.  See Table 9-3 in the SAMD20 datasheet for more details. */
+
+	res = samd_modify_user_row(bank->target, set ? 0x0000 : 0xFFFF,
+			48 + first, 48 + last);
+	if (res != ERROR_OK)
+		LOG_WARNING("SAMD: protect settings were not made persistent!");
+
+	res = ERROR_OK;
+
+exit:
+	samd_protect_check(bank);
+
+	return res;
+}
+
 static int samd_erase(struct flash_bank *bank, int first, int last)
 {
 	int res;
@@ -374,10 +606,10 @@ static int samd_erase(struct flash_bank *bank, int first, int last)
 			return ERROR_FLASH_OPERATION_FAILED;
 		}
 
-		if (!bank->sectors[s].is_erased) {
+		if (bank->sectors[s].is_erased != 1) {
 			/* For each row in that sector */
 			for (int r = s * rows_in_sector; r < (s + 1) * rows_in_sector; r++) {
-				res = samd_erase_row(bank, r * chip->page_size * 4);
+				res = samd_erase_row(bank->target, r * chip->page_size * 4);
 				if (res != ERROR_OK) {
 					LOG_ERROR("SAMD: failed to erase sector %d", s);
 					return res;
@@ -424,7 +656,7 @@ static int samd_write_row(struct flash_bank *bank, uint32_t address,
 	}
 
 	/* Erase the row that we'll be writing to */
-	res = samd_erase_row(bank, address);
+	res = samd_erase_row(bank->target, address);
 	if (res != ERROR_OK)
 		return res;
 
@@ -442,7 +674,10 @@ static int samd_write_row(struct flash_bank *bank, uint32_t address,
 			return res;
 		}
 
-		error = samd_check_error(bank);
+		/* Access through AHB is stalled while flash is being programmed */
+		usleep(200);
+
+		error = samd_check_error(bank->target);
 		if (error)
 			return ERROR_FAIL;
 
@@ -606,6 +841,166 @@ COMMAND_HANDLER(samd_handle_info_command)
 	return ERROR_OK;
 }
 
+COMMAND_HANDLER(samd_handle_chip_erase_command)
+{
+	struct target *target = get_current_target(CMD_CTX);
+
+	if (target) {
+		/* Enable access to the DSU by disabling the write protect bit */
+		target_write_u32(target, SAMD_PAC1, (1<<1));
+		/* Tell the DSU to perform a full chip erase.  It takes about 240ms to
+		 * perform the erase. */
+		target_write_u8(target, SAMD_DSU, (1<<4));
+
+		command_print(CMD_CTX, "chip erased");
+	}
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(samd_handle_set_security_command)
+{
+	int res = ERROR_OK;
+	struct target *target = get_current_target(CMD_CTX);
+
+	if (CMD_ARGC < 1 || (CMD_ARGC >= 1 && (strcmp(CMD_ARGV[0], "enable")))) {
+		command_print(CMD_CTX, "supply the \"enable\" argument to proceed.");
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	if (target) {
+		if (target->state != TARGET_HALTED) {
+			LOG_ERROR("Target not halted");
+			return ERROR_TARGET_NOT_HALTED;
+		}
+
+		res = samd_issue_nvmctrl_command(target, SAMD_NVM_CMD_SSB);
+
+		/* Check (and clear) error conditions */
+		if (res == ERROR_OK)
+			command_print(CMD_CTX, "chip secured on next power-cycle");
+		else
+			command_print(CMD_CTX, "failed to secure chip");
+	}
+
+	return res;
+}
+
+COMMAND_HANDLER(samd_handle_eeprom_command)
+{
+	int res = ERROR_OK;
+	struct target *target = get_current_target(CMD_CTX);
+
+	if (target) {
+		if (target->state != TARGET_HALTED) {
+			LOG_ERROR("Target not halted");
+			return ERROR_TARGET_NOT_HALTED;
+		}
+
+		if (CMD_ARGC >= 1) {
+			int val = atoi(CMD_ARGV[0]);
+			uint32_t code;
+
+			if (val == 0)
+				code = 7;
+			else {
+				/* Try to match size in bytes with corresponding size code */
+				for (code = 0; code <= 6; code++) {
+					if (val == (2 << (13 - code)))
+						break;
+				}
+
+				if (code > 6) {
+					command_print(CMD_CTX, "Invalid EEPROM size.  Please see "
+							"datasheet for a list valid sizes.");
+					return ERROR_COMMAND_SYNTAX_ERROR;
+				}
+			}
+
+			res = samd_modify_user_row(target, code, 4, 6);
+		} else {
+			uint16_t val;
+			res = target_read_u16(target, SAMD_USER_ROW, &val);
+			if (res == ERROR_OK) {
+				uint32_t size = ((val >> 4) & 0x7); /* grab size code */
+
+				if (size == 0x7)
+					command_print(CMD_CTX, "EEPROM is disabled");
+				else {
+					/* Otherwise, 6 is 256B, 0 is 16KB */
+					command_print(CMD_CTX, "EEPROM size is %u bytes",
+							(2 << (13 - size)));
+				}
+			}
+		}
+	}
+
+	return res;
+}
+
+COMMAND_HANDLER(samd_handle_bootloader_command)
+{
+	int res = ERROR_OK;
+	struct target *target = get_current_target(CMD_CTX);
+
+	if (target) {
+		if (target->state != TARGET_HALTED) {
+			LOG_ERROR("Target not halted");
+			return ERROR_TARGET_NOT_HALTED;
+		}
+
+		/* Retrieve the MCU's 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;
+		}
+
+		if (CMD_ARGC >= 1) {
+			int val = atoi(CMD_ARGV[0]);
+			uint32_t code;
+
+			if (val == 0)
+				code = 7;
+			else {
+				/* Try to match size in bytes with corresponding size code */
+				for (code = 0; code <= 6; code++) {
+					if ((unsigned int)val == (2UL << (8UL - code)) * page_size)
+						break;
+				}
+
+				if (code > 6) {
+					command_print(CMD_CTX, "Invalid bootloader size.  Please "
+							"see datasheet for a list valid sizes.");
+					return ERROR_COMMAND_SYNTAX_ERROR;
+				}
+
+			}
+
+			res = samd_modify_user_row(target, code, 0, 2);
+		} else {
+			uint16_t val;
+			res = target_read_u16(target, SAMD_USER_ROW, &val);
+			if (res == ERROR_OK) {
+				uint32_t size = (val & 0x7); /* grab size code */
+				uint32_t nb;
+
+				if (size == 0x7)
+					nb = 0;
+				else
+					nb = (2 << (8 - size)) * page_size;
+
+				/* There are 4 pages per row */
+				command_print(CMD_CTX, "Bootloader size is %" PRIu32 " bytes (%" PRIu32 " rows)",
+					   nb, (uint32_t)(nb / (page_size * 4)));
+			}
+		}
+	}
+
+	return res;
+}
+
 static const struct command_registration at91samd_exec_command_handlers[] = {
 	{
 		.name = "info",
@@ -614,6 +1009,42 @@ static const struct command_registration at91samd_exec_command_handlers[] = {
 		.help = "Print information about the current at91samd chip"
 			"and its flash configuration.",
 	},
+	{
+		.name = "chip-erase",
+		.handler = samd_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 = "set-security",
+		.handler = samd_handle_set_security_command,
+		.mode = COMMAND_EXEC,
+		.help = "Secure the chip's Flash by setting the Security Bit."
+			"This makes it impossible to read the Flash contents."
+			"The only way to undo this is to issue the chip-erase"
+			"command.",
+	},
+	{
+		.name = "eeprom",
+		.usage = "[size_in_bytes]",
+		.handler = samd_handle_eeprom_command,
+		.mode = COMMAND_EXEC,
+		.help = "Show or set the EEPROM size setting, stored in the User Row."
+			"Please see Table 20-3 of the SAMD20 datasheet for allowed values."
+			"Changes are stored immediately but take affect after the MCU is"
+			"reset.",
+	},
+	{
+		.name = "bootloader",
+		.usage = "[size_in_bytes]",
+		.handler = samd_handle_bootloader_command,
+		.mode = COMMAND_EXEC,
+		.help = "Show or set the bootloader size, stored in the User Row."
+			"Please see Table 20-2 of the SAMD20 datasheet for allowed values."
+			"Changes are stored immediately but take affect after the MCU is"
+			"reset.",
+	},
 	COMMAND_REGISTRATION_DONE
 };
 

src/flash/nor/avrf.c

@@ -69,6 +69,7 @@ static const struct avrf_type avft_chips_info[] = {
  */
 	{"atmega128", 0x9702, 256, 512, 8, 512},
 	{"at90can128", 0x9781, 256, 512, 8, 512},
+	{"at90usb128", 0x9782, 256, 512, 8, 512},
 	{"atmega164p", 0x940a, 128, 128, 4, 128},
 	{"atmega324p", 0x9508, 128, 256, 4, 256},
 	{"atmega324pa", 0x9511, 128, 256, 4, 256},

src/flash/nor/drivers.c

@@ -56,6 +56,9 @@ extern struct flash_driver mdr_flash;
 extern struct flash_driver mini51_flash;
 extern struct flash_driver nuc1x_flash;
 extern struct flash_driver nrf51_flash;
+extern struct flash_driver mrvlqspi_flash;
+extern struct flash_driver psoc4_flash;
+extern struct flash_driver sim3x_flash;
 
 /**
  * The list of built-in flash drivers.
@@ -96,6 +99,9 @@ static struct flash_driver *flash_drivers[] = {
 	&mini51_flash,
 	&nuc1x_flash,
 	&nrf51_flash,
+	&mrvlqspi_flash,
+	&psoc4_flash,
+	&sim3x_flash,
 	NULL,
 };
 

src/flash/nor/efm32.c

@@ -10,7 +10,10 @@
  *                                                                         *
  *   Copyright (C) 2013 by Roman Dmitrienko                                *
  *   me@iamroman.org                                                       *
- *
+ *                                                                         *
+ *   Copyright (C) 2014 Nemui Trinomius                                    *
+ *   nemuisan_kawausogasuki@live.jp                                        *
+ *                                                                         *
  *   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     *
@@ -43,6 +46,9 @@
 #define EFM_FAMILY_ID_TINY_GECKO        73
 #define EFM_FAMILY_ID_LEOPARD_GECKO     74
 #define EFM_FAMILY_ID_WONDER_GECKO      75
+#define EFM_FAMILY_ID_ZERO_GECKO        76
+#define EZR_FAMILY_ID_WONDER_GECKO		120
+#define EZR_FAMILY_ID_LEOPARD_GECKO		121
 
 #define EFM32_FLASH_ERASE_TMO           100
 #define EFM32_FLASH_WDATAREADY_TMO      100
@@ -57,7 +63,7 @@
 #define EFM32_MSC_LOCK_BITS             (EFM32_MSC_INFO_BASE+0x4000)
 #define EFM32_MSC_DEV_INFO              (EFM32_MSC_INFO_BASE+0x8000)
 
-/* PAGE_SIZE is only present in Leopard and Giant Gecko MCUs */
+/* PAGE_SIZE is only present in Leopard, Giant and Wonder Gecko MCUs */
 #define EFM32_MSC_DI_PAGE_SIZE          (EFM32_MSC_DEV_INFO+0x1e7)
 #define EFM32_MSC_DI_FLASH_SZ           (EFM32_MSC_DEV_INFO+0x1f8)
 #define EFM32_MSC_DI_RAM_SZ             (EFM32_MSC_DEV_INFO+0x1fa)
@@ -141,9 +147,11 @@ static int efm32x_read_info(struct flash_bank *bank,
 	if (((cpuid >> 4) & 0xfff) == 0xc23) {
 		/* Cortex M3 device */
 	} else if (((cpuid >> 4) & 0xfff) == 0xc24) {
-		/* Cortex M4 device */
+		/* Cortex M4 device(WONDER GECKO) */
+	} else if (((cpuid >> 4) & 0xfff) == 0xc60) {
+		/* Cortex M0plus device(ZERO GECKO) */
 	} else {
-		LOG_ERROR("Target is not CortexM3 or M4");
+		LOG_ERROR("Target is not Cortex-Mx Device");
 		return ERROR_FAIL;
 	}
 
@@ -170,6 +178,8 @@ static int efm32x_read_info(struct flash_bank *bank,
 	if (EFM_FAMILY_ID_GECKO == efm32_info->part_family ||
 			EFM_FAMILY_ID_TINY_GECKO == efm32_info->part_family)
 		efm32_info->page_size = 512;
+	else if (EFM_FAMILY_ID_ZERO_GECKO == efm32_info->part_family)
+		efm32_info->page_size = 1024;
 	else if (EFM_FAMILY_ID_GIANT_GECKO == efm32_info->part_family ||
 			EFM_FAMILY_ID_LEOPARD_GECKO == efm32_info->part_family) {
 		if (efm32_info->prod_rev >= 18) {
@@ -194,7 +204,9 @@ static int efm32x_read_info(struct flash_bank *bank,
 			LOG_ERROR("Invalid page size %u", efm32_info->page_size);
 			return ERROR_FAIL;
 		}
-	} else if (EFM_FAMILY_ID_WONDER_GECKO == efm32_info->part_family) {
+	} else if (EFM_FAMILY_ID_WONDER_GECKO == efm32_info->part_family ||
+			EZR_FAMILY_ID_WONDER_GECKO == efm32_info->part_family ||
+			EZR_FAMILY_ID_LEOPARD_GECKO == efm32_info->part_family) {
 		uint8_t pg_size = 0;
 		ret = target_read_u8(bank->target, EFM32_MSC_DI_PAGE_SIZE,
 			&pg_size);
@@ -838,11 +850,16 @@ static int efm32x_probe(struct flash_bank *bank)
 			LOG_INFO("Tiny Gecko MCU detected");
 			break;
 		case EFM_FAMILY_ID_LEOPARD_GECKO:
+		case EZR_FAMILY_ID_LEOPARD_GECKO:
 			LOG_INFO("Leopard Gecko MCU detected");
 			break;
 		case EFM_FAMILY_ID_WONDER_GECKO:
+		case EZR_FAMILY_ID_WONDER_GECKO:
 			LOG_INFO("Wonder Gecko MCU detected");
 			break;
+		case EFM_FAMILY_ID_ZERO_GECKO:
+			LOG_INFO("Zero Gecko MCU detected");
+			break;
 		default:
 			LOG_ERROR("Unsupported MCU family %d",
 				efm32_mcu_info.part_family);
@@ -933,7 +950,15 @@ static int get_efm32x_info(struct flash_bank *bank, char *buf, int buf_size)
 		return ret;
 	}
 
-	printed = snprintf(buf, buf_size, "EFM32 ");
+	switch (info.part_family) {
+		case EZR_FAMILY_ID_WONDER_GECKO:
+		case EZR_FAMILY_ID_LEOPARD_GECKO:
+			printed = snprintf(buf, buf_size, "EZR32 ");
+			break;
+		default:
+			printed = snprintf(buf, buf_size, "EFM32 ");
+	}
+
 	buf += printed;
 	buf_size -= printed;
 
@@ -951,11 +976,16 @@ static int get_efm32x_info(struct flash_bank *bank, char *buf, int buf_size)
 			printed = snprintf(buf, buf_size, "Tiny Gecko");
 			break;
 		case EFM_FAMILY_ID_LEOPARD_GECKO:
+		case EZR_FAMILY_ID_LEOPARD_GECKO:
 			printed = snprintf(buf, buf_size, "Leopard Gecko");
 			break;
 		case EFM_FAMILY_ID_WONDER_GECKO:
+		case EZR_FAMILY_ID_WONDER_GECKO:
 			printed = snprintf(buf, buf_size, "Wonder Gecko");
 			break;
+		case EFM_FAMILY_ID_ZERO_GECKO:
+			printed = snprintf(buf, buf_size, "Zero Gecko");
+			break;
 	}
 
 	buf += printed;

src/flash/nor/fm3.c

@@ -240,7 +240,7 @@ static int fm3_erase(struct flash_bank *bank, int first, int last)
 	/* R0 keeps Flash Sequence address 1     (u32FlashSeq1)    */
 	/* R1 keeps Flash Sequence address 2     (u32FlashSeq2)    */
 	/* R2 keeps Flash Offset address         (ofs)			   */
-	const uint8_t fm3_flash_erase_sector_code[] = {
+	static const uint8_t fm3_flash_erase_sector_code[] = {
 						/*    *(uint16_t*)u32FlashSeq1 = 0xAA; */
 		0xAA, 0x24,		/*        MOVS  R4, #0xAA              */
 		0x04, 0x80,		/*        STRH  R4, [R0, #0]           */
@@ -849,7 +849,7 @@ static int fm3_chip_erase(struct flash_bank *bank)
 	/* RAMCODE used for fm3 Flash chip erase:				   */
 	/* R0 keeps Flash Sequence address 1     (u32FlashSeq1)    */
 	/* R1 keeps Flash Sequence address 2     (u32FlashSeq2)    */
-	const uint8_t fm3_flash_erase_chip_code[] = {
+	static const uint8_t fm3_flash_erase_chip_code[] = {
 						/*    *(uint16_t*)u32FlashSeq1 = 0xAA; */
 		0xAA, 0x22,		/*        MOVS  R2, #0xAA              */
 		0x02, 0x80,		/*        STRH  R2, [R0, #0]           */

src/flash/nor/kinetis.c

@@ -31,10 +31,12 @@
 #include "config.h"
 #endif
 
+#include "jtag/interface.h"
 #include "imp.h"
 #include <helper/binarybuffer.h>
 #include <target/algorithm.h>
 #include <target/armv7m.h>
+#include <target/cortex_m.h>
 
 /*
  * Implementation Notes
@@ -196,6 +198,289 @@ struct kinetis_flash_bank {
 	} flash_class;
 };
 
+
+
+#define MDM_REG_STAT		0x00
+#define MDM_REG_CTRL		0x04
+#define MDM_REG_ID		0xfc
+
+#define MDM_STAT_FMEACK		(1<<0)
+#define MDM_STAT_FREADY		(1<<1)
+#define MDM_STAT_SYSSEC		(1<<2)
+#define MDM_STAT_SYSRES		(1<<3)
+#define MDM_STAT_FMEEN		(1<<5)
+#define MDM_STAT_BACKDOOREN	(1<<6)
+#define MDM_STAT_LPEN		(1<<7)
+#define MDM_STAT_VLPEN		(1<<8)
+#define MDM_STAT_LLSMODEXIT	(1<<9)
+#define MDM_STAT_VLLSXMODEXIT	(1<<10)
+#define MDM_STAT_CORE_HALTED	(1<<16)
+#define MDM_STAT_CORE_SLEEPDEEP	(1<<17)
+#define MDM_STAT_CORESLEEPING	(1<<18)
+
+#define MEM_CTRL_FMEIP		(1<<0)
+#define MEM_CTRL_DBG_DIS	(1<<1)
+#define MEM_CTRL_DBG_REQ	(1<<2)
+#define MEM_CTRL_SYS_RES_REQ	(1<<3)
+#define MEM_CTRL_CORE_HOLD_RES	(1<<4)
+#define MEM_CTRL_VLLSX_DBG_REQ	(1<<5)
+#define MEM_CTRL_VLLSX_DBG_ACK	(1<<6)
+#define MEM_CTRL_VLLSX_STAT_ACK	(1<<7)
+
+#define MDM_ACCESS_TIMEOUT	3000 /* iterations */
+
+static int kinetis_mdm_write_register(struct adiv5_dap *dap, unsigned reg, uint32_t value)
+{
+	int retval;
+	LOG_DEBUG("MDM_REG[0x%02x] <- %08" PRIX32, reg, value);
+
+	retval = dap_queue_ap_write(dap, reg, value);
+	if (retval != ERROR_OK) {
+		LOG_DEBUG("MDM: failed to queue a write request");
+		return retval;
+	}
+
+	retval = dap_run(dap);
+	if (retval != ERROR_OK) {
+		LOG_DEBUG("MDM: dap_run failed");
+		return retval;
+	}
+
+
+	return ERROR_OK;
+}
+
+static int kinetis_mdm_read_register(struct adiv5_dap *dap, unsigned reg, uint32_t *result)
+{
+	int retval;
+	retval = dap_queue_ap_read(dap, reg, result);
+	if (retval != ERROR_OK) {
+		LOG_DEBUG("MDM: failed to queue a read request");
+		return retval;
+	}
+
+	retval = dap_run(dap);
+	if (retval != ERROR_OK) {
+		LOG_DEBUG("MDM: dap_run failed");
+		return retval;
+	}
+
+	LOG_DEBUG("MDM_REG[0x%02x]: %08" PRIX32, reg, *result);
+	return ERROR_OK;
+}
+
+static int kinetis_mdm_poll_register(struct adiv5_dap *dap, unsigned reg, uint32_t mask, uint32_t value)
+{
+	uint32_t val;
+	int retval;
+	int timeout = MDM_ACCESS_TIMEOUT;
+
+	do {
+		retval = kinetis_mdm_read_register(dap, reg, &val);
+		if (retval != ERROR_OK || (val & mask) == value)
+			return retval;
+
+		alive_sleep(1);
+	} while (timeout--);
+
+	LOG_DEBUG("MDM: polling timed out");
+	return ERROR_FAIL;
+}
+
+/*
+ * This function implements the procedure to mass erase the flash via
+ * SWD/JTAG on Kinetis K and L series of devices as it is described in
+ * AN4835 "Production Flash Programming Best Practices for Kinetis K-
+ * and L-series MCUs" Section 4.2.1
+ */
+COMMAND_HANDLER(kinetis_mdm_mass_erase)
+{
+	struct target *target = get_current_target(CMD_CTX);
+	struct cortex_m_common *cortex_m = target_to_cm(target);
+	struct adiv5_dap *dap = cortex_m->armv7m.arm.dap;
+
+	if (!dap) {
+		LOG_ERROR("Cannot perform mass erase with a high-level adapter");
+		return ERROR_FAIL;
+	}
+
+	int retval;
+	const uint8_t original_ap = dap->ap_current;
+
+	/*
+	 * ... Power on the processor, or if power has already been
+	 * applied, assert the RESET pin to reset the processor. For
+	 * devices that do not have a RESET pin, write the System
+	 * Reset Request bit in the MDM-AP control register after
+	 * establishing communication...
+	 */
+
+	/* assert SRST */
+	if (jtag_get_reset_config() & RESET_HAS_SRST)
+		adapter_assert_reset();
+	else
+		LOG_WARNING("Attempting mass erase without hardware reset. This is not reliable; "
+			    "it's recommended you connect SRST and use ``reset_config srst_only''.");
+
+	dap_ap_select(dap, 1);
+
+	retval = kinetis_mdm_write_register(dap, MDM_REG_CTRL, MEM_CTRL_SYS_RES_REQ);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/*
+	 * ... Read the MDM-AP status register until the Flash Ready bit sets...
+	 */
+	retval = kinetis_mdm_poll_register(dap, MDM_REG_STAT,
+					   MDM_STAT_FREADY | MDM_STAT_SYSRES,
+					   MDM_STAT_FREADY);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("MDM : flash ready timeout");
+		return retval;
+	}
+
+	/*
+	 * ... Write the MDM-AP control register to set the Flash Mass
+	 * Erase in Progress bit. This will start the mass erase
+	 * process...
+	 */
+	retval = kinetis_mdm_write_register(dap, MDM_REG_CTRL,
+					    MEM_CTRL_SYS_RES_REQ | MEM_CTRL_FMEIP);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* As a sanity check make sure that device started mass erase procedure */
+	retval = kinetis_mdm_poll_register(dap, MDM_REG_STAT,
+					   MDM_STAT_FMEACK, MDM_STAT_FMEACK);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/*
+	 * ... Read the MDM-AP control register until the Flash Mass
+	 * Erase in Progress bit clears...
+	 */
+	retval = kinetis_mdm_poll_register(dap, MDM_REG_CTRL,
+					   MEM_CTRL_FMEIP,
+					   0);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/*
+	 * ... Negate the RESET signal or clear the System Reset Request
+	 * bit in the MDM-AP control register...
+	 */
+	retval = kinetis_mdm_write_register(dap, MDM_REG_CTRL, 0);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (jtag_get_reset_config() & RESET_HAS_SRST)
+		adapter_deassert_reset();
+
+	dap_ap_select(dap, original_ap);
+	return ERROR_OK;
+}
+
+static const uint32_t kinetis_known_mdm_ids[] = {
+	0x001C0000,	/* Kinetis-K Series */
+	0x001C0020,	/* Kinetis-L/M/V/E Series */
+};
+
+/*
+ * This function implements the procedure to connect to
+ * SWD/JTAG on Kinetis K and L series of devices as it is described in
+ * AN4835 "Production Flash Programming Best Practices for Kinetis K-
+ * and L-series MCUs" Section 4.1.1
+ */
+COMMAND_HANDLER(kinetis_check_flash_security_status)
+{
+	struct target *target = get_current_target(CMD_CTX);
+	struct cortex_m_common *cortex_m = target_to_cm(target);
+	struct adiv5_dap *dap = cortex_m->armv7m.arm.dap;
+
+	if (!dap) {
+		LOG_WARNING("Cannot check flash security status with a high-level adapter");
+		return ERROR_OK;
+	}
+
+	uint32_t val;
+	int retval;
+	const uint8_t origninal_ap = dap->ap_current;
+
+	dap_ap_select(dap, 1);
+
+
+	/*
+	 * ... The MDM-AP ID register can be read to verify that the
+	 * connection is working correctly...
+	 */
+	retval = kinetis_mdm_read_register(dap, MDM_REG_ID, &val);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("MDM: failed to read ID register");
+		goto fail;
+	}
+
+	bool found = false;
+	for (size_t i = 0; i < ARRAY_SIZE(kinetis_known_mdm_ids); i++) {
+		if (val == kinetis_known_mdm_ids[i]) {
+			found = true;
+			break;
+		}
+	}
+
+	if (!found)
+		LOG_WARNING("MDM: unknown ID %08" PRIX32, val);
+
+	/*
+	 * ... Read the MDM-AP status register until the Flash Ready bit sets...
+	 */
+	retval = kinetis_mdm_poll_register(dap, MDM_REG_STAT,
+					   MDM_STAT_FREADY,
+					   MDM_STAT_FREADY);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("MDM: flash ready timeout");
+		goto fail;
+	}
+
+	/*
+	 * ... Read the System Security bit to determine if security is enabled.
+	 * If System Security = 0, then proceed. If System Security = 1, then
+	 * communication with the internals of the processor, including the
+	 * flash, will not be possible without issuing a mass erase command or
+	 * unsecuring the part through other means (backdoor key unlock)...
+	 */
+	retval = kinetis_mdm_read_register(dap, MDM_REG_STAT, &val);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("MDM: failed to read MDM_REG_STAT");
+		goto fail;
+	}
+
+	if (val & MDM_STAT_SYSSEC) {
+		jtag_poll_set_enabled(false);
+
+		LOG_WARNING("*********** ATTENTION! ATTENTION! ATTENTION! ATTENTION! **********");
+		LOG_WARNING("****                                                          ****");
+		LOG_WARNING("**** Your Kinetis MCU is in secured state, which means that,  ****");
+		LOG_WARNING("**** with exception for very basic communication, JTAG/SWD    ****");
+		LOG_WARNING("**** interface will NOT work. In order to restore its         ****");
+		LOG_WARNING("**** functionality please issue 'kinetis mdm mass_erase'      ****");
+		LOG_WARNING("**** command, power cycle the MCU and restart OpenOCD.        ****");
+		LOG_WARNING("****                                                          ****");
+		LOG_WARNING("*********** ATTENTION! ATTENTION! ATTENTION! ATTENTION! **********");
+	} else {
+		LOG_INFO("MDM: Chip is unsecured. Continuing.");
+		jtag_poll_set_enabled(true);
+	}
+
+	dap_ap_select(dap, origninal_ap);
+
+	return ERROR_OK;
+
+fail:
+	LOG_ERROR("MDM: Failed to check security status of the MCU. Cannot proceed further");
+	jtag_poll_set_enabled(false);
+	return retval;
+}
+
 FLASH_BANK_COMMAND_HANDLER(kinetis_flash_bank_command)
 {
 	struct kinetis_flash_bank *bank_info;
@@ -512,36 +797,26 @@ static int kinetis_ftfx_command(struct flash_bank *bank, uint8_t fcmd, uint32_t
 	return ERROR_OK;
 }
 
-static int kinetis_mass_erase(struct flash_bank *bank)
+COMMAND_HANDLER(kinetis_securing_test)
 {
 	int result;
 	uint8_t ftfx_fstat;
+	struct target *target = get_current_target(CMD_CTX);
+	struct flash_bank *bank = NULL;
 
-	if (bank->target->state != TARGET_HALTED) {
+	result = get_flash_bank_by_addr(target, 0x00000000, true, &bank);
+	if (result != ERROR_OK)
+		return result;
+
+	assert(bank != NULL);
+
+	if (target->state != TARGET_HALTED) {
 		LOG_ERROR("Target not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
-	/* check if whole bank is blank */
-	LOG_INFO("Execute Erase All Blocks");
-	/* set command and sector address */
-	result = kinetis_ftfx_command(bank, FTFx_CMD_MASSERASE, 0,
-					    0, 0, 0, 0,  0, 0, 0, 0,  &ftfx_fstat);
-	/* Anyway Result, write FSEC to unsecure forcely */
-	/*	if (result != ERROR_OK)
-		return result;*/
-
-	/* Write to MCU security status unsecure in Flash security byte(for Kinetis-L need) */
-	LOG_INFO("Write to MCU security status unsecure Anyway!");
-	uint8_t padding[4] = {0xFE, 0xFF, 0xFF, 0xFF}; /* Write 0xFFFFFFFE */
-
-	result = kinetis_ftfx_command(bank, FTFx_CMD_LWORDPROG, (bank->base + 0x0000040C),
-				padding[3], padding[2], padding[1], padding[0],
-				0, 0, 0, 0,  &ftfx_fstat);
-	if (result != ERROR_OK)
-		return ERROR_FLASH_OPERATION_FAILED;
-
-	return ERROR_OK;
+	return kinetis_ftfx_command(bank, FTFx_CMD_SECTERASE, bank->base + 0x00000400,
+				      0, 0, 0, 0,  0, 0, 0, 0,  &ftfx_fstat);
 }
 
 static int kinetis_erase(struct flash_bank *bank, int first, int last)
@@ -556,9 +831,6 @@ static int kinetis_erase(struct flash_bank *bank, int first, int last)
 	if ((first > bank->num_sectors) || (last > bank->num_sectors))
 		return ERROR_FLASH_OPERATION_FAILED;
 
-	if ((first == 0) && (last == (bank->num_sectors - 1)))
-		return kinetis_mass_erase(bank);
-
 	/*
 	 * FIXME: TODO: use the 'Erase Flash Block' command if the
 	 * requested erase is PFlash or NVM and encompasses the entire
@@ -1150,8 +1422,58 @@ static int kinetis_blank_check(struct flash_bank *bank)
 	return ERROR_OK;
 }
 
+static const struct command_registration kinetis_securtiy_command_handlers[] = {
+	{
+		.name = "check_security",
+		.mode = COMMAND_EXEC,
+		.help = "",
+		.usage = "",
+		.handler = kinetis_check_flash_security_status,
+	},
+	{
+		.name = "mass_erase",
+		.mode = COMMAND_EXEC,
+		.help = "",
+		.usage = "",
+		.handler = kinetis_mdm_mass_erase,
+	},
+	{
+		.name = "test_securing",
+		.mode = COMMAND_EXEC,
+		.help = "",
+		.usage = "",
+		.handler = kinetis_securing_test,
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+static const struct command_registration kinetis_exec_command_handlers[] = {
+	{
+		.name = "mdm",
+		.mode = COMMAND_ANY,
+		.help = "",
+		.usage = "",
+		.chain = kinetis_securtiy_command_handlers,
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+static const struct command_registration kinetis_command_handler[] = {
+	{
+		.name = "kinetis",
+		.mode = COMMAND_ANY,
+		.help = "kinetis NAND flash controller commands",
+		.usage = "",
+		.chain = kinetis_exec_command_handlers,
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+
+
 struct flash_driver kinetis_flash = {
 	.name = "kinetis",
+	.commands = kinetis_command_handler,
 	.flash_bank_command = kinetis_flash_bank_command,
 	.erase = kinetis_erase,
 	.protect = kinetis_protect,

src/flash/nor/lpc2000.c

@@ -5,6 +5,13 @@
  *   LPC1700 support Copyright (C) 2009 by Audrius Urmanavicius            *
  *   didele.deze@gmail.com                                                 *
  *                                                                         *
+ *   LPC1100 variant and auto-probing support Copyright (C) 2014           *
+ *   by Cosmin Gorgovan cosmin [at] linux-geek [dot] org                   *
+ *                                                                         *
+ *   LPC800/LPC1500/LPC54100 support Copyright (C) 2013/2014               *
+ *   by Nemui Trinomius                                                    *
+ *   nemuisan_kawausogasuki@live.jp                                        *
+ *                                                                         *
  *   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     *
@@ -33,7 +40,7 @@
 
 /**
  * @file
- * flash programming support for NXP LPC17xx and LPC2xxx devices.
+ * flash programming support for NXP LPC8xx,LPC1xxx,LPC4xxx,LP5410x and LPC2xxx 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).
@@ -58,36 +65,223 @@
  * lpc1700:
  * - 175x
  * - 176x (tested with LPC1768)
+ * - 177x
+ * - 178x (tested with LPC1788)
  *
- * lpc4300 (also available as lpc1800 - alias)
+ * lpc4000: (lpc1700's alias)
+ * - 407x
+ * - 408x (tested with LPC4088)
+ *
+ * lpc4300: (also available as lpc1800 - alias)
  * - 43x2 | 3 | 5 | 7 (tested with LPC4337/LPC4357)
  * - 18x2 | 3 | 5 | 7
  *
  * lpc800:
- * - 810 | 1 | 2 (tested with LPC810/LPC812)
+ * - 810 | 1 | 2 (tested with LPC810/LPC811/LPC812)
+ * - 822 | 4 (tested with LPC824)
+ *
+ * lpc1100:
+ * - 11xx
+ * - 11Axx
+ * - 11Cxx
+ * - 11Dxx
+ * - 11Exx
+ * - 11Uxx (tested with LPC11U34)
+ * - 131x
+ * - 134x
+ *
+ * lpc1500:
+ * - 15x7 | 8 | 9 (tested with LPC1549)
+ *
+ * lpc54100:
+ * - 54101 | 2 (tested with LPC54102)
+ *
+ * The auto variant auto-detects parts from the following series:
+ * - 11xx
+ * - 11Axx
+ * - 11Cxx
+ * - 11Dxx
+ * - 11Exx
+ * - 11Uxx
+ * - 131x
+ * - 134x
+ * - 175x
+ * - 176x
+ * - 177x
+ * - 178x
+ * - 407x
+ * - 408x
+ * - 81x
+ * - 82x
  */
 
+/* Part IDs for autodetection */
+/* A script which can automatically extract part ids from user manuals is available here:
+ * https://github.com/lgeek/lpc_part_ids
+ */
+#define LPC1110_1      0x0A07102B
+#define LPC1110_2      0x1A07102B
+#define LPC1111_002_1  0x0A16D02B
+#define LPC1111_002_2  0x1A16D02B
+#define LPC1111_101_1  0x041E502B
+#define LPC1111_101_2  0x2516D02B
+#define LPC1111_103_1  0x00010013
+#define LPC1111_201_1  0x0416502B
+#define LPC1111_201_2  0x2516902B
+#define LPC1111_203_1  0x00010012
+#define LPC1112_101_1  0x042D502B
+#define LPC1112_101_2  0x2524D02B
+#define LPC1112_102_1  0x0A24902B
+#define LPC1112_102_2  0x1A24902B
+#define LPC1112_103_1  0x00020023
+#define LPC1112_201_1  0x0425502B
+#define LPC1112_201_2  0x2524902B
+#define LPC1112_203_1  0x00020022
+#define LPC1113_201_1  0x0434502B
+#define LPC1113_201_2  0x2532902B
+#define LPC1113_203_1  0x00030032
+#define LPC1113_301_1  0x0434102B
+#define LPC1113_301_2  0x2532102B
+#define LPC1113_303_1  0x00030030
+#define LPC1114_102_1  0x0A40902B
+#define LPC1114_102_2  0x1A40902B
+#define LPC1114_201_1  0x0444502B
+#define LPC1114_201_2  0x2540902B
+#define LPC1114_203_1  0x00040042
+#define LPC1114_301_1  0x0444102B
+#define LPC1114_301_2  0x2540102B
+#define LPC1114_303_1  0x00040040
+#define LPC1114_323_1  0x00040060
+#define LPC1114_333_1  0x00040070
+#define LPC1115_303_1  0x00050080
+
+#define LPC11A02_1     0x4D4C802B
+#define LPC11A04_1     0x4D80002B
+#define LPC11A11_001_1 0x455EC02B
+#define LPC11A12_101_1 0x4574802B
+#define LPC11A13_201_1 0x458A402B
+#define LPC11A14_301_1 0x35A0002B
+#define LPC11A14_301_2 0x45A0002B
+
+#define LPC11C12_301_1 0x1421102B
+#define LPC11C14_301_1 0x1440102B
+#define LPC11C22_301_1 0x1431102B
+#define LPC11C24_301_1 0x1430102B
+
+#define LPC11E11_101   0x293E902B
+#define LPC11E12_201   0x2954502B
+#define LPC11E13_301   0x296A102B
+#define LPC11E14_401   0x2980102B
+#define LPC11E36_501   0x00009C41
+#define LPC11E37_401   0x00007C45
+#define LPC11E37_501   0x00007C41
+
+#define LPC11U12_201_1 0x095C802B
+#define LPC11U12_201_2 0x295C802B
+#define LPC11U13_201_1 0x097A802B
+#define LPC11U13_201_2 0x297A802B
+#define LPC11U14_201_1 0x0998802B
+#define LPC11U14_201_2 0x2998802B
+#define LPC11U23_301   0x2972402B
+#define LPC11U24_301   0x2988402B
+#define LPC11U24_401   0x2980002B
+#define LPC11U34_311   0x0003D440
+#define LPC11U34_421   0x0001CC40
+#define LPC11U35_401   0x0001BC40
+#define LPC11U35_501   0x0000BC40
+#define LPC11U36_401   0x00019C40
+#define LPC11U37_401   0x00017C40
+#define LPC11U37H_401  0x00007C44
+#define LPC11U37_501   0x00007C40
+
+#define LPC11E66       0x0000DCC1
+#define LPC11E67       0x0000BC81
+#define LPC11E68       0x00007C01
+
+#define LPC11U66       0x0000DCC8
+#define LPC11U67_1     0x0000BC88
+#define LPC11U67_2     0x0000BC80
+#define LPC11U68_1     0x00007C08
+#define LPC11U68_2     0x00007C00
+
+#define LPC1311        0x2C42502B
+#define LPC1311_1      0x1816902B
+#define LPC1313        0x2C40102B
+#define LPC1313_1      0x1830102B
+#define LPC1315        0x3A010523
+#define LPC1316        0x1A018524
+#define LPC1317        0x1A020525
+#define LPC1342        0x3D01402B
+#define LPC1343        0x3D00002B
+#define LPC1345        0x28010541
+#define LPC1346        0x08018542
+#define LPC1347        0x08020543
+
+#define LPC1751_1      0x25001110
+#define LPC1751_2      0x25001118
+#define LPC1752        0x25001121
+#define LPC1754        0x25011722
+#define LPC1756        0x25011723
+#define LPC1758        0x25013F37
+#define LPC1759        0x25113737
+#define LPC1763        0x26012033
+#define LPC1764        0x26011922
+#define LPC1765        0x26013733
+#define LPC1766        0x26013F33
+#define LPC1767        0x26012837
+#define LPC1768        0x26013F37
+#define LPC1769        0x26113F37
+#define LPC1774        0x27011132
+#define LPC1776        0x27191F43
+#define LPC1777        0x27193747
+#define LPC1778        0x27193F47
+#define LPC1785        0x281D1743
+#define LPC1786        0x281D1F43
+#define LPC1787        0x281D3747
+#define LPC1788        0x281D3F47
+
+#define LPC4072        0x47011121
+#define LPC4074        0x47011132
+#define LPC4076        0x47191F43
+#define LPC4078        0x47193F47
+#define LPC4088        0x481D3F47
+
+#define LPC810_021     0x00008100
+#define LPC811_001     0x00008110
+#define LPC812_101     0x00008120
+#define LPC812_101_1   0x00008121
+#define LPC812_101_2   0x00008122
+#define LPC812_101_3   0x00008123
+
+#define LPC822_101     0x00008221
+#define LPC822_101_1   0x00008222
+#define LPC824_201     0x00008241
+#define LPC824_201_1   0x00008242
+
+#define IAP_CODE_LEN 0x34
+
 typedef enum {
 	lpc2000_v1,
 	lpc2000_v2,
 	lpc1700,
 	lpc4300,
 	lpc800,
+	lpc1100,
+	lpc1500,
+	lpc54100,
+	lpc_auto,
 } lpc2000_variant;
 
 struct lpc2000_flash_bank {
 	lpc2000_variant variant;
 	uint32_t cclk;
 	int cmd51_dst_boundary;
-	int cmd51_can_64b;
-	int cmd51_can_256b;
-	int cmd51_can_8192b;
 	int calc_checksum;
 	uint32_t cmd51_max_buffer;
 	int checksum_vector;
 	uint32_t iap_max_stack;
-	uint32_t cmd51_src_offset;
 	uint32_t lpc4300_bank;
+	bool probed;
 };
 
 enum lpc2000_status_codes {
@@ -125,6 +319,10 @@ static int lpc2000_build_sector_list(struct flash_bank *bank)
 	lpc2000_info->cmd51_max_buffer = 4096;
 
 	if (lpc2000_info->variant == lpc2000_v1) {
+		lpc2000_info->cmd51_dst_boundary = 512;
+		lpc2000_info->checksum_vector = 5;
+		lpc2000_info->iap_max_stack = 128;
+
 		/* variant 1 has different layout for 128kb and 256kb flashes */
 		if (bank->size == 128 * 1024) {
 			bank->num_sectors = 16;
@@ -166,6 +364,10 @@ static int lpc2000_build_sector_list(struct flash_bank *bank)
 			exit(-1);
 		}
 	} else if (lpc2000_info->variant == lpc2000_v2) {
+		lpc2000_info->cmd51_dst_boundary = 256;
+		lpc2000_info->checksum_vector = 5;
+		lpc2000_info->iap_max_stack = 128;
+
 		/* variant 2 has a uniform layout, only number of sectors differs */
 		switch (bank->size) {
 			case 4 * 1024:
@@ -228,6 +430,10 @@ static int lpc2000_build_sector_list(struct flash_bank *bank)
 			}
 		}
 	} else if (lpc2000_info->variant == lpc1700) {
+		lpc2000_info->cmd51_dst_boundary = 256;
+		lpc2000_info->checksum_vector = 7;
+		lpc2000_info->iap_max_stack = 128;
+
 		switch (bank->size) {
 			case 4 * 1024:
 				lpc2000_info->cmd51_max_buffer = 256;
@@ -266,13 +472,17 @@ static int lpc2000_build_sector_list(struct flash_bank *bank)
 
 		for (int i = 0; i < bank->num_sectors; i++) {
 			bank->sectors[i].offset = offset;
-			/* sectors 0-15 are 4kB-sized, 16 and above are 32kB-sized for LPC17xx devices */
+			/* sectors 0-15 are 4kB-sized, 16 and above are 32kB-sized for LPC17xx/LPC40xx devices */
 			bank->sectors[i].size = (i < 16) ? 4 * 1024 : 32 * 1024;
 			offset += bank->sectors[i].size;
 			bank->sectors[i].is_erased = -1;
 			bank->sectors[i].is_protected = 1;
 		}
 	} else if (lpc2000_info->variant == lpc4300) {
+		lpc2000_info->cmd51_dst_boundary = 512;
+		lpc2000_info->checksum_vector = 7;
+		lpc2000_info->iap_max_stack = 208;
+
 		switch (bank->size) {
 			case 256 * 1024:
 				bank->num_sectors = 11;
@@ -299,20 +509,108 @@ static int lpc2000_build_sector_list(struct flash_bank *bank)
 			bank->sectors[i].is_protected = 1;
 		}
 
-	}  else if (lpc2000_info->variant == lpc800) {
-			lpc2000_info->cmd51_max_buffer = 1024;
+	} else if (lpc2000_info->variant == lpc800) {
+		lpc2000_info->cmd51_dst_boundary = 64;
+		lpc2000_info->checksum_vector = 7;
+		lpc2000_info->iap_max_stack = 208;		/* 148byte for LPC81x,208byte for LPC82x. */
+		lpc2000_info->cmd51_max_buffer = 256;	/* smallest MCU in the series, LPC810, has 1 kB of SRAM */
+
 		switch (bank->size) {
 			case 4 * 1024:
-				lpc2000_info->cmd51_max_buffer = 256;
 				bank->num_sectors = 4;
 				break;
 			case 8 * 1024:
-				lpc2000_info->cmd51_max_buffer = 512;
 				bank->num_sectors = 8;
 				break;
 			case 16 * 1024:
 				bank->num_sectors = 16;
 				break;
+			case 32 * 1024:
+				lpc2000_info->cmd51_max_buffer = 1024; /* For LPC824, has 8kB of SRAM */
+				bank->num_sectors = 32;
+				break;
+			default:
+				LOG_ERROR("BUG: unknown bank->size encountered");
+				exit(-1);
+		}
+
+		bank->sectors = malloc(sizeof(struct flash_sector) * bank->num_sectors);
+
+		for (int i = 0; i < bank->num_sectors; i++) {
+			bank->sectors[i].offset = offset;
+			/* all sectors are 1kB-sized for LPC8xx devices */
+			bank->sectors[i].size = 1 * 1024;
+			offset += bank->sectors[i].size;
+			bank->sectors[i].is_erased = -1;
+			bank->sectors[i].is_protected = 1;
+		}
+
+	} else if (lpc2000_info->variant == lpc1100) {
+		lpc2000_info->cmd51_dst_boundary = 256;
+		lpc2000_info->checksum_vector = 7;
+		lpc2000_info->iap_max_stack = 128;
+
+		if ((bank->size % (4 * 1024)) != 0) {
+			LOG_ERROR("BUG: unknown bank->size encountered,\nLPC1100 flash size must be a multiple of 4096");
+			exit(-1);
+		}
+		lpc2000_info->cmd51_max_buffer = 512; /* smallest MCU in the series, LPC1110, has 1 kB of SRAM */
+		bank->num_sectors = bank->size / 4096;
+
+		bank->sectors = malloc(sizeof(struct flash_sector) * bank->num_sectors);
+
+		for (int i = 0; i < bank->num_sectors; i++) {
+			bank->sectors[i].offset = offset;
+			/* all sectors are 4kB-sized */
+			bank->sectors[i].size = 4 * 1024;
+			offset += bank->sectors[i].size;
+			bank->sectors[i].is_erased = -1;
+			bank->sectors[i].is_protected = 1;
+		}
+
+	} else if (lpc2000_info->variant == lpc1500) {
+		lpc2000_info->cmd51_dst_boundary = 256;
+		lpc2000_info->checksum_vector = 7;
+		lpc2000_info->iap_max_stack = 128;
+
+		switch (bank->size) {
+			case 64 * 1024:
+				bank->num_sectors = 16;
+				break;
+			case 128 * 1024:
+				bank->num_sectors = 32;
+				break;
+			case 256 * 1024:
+				bank->num_sectors = 64;
+				break;
+			default:
+				LOG_ERROR("BUG: unknown bank->size encountered");
+				exit(-1);
+		}
+
+		bank->sectors = malloc(sizeof(struct flash_sector) * bank->num_sectors);
+
+		for (int i = 0; i < bank->num_sectors; i++) {
+			bank->sectors[i].offset = offset;
+			/* all sectors are 4kB-sized */
+			bank->sectors[i].size = 4 * 1024;
+			offset += bank->sectors[i].size;
+			bank->sectors[i].is_erased = -1;
+			bank->sectors[i].is_protected = 1;
+		}
+
+	} else if (lpc2000_info->variant == lpc54100) {
+		lpc2000_info->cmd51_dst_boundary = 256;
+		lpc2000_info->checksum_vector = 7;
+		lpc2000_info->iap_max_stack = 128;
+
+		switch (bank->size) {
+			case 256 * 1024:
+				bank->num_sectors = 8;
+				break;
+			case 512 * 1024:
+				bank->num_sectors = 16;
+				break;
 			default:
 				LOG_ERROR("BUG: unknown bank->size encountered");
 				exit(-1);
@@ -322,8 +620,8 @@ static int lpc2000_build_sector_list(struct flash_bank *bank)
 
 		for (int i = 0; i < bank->num_sectors; i++) {
 			bank->sectors[i].offset = offset;
-			/* sectors 0-15 are 1kB-sized for LPC8xx devices */
-			bank->sectors[i].size = 1 * 1024;
+			/* all sectors are 32kB-sized */
+			bank->sectors[i].size = 32 * 1024;
 			offset += bank->sectors[i].size;
 			bank->sectors[i].is_erased = -1;
 			bank->sectors[i].is_protected = 1;
@@ -342,7 +640,8 @@ static int lpc2000_build_sector_list(struct flash_bank *bank)
  * 0x0 to 0x7: jump gate (BX to thumb state, b -2 to wait)
  * 0x8 to 0x1f: command parameter table (1+5 words)
  * 0x20 to 0x33: command result table (1+4 words)
- * 0x34 to 0xb3|0x104: stack (only 128b needed for lpc17xx/2000, 208 for lpc43xx and 148b for lpc8xx)
+ * 0x34 to 0xb3|0x104: stack
+ *        (128b needed for lpc1xxx/2000/5410x, 208b for lpc43xx/lpc82x and 148b for lpc81x)
  */
 
 static int lpc2000_iap_working_area_init(struct flash_bank *bank, struct working_area **iap_working_area)
@@ -350,7 +649,7 @@ static int lpc2000_iap_working_area_init(struct flash_bank *bank, struct working
 	struct target *target = bank->target;
 	struct lpc2000_flash_bank *lpc2000_info = bank->driver_priv;
 
-	if (target_alloc_working_area(target, 0x34 + lpc2000_info->iap_max_stack, iap_working_area) != ERROR_OK) {
+	if (target_alloc_working_area(target, IAP_CODE_LEN + lpc2000_info->iap_max_stack, iap_working_area) != ERROR_OK) {
 		LOG_ERROR("no working area specified, can't write LPC2000 internal flash");
 		return ERROR_FLASH_OPERATION_FAILED;
 	}
@@ -360,8 +659,12 @@ static int lpc2000_iap_working_area_init(struct flash_bank *bank, struct working
 	/* write IAP code to working area */
 	switch (lpc2000_info->variant) {
 		case lpc800:
+		case lpc1100:
+		case lpc1500:
 		case lpc1700:
 		case lpc4300:
+		case lpc54100:
+		case lpc_auto:
 			target_buffer_set_u32(target, jump_gate, ARMV4_5_T_BX(12));
 			target_buffer_set_u32(target, jump_gate + 4, ARMV5_T_BKPT(0));
 			break;
@@ -376,14 +679,16 @@ static int lpc2000_iap_working_area_init(struct flash_bank *bank, struct working
 	}
 
 	int retval = target_write_memory(target, (*iap_working_area)->address, 4, 2, jump_gate);
-	if (retval != ERROR_OK)
+	if (retval != ERROR_OK) {
 		LOG_ERROR("Write memory at address 0x%8.8" PRIx32 " failed (check work_area definition)",
 				(*iap_working_area)->address);
+		target_free_working_area(target, *iap_working_area);
+	}
 
 	return retval;
 }
 
-/* call LPC1700/LPC2000 IAP function */
+/* call LPC8xx/LPC1xxx/LPC4xxx/LPC5410x/LPC2000 IAP function */
 
 static int lpc2000_iap_call(struct flash_bank *bank, struct working_area *iap_working_area, int code,
 		uint32_t param_table[5], uint32_t result_table[4])
@@ -392,16 +697,24 @@ static int lpc2000_iap_call(struct flash_bank *bank, struct working_area *iap_wo
 	struct target *target = bank->target;
 
 	struct arm_algorithm arm_algo;	/* for LPC2000 */
-	struct armv7m_algorithm armv7m_info;	/* for LPC1700 */
+	struct armv7m_algorithm armv7m_info;	/* for LPC8xx/LPC1xxx/LPC4xxx/LPC5410x */
 	uint32_t iap_entry_point = 0;	/* to make compiler happier */
 
 	switch (lpc2000_info->variant) {
 		case lpc800:
+		case lpc1100:
 		case lpc1700:
+		case lpc_auto:
 			armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
 			armv7m_info.core_mode = ARM_MODE_THREAD;
 			iap_entry_point = 0x1fff1ff1;
 			break;
+		case lpc1500:
+		case lpc54100:
+			armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
+			armv7m_info.core_mode = ARM_MODE_THREAD;
+			iap_entry_point = 0x03000205;
+			break;
 		case lpc2000_v1:
 		case lpc2000_v2:
 			arm_algo.common_magic = ARM_COMMON_MAGIC;
@@ -448,11 +761,16 @@ static int lpc2000_iap_call(struct flash_bank *bank, struct working_area *iap_wo
 
 	switch (lpc2000_info->variant) {
 		case lpc800:
+		case lpc1100:
+		case lpc1500:
 		case lpc1700:
 		case lpc4300:
+		case lpc54100:
+		case lpc_auto:
 			/* IAP stack */
 			init_reg_param(&reg_params[3], "sp", 32, PARAM_OUT);
-			buf_set_u32(reg_params[3].value, 0, 32, iap_working_area->address + lpc2000_info->cmd51_src_offset);
+			buf_set_u32(reg_params[3].value, 0, 32,
+				iap_working_area->address + IAP_CODE_LEN + lpc2000_info->iap_max_stack);
 
 			/* return address */
 			init_reg_param(&reg_params[4], "lr", 32, PARAM_OUT);
@@ -466,7 +784,8 @@ static int lpc2000_iap_call(struct flash_bank *bank, struct working_area *iap_wo
 		case lpc2000_v2:
 			/* IAP stack */
 			init_reg_param(&reg_params[3], "sp_svc", 32, PARAM_OUT);
-			buf_set_u32(reg_params[3].value, 0, 32, iap_working_area->address + lpc2000_info->cmd51_src_offset);
+			buf_set_u32(reg_params[3].value, 0, 32,
+				iap_working_area->address + IAP_CODE_LEN + lpc2000_info->iap_max_stack);
 
 			/* return address */
 			init_reg_param(&reg_params[4], "lr_svc", 32, PARAM_OUT);
@@ -562,57 +881,43 @@ FLASH_BANK_COMMAND_HANDLER(lpc2000_flash_bank_command)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
 	struct lpc2000_flash_bank *lpc2000_info = calloc(1, sizeof(*lpc2000_info));
+	lpc2000_info->probed = false;
 
 	bank->driver_priv = lpc2000_info;
 
 	if (strcmp(CMD_ARGV[6], "lpc2000_v1") == 0) {
 		lpc2000_info->variant = lpc2000_v1;
-		lpc2000_info->cmd51_dst_boundary = 512;
-		lpc2000_info->cmd51_can_256b = 0;
-		lpc2000_info->cmd51_can_8192b = 1;
-		lpc2000_info->checksum_vector = 5;
-		lpc2000_info->iap_max_stack = 128;
 	} else if (strcmp(CMD_ARGV[6], "lpc2000_v2") == 0) {
 		lpc2000_info->variant = lpc2000_v2;
-		lpc2000_info->cmd51_dst_boundary = 256;
-		lpc2000_info->cmd51_can_256b = 1;
-		lpc2000_info->cmd51_can_8192b = 0;
-		lpc2000_info->checksum_vector = 5;
-		lpc2000_info->iap_max_stack = 128;
-	} else if (strcmp(CMD_ARGV[6], "lpc1700") == 0) {
+	} else if (strcmp(CMD_ARGV[6], "lpc1700") == 0 || strcmp(CMD_ARGV[6], "lpc4000") == 0) {
 		lpc2000_info->variant = lpc1700;
-		lpc2000_info->cmd51_dst_boundary = 256;
-		lpc2000_info->cmd51_can_256b = 1;
-		lpc2000_info->cmd51_can_8192b = 0;
-		lpc2000_info->checksum_vector = 7;
-		lpc2000_info->iap_max_stack = 128;
 	} else if (strcmp(CMD_ARGV[6], "lpc1800") == 0 || strcmp(CMD_ARGV[6], "lpc4300") == 0) {
 		lpc2000_info->variant = lpc4300;
-		lpc2000_info->cmd51_dst_boundary = 512;
-		lpc2000_info->cmd51_can_256b = 0;
-		lpc2000_info->cmd51_can_8192b = 0;
-		lpc2000_info->checksum_vector = 7;
-		lpc2000_info->iap_max_stack = 208;
 	} else if (strcmp(CMD_ARGV[6], "lpc800") == 0) {
 		lpc2000_info->variant = lpc800;
-		lpc2000_info->cmd51_dst_boundary = 64;
-		lpc2000_info->cmd51_can_64b = 1;
-		lpc2000_info->cmd51_can_256b = 0;
-		lpc2000_info->cmd51_can_8192b = 0;
-		lpc2000_info->checksum_vector = 7;
-		lpc2000_info->iap_max_stack = 148;
+	} else if (strcmp(CMD_ARGV[6], "lpc1100") == 0) {
+		lpc2000_info->variant = lpc1100;
+	} else if (strcmp(CMD_ARGV[6], "lpc1500") == 0) {
+		lpc2000_info->variant = lpc1500;
+	} else if (strcmp(CMD_ARGV[6], "lpc54100") == 0) {
+		lpc2000_info->variant = lpc54100;
+	} else if (strcmp(CMD_ARGV[6], "auto") == 0) {
+		lpc2000_info->variant = lpc_auto;
 	} else {
 		LOG_ERROR("unknown LPC2000 variant: %s", CMD_ARGV[6]);
 		free(lpc2000_info);
 		return ERROR_FLASH_BANK_INVALID;
 	}
 
-	/* see lpc2000_iap_working_area_init() for the reason behind the 0x34 value */
-	lpc2000_info->cmd51_src_offset = 0x34 + lpc2000_info->iap_max_stack;
+	/* Maximum size required for the IAP stack.
+	   This value only gets used when probing, only for auto, lpc1100 and lpc1700.
+	   We use the maximum size for any part supported by the driver(!) to be safe
+	   in case the auto variant is mistakenly used on a MCU from one of the series
+	   for which we don't support auto-probing. */
+	lpc2000_info->iap_max_stack = 208;
 
 	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[7], lpc2000_info->cclk);
 	lpc2000_info->calc_checksum = 0;
-	lpc2000_build_sector_list(bank);
 
 	uint32_t temp_base = 0;
 	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], temp_base);
@@ -797,14 +1102,8 @@ static int lpc2000_write(struct flash_bank *bank, const uint8_t *buffer, uint32_
 		uint32_t thisrun_bytes;
 		if (bytes_remaining >= lpc2000_info->cmd51_max_buffer)
 			thisrun_bytes = lpc2000_info->cmd51_max_buffer;
-		else if (bytes_remaining >= 1024)
-			thisrun_bytes = 1024;
-		else if ((bytes_remaining >= 512) || (!lpc2000_info->cmd51_can_256b))
-			thisrun_bytes = 512;
-		else if ((bytes_remaining >= 256) || (!lpc2000_info->cmd51_can_64b))
-			thisrun_bytes = 256;
 		else
-			thisrun_bytes = 64;
+			thisrun_bytes = lpc2000_info->cmd51_dst_boundary;
 
 		/* Prepare sectors */
 		param_table[0] = first_sector;
@@ -890,9 +1189,296 @@ static int lpc2000_write(struct flash_bank *bank, const uint8_t *buffer, uint32_
 	return retval;
 }
 
+static int get_lpc2000_part_id(struct flash_bank *bank, uint32_t *part_id)
+{
+	if (bank->target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	uint32_t param_table[5] = {0};
+	uint32_t result_table[4];
+	struct working_area *iap_working_area;
+
+	int retval = lpc2000_iap_working_area_init(bank, &iap_working_area);
+
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* The status seems to be bogus with the part ID command on some IAP
+	   firmwares, so ignore it. */
+	lpc2000_iap_call(bank, iap_working_area, 54, param_table, result_table);
+
+	struct target *target = bank->target;
+	target_free_working_area(target, iap_working_area);
+
+	/* If the result is zero, the command probably didn't work out. */
+	if (result_table[0] == 0)
+		return LPC2000_INVALID_COMMAND;
+
+	*part_id = result_table[0];
+	return LPC2000_CMD_SUCCESS;
+}
+
+static int lpc2000_auto_probe_flash(struct flash_bank *bank)
+{
+	uint32_t part_id;
+	int retval;
+	struct lpc2000_flash_bank *lpc2000_info = bank->driver_priv;
+
+	if (bank->target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	retval = get_lpc2000_part_id(bank, &part_id);
+	if (retval != LPC2000_CMD_SUCCESS) {
+		LOG_ERROR("Could not get part ID");
+		return retval;
+	}
+
+	switch (part_id) {
+		case LPC1110_1:
+		case LPC1110_2:
+			lpc2000_info->variant = lpc1100;
+			bank->size = 4 * 1024;
+			break;
+
+		case LPC1111_002_1:
+		case LPC1111_002_2:
+		case LPC1111_101_1:
+		case LPC1111_101_2:
+		case LPC1111_103_1:
+		case LPC1111_201_1:
+		case LPC1111_201_2:
+		case LPC1111_203_1:
+		case LPC11A11_001_1:
+		case LPC11E11_101:
+		case LPC1311:
+		case LPC1311_1:
+			lpc2000_info->variant = lpc1100;
+			bank->size = 8 * 1024;
+			break;
+
+		case LPC1112_101_1:
+		case LPC1112_101_2:
+		case LPC1112_102_1:
+		case LPC1112_102_2:
+		case LPC1112_103_1:
+		case LPC1112_201_1:
+		case LPC1112_201_2:
+		case LPC1112_203_1:
+		case LPC11A02_1:
+		case LPC11C12_301_1:
+		case LPC11C22_301_1:
+		case LPC11A12_101_1:
+		case LPC11E12_201:
+		case LPC11U12_201_1:
+		case LPC11U12_201_2:
+		case LPC1342:
+			lpc2000_info->variant = lpc1100;
+			bank->size = 16 * 1024;
+			break;
+
+		case LPC1113_201_1:
+		case LPC1113_201_2:
+		case LPC1113_203_1:
+		case LPC1113_301_1:
+		case LPC1113_301_2:
+		case LPC1113_303_1:
+		case LPC11A13_201_1:
+		case LPC11E13_301:
+		case LPC11U13_201_1:
+		case LPC11U13_201_2:
+		case LPC11U23_301:
+			lpc2000_info->variant = lpc1100;
+			bank->size = 24 * 1024;
+			break;
+
+		case LPC1114_102_1:
+		case LPC1114_102_2:
+		case LPC1114_201_1:
+		case LPC1114_201_2:
+		case LPC1114_203_1:
+		case LPC1114_301_1:
+		case LPC1114_301_2:
+		case LPC1114_303_1:
+		case LPC11A04_1:
+		case LPC11A14_301_1:
+		case LPC11A14_301_2:
+		case LPC11C14_301_1:
+		case LPC11C24_301_1:
+		case LPC11E14_401:
+		case LPC11U14_201_1:
+		case LPC11U14_201_2:
+		case LPC11U24_301:
+		case LPC11U24_401:
+		case LPC1313:
+		case LPC1313_1:
+		case LPC1315:
+		case LPC1343:
+		case LPC1345:
+			lpc2000_info->variant = lpc1100;
+			bank->size = 32 * 1024;
+			break;
+
+		case LPC1751_1:
+		case LPC1751_2:
+			lpc2000_info->variant = lpc1700;
+			bank->size = 32 * 1024;
+			break;
+
+		case LPC11U34_311:
+			lpc2000_info->variant = lpc1100;
+			bank->size = 40 * 1024;
+			break;
+
+		case LPC1114_323_1:
+		case LPC11U34_421:
+		case LPC1316:
+		case LPC1346:
+			lpc2000_info->variant = lpc1100;
+			bank->size = 48 * 1024;
+			break;
+
+		case LPC1114_333_1:
+			lpc2000_info->variant = lpc1100;
+			bank->size = 56 * 1024;
+			break;
+
+		case LPC1115_303_1:
+		case LPC11U35_401:
+		case LPC11U35_501:
+		case LPC11E66:
+		case LPC11U66:
+		case LPC1317:
+		case LPC1347:
+			lpc2000_info->variant = lpc1100;
+			bank->size = 64 * 1024;
+			break;
+
+		case LPC1752:
+		case LPC4072:
+			lpc2000_info->variant = lpc1700;
+			bank->size = 64 * 1024;
+			break;
+
+		case LPC11E36_501:
+		case LPC11U36_401:
+			lpc2000_info->variant = lpc1100;
+			bank->size = 96 * 1024;
+			break;
+
+		case LPC11E37_401:
+		case LPC11E37_501:
+		case LPC11U37_401:
+		case LPC11U37H_401:
+		case LPC11U37_501:
+		case LPC11E67:
+		case LPC11E68:
+		case LPC11U67_1:
+		case LPC11U67_2:
+			lpc2000_info->variant = lpc1100;
+			bank->size = 128 * 1024;
+			break;
+
+		case LPC1754:
+		case LPC1764:
+		case LPC1774:
+		case LPC4074:
+			lpc2000_info->variant = lpc1700;
+			bank->size = 128 * 1024;
+			break;
+
+		case LPC11U68_1:
+		case LPC11U68_2:
+			lpc2000_info->variant = lpc1100;
+			bank->size = 256 * 1024;
+			break;
+
+		case LPC1756:
+		case LPC1763:
+		case LPC1765:
+		case LPC1766:
+		case LPC1776:
+		case LPC1785:
+		case LPC1786:
+		case LPC4076:
+			lpc2000_info->variant = lpc1700;
+			bank->size = 256 * 1024;
+			break;
+
+		case LPC1758:
+		case LPC1759:
+		case LPC1767:
+		case LPC1768:
+		case LPC1769:
+		case LPC1777:
+		case LPC1778:
+		case LPC1787:
+		case LPC1788:
+		case LPC4078:
+		case LPC4088:
+			lpc2000_info->variant = lpc1700;
+			bank->size = 512 * 1024;
+			break;
+
+		case LPC810_021:
+			lpc2000_info->variant = lpc800;
+			bank->size = 4 * 1024;
+			break;
+
+		case LPC811_001:
+			lpc2000_info->variant = lpc800;
+			bank->size = 8 * 1024;
+			break;
+
+		case LPC812_101:
+		case LPC812_101_1:
+		case LPC812_101_2:
+		case LPC812_101_3:
+		case LPC822_101:
+		case LPC822_101_1:
+			lpc2000_info->variant = lpc800;
+			bank->size = 16 * 1024;
+			break;
+
+		case LPC824_201:
+		case LPC824_201_1:
+			lpc2000_info->variant = lpc800;
+			bank->size = 32 * 1024;
+			break;
+
+		default:
+			LOG_ERROR("BUG: unknown Part ID encountered: 0x%" PRIx32, part_id);
+			exit(-1);
+	}
+
+	return ERROR_OK;
+}
+
 static int lpc2000_probe(struct flash_bank *bank)
 {
-	/* we can't probe on an lpc2000 if this is an lpc2xxx, it has the configured flash */
+	int status;
+	uint32_t part_id;
+	struct lpc2000_flash_bank *lpc2000_info = bank->driver_priv;
+
+	if (!lpc2000_info->probed) {
+		if (lpc2000_info->variant == lpc_auto) {
+			status = lpc2000_auto_probe_flash(bank);
+			if (status != ERROR_OK)
+				return status;
+		} else if (lpc2000_info->variant == lpc1100 || lpc2000_info->variant == lpc1700) {
+			status = get_lpc2000_part_id(bank, &part_id);
+			if (status == LPC2000_CMD_SUCCESS)
+				LOG_INFO("If auto-detection fails for this part, please email "
+					"openocd-devel@lists.sourceforge.net, citing part id 0x%" PRIx32 ".\n", part_id);
+		}
+
+		lpc2000_build_sector_list(bank);
+		lpc2000_info->probed = true;
+	}
+
 	return ERROR_OK;
 }
 
@@ -937,23 +1523,15 @@ COMMAND_HANDLER(lpc2000_handle_part_id_command)
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
-	uint32_t param_table[5] = {0};
-	uint32_t result_table[4];
-	struct working_area *iap_working_area;
-
-	retval = lpc2000_iap_working_area_init(bank, &iap_working_area);
-
-	if (retval != ERROR_OK)
-		return retval;
-
-	int status_code = lpc2000_iap_call(bank, iap_working_area, 54, param_table, result_table);
+	uint32_t part_id;
+	int status_code = get_lpc2000_part_id(bank, &part_id);
 	if (status_code != 0x0) {
 		if (status_code == ERROR_FLASH_OPERATION_FAILED) {
 			command_print(CMD_CTX, "no sufficient working area specified, can't access LPC2000 IAP interface");
 		} else
 			command_print(CMD_CTX, "lpc2000 IAP returned status code %i", status_code);
 	} else
-		command_print(CMD_CTX, "lpc2000 part id: 0x%8.8" PRIx32, result_table[0]);
+		command_print(CMD_CTX, "lpc2000 part id: 0x%8.8" PRIx32, part_id);
 
 	return retval;
 }

src/flash/nor/lpcspifi.c

@@ -44,6 +44,10 @@
 #define SSP_PROBE_TIMEOUT (100)
 #define SSP_MAX_TIMEOUT  (3000)
 
+/* Size of the stack to alloc in the working area for the execution of
+ * the ROM spifi_init() function */
+#define SPIFI_INIT_STACK_SIZE  512
+
 struct lpcspifi_flash_bank {
 	int probed;
 	uint32_t ssp_base;
@@ -151,7 +155,7 @@ static int lpcspifi_set_hw_mode(struct flash_bank *bank)
 	uint32_t ssp_base = lpcspifi_info->ssp_base;
 	struct armv7m_algorithm armv7m_info;
 	struct working_area *spifi_init_algorithm;
-	struct reg_param reg_params[1];
+	struct reg_param reg_params[2];
 	int retval = ERROR_OK;
 
 	LOG_DEBUG("Uninitializing LPC43xx SSP");
@@ -187,13 +191,13 @@ static int lpcspifi_set_hw_mode(struct flash_bank *bank)
 
 	LOG_DEBUG("Allocating working area for SPIFI init algorithm");
 	/* Get memory for spifi initialization algorithm */
-	retval = target_alloc_working_area(target, sizeof(spifi_init_code),
-		&spifi_init_algorithm);
+	retval = target_alloc_working_area(target, sizeof(spifi_init_code)
+		+ SPIFI_INIT_STACK_SIZE, &spifi_init_algorithm);
 	if (retval != ERROR_OK) {
 		LOG_ERROR("Insufficient working area to initialize SPIFI "\
 			"module. You must allocate at least %zdB of working "\
 			"area in order to use this driver.",
-			sizeof(spifi_init_code)
+			sizeof(spifi_init_code) + SPIFI_INIT_STACK_SIZE
 		);
 
 		return retval;
@@ -214,6 +218,8 @@ static int lpcspifi_set_hw_mode(struct flash_bank *bank)
 	}
 
 	init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);		/* spifi clk speed */
+	/* the spifi_init() rom API makes use of the stack */
+	init_reg_param(&reg_params[1], "sp", 32, PARAM_OUT);
 
 	/* For now, the algorithm will set up the SPIFI module
 	 * @ the IRC clock speed. In the future, it could be made
@@ -221,10 +227,13 @@ static int lpcspifi_set_hw_mode(struct flash_bank *bank)
 	 * already configured them in order to speed up memory-
 	 * mapped reads. */
 	buf_set_u32(reg_params[0].value, 0, 32, 12);
+	/* valid stack pointer */
+	buf_set_u32(reg_params[1].value, 0, 32, (spifi_init_algorithm->address +
+		sizeof(spifi_init_code) + SPIFI_INIT_STACK_SIZE) & ~7UL);
 
 	/* Run the algorithm */
 	LOG_DEBUG("Running SPIFI init algorithm");
-	retval = target_run_algorithm(target, 0 , NULL, 1, reg_params,
+	retval = target_run_algorithm(target, 0 , NULL, 2, reg_params,
 		spifi_init_algorithm->address,
 		spifi_init_algorithm->address + sizeof(spifi_init_code) - 2,
 		1000, &armv7m_info);
@@ -235,6 +244,7 @@ static int lpcspifi_set_hw_mode(struct flash_bank *bank)
 	target_free_working_area(target, spifi_init_algorithm);
 
 	destroy_reg_param(&reg_params[0]);
+	destroy_reg_param(&reg_params[1]);
 
 	return retval;
 }
@@ -678,7 +688,8 @@ static int lpcspifi_write(struct flash_bank *bank, const uint8_t *buffer,
 		0x00, 0xf0, 0x02, 0xb8, 0x4f, 0xf0, 0x00, 0x08,
 		0x4f, 0xf4, 0x80, 0x4a, 0xc4, 0xf2, 0x0f, 0x0a,
 		0xca, 0xf8, 0xab, 0x80, 0x70, 0x47, 0x00, 0x20,
-		0x50, 0x60, 0x30, 0x46, 0x00, 0xbe, 0xff, 0xff
+		0x50, 0x60, 0xff, 0xf7, 0xef, 0xff, 0x30, 0x46,
+		0x00, 0xbe, 0xff, 0xff
 	};
 
 	if (target_alloc_working_area(target, sizeof(lpcspifi_flash_write_code),

src/flash/nor/mdr.c

@@ -482,6 +482,11 @@ static int mdr_probe(struct flash_bank *bank)
 	page_count = mdr_info->page_count;
 	page_size = bank->size / page_count;
 
+	if (bank->sectors) {
+		free(bank->sectors);
+		bank->sectors = NULL;
+	}
+
 	bank->num_sectors = page_count;
 	bank->sectors = malloc(sizeof(struct flash_sector) * page_count);
 
@@ -516,6 +521,8 @@ static int get_mdr_info(struct flash_bank *bank, char *buf, int buf_size)
 
 struct flash_driver mdr_flash = {
 	.name = "mdr",
+	.usage = "flash bank <name> mdr <base> <size> 0 0 <target#> <type> <page_count> <sec_count>\n"
+	"<type>: 0 for main memory, 1 for info memory",
 	.flash_bank_command = mdr_flash_bank_command,
 	.erase = mdr_erase,
 	.protect = mdr_protect,

src/flash/nor/mini51.c

@@ -19,7 +19,7 @@
  ***************************************************************************/
 
 /*
-	Flash driver for the Nuvoton NuMicro Mini51 series microcontrollers
+	Flash driver for the Nuvoton NuMicro Mini51 and M051 series microcontrollers
 
 	Part		 |APROM Size |Part ID (at 0x5000_0000)
 	----------------------------------------------
@@ -32,6 +32,30 @@
 	MINI54LAN	 16 KB		 0x00205400
 	MINI54ZAN	 16 KB		 0x00205403
 	MINI54TAN	 16 KB		 0x00205404
+	M052LBN		  8 KB		 0x10005200
+	M054LBN		 16 KB		 0x10005400
+	M058LBN		 32 KB		 0x10005800
+	M0516LBN	 64 KB		 0x10005A00
+	M052ZBN		  8 KB		 0x10005203
+	M054ZBN		 16 KB		 0x10005403
+	M058ZBN		 32 KB		 0x10005803
+	M0516ZBN	 64 KB		 0x10005A03
+	M052LDN		  8 KB		 0x20005200
+	M054LDN		 16 KB		 0x20005400
+	M058LDN		 32 KB		 0x20005800
+	M0516LDN	 64 KB		 0x20005A00
+	M052ZDN		  8 KB		 0x20005203
+	M054ZDN		 16 KB		 0x20005403
+	M058ZDN		 32 KB		 0x20005803
+	M0516ZDN	 64 KB		 0x20005A03
+	M052LDE		  8 KB		 0x30005200
+	M054LDE		 16 KB		 0x30005400
+	M058LDE		 32 KB		 0x30005800
+	M0516LDE	 64 KB		 0x30005A00
+	M052ZDE		  8 KB		 0x30005203
+	M054ZDE		 16 KB		 0x30005403
+	M058ZDE		 32 KB		 0x30005803
+	M0516ZDE	 64 KB		 0x30005A03
 
 	Datasheet & TRM
 	---------------
@@ -40,31 +64,18 @@
 
 	http://www.keil.com/dd/docs/datashts/nuvoton/mini51/da00-mini51_52_54c1.pdf
 
+	M051 ISP datasheet pages 190-206:
+	http://www.nuvoton.com/hq/resource-download.jsp?tp_GUID=DA05-M052-54-58-516
+
 	This driver
 	-----------
 
-	* Only erase and write operations have been implemented;
-	* Both operations only support the APROM, not the LDROM;
-	* The TRM suggests that after the boot source has been selected, a software reset should be performed by
-	  setting bit SWRST in ISPCON. However, this doesn't seem to have any effect on the MCU I'm using. At the
-	  moment, the ARM core is reset using the IPRSTC1 register, which seems to do the trick.
+	* chip_erase, erase, read and write operations have been implemented;
+	* All operations support APROM, LDROM, FLASH DATA and CONFIG;
 
 	Flash access limitations
 	------------------------
 
-	APROM can only be modified when the MCU has booted off the LDROM. For write and erase operations, the
-	microcontroller will probably need to be rebooted. Pseudocode:
-
-	* If operation is write or erase, check bit BS (1) in ISPCON (0x5000_C000);
-	* If BS is 0 (APROM):
-		* unlock protected registers by writing 0x59, 0x16, 0x88 to RegLockAddr(0x5000_0100);
-		* set BS to 1 (LDROM);
-		* reboot by setting bit CPU_RST(1) in IPRSTC1 (0x50000008);
-		* poll CPU_RST until it is reset (not sure it's necessary);
-		* <Perform flash operation>
-		* reboot from APROM using the same procedure but writing 0 to BS
-
-
 	For implementing the read operation, please note that the APROM isn't memory mapped when booted from LDROM.
 */
 
@@ -82,42 +93,107 @@
 #define ISPDAT          0x5000C008
 #define ISPCMD          0x5000C00C
 #define ISPTRG          0x5000C010
+/* Undocumented isp register */
+#define ISPUNKNOWN      0x5000C01C
 
-#define PART_ID_MAIN_MASK   0xFFFFFFF8
 #define IPRSTC_CPU_RST      0x02
+
+#define ISPCON_ISPFF        0x40
+#define ISPCON_LDUEN        0x20
+#define ISPCON_CFGUEN       0x10
+#define ISPCON_APUEN        0x08
 #define ISPCON_BS_LDROM     0x02
 #define ISPCON_ISPEN        0x01
-#define ISPCON_SWRST        0x80
-#define ISPCON_ISPFF        0x40
+
+#define ISPCMD_READ         0x00
 #define ISPCMD_PROGRAM      0x21
 #define ISPCMD_ERASE        0x22
+#define ISPCMD_CHIP_ERASE   0x26
+
 #define ISPTRG_ISPGO        0x01
 
-#define MINI51 0x00205100
-#define MINI52 0x00205200
-#define MINI54 0x00205400
-
 #define MINI51_APROM_BASE  0x00000000
+#define MINI51_DATA_BASE   0x0001F000
+#define MINI51_LDROM_BASE  0x00100000
+#define MINI51_CONFIG_BASE 0x00300000
+
 #define MINI51_KB          1024
 #define MINI51_PAGE_SIZE   512
 #define MINI51_TIMEOUT     1000
 
-struct mini51_flash_bank {
-	bool probed;
+
+#define ENSURE_OK(status) if (status != ERROR_OK) return status
+
+#define MINI51_MAX_FLASH_BANKS 4
+
+struct mini51_flash_bank_type {
+	uint32_t base;
+	uint32_t size;
 };
 
-enum mini51_boot_source {
-	APROM = 0,
-	LDROM = 1
+struct mini51_cpu_type {
+	char *name;
+	uint32_t ppid;
+	unsigned n_banks;
+	struct mini51_flash_bank_type bank[MINI51_MAX_FLASH_BANKS];
+};
+
+#define MINI51_BANKS_MINI51(aprom_size) \
+	.n_banks = 3, \
+	{ {MINI51_APROM_BASE, (aprom_size)}, {MINI51_LDROM_BASE, 2*1024}, {MINI51_CONFIG_BASE, 512} }
+
+#define MINI51_BANKS_M051(aprom_size) \
+	.n_banks = 4, \
+	{ {MINI51_APROM_BASE, (aprom_size)}, {MINI51_DATA_BASE, 4*1024}, {MINI51_LDROM_BASE, 4*1024}, \
+	{MINI51_CONFIG_BASE, 1024} }
+
+static const struct mini51_cpu_type mini51_cpu[] = {
+	{ "MINI51LAN", 0x00205100, MINI51_BANKS_MINI51(4*1024) },
+	{ "MINI51ZAN", 0x00205103, MINI51_BANKS_MINI51(4*1024) },
+	{ "MINI51TAN", 0x00205104, MINI51_BANKS_MINI51(4*1024) },
+	{ "MINI52LAN", 0x00205200, MINI51_BANKS_MINI51(8*1024) },
+	{ "MINI52ZAN", 0x00205203, MINI51_BANKS_MINI51(8*1024) },
+	{ "MINI52TAN", 0x00205204, MINI51_BANKS_MINI51(8*1024) },
+	{ "MINI54LAN", 0x00205400, MINI51_BANKS_MINI51(16*1024) },
+	{ "MINI54ZAN", 0x00205403, MINI51_BANKS_MINI51(16*1024) },
+	{ "MINI54TAN", 0x00205404, MINI51_BANKS_MINI51(16*1024) },
+
+	{ "M052LBN",   0x10005200, MINI51_BANKS_M051(8*1024) },
+	{ "M054LBN",   0x10005400, MINI51_BANKS_M051(16*1024) },
+	{ "M058LBN",   0x10005800, MINI51_BANKS_M051(32*1024) },
+	{ "M0516LBN",  0x10005A00, MINI51_BANKS_M051(64*1024) },
+	{ "M052ZBN",   0x10005203, MINI51_BANKS_M051(8*1024) },
+	{ "M054ZBN",   0x10005403, MINI51_BANKS_M051(16*1024) },
+	{ "M058ZBN",   0x10005803, MINI51_BANKS_M051(32*1024) },
+	{ "M0516ZBN",  0x10005A03, MINI51_BANKS_M051(64*1024) },
+	{ "M052LDN",   0x20005200, MINI51_BANKS_M051(8*1024) },
+	{ "M054LDN",   0x20005400, MINI51_BANKS_M051(16*1024) },
+	{ "M058LDN",   0x20005800, MINI51_BANKS_M051(32*1024) },
+	{ "M0516LDN",  0x20005A00, MINI51_BANKS_M051(64*1024) },
+	{ "M052ZDN",   0x20005203, MINI51_BANKS_M051(8*1024) },
+	{ "M054ZDN",   0x20005403, MINI51_BANKS_M051(16*1024) },
+	{ "M058ZDN",   0x20005803, MINI51_BANKS_M051(32*1024) },
+	{ "M0516ZDN",  0x20005A03, MINI51_BANKS_M051(64*1024) },
+	{ "M052LDE",   0x30005200, MINI51_BANKS_M051(8*1024) },
+	{ "M054LDE",   0x30005400, MINI51_BANKS_M051(16*1024) },
+	{ "M058LDE",   0x30005800, MINI51_BANKS_M051(32*1024) },
+	{ "M0516LDE",  0x30005A00, MINI51_BANKS_M051(64*1024) },
+	{ "M052ZDE",   0x30005203, MINI51_BANKS_M051(8*1024) },
+	{ "M054ZDE",   0x30005403, MINI51_BANKS_M051(16*1024) },
+	{ "M058ZDE",   0x30005803, MINI51_BANKS_M051(32*1024) },
+	{ "M0516ZDE",  0x30005A03, MINI51_BANKS_M051(64*1024) },
+};
+
+struct mini51_flash_bank {
+	bool probed;
+	const struct mini51_cpu_type *cpu;
 };
 
 /* Private methods */
 
-static int mini51_unlock_reg(struct flash_bank *bank)
+static int mini51_unlock_reg(struct target *target)
 {
 	int status;
-	struct target *target = bank->target;
-
 	status = target_write_u32(target, REGLOCKADDR, 0x59);
 	if (status != ERROR_OK)
 		return status;
@@ -131,86 +207,125 @@ static int mini51_unlock_reg(struct flash_bank *bank)
 	return ERROR_OK;
 }
 
-static int mini51_reboot_with_source(struct flash_bank *bank,
-				enum mini51_boot_source new_source,
-				enum mini51_boot_source *prev_source)
+
+static int mini51_get_part_id(struct target *target, uint32_t *part_id)
 {
-	uint32_t ispcon;
-	uint32_t isprtc1;
-	bool mini51_reboot = false;
-	int status;
-	int timeout = MINI51_TIMEOUT;
-
-	/* Read current boot source */
-	struct target *target = bank->target;
-	status = target_read_u32(target, ISPCON, &ispcon);
-	if (status != ERROR_OK)
-		return status;
-
-	*prev_source = (ispcon >> 1) & 1;
-
-	if ((new_source == APROM) && (*prev_source != APROM)) {
-		ispcon &= ~ISPCON_BS_LDROM;
-		mini51_reboot = true;
-	} else if ((new_source == LDROM) && (*prev_source != LDROM)) {
-		ispcon |= ISPCON_BS_LDROM;
-		mini51_reboot = true;
-	}
-
-	if (mini51_reboot) {
-		mini51_unlock_reg(bank);
-		status = target_write_u32(target, ISPCON, ispcon);
-		if (status != ERROR_OK)
-			return status;
-
-		status = target_write_u32(target, IPRSTC1, IPRSTC_CPU_RST);
-		if (status != ERROR_OK)
-			return status;
-
-		do {
-			target_read_u32(target, IPRSTC1, &isprtc1);
-			timeout--;
-		} while ((isprtc1 & IPRSTC_CPU_RST) && timeout > 0);
-
-		if (timeout == 0) {
-			LOG_WARNING("Mini51 flash driver: timeout attempting to reboot\n");
-			return ERROR_FLASH_OPERATION_FAILED;
-		}
-	}
-
-	return ERROR_OK;
+	int retu = target_read_u32(target, PART_ID_REG, part_id);
+	LOG_INFO("device id = 0x%08" PRIx32 "", *part_id);
+	return retu;
 }
 
-static int mini51_get_part_id(struct flash_bank *bank, uint32_t *part_id)
-{
-	return target_read_u32(bank->target, PART_ID_REG, part_id);
-}
-
-static int mini51_get_flash_size(struct flash_bank *bank, uint32_t *flash_size)
+static int mini51_get_cpu_type(struct target *target, const struct mini51_cpu_type** cpu)
 {
 	uint32_t part_id;
 	int status;
 
-	status = mini51_get_part_id(bank, &part_id);
-	if (status != ERROR_OK)
-		return status;
+	status = mini51_get_part_id(target, &part_id);
+	ENSURE_OK(status);
 
-	switch (part_id & PART_ID_MAIN_MASK) {
-		case MINI51:
-			*flash_size = 4 * MINI51_KB;
-			break;
-		case MINI52:
-			*flash_size = 8 * MINI51_KB;
-			break;
-		case MINI54:
-			*flash_size = 16 * MINI51_KB;
-			break;
-		default:
-			*flash_size = 0;
-			break;
+	for (size_t i = 0; i < sizeof(mini51_cpu)/sizeof(mini51_cpu[0]); i++) {
+		if (part_id == mini51_cpu[i].ppid) {
+			*cpu = &mini51_cpu[i];
+			LOG_INFO("device name = %s", (*cpu)->name);
+			return ERROR_OK;
+		}
 	}
 
-	return ERROR_OK;
+	return ERROR_FLASH_OPERATION_FAILED;
+}
+
+static int mini51_get_flash_size(struct flash_bank *bank, const struct mini51_cpu_type *cpu, uint32_t *flash_size)
+{
+	for (size_t i = 0; i < cpu->n_banks; i++) {
+		if (bank->base == cpu->bank[i].base) {
+			*flash_size = cpu->bank[i].size;
+			LOG_INFO("bank base = 0x%08" PRIx32 ", size = 0x%08" PRIx32 "", bank->base, *flash_size);
+			return ERROR_OK;
+		}
+	}
+	return ERROR_FLASH_OPERATION_FAILED;
+}
+
+static int mini51_isp_execute(struct target *target)
+{
+	int status;
+	uint32_t ispcon;
+	int timeout;
+	uint32_t isptrg;
+
+	/* start ISP operation */
+	status = target_write_u32(target, ISPTRG, ISPTRG_ISPGO);
+	ENSURE_OK(status);
+
+	/* Wait for for command to finish executing */
+	timeout = MINI51_TIMEOUT;
+	do {
+		target_read_u32(target, ISPTRG, &isptrg);
+		timeout--;
+	} while ((isptrg & ISPTRG_ISPGO) && (timeout > 0));
+	if (timeout == 0) {
+		LOG_WARNING("Mini51 flash driver: Timeout executing flash command\n");
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	/* Check for errors */
+	status = target_read_u32(target, ISPCON, &ispcon);
+	ENSURE_OK(status);
+	if (ispcon & ISPCON_ISPFF) {
+		LOG_WARNING("Mini51 flash driver: operation failed\n");
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+	return status;
+}
+
+static int mini51_isp_execute_cmd(struct target *target, uint32_t cmd, uint32_t address, uint32_t data)
+{
+	int status;
+	status = target_write_u32(target, ISPDAT, data);
+	ENSURE_OK(status);
+	status = target_write_u32(target, ISPADR, address);
+	ENSURE_OK(status);
+	status = target_write_u32(target, ISPCMD, cmd);
+	ENSURE_OK(status);
+
+	status = mini51_isp_execute(target);
+	return status;
+}
+
+static int mini51_isp_execute_cmd_read(struct target *target, uint32_t cmd, uint32_t address, uint32_t *data)
+{
+	int status;
+	status = target_write_u32(target, ISPADR, address);
+	ENSURE_OK(status);
+	status = target_write_u32(target, ISPCMD, cmd);
+	ENSURE_OK(status);
+
+	status = mini51_isp_execute(target);
+	ENSURE_OK(status);
+
+	status = target_read_u32(target, ISPDAT, data);
+	ENSURE_OK(status);
+
+	return status;
+}
+
+static int mini51_isp_enable(struct target *target)
+{
+	int status;
+	uint32_t ispcon;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	status = mini51_unlock_reg(target);
+	ENSURE_OK(status);
+	status = target_read_u32(target, ISPCON, &ispcon);
+	ENSURE_OK(status);
+	ispcon |= ISPCON_ISPEN | ISPCON_LDUEN | ISPCON_APUEN | ISPCON_CFGUEN;
+	status = target_write_u32(target, ISPCON, ispcon);
+	return status;
 }
 
 /* Public (API) methods */
@@ -232,74 +347,45 @@ static int mini51_protect_check(struct flash_bank *bank)
 	return ERROR_FLASH_OPERATION_FAILED;
 }
 
+static int mini51_read(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, uint32_t count)
+{
+	int status;
+	uint32_t ispdat;
+	struct target *target = bank->target;
+
+	if ((offset & 0x3) || (count & 0x3)) {
+		LOG_WARNING("Mini51 flash driver: unaligned access not supported\n");
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	status = mini51_isp_enable(target);
+	ENSURE_OK(status);
+
+	for (uint32_t i = offset; i < offset + count; i += 4) {
+		status = mini51_isp_execute_cmd_read(target, ISPCMD_READ, bank->base + i, &ispdat);
+		memcpy(buffer, &ispdat, sizeof(ispdat));
+		ENSURE_OK(status);
+		buffer += sizeof(ispdat);
+	}
+
+	return ERROR_OK;
+}
+
+
 static int mini51_erase(struct flash_bank *bank, int first, int last)
 {
 	int status;
-	int timeout;
-	uint32_t ispcon;
-	uint32_t isptrg;
-	enum mini51_boot_source new_source;
-	enum mini51_boot_source prev_source;
 	struct target *target = bank->target;
 
-	if (target->state != TARGET_HALTED) {
-		LOG_ERROR("Target not halted");
-		return ERROR_TARGET_NOT_HALTED;
-	}
-
-	/* TODO: add support for erasing the LDROM */
-	new_source = LDROM;
-	status = mini51_reboot_with_source(bank, new_source, &prev_source);
-	if (status != ERROR_OK)
-		return status;
-
 	/* Enable ISP */
-	status = target_read_u32(target, ISPCON, &ispcon);
-	if (status != ERROR_OK)
-		return status;
-	ispcon |= ISPCON_ISPEN;
-	status = target_write_u32(target, ISPCON, ispcon);
+	status = mini51_isp_enable(target);
+	ENSURE_OK(status);
 
 	for (int page_start = first; page_start <= last; page_start++) {
 		/* Set up erase command */
-		status = target_write_u32(target, ISPADR, page_start*MINI51_PAGE_SIZE);
-		if (status != ERROR_OK)
-			return status;
-		status = target_write_u32(target, ISPCMD, ISPCMD_ERASE);
-		if (status != ERROR_OK)
-			return status;
-
-		/* Erase the selected page */
-		status = target_write_u32(target, ISPTRG, ISPTRG_ISPGO);
-		if (status != ERROR_OK)
-			return status;
-
-		/* Wait for for command to finish executing */
-		timeout = MINI51_TIMEOUT;
-		do {
-			target_read_u32(target, ISPTRG, &isptrg);
-			timeout--;
-		} while ((isptrg & ISPTRG_ISPGO) && (timeout > 0));
-		if (timeout == 0) {
-			LOG_WARNING("Mini51 flash driver: Timeout erasing flash\n");
-			return ERROR_FLASH_OPERATION_FAILED;
-		}
-
-		/* Check for errors */
-		status = target_read_u32(target, ISPCON, &ispcon);
-		if (status != ERROR_OK)
-			return status;
-		if (ispcon & ISPCON_ISPFF) {
-			LOG_WARNING("Mini51 flash driver: Erase operation failed\n");
-			return ERROR_FLASH_OPERATION_FAILED;
-		}
-	}
-
-	/* Reboot from previous source */
-	if (prev_source != new_source) {
-		status = mini51_reboot_with_source(bank, prev_source, &new_source);
-		if (status != ERROR_OK)
-			return status;
+		uint32_t address = bank->base + page_start*MINI51_PAGE_SIZE;
+		status = mini51_isp_execute_cmd(target, ISPCMD_ERASE, address, 0);
+		ENSURE_OK(status);
 	}
 
 	return ERROR_OK;
@@ -315,81 +401,22 @@ static int mini51_protect(struct flash_bank *bank, int set, int first, int last)
 static int mini51_write(struct flash_bank *bank, const uint8_t *buffer, uint32_t offset, uint32_t count)
 {
 	int status;
-	int timeout;
-	uint32_t ispcon;
-	uint32_t isptrg;
 	uint32_t ispdat;
-	enum mini51_boot_source new_source;
-	enum mini51_boot_source prev_source;
 	struct target *target = bank->target;
 
-	if (target->state != TARGET_HALTED) {
-		LOG_ERROR("Target not halted");
-		return ERROR_TARGET_NOT_HALTED;
-	}
-
 	if ((offset & 0x3) || (count & 0x3)) {
 		LOG_WARNING("Mini51 flash driver: unaligned access not supported\n");
 		return ERROR_FLASH_OPERATION_FAILED;
 	}
 
-	/* TODO: add support for writing to LDROM */
-	new_source = LDROM;
-	status = mini51_reboot_with_source(bank, new_source, &prev_source);
-	if (status != ERROR_OK)
-		return status;
-
-	/* Enable ISP */
-	status = target_read_u32(target, ISPCON, &ispcon);
-	if (status != ERROR_OK)
-		return status;
-	ispcon |= ISPCON_ISPEN;
-	status = target_write_u32(target, ISPCON, ispcon);
+	status = mini51_isp_enable(target);
+	ENSURE_OK(status);
 
 	for (uint32_t i = offset; i < offset + count; i += 4) {
-		/* Set up program command */
-		status = target_write_u32(target, ISPADR, i);
-		if (status != ERROR_OK)
-			return status;
-		status = target_write_u32(target, ISPCMD, ISPCMD_PROGRAM);
-		if (status != ERROR_OK)
-			return status;
 		memcpy(&ispdat, buffer, sizeof(ispdat));
+		status = mini51_isp_execute_cmd(target, ISPCMD_PROGRAM, bank->base + i, ispdat);
+		ENSURE_OK(status);
 		buffer += sizeof(ispdat);
-		status = target_write_u32(target, ISPDAT, ispdat);
-		if (status != ERROR_OK)
-			return status;
-
-		/* Write the selected word */
-		status = target_write_u32(target, ISPTRG, ISPTRG_ISPGO);
-		if (status != ERROR_OK)
-			return status;
-
-		/* Wait for for command to finish executing */
-		timeout = MINI51_TIMEOUT;
-		do {
-			target_read_u32(target, ISPTRG, &isptrg);
-			timeout--;
-		} while ((isptrg & ISPTRG_ISPGO) && (timeout > 0));
-		if (timeout == 0) {
-			LOG_WARNING("Mini51 flash driver: Timeout programming flash\n");
-			return ERROR_FLASH_OPERATION_FAILED;
-		}
-
-		/* Check for errors */
-		status = target_read_u32(target, ISPCON, &ispcon);
-		if (status != ERROR_OK)
-			return status;
-		if (ispcon & ISPCON_ISPFF) {
-			LOG_WARNING("Mini51 flash driver: Programming operation failed\n");
-			return ERROR_FLASH_OPERATION_FAILED;
-		}
-	}
-
-	if (prev_source != new_source) {
-		status = mini51_reboot_with_source(bank, prev_source, &new_source);
-		if (status != ERROR_OK)
-			return status;
 	}
 
 	return ERROR_OK;
@@ -398,19 +425,26 @@ static int mini51_write(struct flash_bank *bank, const uint8_t *buffer, uint32_t
 static int mini51_probe(struct flash_bank *bank)
 {
 	uint32_t flash_size;
-	int retval;
+	int status;
 	int num_pages;
 	uint32_t offset = 0;
+	const struct mini51_cpu_type *cpu;
+	struct target *target = bank->target;
 
-	retval = mini51_get_flash_size(bank, &flash_size);
-	if (retval != ERROR_OK || flash_size == 0) {
+	status = mini51_get_cpu_type(target, &cpu);
+	if (status != ERROR_OK) {
 		LOG_WARNING("Mini51 flash driver: Failed to detect a known part\n");
 		return ERROR_FLASH_OPERATION_FAILED;
 	}
 
+	status = mini51_get_flash_size(bank, cpu, &flash_size);
+	if (status != ERROR_OK) {
+		LOG_WARNING("Mini51 flash driver: Failed to detect flash size\n");
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
 	num_pages = flash_size / MINI51_PAGE_SIZE;
 
-	bank->base = MINI51_APROM_BASE;
 	bank->num_sectors = num_pages;
 	bank->sectors = malloc(sizeof(struct flash_sector) * num_pages);
 	bank->size = flash_size;
@@ -425,6 +459,7 @@ static int mini51_probe(struct flash_bank *bank)
 
 	struct mini51_flash_bank *mini51_info = bank->driver_priv;
 	mini51_info->probed = true;
+	mini51_info->cpu = cpu;
 
 	return ERROR_OK;
 }
@@ -437,13 +472,111 @@ static int mini51_auto_probe(struct flash_bank *bank)
 	return mini51_probe(bank);
 }
 
+COMMAND_HANDLER(mini51_handle_read_isp_command)
+{
+	uint32_t address;
+	uint32_t ispdat;
+	int status;
+
+	if (CMD_ARGC != 1)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], address);
+
+	struct target *target = get_current_target(CMD_CTX);
+
+	status = mini51_isp_enable(target);
+	ENSURE_OK(status);
+	status = mini51_isp_execute_cmd_read(target, ISPCMD_READ, address, &ispdat);
+	ENSURE_OK(status);
+	LOG_INFO("0x%08" PRIx32 ": 0x%08" PRIx32, address, ispdat);
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(mini51_handle_write_isp_command)
+{
+	uint32_t address;
+	uint32_t ispdat;
+	int status;
+
+	if (CMD_ARGC != 2)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], address);
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], ispdat);
+
+	struct target *target = get_current_target(CMD_CTX);
+
+	status = mini51_isp_enable(target);
+	ENSURE_OK(status);
+	status = mini51_isp_execute_cmd(target, ISPCMD_PROGRAM, address, ispdat);
+	ENSURE_OK(status);
+	LOG_INFO("0x%08" PRIx32 ": 0x%08" PRIx32, address, ispdat);
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(mini51_handle_chip_erase_command)
+{
+	int status;
+	if (CMD_ARGC != 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	struct target *target = get_current_target(CMD_CTX);
+
+	status = mini51_isp_enable(target);
+	ENSURE_OK(status);
+	/* Write one to undocumented flash control register */
+	status = target_write_u32(target, ISPUNKNOWN, 1);
+	ENSURE_OK(status);
+
+	status = mini51_isp_execute_cmd(target, ISPCMD_CHIP_ERASE, 0, 0);
+	ENSURE_OK(status);
+	return ERROR_OK;
+}
+
+static const struct command_registration mini51_exec_command_handlers[] = {
+	{
+		.name = "read_isp",
+		.handler = mini51_handle_read_isp_command,
+		.usage = "address",
+		.mode = COMMAND_EXEC,
+		.help = "read flash through ISP.",
+	},
+	{
+		.name = "write_isp",
+		.handler = mini51_handle_write_isp_command,
+		.usage = "address value",
+		.mode = COMMAND_EXEC,
+		.help = "write flash through ISP.",
+	},
+	{
+		.name = "chip_erase",
+		.handler = mini51_handle_chip_erase_command,
+		.mode = COMMAND_EXEC,
+		.help = "chip erase.",
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+static const struct command_registration mini51_command_handlers[] = {
+	{
+		.name = "mini51",
+		.mode = COMMAND_ANY,
+		.help = "mini51 flash command group",
+		.usage = "",
+		.chain = mini51_exec_command_handlers,
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
 struct flash_driver mini51_flash = {
 	.name = "mini51",
+	.commands = mini51_command_handlers,
 	.flash_bank_command = mini51_flash_bank_command,
 	.erase = mini51_erase,
 	.protect = mini51_protect,
 	.write = mini51_write,
-	.read = default_flash_read,
+	.read = mini51_read,
 	.probe = mini51_probe,
 	.auto_probe = mini51_auto_probe,
 	.erase_check = default_flash_blank_check,

src/flash/nor/mrvlqspi.c

@@ -0,0 +1,961 @@
+/***************************************************************************
+ *   Copyright (C) 2014 by Mahavir Jain <mjain@marvell.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, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.           *
+ *                                                                         *
+ ***************************************************************************/
+
+ /*
+  * This is QSPI flash controller driver for Marvell's Wireless
+  * Microcontroller platform.
+  *
+  * For more information please refer,
+  * https://origin-www.marvell.com/microcontrollers/wi-fi-microcontroller-platform/
+  */
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "imp.h"
+#include "spi.h"
+#include <helper/binarybuffer.h>
+#include <target/algorithm.h>
+#include <target/armv7m.h>
+
+#define QSPI_R_EN (0x0)
+#define QSPI_W_EN (0x1)
+#define QSPI_SS_DISABLE (0x0)
+#define QSPI_SS_ENABLE (0x1)
+#define WRITE_DISBALE (0x0)
+#define WRITE_ENABLE (0x1)
+
+#define QSPI_TIMEOUT (1000)
+#define FIFO_FLUSH_TIMEOUT (1000)
+#define BLOCK_ERASE_TIMEOUT (1000)
+#define CHIP_ERASE_TIMEOUT (10000)
+
+#define SS_EN (1 << 0)
+#define XFER_RDY (1 << 1)
+#define RFIFO_EMPTY (1 << 4)
+#define WFIFO_EMPTY (1 << 6)
+#define WFIFO_FULL (1 << 7)
+#define FIFO_FLUSH (1 << 9)
+#define RW_EN (1 << 13)
+#define XFER_STOP (1 << 14)
+#define XFER_START (1 << 15)
+#define CONF_MASK (0x7)
+#define CONF_OFFSET (10)
+
+#define INS_WRITE_ENABLE 0x06
+#define INS_WRITE_DISABLE 0x04
+#define INS_READ_STATUS 0x05
+#define INS_PAGE_PROGRAM 0x02
+
+#define CNTL 0x0 /* QSPI_BASE + 0x0 */
+#define CONF 0x4
+#define DOUT 0x8
+#define DIN 0xc
+#define INSTR 0x10
+#define ADDR 0x14
+#define RDMODE 0x18
+#define HDRCNT 0x1c
+#define DINCNT 0x20
+
+struct mrvlqspi_flash_bank {
+	int probed;
+	uint32_t reg_base;
+	uint32_t bank_num;
+	const struct flash_device *dev;
+};
+
+static inline uint32_t mrvlqspi_get_reg(struct flash_bank *bank, uint32_t reg)
+{
+	struct mrvlqspi_flash_bank *mrvlqspi_info = bank->driver_priv;
+	return reg + mrvlqspi_info->reg_base;
+}
+
+static inline int mrvlqspi_set_din_cnt(struct flash_bank *bank, uint32_t count)
+{
+	struct target *target = bank->target;
+
+	return target_write_u32(target, mrvlqspi_get_reg(bank, DINCNT), count);
+}
+
+static inline int mrvlqspi_set_addr(struct flash_bank *bank, uint32_t addr)
+{
+	struct target *target = bank->target;
+
+	return target_write_u32(target, mrvlqspi_get_reg(bank, ADDR), addr);
+}
+
+static inline int mrvlqspi_set_instr(struct flash_bank *bank, uint32_t instr)
+{
+	struct target *target = bank->target;
+
+	return target_write_u32(target, mrvlqspi_get_reg(bank, INSTR), instr);
+}
+
+static inline int mrvlqspi_set_hdr_cnt(struct flash_bank *bank, uint32_t hdr_cnt)
+{
+	struct target *target = bank->target;
+
+	return target_write_u32(target, mrvlqspi_get_reg(bank, HDRCNT), hdr_cnt);
+}
+
+static int mrvlqspi_set_conf(struct flash_bank *bank, uint32_t conf_val)
+{
+	int retval;
+	uint32_t regval;
+	struct target *target = bank->target;
+
+	retval = target_read_u32(target,
+			mrvlqspi_get_reg(bank, CONF), &regval);
+	if (retval != ERROR_OK)
+		return retval;
+
+	regval &= ~(CONF_MASK << CONF_OFFSET);
+	regval |= (conf_val << CONF_OFFSET);
+
+	return target_write_u32(target,
+			mrvlqspi_get_reg(bank, CONF), regval);
+}
+
+static int mrvlqspi_set_ss_state(struct flash_bank *bank, bool state, int timeout)
+{
+	int retval;
+	uint32_t regval;
+	struct target *target = bank->target;
+
+	retval = target_read_u32(target,
+			mrvlqspi_get_reg(bank, CNTL), &regval);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (state)
+		regval |= SS_EN;
+	else
+		regval &= ~(SS_EN);
+
+	retval = target_write_u32(target,
+			mrvlqspi_get_reg(bank, CNTL), regval);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* wait for xfer_ready to set */
+	for (;;) {
+		retval = target_read_u32(target,
+				mrvlqspi_get_reg(bank, CNTL), &regval);
+		if (retval != ERROR_OK)
+			return retval;
+		LOG_DEBUG("status: 0x%08" PRIx32, regval);
+		if ((regval & XFER_RDY) == XFER_RDY)
+			break;
+		if (timeout-- <= 0) {
+			LOG_ERROR("timed out waiting for flash");
+			return ERROR_FAIL;
+		}
+		alive_sleep(1);
+	}
+	return ERROR_OK;
+}
+
+static int mrvlqspi_start_transfer(struct flash_bank *bank, bool rw_mode)
+{
+	int retval;
+	uint32_t regval;
+	struct target *target = bank->target;
+
+	retval = mrvlqspi_set_ss_state(bank, QSPI_SS_ENABLE, QSPI_TIMEOUT);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = target_read_u32(target,
+			mrvlqspi_get_reg(bank, CONF), &regval);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (rw_mode)
+		regval |= RW_EN;
+	else
+		regval &= ~(RW_EN);
+
+	regval |= XFER_START;
+
+	retval = target_write_u32(target,
+			mrvlqspi_get_reg(bank, CONF), regval);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return ERROR_OK;
+}
+
+static int mrvlqspi_stop_transfer(struct flash_bank *bank)
+{
+	int retval;
+	uint32_t regval;
+	struct target *target = bank->target;
+	int timeout = QSPI_TIMEOUT;
+
+	/* wait for xfer_ready and wfifo_empty to set */
+	for (;;) {
+		retval = target_read_u32(target,
+				mrvlqspi_get_reg(bank, CNTL), &regval);
+		if (retval != ERROR_OK)
+			return retval;
+		LOG_DEBUG("status: 0x%08" PRIx32, regval);
+		if ((regval & (XFER_RDY | WFIFO_EMPTY)) ==
+					(XFER_RDY | WFIFO_EMPTY))
+			break;
+		if (timeout-- <= 0) {
+			LOG_ERROR("timed out waiting for flash");
+			return ERROR_FAIL;
+		}
+		alive_sleep(1);
+	}
+
+	retval = target_read_u32(target,
+			mrvlqspi_get_reg(bank, CONF), &regval);
+	if (retval != ERROR_OK)
+		return retval;
+
+	regval |= XFER_STOP;
+
+	retval = target_write_u32(target,
+			mrvlqspi_get_reg(bank, CONF), regval);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* wait for xfer_start to reset */
+	for (;;) {
+		retval = target_read_u32(target,
+				mrvlqspi_get_reg(bank, CONF), &regval);
+		if (retval != ERROR_OK)
+			return retval;
+		LOG_DEBUG("status: 0x%08" PRIx32, regval);
+		if ((regval & XFER_START) == 0)
+			break;
+		if (timeout-- <= 0) {
+			LOG_ERROR("timed out waiting for flash");
+			return ERROR_FAIL;
+		}
+		alive_sleep(1);
+	}
+
+	retval = mrvlqspi_set_ss_state(bank, QSPI_SS_DISABLE, QSPI_TIMEOUT);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return ERROR_OK;
+}
+
+static int mrvlqspi_fifo_flush(struct flash_bank *bank, int timeout)
+{
+	int retval;
+	uint32_t val;
+	struct target *target = bank->target;
+
+	retval = target_read_u32(target,
+			mrvlqspi_get_reg(bank, CONF), &val);
+	if (retval != ERROR_OK)
+		return retval;
+
+	val |= FIFO_FLUSH;
+
+	retval = target_write_u32(target,
+			mrvlqspi_get_reg(bank, CONF), val);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* wait for fifo_flush to clear */
+	for (;;) {
+		retval = target_read_u32(target,
+				mrvlqspi_get_reg(bank, CONF), &val);
+		if (retval != ERROR_OK)
+			return retval;
+		LOG_DEBUG("status: 0x%08" PRIX32, val);
+		if ((val & FIFO_FLUSH) == 0)
+			break;
+		if (timeout-- <= 0) {
+			LOG_ERROR("timed out waiting for flash");
+			return ERROR_FAIL;
+		}
+		alive_sleep(1);
+	}
+	return ERROR_OK;
+}
+
+static int mrvlqspi_read_byte(struct flash_bank *bank, uint8_t *data)
+{
+	int retval;
+	uint32_t val;
+	struct target *target = bank->target;
+
+	/* wait for rfifo_empty to reset */
+	for (;;) {
+		retval = target_read_u32(target,
+				mrvlqspi_get_reg(bank, CNTL), &val);
+		if (retval != ERROR_OK)
+			return retval;
+		LOG_DEBUG("status: 0x%08" PRIx32, val);
+		if ((val & RFIFO_EMPTY) == 0)
+			break;
+		usleep(10);
+	}
+
+	retval = target_read_u32(target,
+			mrvlqspi_get_reg(bank, DIN), &val);
+	if (retval != ERROR_OK)
+		return retval;
+
+	*data = val & 0xFF;
+
+	return ERROR_OK;
+}
+
+static int mrvlqspi_flash_busy_status(struct flash_bank *bank, int timeout)
+{
+	uint8_t val;
+	int retval;
+
+	/* Flush read/write fifo's */
+	retval = mrvlqspi_fifo_flush(bank, FIFO_FLUSH_TIMEOUT);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Set instruction/addr count value */
+	retval = mrvlqspi_set_hdr_cnt(bank, 0x1);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Read flash status register in continuous manner */
+	retval = mrvlqspi_set_din_cnt(bank, 0x0);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Set instruction */
+	retval = mrvlqspi_set_instr(bank, INS_READ_STATUS);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Set data and addr pin length */
+	retval = mrvlqspi_set_conf(bank, 0x0);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Enable read mode transfer */
+	retval = mrvlqspi_start_transfer(bank, QSPI_R_EN);
+	if (retval != ERROR_OK)
+		return retval;
+
+	for (;;) {
+		retval = mrvlqspi_read_byte(bank, &val);
+		if (retval != ERROR_OK)
+			return retval;
+		if (!(val & 0x1))
+			break;
+		if (timeout-- <= 0) {
+			LOG_ERROR("timed out waiting for flash");
+			return ERROR_FAIL;
+		}
+		alive_sleep(1);
+	}
+
+	return mrvlqspi_stop_transfer(bank);
+}
+
+static int mrvlqspi_set_write_status(struct flash_bank *bank, bool mode)
+{
+	int retval;
+	uint32_t instr;
+
+	/* Flush read/write fifo's */
+	retval = mrvlqspi_fifo_flush(bank, FIFO_FLUSH_TIMEOUT);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Set instruction/addr count value */
+	retval = mrvlqspi_set_hdr_cnt(bank, 0x1);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (mode)
+		instr = INS_WRITE_ENABLE;
+	else
+		instr = INS_WRITE_DISABLE;
+
+	/* Set instruction */
+	retval = mrvlqspi_set_instr(bank, instr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = mrvlqspi_start_transfer(bank, QSPI_W_EN);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = mrvlqspi_stop_transfer(bank);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return retval;
+}
+
+static int mrvlqspi_read_id(struct flash_bank *bank, uint32_t *id)
+{
+	uint8_t id_buf[3] = {0, 0, 0};
+	int retval, i;
+
+	LOG_DEBUG("Getting ID");
+
+	/* Flush read/write fifo's */
+	retval = mrvlqspi_fifo_flush(bank, FIFO_FLUSH_TIMEOUT);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Set instruction/addr count value */
+	retval = mrvlqspi_set_hdr_cnt(bank, 0x1);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Set count for number of bytes to read */
+	retval = mrvlqspi_set_din_cnt(bank, 0x3);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Set instruction */
+	retval = mrvlqspi_set_instr(bank, SPIFLASH_READ_ID);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Set data and addr pin length */
+	retval = mrvlqspi_set_conf(bank, 0x0);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = mrvlqspi_start_transfer(bank, QSPI_R_EN);
+	if (retval != ERROR_OK)
+		return retval;
+
+	for (i = 0; i < 3; i++) {
+		retval = mrvlqspi_read_byte(bank, &id_buf[i]);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	LOG_DEBUG("ID is 0x%02" PRIx8 " 0x%02" PRIx8 " 0x%02" PRIx8,
+					id_buf[0], id_buf[1], id_buf[2]);
+	retval = mrvlqspi_set_ss_state(bank, QSPI_SS_DISABLE, QSPI_TIMEOUT);
+	if (retval != ERROR_OK)
+		return retval;
+
+	*id = id_buf[2] << 16 | id_buf[1] << 8 | id_buf[0];
+	return ERROR_OK;
+}
+
+static int mrvlqspi_block_erase(struct flash_bank *bank, uint32_t offset)
+{
+	int retval;
+	struct mrvlqspi_flash_bank *mrvlqspi_info = bank->driver_priv;
+
+	/* Set flash write enable */
+	retval = mrvlqspi_set_write_status(bank, WRITE_ENABLE);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Set instruction/addr count value */
+	retval = mrvlqspi_set_hdr_cnt(bank, (0x1 | (0x3 << 4)));
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Set read offset address */
+	retval = mrvlqspi_set_addr(bank, offset);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Set instruction */
+	retval = mrvlqspi_set_instr(bank, mrvlqspi_info->dev->erase_cmd);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = mrvlqspi_start_transfer(bank, QSPI_W_EN);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = mrvlqspi_stop_transfer(bank);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return mrvlqspi_flash_busy_status(bank, BLOCK_ERASE_TIMEOUT);
+}
+
+static int mrvlqspi_bulk_erase(struct flash_bank *bank)
+{
+	int retval;
+	struct mrvlqspi_flash_bank *mrvlqspi_info = bank->driver_priv;
+
+	/* Set flash write enable */
+	retval = mrvlqspi_set_write_status(bank, WRITE_ENABLE);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Set instruction */
+	retval = mrvlqspi_set_instr(bank, mrvlqspi_info->dev->chip_erase_cmd);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = mrvlqspi_start_transfer(bank, QSPI_W_EN);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = mrvlqspi_stop_transfer(bank);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return mrvlqspi_flash_busy_status(bank, CHIP_ERASE_TIMEOUT);
+}
+
+static int mrvlqspi_flash_erase(struct flash_bank *bank, int first, int last)
+{
+	struct target *target = bank->target;
+	struct mrvlqspi_flash_bank *mrvlqspi_info = bank->driver_priv;
+	int retval = ERROR_OK;
+	int sector;
+
+	LOG_DEBUG("erase from sector %d to sector %d", first, last);
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if ((first < 0) || (last < first) || (last >= bank->num_sectors)) {
+		LOG_ERROR("Flash sector invalid");
+		return ERROR_FLASH_SECTOR_INVALID;
+	}
+
+	if (!(mrvlqspi_info->probed)) {
+		LOG_ERROR("Flash bank not probed");
+		return ERROR_FLASH_BANK_NOT_PROBED;
+	}
+
+	for (sector = first; sector <= last; sector++) {
+		if (bank->sectors[sector].is_protected) {
+			LOG_ERROR("Flash sector %d protected", sector);
+			return ERROR_FAIL;
+		}
+	}
+
+	/* If we're erasing the entire chip and the flash supports
+	 * it, use a bulk erase instead of going sector-by-sector. */
+	if (first == 0 && last == (bank->num_sectors - 1)
+		&& mrvlqspi_info->dev->chip_erase_cmd !=
+					mrvlqspi_info->dev->erase_cmd) {
+		LOG_DEBUG("Chip supports the bulk erase command."\
+		" Will use bulk erase instead of sector-by-sector erase.");
+		retval = mrvlqspi_bulk_erase(bank);
+		if (retval == ERROR_OK) {
+			return retval;
+		} else
+			LOG_WARNING("Bulk flash erase failed."
+				" Falling back to sector-by-sector erase.");
+	}
+
+	for (sector = first; sector <= last; sector++) {
+		retval = mrvlqspi_block_erase(bank,
+				sector * mrvlqspi_info->dev->sectorsize);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	return retval;
+}
+
+static int mrvlqspi_flash_write(struct flash_bank *bank, const uint8_t *buffer,
+	uint32_t offset, uint32_t count)
+{
+	struct target *target = bank->target;
+	struct mrvlqspi_flash_bank *mrvlqspi_info = bank->driver_priv;
+	int retval = ERROR_OK;
+	uint32_t page_size, fifo_size;
+	struct working_area *fifo;
+	struct reg_param reg_params[6];
+	struct armv7m_algorithm armv7m_info;
+	struct working_area *write_algorithm;
+	int sector;
+
+	LOG_DEBUG("offset=0x%08" PRIx32 " count=0x%08" PRIx32,
+		offset, count);
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (offset + count > mrvlqspi_info->dev->size_in_bytes) {
+		LOG_WARNING("Writes past end of flash. Extra data discarded.");
+		count = mrvlqspi_info->dev->size_in_bytes - offset;
+	}
+
+	/* Check sector protection */
+	for (sector = 0; sector < bank->num_sectors; sector++) {
+		/* Start offset in or before this sector? */
+		/* End offset in or behind this sector? */
+		if ((offset <
+			(bank->sectors[sector].offset + bank->sectors[sector].size))
+			&& ((offset + count - 1) >= bank->sectors[sector].offset)
+			&& bank->sectors[sector].is_protected) {
+			LOG_ERROR("Flash sector %d protected", sector);
+			return ERROR_FAIL;
+		}
+	}
+
+	page_size = mrvlqspi_info->dev->pagesize;
+
+	/* See contrib/loaders/flash/mrvlqspi.S for src */
+	static const uint8_t mrvlqspi_flash_write_code[] = {
+		0x4f, 0xf0, 0x00, 0x0a, 0xa2, 0x44, 0x92, 0x45,
+		0x7f, 0xf6, 0xfc, 0xaf, 0x00, 0xf0, 0x6b, 0xf8,
+		0x5f, 0xf0, 0x01, 0x08, 0xc5, 0xf8, 0x1c, 0x80,
+		0x5f, 0xf0, 0x06, 0x08, 0xc5, 0xf8, 0x10, 0x80,
+		0x5f, 0xf0, 0x01, 0x09, 0x00, 0xf0, 0x6b, 0xf8,
+		0x00, 0xf0, 0x7d, 0xf8, 0x5f, 0xf0, 0x31, 0x08,
+		0xc5, 0xf8, 0x1c, 0x80, 0x90, 0x46, 0xc5, 0xf8,
+		0x14, 0x80, 0x5f, 0xf0, 0x02, 0x08, 0xc5, 0xf8,
+		0x10, 0x80, 0x5f, 0xf0, 0x01, 0x09, 0x00, 0xf0,
+		0x5a, 0xf8, 0xd0, 0xf8, 0x00, 0x80, 0xb8, 0xf1,
+		0x00, 0x0f, 0x00, 0xf0, 0x8b, 0x80, 0x47, 0x68,
+		0x47, 0x45, 0x3f, 0xf4, 0xf6, 0xaf, 0x17, 0xf8,
+		0x01, 0x9b, 0x00, 0xf0, 0x30, 0xf8, 0x8f, 0x42,
+		0x28, 0xbf, 0x00, 0xf1, 0x08, 0x07, 0x47, 0x60,
+		0x01, 0x3b, 0x00, 0x2b, 0x00, 0xf0, 0x05, 0x80,
+		0x02, 0xf1, 0x01, 0x02, 0x92, 0x45, 0x7f, 0xf4,
+		0xe4, 0xaf, 0x00, 0xf0, 0x50, 0xf8, 0xa2, 0x44,
+		0x00, 0xf0, 0x2d, 0xf8, 0x5f, 0xf0, 0x01, 0x08,
+		0xc5, 0xf8, 0x1c, 0x80, 0x5f, 0xf0, 0x00, 0x08,
+		0xc5, 0xf8, 0x20, 0x80, 0x5f, 0xf0, 0x05, 0x08,
+		0xc5, 0xf8, 0x10, 0x80, 0x5f, 0xf0, 0x00, 0x09,
+		0x00, 0xf0, 0x29, 0xf8, 0x00, 0xf0, 0x13, 0xf8,
+		0x09, 0xf0, 0x01, 0x09, 0xb9, 0xf1, 0x00, 0x0f,
+		0xf8, 0xd1, 0x00, 0xf0, 0x34, 0xf8, 0x00, 0x2b,
+		0xa4, 0xd1, 0x00, 0xf0, 0x53, 0xb8, 0xd5, 0xf8,
+		0x00, 0x80, 0x5f, 0xea, 0x08, 0x68, 0xfa, 0xd4,
+		0xc5, 0xf8, 0x08, 0x90, 0x70, 0x47, 0xd5, 0xf8,
+		0x00, 0x80, 0x5f, 0xea, 0xc8, 0x68, 0xfa, 0xd4,
+		0xd5, 0xf8, 0x0c, 0x90, 0x70, 0x47, 0xd5, 0xf8,
+		0x04, 0x80, 0x48, 0xf4, 0x00, 0x78, 0xc5, 0xf8,
+		0x04, 0x80, 0xd5, 0xf8, 0x04, 0x80, 0x5f, 0xea,
+		0x88, 0x58, 0xfa, 0xd4, 0x70, 0x47, 0xd5, 0xf8,
+		0x00, 0x80, 0x48, 0xf0, 0x01, 0x08, 0xc5, 0xf8,
+		0x00, 0x80, 0xd5, 0xf8, 0x00, 0x80, 0x5f, 0xea,
+		0x88, 0x78, 0xfa, 0xd5, 0xd5, 0xf8, 0x04, 0x80,
+		0x69, 0xf3, 0x4d, 0x38, 0x48, 0xf4, 0x00, 0x48,
+		0xc5, 0xf8, 0x04, 0x80, 0x70, 0x47, 0xd5, 0xf8,
+		0x00, 0x80, 0x5f, 0xea, 0x88, 0x78, 0xfa, 0xd5,
+		0xd5, 0xf8, 0x00, 0x80, 0x5f, 0xea, 0x48, 0x68,
+		0xfa, 0xd5, 0xd5, 0xf8, 0x04, 0x80, 0x48, 0xf4,
+		0x80, 0x48, 0xc5, 0xf8, 0x04, 0x80, 0xd5, 0xf8,
+		0x04, 0x80, 0x5f, 0xea, 0x08, 0x48, 0xfa, 0xd4,
+		0xd5, 0xf8, 0x00, 0x80, 0x28, 0xf0, 0x01, 0x08,
+		0xc5, 0xf8, 0x00, 0x80, 0xd5, 0xf8, 0x00, 0x80,
+		0x5f, 0xea, 0x88, 0x78, 0xfa, 0xd5, 0x70, 0x47,
+		0x00, 0x20, 0x50, 0x60, 0x30, 0x46, 0x00, 0xbe
+	};
+
+	if (target_alloc_working_area(target, sizeof(mrvlqspi_flash_write_code),
+			&write_algorithm) != ERROR_OK) {
+		LOG_ERROR("Insufficient working area. You must configure"\
+			" a working area > %zdB in order to write to SPIFI flash.",
+			sizeof(mrvlqspi_flash_write_code));
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	};
+
+	retval = target_write_buffer(target, write_algorithm->address,
+			sizeof(mrvlqspi_flash_write_code),
+			mrvlqspi_flash_write_code);
+	if (retval != ERROR_OK) {
+		target_free_working_area(target, write_algorithm);
+		return retval;
+	}
+
+	/* FIFO allocation */
+	fifo_size = target_get_working_area_avail(target);
+
+	if (fifo_size == 0) {
+		/* if we already allocated the writing code but failed to get fifo
+		 * space, free the algorithm */
+		target_free_working_area(target, write_algorithm);
+
+		LOG_ERROR("Insufficient working area. Please allocate at least"\
+			" %zdB of working area to enable flash writes.",
+			sizeof(mrvlqspi_flash_write_code) + 1
+		);
+
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	} else if (fifo_size < page_size)
+		LOG_WARNING("Working area size is limited; flash writes may be"\
+			" slow. Increase working area size to at least %zdB"\
+			" to reduce write times.",
+			(size_t)(sizeof(mrvlqspi_flash_write_code) + page_size)
+		);
+
+	if (target_alloc_working_area(target, fifo_size, &fifo) != ERROR_OK) {
+		target_free_working_area(target, write_algorithm);
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	};
+
+	armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
+	armv7m_info.core_mode = ARM_MODE_THREAD;
+
+	init_reg_param(&reg_params[0], "r0", 32, PARAM_IN_OUT);	/* buffer start, status (out) */
+	init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);	/* buffer end */
+	init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);	/* target address */
+	init_reg_param(&reg_params[3], "r3", 32, PARAM_OUT);	/* count (halfword-16bit) */
+	init_reg_param(&reg_params[4], "r4", 32, PARAM_OUT);	/* page size */
+	init_reg_param(&reg_params[5], "r5", 32, PARAM_OUT);	/* qspi base address */
+
+	buf_set_u32(reg_params[0].value, 0, 32, fifo->address);
+	buf_set_u32(reg_params[1].value, 0, 32, fifo->address + fifo->size);
+	buf_set_u32(reg_params[2].value, 0, 32, offset);
+	buf_set_u32(reg_params[3].value, 0, 32, count);
+	buf_set_u32(reg_params[4].value, 0, 32, page_size);
+	buf_set_u32(reg_params[5].value, 0, 32, (uint32_t) mrvlqspi_info->reg_base);
+
+	retval = target_run_flash_async_algorithm(target, buffer, count, 1,
+			0, NULL,
+			6, reg_params,
+			fifo->address, fifo->size,
+			write_algorithm->address, 0,
+			&armv7m_info
+	);
+
+	if (retval != ERROR_OK)
+		LOG_ERROR("Error executing flash write algorithm");
+
+	target_free_working_area(target, fifo);
+	target_free_working_area(target, write_algorithm);
+
+	destroy_reg_param(&reg_params[0]);
+	destroy_reg_param(&reg_params[1]);
+	destroy_reg_param(&reg_params[2]);
+	destroy_reg_param(&reg_params[3]);
+	destroy_reg_param(&reg_params[4]);
+	destroy_reg_param(&reg_params[5]);
+
+	return retval;
+}
+
+int mrvlqspi_flash_read(struct flash_bank *bank, uint8_t *buffer,
+				uint32_t offset, uint32_t count)
+{
+	struct target *target = bank->target;
+	struct mrvlqspi_flash_bank *mrvlqspi_info = bank->driver_priv;
+	int retval;
+	uint32_t i;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (!(mrvlqspi_info->probed)) {
+		LOG_ERROR("Flash bank not probed");
+		return ERROR_FLASH_BANK_NOT_PROBED;
+	}
+
+	/* Flush read/write fifo's */
+	retval = mrvlqspi_fifo_flush(bank, FIFO_FLUSH_TIMEOUT);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Set instruction/addr count value */
+	retval = mrvlqspi_set_hdr_cnt(bank, (0x1 | (0x3 << 4)));
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Set count for number of bytes to read */
+	retval = mrvlqspi_set_din_cnt(bank, count);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Set read address */
+	retval = mrvlqspi_set_addr(bank, offset);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Set instruction */
+	retval = mrvlqspi_set_instr(bank, SPIFLASH_READ);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Set data and addr pin length */
+	retval = mrvlqspi_set_conf(bank, 0x0);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = mrvlqspi_start_transfer(bank, QSPI_R_EN);
+	if (retval != ERROR_OK)
+		return retval;
+
+	for (i = 0; i < count; i++) {
+		retval = mrvlqspi_read_byte(bank, &buffer[i]);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	retval = mrvlqspi_set_ss_state(bank, QSPI_SS_DISABLE, QSPI_TIMEOUT);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return ERROR_OK;
+}
+
+static int mrvlqspi_probe(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	struct mrvlqspi_flash_bank *mrvlqspi_info = bank->driver_priv;
+	uint32_t id = 0;
+	int retval;
+	struct flash_sector *sectors;
+
+	/* If we've already probed, we should be fine to skip this time. */
+	if (mrvlqspi_info->probed)
+		return ERROR_OK;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	mrvlqspi_info->probed = 0;
+	mrvlqspi_info->bank_num = bank->bank_number;
+
+	/* Read flash JEDEC ID */
+	retval = mrvlqspi_read_id(bank, &id);
+	if (retval != ERROR_OK)
+		return retval;
+
+	mrvlqspi_info->dev = NULL;
+	for (const struct flash_device *p = flash_devices; p->name ; p++)
+		if (p->device_id == id) {
+			mrvlqspi_info->dev = p;
+			break;
+		}
+
+	if (!mrvlqspi_info->dev) {
+		LOG_ERROR("Unknown flash device ID 0x%08" PRIx32, id);
+		return ERROR_FAIL;
+	}
+
+	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;
+
+	/* create and fill sectors array */
+	bank->num_sectors = mrvlqspi_info->dev->size_in_bytes /
+					mrvlqspi_info->dev->sectorsize;
+	sectors = malloc(sizeof(struct flash_sector) * bank->num_sectors);
+	if (sectors == NULL) {
+		LOG_ERROR("not enough memory");
+		return ERROR_FAIL;
+	}
+
+	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].is_erased = -1;
+		sectors[sector].is_protected = 0;
+	}
+
+	bank->sectors = sectors;
+	mrvlqspi_info->probed = 1;
+
+	return ERROR_OK;
+}
+
+static int mrvlqspi_auto_probe(struct flash_bank *bank)
+{
+	struct mrvlqspi_flash_bank *mrvlqspi_info = bank->driver_priv;
+	if (mrvlqspi_info->probed)
+		return ERROR_OK;
+	return mrvlqspi_probe(bank);
+}
+
+static int mrvlqspi_flash_erase_check(struct flash_bank *bank)
+{
+	/* Not implemented yet */
+	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;
+
+	if (!(mrvlqspi_info->probed)) {
+		snprintf(buf, buf_size,
+			"\nQSPI flash bank not probed yet\n");
+		return ERROR_OK;
+	}
+
+	snprintf(buf, buf_size, "\nQSPI flash information:\n"
+		"  Device \'%s\' ID 0x%08" PRIx32 "\n",
+		mrvlqspi_info->dev->name, mrvlqspi_info->dev->device_id);
+
+	return ERROR_OK;
+}
+
+FLASH_BANK_COMMAND_HANDLER(mrvlqspi_flash_bank_command)
+{
+	struct mrvlqspi_flash_bank *mrvlqspi_info;
+
+	if (CMD_ARGC < 7)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	mrvlqspi_info = malloc(sizeof(struct mrvlqspi_flash_bank));
+	if (mrvlqspi_info == NULL) {
+		LOG_ERROR("not enough memory");
+		return ERROR_FAIL;
+	}
+
+	/* Get QSPI controller register map base address */
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[6], mrvlqspi_info->reg_base);
+	bank->driver_priv = mrvlqspi_info;
+	mrvlqspi_info->probed = 0;
+
+	return ERROR_OK;
+}
+
+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,
+};

src/flash/nor/non_cfi.c

@@ -33,7 +33,7 @@
 #define ERASE_REGION(num, size) (((size/256) << 16) | (num-1))
 
 /* non-CFI compatible flashes */
-static struct non_cfi non_cfi_flashes[] = {
+static const struct non_cfi non_cfi_flashes[] = {
 	{
 		.mfr = CFI_MFR_SST,
 		.id = 0xd4,
@@ -472,7 +472,7 @@ void cfi_fixup_non_cfi(struct flash_bank *bank)
 {
 	unsigned int mask;
 	struct cfi_flash_bank *cfi_info = bank->driver_priv;
-	struct non_cfi *non_cfi = non_cfi_flashes;
+	const struct non_cfi *non_cfi = non_cfi_flashes;
 
 	if (cfi_info->x16_as_x8)
 		mask = 0xFF;

src/flash/nor/nrf51.c

@@ -1,6 +1,8 @@
 /***************************************************************************
  *   Copyright (C) 2013 Synapse Product Development                        *
  *   Andrey Smirnov <andrew.smironv@gmail.com>                             *
+ *   Angus Gratton <gus@projectgus.com>                                    *
+ *   Erdem U. Altunyurt <spamjunkeater@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  *
@@ -23,6 +25,9 @@
 #endif
 
 #include "imp.h"
+#include <target/algorithm.h>
+#include <target/armv7m.h>
+#include <helper/types.h>
 
 enum {
 	NRF51_FLASH_BASE = 0x00000000,
@@ -73,7 +78,7 @@ enum nrf51_uicr_registers {
 	NRF51_UICR_BASE = 0x10001000, /* User Information
 				       * Configuration Regsters */
 
-	NRF51_UICR_SIZE = 252,
+	NRF51_UICR_SIZE = 0x100,
 
 #define NRF51_UICR_REG(offset) (NRF51_UICR_BASE + offset)
 
@@ -123,63 +128,26 @@ struct nrf51_device_spec {
 	unsigned int flash_size_kb;
 };
 
+/* The known devices table below is derived from the "nRF51 Series
+ * Compatibility Matrix" document, which can be found by searching for
+ * ATTN-51 on the Nordic Semi website:
+ *
+ * http://www.nordicsemi.com/eng/content/search?SearchText=ATTN-51
+ *
+ * Up to date with Matrix v2.0, plus some additional HWIDs.
+ *
+ * The additional HWIDs apply where the build code in the matrix is
+ * shown as Gx0, Bx0, etc. In these cases the HWID in the matrix is
+ * for x==0, x!=0 means different (unspecified) HWIDs.
+ */
 static const struct nrf51_device_spec nrf51_known_devices_table[] = {
+	/* nRF51822 Devices (IC rev 1). */
 	{
 		.hwid		= 0x001D,
 		.variant	= "QFAA",
 		.build_code	= "CA/C0",
 		.flash_size_kb	= 256,
 	},
-	{
-		.hwid		= 0x002A,
-		.variant	= "QFAA",
-		.build_code	= "FA",
-		.flash_size_kb	= 256,
-	},
-	{
-		.hwid		= 0x0044,
-		.variant	= "QFAA",
-		.build_code	= "GC",
-		.flash_size_kb	= 256,
-	},
-	{
-		.hwid		= 0x003C,
-		.variant	= "QFAA",
-		.build_code	= "G0",
-		.flash_size_kb	= 256,
-	},
-
-	{
-		.hwid		= 0x0020,
-		.variant	= "CEAA",
-		.build_code	= "BA",
-		.flash_size_kb	= 256,
-	},
-	{
-		.hwid		= 0x002F,
-		.variant	= "CEAA",
-		.build_code	= "B0",
-		.flash_size_kb	= 256,
-	},
-	{
-		.hwid		= 0x0040,
-		.variant	= "CEAA",
-		.build_code	= "CA",
-		.flash_size_kb	= 256,
-	},
-	{
-		.hwid		= 0x0047,
-		.variant	= "CEAA",
-		.build_code	= "DA",
-		.flash_size_kb	= 256,
-	},
-	{
-		.hwid		= 0x004D,
-		.variant	= "CEAA",
-		.build_code	= "D0",
-		.flash_size_kb	= 256,
-	},
-
 	{
 		.hwid		= 0x0026,
 		.variant	= "QFAB",
@@ -192,13 +160,200 @@ static const struct nrf51_device_spec nrf51_known_devices_table[] = {
 		.build_code	= "A0",
 		.flash_size_kb	= 128,
 	},
+	{
+		.hwid		= 0x0020,
+		.variant	= "CEAA",
+		.build_code	= "BA",
+		.flash_size_kb	= 256,
+	},
+	{
+		.hwid		= 0x002F,
+		.variant	= "CEAA",
+		.build_code	= "B0",
+		.flash_size_kb	= 256,
+	},
+
+	/* nRF51822 Devices (IC rev 2). */
+	{
+		.hwid		= 0x002A,
+		.variant	= "QFAA",
+		.build_code	= "FA0",
+		.flash_size_kb	= 256,
+	},
+	{
+		.hwid		= 0x0044,
+		.variant	= "QFAA",
+		.build_code	= "GC0",
+		.flash_size_kb	= 256,
+	},
+	{
+		.hwid		= 0x003C,
+		.variant	= "QFAA",
+		.build_code	= "G0",
+		.flash_size_kb	= 256,
+	},
 	{
 		.hwid		= 0x004C,
 		.variant	= "QFAB",
 		.build_code	= "B0",
 		.flash_size_kb	= 128,
 	},
+	{
+		.hwid		= 0x0040,
+		.variant	= "CEAA",
+		.build_code	= "CA0",
+		.flash_size_kb	= 256,
+	},
+	{
+		.hwid		= 0x0047,
+		.variant	= "CEAA",
+		.build_code	= "DA0",
+		.flash_size_kb	= 256,
+	},
+	{
+		.hwid		= 0x004D,
+		.variant	= "CEAA",
+		.build_code	= "D00",
+		.flash_size_kb	= 256,
+	},
 
+	/* nRF51822 Devices (IC rev 3). */
+	{
+		.hwid		= 0x0072,
+		.variant	= "QFAA",
+		.build_code	= "H0",
+		.flash_size_kb	= 256,
+	},
+	{
+		.hwid		= 0x007B,
+		.variant	= "QFAB",
+		.build_code	= "C0",
+		.flash_size_kb	= 128,
+	},
+	{
+		.hwid		= 0x0083,
+		.variant	= "QFAC",
+		.build_code	= "A0",
+		.flash_size_kb	= 256,
+	},
+	{
+		.hwid		= 0x007D,
+		.variant	= "CDAB",
+		.build_code	= "A0",
+		.flash_size_kb	= 128,
+	},
+	{
+		.hwid		= 0x0079,
+		.variant	= "CEAA",
+		.build_code	= "E0",
+		.flash_size_kb	= 256,
+	},
+	{
+		.hwid		= 0x0087,
+		.variant	= "CFAC",
+		.build_code	= "A0",
+		.flash_size_kb	= 256,
+	},
+
+	/* nRF51422 Devices (IC rev 1). */
+	{
+		.hwid		= 0x001E,
+		.variant	= "QFAA",
+		.build_code	= "CA",
+		.flash_size_kb	= 256,
+	},
+	{
+		.hwid		= 0x0024,
+		.variant	= "QFAA",
+		.build_code	= "C0",
+		.flash_size_kb	= 256,
+	},
+	{
+		.hwid		= 0x0031,
+		.variant	= "CEAA",
+		.build_code	= "A0A",
+		.flash_size_kb	= 256,
+	},
+
+	/* nRF51422 Devices (IC rev 2). */
+	{
+		.hwid		= 0x002D,
+		.variant	= "QFAA",
+		.build_code	= "DAA",
+		.flash_size_kb	= 256,
+	},
+	{
+		.hwid		= 0x002E,
+		.variant	= "QFAA",
+		.build_code	= "E0",
+		.flash_size_kb	= 256,
+	},
+	{
+		.hwid		= 0x0061,
+		.variant	= "QFAB",
+		.build_code	= "A00",
+		.flash_size_kb	= 128,
+	},
+	{
+		.hwid		= 0x0050,
+		.variant	= "CEAA",
+		.build_code	= "B0",
+		.flash_size_kb	= 256,
+	},
+
+	/* nRF51422 Devices (IC rev 3). */
+	{
+		.hwid		= 0x0073,
+		.variant	= "QFAA",
+		.build_code	= "F0",
+		.flash_size_kb	= 256,
+	},
+	{
+		.hwid		= 0x007C,
+		.variant	= "QFAB",
+		.build_code	= "B0",
+		.flash_size_kb	= 128,
+	},
+	{
+		.hwid		= 0x0085,
+		.variant	= "QFAC",
+		.build_code	= "A0",
+		.flash_size_kb	= 256,
+	},
+	{
+		.hwid		= 0x0086,
+		.variant	= "QFAC",
+		.build_code	= "A1",
+		.flash_size_kb	= 256,
+	},
+	{
+		.hwid		= 0x007E,
+		.variant	= "CDAB",
+		.build_code	= "A0",
+		.flash_size_kb	= 128,
+	},
+	{
+		.hwid		= 0x007A,
+		.variant	= "CEAA",
+		.build_code	= "C0",
+		.flash_size_kb	= 256,
+	},
+	{
+		.hwid		= 0x0088,
+		.variant	= "CFAC",
+		.build_code	= "A0",
+		.flash_size_kb	= 256,
+	},
+
+	/* Some early nRF51-DK (PCA10028) & nRF51-Dongle (PCA10031) boards
+	   with built-in jlink seem to use engineering samples not listed
+	   in the nRF51 Series Compatibility Matrix V1.0. */
+	{
+		.hwid		= 0x0071,
+		.variant	= "QFAC",
+		.build_code	= "AB",
+		.flash_size_kb	= 256,
+	},
 };
 
 static int nrf51_bank_is_probed(struct flash_bank *bank)
@@ -248,6 +403,7 @@ static int nrf51_wait_for_nvmc(struct nrf51_info *chip)
 		alive_sleep(1);
 	} while (timeout--);
 
+	LOG_DEBUG("Timed out waiting for NVMC_READY");
 	return ERROR_FLASH_BUSY;
 }
 
@@ -557,19 +713,25 @@ static struct flash_sector *nrf51_find_sector_by_address(struct flash_bank *bank
 
 static int nrf51_erase_all(struct nrf51_info *chip)
 {
+	LOG_DEBUG("Erasing all non-volatile memory");
 	return nrf51_nvmc_generic_erase(chip,
 					NRF51_NVMC_ERASEALL,
 					0x00000001);
 }
 
-static int nrf51_erase_page(struct nrf51_info *chip, struct flash_sector *sector)
+static int nrf51_erase_page(struct flash_bank *bank,
+							struct nrf51_info *chip,
+							struct flash_sector *sector)
 {
 	int res;
 
-	if (sector->is_protected)
+	LOG_DEBUG("Erasing page at 0x%"PRIx32, sector->offset);
+	if (sector->is_protected) {
+		LOG_ERROR("Cannot erase protected sector at 0x%" PRIx32, sector->offset);
 		return ERROR_FAIL;
+	}
 
-	if (sector->offset == NRF51_UICR_BASE) {
+	if (bank->base == NRF51_UICR_BASE) {
 		uint32_t ppfc;
 		res = target_read_u32(chip->target, NRF51_FICR_PPFC,
 				      &ppfc);
@@ -579,6 +741,12 @@ static int nrf51_erase_page(struct nrf51_info *chip, struct flash_sector *sector
 		}
 
 		if ((ppfc & 0xFF) == 0xFF) {
+			/* We can't erase the UICR.  Double-check to
+			   see if it's already erased before complaining. */
+			default_flash_blank_check(bank);
+			if (sector->is_erased == 1)
+				return ERROR_OK;
+
 			LOG_ERROR("The chip was not pre-programmed with SoftDevice stack and UICR cannot be erased separately. Please issue mass erase before trying to write to this region");
 			return ERROR_FAIL;
 		};
@@ -600,55 +768,162 @@ static int nrf51_erase_page(struct nrf51_info *chip, struct flash_sector *sector
 	return res;
 }
 
-static int nrf51_ll_flash_write(struct nrf51_info *chip, uint32_t offset, const uint8_t *buffer, uint32_t buffer_size)
+static const uint8_t nrf51_flash_write_code[] = {
+	/* See contrib/loaders/flash/cortex-m0.S */
+/* <wait_fifo>: */
+	0x0d, 0x68,		/* ldr	r5,	[r1,	#0] */
+	0x00, 0x2d,		/* cmp	r5,	#0 */
+	0x0b, 0xd0,		/* beq.n	1e <exit> */
+	0x4c, 0x68,		/* ldr	r4,	[r1,	#4] */
+	0xac, 0x42,		/* cmp	r4,	r5 */
+	0xf9, 0xd0,		/* beq.n	0 <wait_fifo> */
+	0x20, 0xcc,		/* ldmia	r4!,	{r5} */
+	0x20, 0xc3,		/* stmia	r3!,	{r5} */
+	0x94, 0x42,		/* cmp	r4,	r2 */
+	0x01, 0xd3,		/* bcc.n	18 <no_wrap> */
+	0x0c, 0x46,		/* mov	r4,	r1 */
+	0x08, 0x34,		/* adds	r4,	#8 */
+/* <no_wrap>: */
+	0x4c, 0x60,		/* str	r4, [r1,	#4] */
+	0x04, 0x38,		/* subs	r0, #4 */
+	0xf0, 0xd1,		/* bne.n	0 <wait_fifo> */
+/* <exit>: */
+	0x00, 0xbe		/* bkpt	0x0000 */
+};
+
+
+/* Start a low level flash write for the specified region */
+static int nrf51_ll_flash_write(struct nrf51_info *chip, uint32_t offset, const uint8_t *buffer, uint32_t bytes)
 {
-	int res;
-	assert(buffer_size % 4 == 0);
+	struct target *target = chip->target;
+	uint32_t buffer_size = 8192;
+	struct working_area *write_algorithm;
+	struct working_area *source;
+	uint32_t address = NRF51_FLASH_BASE + offset;
+	struct reg_param reg_params[4];
+	struct armv7m_algorithm armv7m_info;
+	int retval = ERROR_OK;
 
-	for (; buffer_size > 0; buffer_size -= 4) {
-		res = target_write_memory(chip->target, offset, 4, 1, buffer);
-		if (res != ERROR_OK)
-			return res;
 
-		res = nrf51_wait_for_nvmc(chip);
-		if (res != ERROR_OK)
-			return res;
+	LOG_DEBUG("Writing buffer to flash offset=0x%"PRIx32" bytes=0x%"PRIx32, offset, bytes);
+	assert(bytes % 4 == 0);
 
-		offset += 4;
-		buffer += 4;
+	/* allocate working area with flash programming code */
+	if (target_alloc_working_area(target, sizeof(nrf51_flash_write_code),
+			&write_algorithm) != ERROR_OK) {
+		LOG_WARNING("no working area available, falling back to slow memory writes");
+
+		for (; bytes > 0; bytes -= 4) {
+			retval = target_write_memory(chip->target, offset, 4, 1, buffer);
+			if (retval != ERROR_OK)
+				return retval;
+
+			retval = nrf51_wait_for_nvmc(chip);
+			if (retval != ERROR_OK)
+				return retval;
+
+			offset += 4;
+			buffer += 4;
+		}
+
+		return ERROR_OK;
 	}
 
-	return ERROR_OK;
+	LOG_WARNING("using fast async flash loader. This is currently supported");
+	LOG_WARNING("only with ST-Link and CMSIS-DAP. If you have issues, add");
+	LOG_WARNING("\"set WORKAREASIZE 0\" before sourcing nrf51.cfg to disable it");
+
+	retval = target_write_buffer(target, write_algorithm->address,
+				sizeof(nrf51_flash_write_code),
+				nrf51_flash_write_code);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* memory buffer */
+	while (target_alloc_working_area(target, buffer_size, &source) != ERROR_OK) {
+		buffer_size /= 2;
+		buffer_size &= ~3UL; /* Make sure it's 4 byte aligned */
+		if (buffer_size <= 256) {
+			/* free working area, write algorithm already allocated */
+			target_free_working_area(target, write_algorithm);
+
+			LOG_WARNING("No large enough working area available, can't do block memory writes");
+			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+		}
+	}
+
+	armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
+	armv7m_info.core_mode = ARM_MODE_THREAD;
+
+	init_reg_param(&reg_params[0], "r0", 32, PARAM_IN_OUT);	/* byte count */
+	init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);	/* buffer start */
+	init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);	/* buffer end */
+	init_reg_param(&reg_params[3], "r3", 32, PARAM_IN_OUT);	/* target address */
+
+	buf_set_u32(reg_params[0].value, 0, 32, bytes);
+	buf_set_u32(reg_params[1].value, 0, 32, source->address);
+	buf_set_u32(reg_params[2].value, 0, 32, source->address + source->size);
+	buf_set_u32(reg_params[3].value, 0, 32, address);
+
+	retval = target_run_flash_async_algorithm(target, buffer, bytes/4, 4,
+			0, NULL,
+			4, reg_params,
+			source->address, source->size,
+			write_algorithm->address, 0,
+			&armv7m_info);
+
+	target_free_working_area(target, source);
+	target_free_working_area(target, write_algorithm);
+
+	destroy_reg_param(&reg_params[0]);
+	destroy_reg_param(&reg_params[1]);
+	destroy_reg_param(&reg_params[2]);
+	destroy_reg_param(&reg_params[3]);
+
+	return retval;
 }
 
-static int nrf51_write_page(struct flash_bank *bank, uint32_t offset, const uint8_t *buffer)
+/* Check and erase flash sectors in specified range then start a low level page write.
+   start/end must be sector aligned.
+*/
+static int nrf51_write_pages(struct flash_bank *bank, uint32_t start, uint32_t end, const uint8_t *buffer)
 {
-	assert(offset % 4 == 0);
 	int res = ERROR_FAIL;
 	struct nrf51_info *chip = bank->driver_priv;
-	struct flash_sector *sector = nrf51_find_sector_by_address(bank, offset);
+	struct flash_sector *sector;
+	uint32_t offset;
 
-	if (!sector)
-		return ERROR_FLASH_SECTOR_INVALID;
+	assert(start % chip->code_page_size == 0);
+	assert(end % chip->code_page_size == 0);
 
-	if (sector->is_protected)
-		goto error;
+	/* Erase all sectors */
+	for (offset = start; offset < end; offset += chip->code_page_size) {
+		sector = nrf51_find_sector_by_address(bank, offset);
+		if (!sector) {
+			LOG_ERROR("Invalid sector @ 0x%08"PRIx32, offset);
+			return ERROR_FLASH_SECTOR_INVALID;
+		}
 
-	if (!sector->is_erased) {
-		res = nrf51_erase_page(chip, sector);
-		if (res != ERROR_OK) {
-			LOG_ERROR("Failed to erase sector @ 0x%08"PRIx32, sector->offset);
+		if (sector->is_protected) {
+			LOG_ERROR("Can't erase protected sector @ 0x%08"PRIx32, offset);
 			goto error;
 		}
+
+		if (sector->is_erased != 1) {	/* 1 = erased, 0= not erased, -1 = unknown */
+			res = nrf51_erase_page(bank, chip, sector);
+			if (res != ERROR_OK) {
+				LOG_ERROR("Failed to erase sector @ 0x%08"PRIx32, sector->offset);
+				goto error;
+			}
+		}
+		sector->is_erased = 0;
 	}
 
 	res = nrf51_nvmc_write_enable(chip);
 	if (res != ERROR_OK)
 		goto error;
 
-	sector->is_erased = 0;
-
-	res = nrf51_ll_flash_write(chip, offset, buffer, chip->code_page_size);
+	res = nrf51_ll_flash_write(chip, start, buffer, (end - start));
 	if (res != ERROR_OK)
 		goto set_read_only;
 
@@ -657,7 +932,7 @@ static int nrf51_write_page(struct flash_bank *bank, uint32_t offset, const uint
 set_read_only:
 	nrf51_nvmc_read_only(chip);
 error:
-	LOG_ERROR("Failed to write sector @ 0x%08"PRIx32, sector->offset);
+	LOG_ERROR("Failed to write to nrf51 flash");
 	return res;
 }
 
@@ -672,7 +947,7 @@ static int nrf51_erase(struct flash_bank *bank, int first, int last)
 
 	/* For each sector to be erased */
 	for (int s = first; s <= last && res == ERROR_OK; s++)
-		res = nrf51_erase_page(chip, &bank->sectors[s]);
+		res = nrf51_erase_page(bank, chip, &bank->sectors[s]);
 
 	return res;
 }
@@ -681,79 +956,51 @@ static int nrf51_code_flash_write(struct flash_bank *bank,
 				  struct nrf51_info *chip,
 				  const uint8_t *buffer, uint32_t offset, uint32_t count)
 {
+
 	int res;
-	struct {
-		uint32_t start, end;
-	} region;
+	/* Need to perform reads to fill any gaps we need to preserve in the first page,
+	   before the start of buffer, or in the last page, after the end of buffer */
+	uint32_t first_page = offset/chip->code_page_size;
+	uint32_t last_page = DIV_ROUND_UP(offset+count, chip->code_page_size);
 
-	region.start = offset;
-	region.end   = offset + count;
+	uint32_t first_page_offset = first_page * chip->code_page_size;
+	uint32_t last_page_offset = last_page * chip->code_page_size;
 
-	struct {
-		size_t   length;
-		const uint8_t *buffer;
-	}  start_extra, end_extra;
+	LOG_DEBUG("Padding write from 0x%08"PRIx32"-0x%08"PRIx32" as 0x%08"PRIx32"-0x%08"PRIx32,
+		offset, offset+count, first_page_offset, last_page_offset);
 
-	start_extra.length	= region.start % chip->code_page_size;
-	start_extra.buffer	= buffer;
-	end_extra.length	= region.end  % chip->code_page_size;
-	end_extra.buffer	= buffer + count - end_extra.length;
-
-	if (start_extra.length) {
-		uint8_t page[chip->code_page_size];
+	uint32_t page_cnt = last_page - first_page;
+	uint8_t buffer_to_flash[page_cnt*chip->code_page_size];
 
+	/* Fill in any space between start of first page and start of buffer */
+	uint32_t pre = offset - first_page_offset;
+	if (pre > 0) {
 		res = target_read_memory(bank->target,
-					 region.start - start_extra.length,
-					 1, start_extra.length, page);
-		if (res != ERROR_OK)
-			return res;
-
-		memcpy(page + start_extra.length,
-		       start_extra.buffer,
-		       chip->code_page_size - start_extra.length);
-
-		res = nrf51_write_page(bank,
-				       region.start - start_extra.length,
-				       page);
+					first_page_offset,
+					1,
+					pre,
+					buffer_to_flash);
 		if (res != ERROR_OK)
 			return res;
 	}
 
-	if (end_extra.length) {
-		uint8_t page[chip->code_page_size];
+	/* Fill in main contents of buffer */
+	memcpy(buffer_to_flash+pre, buffer, count);
 
+	/* Fill in any space between end of buffer and end of last page */
+	uint32_t post = last_page_offset - (offset+count);
+	if (post > 0) {
 		/* Retrieve the full row contents from Flash */
 		res = target_read_memory(bank->target,
-					 region.end,
-					 1,
-					 (chip->code_page_size - end_extra.length),
-					 page + end_extra.length);
-		if (res != ERROR_OK)
-			return res;
-
-		memcpy(page, end_extra.buffer, end_extra.length);
-
-		res = nrf51_write_page(bank,
-				       region.end - end_extra.length,
-				       page);
+					offset + count,
+					1,
+					post,
+					buffer_to_flash+pre+count);
 		if (res != ERROR_OK)
 			return res;
 	}
 
-
-	region.start += start_extra.length;
-	region.end   -= end_extra.length;
-
-	for (uint32_t address = region.start; address < region.end;
-	     address += chip->code_page_size) {
-		res = nrf51_write_page(bank, address, &buffer[address - region.start]);
-
-		if (res != ERROR_OK)
-			return res;
-
-	}
-
-	return ERROR_OK;
+	return nrf51_write_pages(bank, first_page_offset, last_page_offset, buffer_to_flash);
 }
 
 static int nrf51_uicr_flash_write(struct flash_bank *bank,
@@ -776,8 +1023,8 @@ static int nrf51_uicr_flash_write(struct flash_bank *bank,
 	if (res != ERROR_OK)
 		return res;
 
-	if (!sector->is_erased) {
-		res = nrf51_erase_page(chip, sector);
+	if (sector->is_erased != 1) {
+		res = nrf51_erase_page(bank, chip, sector);
 		if (res != ERROR_OK)
 			return res;
 	}
@@ -920,8 +1167,9 @@ static int nrf51_info(struct flash_bank *bank, char *buf, int buf_size)
 	if (res != ERROR_OK)
 		return res;
 
-	struct {
-		uint32_t address, value;
+	static struct {
+		const uint32_t address;
+		uint32_t value;
 	} ficr[] = {
 		{ .address = NRF51_FICR_CODEPAGESIZE	},
 		{ .address = NRF51_FICR_CODESIZE	},

src/flash/nor/nuc1x.c

@@ -328,7 +328,7 @@ static int nuc1x_erase(struct flash_bank *bank, int first, int last)
 		return retval;
 
 	for (i = first; i <= last; i++) {
-		LOG_DEBUG("erasing sector %d at addresss 0x%" PRIx32 "", i, bank->base + bank->sectors[i].offset);
+		LOG_DEBUG("erasing sector %d at address 0x%" PRIx32 "", i, bank->base + bank->sectors[i].offset);
 		retval = target_write_u32(target, NUC1X_FLASH_ISPADR, bank->base + bank->sectors[i].offset);
 		if (retval != ERROR_OK)
 			return retval;
@@ -389,7 +389,7 @@ static int nuc1x_write(struct flash_bank *bank, const uint8_t *buffer,
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
-	LOG_INFO("Novoton NUC: FLASH Write ...");
+	LOG_INFO("Nuvoton NUC: FLASH Write ...");
 
 	int retval = nuc1x_reset2lprom(bank);
 	if (retval != ERROR_OK)
@@ -452,7 +452,7 @@ static int nuc1x_write(struct flash_bank *bank, const uint8_t *buffer,
 			if (retval != ERROR_OK)
 				return retval;
 		} else {
-			LOG_DEBUG("writed OK");
+			LOG_DEBUG("Write OK");
 		}
 	}
 
@@ -520,7 +520,7 @@ static int nuc1x_probe(struct flash_bank *bank)
 
 	nuc1x_info->probed = 1;
 
-	LOG_DEBUG("Novoton NUC: Probed ...");
+	LOG_DEBUG("Nuvoton NUC: Probed ...");
 
 	return ERROR_OK;
 }
@@ -574,7 +574,7 @@ static int nuc1x_mass_erase(struct flash_bank *bank)
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
-	LOG_INFO("Novoton NUC: Chip Erase ... (may take several seconds)");
+	LOG_INFO("Nuvoton NUC: Chip Erase ... (may take several seconds)");
 
 	return retval;
 }

src/flash/nor/psoc4.c

@@ -0,0 +1,809 @@
+/***************************************************************************
+ *   Copyright (C) 2005 by Dominic Rath                                    *
+ *   Dominic.Rath@gmx.de                                                   *
+ *                                                                         *
+ *   Copyright (C) 2008 by Spencer Oliver                                  *
+ *   spen@spen-soft.co.uk                                                  *
+ *                                                                         *
+ *   Copyright (C) 2011 by Andreas Fritiofson                              *
+ *   andreas.fritiofson@gmail.com                                          *
+ *                                                                         *
+ *   Copyright (C) 2014 by Tomas Vanek (PSoC 4 support derived from STM32) *
+ *   vanekt@fbl.cz                                                         *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (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.                          *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "imp.h"
+#include <helper/binarybuffer.h>
+#include <jtag/jtag.h>
+#include <target/algorithm.h>
+#include <target/armv7m.h>
+
+/* device documets:
+
+ PSoC(R) 4: PSoC 4200 Family Datasheet
+	Document Number: 001-87197 Rev. *B  Revised August 29, 2013
+
+ PSoC 4100/4200 Family PSoC(R) 4 Architecture TRM
+	Document No. 001-85634 Rev. *C March 25, 2014
+
+ PSoC(R) 4 Registers TRM Spec.
+	Document No. 001-85847 Rev. *A June 25, 2013
+
+ CY8C41xx, CY8C42xx Programming Specifications
+	Document No. 001-81799 Rev. *C March 4, 2014
+*/
+
+/* register locations */
+#define PSOC4_CPUSS_SYSREQ	0x40000004
+#define PSOC4_CPUSS_SYSARG	0x40000008
+#define PSOC4_TEST_MODE		0x40030014
+#define PSOC4_SPCIF_GEOMETRY	0x400E0000
+
+#define PSOC4_SFLASH_MACRO	0x0ffff000
+
+/* constants */
+#define PSOC4_SROM_KEY1			0xb6
+#define PSOC4_SROM_KEY2			0xd3
+#define PSOC4_SROM_SYSREQ_BIT		(1<<31)
+#define PSOC4_SROM_HMASTER_BIT		(1<<30)
+#define PSOC4_SROM_PRIVILEGED_BIT	(1<<28)
+#define PSOC4_SROM_STATUS_SUCCEEDED	0xa0000000
+#define PSOC4_SROM_STATUS_FAILED	0xf0000000
+
+#define PSOC4_CMD_GET_SILICON_ID	0
+#define PSOC4_CMD_LOAD_LATCH		4
+#define PSOC4_CMD_WRITE_ROW		5
+#define PSOC4_CMD_PROGRAM_ROW		6
+#define PSOC4_CMD_ERASE_ALL		0xa
+#define PSOC4_CMD_CHECKSUM		0xb
+#define PSOC4_CMD_WRITE_PROTECTION	0xd
+
+#define PSOC4_CHIP_PROT_VIRGIN		0x0
+#define PSOC4_CHIP_PROT_OPEN		0x1
+#define PSOC4_CHIP_PROT_PROTECTED	0x2
+#define PSOC4_CHIP_PROT_KILL		0x4
+
+
+struct psoc4_chip_details {
+	uint16_t id;
+	const char *type;
+	const char *package;
+	uint32_t flash_size_in_kb;
+};
+
+/* list of PSoC 4 chips
+ * flash_size_in_kb is not necessary as it can be decoded from SPCIF_GEOMETRY
+ */
+const struct psoc4_chip_details psoc4_devices[] = {
+	/* 4200 series */
+	{ 0x04A6, "CY8C4245PVI-482", "SSOP-28", .flash_size_in_kb = 32 },
+	{ 0x04B6, "CY8C4245LQI-483", "QFN-40",  .flash_size_in_kb = 32 },
+	{ 0x04C8, "CY8C4245AXI-483", "TQFP-44", .flash_size_in_kb = 32 },
+	{ 0x04FB, "CY8C4245AXI-473", "TQFP-44", .flash_size_in_kb = 32 },
+	{ 0x04F0, "CY8C4244PVI-432", "SSOP-28", .flash_size_in_kb = 16 },
+	{ 0x04F1, "CY8C4244PVI-442", "SSOP-28", .flash_size_in_kb = 16 },
+	{ 0x04F6, "CY8C4244LQI-443", "QFN-40",  .flash_size_in_kb = 16 },
+	{ 0x04FA, "CY8C4244AXI-443", "TQFP-44", .flash_size_in_kb = 16 },
+
+	/* 4100 series */
+	{ 0x0410, "CY8C4124PVI-432", "SSOP-28", .flash_size_in_kb = 16 },
+	{ 0x0411, "CY8C4124PVI-442", "SSOP-28", .flash_size_in_kb = 16 },
+	{ 0x0416, "CY8C4124LQI-443", "QFN-40",  .flash_size_in_kb = 16 },
+	{ 0x041A, "CY8C4124AXI-443", "TQFP-44", .flash_size_in_kb = 16 },
+	{ 0x041B, "CY8C4125AXI-473", "TQFP-44", .flash_size_in_kb = 32 },
+	{ 0x0412, "CY8C4125PVI-482", "SSOP-28", .flash_size_in_kb = 32 },
+	{ 0x0417, "CY8C4125LQI-483", "QFN-40",  .flash_size_in_kb = 32 },
+	{ 0x041C, "CY8C4125AXI-483", "TQFP-44", .flash_size_in_kb = 32 },
+
+	/* CCG1 series */
+	{ 0x0490, "CYPD1103-35FNXI", "CSP-35",  .flash_size_in_kb = 32 },
+	{ 0x0489, "CYPD1121-40LQXI", "QFN-40",  .flash_size_in_kb = 32 },
+	{ 0x048A, "CYPD1122-40LQXI", "QFN-40",  .flash_size_in_kb = 32 },
+	{ 0x0491, "CYPD1131-35FNXI", "CSP-35",  .flash_size_in_kb = 32 },
+	{ 0x0498, "CYPD1132-16SXI",  "SOIC-16", .flash_size_in_kb = 32 },
+	{ 0x0481, "CYPD1134-28PVXI", "SSOP-28", .flash_size_in_kb = 32 },
+	{ 0x048B, "CYPD1134-40LQXI", "QFN-40",  .flash_size_in_kb = 32 },
+};
+
+
+struct psoc4_flash_bank {
+	uint32_t row_size;
+	uint32_t user_bank_size;
+	int probed;
+	uint32_t silicon_id;
+	uint8_t chip_protection;
+	uint8_t cmd_program_row;
+};
+
+
+static const struct psoc4_chip_details *psoc4_details_by_id(uint32_t silicon_id)
+{
+	const struct psoc4_chip_details *p = psoc4_devices;
+	unsigned int i;
+	uint16_t id = silicon_id >> 16; /* ignore die revision */
+	for (i = 0; i < sizeof(psoc4_devices)/sizeof(psoc4_devices[0]); i++, p++) {
+		if (p->id == id)
+			return p;
+	}
+	LOG_DEBUG("Unknown PSoC 4 device silicon id 0x%08" PRIx32 ".", silicon_id);
+	return NULL;
+}
+
+static const char *psoc4_decode_chip_protection(uint8_t protection)
+{
+	switch (protection) {
+	case PSOC4_CHIP_PROT_VIRGIN:
+		return "protection VIRGIN";
+	case PSOC4_CHIP_PROT_OPEN:
+		return "protection open";
+	case PSOC4_CHIP_PROT_PROTECTED:
+		return "PROTECTED";
+	case PSOC4_CHIP_PROT_KILL:
+		return "protection KILL";
+	default:
+		LOG_WARNING("Unknown protection state 0x%02" PRIx8 "", protection);
+		return "";
+	}
+}
+
+
+/* flash bank <name> psoc <base> <size> 0 0 <target#>
+ */
+FLASH_BANK_COMMAND_HANDLER(psoc4_flash_bank_command)
+{
+	struct psoc4_flash_bank *psoc4_info;
+
+	if (CMD_ARGC < 6)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	psoc4_info = calloc(1, sizeof(struct psoc4_flash_bank));
+
+	bank->driver_priv = psoc4_info;
+	psoc4_info->user_bank_size = bank->size;
+
+	return ERROR_OK;
+}
+
+
+/* PSoC 4 system ROM request
+ *  Setting SROM_SYSREQ_BIT in CPUSS_SYSREQ register runs NMI service
+ *  in sysrem ROM. Algorithm just waits for NMI to finish.
+ *  When sysreq_params_size == 0 only one parameter is passed in CPUSS_SYSARG register.
+ *  Otherwise address of memory parameter block is set in CPUSS_SYSARG
+ *  and the first parameter is written to the first word of parameter block
+ */
+static int psoc4_sysreq(struct target *target, uint8_t cmd, uint16_t cmd_param,
+		uint32_t *sysreq_params, uint32_t sysreq_params_size)
+{
+	struct working_area *sysreq_wait_algorithm;
+	struct working_area *sysreq_mem;
+
+	struct reg_param reg_params[1];
+	struct armv7m_algorithm armv7m_info;
+
+	int retval = ERROR_OK;
+
+	uint32_t param1 = PSOC4_SROM_KEY1
+			 | ((PSOC4_SROM_KEY2 + cmd) << 8)
+			 | (cmd_param << 16);
+
+	static uint8_t psoc4_sysreq_wait_code[] = {
+		/* system request NMI is served immediately after algo run
+       now we are done: break */
+		0x00, 0xbe,		/* bkpt 0 */
+	};
+
+	const int code_words = (sizeof(psoc4_sysreq_wait_code) + 3) / 4;
+					/* stack must be aligned */
+	const int stack_size = 196;
+	/* tested stack sizes on PSoC 4:
+		ERASE_ALL	144
+		PROGRAM_ROW	112
+		other sysreq	 68
+	*/
+
+	/* allocate area for sysreq wait code and stack */
+	if (target_alloc_working_area(target, code_words * 4 + stack_size,
+			&sysreq_wait_algorithm) != ERROR_OK) {
+		LOG_DEBUG("no working area for sysreq code");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	};
+
+	/* Write the code */
+	retval = target_write_buffer(target,
+			sysreq_wait_algorithm->address,
+			sizeof(psoc4_sysreq_wait_code),
+			psoc4_sysreq_wait_code);
+	if (retval != ERROR_OK) {
+		/* we already allocated the writing code, but failed to get a
+		 * buffer, free the algorithm */
+		goto cleanup_algo;
+	}
+
+	if (sysreq_params_size) {
+		/* Allocate memory for sysreq_params */
+		retval = target_alloc_working_area(target, sysreq_params_size, &sysreq_mem);
+		if (retval != ERROR_OK) {
+			LOG_WARNING("no working area for sysreq parameters");
+
+			/* we already allocated the writing code, but failed to get a
+			 * buffer, free the algorithm */
+			retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+			goto cleanup_algo;
+		}
+
+		/* Write sysreq_params */
+		sysreq_params[0] = param1;
+		retval = target_write_buffer(target, sysreq_mem->address,
+				sysreq_params_size, (uint8_t *)sysreq_params);
+		if (retval != ERROR_OK)
+			goto cleanup_mem;
+
+		/* Set address of sysreq parameters block */
+		retval = target_write_u32(target, PSOC4_CPUSS_SYSARG, sysreq_mem->address);
+		if (retval != ERROR_OK)
+			goto cleanup_mem;
+
+	} else {
+		/* Sysreq without memory block of parameters */
+		/* Set register parameter */
+		retval = target_write_u32(target, PSOC4_CPUSS_SYSARG, param1);
+		if (retval != ERROR_OK)
+			goto cleanup_mem;
+	}
+
+	armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
+	armv7m_info.core_mode = ARM_MODE_THREAD;
+
+	/* sysreq stack */
+	init_reg_param(&reg_params[0], "sp", 32, PARAM_OUT);
+	buf_set_u32(reg_params[0].value, 0, 32,
+		    sysreq_wait_algorithm->address + sysreq_wait_algorithm->size);
+
+	struct armv7m_common *armv7m = target_to_armv7m(target);
+	if (armv7m == NULL) {
+
+		/* something is very wrong if armv7m is NULL */
+		LOG_ERROR("unable to get armv7m target");
+		goto cleanup;
+	}
+
+	/* Set SROM request */
+	retval = target_write_u32(target, PSOC4_CPUSS_SYSREQ,
+				  PSOC4_SROM_SYSREQ_BIT | PSOC4_SROM_HMASTER_BIT | cmd);
+	if (retval != ERROR_OK)
+		goto cleanup;
+
+	/* Execute wait code */
+	retval = target_run_algorithm(target, 0, NULL,
+				sizeof(reg_params) / sizeof(*reg_params), reg_params,
+				sysreq_wait_algorithm->address, 0, 1000, &armv7m_info);
+	if (retval != ERROR_OK)
+		LOG_ERROR("sysreq wait code execution failed");
+
+cleanup:
+	destroy_reg_param(&reg_params[0]);
+
+cleanup_mem:
+	if (sysreq_params_size)
+		target_free_working_area(target, sysreq_mem);
+
+cleanup_algo:
+	target_free_working_area(target, sysreq_wait_algorithm);
+
+	return retval;
+}
+
+
+/* helper routine to get silicon ID from a PSoC 4 chip */
+static int psoc4_get_silicon_id(struct target *target, uint32_t *silicon_id, uint8_t *protection)
+{
+	uint32_t params = PSOC4_SROM_KEY1
+			 | ((PSOC4_SROM_KEY2 + PSOC4_CMD_GET_SILICON_ID) << 8);
+	uint32_t part0, part1;
+
+	int retval = psoc4_sysreq(target, PSOC4_CMD_GET_SILICON_ID, 0, NULL, 0);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = target_read_u32(target, PSOC4_CPUSS_SYSARG, &part0);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (part0 == params) {
+		LOG_ERROR("sysreq silicon id request not served");
+		return ERROR_FAIL;
+	}
+
+	retval = target_read_u32(target, PSOC4_CPUSS_SYSREQ, &part1);
+	if (retval != ERROR_OK)
+		return retval;
+
+	uint32_t silicon = ((part0 & 0xffff) << 16)
+			| (((part0 >> 16) & 0xff) << 8)
+			| (part1 & 0xff);
+	uint8_t prot = (part1 >> 12) & 0xff;
+
+	if (silicon_id)
+			*silicon_id = silicon;
+	if (protection)
+			*protection = prot;
+
+	LOG_DEBUG("silicon id: 0x%08" PRIx32 "", silicon);
+	LOG_DEBUG("protection: 0x%02" PRIx8 "", prot);
+	return retval;
+}
+
+
+static int psoc4_protect_check(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	struct psoc4_flash_bank *psoc4_info = bank->driver_priv;
+
+	uint32_t prot_addr = PSOC4_SFLASH_MACRO;
+	uint32_t protection;
+	int i, s;
+	int num_bits;
+	int retval = ERROR_OK;
+
+	num_bits = bank->num_sectors;
+
+	for (i = 0; i < num_bits; i += 32) {
+		retval = target_read_u32(target, prot_addr, &protection);
+		if (retval != ERROR_OK)
+			return retval;
+
+		prot_addr += 4;
+
+		for (s = 0; s < 32; s++) {
+			if (i + s >= num_bits)
+				break;
+			bank->sectors[i + s].is_protected = (protection & (1 << s)) ? 1 : 0;
+		}
+	}
+
+	retval = psoc4_get_silicon_id(target, NULL, &(psoc4_info->chip_protection));
+	return retval;
+}
+
+
+static int psoc4_mass_erase(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	int i;
+
+	if (bank->target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* Call "Erase All" system ROM API */
+	uint32_t param;
+	int retval = psoc4_sysreq(target, PSOC4_CMD_ERASE_ALL,
+			0,
+			&param, sizeof(param));
+
+	if (retval == ERROR_OK)
+		/* set all sectors as erased */
+		for (i = 0; i < bank->num_sectors; i++)
+			bank->sectors[i].is_erased = 1;
+
+	return retval;
+}
+
+
+static int psoc4_erase(struct flash_bank *bank, int first, int last)
+{
+	struct psoc4_flash_bank *psoc4_info = bank->driver_priv;
+	if (psoc4_info->cmd_program_row == PSOC4_CMD_WRITE_ROW) {
+		LOG_INFO("Autoerase enabled, erase command ignored");
+		return ERROR_OK;
+	}
+
+	if ((first == 0) && (last == (bank->num_sectors - 1)))
+		return psoc4_mass_erase(bank);
+
+	LOG_ERROR("Only mass erase available");
+
+	return ERROR_FAIL;
+}
+
+
+static int psoc4_protect(struct flash_bank *bank, int set, int first, int last)
+{
+	struct target *target = bank->target;
+	struct psoc4_flash_bank *psoc4_info = bank->driver_priv;
+
+	if (psoc4_info->probed == 0)
+		return ERROR_FAIL;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	uint32_t *sysrq_buffer = NULL;
+	int retval;
+	int num_bits = bank->num_sectors;
+	const int param_sz = 8;
+	int prot_sz = num_bits / 8;
+	int chip_prot = PSOC4_CHIP_PROT_OPEN;
+	int flash_macro = 0; /* PSoC 42xx has only macro 0 */
+	int i;
+
+	sysrq_buffer = calloc(1, param_sz + prot_sz);
+	if (sysrq_buffer == NULL) {
+		LOG_ERROR("no memory for row buffer");
+		return ERROR_FAIL;
+	}
+
+	for (i = first; i < num_bits && i <= last; i++)
+		bank->sectors[i].is_protected = set;
+
+	uint32_t *p = sysrq_buffer + 2;
+	for (i = 0; i < num_bits; i++) {
+		if (bank->sectors[i].is_protected)
+			p[i / 32] |= 1 << (i % 32);
+	}
+
+	/* Call "Load Latch" system ROM API */
+	sysrq_buffer[1] = prot_sz - 1;
+	retval = psoc4_sysreq(target, PSOC4_CMD_LOAD_LATCH,
+			0,	/* Byte number in latch from what to write */
+			sysrq_buffer, param_sz + psoc4_info->row_size);
+	if (retval != ERROR_OK)
+		goto cleanup;
+
+	/* Call "Write Protection" system ROM API */
+	retval = psoc4_sysreq(target, PSOC4_CMD_WRITE_PROTECTION,
+			chip_prot | (flash_macro << 8), NULL, 0);
+cleanup:
+	if (retval != ERROR_OK)
+		psoc4_protect_check(bank);
+
+	if (sysrq_buffer)
+		free(sysrq_buffer);
+
+	return retval;
+}
+
+
+COMMAND_HANDLER(psoc4_handle_flash_autoerase_command)
+{
+	if (CMD_ARGC < 1)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	struct psoc4_flash_bank *psoc4_info = bank->driver_priv;
+	bool enable = psoc4_info->cmd_program_row == PSOC4_CMD_WRITE_ROW;
+
+	if (CMD_ARGC >= 2)
+		COMMAND_PARSE_ON_OFF(CMD_ARGV[1], enable);
+
+	if (enable) {
+		psoc4_info->cmd_program_row = PSOC4_CMD_WRITE_ROW;
+		LOG_INFO("Flash auto-erase enabled, non mass erase commands will be ignored.");
+	} else {
+		psoc4_info->cmd_program_row = PSOC4_CMD_PROGRAM_ROW;
+		LOG_INFO("Flash auto-erase disabled. Use psoc mass_erase before flash programming.");
+	}
+
+	return retval;
+}
+
+
+static int psoc4_write(struct flash_bank *bank, const uint8_t *buffer,
+		uint32_t offset, uint32_t count)
+{
+	struct psoc4_flash_bank *psoc4_info = bank->driver_priv;
+	struct target *target = bank->target;
+	uint32_t *sysrq_buffer = NULL;
+	int retval = ERROR_OK;
+	const int param_sz = 8;
+
+	if (bank->target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (offset & 0x1) {
+		LOG_ERROR("offset 0x%08" PRIx32 " breaks required 2-byte alignment", offset);
+		return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
+	}
+
+	sysrq_buffer = malloc(param_sz + psoc4_info->row_size);
+	if (sysrq_buffer == NULL) {
+		LOG_ERROR("no memory for row buffer");
+		return ERROR_FAIL;
+	}
+
+	uint8_t *row_buffer = (uint8_t *)sysrq_buffer + param_sz;
+	uint32_t row_num = offset / psoc4_info->row_size;
+	uint32_t row_offset = offset - row_num * psoc4_info->row_size;
+	if (row_offset)
+		memset(row_buffer, 0, row_offset);
+
+	bool save_poll = jtag_poll_get_enabled();
+	jtag_poll_set_enabled(false);
+
+	while (count) {
+		uint32_t chunk_size = psoc4_info->row_size - row_offset;
+		if (chunk_size > count) {
+			chunk_size = count;
+			memset(row_buffer + chunk_size, 0, psoc4_info->row_size - chunk_size);
+		}
+		memcpy(row_buffer + row_offset, buffer, chunk_size);
+		LOG_DEBUG("offset / row: 0x%08" PRIx32 " / %" PRIu32 ", size %" PRIu32 "",
+				offset, row_offset, chunk_size);
+
+		/* Call "Load Latch" system ROM API */
+		sysrq_buffer[1] = psoc4_info->row_size - 1;
+		retval = psoc4_sysreq(target, PSOC4_CMD_LOAD_LATCH,
+				0,	/* Byte number in latch from what to write */
+				sysrq_buffer, param_sz + psoc4_info->row_size);
+		if (retval != ERROR_OK)
+			goto cleanup;
+
+		/* Call "Program Row" or "Write Row" system ROM API */
+		uint32_t sysrq_param;
+		retval = psoc4_sysreq(target, psoc4_info->cmd_program_row,
+				row_num & 0xffff,
+				&sysrq_param, sizeof(sysrq_param));
+		if (retval != ERROR_OK)
+			goto cleanup;
+
+		buffer += chunk_size;
+		row_num++;
+		row_offset = 0;
+		count -= chunk_size;
+	}
+
+cleanup:
+	jtag_poll_set_enabled(save_poll);
+
+	if (sysrq_buffer)
+		free(sysrq_buffer);
+
+	return retval;
+}
+
+
+static int psoc4_probe(struct flash_bank *bank)
+{
+	struct psoc4_flash_bank *psoc4_info = bank->driver_priv;
+	struct target *target = bank->target;
+	uint32_t flash_size_in_kb = 0;
+	uint32_t max_flash_size_in_kb;
+	uint32_t cpu_id;
+	uint32_t silicon_id;
+	uint32_t row_size;
+	uint32_t base_address = 0x00000000;
+	uint8_t protection;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	psoc4_info->probed = 0;
+	psoc4_info->cmd_program_row = PSOC4_CMD_PROGRAM_ROW;
+
+	/* Get the CPUID from the ARM Core
+	 * http://infocenter.arm.com/help/topic/com.arm.doc.ddi0432c/DDI0432C_cortex_m0_r0p0_trm.pdf 4.2.1 */
+	int retval = target_read_u32(target, 0xE000ED00, &cpu_id);
+	if (retval != ERROR_OK)
+		return retval;
+
+	LOG_DEBUG("cpu id = 0x%08" PRIx32 "", cpu_id);
+
+	/* set page size, protection granularity and max flash size depending on family */
+	switch ((cpu_id >> 4) & 0xFFF) {
+	case 0xc20: /* M0 -> PSoC4 */
+		row_size = 128;
+		max_flash_size_in_kb = 32;
+		break;
+	default:
+		LOG_WARNING("Cannot identify target as a PSoC 4 family.");
+		return ERROR_FAIL;
+	}
+
+	uint32_t spcif_geometry;
+	retval = target_read_u32(target, PSOC4_SPCIF_GEOMETRY, &spcif_geometry);
+	if (retval == ERROR_OK) {
+		row_size = 128 * ((spcif_geometry >> 22) & 3);
+		flash_size_in_kb = (spcif_geometry & 0xffff) * 256 / 1024;
+		LOG_INFO("SPCIF geometry: %" PRIu32 " kb flash, row %" PRIu32 " bytes.",
+			 flash_size_in_kb, row_size);
+	}
+
+	/* Early revisions of ST-Link v2 have some problem reading PSOC4_SPCIF_GEOMETRY
+		and an error is reported late. Dummy read gets this error. */
+	uint32_t dummy;
+	target_read_u32(target, PSOC4_CPUSS_SYSREQ, &dummy);
+
+	/* get silicon ID from target. */
+	retval = psoc4_get_silicon_id(target, &silicon_id, &protection);
+	if (retval != ERROR_OK)
+		return retval;
+
+	const struct psoc4_chip_details *details = psoc4_details_by_id(silicon_id);
+	if (details) {
+		LOG_INFO("%s device detected.", details->type);
+		if (flash_size_in_kb == 0)
+			flash_size_in_kb = details->flash_size_in_kb;
+		else if (flash_size_in_kb != details->flash_size_in_kb)
+			LOG_ERROR("Flash size mismatch");
+	}
+
+	psoc4_info->row_size = row_size;
+	psoc4_info->silicon_id = silicon_id;
+	psoc4_info->chip_protection = protection;
+
+	/* failed reading flash size or flash size invalid (early silicon),
+	 * default to max target family */
+	if (retval != ERROR_OK || flash_size_in_kb == 0xffff || flash_size_in_kb == 0) {
+		LOG_WARNING("PSoC 4 flash size failed, probe inaccurate - assuming %" PRIu32 " k flash",
+			max_flash_size_in_kb);
+		flash_size_in_kb = max_flash_size_in_kb;
+	}
+
+	/* 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 (psoc4_info->user_bank_size) {
+		LOG_INFO("ignoring flash probed value, using configured bank size");
+		flash_size_in_kb = psoc4_info->user_bank_size / 1024;
+	}
+
+	LOG_INFO("flash size = %" PRIu32 " kbytes", flash_size_in_kb);
+
+	/* did we assign flash size? */
+	assert(flash_size_in_kb != 0xffff);
+
+	/* calculate numbers of pages */
+	uint32_t num_rows = flash_size_in_kb * 1024 / row_size;
+
+	/* check that calculation result makes sense */
+	assert(num_rows > 0);
+
+	if (bank->sectors) {
+		free(bank->sectors);
+		bank->sectors = NULL;
+	}
+
+	bank->base = base_address;
+	bank->size = num_rows * row_size;
+	bank->num_sectors = num_rows;
+	bank->sectors = malloc(sizeof(struct flash_sector) * num_rows);
+
+	uint32_t i;
+	for (i = 0; i < num_rows; i++) {
+		bank->sectors[i].offset = i * row_size;
+		bank->sectors[i].size = row_size;
+		bank->sectors[i].is_erased = -1;
+		bank->sectors[i].is_protected = 1;
+	}
+
+	LOG_INFO("flash bank set %" PRIu32 " rows", num_rows);
+	psoc4_info->probed = 1;
+
+	return ERROR_OK;
+}
+
+static int psoc4_auto_probe(struct flash_bank *bank)
+{
+	struct psoc4_flash_bank *psoc4_info = bank->driver_priv;
+	if (psoc4_info->probed)
+		return ERROR_OK;
+	return psoc4_probe(bank);
+}
+
+
+static int get_psoc4_info(struct flash_bank *bank, char *buf, int buf_size)
+{
+	struct psoc4_flash_bank *psoc4_info = bank->driver_priv;
+	int printed = 0;
+
+	if (psoc4_info->probed == 0)
+		return ERROR_FAIL;
+
+	const struct psoc4_chip_details *details = psoc4_details_by_id(psoc4_info->silicon_id);
+
+	if (details) {
+		uint32_t chip_revision = psoc4_info->silicon_id & 0xffff;
+		printed = snprintf(buf, buf_size, "PSoC 4 %s rev 0x%04" PRIx32 " package %s",
+				details->type, chip_revision, details->package);
+	} else
+		printed = snprintf(buf, buf_size, "PSoC 4 silicon id 0x%08" PRIx32 "",
+				psoc4_info->silicon_id);
+
+	buf += printed;
+	buf_size -= printed;
+
+	const char *prot_txt = psoc4_decode_chip_protection(psoc4_info->chip_protection);
+	uint32_t size_in_kb = bank->size / 1024;
+	snprintf(buf, buf_size, " flash %" PRIu32 " kb %s", size_in_kb, prot_txt);
+	return ERROR_OK;
+}
+
+
+COMMAND_HANDLER(psoc4_handle_mass_erase_command)
+{
+	if (CMD_ARGC < 1)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	retval = psoc4_mass_erase(bank);
+	if (retval == ERROR_OK)
+		command_print(CMD_CTX, "psoc mass erase complete");
+	else
+		command_print(CMD_CTX, "psoc mass erase failed");
+
+	return retval;
+}
+
+
+static const struct command_registration psoc4_exec_command_handlers[] = {
+	{
+		.name = "mass_erase",
+		.handler = psoc4_handle_mass_erase_command,
+		.mode = COMMAND_EXEC,
+		.usage = "bank_id",
+		.help = "Erase entire flash device.",
+	},
+	{
+		.name = "flash_autoerase",
+		.handler = psoc4_handle_flash_autoerase_command,
+		.mode = COMMAND_EXEC,
+		.usage = "bank_id on|off",
+		.help = "Set autoerase mode for flash bank.",
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+static const struct command_registration psoc4_command_handlers[] = {
+	{
+		.name = "psoc4",
+		.mode = COMMAND_ANY,
+		.help = "PSoC 4 flash command group",
+		.usage = "",
+		.chain = psoc4_exec_command_handlers,
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+struct flash_driver psoc4_flash = {
+	.name = "psoc4",
+	.commands = psoc4_command_handlers,
+	.flash_bank_command = psoc4_flash_bank_command,
+	.erase = psoc4_erase,
+	.protect = psoc4_protect,
+	.write = psoc4_write,
+	.read = default_flash_read,
+	.probe = psoc4_probe,
+	.auto_probe = psoc4_auto_probe,
+	.erase_check = default_flash_blank_check,
+	.protect_check = psoc4_protect_check,
+	.info = get_psoc4_info,
+};

src/flash/nor/spi.c

@@ -49,8 +49,11 @@ const struct flash_device flash_devices[] = {
 	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("atmel 25f512",   0x52, 0xc7, 0x0065001f, 0x80,  0x8000,  0x10000),
@@ -68,7 +71,9 @@ const struct flash_device flash_devices[] = {
 	FLASH_ID("mac 25l6405",    0xd8, 0xc7, 0x001720c2, 0x100, 0x10000, 0x800000),
 	FLASH_ID("mcr n25q064",    0xd8, 0xc7, 0x0017ba20, 0x100, 0x10000, 0x800000),
 	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("gd gd25q20",     0x20, 0xc7, 0x00c84012, 0x100, 0x1000, 0x80000),
 	FLASH_ID(NULL,             0,    0,	   0,          0,     0,       0)
 };

src/flash/nor/stellaris.c

@@ -69,6 +69,8 @@
 #define FLASH_CRIS	(FLASH_CONTROL_BASE | 0x00C)
 #define FLASH_CIM	(FLASH_CONTROL_BASE | 0x010)
 #define FLASH_MISC	(FLASH_CONTROL_BASE | 0x014)
+#define FLASH_FSIZE	(FLASH_CONTROL_BASE | 0xFC0)
+#define FLASH_SSIZE	(FLASH_CONTROL_BASE | 0xFC4)
 
 #define AMISC	1
 #define PMISC	2
@@ -98,19 +100,16 @@ struct stellaris_flash_bank {
 	uint32_t did1;
 	uint32_t dc0;
 	uint32_t dc1;
+	uint32_t fsize;
+	uint32_t ssize;
 
 	const char *target_name;
 	uint8_t target_class;
 
 	uint32_t sramsiz;
-	uint32_t flshsz;
 	/* flash geometry */
 	uint32_t num_pages;
 	uint32_t pagesize;
-	uint32_t pages_in_lockregion;
-
-	/* nv memory bits */
-	uint16_t num_lockbits;
 
 	/* main clock status */
 	uint32_t rcc;
@@ -361,70 +360,96 @@ static const struct {
 	{0x06, 0x7D, "LM3S9U90"},
 	{0x06, 0x90, "LM3S9U92"},
 	{0x06, 0x9B, "LM3S9U96"},
-	{0x05, 0x18, "LM4F110B2QR"},
-	{0x05, 0x19, "LM4F110C4QR"},
-	{0x05, 0x10, "LM4F110E5QR"},
-	{0x05, 0x11, "LM4F110H5QR"},
-	{0x05, 0x22, "LM4F111B2QR"},
-	{0x05, 0x23, "LM4F111C4QR"},
-	{0x05, 0x20, "LM4F111E5QR"},
-	{0x05, 0x21, "LM4F111H5QR"},
-	{0x05, 0x36, "LM4F112C4QC"},
-	{0x05, 0x30, "LM4F112E5QC"},
-	{0x05, 0x31, "LM4F112H5QC"},
-	{0x05, 0x35, "LM4F112H5QD"},
-	{0x05, 0x01, "LM4F120B2QR"},
-	{0x05, 0x02, "LM4F120C4QR"},
-	{0x05, 0x03, "LM4F120E5QR"},
-	{0x05, 0x04, "LM4F120H5QR"},
-	{0x05, 0x08, "LM4F121B2QR"},
-	{0x05, 0x09, "LM4F121C4QR"},
-	{0x05, 0x0A, "LM4F121E5QR"},
-	{0x05, 0x0B, "LM4F121H5QR"},
-	{0x05, 0xD0, "LM4F122C4QC"},
-	{0x05, 0xD1, "LM4F122E5QC"},
-	{0x05, 0xD2, "LM4F122H5QC"},
-	{0x05, 0xD6, "LM4F122H5QD"},
-	{0x05, 0x48, "LM4F130C4QR"},
-	{0x05, 0x40, "LM4F130E5QR"},
-	{0x05, 0x41, "LM4F130H5QR"},
-	{0x05, 0x52, "LM4F131C4QR"},
-	{0x05, 0x50, "LM4F131E5QR"},
-	{0x05, 0x51, "LM4F131H5QR"},
-	{0x05, 0x66, "LM4F132C4QC"},
-	{0x05, 0x60, "LM4F132E5QC"},
-	{0x05, 0x61, "LM4F132H5QC"},
-	{0x05, 0x65, "LM4F132H5QD"},
-	{0x05, 0x70, "LM4F210E5QR"},
-	{0x05, 0x73, "LM4F210H5QR"},
-	{0x05, 0x80, "LM4F211E5QR"},
-	{0x05, 0x83, "LM4F211H5QR"},
-	{0x05, 0xE9, "LM4F212H5BB"},
-	{0x05, 0xC4, "LM4F212H5QC"},
-	{0x05, 0xC6, "LM4F212H5QD"},
-	{0x05, 0xA0, "LM4F230E5QR"},
-	{0x05, 0xA1, "LM4F230H5QR"},
-	{0x05, 0xB0, "LM4F231E5QR"},
-	{0x05, 0xB1, "LM4F231H5QR"},
-	{0x05, 0xC0, "LM4F232E5QC"},
-	{0x05, 0xE3, "LM4F232H5BB"},
-	{0x05, 0xC1, "LM4F232H5QC"},
-	{0x05, 0xC5, "LM4F232H5QD"},
-	{0x05, 0xE5, "LM4FS1AH5BB"},
-	{0x05, 0xEA, "LM4FS1GH5BB"},
-	{0x05, 0xE4, "LM4FS99H5BB"},
+	{0x05, 0x01, "LM4F120B2QR/TM4C1233C3PM"},
+	{0x05, 0x02, "LM4F120C4QR/TM4C1233D5PM"},
+	{0x05, 0x03, "LM4F120E5QR/TM4C1233E6PM"},
+	{0x05, 0x04, "LM4F120H5QR/TM4C1233H6PM"},
+	{0x05, 0x08, "LM4F121B2QR/TM4C1232C3PM"},
+	{0x05, 0x09, "LM4F121C4QR/TM4C1232D5PM"},
+	{0x05, 0x0A, "LM4F121E5QR/TM4C1232E6PM"},
+	{0x05, 0x0B, "LM4F121H5QR/TM4C1232H6PM"},
+	{0x05, 0x10, "LM4F110E5QR/TM4C1231E6PM"},
+	{0x05, 0x11, "LM4F110H5QR/TM4C1231H6PM"},
+	{0x05, 0x18, "LM4F110B2QR/TM4C1231C3PM"},
+	{0x05, 0x19, "LM4F110C4QR/TM4C1231D5PM"},
+	{0x05, 0x20, "LM4F111E5QR/TM4C1230E6PM"},
+	{0x05, 0x21, "LM4F111H5QR/TM4C1230H6PM"},
+	{0x05, 0x22, "LM4F111B2QR/TM4C1230C3PM"},
+	{0x05, 0x23, "LM4F111C4QR/TM4C1230D5PM"},
+	{0x05, 0x30, "LM4F112E5QC/TM4C1231E6PZ"},
+	{0x05, 0x31, "LM4F112H5QC/TM4C1231H6PZ"},
+	{0x05, 0x35, "LM4F112H5QD/TM4C1231H6PGE"},
+	{0x05, 0x36, "LM4F112C4QC/TM4C1231D5PZ"},
+	{0x05, 0x40, "LM4F130E5QR/TM4C1237E6PM"},
+	{0x05, 0x41, "LM4F130H5QR/TM4C1237H6PM"},
+	{0x05, 0x48, "LM4F130C4QR/TM4C1237D5PM"},
+	{0x05, 0x50, "LM4F131E5QR/TM4C1236E6PM"},
+	{0x05, 0x51, "LM4F131H5QR/TM4C1236H6PM"},
+	{0x05, 0x52, "LM4F131C4QR/TM4C1236D5PM"},
+	{0x05, 0x60, "LM4F132E5QC/TM4C1237E6PZ"},
+	{0x05, 0x61, "LM4F132H5QC/TM4C1237H6PZ"},
+	{0x05, 0x65, "LM4F132H5QD/TM4C1237H6PGE"},
+	{0x05, 0x66, "LM4F132C4QC/TM4C1237D5PZ"},
+	{0x05, 0x70, "LM4F210E5QR/TM4C123BE6PM"},
+	{0x05, 0x73, "LM4F210H5QR/TM4C123BH6PM"},
+	{0x05, 0x80, "LM4F211E5QR/TM4C123AE6PM"},
+	{0x05, 0x83, "LM4F211H5QR/TM4C123AH6PM"},
+	{0x05, 0xA0, "LM4F230E5QR/TM4C123GE6PM"},
+	{0x05, 0xA1, "LM4F230H5QR/TM4C123GH6PM"},
+	{0x05, 0xB0, "LM4F231E5QR/TM4C123FE6PM"},
+	{0x05, 0xB1, "LM4F231H5QR/TM4C123FH6PM"},
+	{0x05, 0xC0, "LM4F232E5QC/TM4C123GE6PZ"},
+	{0x05, 0xC1, "LM4F232H5QC/TM4C123GH6PZ"},
+	{0x05, 0xC3, "LM4F212E5QC/TM4C123BE6PZ"},
+	{0x05, 0xC4, "LM4F212H5QC/TM4C123BH6PZ"},
+	{0x05, 0xC5, "LM4F232H5QD/TM4C123GH6PGE"},
+	{0x05, 0xC6, "LM4F212H5QD/TM4C123BH6PGE"},
+	{0x05, 0xD0, "LM4F122C4QC/TM4C1233D5PZ"},
+	{0x05, 0xD1, "LM4F122E5QC/TM4C1233E6PZ"},
+	{0x05, 0xD2, "LM4F122H5QC/TM4C1233H6PZ"},
+	{0x05, 0xD6, "LM4F122H5QD/TM4C1233H6PGE"},
 	{0x05, 0xE1, "LM4FSXLH5BB"},
+	{0x05, 0xE3, "LM4F232H5BB/TM4C123GH6ZRB"},
+	{0x05, 0xE4, "LM4FS99H5BB"},
+	{0x05, 0xE5, "LM4FS1AH5BB"},
+	{0x05, 0xE9, "LM4F212H5BB/TM4C123BH6ZRB"},
+	{0x05, 0xEA, "LM4FS1GH5BB"},
+	{0x05, 0xF0, "TM4C123GH6ZXR"},
+	{0x0A, 0x19, "TM4C1290NCPDT"},
+	{0x0A, 0x1B, "TM4C1290NCZAD"},
+	{0x0A, 0x1C, "TM4C1292NCPDT"},
+	{0x0A, 0x1E, "TM4C1292NCZAD"},
+	{0x0A, 0x1F, "TM4C1294NCPDT"},
+	{0x0A, 0x21, "TM4C1294NCZAD"},
+	{0x0A, 0x22, "TM4C1297NCZAD"},
+	{0x0A, 0x23, "TM4C1299NCZAD"},
+	{0x0A, 0x24, "TM4C129CNCPDT"},
+	{0x0A, 0x26, "TM4C129CNCZAD"},
+	{0x0A, 0x27, "TM4C129DNCPDT"},
+	{0x0A, 0x29, "TM4C129DNCZAD"},
+	{0x0A, 0x2D, "TM4C129ENCPDT"},
+	{0x0A, 0x2F, "TM4C129ENCZAD"},
+	{0x0A, 0x30, "TM4C129LNCZAD"},
+	{0x0A, 0x32, "TM4C129XNCZAD"},
+	{0x0A, 0x34, "TM4C1294KCPDT"},
+	{0x0A, 0x35, "TM4C129EKCPDT"},
+	{0x0A, 0x36, "TM4C1299KCZAD"},
+	{0x0A, 0x37, "TM4C129XKCZAD"},
 	{0xFF, 0x00, "Unknown Part"}
 };
 
-static const char *StellarisClassname[7] = {
+static const char * const StellarisClassname[] = {
 	"Sandstorm",
 	"Fury",
 	"Unknown",
 	"DustDevil",
 	"Tempest",
-	"Blizzard",
-	"Firestorm"
+	"Blizzard/TM4C123x",
+	"Firestorm",
+	"",
+	"",
+	"",
+	"Snowflake",
 };
 
 /***************************************************************************
@@ -482,36 +507,27 @@ static int get_stellaris_info(struct flash_bank *bank, char *buf, int buf_size)
 	printed = snprintf(buf,
 			   buf_size,
 			   "did1: 0x%8.8" PRIx32 ", arch: 0x%4.4" PRIx32
-			   ", eproc: %s, ramsize: %ik, flashsize: %ik\n",
+			   ", eproc: %s, ramsize: %" PRIu32 "k, flashsize: %" PRIu32 "k\n",
 			   stellaris_info->did1,
 			   stellaris_info->did1,
 			   "ARMv7M",
-			   (int)((1 + ((stellaris_info->dc0 >> 16) & 0xFFFF))/4),
-			   (int)((1 + (stellaris_info->dc0 & 0xFFFF))*2));
+			   stellaris_info->sramsiz,
+			   (uint32_t)(stellaris_info->num_pages * stellaris_info->pagesize / 1024));
 	buf += printed;
 	buf_size -= printed;
 
-	printed = snprintf(buf,
+	snprintf(buf,
 			   buf_size,
 			   "master clock: %ikHz%s, "
-			   "rcc is 0x%" PRIx32 ", rcc2 is 0x%" PRIx32 "\n",
+			   "rcc is 0x%" PRIx32 ", rcc2 is 0x%" PRIx32 ", "
+			   "pagesize: %" PRIu32 ", pages: %" PRIu32,
 			   (int)(stellaris_info->mck_freq / 1000),
 			   stellaris_info->mck_desc,
 			   stellaris_info->rcc,
-			   stellaris_info->rcc2);
-	buf += printed;
-	buf_size -= printed;
+			   stellaris_info->rcc2,
+			   stellaris_info->pagesize,
+			   stellaris_info->num_pages);
 
-	if (stellaris_info->num_lockbits > 0) {
-		snprintf(buf,
-				buf_size,
-				"pagesize: %" PRIi32 ", pages: %d, "
-				"lockbits: %i, pages per lockbit: %i\n",
-				stellaris_info->pagesize,
-				(unsigned) stellaris_info->num_pages,
-				stellaris_info->num_lockbits,
-				(unsigned) stellaris_info->pages_in_lockregion);
-	}
 	return ERROR_OK;
 }
 
@@ -666,7 +682,7 @@ static int stellaris_read_part_info(struct flash_bank *bank)
 	LOG_DEBUG("did0 0x%" PRIx32 ", did1 0x%" PRIx32 ", dc0 0x%" PRIx32 ", dc1 0x%" PRIx32 "",
 		  did0, did1, stellaris_info->dc0, stellaris_info->dc1);
 
-	ver = did0 >> 28;
+	ver = DID0_VER(did0);
 	if ((ver != 0) && (ver != 1)) {
 		LOG_WARNING("Unknown did0 version, cannot identify target");
 		return ERROR_FLASH_OPERATION_FAILED;
@@ -724,6 +740,7 @@ static int stellaris_read_part_info(struct flash_bank *bank)
 		case 4:			/* Tempest */
 		case 5:			/* Blizzard */
 		case 6:			/* Firestorm */
+		case 0xa:		/* Snowflake */
 			stellaris_info->iosc_freq = 16000000;	/* +/- 1% */
 			stellaris_info->iosc_desc = " (±1%)";
 			/* FALL THROUGH */
@@ -747,10 +764,26 @@ static int stellaris_read_part_info(struct flash_bank *bank)
 	stellaris_info->did0 = did0;
 	stellaris_info->did1 = did1;
 
-	stellaris_info->num_lockbits = 1 + (stellaris_info->dc0 & 0xFFFF);
-	stellaris_info->num_pages = 2 * (1 + (stellaris_info->dc0 & 0xFFFF));
-	stellaris_info->pagesize = 1024;
-	stellaris_info->pages_in_lockregion = 2;
+	if (stellaris_info->target_class == 5) { /* Blizzard */
+		target_read_u32(target, FLASH_FSIZE, &stellaris_info->fsize);
+		target_read_u32(target, FLASH_SSIZE, &stellaris_info->ssize);
+
+		stellaris_info->num_pages = 2 * (1 + (stellaris_info->fsize & 0xFFFF));
+		stellaris_info->sramsiz = (1 + (stellaris_info->ssize & 0xFFFF)) / 4;
+		stellaris_info->pagesize = 1024;
+	} else if (stellaris_info->target_class == 0xa) { /* Snowflake */
+		target_read_u32(target, FLASH_FSIZE, &stellaris_info->fsize);
+		target_read_u32(target, FLASH_SSIZE, &stellaris_info->ssize);
+
+		stellaris_info->pagesize = (1 << ((stellaris_info->fsize >> 16) & 7)) * 1024;
+		stellaris_info->num_pages = 2048 * (1 + (stellaris_info->fsize & 0xFFFF)) /
+			stellaris_info->pagesize;
+		stellaris_info->sramsiz = (1 + (stellaris_info->ssize & 0xFFFF)) / 4;
+	} else {
+		stellaris_info->num_pages = 2 * (1 + (stellaris_info->dc0 & 0xFFFF));
+		stellaris_info->sramsiz = (1 + ((stellaris_info->dc0 >> 16) & 0xFFFF)) / 4;
+		stellaris_info->pagesize = 1024;
+	}
 
 	/* REVISIT for at least Tempest parts, read NVMSTAT.FWB too.
 	 * That exposes a 32-word Flash Write Buffer ... enabling
@@ -767,9 +800,12 @@ static int stellaris_read_part_info(struct flash_bank *bank)
 static int stellaris_protect_check(struct flash_bank *bank)
 {
 	struct stellaris_flash_bank *stellaris = bank->driver_priv;
+	struct target *target = bank->target;
+	uint32_t flash_sizek = stellaris->pagesize / 1024 *
+		stellaris->num_pages;
+	uint32_t fmppe_addr;
 	int status = ERROR_OK;
 	unsigned i;
-	unsigned page;
 
 	if (stellaris->did1 == 0)
 		return ERROR_FLASH_BANK_NOT_PROBED;
@@ -781,32 +817,32 @@ static int stellaris_protect_check(struct flash_bank *bank)
 	 * to report any pages that we can't write.  Ignore the Read Enable
 	 * register (FMPRE).
 	 */
-	for (i = 0, page = 0;
-			i < DIV_ROUND_UP(stellaris->num_lockbits, 32u);
-			i++) {
-		uint32_t lockbits;
 
-		status = target_read_u32(bank->target,
-				SCB_BASE + (i ? (FMPPE0 + 4 * i) : FMPPE),
-				&lockbits);
-		LOG_DEBUG("FMPPE%d = %#8.8x (status %d)", i,
-				(unsigned) lockbits, status);
-		if (status != ERROR_OK)
-			goto done;
+	if (stellaris->target_class >= 0x0a || flash_sizek > 64)
+		fmppe_addr = SCB_BASE | FMPPE0;
+	else
+		fmppe_addr = SCB_BASE | FMPPE;
 
-		for (unsigned j = 0; j < 32; j++) {
-			unsigned k;
+	unsigned int page = 0, lockbitnum, lockbitcnt = flash_sizek / 2;
+	unsigned int bits_per_page = stellaris->pagesize / 2048;
+	/* Every lock bit always corresponds to a 2k region */
+	for (lockbitnum = 0; lockbitnum < lockbitcnt; lockbitnum += 32) {
+		uint32_t fmppe;
 
-			for (k = 0; k < stellaris->pages_in_lockregion; k++) {
-				if (page >= (unsigned) bank->num_sectors)
-					goto done;
-				bank->sectors[page++].is_protected =
-						!(lockbits & (1 << j));
+		target_read_u32(target, fmppe_addr, &fmppe);
+		for (i = 0; i < 32 && lockbitnum + i < lockbitcnt; i++) {
+			bool protect = !(fmppe & (1 << i));
+			if (bits_per_page) {
+				bank->sectors[page++].is_protected = protect;
+				i += bits_per_page - 1;
+			} else { /* 1024k pages, every lockbit covers 2 pages */
+				bank->sectors[page++].is_protected = protect;
+				bank->sectors[page++].is_protected = protect;
 			}
 		}
+		fmppe_addr += 4;
 	}
 
-done:
 	return status;
 }
 
@@ -870,13 +906,12 @@ static int stellaris_erase(struct flash_bank *bank, int first, int last)
 
 static int stellaris_protect(struct flash_bank *bank, int set, int first, int last)
 {
-	uint32_t fmppe, flash_fmc, flash_cris;
-	int lockregion;
-
-	struct stellaris_flash_bank *stellaris_info = bank->driver_priv;
+	struct stellaris_flash_bank *stellaris = bank->driver_priv;
 	struct target *target = bank->target;
+	uint32_t flash_fmc, flash_cris;
+	unsigned int bits_per_page = stellaris->pagesize / 2048;
 
-	if (bank->target->state != TARGET_HALTED) {
+	if (target->state != TARGET_HALTED) {
 		LOG_ERROR("Target not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
@@ -887,14 +922,18 @@ static int stellaris_protect(struct flash_bank *bank, int set, int first, int la
 		return ERROR_COMMAND_SYNTAX_ERROR;
 	}
 
-	if (stellaris_info->did1 == 0)
+	if (stellaris->did1 == 0)
 		return ERROR_FLASH_BANK_NOT_PROBED;
 
-	/* lockregions are 2 pages ... must protect [even..odd] */
-	if ((first < 0) || (first & 1)
-			|| (last < first) || !(last & 1)
-			|| (last >= 2 * stellaris_info->num_lockbits)) {
-		LOG_ERROR("Can't protect unaligned or out-of-range pages.");
+	if (stellaris->target_class == 0x03 &&
+	    !((stellaris->did0 >> 8) & 0xFF) &&
+	    !((stellaris->did0) & 0xFF)) {
+		LOG_ERROR("DustDevil A0 parts can't be unprotected, see errata; refusing to proceed");
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	if (!bits_per_page && (first % 2 || !(last % 2))) {
+		LOG_ERROR("Can't protect unaligned pages");
 		return ERROR_FLASH_SECTOR_INVALID;
 	}
 
@@ -902,57 +941,60 @@ static int stellaris_protect(struct flash_bank *bank, int set, int first, int la
 	stellaris_read_clock_info(bank);
 	stellaris_set_flash_timing(bank);
 
-	/* convert from pages to lockregions */
-	first /= 2;
-	last /= 2;
-
-	/* FIXME this assumes single FMPPE, for a max of 64K of flash!!
-	 * Current parts can be much bigger.
-	 */
-	if (last >= 32) {
-		LOG_ERROR("No support yet for protection > 64K");
-		return ERROR_FLASH_OPERATION_FAILED;
-	}
-
-	target_read_u32(target, SCB_BASE | FMPPE, &fmppe);
-
-	for (lockregion = first; lockregion <= last; lockregion++)
-		fmppe &= ~(1 << lockregion);
-
 	/* Clear and disable flash programming interrupts */
 	target_write_u32(target, FLASH_CIM, 0);
 	target_write_u32(target, FLASH_MISC, PMISC | AMISC);
 
-	/* REVISIT this clobbers state set by any halted firmware ...
-	 * it might want to process those IRQs.
-	 */
+	uint32_t flash_sizek = stellaris->pagesize / 1024 *
+		stellaris->num_pages;
+	uint32_t fmppe_addr;
 
-	LOG_DEBUG("fmppe 0x%" PRIx32 "", fmppe);
-	target_write_u32(target, SCB_BASE | FMPPE, fmppe);
+	if (stellaris->target_class >= 0x0a || flash_sizek > 64)
+		fmppe_addr = SCB_BASE | FMPPE0;
+	else
+		fmppe_addr = SCB_BASE | FMPPE;
 
-	/* Commit FMPPE */
-	target_write_u32(target, FLASH_FMA, 1);
+	int page = 0;
+	unsigned int lockbitnum, lockbitcnt = flash_sizek / 2;
+	/* Every lock bit always corresponds to a 2k region */
+	for (lockbitnum = 0; lockbitnum < lockbitcnt; lockbitnum += 32) {
+		uint32_t fmppe;
 
-	/* Write commit command */
-	/* REVISIT safety check, since this cannot be undone
-	 * except by the "Recover a locked device" procedure.
-	 * REVISIT DustDevil-A0 parts have an erratum making FMPPE commits
-	 * inadvisable ... it makes future mass erase operations fail.
-	 */
-	LOG_WARNING("Flash protection cannot be removed once committed, commit is NOT executed !");
-	/* target_write_u32(target, FLASH_FMC, FMC_WRKEY | FMC_COMT); */
+		target_read_u32(target, fmppe_addr, &fmppe);
+		for (unsigned int i = 0;
+		     i < 32 && lockbitnum + i < lockbitcnt;
+		     i++) {
+			if (page >= first && page <= last)
+				fmppe &= ~(1 << i);
 
-	/* Wait until erase complete */
-	do {
-		target_read_u32(target, FLASH_FMC, &flash_fmc);
-	} while (flash_fmc & FMC_COMT);
+			if (bits_per_page) {
+				if (!((i + 1) % bits_per_page))
+					page++;
+			} else { /* 1024k pages, every lockbit covers 2 pages */
+				page += 2;
+			}
+		}
+		target_write_u32(target, fmppe_addr, fmppe);
 
-	/* Check acess violations */
-	target_read_u32(target, FLASH_CRIS, &flash_cris);
-	if (flash_cris & (AMASK)) {
-		LOG_WARNING("Error setting flash page protection,  flash_cris 0x%" PRIx32 "", flash_cris);
-		target_write_u32(target, FLASH_CRIS, 0);
-		return ERROR_FLASH_OPERATION_FAILED;
+		/* Commit FMPPE* */
+		target_write_u32(target, FLASH_FMA, 1 + lockbitnum / 16);
+		/* Write commit command */
+		target_write_u32(target, FLASH_FMC, FMC_WRKEY | FMC_COMT);
+
+		/* Wait until commit complete */
+		do {
+			target_read_u32(target, FLASH_FMC, &flash_fmc);
+		} while (flash_fmc & FMC_COMT);
+
+		/* Check access violations */
+		target_read_u32(target, FLASH_CRIS, &flash_cris);
+		if (flash_cris & (AMASK)) {
+			LOG_WARNING("Error setting flash page protection,  flash_cris 0x%" PRIx32 "", flash_cris);
+			target_write_u32(target, FLASH_CRIS, 0);
+			return ERROR_FLASH_OPERATION_FAILED;
+		}
+
+		fmppe_addr += 4;
 	}
 
 	return ERROR_OK;
@@ -1216,7 +1258,7 @@ static int stellaris_probe(struct flash_bank *bank)
 	}
 
 	/* provide this for the benefit of the NOR flash framework */
-	bank->size = 1024 * stellaris_info->num_pages;
+	bank->size = stellaris_info->num_pages * stellaris_info->pagesize;
 	bank->num_sectors = stellaris_info->num_pages;
 	bank->sectors = calloc(bank->num_sectors, sizeof(struct flash_sector));
 	for (int i = 0; i < bank->num_sectors; i++) {
@@ -1317,9 +1359,12 @@ COMMAND_HANDLER(stellaris_handle_recover_command)
 	struct flash_bank *bank;
 	int retval;
 
-	retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
-	if (retval != ERROR_OK)
-		return retval;
+	if (CMD_ARGC != 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	bank = get_flash_bank_by_num_noprobe(0);
+	if (!bank)
+		return ERROR_FAIL;
 
 	/* REVISIT ... it may be worth sanity checking that the AP is
 	 * inactive before we start.  ARM documents that switching a DP's
@@ -1327,6 +1372,12 @@ COMMAND_HANDLER(stellaris_handle_recover_command)
 	 * cycle to recover.
 	 */
 
+	Jim_Eval_Named(CMD_CTX->interp, "catch { hla_command \"debug unlock\" }", 0, 0);
+	if (!strcmp(Jim_GetString(Jim_GetResult(CMD_CTX->interp), NULL), "0")) {
+		retval = ERROR_OK;
+		goto user_action;
+	}
+
 	/* assert SRST */
 	if (!(jtag_get_reset_config() & RESET_HAS_SRST)) {
 		LOG_ERROR("Can't recover Stellaris flash without SRST");
@@ -1351,6 +1402,7 @@ COMMAND_HANDLER(stellaris_handle_recover_command)
 	/* wait 400+ msec ... OK, "1+ second" is simpler */
 	usleep(1000);
 
+user_action:
 	/* USER INTERVENTION required for the power cycle
 	 * Restarting OpenOCD is likely needed because of mode switching.
 	 */
@@ -1373,7 +1425,7 @@ static const struct command_registration stellaris_exec_command_handlers[] = {
 		.name = "recover",
 		.handler = stellaris_handle_recover_command,
 		.mode = COMMAND_EXEC,
-		.usage = "bank_id",
+		.usage = "",
 		.help = "recover (and erase) locked device",
 	},
 	COMMAND_REGISTRATION_DONE

src/flash/nor/stm32f1x.c

@@ -894,7 +894,7 @@ static int stm32x_probe(struct flash_bank *bank)
 		stm32x_info->ppage_size = 4;
 		max_flash_size_in_kb = 128;
 		break;
-	case 0x422: /* stm32f30x */
+	case 0x422: /* stm32f302/3xb/c */
 		page_size = 2048;
 		stm32x_info->ppage_size = 2;
 		max_flash_size_in_kb = 256;
@@ -902,6 +902,14 @@ static int stm32x_probe(struct flash_bank *bank)
 		stm32x_info->option_offset = 6;
 		stm32x_info->default_rdp = 0x55AA;
 		break;
+	case 0x446: /* stm32f303xD/E */
+		page_size = 2048;
+		stm32x_info->ppage_size = 2;
+		max_flash_size_in_kb = 512;
+		stm32x_info->user_data_offset = 16;
+		stm32x_info->option_offset = 6;
+		stm32x_info->default_rdp = 0x55AA;
+		break;
 	case 0x428: /* value line High density */
 		page_size = 2048;
 		stm32x_info->ppage_size = 4;
@@ -921,8 +929,18 @@ static int stm32x_probe(struct flash_bank *bank)
 		stm32x_info->option_offset = 6;
 		stm32x_info->default_rdp = 0x55AA;
 		break;
+	case 0x438: /* stm32f33x */
+	case 0x439: /* stm32f302x6/8 */
+		page_size = 2048;
+		stm32x_info->ppage_size = 2;
+		max_flash_size_in_kb = 64;
+		stm32x_info->user_data_offset = 16;
+		stm32x_info->option_offset = 6;
+		stm32x_info->default_rdp = 0x55AA;
+		break;
 	case 0x440: /* stm32f05x */
 	case 0x444: /* stm32f03x */
+	case 0x445: /* stm32f04x */
 		page_size = 1024;
 		stm32x_info->ppage_size = 4;
 		max_flash_size_in_kb = 64;
@@ -931,9 +949,10 @@ static int stm32x_probe(struct flash_bank *bank)
 		stm32x_info->default_rdp = 0x55AA;
 		break;
 	case 0x448: /* stm32f07x */
+	case 0x442: /* stm32f09x */
 		page_size = 2048;
 		stm32x_info->ppage_size = 4;
-		max_flash_size_in_kb = 128;
+		max_flash_size_in_kb = 256;
 		stm32x_info->user_data_offset = 16;
 		stm32x_info->option_offset = 6;
 		stm32x_info->default_rdp = 0x55AA;
@@ -1022,6 +1041,21 @@ COMMAND_HANDLER(stm32x_handle_part_id_command)
 }
 #endif
 
+static const char *get_stm32f0_revision(uint16_t rev_id)
+{
+	const char *rev_str = NULL;
+
+	switch (rev_id) {
+	case 0x1000:
+		rev_str = "1.0";
+		break;
+	case 0x2000:
+		rev_str = "2.0";
+		break;
+	}
+	return rev_str;
+}
+
 static int get_stm32x_info(struct flash_bank *bank, char *buf, int buf_size)
 {
 	uint32_t dbgmcu_idcode;
@@ -1116,7 +1150,7 @@ static int get_stm32x_info(struct flash_bank *bank, char *buf, int buf_size)
 		break;
 
 	case 0x422:
-		device_str = "STM32F30x";
+		device_str = "STM32F302xB/C";
 
 		switch (rev_id) {
 		case 0x1000:
@@ -1175,46 +1209,62 @@ static int get_stm32x_info(struct flash_bank *bank, char *buf, int buf_size)
 		}
 		break;
 
-	case 0x444:
-		device_str = "STM32F03x";
+	case 0x438:
+		device_str = "STM32F33x";
 
 		switch (rev_id) {
 		case 0x1000:
-			rev_str = "1.0";
-			break;
-
-		case 0x2000:
-			rev_str = "2.0";
+			rev_str = "A";
 			break;
 		}
 		break;
 
-	case 0x440:
-		device_str = "STM32F05x";
+	case 0x439:
+		device_str = "STM32F302x6/8";
 
 		switch (rev_id) {
 		case 0x1000:
-			rev_str = "1.0";
+			rev_str = "A";
 			break;
 
-		case 0x2000:
-			rev_str = "2.0";
+		case 0x1001:
+			rev_str = "Z";
+			break;
+		}
+		break;
+
+	case 0x444:
+		device_str = "STM32F03x";
+		rev_str = get_stm32f0_revision(rev_id);
+		break;
+
+	case 0x440:
+		device_str = "STM32F05x";
+		rev_str = get_stm32f0_revision(rev_id);
+		break;
+
+	case 0x445:
+		device_str = "STM32F04x";
+		rev_str = get_stm32f0_revision(rev_id);
+		break;
+
+	case 0x446:
+		device_str = "STM32F303xD/E";
+		switch (rev_id) {
+		case 0x1000:
+			rev_str = "A";
 			break;
 		}
 		break;
 
 	case 0x448:
 		device_str = "STM32F07x";
+		rev_str = get_stm32f0_revision(rev_id);
+		break;
 
-		switch (rev_id) {
-		case 0x1000:
-			rev_str = "1.0";
-			break;
-
-		case 0x2000:
-			rev_str = "2.0";
-			break;
-		}
+	case 0x442:
+		device_str = "STM32F09x";
+		rev_str = get_stm32f0_revision(rev_id);
 		break;
 
 	default:

src/flash/nor/stm32f2x.c

@@ -344,8 +344,8 @@ static int stm32x_write_options(struct flash_bank *bank)
 
 	/* rebuild option data */
 	optiondata = stm32x_info->option_bytes.user_options;
-	buf_set_u32(&optiondata, 8, 8, stm32x_info->option_bytes.RDP);
-	buf_set_u32(&optiondata, 16, 12, stm32x_info->option_bytes.protection);
+	optiondata |= stm32x_info->option_bytes.RDP << 8;
+	optiondata |= (stm32x_info->option_bytes.protection & 0x0fff) << 16;
 
 	/* program options */
 	retval = target_write_u32(target, STM32_FLASH_OPTCR, optiondata);
@@ -355,7 +355,7 @@ static int stm32x_write_options(struct flash_bank *bank)
 	if (stm32x_info->has_large_mem) {
 
 		uint32_t optiondata2 = 0;
-		buf_set_u32(&optiondata2, 16, 12, stm32x_info->option_bytes.protection >> 12);
+		optiondata2 |= (stm32x_info->option_bytes.protection & 0x00fff000) << 4;
 		retval = target_write_u32(target, STM32_FLASH_OPTCR1, optiondata2);
 		if (retval != ERROR_OK)
 			return retval;
@@ -372,7 +372,7 @@ static int stm32x_write_options(struct flash_bank *bank)
 		return retval;
 
 	/* relock registers */
-	retval = target_write_u32(target, STM32_FLASH_OPTCR, OPT_LOCK);
+	retval = target_write_u32(target, STM32_FLASH_OPTCR, optiondata | OPT_LOCK);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -779,7 +779,9 @@ static int stm32x_probe(struct flash_bank *bank)
 	case 0x423:
 		max_flash_size_in_kb = 256;
 		break;
+	case 0x431:
 	case 0x433:
+	case 0x421:
 		max_flash_size_in_kb = 512;
 		break;
 	default:
@@ -926,10 +928,27 @@ static int get_stm32x_info(struct flash_bank *bank, char *buf, int buf_size)
 		case 0x1003:
 			rev_str = "Y";
 			break;
+
+		case 0x1007:
+			rev_str = "1";
+			break;
+
+		case 0x2001:
+			rev_str = "3";
+			break;
 		}
 		break;
+	case 0x421:
+		device_str = "STM32F446";
 
+		switch (rev_id) {
+		case 0x1000:
+			rev_str = "A";
+			break;
+		}
+		break;
 	case 0x423:
+	case 0x431:
 	case 0x433:
 		device_str = "STM32F4xx (Low Power)";
 

src/flash/nor/stm32lx.c

@@ -36,16 +36,15 @@
 
 /* stm32lx flash register locations */
 
-#define FLASH_BASE		0x40023C00
-#define FLASH_ACR		0x40023C00
-#define FLASH_PECR		0x40023C04
-#define FLASH_PDKEYR	0x40023C08
-#define FLASH_PEKEYR	0x40023C0C
-#define FLASH_PRGKEYR	0x40023C10
-#define FLASH_OPTKEYR	0x40023C14
-#define FLASH_SR		0x40023C18
-#define FLASH_OBR		0x40023C1C
-#define FLASH_WRPR		0x40023C20
+#define FLASH_ACR		0x00
+#define FLASH_PECR		0x04
+#define FLASH_PDKEYR	0x08
+#define FLASH_PEKEYR	0x0C
+#define FLASH_PRGKEYR	0x10
+#define FLASH_OPTKEYR	0x14
+#define FLASH_SR		0x18
+#define FLASH_OBR		0x1C
+#define FLASH_WRPR		0x20
 
 /* FLASH_ACR bites */
 #define FLASH_ACR__LATENCY		(1<<0)
@@ -86,44 +85,151 @@
 #define OPTKEY2			0x24252627
 
 /* other registers */
-#define DBGMCU_IDCODE	0xE0042000
-#define F_SIZE			0x1FF8004C
-#define F_SIZE_MP		0x1FF800CC /* on 0x427 Medium+ and 0x436 HD devices */
+#define DBGMCU_IDCODE		0xE0042000
+#define DBGMCU_IDCODE_L0	0x40015800
 
 /* Constants */
-#define FLASH_PAGE_SIZE 256
 #define FLASH_SECTOR_SIZE 4096
-#define FLASH_PAGES_PER_SECTOR 16
 #define FLASH_BANK0_ADDRESS 0x08000000
 
-/* stm32lx option byte register location */
-#define OB_RDP			0x1FF80000
-#define OB_USER			0x1FF80004
-#define OB_WRP0_1		0x1FF80008
-#define OB_WRP2_3		0x1FF8000C
+/* option bytes */
+#define OPTION_BYTES_ADDRESS 0x1FF80000
 
-/* OB_RDP values */
-#define OB_RDP__LEVEL0	0xFF5500AA
-#define OB_RDP__LEVEL1	0xFFFF0000
-
-/* stm32lx RCC register locations */
-#define RCC_CR		0x40023800
-#define RCC_ICSCR	0x40023804
-#define RCC_CFGR	0x40023808
-
-/* RCC_ICSCR bits */
-#define RCC_ICSCR__MSIRANGE_MASK	(7<<13)
+#define OPTION_BYTE_0_PR1 0xFFFF0000
+#define OPTION_BYTE_0_PR0 0xFF5500AA
 
 static int stm32lx_unlock_program_memory(struct flash_bank *bank);
 static int stm32lx_lock_program_memory(struct flash_bank *bank);
 static int stm32lx_enable_write_half_page(struct flash_bank *bank);
 static int stm32lx_erase_sector(struct flash_bank *bank, int sector);
 static int stm32lx_wait_until_bsy_clear(struct flash_bank *bank);
+static int stm32lx_mass_erase(struct flash_bank *bank);
+static int stm32lx_wait_until_bsy_clear_timeout(struct flash_bank *bank, int timeout);
+
+struct stm32lx_rev {
+	uint16_t rev;
+	const char *str;
+};
+
+struct stm32lx_part_info {
+	uint16_t id;
+	const char *device_str;
+	const struct stm32lx_rev *revs;
+	size_t num_revs;
+	unsigned int page_size;
+	unsigned int pages_per_sector;
+	uint16_t max_flash_size_kb;
+	uint16_t first_bank_size_kb; /* used when has_dual_banks is true */
+	bool has_dual_banks;
+
+	uint32_t flash_base;	/* Flash controller registers location */
+	uint32_t fsize_base;	/* Location of FSIZE register */
+};
 
 struct stm32lx_flash_bank {
 	int probed;
-	bool has_dual_banks;
+	uint32_t idcode;
 	uint32_t user_bank_size;
+	uint32_t flash_base;
+
+	const struct stm32lx_part_info *part_info;
+};
+
+static const struct stm32lx_rev stm32_416_revs[] = {
+	{ 0x1000, "A" }, { 0x1008, "Y" }, { 0x1038, "W" }, { 0x1078, "V" },
+};
+static const struct stm32lx_rev stm32_417_revs[] = {
+	{ 0x1000, "A" }, { 0x1008, "Z" },
+};
+static const struct stm32lx_rev stm32_427_revs[] = {
+	{ 0x1000, "A" }, { 0x1018, "Y" }, { 0x1038, "X" },
+};
+static const struct stm32lx_rev stm32_429_revs[] = {
+	{ 0x1000, "A" }, { 0x1018, "Z" },
+};
+static const struct stm32lx_rev stm32_436_revs[] = {
+	{ 0x1000, "A" }, { 0x1008, "Z" }, { 0x1018, "Y" },
+};
+static const struct stm32lx_rev stm32_437_revs[] = {
+	{ 0x1000, "A" },
+};
+
+static const struct stm32lx_part_info stm32lx_parts[] = {
+	{
+		.id					= 0x416,
+		.revs				= stm32_416_revs,
+		.num_revs			= ARRAY_SIZE(stm32_416_revs),
+		.device_str			= "STM32L1xx (Cat.1 - Low/Medium Density)",
+		.page_size			= 256,
+		.pages_per_sector	= 16,
+		.max_flash_size_kb	= 128,
+		.has_dual_banks		= false,
+		.flash_base			= 0x40023C00,
+		.fsize_base			= 0x1FF8004C,
+	},
+	{
+		.id					= 0x417,
+		.revs				= stm32_417_revs,
+		.num_revs			= ARRAY_SIZE(stm32_417_revs),
+		.device_str			= "STM32L0xx",
+		.page_size			= 128,
+		.pages_per_sector	= 32,
+		.max_flash_size_kb	= 64,
+		.has_dual_banks		= false,
+		.flash_base			= 0x40022000,
+		.fsize_base			= 0x1FF8007C,
+	},
+	{
+		.id					= 0x427,
+		.revs				= stm32_427_revs,
+		.num_revs			= ARRAY_SIZE(stm32_427_revs),
+		.device_str			= "STM32L1xx (Cat.3 - Medium+ Density)",
+		.page_size			= 256,
+		.pages_per_sector	= 16,
+		.max_flash_size_kb	= 256,
+		.first_bank_size_kb	= 192,
+		.has_dual_banks		= true,
+		.flash_base			= 0x40023C00,
+		.fsize_base			= 0x1FF800CC,
+	},
+	{
+		.id					= 0x429,
+		.revs				= stm32_429_revs,
+		.num_revs			= ARRAY_SIZE(stm32_429_revs),
+		.device_str			= "STM32L1xx (Cat.2)",
+		.page_size			= 256,
+		.pages_per_sector	= 16,
+		.max_flash_size_kb	= 128,
+		.has_dual_banks		= false,
+		.flash_base			= 0x40023C00,
+		.fsize_base			= 0x1FF8004C,
+	},
+	{
+		.id					= 0x436,
+		.revs				= stm32_436_revs,
+		.num_revs			= ARRAY_SIZE(stm32_436_revs),
+		.device_str			= "STM32L1xx (Cat.4/Cat.3 - Medium+/High Density)",
+		.page_size			= 256,
+		.pages_per_sector	= 16,
+		.max_flash_size_kb	= 384,
+		.first_bank_size_kb	= 192,
+		.has_dual_banks		= true,
+		.flash_base			= 0x40023C00,
+		.fsize_base			= 0x1FF800CC,
+	},
+	{
+		.id					= 0x437,
+		.revs				= stm32_437_revs,
+		.num_revs			= ARRAY_SIZE(stm32_437_revs),
+		.device_str			= "STM32L1xx (Cat.5/Cat.6)",
+		.page_size			= 256,
+		.pages_per_sector	= 16,
+		.max_flash_size_kb	= 512,
+		.first_bank_size_kb	= 256,
+		.has_dual_banks		= true,
+		.flash_base			= 0x40023C00,
+		.fsize_base			= 0x1FF800CC,
+	},
 };
 
 /* flash bank stm32lx <base> <size> 0 0 <target#>
@@ -135,7 +241,7 @@ FLASH_BANK_COMMAND_HANDLER(stm32lx_flash_bank_command)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
 	/* Create the bank structure */
-	stm32lx_info = malloc(sizeof(struct stm32lx_flash_bank));
+	stm32lx_info = calloc(1, sizeof(*stm32lx_info));
 
 	/* Check allocation */
 	if (stm32lx_info == NULL) {
@@ -146,7 +252,6 @@ FLASH_BANK_COMMAND_HANDLER(stm32lx_flash_bank_command)
 	bank->driver_priv = stm32lx_info;
 
 	stm32lx_info->probed = 0;
-	stm32lx_info->has_dual_banks = false;
 	stm32lx_info->user_bank_size = bank->size;
 
 	/* the stm32l erased value is 0x00 */
@@ -155,10 +260,37 @@ FLASH_BANK_COMMAND_HANDLER(stm32lx_flash_bank_command)
 	return ERROR_OK;
 }
 
+COMMAND_HANDLER(stm32lx_handle_mass_erase_command)
+{
+	int i;
+
+	if (CMD_ARGC < 1)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	retval = stm32lx_mass_erase(bank);
+	if (retval == ERROR_OK) {
+		/* set all sectors as erased */
+		for (i = 0; i < bank->num_sectors; i++)
+			bank->sectors[i].is_erased = 1;
+
+		command_print(CMD_CTX, "stm32lx mass erase complete");
+	} else {
+		command_print(CMD_CTX, "stm32lx mass erase failed");
+	}
+
+	return retval;
+}
+
 static int stm32lx_protect_check(struct flash_bank *bank)
 {
 	int retval;
 	struct target *target = bank->target;
+	struct stm32lx_flash_bank *stm32lx_info = bank->driver_priv;
 
 	uint32_t wrpr;
 
@@ -166,11 +298,12 @@ static int stm32lx_protect_check(struct flash_bank *bank)
 	 * Read the WRPR word, and check each bit (corresponding to each
 	 * flash sector
 	 */
-	retval = target_read_u32(target, FLASH_WRPR, &wrpr);
+	retval = target_read_u32(target, stm32lx_info->flash_base + FLASH_WRPR,
+			&wrpr);
 	if (retval != ERROR_OK)
 		return retval;
 
-	for (int i = 0; i < 32; i++) {
+	for (int i = 0; i < bank->num_sectors; i++) {
 		if (wrpr & (1 << i))
 			bank->sectors[i].is_protected = 1;
 		else
@@ -216,6 +349,9 @@ static int stm32lx_write_half_pages(struct flash_bank *bank, const uint8_t *buff
 		uint32_t offset, uint32_t count)
 {
 	struct target *target = bank->target;
+	struct stm32lx_flash_bank *stm32lx_info = bank->driver_priv;
+
+	uint32_t hp_nb = stm32lx_info->part_info->page_size / 2;
 	uint32_t buffer_size = 16384;
 	struct working_area *write_algorithm;
 	struct working_area *source;
@@ -244,10 +380,9 @@ static int stm32lx_write_half_pages(struct flash_bank *bank, const uint8_t *buff
 		0x00, 0xbe,             /* bkpt 0 */
 	};
 
-	/* Check if there is an even number of half pages (128bytes) */
-	if (count % 128) {
-		LOG_ERROR("there should be an even number "
-				"of half pages = 128 bytes (count = %" PRIi32 " bytes)", count);
+	/* Make sure we're performing a half-page aligned write. */
+	if (count % hp_nb) {
+		LOG_ERROR("The byte count must be %" PRIu32 "B-aligned but count is %" PRIi32 "B)", hp_nb, count);
 		return ERROR_FAIL;
 	}
 
@@ -275,7 +410,7 @@ static int stm32lx_write_half_pages(struct flash_bank *bank, const uint8_t *buff
 		else
 			buffer_size /= 2;
 
-		if (buffer_size <= 256) {
+		if (buffer_size <= stm32lx_info->part_info->page_size) {
 			/* we already allocated the writing code, but failed to get a
 			 * buffer, free the algorithm */
 			target_free_working_area(target, write_algorithm);
@@ -372,7 +507,7 @@ static int stm32lx_write_half_pages(struct flash_bank *bank, const uint8_t *buff
 
 		while (count > 0) {
 			uint32_t this_count;
-			this_count = (count > 128) ? 128 : count;
+			this_count = (count > hp_nb) ? hp_nb : count;
 
 			/* Write the next half pages */
 			retval = target_write_buffer(target, address, this_count, buffer);
@@ -407,7 +542,9 @@ static int stm32lx_write(struct flash_bank *bank, const uint8_t *buffer,
 		uint32_t offset, uint32_t count)
 {
 	struct target *target = bank->target;
+	struct stm32lx_flash_bank *stm32lx_info = bank->driver_priv;
 
+	uint32_t hp_nb = stm32lx_info->part_info->page_size / 2;
 	uint32_t halfpages_number;
 	uint32_t bytes_remaining = 0;
 	uint32_t address = bank->base + offset;
@@ -431,8 +568,8 @@ static int stm32lx_write(struct flash_bank *bank, const uint8_t *buffer,
 	/* first we need to write any unaligned head bytes upto
 	 * the next 128 byte page */
 
-	if (offset % 128)
-		bytes_remaining = MIN(count, 128 - (offset % 128));
+	if (offset % hp_nb)
+		bytes_remaining = MIN(count, hp_nb - (offset % hp_nb));
 
 	while (bytes_remaining > 0) {
 		uint8_t value[4] = {0xff, 0xff, 0xff, 0xff};
@@ -458,13 +595,13 @@ static int stm32lx_write(struct flash_bank *bank, const uint8_t *buffer,
 	count -= bytes_written;
 
 	/* this should always pass this check here */
-	assert((offset % 128) == 0);
+	assert((offset % hp_nb) == 0);
 
 	/* calculate half pages */
-	halfpages_number = count / 128;
+	halfpages_number = count / hp_nb;
 
 	if (halfpages_number) {
-		retval = stm32lx_write_half_pages(bank, buffer + bytes_written, offset, 128 * halfpages_number);
+		retval = stm32lx_write_half_pages(bank, buffer + bytes_written, offset, hp_nb * halfpages_number);
 		if (retval == ERROR_TARGET_RESOURCE_NOT_AVAILABLE) {
 			/* attempt slow memory writes */
 			LOG_WARNING("couldn't use block writes, falling back to single memory accesses");
@@ -476,7 +613,7 @@ static int stm32lx_write(struct flash_bank *bank, const uint8_t *buffer,
 	}
 
 	/* write any remaining bytes */
-	uint32_t page_bytes_written = 128 * halfpages_number;
+	uint32_t page_bytes_written = hp_nb * halfpages_number;
 	bytes_written += page_bytes_written;
 	address += page_bytes_written;
 	bytes_remaining = count - page_bytes_written;
@@ -513,65 +650,57 @@ reset_pg_and_lock:
 	return retval;
 }
 
+static int stm32lx_read_id_code(struct target *target, uint32_t *id)
+{
+	/* read stm32 device id register */
+	int retval = target_read_u32(target, DBGMCU_IDCODE, id);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* STM32L0 parts will have 0 there, try reading the L0's location for
+	 * DBG_IDCODE in case this is an L0 part. */
+	if (*id == 0)
+		retval = target_read_u32(target, DBGMCU_IDCODE_L0, id);
+
+	return retval;
+}
+
 static int stm32lx_probe(struct flash_bank *bank)
 {
 	struct target *target = bank->target;
 	struct stm32lx_flash_bank *stm32lx_info = bank->driver_priv;
 	int i;
 	uint16_t flash_size_in_kb;
-	uint16_t max_flash_size_in_kb;
 	uint32_t device_id;
 	uint32_t base_address = FLASH_BANK0_ADDRESS;
 	uint32_t second_bank_base;
-	uint32_t first_bank_size_in_kb;
 
 	stm32lx_info->probed = 0;
+	stm32lx_info->part_info = NULL;
 
-	/* read stm32 device id register */
-	int retval = target_read_u32(target, DBGMCU_IDCODE, &device_id);
+	int retval = stm32lx_read_id_code(bank->target, &device_id);
 	if (retval != ERROR_OK)
 		return retval;
 
+	stm32lx_info->idcode = device_id;
+
 	LOG_DEBUG("device id = 0x%08" PRIx32 "", device_id);
 
-	/* set max flash size depending on family */
-	switch (device_id & 0xfff) {
-	case 0x416:
-		max_flash_size_in_kb = 128;
-		break;
-	case 0x427:
-		/* single bank, high density */
-		max_flash_size_in_kb = 256;
-		break;
-	case 0x436:
-		/* According to ST, the devices with id 0x436 have dual bank flash and comes with
-		 * a total flash size of 384k or 256kb. However, the first bank is always 192kb,
-		 * and second one holds the rest. The reason is that the 256kb version is actually
-		 * the same physical flash but only the first 256kb are verified.
-		 */
-		max_flash_size_in_kb = 384;
-		first_bank_size_in_kb = 192;
-		stm32lx_info->has_dual_banks = true;
-		break;
-	case 0x437:
-		/* Dual bank, high density */
-		max_flash_size_in_kb = 512;
-		first_bank_size_in_kb = 192;
-		stm32lx_info->has_dual_banks = true;
-		break;
-	default:
+	for (unsigned int n = 0; n < ARRAY_SIZE(stm32lx_parts); n++) {
+		if ((device_id & 0xfff) == stm32lx_parts[n].id)
+			stm32lx_info->part_info = &stm32lx_parts[n];
+	}
+
+	if (!stm32lx_info->part_info) {
 		LOG_WARNING("Cannot identify target as a STM32L family.");
 		return ERROR_FAIL;
 	}
 
-	/* Get the flash size from target.  0x427 and 0x436 devices use a
-	 * different location for the Flash Size register, please see RM0038 r8 or
-	 * newer. */
-	if ((device_id & 0xfff) == 0x427 || (device_id & 0xfff) == 0x436 ||
-		(device_id & 0xfff) == 0x437)
-			retval = target_read_u16(target, F_SIZE_MP, &flash_size_in_kb);
-	else
-			retval = target_read_u16(target, F_SIZE, &flash_size_in_kb);
+	stm32lx_info->flash_base = stm32lx_info->part_info->flash_base;
+
+	/* Get the flash size from target. */
+	retval = target_read_u16(target, stm32lx_info->part_info->fsize_base,
+			&flash_size_in_kb);
 
 	/* 0x436 devices report their flash size as a 0 or 1 code indicating 384K
 	 * or 256K, respectively.  Please see RM0038 r8 or newer and refer to
@@ -587,26 +716,29 @@ static int stm32lx_probe(struct flash_bank *bank)
 	 * default to max target family */
 	if (retval != ERROR_OK || flash_size_in_kb == 0xffff || flash_size_in_kb == 0) {
 		LOG_WARNING("STM32L flash size failed, probe inaccurate - assuming %dk flash",
-			max_flash_size_in_kb);
-		flash_size_in_kb = max_flash_size_in_kb;
-	} else if (flash_size_in_kb > max_flash_size_in_kb) {
+			stm32lx_info->part_info->max_flash_size_kb);
+		flash_size_in_kb = stm32lx_info->part_info->max_flash_size_kb;
+	} else if (flash_size_in_kb > stm32lx_info->part_info->max_flash_size_kb) {
 		LOG_WARNING("STM32L probed flash size assumed incorrect since FLASH_SIZE=%dk > %dk, - assuming %dk flash",
-			flash_size_in_kb, max_flash_size_in_kb, max_flash_size_in_kb);
-		flash_size_in_kb = max_flash_size_in_kb;
+			flash_size_in_kb, stm32lx_info->part_info->max_flash_size_kb,
+			stm32lx_info->part_info->max_flash_size_kb);
+		flash_size_in_kb = stm32lx_info->part_info->max_flash_size_kb;
 	}
 
-	if (stm32lx_info->has_dual_banks) {
+	if (stm32lx_info->part_info->has_dual_banks) {
 		/* Use the configured base address to determine if this is the first or second flash bank.
 		 * Verify that the base address is reasonably correct and determine the flash bank size
 		 */
-		second_bank_base = base_address + first_bank_size_in_kb * 1024;
-		if (bank->base == second_bank_base) {
+		second_bank_base = base_address +
+			stm32lx_info->part_info->first_bank_size_kb * 1024;
+		if (bank->base == second_bank_base || !bank->base) {
 			/* This is the second bank  */
 			base_address = second_bank_base;
-			flash_size_in_kb = flash_size_in_kb - first_bank_size_in_kb;
-		} else if (bank->base == 0 || bank->base == base_address) {
+			flash_size_in_kb = flash_size_in_kb -
+				stm32lx_info->part_info->first_bank_size_kb;
+		} else if (bank->base == base_address) {
 			/* This is the first bank */
-			flash_size_in_kb = first_bank_size_in_kb;
+			flash_size_in_kb = stm32lx_info->part_info->first_bank_size_kb;
 		} else {
 			LOG_WARNING("STM32L flash bank base address config is incorrect."
 				    " 0x%" PRIx32 " but should rather be 0x%" PRIx32 " or 0x%" PRIx32,
@@ -626,9 +758,6 @@ static int stm32lx_probe(struct flash_bank *bank)
 		LOG_INFO("ignoring flash probed value, using configured bank size: %dkbytes", flash_size_in_kb);
 	}
 
-	/* STM32L - we have 32 sectors, 16 pages per sector -> 512 pages
-	 * 16 pages for a protection area */
-
 	/* calculate numbers of sectors (4kB per sector) */
 	int num_sectors = (flash_size_in_kb * 1024) / FLASH_SECTOR_SIZE;
 
@@ -718,95 +847,56 @@ static int stm32lx_erase_check(struct flash_bank *bank)
 	return retval;
 }
 
+/* This method must return a string displaying information about the bank */
 static int stm32lx_get_info(struct flash_bank *bank, char *buf, int buf_size)
 {
-	/* This method must return a string displaying information about the bank */
+	struct stm32lx_flash_bank *stm32lx_info = bank->driver_priv;
 
-	uint32_t dbgmcu_idcode;
-
-	/* read stm32 device id register */
-	int retval = target_read_u32(bank->target, DBGMCU_IDCODE, &dbgmcu_idcode);
-	if (retval != ERROR_OK)
-		return retval;
-
-	uint16_t device_id = dbgmcu_idcode & 0xfff;
-	uint16_t rev_id = dbgmcu_idcode >> 16;
-	const char *device_str;
-	const char *rev_str = NULL;
-
-	switch (device_id) {
-	case 0x416:
-		device_str = "STM32L1xx (Low/Medium Density)";
-
-		switch (rev_id) {
-		case 0x1000:
-			rev_str = "A";
-			break;
-
-		case 0x1008:
-			rev_str = "Y";
-			break;
-
-		case 0x1018:
-			rev_str = "X";
-			break;
-
-		case 0x1038:
-			rev_str = "W";
-			break;
-
-		case 0x1078:
-			rev_str = "V";
-			break;
+	if (!stm32lx_info->probed) {
+		int retval = stm32lx_probe(bank);
+		if (retval != ERROR_OK) {
+			snprintf(buf, buf_size,
+				"Unable to find bank information.");
+			return retval;
 		}
-		break;
-
-	case 0x427:
-		device_str = "STM32L1xx (Medium+ Density)";
-
-		switch (rev_id) {
-		case 0x1018:
-			rev_str = "A";
-			break;
-		}
-		break;
-
-	case 0x436:
-		device_str = "STM32L1xx (Medium+/High Density)";
-
-		switch (rev_id) {
-		case 0x1000:
-			rev_str = "A";
-			break;
-
-		case 0x1008:
-			rev_str = "Z";
-			break;
-
-		case 0x1018:
-			rev_str = "Y";
-			break;
-		}
-		break;
-
-	case 0x437:
-		device_str = "STM32L1xx (Medium+/High Density)";
-		break;
-
-	default:
-		snprintf(buf, buf_size, "Cannot identify target as a STM32L1");
-		return ERROR_FAIL;
 	}
 
-	if (rev_str != NULL)
-		snprintf(buf, buf_size, "%s - Rev: %s", device_str, rev_str);
-	else
-		snprintf(buf, buf_size, "%s - Rev: unknown (0x%04x)", device_str, rev_id);
+	const struct stm32lx_part_info *info = stm32lx_info->part_info;
 
-	return ERROR_OK;
+	if (info) {
+		const char *rev_str = NULL;
+		uint16_t rev_id = stm32lx_info->idcode >> 16;
+
+		for (unsigned int i = 0; i < info->num_revs; i++)
+			if (rev_id == info->revs[i].rev)
+				rev_str = info->revs[i].str;
+
+		if (rev_str != NULL) {
+			snprintf(buf, buf_size,
+				"%s - Rev: %s",
+				stm32lx_info->part_info->device_str, rev_str);
+		} else {
+			snprintf(buf, buf_size,
+				"%s - Rev: unknown (0x%04x)",
+				stm32lx_info->part_info->device_str, rev_id);
+		}
+
+		return ERROR_OK;
+	} else {
+		snprintf(buf, buf_size, "Cannot identify target as a STM32Lx");
+
+		return ERROR_FAIL;
+	}
 }
 
 static const struct command_registration stm32lx_exec_command_handlers[] = {
+	{
+		.name = "mass_erase",
+		.handler = stm32lx_handle_mass_erase_command,
+		.mode = COMMAND_EXEC,
+		.usage = "bank_id",
+		.help = "Erase entire flash device. including available EEPROM",
+	},
 	COMMAND_REGISTRATION_DONE
 };
 
@@ -840,6 +930,7 @@ struct flash_driver stm32lx_flash = {
 static int stm32lx_unlock_program_memory(struct flash_bank *bank)
 {
 	struct target *target = bank->target;
+	struct stm32lx_flash_bank *stm32lx_info = bank->driver_priv;
 	int retval;
 	uint32_t reg32;
 
@@ -849,7 +940,8 @@ static int stm32lx_unlock_program_memory(struct flash_bank *bank)
 	 */
 
 	/* check flash is not already unlocked */
-	retval = target_read_u32(target, FLASH_PECR, &reg32);
+	retval = target_read_u32(target, stm32lx_info->flash_base + FLASH_PECR,
+			&reg32);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -857,16 +949,19 @@ static int stm32lx_unlock_program_memory(struct flash_bank *bank)
 		return ERROR_OK;
 
 	/* To unlock the PECR write the 2 PEKEY to the PEKEYR register */
-	retval = target_write_u32(target, FLASH_PEKEYR, PEKEY1);
+	retval = target_write_u32(target, stm32lx_info->flash_base + FLASH_PEKEYR,
+			PEKEY1);
 	if (retval != ERROR_OK)
 		return retval;
 
-	retval = target_write_u32(target, FLASH_PEKEYR, PEKEY2);
+	retval = target_write_u32(target, stm32lx_info->flash_base + FLASH_PEKEYR,
+			PEKEY2);
 	if (retval != ERROR_OK)
 		return retval;
 
 	/* Make sure it worked */
-	retval = target_read_u32(target, FLASH_PECR, &reg32);
+	retval = target_read_u32(target, stm32lx_info->flash_base + FLASH_PECR,
+			&reg32);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -875,15 +970,18 @@ static int stm32lx_unlock_program_memory(struct flash_bank *bank)
 		return ERROR_FLASH_OPERATION_FAILED;
 	}
 
-	retval = target_write_u32(target, FLASH_PRGKEYR, PRGKEY1);
+	retval = target_write_u32(target, stm32lx_info->flash_base + FLASH_PRGKEYR,
+			PRGKEY1);
 	if (retval != ERROR_OK)
 		return retval;
-	retval = target_write_u32(target, FLASH_PRGKEYR, PRGKEY2);
+	retval = target_write_u32(target, stm32lx_info->flash_base + FLASH_PRGKEYR,
+			PRGKEY2);
 	if (retval != ERROR_OK)
 		return retval;
 
 	/* Make sure it worked */
-	retval = target_read_u32(target, FLASH_PECR, &reg32);
+	retval = target_read_u32(target, stm32lx_info->flash_base + FLASH_PECR,
+			&reg32);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -898,6 +996,7 @@ static int stm32lx_unlock_program_memory(struct flash_bank *bank)
 static int stm32lx_enable_write_half_page(struct flash_bank *bank)
 {
 	struct target *target = bank->target;
+	struct stm32lx_flash_bank *stm32lx_info = bank->driver_priv;
 	int retval;
 	uint32_t reg32;
 
@@ -908,21 +1007,25 @@ static int stm32lx_enable_write_half_page(struct flash_bank *bank)
 	if (retval != ERROR_OK)
 		return retval;
 
-	retval = target_read_u32(target, FLASH_PECR, &reg32);
+	retval = target_read_u32(target, stm32lx_info->flash_base + FLASH_PECR,
+			&reg32);
 	if (retval != ERROR_OK)
 		return retval;
 
 	reg32 |= FLASH_PECR__FPRG;
-	retval = target_write_u32(target, FLASH_PECR, reg32);
+	retval = target_write_u32(target, stm32lx_info->flash_base + FLASH_PECR,
+			reg32);
 	if (retval != ERROR_OK)
 		return retval;
 
-	retval = target_read_u32(target, FLASH_PECR, &reg32);
+	retval = target_read_u32(target, stm32lx_info->flash_base + FLASH_PECR,
+			&reg32);
 	if (retval != ERROR_OK)
 		return retval;
 
 	reg32 |= FLASH_PECR__PROG;
-	retval = target_write_u32(target, FLASH_PECR, reg32);
+	retval = target_write_u32(target, stm32lx_info->flash_base + FLASH_PECR,
+			reg32);
 
 	return retval;
 }
@@ -930,26 +1033,31 @@ static int stm32lx_enable_write_half_page(struct flash_bank *bank)
 static int stm32lx_lock_program_memory(struct flash_bank *bank)
 {
 	struct target *target = bank->target;
+	struct stm32lx_flash_bank *stm32lx_info = bank->driver_priv;
 	int retval;
 	uint32_t reg32;
 
 	/* To lock the program memory, simply set the lock bit and lock PECR */
 
-	retval = target_read_u32(target, FLASH_PECR, &reg32);
+	retval = target_read_u32(target, stm32lx_info->flash_base + FLASH_PECR,
+			&reg32);
 	if (retval != ERROR_OK)
 		return retval;
 
 	reg32 |= FLASH_PECR__PRGLOCK;
-	retval = target_write_u32(target, FLASH_PECR, reg32);
+	retval = target_write_u32(target, stm32lx_info->flash_base + FLASH_PECR,
+			reg32);
 	if (retval != ERROR_OK)
 		return retval;
 
-	retval = target_read_u32(target, FLASH_PECR, &reg32);
+	retval = target_read_u32(target, stm32lx_info->flash_base + FLASH_PECR,
+			&reg32);
 	if (retval != ERROR_OK)
 		return retval;
 
 	reg32 |= FLASH_PECR__PELOCK;
-	retval = target_write_u32(target, FLASH_PECR, reg32);
+	retval = target_write_u32(target, stm32lx_info->flash_base + FLASH_PECR,
+			reg32);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -959,11 +1067,12 @@ static int stm32lx_lock_program_memory(struct flash_bank *bank)
 static int stm32lx_erase_sector(struct flash_bank *bank, int sector)
 {
 	struct target *target = bank->target;
+	struct stm32lx_flash_bank *stm32lx_info = bank->driver_priv;
 	int retval;
 	uint32_t reg32;
 
 	/*
-	 * To erase a sector (i.e. FLASH_PAGES_PER_SECTOR pages),
+	 * To erase a sector (i.e. stm32lx_info->part_info.pages_per_sector pages),
 	 * first unlock the memory, loop over the pages of this sector
 	 * and write 0x0 to its first word.
 	 */
@@ -972,9 +1081,11 @@ static int stm32lx_erase_sector(struct flash_bank *bank, int sector)
 	if (retval != ERROR_OK)
 		return retval;
 
-	for (int page = 0; page < FLASH_PAGES_PER_SECTOR; page++) {
+	for (int page = 0; page < (int)stm32lx_info->part_info->pages_per_sector;
+			page++) {
 		reg32 = FLASH_PECR__PROG | FLASH_PECR__ERASE;
-		retval = target_write_u32(target, FLASH_PECR, reg32);
+		retval = target_write_u32(target,
+				stm32lx_info->flash_base + FLASH_PECR, reg32);
 		if (retval != ERROR_OK)
 			return retval;
 
@@ -983,7 +1094,7 @@ static int stm32lx_erase_sector(struct flash_bank *bank, int sector)
 			return retval;
 
 		uint32_t addr = bank->base + bank->sectors[sector].offset + (page
-				* FLASH_PAGE_SIZE);
+				* stm32lx_info->part_info->page_size);
 		retval = target_write_u32(target, addr, 0x0);
 		if (retval != ERROR_OK)
 			return retval;
@@ -1000,21 +1111,79 @@ static int stm32lx_erase_sector(struct flash_bank *bank, int sector)
 	return ERROR_OK;
 }
 
-static int stm32lx_wait_until_bsy_clear(struct flash_bank *bank)
+static inline int stm32lx_get_flash_status(struct flash_bank *bank, uint32_t *status)
 {
 	struct target *target = bank->target;
-	uint32_t status;
-	int retval = ERROR_OK;
-	int timeout = 100;
+	struct stm32lx_flash_bank *stm32lx_info = bank->driver_priv;
 
-	/* wait for busy to clear */
-	for (;;) {
-		retval = target_read_u32(target, FLASH_SR, &status);
+	return target_read_u32(target, stm32lx_info->flash_base + FLASH_SR, status);
+}
+
+static int stm32lx_wait_until_bsy_clear(struct flash_bank *bank)
+{
+	return stm32lx_wait_until_bsy_clear_timeout(bank, 100);
+}
+
+static int stm32lx_unlock_options_bytes(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	struct stm32lx_flash_bank *stm32lx_info = bank->driver_priv;
+	int retval;
+	uint32_t reg32;
+
+	/*
+	* Unlocking the options bytes is done by unlocking the PECR,
+	* then by writing the 2 FLASH_PEKEYR to the FLASH_OPTKEYR register
+	*/
+
+	/* check flash is not already unlocked */
+	retval = target_read_u32(target, stm32lx_info->flash_base + FLASH_PECR, &reg32);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if ((reg32 & FLASH_PECR__OPTLOCK) == 0)
+		return ERROR_OK;
+
+	if ((reg32 & FLASH_PECR__PELOCK) != 0) {
+
+		retval = target_write_u32(target, stm32lx_info->flash_base + FLASH_PEKEYR, PEKEY1);
 		if (retval != ERROR_OK)
 			return retval;
 
+		retval = target_write_u32(target, stm32lx_info->flash_base + FLASH_PEKEYR, PEKEY2);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	/* To unlock the PECR write the 2 OPTKEY to the FLASH_OPTKEYR register */
+	retval = target_write_u32(target, stm32lx_info->flash_base + FLASH_OPTKEYR, OPTKEY1);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = target_write_u32(target, stm32lx_info->flash_base + FLASH_OPTKEYR, OPTKEY2);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return ERROR_OK;
+}
+
+static int stm32lx_wait_until_bsy_clear_timeout(struct flash_bank *bank, int timeout)
+{
+	struct target *target = bank->target;
+	struct stm32lx_flash_bank *stm32lx_info = bank->driver_priv;
+	uint32_t status;
+	int retval = ERROR_OK;
+
+	/* wait for busy to clear */
+	for (;;) {
+		retval = stm32lx_get_flash_status(bank, &status);
+		if (retval != ERROR_OK)
+			return retval;
+
+		LOG_DEBUG("status: 0x%" PRIx32 "", status);
 		if ((status & FLASH_SR__BSY) == 0)
 			break;
+
 		if (timeout-- <= 0) {
 			LOG_ERROR("timed out waiting for flash");
 			return ERROR_FAIL;
@@ -1032,5 +1201,87 @@ static int stm32lx_wait_until_bsy_clear(struct flash_bank *bank)
 		retval = ERROR_FAIL;
 	}
 
+	/* Clear but report errors */
+	if (status & FLASH_SR__OPTVERR) {
+		/* If this operation fails, we ignore it and report the original retval */
+		target_write_u32(target, stm32lx_info->flash_base + FLASH_SR, status & FLASH_SR__OPTVERR);
+	}
+
 	return retval;
 }
+
+static int stm32lx_obl_launch(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	struct stm32lx_flash_bank *stm32lx_info = bank->driver_priv;
+	int retval;
+
+	/* This will fail as the target gets immediately rebooted */
+	target_write_u32(target, stm32lx_info->flash_base + FLASH_PECR,
+			 FLASH_PECR__OBL_LAUNCH);
+
+	size_t tries = 10;
+	do {
+		target_halt(target);
+		retval = target_poll(target);
+	} while (--tries > 0 &&
+		 (retval != ERROR_OK || target->state != TARGET_HALTED));
+
+	return tries ? ERROR_OK : ERROR_FAIL;
+}
+
+static int stm32lx_mass_erase(struct flash_bank *bank)
+{
+	int retval;
+	struct target *target = bank->target;
+	struct stm32lx_flash_bank *stm32lx_info = NULL;
+	uint32_t reg32;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	stm32lx_info = bank->driver_priv;
+
+	retval = stm32lx_unlock_options_bytes(bank);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* mass erase flash memory */
+	/* set the RDP protection level to 1 */
+	retval = target_write_u32(target, OPTION_BYTES_ADDRESS, OPTION_BYTE_0_PR1);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = stm32lx_obl_launch(bank);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = stm32lx_unlock_options_bytes(bank);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* set the RDP protection level to 0 */
+	retval = target_write_u32(target, OPTION_BYTES_ADDRESS, OPTION_BYTE_0_PR0);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = stm32lx_wait_until_bsy_clear_timeout(bank, 30000);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = stm32lx_obl_launch(bank);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = target_read_u32(target, stm32lx_info->flash_base + FLASH_PECR, &reg32);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = target_write_u32(target, stm32lx_info->flash_base + FLASH_PECR, reg32 | FLASH_PECR__OPTLOCK);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return ERROR_OK;
+}

src/flash/nor/tcl.c

@@ -31,11 +31,18 @@
  * Implements Tcl commands used to access NOR flash facilities.
  */
 
-COMMAND_HELPER(flash_command_get_bank, unsigned name_index,
-	struct flash_bank **bank)
+COMMAND_HELPER(flash_command_get_bank_maybe_probe, unsigned name_index,
+	       struct flash_bank **bank, bool do_probe)
 {
 	const char *name = CMD_ARGV[name_index];
-	int retval = get_flash_bank_by_name(name, bank);
+	int retval;
+	if (do_probe) {
+		retval = get_flash_bank_by_name(name, bank);
+	} else {
+		*bank  = get_flash_bank_by_name_noprobe(name);
+		retval = ERROR_OK;
+	}
+
 	if (retval != ERROR_OK)
 		return retval;
 	if (*bank)
@@ -44,7 +51,20 @@ COMMAND_HELPER(flash_command_get_bank, unsigned name_index,
 	unsigned bank_num;
 	COMMAND_PARSE_NUMBER(uint, name, bank_num);
 
-	return get_flash_bank_by_num(bank_num, bank);
+	if (do_probe) {
+		return get_flash_bank_by_num(bank_num, bank);
+	} else {
+		*bank  = get_flash_bank_by_num_noprobe(bank_num);
+		retval = (bank) ? ERROR_OK : ERROR_FAIL;
+		return retval;
+	}
+}
+
+COMMAND_HELPER(flash_command_get_bank, unsigned name_index,
+	struct flash_bank **bank)
+{
+	return CALL_COMMAND_HANDLER(flash_command_get_bank_maybe_probe,
+				    name_index, bank, true);
 }
 
 COMMAND_HANDLER(handle_flash_info_command)
@@ -121,7 +141,7 @@ COMMAND_HANDLER(handle_flash_probe_command)
 	if (CMD_ARGC != 1)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
-	retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &p);
+	retval = CALL_COMMAND_HANDLER(flash_command_get_bank_maybe_probe, 0, &p, false);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -794,7 +814,7 @@ COMMAND_HANDLER(handle_flash_bank_command)
 	int retval;
 	retval = CALL_COMMAND_HANDLER(driver->flash_bank_command, c);
 	if (ERROR_OK != retval) {
-		LOG_ERROR("'%s' driver rejected flash bank at 0x%8.8" PRIx32 "Usage %s",
+		LOG_ERROR("'%s' driver rejected flash bank at 0x%8.8" PRIx32 "; usage: %s",
 			driver_name, c->base, driver->usage);
 		free(c);
 		return retval;

src/flash/startup.tcl

@@ -3,16 +3,28 @@
 #
 # program utility proc
 # usage: program filename
-# optional args: verify, reset and address
+# optional args: verify, reset, exit and address
 #
 
+proc program_error {description exit} {
+	if {$exit == 1} {
+		echo $description
+		shutdown error
+	}
+
+	error $description
+}
+
 proc program {filename args} {
+	set exit 0
 
 	foreach arg $args {
 		if {[string equal $arg "verify"]} {
 			set verify 1
 		} elseif {[string equal $arg "reset"]} {
 			set reset 1
+		} elseif {[string equal $arg "exit"]} {
+			set exit 1
 		} else {
 			set address $arg
 		}
@@ -20,16 +32,12 @@ proc program {filename args} {
 
 	# make sure init is called
 	if {[catch {init}] != 0} {
-		echo "** OpenOCD init Failed **"
-		shutdown
-		return
+		program_error "** OpenOCD init failed **" 1
 	}
 
 	# reset target and call any init scripts
 	if {[catch {reset init}] != 0} {
-		echo "** Unable to reset target **"
-		shutdown
-		return
+		program_error "** Unable to reset target **" $exit
 	}
 
 	# start programming phase
@@ -48,7 +56,7 @@ proc program {filename args} {
 			if {[catch {eval verify_image $flash_args}] == 0} {
 				echo "** Verified OK **"
 			} else {
-				echo "** Verify Failed **"
+				program_error "** Verify Failed **" $exit
 			}
 		}
 
@@ -60,15 +68,17 @@ proc program {filename args} {
 			reset run
 		}
 	} else {
-		echo "** Programming Failed **"
+		program_error "** Programming Failed **" $exit
 	}
 
-	# shutdown OpenOCD
-	shutdown
+	if {$exit == 1} {
+		shutdown
+	}
+	return
 }
 
-add_help_text program "write an image to flash, address is only required for binary images. verify, reset are optional"
-add_usage_text program "<filename> \[address\] \[verify\] \[reset\]"
+add_help_text program "write an image to flash, address is only required for binary images. verify, reset, exit are optional"
+add_usage_text program "<filename> \[address\] \[verify\] \[reset\] \[exit\]"
 
 # stm32f0x uses the same flash driver as the stm32f1x
 # this alias enables the use of either name.

src/helper/Makefile.am

@@ -44,18 +44,9 @@ noinst_HEADERS = \
 	replacements.h \
 	fileio.h \
 	system.h \
-	bin2char.c \
+	bin2char.sh \
 	jim-nvp.h
 
 EXTRA_DIST = startup.tcl
 
-BIN2C = bin2char$(EXEEXT_FOR_BUILD)
-
-BUILT_SOURCES = $(BIN2C)
-
-$(BIN2C): bin2char.c
-	${CC_FOR_BUILD} ${CFLAGS_FOR_BUILD} $(srcdir)/bin2char.c -o $@
-
-CLEANFILES = bin2char$(EXEEXT_FOR_BUILD)
-
 MAINTAINERCLEANFILES = $(srcdir)/Makefile.in

src/helper/bin2char.sh

@@ -0,0 +1,14 @@
+#!/bin/sh
+
+[ $# != 0 ] && {
+    echo "Usage: $0"
+    echo
+    echo "Read binary data from standard input and write it as a comma separated"
+    echo "list of hexadecimal byte values to standard ouput. The output is usable"
+    echo "as a C array initializer. It is terminated with a comma so it can be"
+    echo "continued e.g. for zero termination."
+    exit 1
+}
+
+echo "/* Autogenerated with $0 */"
+od -v -A n -t x1 | sed 's/ *\(..\) */0x\1,/g'

src/helper/binarybuffer.c

@@ -231,7 +231,7 @@ char *buf_to_str(const void *_buf, unsigned buf_len, unsigned radix)
 		}
 	}
 
-	const char *DIGITS = "0123456789ABCDEF";
+	const char * const DIGITS = "0123456789ABCDEF";
 	for (unsigned j = 0; j < str_len; j++)
 		str[j] = DIGITS[(int)str[j]];
 

src/helper/binarybuffer.h

@@ -39,7 +39,7 @@
  * @param num The number of bits from @c value to copy (1-32).
  * @param value Up to 32 bits that will be copied to _buffer.
  */
-static inline void buf_set_u32(void *_buffer,
+static inline void buf_set_u32(uint8_t *_buffer,
 	unsigned first, unsigned num, uint32_t value)
 {
 	uint8_t *buffer = _buffer;
@@ -68,7 +68,7 @@ static inline void buf_set_u32(void *_buffer,
  * @param num The number of bits from @c value to copy (1-64).
  * @param value Up to 64 bits that will be copied to _buffer.
  */
-static inline void buf_set_u64(void *_buffer,
+static inline void buf_set_u64(uint8_t *_buffer,
 	unsigned first, unsigned num, uint64_t value)
 {
 	uint8_t *buffer = _buffer;
@@ -106,7 +106,7 @@ static inline void buf_set_u64(void *_buffer,
  * @param num The number of bits from @c _buffer to read (1-32).
  * @returns Up to 32-bits that were read from @c _buffer.
  */
-static inline uint32_t buf_get_u32(const void *_buffer,
+static inline uint32_t buf_get_u32(const uint8_t *_buffer,
 	unsigned first, unsigned num)
 {
 	const uint8_t *buffer = _buffer;
@@ -135,7 +135,7 @@ static inline uint32_t buf_get_u32(const void *_buffer,
  * @param num The number of bits from @c _buffer to read (1-64).
  * @returns Up to 64-bits that were read from @c _buffer.
  */
-static inline uint64_t buf_get_u64(const void *_buffer,
+static inline uint64_t buf_get_u64(const uint8_t *_buffer,
 	unsigned first, unsigned num)
 {
 	const uint8_t *buffer = _buffer;

src/helper/command.c

@@ -121,7 +121,7 @@ static int command_retval_set(Jim_Interp *interp, int retval)
 	if (return_retval != NULL)
 		*return_retval = retval;
 
-	return (retval == ERROR_OK) ? JIM_OK : JIM_ERR;
+	return (retval == ERROR_OK) ? JIM_OK : retval;
 }
 
 extern struct command_context *global_cmd_ctx;
@@ -365,7 +365,7 @@ static int register_command_handler(struct command_context *cmd_ctx,
 
 	LOG_DEBUG("registering '%s'...", ocd_name);
 
-	Jim_CmdProc func = c->handler ? &script_command : &command_unknown;
+	Jim_CmdProc *func = c->handler ? &script_command : &command_unknown;
 	int retval = Jim_CreateCommand(interp, ocd_name, func, c, NULL);
 	free(ocd_name);
 	if (JIM_OK != retval)
@@ -659,21 +659,7 @@ int command_run_line(struct command_context *context, char *line)
 		}
 		Jim_DeleteAssocData(interp, "context");
 	}
-	if (retcode == JIM_ERR) {
-		if (retval != ERROR_COMMAND_CLOSE_CONNECTION) {
-			/* We do not print the connection closed error message */
-			Jim_MakeErrorMessage(interp);
-			LOG_USER("%s", Jim_GetString(Jim_GetResult(interp), NULL));
-		}
-		if (retval == ERROR_OK) {
-			/* It wasn't a low level OpenOCD command that failed */
-			return ERROR_FAIL;
-		}
-		return retval;
-	} else if (retcode == JIM_EXIT) {
-		/* ignore.
-		 * exit(Jim_GetExitCode(interp)); */
-	} else {
+	if (retcode == JIM_OK) {
 		const char *result;
 		int reslen;
 
@@ -693,7 +679,22 @@ int command_run_line(struct command_context *context, char *line)
 			LOG_USER_N("\n");
 		}
 		retval = ERROR_OK;
+	} else if (retcode == JIM_EXIT) {
+		/* ignore.
+		 * exit(Jim_GetExitCode(interp)); */
+	} else if (retcode == ERROR_COMMAND_CLOSE_CONNECTION) {
+		return retcode;
+	} else {
+		Jim_MakeErrorMessage(interp);
+		LOG_USER("%s", Jim_GetString(Jim_GetResult(interp), NULL));
+
+		if (retval == ERROR_OK) {
+			/* It wasn't a low level OpenOCD command that failed */
+			return ERROR_FAIL;
+		}
+		return retval;
 	}
+
 	return retval;
 }
 
@@ -1070,8 +1071,10 @@ static int jim_command_type(Jim_Interp *interp, int argc, Jim_Obj *const *argv)
 		Jim_SetResultString(interp, "native", -1);
 	else if (c->handler)
 		Jim_SetResultString(interp, "simple", -1);
-	else
+	else if (remaining == 0)
 		Jim_SetResultString(interp, "group", -1);
+	else
+		Jim_SetResultString(interp, "unknown", -1);
 
 	return JIM_OK;
 }

src/helper/command.h

@@ -168,7 +168,7 @@ struct command {
 	struct command *parent;
 	struct command *children;
 	command_handler_t handler;
-	Jim_CmdProc jim_handler;
+	Jim_CmdProc *jim_handler;
 	void *jim_handler_data;
 	enum command_mode mode;
 	struct command *next;
@@ -204,7 +204,7 @@ char *command_name(struct command *c, char delim);
 struct command_registration {
 	const char *name;
 	command_handler_t handler;
-	Jim_CmdProc jim_handler;
+	Jim_CmdProc *jim_handler;
 	void *jim_handler_data;
 	enum command_mode mode;
 	const char *help;

src/helper/log.c

@@ -52,7 +52,7 @@ static long long current_time;
 
 static long long start;
 
-static char *log_strings[5] = {
+static const char * const log_strings[5] = {
 	"User : ",
 	"Error: ",
 	"Warn : ",	/* want a space after each colon, all same width, colons aligned */

src/helper/log.h

@@ -138,5 +138,7 @@ extern int debug_level;
  * make no assumptions about what went wrong and try to handle the problem.
  */
 #define ERROR_FAIL						(-4)
+#define ERROR_WAIT						(-5)
+
 
 #endif	/* LOG_H */

src/helper/options.c

@@ -141,7 +141,6 @@ static void add_default_dirs(void)
 int parse_cmdline_args(struct command_context *cmd_ctx, int argc, char *argv[])
 {
 	int c;
-	char command_buffer[128];
 
 	while (1) {
 		/* getopt_long stores the option index here. */
@@ -164,24 +163,26 @@ int parse_cmdline_args(struct command_context *cmd_ctx, int argc, char *argv[])
 				break;
 			case 'f':		/* --file | -f */
 			{
-				snprintf(command_buffer, 128, "script {%s}", optarg);
-				add_config_command(command_buffer);
+				char *command = alloc_printf("script {%s}", optarg);
+				add_config_command(command);
+				free(command);
 				break;
 			}
 			case 's':		/* --search | -s */
 				add_script_search_dir(optarg);
 				break;
 			case 'd':		/* --debug | -d */
-				if (optarg)
-					snprintf(command_buffer, 128, "debug_level %s", optarg);
-				else
-					snprintf(command_buffer, 128, "debug_level 3");
-				command_run_line(cmd_ctx, command_buffer);
+			{
+				char *command = alloc_printf("debug_level %s", optarg ? optarg : "3");
+				command_run_line(cmd_ctx, command);
+				free(command);
 				break;
+			}
 			case 'l':		/* --log_output | -l */
 				if (optarg) {
-					snprintf(command_buffer, 128, "log_output %s", optarg);
-					command_run_line(cmd_ctx, command_buffer);
+					char *command = alloc_printf("log_output %s", optarg);
+					command_run_line(cmd_ctx, command);
+					free(command);
 				}
 				break;
 			case 'c':		/* --command | -c */

src/helper/replacements.h

@@ -135,7 +135,6 @@ static inline unsigned usleep(unsigned int usecs)
 /* Windows specific */
 #ifdef _WIN32
 
-#define WIN32_LEAN_AND_MEAN
 #include <windows.h>
 #include <time.h>
 

src/helper/startup.tcl

@@ -16,10 +16,12 @@ proc exit {} {
 proc ocd_bouncer {name args} {
 	set cmd [format "ocd_%s" $name]
 	set type [eval ocd_command type $cmd $args]
+	set errcode error
 	if {$type == "native"} {
 		return [eval $cmd $args]
 	} else {if {$type == "simple"} {
-		if {[catch {eval $cmd $args}] == 0} {
+		set errcode [catch {eval $cmd $args}]
+		if {$errcode == 0} {
 			return ""
 		} else {
 			# 'classic' commands output error message as part of progress output
@@ -30,9 +32,9 @@ proc ocd_bouncer {name args} {
 		set errmsg [format "%s: command requires more arguments" \
 			[concat $name " " $args]]
 	} else {
-		set errmsg [format "Unknown command type: %s" $type]
+		set errmsg [format "invalid subcommand \"%s\"" $args]
 	}}}
-	return -code error $errmsg
+	return -code $errcode $errmsg
 }
 
 # Try flipping / and \ to find file if the filename does not

src/helper/types.h

@@ -270,6 +270,25 @@ static inline void buf_bswap32(uint8_t *dst, const uint8_t *src, size_t len)
 	}
 }
 
+/**
+ * Calculate the (even) parity of a 32-bit datum.
+ * @param x The datum.
+ * @return 1 if the number of set bits in x is odd, 0 if it is even.
+ */
+static inline int parity_u32(uint32_t x)
+{
+#ifdef __GNUC__
+	return __builtin_parityl(x);
+#else
+	x ^= x >> 16;
+	x ^= x >> 8;
+	x ^= x >> 4;
+	x ^= x >> 2;
+	x ^= x >> 1;
+	return x & 1;
+#endif
+}
+
 #if defined(__ECOS)
 
 /* eCos plain lacks these definition... A series of upstream patches

src/jtag/adapter.c

@@ -48,7 +48,7 @@
  */
 
 extern struct jtag_interface *jtag_interface;
-const char *jtag_only[] = { "jtag", NULL };
+const char * const jtag_only[] = { "jtag", NULL };
 
 static int jim_adapter_name(Jim_Interp *interp, int argc, Jim_Obj * const *argv)
 {

src/jtag/aice/aice_interface.c

@@ -31,7 +31,7 @@
 #include "aice_usb.h"
 
 #define AICE_KHZ_TO_SPEED_MAP_SIZE	16
-static int aice_khz_to_speed_map[AICE_KHZ_TO_SPEED_MAP_SIZE] = {
+static const int aice_khz_to_speed_map[AICE_KHZ_TO_SPEED_MAP_SIZE] = {
 	30000,
 	15000,
 	7500,

src/jtag/aice/aice_interface.h

@@ -22,9 +22,9 @@
 
 struct aice_interface_param_s {
 	/** */
-	char *device_desc;
+	const char *device_desc;
 	/** */
-	char *serial;
+	const char *serial;
 	/** */
 	uint16_t vid;
 	/** */

src/jtag/aice/aice_port.h

@@ -106,9 +106,9 @@ enum aice_command_mode {
 
 struct aice_port_param_s {
 	/** */
-	char *device_desc;
+	const char *device_desc;
 	/** */
-	char *serial;
+	const char *serial;
 	/** */
 	uint16_t vid;
 	/** */
@@ -225,7 +225,7 @@ struct aice_port_api_s {
 /** */
 struct aice_port {
 	/** */
-	char *name;
+	const char *name;
 	/** */
 	int type;
 	/** */

src/jtag/aice/aice_usb.c

@@ -2099,7 +2099,7 @@ static int aice_usb_open(struct aice_port_param_s *param)
 	const uint16_t pids[] = { param->pid, 0 };
 	struct jtag_libusb_device_handle *devh;
 
-	if (jtag_libusb_open(vids, pids, &devh) != ERROR_OK)
+	if (jtag_libusb_open(vids, pids, NULL, &devh) != ERROR_OK)
 		return ERROR_FAIL;
 
 	/* BE ***VERY CAREFUL*** ABOUT MAKING CHANGES IN THIS
@@ -2123,7 +2123,7 @@ static int aice_usb_open(struct aice_port_param_s *param)
 	/* reopen jlink after usb_reset
 	 * on win32 this may take a second or two to re-enumerate */
 	int retval;
-	while ((retval = jtag_libusb_open(vids, pids, &devh)) != ERROR_OK) {
+	while ((retval = jtag_libusb_open(vids, pids, NULL, &devh)) != ERROR_OK) {
 		usleep(1000);
 		timeout--;
 		if (!timeout)
@@ -2136,13 +2136,11 @@ static int aice_usb_open(struct aice_port_param_s *param)
 #endif
 
 	/* usb_set_configuration required under win32 */
-	struct jtag_libusb_device *udev = jtag_libusb_get_device(devh);
 	jtag_libusb_set_configuration(devh, 0);
-	jtag_libusb_claim_interface(devh, 0);
 
 	unsigned int aice_read_ep;
 	unsigned int aice_write_ep;
-	jtag_libusb_get_endpoints(udev, &aice_read_ep, &aice_write_ep);
+	jtag_libusb_choose_interface(devh, &aice_read_ep, &aice_write_ep, -1, -1, -1);
 
 	aice_handler.usb_read_ep = aice_read_ep;
 	aice_handler.usb_write_ep = aice_write_ep;

src/jtag/core.c

@@ -90,11 +90,6 @@ static int jtag_srst = -1;
  * List all TAPs that have been created.
  */
 static struct jtag_tap *__jtag_all_taps;
-/**
- * The number of TAPs in the __jtag_all_taps list, used to track the
- * assigned chain position to new TAPs
- */
-static unsigned jtag_num_taps;
 
 static enum reset_types jtag_reset_config = RESET_NONE;
 tap_state_t cmd_queue_cur_state = TAP_RESET;
@@ -166,6 +161,9 @@ bool is_jtag_poll_safe(void)
 	 * It is also implicitly disabled while TRST is active and
 	 * while SRST is gating the JTAG clock.
 	 */
+	if (!transport_is_jtag())
+		return jtag_poll;
+
 	if (!jtag_poll || jtag_trst != 0)
 		return false;
 	return jtag_srst == 0 || (jtag_reset_config & RESET_SRST_NO_GATING);
@@ -190,7 +188,13 @@ struct jtag_tap *jtag_all_taps(void)
 
 unsigned jtag_tap_count(void)
 {
-	return jtag_num_taps;
+	struct jtag_tap *t = jtag_all_taps();
+	unsigned n = 0;
+	while (t) {
+		n++;
+		t = t->next_tap;
+	}
+	return n;
 }
 
 unsigned jtag_tap_count_enabled(void)
@@ -208,12 +212,15 @@ unsigned jtag_tap_count_enabled(void)
 /** Append a new TAP to the chain of all taps. */
 void jtag_tap_add(struct jtag_tap *t)
 {
-	t->abs_chain_position = jtag_num_taps++;
+	unsigned jtag_num_taps = 0;
 
 	struct jtag_tap **tap = &__jtag_all_taps;
-	while (*tap != NULL)
+	while (*tap != NULL) {
+		jtag_num_taps++;
 		tap = &(*tap)->next_tap;
+	}
 	*tap = t;
+	t->abs_chain_position = jtag_num_taps;
 }
 
 /* returns a pointer to the n-th device in the scan chain */
@@ -642,6 +649,12 @@ void swd_add_reset(int req_srst)
 			if (adapter_nsrst_delay)
 				jtag_add_sleep(adapter_nsrst_delay * 1000);
 		}
+
+		retval = jtag_execute_queue();
+		if (retval != ERROR_OK) {
+			LOG_ERROR("SRST timings error");
+			return;
+		}
 	}
 }
 
@@ -879,11 +892,7 @@ void jtag_sleep(uint32_t us)
 		alive_sleep((us+999)/1000);
 }
 
-/* Maximum number of enabled JTAG devices we expect in the scan chain,
- * plus one (to detect garbage at the end).  Devices that don't support
- * IDCODE take up fewer bits, possibly allowing a few more devices.
- */
-#define JTAG_MAX_CHAIN_SIZE 20
+#define JTAG_MAX_AUTO_TAPS 20
 
 #define EXTRACT_MFG(X)  (((X) & 0xffe) >> 1)
 #define EXTRACT_PART(X) (((X) & 0xffff000) >> 12)
@@ -906,7 +915,7 @@ static int jtag_examine_chain_execute(uint8_t *idcode_buffer, unsigned num_idcod
 	};
 
 	/* initialize to the end of chain ID value */
-	for (unsigned i = 0; i < JTAG_MAX_CHAIN_SIZE; i++)
+	for (unsigned i = 0; i < num_idcode; i++)
 		buf_set_u32(idcode_buffer, i * 32, 32, END_OF_CHAIN_FLAG);
 
 	jtag_add_plain_dr_scan(field.num_bits, field.out_value, field.in_value, TAP_DRPAUSE);
@@ -991,21 +1000,16 @@ static bool jtag_examine_chain_end(uint8_t *idcodes, unsigned count, unsigned ma
 
 static bool jtag_examine_chain_match_tap(const struct jtag_tap *tap)
 {
-	uint32_t idcode = tap->idcode;
 
-	/* ignore expected BYPASS codes; warn otherwise */
-	if (0 == tap->expected_ids_cnt && !idcode)
+	if (tap->expected_ids_cnt == 0 || !tap->hasidcode)
 		return true;
 
 	/* optionally ignore the JTAG version field - bits 28-31 of IDCODE */
 	uint32_t mask = tap->ignore_version ? ~(0xf << 28) : ~0;
-
-	idcode &= mask;
+	uint32_t idcode = tap->idcode & mask;
 
 	/* Loop over the expected identification codes and test for a match */
-	unsigned ii, limit = tap->expected_ids_cnt;
-
-	for (ii = 0; ii < limit; ii++) {
+	for (unsigned ii = 0; ii < tap->expected_ids_cnt; ii++) {
 		uint32_t expected = tap->expected_ids[ii] & mask;
 
 		if (idcode == expected)
@@ -1019,10 +1023,10 @@ static bool jtag_examine_chain_match_tap(const struct jtag_tap *tap)
 	/* If none of the expected ids matched, warn */
 	jtag_examine_chain_display(LOG_LVL_WARNING, "UNEXPECTED",
 		tap->dotted_name, tap->idcode);
-	for (ii = 0; ii < limit; ii++) {
+	for (unsigned ii = 0; ii < tap->expected_ids_cnt; ii++) {
 		char msg[32];
 
-		snprintf(msg, sizeof(msg), "expected %u of %u", ii + 1, limit);
+		snprintf(msg, sizeof(msg), "expected %u of %u", ii + 1, tap->expected_ids_cnt);
 		jtag_examine_chain_display(LOG_LVL_ERROR, msg,
 			tap->dotted_name, tap->expected_ids[ii]);
 	}
@@ -1034,130 +1038,108 @@ static bool jtag_examine_chain_match_tap(const struct jtag_tap *tap)
  */
 static int jtag_examine_chain(void)
 {
-	uint8_t idcode_buffer[JTAG_MAX_CHAIN_SIZE * 4];
-	unsigned bit_count;
 	int retval;
-	int tapcount = 0;
-	bool autoprobe = false;
+	unsigned max_taps = jtag_tap_count();
+
+	/* Autoprobe up to this many. */
+	if (max_taps < JTAG_MAX_AUTO_TAPS)
+		max_taps = JTAG_MAX_AUTO_TAPS;
+
+	/* Add room for end-of-chain marker. */
+	max_taps++;
+
+	uint8_t *idcode_buffer = malloc(max_taps * 4);
+	if (idcode_buffer == NULL)
+		return ERROR_JTAG_INIT_FAILED;
 
 	/* DR scan to collect BYPASS or IDCODE register contents.
 	 * Then make sure the scan data has both ones and zeroes.
 	 */
 	LOG_DEBUG("DR scan interrogation for IDCODE/BYPASS");
-	retval = jtag_examine_chain_execute(idcode_buffer, JTAG_MAX_CHAIN_SIZE);
+	retval = jtag_examine_chain_execute(idcode_buffer, max_taps);
 	if (retval != ERROR_OK)
-		return retval;
-	if (!jtag_examine_chain_check(idcode_buffer, JTAG_MAX_CHAIN_SIZE))
-		return ERROR_JTAG_INIT_FAILED;
+		goto out;
+	if (!jtag_examine_chain_check(idcode_buffer, max_taps)) {
+		retval = ERROR_JTAG_INIT_FAILED;
+		goto out;
+	}
 
-	/* point at the 1st tap */
+	/* Point at the 1st predefined tap, if any */
 	struct jtag_tap *tap = jtag_tap_next_enabled(NULL);
 
-	if (!tap)
-		autoprobe = true;
-
-	for (bit_count = 0;
-	     tap && bit_count < (JTAG_MAX_CHAIN_SIZE * 32) - 31;
-	     tap = jtag_tap_next_enabled(tap)) {
+	unsigned bit_count = 0;
+	unsigned autocount = 0;
+	for (unsigned i = 0; i < max_taps; i++) {
+		assert(bit_count < max_taps * 32);
 		uint32_t idcode = buf_get_u32(idcode_buffer, bit_count, 32);
 
+		/* No predefined TAP? Auto-probe. */
+		if (tap == NULL) {
+			/* Is there another TAP? */
+			if (jtag_idcode_is_final(idcode))
+				break;
+
+			/* Default everything in this TAP except IR length.
+			 *
+			 * REVISIT create a jtag_alloc(chip, tap) routine, and
+			 * share it with jim_newtap_cmd().
+			 */
+			tap = calloc(1, sizeof *tap);
+			if (!tap) {
+				retval = ERROR_FAIL;
+				goto out;
+			}
+
+			tap->chip = alloc_printf("auto%u", autocount++);
+			tap->tapname = strdup("tap");
+			tap->dotted_name = alloc_printf("%s.%s", tap->chip, tap->tapname);
+
+			tap->ir_length = 0; /* ... signifying irlen autoprobe */
+			tap->ir_capture_mask = 0x03;
+			tap->ir_capture_value = 0x01;
+
+			tap->enabled = true;
+
+			jtag_tap_init(tap);
+		}
+
 		if ((idcode & 1) == 0) {
 			/* Zero for LSB indicates a device in bypass */
-			LOG_INFO("TAP %s does not have IDCODE",
-				tap->dotted_name);
-			idcode = 0;
+			LOG_INFO("TAP %s does not have IDCODE", tap->dotted_name);
 			tap->hasidcode = false;
+			tap->idcode = 0;
 
 			bit_count += 1;
 		} else {
 			/* Friendly devices support IDCODE */
 			tap->hasidcode = true;
-			jtag_examine_chain_display(LOG_LVL_INFO,
-				"tap/device found",
-				tap->dotted_name, idcode);
+			tap->idcode = idcode;
+			jtag_examine_chain_display(LOG_LVL_INFO, "tap/device found", tap->dotted_name, idcode);
 
 			bit_count += 32;
 		}
-		tap->idcode = idcode;
 
 		/* ensure the TAP ID matches what was expected */
 		if (!jtag_examine_chain_match_tap(tap))
 			retval = ERROR_JTAG_INIT_SOFT_FAIL;
-	}
 
-	/* Fail if too many TAPs were enabled for us to verify them all. */
-	if (tap) {
-		LOG_ERROR("Too many TAPs enabled; '%s' ignored.",
-			tap->dotted_name);
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	/* if autoprobing, the tap list is still empty ... populate it! */
-	while (autoprobe && bit_count < (JTAG_MAX_CHAIN_SIZE * 32) - 31) {
-		uint32_t idcode;
-		char buf[12];
-
-		/* Is there another TAP? */
-		idcode = buf_get_u32(idcode_buffer, bit_count, 32);
-		if (jtag_idcode_is_final(idcode))
-			break;
-
-		/* Default everything in this TAP except IR length.
-		 *
-		 * REVISIT create a jtag_alloc(chip, tap) routine, and
-		 * share it with jim_newtap_cmd().
-		 */
-		tap = calloc(1, sizeof *tap);
-		if (!tap)
-			return ERROR_FAIL;
-
-		sprintf(buf, "auto%d", tapcount++);
-		tap->chip = strdup(buf);
-		tap->tapname = strdup("tap");
-
-		sprintf(buf, "%s.%s", tap->chip, tap->tapname);
-		tap->dotted_name = strdup(buf);
-
-		/* tap->ir_length == 0 ... signifying irlen autoprobe */
-		tap->ir_capture_mask = 0x03;
-		tap->ir_capture_value = 0x01;
-
-		tap->enabled = true;
-
-		if ((idcode & 1) == 0) {
-			bit_count += 1;
-			tap->hasidcode = false;
-		} else {
-			bit_count += 32;
-			tap->hasidcode = true;
-			tap->idcode = idcode;
-
-			tap->expected_ids_cnt = 1;
-			tap->expected_ids = malloc(sizeof(uint32_t));
-			tap->expected_ids[0] = idcode;
-		}
-
-		LOG_WARNING("AUTO %s - use \"jtag newtap "
-			"%s %s -expected-id 0x%8.8" PRIx32 " ...\"",
-			tap->dotted_name, tap->chip, tap->tapname,
-			tap->idcode);
-
-		jtag_tap_init(tap);
+		tap = jtag_tap_next_enabled(tap);
 	}
 
 	/* After those IDCODE or BYPASS register values should be
 	 * only the data we fed into the scan chain.
 	 */
-	if (jtag_examine_chain_end(idcode_buffer, bit_count,
-		    8 * sizeof(idcode_buffer))) {
-		LOG_ERROR("double-check your JTAG setup (interface, "
-			"speed, missing TAPs, ...)");
-		return ERROR_JTAG_INIT_FAILED;
+	if (jtag_examine_chain_end(idcode_buffer, bit_count, max_taps * 32)) {
+		LOG_ERROR("double-check your JTAG setup (interface, speed, ...)");
+		retval = ERROR_JTAG_INIT_FAILED;
+		goto out;
 	}
 
 	/* Return success or, for backwards compatibility if only
 	 * some IDCODE values mismatched, a soft/continuable fault.
 	 */
+out:
+	free(idcode_buffer);
 	return retval;
 }
 
@@ -1220,7 +1202,7 @@ static int jtag_validate_ircapture(void)
 		 * least two bits.  Guessing will fail if (a) any TAP does
 		 * not conform to the JTAG spec; or (b) when the upper bits
 		 * captured from some conforming TAP are nonzero.  Or if
-		 * (c) an IR length is longer than 32 bits -- which is only
+		 * (c) an IR length is longer than JTAG_IRLEN_MAX bits,
 		 * an implementation limit, which could someday be raised.
 		 *
 		 * REVISIT optimization:  if there's a *single* TAP we can
@@ -1235,11 +1217,12 @@ static int jtag_validate_ircapture(void)
 		if (tap->ir_length == 0) {
 			tap->ir_length = 2;
 			while ((val = buf_get_u64(ir_test, chain_pos, tap->ir_length + 1)) == 1
-					&& tap->ir_length <= 64) {
+					&& tap->ir_length < JTAG_IRLEN_MAX) {
 				tap->ir_length++;
 			}
-			LOG_WARNING("AUTO %s - use \"... -irlen %d\"",
-				jtag_tap_name(tap), tap->ir_length);
+			LOG_WARNING("AUTO %s - use \"jtag newtap " "%s %s -irlen %d "
+					"-expected-id 0x%08" PRIx32 "\"",
+					tap->dotted_name, tap->chip, tap->tapname, tap->ir_length, tap->idcode);
 		}
 
 		/* Validate the two LSBs, which must be 01 per JTAG spec.
@@ -1323,9 +1306,10 @@ void jtag_tap_free(struct jtag_tap *tap)
 {
 	jtag_unregister_event_callback(&jtag_reset_callback, tap);
 
-	/** @todo is anything missing? no memory leaks please */
 	free(tap->expected);
+	free(tap->expected_mask);
 	free(tap->expected_ids);
+	free(tap->cur_instr);
 	free(tap->chip);
 	free(tap->tapname);
 	free(tap->dotted_name);
@@ -1544,11 +1528,12 @@ int jtag_init_reset(struct command_context *cmd_ctx)
 	 * REVISIT once Tcl code can read the reset_config modes, this won't
 	 * need to be a C routine at all...
 	 */
-	jtag_add_reset(1, 0);	/* TAP_RESET, using TMS+TCK or TRST */
 	if (jtag_reset_config & RESET_HAS_SRST) {
 		jtag_add_reset(1, 1);
 		if ((jtag_reset_config & RESET_SRST_PULLS_TRST) == 0)
 			jtag_add_reset(0, 1);
+	} else {
+		jtag_add_reset(1, 0);	/* TAP_RESET, using TMS+TCK or TRST */
 	}
 
 	/* some targets enable us to connect with srst asserted */
@@ -1822,8 +1807,6 @@ void adapter_assert_reset(void)
 			jtag_add_reset(0, 1);
 	} else if (transport_is_swd())
 		swd_add_reset(1);
-	else if (transport_is_cmsis_dap())
-		swd_add_reset(1);  /* FIXME */
 	else if (get_current_transport() != NULL)
 		LOG_ERROR("reset is not supported on %s",
 			get_current_transport()->name);
@@ -1837,11 +1820,31 @@ void adapter_deassert_reset(void)
 		jtag_add_reset(0, 0);
 	else if (transport_is_swd())
 		swd_add_reset(0);
-	else if (transport_is_cmsis_dap())
-		swd_add_reset(0);  /* FIXME */
 	else if (get_current_transport() != NULL)
 		LOG_ERROR("reset is not supported on %s",
 			get_current_transport()->name);
 	else
 		LOG_ERROR("transport is not selected");
 }
+
+int adapter_config_trace(bool enabled, enum tpio_pin_protocol pin_protocol,
+			 uint32_t port_size, unsigned int *trace_freq)
+{
+	if (jtag->config_trace)
+		return jtag->config_trace(enabled, pin_protocol, port_size,
+					  trace_freq);
+	else if (enabled) {
+		LOG_ERROR("The selected interface does not support tracing");
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
+int adapter_poll_trace(uint8_t *buf, size_t *size)
+{
+	if (jtag->poll_trace)
+		return jtag->poll_trace(buf, size);
+
+	return ERROR_FAIL;
+}

src/jtag/drivers/amt_jtagaccel.c

@@ -124,7 +124,7 @@ static const int data_mode = IEEE1284_MODE_EPP | IEEE1284_DATA;
  * 4: Shift-IR
  * 5: Pause-IR
  */
-static uint8_t amt_jtagaccel_tap_move[6][6][2] = {
+static const uint8_t amt_jtagaccel_tap_move[6][6][2] = {
 	/*	   RESET         IDLE        DRSHIFT       DRPAUSE       IRSHIFT       IRPAUSE             */
 	{ {0x1f, 0x00}, {0x0f, 0x00}, {0x05, 0x00}, {0x0a, 0x00}, {0x06, 0x00}, {0x96, 0x00} },	/* RESET */
 	{ {0x1f, 0x00}, {0x00, 0x00}, {0x04, 0x00}, {0x05, 0x00}, {0x06, 0x00}, {0x0b, 0x00} },	/* IDLE */

src/jtag/drivers/at91rm9200.c

@@ -79,7 +79,7 @@
 #define P31	(1 << 31)
 
 struct device_t {
-	char *name;
+	const char *name;
 	int TDO_PIO;	/* PIO holding TDO */
 	uint32_t TDO_MASK;	/* TDO bitmask */
 	int TRST_PIO;	/* PIO holding TRST */
@@ -94,7 +94,7 @@ struct device_t {
 	uint32_t SRST_MASK;	/* SRST bitmask */
 };
 
-static struct device_t devices[] = {
+static const struct device_t devices[] = {
 	{ "rea_ecr", PIOD, P27, PIOA, NC, PIOD, P23, PIOD, P24, PIOD, P26, PIOC, P5 },
 	{ .name = NULL },
 };
@@ -104,7 +104,7 @@ static char *at91rm9200_device;
 
 /* interface variables
  */
-static struct device_t *device;
+static const struct device_t *device;
 static int dev_mem_fd;
 static void *sys_controller;
 static uint32_t *pio_base;
@@ -196,7 +196,7 @@ struct jtag_interface at91rm9200_interface = {
 
 static int at91rm9200_init(void)
 {
-	struct device_t *cur_device;
+	const struct device_t *cur_device;
 
 	cur_device = devices;
 

src/jtag/drivers/bitbang.c

@@ -21,6 +21,9 @@
  *   51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.           *
  ***************************************************************************/
 
+/* 2014-12: Addition of the SWD protocol support is based on the initial work
+ * by Paul Fertser and modifications by Jean-Christian de Rivaz. */
+
 #ifdef HAVE_CONFIG_H
 #include "config.h"
 #endif
@@ -29,6 +32,9 @@
 #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.
@@ -337,3 +343,209 @@ int bitbang_execute_queue(void)
 
 	return retval;
 }
+
+
+bool swd_mode;
+static int queued_retval;
+
+static int bitbang_swd_init(void)
+{
+	LOG_DEBUG("bitbang_swd_init");
+	swd_mode = true;
+	return ERROR_OK;
+}
+
+static void bitbang_exchange(bool rnw, uint8_t buf[], unsigned int offset, unsigned int bit_cnt)
+{
+	LOG_DEBUG("bitbang_exchange");
+	int tdi;
+
+	for (unsigned int i = offset; i < bit_cnt + offset; i++) {
+		int bytec = i/8;
+		int bcval = 1 << (i % 8);
+		tdi = !rnw && (buf[bytec] & bcval);
+
+		bitbang_interface->write(0, 0, tdi);
+
+		if (rnw && buf) {
+			if (bitbang_interface->swdio_read())
+				buf[bytec] |= bcval;
+			else
+				buf[bytec] &= ~bcval;
+		}
+
+		bitbang_interface->write(1, 0, tdi);
+	}
+}
+
+int bitbang_swd_switch_seq(struct adiv5_dap *dap, enum swd_special_seq seq)
+{
+	LOG_DEBUG("bitbang_swd_switch_seq");
+
+	switch (seq) {
+	case LINE_RESET:
+		LOG_DEBUG("SWD line reset");
+		bitbang_exchange(false, (uint8_t *)swd_seq_line_reset, 0, swd_seq_line_reset_len);
+		break;
+	case JTAG_TO_SWD:
+		LOG_DEBUG("JTAG-to-SWD");
+		bitbang_exchange(false, (uint8_t *)swd_seq_jtag_to_swd, 0, swd_seq_jtag_to_swd_len);
+		break;
+	case SWD_TO_JTAG:
+		LOG_DEBUG("SWD-to-JTAG");
+		bitbang_exchange(false, (uint8_t *)swd_seq_swd_to_jtag, 0, swd_seq_swd_to_jtag_len);
+		break;
+	default:
+		LOG_ERROR("Sequence %d not supported", seq);
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
+void bitbang_switch_to_swd(void)
+{
+	LOG_DEBUG("bitbang_switch_to_swd");
+	bitbang_exchange(false, (uint8_t *)swd_seq_jtag_to_swd, 0, swd_seq_jtag_to_swd_len);
+}
+
+static void swd_clear_sticky_errors(struct adiv5_dap *dap)
+{
+	const struct swd_driver *swd = jtag_interface->swd;
+	assert(swd);
+
+	swd->write_reg(dap, swd_cmd(false,  false, DP_ABORT),
+		STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR);
+}
+
+static void bitbang_swd_read_reg(struct adiv5_dap *dap, uint8_t cmd, uint32_t *value)
+{
+	LOG_DEBUG("bitbang_swd_read_reg");
+	assert(cmd & SWD_CMD_RnW);
+
+	if (queued_retval != ERROR_OK) {
+		LOG_DEBUG("Skip bitbang_swd_read_reg because queued_retval=%d", queued_retval);
+		return;
+	}
+
+	for (;;) {
+		uint8_t trn_ack_data_parity_trn[DIV_ROUND_UP(4 + 3 + 32 + 1 + 4, 8)];
+
+		cmd |= SWD_CMD_START | (1 << 7);
+		bitbang_exchange(false, &cmd, 0, 8);
+
+		bitbang_interface->swdio_drive(false);
+		bitbang_exchange(true, trn_ack_data_parity_trn, 0, 1 + 3 + 32 + 1 + 1);
+		bitbang_interface->swdio_drive(true);
+
+		int ack = buf_get_u32(trn_ack_data_parity_trn, 1, 3);
+		uint32_t data = buf_get_u32(trn_ack_data_parity_trn, 1 + 3, 32);
+		int parity = buf_get_u32(trn_ack_data_parity_trn, 1 + 3 + 32, 1);
+
+		LOG_DEBUG("%s %s %s reg %X = %08"PRIx32,
+			  ack == SWD_ACK_OK ? "OK" : ack == SWD_ACK_WAIT ? "WAIT" : ack == SWD_ACK_FAULT ? "FAULT" : "JUNK",
+			  cmd & SWD_CMD_APnDP ? "AP" : "DP",
+			  cmd & SWD_CMD_RnW ? "read" : "write",
+			  (cmd & SWD_CMD_A32) >> 1,
+			  data);
+
+		switch (ack) {
+		 case SWD_ACK_OK:
+			if (parity != parity_u32(data)) {
+				LOG_DEBUG("Wrong parity detected");
+				queued_retval = ERROR_FAIL;
+				return;
+			}
+			if (value)
+				*value = data;
+			if (cmd & SWD_CMD_APnDP)
+				bitbang_exchange(true, NULL, 0, dap->memaccess_tck);
+			return;
+		 case SWD_ACK_WAIT:
+			LOG_DEBUG("SWD_ACK_WAIT");
+			swd_clear_sticky_errors(dap);
+			break;
+		 case SWD_ACK_FAULT:
+			LOG_DEBUG("SWD_ACK_FAULT");
+			queued_retval = ack;
+			return;
+		 default:
+			LOG_DEBUG("No valid acknowledge: ack=%d", ack);
+			queued_retval = ack;
+			return;
+		}
+	}
+}
+
+static void bitbang_swd_write_reg(struct adiv5_dap *dap, uint8_t cmd, uint32_t value)
+{
+	LOG_DEBUG("bitbang_swd_write_reg");
+	assert(!(cmd & SWD_CMD_RnW));
+
+	if (queued_retval != ERROR_OK) {
+		LOG_DEBUG("Skip bitbang_swd_write_reg because queued_retval=%d", queued_retval);
+		return;
+	}
+
+	for (;;) {
+		uint8_t trn_ack_data_parity_trn[DIV_ROUND_UP(4 + 3 + 32 + 1 + 4, 8)];
+		buf_set_u32(trn_ack_data_parity_trn, 1 + 3 + 1, 32, value);
+		buf_set_u32(trn_ack_data_parity_trn, 1 + 3 + 1 + 32, 1, parity_u32(value));
+
+		cmd |= SWD_CMD_START | (1 << 7);
+		bitbang_exchange(false, &cmd, 0, 8);
+
+		bitbang_interface->swdio_drive(false);
+		bitbang_exchange(true, trn_ack_data_parity_trn, 0, 1 + 3 + 1);
+		bitbang_interface->swdio_drive(true);
+		bitbang_exchange(false, trn_ack_data_parity_trn, 1 + 3 + 1, 32 + 1);
+
+		int ack = buf_get_u32(trn_ack_data_parity_trn, 1, 3);
+		LOG_DEBUG("%s %s %s reg %X = %08"PRIx32,
+			  ack == SWD_ACK_OK ? "OK" : ack == SWD_ACK_WAIT ? "WAIT" : ack == SWD_ACK_FAULT ? "FAULT" : "JUNK",
+			  cmd & SWD_CMD_APnDP ? "AP" : "DP",
+			  cmd & SWD_CMD_RnW ? "read" : "write",
+			  (cmd & SWD_CMD_A32) >> 1,
+			  buf_get_u32(trn_ack_data_parity_trn, 1 + 3 + 1, 32));
+
+		switch (ack) {
+		 case SWD_ACK_OK:
+			if (cmd & SWD_CMD_APnDP)
+				bitbang_exchange(true, NULL, 0, dap->memaccess_tck);
+			return;
+		 case SWD_ACK_WAIT:
+			LOG_DEBUG("SWD_ACK_WAIT");
+			swd_clear_sticky_errors(dap);
+			break;
+		 case SWD_ACK_FAULT:
+			LOG_DEBUG("SWD_ACK_FAULT");
+			queued_retval = ack;
+			return;
+		 default:
+			LOG_DEBUG("No valid acknowledge: ack=%d", ack);
+			queued_retval = ack;
+			return;
+		}
+	}
+}
+
+static int bitbang_swd_run_queue(struct adiv5_dap *dap)
+{
+	LOG_DEBUG("bitbang_swd_run_queue");
+	/* A transaction must be followed by another transaction or at least 8 idle cycles to
+	 * ensure that data is clocked through the AP. */
+	bitbang_exchange(true, NULL, 0, 8);
+
+	int retval = queued_retval;
+	queued_retval = ERROR_OK;
+	LOG_DEBUG("SWD queue return value: %02x", retval);
+	return retval;
+}
+
+const struct swd_driver bitbang_swd = {
+	.init = bitbang_swd_init,
+	.switch_seq = bitbang_swd_switch_seq,
+	.read_reg = bitbang_swd_read_reg,
+	.write_reg = bitbang_swd_write_reg,
+	.run = bitbang_swd_run_queue,
+};

src/jtag/drivers/bitbang.h

@@ -24,6 +24,8 @@
 #ifndef BITBANG_H
 #define BITBANG_H
 
+#include <jtag/swd.h>
+
 struct bitbang_interface {
 	/* low level callbacks (for bitbang)
 	 */
@@ -31,10 +33,18 @@ struct bitbang_interface {
 	void (*write)(int tck, int tms, int tdi);
 	void (*reset)(int trst, int srst);
 	void (*blink)(int on);
+	int (*swdio_read)(void);
+	void (*swdio_drive)(bool on);
 };
 
+const struct swd_driver bitbang_swd;
+
+extern bool swd_mode;
+
 int bitbang_execute_queue(void);
 
 extern struct bitbang_interface *bitbang_interface;
+void bitbang_switch_to_swd(void);
+int bitbang_swd_switch_seq(struct adiv5_dap *dap, enum swd_special_seq seq);
 
 #endif /* BITBANG_H */

src/jtag/drivers/buspirate.c

@@ -42,6 +42,7 @@ static void buspirate_path_move(int num_states, tap_state_t *path);
 static void buspirate_runtest(int num_cycles);
 static void buspirate_scan(bool ir_scan, enum scan_type type,
 	uint8_t *buffer, int scan_size, struct scan_command *command);
+static void buspirate_stableclocks(int num_cycles);
 
 #define CMD_UNKNOWN       0x00
 #define CMD_PORT_MODE     0x01
@@ -192,6 +193,10 @@ static int buspirate_execute_queue(void)
 			buspirate_tap_execute();
 			jtag_sleep(cmd->cmd.sleep->us);
 				break;
+		case JTAG_STABLECLOCKS:
+			DEBUG_JTAG_IO("stable clock %i cycles", cmd->cmd.stableclocks->num_cycles);
+			buspirate_stableclocks(cmd->cmd.stableclocks->num_cycles);
+				break;
 		default:
 			LOG_ERROR("BUG: unknown JTAG command type encountered");
 			exit(-1);
@@ -602,6 +607,16 @@ static void buspirate_scan(bool ir_scan, enum scan_type type,
 		buspirate_state_move();
 }
 
+static void buspirate_stableclocks(int num_cycles)
+{
+	int i;
+	int tms = (tap_get_state() == TAP_RESET ? 1 : 0);
+
+	buspirate_tap_make_space(0, num_cycles);
+
+	for (i = 0; i < num_cycles; i++)
+		buspirate_tap_append(tms, 0);
+}
 
 /************************* TAP related stuff **********/
 

src/jtag/drivers/cmsis_dap_usb.c

@@ -1,4 +1,7 @@
 /***************************************************************************
+ *   Copyright (C) 2014 by Paul Fertser                                    *
+ *   fercerpav@gmail.com                                                   *
+ *                                                                         *
  *   Copyright (C) 2013 by mike brown                                      *
  *   mike@theshedworks.org.uk                                              *
  *                                                                         *
@@ -33,12 +36,6 @@
 
 #include <hidapi.h>
 
-#ifdef _DEBUG_JTAG_IO_
- #define DEBUG_IO(expr...) LOG_DEBUG(expr)
-#else
- #define DEBUG_IO(expr...) do {} while (0)
-#endif
-
 /*
  * See CMSIS-DAP documentation:
  * Version 0.01 - Beta.
@@ -60,6 +57,8 @@
 /* vid = pid = 0 marks the end of the list */
 static uint16_t cmsis_dap_vid[MAX_USB_IDS + 1] = { 0 };
 static uint16_t cmsis_dap_pid[MAX_USB_IDS + 1] = { 0 };
+static wchar_t *cmsis_dap_serial;
+static bool swd_mode;
 
 #define PACKET_SIZE       (64 + 1)	/* 64 bytes plus report id */
 #define USB_TIMEOUT       1000
@@ -136,7 +135,7 @@ static uint16_t cmsis_dap_pid[MAX_USB_IDS + 1] = { 0 };
 /* CMSIS-DAP Vendor Commands
  * None as yet... */
 
-static char *info_caps_str[] = {
+static const char * const info_caps_str[] = {
 	"SWD  Supported",
 	"JTAG Supported"
 };
@@ -153,6 +152,17 @@ struct cmsis_dap {
 	uint8_t mode;
 };
 
+struct pending_transfer_result {
+	uint8_t cmd;
+	uint32_t data;
+	void *buffer;
+};
+
+static int pending_transfer_count, pending_queue_len;
+static struct pending_transfer_result *pending_transfers;
+
+static int queued_retval;
+
 static struct cmsis_dap *cmsis_dap_handle;
 
 static int cmsis_dap_usb_open(void)
@@ -161,15 +171,19 @@ static int cmsis_dap_usb_open(void)
 	int i;
 	struct hid_device_info *devs, *cur_dev;
 	unsigned short target_vid, target_pid;
+	wchar_t *target_serial = NULL;
+
+	bool found = false;
+	bool serial_found = false;
 
 	target_vid = 0;
 	target_pid = 0;
 
 	/*
-	The CMSIS-DAP specification stipulates:
-	"The Product String must contain "CMSIS-DAP" somewhere in the string. This is used by the
-	debuggers to idenify a CMSIS-DAP compliant Debug Unit that is connected to a host computer."
-	*/
+	 * The CMSIS-DAP specification stipulates:
+	 * "The Product String must contain "CMSIS-DAP" somewhere in the string. This is used by the
+	 * debuggers to identify a CMSIS-DAP compliant Debug Unit that is connected to a host computer."
+	 */
 	devs = hid_enumerate(0x0, 0x0);
 	cur_dev = devs;
 	while (NULL != cur_dev) {
@@ -178,21 +192,34 @@ static int cmsis_dap_usb_open(void)
 				LOG_DEBUG("Cannot read product string of device 0x%x:0x%x",
 					  cur_dev->vendor_id, cur_dev->product_id);
 			} else {
-				if (wcsstr(cur_dev->product_string, L"CMSIS-DAP"))
-					/*
-					if the user hasn't specified VID:PID *and*
-					product string contains "CMSIS-DAP", pick it
-					*/
-					break;
+				if (wcsstr(cur_dev->product_string, L"CMSIS-DAP")) {
+					/* if the user hasn't specified VID:PID *and*
+					 * product string contains "CMSIS-DAP", pick it
+					 */
+					found = true;
+				}
 			}
 		} else {
-			/*
-			otherwise, exhaustively compare against all VID:PID in list
-			*/
+			/* otherwise, exhaustively compare against all VID:PID in list */
 			for (i = 0; cmsis_dap_vid[i] || cmsis_dap_pid[i]; i++) {
 				if ((cmsis_dap_vid[i] == cur_dev->vendor_id) && (cmsis_dap_pid[i] == cur_dev->product_id))
-					break;
+					found = true;
 			}
+
+			if (cmsis_dap_vid[i] || cmsis_dap_pid[i])
+				found = true;
+		}
+
+		if (found) {
+			/* we have found an adapter, so exit further checks */
+			/* check serial number matches if given */
+			if (cmsis_dap_serial != NULL) {
+				if (wcscmp(cmsis_dap_serial, cur_dev->serial_number) == 0) {
+					serial_found = true;
+					break;
+				}
+			} else
+				break;
 		}
 
 		cur_dev = cur_dev->next;
@@ -201,19 +228,26 @@ static int cmsis_dap_usb_open(void)
 	if (NULL != cur_dev) {
 		target_vid = cur_dev->vendor_id;
 		target_pid = cur_dev->product_id;
+		if (serial_found)
+			target_serial = cmsis_dap_serial;
 	}
 
 	hid_free_enumeration(devs);
 
+	if (target_vid == 0 && target_pid == 0) {
+		LOG_ERROR("unable to find CMSIS-DAP device");
+		return ERROR_FAIL;
+	}
+
 	if (hid_init() != 0) {
 		LOG_ERROR("unable to open HIDAPI");
 		return ERROR_FAIL;
 	}
 
-	dev = hid_open(target_vid, target_pid, NULL);
+	dev = hid_open(target_vid, target_pid, target_serial);
 
 	if (dev == NULL) {
-		LOG_ERROR("unable to open CMSIS-DAP device");
+		LOG_ERROR("unable to open CMSIS-DAP device 0x%x:0x%x", target_vid, target_pid);
 		return ERROR_FAIL;
 	}
 
@@ -237,6 +271,8 @@ static int cmsis_dap_usb_open(void)
 	int packet_size = PACKET_SIZE;
 
 	/* atmel cmsis-dap uses 512 byte reports */
+	/* TODO: HID report descriptor should be parsed instead of
+	 * hardcoding a match by VID */
 	if (target_vid == 0x03eb)
 		packet_size = 512 + 1;
 
@@ -256,13 +292,13 @@ static void cmsis_dap_usb_close(struct cmsis_dap *dap)
 	hid_close(dap->dev_handle);
 	hid_exit();
 
-	if (cmsis_dap_handle->packet_buffer)
-		free(cmsis_dap_handle->packet_buffer);
-
-	if (cmsis_dap_handle) {
-		free(cmsis_dap_handle);
-		cmsis_dap_handle = NULL;
-	}
+	free(cmsis_dap_handle->packet_buffer);
+	free(cmsis_dap_handle);
+	cmsis_dap_handle = NULL;
+	free(cmsis_dap_serial);
+	cmsis_dap_serial = NULL;
+	free(pending_transfers);
+	pending_transfers = NULL;
 
 	return;
 }
@@ -271,7 +307,7 @@ static void cmsis_dap_usb_close(struct cmsis_dap *dap)
 static int cmsis_dap_usb_xfer(struct cmsis_dap *dap, int txlen)
 {
 	/* Pad the rest of the TX buffer with 0's */
-	memset(dap->packet_buffer + txlen, 0, dap->packet_size - 1 - txlen);
+	memset(dap->packet_buffer + txlen, 0, dap->packet_size - txlen);
 
 	/* write data to device */
 	int retval = hid_write(dap->dev_handle, dap->packet_buffer, dap->packet_size);
@@ -418,7 +454,7 @@ static int cmsis_dap_cmd_DAP_Disconnect(void)
 	return ERROR_OK;
 }
 
-static int cmsis_dap_cmd_DAP_TFER_Configure(uint8_t idle, uint16_t delay, uint16_t retry)
+static int cmsis_dap_cmd_DAP_TFER_Configure(uint8_t idle, uint16_t retry_count, uint16_t match_retry)
 {
 	int retval;
 	uint8_t *buffer = cmsis_dap_handle->packet_buffer;
@@ -426,10 +462,10 @@ static int cmsis_dap_cmd_DAP_TFER_Configure(uint8_t idle, uint16_t delay, uint16
 	buffer[0] = 0;	/* report number */
 	buffer[1] = CMD_DAP_TFER_CONFIGURE;
 	buffer[2] = idle;
-	buffer[3] = delay & 0xff;
-	buffer[4] = (delay >> 8) & 0xff;
-	buffer[5] = retry & 0xff;
-	buffer[6] = (retry >> 8) & 0xff;
+	buffer[3] = retry_count & 0xff;
+	buffer[4] = (retry_count >> 8) & 0xff;
+	buffer[5] = match_retry & 0xff;
+	buffer[6] = (match_retry >> 8) & 0xff;
 	retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 7);
 
 	if (retval != ERROR_OK || buffer[1] != DAP_OK) {
@@ -479,101 +515,141 @@ static int cmsis_dap_cmd_DAP_Delay(uint16_t delay_us)
 }
 #endif
 
-static int cmsis_dap_swd_read_reg(uint8_t cmd, uint32_t *value)
-{
-	uint8_t *buffer = cmsis_dap_handle->packet_buffer;
-	int retval;
-	uint32_t val;
-
-	DEBUG_IO("CMSIS-DAP: Read  Reg 0x%02" PRIx8, cmd);
-
-	buffer[0] = 0;	/* report number */
-	buffer[1] = CMD_DAP_TFER;
-	buffer[2] = 0x00;
-	buffer[3] = 0x01;
-	buffer[4] = cmd;
-	retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 5);
-
-	/* TODO - need better response checking */
-	if (retval != ERROR_OK || buffer[1] != 0x01) {
-		LOG_ERROR("CMSIS-DAP: Read Error (0x%02" PRIx8 ")", buffer[2]);
-		return buffer[2];
-	}
-
-	val = le_to_h_u32(&buffer[3]);
-	DEBUG_IO("0x%08" PRIx32, val);
-
-	if (value)
-		*value = val;
-
-	return retval;
-}
-
-static int cmsis_dap_swd_write_reg(uint8_t cmd, uint32_t value)
+static int cmsis_dap_swd_run_queue(struct adiv5_dap *dap)
 {
 	uint8_t *buffer = cmsis_dap_handle->packet_buffer;
 
-	DEBUG_IO("CMSIS-DAP: Write Reg 0x%02" PRIx8 " 0x%08" PRIx32, cmd, value);
+	LOG_DEBUG("Executing %d queued transactions", pending_transfer_count);
 
-	buffer[0] = 0;	/* report number */
-	buffer[1] = CMD_DAP_TFER;
-	buffer[2] = 0x00;
-	buffer[3] = 0x01;
-	buffer[4] = cmd;
-	buffer[5] = (value) & 0xff;
-	buffer[6] = (value >> 8) & 0xff;
-	buffer[7] = (value >> 16) & 0xff;
-	buffer[8] = (value >> 24) & 0xff;
-	int retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 9);
-
-	if (buffer[1] != 0x01) {
-		LOG_ERROR("CMSIS-DAP: Write Error (0x%02" PRIx8 ")", buffer[2]);
-		retval = buffer[2];
+	if (queued_retval != ERROR_OK) {
+		LOG_DEBUG("Skipping due to previous errors: %d", queued_retval);
+		goto skip;
 	}
 
-	return retval;
-}
+	if (!pending_transfer_count)
+		goto skip;
 
-static int cmsis_dap_swd_read_block(uint8_t cmd, uint32_t blocksize, uint8_t *dest_buf)
-{
-	uint8_t *buffer;
-	int tfer_sz;
-	int retval = ERROR_OK;
-	uint16_t read_count;
+	size_t idx = 0;
+	buffer[idx++] = 0;	/* report number */
+	buffer[idx++] = CMD_DAP_TFER;
+	buffer[idx++] = 0x00;	/* DAP Index */
+	buffer[idx++] = pending_transfer_count;
 
-	DEBUG_IO("CMSIS-DAP: Read Block 0x%02" PRIx8 " %" PRIu32, cmd, blocksize);
+	for (int i = 0; i < pending_transfer_count; i++) {
+		uint8_t cmd = pending_transfers[i].cmd;
+		uint32_t data = pending_transfers[i].data;
 
-	while (blocksize) {
+		LOG_DEBUG("%s %s reg %x %"PRIx32,
+				cmd & SWD_CMD_APnDP ? "AP" : "DP",
+				cmd & SWD_CMD_RnW ? "read" : "write",
+			  (cmd & SWD_CMD_A32) >> 1, data);
 
-		buffer = cmsis_dap_handle->packet_buffer;
-		tfer_sz = blocksize;
-		if (tfer_sz > 15)
-			tfer_sz = 8;
-
-		buffer[0] = 0;	/* report number */
-		buffer[1] = CMD_DAP_TFER_BLOCK;
-		buffer[2] = 0x00;
-		buffer[3] = tfer_sz;
-		buffer[4] = 0x00;
-		buffer[5] = cmd;
-		retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 6);
-
-		read_count = le_to_h_u16(&buffer[1]);
-		if (read_count != tfer_sz) {
-			LOG_ERROR("CMSIS-DAP: Block Read Error (0x%02" PRIx8 ")", buffer[3]);
-			retval = buffer[3];
+		/* When proper WAIT handling is implemented in the
+		 * common SWD framework, this kludge can be
+		 * removed. However, this might lead to minor
+		 * performance degradation as the adapter wouldn't be
+		 * able to automatically retry anything (because ARM
+		 * has forgotten to implement sticky error flags
+		 * clearing). See also comments regarding
+		 * cmsis_dap_cmd_DAP_TFER_Configure() and
+		 * cmsis_dap_cmd_DAP_SWD_Configure() in
+		 * cmsis_dap_init().
+		 */
+		if (!(cmd & SWD_CMD_RnW) &&
+		    !(cmd & SWD_CMD_APnDP) &&
+		    (cmd & SWD_CMD_A32) >> 1 == DP_CTRL_STAT &&
+		    (data & CORUNDETECT)) {
+			LOG_DEBUG("refusing to enable sticky overrun detection");
+			data &= ~CORUNDETECT;
 		}
 
-		read_count *= 4;
-		memcpy(dest_buf, &buffer[4], read_count);
-
-		dest_buf += read_count;
-		blocksize -= tfer_sz;
+		buffer[idx++] = (cmd >> 1) & 0x0f;
+		if (!(cmd & SWD_CMD_RnW)) {
+			buffer[idx++] = (data) & 0xff;
+			buffer[idx++] = (data >> 8) & 0xff;
+			buffer[idx++] = (data >> 16) & 0xff;
+			buffer[idx++] = (data >> 24) & 0xff;
+		}
 	}
 
+	queued_retval = cmsis_dap_usb_xfer(cmsis_dap_handle, idx);
+	if (queued_retval != ERROR_OK)
+		goto skip;
+
+	idx = 2;
+	uint8_t ack = buffer[idx] & 0x07;
+	if (ack != SWD_ACK_OK || (buffer[idx] & 0x08)) {
+		LOG_DEBUG("SWD ack not OK: %d %s", buffer[idx-1],
+			  ack == SWD_ACK_WAIT ? "WAIT" : ack == SWD_ACK_FAULT ? "FAULT" : "JUNK");
+		queued_retval = ack == SWD_ACK_WAIT ? ERROR_WAIT : ERROR_FAIL;
+		goto skip;
+	}
+	idx++;
+
+	if (pending_transfer_count != buffer[1])
+		LOG_ERROR("CMSIS-DAP transfer count mismatch: expected %d, got %d",
+			  pending_transfer_count, buffer[1]);
+
+	for (int i = 0; i < buffer[1]; i++) {
+		if (pending_transfers[i].cmd & SWD_CMD_RnW) {
+			static uint32_t last_read;
+			uint32_t data = le_to_h_u32(&buffer[idx]);
+			uint32_t tmp = data;
+			idx += 4;
+
+			LOG_DEBUG("Read result: %"PRIx32, data);
+
+			/* Imitate posted AP reads */
+			if ((pending_transfers[i].cmd & SWD_CMD_APnDP) ||
+			    ((pending_transfers[i].cmd & SWD_CMD_A32) >> 1 == DP_RDBUFF)) {
+				tmp = last_read;
+				last_read = data;
+			}
+
+			if (pending_transfers[i].buffer)
+				*(uint32_t *)pending_transfers[i].buffer = tmp;
+		}
+	}
+
+skip:
+	pending_transfer_count = 0;
+	int retval = queued_retval;
+	queued_retval = ERROR_OK;
+
 	return retval;
 }
 
+static void cmsis_dap_swd_queue_cmd(struct adiv5_dap *dap, uint8_t cmd, uint32_t *dst, uint32_t data)
+{
+	if (pending_transfer_count == pending_queue_len) {
+		/* Not enough room in the queue. Run the queue. */
+		queued_retval = cmsis_dap_swd_run_queue(dap);
+	}
+
+	if (queued_retval != ERROR_OK)
+		return;
+
+	pending_transfers[pending_transfer_count].data = data;
+	pending_transfers[pending_transfer_count].cmd = cmd;
+	if (cmd & SWD_CMD_RnW) {
+		/* Queue a read transaction */
+		pending_transfers[pending_transfer_count].buffer = dst;
+	}
+	pending_transfer_count++;
+}
+
+static void cmsis_dap_swd_write_reg(struct adiv5_dap *dap, uint8_t cmd, uint32_t value)
+{
+	assert(!(cmd & SWD_CMD_RnW));
+	cmsis_dap_swd_queue_cmd(dap, cmd, NULL, value);
+}
+
+static void cmsis_dap_swd_read_reg(struct adiv5_dap *dap, uint8_t cmd, uint32_t *value)
+{
+	assert(cmd & SWD_CMD_RnW);
+	cmsis_dap_swd_queue_cmd(dap, cmd, value, 0);
+}
+
 static int cmsis_dap_get_version_info(void)
 {
 	uint8_t *data;
@@ -631,115 +707,82 @@ static int cmsis_dap_get_status(void)
 	return retval;
 }
 
-static int cmsis_dap_reset_link(void)
+static int cmsis_dap_swd_switch_seq(struct adiv5_dap *dap, enum swd_special_seq seq)
 {
 	uint8_t *buffer = cmsis_dap_handle->packet_buffer;
+	const uint8_t *s;
+	unsigned int s_len;
+	int retval;
 
-	LOG_DEBUG("CMSIS-DAP: cmsis_dap_reset_link");
-	LOG_INFO("DAP_SWJ Sequence (reset: 50+ '1' followed by 0)");
-
-	/* reset line with SWDIO high for >50 cycles */
-	buffer[0] = 0;	/* report number */
-	buffer[1] = CMD_DAP_SWJ_SEQ;
-	buffer[2] = 7 * 8;
-	buffer[3] = 0xff;
-	buffer[4] = 0xff;
-	buffer[5] = 0xff;
-	buffer[6] = 0xff;
-	buffer[7] = 0xff;
-	buffer[8] = 0xff;
-	buffer[9] = 0xff;
-	int retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 10);
-
-	if (retval != ERROR_OK || buffer[1] != DAP_OK)
-		return ERROR_FAIL;
-
-	/* 16bit JTAG-SWD sequence */
-	buffer[0] = 0;	/* report number */
-	buffer[1] = CMD_DAP_SWJ_SEQ;
-	buffer[2] = 2 * 8;
-	buffer[3] = 0x9e;
-	buffer[4] = 0xe7;
-	retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 5);
-
-	if (retval != ERROR_OK || buffer[1] != DAP_OK)
-		return ERROR_FAIL;
-
-	/* another reset just incase */
-	buffer[0] = 0;	/* report number */
-	buffer[1] = CMD_DAP_SWJ_SEQ;
-	buffer[2] = 7 * 8;
-	buffer[3] = 0xff;
-	buffer[4] = 0xff;
-	buffer[5] = 0xff;
-	buffer[6] = 0xff;
-	buffer[7] = 0xff;
-	buffer[8] = 0xff;
-	buffer[9] = 0xff;
-	retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 10);
-
-	if (retval != ERROR_OK || buffer[1] != DAP_OK)
-		return ERROR_FAIL;
-
-	/* 16 cycle idle period */
-	buffer[0] = 0;	/* report number */
-	buffer[1] = CMD_DAP_SWJ_SEQ;
-	buffer[2] = 2 * 8;
-	buffer[3] = 0x00;
-	buffer[4] = 0x00;
-	retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 5);
-
-	if (retval != ERROR_OK || buffer[1] != DAP_OK)
-		return ERROR_FAIL;
-
-	DEBUG_IO("DAP Read IDCODE");
-
-	/* read the id code is always the next sequence */
-	buffer[0] = 0;	/* report number */
-	buffer[1] = CMD_DAP_TFER;
-	buffer[2] = 0x00;
-	buffer[3] = 0x01;
-	buffer[4] = 0x02;
-	retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 5);
-
+	/* When we are reconnecting, DAP_Connect needs to be rerun, at
+	 * least on Keil ULINK-ME */
+	retval = cmsis_dap_cmd_DAP_Connect(seq == LINE_RESET || seq == JTAG_TO_SWD ?
+					   CONNECT_SWD : CONNECT_JTAG);
 	if (retval != ERROR_OK)
 		return retval;
 
-	if (buffer[1] == 0) {
-		LOG_DEBUG("Result 0x%02" PRIx8 " 0x%02" PRIx8, buffer[1], buffer[2]);
-
-		LOG_DEBUG("DAP Reset Target");
-		buffer[0] = 0;	/* report number */
-		buffer[1] = CMD_DAP_RESET_TARGET;
-		retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 2);
-		LOG_DEBUG("Result 0x%02" PRIx8 " 0x%02" PRIx8, buffer[1], buffer[2]);
-
-		LOG_DEBUG("DAP Write Abort");
-		buffer[0] = 0;	/* report number */
-		buffer[1] = CMD_DAP_WRITE_ABORT;
-		buffer[2] = 0x00;
-		buffer[3] = 0x1e/*0x1f*/;
-		buffer[4] = 0x00;
-		buffer[5] = 0x00;
-		buffer[6] = 0x00;
-		retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 7);
-		LOG_DEBUG("Result 0x%02" PRIx8, buffer[1]);
-
-		return 0x80 + buffer[1];
+	switch (seq) {
+	case LINE_RESET:
+		LOG_DEBUG("SWD line reset");
+		s = swd_seq_line_reset;
+		s_len = swd_seq_line_reset_len;
+		break;
+	case JTAG_TO_SWD:
+		LOG_DEBUG("JTAG-to-SWD");
+		s = swd_seq_jtag_to_swd;
+		s_len = swd_seq_jtag_to_swd_len;
+		break;
+	case SWD_TO_JTAG:
+		LOG_DEBUG("SWD-to-JTAG");
+		s = swd_seq_swd_to_jtag;
+		s_len = swd_seq_swd_to_jtag_len;
+		break;
+	default:
+		LOG_ERROR("Sequence %d not supported", seq);
+		return ERROR_FAIL;
 	}
 
-	LOG_DEBUG("DAP Write Abort");
 	buffer[0] = 0;	/* report number */
-	buffer[1] = CMD_DAP_WRITE_ABORT;
-	buffer[2] = 0x00;
-	buffer[3] = 0x1e;
-	buffer[4] = 0x00;
-	buffer[5] = 0x00;
-	buffer[6] = 0x00;
-	retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 7);
-	LOG_DEBUG("Result 0x%02" PRIx8, buffer[1]);
+	buffer[1] = CMD_DAP_SWJ_SEQ;
+	buffer[2] = s_len;
+	bit_copy(&buffer[3], 0, s, 0, s_len);
 
-	return retval;
+	retval = cmsis_dap_usb_xfer(cmsis_dap_handle, DIV_ROUND_UP(s_len, 8) + 3);
+
+	if (retval != ERROR_OK || buffer[1] != DAP_OK)
+		return ERROR_FAIL;
+
+	return ERROR_OK;
+}
+
+static int cmsis_dap_swd_open(void)
+{
+	int retval;
+
+	if (cmsis_dap_handle == NULL) {
+		/* SWD init */
+		retval = cmsis_dap_usb_open();
+		if (retval != ERROR_OK)
+			return retval;
+
+		retval = cmsis_dap_get_caps_info();
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	if (!(cmsis_dap_handle->caps & INFO_CAPS_SWD)) {
+		LOG_ERROR("CMSIS-DAP: SWD not supported");
+		return ERROR_JTAG_DEVICE_ERROR;
+	}
+
+	retval = cmsis_dap_cmd_DAP_Connect(CONNECT_SWD);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Add more setup here.??... */
+
+	LOG_INFO("CMSIS-DAP: Interface Initialised (SWD)");
+	return ERROR_OK;
 }
 
 static int cmsis_dap_init(void)
@@ -747,6 +790,12 @@ static int cmsis_dap_init(void)
 	int retval;
 	uint8_t *data;
 
+	if (swd_mode) {
+		retval = cmsis_dap_swd_open();
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
 	if (cmsis_dap_handle == NULL) {
 
 		/* JTAG init */
@@ -783,6 +832,16 @@ static int cmsis_dap_init(void)
 	if (data[0] == 2) {  /* short */
 		uint16_t pkt_sz = data[1] + (data[2] << 8);
 
+		/* 4 bytes of command header + 5 bytes per register
+		 * write. For bulk read sequences just 4 bytes are
+		 * needed per transfer, so this is suboptimal. */
+		pending_queue_len = (pkt_sz - 4) / 5;
+		pending_transfers = malloc(pending_queue_len * sizeof(*pending_transfers));
+		if (!pending_transfers) {
+			LOG_ERROR("Unable to allocate memory for CMSIS-DAP queue");
+			return ERROR_FAIL;
+		}
+
 		if (cmsis_dap_handle->packet_size != pkt_sz + 1) {
 			/* reallocate buffer */
 			cmsis_dap_handle->packet_size = pkt_sz + 1;
@@ -814,14 +873,19 @@ static int cmsis_dap_init(void)
 
 	/* Now try to connect to the target
 	 * TODO: This is all SWD only @ present */
-	retval = cmsis_dap_cmd_DAP_SWJ_Clock(100);		/* 100kHz */
+	retval = cmsis_dap_cmd_DAP_SWJ_Clock(jtag_get_speed_khz());
 	if (retval != ERROR_OK)
 		return ERROR_FAIL;
 
+	/* Ask CMSIS-DAP to automatically retry on receiving WAIT for
+	 * up to 64 times. This must be changed to 0 if sticky
+	 * overrun detection is enabled. */
 	retval = cmsis_dap_cmd_DAP_TFER_Configure(0, 64, 0);
 	if (retval != ERROR_OK)
 		return ERROR_FAIL;
-	retval = cmsis_dap_cmd_DAP_SWD_Configure(0x00);
+	/* Data Phase (bit 2) must be set to 1 if sticky overrun
+	 * detection is enabled */
+	retval = cmsis_dap_cmd_DAP_SWD_Configure(0);	/* 1 TRN, no Data Phase */
 	if (retval != ERROR_OK)
 		return ERROR_FAIL;
 
@@ -841,47 +905,16 @@ static int cmsis_dap_init(void)
 		}
 	}
 
-	retval = cmsis_dap_reset_link();
-	if (retval != ERROR_OK)
-		return ERROR_FAIL;
-
-	cmsis_dap_cmd_DAP_LED(0x00);				/* Both LEDs off */
+	cmsis_dap_cmd_DAP_LED(0x00);			/* Both LEDs off */
 
 	LOG_INFO("CMSIS-DAP: Interface ready");
 
 	return ERROR_OK;
 }
 
-static int cmsis_dap_swd_init(uint8_t trn)
+static int cmsis_dap_swd_init(void)
 {
-	int retval;
-
-	DEBUG_IO("CMSIS-DAP: cmsis_dap_swd_init");
-
-	if (cmsis_dap_handle == NULL) {
-
-		/* SWD init */
-		retval = cmsis_dap_usb_open();
-		if (retval != ERROR_OK)
-			return retval;
-
-		retval = cmsis_dap_get_caps_info();
-		if (retval != ERROR_OK)
-			return retval;
-	}
-
-	if (!(cmsis_dap_handle->caps & INFO_CAPS_SWD)) {
-		LOG_ERROR("CMSIS-DAP: SWD not supported");
-		return ERROR_JTAG_DEVICE_ERROR;
-	}
-
-	retval = cmsis_dap_cmd_DAP_Connect(CONNECT_SWD);
-	if (retval != ERROR_OK)
-		return retval;
-
-	/* Add more setup here.??... */
-
-	LOG_INFO("CMSIS-DAP: Interface Initialised (SWD)");
+	swd_mode = true;
 	return ERROR_OK;
 }
 
@@ -966,6 +999,14 @@ static int cmsis_dap_khz(int khz, int *jtag_speed)
 	return ERROR_OK;
 }
 
+static int_least32_t cmsis_dap_swd_frequency(struct adiv5_dap *dap, int_least32_t hz)
+{
+	if (hz > 0)
+		cmsis_dap_speed(hz / 1000);
+
+	return hz;
+}
+
 COMMAND_HANDLER(cmsis_dap_handle_info_command)
 {
 	if (cmsis_dap_get_version_info() == ERROR_OK)
@@ -1004,6 +1045,27 @@ COMMAND_HANDLER(cmsis_dap_handle_vid_pid_command)
 	return ERROR_OK;
 }
 
+COMMAND_HANDLER(cmsis_dap_handle_serial_command)
+{
+	if (CMD_ARGC == 1) {
+		size_t len = mbstowcs(NULL, CMD_ARGV[0], 0);
+		cmsis_dap_serial = calloc(len + 1, sizeof(wchar_t));
+		if (cmsis_dap_serial == NULL) {
+			LOG_ERROR("unable to allocate memory");
+			return ERROR_OK;
+		}
+		if (mbstowcs(cmsis_dap_serial, CMD_ARGV[0], len + 1) == (size_t)-1) {
+			free(cmsis_dap_serial);
+			cmsis_dap_serial = NULL;
+			LOG_ERROR("unable to convert serial");
+		}
+	} else {
+		LOG_ERROR("expected exactly one argument to cmsis_dap_serial <serial-number>");
+	}
+
+	return ERROR_OK;
+}
+
 static const struct command_registration cmsis_dap_subcommand_handlers[] = {
 	{
 		.name = "info",
@@ -1015,73 +1077,6 @@ static const struct command_registration cmsis_dap_subcommand_handlers[] = {
 	COMMAND_REGISTRATION_DONE
 };
 
-COMMAND_HANDLER(cmsis_dap_reset_command)
-{
-	LOG_DEBUG("cmsis_dap_reset_command");
-	return ERROR_OK;
-}
-
-COMMAND_HANDLER(cmsis_dap_jtag_command)
-{
-	LOG_DEBUG("cmsis_dap_jtag_command");
-	return ERROR_OK;
-}
-
-static const struct command_registration cmsis_dap_jtag_subcommand_handlers[] = {
-	{
-		.name = "init",
-		.mode = COMMAND_ANY,
-		.handler = cmsis_dap_jtag_command,
-		.usage = ""
-	 },
-	{
-		.name = "arp_init",
-		.mode = COMMAND_ANY,
-		.handler = cmsis_dap_jtag_command,
-		.usage = ""
-	 },
-	{
-		.name = "arp_init-reset",
-		.mode = COMMAND_ANY,
-		.handler = cmsis_dap_reset_command,
-		.usage = ""
-	 },
-	{
-		.name = "tapisenabled",
-		.mode = COMMAND_EXEC,
-		.jim_handler = jim_jtag_tap_enabler,
-	 },
-	{
-		.name = "tapenable",
-		.mode = COMMAND_EXEC,
-		.jim_handler = jim_jtag_tap_enabler,
-	 },
-	{
-		.name = "tapdisable",
-		.mode = COMMAND_EXEC,
-		.handler = cmsis_dap_jtag_command,
-		.usage = "",
-	 },
-	{
-		.name = "configure",
-		.mode = COMMAND_EXEC,
-		.handler = cmsis_dap_jtag_command,
-		.usage = "",
-	 },
-	{
-		.name = "cget",
-		.mode = COMMAND_EXEC,
-		.jim_handler = jim_jtag_configure,
-	 },
-	{
-		.name = "names",
-		.mode = COMMAND_ANY,
-		.handler = cmsis_dap_jtag_command,
-		.usage = "",
-	 },
-	 COMMAND_REGISTRATION_DONE
-};
-
 static const struct command_registration cmsis_dap_command_handlers[] = {
 	{
 		.name = "cmsis-dap",
@@ -1098,24 +1093,25 @@ static const struct command_registration cmsis_dap_command_handlers[] = {
 		.usage = "(vid pid)* ",
 	},
 	{
-		/* this is currently a nasty hack so we get
-		 * reset working with non jtag interfaces */
-		.name = "jtag",
-		.mode = COMMAND_ANY,
-		.usage = "",
-		.chain = cmsis_dap_jtag_subcommand_handlers,
+		.name = "cmsis_dap_serial",
+		.handler = &cmsis_dap_handle_serial_command,
+		.mode = COMMAND_CONFIG,
+		.help = "set the serial number of the adapter",
+		.usage = "serial_string",
 	},
 	COMMAND_REGISTRATION_DONE
 };
 
 static const struct swd_driver cmsis_dap_swd_driver = {
-	.init       = cmsis_dap_swd_init,
-	.read_reg   = cmsis_dap_swd_read_reg,
-	.write_reg  = cmsis_dap_swd_write_reg,
-	.read_block = cmsis_dap_swd_read_block
+	.init = cmsis_dap_swd_init,
+	.frequency = cmsis_dap_swd_frequency,
+	.switch_seq = cmsis_dap_swd_switch_seq,
+	.read_reg = cmsis_dap_swd_read_reg,
+	.write_reg = cmsis_dap_swd_write_reg,
+	.run = cmsis_dap_swd_run_queue,
 };
 
-const char *cmsis_dap_transport[] = {"cmsis-dap", NULL};
+static const char * const cmsis_dap_transport[] = { "swd", NULL };
 
 struct jtag_interface cmsis_dap_interface = {
 	.name = "cmsis-dap",

src/jtag/drivers/ft2232.c

@@ -164,7 +164,7 @@ static uint16_t ft2232_vid[MAX_USB_IDS + 1] = { 0x0403, 0 };
 static uint16_t ft2232_pid[MAX_USB_IDS + 1] = { 0x6010, 0 };
 
 struct ft2232_layout {
-	char *name;
+	const char *name;
 	int (*init)(void);
 	void (*reset)(int trst, int srst);
 	void (*blink)(void);

src/jtag/drivers/ftdi.c

@@ -72,6 +72,7 @@
 
 /* project specific includes */
 #include <jtag/interface.h>
+#include <jtag/swd.h>
 #include <transport/transport.h>
 #include <helper/time_support.h>
 
@@ -85,11 +86,14 @@
 #include "mpsse.h"
 
 #define JTAG_MODE (LSB_FIRST | POS_EDGE_IN | NEG_EDGE_OUT)
+#define SWD_MODE (LSB_FIRST | POS_EDGE_IN | NEG_EDGE_OUT)
 
 static char *ftdi_device_desc;
 static char *ftdi_serial;
 static uint8_t ftdi_channel;
 
+static bool swd_mode;
+
 #define MAX_USB_IDS 8
 /* vid = pid = 0 marks the end of the list */
 static uint16_t ftdi_vid[MAX_USB_IDS + 1] = { 0 };
@@ -108,8 +112,23 @@ struct signal {
 
 static struct signal *signals;
 
+/* FIXME: Where to store per-instance data? We need an SWD context. */
+static struct swd_cmd_queue_entry {
+	uint8_t cmd;
+	uint32_t *dst;
+	uint8_t trn_ack_data_parity_trn[DIV_ROUND_UP(4 + 3 + 32 + 1 + 4, 8)];
+} *swd_cmd_queue;
+static size_t swd_cmd_queue_length;
+static size_t swd_cmd_queue_alloced;
+static int queued_retval;
+static int freq;
+
 static uint16_t output;
 static uint16_t direction;
+static uint16_t jtag_output_init;
+static uint16_t jtag_direction_init;
+
+static int ftdi_swd_switch_seq(struct adiv5_dap *dap, enum swd_special_seq seq);
 
 static struct signal *find_signal_by_name(const char *name)
 {
@@ -174,14 +193,19 @@ static int ftdi_set_signal(const struct signal *s, char value)
 		return ERROR_FAIL;
 	}
 
+	uint16_t old_output = output;
+	uint16_t old_direction = direction;
+
 	output = data ? output | s->data_mask : output & ~s->data_mask;
 	if (s->oe_mask == s->data_mask)
 		direction = oe ? direction | s->oe_mask : direction & ~s->oe_mask;
 	else
 		output = oe ? output | s->oe_mask : output & ~s->oe_mask;
 
-	mpsse_set_data_bits_low_byte(mpsse_ctx, output & 0xff, direction & 0xff);
-	mpsse_set_data_bits_high_byte(mpsse_ctx, output >> 8, direction >> 8);
+	if ((output & 0xff) != (old_output & 0xff) || (direction & 0xff) != (old_direction & 0xff))
+		mpsse_set_data_bits_low_byte(mpsse_ctx, output & 0xff, direction & 0xff);
+	if ((output >> 8 != old_output >> 8) || (direction >> 8 != old_direction >> 8))
+		mpsse_set_data_bits_high_byte(mpsse_ctx, output >> 8, direction >> 8);
 
 	return ERROR_OK;
 }
@@ -477,7 +501,8 @@ static void ftdi_execute_reset(struct jtag_command *cmd)
 			ftdi_set_signal(trst, '0');
 		else
 			LOG_ERROR("Can't assert TRST: nTRST signal is not defined");
-	} else if (trst && cmd->cmd.reset->trst == 0) {
+	} else if (trst && jtag_get_reset_config() & RESET_HAS_TRST &&
+			cmd->cmd.reset->trst == 0) {
 		if (jtag_get_reset_config() & RESET_TRST_OPEN_DRAIN)
 			ftdi_set_signal(trst, 'z');
 		else
@@ -490,7 +515,8 @@ static void ftdi_execute_reset(struct jtag_command *cmd)
 			ftdi_set_signal(srst, '0');
 		else
 			LOG_ERROR("Can't assert SRST: nSRST signal is not defined");
-	} else if (srst && cmd->cmd.reset->srst == 0) {
+	} else if (srst && jtag_get_reset_config() & RESET_HAS_SRST &&
+			cmd->cmd.reset->srst == 0) {
 		if (jtag_get_reset_config() & RESET_SRST_PUSH_PULL)
 			ftdi_set_signal(srst, '1');
 		else
@@ -608,11 +634,27 @@ static int ftdi_initialize(void)
 	if (!mpsse_ctx)
 		return ERROR_JTAG_INIT_FAILED;
 
+	output = jtag_output_init;
+	direction = jtag_direction_init;
+
+	if (swd_mode) {
+		struct signal *sig = find_signal_by_name("SWD_EN");
+		if (!sig) {
+			LOG_ERROR("SWD mode is active but SWD_EN signal is not defined");
+			return ERROR_JTAG_INIT_FAILED;
+		}
+		/* A dummy SWD_EN would have zero mask */
+		if (sig->data_mask)
+			ftdi_set_signal(sig, '1');
+	}
+
 	mpsse_set_data_bits_low_byte(mpsse_ctx, output & 0xff, direction & 0xff);
 	mpsse_set_data_bits_high_byte(mpsse_ctx, output >> 8, direction >> 8);
 
 	mpsse_loopback_config(mpsse_ctx, false);
 
+	freq = mpsse_set_frequency(mpsse_ctx, jtag_get_speed_khz() * 1000);
+
 	return mpsse_flush(mpsse_ctx);
 }
 
@@ -620,6 +662,8 @@ static int ftdi_quit(void)
 {
 	mpsse_close(mpsse_ctx);
 
+	free(swd_cmd_queue);
+
 	return ERROR_OK;
 }
 
@@ -664,8 +708,8 @@ COMMAND_HANDLER(ftdi_handle_layout_init_command)
 	if (CMD_ARGC != 2)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
-	COMMAND_PARSE_NUMBER(u16, CMD_ARGV[0], output);
-	COMMAND_PARSE_NUMBER(u16, CMD_ARGV[1], direction);
+	COMMAND_PARSE_NUMBER(u16, CMD_ARGV[0], jtag_output_init);
+	COMMAND_PARSE_NUMBER(u16, CMD_ARGV[1], jtag_direction_init);
 
 	return ERROR_OK;
 }
@@ -692,6 +736,19 @@ COMMAND_HANDLER(ftdi_handle_layout_signal_command)
 		} else if (strcmp("-noe", CMD_ARGV[i]) == 0) {
 			invert_oe = true;
 			COMMAND_PARSE_NUMBER(u16, CMD_ARGV[i + 1], oe_mask);
+		} else if (!strcmp("-alias", CMD_ARGV[i]) ||
+			   !strcmp("-nalias", CMD_ARGV[i])) {
+			if (!strcmp("-nalias", CMD_ARGV[i]))
+				invert_data = true;
+			struct signal *sig = find_signal_by_name(CMD_ARGV[i + 1]);
+			if (!sig) {
+				LOG_ERROR("signal %s is not defined", CMD_ARGV[i + 1]);
+				return ERROR_FAIL;
+			}
+			data_mask = sig->data_mask;
+			oe_mask = sig->oe_mask;
+			invert_oe = sig->invert_oe;
+			invert_data ^= sig->invert_data;
 		} else {
 			LOG_ERROR("unknown option '%s'", CMD_ARGV[i]);
 			return ERROR_COMMAND_SYNTAX_ERROR;
@@ -811,7 +868,7 @@ static const struct command_registration ftdi_command_handlers[] = {
 		.mode = COMMAND_ANY,
 		.help = "define a signal controlled by one or more FTDI GPIO as data "
 			"and/or output enable",
-		.usage = "name [-data mask|-ndata mask] [-oe mask|-noe mask]",
+		.usage = "name [-data mask|-ndata mask] [-oe mask|-noe mask] [-alias|-nalias name]",
 	},
 	{
 		.name = "ftdi_set_signal",
@@ -830,11 +887,240 @@ static const struct command_registration ftdi_command_handlers[] = {
 	COMMAND_REGISTRATION_DONE
 };
 
+static int create_default_signal(const char *name, uint16_t data_mask)
+{
+	struct signal *sig = create_signal(name);
+	if (!sig) {
+		LOG_ERROR("failed to create signal %s", name);
+		return ERROR_FAIL;
+	}
+	sig->invert_data = false;
+	sig->data_mask = data_mask;
+	sig->invert_oe = false;
+	sig->oe_mask = 0;
+
+	return ERROR_OK;
+}
+
+static int create_signals(void)
+{
+	if (create_default_signal("TCK", 0x01) != ERROR_OK)
+		return ERROR_FAIL;
+	if (create_default_signal("TDI", 0x02) != ERROR_OK)
+		return ERROR_FAIL;
+	if (create_default_signal("TDO", 0x04) != ERROR_OK)
+		return ERROR_FAIL;
+	if (create_default_signal("TMS", 0x08) != ERROR_OK)
+		return ERROR_FAIL;
+	return ERROR_OK;
+}
+
+static int ftdi_swd_init(void)
+{
+	LOG_INFO("FTDI SWD mode enabled");
+	swd_mode = true;
+
+	if (create_signals() != ERROR_OK)
+		return ERROR_FAIL;
+
+	swd_cmd_queue_alloced = 10;
+	swd_cmd_queue = malloc(swd_cmd_queue_alloced * sizeof(*swd_cmd_queue));
+
+	return swd_cmd_queue != NULL ? ERROR_OK : ERROR_FAIL;
+}
+
+static void ftdi_swd_swdio_en(bool enable)
+{
+	struct signal *oe = find_signal_by_name("SWDIO_OE");
+	if (oe)
+		ftdi_set_signal(oe, enable ? '1' : '0');
+}
+
+/**
+ * Flush the MPSSE queue and process the SWD transaction queue
+ * @param dap
+ * @return
+ */
+static int ftdi_swd_run_queue(struct adiv5_dap *dap)
+{
+	LOG_DEBUG("Executing %zu queued transactions", swd_cmd_queue_length);
+	int retval;
+	struct signal *led = find_signal_by_name("LED");
+
+	if (queued_retval != ERROR_OK) {
+		LOG_DEBUG("Skipping due to previous errors: %d", queued_retval);
+		goto skip;
+	}
+
+	/* A transaction must be followed by another transaction or at least 8 idle cycles to
+	 * ensure that data is clocked through the AP. */
+	mpsse_clock_data_out(mpsse_ctx, NULL, 0, 8, SWD_MODE);
+
+	/* Terminate the "blink", if the current layout has that feature */
+	if (led)
+		ftdi_set_signal(led, '0');
+
+	queued_retval = mpsse_flush(mpsse_ctx);
+	if (queued_retval != ERROR_OK) {
+		LOG_ERROR("MPSSE failed");
+		goto skip;
+	}
+
+	for (size_t i = 0; i < swd_cmd_queue_length; i++) {
+		int ack = buf_get_u32(swd_cmd_queue[i].trn_ack_data_parity_trn, 1, 3);
+
+		LOG_DEBUG("%s %s %s reg %X = %08"PRIx32,
+				ack == SWD_ACK_OK ? "OK" : ack == SWD_ACK_WAIT ? "WAIT" : ack == SWD_ACK_FAULT ? "FAULT" : "JUNK",
+				swd_cmd_queue[i].cmd & SWD_CMD_APnDP ? "AP" : "DP",
+				swd_cmd_queue[i].cmd & SWD_CMD_RnW ? "read" : "write",
+				(swd_cmd_queue[i].cmd & SWD_CMD_A32) >> 1,
+				buf_get_u32(swd_cmd_queue[i].trn_ack_data_parity_trn,
+						1 + 3 + (swd_cmd_queue[i].cmd & SWD_CMD_RnW ? 0 : 1), 32));
+
+		if (ack != SWD_ACK_OK) {
+			queued_retval = ack == SWD_ACK_WAIT ? ERROR_WAIT : ERROR_FAIL;
+			goto skip;
+
+		} else if (swd_cmd_queue[i].cmd & SWD_CMD_RnW) {
+			uint32_t data = buf_get_u32(swd_cmd_queue[i].trn_ack_data_parity_trn, 1 + 3, 32);
+			int parity = buf_get_u32(swd_cmd_queue[i].trn_ack_data_parity_trn, 1 + 3 + 32, 1);
+
+			if (parity != parity_u32(data)) {
+				LOG_ERROR("SWD Read data parity mismatch");
+				queued_retval = ERROR_FAIL;
+				goto skip;
+			}
+
+			if (swd_cmd_queue[i].dst != NULL)
+				*swd_cmd_queue[i].dst = data;
+		}
+	}
+
+skip:
+	swd_cmd_queue_length = 0;
+	retval = queued_retval;
+	queued_retval = ERROR_OK;
+
+	/* Queue a new "blink" */
+	if (led && retval == ERROR_OK)
+		ftdi_set_signal(led, '1');
+
+	return retval;
+}
+
+static void ftdi_swd_queue_cmd(struct adiv5_dap *dap, uint8_t cmd, uint32_t *dst, uint32_t data)
+{
+	if (swd_cmd_queue_length >= swd_cmd_queue_alloced) {
+		/* Not enough room in the queue. Run the queue and increase its size for next time.
+		 * Note that it's not possible to avoid running the queue here, because mpsse contains
+		 * pointers into the queue which may be invalid after the realloc. */
+		queued_retval = ftdi_swd_run_queue(dap);
+		struct swd_cmd_queue_entry *q = realloc(swd_cmd_queue, swd_cmd_queue_alloced * 2 * sizeof(*swd_cmd_queue));
+		if (q != NULL) {
+			swd_cmd_queue = q;
+			swd_cmd_queue_alloced *= 2;
+			LOG_DEBUG("Increased SWD command queue to %zu elements", swd_cmd_queue_alloced);
+		}
+	}
+
+	if (queued_retval != ERROR_OK)
+		return;
+
+	size_t i = swd_cmd_queue_length++;
+	swd_cmd_queue[i].cmd = cmd | SWD_CMD_START | SWD_CMD_PARK;
+
+	mpsse_clock_data_out(mpsse_ctx, &swd_cmd_queue[i].cmd, 0, 8, SWD_MODE);
+
+	if (swd_cmd_queue[i].cmd & SWD_CMD_RnW) {
+		/* Queue a read transaction */
+		swd_cmd_queue[i].dst = dst;
+
+		ftdi_swd_swdio_en(false);
+		mpsse_clock_data_in(mpsse_ctx, swd_cmd_queue[i].trn_ack_data_parity_trn,
+				0, 1 + 3 + 32 + 1 + 1, SWD_MODE);
+		ftdi_swd_swdio_en(true);
+	} else {
+		/* Queue a write transaction */
+		ftdi_swd_swdio_en(false);
+
+		mpsse_clock_data_in(mpsse_ctx, swd_cmd_queue[i].trn_ack_data_parity_trn,
+				0, 1 + 3 + 1, SWD_MODE);
+
+		ftdi_swd_swdio_en(true);
+
+		buf_set_u32(swd_cmd_queue[i].trn_ack_data_parity_trn, 1 + 3 + 1, 32, data);
+		buf_set_u32(swd_cmd_queue[i].trn_ack_data_parity_trn, 1 + 3 + 1 + 32, 1, parity_u32(data));
+
+		mpsse_clock_data_out(mpsse_ctx, swd_cmd_queue[i].trn_ack_data_parity_trn,
+				1 + 3 + 1, 32 + 1, SWD_MODE);
+	}
+
+	/* Insert idle cycles after AP accesses to avoid WAIT */
+	if (cmd & SWD_CMD_APnDP)
+		mpsse_clock_data_out(mpsse_ctx, NULL, 0, dap->memaccess_tck, SWD_MODE);
+
+}
+
+static void ftdi_swd_read_reg(struct adiv5_dap *dap, uint8_t cmd, uint32_t *value)
+{
+	assert(cmd & SWD_CMD_RnW);
+	ftdi_swd_queue_cmd(dap, cmd, value, 0);
+}
+
+static void ftdi_swd_write_reg(struct adiv5_dap *dap, uint8_t cmd, uint32_t value)
+{
+	assert(!(cmd & SWD_CMD_RnW));
+	ftdi_swd_queue_cmd(dap, cmd, NULL, value);
+}
+
+static int_least32_t ftdi_swd_frequency(struct adiv5_dap *dap, int_least32_t hz)
+{
+	if (hz > 0)
+		freq = mpsse_set_frequency(mpsse_ctx, hz);
+
+	return freq;
+}
+
+static int ftdi_swd_switch_seq(struct adiv5_dap *dap, enum swd_special_seq seq)
+{
+	switch (seq) {
+	case LINE_RESET:
+		LOG_DEBUG("SWD line reset");
+		mpsse_clock_data_out(mpsse_ctx, swd_seq_line_reset, 0, swd_seq_line_reset_len, SWD_MODE);
+		break;
+	case JTAG_TO_SWD:
+		LOG_DEBUG("JTAG-to-SWD");
+		mpsse_clock_data_out(mpsse_ctx, swd_seq_jtag_to_swd, 0, swd_seq_jtag_to_swd_len, SWD_MODE);
+		break;
+	case SWD_TO_JTAG:
+		LOG_DEBUG("SWD-to-JTAG");
+		mpsse_clock_data_out(mpsse_ctx, swd_seq_swd_to_jtag, 0, swd_seq_swd_to_jtag_len, SWD_MODE);
+		break;
+	default:
+		LOG_ERROR("Sequence %d not supported", seq);
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
+static const struct swd_driver ftdi_swd = {
+	.init = ftdi_swd_init,
+	.frequency = ftdi_swd_frequency,
+	.switch_seq = ftdi_swd_switch_seq,
+	.read_reg = ftdi_swd_read_reg,
+	.write_reg = ftdi_swd_write_reg,
+	.run = ftdi_swd_run_queue,
+};
+
+static const char * const ftdi_transports[] = { "jtag", "swd", NULL };
+
 struct jtag_interface ftdi_interface = {
 	.name = "ftdi",
 	.supported = DEBUG_CAP_TMS_SEQ,
 	.commands = ftdi_command_handlers,
-	.transports = jtag_only,
+	.transports = ftdi_transports,
+	.swd = &ftdi_swd,
 
 	.init = ftdi_initialize,
 	.quit = ftdi_quit,

src/jtag/drivers/jlink.c

@@ -29,6 +29,7 @@
 #endif
 
 #include <jtag/interface.h>
+#include <jtag/swd.h>
 #include <jtag/commands.h>
 #include "libusb_common.h"
 
@@ -46,16 +47,12 @@
  * pid = ( usb_address > 0x4) ? 0x0101 : (0x101 + usb_address)
  */
 
-#define JLINK_OB_PID  0x0105
+#define JLINK_USB_INTERFACE_CLASS 0xff
+#define JLINK_USB_INTERFACE_SUBCLASS 0xff
+#define JLINK_USB_INTERFACE_PROTOCOL 0xff
 
-#define JLINK_WRITE_ENDPOINT	0x02
-#define JLINK_READ_ENDPOINT		0x81
-
-#define JLINK_OB_WRITE_ENDPOINT	0x06
-#define JLINK_OB_READ_ENDPOINT	0x85
-
-static unsigned int jlink_write_ep = JLINK_WRITE_ENDPOINT;
-static unsigned int jlink_read_ep = JLINK_READ_ENDPOINT;
+static unsigned int jlink_write_ep;
+static unsigned int jlink_read_ep;
 static unsigned int jlink_hw_jtag_version = 2;
 
 #define JLINK_USB_TIMEOUT 1000
@@ -80,6 +77,7 @@ static uint8_t usb_out_buffer[JLINK_OUT_BUFFER_SIZE];
 #define EMU_CMD_SET_SPEED		0x05
 #define EMU_CMD_GET_STATE		0x07
 #define EMU_CMD_SET_KS_POWER	0x08
+#define EMU_CMD_REGISTER		0x09
 #define EMU_CMD_GET_SPEEDS		0xc0
 #define EMU_CMD_GET_HW_INFO		0xc1
 #define EMU_CMD_GET_COUNTERS	0xc2
@@ -115,6 +113,10 @@ static uint8_t usb_out_buffer[JLINK_OUT_BUFFER_SIZE];
 #define EMU_CMD_WRITE_MEM_ARM79		0xf7
 #define EMU_CMD_READ_MEM_ARM79		0xf8
 
+/* Register subcommands */
+#define REG_CMD_REGISTER		100
+#define REG_CMD_UNREGISTER		101
+
 /* bits return from EMU_CMD_GET_CAPS */
 #define EMU_CAP_RESERVED_1		0
 #define EMU_CAP_GET_HW_VERSION		1
@@ -149,7 +151,7 @@ static uint8_t usb_out_buffer[JLINK_OUT_BUFFER_SIZE];
 #define EMU_CAP_RAWTRACE		30
 #define EMU_CAP_RESERVED_3		31
 
-static char *jlink_cap_str[] = {
+static const char * const jlink_cap_str[] = {
 	"Always 1.",
 	"Supports command EMU_CMD_GET_HARDWARE_VERSION",
 	"Supports command EMU_CMD_WRITE_DCC",
@@ -188,18 +190,20 @@ static char *jlink_cap_str[] = {
 #define JLINK_MAX_SPEED 12000
 
 /* J-Link hardware versions */
-#define JLINK_HW_TYPE_JLINK	0
-#define JLINK_HW_TYPE_JTRACE	1
-#define JLINK_HW_TYPE_FLASHER	2
-#define JLINK_HW_TYPE_JLINK_PRO	3
-#define JLINK_HW_TYPE_MAX	4
+#define JLINK_HW_TYPE_JLINK				0
+#define JLINK_HW_TYPE_JTRACE			1
+#define JLINK_HW_TYPE_FLASHER			2
+#define JLINK_HW_TYPE_JLINK_PRO			3
+#define JLINK_HW_TYPE_JLINK_LITE_ADI	5
+#define JLINK_HW_TYPE_JLINK_LITE_XMC4000	16
+#define JLINK_HW_TYPE_JLINK_LITE_XMC4200	17
+#define JLINK_HW_TYPE_LPCLINK2			18
 
-static char *jlink_hw_type_str[] = {
-	"J-Link",
-	"J-Trace",
-	"Flasher",
-	"J-Link Pro",
-};
+/* Interface selection */
+#define JLINK_TIF_JTAG		0
+#define JLINK_TIF_SWD		1
+#define JLINK_SWD_DIR_IN	0
+#define JLINK_SWD_DIR_OUT	1
 
 /* Queue command functions */
 static void jlink_end_state(tap_state_t state);
@@ -211,6 +215,9 @@ static void jlink_scan(bool ir_scan, enum scan_type type, uint8_t *buffer,
 static void jlink_reset(int trst, int srst);
 static void jlink_simple_command(uint8_t command);
 static int jlink_get_status(void);
+static int jlink_swd_run_queue(struct adiv5_dap *dap);
+static void jlink_swd_queue_cmd(struct adiv5_dap *dap, uint8_t cmd, uint32_t *dst, uint32_t data);
+static int jlink_swd_switch_seq(struct adiv5_dap *dap, enum swd_special_seq seq);
 
 /* J-Link tap buffer functions */
 static void jlink_tap_init(void);
@@ -251,9 +258,14 @@ static struct jlink *jlink_handle;
 static uint16_t vids[] = { 0x1366, 0x1366, 0x1366, 0x1366, 0x1366, 0 };
 static uint16_t pids[] = { 0x0101, 0x0102, 0x0103, 0x0104, 0x0105, 0 };
 
+static char *jlink_serial;
+
 static uint32_t jlink_caps;
 static uint32_t jlink_hw_type;
 
+static int queued_retval;
+static bool swd_mode;
+
 /* 256 byte non-volatile memory */
 struct jlink_config {
 	uint8_t	usb_address;
@@ -422,6 +434,41 @@ static int jlink_khz(int khz, int *jtag_speed)
 	return ERROR_OK;
 }
 
+static int jlink_register(void)
+{
+	int result;
+	usb_out_buffer[0] = EMU_CMD_REGISTER;
+	usb_out_buffer[1] = REG_CMD_REGISTER;
+	/* 2 - 11 is "additional parameter",
+	 * 12 - 13 is connection handle, zero initially */
+	memset(&usb_out_buffer[2], 0, 10 + 2);
+
+	result = jlink_usb_write(jlink_handle, 14);
+	if (result != 14) {
+		LOG_ERROR("J-Link register write failed (%d)", result);
+		return ERROR_JTAG_DEVICE_ERROR;
+	}
+
+	/* Returns:
+	 * 0 - 1 connection handle,
+	 * 2 - 3 number of information entities,
+	 * 4 - 5 size of a single information struct,
+	 * 6 - 7 number of additional bytes,
+	 * 8 - ... reply data
+	 *
+	 * Try to read the whole USB bulk packet
+	 */
+	result = jtag_libusb_bulk_read(jlink_handle->usb_handle, jlink_read_ep,
+				       (char *)usb_in_buffer, sizeof(usb_in_buffer),
+				       JLINK_USB_TIMEOUT);
+	if (!result) {
+		LOG_ERROR("J-Link register read failed (0 bytes received)");
+		return ERROR_JTAG_DEVICE_ERROR;
+	}
+
+	return ERROR_OK;
+}
+
 /*
  * select transport interface
  *
@@ -495,28 +542,6 @@ static int jlink_init(void)
 		return ERROR_JTAG_INIT_FAILED;
 	}
 
-	/*
-	 * The next three instructions were added after discovering a problem
-	 * while using an oscilloscope.
-	 * For the V8 SAM-ICE dongle (and likely other j-link device variants),
-	 * the reset line to the target microprocessor was found to cycle only
-	 * intermittently during emulator startup (even after encountering the
-	 * downstream reset instruction later in the code).
-	 * This was found to create two issues:
-	 * 1) In general it is a bad practice to not reset a CPU to a known
-	 * state when starting an emulator and
-	 * 2) something critical happens inside the dongle when it does the
-	 * first read following a new USB session.
-	 * Keeping the processor in reset during the first read collecting
-	 * version information seems to prevent errant
-	 * "J-Link command EMU_CMD_VERSION failed" issues.
-	 */
-
-	LOG_INFO("J-Link initialization started / target CPU reset initiated");
-	jlink_simple_command(EMU_CMD_HW_TRST0);
-	jlink_simple_command(EMU_CMD_HW_RESET0);
-	usleep(1000);
-
 	jlink_hw_jtag_version = 2;
 
 	if (jlink_get_version_info() == ERROR_OK) {
@@ -524,16 +549,26 @@ static int jlink_init(void)
 		jlink_get_status();
 	}
 
+	/* Registration is sometimes necessary for SWD to work */
+	if (jlink_caps & (1<<EMU_CAP_REGISTER))
+		jlink_register();
+
 	/*
 	 * Some versions of Segger's software do not select JTAG interface by default.
 	 *
 	 * Segger recommends to select interface necessarily as a part of init process,
 	 * in case any previous session leaves improper interface selected.
-	 *
-	 * Until SWD implemented, select only JTAG interface here.
 	 */
+	int retval;
 	if (jlink_caps & (1<<EMU_CAP_SELECT_IF))
-		jlink_select_interface(0);
+		retval = jlink_select_interface(swd_mode ? JLINK_TIF_SWD : JLINK_TIF_JTAG);
+	else
+		retval = swd_mode ? ERROR_JTAG_DEVICE_ERROR : ERROR_OK;
+
+	if (retval != ERROR_OK) {
+		LOG_ERROR("Selected transport mode is not supported.");
+		return ERROR_JTAG_INIT_FAILED;
+	}
 
 	LOG_INFO("J-Link JTAG Interface ready");
 
@@ -541,11 +576,15 @@ static int jlink_init(void)
 	jtag_sleep(3000);
 	jlink_tap_init();
 
-	/* v5/6 jlink seems to have an issue if the first tap move
-	 * is not divisible by 8, so we send a TLR on first power up */
-	for (i = 0; i < 8; i++)
-		jlink_tap_append_step(1, 0);
-	jlink_tap_execute();
+	jlink_speed(jtag_get_speed_khz());
+
+	if (!swd_mode) {
+		/* v5/6 jlink seems to have an issue if the first tap move
+		 * is not divisible by 8, so we send a TLR on first power up */
+		for (i = 0; i < 8; i++)
+			jlink_tap_append_step(1, 0);
+		jlink_tap_execute();
+	}
 
 	return ERROR_OK;
 }
@@ -921,10 +960,35 @@ static int jlink_get_version_info(void)
 
 		LOG_INFO("J-Link hw version %i", (int)jlink_hw_version);
 
-		if (jlink_hw_type >= JLINK_HW_TYPE_MAX)
-			LOG_INFO("J-Link hw type uknown 0x%" PRIx32, jlink_hw_type);
-		else
-			LOG_INFO("J-Link hw type %s", jlink_hw_type_str[jlink_hw_type]);
+		switch (jlink_hw_type) {
+			case JLINK_HW_TYPE_JLINK:
+				LOG_INFO("J-Link hw type J-Link");
+				break;
+			case JLINK_HW_TYPE_JTRACE:
+				LOG_INFO("J-Link hw type J-Trace");
+				break;
+			case JLINK_HW_TYPE_FLASHER:
+				LOG_INFO("J-Link hw type Flasher");
+				break;
+			case JLINK_HW_TYPE_JLINK_PRO:
+				LOG_INFO("J-Link hw type J-Link Pro");
+				break;
+			case JLINK_HW_TYPE_JLINK_LITE_ADI:
+				LOG_INFO("J-Link hw type J-Link Lite-ADI");
+				break;
+			case JLINK_HW_TYPE_JLINK_LITE_XMC4000:
+				LOG_INFO("J-Link hw type J-Link Lite-XMC4000");
+				break;
+			case JLINK_HW_TYPE_JLINK_LITE_XMC4200:
+				LOG_INFO("J-Link hw type J-Link Lite-XMC4200");
+				break;
+			case JLINK_HW_TYPE_LPCLINK2:
+				LOG_INFO("J-Link hw type J-Link on LPC-Link2");
+				break;
+			default:
+				LOG_INFO("J-Link hw type unknown 0x%" PRIx32, jlink_hw_type);
+				break;
+		}
 	}
 
 	if (jlink_caps & (1 << EMU_CAP_GET_MAX_BLOCK_SIZE)) {
@@ -965,6 +1029,19 @@ COMMAND_HANDLER(jlink_pid_command)
 	return ERROR_OK;
 }
 
+COMMAND_HANDLER(jlink_serial_command)
+{
+	if (CMD_ARGC != 1) {
+		LOG_ERROR("Need exactly one argument to jlink_serial");
+		return ERROR_FAIL;
+	}
+	if (jlink_serial)
+		free(jlink_serial);
+	jlink_serial = strdup(CMD_ARGV[0]);
+
+	return ERROR_OK;
+}
+
 COMMAND_HANDLER(jlink_handle_jlink_info_command)
 {
 	if (jlink_get_version_info() == ERROR_OK) {
@@ -1281,6 +1358,12 @@ static const struct command_registration jlink_subcommand_handlers[] = {
 		.mode = COMMAND_CONFIG,
 		.help = "set the pid of the interface we want to use",
 	},
+	{
+		.name = "serial",
+		.handler = &jlink_serial_command,
+		.mode = COMMAND_CONFIG,
+		.help = "set the serial number of the J-Link adapter we want to use"
+	},
 	COMMAND_REGISTRATION_DONE
 };
 
@@ -1294,10 +1377,50 @@ static const struct command_registration jlink_command_handlers[] = {
 	COMMAND_REGISTRATION_DONE
 };
 
+static int jlink_swd_init(void)
+{
+	LOG_INFO("JLink SWD mode enabled");
+	swd_mode = true;
+
+	return ERROR_OK;
+}
+
+static void jlink_swd_write_reg(struct adiv5_dap *dap, uint8_t cmd, uint32_t value)
+{
+	assert(!(cmd & SWD_CMD_RnW));
+	jlink_swd_queue_cmd(dap, cmd, NULL, value);
+}
+
+static void jlink_swd_read_reg(struct adiv5_dap *dap, uint8_t cmd, uint32_t *value)
+{
+	assert(cmd & SWD_CMD_RnW);
+	jlink_swd_queue_cmd(dap, cmd, value, 0);
+}
+
+static int_least32_t jlink_swd_frequency(struct adiv5_dap *dap, int_least32_t hz)
+{
+	if (hz > 0)
+		jlink_speed(hz / 1000);
+
+	return hz;
+}
+
+static const struct swd_driver jlink_swd = {
+	.init = jlink_swd_init,
+	.frequency = jlink_swd_frequency,
+	.switch_seq = jlink_swd_switch_seq,
+	.read_reg = jlink_swd_read_reg,
+	.write_reg = jlink_swd_write_reg,
+	.run = jlink_swd_run_queue,
+};
+
+static const char * const jlink_transports[] = { "jtag", "swd", NULL };
+
 struct jtag_interface jlink_interface = {
 	.name = "jlink",
 	.commands = jlink_command_handlers,
-	.transports = jtag_only,
+	.transports = jlink_transports,
+	.swd = &jlink_swd,
 
 	.execute_queue = jlink_execute_queue,
 	.speed = jlink_speed,
@@ -1312,6 +1435,7 @@ struct jtag_interface jlink_interface = {
 
 
 static unsigned tap_length;
+/* In SWD mode use tms buffer for direction control */
 static uint8_t tms_buffer[JLINK_TAP_BUFFER_SIZE];
 static uint8_t tdi_buffer[JLINK_TAP_BUFFER_SIZE];
 static uint8_t tdo_buffer[JLINK_TAP_BUFFER_SIZE];
@@ -1320,7 +1444,7 @@ struct pending_scan_result {
 	int first;	/* First bit position in tdo_buffer to read */
 	int length; /* Number of bits to read */
 	struct scan_command *command; /* Corresponding scan command */
-	uint8_t *buffer;
+	void *buffer;
 };
 
 #define MAX_PENDING_SCAN_RESULTS 256
@@ -1430,7 +1554,8 @@ static int jlink_tap_execute(void)
 
 	result = use_jtag3 ? usb_in_buffer[byte_length] : 0;
 	if (result != 0) {
-		LOG_ERROR("jlink_tap_execute failed, result %d", result);
+		LOG_ERROR("jlink_tap_execute failed, result %d (%s)", result,
+			  result == 1 ? "adaptive clocking timeout" : "unknown");
 		jlink_tap_init();
 		return ERROR_JTAG_QUEUE_FAILED;
 	}
@@ -1464,13 +1589,191 @@ static int jlink_tap_execute(void)
 	return ERROR_OK;
 }
 
+static void fill_buffer(uint8_t *buf, uint32_t val, uint32_t len)
+{
+	unsigned int tap_pos = tap_length;
+
+	while (len > 32) {
+		buf_set_u32(buf, tap_pos, 32, val);
+		len -= 32;
+		tap_pos += 32;
+	}
+	if (len)
+		buf_set_u32(buf, tap_pos, len, val);
+}
+
+static void jlink_queue_data_out(const uint8_t *data, uint32_t len)
+{
+	const uint32_t dir_out = 0xffffffff;
+
+	if (data)
+		bit_copy(tdi_buffer, tap_length, data, 0, len);
+	else
+		fill_buffer(tdi_buffer, 0, len);
+	fill_buffer(tms_buffer, dir_out, len);
+	tap_length += len;
+}
+
+static void jlink_queue_data_in(uint32_t len)
+{
+	const uint32_t dir_in = 0;
+
+	fill_buffer(tms_buffer, dir_in, len);
+	tap_length += len;
+}
+
+static int jlink_swd_switch_seq(struct adiv5_dap *dap, enum swd_special_seq seq)
+{
+	const uint8_t *s;
+	unsigned int s_len;
+
+	switch (seq) {
+	case LINE_RESET:
+		LOG_DEBUG("SWD line reset");
+		s = swd_seq_line_reset;
+		s_len = swd_seq_line_reset_len;
+		break;
+	case JTAG_TO_SWD:
+		LOG_DEBUG("JTAG-to-SWD");
+		s = swd_seq_jtag_to_swd;
+		s_len = swd_seq_jtag_to_swd_len;
+		break;
+	case SWD_TO_JTAG:
+		LOG_DEBUG("SWD-to-JTAG");
+		s = swd_seq_swd_to_jtag;
+		s_len = swd_seq_swd_to_jtag_len;
+		break;
+	default:
+		LOG_ERROR("Sequence %d not supported", seq);
+		return ERROR_FAIL;
+	}
+
+	jlink_queue_data_out(s, s_len);
+
+	return ERROR_OK;
+}
+
+static int jlink_swd_run_queue(struct adiv5_dap *dap)
+{
+	LOG_DEBUG("Executing %d queued transactions", pending_scan_results_length);
+	int retval;
+
+	if (queued_retval != ERROR_OK) {
+		LOG_DEBUG("Skipping due to previous errors: %d", queued_retval);
+		goto skip;
+	}
+
+	/* A transaction must be followed by another transaction or at least 8 idle cycles to
+	 * ensure that data is clocked through the AP. */
+	jlink_queue_data_out(NULL, 8);
+
+	size_t byte_length = DIV_ROUND_UP(tap_length, 8);
+
+	/* There's a comment in jlink_tap_execute saying JLink returns
+	 * an extra NULL in packet when size of incoming message is a
+	 * multiple of 64. Someone should verify if that's still the
+	 * case with the current jlink firmware */
+
+	usb_out_buffer[0] = EMU_CMD_HW_JTAG3;
+	usb_out_buffer[1] = 0;
+	usb_out_buffer[2] = (tap_length >> 0) & 0xff;
+	usb_out_buffer[3] = (tap_length >> 8) & 0xff;
+	memcpy(usb_out_buffer + 4, tms_buffer, byte_length);
+	memcpy(usb_out_buffer + 4 + byte_length, tdi_buffer, byte_length);
+
+	retval = jlink_usb_message(jlink_handle, 4 + 2 * byte_length,
+				   byte_length + 1);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("jlink_swd_run_queue failed USB io (%d)", retval);
+		goto skip;
+	}
+
+	retval = usb_in_buffer[byte_length];
+	if (retval) {
+		LOG_ERROR("jlink_swd_run_queue failed, result %d", retval);
+		goto skip;
+	}
+
+	for (int i = 0; i < pending_scan_results_length; i++) {
+		int ack = buf_get_u32(usb_in_buffer, pending_scan_results_buffer[i].first, 3);
+
+		if (ack != SWD_ACK_OK) {
+			LOG_DEBUG("SWD ack not OK: %d %s", ack,
+				  ack == SWD_ACK_WAIT ? "WAIT" : ack == SWD_ACK_FAULT ? "FAULT" : "JUNK");
+			queued_retval = ack == SWD_ACK_WAIT ? ERROR_WAIT : ERROR_FAIL;
+			goto skip;
+		} else if (pending_scan_results_buffer[i].length) {
+			uint32_t data = buf_get_u32(usb_in_buffer, 3 + pending_scan_results_buffer[i].first, 32);
+			int parity = buf_get_u32(usb_in_buffer, 3 + 32 + pending_scan_results_buffer[i].first, 1);
+
+			if (parity != parity_u32(data)) {
+				LOG_ERROR("SWD Read data parity mismatch");
+				queued_retval = ERROR_FAIL;
+				goto skip;
+			}
+
+			if (pending_scan_results_buffer[i].buffer)
+				*(uint32_t *)pending_scan_results_buffer[i].buffer = data;
+		}
+	}
+
+skip:
+	jlink_tap_init();
+	retval = queued_retval;
+	queued_retval = ERROR_OK;
+
+	return retval;
+}
+
+static void jlink_swd_queue_cmd(struct adiv5_dap *dap, uint8_t cmd, uint32_t *dst, uint32_t data)
+{
+	uint8_t data_parity_trn[DIV_ROUND_UP(32 + 1, 8)];
+	if (tap_length + 46 + 8 + dap->memaccess_tck >= sizeof(tdi_buffer) * 8 ||
+	    pending_scan_results_length == MAX_PENDING_SCAN_RESULTS) {
+		/* Not enough room in the queue. Run the queue. */
+		queued_retval = jlink_swd_run_queue(dap);
+	}
+
+	if (queued_retval != ERROR_OK)
+		return;
+
+	cmd |= SWD_CMD_START | SWD_CMD_PARK;
+
+	jlink_queue_data_out(&cmd, 8);
+
+	pending_scan_results_buffer[pending_scan_results_length].first = tap_length;
+
+	if (cmd & SWD_CMD_RnW) {
+		/* Queue a read transaction */
+		pending_scan_results_buffer[pending_scan_results_length].length = 32;
+		pending_scan_results_buffer[pending_scan_results_length].buffer = dst;
+
+		jlink_queue_data_in(1 + 3 + 32 + 1 + 1);
+	} else {
+		/* Queue a write transaction */
+		pending_scan_results_buffer[pending_scan_results_length].length = 0;
+		jlink_queue_data_in(1 + 3 + 1);
+
+		buf_set_u32(data_parity_trn, 0, 32, data);
+		buf_set_u32(data_parity_trn, 32, 1, parity_u32(data));
+
+		jlink_queue_data_out(data_parity_trn, 32 + 1);
+	}
+
+	pending_scan_results_length++;
+
+	/* Insert idle cycles after AP accesses to avoid WAIT */
+	if (cmd & SWD_CMD_APnDP)
+		jlink_queue_data_out(NULL, dap->memaccess_tck);
+}
+
 /*****************************************************************************/
 /* JLink USB low-level functions */
 
 static struct jlink *jlink_usb_open()
 {
 	struct jtag_libusb_device_handle *devh;
-	if (jtag_libusb_open(vids, pids, &devh) != ERROR_OK)
+	if (jtag_libusb_open(vids, pids, jlink_serial, &devh) != ERROR_OK)
 		return NULL;
 
 	/* BE ***VERY CAREFUL*** ABOUT MAKING CHANGES IN THIS
@@ -1484,7 +1787,15 @@ static struct jlink *jlink_usb_open()
 	 * committing them!
 	 */
 
-#if IS_WIN32 == 0
+/* This entire block can probably be removed. It was a workaround for
+ * libusb0.1 and old JLink firmware. It has already be removed for
+ * windows and causing problems (LPC Link-2 with JLink firmware) on
+ * Linux with libusb1.0.
+ *
+ * However, for now the behavior will be left unchanged for non-windows
+ * platforms using libusb0.1 due to lack of testing.
+ */
+#if IS_WIN32 == 0 && HAVE_LIBUSB1 == 0
 
 	jtag_libusb_reset_device(devh);
 
@@ -1494,7 +1805,7 @@ static struct jlink *jlink_usb_open()
 	/* reopen jlink after usb_reset
 	 * on win32 this may take a second or two to re-enumerate */
 	int retval;
-	while ((retval = jtag_libusb_open(vids, pids, &devh)) != ERROR_OK) {
+	while ((retval = jtag_libusb_open(vids, pids, jlink_serial, &devh)) != ERROR_OK) {
 		usleep(1000);
 		timeout--;
 		if (!timeout)
@@ -1506,29 +1817,15 @@ static struct jlink *jlink_usb_open()
 
 #endif
 
-	/* usb_set_configuration required under win32 */
-	struct jtag_libusb_device *udev = jtag_libusb_get_device(devh);
+	/* usb_set_configuration is only required under win32
+	 * with libusb 0.1 and libusb0.sys. For libusb 1.0 it is a no-op
+	 * since the configuration is already set. */
 	jtag_libusb_set_configuration(devh, 0);
-	jtag_libusb_claim_interface(devh, 0);
 
-#if 0
-	/*
-	 * This makes problems under Mac OS X. And is not needed
-	 * under Windows. Hopefully this will not break a linux build
-	 */
-	usb_set_altinterface(result->usb_handle, 0);
-#endif
-
-	/* Use the OB endpoints if the JLink we matched is a Jlink-OB adapter */
-	uint16_t matched_pid;
-	if (jtag_libusb_get_pid(udev, &matched_pid) == ERROR_OK) {
-		if (matched_pid == JLINK_OB_PID) {
-			jlink_read_ep = JLINK_OB_WRITE_ENDPOINT;
-			jlink_write_ep = JLINK_OB_READ_ENDPOINT;
-		}
-	}
-
-	jtag_libusb_get_endpoints(udev, &jlink_read_ep, &jlink_write_ep);
+	jtag_libusb_choose_interface(devh, &jlink_read_ep, &jlink_write_ep,
+				     JLINK_USB_INTERFACE_CLASS,
+				     JLINK_USB_INTERFACE_SUBCLASS,
+				     JLINK_USB_INTERFACE_PROTOCOL);
 
 	struct jlink *result = malloc(sizeof(struct jlink));
 	result->usb_handle = devh;

src/jtag/drivers/libusb0_common.c

@@ -37,9 +37,40 @@ static bool jtag_libusb_match(struct jtag_libusb_device *dev,
 	return false;
 }
 
+/* Returns true if the string descriptor indexed by str_index in device matches string */
+static bool string_descriptor_equal(usb_dev_handle *device, uint8_t str_index,
+									const char *string)
+{
+	int retval;
+	bool matched;
+	char desc_string[256+1]; /* Max size of string descriptor */
+
+	if (str_index == 0)
+		return false;
+
+	retval = usb_get_string_simple(device, str_index,
+			desc_string, sizeof(desc_string)-1);
+	if (retval < 0) {
+		LOG_ERROR("usb_get_string_simple() failed with %d", retval);
+		return false;
+	}
+
+	/* Null terminate descriptor string in case it needs to be logged. */
+	desc_string[sizeof(desc_string)-1] = '\0';
+
+	matched = strncmp(string, desc_string, sizeof(desc_string)) == 0;
+	if (!matched)
+		LOG_DEBUG("Device serial number '%s' doesn't match requested serial '%s'",
+			desc_string, string);
+	return matched;
+}
+
 int jtag_libusb_open(const uint16_t vids[], const uint16_t pids[],
+		const char *serial,
 		struct jtag_libusb_device_handle **out)
 {
+	int retval = -ENODEV;
+	struct jtag_libusb_device_handle *libusb_handle;
 	usb_init();
 
 	usb_find_busses();
@@ -52,13 +83,24 @@ int jtag_libusb_open(const uint16_t vids[], const uint16_t pids[],
 			if (!jtag_libusb_match(dev, vids, pids))
 				continue;
 
-			*out = usb_open(dev);
-			if (NULL == *out)
-				return -errno;
-			return 0;
+			libusb_handle = usb_open(dev);
+			if (NULL == libusb_handle) {
+				retval = -errno;
+				continue;
+			}
+
+			/* Device must be open to use libusb_get_string_descriptor_ascii. */
+			if (serial != NULL &&
+					!string_descriptor_equal(libusb_handle, dev->descriptor.iSerialNumber, serial)) {
+				usb_close(libusb_handle);
+				continue;
+			}
+			*out = libusb_handle;
+			retval = 0;
+			break;
 		}
 	}
-	return -ENODEV;
+	return retval;
 }
 
 void jtag_libusb_close(jtag_libusb_device_handle *dev)
@@ -103,14 +145,23 @@ int jtag_libusb_set_configuration(jtag_libusb_device_handle *devh,
 			udev->config[configuration].bConfigurationValue);
 }
 
-int jtag_libusb_get_endpoints(struct jtag_libusb_device *udev,
+int jtag_libusb_choose_interface(struct jtag_libusb_device_handle *devh,
 		unsigned int *usb_read_ep,
-		unsigned int *usb_write_ep)
+		unsigned int *usb_write_ep,
+		int bclass, int subclass, int protocol)
 {
+	struct jtag_libusb_device *udev = jtag_libusb_get_device(devh);
 	struct usb_interface *iface = udev->config->interface;
 	struct usb_interface_descriptor *desc = iface->altsetting;
 
+	*usb_read_ep = *usb_write_ep = 0;
+
 	for (int i = 0; i < desc->bNumEndpoints; i++) {
+		if ((bclass > 0 && desc->bInterfaceClass != bclass) ||
+		    (subclass > 0 && desc->bInterfaceSubClass != subclass) ||
+		    (protocol > 0 && desc->bInterfaceProtocol != protocol))
+			continue;
+
 		uint8_t epnum = desc->endpoint[i].bEndpointAddress;
 		bool is_input = epnum & 0x80;
 		LOG_DEBUG("usb ep %s %02x", is_input ? "in" : "out", epnum);
@@ -118,20 +169,22 @@ int jtag_libusb_get_endpoints(struct jtag_libusb_device *udev,
 			*usb_read_ep = epnum;
 		else
 			*usb_write_ep = epnum;
+
+		if (*usb_read_ep && *usb_write_ep) {
+			LOG_DEBUG("Claiming interface %d", (int)desc->bInterfaceNumber);
+			usb_claim_interface(devh, (int)desc->bInterfaceNumber);
+			return ERROR_OK;
+		}
 	}
 
-	return 0;
+	return ERROR_FAIL;
 }
 
 int jtag_libusb_get_pid(struct jtag_libusb_device *dev, uint16_t *pid)
 {
-	struct libusb_device_descriptor dev_desc;
+	if (!dev)
+		return ERROR_FAIL;
 
-	if (libusb_get_device_descriptor(dev, &dev_desc) == 0) {
-		*pid = dev_desc.idProduct;
-
-		return 0;
-	}
-
-	return -ENODEV;
+	*pid = dev->descriptor.idProduct;
+	return ERROR_OK;
 }

src/jtag/drivers/libusb0_common.h

@@ -54,6 +54,7 @@ static inline int jtag_libusb_release_interface(jtag_libusb_device_handle *devh,
 }
 
 int jtag_libusb_open(const uint16_t vids[], const uint16_t pids[],
+		const char *serial,
 		struct jtag_libusb_device_handle **out);
 void jtag_libusb_close(jtag_libusb_device_handle *dev);
 int jtag_libusb_control_transfer(jtag_libusb_device_handle *dev,
@@ -65,9 +66,10 @@ int jtag_libusb_bulk_read(struct jtag_libusb_device_handle *dev, int ep,
 		char *bytes, int size, int timeout);
 int jtag_libusb_set_configuration(jtag_libusb_device_handle *devh,
 		int configuration);
-int jtag_libusb_get_endpoints(struct jtag_libusb_device *udev,
+int jtag_libusb_choose_interface(struct jtag_libusb_device_handle *devh,
 		unsigned int *usb_read_ep,
-		unsigned int *usb_write_ep);
+		unsigned int *usb_write_ep,
+		int bclass, int subclass, int protocol);
 int jtag_libusb_get_pid(struct jtag_libusb_device *dev, uint16_t *pid);
 
 #endif /* JTAG_USB_COMMON_H */

src/jtag/drivers/libusb1_common.c

@@ -28,25 +28,53 @@
 static struct libusb_context *jtag_libusb_context; /**< Libusb context **/
 static libusb_device **devs; /**< The usb device list **/
 
-static bool jtag_libusb_match(struct jtag_libusb_device *dev,
+static bool jtag_libusb_match(struct libusb_device_descriptor *dev_desc,
 		const uint16_t vids[], const uint16_t pids[])
 {
-	struct libusb_device_descriptor dev_desc;
-
 	for (unsigned i = 0; vids[i]; i++) {
-		if (libusb_get_device_descriptor(dev, &dev_desc) == 0) {
-			if (dev_desc.idVendor == vids[i] &&
-				dev_desc.idProduct == pids[i])
-				return true;
+		if (dev_desc->idVendor == vids[i] &&
+			dev_desc->idProduct == pids[i]) {
+			return true;
 		}
 	}
 	return false;
 }
 
+/* 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)
+{
+	int retval;
+	bool matched;
+	char desc_string[256+1]; /* Max size of string descriptor */
+
+	if (str_index == 0)
+		return false;
+
+	retval = libusb_get_string_descriptor_ascii(device, str_index,
+			(unsigned char *)desc_string, sizeof(desc_string)-1);
+	if (retval < 0) {
+		LOG_ERROR("libusb_get_string_descriptor_ascii() failed with %d", retval);
+		return false;
+	}
+
+	/* Null terminate descriptor string in case it needs to be logged. */
+	desc_string[sizeof(desc_string)-1] = '\0';
+
+	matched = strncmp(string, desc_string, sizeof(desc_string)) == 0;
+	if (!matched)
+		LOG_DEBUG("Device serial number '%s' doesn't match requested serial '%s'",
+			desc_string, string);
+	return matched;
+}
+
 int jtag_libusb_open(const uint16_t vids[], const uint16_t pids[],
+		const char *serial,
 		struct jtag_libusb_device_handle **out)
 {
 	int cnt, idx, errCode;
+	int retval = -ENODEV;
+	struct jtag_libusb_device_handle *libusb_handle = NULL;
 
 	if (libusb_init(&jtag_libusb_context) < 0)
 		return -ENODEV;
@@ -54,22 +82,37 @@ int jtag_libusb_open(const uint16_t vids[], const uint16_t pids[],
 	cnt = libusb_get_device_list(jtag_libusb_context, &devs);
 
 	for (idx = 0; idx < cnt; idx++) {
-		if (!jtag_libusb_match(devs[idx], vids, pids))
+		struct libusb_device_descriptor dev_desc;
+
+		if (libusb_get_device_descriptor(devs[idx], &dev_desc) != 0)
 			continue;
 
-		errCode = libusb_open(devs[idx], out);
+		if (!jtag_libusb_match(&dev_desc, vids, pids))
+			continue;
 
-		/** Free the device list **/
-		libusb_free_device_list(devs, 1);
+		errCode = libusb_open(devs[idx], &libusb_handle);
 
 		if (errCode) {
 			LOG_ERROR("libusb_open() failed with %s",
 				  libusb_error_name(errCode));
-			return errCode;
+			continue;
 		}
-		return 0;
+
+		/* Device must be open to use libusb_get_string_descriptor_ascii. */
+		if (serial != NULL &&
+				!string_descriptor_equal(libusb_handle, dev_desc.iSerialNumber, serial)) {
+			libusb_close(libusb_handle);
+			continue;
+		}
+
+		/* Success. */
+		*out = libusb_handle;
+		retval = 0;
+		break;
 	}
-	return -ENODEV;
+	if (cnt >= 0)
+		libusb_free_device_list(devs, 1);
+	return retval;
 }
 
 void jtag_libusb_close(jtag_libusb_device_handle *dev)
@@ -122,49 +165,75 @@ int jtag_libusb_set_configuration(jtag_libusb_device_handle *devh,
 	int retCode = -99;
 
 	struct libusb_config_descriptor *config = NULL;
+	int current_config = -1;
 
-	libusb_get_config_descriptor(udev, configuration, &config);
-	retCode = libusb_set_configuration(devh, config->bConfigurationValue);
+	retCode = libusb_get_configuration(devh, &current_config);
+	if (retCode != 0)
+		return retCode;
+
+	retCode = libusb_get_config_descriptor(udev, configuration, &config);
+	if (retCode != 0 || config == NULL)
+		return retCode;
+
+	/* Only change the configuration if it is not already set to the
+	   same one. Otherwise this issues a lightweight reset and hangs
+	   LPC-Link2 with JLink firmware. */
+	if (current_config != config->bConfigurationValue)
+		retCode = libusb_set_configuration(devh, config->bConfigurationValue);
 
 	libusb_free_config_descriptor(config);
 
 	return retCode;
 }
 
-int jtag_libusb_get_endpoints(struct jtag_libusb_device *udev,
+int jtag_libusb_choose_interface(struct jtag_libusb_device_handle *devh,
 		unsigned int *usb_read_ep,
-		unsigned int *usb_write_ep)
+		unsigned int *usb_write_ep,
+		int bclass, int subclass, int protocol)
 {
+	struct jtag_libusb_device *udev = jtag_libusb_get_device(devh);
 	const struct libusb_interface *inter;
 	const struct libusb_interface_descriptor *interdesc;
 	const struct libusb_endpoint_descriptor *epdesc;
 	struct libusb_config_descriptor *config;
 
+	*usb_read_ep = *usb_write_ep = 0;
+
 	libusb_get_config_descriptor(udev, 0, &config);
 	for (int i = 0; i < (int)config->bNumInterfaces; i++) {
 		inter = &config->interface[i];
 
-		for (int j = 0; j < inter->num_altsetting; j++) {
-			interdesc = &inter->altsetting[j];
-			for (int k = 0;
-				k < (int)interdesc->bNumEndpoints; k++) {
-				epdesc = &interdesc->endpoint[k];
+		interdesc = &inter->altsetting[0];
+		for (int k = 0;
+		     k < (int)interdesc->bNumEndpoints; k++) {
+			if ((bclass > 0 && interdesc->bInterfaceClass != bclass) ||
+			    (subclass > 0 && interdesc->bInterfaceSubClass != subclass) ||
+			    (protocol > 0 && interdesc->bInterfaceProtocol != protocol))
+				continue;
 
-				uint8_t epnum = epdesc->bEndpointAddress;
-				bool is_input = epnum & 0x80;
-				LOG_DEBUG("usb ep %s %02x",
-					is_input ? "in" : "out", epnum);
+			epdesc = &interdesc->endpoint[k];
 
-				if (is_input)
-					*usb_read_ep = epnum;
-				else
-					*usb_write_ep = epnum;
+			uint8_t epnum = epdesc->bEndpointAddress;
+			bool is_input = epnum & 0x80;
+			LOG_DEBUG("usb ep %s %02x",
+				  is_input ? "in" : "out", epnum);
+
+			if (is_input)
+				*usb_read_ep = epnum;
+			else
+				*usb_write_ep = epnum;
+
+			if (*usb_read_ep && *usb_write_ep) {
+				LOG_DEBUG("Claiming interface %d", (int)interdesc->bInterfaceNumber);
+				libusb_claim_interface(devh, (int)interdesc->bInterfaceNumber);
+				libusb_free_config_descriptor(config);
+				return ERROR_OK;
 			}
 		}
 	}
 	libusb_free_config_descriptor(config);
 
-	return 0;
+	return ERROR_FAIL;
 }
 
 int jtag_libusb_get_pid(struct jtag_libusb_device *dev, uint16_t *pid)
@@ -174,8 +243,8 @@ int jtag_libusb_get_pid(struct jtag_libusb_device *dev, uint16_t *pid)
 	if (libusb_get_device_descriptor(dev, &dev_desc) == 0) {
 		*pid = dev_desc.idProduct;
 
-		return 0;
+		return ERROR_OK;
 	}
 
-	return -ENODEV;
+	return ERROR_FAIL;
 }

src/jtag/drivers/libusb1_common.h

@@ -48,6 +48,7 @@ static inline int jtag_libusb_release_interface(jtag_libusb_device_handle *devh,
 }
 
 int jtag_libusb_open(const uint16_t vids[], const uint16_t pids[],
+		const char *serial,
 		struct jtag_libusb_device_handle **out);
 void jtag_libusb_close(jtag_libusb_device_handle *dev);
 int jtag_libusb_control_transfer(jtag_libusb_device_handle *dev,
@@ -59,9 +60,23 @@ int jtag_libusb_bulk_read(struct jtag_libusb_device_handle *dev, int ep,
 		char *bytes, int size, int timeout);
 int jtag_libusb_set_configuration(jtag_libusb_device_handle *devh,
 		int configuration);
-int jtag_libusb_get_endpoints(struct jtag_libusb_device *udev,
+/**
+ * Find the first interface optionally matching class, subclass and
+ * protocol and claim it.
+ * @param devh _libusb_ device handle.
+ * @param usb_read_ep A pointer to a variable where the _IN_ endpoint
+ *	number will be stored.
+ * @param usb_write_ep A pointer to a variable where the _OUT_ endpoint
+ *	number will be stored.
+ * @param bclass `bInterfaceClass` to match, or -1 to ignore this field.
+ * @param subclass `bInterfaceSubClass` to match, or -1 to ignore this field.
+ * @param protocol `bInterfaceProtocol` to match, or -1 to ignore this field.
+ * @returns Returns ERROR_OK on success, ERROR_FAIL otherwise.
+ */
+int jtag_libusb_choose_interface(struct jtag_libusb_device_handle *devh,
 		unsigned int *usb_read_ep,
-		unsigned int *usb_write_ep);
+		unsigned int *usb_write_ep,
+		int bclass, int subclass, int protocol);
 int jtag_libusb_get_pid(struct jtag_libusb_device *dev, uint16_t *pid);
 
 #endif /* JTAG_USB_COMMON_H */

src/jtag/drivers/mpsse.c

@@ -98,7 +98,7 @@ static bool string_descriptor_equal(libusb_device_handle *device, uint8_t str_in
 	retval = libusb_get_string_descriptor_ascii(device, str_index, (unsigned char *)desc_string,
 			sizeof(desc_string));
 	if (retval < 0) {
-		LOG_ERROR("libusb_get_string_descriptor_ascii() failed with %d", retval);
+		LOG_ERROR("libusb_get_string_descriptor_ascii() failed with %s", libusb_error_name(retval));
 		return false;
 	}
 	return strncmp(string, desc_string, sizeof(desc_string)) == 0;
@@ -118,14 +118,14 @@ static bool open_matching_device(struct mpsse_ctx *ctx, const uint16_t *vid, con
 	bool found = false;
 	ssize_t cnt = libusb_get_device_list(ctx->usb_ctx, &list);
 	if (cnt < 0)
-		LOG_ERROR("libusb_get_device_list() failed with %zi", cnt);
+		LOG_ERROR("libusb_get_device_list() failed with %s", libusb_error_name(cnt));
 
 	for (ssize_t i = 0; i < cnt; i++) {
 		libusb_device *device = list[i];
 
 		err = libusb_get_device_descriptor(device, &desc);
 		if (err != LIBUSB_SUCCESS) {
-			LOG_ERROR("libusb_get_device_descriptor() failed with %d", err);
+			LOG_ERROR("libusb_get_device_descriptor() failed with %s", libusb_error_name(err));
 			continue;
 		}
 
@@ -164,7 +164,7 @@ static bool open_matching_device(struct mpsse_ctx *ctx, const uint16_t *vid, con
 
 	err = libusb_get_config_descriptor(libusb_get_device(ctx->usb_dev), 0, &config0);
 	if (err != LIBUSB_SUCCESS) {
-		LOG_ERROR("libusb_get_config_descriptor() failed with %d", err);
+		LOG_ERROR("libusb_get_config_descriptor() failed with %s", libusb_error_name(err));
 		libusb_close(ctx->usb_dev);
 		return false;
 	}
@@ -173,14 +173,14 @@ static bool open_matching_device(struct mpsse_ctx *ctx, const uint16_t *vid, con
 	int cfg;
 	err = libusb_get_configuration(ctx->usb_dev, &cfg);
 	if (err != LIBUSB_SUCCESS) {
-		LOG_ERROR("libusb_get_configuration() failed with %d", err);
+		LOG_ERROR("libusb_get_configuration() failed with %s", libusb_error_name(err));
 		goto error;
 	}
 
 	if (desc.bNumConfigurations > 0 && cfg != config0->bConfigurationValue) {
 		err = libusb_set_configuration(ctx->usb_dev, config0->bConfigurationValue);
 		if (err != LIBUSB_SUCCESS) {
-			LOG_ERROR("libusb_set_configuration() failed with %d", err);
+			LOG_ERROR("libusb_set_configuration() failed with %s", libusb_error_name(err));
 			goto error;
 		}
 	}
@@ -189,13 +189,13 @@ static bool open_matching_device(struct mpsse_ctx *ctx, const uint16_t *vid, con
 	err = libusb_detach_kernel_driver(ctx->usb_dev, ctx->interface);
 	if (err != LIBUSB_SUCCESS && err != LIBUSB_ERROR_NOT_FOUND
 			&& err != LIBUSB_ERROR_NOT_SUPPORTED) {
-		LOG_ERROR("libusb_detach_kernel_driver() failed with %d", err);
+		LOG_ERROR("libusb_detach_kernel_driver() failed with %s", libusb_error_name(err));
 		goto error;
 	}
 
 	err = libusb_claim_interface(ctx->usb_dev, ctx->interface);
 	if (err != LIBUSB_SUCCESS) {
-		LOG_ERROR("libusb_claim_interface() failed with %d", err);
+		LOG_ERROR("libusb_claim_interface() failed with %s", libusb_error_name(err));
 		goto error;
 	}
 
@@ -204,7 +204,7 @@ static bool open_matching_device(struct mpsse_ctx *ctx, const uint16_t *vid, con
 			SIO_RESET_REQUEST, SIO_RESET_SIO,
 			ctx->index, NULL, 0, ctx->usb_write_timeout);
 	if (err < 0) {
-		LOG_ERROR("failed to reset FTDI device: %d", err);
+		LOG_ERROR("failed to reset FTDI device: %s", libusb_error_name(err));
 		goto error;
 	}
 
@@ -288,7 +288,7 @@ struct mpsse_ctx *mpsse_open(const uint16_t *vid, const uint16_t *pid, const cha
 
 	err = libusb_init(&ctx->usb_ctx);
 	if (err != LIBUSB_SUCCESS) {
-		LOG_ERROR("libusb_init() failed with %d", err);
+		LOG_ERROR("libusb_init() failed with %s", libusb_error_name(err));
 		goto error;
 	}
 
@@ -310,7 +310,7 @@ struct mpsse_ctx *mpsse_open(const uint16_t *vid, const uint16_t *pid, const cha
 			SIO_SET_LATENCY_TIMER_REQUEST, 255, ctx->index, NULL, 0,
 			ctx->usb_write_timeout);
 	if (err < 0) {
-		LOG_ERROR("unable to set latency timer: %d", err);
+		LOG_ERROR("unable to set latency timer: %s", libusb_error_name(err));
 		goto error;
 	}
 
@@ -323,7 +323,7 @@ struct mpsse_ctx *mpsse_open(const uint16_t *vid, const uint16_t *pid, const cha
 			0,
 			ctx->usb_write_timeout);
 	if (err < 0) {
-		LOG_ERROR("unable to set MPSSE bitmode: %d", err);
+		LOG_ERROR("unable to set MPSSE bitmode: %s", libusb_error_name(err));
 		goto error;
 	}
 
@@ -368,14 +368,14 @@ void mpsse_purge(struct mpsse_ctx *ctx)
 	err = libusb_control_transfer(ctx->usb_dev, FTDI_DEVICE_OUT_REQTYPE, SIO_RESET_REQUEST,
 			SIO_RESET_PURGE_RX, ctx->index, NULL, 0, ctx->usb_write_timeout);
 	if (err < 0) {
-		LOG_ERROR("unable to purge ftdi rx buffers: %d", err);
+		LOG_ERROR("unable to purge ftdi rx buffers: %s", libusb_error_name(err));
 		return;
 	}
 
 	err = libusb_control_transfer(ctx->usb_dev, FTDI_DEVICE_OUT_REQTYPE, SIO_RESET_REQUEST,
 			SIO_RESET_PURGE_TX, ctx->index, NULL, 0, ctx->usb_write_timeout);
 	if (err < 0) {
-		LOG_ERROR("unable to purge ftdi tx buffers: %d", err);
+		LOG_ERROR("unable to purge ftdi tx buffers: %s", libusb_error_name(err));
 		return;
 	}
 }
@@ -837,7 +837,7 @@ int mpsse_flush(struct mpsse_ctx *ctx)
 	}
 
 	if (retval != LIBUSB_SUCCESS) {
-		LOG_ERROR("libusb_handle_events() failed with %d", retval);
+		LOG_ERROR("libusb_handle_events() failed with %s", libusb_error_name(retval));
 		retval = ERROR_FAIL;
 	} else if (write_result.transferred < ctx->write_count) {
 		LOG_ERROR("ftdi device did not accept all data: %d, tried %d",

src/jtag/drivers/opendous.c

@@ -36,13 +36,12 @@
 #include <jtag/commands.h>
 #include "libusb_common.h"
 #include <string.h>
-#include <sys/timeb.h>
 #include <time.h>
 
 #define OPENDOUS_MAX_VIDS_PIDS 4
 /* define some probes with similar interface */
 struct opendous_probe {
-	char *name;
+	const char *name;
 	uint16_t VID[OPENDOUS_MAX_VIDS_PIDS];
 	uint16_t PID[OPENDOUS_MAX_VIDS_PIDS];
 	uint8_t READ_EP;
@@ -51,7 +50,7 @@ struct opendous_probe {
 	int BUFFERSIZE;
 };
 
-static struct opendous_probe opendous_probes[] = {
+static const struct opendous_probe opendous_probes[] = {
 	{"usbprog-jtag",	{0x1781, 0},			{0x0C63, 0},			0x82, 0x02, 0x00, 510 },
 	{"opendous",		{0x1781, 0x03EB, 0},	{0xC0C0, 0x204F, 0},	0x81, 0x02, 0x00, 360 },
 	{"usbvlab",			{0x16C0, 0},			{0x05DC, 0},			0x81, 0x02, 0x01, 360 },
@@ -110,7 +109,7 @@ static struct pending_scan_result *pending_scan_results_buffer;
 #define FUNC_READ_DATA        0x51
 
 static char *opendous_type;
-static struct opendous_probe *opendous_probe;
+static const struct opendous_probe *opendous_probe;
 
 /* External interface functions */
 static int opendous_execute_queue(void);
@@ -150,9 +149,7 @@ static int opendous_usb_read(struct opendous_jtag *opendous_jtag);
 int opendous_get_version_info(void);
 
 #ifdef _DEBUG_USB_COMMS_
-char time_str[50];
 static void opendous_debug_buffer(uint8_t *buffer, int length);
-char *opendous_get_time(char *);
 #endif
 
 static struct opendous_jtag *opendous_jtag_handle;
@@ -324,7 +321,7 @@ static int opendous_execute_queue(void)
 static int opendous_init(void)
 {
 	int check_cnt;
-	struct opendous_probe *cur_opendous_probe;
+	const struct opendous_probe *cur_opendous_probe;
 
 	cur_opendous_probe = opendous_probes;
 
@@ -712,7 +709,7 @@ struct opendous_jtag *opendous_usb_open(void)
 	struct opendous_jtag *result;
 
 	struct jtag_libusb_device_handle *devh;
-	if (jtag_libusb_open(opendous_probe->VID, opendous_probe->PID, &devh) != ERROR_OK)
+	if (jtag_libusb_open(opendous_probe->VID, opendous_probe->PID, NULL, &devh) != ERROR_OK)
 		return NULL;
 
 	jtag_libusb_set_configuration(devh, 0);
@@ -760,7 +757,7 @@ int opendous_usb_write(struct opendous_jtag *opendous_jtag, int out_length)
 	}
 
 #ifdef _DEBUG_USB_COMMS_
-	LOG_DEBUG("%s: USB write begin", opendous_get_time(time_str));
+	LOG_DEBUG("USB write begin");
 #endif
 	if (opendous_probe->CONTROL_TRANSFER) {
 		result = jtag_libusb_control_transfer(opendous_jtag->usb_handle,
@@ -771,7 +768,7 @@ int opendous_usb_write(struct opendous_jtag *opendous_jtag, int out_length)
 			(char *)usb_out_buffer, out_length, OPENDOUS_USB_TIMEOUT);
 	}
 #ifdef _DEBUG_USB_COMMS_
-	LOG_DEBUG("%s: USB write end: %d bytes", opendous_get_time(time_str), result);
+	LOG_DEBUG("USB write end: %d bytes", result);
 #endif
 
 	DEBUG_JTAG_IO("opendous_usb_write, out_length = %d, result = %d", out_length, result);
@@ -786,7 +783,7 @@ int opendous_usb_write(struct opendous_jtag *opendous_jtag, int out_length)
 int opendous_usb_read(struct opendous_jtag *opendous_jtag)
 {
 #ifdef _DEBUG_USB_COMMS_
-	LOG_DEBUG("%s: USB read begin", opendous_get_time(time_str));
+	LOG_DEBUG("USB read begin");
 #endif
 	int result;
 	if (opendous_probe->CONTROL_TRANSFER) {
@@ -798,7 +795,7 @@ int opendous_usb_read(struct opendous_jtag *opendous_jtag)
 			(char *)usb_in_buffer, OPENDOUS_IN_BUFFER_SIZE, OPENDOUS_USB_TIMEOUT);
 	}
 #ifdef _DEBUG_USB_COMMS_
-	LOG_DEBUG("%s: USB read end: %d bytes", opendous_get_time(time_str), result);
+	LOG_DEBUG("USB read end: %d bytes", result);
 #endif
 	DEBUG_JTAG_IO("opendous_usb_read, result = %d", result);
 
@@ -827,15 +824,4 @@ void opendous_debug_buffer(uint8_t *buffer, int length)
 		LOG_DEBUG("%s", line);
 	}
 }
-
-char *opendous_get_time(char *str)
-{
-	struct timeb timebuffer;
-	char *timeline;
-
-	ftime(&timebuffer);
-	timeline = ctime(&(timebuffer.time));
-	snprintf(str, 49, "%.8s.%hu", &timeline[11], timebuffer.millitm);
-	return str;
-}
 #endif

src/jtag/drivers/osbdm.c

@@ -107,7 +107,7 @@ static struct queue *queue_alloc(void)
 	return queue;
 }
 
-/* Size of usb communnication buffer */
+/* Size of usb communication buffer */
 #define OSBDM_USB_BUFSIZE 64
 /* Timeout for USB transfer, ms */
 #define OSBDM_USB_TIMEOUT 1000
@@ -150,7 +150,7 @@ static int osbdm_send_and_recv(struct osbdm *osbdm)
 		(char *)osbdm->buffer, osbdm->count, OSBDM_USB_TIMEOUT);
 
 	if (count != osbdm->count) {
-		LOG_ERROR("OSBDM communnication error: can't write");
+		LOG_ERROR("OSBDM communication error: can't write");
 		return ERROR_FAIL;
 	}
 
@@ -165,22 +165,22 @@ static int osbdm_send_and_recv(struct osbdm *osbdm)
 	 */
 
 	if (osbdm->count < 0) {
-		LOG_ERROR("OSBDM communnication error: can't read");
+		LOG_ERROR("OSBDM communication error: can't read");
 		return ERROR_FAIL;
 	}
 
 	if (osbdm->count < 2) {
-		LOG_ERROR("OSBDM communnication error: answer too small");
+		LOG_ERROR("OSBDM communication error: reply too small");
 		return ERROR_FAIL;
 	}
 
 	if (osbdm->count != osbdm->buffer[1])  {
-		LOG_ERROR("OSBDM communnication error: answer size mismatch");
+		LOG_ERROR("OSBDM communication error: reply size mismatch");
 		return ERROR_FAIL;
 	}
 
 	if (cmd_saved != osbdm->buffer[0]) {
-		LOG_ERROR("OSBDM communnication error: answer command mismatch");
+		LOG_ERROR("OSBDM communication error: reply command mismatch");
 		return ERROR_FAIL;
 	}
 
@@ -219,7 +219,7 @@ static int osbdm_swap(struct osbdm *osbdm, void *tms, void *tdi,
 	}
 
 	if (length <= 0) {
-		LOG_ERROR("BUG: bit sequence equal or less to 0");
+		LOG_ERROR("BUG: bit sequence equal or less than 0");
 		return ERROR_FAIL;
 	}
 
@@ -271,7 +271,7 @@ static int osbdm_swap(struct osbdm *osbdm, void *tms, void *tdi,
 	/*	Extra check
 	 */
 	if (((osbdm->buffer[2] << 8) | osbdm->buffer[3]) != 2 * swap_count) {
-		LOG_ERROR("OSBDM communnication error: not proper answer to swap command");
+		LOG_ERROR("OSBDM communication error: invalid swap command reply");
 		return ERROR_FAIL;
 	}
 
@@ -375,7 +375,7 @@ static int osbdm_flush(struct osbdm *osbdm, struct queue* queue)
 static int osbdm_open(struct osbdm *osbdm)
 {
 	(void)memset(osbdm, 0, sizeof(*osbdm));
-	if (jtag_libusb_open(osbdm_vid, osbdm_pid, &osbdm->devh) != ERROR_OK)
+	if (jtag_libusb_open(osbdm_vid, osbdm_pid, NULL, &osbdm->devh) != ERROR_OK)
 		return ERROR_FAIL;
 
 	if (jtag_libusb_claim_interface(osbdm->devh, 0) != ERROR_OK)
@@ -678,7 +678,7 @@ static int osbdm_init(void)
 		return ERROR_FAIL;
 	} else {
 		/* Device successfully opened */
-		LOG_INFO("OSBDM has opened");
+		LOG_DEBUG("OSBDM init");
 	}
 
 	/* Perform initialize command */

src/jtag/drivers/parport.c

@@ -60,7 +60,7 @@
 /* parallel port cable description
  */
 struct cable {
-	char *name;
+	const char *name;
 	uint8_t TDO_MASK;	/* status port bit containing current TDO value */
 	uint8_t TRST_MASK;	/* data port bit for TRST */
 	uint8_t TMS_MASK;	/* data port bit for TMS */
@@ -74,7 +74,7 @@ struct cable {
 	uint8_t LED_MASK;	/* data port bit for LED */
 };
 
-static struct cable cables[] = {
+static const struct cable cables[] = {
 	/* name				tdo   trst  tms   tck   tdi   srst  o_inv i_inv init  exit  led */
 	{ "wiggler",			0x80, 0x10, 0x02, 0x04, 0x08, 0x01, 0x01, 0x80, 0x80, 0x80, 0x00 },
 	{ "wiggler2",			0x80, 0x10, 0x02, 0x04, 0x08, 0x01, 0x01, 0x80, 0x80, 0x00, 0x20 },
@@ -108,7 +108,7 @@ static int wait_states;
 
 /* interface variables
  */
-static struct cable *cable;
+static const struct cable *cable;
 static uint8_t dataport_value;
 
 #if PARPORT_USE_PPDEV == 1
@@ -262,7 +262,7 @@ static struct bitbang_interface parport_bitbang = {
 
 static int parport_init(void)
 {
-	struct cable *cur_cable;
+	const struct cable *cur_cable;
 #if PARPORT_USE_PPDEV == 1
 	char buffer[256];
 #endif

src/jtag/drivers/stlink_usb.c

@@ -66,6 +66,11 @@
  * 8bit read/writes to max 64 bytes. */
 #define STLINK_MAX_RW8		(64)
 
+/* "WAIT" responses will be retried (with exponential backoff) at
+ * most this many times before failing to caller.
+ */
+#define MAX_WAIT_RETRIES 8
+
 enum stlink_jtag_api_version {
 	STLINK_JTAG_API_V1 = 1,
 	STLINK_JTAG_API_V2,
@@ -119,18 +124,19 @@ struct stlink_usb_handle_s {
 	struct {
 		/** whether SWO tracing is enabled or not */
 		bool enabled;
-		/** trace data destination file */
-		FILE *output_f;
-		/** trace module source clock (for prescaler) */
+		/** trace module source clock */
 		uint32_t source_hz;
-		/** trace module clock prescaler */
-		uint32_t prescale;
 	} trace;
+	/** reconnect is needed next time we try to query the
+	 * status */
+	bool reconnect_pending;
 };
 
 #define STLINK_DEBUG_ERR_OK            0x80
 #define STLINK_DEBUG_ERR_FAULT         0x81
 #define STLINK_SWD_AP_WAIT             0x10
+#define STLINK_JTAG_WRITE_ERROR        0x0c
+#define STLINK_JTAG_WRITE_VERIF_ERROR  0x0d
 #define STLINK_SWD_DP_WAIT             0x14
 
 #define STLINK_CORE_RUNNING            0x80
@@ -196,6 +202,7 @@ struct stlink_usb_handle_s {
 #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_DRIVE_NRST_LOW   0x00
 #define STLINK_DEBUG_APIV2_DRIVE_NRST_HIGH  0x01
@@ -218,6 +225,24 @@ enum stlink_mode {
 #define REQUEST_SENSE        0x03
 #define REQUEST_SENSE_LENGTH 18
 
+static const struct {
+	int speed;
+	int speed_divisor;
+} stlink_khz_to_speed_map[] = {
+	{4000, 0},
+	{1800, 1}, /* default */
+	{1200, 2},
+	{950,  3},
+	{480,  7},
+	{240, 15},
+	{125, 31},
+	{100, 40},
+	{50,  79},
+	{25, 158},
+	{15, 265},
+	{5,  798}
+};
+
 static void stlink_usb_init_buffer(void *handle, uint8_t direction, uint32_t size);
 
 /** */
@@ -340,6 +365,70 @@ static int stlink_usb_xfer(void *handle, const uint8_t *buf, int size)
 	return ERROR_OK;
 }
 
+
+/**
+    Converts an STLINK status code held in the first byte of a response
+    to an openocd error, logs any error/wait status as debug output.
+*/
+static int stlink_usb_error_check(void *handle)
+{
+	struct stlink_usb_handle_s *h = handle;
+
+	assert(handle != NULL);
+
+	/* TODO: no error checking yet on api V1 */
+	if (h->jtag_api == STLINK_JTAG_API_V1)
+		h->databuf[0] = STLINK_DEBUG_ERR_OK;
+
+	switch (h->databuf[0]) {
+		case STLINK_DEBUG_ERR_OK:
+			return ERROR_OK;
+		case STLINK_DEBUG_ERR_FAULT:
+			LOG_DEBUG("SWD fault response (0x%x)", STLINK_DEBUG_ERR_FAULT);
+			return ERROR_FAIL;
+		case STLINK_SWD_AP_WAIT:
+			LOG_DEBUG("wait status SWD_AP_WAIT (0x%x)", STLINK_SWD_AP_WAIT);
+			return ERROR_WAIT;
+		case STLINK_SWD_DP_WAIT:
+			LOG_DEBUG("wait status SWD_DP_WAIT (0x%x)", STLINK_SWD_AP_WAIT);
+			return ERROR_WAIT;
+		case STLINK_JTAG_WRITE_ERROR:
+			LOG_DEBUG("Write error");
+			return ERROR_FAIL;
+		case STLINK_JTAG_WRITE_VERIF_ERROR:
+			LOG_DEBUG("Verify error");
+			return ERROR_FAIL;
+		default:
+			LOG_DEBUG("unknown/unexpected STLINK status code 0x%x", h->databuf[0]);
+			return ERROR_FAIL;
+	}
+}
+
+
+/** Issue an STLINK command via USB transfer, with retries on any wait status responses.
+
+    Works for commands where the STLINK_DEBUG status is returned in the first
+    byte of the response packet.
+
+    Returns an openocd result code.
+*/
+static int stlink_cmd_allow_retry(void *handle, const uint8_t *buf, int size)
+{
+	int retries = 0;
+	int res;
+	while (1) {
+		res = stlink_usb_xfer(handle, buf, size);
+		if (res != ERROR_OK)
+			return res;
+		res = stlink_usb_error_check(handle);
+		if (res == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
+			usleep((1<<retries++) * 1000);
+			continue;
+		}
+		return res;
+	}
+}
+
 /** */
 static int stlink_usb_read_trace(void *handle, const uint8_t *buf, int size)
 {
@@ -391,40 +480,6 @@ static void stlink_usb_init_buffer(void *handle, uint8_t direction, uint32_t siz
 		stlink_usb_xfer_v1_create_cmd(handle, direction, size);
 }
 
-static const char * const stlink_usb_error_msg[] = {
-	"unknown"
-};
-
-/** */
-static int stlink_usb_error_check(void *handle)
-{
-	int res;
-	const char *err_msg = 0;
-	struct stlink_usb_handle_s *h = handle;
-
-	assert(handle != NULL);
-
-	/* TODO: no error checking yet on api V1 */
-	if (h->jtag_api == STLINK_JTAG_API_V1)
-		h->databuf[0] = STLINK_DEBUG_ERR_OK;
-
-	switch (h->databuf[0]) {
-		case STLINK_DEBUG_ERR_OK:
-			res = ERROR_OK;
-			break;
-		case STLINK_DEBUG_ERR_FAULT:
-		default:
-			err_msg = stlink_usb_error_msg[0];
-			res = ERROR_FAIL;
-			break;
-	}
-
-	if (res != ERROR_OK)
-		LOG_DEBUG("status error: %d ('%s')", h->databuf[0], err_msg);
-
-	return res;
-}
-
 /** */
 static int stlink_usb_version(void *handle)
 {
@@ -502,6 +557,31 @@ static int stlink_usb_check_voltage(void *handle, float *target_voltage)
 	return ERROR_OK;
 }
 
+static int stlink_usb_set_swdclk(void *handle, uint16_t clk_divisor)
+{
+	struct stlink_usb_handle_s *h = handle;
+
+	assert(handle != NULL);
+
+	/* only supported by stlink/v2 and for firmware >= 22 */
+	if (h->version.stlink == 1 || h->version.jtag < 22)
+		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_SWD_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)
 {
@@ -527,7 +607,6 @@ static int stlink_usb_current_mode(void *handle, uint8_t *mode)
 /** */
 static int stlink_usb_mode_enter(void *handle, enum stlink_mode type)
 {
-	int res;
 	int rx_size = 0;
 	struct stlink_usb_handle_s *h = handle;
 
@@ -569,14 +648,7 @@ static int stlink_usb_mode_enter(void *handle, enum stlink_mode type)
 			return ERROR_FAIL;
 	}
 
-	res = stlink_usb_xfer(handle, h->databuf, rx_size);
-
-	if (res != ERROR_OK)
-		return res;
-
-	res = stlink_usb_error_check(h);
-
-	return res;
+	return stlink_cmd_allow_retry(handle, h->databuf, rx_size);
 }
 
 /** */
@@ -618,6 +690,20 @@ static int stlink_usb_mode_leave(void *handle, enum stlink_mode type)
 
 static int stlink_usb_assert_srst(void *handle, int srst);
 
+static enum stlink_mode stlink_get_mode(enum hl_transports t)
+{
+	switch (t) {
+	case HL_TRANSPORT_SWD:
+		return STLINK_MODE_DEBUG_SWD;
+	case HL_TRANSPORT_JTAG:
+		return STLINK_MODE_DEBUG_JTAG;
+	case HL_TRANSPORT_SWIM:
+		return STLINK_MODE_DEBUG_SWIM;
+	default:
+		return STLINK_MODE_UNKNOWN;
+	}
+}
+
 /** */
 static int stlink_usb_init_mode(void *handle, bool connect_under_reset)
 {
@@ -691,20 +777,7 @@ static int stlink_usb_init_mode(void *handle, bool connect_under_reset)
 	LOG_DEBUG("MODE: 0x%02X", mode);
 
 	/* set selected mode */
-	switch (h->transport) {
-		case HL_TRANSPORT_SWD:
-			emode = STLINK_MODE_DEBUG_SWD;
-			break;
-		case HL_TRANSPORT_JTAG:
-			emode = STLINK_MODE_DEBUG_JTAG;
-			break;
-		case HL_TRANSPORT_SWIM:
-			emode = STLINK_MODE_DEBUG_SWIM;
-			break;
-		default:
-			emode = STLINK_MODE_UNKNOWN;
-			break;
-	}
+	emode = stlink_get_mode(h->transport);
 
 	if (emode == STLINK_MODE_UNKNOWN) {
 		LOG_ERROR("selected mode (transport) not supported");
@@ -771,19 +844,16 @@ static int stlink_usb_v2_read_debug_reg(void *handle, uint32_t addr, uint32_t *v
 	h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
 	h->cmdidx += 4;
 
-	res = stlink_usb_xfer(handle, h->databuf, 8);
-
+	res = stlink_cmd_allow_retry(handle, h->databuf, 8);
 	if (res != ERROR_OK)
 		return res;
 
 	*val = le_to_h_u32(h->databuf + 4);
-
-	return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
+	return ERROR_OK;
 }
 
 static int stlink_usb_write_debug_reg(void *handle, uint32_t addr, uint32_t val)
 {
-	int res;
 	struct stlink_usb_handle_s *h = handle;
 
 	assert(handle != NULL);
@@ -800,16 +870,11 @@ static int stlink_usb_write_debug_reg(void *handle, uint32_t addr, uint32_t val)
 	h_u32_to_le(h->cmdbuf+h->cmdidx, val);
 	h->cmdidx += 4;
 
-	res = stlink_usb_xfer(handle, h->databuf, 2);
-
-	if (res != ERROR_OK)
-		return res;
-
-	return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
+	return stlink_cmd_allow_retry(handle, h->databuf, 2);
 }
 
 /** */
-static void stlink_usb_trace_read(void *handle)
+static int stlink_usb_trace_read(void *handle, uint8_t *buf, size_t *size)
 {
 	struct stlink_usb_handle_s *h = handle;
 
@@ -824,29 +889,20 @@ static void stlink_usb_trace_read(void *handle)
 		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GET_TRACE_NB;
 
 		res = stlink_usb_xfer(handle, h->databuf, 2);
-		if (res == ERROR_OK) {
-			uint8_t buf[STLINK_TRACE_SIZE];
-			size_t size = le_to_h_u16(h->databuf);
+		if (res != ERROR_OK)
+			return res;
 
-			if (size > 0) {
-				size = size < sizeof(buf) ? size : sizeof(buf) - 1;
+		size_t bytes_avail = le_to_h_u16(h->databuf);
+		*size = bytes_avail < *size ? bytes_avail : *size - 1;
 
-				res = stlink_usb_read_trace(handle, buf, size);
-				if (res == ERROR_OK) {
-					if (h->trace.output_f) {
-						/* Log retrieved trace output */
-						if (fwrite(buf, 1, size, h->trace.output_f) > 0)
-							fflush(h->trace.output_f);
-					}
-				}
-			}
+		if (*size > 0) {
+			res = stlink_usb_read_trace(handle, buf, *size);
+			if (res != ERROR_OK)
+				return res;
+			return ERROR_OK;
 		}
 	}
-}
-
-static int stlink_usb_trace_read_callback(void *handle)
-{
-	stlink_usb_trace_read(handle);
+	*size = 0;
 	return ERROR_OK;
 }
 
@@ -864,8 +920,6 @@ static enum target_state stlink_usb_v2_get_status(void *handle)
 	else if (status & S_RESET_ST)
 		return TARGET_RESET;
 
-	stlink_usb_trace_read(handle);
-
 	return TARGET_RUNNING;
 }
 
@@ -877,8 +931,22 @@ static enum target_state stlink_usb_state(void *handle)
 
 	assert(handle != NULL);
 
-	if (h->jtag_api == STLINK_JTAG_API_V2)
-		return stlink_usb_v2_get_status(handle);
+	if (h->reconnect_pending) {
+		LOG_INFO("Previous state query failed, trying to reconnect");
+		res = stlink_usb_mode_enter(handle, stlink_get_mode(h->transport));
+
+		if (res != ERROR_OK)
+			return TARGET_UNKNOWN;
+
+		h->reconnect_pending = false;
+	}
+
+	if (h->jtag_api == STLINK_JTAG_API_V2) {
+		res = stlink_usb_v2_get_status(handle);
+		if (res == TARGET_UNKNOWN)
+			h->reconnect_pending = true;
+		return res;
+	}
 
 	stlink_usb_init_buffer(handle, h->rx_ep, 2);
 
@@ -895,48 +963,18 @@ static enum target_state stlink_usb_state(void *handle)
 	if (h->databuf[0] == STLINK_CORE_HALTED)
 		return TARGET_HALTED;
 
+	h->reconnect_pending = true;
+
 	return TARGET_UNKNOWN;
 }
 
-/** */
-static int stlink_usb_reset(void *handle)
-{
-	int res;
-	struct stlink_usb_handle_s *h = handle;
-
-	assert(handle != NULL);
-
-	stlink_usb_init_buffer(handle, h->rx_ep, 2);
-
-	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
-
-	if (h->jtag_api == STLINK_JTAG_API_V1)
-		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_RESETSYS;
-	else
-		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_RESETSYS;
-
-	res = stlink_usb_xfer(handle, h->databuf, 2);
-
-	if (res != ERROR_OK)
-		return res;
-
-	LOG_DEBUG("RESET: 0x%08X", h->databuf[0]);
-
-	/* the following is not a error under swd (using hardware srst), so return success */
-	if (h->databuf[0] == STLINK_SWD_AP_WAIT || h->databuf[0] == STLINK_SWD_DP_WAIT)
-		return ERROR_OK;
-
-	return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
-}
-
 static int stlink_usb_assert_srst(void *handle, int srst)
 {
-	int res;
 	struct stlink_usb_handle_s *h = handle;
 
 	assert(handle != NULL);
 
-	if (h->jtag_api == STLINK_JTAG_API_V1)
+	if (h->version.stlink == 1)
 		return ERROR_COMMAND_NOTFOUND;
 
 	stlink_usb_init_buffer(handle, h->rx_ep, 2);
@@ -945,68 +983,7 @@ static int stlink_usb_assert_srst(void *handle, int srst)
 	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_DRIVE_NRST;
 	h->cmdbuf[h->cmdidx++] = srst;
 
-	res = stlink_usb_xfer(handle, h->databuf, 2);
-
-	if (res != ERROR_OK)
-		return res;
-
-	return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
-}
-
-/** */
-static int stlink_configure_target_trace_port(void *handle)
-{
-	int res;
-	uint32_t reg;
-	struct stlink_usb_handle_s *h = handle;
-
-	assert(handle != NULL);
-
-	/* configure the TPI */
-
-	/* enable the trace subsystem */
-	res = stlink_usb_v2_read_debug_reg(handle, DCB_DEMCR, &reg);
-	if (res != ERROR_OK)
-		goto out;
-	res = stlink_usb_write_debug_reg(handle, DCB_DEMCR, TRCENA|reg);
-	if (res != ERROR_OK)
-		goto out;
-	/* set the TPI clock prescaler */
-	res = stlink_usb_write_debug_reg(handle, TPI_ACPR, h->trace.prescale);
-	if (res != ERROR_OK)
-		goto out;
-	/* select the pin protocol.  The STLinkv2 only supports asynchronous
-	 * UART emulation (NRZ) mode, so that's what we pick. */
-	res = stlink_usb_write_debug_reg(handle, TPI_SPPR, 0x02);
-	if (res != ERROR_OK)
-		goto out;
-	/* disable continuous formatting */
-	res = stlink_usb_write_debug_reg(handle, TPI_FFCR, (1<<8));
-	if (res != ERROR_OK)
-		goto out;
-
-	/* configure the ITM */
-
-	/* unlock access to the ITM registers */
-	res = stlink_usb_write_debug_reg(handle, ITM_LAR, 0xC5ACCE55);
-	if (res != ERROR_OK)
-		goto out;
-	/* enable trace with ATB ID 1 */
-	res = stlink_usb_write_debug_reg(handle, ITM_TCR, (1<<16)|(1<<0)|(1<<2));
-	if (res != ERROR_OK)
-		goto out;
-	/* trace privilege */
-	res = stlink_usb_write_debug_reg(handle, ITM_TPR, 1);
-	if (res != ERROR_OK)
-		goto out;
-	/* trace port enable (port 0) */
-	res = stlink_usb_write_debug_reg(handle, ITM_TER, (1<<0));
-	if (res != ERROR_OK)
-		goto out;
-
-	res = ERROR_OK;
-out:
-	return res;
+	return stlink_cmd_allow_retry(handle, h->databuf, 2);
 }
 
 /** */
@@ -1019,17 +996,15 @@ static void stlink_usb_trace_disable(void *handle)
 
 	assert(h->version.jtag >= STLINK_TRACE_MIN_VERSION);
 
-	LOG_DEBUG("Tracing: disable\n");
+	LOG_DEBUG("Tracing: disable");
 
 	stlink_usb_init_buffer(handle, h->rx_ep, 2);
 	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_STOP_TRACE_RX;
 	res = stlink_usb_xfer(handle, h->databuf, 2);
 
-	if (res == ERROR_OK) {
+	if (res == ERROR_OK)
 		h->trace.enabled = false;
-		target_unregister_timer_callback(stlink_usb_trace_read_callback, handle);
-	}
 }
 
 
@@ -1042,38 +1017,20 @@ static int stlink_usb_trace_enable(void *handle)
 	assert(handle != NULL);
 
 	if (h->version.jtag >= STLINK_TRACE_MIN_VERSION) {
-		uint32_t trace_hz;
-
-		res = stlink_configure_target_trace_port(handle);
-		if (res != ERROR_OK)
-			LOG_ERROR("Unable to configure tracing on target\n");
-
-		trace_hz = h->trace.prescale > 0 ?
-			h->trace.source_hz / (h->trace.prescale + 1) :
-			h->trace.source_hz;
-
 		stlink_usb_init_buffer(handle, h->rx_ep, 10);
 
 		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_START_TRACE_RX;
 		h_u16_to_le(h->cmdbuf+h->cmdidx, (uint16_t)STLINK_TRACE_SIZE);
 		h->cmdidx += 2;
-		h_u32_to_le(h->cmdbuf+h->cmdidx, trace_hz);
+		h_u32_to_le(h->cmdbuf+h->cmdidx, h->trace.source_hz);
 		h->cmdidx += 4;
 
 		res = stlink_usb_xfer(handle, h->databuf, 2);
 
 		if (res == ERROR_OK)  {
 			h->trace.enabled = true;
-			LOG_DEBUG("Tracing: recording at %" PRIu32 "Hz\n", trace_hz);
-			/* We need the trace read function to be called at a
-			 * high-enough frequency to ensure reasonable
-			 * "timeliness" in processing ITM/DWT data.
-			 * TODO: An alternative could be using the asynchronous
-			 * features of the libusb-1.0 API to queue up one or more
-			 * reads in advance and requeue them once they are
-			 * completed. */
-			target_register_timer_callback(stlink_usb_trace_read_callback, 1, 1, handle);
+			LOG_DEBUG("Tracing: recording at %" PRIu32 "Hz", h->trace.source_hz);
 		}
 	} else {
 		LOG_ERROR("Tracing is not supported by this version.");
@@ -1083,6 +1040,35 @@ static int stlink_usb_trace_enable(void *handle)
 	return res;
 }
 
+/** */
+static int stlink_usb_reset(void *handle)
+{
+	struct stlink_usb_handle_s *h = handle;
+	int retval;
+
+	assert(handle != NULL);
+
+	stlink_usb_init_buffer(handle, h->rx_ep, 2);
+
+	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+
+	if (h->jtag_api == STLINK_JTAG_API_V1)
+		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_RESETSYS;
+	else
+		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_RESETSYS;
+
+	retval = stlink_cmd_allow_retry(handle, h->databuf, 2);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (h->trace.enabled) {
+		stlink_usb_trace_disable(h);
+		return stlink_usb_trace_enable(h);
+	}
+
+	return ERROR_OK;
+}
+
 /** */
 static int stlink_usb_run(void *handle)
 {
@@ -1094,14 +1080,6 @@ static int stlink_usb_run(void *handle)
 	if (h->jtag_api == STLINK_JTAG_API_V2) {
 		res = stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_DEBUGEN);
 
-		/* Try to start tracing, if requested */
-		if (res == ERROR_OK && h->trace.source_hz && !h->trace.enabled) {
-			if (stlink_usb_trace_enable(handle) == ERROR_OK)
-				LOG_DEBUG("Tracing: enabled\n");
-			else
-				LOG_ERROR("Tracing: enable failed\n");
-		}
-
 		return res;
 	}
 
@@ -1110,12 +1088,7 @@ static int stlink_usb_run(void *handle)
 	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_RUNCORE;
 
-	res = stlink_usb_xfer(handle, h->databuf, 2);
-
-	if (res != ERROR_OK)
-		return res;
-
-	return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
+	return stlink_cmd_allow_retry(handle, h->databuf, 2);
 }
 
 /** */
@@ -1129,9 +1102,6 @@ static int stlink_usb_halt(void *handle)
 	if (h->jtag_api == STLINK_JTAG_API_V2) {
 		res = stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_DEBUGEN);
 
-		if (res == ERROR_OK && h->trace.enabled)
-			stlink_usb_trace_disable(handle);
-
 		return res;
 	}
 
@@ -1140,18 +1110,12 @@ static int stlink_usb_halt(void *handle)
 	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_FORCEDEBUG;
 
-	res = stlink_usb_xfer(handle, h->databuf, 2);
-
-	if (res != ERROR_OK)
-		return res;
-
-	return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
+	return stlink_cmd_allow_retry(handle, h->databuf, 2);
 }
 
 /** */
 static int stlink_usb_step(void *handle)
 {
-	int res;
 	struct stlink_usb_handle_s *h = handle;
 
 	assert(handle != NULL);
@@ -1169,12 +1133,7 @@ static int stlink_usb_step(void *handle)
 	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_STEPCORE;
 
-	res = stlink_usb_xfer(handle, h->databuf, 2);
-
-	if (res != ERROR_OK)
-		return res;
-
-	return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
+	return stlink_cmd_allow_retry(handle, h->databuf, 2);
 }
 
 /** */
@@ -1218,25 +1177,24 @@ static int stlink_usb_read_reg(void *handle, int num, uint32_t *val)
 		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READREG;
 	h->cmdbuf[h->cmdidx++] = num;
 
-	res = stlink_usb_xfer(handle, h->databuf, h->jtag_api == STLINK_JTAG_API_V1 ? 4 : 8);
-
-	if (res != ERROR_OK)
-		return res;
-
-	if (h->jtag_api == STLINK_JTAG_API_V1)
+	if (h->jtag_api == STLINK_JTAG_API_V1) {
+		res = stlink_usb_xfer(handle, h->databuf, 4);
+		if (res != ERROR_OK)
+			return res;
 		*val = le_to_h_u32(h->databuf);
-	else {
+		return ERROR_OK;
+	} else {
+		res = stlink_cmd_allow_retry(handle, h->databuf, 8);
+		if (res != ERROR_OK)
+			return res;
 		*val = le_to_h_u32(h->databuf + 4);
-		return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
+		return ERROR_OK;
 	}
-
-	return ERROR_OK;
 }
 
 /** */
 static int stlink_usb_write_reg(void *handle, int num, uint32_t val)
 {
-	int res;
 	struct stlink_usb_handle_s *h = handle;
 
 	assert(handle != NULL);
@@ -1252,12 +1210,7 @@ static int stlink_usb_write_reg(void *handle, int num, uint32_t val)
 	h_u32_to_le(h->cmdbuf+h->cmdidx, val);
 	h->cmdidx += 4;
 
-	res = stlink_usb_xfer(handle, h->databuf, 2);
-
-	if (res != ERROR_OK)
-		return res;
-
-	return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
+	return stlink_cmd_allow_retry(handle, h->databuf, 2);
 }
 
 static int stlink_usb_get_rw_status(void *handle)
@@ -1280,7 +1233,7 @@ static int stlink_usb_get_rw_status(void *handle)
 	if (res != ERROR_OK)
 		return res;
 
-	return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : res;
+	return stlink_usb_error_check(h);
 }
 
 /** */
@@ -1433,6 +1386,7 @@ static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size,
 {
 	int retval = ERROR_OK;
 	uint32_t bytes_remaining;
+	int retries = 0;
 	struct stlink_usb_handle_s *h = handle;
 
 	/* calculate byte count */
@@ -1463,6 +1417,10 @@ static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size,
 
 				uint32_t head_bytes = 4 - (addr % 4);
 				retval = stlink_usb_read_mem8(handle, addr, head_bytes, buffer);
+				if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
+					usleep((1<<retries++) * 1000);
+					continue;
+				}
 				if (retval != ERROR_OK)
 					return retval;
 				buffer += head_bytes;
@@ -1478,6 +1436,10 @@ static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size,
 		} else
 			retval = stlink_usb_read_mem8(handle, addr, bytes_remaining, buffer);
 
+		if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
+			usleep((1<<retries++) * 1000);
+			continue;
+		}
 		if (retval != ERROR_OK)
 			return retval;
 
@@ -1494,6 +1456,7 @@ static int stlink_usb_write_mem(void *handle, uint32_t addr, uint32_t size,
 {
 	int retval = ERROR_OK;
 	uint32_t bytes_remaining;
+	int retries = 0;
 	struct stlink_usb_handle_s *h = handle;
 
 	/* calculate byte count */
@@ -1524,6 +1487,10 @@ static int stlink_usb_write_mem(void *handle, uint32_t addr, uint32_t size,
 
 				uint32_t head_bytes = 4 - (addr % 4);
 				retval = stlink_usb_write_mem8(handle, addr, head_bytes, buffer);
+				if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
+					usleep((1<<retries++) * 1000);
+					continue;
+				}
 				if (retval != ERROR_OK)
 					return retval;
 				buffer += head_bytes;
@@ -1539,6 +1506,10 @@ static int stlink_usb_write_mem(void *handle, uint32_t addr, uint32_t size,
 
 		} else
 			retval = stlink_usb_write_mem8(handle, addr, bytes_remaining, buffer);
+		if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
+			usleep((1<<retries++) * 1000);
+			continue;
+		}
 		if (retval != ERROR_OK)
 			return retval;
 
@@ -1551,14 +1522,72 @@ static int stlink_usb_write_mem(void *handle, uint32_t addr, uint32_t size,
 }
 
 /** */
-static int stlink_usb_close(void *fd)
+static int stlink_usb_override_target(const char *targetname)
 {
-	struct stlink_usb_handle_s *h = fd;
+	return !strcmp(targetname, "cortex_m");
+}
 
-	if (h->fd)
+static int stlink_speed(void *handle, int khz, bool query)
+{
+	unsigned i;
+	int speed_index = -1;
+	int speed_diff = INT_MAX;
+	struct stlink_usb_handle_s *h = handle;
+
+	/* only supported by stlink/v2 and for firmware >= 22 */
+	if (h && (h->version.stlink == 1 || h->version.jtag < 22))
+		return khz;
+
+	for (i = 0; i < ARRAY_SIZE(stlink_khz_to_speed_map); i++) {
+		if (khz == stlink_khz_to_speed_map[i].speed) {
+			speed_index = i;
+			break;
+		} else {
+			int current_diff = khz - stlink_khz_to_speed_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) {
+				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;
+		match = false;
+	} else if (i == ARRAY_SIZE(stlink_khz_to_speed_map))
+		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);
+	}
+
+	if (h && !query) {
+		int result = stlink_usb_set_swdclk(h, stlink_khz_to_speed_map[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;
+}
+
+/** */
+static int stlink_usb_close(void *handle)
+{
+	struct stlink_usb_handle_s *h = handle;
+
+	if (h && h->fd)
 		jtag_libusb_close(h->fd);
 
-	free(fd);
+	free(h);
 
 	return ERROR_OK;
 }
@@ -1583,9 +1612,11 @@ static int stlink_usb_open(struct hl_interface_param_s *param, void **fd)
 
 	const uint16_t vids[] = { param->vid, 0 };
 	const uint16_t pids[] = { param->pid, 0 };
+	const char *serial = param->serial;
 
-	LOG_DEBUG("transport: %d vid: 0x%04x pid: 0x%04x", param->transport,
-		param->vid, param->pid);
+	LOG_DEBUG("transport: %d vid: 0x%04x pid: 0x%04x serial: %s",
+			param->transport, param->vid, param->pid,
+			param->serial ? param->serial : "");
 
 	/*
 	  On certain host USB configurations(e.g. MacBook Air)
@@ -1597,7 +1628,7 @@ static int stlink_usb_open(struct hl_interface_param_s *param, void **fd)
 	  in order to become operational.
 	 */
 	do {
-		if (jtag_libusb_open(vids, pids, &h->fd) != ERROR_OK) {
+		if (jtag_libusb_open(vids, pids, serial, &h->fd) != ERROR_OK) {
 			LOG_ERROR("open failed");
 			goto error_open;
 		}
@@ -1701,25 +1732,24 @@ static int stlink_usb_open(struct hl_interface_param_s *param, void **fd)
 	/* set the used jtag api, this will default to the newest supported version */
 	h->jtag_api = api;
 
-	if (h->jtag_api >= 2 && param->trace_source_hz > 0) {
-		uint32_t prescale;
-
-		prescale = param->trace_source_hz > STLINK_TRACE_MAX_HZ ?
-			(param->trace_source_hz / STLINK_TRACE_MAX_HZ) - 1 : 0;
-
-		h->trace.output_f = param->trace_f;
-		h->trace.source_hz = param->trace_source_hz;
-		h->trace.prescale = prescale;
-	}
-
 	/* initialize the debug hardware */
 	err = stlink_usb_init_mode(h, param->connect_under_reset);
 
 	if (err != ERROR_OK) {
-		LOG_ERROR("init mode failed");
+		LOG_ERROR("init mode failed (unable to connect to the target)");
 		goto error_open;
 	}
 
+	/* 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:");
+
+		for (unsigned i = 0; i < ARRAY_SIZE(stlink_khz_to_speed_map); i++)
+			LOG_DEBUG("%d kHz", stlink_khz_to_speed_map[i].speed);
+
+		stlink_speed(h, param->initial_interface_speed, false);
+	}
+
 	/* get cpuid, so we can determine the max page size
 	 * start with a safe default */
 	h->max_mem_packet = (1 << 10);
@@ -1747,6 +1777,36 @@ error_open:
 	return ERROR_FAIL;
 }
 
+int stlink_config_trace(void *handle, bool enabled, enum tpio_pin_protocol pin_protocol,
+			uint32_t port_size, unsigned int *trace_freq)
+{
+	struct stlink_usb_handle_s *h = handle;
+
+	if (enabled && (h->jtag_api < 2 || pin_protocol != ASYNC_UART)) {
+		LOG_ERROR("The attached ST-LINK version doesn't support this trace mode");
+		return ERROR_FAIL;
+	}
+
+	if (!enabled) {
+		stlink_usb_trace_disable(h);
+		return ERROR_OK;
+	}
+
+	if (*trace_freq > STLINK_TRACE_MAX_HZ) {
+		LOG_ERROR("ST-LINK doesn't support SWO frequency higher than %u",
+			  STLINK_TRACE_MAX_HZ);
+		return ERROR_FAIL;
+	}
+
+	stlink_usb_trace_disable(h);
+
+	if (!*trace_freq)
+		*trace_freq = STLINK_TRACE_MAX_HZ;
+	h->trace.source_hz = *trace_freq;
+
+	return stlink_usb_trace_enable(h);
+}
+
 /** */
 struct hl_layout_api_s stlink_usb_layout_api = {
 	/** */
@@ -1778,5 +1838,13 @@ struct hl_layout_api_s stlink_usb_layout_api = {
 	/** */
 	.write_mem = stlink_usb_write_mem,
 	/** */
-	.write_debug_reg = stlink_usb_write_debug_reg
+	.write_debug_reg = stlink_usb_write_debug_reg,
+	/** */
+	.override_target = stlink_usb_override_target,
+	/** */
+	.speed = stlink_speed,
+	/** */
+	.config_trace = stlink_config_trace,
+	/** */
+	.poll_trace = stlink_usb_trace_read,
 };

src/jtag/drivers/sysfsgpio.c

@@ -16,6 +16,10 @@
  *   Free Software Foundation, Inc.,                                       *
  *   51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.           *
  ***************************************************************************/
+
+/* 2014-12: Addition of the SWD protocol support is based on the initial work
+ * on bcm2835gpio.c by Paul Fertser and modifications by Jean-Christian de Rivaz. */
+
 /**
  * @file
  * This driver implements a bitbang jtag interface using gpio lines via
@@ -139,17 +143,26 @@ static int setup_sysfs_gpio(int gpio, int is_output, int init_high)
 	}
 
 	snprintf(buf, sizeof(buf), "/sys/class/gpio/gpio%d/value", gpio);
-	if (is_output)
-		ret = open(buf, O_WRONLY | O_NONBLOCK | O_SYNC);
-	else
-		ret = open(buf, O_RDONLY | O_NONBLOCK | O_SYNC);
-
-	if (ret < 0)
+	ret = open(buf, O_RDWR | O_NONBLOCK | O_SYNC);
+	if (ret < 0) {
+		LOG_ERROR("Couldn't open value for gpio %d", gpio);
+		perror("sysfsgpio: ");
 		unexport_sysfs_gpio(gpio);
+	}
 
 	return ret;
 }
 
+/* gpio numbers for each gpio. Negative values are invalid */
+static int tck_gpio = -1;
+static int tms_gpio = -1;
+static int tdi_gpio = -1;
+static int tdo_gpio = -1;
+static int trst_gpio = -1;
+static int srst_gpio = -1;
+static int swclk_gpio = -1;
+static int swdio_gpio = -1;
+
 /*
  * file descriptors for /sys/class/gpio/gpioXX/value
  * Set up during init.
@@ -160,6 +173,72 @@ static int tdi_fd = -1;
 static int tdo_fd = -1;
 static int trst_fd = -1;
 static int srst_fd = -1;
+static int swclk_fd = -1;
+static int swdio_fd = -1;
+
+static int last_swclk;
+static int last_swdio;
+static bool last_stored;
+static bool swdio_input;
+
+static void sysfsgpio_swdio_drive(bool is_output)
+{
+	char buf[40];
+	int ret;
+
+	snprintf(buf, sizeof(buf), "/sys/class/gpio/gpio%d/direction", swdio_gpio);
+	ret = open_write_close(buf, is_output ? "high" : "in");
+	if (ret < 0) {
+		LOG_ERROR("Couldn't set direction for gpio %d", swdio_gpio);
+		perror("sysfsgpio: ");
+	}
+
+	last_stored = false;
+	swdio_input = !is_output;
+}
+
+static int sysfsgpio_swdio_read(void)
+{
+	char buf[1];
+
+	/* important to seek to signal sysfs of new read */
+	lseek(swdio_fd, 0, SEEK_SET);
+	int ret = read(swdio_fd, &buf, sizeof(buf));
+
+	if (ret < 0) {
+		LOG_WARNING("reading swdio failed");
+		return 0;
+	}
+
+	return buf[0] == '1';
+}
+
+static void sysfsgpio_swdio_write(int swclk, int swdio)
+{
+	const char one[] = "1";
+	const char zero[] = "0";
+
+	size_t bytes_written;
+
+	if (!swdio_input) {
+		if (!last_stored || (swdio != last_swdio)) {
+			bytes_written = write(swdio_fd, swdio ? &one : &zero, 1);
+			if (bytes_written != 1)
+				LOG_WARNING("writing swdio failed");
+		}
+	}
+
+	/* write swclk last */
+	if (!last_stored || (swclk != last_swclk)) {
+		bytes_written = write(swclk_fd, swclk ? &one : &zero, 1);
+		if (bytes_written != 1)
+			LOG_WARNING("writing swclk failed");
+	}
+
+	last_swdio = swdio;
+	last_swclk = swclk;
+	last_stored = true;
+}
 
 /*
  * Bitbang interface read of TDO
@@ -191,6 +270,11 @@ static int sysfsgpio_read(void)
  */
 static void sysfsgpio_write(int tck, int tms, int tdi)
 {
+	if (swd_mode) {
+		sysfsgpio_swdio_write(tck, tdi);
+		return;
+	}
+
 	const char one[] = "1";
 	const char zero[] = "0";
 
@@ -239,6 +323,7 @@ static void sysfsgpio_write(int tck, int tms, int tdi)
  */
 static void sysfsgpio_reset(int trst, int srst)
 {
+	LOG_DEBUG("sysfsgpio_reset");
 	const char one[] = "1";
 	const char zero[] = "0";
 	size_t bytes_written;
@@ -258,14 +343,6 @@ static void sysfsgpio_reset(int trst, int srst)
 	}
 }
 
-/* gpio numbers for each gpio. Negative values are invalid */
-static int tck_gpio = -1;
-static int tms_gpio = -1;
-static int tdi_gpio = -1;
-static int tdo_gpio = -1;
-static int trst_gpio = -1;
-static int srst_gpio = -1;
-
 COMMAND_HANDLER(sysfsgpio_handle_jtag_gpionums)
 {
 	if (CMD_ARGC == 4) {
@@ -338,6 +415,40 @@ COMMAND_HANDLER(sysfsgpio_handle_jtag_gpionum_trst)
 	return ERROR_OK;
 }
 
+COMMAND_HANDLER(sysfsgpio_handle_swd_gpionums)
+{
+	if (CMD_ARGC == 2) {
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], swclk_gpio);
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[1], swdio_gpio);
+	} else if (CMD_ARGC != 0) {
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	command_print(CMD_CTX,
+			"SysfsGPIO nums: swclk = %d, swdio = %d",
+			swclk_gpio, swdio_gpio);
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(sysfsgpio_handle_swd_gpionum_swclk)
+{
+	if (CMD_ARGC == 1)
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], swclk_gpio);
+
+	command_print(CMD_CTX, "SysfsGPIO num: swclk = %d", swclk_gpio);
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(sysfsgpio_handle_swd_gpionum_swdio)
+{
+	if (CMD_ARGC == 1)
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], swdio_gpio);
+
+	command_print(CMD_CTX, "SysfsGPIO num: swdio = %d", swdio_gpio);
+	return ERROR_OK;
+}
+
 static const struct command_registration sysfsgpio_command_handlers[] = {
 	{
 		.name = "sysfsgpio_jtag_nums",
@@ -382,17 +493,39 @@ static const struct command_registration sysfsgpio_command_handlers[] = {
 		.mode = COMMAND_CONFIG,
 		.help = "gpio number for 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)* ",
+	},
+	{
+		.name = "sysfsgpio_swclk_num",
+		.handler = &sysfsgpio_handle_swd_gpionum_swclk,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio number for swclk.",
+	},
+	{
+		.name = "sysfsgpio_swdio_num",
+		.handler = &sysfsgpio_handle_swd_gpionum_swdio,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio number for swdio.",
+	},
 	COMMAND_REGISTRATION_DONE
 };
 
 static int sysfsgpio_init(void);
 static int sysfsgpio_quit(void);
 
+static const char * const sysfsgpio_transports[] = { "jtag", "swd", NULL };
+
 struct jtag_interface sysfsgpio_interface = {
 	.name = "sysfsgpio",
 	.supported = DEBUG_CAP_TMS_SEQ,
 	.execute_queue = bitbang_execute_queue,
-	.transports = jtag_only,
+	.transports = sysfsgpio_transports,
+	.swd = &bitbang_swd,
 	.commands = sysfsgpio_command_handlers,
 	.init = sysfsgpio_init,
 	.quit = sysfsgpio_quit,
@@ -402,6 +535,8 @@ static struct bitbang_interface sysfsgpio_bitbang = {
 	.read = sysfsgpio_read,
 	.write = sysfsgpio_write,
 	.reset = sysfsgpio_reset,
+	.swdio_read = sysfsgpio_swdio_read,
+	.swdio_drive = sysfsgpio_swdio_drive,
 	.blink = 0
 };
 
@@ -426,68 +561,113 @@ static void cleanup_all_fds(void)
 	cleanup_fd(srst_fd, srst_gpio);
 }
 
+static bool sysfsgpio_jtag_mode_possible(void)
+{
+	if (!is_gpio_valid(tck_gpio))
+		return 0;
+	if (!is_gpio_valid(tms_gpio))
+		return 0;
+	if (!is_gpio_valid(tdi_gpio))
+		return 0;
+	if (!is_gpio_valid(tdo_gpio))
+		return 0;
+	return 1;
+}
+
+static bool sysfsgpio_swd_mode_possible(void)
+{
+	if (!is_gpio_valid(swclk_gpio))
+		return 0;
+	if (!is_gpio_valid(swdio_gpio))
+		return 0;
+	return 1;
+}
+
 static int sysfsgpio_init(void)
 {
 	bitbang_interface = &sysfsgpio_bitbang;
 
-	LOG_INFO("SysfsGPIO JTAG bitbang driver");
+	LOG_INFO("SysfsGPIO JTAG/SWD bitbang driver");
 
-	if (!(is_gpio_valid(tck_gpio)
-			&& is_gpio_valid(tms_gpio)
-			&& is_gpio_valid(tdi_gpio)
-			&& is_gpio_valid(tdo_gpio))) {
-		if (!is_gpio_valid(tck_gpio))
-			LOG_ERROR("gpio num for tck is invalid");
-		if (!is_gpio_valid(tms_gpio))
-			LOG_ERROR("gpio num for tms is invalid");
-		if (!is_gpio_valid(tdo_gpio))
-			LOG_ERROR("gpio num for tdo is invalid");
-		if (!is_gpio_valid(tdi_gpio))
-			LOG_ERROR("gpio num for tdi is invalid");
-
-		LOG_ERROR("Require tck, tms, tdi and tdo gpios to all be specified");
+	if (sysfsgpio_jtag_mode_possible()) {
+		if (sysfsgpio_swd_mode_possible())
+			LOG_INFO("JTAG and SWD modes enabled");
+		else
+			LOG_INFO("JTAG only mode enabled (specify swclk and swdio gpio to add SWD mode)");
+		if (!is_gpio_valid(trst_gpio) && !is_gpio_valid(srst_gpio)) {
+			LOG_ERROR("Require at least one of trst or srst gpios to be specified");
+			return ERROR_JTAG_INIT_FAILED;
+		}
+	} else if (sysfsgpio_swd_mode_possible()) {
+		LOG_INFO("SWD only mode enabled (specify tck, tms, tdi and tdo gpios to add JTAG mode)");
+	} else {
+		LOG_ERROR("Require tck, tms, tdi and tdo gpios for JTAG mode and/or swclk and swdio gpio for SWD mode");
 		return ERROR_JTAG_INIT_FAILED;
 	}
 
-	if (!is_gpio_valid(trst_gpio) && !is_gpio_valid(srst_gpio)) {
-		LOG_ERROR("Require at least one of trst or srst gpios to be specified");
-		return ERROR_JTAG_INIT_FAILED;
-	}
 
 	/*
 	 * Configure TDO as an input, and TDI, TCK, TMS, TRST, SRST
 	 * as outputs.  Drive TDI and TCK low, and TMS/TRST/SRST high.
+	 * For SWD, SWCLK and SWDIO are configures as output high.
 	 */
-	tck_fd = setup_sysfs_gpio(tck_gpio, 1, 0);
-	if (tck_fd < 0)
-		goto out_error;
+	if (tck_gpio >= 0) {
+		tck_fd = setup_sysfs_gpio(tck_gpio, 1, 0);
+		if (tck_fd < 0)
+			goto out_error;
+	}
 
-	tms_fd = setup_sysfs_gpio(tms_gpio, 1, 1);
-	if (tms_fd < 0)
-		goto out_error;
+	if (tms_gpio >= 0) {
+		tms_fd = setup_sysfs_gpio(tms_gpio, 1, 1);
+		if (tms_fd < 0)
+			goto out_error;
+	}
 
-	tdi_fd = setup_sysfs_gpio(tdi_gpio, 1, 0);
-	if (tdi_fd < 0)
-		goto out_error;
+	if (tdi_gpio >= 0) {
+		tdi_fd = setup_sysfs_gpio(tdi_gpio, 1, 0);
+		if (tdi_fd < 0)
+			goto out_error;
+	}
 
-	tdo_fd = setup_sysfs_gpio(tdo_gpio, 0, 0);
-	if (tdo_fd < 0)
-		goto out_error;
+	if (tdo_gpio >= 0) {
+		tdo_fd = setup_sysfs_gpio(tdo_gpio, 0, 0);
+		if (tdo_fd < 0)
+			goto out_error;
+	}
 
 	/* assume active low*/
-	if (trst_gpio > 0) {
+	if (trst_gpio >= 0) {
 		trst_fd = setup_sysfs_gpio(trst_gpio, 1, 1);
 		if (trst_fd < 0)
 			goto out_error;
 	}
 
 	/* assume active low*/
-	if (srst_gpio > 0) {
+	if (srst_gpio >= 0) {
 		srst_fd = setup_sysfs_gpio(srst_gpio, 1, 1);
 		if (srst_fd < 0)
 			goto out_error;
 	}
 
+	if (swclk_gpio >= 0) {
+		swclk_fd = setup_sysfs_gpio(swclk_gpio, 1, 0);
+		if (swclk_fd < 0)
+			goto out_error;
+	}
+
+	if (swdio_gpio >= 0) {
+		swdio_fd = setup_sysfs_gpio(swdio_gpio, 1, 0);
+		if (swdio_fd < 0)
+			goto out_error;
+	}
+
+	if (sysfsgpio_swd_mode_possible()) {
+		if (swd_mode)
+			bitbang_swd_switch_seq(NULL, JTAG_TO_SWD);
+		else
+			bitbang_swd_switch_seq(NULL, SWD_TO_JTAG);
+	}
+
 	return ERROR_OK;
 
 out_error:

src/jtag/drivers/ti_icdi_usb.c

@@ -645,10 +645,18 @@ static int icdi_usb_write_mem(void *handle, uint32_t addr, uint32_t size,
 	return retval;
 }
 
+static int icdi_usb_override_target(const char *targetname)
+{
+	return !strcmp(targetname, "cortex_m");
+}
+
 static int icdi_usb_close(void *handle)
 {
 	struct icdi_usb_handle_s *h = handle;
 
+	if (!h)
+		return ERROR_OK;
+
 	if (h->usb_dev)
 		libusb_close(h->usb_dev);
 
@@ -770,5 +778,7 @@ struct hl_layout_api_s icdi_usb_layout_api = {
 	.write_reg = icdi_usb_write_reg,
 	.read_mem = icdi_usb_read_mem,
 	.write_mem = icdi_usb_write_mem,
-	.write_debug_reg = icdi_usb_write_debug_reg
+	.write_debug_reg = icdi_usb_write_debug_reg,
+	.override_target = icdi_usb_override_target,
+	.custom_command = icdi_send_remote_cmd,
 };

src/jtag/drivers/ulink.c

@@ -461,9 +461,6 @@ int ulink_write_firmware_section(struct ulink *device,
 	LOG_DEBUG("section %02i at addr 0x%04x (size 0x%04x)", section_index, addr,
 		size);
 
-	if (data == NULL)
-		return ERROR_FAIL;
-
 	/* Copy section contents to local buffer */
 	ret = image_read_section(firmware_image, section_index, 0, size, data,
 			&size_read);

src/jtag/drivers/usb_blaster/ublast2_access_libusb.c

@@ -74,9 +74,6 @@ static int ublast2_write_firmware_section(struct jtag_libusb_device_handle *libu
 	LOG_DEBUG("section %02i at addr 0x%04x (size 0x%04x)", section_index, addr,
 		size);
 
-	if (data == NULL)
-		return ERROR_FAIL;
-
 	/* Copy section contents to local buffer */
 	int ret = image_read_section(firmware_image, section_index, 0, size, data,
 			&size_read);
@@ -186,7 +183,7 @@ static int ublast2_libusb_init(struct ublast_lowlevel *low)
 	bool renumeration = false;
 	int ret;
 
-	if (jtag_libusb_open(vids, pids, &temp) == ERROR_OK) {
+	if (jtag_libusb_open(vids, pids, NULL, &temp) == ERROR_OK) {
 		LOG_INFO("Altera USB-Blaster II (uninitialized) found");
 		LOG_INFO("Loading firmware...");
 		ret = load_usb_blaster_firmware(temp, low);
@@ -200,13 +197,13 @@ static int ublast2_libusb_init(struct ublast_lowlevel *low)
 	const uint16_t pids_renum[] = { low->ublast_pid, 0 };
 
 	if (renumeration == false) {
-		if (jtag_libusb_open(vids_renum, pids_renum, &low->libusb_dev) != ERROR_OK) {
+		if (jtag_libusb_open(vids_renum, pids_renum, NULL, &low->libusb_dev) != ERROR_OK) {
 			LOG_ERROR("Altera USB-Blaster II not found");
 			return ERROR_FAIL;
 		}
 	} else {
 		int retry = 10;
-		while (jtag_libusb_open(vids_renum, pids_renum, &low->libusb_dev) != ERROR_OK && retry--) {
+		while (jtag_libusb_open(vids_renum, pids_renum, NULL, &low->libusb_dev) != ERROR_OK && retry--) {
 			usleep(1000000);
 			LOG_INFO("Waiting for renumerate...");
 		}

src/jtag/drivers/usb_blaster/usb_blaster.c

@@ -756,11 +756,41 @@ static void ublast_usleep(int us)
 	jtag_sleep(us);
 }
 
+static void ublast_initial_wipeout(void)
+{
+	static uint8_t tms_reset = 0xff;
+	uint8_t out_value;
+	uint32_t retlen;
+	int i;
+
+	out_value = ublast_build_out(SCAN_OUT);
+	for (i = 0; i < BUF_LEN; i++)
+		info.buf[i] = out_value | ((i % 2) ? TCK : 0);
+
+	/*
+	 * Flush USB-Blaster queue fifos
+	 *  - empty the write FIFO (128 bytes)
+	 *  - empty the read FIFO (384 bytes)
+	 */
+	ublast_buf_write(info.buf, BUF_LEN, &retlen);
+	/*
+	 * Put JTAG in RESET state (five 1 on TMS)
+	 */
+	ublast_tms_seq(&tms_reset, 5);
+	tap_set_state(TAP_RESET);
+}
+
 static int ublast_execute_queue(void)
 {
 	struct jtag_command *cmd;
+	static int first_call = 1;
 	int ret = ERROR_OK;
 
+	if (first_call) {
+		first_call--;
+		ublast_initial_wipeout();
+	}
+
 	for (cmd = jtag_command_queue; ret == ERROR_OK && cmd != NULL;
 	     cmd = cmd->next) {
 		switch (cmd->type) {
@@ -801,14 +831,12 @@ static int ublast_execute_queue(void)
  *
  * Initialize the device :
  *  - open the USB device
- *  - empty the write FIFO (128 bytes)
- *  - empty the read FIFO (384 bytes)
+ *  - pretend it's initialized while actual init is delayed until first jtag command
  *
  * Returns ERROR_OK if USB device found, error if not.
  */
 static int ublast_init(void)
 {
-	static uint8_t tms_reset = 0xff;
 	int ret, i;
 
 	if (info.lowlevel_name) {
@@ -846,18 +874,13 @@ static int ublast_init(void)
 	info.flags |= info.drv->flags;
 
 	ret = info.drv->open(info.drv);
-	if (ret == ERROR_OK) {
-		/*
-		 * Flush USB-Blaster queue fifos
-		 */
-		uint32_t retlen;
-		ublast_buf_write(info.buf, BUF_LEN, &retlen);
-		/*
-		 * Put JTAG in RESET state (five 1 on TMS)
-		 */
-		ublast_tms_seq(&tms_reset, 5);
-		tap_set_state(TAP_RESET);
-	}
+
+	/*
+	 * Let lie here : the TAP is in an unknown state, but the first
+	 * execute_queue() will trigger a ublast_initial_wipeout(), which will
+	 * put the TAP in RESET.
+	 */
+	tap_set_state(TAP_RESET);
 	return ret;
 }
 

src/jtag/drivers/versaloon/usbtoxxx/usbtoswd.c

@@ -30,7 +30,7 @@
 #include "usbtoxxx.h"
 #include "usbtoxxx_internal.h"
 
-RESULT usbtoswd_callback(void *p, uint8_t *src, uint8_t *processed)
+RESULT usbtoswd_read_callback(void *p, uint8_t *src, uint8_t *processed)
 {
 	struct versaloon_pending_t *pending = (struct versaloon_pending_t *)p;
 
@@ -40,6 +40,18 @@ RESULT usbtoswd_callback(void *p, uint8_t *src, uint8_t *processed)
 	return ERROR_OK;
 }
 
+RESULT usbtoswd_write_callback(void *p, uint8_t *src, uint8_t *processed)
+{
+	struct versaloon_pending_t *pending = (struct versaloon_pending_t *)p;
+
+	if (pending->extra_data != NULL)
+		*((uint8_t *)pending->extra_data) = src[0];
+
+	/* mark it processed to ignore other input data */
+	*processed = 1;
+	return ERROR_OK;
+}
+
 RESULT usbtoswd_init(uint8_t interface_index)
 {
 	return usbtoxxx_init_command(USB_TO_SWD, interface_index);
@@ -69,7 +81,8 @@ RESULT usbtoswd_config(uint8_t interface_index, uint8_t trn, uint16_t retry,
 	return usbtoxxx_conf_command(USB_TO_SWD, interface_index, cfg_buf, 5);
 }
 
-RESULT usbtoswd_seqout(uint8_t interface_index, uint8_t *data, uint16_t bitlen)
+RESULT usbtoswd_seqout(uint8_t interface_index, const uint8_t *data,
+	uint16_t bitlen)
 {
 	uint16_t bytelen = (bitlen + 7) >> 3;
 
@@ -130,14 +143,16 @@ RESULT usbtoswd_transact(uint8_t interface_index, uint8_t request,
 		memset(buff + 1, 0, 4);
 
 	versaloon_set_extra_data(ack);
-	versaloon_set_callback(usbtoswd_callback);
 	if (request & 0x04) {
 		/* read */
-		return usbtoxxx_inout_command(USB_TO_SWD, interface_index, buff, 5, 5,
-			(uint8_t *)data, 1, 4, 0);
+		versaloon_set_callback(usbtoswd_read_callback);
 	} else {
 		/* write */
-		return usbtoxxx_inout_command(USB_TO_SWD, interface_index, buff, 5, 5,
-			NULL, 0, 0, 0);
+		versaloon_set_callback(usbtoswd_write_callback);
 	}
+
+	/* Input buffer must be passed even for write operations. Otherwise
+	 * the callback function is not called and ack value is not set. */
+	return usbtoxxx_inout_command(USB_TO_SWD, interface_index, buff, 5, 5,
+		(uint8_t *)data, 1, 4, 0);
 }

src/jtag/drivers/versaloon/usbtoxxx/usbtoxxx.h

@@ -183,7 +183,8 @@ RESULT usbtoswd_init(uint8_t interface_index);
 RESULT usbtoswd_fini(uint8_t interface_index);
 RESULT usbtoswd_config(uint8_t interface_index, uint8_t trn, uint16_t retry,
 		       uint16_t dly);
-RESULT usbtoswd_seqout(uint8_t interface_index, uint8_t *data, uint16_t bitlen);
+RESULT usbtoswd_seqout(uint8_t interface_index, const uint8_t *data,
+		       uint16_t bitlen);
 RESULT usbtoswd_seqin(uint8_t interface_index, uint8_t *data, uint16_t bitlen);
 RESULT usbtoswd_transact(uint8_t interface_index, uint8_t request,
 			 uint32_t *data, uint8_t *ack);

src/jtag/drivers/versaloon/versaloon.c

@@ -23,6 +23,7 @@
 
 #include <stdio.h>
 #include <string.h>
+#include <libusb.h>
 
 #include "versaloon_include.h"
 #include "versaloon.h"
@@ -36,7 +37,7 @@ uint16_t versaloon_buf_size;
 struct versaloon_pending_t versaloon_pending[VERSALOON_MAX_PENDING_NUMBER];
 uint16_t versaloon_pending_idx;
 
-usb_dev_handle *versaloon_usb_device_handle;
+libusb_device_handle *versaloon_usb_device_handle;
 static uint32_t versaloon_usb_to = VERSALOON_TIMEOUT;
 
 RESULT versaloon_init(void);
@@ -196,6 +197,7 @@ RESULT versaloon_add_pending(uint8_t type, uint8_t cmd, uint16_t actual_szie,
 RESULT versaloon_send_command(uint16_t out_len, uint16_t *inlen)
 {
 	int ret;
+	int transferred;
 
 #if PARAM_CHECK
 	if (NULL == versaloon_buf) {
@@ -208,25 +210,24 @@ RESULT versaloon_send_command(uint16_t out_len, uint16_t *inlen)
 	}
 #endif
 
-	ret = usb_bulk_write(versaloon_usb_device_handle,
-			versaloon_interface.usb_setting.ep_out, (char *)versaloon_buf,
-			out_len, versaloon_usb_to);
-	if (ret != out_len) {
-		LOG_ERROR(ERRMSG_FAILURE_OPERATION_ERRSTRING, "send usb data",
-			usb_strerror());
+	ret = libusb_bulk_transfer(versaloon_usb_device_handle,
+			versaloon_interface.usb_setting.ep_out,
+			versaloon_buf, out_len, &transferred, versaloon_usb_to);
+	if (0 != ret || transferred != out_len) {
+		LOG_ERROR(ERRMSG_FAILURE_OPERATION, "send usb data");
 		return ERRCODE_FAILURE_OPERATION;
 	}
 
 	if (inlen != NULL) {
-		ret = usb_bulk_read(versaloon_usb_device_handle,
-				versaloon_interface.usb_setting.ep_in, (char *)versaloon_buf,
-				versaloon_interface.usb_setting.buf_size, versaloon_usb_to);
-		if (ret > 0) {
-			*inlen = (uint16_t)ret;
+		ret = libusb_bulk_transfer(versaloon_usb_device_handle,
+			versaloon_interface.usb_setting.ep_in,
+			versaloon_buf, versaloon_interface.usb_setting.buf_size,
+			&transferred, versaloon_usb_to);
+		if (0 == ret) {
+			*inlen = (uint16_t)transferred;
 			return ERROR_OK;
 		} else {
-			LOG_ERROR(ERRMSG_FAILURE_OPERATION_ERRSTRING, "receive usb data",
-				usb_strerror());
+			LOG_ERROR(ERRMSG_FAILURE_OPERATION, "receive usb data");
 			return ERROR_FAIL;
 		}
 	} else

src/jtag/drivers/versaloon/versaloon.h

@@ -20,6 +20,8 @@
 #ifndef __VERSALOON_H_INCLUDED__
 #define __VERSALOON_H_INCLUDED__
 
+#include <libusb.h>
+
 struct usart_status_t {
 	uint32_t tx_buff_avail;
 	uint32_t tx_buff_size;
@@ -59,7 +61,8 @@ struct interface_swd_t {
 	RESULT(*fini)(uint8_t interface_index);
 	RESULT(*config)(uint8_t interface_index, uint8_t trn, uint16_t retry,
 		uint16_t dly);
-	RESULT(*seqout)(uint8_t interface_index, uint8_t *data, uint16_t bitlen);
+	RESULT(*seqout)(uint8_t interface_index, const uint8_t *data,
+			uint16_t bitlen);
 	RESULT(*seqin)(uint8_t interface_index, uint8_t *data, uint16_t bitlen);
 	RESULT(*transact)(uint8_t interface_index, uint8_t request,
 		uint32_t *data, uint8_t *ack);
@@ -106,7 +109,7 @@ struct versaloon_interface_t {
 };
 
 extern struct versaloon_interface_t versaloon_interface;
-extern usb_dev_handle *versaloon_usb_device_handle;
+extern libusb_device_handle *versaloon_usb_device_handle;
 
 #endif /* __VERSALOON_H_INCLUDED__ */
 

src/jtag/drivers/versaloon/versaloon_include.h

@@ -21,7 +21,6 @@
 /* according to different platform */
 #include <jtag/interface.h>
 #include <jtag/commands.h>
-#include "usb_common.h"
 
 #define PARAM_CHECK							1
 
@@ -49,7 +48,6 @@
 #define ERRMSG_NOT_SUPPORT_BY				"%s is not supported by %s."
 
 #define ERRMSG_FAILURE_OPERATION			"Fail to %s."
-#define ERRMSG_FAILURE_OPERATION_ERRSTRING	"Fail to %s, error string is %s."
 #define ERRMSG_FAILURE_OPERATION_MESSAGE	"Fail to %s, %s"
 #define ERRCODE_FAILURE_OPERATION			ERROR_FAIL
 

src/jtag/drivers/vsllink.c

@@ -28,7 +28,8 @@
 
 #include <jtag/interface.h>
 #include <jtag/commands.h>
-#include "usb_common.h"
+#include <jtag/swd.h>
+#include <libusb.h>
 
 #include "versaloon/versaloon_include.h"
 #include "versaloon/versaloon.h"
@@ -70,12 +71,19 @@ static void vsllink_tap_ensure_pending(int scans);
 static void vsllink_tap_append_scan(int length, uint8_t *buffer,
 		struct scan_command *command);
 
+/* VSLLink SWD functions */
+static int_least32_t vsllink_swd_frequency(struct adiv5_dap *dap,
+		int_least32_t hz);
+static int vsllink_swd_switch_seq(struct adiv5_dap *dap,
+		enum swd_special_seq seq);
+
 /* VSLLink lowlevel functions */
 struct vsllink {
-	struct usb_dev_handle *usb_handle;
+	struct libusb_context *libusb_ctx;
+	struct libusb_device_handle *usb_device_handle;
 };
 
-static struct vsllink *vsllink_usb_open(void);
+static int vsllink_usb_open(struct vsllink *vsllink);
 static void vsllink_usb_close(struct vsllink *vsllink);
 
 #if defined _DEBUG_JTAG_IO_
@@ -88,7 +96,9 @@ static uint8_t *tms_buffer;
 static uint8_t *tdi_buffer;
 static uint8_t *tdo_buffer;
 
-struct vsllink *vsllink_handle;
+static bool swd_mode;
+
+static struct vsllink *vsllink_handle;
 
 static int vsllink_execute_queue(void)
 {
@@ -232,6 +242,11 @@ static int vsllink_execute_queue(void)
 
 static int vsllink_speed(int speed)
 {
+	if (swd_mode) {
+		vsllink_swd_frequency(NULL, speed * 1000);
+		return ERROR_OK;
+	}
+
 	versaloon_interface.adaptors.jtag_raw.config(0, (uint16_t)speed);
 	return versaloon_interface.adaptors.peripheral_commit();
 }
@@ -271,20 +286,34 @@ static int vsllink_quit(void)
 	versaloon_interface.adaptors.gpio.config(0, GPIO_SRST | GPIO_TRST,
 		0, 0, GPIO_SRST | GPIO_TRST);
 	versaloon_interface.adaptors.gpio.fini(0);
-	versaloon_interface.adaptors.jtag_raw.fini(0);
+
+	if (swd_mode)
+		versaloon_interface.adaptors.swd.fini(0);
+	else
+		versaloon_interface.adaptors.jtag_raw.fini(0);
+
 	versaloon_interface.adaptors.peripheral_commit();
 	versaloon_interface.fini();
 
 	vsllink_free_buffer();
 	vsllink_usb_close(vsllink_handle);
 
+	free(vsllink_handle);
+
 	return ERROR_OK;
 }
 
-static int vsllink_init(void)
+static int vsllink_interface_init(void)
 {
-	vsllink_handle = vsllink_usb_open();
-	if (vsllink_handle == 0) {
+	vsllink_handle = malloc(sizeof(struct vsllink));
+	if (NULL == vsllink_handle) {
+		LOG_ERROR("unable to allocate memory");
+		return ERROR_FAIL;
+	}
+
+	libusb_init(&vsllink_handle->libusb_ctx);
+
+	if (ERROR_OK != vsllink_usb_open(vsllink_handle)) {
 		LOG_ERROR("Can't find USB JTAG Interface!" \
 			"Please check connection and permissions.");
 		return ERROR_JTAG_INIT_FAILED;
@@ -292,7 +321,7 @@ static int vsllink_init(void)
 	LOG_DEBUG("vsllink found on %04X:%04X",
 		versaloon_interface.usb_setting.vid,
 		versaloon_interface.usb_setting.pid);
-	versaloon_usb_device_handle = vsllink_handle->usb_handle;
+	versaloon_usb_device_handle = vsllink_handle->usb_device_handle;
 
 	if (ERROR_OK != versaloon_interface.init())
 		return ERROR_FAIL;
@@ -301,22 +330,46 @@ static int vsllink_init(void)
 		return ERROR_FAIL;
 	}
 
-	/* malloc buffer size for tap */
-	tap_buffer_size = versaloon_interface.usb_setting.buf_size / 2 - 32;
-	vsllink_free_buffer();
-	tdi_buffer = malloc(tap_buffer_size);
-	tdo_buffer = malloc(tap_buffer_size);
-	tms_buffer = malloc(tap_buffer_size);
-	if ((NULL == tdi_buffer) || (NULL == tdo_buffer) || (NULL == tms_buffer)) {
-		vsllink_quit();
-		return ERROR_FAIL;
+	return ERROR_OK;
+}
+
+static int vsllink_init(void)
+{
+	int retval = vsllink_interface_init();
+	if (ERROR_OK != retval)
+		return retval;
+
+	versaloon_interface.adaptors.gpio.init(0);
+	versaloon_interface.adaptors.gpio.config(0, GPIO_SRST, 0, GPIO_SRST,
+		GPIO_SRST);
+	versaloon_interface.adaptors.delay.delayms(100);
+	versaloon_interface.adaptors.peripheral_commit();
+
+	if (swd_mode) {
+		versaloon_interface.adaptors.gpio.config(0, GPIO_TRST, 0,
+			GPIO_TRST, GPIO_TRST);
+		versaloon_interface.adaptors.swd.init(0);
+		vsllink_swd_frequency(NULL, jtag_get_speed_khz() * 1000);
+		vsllink_swd_switch_seq(NULL, JTAG_TO_SWD);
+
+	} else {
+		/* malloc buffer size for tap */
+		tap_buffer_size = versaloon_interface.usb_setting.buf_size / 2 - 32;
+		vsllink_free_buffer();
+		tdi_buffer = malloc(tap_buffer_size);
+		tdo_buffer = malloc(tap_buffer_size);
+		tms_buffer = malloc(tap_buffer_size);
+		if ((NULL == tdi_buffer) || (NULL == tdo_buffer) || (NULL == tms_buffer)) {
+			vsllink_quit();
+			return ERROR_FAIL;
+		}
+
+		versaloon_interface.adaptors.jtag_raw.init(0);
+		versaloon_interface.adaptors.jtag_raw.config(0, jtag_get_speed_khz());
+		versaloon_interface.adaptors.gpio.config(0, GPIO_SRST | GPIO_TRST,
+			GPIO_TRST, GPIO_SRST, GPIO_SRST);
 	}
 
-	versaloon_interface.adaptors.jtag_raw.init(0);
-	versaloon_interface.adaptors.jtag_raw.config(0, jtag_get_speed_khz());
-	versaloon_interface.adaptors.gpio.init(0);
-	versaloon_interface.adaptors.gpio.config(0, GPIO_SRST | GPIO_TRST,
-		GPIO_TRST, GPIO_SRST, GPIO_SRST);
 	if (ERROR_OK != versaloon_interface.adaptors.peripheral_commit())
 		return ERROR_FAIL;
 
@@ -442,10 +495,13 @@ static void vsllink_reset(int trst, int srst)
 	else
 		versaloon_interface.adaptors.gpio.config(0, GPIO_SRST, GPIO_SRST, 0, 0);
 
-	if (!trst)
-		versaloon_interface.adaptors.gpio.out(0, GPIO_TRST, GPIO_TRST);
-	else
-		versaloon_interface.adaptors.gpio.out(0, GPIO_TRST, 0);
+	if (!swd_mode) {
+		if (!trst)
+			versaloon_interface.adaptors.gpio.out(0, GPIO_TRST, GPIO_TRST);
+		else
+			versaloon_interface.adaptors.gpio.out(0, GPIO_TRST, 0);
+	}
+
 	versaloon_interface.adaptors.peripheral_commit();
 }
 
@@ -660,138 +716,189 @@ static int vsllink_jtag_execute(void)
 
 static int vsllink_tap_execute(void)
 {
+	if (swd_mode)
+		return ERROR_OK;
+
 	return vsllink_jtag_execute();
 }
 
+static int vsllink_swd_init(void)
+{
+	LOG_INFO("VSLLink SWD mode enabled");
+	swd_mode = true;
+
+	return ERROR_OK;
+}
+
+static int_least32_t vsllink_swd_frequency(struct adiv5_dap *dap,
+		int_least32_t hz)
+{
+	const int_least32_t delay2hz[] = {
+		1850000, 235000, 130000, 102000, 85000, 72000
+	};
+
+	if (hz > 0) {
+		uint16_t delay = UINT16_MAX;
+
+		for (uint16_t i = 0; i < ARRAY_SIZE(delay2hz); i++) {
+			if (hz >= delay2hz[i]) {
+				hz = delay2hz[i];
+				delay = i;
+				break;
+			}
+		}
+
+		if (delay == UINT16_MAX)
+			delay = (500000 / hz) - 1;
+
+		/* Calculate retry count after a WAIT response. This will give
+		 * a retry timeout at about ~250 ms. 54 is the number of bits
+		 * found in a transaction. */
+		uint16_t retry_count = 250 * hz / 1000 / 54;
+
+		LOG_DEBUG("SWD delay: %d, retry count: %d", delay, retry_count);
+
+		versaloon_interface.adaptors.swd.config(0, 2, retry_count, delay);
+	}
+
+	return hz;
+}
+
+static int vsllink_swd_switch_seq(struct adiv5_dap *dap,
+		enum swd_special_seq seq)
+{
+	switch (seq) {
+	case LINE_RESET:
+		LOG_DEBUG("SWD line reset");
+		versaloon_interface.adaptors.swd.seqout(0, swd_seq_line_reset,
+				swd_seq_line_reset_len);
+		break;
+	case JTAG_TO_SWD:
+		LOG_DEBUG("JTAG-to-SWD");
+		versaloon_interface.adaptors.swd.seqout(0, swd_seq_jtag_to_swd,
+				swd_seq_jtag_to_swd_len);
+		break;
+	case SWD_TO_JTAG:
+		LOG_DEBUG("SWD-to-JTAG");
+		versaloon_interface.adaptors.swd.seqout(0, swd_seq_swd_to_jtag,
+				swd_seq_swd_to_jtag_len);
+		break;
+	default:
+		LOG_ERROR("Sequence %d not supported", seq);
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
+static void vsllink_swd_read_reg(struct adiv5_dap *dap, uint8_t cmd,
+		uint32_t *value)
+{
+	versaloon_interface.adaptors.swd.transact(0, cmd, value, NULL);
+}
+
+static void vsllink_swd_write_reg(struct adiv5_dap *dap, uint8_t cmd,
+		uint32_t value)
+{
+	versaloon_interface.adaptors.swd.transact(0, cmd, &value, NULL);
+}
+
+static int vsllink_swd_run_queue(struct adiv5_dap *dap)
+{
+	return versaloon_interface.adaptors.peripheral_commit();
+}
+
 /****************************************************************************
  * VSLLink USB low-level functions */
 
-static uint8_t usb_check_string(usb_dev_handle *usb, uint8_t stringidx,
-	char *string, char *buff, uint16_t buf_size)
+static int vsllink_check_usb_strings(
+	struct libusb_device_handle *usb_device_handle,
+	struct libusb_device_descriptor *usb_desc)
 {
-	int len;
-	uint8_t alloced = 0;
-	uint8_t ret = 1;
+	char desc_string[256];
+	int retval;
 
-	if (NULL == buff) {
-		buf_size = 256;
-		buff = malloc(buf_size);
-		if (NULL == buff) {
-			ret = 0;
-			goto free_and_return;
-		}
-		alloced = 1;
+	if (NULL != versaloon_interface.usb_setting.serialstring) {
+		retval = libusb_get_string_descriptor_ascii(usb_device_handle,
+			usb_desc->iSerialNumber, (unsigned char *)desc_string,
+			sizeof(desc_string));
+		if (retval < 0)
+			return ERROR_FAIL;
+
+		if (strncmp(desc_string, versaloon_interface.usb_setting.serialstring,
+				sizeof(desc_string)))
+			return ERROR_FAIL;
 	}
 
-	strcpy(buff, "");
-	len = usb_get_string_simple(usb, stringidx, buff, buf_size);
-	if ((len < 0) || ((size_t)len != strlen(buff))) {
-		ret = 0;
-		goto free_and_return;
-	}
+	retval = libusb_get_string_descriptor_ascii(usb_device_handle,
+		usb_desc->iProduct, (unsigned char *)desc_string,
+		sizeof(desc_string));
+	if (retval < 0)
+		return ERROR_FAIL;
 
-	buff[len] = '\0';
-	if ((string != NULL) && strcmp(buff, string)) {
-		ret = 0;
-		goto free_and_return;
-	}
+	if (strstr(desc_string, "Versaloon") == NULL)
+		return ERROR_FAIL;
 
-free_and_return:
-	if (alloced && (buff != NULL)) {
-		free(buff);
-		buff = NULL;
-	}
-	return ret;
+	return ERROR_OK;
 }
 
-static usb_dev_handle *find_usb_device(uint16_t VID, uint16_t PID, uint8_t interface,
-		char *serialstring, char *productstring)
+static int vsllink_usb_open(struct vsllink *vsllink)
 {
-	usb_dev_handle *dev_handle = NULL;
-	struct usb_bus *busses;
-	struct usb_bus *bus;
-	struct usb_device *dev;
+	ssize_t num_devices, i;
+	libusb_device **usb_devices;
+	struct libusb_device_descriptor usb_desc;
+	struct libusb_device_handle *usb_device_handle;
+	int retval;
 
-	usb_init();
-	usb_find_busses();
-	usb_find_devices();
-	busses = usb_get_busses();
+	num_devices = libusb_get_device_list(vsllink->libusb_ctx, &usb_devices);
 
-	for (bus = busses; bus; bus = bus->next) {
-		for (dev = bus->devices; dev; dev = dev->next) {
-			if ((dev->descriptor.idVendor == VID)
-			    && (dev->descriptor.idProduct == PID)) {
-				dev_handle = usb_open(dev);
-				if (NULL == dev_handle) {
-					LOG_ERROR("failed to open %04X:%04X, %s", VID, PID,
-						usb_strerror());
-					continue;
-				}
+	if (num_devices <= 0)
+		return ERROR_FAIL;
 
-				/* check description string */
-				if ((productstring != NULL && !usb_check_string(dev_handle,
-						dev->descriptor.iProduct, productstring, NULL, 0))
-					|| (serialstring != NULL && !usb_check_string(dev_handle,
-								dev->descriptor.iSerialNumber, serialstring, NULL, 0))) {
-					usb_close(dev_handle);
-					dev_handle = NULL;
-					continue;
-				}
+	for (i = 0; i < num_devices; i++) {
+		libusb_device *device = usb_devices[i];
 
-				if (usb_claim_interface(dev_handle, interface) != 0) {
-					LOG_ERROR(ERRMSG_FAILURE_OPERATION_MESSAGE,
-						"claim interface", usb_strerror());
-					usb_close(dev_handle);
-					dev_handle = NULL;
-					continue;
-				}
+		retval = libusb_get_device_descriptor(device, &usb_desc);
+		if (retval != 0)
+			continue;
 
-				if (dev_handle != NULL)
-					return dev_handle;
-			}
-		}
+		if (usb_desc.idVendor != versaloon_interface.usb_setting.vid ||
+			usb_desc.idProduct != versaloon_interface.usb_setting.pid)
+			continue;
+
+		retval = libusb_open(device, &usb_device_handle);
+		if (retval != 0)
+			continue;
+
+		retval = vsllink_check_usb_strings(usb_device_handle, &usb_desc);
+		if (ERROR_OK == retval)
+			break;
+
+		libusb_close(usb_device_handle);
 	}
 
-	return dev_handle;
-}
+	libusb_free_device_list(usb_devices, 1);
 
-static struct vsllink *vsllink_usb_open(void)
-{
-	usb_init();
+	if (i == num_devices)
+		return ERROR_FAIL;
 
-	struct usb_dev_handle *dev;
+	retval = libusb_claim_interface(usb_device_handle,
+		versaloon_interface.usb_setting.interface);
+	if (retval != 0) {
+		LOG_ERROR("unable to claim interface");
+		libusb_close(usb_device_handle);
+		return ERROR_FAIL;
+	}
 
-	dev = find_usb_device(versaloon_interface.usb_setting.vid,
-			versaloon_interface.usb_setting.pid,
-			versaloon_interface.usb_setting.interface,
-			versaloon_interface.usb_setting.serialstring, "Versaloon");
-	if (NULL == dev)
-		return NULL;
-
-	struct vsllink *result = malloc(sizeof(struct vsllink));
-	result->usb_handle = dev;
-	return result;
+	vsllink->usb_device_handle = usb_device_handle;
+	return ERROR_OK;
 }
 
 static void vsllink_usb_close(struct vsllink *vsllink)
 {
-	int ret;
-
-	ret = usb_release_interface(vsllink->usb_handle,
-			versaloon_interface.usb_setting.interface);
-	if (ret != 0) {
-		LOG_ERROR("fail to release interface %d, %d returned",
-			versaloon_interface.usb_setting.interface, ret);
-		exit(-1);
-	}
-
-	ret = usb_close(vsllink->usb_handle);
-	if (ret != 0) {
-		LOG_ERROR("fail to close usb, %d returned", ret);
-		exit(-1);
-	}
-
-	free(vsllink);
+	libusb_release_interface(vsllink->usb_device_handle,
+		versaloon_interface.usb_setting.interface);
+	libusb_close(vsllink->usb_device_handle);
 }
 
 #define BYTES_PER_LINE  16
@@ -849,13 +956,23 @@ static const struct command_registration vsllink_command_handlers[] = {
 	COMMAND_REGISTRATION_DONE
 };
 
-static const char *vsllink_transports[] = {"jtag", "swd", NULL};
+static const char * const vsllink_transports[] = {"jtag", "swd", NULL};
+
+static const struct swd_driver vsllink_swd_driver = {
+	.init = vsllink_swd_init,
+	.frequency = vsllink_swd_frequency,
+	.switch_seq = vsllink_swd_switch_seq,
+	.read_reg = vsllink_swd_read_reg,
+	.write_reg = vsllink_swd_write_reg,
+	.run = vsllink_swd_run_queue,
+};
 
 struct jtag_interface vsllink_interface = {
 	.name = "vsllink",
 	.supported = DEBUG_CAP_TMS_SEQ,
 	.commands = vsllink_command_handlers,
 	.transports = vsllink_transports,
+	.swd = &vsllink_swd_driver,
 
 	.init = vsllink_init,
 	.quit = vsllink_quit,

src/jtag/hla/hla_interface.c

@@ -37,7 +37,7 @@
 
 #include <target/target.h>
 
-static struct hl_interface_s hl_if = { {0, 0, 0, 0, 0, HL_TRANSPORT_UNKNOWN, false, NULL, 0}, 0, 0 };
+static struct hl_interface_s hl_if = { {0, 0, 0, 0, 0, HL_TRANSPORT_UNKNOWN, false, -1}, 0, 0 };
 
 int hl_interface_open(enum hl_transports tr)
 {
@@ -92,8 +92,11 @@ int hl_interface_init_target(struct target *t)
 	}
 
 	if (found == 0) {
-		LOG_ERROR("hl_interface_init_target: target not found: idcode: 0x%08" PRIx32,
-				t->tap->idcode);
+		LOG_WARNING("UNEXPECTED idcode: 0x%08" PRIx32, t->tap->idcode);
+		for (ii = 0; ii < limit; ii++)
+			LOG_ERROR("expected %u of %u: 0x%08" PRIx32, ii + 1, limit,
+				t->tap->expected_ids[ii]);
+
 		return ERROR_FAIL;
 	}
 
@@ -115,11 +118,8 @@ static int hl_interface_quit(void)
 {
 	LOG_DEBUG("hl_interface_quit");
 
-	if (hl_if.param.trace_f) {
-		fclose(hl_if.param.trace_f);
-		hl_if.param.trace_f = NULL;
-	}
-	hl_if.param.trace_source_hz = 0;
+	if (hl_if.layout->api->close)
+		hl_if.layout->api->close(hl_if.handle);
 
 	return ERROR_OK;
 }
@@ -145,6 +145,71 @@ int hl_interface_init_reset(void)
 	return ERROR_OK;
 }
 
+static int hl_interface_khz(int khz, int *jtag_speed)
+{
+	if (hl_if.layout->api->speed == NULL)
+		return ERROR_OK;
+
+	*jtag_speed = hl_if.layout->api->speed(hl_if.handle, khz, true);
+	return ERROR_OK;
+}
+
+static int hl_interface_speed_div(int speed, int *khz)
+{
+	*khz = speed;
+	return ERROR_OK;
+}
+
+static int hl_interface_speed(int speed)
+{
+	if (hl_if.layout->api->speed == NULL)
+		return ERROR_OK;
+
+	if (hl_if.handle == NULL) {
+		/* pass speed as initial param as interface not open yet */
+		hl_if.param.initial_interface_speed = speed;
+		return ERROR_OK;
+	}
+
+	hl_if.layout->api->speed(hl_if.handle, speed, false);
+
+	return ERROR_OK;
+}
+
+int hl_interface_override_target(const char **targetname)
+{
+	if (hl_if.layout->api->override_target) {
+		if (hl_if.layout->api->override_target(*targetname)) {
+			*targetname = "hla_target";
+			return ERROR_OK;
+		} else
+			return ERROR_FAIL;
+	}
+	return ERROR_FAIL;
+}
+
+int hl_interface_config_trace(bool enabled, enum tpio_pin_protocol pin_protocol,
+			      uint32_t port_size, unsigned int *trace_freq)
+{
+	if (hl_if.layout->api->config_trace)
+		return hl_if.layout->api->config_trace(hl_if.handle, enabled, pin_protocol,
+						       port_size, trace_freq);
+	else if (enabled) {
+		LOG_ERROR("The selected interface does not support tracing");
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
+int hl_interface_poll_trace(uint8_t *buf, size_t *size)
+{
+	if (hl_if.layout->api->poll_trace)
+		return hl_if.layout->api->poll_trace(hl_if.handle, buf, size);
+
+	return ERROR_FAIL;
+}
+
 COMMAND_HANDLER(hl_interface_handle_device_desc_command)
 {
 	LOG_DEBUG("hl_interface_handle_device_desc_command");
@@ -214,27 +279,17 @@ COMMAND_HANDLER(hl_interface_handle_vid_pid_command)
 	return ERROR_OK;
 }
 
-COMMAND_HANDLER(interface_handle_trace_command)
+COMMAND_HANDLER(interface_handle_hla_command)
 {
-	FILE *f = NULL;
-	unsigned source_hz;
-
-	if ((CMD_ARGC < 1) || (CMD_ARGC > 2))
+	if (CMD_ARGC != 1)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
-	COMMAND_PARSE_NUMBER(uint, CMD_ARGV[0], source_hz);
-	if (source_hz == 0) {
-		return ERROR_COMMAND_SYNTAX_ERROR;
+	if (!hl_if.layout->api->custom_command) {
+		LOG_ERROR("The selected adapter doesn't support custom commands");
+		return ERROR_FAIL;
 	}
 
-	if (CMD_ARGC == 2) {
-		f = fopen(CMD_ARGV[1], "a");
-		if (!f)
-			return ERROR_COMMAND_SYNTAX_ERROR;
-	}
-
-	hl_if.param.trace_f = f;
-	hl_if.param.trace_source_hz = source_hz;
+	hl_if.layout->api->custom_command(hl_if.handle, CMD_ARGV[0]);
 
 	return ERROR_OK;
 }
@@ -269,11 +324,11 @@ static const struct command_registration hl_interface_command_handlers[] = {
 	 .usage = "(vid pid)* ",
 	 },
 	 {
-	 .name = "trace",
-	 .handler = &interface_handle_trace_command,
-	 .mode = COMMAND_CONFIG,
-	 .help = "configure trace reception",
-	 .usage = "source_lock_hz [destination_path]",
+	 .name = "hla_command",
+	 .handler = &interface_handle_hla_command,
+	 .mode = COMMAND_EXEC,
+	 .help = "execute a custom adapter-specific command",
+	 .usage = "hla_command <command>",
 	 },
 	COMMAND_REGISTRATION_DONE
 };
@@ -286,4 +341,9 @@ struct jtag_interface hl_interface = {
 	.init = hl_interface_init,
 	.quit = hl_interface_quit,
 	.execute_queue = hl_interface_execute_queue,
+	.speed = &hl_interface_speed,
+	.khz = &hl_interface_khz,
+	.speed_div = &hl_interface_speed_div,
+	.config_trace = &hl_interface_config_trace,
+	.poll_trace = &hl_interface_poll_trace,
 };

src/jtag/hla/hla_interface.h

@@ -33,9 +33,9 @@ extern const char *hl_transports[];
 
 struct hl_interface_param_s {
 	/** */
-	char *device_desc;
+	const char *device_desc;
 	/** */
-	char *serial;
+	const char *serial;
 	/** */
 	uint16_t vid;
 	/** */
@@ -46,10 +46,8 @@ struct hl_interface_param_s {
 	enum hl_transports transport;
 	/** */
 	bool connect_under_reset;
-	/** Output file for trace data (if any) */
-	FILE *trace_f;
-	/** Trace module source clock rate */
-	uint32_t trace_source_hz;
+	/** Initial interface clock clock speed */
+	int initial_interface_speed;
 };
 
 struct hl_interface_s {
@@ -67,5 +65,6 @@ int hl_interface_open(enum hl_transports tr);
 
 int hl_interface_init_target(struct target *t);
 int hl_interface_init_reset(void);
+int hl_interface_override_target(const char **targetname);
 
 #endif /* _HL_INTERFACE */

src/jtag/hla/hla_layout.h

@@ -24,6 +24,8 @@
 #ifndef _HL_LAYOUT_H
 #define _HL_LAYOUT_H
 
+#include <target/armv7m_trace.h>
+
 /** */
 struct hl_interface_s;
 struct hl_interface_param_s;
@@ -74,7 +76,38 @@ struct hl_layout_api_s {
 	 */
 	int (*idcode) (void *handle, uint32_t *idcode);
 	/** */
-	enum target_state (*state) (void *handle);
+	int (*override_target) (const char *targetname);
+	/** */
+	int (*custom_command) (void *handle, const char *command);
+	/** */
+	int (*speed)(void *handle, int khz, bool query);
+	/**
+	 * Configure trace parameters for the adapter
+	 *
+	 * @param handle A handle to adapter
+	 * @param enabled Whether to enable trace
+	 * @param pin_protocol Configured pin protocol
+	 * @param port_size Trace port width for sync mode
+	 * @param trace_freq A pointer to the configured trace
+	 * frequency; if it points to 0, the adapter driver must write
+	 * its maximum supported rate there
+	 * @returns ERROR_OK on success, an error code on failure.
+	 */
+	int (*config_trace)(void *handle, bool enabled, enum tpio_pin_protocol pin_protocol,
+			    uint32_t port_size, unsigned int *trace_freq);
+	/**
+	 * Poll for new trace data
+	 *
+	 * @param handle A handle to adapter
+	 * @param buf A pointer to buffer to store received data
+	 * @param size A pointer to buffer size; must be filled with
+	 * the actual amount of bytes written
+	 *
+	 * @returns ERROR_OK on success, an error code on failure.
+	 */
+	int (*poll_trace)(void *handle, uint8_t *buf, size_t *size);
+	/** */
+	enum target_state (*state) (void *fd);
 };
 
 /** */

src/jtag/hla/hla_tcl.c

@@ -59,7 +59,13 @@ static int jim_newtap_expected_id(Jim_Nvp *n, Jim_GetOptInfo *goi,
 	return JIM_OK;
 }
 
-#define NTAP_OPT_EXPECTED_ID 0
+#define NTAP_OPT_IRLEN     0
+#define NTAP_OPT_IRMASK    1
+#define NTAP_OPT_IRCAPTURE 2
+#define NTAP_OPT_ENABLED   3
+#define NTAP_OPT_DISABLED  4
+#define NTAP_OPT_EXPECTED_ID 5
+#define NTAP_OPT_VERSION   6
 
 static int jim_hl_newtap_cmd(Jim_GetOptInfo *goi)
 {
@@ -69,8 +75,14 @@ static int jim_hl_newtap_cmd(Jim_GetOptInfo *goi)
 	Jim_Nvp *n;
 	char *cp;
 	const Jim_Nvp opts[] = {
-		{.name = "-expected-id", .value = NTAP_OPT_EXPECTED_ID},
-		{.name = NULL, .value = -1},
+		{ .name = "-irlen",       .value = NTAP_OPT_IRLEN },
+		{ .name = "-irmask",       .value = NTAP_OPT_IRMASK },
+		{ .name = "-ircapture",       .value = NTAP_OPT_IRCAPTURE },
+		{ .name = "-enable",       .value = NTAP_OPT_ENABLED },
+		{ .name = "-disable",       .value = NTAP_OPT_DISABLED },
+		{ .name = "-expected-id",       .value = NTAP_OPT_EXPECTED_ID },
+		{ .name = "-ignore-version",       .value = NTAP_OPT_VERSION },
+		{ .name = NULL, .value = -1},
 	};
 
 	pTap = calloc(1, sizeof(struct jtag_tap));
@@ -121,6 +133,12 @@ static int jim_hl_newtap_cmd(Jim_GetOptInfo *goi)
 				return e;
 			}
 			break;
+		case NTAP_OPT_IRLEN:
+		case NTAP_OPT_IRMASK:
+		case NTAP_OPT_IRCAPTURE:
+			/* dummy read to ignore the next argument */
+			Jim_GetOpt_Wide(goi, NULL);
+			break;
 		}		/* switch (n->value) */
 	}			/* while (goi->argc) */
 

src/jtag/hla/hla_transport.c

@@ -134,6 +134,12 @@ static const struct command_registration stlink_transport_command_handlers[] = {
 	 .usage = "",
 	 .chain = hl_transport_jtag_subcommand_handlers,
 	 },
+	{
+	 .name = "jtag_ntrst_delay",
+	 .mode = COMMAND_ANY,
+	 .handler = hl_transport_jtag_command,
+	 .usage = "",
+	 },
 	COMMAND_REGISTRATION_DONE
 };
 
@@ -204,12 +210,14 @@ static struct transport hl_swd_transport = {
 	.name = "hla_swd",
 	.select = hl_transport_select,
 	.init = hl_transport_init,
+	.override_target = hl_interface_override_target,
 };
 
 static struct transport hl_jtag_transport = {
 	.name = "hla_jtag",
 	.select = hl_transport_select,
 	.init = hl_transport_init,
+	.override_target = hl_interface_override_target,
 };
 
 static struct transport stlink_swim_transport = {

src/jtag/interface.h

@@ -28,6 +28,7 @@
 #define OPENOCD_JTAG_INTERFACE_H
 
 #include <jtag/jtag.h>
+#include <target/armv7m_trace.h>
 
 /* @file
  * The "Cable Helper API" is what the cable drivers can use to help
@@ -198,7 +199,7 @@ static inline tap_state_t jtag_debug_state_machine(const void *tms_buf,
  */
 struct jtag_interface {
 	/** The name of the JTAG interface driver. */
-	char *name;
+	const char * const name;
 
 	/**
 	 * Bit vector listing capabilities exposed by this driver.
@@ -207,7 +208,7 @@ struct jtag_interface {
 #define DEBUG_CAP_TMS_SEQ	(1 << 0)
 
 	/** transports supported in C code (NULL terminated vector) */
-	const char **transports;
+	const char * const *transports;
 
 	const struct swd_driver *swd;
 
@@ -298,11 +299,39 @@ struct jtag_interface {
 	 * @returns ERROR_OK on success, or an error code on failure.
 	 */
 	int (*srst_asserted)(int *srst_asserted);
+
+	/**
+	 * Configure trace parameters for the adapter
+	 *
+	 * @param enabled Whether to enable trace
+	 * @param pin_protocol Configured pin protocol
+	 * @param port_size Trace port width for sync mode
+	 * @param trace_freq A pointer to the configured trace
+	 * frequency; if it points to 0, the adapter driver must write
+	 * its maximum supported rate there
+	 * @returns ERROR_OK on success, an error code on failure.
+	 */
+	int (*config_trace)(bool enabled, enum tpio_pin_protocol pin_protocol,
+			    uint32_t port_size, unsigned int *trace_freq);
+
+	/**
+	 * Poll for new trace data
+	 *
+	 * @param buf A pointer to buffer to store received data
+	 * @param size A pointer to buffer size; must be filled with
+	 * the actual amount of bytes written
+	 *
+	 * @returns ERROR_OK on success, an error code on failure.
+	 */
+	int (*poll_trace)(uint8_t *buf, size_t *size);
 };
 
-extern const char *jtag_only[];
+extern const char * const jtag_only[];
 
 void adapter_assert_reset(void);
 void adapter_deassert_reset(void);
+int adapter_config_trace(bool enabled, enum tpio_pin_protocol pin_protocol,
+			 uint32_t port_size, unsigned int *trace_freq);
+int adapter_poll_trace(uint8_t *buf, size_t *size);
 
 #endif /* OPENOCD_JTAG_INTERFACE_H */

src/jtag/startup.tcl

@@ -21,7 +21,9 @@ proc jtag_init {} {
 # startup (at OpenOCD server startup, when JTAG may not yet work); and
 # potentially more (for reset types like cold, warm, etc)
 proc init_reset { mode } {
-	jtag arp_init-reset
+	if {[using_jtag]} {
+		jtag arp_init-reset
+	}
 }
 
 #########
@@ -79,12 +81,38 @@ proc srst_asserted {} {
 # measure actual JTAG clock
 proc measure_clk {} {
 	set start_time [ms];
-	runtest 10000000; 
-	echo "Running at more than [expr 10000.0 / ([ms]-$start_time)] kHz";
+        set iterations 10000000;
+	runtest $iterations;
+	echo "Running at more than [expr $iterations.0 / ([ms]-$start_time)] kHz";
 }
 
 add_help_text measure_clk "Runs a test to measure the JTAG clk. Useful with RCLK / RTCK."
 
+proc default_to_jtag { f args } {
+	set current_transport [transport select]
+	if {[using_jtag]} {
+		eval $f $args
+	} {
+		error "session transport is \"$current_transport\" but your config requires JTAG"
+	}
+}
+
+proc jtag args {
+	eval default_to_jtag jtag $args
+}
+
+proc jtag_rclk args {
+	eval default_to_jtag jtag_rclk $args
+}
+
+proc jtag_ntrst_delay args {
+	eval default_to_jtag jtag_ntrst_delay $args
+}
+
+proc jtag_ntrst_assert_width args {
+	eval default_to_jtag jtag_ntrst_assert_width $args
+}
+
 # BEGIN MIGRATION AIDS ...  these adapter operations originally had
 # JTAG-specific names despite the fact that the operations were not
 # specific to JTAG, or otherewise had troublesome/misleading names.

src/jtag/swd.h

@@ -20,6 +20,8 @@
 #ifndef SWD_H
 #define SWD_H
 
+#include <target/arm_adi_v5.h>
+
 /* Bits in SWD command packets, written from host to target
  * first bit on the wire is START
  */
@@ -29,20 +31,9 @@
 #define SWD_CMD_A32	(3 << 3)		/* bits A[3:2] of register addr */
 #define SWD_CMD_PARITY	(1 << 5)	/* parity of APnDP|RnW|A32 */
 #define SWD_CMD_STOP	(0 << 6)	/* always clear for synch SWD */
-#define SWD_CMD_PARK	(0 << 7)	/* not driven by host (pull high) */
+#define SWD_CMD_PARK	(1 << 7)	/* driven high by host */
 /* followed by TRN, 3-bits of ACK, TRN */
 
-/* pbit16 holds precomputed parity bits for each nibble */
-#define pbit(parity, nibble) (parity << nibble)
-
-static const uint16_t pbit16 =
-	pbit(0, 0) | pbit(1, 1) | pbit(1, 2) | pbit(0, 3)
-	| pbit(1, 4) | pbit(0, 5) | pbit(0, 6) | pbit(1, 7)
-	| pbit(1, 8) | pbit(0, 9) | pbit(0, 0xa) | pbit(1, 0xb)
-	| pbit(0, 0xc) | pbit(1, 0xd) | pbit(1, 0xe) | pbit(0, 0xf);
-
-#define nibble_parity(nibble) (pbit16 & pbit(1, nibble))
-
 /**
  * Construct a "cmd" byte, in lSB bit order, which swd_driver.read_reg()
  * and swd_driver.write_reg() methods will use directly.
@@ -54,7 +45,7 @@ static inline uint8_t swd_cmd(bool is_read, bool is_ap, uint8_t regnum)
 		| ((regnum & 0xc) << 1);
 
 	/* 8 cmd bits 4:1 may be set */
-	if (nibble_parity(cmd >> 1))
+	if (parity_u32(cmd))
 		cmd |= SWD_CMD_PARITY;
 
 	/* driver handles START, STOP, and TRN */
@@ -64,63 +55,146 @@ 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> */
 
-/*
- * FOR NOW  ... SWD driver ops are synchronous and return ACK
- * status ... no quueueing.
+/**
+ * Line reset.
  *
- * Individual ops are request/response, and fast-fail permits much
- * better fault handling.  Upper layers may queue if desired.
+ * Line reset is at least 50 SWCLK cycles with SWDIO driven high, followed
+ * by at least one idle (low) cycle.
  */
+static const uint8_t swd_seq_line_reset[] = {
+	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x03
+};
+static const unsigned swd_seq_line_reset_len = 51;
+
+/**
+ * JTAG-to-SWD sequence.
+ *
+ * The JTAG-to-SWD 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 a line reset in case the SWJ-DP was
+ * already in SWD mode.
+ */
+static const uint8_t swd_seq_jtag_to_swd[] = {
+	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x7b, 0x9e,
+	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x0f,
+};
+static const unsigned swd_seq_jtag_to_swd_len = 118;
+
+/**
+ * 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.
+ */
+static const uint8_t swd_seq_swd_to_jtag[] = {
+	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf3, 0x9c, 0xff
+};
+static const unsigned swd_seq_swd_to_jtag_len = 71;
+
+/**
+ * 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.
+ */
+static const uint8_t swd_seq_swd_to_dormant[] = {
+	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf3, 0x8e, 0x03
+};
+static const unsigned swd_seq_swd_to_dormant_len = 66;
+
+/**
+ * Dormant-to-SWD 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 SWD the activation code
+ * is an 8-bit sequence. The sequence ends with a line reset.
+ */
+static const uint8_t swd_seq_dormant_to_swd[] = {
+	0xff,
+	0x92, 0xf3, 0x09, 0x62, 0x95, 0x2d, 0x85, 0x86,
+	0xe9, 0xaf, 0xdd, 0xe3, 0xa2, 0x0e, 0xbc, 0x19,
+	0x10, 0xfa, 0xff, 0xff, 0xff, 0xff, 0xff, 0x3f
+};
+static const unsigned swd_seq_dormant_to_swd_len = 199;
+
+enum swd_special_seq {
+	LINE_RESET,
+	JTAG_TO_SWD,
+	SWD_TO_JTAG,
+	SWD_TO_DORMANT,
+	DORMANT_TO_SWD,
+};
 
 struct swd_driver {
 	/**
-	 * Initialize the debug link so it can perform
-	 * synchronous SWD operations.
-	 * @param trn value from WCR: how many clocks
-	 * to not drive the SWDIO line at certain points in
-	 * the SWD protocol (at least 1 clock).
+	 * Initialize the debug link so it can perform SWD operations.
 	 *
 	 * As an example, this would switch a dual-mode debug adapter
 	 * into SWD mode and out of JTAG mode.
-	  *
-	  * @return ERROR_OK on success, else a negative fault code.
+	 *
+	 * @return ERROR_OK on success, else a negative fault code.
 	 */
-	int (*init)(uint8_t trn);
+	int (*init)(void);
 
+	/**
+	 * Set the SWCLK frequency of the SWD link.
+	 *
+	 * The driver should round the desired value, downwards if possible, to
+	 * the nearest supported frequency. A negative value should be ignored
+	 * and can be used to query the current setting. If the driver does not
+	 * support a variable frequency a fixed, nominal, value should be
+	 * returned.
+	 *
+	 * If the frequency is increased, it must not apply before the currently
+	 * queued transactions are executed. If the frequency is lowered, it may
+	 * apply immediately.
+	 *
+	 * @param dap The DAP controlled by the SWD link.
+	 * @param hz The desired frequency in Hz.
+	 * @return The actual resulting frequency after rounding.
+	 */
+	int_least32_t (*frequency)(struct adiv5_dap *dap, int_least32_t hz);
 
-	 /**
-	  * Synchronous read of an AP or DP register.
-	  *
-	  * @param cmd with APnDP/RnW/addr/parity bits
-	  * @param where to store value to read from register
-	  *
-	  * @return SWD_ACK_* code for the transaction
-	  *		or (negative) fault code
-	  */
-	 int (*read_reg)(uint8_t cmd, uint32_t *value);
+	/**
+	 * Queue a special SWDIO sequence.
+	 *
+	 * @param dap The DAP controlled by the SWD link.
+	 * @param seq The special sequence to generate.
+	 * @return ERROR_OK if the sequence was queued, negative error if the
+	 * sequence is unsupported.
+	 */
+	int (*switch_seq)(struct adiv5_dap *dap, enum swd_special_seq seq);
 
-	 /**
-	  * Synchronous write of an AP or DP register.
-	  *
-	  * @param cmd with APnDP/RnW/addr/parity bits
-	  * @param value to be written to the register
-	  *
-	  * @return SWD_ACK_* code for the transaction
-	  *		or (negative) fault code
-	  */
-	 int (*write_reg)(uint8_t cmd, uint32_t value);
+	/**
+	 * Queued read of an AP or DP register.
+	 *
+	 * @param dap The DAP controlled by the SWD link.
+	 * @param Command byte with APnDP/RnW/addr/parity bits
+	 * @param Where to store value to read from register
+	 */
+	void (*read_reg)(struct adiv5_dap *dap, uint8_t cmd, uint32_t *value);
 
-	 /**
-	  * Synchronous block read of an AP or DP register.
-	  *
-	  * @param cmd with APnDP/RnW/addr/parity bits
-	  * @param number of reads from register to be executed
-	  * @param buffer to store data read from register
-	  *
-	  * @return SWD_ACK_* code for the transaction
-	  *		or (negative) fault code
-	  */
-	 int (*read_block)(uint8_t cmd, uint32_t blocksize, uint8_t *buffer);
+	/**
+	 * Queued write of an AP or DP register.
+	 *
+	 * @param dap The DAP controlled by the SWD link.
+	 * @param Command byte with APnDP/RnW/addr/parity bits
+	 * @param Value to be written to the register
+	 */
+	void (*write_reg)(struct adiv5_dap *dap, uint8_t cmd, uint32_t value);
+
+	/**
+	 * Execute any queued transactions and collect the result.
+	 *
+	 * @param dap The DAP controlled by the SWD link.
+	 * @return ERROR_OK on success, Ack response code on WAIT/FAULT
+	 * or negative error code on other kinds of failure.
+	 */
+	int (*run)(struct adiv5_dap *dap);
 
 	/**
 	 * Configures data collection from the Single-wire
@@ -131,16 +205,15 @@ struct swd_driver {
 	 * is normally connected to a microcontroller's UART TX,
 	 * but which may instead be connected to SWO for use in
 	 * collecting ITM (and possibly ETM) trace data.
-	  *
-	  * @return ERROR_OK on success, else a negative fault code.
+	 *
+	 * @return ERROR_OK on success, else a negative fault code.
 	 */
-	int *(*trace)(bool swo);
+	int *(*trace)(struct adiv5_dap *dap, bool swo);
 };
 
 int swd_init_reset(struct command_context *cmd_ctx);
 void swd_add_reset(int req_srst);
 
 bool transport_is_swd(void);
-bool transport_is_cmsis_dap(void);
 
 #endif /* SWD_H */