The openocd-0.7.0-rc2 release candidate.

Change-Id: I0a3576dd098d73437547b619c726cacd8f1dba64 Signed-off-by: Freddie Chopin <freddie.chopin@gmail.com>
target: rename cortex_a8 to cortex_a
2013-04-28 12:38:42 +02:00 · 2013-04-28 08:56:04 +00:00 · 2013-04-28 08:55:31 +00:00 · 2013-04-28 08:22:46 +00:00 · 2013-04-28 07:38:53 +00:00 · 2013-04-28 07:37:03 +00:00
212 changed files with 11998 additions and 4250 deletions
--- a/Doxyfile.in
+++ b/Doxyfile.in
@@ -313,7 +313,7 @@ EXTRACT_STATIC         = YES
 # defined locally in source files will be included in the documentation.
 # If set to NO only classes defined in header files are included.

-EXTRACT_LOCAL_CLASSES  = NO
+EXTRACT_LOCAL_CLASSES  = YES

 # This flag is only useful for Objective-C code. When set to YES local
 # methods, which are defined in the implementation section but not in
--- a/49
+++ b/49
@@ -1,10 +1,10 @@
 // This file is part of the Doxygen Developer Manual
 /** @page patchguide Patch Guidelines

-@b NB! If you're behind a corporate wall with http only access to the
+\attention If you're behind a corporate wall with http only access to the
 world, you can still use these instructions!

-@b NB2! You can't send patches to the mailing list anymore at all. Nowadays
+\attention You can't send patches to the mailing list anymore at all. Nowadays
 you are expected to send patches to the OpenOCD Gerrit GIT server for a
 review.

@@ -47,16 +47,22 @@ Add yourself to the GPL copyright for non-trivial changes.
     add a username of your choice.
     Your username will be required in step 3 and substituted wherever
     the string 'USERNAME' is found.
-  -# Add an SSH public key following the directions on github:
-     https://help.github.com/articles/generating-ssh-keys
+  -# Create an SSH public key following the directions on github:
+     https://help.github.com/articles/generating-ssh-keys . You can skip step 3
+     (adding key to Github account) and 4 (testing) - these are useful only if
+     you actually use Github or want to test whether the new key works fine.
+  -# Add this new SSH key to your Gerrit account:
+     go to 'Settings' > 'SSH Public Keys', paste the contents of
+     ~/.ssh/id_rsa.pub into the text field (if it's not visible click on
+     'Add Key ...' button) and confirm by clicking 'Add' button.
 -# Clone the git repository, rather than just download the source:
 @code
- git clone git://openocd.git.sourceforge.net/gitroot/openocd/openocd
+ git clone git://git.code.sf.net/p/openocd/code openocd
 @endcode
   or if you have problems with the "git:" protocol, use
   the slower http protocol:
 @code
- git clone http://repo.or.cz/r/openocd.git
+ git clone http://git.code.sf.net/p/openocd/code openocd
 @endcode
 -# Set up Gerrit with your local repository. All this does it
 to instruct git locally how to send off the changes.
@@ -67,8 +73,13 @@ git config remote.review.push HEAD:refs/publish/master
@endcode
  Or with http only:
@code
-git remote add review http://openocd.zylin.com/p/openocd.git
+git remote add review http://USERNAME@openocd.zylin.com/p/openocd.git
 git config remote.review.push HEAD:refs/publish/master
+@endcode
+  The http password is configured from your gerrit settings - http://openocd.zylin.com/#/settings/http-password.
+  \note If you want to simplify http access you can also add your http password to the url as follows:
+@code
+git remote add review http://USERNAME:PASSWORD@openocd.zylin.com/p/openocd.git
@endcode
  -# You will need to install this hook, we will look into a better solution:
@code
@@ -80,7 +91,7 @@ wget http://openocd.zylin.com/tools/hooks/commit-msg
 mv commit-msg .git/hooks
 chmod +x .git/hooks/commit-msg
@endcode
-@b NOTE A script exists to simplify the two items above. execute:
+  \note A script exists to simplify the two items above. execute:
@code
 tools/initial.sh <username>
@endcode
@@ -101,16 +112,19 @@ while(!done) {
  run tools/checkpatch.sh to verify your patch style is ok.
 }
@endcode
-   @b TIP! use "git add ." before commit to add new files.
+   \note use "git add ." before commit to add new files.
+
+   Comment template, notice the short first line w/topic. The topic field
+   should identify the main part or subsystem the patch touches. Check
+   git log for examples.
@code
--- example comment, notice the short first line w/topic ---
-topic: short comment
+topic: Short comment
 <blank line>
-longer comments over several
-lines...
+Longer comments over several lines, explaining (where applicable) the
+reason for the patch and the general idea the solution is based on,
+any major design decisions, etc...
 <blank line>
 Signed-off-by: ...
-----
@endcode
 -# Next you need to make sure that your patches
   are on top of the latest stuff on the server and
@@ -149,6 +163,13 @@ master branch will be much reduced.
 If a contributor pushes a patch, it is considered good form if another
 contributor actually approves and submits that patch.

+It should be noted that a negative review in Gerrit ("-1" or "-2") may (but does
+not have to) be disregarded if all conditions listed below are met:
+
+- the concerns raised in the review have been addressed (or explained),
+- reviewer does not re-examine the change in a month,
+- reviewer does not answer e-mails for another month.
+
@section browsing Browsing Patches
 All OpenOCD patches can be reviewed <a href="http://openocd.zylin.com/">here</a>.
 */
--- a/39
+++ b/39
@@ -4,41 +4,30 @@ repository history for details about what changed, including
 bugfixes and other issues not mentioned here.

 JTAG Layer:
-	New STLINK V1/V2 JTAG/SWD adapter support.
-	New OSJTAG adapter support.
-	New Tincantools Flyswatter2 support.
-	Improved ULINK driver.
-	Improved RLINK driver.
-	Support for adapters based on FT232H chips.
-	New experimental driver for FTDI based adapters, using libusb-1.0 in asynchronous mode.
+	New TI ICDI adapter support.
+	Support Latest OSBDM firmware.
+	Improved MIPS EJTAG Support.

 Boundary Scan:

 Target Layer:
-	New Cortex-M0 support.
-	New Cortex-M4 support.
-	Improved Working area algorithm.
-	New RTOS support. Currently linux, FreeRTOS, ThreadX and eCos.
-	Connecting under reset to Cortex-Mx and MIPS chips.
+	New ARMv7R and Cortex-R4 support.
+	Added ChibiOS/RT support.

 Flash Layer:
-	New SST39WF1601 support.
-	New EN29LV800BB support.
-	New async algorithm support for selected targets, stm32, stellaris and pic32.
-	New Atmel SAM3S, SAM3N support.
-	New ST STM32L support.
-	New Microchip PIC32MX1xx/2xx support.
-	New Freescale Kinetis K40 support.
+	New NXP LPC1850 support.
+	New NXP LPC4300 support.
+	New NXP SPIFI support.
+	New Energy Micro EFM32 support.
+	New ST STM32W support.
+	New ST STM32f2 write protection and lock/unlock support.
+	Ability to override STM32 flash bank size.

 Board, Target, and Interface Configuration Scripts:
-	Support Dangerous Prototypes Bus Blaster.
-	Support ST SPEAr Family.
-	Support Gumstix Verdex boards.
-	Support TI Beaglebone.
+	Support Freescale i.MX6 series targets.

 Documentation:
-	Improved HACKING info for submitting patches.
-	Fixed numerous broken links.
+	New MIPS debugging info.

 Build and Release:

--- a/NEWS-0.6.0
+++ b/NEWS-0.6.0
@@ -0,0 +1,54 @@
+This file includes highlights of the changes made in the
+OpenOCD  source archive release.  See the
+repository history for details about what changed, including
+bugfixes and other issues not mentioned here.
+
+JTAG Layer:
+	New STLINK V1/V2 JTAG/SWD adapter support.
+	New OSJTAG adapter support.
+	New Tincantools Flyswatter2 support.
+	Improved ULINK driver.
+	Improved RLINK driver.
+	Support for adapters based on FT232H chips.
+	New experimental driver for FTDI based adapters, using libusb-1.0 in asynchronous mode.
+
+Boundary Scan:
+
+Target Layer:
+	New Cortex-M0 support.
+	New Cortex-M4 support.
+	Improved Working area algorithm.
+	New RTOS support. Currently linux, FreeRTOS, ThreadX and eCos.
+	Connecting under reset to Cortex-Mx and MIPS chips.
+
+Flash Layer:
+	New SST39WF1601 support.
+	New EN29LV800BB support.
+	New async algorithm support for selected targets, stm32, stellaris and pic32.
+	New Atmel SAM3S, SAM3N support.
+	New ST STM32L support.
+	New Microchip PIC32MX1xx/2xx support.
+	New Freescale Kinetis K40 support.
+
+Board, Target, and Interface Configuration Scripts:
+	Support Dangerous Prototypes Bus Blaster.
+	Support ST SPEAr Family.
+	Support Gumstix Verdex boards.
+	Support TI Beaglebone.
+
+Documentation:
+	Improved HACKING info for submitting patches.
+	Fixed numerous broken links.
+
+Build and Release:
+
+For more details about what has changed since the last release,
+see the git repository history.  With gitweb, you can browse that
+in various levels of detail.
+
+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).
--- a/11
+++ b/11
@@ -215,6 +215,8 @@ options may be available there:
                          (for x86 only)
  --enable-parport-giveio Enable use of giveio for parport (for CygWin only)

+  --enable-ftdi           Enable building support for the MPSSE mode of FTDI
+                          based devices, using libusb-1.0 in asynchronous mode

  --enable-ft2232_libftdi Enable building support for FT2232 based devices
                          using the libftdi driver, opensource alternate of
@@ -273,12 +275,16 @@ options may be available there:

  --enable-stlink         Enable building support for the ST-Link JTAG
                          Programmer
+  --enable-ti-icdi        Enable building support for the TI/Stellaris ICDI
+                          JTAG Programmer

  --enable-osbdm          Enable building support for the OSBDM (JTAG only)
                          Programmer

  --enable-opendous       Enable building support for the estick/opendous JTAG
                          Programmer
+  --enable-sysfsgpio      Enable building support for programming driven via
+                          sysfs gpios.

  --enable-minidriver-dummy
                          Enable the dummy minidriver.
@@ -417,7 +423,7 @@ Obtaining OpenOCD From GIT
 You can download the current GIT version with a GIT client of your
 choice from the main repository:

-   git://openocd.git.sourceforge.net/gitroot/openocd/openocd
+   git://git.code.sf.net/p/openocd/code

 You may prefer to use a mirror:

@@ -428,7 +434,7 @@ Using the GIT command line client, you might use the following command
 to set up a local copy of the current repository (make sure there is no
 directory called "openocd" in the current directory):

-   git clone git://openocd.git.sourceforge.net/gitroot/openocd/openocd
+   git clone git://git.code.sf.net/p/openocd/code openocd

 Then you can update that at your convenience using

@@ -437,7 +443,6 @@ Then you can update that at your convenience using
 There is also a gitweb interface, which you can use either to browse
 the repository or to download arbitrary snapshots using HTTP:

-   http://openocd.git.sourceforge.net/git/gitweb.cgi?p=openocd/openocd
   http://repo.or.cz/w/openocd.git

 Snapshots are compressed tarballs of the source tree, about 1.3 MBytes
--- a/configure.ac
+++ b/configure.ac
@@ -1,5 +1,5 @@
 AC_PREREQ(2.60)
-AC_INIT([openocd], [0.6.0-rc2],
+AC_INIT([openocd], [0.7.0-rc2],
  [OpenOCD Mailing List <openocd-devel@lists.sourceforge.net>])
 AC_CONFIG_SRCDIR([src/openocd.c])

@@ -8,7 +8,7 @@ m4_include([config_subdir.m4])dnl
 AM_INIT_AUTOMAKE([-Wall -Wno-portability dist-bzip2 dist-zip])
 AM_MAINTAINER_MODE

-AM_CONFIG_HEADER([config.h])
+AC_CONFIG_HEADERS([config.h])
 AH_BOTTOM([
 #include <helper/system.h>
 #include <helper/types.h>
@@ -224,7 +224,7 @@ __EOF__
 # In case (1) and (2) we need to know where the package was unpacked.

 AC_ARG_WITH(ftd2xx-win32-zipdir,
-  AS_HELP_STRING([--with-ftd2xx-win32-zipdir],[Where (CYGWIN/MINGW) the zip file from ftdichip.com was unpacked <default=search>]),
+  AS_HELP_STRING([--with-ftd2xx-win32-zipdir],[Where (CYGWIN/MINGW) the zip file from ftdichip.com was unpacked (default=search)]),
  [
  # option present
  if test -d $with_ftd2xx_win32_zipdir
@@ -237,7 +237,7 @@ AC_ARG_WITH(ftd2xx-win32-zipdir,
  ], [true])

 AC_ARG_WITH(ftd2xx-linux-tardir,
-  AS_HELP_STRING([--with-ftd2xx-linux-tardir], [Where (Linux/Unix) the tar file from ftdichip.com was unpacked <default=search>]),
+  AS_HELP_STRING([--with-ftd2xx-linux-tardir], [Where (Linux/Unix) the tar file from ftdichip.com was unpacked (default=search)]),
  [
  # Option present
  if test $is_win32 = yes ; then
@@ -254,7 +254,7 @@ AC_ARG_WITH(ftd2xx-linux-tardir,

 AC_ARG_WITH(ftd2xx-lib,
  AS_HELP_STRING([--with-ftd2xx-lib],
-    [Use static or shared ftd2xx libs on default static]),
+    [Use static or shared ftd2xx libs (default=static)]),
  [
  case "$withval" in
  static)
@@ -477,7 +477,11 @@ AC_ARG_ENABLE([buspirate],

 AC_ARG_ENABLE([stlink],
  AS_HELP_STRING([--enable-stlink], [Enable building support for the ST-Link JTAG Programmer]),
-  [build_stlink=$enableval], [build_stlink=no])
+  [build_hladapter_stlink=$enableval], [build_hladapter_stlink=no])
+
+AC_ARG_ENABLE([ti-icdi],
+  AS_HELP_STRING([--enable-ti-icdi], [Enable building support for the TI ICDI JTAG Programmer]),
+  [build_hladapter_icdi=$enableval], [build_hladapter_icdi=no])

 AC_ARG_ENABLE([osbdm],
  AS_HELP_STRING([--enable-osbdm], [Enable building support for the OSBDM (JTAG only) Programmer]),
@@ -487,6 +491,10 @@ AC_ARG_ENABLE([opendous],
  AS_HELP_STRING([--enable-opendous], [Enable building support for the estick/opendous JTAG Programmer]),
  [build_opendous=$enableval], [build_opendous=no])

+AC_ARG_ENABLE([sysfsgpio],
+  AS_HELP_STRING([--enable-sysfsgpio], [Enable building support for programming driven via sysfs gpios.]),
+  [build_sysfsgpio=$enableval], [build_sysfsgpio=no])
+
 AC_ARG_ENABLE([minidriver_dummy],
  AS_HELP_STRING([--enable-minidriver-dummy], [Enable the dummy minidriver.]),
  [build_minidriver_dummy=$enableval], [build_minidriver_dummy=no])
@@ -786,10 +794,10 @@ else
  AC_DEFINE([BUILD_BUSPIRATE], [0], [0 if you don't want the Buspirate JTAG driver.])
 fi

-if test $build_stlink = yes; then
-  AC_DEFINE([BUILD_STLINK], [1], [1 if you want the ST-Link JTAG driver.])
+if test $build_hladapter_stlink = yes -o $build_hladapter_icdi = yes; then
+  AC_DEFINE([BUILD_HLADAPTER], [1], [1 if you want the High Level JTAG driver.])
 else
-  AC_DEFINE([BUILD_STLINK], [0], [0 if you don't want the ST-Link JTAG driver.])
+  AC_DEFINE([BUILD_HLADAPTER], [0], [0 if you don't want the High Level JTAG driver.])
 fi

 if test $build_osbdm = yes; then
@@ -819,6 +827,12 @@ else
  AC_DEFINE([BUILD_REMOTE_BITBANG], [0], [0 if you don't want the Remote Bitbang JTAG driver.])
 fi

+if test $build_sysfsgpio = yes; then
+  build_bitbang=yes
+  AC_DEFINE([BUILD_SYSFSGPIO], [1], [1 if you want the SysfsGPIO driver.])
+else
+  AC_DEFINE([BUILD_SYSFSGPIO], [0], [0 if you don't want SysfsGPIO driver.])
+fi
 #-- Deal with MingW/Cygwin FTD2XX issues

 if test $is_win32 = yes; then
@@ -876,7 +890,7 @@ then
   AC_MSG_ERROR([The option: with_ftd2xx_linux_tardir is for LINUX only.])
 fi

-if test $build_ft2232_ftd2xx = yes -o $build_presto_ftd2xx = yes ; then
+if test $build_ft2232_ftd2xx = yes -o $build_presto_ftd2xx = yes -o $build_usb_blaster_ftd2xx = yes ; then
   AC_MSG_CHECKING([for libftd2xx.a (darwin)])

   if test ! -f /usr/local/include/ftd2xx.h ; then
@@ -897,7 +911,7 @@ then
   AC_MSG_ERROR([The option: --with-ftd2xx-win32-zipdir is for win32 only])
 fi

-if test $build_ft2232_ftd2xx = yes -o $build_presto_ftd2xx = yes ; then
+if test $build_ft2232_ftd2xx = yes -o $build_presto_ftd2xx = yes -o $build_usb_blaster_ftd2xx = yes ; then
   # Must be linux
   if test $host_os != linux-gnu && test $host_os != linux ; then
      AC_MSG_ERROR([The (linux) ftd2xx library from FTDICHIP.com is linux only. Try --enable-ft2232-libftdi instead])
@@ -947,7 +961,7 @@ if test $build_ft2232_ftd2xx = yes -o $build_presto_ftd2xx = yes ; then
      fi
    fi
    LDFLAGS="${LDFLAGS} ${FTD2XX_LDFLAGS}"
-    LIBS="${LIBS} ${FTD2XX_LIB}"
+    LIBS="${FTD2XX_LIB} ${LIBS}"
    AC_MSG_RESULT([${FTD2XX_LDFLAGS} ${FTD2XX_LIB}])
  else
    AC_CHECK_HEADER([ftd2xx.h],[],[
@@ -960,7 +974,7 @@ if test $build_ft2232_ftd2xx = yes -o $build_presto_ftd2xx = yes ; then
 fi
 fi # linux

-if test $build_ft2232_ftd2xx = yes -o $build_presto_ftd2xx = yes; then
+if test $build_ft2232_ftd2xx = yes -o $build_presto_ftd2xx = yes -o $build_usb_blaster_ftd2xx = yes ; then

 # Before we go any further - make sure we can *BUILD* and *RUN*
 # a simple app with the "ftd2xx.lib" file - in what ever form we where given
@@ -1132,8 +1146,8 @@ fi

 # Check for libusb1 ported drivers.
 build_usb_ng=no
-if test $build_jlink = yes -o $build_stlink = yes -o $build_osbdm = yes -o \
-  $build_opendous = yes -o $build_ftdi = yes
+if test $build_jlink = yes -o $build_hladapter_stlink = yes -o $build_osbdm = yes -o \
+  $build_opendous = yes -o $build_ftdi = yes -o $build_hladapter_icdi = yes
 then
  build_usb_ng=yes
 fi
@@ -1182,9 +1196,10 @@ AM_CONDITIONAL([ULINK], [test $build_ulink = yes])
 AM_CONDITIONAL([ARMJTAGEW], [test $build_armjtagew = yes])
 AM_CONDITIONAL([REMOTE_BITBANG], [test $build_remote_bitbang = yes])
 AM_CONDITIONAL([BUSPIRATE], [test $build_buspirate = yes])
-AM_CONDITIONAL([STLINK], [test $build_stlink = yes])
+AM_CONDITIONAL([HLADAPTER], [test $build_hladapter_stlink = yes -o $build_hladapter_icdi = yes])
 AM_CONDITIONAL([OSBDM], [test $build_osbdm = yes])
 AM_CONDITIONAL([OPENDOUS], [test $build_opendous = yes])
+AM_CONDITIONAL([SYSFSGPIO], [test $build_sysfsgpio = yes])
 AM_CONDITIONAL([USB], [test $build_usb = yes])
 AM_CONDITIONAL([USB_NG], [test $build_usb_ng = yes])
 AM_CONDITIONAL([USE_LIBUSB0], [test $use_libusb0 = yes])
@@ -1297,7 +1312,7 @@ AC_CONFIG_FILES([
  src/helper/Makefile
  src/jtag/Makefile
  src/jtag/drivers/Makefile
-  src/jtag/stlink/Makefile
+  src/jtag/hla/Makefile
  src/transport/Makefile
  src/xsvf/Makefile
  src/svf/Makefile
--- a/contrib/loaders/flash/armv7m_io.s
+++ b/contrib/loaders/flash/armv7m_io.s
@@ -0,0 +1,60 @@
+/***************************************************************************
+ *   Copyright (C) 2013 by Henrik Nilsson                                  *
+ *   henrik.nilsson@bytequest.se                                           *
+ *                                                                         *
+ *   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.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+
+	.text
+	.syntax unified
+	.arch armv7-m
+	.thumb
+	.thumb_func
+
+	.align 4
+
+/* Inputs:
+ *  r0	buffer address
+ *  r1	NAND data address (byte wide)
+ *  r2	buffer length
+ */
+read:
+	ldrb	r3, [r1]
+	strb	r3, [r0], #1
+	subs	r2, r2, #1
+	bne		read
+
+done_read:
+	bkpt #0
+
+	.align 4
+
+/* Inputs:
+ *  r0	NAND data address (byte wide)
+ *  r1	buffer address
+ *  r2	buffer length
+ */
+write:
+	ldrb	r3, [r1], #1
+	strb	r3, [r0]
+	subs	r2, r2, #1
+	bne		write
+
+done_write:
+	bkpt #0
+
+	.end
+
--- a/contrib/loaders/flash/efm32.S
+++ b/contrib/loaders/flash/efm32.S
@@ -0,0 +1,114 @@
+/***************************************************************************
+ *   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, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+
+	.text
+	.syntax unified
+	.cpu cortex-m0
+	.thumb
+	.thumb_func
+
+	/* Params:
+	 * r0 - flash base (in), status (out)
+	 * r1 - count (word-32bit)
+	 * r2 - workarea start
+	 * r3 - workarea end
+	 * r4 - target address
+	 * Clobbered:
+	 * r5 - rp
+	 * r6 - wp, tmp
+	 * r7 - tmp
+	 */
+
+/* offsets of registers from flash reg base */
+#define EFM32_MSC_WRITECTRL_OFFSET      0x008
+#define EFM32_MSC_WRITECMD_OFFSET       0x00c
+#define EFM32_MSC_ADDRB_OFFSET          0x010
+#define EFM32_MSC_WDATA_OFFSET          0x018
+#define EFM32_MSC_STATUS_OFFSET         0x01c
+#define EFM32_MSC_LOCK_OFFSET           0x03c
+
+	/* unlock MSC */
+	ldr     r6, =#0x1b71
+	str     r6, [r0, #EFM32_MSC_LOCK_OFFSET]
+	/* set WREN to 1 */
+	movs    r6, #1
+	str     r6, [r0, #EFM32_MSC_WRITECTRL_OFFSET]
+
+wait_fifo:
+	ldr     r6, [r2, #0]    /* read wp */
+	cmp     r6, #0          /* abort if wp == 0 */
+	beq     exit
+	ldr     r5, [r2, #4]    /* read rp */
+	cmp     r5, r6          /* wait until rp != wp */
+	beq     wait_fifo
+
+	/* store address in MSC_ADDRB */
+	str     r4, [r0, #EFM32_MSC_ADDRB_OFFSET]
+	/* set LADDRIM bit */
+	movs    r6, #1
+	str     r6, [r0, #EFM32_MSC_WRITECMD_OFFSET]
+	/* check status for INVADDR and/or LOCKED */
+	ldr     r6, [r0, #EFM32_MSC_STATUS_OFFSET]
+	movs    r7, #6
+	tst     r6, r7
+	bne     error
+
+	/* wait for WDATAREADY */
+wait_wdataready:
+	ldr     r6, [r0, #EFM32_MSC_STATUS_OFFSET]
+	movs    r7, #8
+	tst     r6, r7
+	beq     wait_wdataready
+
+	/* load data to WDATA */
+	ldr     r6, [r5]
+	str     r6, [r0, #EFM32_MSC_WDATA_OFFSET]
+	/* set WRITEONCE bit */
+	movs    r6, #8
+	str     r6, [r0, #EFM32_MSC_WRITECMD_OFFSET]
+
+	adds    r5, #4          /* rp++ */
+	adds    r4, #4          /* target_address++ */
+
+	/* wait until BUSY flag is reset */
+busy:
+	ldr     r6, [r0, #EFM32_MSC_STATUS_OFFSET]
+	movs    r7, #1
+	tst     r6, r7
+	bne     busy
+
+	cmp     r5, r3          /* wrap rp at end of buffer */
+	bcc     no_wrap
+	mov     r5, r2
+	adds    r5, #8
+no_wrap:
+	str     r5, [r2, #4]    /* store rp */
+	subs    r1, r1, #1      /* decrement word count */
+	cmp     r1, #0
+	beq     exit            /* loop if not done */
+	b       wait_fifo
+error:
+	movs    r0, #0
+	str     r0, [r2, #4]    /* set rp = 0 on error */
+exit:
+	mov     r0, r6          /* return status in r0 */
+	bkpt    #0
--- a/contrib/loaders/flash/lpcspifi_erase.S
+++ b/contrib/loaders/flash/lpcspifi_erase.S
@@ -0,0 +1,176 @@
+/***************************************************************************
+ *   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.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+
+	.text
+	.syntax unified
+	.cpu cortex-m3
+	.thumb
+	.thumb_func
+
+/*
+ * Params :
+ * r0 = start address, status (out)
+ * r1 = count
+ * r2 = erase command
+ * r3 = block size
+ */
+
+#define SSP_BASE_HIGH				0x4008
+#define SSP_BASE_LOW				0x3000
+#define SSP_CR0_OFFSET				0x00
+#define SSP_CR1_OFFSET				0x04
+#define SSP_DATA_OFFSET 			0x08
+#define SSP_CPSR_OFFSET 			0x10
+#define SSP_SR_OFFSET				0x0c
+
+#define SSP_CLOCK_BASE_HIGH 		0x4005
+#define SSP_CLOCK_BASE_LOW 			0x0000
+#define SSP_BRANCH_CLOCK_BASE_HIGH 	0x4005
+#define SSP_BRANCH_CLOCK_BASE_LOW	0x2000
+#define SSP_BASE_CLOCK_OFFSET		0x94
+#define SSP_BRANCH_CLOCK_OFFSET		0x700
+
+#define IOCONFIG_BASE_HIGH			0x4008
+#define IOCONFIG_BASE_LOW			0x6000
+#define IOCONFIG_SCK_OFFSET			0x18c
+#define IOCONFIG_HOLD_OFFSET		0x190
+#define IOCONFIG_WP_OFFSET			0x194
+#define IOCONFIG_MISO_OFFSET		0x198
+#define IOCONFIG_MOSI_OFFSET		0x19c
+#define IOCONFIG_CS_OFFSET			0x1a0
+
+#define IO_BASE_HIGH 				0x400f
+#define IO_BASE_LOW 				0x4000
+#define IO_CS_OFFSET 				0xab
+#define IODIR_BASE_HIGH 			0x400f
+#define IODIR_BASE_LOW				0x6000
+#define IO_CS_DIR_OFFSET 			0x14
+
+
+setup: /* Initialize SSP pins and module */
+	mov.w	r10, #IOCONFIG_BASE_LOW
+	movt	r10, #IOCONFIG_BASE_HIGH
+	mov.w	r8, #0xea
+	str.w	r8, [r10, #IOCONFIG_SCK_OFFSET]		/* Configure SCK pin function */
+	mov.w	r8, #0x40
+	str.w	r8, [r10, #IOCONFIG_HOLD_OFFSET]	/* Configure /HOLD pin function */
+	mov.w	r8, #0x40
+	str.w	r8, [r10, #IOCONFIG_WP_OFFSET]		/* Configure /WP pin function */
+	mov.w	r8, #0xed
+	str.w	r8, [r10, #IOCONFIG_MISO_OFFSET]	/* Configure MISO pin function */
+	mov.w	r8, #0xed
+	str.w	r8, [r10, #IOCONFIG_MOSI_OFFSET]	/* Configure MOSI pin function */
+	mov.w	r8, #0x44
+	str.w	r8, [r10, #IOCONFIG_CS_OFFSET]		/* Configure CS pin function */
+
+	mov.w	r10, #IODIR_BASE_LOW
+	movt	r10, #IODIR_BASE_HIGH
+	mov.w	r8, #0x800
+	str 	r8, [r10, #IO_CS_DIR_OFFSET]		/* Set CS as output */
+	mov.w	r10, #IO_BASE_LOW
+	movt	r10, #IO_BASE_HIGH
+	mov.w	r8, #0xff
+	str.w	r8, [r10, #IO_CS_OFFSET]			/* Set CS high */
+
+	mov.w 	r10, #SSP_CLOCK_BASE_LOW
+	movt 	r10, #SSP_CLOCK_BASE_HIGH
+	mov.w 	r8, #0x0000
+	movt 	r8, #0x0100
+	str.w 	r8, [r10, #SSP_BASE_CLOCK_OFFSET] 	/* Configure SSP0 base clock (use 12 MHz IRC) */
+
+	mov.w 	r10, #SSP_BRANCH_CLOCK_BASE_LOW
+	movt 	r10, #SSP_BRANCH_CLOCK_BASE_HIGH
+	mov.w 	r8, #0x01
+	str.w 	r8, [r10, #SSP_BRANCH_CLOCK_OFFSET] /* Configure (enable) SSP0 branch clock */
+
+	mov.w 	r10, #SSP_BASE_LOW
+	movt	r10, #SSP_BASE_HIGH
+	mov.w 	r8, #0x07
+	str.w 	r8, [r10, #SSP_CR0_OFFSET] 			/* Set clock postscale */
+	mov.w 	r8, #0x02
+	str.w 	r8, [r10, #SSP_CPSR_OFFSET] 		/* Set clock prescale */
+	str.w 	r8, [r10, #SSP_CR1_OFFSET] 			/* Enable SSP in SPI mode */
+write_enable:
+	bl 		cs_down
+	mov.w 	r9, #0x06 		/* Send the write enable command */
+	bl 		write_data
+	bl 		cs_up
+
+	bl 		cs_down
+	mov.w 	r9, #0x05 		/* Get status register */
+	bl 		write_data
+	mov.w 	r9, #0x00 		/* Dummy data to clock in status */
+	bl 		write_data
+	bl 		cs_up
+
+	tst 	r9, #0x02 		/* If the WE bit isn't set, we have a problem. */
+	beq 	error
+erase:
+	bl 		cs_down
+	mov.w 	r9, r2 			/* Send the erase command */
+	bl 		write_data
+write_address:
+	lsr 	r9, r0, #16 	/* Send the current 24-bit write address, MSB first */
+	bl 		write_data
+	lsr 	r9, r0, #8
+	bl 		write_data
+	mov.w 	r9, r0
+	bl 		write_data
+	bl 		cs_up
+wait_flash_busy:			/* Wait for the flash to finish the previous erase */
+	bl 		cs_down
+	mov.w 	r9, #0x05 		/* Get status register */
+	bl 		write_data
+	mov.w 	r9, #0x00 		/* Dummy data to clock in status */
+	bl 		write_data
+	bl 		cs_up
+	tst 	r9, #0x01 		/* If it isn't done, keep waiting */
+	bne 	wait_flash_busy
+
+	subs	r1, r1, #1					/* decrement count */
+	cbz		r1, exit 					/* Exit if we have written everything */
+	add 	r0, r3						/* Move the address up by the block size */
+	b 		write_enable 				/* Start a new block erase */
+write_data: 							/* Send/receive 1 byte of data over SSP */
+	mov.w	r10, #SSP_BASE_LOW
+	movt	r10, #SSP_BASE_HIGH
+	str.w 	r9, [r10, #SSP_DATA_OFFSET]	/* Write supplied data to the SSP data reg */
+wait_transmit:
+	ldr 	r9, [r10, #SSP_SR_OFFSET] 	/* Check SSP status */
+	tst 	r9, #0x0010					/* Check if BSY bit is set */
+	bne 	wait_transmit 				/* If still transmitting, keep waiting */
+	ldr 	r9, [r10, #SSP_DATA_OFFSET]	/* Load received data */
+	bx 		lr 							/* Exit subroutine */
+cs_up:
+	mov.w 	r8, #0xff
+	b 		cs_write
+cs_down:
+	mov.w 	r8, #0x0000
+cs_write:
+	mov.w 	r10, #IO_BASE_LOW
+	movt	r10, #IO_BASE_HIGH
+	str.w 	r8, [r10, #IO_CS_OFFSET]
+	bx 		lr
+error:
+	movs	r0, #0
+exit:
+	bkpt 	#0x00
+
+	.end
--- a/contrib/loaders/flash/lpcspifi_init.S
+++ b/contrib/loaders/flash/lpcspifi_init.S
@@ -0,0 +1,102 @@
+/***************************************************************************
+ *   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.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+
+/***************************************************************************
+*	This is an algorithm for the LPC43xx family (and probably the LPC18xx  *
+*	family as well, though they have not been tested) that will initialize *
+*	memory-mapped SPI flash accesses. Unfortunately NXP has published      *
+*	neither the ROM source code that performs this initialization nor the  *
+*	register descriptions necessary to do so, so this code is necessary to *
+*	call into the ROM SPIFI API.                                           *
+***************************************************************************/
+
+	.text
+	.syntax unified
+	.arch armv7-m
+	.thumb
+	.thumb_func
+
+	.align 2
+
+/*
+ * Params :
+ * r0 = spifi clock speed
+ */
+
+#define IOCONFIG_BASE_HIGH          0x4008
+#define IOCONFIG_BASE_LOW           0x6000
+#define IOCONFIG_SCK_OFFSET         0x18c
+#define IOCONFIG_HOLD_OFFSET        0x190
+#define IOCONFIG_WP_OFFSET          0x194
+#define IOCONFIG_MISO_OFFSET        0x198
+#define IOCONFIG_MOSI_OFFSET        0x19c
+#define IOCONFIG_CS_OFFSET          0x1a0
+
+#define SPIFI_ROM_TABLE_BASE_HIGH   0x1040
+#define SPIFI_ROM_TABLE_BASE_LOW    0x0118
+
+code:
+	mov.w 	r8, r0
+	sub		sp, #0x84
+	add		r7, sp, #0x0
+	/* Initialize SPIFI pins */
+	mov.w	r3, #IOCONFIG_BASE_LOW
+	movt	r3, #IOCONFIG_BASE_HIGH
+	mov.w	r2, #0xf3
+	str.w 	r2, [r3, #IOCONFIG_SCK_OFFSET]
+	mov.w	r3, #IOCONFIG_BASE_LOW
+	movt	r3, #IOCONFIG_BASE_HIGH
+	mov.w	r2, #IOCONFIG_BASE_LOW
+	movt	r2, #IOCONFIG_BASE_HIGH
+	mov.w	r1, #IOCONFIG_BASE_LOW
+	movt	r1, #IOCONFIG_BASE_HIGH
+	mov.w	r0, #IOCONFIG_BASE_LOW
+	movt	r0, #IOCONFIG_BASE_HIGH
+	mov.w	r4, #0xd3
+	str.w	r4, [r0, #IOCONFIG_MOSI_OFFSET]
+	mov	r0, r4
+	str.w	r0, [r1, #IOCONFIG_MISO_OFFSET]
+	mov	r1, r0
+	str.w	r1, [r2, #IOCONFIG_WP_OFFSET]
+	str.w	r1, [r3, #IOCONFIG_HOLD_OFFSET]
+	mov.w	r3, #IOCONFIG_BASE_LOW
+	movt	r3, #IOCONFIG_BASE_HIGH
+	mov.w	r2, #0x13
+	str.w	r2, [r3, #IOCONFIG_CS_OFFSET]
+
+	/* Perform SPIFI init. See spifi_rom_api.h (in NXP lpc43xx driver package) for details */
+	/* on initialization arguments. */
+	movw 	r3, #SPIFI_ROM_TABLE_BASE_LOW      /* The ROM API table is located @ 0x10400118, and			*/
+	movt 	r3, #SPIFI_ROM_TABLE_BASE_HIGH     /* the first pointer in the struct is to the init function. */
+	ldr 	r3, [r3, #0x0]
+	ldr 	r4, [r3, #0x0]		                 /* Grab the init function pointer from the table */
+	/* Set up function arguments */
+	movw 	r0, #0x3b4
+	movt 	r0, #0x1000		    	              /* Pointer to a SPIFI data struct that we don't care about */
+	mov.w 	r1, #0x3                        /* "csHigh". Not 100% sure what this does. */
+	mov.w 	r2, #0xc0 			              /* The configuration word: S_RCVCLOCK | S_FULLCLK */
+	mov.w 	r3, r8 				              /* SPIFI clock speed (12MHz) */
+	blx 	r4					                    /* Call the init function */
+	b 		done
+
+done:
+	bkpt 	#0
+
+	.end
--- a/contrib/loaders/flash/lpcspifi_write.S
+++ b/contrib/loaders/flash/lpcspifi_write.S
@@ -0,0 +1,210 @@
+/***************************************************************************
+ *   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.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+
+	.text
+	.syntax unified
+	.cpu cortex-m3
+	.thumb
+	.thumb_func
+
+/*
+ * Params :
+ * r0 = workarea start, status (out)
+ * r1 = workarea end
+ * r2 = target address (offset from flash base)
+ * r3 = count (bytes)
+ * r4 = page size
+ * Clobbered:
+ * r7 - rp
+ * r8 - wp, tmp
+ * r9 - send/receive data
+ * r10 - temp
+ * r11 - current page end address
+ */
+
+#define SSP_BASE_HIGH				0x4008
+#define SSP_BASE_LOW				0x3000
+#define SSP_CR0_OFFSET				0x00
+#define SSP_CR1_OFFSET				0x04
+#define SSP_DATA_OFFSET 			0x08
+#define SSP_CPSR_OFFSET 			0x10
+#define SSP_SR_OFFSET				0x0c
+
+#define SSP_CLOCK_BASE_HIGH 		0x4005
+#define SSP_CLOCK_BASE_LOW 			0x0000
+#define SSP_BRANCH_CLOCK_BASE_HIGH 	0x4005
+#define SSP_BRANCH_CLOCK_BASE_LOW	0x2000
+#define SSP_BASE_CLOCK_OFFSET		0x94
+#define SSP_BRANCH_CLOCK_OFFSET		0x700
+
+#define IOCONFIG_BASE_HIGH			0x4008
+#define IOCONFIG_BASE_LOW			0x6000
+#define IOCONFIG_SCK_OFFSET			0x18c
+#define IOCONFIG_HOLD_OFFSET		0x190
+#define IOCONFIG_WP_OFFSET			0x194
+#define IOCONFIG_MISO_OFFSET		0x198
+#define IOCONFIG_MOSI_OFFSET		0x19c
+#define IOCONFIG_CS_OFFSET			0x1a0
+
+#define IO_BASE_HIGH 				0x400f
+#define IO_BASE_LOW 				0x4000
+#define IO_CS_OFFSET 				0xab
+#define IODIR_BASE_HIGH 			0x400f
+#define IODIR_BASE_LOW				0x6000
+#define IO_CS_DIR_OFFSET 			0x14
+
+
+setup: /* Initialize SSP pins and module */
+	mov.w	r10, #IOCONFIG_BASE_LOW
+	movt	r10, #IOCONFIG_BASE_HIGH
+	mov.w	r8, #0xea
+	str.w	r8, [r10, #IOCONFIG_SCK_OFFSET]		/* Configure SCK pin function */
+	mov.w	r8, #0x40
+	str.w	r8, [r10, #IOCONFIG_HOLD_OFFSET]	/* Configure /HOLD pin function */
+	mov.w	r8, #0x40
+	str.w	r8, [r10, #IOCONFIG_WP_OFFSET]		/* Configure /WP pin function */
+	mov.w	r8, #0xed
+	str.w	r8, [r10, #IOCONFIG_MISO_OFFSET]	/* Configure MISO pin function */
+	mov.w	r8, #0xed
+	str.w	r8, [r10, #IOCONFIG_MOSI_OFFSET]	/* Configure MOSI pin function */
+	mov.w	r8, #0x44
+	str.w	r8, [r10, #IOCONFIG_CS_OFFSET]		/* Configure CS pin function */
+
+	mov.w	r10, #IODIR_BASE_LOW
+	movt	r10, #IODIR_BASE_HIGH
+	mov.w	r8, #0x800
+	str 	r8, [r10, #IO_CS_DIR_OFFSET]		/* Set CS as output */
+	mov.w	r10, #IO_BASE_LOW
+	movt	r10, #IO_BASE_HIGH
+	mov.w	r8, #0xff
+	str.w	r8, [r10, #IO_CS_OFFSET]			/* Set CS high */
+
+	mov.w 	r10, #SSP_CLOCK_BASE_LOW
+	movt 	r10, #SSP_CLOCK_BASE_HIGH
+	mov.w 	r8, #0x0000
+	movt 	r8, #0x0100
+	str.w 	r8, [r10, #SSP_BASE_CLOCK_OFFSET] 	/* Configure SSP0 base clock (use 12 MHz IRC) */
+
+	mov.w 	r10, #SSP_BRANCH_CLOCK_BASE_LOW
+	movt 	r10, #SSP_BRANCH_CLOCK_BASE_HIGH
+	mov.w 	r8, #0x01
+	str.w 	r8, [r10, #SSP_BRANCH_CLOCK_OFFSET] /* Configure (enable) SSP0 branch clock */
+
+	mov.w 	r10, #SSP_BASE_LOW
+	movt	r10, #SSP_BASE_HIGH
+	mov.w 	r8, #0x07
+	str.w 	r8, [r10, #SSP_CR0_OFFSET] 			/* Set clock postscale */
+	mov.w 	r8, #0x02
+	str.w 	r8, [r10, #SSP_CPSR_OFFSET] 		/* Set clock prescale */
+	str.w 	r8, [r10, #SSP_CR1_OFFSET] 			/* Enable SSP in SPI mode */
+
+	mov.w 	r11, #0x00
+find_next_page_boundary:
+	add 	r11, r4			/* Increment to the next page */
+	cmp 	r11, r2
+	/* If we have not reached the next page boundary after the target address, keep going */
+	bls 	find_next_page_boundary
+write_enable:
+	bl 		cs_down
+	mov.w 	r9, #0x06 		/* Send the write enable command */
+	bl 		write_data
+	bl 		cs_up
+
+	bl 		cs_down
+	mov.w 	r9, #0x05 		/* Get status register */
+	bl 		write_data
+	mov.w 	r9, #0x00 		/* Dummy data to clock in status */
+	bl 		write_data
+	bl 		cs_up
+
+	tst 	r9, #0x02 		/* If the WE bit isn't set, we have a problem. */
+	beq 	error
+page_program:
+	bl 		cs_down
+	mov.w 	r9, #0x02 		/* Send the page program command */
+	bl 		write_data
+write_address:
+	lsr 	r9, r2, #16 	/* Send the current 24-bit write address, MSB first */
+	bl 		write_data
+	lsr 	r9, r2, #8
+	bl 		write_data
+	mov.w 	r9, r2
+	bl 		write_data
+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 */
+	cbz		r3, exit 		/* Exit if we have written everything */
+
+	add 	r2, #1 			/* Increment flash address by 1 */
+	cmp 	r11, r2   		/* See if we have reached the end of a page */
+	bne 	wait_fifo 		/* If not, keep writing bytes */
+	bl 		cs_up			/* Otherwise, end the command and keep going w/ the next page */
+	add 	r11, r4 		/* Move up the end-of-page address by the page size*/
+wait_flash_busy:			/* Wait for the flash to finish the previous page write */
+	bl 		cs_down
+	mov.w 	r9, #0x05 					/* Get status register */
+	bl 		write_data
+	mov.w 	r9, #0x00 					/* Dummy data to clock in status */
+	bl 		write_data
+	bl 		cs_up
+	tst 	r9, #0x01 					/* If it isn't done, keep waiting */
+	bne 	wait_flash_busy
+	b 		write_enable 				/* If it is done, start a new page write */
+write_data: 							/* Send/receive 1 byte of data over SSP */
+	mov.w	r10, #SSP_BASE_LOW
+	movt	r10, #SSP_BASE_HIGH
+	str.w 	r9, [r10, #SSP_DATA_OFFSET]	/* Write supplied data to the SSP data reg */
+wait_transmit:
+	ldr 	r9, [r10, #SSP_SR_OFFSET] 	/* Check SSP status */
+	tst 	r9, #0x0010					/* Check if BSY bit is set */
+	bne 	wait_transmit 				/* If still transmitting, keep waiting */
+	ldr 	r9, [r10, #SSP_DATA_OFFSET]	/* Load received data */
+	bx 		lr 							/* Exit subroutine */
+cs_up:
+	mov.w 	r8, #0xff
+	b 		cs_write
+cs_down:
+	mov.w 	r8, #0x0000
+cs_write:
+	mov.w 	r10, #IO_BASE_LOW
+	movt	r10, #IO_BASE_HIGH
+	str.w 	r8, [r10, #IO_CS_OFFSET]
+	bx 		lr
+error:
+	movs	r0, #0
+	str 	r0, [r2, #4]	/* set rp = 0 on error */
+exit:
+	mov 	r0, r6
+	bkpt 	#0x00
+
+	.end
--- a/contrib/openocd.udev
+++ b/contrib/openocd.udev
@@ -49,12 +49,15 @@ ATTRS{idVendor}=="0640", ATTRS{idProduct}=="002c", MODE="664", GROUP="plugdev"
 # Hitex STM32-PerformanceStick
 ATTRS{idVendor}=="0640", ATTRS{idProduct}=="002d", MODE="664", GROUP="plugdev"

-# TI/Luminary Stellaris Evaluation Board (several)
+# TI/Luminary Stellaris Evaluation Board FTDI (several)
 ATTRS{idVendor}=="0403", ATTRS{idProduct}=="bcd9", MODE="664", GROUP="plugdev"

-# TI/Luminary Stellaris In-Circuit Debug Interface (ICDI) Board
+# TI/Luminary Stellaris In-Circuit Debug Interface FTDI (ICDI) Board
 ATTRS{idVendor}=="0403", ATTRS{idProduct}=="bcda", MODE="664", GROUP="plugdev"

+# TI/Luminary Stellaris In-Circuit Debug Interface (ICDI) Board
+ATTRS{idVendor}=="1cbe", ATTRS{idProduct}=="00fd", MODE="664", GROUP="plugdev"
+
 # Xverve Signalyzer Tool (DT-USB-ST)
 ATTRS{idVendor}=="0403", ATTRS{idProduct}=="bca0", MODE="664", GROUP="plugdev"

--- a/doc/manual/main.txt
+++ b/doc/manual/main.txt
@@ -82,6 +82,7 @@ modules are stacked in the current implementation (from bottom to top):
 - @subpage targetdocs
  - @ref targetarm
  - @ref targetnotarm
+  - @ref targetmips
  - @ref targetregister
  - @ref targetimage
  - @ref targettrace
--- a/doc/manual/target.txt
+++ b/doc/manual/target.txt
@@ -9,6 +9,7 @@ The Target Support module contains APIs that cover several functional areas:

  - @subpage targetarm
  - @subpage targetnotarm
+  - @subpage targetmips
  - @subpage targetregister
  - @subpage targetimage
  - @subpage targettrace
--- a/doc/manual/target/mips.txt
+++ b/doc/manual/target/mips.txt
@@ -0,0 +1,536 @@
+/** @page targetmips OpenOCD MIPS Targets
+
+@section ejatgmem EJTAG Memory Addresses
+
+An optional uncached and unmapped debug segment dseg (EJTAG area) appears in the address range
+0xFFFF FFFF FF20 0000 to 0xFFFF FFFF FF3F FFFF. The dseg segment thereby appears in the kseg part of the
+compatibility segment, and access to kseg is possible with the dseg segment.
+
+The dseg segment is subdivided into dmseg (EJTAG memory) segment and the drseg (EJTAG registers) segment. The
+dmseg segment is used when the probe services the memory segment. The drseg segment is used when the
+memory-mapped debug registers are accessed. Table 5-2 shows the subdivision and attributes for the segments.
+
+dseg is divided in :
+
+  - dmseg (0xFFFF FFFF FF20 0000 to 0xFFFF FFFF FF2F FFFF)
+  - drseg (0xFFFF FFFF FF30 0000 to 0xFFFF FFFF FF3F FFFF)
+
+Because the dseg segment is serviced exclusively by the EJTAG features, there
+are no physical address per se. Instead the lower 21 bits of the virtual address select
+the appropriate reference in either EJTAG memory or registers. References are not mapped through the
+TLB, nor do the accesses appear on the external system memory interface.
+
+Both of this memory segments are Uncached.
+
+On debug exception (break) CPU jumps to the beginning of dmseg. This some kind of memory shared
+between CPU and EJTAG dongle.
+
+There CPU stops (correct terminology is : stalls, because it stops it's pipeline), and is waiting for some action of dongle.
+
+If the dongle gives it instruction, CPU executes it, augments it's PC to 0xFFFF FFFF FF20 0001 - but it again points to dmseg area,
+so it stops waiting for next instruction.
+
+This will all become clear later, after reading following prerequisite chapters.
+
+@section impflags Important flags
+
+@subsection	pnnw PNnW
+
+Indicates read or write of a pending processor access:
+
+  - 0 : Read processor access, for a fetch/load access
+  - 1 : Write processor access, for a store access
+
+This value is defined only when a processor access is pending.
+
+Processor will do the action for us : it can for example read internal state (register values),
+and send us back the information via EJTAG memory (dmseg), or it can take some data from dmseg and write it into the registers or RAM.
+
+Every time when it sees address (i.e. when this address is the part of the opcode it is executing, wether it is instruction or data fetch)
+that falls into dmseg, processor stalls. That acutally meand that CPU stops it's pipeline and it is waitning for dongle to take some action.
+
+CPU is now either waiting for dongle to take some data from dmseg (if we requested for CPU do give us internal state, for example),
+or it will wait for some data from dongle (if it needs following instruction because it did previous, or if the operand address of the currently executed opcode
+falls somewhere (anywhere) in dmseg (0xff..ff20000 - 0xff..ff2fffff)).
+
+Bit PNnW describes character of CPU access to EJTAG memory (the memry where dongle puts/takes data) - CPU can either READ for it (PNnW == 0) or
+WRITE to it (PNnW == 1).
+
+By reading PNnW bit OpenOCD will know if it has to send (PNnW == 0) or to take (PNnW == 1) data (from dmseg, via dongle).
+
+@subsection	pracc PrAcc
+
+Indicates a pending processor access and controls finishing of a pending processor access.
+
+When read:
+
+  - 0 : No pending processor access
+  - 1 : Pending processor access
+
+A write of 0 finishes a processor access if pending;
+otherwise operation of the processor is UNDEFINED
+if the bit is written to 0 when no processor access is
+pending. A write of 1 is ignored.
+
+A successful FASTDATA access will clear this bit.
+
+As noted above, on any access to dmseg, processor will stall. It waits for dongle to do some action - either to take or put some data.
+OpenOCD can figure out which action has to be taken by reading PrAcc bit.
+
+Once action from dongle has been done, i.e. after the data is taken/put, OpenOCD can signal to CPU to proceed with executing the instruction.
+This can be the next instruction (if previous was finished before pending), or the same instruction - if for example CPU was waiting on dongle
+to give it an operand, because it saw in the instruction opcode that operand address is somewhere in dmseg. That prowoked the CPU to stall (it tried operand fetch to dmseg and stopped),
+and PNnW bit is 0 (CPU does read from dmseg), and PrAcc is 1 (CPU is pending on dmseg access).
+
+@subsection spracc	SPrAcc
+
+Shifting in a zero value requests completion of the Fastdata access.
+
+The PrAcc bit in the EJTAG Control register is overwritten with zero when the access
+succeeds. (The access succeeds if PrAcc is one and the operation address is in the legal dmseg segment
+Fastdata area.)
+
+When successful, a one is shifted out. Shifting out a zero indicates a Fastdata access failure.
+Shifting in a one does not complete the Fastdata access and the PrAcc bit is unchanged. Shifting out a
+one indicates that the access would have been successful if allowed to complete and a zero indicates
+the access would not have successfully completed.
+
+@section fdreg Fastdata Register (TAP Instruction FASTDATA)
+
+The width of the Fastdata register is 1 bit.
+
+During a Fastdata access, the Fastdata register is written and read, i.e., a bit is
+shifted in and a bit is shifted out.
+
+Also during a Fastdata access, the Fastdata register value shifted in specifies whether the Fastdata
+access should be completed or not. The value shifted out is a flag that indicates whether the Fastdata access was
+successful or not (if completion was requested).
+
+@section ejtagacc EJTAG Access Implementation
+
+OpenOCD reads/writes data to JTAG via mips_m4k_read_memory() and mips_m4k_write_memory() functions defined in src/target/mips_m4k.c.
+Internally, these functions call mips32_pracc_read_mem() and mips32_pracc_write_mem() defined in src/target/mips32_pracc.c
+
+Let's take for example function mips32_pracc_read_mem32() which describes CPU reads (fetches) from dmseg (EJTAG memory) :
+
+@code
+static const uint32_t code[] = {
+														/* start: */
+	MIPS32_MTC0(15,31,0),								/* move $15 to COP0 DeSave */
+	MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),			/* $15 = MIPS32_PRACC_STACK */
+	MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
+	MIPS32_SW(8,0,15),									/* sw $8,($15) */
+	MIPS32_SW(9,0,15),									/* sw $9,($15) */
+	MIPS32_SW(10,0,15),									/* sw $10,($15) */
+	MIPS32_SW(11,0,15),									/* sw $11,($15) */
+
+	MIPS32_LUI(8,UPPER16(MIPS32_PRACC_PARAM_IN)),		/* $8 = MIPS32_PRACC_PARAM_IN */
+	MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_PARAM_IN)),
+	MIPS32_LW(9,0,8),									/* $9 = mem[$8]; read addr */
+	MIPS32_LW(10,4,8),									/* $10 = mem[$8 + 4]; read count */
+	MIPS32_LUI(11,UPPER16(MIPS32_PRACC_PARAM_OUT)),		/* $11 = MIPS32_PRACC_PARAM_OUT */
+	MIPS32_ORI(11,11,LOWER16(MIPS32_PRACC_PARAM_OUT)),
+														/* loop: */
+	MIPS32_BEQ(0,10,8),									/* beq 0, $10, end */
+	MIPS32_NOP,
+
+	MIPS32_LW(8,0,9),									/* lw $8,0($9), Load $8 with the word @mem[$9] */
+	MIPS32_SW(8,0,11),									/* sw $8,0($11) */
+
+	MIPS32_ADDI(10,10,NEG16(1)),						/* $10-- */
+	MIPS32_ADDI(9,9,4),									/* $1 += 4 */
+	MIPS32_ADDI(11,11,4),								/* $11 += 4 */
+
+	MIPS32_B(NEG16(8)),									/* b loop */
+	MIPS32_NOP,
+                                          				/* end: */
+	MIPS32_LW(11,0,15),									/* lw $11,($15) */
+	MIPS32_LW(10,0,15),									/* lw $10,($15) */
+	MIPS32_LW(9,0,15),									/* lw $9,($15) */
+	MIPS32_LW(8,0,15),									/* lw $8,($15) */
+	MIPS32_B(NEG16(27)),								/* b start */
+	MIPS32_MFC0(15,31,0),								/* move COP0 DeSave to $15 */
+};
+@endcode
+
+We have to pass this code to CPU via dongle via dmseg.
+
+After debug exception CPU will find itself stalling at the begining of the dmseg. It waits for the first instruction from dongle.
+This is MIPS32_MTC0(15,31,0), so CPU saves C0 and continues to addr 0xFF20 0001, which falls also to dmseg, so it stalls.
+Dongle proceeds giving to CPU one by one instruction in this manner.
+
+However, things are not so simple. If you take a look at the program, you will see that some instructions take operands. If it has to take
+operand from the address in dmseg, CPU will stall witing for the dongle to do the action of passing the operand and signal this by putting PrAcc to 0.
+If this operand is somewhere in RAM, CPU will not stall (it stalls only on dmseg), but it will just take it and proceed to nex instruction. But since PC for next instruction
+points to dmseg, it will stall, so that dongle can pass next instruction.
+
+Some instuctions are jumps (if these are jumps in dmseg addr, CPU will jump and then stall. If this is jump to some address in RAM, CPU will jump and just proceed -
+will not stall on addresses in RAM).
+
+To have information about CPU is currently (does it stalls wanting on operand or it jumped somewhere waiting for next instruction),
+OpenOCD has to call TAP ADDRESS instruction, which will ask CPU to give us his address within EJTAG memory :
+
+@code
+address = data = 0;
+mips_ejtag_set_instr(ejtag_info, EJTAG_INST_ADDRESS);
+mips_ejtag_drscan_32(ejtag_info, &address);
+@endcode
+
+And then, upon the results, we can conclude where it is in our code so far, so we can give it what it wants next :
+
+@code
+if ((address >= MIPS32_PRACC_PARAM_IN)
+	&& (address <= MIPS32_PRACC_PARAM_IN + ctx->num_iparam * 4))
+{
+	offset = (address - MIPS32_PRACC_PARAM_IN) / 4;
+	data = ctx->local_iparam[offset];
+}
+else if ((address >= MIPS32_PRACC_PARAM_OUT)
+	&& (address <= MIPS32_PRACC_PARAM_OUT + ctx->num_oparam * 4))
+{
+	offset = (address - MIPS32_PRACC_PARAM_OUT) / 4;
+	data = ctx->local_oparam[offset];
+}
+else if ((address >= MIPS32_PRACC_TEXT)
+	&& (address <= MIPS32_PRACC_TEXT + ctx->code_len * 4))
+{
+	offset = (address - MIPS32_PRACC_TEXT) / 4;
+	data = ctx->code[offset];
+}
+else if (address == MIPS32_PRACC_STACK)
+{
+	/* save to our debug stack */
+	data = ctx->stack[--ctx->stack_offset];
+}
+else
+{
+	/* TODO: send JMP 0xFF200000 instruction.
+		Hopefully processor jump back to start of debug vector */
+	data = 0;
+	LOG_ERROR("Error reading unexpected address 0x%8.8" PRIx32 "", address);
+	return ERROR_JTAG_DEVICE_ERROR;
+}
+@endcode
+
+i.e. if CPU is stalling on addresses in dmseg that are reserved for input parameters, we can conclude that it actually tried to take (read)
+parametar from there, and saw that address of param falls in dmseg, so it stopped. Obviously, now dongle have to give to it operand.
+
+Similarly, mips32_pracc_exec_write() describes CPU writes into EJTAG memory (dmseg).
+Obvioulsy, code is RO, and CPU can change only parameters :
+
+@code
+mips_ejtag_set_instr(ctx->ejtag_info, EJTAG_INST_DATA);
+mips_ejtag_drscan_32(ctx->ejtag_info, &data);
+
+/* Clear access pending bit */
+ejtag_ctrl = ejtag_info->ejtag_ctrl & ~EJTAG_CTRL_PRACC;
+mips_ejtag_set_instr(ctx->ejtag_info, EJTAG_INST_CONTROL);
+mips_ejtag_drscan_32(ctx->ejtag_info, &ejtag_ctrl);
+
+//jtag_add_clocks(5);
+jtag_execute_queue();
+
+if ((address >= MIPS32_PRACC_PARAM_IN)
+	&& (address <= MIPS32_PRACC_PARAM_IN + ctx->num_iparam * 4))
+{
+	offset = (address - MIPS32_PRACC_PARAM_IN) / 4;
+	ctx->local_iparam[offset] = data;
+}
+else if ((address >= MIPS32_PRACC_PARAM_OUT)
+	&& (address <= MIPS32_PRACC_PARAM_OUT + ctx->num_oparam * 4))
+{
+	offset = (address - MIPS32_PRACC_PARAM_OUT) / 4;
+	ctx->local_oparam[offset] = data;
+}
+else if (address == MIPS32_PRACC_STACK)
+{
+	/* save data onto our stack */
+	ctx->stack[ctx->stack_offset++] = data;
+}
+else
+{
+	LOG_ERROR("Error writing unexpected address 0x%8.8" PRIx32 "", address);
+	return ERROR_JTAG_DEVICE_ERROR;
+}
+@endcode
+
+CPU loops here :
+
+@code
+while (1)
+{
+	if ((retval = wait_for_pracc_rw(ejtag_info, &ejtag_ctrl)) != ERROR_OK)
+		return retval;
+
+	address = data = 0;
+	mips_ejtag_set_instr(ejtag_info, EJTAG_INST_ADDRESS);
+	mips_ejtag_drscan_32(ejtag_info, &address);
+
+	/* Check for read or write */
+	if (ejtag_ctrl & EJTAG_CTRL_PRNW)
+	{
+		if ((retval = mips32_pracc_exec_write(&ctx, address)) != ERROR_OK)
+			return retval;
+	}
+	else
+	{
+		/* Check to see if its reading at the debug vector. The first pass through
+		 * the module is always read at the vector, so the first one we allow.  When
+		 * the second read from the vector occurs we are done and just exit. */
+		if ((address == MIPS32_PRACC_TEXT) && (pass++))
+		{
+			break;
+		}
+
+		if ((retval = mips32_pracc_exec_read(&ctx, address)) != ERROR_OK)
+			return retval;
+	}
+
+	if (cycle == 0)
+		break;
+}
+@endcode
+
+and using presented R (mips32_pracc_exec_read()) and W (mips32_pracc_exec_write()) functions it reads in the code (RO) and reads and writes operands (RW).
+
+@section fdimpl OpenOCD FASTDATA Implementation
+
+OpenOCD FASTDATA write function, mips32_pracc_fastdata_xfer() is called from bulk_write_memory callback, which writes a count items of 4 bytes
+to the memory of a target at the an address given.  Because it operates only on whole words, this should be faster than target_write_memory().
+
+In order to implement FASTDATA write, mips32_pracc_fastdata_xfer() uses the following handler :
+
+@code
+uint32_t handler_code[] = {
+	/* caution when editing, table is modified below */
+	/* r15 points to the start of this code */
+	MIPS32_SW(8,MIPS32_FASTDATA_HANDLER_SIZE - 4,15),
+	MIPS32_SW(9,MIPS32_FASTDATA_HANDLER_SIZE - 8,15),
+	MIPS32_SW(10,MIPS32_FASTDATA_HANDLER_SIZE - 12,15),
+	MIPS32_SW(11,MIPS32_FASTDATA_HANDLER_SIZE - 16,15),
+	/* start of fastdata area in t0 */
+	MIPS32_LUI(8,UPPER16(MIPS32_PRACC_FASTDATA_AREA)),
+	MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_FASTDATA_AREA)),
+	MIPS32_LW(9,0,8),								/* start addr in t1 */
+	MIPS32_LW(10,0,8),								/* end addr to t2 */
+													/* loop: */
+	/* 8 */ MIPS32_LW(11,0,0),						/* lw t3,[t8 | r9] */
+	/* 9 */ MIPS32_SW(11,0,0),						/* sw t3,[r9 | r8] */
+	MIPS32_BNE(10,9,NEG16(3)),						/* bne $t2,t1,loop */
+	MIPS32_ADDI(9,9,4),								/* addi t1,t1,4 */
+
+	MIPS32_LW(8,MIPS32_FASTDATA_HANDLER_SIZE - 4,15),
+	MIPS32_LW(9,MIPS32_FASTDATA_HANDLER_SIZE - 8,15),
+	MIPS32_LW(10,MIPS32_FASTDATA_HANDLER_SIZE - 12,15),
+	MIPS32_LW(11,MIPS32_FASTDATA_HANDLER_SIZE - 16,15),
+
+	MIPS32_LUI(15,UPPER16(MIPS32_PRACC_TEXT)),
+	MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_TEXT)),
+	MIPS32_JR(15),									/* jr start */
+	MIPS32_MFC0(15,31,0),							/* move COP0 DeSave to $15 */
+};
+@endcode
+
+In the begining and the end of the handler we have fuction prologue (save the regs that will be clobbered) and epilogue (restore regs),
+and in the very end, after all the xfer have been done, we do jump to the MIPS32_PRACC_TEXT address, i.e. Debug Exception Vector location.
+We will use this fact (that we came back to MIPS32_PRACC_TEXT)  to verify later if all the handler is executed (because when in RAM,
+processor do not stall - it executes all instructions untill one of them do not demand access to dmseg (if one of it's opernads is there)).
+
+This handler is put into the RAM and executed from there, and not instruction by  instruction, like in previous simple write
+(mips_m4k_write_memory()) and read (mips_m4k_read_memory()) functions.
+
+N.B. When it is executing this code in RAM, CPU will not stall on instructions, but execute all until it comes to the :
+
+@code
+MIPS32_LW(9,0,8) /* start addr in t1 */
+@endcode
+
+and there it will stall - because it will see that one of the operands have to be fetched from dmseg (EJTAG memory, in this case FASTDATA memory segment).
+
+This handler is loaded in the RAM, ath the reserved location "work_area". This work_area is configured in OpenOCD configuration script and should be selected
+in that way that it is not clobbered (overwritten) by data we want to write-in using FASTDATA.
+
+What is executed instruction by instruction which is passed by dongle (via EJATG memory) is small jump code, which jumps at the handler in RAM.
+CPU stalls on dmseg when receiving these jmp_code instructions, but once it jumps in RAM, CPU do not stall anymore and executes bunch of handler instructions.
+Untill it comes to the first instruction which has an operand in FASTDATA area. There it stalls and waits on action from probe.
+It happens actually when CPU comes to this loop :
+
+@code
+MIPS32_LW(9,0,8),								/* start addr in t1 */
+MIPS32_LW(10,0,8),								/* end addr to t2 */
+												/* loop: */
+/* 8 */ MIPS32_LW(11,0,0),						/* lw t3,[t8 | r9] */
+/* 9 */ MIPS32_SW(11,0,0),						/* sw t3,[r9 | r8] */
+MIPS32_BNE(10,9,NEG16(3)),						/* bne $t2,t1,loop */
+@endcode
+
+and then it stalls because operand in r8 points to FASTDATA area.
+
+OpenOCD first verifies that CPU came to this place by :
+
+@code
+/* next fetch to dmseg should be in FASTDATA_AREA, check */
+address = 0;
+mips_ejtag_set_instr(ejtag_info, EJTAG_INST_ADDRESS);
+mips_ejtag_drscan_32(ejtag_info, &address);
+
+if (address != MIPS32_PRACC_FASTDATA_AREA)
+	return ERROR_FAIL;
+@endcode
+
+and then passes to CPU start and end address of the loop region for handler in RAM.
+
+In the loop in handler, CPU sees that it has to take and operand from FSTDATA area (to write it to the dst in RAM after), and so it stalls, putting PrAcc to "1".
+OpenOCD fills the data via this loop :
+
+@code
+for (i = 0; i < count; i++)
+{
+	/* Send the data out using fastdata (clears the access pending bit) */
+	mips_ejtag_set_instr(ejtag_info, EJTAG_INST_FASTDATA);
+	if ((retval = mips_ejtag_fastdata_scan(ejtag_info, write_t, buf++)) != ERROR_OK)
+		return retval;
+}
+@endcode
+
+Each time when OpenOCD fills data to CPU (via dongle, via dmseg), CPU takes it and proceeds in executing the endler. However, since handler is in a assembly loop,
+CPU comes to next instruction which also fetches data from FASTDATA area. So it stalls.
+Then OpenOCD fills the data again, from it's (OpenOCD's) loop. And this game continues untill all the data has been filled.
+
+After the last data has beend given to CPU it sees that it reached the end address, so it proceeds with next instruction. However, rhis instruction do not point into dmseg, so
+CPU executes bunch of handler instructions (all prologue) and in the end jumps to MIPS32_PRACC_TEXT address.
+
+On it's side, OpenOCD checks in CPU has jumped back to MIPS32_PRACC_TEXT, which is the confirmation that it correclty executed all the rest of the handler in RAM,
+and that is not stuck somewhere in the RAM, or stalling on some acces in dmseg - that would be an error :
+
+@code
+address = 0;
+mips_ejtag_set_instr(ejtag_info, EJTAG_INST_ADDRESS);
+mips_ejtag_drscan_32(ejtag_info, &address);
+
+if (address != MIPS32_PRACC_TEXT)
+	LOG_ERROR("mini program did not return to start");
+@endcode
+
+@section fdejtagspec EJTAG spec on FASTDATA access
+
+The width of the Fastdata register is 1 bit. During a Fastdata access, the Fastdata register is written and read, i.e., a bit
+is shifted in and a bit is shifted out. During a Fastdata access, the Fastdata register value shifted in specifies whether
+the Fastdata access should be completed or not. The value shifted out is a flag that indicates whether the Fastdata
+access was successful or not (if completion was requested).
+
+The FASTDATA access is used for efficient block transfers between dmseg (on the probe) and target memory (on the
+processor). An "upload" is defined as a sequence of processor loads from target memory and stores to dmseg. A
+"download" is a sequence of processor loads from dmseg and stores to target memory. The "Fastdata area" specifies
+the legal range of dmseg addresses (0xFF20.0000 - 0xFF20.000F) that can be used for uploads and downloads. The
+Data + Fastdata registers (selected with the FASTDATA instruction) allow efficient completion of pending Fastdata
+area accesses.
+During Fastdata uploads and downloads, the processor will stall on accesses to the Fastdata area. The PrAcc (processor
+access pending bit) will be 1 indicating the probe is required to complete the access. Both upload and download
+accesses are attempted by shifting in a zero SPrAcc value (to request access completion) and shifting out SPrAcc to
+see if the attempt will be successful (i.e., there was an access pending and a legal Fastdata area address was used).
+Downloads will also shift in the data to be used to satisfy the load from dmseg’s Fastdata area, while uploads will
+shift out the data being stored to dmseg’s Fastdata area.
+As noted above, two conditions must be true for the Fastdata access to succeed. These are:
+
+ - PrAcc must be 1, i.e., there must be a pending processor access.
+ - The Fastdata operation must use a valid Fastdata area address in dmseg (0xFF20.0000 to 0xFF20.000F).
+
+Basically, because FASTDATA area in dmseg is 16 bytes, we transfer (0xFF20.0000 - 0xFF20.000F)
+FASTDATA scan TAP instruction selects the Data and the Fastdata registers at once.
+
+They come in order :
+TDI -> | Data register| -> | Fastdata register | -> TDO
+
+FASTDATA register is 1-bit width register. It takes in SPrAcc bit which should be shifted first,
+followed by 32 bit of data.
+
+Scan width of FASTDTA is 33 bits in total : 33 bits are shifted in and 33 bits are shifted out.
+
+First bit that is shifted out is SPrAcc that comes out of Fastdata register and should give us status on FATSDATA write we want to do.
+
+@section fdcheck OpenOCD misses FASTDATA check
+
+Download flow (probe -> target block transfer) :
+
+1) Probe transfer target execution to a loop in target memory doing a fixed number of "loads" to fastdata area of dmseg (and stores to the target download destination.)
+
+2) Probe loops attempting to satisfy the loads "expected" from the target.
+   On FASTDATA access "successful" move on to next "load".
+   On FASTDATA access "failure" repeat until "successful" or timeout.
+   (A "failure" is an attempt to satisfy an access when none are pending.)
+
+Note: A failure may have a recoverable (and even expected) cause like slow target execution of the load loop. Other failures may be due to unexpected more troublesome causes like an exception while in debug mode or a target hang on a bad target memory access.
+
+Shifted out SPrAcc bit inform us that there was CPU access pendingand that it can be complete.
+
+Basically, we should do following procedure :
+
+ - Download (dongle -> CPU) :
+You shift "download" DATA and FASTDATA[SPrAcc] = 0 (33 bit scan) into the target. If the value of FASTDATA[SPrAcc] shifted out is "1" then an access was pending when you started the scan and it is now complete.
+
+If SPrAcc is 0 then no access was pending to the fastdata area. (Repeat attempt to complete the access you expect for this data word. Timeout if you think the access is "long overdue" as something unexpected has happened.)
+
+ - Upload (CPU -> dongle) :
+You shift "dummy" DATA and FASTDATA[SPrAcc] = 0 (33 bit scan) into the target. If the value of FASTDATA[SPrAcc] shifted out is "1" then an access was pending when you started the scan and it is now complete. The "upload" is the DATA shifted out of the target.
+
+If SPrAcc is 0 then no access was pending to the fastdata area. (Repeat attempt to complete the access you expect for this data word. Timeout if you think the access is "long overdue" as something unexpected has happened.)
+
+Basically, if checking first (before scan) if CPU is pending on FASTDATA access (PrAcc is "1"), like this
+
+@code
+wait(ready);
+do_scan();
+@endcode
+
+which is inefficient, we should do it like this :
+
+@code
+BEGIN :
+	do_scan();
+	if (!was_ready)
+	goto BEGIN;
+@endcode
+
+by checking SPrAcc that we shifted out.
+
+If some FASTDATA write fails, OpenOCD will continue with it's loop (on the host side), but CPU will rest pending (on the target side)
+waiting for correct FASTDATA write.
+
+Since OpenOCD goes ahead, it will eventually finish it's loop, and proceede to check if CPU took all the data. But since CPU did not took all the data,
+it is still turns in handler's loop in RAM, stalling on Fastdata area so this check :
+
+@code
+address = 0;
+mips_ejtag_set_instr(ejtag_info, EJTAG_INST_ADDRESS);
+retval = mips_ejtag_drscan_32(ejtag_info, &address);
+if (retval != ERROR_OK)
+	return retval;
+
+if (address != MIPS32_PRACC_TEXT)
+	LOG_ERROR("mini program did not return to start");
+@endcode
+
+fails, and that gives us enough information of the failure.
+
+In this case, we can lower the JTAG frquency and try again, bacuse most probable reason of this failure is that we tried FASTDATA upload before CPU arrived to rise PrAcc (i.e. before it was pending on access).
+However, the reasons for failure might be numerous : reset, exceptions which can occur in debug mode, bus hangs, etc.
+
+If lowering the JTAG freq does not work either, we can fall back to more robust solution with patch posted below.
+
+To summarize, FASTDATA communication goes as following :
+
+-# CPU jumps to Debug Exception Vector Location 0xFF200200 in dmseg and it stalls, pending and waiting for EJTAG to give it first debug instruction and signall it by putting PrAcc to "0"
+-# When PrAcc goes to "0" CPU execute one opcode sent by EJTAG via DATA reg. Then it pends on next access, waiting for PrAcc to be put to "0" again
+-# Following this game, OpenOCD first loads handler code in RAM, and then sends the jmp_code - instruction by instruction via DATA reg, which redirects CPU to handler previously set up in RAM
+-# Once in RAM CPU does not pend on any instruction, but it executes all handler instructions untill first "fetch" to Fastdata area - then it stops and pends.
+-# So - when it comes to any instruction (opcode) in this handler in RAM which reads (or writes) to Fastdata area (0xF..F20.0000 to 0xF..F20.000F), CPU stops (i.e. stalls access).
+   I.e. it stops on this lw opcode and waits to FASTDATA TAP command from the probe.
+-# CPU continues only if OpenOCD shifted in SPrAcc "0" (and if the PrAcc was "1"). It shifts-out "1" to tell us that it was OK (processor was stalled, so it can complete the access),
+   and that it continued execution of the handler in RAM.
+-# If PrAcc was not "1" CPU will not continue (go to next instruction), but will shift-out "0" and keep stalling on the same instruction of my handler in RAM.
+-# When Fastdata loop is finished, CPU executes all following hadler instructions in RAM (prologue).
+-# In the end of my handler in RAM, I jumps back to begining of Debug Exception Vector Location 0xFF200200 in dmseg.
+-# When it jumps back to 0xFF200200 in dmseg processor stops and pends, waiting for OpenOCD to send it instruction via DATA reg and signal it by putting PrAcc to "0".
+
+*/
--- a/doc/openocd.1
+++ b/doc/openocd.1
@@ -78,7 +78,7 @@ Show a help text and exit.
 Show version information and exit.
 .SH "BUGS"
 Please report any bugs on the mailing list at
-.BR openocd\-development@lists.berlios.de .
+.BR openocd\-devel@lists.sourceforge.net .
 .SH "LICENCE"
 .B OpenOCD
 is covered by the GNU General Public License (GPL), version 2 or later.
--- a/doc/openocd.texi
+++ b/doc/openocd.texi
--- a/2
+++ b/2
--- a/src/flash/nand/arm_io.c
+++ b/src/flash/nand/arm_io.c
@@ -28,6 +28,7 @@
 #include "arm_io.h"
 #include <helper/binarybuffer.h>
 #include <target/arm.h>
+#include <target/armv7m.h>
 #include <target/algorithm.h>

 /**
@@ -78,14 +79,13 @@ static int arm_code_to_working_area(struct target *target,

 /**
 * ARM-specific bulk write from buffer to address of 8-bit wide NAND.
- * For now this only supports ARMv4 and ARMv5 cores.
+ * For now this supports ARMv4,ARMv5 and ARMv7-M cores.
 *
 * Enhancements to target_run_algorithm() could enable:
 *   - ARMv6 and ARMv7 cores in ARM mode
 *
 * Different code fragments could handle:
- *   - Thumb2 cores like Cortex-M (needs different byteswapping)
- *   - 16-bit wide data (needs different setup too)
+ *   - 16-bit wide data (needs different setup)
 *
 * @param nand Pointer to the arm_nand_data struct that defines the I/O
 * @param data Pointer to the data to be copied to flash
@@ -95,7 +95,9 @@ static int arm_code_to_working_area(struct target *target,
 int arm_nandwrite(struct arm_nand_data *nand, uint8_t *data, int size)
 {
 	struct target *target = nand->target;
-	struct arm_algorithm algo;
+	struct arm_algorithm armv4_5_algo;
+	struct armv7m_algorithm armv7m_algo;
+	void *arm_algo;
 	struct arm *arm = target->arch_info;
 	struct reg_param reg_params[3];
 	uint32_t target_buf;
@@ -107,7 +109,7 @@ int arm_nandwrite(struct arm_nand_data *nand, uint8_t *data, int size)
 	 *  r1	buffer address
 	 *  r2	buffer length
 	 */
-	static const uint32_t code[] = {
+	static const uint32_t code_armv4_5[] = {
 		0xe4d13001,	/* s: ldrb  r3, [r1], #1 */
 		0xe5c03000,	/*    strb  r3, [r0]     */
 		0xe2522001,	/*    subs  r2, r2, #1   */
@@ -117,8 +119,41 @@ int arm_nandwrite(struct arm_nand_data *nand, uint8_t *data, int size)
 		0xe1200070,	/* e: bkpt  #0           */
 	};

+	/* Inputs:
+	 *  r0	NAND data address (byte wide)
+	 *  r1	buffer address
+	 *  r2	buffer length
+	 *
+	 * see contrib/loaders/flash/armv7m_io.s for src
+	 */
+	static const uint32_t code_armv7m[] = {
+		0x3b01f811,
+		0x3a017003,
+		0xaffaf47f,
+		0xbf00be00,
+	};
+
+	int target_code_size = 0;
+	const uint32_t *target_code_src = NULL;
+
+	/* set up algorithm */
+	if (is_armv7m(target_to_armv7m(target))) {  /* armv7m target */
+		armv7m_algo.common_magic = ARMV7M_COMMON_MAGIC;
+		armv7m_algo.core_mode = ARM_MODE_THREAD;
+		arm_algo = &armv7m_algo;
+		target_code_size = sizeof(code_armv7m);
+		target_code_src = code_armv7m;
+	} else {
+		armv4_5_algo.common_magic = ARM_COMMON_MAGIC;
+		armv4_5_algo.core_mode = ARM_MODE_SVC;
+		armv4_5_algo.core_state = ARM_STATE_ARM;
+		arm_algo = &armv4_5_algo;
+		target_code_size = sizeof(code_armv4_5);
+		target_code_src = code_armv4_5;
+	}
+
 	if (nand->op != ARM_NAND_WRITE || !nand->copy_area) {
-		retval = arm_code_to_working_area(target, code, sizeof(code),
+		retval = arm_code_to_working_area(target, target_code_src, target_code_size,
 				nand->chunk_size, &nand->copy_area);
 		if (retval != ERROR_OK)
 			return retval;
@@ -127,20 +162,12 @@ int arm_nandwrite(struct arm_nand_data *nand, uint8_t *data, int size)
 	nand->op = ARM_NAND_WRITE;

 	/* copy data to work area */
-	target_buf = nand->copy_area->address + sizeof(code);
-	retval = target_bulk_write_memory(target, target_buf, size / 4, data);
-	if (retval == ERROR_OK && (size & 3) != 0)
-		retval = target_write_memory(target,
-				target_buf + (size & ~3),
-				1, size & 3, data + (size & ~3));
+	target_buf = nand->copy_area->address + target_code_size;
+	retval = target_write_buffer(target, target_buf, size, data);
 	if (retval != ERROR_OK)
 		return retval;

-	/* set up algorithm and parameters */
-	algo.common_magic = ARM_COMMON_MAGIC;
-	algo.core_mode = ARM_MODE_SVC;
-	algo.core_state = ARM_STATE_ARM;
-
+	/* set up parameters */
 	init_reg_param(&reg_params[0], "r0", 32, PARAM_IN);
 	init_reg_param(&reg_params[1], "r1", 32, PARAM_IN);
 	init_reg_param(&reg_params[2], "r2", 32, PARAM_IN);
@@ -151,11 +178,11 @@ int arm_nandwrite(struct arm_nand_data *nand, uint8_t *data, int size)

 	/* armv4 must exit using a hardware breakpoint */
 	if (arm->is_armv4)
-		exit_var = nand->copy_area->address + sizeof(code) - 4;
+		exit_var = nand->copy_area->address + target_code_size - 4;

 	/* use alg to write data from work area to NAND chip */
 	retval = target_run_algorithm(target, 0, NULL, 3, reg_params,
-			nand->copy_area->address, exit_var, 1000, &algo);
+			nand->copy_area->address, exit_var, 1000, arm_algo);
 	if (retval != ERROR_OK)
 		LOG_ERROR("error executing hosted NAND write");

@@ -178,7 +205,9 @@ int arm_nandwrite(struct arm_nand_data *nand, uint8_t *data, int size)
 int arm_nandread(struct arm_nand_data *nand, uint8_t *data, uint32_t size)
 {
 	struct target *target = nand->target;
-	struct arm_algorithm algo;
+	struct arm_algorithm armv4_5_algo;
+	struct armv7m_algorithm armv7m_algo;
+	void *arm_algo;
 	struct arm *arm = target->arch_info;
 	struct reg_param reg_params[3];
 	uint32_t target_buf;
@@ -190,7 +219,7 @@ int arm_nandread(struct arm_nand_data *nand, uint8_t *data, uint32_t size)
 	 *  r1	NAND data address (byte wide)
 	 *  r2	buffer length
 	 */
-	static const uint32_t code[] = {
+	static const uint32_t code_armv4_5[] = {
 		0xe5d13000,	/* s: ldrb  r3, [r1]     */
 		0xe4c03001,	/*    strb  r3, [r0], #1 */
 		0xe2522001,	/*    subs  r2, r2, #1   */
@@ -200,22 +229,51 @@ int arm_nandread(struct arm_nand_data *nand, uint8_t *data, uint32_t size)
 		0xe1200070,	/* e: bkpt  #0           */
 	};

+	/* Inputs:
+	 *  r0	buffer address
+	 *  r1	NAND data address (byte wide)
+	 *  r2	buffer length
+	 *
+	 * see contrib/loaders/flash/armv7m_io.s for src
+	 */
+	static const uint32_t code_armv7m[] = {
+		0xf800780b,
+		0x3a013b01,
+		0xaffaf47f,
+		0xbf00be00,
+	};
+
+	int target_code_size = 0;
+	const uint32_t *target_code_src = NULL;
+
+	/* set up algorithm */
+	if (is_armv7m(target_to_armv7m(target))) {  /* armv7m target */
+		armv7m_algo.common_magic = ARMV7M_COMMON_MAGIC;
+		armv7m_algo.core_mode = ARM_MODE_THREAD;
+		arm_algo = &armv7m_algo;
+		target_code_size = sizeof(code_armv7m);
+		target_code_src = code_armv7m;
+	} else {
+		armv4_5_algo.common_magic = ARM_COMMON_MAGIC;
+		armv4_5_algo.core_mode = ARM_MODE_SVC;
+		armv4_5_algo.core_state = ARM_STATE_ARM;
+		arm_algo = &armv4_5_algo;
+		target_code_size = sizeof(code_armv4_5);
+		target_code_src = code_armv4_5;
+	}
+
 	/* create the copy area if not yet available */
 	if (nand->op != ARM_NAND_READ || !nand->copy_area) {
-		retval = arm_code_to_working_area(target, code, sizeof(code),
+		retval = arm_code_to_working_area(target, target_code_src, target_code_size,
 				nand->chunk_size, &nand->copy_area);
 		if (retval != ERROR_OK)
 			return retval;
 	}

 	nand->op = ARM_NAND_READ;
-	target_buf = nand->copy_area->address + sizeof(code);
-
-	/* set up algorithm and parameters */
-	algo.common_magic = ARM_COMMON_MAGIC;
-	algo.core_mode = ARM_MODE_SVC;
-	algo.core_state = ARM_STATE_ARM;
+	target_buf = nand->copy_area->address + target_code_size;

+	/* set up parameters */
 	init_reg_param(&reg_params[0], "r0", 32, PARAM_IN);
 	init_reg_param(&reg_params[1], "r1", 32, PARAM_IN);
 	init_reg_param(&reg_params[2], "r2", 32, PARAM_IN);
@@ -226,11 +284,11 @@ int arm_nandread(struct arm_nand_data *nand, uint8_t *data, uint32_t size)

 	/* armv4 must exit using a hardware breakpoint */
 	if (arm->is_armv4)
-		exit_var = nand->copy_area->address + sizeof(code) - 4;
+		exit_var = nand->copy_area->address + target_code_size - 4;

 	/* use alg to write data from NAND chip to work area */
 	retval = target_run_algorithm(target, 0, NULL, 3, reg_params,
-			nand->copy_area->address, exit_var, 1000, &algo);
+			nand->copy_area->address, exit_var, 1000, arm_algo);
 	if (retval != ERROR_OK)
 		LOG_ERROR("error executing hosted NAND read");

--- a/src/flash/nor/Makefile.am
+++ b/src/flash/nor/Makefile.am
@@ -14,14 +14,17 @@ NOR_DRIVERS = \
 	at91sam7.c \
 	avrf.c \
 	cfi.c \
+	efm32.c \
 	em357.c \
 	faux.c \
 	lpc2000.c \
 	lpc288x.c \
 	lpc2900.c \
+	lpcspifi.c \
 	non_cfi.c \
 	ocl.c \
 	pic32mx.c \
+	spi.c \
 	stmsmi.c \
 	stellaris.c \
 	stm32f1x.c \
@@ -42,6 +45,7 @@ noinst_HEADERS = \
 	driver.h \
 	imp.h \
 	non_cfi.h \
-	ocl.h
+	ocl.h \
+	spi.h

 MAINTAINERCLEANFILES = $(srcdir)/Makefile.in
--- a/src/flash/nor/aduc702x.c
+++ b/src/flash/nor/aduc702x.c
@@ -43,21 +43,12 @@ static int aduc702x_set_write_enable(struct target *target, int enable);
 #define ADUC702x_FLASH_FEEPRO           (6*4)
 #define ADUC702x_FLASH_FEEHIDE          (7*4)

-struct aduc702x_flash_bank {
-	struct working_area *write_algorithm;
-};
-
 /* flash bank aduc702x 0 0 0 0 <target#>
 * The ADC7019-28 devices all have the same flash layout */
 FLASH_BANK_COMMAND_HANDLER(aduc702x_flash_bank_command)
 {
-	struct aduc702x_flash_bank *nbank;
-
-	nbank = malloc(sizeof(struct aduc702x_flash_bank));
-
 	bank->base = 0x80000;
 	bank->size = 0xF800;	/* top 4k not accessible */
-	bank->driver_priv = nbank;

 	aduc702x_build_sector_list(bank);

@@ -157,9 +148,9 @@ static int aduc702x_write_block(struct flash_bank *bank,
 	uint32_t offset,
 	uint32_t count)
 {
-	struct aduc702x_flash_bank *aduc702x_info = bank->driver_priv;
 	struct target *target = bank->target;
 	uint32_t buffer_size = 7000;
+	struct working_area *write_algorithm;
 	struct working_area *source;
 	uint32_t address = bank->base + offset;
 	struct reg_param reg_params[6];
@@ -210,12 +201,12 @@ static int aduc702x_write_block(struct flash_bank *bank,

 	/* flash write code */
 	if (target_alloc_working_area(target, sizeof(aduc702x_flash_write_code),
-			&aduc702x_info->write_algorithm) != ERROR_OK) {
+			&write_algorithm) != ERROR_OK) {
 		LOG_WARNING("no working area available, can't do block memory writes");
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 	}

-	retval = target_write_buffer(target, aduc702x_info->write_algorithm->address,
+	retval = target_write_buffer(target, write_algorithm->address,
 			sizeof(aduc702x_flash_write_code), (uint8_t *)aduc702x_flash_write_code);
 	if (retval != ERROR_OK)
 		return retval;
@@ -224,10 +215,9 @@ static int aduc702x_write_block(struct flash_bank *bank,
 	while (target_alloc_working_area_try(target, buffer_size, &source) != ERROR_OK) {
 		buffer_size /= 2;
 		if (buffer_size <= 256) {
-			/* if we already allocated the writing code, but failed to get a buffer,
+			/* we already allocated the writing code, but failed to get a buffer,
 			 *free the algorithm */
-			if (aduc702x_info->write_algorithm)
-				target_free_working_area(target, aduc702x_info->write_algorithm);
+			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;
@@ -257,8 +247,8 @@ static int aduc702x_write_block(struct flash_bank *bank,
 		buf_set_u32(reg_params[4].value, 0, 32, 0xFFFFF800);

 		retval = target_run_algorithm(target, 0, NULL, 5,
-				reg_params, aduc702x_info->write_algorithm->address,
-				aduc702x_info->write_algorithm->address +
+				reg_params, write_algorithm->address,
+				write_algorithm->address +
 				sizeof(aduc702x_flash_write_code) - 4,
 				10000, &arm_algo);
 		if (retval != ERROR_OK) {
@@ -279,7 +269,7 @@ static int aduc702x_write_block(struct flash_bank *bank,
 	}

 	target_free_working_area(target, source);
-	target_free_working_area(target, aduc702x_info->write_algorithm);
+	target_free_working_area(target, write_algorithm);

 	destroy_reg_param(&reg_params[0]);
 	destroy_reg_param(&reg_params[1]);
--- a/src/flash/nor/at91sam3.c
+++ b/src/flash/nor/at91sam3.c
@@ -89,12 +89,11 @@
 #define         AT91C_EFC_FCMD_WP                   (0x1)	/* (EFC) Write Page */
 #define         AT91C_EFC_FCMD_WPL                  (0x2)	/* (EFC) Write Page and Lock */
 #define         AT91C_EFC_FCMD_EWP                  (0x3)	/* (EFC) Erase Page and Write Page */
-#define         AT91C_EFC_FCMD_EWPL                 (0x4)	/* (EFC) Erase Page and Write Page
-								 * then Lock */
+#define         AT91C_EFC_FCMD_EWPL                 (0x4)	/* (EFC) Erase Page and Write Page then Lock */
 #define         AT91C_EFC_FCMD_EA                   (0x5)	/* (EFC) Erase All */
-/* cmd6 is not present int he at91sam3u4/2/1 data sheet table 17-2 */
+/* cmd6 is not present in the at91sam3u4/2/1 data sheet table 17-2 */
 /* #define      AT91C_EFC_FCMD_EPL                  (0x6) // (EFC) Erase plane? */
-/* cmd7 is not present int he at91sam3u4/2/1 data sheet table 17-2 */
+/* cmd7 is not present in the at91sam3u4/2/1 data sheet table 17-2 */
 /* #define      AT91C_EFC_FCMD_EPA                  (0x7) // (EFC) Erase pages? */
 #define         AT91C_EFC_FCMD_SLB                  (0x8)	/* (EFC) Set Lock Bit */
 #define         AT91C_EFC_FCMD_CLB                  (0x9)	/* (EFC) Clear Lock Bit */
@@ -197,7 +196,7 @@ struct sam3_bank_private {

 	/* so we can find the chip we belong to */
 	struct sam3_chip *pChip;
-	/* so we can find the orginal bank pointer */
+	/* so we can find the original bank pointer */
 	struct flash_bank *pBank;
 	unsigned bank_number;
 	uint32_t controller_address;
@@ -213,7 +212,7 @@ struct sam3_bank_private {
 struct sam3_chip_details {
 	/* THERE ARE DRAGONS HERE.. */
 	/* note: If you add pointers here */
-	/* becareful about them as they */
+	/* be careful about them as they */
 	/* may need to be updated inside */
 	/* the function: "sam3_GetDetails() */
 	/* which copy/overwrites the */
@@ -2400,8 +2399,8 @@ static void sam3_explain_ckgr_plla(struct sam3_chip *pChip)
 		LOG_USER("\tPLLA Freq: (Disabled,mula = 0)");
 	else if (diva == 0)
 		LOG_USER("\tPLLA Freq: (Disabled,diva = 0)");
-	else if (diva == 1) {
-		pChip->cfg.plla_freq = (pChip->cfg.mainosc_freq * (mula + 1));
+	else if (diva >= 1) {
+		pChip->cfg.plla_freq = (pChip->cfg.mainosc_freq * (mula + 1) / diva);
 		LOG_USER("\tPLLA Freq: %3.03f MHz",
 			_tomhz(pChip->cfg.plla_freq));
 	}
@@ -2615,7 +2614,7 @@ static int sam3_ReadAllRegs(struct sam3_chip *pChip)
 		r = sam3_ReadThisReg(pChip,
 				sam3_get_reg_ptr(&(pChip->cfg), pReg));
 		if (r != ERROR_OK) {
-			LOG_ERROR("Cannot read SAM3 registere: %s @ 0x%08x, Error: %d",
+			LOG_ERROR("Cannot read SAM3 register: %s @ 0x%08x, Error: %d",
 				pReg->name, ((unsigned)(pReg->address)), r);
 			return r;
 		}
@@ -2973,7 +2972,7 @@ static int sam3_erase(struct flash_bank *bank, int first, int last)
 		LOG_DEBUG("Here");
 		return FLASHD_EraseEntireBank(pPrivate);
 	}
-	LOG_INFO("sam3 auto-erases while programing (request ignored)");
+	LOG_INFO("sam3 auto-erases while programming (request ignored)");
 	return ERROR_OK;
 }

@@ -3018,7 +3017,7 @@ static int sam3_page_read(struct sam3_bank_private *pPrivate, unsigned pagenum,
 	int r;

 	adr = pagenum * pPrivate->page_size;
-	adr += adr + pPrivate->base_address;
+	adr += pPrivate->base_address;

 	r = target_read_memory(pPrivate->pChip->target,
 			adr,
@@ -3126,7 +3125,7 @@ static int sam3_page_write(struct sam3_bank_private *pPrivate, unsigned pagenum,
 	int r;

 	adr = pagenum * pPrivate->page_size;
-	adr += (adr + pPrivate->base_address);
+	adr += pPrivate->base_address;

 	/* Get flash mode register value */
 	r = target_read_u32(pPrivate->pChip->target, pPrivate->controller_address, &fmr);
--- a/src/flash/nor/at91sam4.c
+++ b/src/flash/nor/at91sam4.c
@@ -70,14 +70,20 @@
 /* at91sam4s series (has always one flash bank)*/
 #define FLASH_BANK_BASE_S   0x00400000

+/* at91sam4sd series (two one flash banks), first bank address */
+#define FLASH_BANK0_BASE_SD FLASH_BANK_BASE_S
+/* at91sam4sd16x, second bank address */
+#define FLASH_BANK1_BASE_1024K_SD (FLASH_BANK0_BASE_SD+(1024*1024/2))
+/* at91sam4sd32x, second bank address */
+#define FLASH_BANK1_BASE_2048K_SD (FLASH_BANK0_BASE_SD+(2048*1024/2))
+
 #define         AT91C_EFC_FCMD_GETD                 (0x0)	/* (EFC) Get Flash Descriptor */
 #define         AT91C_EFC_FCMD_WP                   (0x1)	/* (EFC) Write Page */
 #define         AT91C_EFC_FCMD_WPL                  (0x2)	/* (EFC) Write Page and Lock */
 #define         AT91C_EFC_FCMD_EWP                  (0x3)	/* (EFC) Erase Page and Write Page */
-#define         AT91C_EFC_FCMD_EWPL                 (0x4)	/* (EFC) Erase Page and Write Page
-								 * then Lock */
+#define         AT91C_EFC_FCMD_EWPL                 (0x4)	/* (EFC) Erase Page and Write Page then Lock */
 #define         AT91C_EFC_FCMD_EA                   (0x5)	/* (EFC) Erase All */
-/* cmd6 is not present int he at91sam4u4/2/1 data sheet table 19-2 */
+/* cmd6 is not present in the at91sam4u4/2/1 data sheet table 19-2 */
 /* #define      AT91C_EFC_FCMD_EPL                  (0x6) // (EFC) Erase plane? */
 #define			AT91C_EFC_FCMD_EPA                  (0x7)     /* (EFC) Erase pages */
 #define         AT91C_EFC_FCMD_SLB                  (0x8)	/* (EFC) Set Lock Bit */
@@ -166,7 +172,7 @@ struct sam4_bank_private {

 	/* so we can find the chip we belong to */
 	struct sam4_chip *pChip;
-	/* so we can find the orginal bank pointer */
+	/* so we can find the original bank pointer */
 	struct flash_bank *pBank;
 	unsigned bank_number;
 	uint32_t controller_address;
@@ -182,7 +188,7 @@ struct sam4_bank_private {
 struct sam4_chip_details {
 	/* THERE ARE DRAGONS HERE.. */
 	/* note: If you add pointers here */
-	/* becareful about them as they */
+	/* be careful about them as they */
 	/* may need to be updated inside */
 	/* the function: "sam4_GetDetails() */
 	/* which copy/overwrites the */
@@ -248,7 +254,7 @@ static struct sam4_chip *get_current_sam4(struct command_context *cmd_ctx)
 }

 /*The actual sector size of the SAM4S flash memory is 65536 bytes. 16 sectors for a 1024KB device*/
-/*The lockregions are 8KB per lock reqion, with a 1024KB device having 128 lock reqions. */
+/*The lockregions are 8KB per lock region, with a 1024KB device having 128 lock regions. */
 /*For the best results, nsectors are thus set to the amount of lock regions, and the sector_size*/
 /*set to the lock region size.  Page erases are used to erase 8KB sections when programming*/

@@ -453,6 +459,51 @@ static const struct sam4_chip_details all_sam4_details[] = {
 		  },
 		},
 	},
+
+	/*at91sam4sd32c*/
+	{
+		.chipid_cidr    = 0x29a70ee0,
+		.name           = "at91sam4sd32c",
+		.total_flash_size     = 2048 * 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 =  1024 * 1024,
+				.nsectors   =  128,
+				.sector_size = 8192,
+				.page_size   = 512,
+			},
+
+/*		.bank[1] = { */
+			{
+				.probed = 0,
+				.pChip  = NULL,
+				.pBank  = NULL,
+				.bank_number = 1,
+				.base_address = FLASH_BANK1_BASE_2048K_SD,
+				.controller_address = 0x400e0c00,
+				.flash_wait_states = 6,	/* workaround silicon bug */
+				.present = 1,
+				.size_bytes =  1024 * 1024,
+				.nsectors   =  128,
+				.sector_size = 8192,
+				.page_size   = 512,
+			},
+		},
+	},
+
 	/* terminate */
 	{
 		.chipid_cidr    = 0,
@@ -722,10 +773,17 @@ static int FLASHD_ErasePages(struct sam4_bank_private *pPrivate,
 			break;
 	}

+	/* AT91C_EFC_FCMD_EPA
+	 * According to the datasheet FARG[15:2] defines the page from which
+	 * the erase will start.This page must be modulo 4, 8, 16 or 32
+	 * according to the number of pages to erase. FARG[1:0] defines the
+	 * number of pages to be erased. Previously (firstpage << 2) was used
+	 * to conform to this, seems it should not be shifted...
+	 */
 	return EFC_PerformCommand(pPrivate,
 		/* send Erase Page */
 		AT91C_EFC_FCMD_EPA,
-		(firstPage << 2) | erasePages,
+		(firstPage) | erasePages,
 		status);
 }

@@ -1401,7 +1459,7 @@ static int sam4_ReadAllRegs(struct sam4_chip *pChip)
 		r = sam4_ReadThisReg(pChip,
 				sam4_get_reg_ptr(&(pChip->cfg), pReg));
 		if (r != ERROR_OK) {
-			LOG_ERROR("Cannot read SAM4 registere: %s @ 0x%08x, Error: %d",
+			LOG_ERROR("Cannot read SAM4 register: %s @ 0x%08x, Error: %d",
 				pReg->name, ((unsigned)(pReg->address)), r);
 			return r;
 		}
@@ -1512,19 +1570,29 @@ FLASH_BANK_COMMAND_HANDLER(sam4_flash_bank_command)
 	switch (bank->base) {
 		default:
 			LOG_ERROR("Address 0x%08x invalid bank address (try 0x%08x"
-			"[at91sam4s series] )",
-			((unsigned int)(bank->base)),
-			((unsigned int)(FLASH_BANK_BASE_S)));
+				"[at91sam4s series] )",
+				((unsigned int)(bank->base)),
+				((unsigned int)(FLASH_BANK_BASE_S)));
 			return ERROR_FAIL;
 			break;

 		/* at91sam4s series only has bank 0*/
+		/* at91sam4sd series has the same address for bank 0 (FLASH_BANK0_BASE_SD)*/
 		case FLASH_BANK_BASE_S:
 			bank->driver_priv = &(pChip->details.bank[0]);
 			bank->bank_number = 0;
 			pChip->details.bank[0].pChip = pChip;
 			pChip->details.bank[0].pBank = bank;
 			break;
+
+		/* Bank 1 of at91sam4sd series */
+		case FLASH_BANK1_BASE_1024K_SD:
+		case FLASH_BANK1_BASE_2048K_SD:
+			bank->driver_priv = &(pChip->details.bank[1]);
+			bank->bank_number = 1;
+			pChip->details.bank[1].pChip = pChip;
+			pChip->details.bank[1].pBank = bank;
+			break;
 	}

 	/* we initialize after probing. */
@@ -1704,7 +1772,7 @@ static int sam4_erase(struct flash_bank *bank, int first, int last)
 		LOG_DEBUG("Here");
 		return FLASHD_EraseEntireBank(pPrivate);
 	}
-	LOG_INFO("sam4 does not auto-erase while programing (Erasing relevant sectors)");
+	LOG_INFO("sam4 does not auto-erase while programming (Erasing relevant sectors)");
 	LOG_INFO("sam4 First: 0x%08x Last: 0x%08x", (unsigned int)(first), (unsigned int)(last));
 	for (i = first; i <= last; i++) {
 		/*16 pages equals 8KB - Same size as a lock region*/
@@ -1714,7 +1782,7 @@ static int sam4_erase(struct flash_bank *bank, int first, int last)
 			LOG_ERROR("SAM4: Error performing Erase page @ lock region number %d",
 				(unsigned int)(i));
 		if (status & (1 << 2)) {
-			LOG_ERROR("SAM4: Lock Reqion %d is locked", (unsigned int)(i));
+			LOG_ERROR("SAM4: Lock Region %d is locked", (unsigned int)(i));
 			return ERROR_FAIL;
 		}
 		if (status & (1 << 1)) {
--- a/src/flash/nor/cfi.c
+++ b/src/flash/nor/cfi.c
@@ -51,7 +51,7 @@ static struct cfi_unlock_addresses cfi_unlock_addresses[] = {
 	[CFI_UNLOCK_5555_2AAA] = { .unlock1 = 0x5555, .unlock2 = 0x2aaa },
 };

-/* CFI fixups foward declarations */
+/* CFI fixups forward declarations */
 static void cfi_fixup_0002_erase_regions(struct flash_bank *bank, void *param);
 static void cfi_fixup_0002_unlock_addresses(struct flash_bank *bank, void *param);
 static void cfi_fixup_reversed_erase_regions(struct flash_bank *bank, void *param);
@@ -109,7 +109,6 @@ static void cfi_fixup(struct flash_bank *bank, const struct cfi_fixup *fixups)
 	}
 }

-/* inline uint32_t flash_address(struct flash_bank *bank, int sector, uint32_t offset) */
 static inline uint32_t flash_address(struct flash_bank *bank, int sector, uint32_t offset)
 {
 	struct cfi_flash_bank *cfi_info = bank->driver_priv;
@@ -807,7 +806,7 @@ FLASH_BANK_COMMAND_HANDLER(cfi_flash_bank_command)
 	 * - not exceed max value;
 	 * - not be null;
 	 * - be equal to a power of 2.
-	 * bus must be wide enought to hold one chip */
+	 * bus must be wide enough to hold one chip */
 	if ((bank->chip_width > CFI_MAX_CHIP_WIDTH)
 			|| (bank->bus_width > CFI_MAX_BUS_WIDTH)
 			|| (bank->chip_width == 0)
@@ -825,8 +824,6 @@ FLASH_BANK_COMMAND_HANDLER(cfi_flash_bank_command)
 	cfi_info->pri_ext = NULL;
 	bank->driver_priv = cfi_info;

-	cfi_info->write_algorithm = NULL;
-
 	cfi_info->x16_as_x8 = 0;
 	cfi_info->jedec_probe = 0;
 	cfi_info->not_cfi = 0;
@@ -838,8 +835,6 @@ FLASH_BANK_COMMAND_HANDLER(cfi_flash_bank_command)
 			cfi_info->jedec_probe = 1;
 	}

-	cfi_info->write_algorithm = NULL;
-
 	/* bank wasn't probed yet */
 	cfi_info->qry[0] = 0xff;

@@ -875,9 +870,8 @@ static int cfi_intel_erase(struct flash_bank *bank, int first, int last)
 			if (retval != ERROR_OK)
 				return retval;

-			LOG_ERROR("couldn't erase block %i of flash bank at base 0x%" PRIx32,
-				i,
-				bank->base);
+			LOG_ERROR("couldn't erase block %i of flash bank at base 0x%"
+					PRIx32, i, bank->base);
 			return ERROR_FLASH_OPERATION_FAILED;
 		}
 	}
@@ -1143,10 +1137,10 @@ static uint32_t cfi_command_val(struct flash_bank *bank, uint8_t cmd)
 static int cfi_intel_write_block(struct flash_bank *bank, uint8_t *buffer,
 	uint32_t address, uint32_t count)
 {
-	struct cfi_flash_bank *cfi_info = bank->driver_priv;
 	struct target *target = bank->target;
 	struct reg_param reg_params[7];
 	struct arm_algorithm arm_algo;
+	struct working_area *write_algorithm;
 	struct working_area *source = NULL;
 	uint32_t buffer_size = 32768;
 	uint32_t write_command_val, busy_pattern_val, error_pattern_val;
@@ -1163,56 +1157,56 @@ static int cfi_intel_write_block(struct flash_bank *bank, uint8_t *buffer,

 	/* see contib/loaders/flash/armv4_5_cfi_intel_32.s for src */
 	static const uint32_t word_32_code[] = {
-		0xe4904004,	/* loop:	ldr r4, [r0], #4 */
-		0xe5813000,	/*              str r3, [r1] */
-		0xe5814000,	/*              str r4, [r1] */
-		0xe5914000,	/* busy:  ldr r4, [r1] */
-		0xe0047005,	/*		and r7, r4, r5 */
-		0xe1570005,	/*		cmp r7, r5 */
-		0x1afffffb,	/*		bne busy */
-		0xe1140006,	/*              tst r4, r6 */
-		0x1a000003,	/*		bne done */
-		0xe2522001,	/*		subs r2, r2, #1 */
-		0x0a000001,	/*              beq done */
-		0xe2811004,	/*		add r1, r1 #4 */
-		0xeafffff2,	/*              b loop */
-		0xeafffffe	/* done:	b -2 */
+		0xe4904004,	/* loop: ldr r4, [r0], #4 */
+		0xe5813000,	/*       str r3, [r1] */
+		0xe5814000,	/*       str r4, [r1] */
+		0xe5914000,	/* busy: ldr r4, [r1] */
+		0xe0047005,	/*        and r7, r4, r5 */
+		0xe1570005,	/*       cmp r7, r5 */
+		0x1afffffb,	/*       bne busy */
+		0xe1140006,	/*       tst r4, r6 */
+		0x1a000003,	/*       bne done */
+		0xe2522001,	/*       subs r2, r2, #1 */
+		0x0a000001,	/*       beq done */
+		0xe2811004,	/*       add r1, r1 #4 */
+		0xeafffff2,	/*       b loop */
+		0xeafffffe	/* done: b -2 */
 	};

 	/* see contib/loaders/flash/armv4_5_cfi_intel_16.s for src */
 	static const uint32_t word_16_code[] = {
-		0xe0d040b2,	/* loop:	ldrh r4, [r0], #2 */
-		0xe1c130b0,	/*              strh r3, [r1] */
-		0xe1c140b0,	/*              strh r4, [r1] */
-		0xe1d140b0,	/* busy	ldrh r4, [r1] */
-		0xe0047005,	/*		and r7, r4, r5 */
-		0xe1570005,	/*		cmp r7, r5 */
-		0x1afffffb,	/*		bne busy */
-		0xe1140006,	/*              tst r4, r6 */
-		0x1a000003,	/*		bne done */
-		0xe2522001,	/*		subs r2, r2, #1 */
-		0x0a000001,	/*              beq done */
-		0xe2811002,	/*		add r1, r1 #2 */
-		0xeafffff2,	/*              b loop */
+		0xe0d040b2,	/* loop: ldrh r4, [r0], #2 */
+		0xe1c130b0,	/*       strh r3, [r1] */
+		0xe1c140b0,	/*       strh r4, [r1] */
+		0xe1d140b0,	/* busy  ldrh r4, [r1] */
+		0xe0047005,	/*       and r7, r4, r5 */
+		0xe1570005,	/*       cmp r7, r5 */
+		0x1afffffb,	/*       bne busy */
+		0xe1140006,	/*       tst r4, r6 */
+		0x1a000003,	/*       bne done */
+		0xe2522001,	/*       subs r2, r2, #1 */
+		0x0a000001,	/*       beq done */
+		0xe2811002,	/*       add r1, r1 #2 */
+		0xeafffff2,	/*       b loop */
 		0xeafffffe	/* done:	b -2 */
 	};

 	/* see contib/loaders/flash/armv4_5_cfi_intel_8.s for src */
 	static const uint32_t word_8_code[] = {
-		0xe4d04001,	/* loop:	ldrb r4, [r0], #1 */
-		0xe5c13000,	/*              strb r3, [r1] */
-		0xe5c14000,	/*              strb r4, [r1] */
-		0xe5d14000,	/* busy	ldrb r4, [r1] */
-		0xe0047005,	/*		and r7, r4, r5 */
-		0xe1570005,	/*		cmp r7, r5 */
-		0x1afffffb,	/*		bne busy */
-		0xe1140006,	/*              tst r4, r6 */
-		0x1a000003,	/*		bne done */
-		0xe2522001,	/*		subs r2, r2, #1 */
-		0x0a000001,	/*              beq done */
-		0xe2811001,	/*		add r1, r1 #1 */
-		0xeafffff2,	/*              b loop */
-		0xeafffffe	/* done:	b -2 */
+		0xe4d04001,	/* loop: ldrb r4, [r0], #1 */
+		0xe5c13000,	/*       strb r3, [r1] */
+		0xe5c14000,	/*       strb r4, [r1] */
+		0xe5d14000,	/* busy  ldrb r4, [r1] */
+		0xe0047005,	/*       and r7, r4, r5 */
+		0xe1570005,	/*       cmp r7, r5 */
+		0x1afffffb,	/*       bne busy */
+		0xe1140006,	/*       tst r4, r6 */
+		0x1a000003,	/*       bne done */
+		0xe2522001,	/*       subs r2, r2, #1 */
+		0x0a000001,	/*       beq done */
+		0xe2811001,	/*       add r1, r1 #1 */
+		0xeafffff2,	/*       b loop */
+		0xeafffffe	/* done: b -2 */
 	};
 	uint8_t target_code[4*CFI_MAX_INTEL_CODESIZE];
 	const uint32_t *target_code_src;
@@ -1236,7 +1230,7 @@ static int cfi_intel_write_block(struct flash_bank *bank, uint8_t *buffer,
 	 * if not we only need target_code_size. */

 	/* However, we don't want to create multiple code paths, so we
-	 * do the unecessary evaluation of target_code_src, which the
+	 * do the unnecessary evaluation of target_code_src, which the
 	 * compiler will probably nicely optimize away if not needed */

 	/* prepare algorithm code for target endian */
@@ -1260,35 +1254,33 @@ static int cfi_intel_write_block(struct flash_bank *bank, uint8_t *buffer,
 	}

 	/* flash write code */
-	if (!cfi_info->write_algorithm) {
-		if (target_code_size > sizeof(target_code)) {
-			LOG_WARNING("Internal error - target code buffer to small. "
+	if (target_code_size > sizeof(target_code)) {
+		LOG_WARNING("Internal error - target code buffer to small. "
 				"Increase CFI_MAX_INTEL_CODESIZE and recompile.");
-			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
-		}
-		cfi_fix_code_endian(target, target_code, target_code_src, target_code_size / 4);
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+	cfi_fix_code_endian(target, target_code, target_code_src, target_code_size / 4);

-		/* Get memory for block write handler */
-		retval = target_alloc_working_area(target,
-				target_code_size,
-				&cfi_info->write_algorithm);
-		if (retval != ERROR_OK) {
-			LOG_WARNING("No working area available, can't do block memory writes");
-			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
-		}
-		;
+	/* Get memory for block write handler */
+	retval = target_alloc_working_area(target,
+			target_code_size,
+			&write_algorithm);
+	if (retval != ERROR_OK) {
+		LOG_WARNING("No working area available, can't do block memory writes");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+	;

-		/* write algorithm code to working area */
-		retval = target_write_buffer(target, cfi_info->write_algorithm->address,
-				target_code_size, target_code);
-		if (retval != ERROR_OK) {
-			LOG_ERROR("Unable to write block write code to target");
-			goto cleanup;
-		}
+	/* write algorithm code to working area */
+	retval = target_write_buffer(target, write_algorithm->address,
+			target_code_size, target_code);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("Unable to write block write code to target");
+		goto cleanup;
 	}

 	/* Get a workspace buffer for the data to flash starting with 32k size.
-	   Half size until buffer would be smaller 256 Bytem then fail back */
+	 * Half size until buffer would be smaller 256 Bytes then fail back */
 	/* FIXME Why 256 bytes, why not 32 bytes (smallest flash write page */
 	while (target_alloc_working_area_try(target, buffer_size, &source) != ERROR_OK) {
 		buffer_size /= 2;
@@ -1340,8 +1332,8 @@ static int cfi_intel_write_block(struct flash_bank *bank, uint8_t *buffer,

 		/* Execute algorithm, assume breakpoint for last instruction */
 		retval = target_run_algorithm(target, 0, NULL, 7, reg_params,
-				cfi_info->write_algorithm->address,
-				cfi_info->write_algorithm->address + target_code_size -
+				write_algorithm->address,
+				write_algorithm->address + target_code_size -
 				sizeof(uint32_t),
 				10000,	/* 10s should be enough for max. 32k of data */
 				&arm_algo);
@@ -1361,7 +1353,7 @@ static int cfi_intel_write_block(struct flash_bank *bank, uint8_t *buffer,
 		/* Check return value from algo code */
 		wsm_error = buf_get_u32(reg_params[4].value, 0, 32) & error_pattern_val;
 		if (wsm_error) {
-			/* read status register (outputs debug inforation) */
+			/* read status register (outputs debug information) */
 			uint8_t status;
 			cfi_intel_wait_status_busy(bank, 100, &status);
 			cfi_intel_clear_status_register(bank);
@@ -1381,10 +1373,7 @@ cleanup:
 	if (source)
 		target_free_working_area(target, source);

-	if (cfi_info->write_algorithm) {
-		target_free_working_area(target, cfi_info->write_algorithm);
-		cfi_info->write_algorithm = NULL;
-	}
+	target_free_working_area(target, write_algorithm);

 	destroy_reg_param(&reg_params[0]);
 	destroy_reg_param(&reg_params[1]);
@@ -1405,6 +1394,7 @@ static int cfi_spansion_write_block_mips(struct flash_bank *bank, uint8_t *buffe
 	struct target *target = bank->target;
 	struct reg_param reg_params[10];
 	struct mips32_algorithm mips32_info;
+	struct working_area *write_algorithm;
 	struct working_area *source;
 	uint32_t buffer_size = 32768;
 	uint32_t status;
@@ -1429,67 +1419,50 @@ static int cfi_spansion_write_block_mips(struct flash_bank *bank, uint8_t *buffe

 	static const uint32_t mips_word_16_code[] = {
 		/* start:	*/
-		MIPS32_LHU(9, 0, 4),		/* lhu $t1, ($a0)		; out = &saddr				*/
-		MIPS32_ADDI(4, 4, 2),		/* addi $a0, $a0, 2		; saddr += 2				*/
-		MIPS32_SH(13, 0, 12),		/* sh $t5, ($t4)		; *fl_unl_addr1 =
-						 *fl_unl_cmd1		*/
-		MIPS32_SH(15, 0, 14),		/* sh $t7, ($t6)		; *fl_unl_addr2 =
-						 *fl_unl_cmd2		*/
-		MIPS32_SH(7, 0, 12),		/* sh $a3, ($t4)		; *fl_unl_addr1 =
-						 *fl_write_cmd		*/
-		MIPS32_SH(9, 0, 5),		/* sh $t1, ($a1)		; *daddr = out				*/
-		MIPS32_NOP,			/* nop									*/
+		MIPS32_LHU(9, 0, 4),			/* lhu $t1, ($a0)		; out = &saddr */
+		MIPS32_ADDI(4, 4, 2),			/* addi $a0, $a0, 2		; saddr += 2 */
+		MIPS32_SH(13, 0, 12),			/* sh $t5, ($t4)		; *fl_unl_addr1 = fl_unl_cmd1 */
+		MIPS32_SH(15, 0, 14),			/* sh $t7, ($t6)		; *fl_unl_addr2 = fl_unl_cmd2 */
+		MIPS32_SH(7, 0, 12),			/* sh $a3, ($t4)		; *fl_unl_addr1 = fl_write_cmd */
+		MIPS32_SH(9, 0, 5),				/* sh $t1, ($a1)		; *daddr = out */
+		MIPS32_NOP,						/* nop */
 		/* busy:	*/
-		MIPS32_LHU(10, 0, 5),		/* lhu $t2, ($a1)		; temp1 = *daddr			*/
-		MIPS32_XOR(11, 9, 10),		/* xor $t3, $a0, $t2		; temp2 = out ^
-						 *temp1;			*/
-		MIPS32_AND(11, 8, 11),		/* and $t3, $t0, $t3		; temp2 = temp2 &
-						 *DQ7mask		*/
-		MIPS32_BNE(11, 8, 13),		/* bne $t3, $t0, cont		; if (temp2 !=
-						 *DQ7mask) goto cont	*/
-		MIPS32_NOP,			/* nop									*/
+		MIPS32_LHU(10, 0, 5),			/* lhu $t2, ($a1)		; temp1 = *daddr */
+		MIPS32_XOR(11, 9, 10),			/* xor $t3, $a0, $t2	; temp2 = out ^ temp1; */
+		MIPS32_AND(11, 8, 11),			/* and $t3, $t0, $t3	; temp2 = temp2 & DQ7mask */
+		MIPS32_BNE(11, 8, 13),			/* bne $t3, $t0, cont	; if (temp2 != DQ7mask) goto cont */
+		MIPS32_NOP,						/* nop									*/

-		MIPS32_SRL(10, 8, 2),		/* srl $t2,$t0,2		; temp1 = DQ7mask >>
-						 *2			*/
-		MIPS32_AND(11, 10, 11),		/* and $t3, $t2, $t3		; temp2 = temp2 &
-						 *temp1			*/
-		MIPS32_BNE(11, 10, NEG16(8)),	/* bne $t3, $t2, busy		; if (temp2 !=
-						 *temp1) goto busy		*/
-		MIPS32_NOP,			/* nop									*/
+		MIPS32_SRL(10, 8, 2),			/* srl $t2,$t0,2		; temp1 = DQ7mask >> 2 */
+		MIPS32_AND(11, 10, 11),			/* and $t3, $t2, $t3	; temp2 = temp2 & temp1	*/
+		MIPS32_BNE(11, 10, NEG16(8)),	/* bne $t3, $t2, busy	; if (temp2 != temp1) goto busy	*/
+		MIPS32_NOP,						/* nop									*/

-		MIPS32_LHU(10, 0, 5),		/* lhu $t2, ($a1)		; temp1 = *daddr			*/
-		MIPS32_XOR(11, 9, 10),		/* xor $t3, $a0, $t2		; temp2 = out ^
-						 *temp1;			*/
-		MIPS32_AND(11, 8, 11),		/* and $t3, $t0, $t3		; temp2 = temp2 &
-						 *DQ7mask		*/
-		MIPS32_BNE(11, 8, 4),		/* bne $t3, $t0, cont		; if (temp2 !=
-						 *DQ7mask) goto cont	*/
-		MIPS32_NOP,			/* nop									*/
+		MIPS32_LHU(10, 0, 5),			/* lhu $t2, ($a1)		; temp1 = *daddr */
+		MIPS32_XOR(11, 9, 10),			/* xor $t3, $a0, $t2	; temp2 = out ^ temp1; */
+		MIPS32_AND(11, 8, 11),			/* and $t3, $t0, $t3	; temp2 = temp2 & DQ7mask */
+		MIPS32_BNE(11, 8, 4),			/* bne $t3, $t0, cont	; if (temp2 != DQ7mask) goto cont */
+		MIPS32_NOP,						/* nop */

-		MIPS32_XOR(9, 9, 9),		/* xor $t1, $t1, $t1		; out = 0				*/
-		MIPS32_BEQ(9, 0, 11),		/* beq $t1, $zero, done		; if (out == 0) goto
-						 *done		*/
-		MIPS32_NOP,			/* nop									*/
+		MIPS32_XOR(9, 9, 9),			/* xor $t1, $t1, $t1	; out = 0 */
+		MIPS32_BEQ(9, 0, 11),			/* beq $t1, $zero, done	; if (out == 0) goto done */
+		MIPS32_NOP,						/* nop */
 		/* cont:	*/
-		MIPS32_ADDI(6, 6, NEG16(1)),	/* addi, $a2, $a2, -1		; numwrites--				*/
-		MIPS32_BNE(6, 0, 5),		/* bne $a2, $zero, cont2	; if (numwrite != 0)
-						 *goto cont2		*/
-		MIPS32_NOP,			/* nop									*/
+		MIPS32_ADDI(6, 6, NEG16(1)),	/* addi, $a2, $a2, -1	; numwrites-- */
+		MIPS32_BNE(6, 0, 5),			/* bne $a2, $zero, cont2	; if (numwrite != 0) goto cont2 */
+		MIPS32_NOP,						/* nop */

-		MIPS32_LUI(9, 0),		/* lui $t1, 0								*/
-		MIPS32_ORI(9, 9, 0x80),		/* ori $t1, $t1, 0x80		; out = 0x80				*/
+		MIPS32_LUI(9, 0),				/* lui $t1, 0 */
+		MIPS32_ORI(9, 9, 0x80),			/* ori $t1, $t1, 0x80	; out = 0x80 */

-		MIPS32_B(4),			/* b done			; goto done				*/
-		MIPS32_NOP,			/* nop									*/
+		MIPS32_B(4),					/* b done			; goto done */
+		MIPS32_NOP,						/* nop */
 		/* cont2:	*/
-		MIPS32_ADDI(5, 5, 2),		/* addi $a0, $a0, 2		; daddr += 2				*/
-		MIPS32_B(NEG16(33)),		/* b start			; goto start				*/
-		MIPS32_NOP,			/* nop									*/
-		/* done:
-		 *MIPS32_B(NEG16(1)),	*/	/* b done			; goto done				*/
-		MIPS32_SDBBP,			/* sdbbp			; break();				*/
-		/*MIPS32_B(NEG16(33)),	*/	/* b start			; goto start
-		 * MIPS32_NOP, */
+		MIPS32_ADDI(5, 5, 2),			/* addi $a0, $a0, 2	; daddr += 2 */
+		MIPS32_B(NEG16(33)),			/* b start			; goto start */
+		MIPS32_NOP,						/* nop */
+		/* done: */
+		MIPS32_SDBBP,					/* sdbbp			; break(); */
 	};

 	mips32_info.common_magic = MIPS32_COMMON_MAGIC;
@@ -1518,44 +1491,42 @@ static int cfi_spansion_write_block_mips(struct flash_bank *bank, uint8_t *buffe
 	}

 	/* flash write code */
-	if (!cfi_info->write_algorithm) {
-		uint8_t *target_code;
+	uint8_t *target_code;

-		/* convert bus-width dependent algorithm code to correct endiannes */
-		target_code = malloc(target_code_size);
-		if (target_code == NULL) {
-			LOG_ERROR("Out of memory");
-			return ERROR_FAIL;
-		}
-		cfi_fix_code_endian(target, target_code, target_code_src, target_code_size / 4);
-
-		/* allocate working area */
-		retval = target_alloc_working_area(target, target_code_size,
-				&cfi_info->write_algorithm);
-		if (retval != ERROR_OK) {
-			free(target_code);
-			return retval;
-		}
-
-		/* write algorithm code to working area */
-		retval = target_write_buffer(target, cfi_info->write_algorithm->address,
-				target_code_size, target_code);
-		if (retval != ERROR_OK) {
-			free(target_code);
-			return retval;
-		}
-
-		free(target_code);
+	/* convert bus-width dependent algorithm code to correct endianness */
+	target_code = malloc(target_code_size);
+	if (target_code == NULL) {
+		LOG_ERROR("Out of memory");
+		return ERROR_FAIL;
 	}
+	cfi_fix_code_endian(target, target_code, target_code_src, target_code_size / 4);
+
+	/* allocate working area */
+	retval = target_alloc_working_area(target, target_code_size,
+			&write_algorithm);
+	if (retval != ERROR_OK) {
+		free(target_code);
+		return retval;
+	}
+
+	/* write algorithm code to working area */
+	retval = target_write_buffer(target, write_algorithm->address,
+			target_code_size, target_code);
+	if (retval != ERROR_OK) {
+		free(target_code);
+		return retval;
+	}
+
+	free(target_code);
+
 	/* the following code still assumes target code is fixed 24*4 bytes */

 	while (target_alloc_working_area_try(target, buffer_size, &source) != ERROR_OK) {
 		buffer_size /= 2;
 		if (buffer_size <= 256) {
-			/* if we already allocated the writing code, but failed to get a
+			/* we already allocated the writing code, but failed to get a
 			 * buffer, free the algorithm */
-			if (cfi_info->write_algorithm)
-				target_free_working_area(target, cfi_info->write_algorithm);
+			target_free_working_area(target, write_algorithm);

 			LOG_WARNING(
 				"not enough working area available, can't do block memory writes");
@@ -1593,8 +1564,8 @@ static int cfi_spansion_write_block_mips(struct flash_bank *bank, uint8_t *buffe
 		buf_set_u32(reg_params[9].value, 0, 32, 0x55555555);

 		retval = target_run_algorithm(target, 0, NULL, 10, reg_params,
-				cfi_info->write_algorithm->address,
-				cfi_info->write_algorithm->address + ((target_code_size) - 4),
+				write_algorithm->address,
+				write_algorithm->address + ((target_code_size) - 4),
 				10000, &mips32_info);
 		if (retval != ERROR_OK)
 			break;
@@ -1637,6 +1608,7 @@ static int cfi_spansion_write_block(struct flash_bank *bank, uint8_t *buffer,
 	void *arm_algo;
 	struct arm_algorithm armv4_5_algo;
 	struct armv7m_algorithm armv7m_algo;
+	struct working_area *write_algorithm;
 	struct working_area *source;
 	uint32_t buffer_size = 32768;
 	uint32_t status;
@@ -1668,8 +1640,7 @@ static int cfi_spansion_write_block(struct flash_bank *bank, uint8_t *buffer,
 		0xe5883000,		/* str	r3, [r8]				*/
 		0xe5815000,		/* str	r5, [r1]				*/
 		0xe1a00000,		/* nop							*/
-		/*
-		 * 00008110 <sp_32_busy>:		*/
+		/* 00008110 <sp_32_busy>:		*/
 		0xe5916000,		/* ldr	r6, [r1]				*/
 		0xe0257006,		/* eor	r7, r5, r6				*/
 		0xe0147007,		/* ands	r7, r4, r7				*/
@@ -1682,15 +1653,13 @@ static int cfi_spansion_write_block(struct flash_bank *bank, uint8_t *buffer,
 		0x0a000001,		/* beq	8140 <sp_32_cont> ; b if DQ7 == Data7 */
 		0xe3a05000,		/* mov	r5, #0	; 0x0 - return 0x00, error */
 		0x1a000004,		/* bne	8154 <sp_32_done>		*/
-		/*
-		 * 00008140 <sp_32_cont>:		*/
+		/* 00008140 <sp_32_cont>:		*/
 		0xe2522001,		/* subs	r2, r2, #1	; 0x1		*/
 		0x03a05080,		/* moveq	r5, #128	; 0x80	*/
 		0x0a000001,		/* beq	8154 <sp_32_done>		*/
 		0xe2811004,		/* add	r1, r1, #4	; 0x4		*/
 		0xeaffffe8,		/* b	8100 <sp_32_code>		*/
-		/*
-		 * 00008154 <sp_32_done>:		*/
+		/* 00008154 <sp_32_done>:		*/
 		0xeafffffe		/* b	8154 <sp_32_done>		*/
 	};

@@ -1703,8 +1672,7 @@ static int cfi_spansion_write_block(struct flash_bank *bank, uint8_t *buffer,
 		0xe1c830b0,		/* strh	r3, [r8]				*/
 		0xe1c150b0,		/* strh	r5, [r1]				*/
 		0xe1a00000,		/* nop			(mov r0,r0)		*/
-		/*
-		 * 00008168 <sp_16_busy>:		*/
+		/* 00008168 <sp_16_busy>:		*/
 		0xe1d160b0,		/* ldrh	r6, [r1]				*/
 		0xe0257006,		/* eor	r7, r5, r6				*/
 		0xe0147007,		/* ands	r7, r4, r7				*/
@@ -1717,15 +1685,13 @@ static int cfi_spansion_write_block(struct flash_bank *bank, uint8_t *buffer,
 		0x0a000001,		/* beq	8198 <sp_16_cont>		*/
 		0xe3a05000,		/* mov	r5, #0	; 0x0			*/
 		0x1a000004,		/* bne	81ac <sp_16_done>		*/
-		/*
-		 * 00008198 <sp_16_cont>:		*/
+		/* 00008198 <sp_16_cont>:		*/
 		0xe2522001,	/* subs	r2, r2, #1	; 0x1		*/
 		0x03a05080,	/* moveq	r5, #128	; 0x80	*/
 		0x0a000001,	/* beq	81ac <sp_16_done>		*/
 		0xe2811002,	/* add	r1, r1, #2	; 0x2		*/
 		0xeaffffe8,	/* b	8158 <sp_16_code>		*/
-		/*
-		 * 000081ac <sp_16_done>:		*/
+		/* 000081ac <sp_16_done>:		*/
 		0xeafffffe		/* b	81ac <sp_16_done>		*/
 	};

@@ -1760,8 +1726,7 @@ static int cfi_spansion_write_block(struct flash_bank *bank, uint8_t *buffer,
 		0xe1c830b0,		/* strh	r3, [r8]				*/
 		0xe1c150b0,		/* strh	r5, [r1]				*/
 		0xe1a00000,		/* nop			(mov r0,r0)		*/
-		/*
-		 * <sp_16_busy>:				*/
+		/* <sp_16_busy>:				*/
 		0xe1d160b0,		/* ldrh	r6, [r1]				*/
 		0xe0257006,		/* eor	r7, r5, r6				*/
 		0xe2177080,		/* ands	r7, #0x80				*/
@@ -1772,8 +1737,7 @@ static int cfi_spansion_write_block(struct flash_bank *bank, uint8_t *buffer,
 		0x0a000001,		/* beq	81ac <sp_16_done>		*/
 		0xe2811002,		/* add	r1, r1, #2	; 0x2		*/
 		0xeafffff0,		/* b	8158 <sp_16_code>		*/
-		/*
-		 * 000081ac <sp_16_done>:		*/
+		/* 000081ac <sp_16_done>:		*/
 		0xeafffffe		/* b	81ac <sp_16_done>		*/
 	};

@@ -1786,8 +1750,7 @@ static int cfi_spansion_write_block(struct flash_bank *bank, uint8_t *buffer,
 		0xe5c83000,		/* strb	r3, [r8]				*/
 		0xe5c15000,		/* strb	r5, [r1]				*/
 		0xe1a00000,		/* nop			(mov r0,r0)		*/
-		/*
-		 * 000081c0 <sp_8_busy>:		*/
+		/* 000081c0 <sp_8_busy>:		*/
 		0xe5d16000,		/* ldrb	r6, [r1]				*/
 		0xe0257006,		/* eor	r7, r5, r6				*/
 		0xe0147007,		/* ands	r7, r4, r7				*/
@@ -1800,24 +1763,22 @@ static int cfi_spansion_write_block(struct flash_bank *bank, uint8_t *buffer,
 		0x0a000001,		/* beq	81f0 <sp_8_cont>		*/
 		0xe3a05000,		/* mov	r5, #0	; 0x0			*/
 		0x1a000004,		/* bne	8204 <sp_8_done>		*/
-		/*
-		 * 000081f0 <sp_8_cont>:		*/
+		/* 000081f0 <sp_8_cont>:		*/
 		0xe2522001,		/* subs	r2, r2, #1	; 0x1		*/
 		0x03a05080,		/* moveq	r5, #128	; 0x80	*/
 		0x0a000001,		/* beq	8204 <sp_8_done>		*/
 		0xe2811001,		/* add	r1, r1, #1	; 0x1		*/
 		0xeaffffe8,		/* b	81b0 <sp_16_code_end>	*/
-		/*
-		 * 00008204 <sp_8_done>:		*/
+		/* 00008204 <sp_8_done>:		*/
 		0xeafffffe		/* b	8204 <sp_8_done>		*/
 	};

 	if (strncmp(target_type_name(target), "mips_m4k", 8) == 0)
 		return cfi_spansion_write_block_mips(bank, buffer, address, count);

-	if (is_armv7m(target_to_armv7m(target))) {	/* Cortex-M3 target */
+	if (is_armv7m(target_to_armv7m(target))) {	/* armv7m target */
 		armv7m_algo.common_magic = ARMV7M_COMMON_MAGIC;
-		armv7m_algo.core_mode = ARMV7M_MODE_HANDLER;
+		armv7m_algo.core_mode = ARM_MODE_THREAD;
 		arm_algo = &armv7m_algo;
 	} else if (is_arm(target_to_arm(target))) {
 		/* All other ARM CPUs have 32 bit instructions */
@@ -1845,10 +1806,8 @@ static int cfi_spansion_write_block(struct flash_bank *bank, uint8_t *buffer,
 		case 2:
 			/* Check for DQ5 support */
 			if (cfi_info->status_poll_mask & (1 << 5)) {
-				if (is_armv7m(target_to_armv7m(target))) {	/*
-												 *cortex-m3
-												 *target
-												 **/
+				if (is_armv7m(target_to_armv7m(target))) {
+					/* armv7m target */
 					target_code_src = armv7m_word_16_code;
 					target_code_size = sizeof(armv7m_word_16_code);
 				} else { /* armv4_5 target */
@@ -1880,44 +1839,42 @@ static int cfi_spansion_write_block(struct flash_bank *bank, uint8_t *buffer,
 	}

 	/* flash write code */
-	if (!cfi_info->write_algorithm) {
-		uint8_t *target_code;
+	uint8_t *target_code;

-		/* convert bus-width dependent algorithm code to correct endiannes */
-		target_code = malloc(target_code_size);
-		if (target_code == NULL) {
-			LOG_ERROR("Out of memory");
-			return ERROR_FAIL;
-		}
-		cfi_fix_code_endian(target, target_code, target_code_src, target_code_size / 4);
-
-		/* allocate working area */
-		retval = target_alloc_working_area(target, target_code_size,
-				&cfi_info->write_algorithm);
-		if (retval != ERROR_OK) {
-			free(target_code);
-			return retval;
-		}
-
-		/* write algorithm code to working area */
-		retval = target_write_buffer(target, cfi_info->write_algorithm->address,
-				target_code_size, target_code);
-		if (retval != ERROR_OK) {
-			free(target_code);
-			return retval;
-		}
-
-		free(target_code);
+	/* convert bus-width dependent algorithm code to correct endianness */
+	target_code = malloc(target_code_size);
+	if (target_code == NULL) {
+		LOG_ERROR("Out of memory");
+		return ERROR_FAIL;
 	}
+	cfi_fix_code_endian(target, target_code, target_code_src, target_code_size / 4);
+
+	/* allocate working area */
+	retval = target_alloc_working_area(target, target_code_size,
+			&write_algorithm);
+	if (retval != ERROR_OK) {
+		free(target_code);
+		return retval;
+	}
+
+	/* write algorithm code to working area */
+	retval = target_write_buffer(target, write_algorithm->address,
+			target_code_size, target_code);
+	if (retval != ERROR_OK) {
+		free(target_code);
+		return retval;
+	}
+
+	free(target_code);
+
 	/* the following code still assumes target code is fixed 24*4 bytes */

 	while (target_alloc_working_area_try(target, buffer_size, &source) != ERROR_OK) {
 		buffer_size /= 2;
 		if (buffer_size <= 256) {
-			/* if we already allocated the writing code, but failed to get a
+			/* we already allocated the writing code, but failed to get a
 			 * buffer, free the algorithm */
-			if (cfi_info->write_algorithm)
-				target_free_working_area(target, cfi_info->write_algorithm);
+			target_free_working_area(target, write_algorithm);

 			LOG_WARNING(
 				"not enough working area available, can't do block memory writes");
@@ -1955,8 +1912,8 @@ static int cfi_spansion_write_block(struct flash_bank *bank, uint8_t *buffer,
 		buf_set_u32(reg_params[9].value, 0, 32, 0x55555555);

 		retval = target_run_algorithm(target, 0, NULL, 10, reg_params,
-				cfi_info->write_algorithm->address,
-				cfi_info->write_algorithm->address + ((target_code_size) - 4),
+				write_algorithm->address,
+				write_algorithm->address + ((target_code_size) - 4),
 				10000, arm_algo);
 		if (retval != ERROR_OK)
 			break;
@@ -2637,7 +2594,7 @@ static int cfi_probe(struct flash_bank *bank)
 		 * a single bus sequence with address = 0x55, data = 0x98 should put
 		 * the device into CFI query mode.
 		 *
-		 * SST flashes clearly violate this, and we will consider them incompatbile for now
+		 * SST flashes clearly violate this, and we will consider them incompatible for now
 		 */

 		retval = cfi_query_string(bank, 0x55);
--- a/src/flash/nor/cfi.h
+++ b/src/flash/nor/cfi.h
@@ -25,8 +25,6 @@
 #define CFI_STATUS_POLL_MASK_DQ6_DQ7     0xC0 /* DQ6..DQ7 */

 struct cfi_flash_bank {
-	struct working_area *write_algorithm;
-
 	int x16_as_x8;
 	int jedec_probe;
 	int not_cfi;
--- a/src/flash/nor/drivers.c
+++ b/src/flash/nor/drivers.c
@@ -25,6 +25,7 @@
 extern struct flash_driver lpc2000_flash;
 extern struct flash_driver lpc288x_flash;
 extern struct flash_driver lpc2900_flash;
+extern struct flash_driver lpcspifi_flash;
 extern struct flash_driver cfi_flash;
 extern struct flash_driver at91sam3_flash;
 extern struct flash_driver at91sam4_flash;
@@ -48,6 +49,7 @@ extern struct flash_driver em357_flash;
 extern struct flash_driver dsp5680xx_flash;
 extern struct flash_driver fm3_flash;
 extern struct flash_driver kinetis_flash;
+extern struct flash_driver efm32_flash;

 /**
 * The list of built-in flash drivers.
@@ -57,6 +59,7 @@ static struct flash_driver *flash_drivers[] = {
 	&lpc2000_flash,
 	&lpc288x_flash,
 	&lpc2900_flash,
+	&lpcspifi_flash,
 	&cfi_flash,
 	&at91sam7_flash,
 	&at91sam3_flash,
@@ -80,6 +83,7 @@ static struct flash_driver *flash_drivers[] = {
 	&fm3_flash,
 	&dsp5680xx_flash,
 	&kinetis_flash,
+	&efm32_flash,
 	NULL,
 };

--- a/src/flash/nor/dsp5680xx_flash.c
+++ b/src/flash/nor/dsp5680xx_flash.c
@@ -45,10 +45,6 @@
 #include <target/algorithm.h>
 #include <target/dsp5680xx.h>

-struct dsp5680xx_flash_bank {
-	struct working_area *write_algorithm;
-};
-
 static int dsp5680xx_build_sector_list(struct flash_bank *bank)
 {
 	uint32_t offset = HFM_FLASH_BASE_ADDR;
@@ -71,13 +67,8 @@ static int dsp5680xx_build_sector_list(struct flash_bank *bank)
 /* flash bank dsp5680xx 0 0 0 0 <target#> */
 FLASH_BANK_COMMAND_HANDLER(dsp5680xx_flash_bank_command)
 {
-	struct dsp5680xx_flash_bank *nbank;
-
-	nbank = malloc(sizeof(struct dsp5680xx_flash_bank));
-
 	bank->base = HFM_FLASH_BASE_ADDR;
 	bank->size = HFM_SIZE_BYTES; /* top 4k not accessible */
-	bank->driver_priv = nbank;
 	bank->num_sectors = HFM_SECTOR_COUNT;
 	dsp5680xx_build_sector_list(bank);

--- a/src/flash/nor/efm32.c
+++ b/src/flash/nor/efm32.c
@@ -0,0 +1,985 @@
+/***************************************************************************
+ *   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) 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, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "imp.h"
+#include <helper/binarybuffer.h>
+#include <target/algorithm.h>
+#include <target/armv7m.h>
+#include <target/cortex_m.h>
+
+/* keep family IDs in decimal */
+#define EFM_FAMILY_ID_GECKO             71
+#define EFM_FAMILY_ID_GIANT_GECKO       72
+#define EFM_FAMILY_ID_TINY_GECKO        73
+#define EFM_FAMILY_ID_LEOPARD_GECKO     74
+
+#define EFM32_FLASH_ERASE_TMO           100
+#define EFM32_FLASH_WDATAREADY_TMO      100
+#define EFM32_FLASH_WRITE_TMO           100
+
+/* size in bytes, not words; must fit all Gecko devices */
+#define LOCKBITS_PAGE_SZ                512
+
+#define EFM32_MSC_INFO_BASE             0x0fe00000
+
+#define EFM32_MSC_USER_DATA             EFM32_MSC_INFO_BASE
+#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 */
+#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)
+#define EFM32_MSC_DI_PART_NUM           (EFM32_MSC_DEV_INFO+0x1fc)
+#define EFM32_MSC_DI_PART_FAMILY        (EFM32_MSC_DEV_INFO+0x1fe)
+#define EFM32_MSC_DI_PROD_REV           (EFM32_MSC_DEV_INFO+0x1ff)
+
+#define EFM32_MSC_REGBASE               0x400c0000
+#define EFM32_MSC_WRITECTRL             (EFM32_MSC_REGBASE+0x008)
+#define EFM32_MSC_WRITECTRL_WREN_MASK   0x1
+#define EFM32_MSC_WRITECMD              (EFM32_MSC_REGBASE+0x00c)
+#define EFM32_MSC_WRITECMD_LADDRIM_MASK 0x1
+#define EFM32_MSC_WRITECMD_ERASEPAGE_MASK 0x2
+#define EFM32_MSC_WRITECMD_WRITEONCE_MASK 0x8
+#define EFM32_MSC_ADDRB                 (EFM32_MSC_REGBASE+0x010)
+#define EFM32_MSC_WDATA                 (EFM32_MSC_REGBASE+0x018)
+#define EFM32_MSC_STATUS                (EFM32_MSC_REGBASE+0x01c)
+#define EFM32_MSC_STATUS_BUSY_MASK      0x1
+#define EFM32_MSC_STATUS_LOCKED_MASK    0x2
+#define EFM32_MSC_STATUS_INVADDR_MASK   0x4
+#define EFM32_MSC_STATUS_WDATAREADY_MASK 0x8
+#define EFM32_MSC_STATUS_WORDTIMEOUT_MASK 0x10
+#define EFM32_MSC_STATUS_ERASEABORTED_MASK 0x20
+#define EFM32_MSC_LOCK                  (EFM32_MSC_REGBASE+0x03c)
+#define EFM32_MSC_LOCK_LOCKKEY          0x1b71
+
+struct efm32x_flash_bank {
+	int probed;
+	uint8_t lb_page[LOCKBITS_PAGE_SZ];
+};
+
+struct efm32_info {
+	uint16_t flash_sz_kib;
+	uint16_t ram_sz_kib;
+	uint16_t part_num;
+	uint8_t part_family;
+	uint8_t prod_rev;
+	uint16_t page_size;
+};
+
+static int efm32x_write(struct flash_bank *bank, uint8_t *buffer,
+	uint32_t offset, uint32_t count);
+
+static int efm32x_get_flash_size(struct flash_bank *bank, uint16_t *flash_sz)
+{
+	return target_read_u16(bank->target, EFM32_MSC_DI_FLASH_SZ, flash_sz);
+}
+
+static int efm32x_get_ram_size(struct flash_bank *bank, uint16_t *ram_sz)
+{
+	return target_read_u16(bank->target, EFM32_MSC_DI_RAM_SZ, ram_sz);
+}
+
+static int efm32x_get_part_num(struct flash_bank *bank, uint16_t *pnum)
+{
+	return target_read_u16(bank->target, EFM32_MSC_DI_PART_NUM, pnum);
+}
+
+static int efm32x_get_part_family(struct flash_bank *bank, uint8_t *pfamily)
+{
+	return target_read_u8(bank->target, EFM32_MSC_DI_PART_FAMILY, pfamily);
+}
+
+static int efm32x_get_prod_rev(struct flash_bank *bank, uint8_t *prev)
+{
+	return target_read_u8(bank->target, EFM32_MSC_DI_PROD_REV, prev);
+}
+
+static int efm32x_read_info(struct flash_bank *bank,
+	struct efm32_info *efm32_info)
+{
+	int ret;
+	uint32_t cpuid = 0;
+
+	memset(efm32_info, 0, sizeof(struct efm32_info));
+
+	ret = target_read_u32(bank->target, CPUID, &cpuid);
+	if (ERROR_OK != ret)
+		return ret;
+
+	if (((cpuid >> 4) & 0xfff) == 0xc23) {
+		/* Cortex M3 device */
+	} else {
+		LOG_ERROR("Target is not CortexM3");
+		return ERROR_FAIL;
+	}
+
+	ret = efm32x_get_flash_size(bank, &(efm32_info->flash_sz_kib));
+	if (ERROR_OK != ret)
+		return ret;
+
+	ret = efm32x_get_ram_size(bank, &(efm32_info->ram_sz_kib));
+	if (ERROR_OK != ret)
+		return ret;
+
+	ret = efm32x_get_part_num(bank, &(efm32_info->part_num));
+	if (ERROR_OK != ret)
+		return ret;
+
+	ret = efm32x_get_part_family(bank, &(efm32_info->part_family));
+	if (ERROR_OK != ret)
+		return ret;
+
+	ret = efm32x_get_prod_rev(bank, &(efm32_info->prod_rev));
+	if (ERROR_OK != ret)
+		return ret;
+
+	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_GIANT_GECKO == efm32_info->part_family ||
+			EFM_FAMILY_ID_LEOPARD_GECKO == efm32_info->part_family) {
+		if (efm32_info->prod_rev >= 18) {
+			uint8_t pg_size = 0;
+			ret = target_read_u8(bank->target, EFM32_MSC_DI_PAGE_SIZE,
+				&pg_size);
+			if (ERROR_OK != ret)
+				return ret;
+
+			efm32_info->page_size = (1 << ((pg_size+10) & 0xff));
+		} else {
+			/* EFM32 GG/LG errata: MEM_INFO_PAGE_SIZE is invalid
+			   for MCUs with PROD_REV < 18 */
+			if (efm32_info->flash_sz_kib < 512)
+				efm32_info->page_size = 2048;
+			else
+				efm32_info->page_size = 4096;
+		}
+
+		if ((2048 != efm32_info->page_size) &&
+				(4096 != efm32_info->page_size)) {
+			LOG_ERROR("Invalid page size %u", efm32_info->page_size);
+			return ERROR_FAIL;
+		}
+	} else {
+		LOG_ERROR("Unknown MCU family %d", efm32_info->part_family);
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
+/* flash bank efm32 <base> <size> 0 0 <target#>
+ */
+FLASH_BANK_COMMAND_HANDLER(efm32x_flash_bank_command)
+{
+	struct efm32x_flash_bank *efm32x_info;
+
+	if (CMD_ARGC < 6)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	efm32x_info = malloc(sizeof(struct efm32x_flash_bank));
+
+	bank->driver_priv = efm32x_info;
+	efm32x_info->probed = 0;
+	memset(efm32x_info->lb_page, 0xff, LOCKBITS_PAGE_SZ);
+
+	return ERROR_OK;
+}
+
+/* set or reset given bits in a register */
+static int efm32x_set_reg_bits(struct flash_bank *bank, uint32_t reg,
+	uint32_t bitmask, int set)
+{
+	int ret = 0;
+	uint32_t reg_val = 0;
+
+	ret = target_read_u32(bank->target, reg, &reg_val);
+	if (ERROR_OK != ret)
+		return ret;
+
+	if (set)
+		reg_val |= bitmask;
+	else
+		reg_val &= ~bitmask;
+
+	return target_write_u32(bank->target, reg, reg_val);
+}
+
+static int efm32x_set_wren(struct flash_bank *bank, int write_enable)
+{
+	return efm32x_set_reg_bits(bank, EFM32_MSC_WRITECTRL,
+		EFM32_MSC_WRITECTRL_WREN_MASK, write_enable);
+}
+
+static int efm32x_msc_lock(struct flash_bank *bank, int lock)
+{
+	return target_write_u32(bank->target, EFM32_MSC_LOCK,
+		(lock ? 0 : EFM32_MSC_LOCK_LOCKKEY));
+}
+
+static int efm32x_wait_status(struct flash_bank *bank, int timeout,
+	uint32_t wait_mask, int wait_for_set)
+{
+	int ret = 0;
+	uint32_t status = 0;
+
+	while (1) {
+		ret = target_read_u32(bank->target, EFM32_MSC_STATUS, &status);
+		if (ERROR_OK != ret)
+			break;
+
+		LOG_DEBUG("status: 0x%" PRIx32 "", status);
+
+		if (((status & wait_mask) == 0) && (0 == wait_for_set))
+			break;
+		else if (((status & wait_mask) != 0) && wait_for_set)
+			break;
+
+		if (timeout-- <= 0) {
+			LOG_ERROR("timed out waiting for MSC status");
+			return ERROR_FAIL;
+		}
+
+		alive_sleep(1);
+	}
+
+	if (status & EFM32_MSC_STATUS_ERASEABORTED_MASK)
+		LOG_WARNING("page erase was aborted");
+
+	return ret;
+}
+
+static int efm32x_erase_page(struct flash_bank *bank, uint32_t addr)
+{
+	/* this function DOES NOT set WREN; must be set already */
+	/* 1. write address to ADDRB
+	   2. write LADDRIM
+	   3. check status (INVADDR, LOCKED)
+	   4. write ERASEPAGE
+	   5. wait until !STATUS_BUSY
+	 */
+	int ret = 0;
+	uint32_t status = 0;
+
+	LOG_DEBUG("erasing flash page at 0x%08x", addr);
+
+	ret = target_write_u32(bank->target, EFM32_MSC_ADDRB, addr);
+	if (ERROR_OK != ret)
+		return ret;
+
+	ret = efm32x_set_reg_bits(bank, EFM32_MSC_WRITECMD,
+		EFM32_MSC_WRITECMD_LADDRIM_MASK, 1);
+	if (ERROR_OK != ret)
+		return ret;
+
+	ret = target_read_u32(bank->target, EFM32_MSC_STATUS, &status);
+	if (ERROR_OK != ret)
+		return ret;
+
+	LOG_DEBUG("status 0x%x", status);
+
+	if (status & EFM32_MSC_STATUS_LOCKED_MASK) {
+		LOG_ERROR("Page is locked");
+		return ERROR_FAIL;
+	} else if (status & EFM32_MSC_STATUS_INVADDR_MASK) {
+		LOG_ERROR("Invalid address 0x%x", addr);
+		return ERROR_FAIL;
+	}
+
+	ret = efm32x_set_reg_bits(bank, EFM32_MSC_WRITECMD,
+		EFM32_MSC_WRITECMD_ERASEPAGE_MASK, 1);
+	if (ERROR_OK != ret)
+		return ret;
+
+	return efm32x_wait_status(bank, EFM32_FLASH_ERASE_TMO,
+		EFM32_MSC_STATUS_BUSY_MASK, 0);
+}
+
+static int efm32x_erase(struct flash_bank *bank, int first, int last)
+{
+	struct target *target = bank->target;
+	int i = 0;
+	int ret = 0;
+
+	if (TARGET_HALTED != target->state) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	efm32x_msc_lock(bank, 0);
+	ret = efm32x_set_wren(bank, 1);
+	if (ERROR_OK != ret) {
+		LOG_ERROR("Failed to enable MSC write");
+		return ret;
+	}
+
+	for (i = first; i <= last; i++) {
+		ret = efm32x_erase_page(bank, bank->sectors[i].offset);
+		if (ERROR_OK != ret)
+			LOG_ERROR("Failed to erase page %d", i);
+	}
+
+	ret = efm32x_set_wren(bank, 0);
+	efm32x_msc_lock(bank, 1);
+
+	return ret;
+}
+
+static int efm32x_read_lock_data(struct flash_bank *bank)
+{
+	struct efm32x_flash_bank *efm32x_info = bank->driver_priv;
+	struct target *target = bank->target;
+	int i = 0;
+	int data_size = 0;
+	uint32_t *ptr = NULL;
+	int ret = 0;
+
+	assert(!(bank->num_sectors & 0x1f));
+
+	data_size = bank->num_sectors / 8; /* number of data bytes */
+	data_size /= 4; /* ...and data dwords */
+
+	ptr = (uint32_t *)efm32x_info->lb_page;
+
+	for (i = 0; i < data_size; i++, ptr++) {
+		ret = target_read_u32(target, EFM32_MSC_LOCK_BITS+i*4, ptr);
+		if (ERROR_OK != ret) {
+			LOG_ERROR("Failed to read PLW %d", i);
+			return ret;
+		}
+	}
+
+	/* also, read ULW, DLW and MLW */
+
+	/* ULW, word 126 */
+	ptr = ((uint32_t *)efm32x_info->lb_page) + 126;
+	ret = target_read_u32(target, EFM32_MSC_LOCK_BITS+126*4, ptr);
+	if (ERROR_OK != ret) {
+		LOG_ERROR("Failed to read ULW");
+		return ret;
+	}
+
+	/* DLW, word 127 */
+	ptr = ((uint32_t *)efm32x_info->lb_page) + 127;
+	ret = target_read_u32(target, EFM32_MSC_LOCK_BITS+127*4, ptr);
+	if (ERROR_OK != ret) {
+		LOG_ERROR("Failed to read DLW");
+		return ret;
+	}
+
+	/* MLW, word 125, present in GG and LG */
+	ptr = ((uint32_t *)efm32x_info->lb_page) + 125;
+	ret = target_read_u32(target, EFM32_MSC_LOCK_BITS+125*4, ptr);
+	if (ERROR_OK != ret) {
+		LOG_ERROR("Failed to read MLW");
+		return ret;
+	}
+
+	return ERROR_OK;
+}
+
+static int efm32x_write_lock_data(struct flash_bank *bank)
+{
+	struct efm32x_flash_bank *efm32x_info = bank->driver_priv;
+	int ret = 0;
+
+	ret = efm32x_erase_page(bank, EFM32_MSC_LOCK_BITS);
+	if (ERROR_OK != ret) {
+		LOG_ERROR("Failed to erase LB page");
+		return ret;
+	}
+
+	return efm32x_write(bank, efm32x_info->lb_page, EFM32_MSC_LOCK_BITS,
+		LOCKBITS_PAGE_SZ);
+}
+
+static int efm32x_get_page_lock(struct flash_bank *bank, size_t page)
+{
+	struct efm32x_flash_bank *efm32x_info = bank->driver_priv;
+	uint32_t dw = ((uint32_t *)efm32x_info->lb_page)[page >> 5];
+	uint32_t mask = 0;
+
+	mask = 1 << (page & 0x1f);
+
+	return (dw & mask) ? 0 : 1;
+}
+
+static int efm32x_set_page_lock(struct flash_bank *bank, size_t page, int set)
+{
+	struct efm32x_flash_bank *efm32x_info = bank->driver_priv;
+	uint32_t *dw = &((uint32_t *)efm32x_info->lb_page)[page >> 5];
+	uint32_t mask = 0;
+
+	mask = 1 << (page & 0x1f);
+
+	if (!set)
+		*dw |= mask;
+	else
+		*dw &= ~mask;
+
+	return ERROR_OK;
+}
+
+static int efm32x_protect(struct flash_bank *bank, int set, int first, int last)
+{
+	struct target *target = bank->target;
+	int i = 0;
+	int ret = 0;
+
+	if (!set) {
+		LOG_ERROR("Erase device data to reset page locks");
+		return ERROR_FAIL;
+	}
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	for (i = first; i <= last; i++) {
+		ret = efm32x_set_page_lock(bank, i, set);
+		if (ERROR_OK != ret) {
+			LOG_ERROR("Failed to set lock on page %d", i);
+			return ret;
+		}
+	}
+
+	ret = efm32x_write_lock_data(bank);
+	if (ERROR_OK != ret) {
+		LOG_ERROR("Failed to write LB page");
+		return ret;
+	}
+
+	return ERROR_OK;
+}
+
+static int efm32x_write_block(struct flash_bank *bank, uint8_t *buf,
+	uint32_t offset, uint32_t count)
+{
+	struct target *target = bank->target;
+	uint32_t buffer_size = 16384;
+	struct working_area *write_algorithm;
+	struct working_area *source;
+	uint32_t address = bank->base + offset;
+	struct reg_param reg_params[5];
+	struct armv7m_algorithm armv7m_info;
+	int ret = ERROR_OK;
+
+	/* see contrib/loaders/flash/efm32.S for src */
+	static const uint8_t efm32x_flash_write_code[] = {
+		/* #define EFM32_MSC_WRITECTRL_OFFSET      0x008 */
+		/* #define EFM32_MSC_WRITECMD_OFFSET       0x00c */
+		/* #define EFM32_MSC_ADDRB_OFFSET          0x010 */
+		/* #define EFM32_MSC_WDATA_OFFSET          0x018 */
+		/* #define EFM32_MSC_STATUS_OFFSET         0x01c */
+		/* #define EFM32_MSC_LOCK_OFFSET           0x03c */
+
+			0x15, 0x4e,    /* ldr     r6, =#0x1b71 */
+			0xc6, 0x63,    /* str     r6, [r0, #EFM32_MSC_LOCK_OFFSET] */
+			0x01, 0x26,    /* movs    r6, #1 */
+			0x86, 0x60,    /* str     r6, [r0, #EFM32_MSC_WRITECTRL_OFFSET] */
+
+		/* wait_fifo: */
+			0x16, 0x68,    /* ldr     r6, [r2, #0] */
+			0x00, 0x2e,    /* cmp     r6, #0 */
+			0x22, 0xd0,    /* beq     exit */
+			0x55, 0x68,    /* ldr     r5, [r2, #4] */
+			0xb5, 0x42,    /* cmp     r5, r6 */
+			0xf9, 0xd0,    /* beq     wait_fifo */
+
+			0x04, 0x61,    /* str     r4, [r0, #EFM32_MSC_ADDRB_OFFSET] */
+			0x01, 0x26,    /* movs    r6, #1 */
+			0xc6, 0x60,    /* str     r6, [r0, #EFM32_MSC_WRITECMD_OFFSET] */
+			0xc6, 0x69,    /* ldr     r6, [r0, #EFM32_MSC_STATUS_OFFSET] */
+			0x06, 0x27,    /* movs    r7, #6 */
+			0x3e, 0x42,    /* tst     r6, r7 */
+			0x16, 0xd1,    /* bne     error */
+
+		/* wait_wdataready: */
+			0xc6, 0x69,    /* ldr     r6, [r0, #EFM32_MSC_STATUS_OFFSET] */
+			0x08, 0x27,    /* movs    r7, #8 */
+			0x3e, 0x42,    /* tst     r6, r7 */
+			0xfb, 0xd0,    /* beq     wait_wdataready */
+
+			0x2e, 0x68,    /* ldr     r6, [r5] */
+			0x86, 0x61,    /* str     r6, [r0, #EFM32_MSC_WDATA_OFFSET] */
+			0x08, 0x26,    /* movs    r6, #8 */
+			0xc6, 0x60,    /* str     r6, [r0, #EFM32_MSC_WRITECMD_OFFSET] */
+
+			0x04, 0x35,    /* adds    r5, #4 */
+			0x04, 0x34,    /* adds    r4, #4 */
+
+		/* busy: */
+			0xc6, 0x69,    /* ldr     r6, [r0, #EFM32_MSC_STATUS_OFFSET] */
+			0x01, 0x27,    /* movs    r7, #1 */
+			0x3e, 0x42,    /* tst     r6, r7 */
+			0xfb, 0xd1,    /* bne     busy */
+
+			0x9d, 0x42,    /* cmp     r5, r3 */
+			0x01, 0xd3,    /* bcc     no_wrap */
+			0x15, 0x46,    /* mov     r5, r2 */
+			0x08, 0x35,    /* adds    r5, #8 */
+
+		/* no_wrap: */
+			0x55, 0x60,    /* str     r5, [r2, #4] */
+			0x01, 0x39,    /* subs    r1, r1, #1 */
+			0x00, 0x29,    /* cmp     r1, #0 */
+			0x02, 0xd0,    /* beq     exit */
+			0xdb, 0xe7,    /* b       wait_fifo */
+
+		/* error: */
+			0x00, 0x20,    /* movs    r0, #0 */
+			0x50, 0x60,    /* str     r0, [r2, #4] */
+
+		/* exit: */
+			0x30, 0x46,    /* mov     r0, r6 */
+			0x00, 0xbe,    /* bkpt    #0 */
+
+		/* LOCKKEY */
+			0x71, 0x1b, 0x00, 0x00
+	};
+
+	/* flash write code */
+	if (target_alloc_working_area(target, sizeof(efm32x_flash_write_code),
+			&write_algorithm) != ERROR_OK) {
+		LOG_WARNING("no working area available, can't do block memory writes");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	};
+
+	ret = target_write_buffer(target, write_algorithm->address,
+		sizeof(efm32x_flash_write_code),
+		(uint8_t *)efm32x_flash_write_code);
+	if (ret != ERROR_OK)
+		return ret;
+
+	/* memory buffer */
+	while (target_alloc_working_area_try(target, buffer_size, &source) != ERROR_OK) {
+		buffer_size /= 2;
+		buffer_size &= ~3UL; /* Make sure it's 4 byte aligned */
+		if (buffer_size <= 256) {
+			/* we already allocated the writing code, but failed to get a
+			 * buffer, free the algorithm */
+			target_free_working_area(target, write_algorithm);
+
+			LOG_WARNING("no large enough working area available, can't do block memory writes");
+			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+		}
+	};
+
+	init_reg_param(&reg_params[0], "r0", 32, PARAM_IN_OUT);	/* flash base (in), status (out) */
+	init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);	/* count (word-32bit) */
+	init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);	/* buffer start */
+	init_reg_param(&reg_params[3], "r3", 32, PARAM_OUT);	/* buffer end */
+	init_reg_param(&reg_params[4], "r4", 32, PARAM_IN_OUT);	/* target address */
+
+	buf_set_u32(reg_params[0].value, 0, 32, EFM32_MSC_REGBASE);
+	buf_set_u32(reg_params[1].value, 0, 32, count);
+	buf_set_u32(reg_params[2].value, 0, 32, source->address);
+	buf_set_u32(reg_params[3].value, 0, 32, source->address + source->size);
+	buf_set_u32(reg_params[4].value, 0, 32, address);
+
+	armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
+	armv7m_info.core_mode = ARM_MODE_THREAD;
+
+	ret = target_run_flash_async_algorithm(target, buf, count, 4,
+			0, NULL,
+			5, reg_params,
+			source->address, source->size,
+			write_algorithm->address, 0,
+			&armv7m_info);
+
+	if (ret == ERROR_FLASH_OPERATION_FAILED) {
+		LOG_ERROR("flash write failed at address 0x%"PRIx32,
+				buf_get_u32(reg_params[4].value, 0, 32));
+
+		if (buf_get_u32(reg_params[0].value, 0, 32) &
+				EFM32_MSC_STATUS_LOCKED_MASK) {
+			LOG_ERROR("flash memory write protected");
+		}
+
+		if (buf_get_u32(reg_params[0].value, 0, 32) &
+				EFM32_MSC_STATUS_INVADDR_MASK) {
+			LOG_ERROR("invalid flash memory write address");
+		}
+	}
+
+	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]);
+	destroy_reg_param(&reg_params[4]);
+
+	return ret;
+}
+
+static int efm32x_write_word(struct flash_bank *bank, uint32_t addr,
+	uint32_t val)
+{
+	/* this function DOES NOT set WREN; must be set already */
+	/* 1. write address to ADDRB
+	   2. write LADDRIM
+	   3. check status (INVADDR, LOCKED)
+	   4. wait for WDATAREADY
+	   5. write data to WDATA
+	   6. write WRITECMD_WRITEONCE to WRITECMD
+	   7. wait until !STATUS_BUSY
+	 */
+
+	/* FIXME: EFM32G ref states (7.3.2) that writes should be
+	 * performed twice per dword */
+
+	int ret = 0;
+	uint32_t status = 0;
+
+	/* if not called, GDB errors will be reported during large writes */
+	keep_alive();
+
+	ret = target_write_u32(bank->target, EFM32_MSC_ADDRB, addr);
+	if (ERROR_OK != ret)
+		return ret;
+
+	ret = efm32x_set_reg_bits(bank, EFM32_MSC_WRITECMD,
+		EFM32_MSC_WRITECMD_LADDRIM_MASK, 1);
+	if (ERROR_OK != ret)
+		return ret;
+
+	ret = target_read_u32(bank->target, EFM32_MSC_STATUS, &status);
+	if (ERROR_OK != ret)
+		return ret;
+
+	LOG_DEBUG("status 0x%x", status);
+
+	if (status & EFM32_MSC_STATUS_LOCKED_MASK) {
+		LOG_ERROR("Page is locked");
+		return ERROR_FAIL;
+	} else if (status & EFM32_MSC_STATUS_INVADDR_MASK) {
+		LOG_ERROR("Invalid address 0x%x", addr);
+		return ERROR_FAIL;
+	}
+
+	ret = efm32x_wait_status(bank, EFM32_FLASH_WDATAREADY_TMO,
+		EFM32_MSC_STATUS_WDATAREADY_MASK, 1);
+	if (ERROR_OK != ret) {
+		LOG_ERROR("Wait for WDATAREADY failed");
+		return ret;
+	}
+
+	ret = target_write_u32(bank->target, EFM32_MSC_WDATA, val);
+	if (ERROR_OK != ret) {
+		LOG_ERROR("WDATA write failed");
+		return ret;
+	}
+
+	ret = target_write_u32(bank->target, EFM32_MSC_WRITECMD,
+		EFM32_MSC_WRITECMD_WRITEONCE_MASK);
+	if (ERROR_OK != ret) {
+		LOG_ERROR("WRITECMD write failed");
+		return ret;
+	}
+
+	ret = efm32x_wait_status(bank, EFM32_FLASH_WRITE_TMO,
+		EFM32_MSC_STATUS_BUSY_MASK, 0);
+	if (ERROR_OK != ret) {
+		LOG_ERROR("Wait for BUSY failed");
+		return ret;
+	}
+
+	return ERROR_OK;
+}
+
+static int efm32x_write(struct flash_bank *bank, uint8_t *buffer,
+		uint32_t offset, uint32_t count)
+{
+	struct target *target = bank->target;
+	uint8_t *new_buffer = NULL;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (offset & 0x3) {
+		LOG_ERROR("offset 0x%" PRIx32 " breaks required 4-byte "
+			"alignment", offset);
+		return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
+	}
+
+	if (count & 0x3) {
+		uint32_t old_count = count;
+		count = (old_count | 3) + 1;
+		new_buffer = malloc(count);
+		if (new_buffer == NULL) {
+			LOG_ERROR("odd number of bytes to write and no memory "
+				"for padding buffer");
+			return ERROR_FAIL;
+		}
+		LOG_INFO("odd number of bytes to write (%d), extending to %d "
+			"and padding with 0xff", old_count, count);
+		memset(buffer, 0xff, count);
+		buffer = memcpy(new_buffer, buffer, old_count);
+	}
+
+	uint32_t words_remaining = count / 4;
+	int retval, retval2;
+
+	/* unlock flash registers */
+	efm32x_msc_lock(bank, 0);
+	retval = efm32x_set_wren(bank, 1);
+	if (retval != ERROR_OK)
+		goto cleanup;
+
+	/* try using a block write */
+	retval = efm32x_write_block(bank, buffer, offset, words_remaining);
+
+	if (retval == ERROR_TARGET_RESOURCE_NOT_AVAILABLE) {
+		/* if block write failed (no sufficient working area),
+		 * we use normal (slow) single word accesses */
+		LOG_WARNING("couldn't use block writes, falling back to single "
+			"memory accesses");
+
+		while (words_remaining > 0) {
+			uint32_t value;
+			memcpy(&value, buffer, sizeof(uint32_t));
+
+			retval = efm32x_write_word(bank, offset, value);
+			if (retval != ERROR_OK)
+				goto reset_pg_and_lock;
+
+			words_remaining--;
+			buffer += 4;
+			offset += 4;
+		}
+	}
+
+reset_pg_and_lock:
+	retval2 = efm32x_set_wren(bank, 0);
+	efm32x_msc_lock(bank, 1);
+	if (retval == ERROR_OK)
+		retval = retval2;
+
+cleanup:
+	if (new_buffer)
+		free(new_buffer);
+
+	return retval;
+}
+
+static int efm32x_probe(struct flash_bank *bank)
+{
+	struct efm32x_flash_bank *efm32x_info = bank->driver_priv;
+	struct efm32_info efm32_mcu_info;
+	int ret;
+	int i;
+	uint32_t base_address = 0x00000000;
+
+	efm32x_info->probed = 0;
+	memset(efm32x_info->lb_page, 0xff, LOCKBITS_PAGE_SZ);
+
+	ret = efm32x_read_info(bank, &efm32_mcu_info);
+	if (ERROR_OK != ret)
+		return ret;
+
+	switch (efm32_mcu_info.part_family) {
+		case EFM_FAMILY_ID_GECKO:
+			LOG_INFO("Gecko MCU detected");
+			break;
+		case EFM_FAMILY_ID_GIANT_GECKO:
+			LOG_INFO("Giant Gecko MCU detected");
+			break;
+		case EFM_FAMILY_ID_TINY_GECKO:
+			LOG_INFO("Tiny Gecko MCU detected");
+			break;
+		case EFM_FAMILY_ID_LEOPARD_GECKO:
+			LOG_INFO("Leopard Gecko MCU detected");
+			break;
+		default:
+			LOG_ERROR("Unsupported MCU family %d",
+				efm32_mcu_info.part_family);
+			return ERROR_FAIL;
+	}
+
+	LOG_INFO("flash size = %dkbytes", efm32_mcu_info.flash_sz_kib);
+	LOG_INFO("flash page size = %dbytes", efm32_mcu_info.page_size);
+
+	assert(0 != efm32_mcu_info.page_size);
+
+	int num_pages = efm32_mcu_info.flash_sz_kib * 1024 /
+		efm32_mcu_info.page_size;
+
+	assert(num_pages > 0);
+
+	if (bank->sectors) {
+		free(bank->sectors);
+		bank->sectors = NULL;
+	}
+
+	bank->base = base_address;
+	bank->size = (num_pages * efm32_mcu_info.page_size);
+	bank->num_sectors = num_pages;
+
+	ret = efm32x_read_lock_data(bank);
+	if (ERROR_OK != ret) {
+		LOG_ERROR("Failed to read LB data");
+		return ret;
+	}
+
+	bank->sectors = malloc(sizeof(struct flash_sector) * num_pages);
+
+	for (i = 0; i < num_pages; i++) {
+		bank->sectors[i].offset = i * efm32_mcu_info.page_size;
+		bank->sectors[i].size = efm32_mcu_info.page_size;
+		bank->sectors[i].is_erased = -1;
+		bank->sectors[i].is_protected = 1;
+	}
+
+	efm32x_info->probed = 1;
+
+	return ERROR_OK;
+}
+
+static int efm32x_auto_probe(struct flash_bank *bank)
+{
+	struct efm32x_flash_bank *efm32x_info = bank->driver_priv;
+	if (efm32x_info->probed)
+		return ERROR_OK;
+	return efm32x_probe(bank);
+}
+
+static int efm32x_protect_check(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	int ret = 0;
+	int i = 0;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	ret = efm32x_read_lock_data(bank);
+	if (ERROR_OK != ret) {
+		LOG_ERROR("Failed to read LB data");
+		return ret;
+	}
+
+	assert(NULL != bank->sectors);
+
+	for (i = 0; i < bank->num_sectors; i++)
+		bank->sectors[i].is_protected = efm32x_get_page_lock(bank, i);
+
+	return ERROR_OK;
+}
+
+static int get_efm32x_info(struct flash_bank *bank, char *buf, int buf_size)
+{
+	struct efm32_info info;
+	int ret = 0;
+	int printed = 0;
+
+	ret = efm32x_read_info(bank, &info);
+	if (ERROR_OK != ret) {
+		LOG_ERROR("Failed to read EFM32 info");
+		return ret;
+	}
+
+	printed = snprintf(buf, buf_size, "EFM32 ");
+	buf += printed;
+	buf_size -= printed;
+
+	if (0 >= buf_size)
+		return ERROR_BUF_TOO_SMALL;
+
+	switch (info.part_family) {
+		case EFM_FAMILY_ID_GECKO:
+			printed = snprintf(buf, buf_size, "Gecko");
+			break;
+		case EFM_FAMILY_ID_GIANT_GECKO:
+			printed = snprintf(buf, buf_size, "Giant Gecko");
+			break;
+		case EFM_FAMILY_ID_TINY_GECKO:
+			printed = snprintf(buf, buf_size, "Tiny Gecko");
+			break;
+		case EFM_FAMILY_ID_LEOPARD_GECKO:
+			printed = snprintf(buf, buf_size, "Leopard Gecko");
+			break;
+	}
+
+	buf += printed;
+	buf_size -= printed;
+
+	if (0 >= buf_size)
+		return ERROR_BUF_TOO_SMALL;
+
+	printed = snprintf(buf, buf_size, " - Rev: %d", info.prod_rev);
+	buf += printed;
+	buf_size -= printed;
+
+	if (0 >= buf_size)
+		return ERROR_BUF_TOO_SMALL;
+
+	return ERROR_OK;
+}
+
+static const struct command_registration efm32x_exec_command_handlers[] = {
+	COMMAND_REGISTRATION_DONE
+};
+
+static const struct command_registration efm32x_command_handlers[] = {
+	{
+		.name = "efm32",
+		.mode = COMMAND_ANY,
+		.help = "efm32 flash command group",
+		.usage = "",
+		.chain = efm32x_exec_command_handlers,
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+struct flash_driver efm32_flash = {
+	.name = "efm32",
+	.commands = efm32x_command_handlers,
+	.flash_bank_command = efm32x_flash_bank_command,
+	.erase = efm32x_erase,
+	.protect = efm32x_protect,
+	.write = efm32x_write,
+	.read = default_flash_read,
+	.probe = efm32x_probe,
+	.auto_probe = efm32x_auto_probe,
+	.erase_check = default_flash_blank_check,
+	.protect_check = efm32x_protect_check,
+	.info = get_efm32x_info,
+};
--- a/src/flash/nor/em357.c
+++ b/src/flash/nor/em357.c
@@ -88,7 +88,6 @@ struct em357_options {

 struct em357_flash_bank {
 	struct em357_options option_bytes;
-	struct working_area *write_algorithm;
 	int ppage_size;
 	int probed;
 };
@@ -107,7 +106,6 @@ FLASH_BANK_COMMAND_HANDLER(em357_flash_bank_command)
 	em357_info = malloc(sizeof(struct em357_flash_bank));
 	bank->driver_priv = em357_info;

-	em357_info->write_algorithm = NULL;
 	em357_info->probed = 0;

 	return ERROR_OK;
@@ -360,6 +358,9 @@ static int em357_erase(struct flash_bank *bank, int first, int last)
 	if ((first == 0) && (last == (bank->num_sectors - 1)))
 		return em357_mass_erase(bank);

+	/* Enable FPEC clock */
+	target_write_u32(target, EM357_FPEC_CLK, 0x00000001);
+
 	/* unlock flash registers */
 	int retval = target_write_u32(target, EM357_FLASH_KEYR, KEY1);
 	if (retval != ERROR_OK)
@@ -457,9 +458,9 @@ static int em357_protect(struct flash_bank *bank, int set, int first, int last)
 static int em357_write_block(struct flash_bank *bank, uint8_t *buffer,
 	uint32_t offset, uint32_t count)
 {
-	struct em357_flash_bank *em357_info = bank->driver_priv;
 	struct target *target = bank->target;
 	uint32_t buffer_size = 16384;
+	struct working_area *write_algorithm;
 	struct working_area *source;
 	uint32_t address = bank->base + offset;
 	struct reg_param reg_params[4];
@@ -497,13 +498,13 @@ static int em357_write_block(struct flash_bank *bank, uint8_t *buffer,

 	/* flash write code */
 	if (target_alloc_working_area(target, sizeof(em357_flash_write_code),
-			&em357_info->write_algorithm) != ERROR_OK) {
+			&write_algorithm) != ERROR_OK) {
 		LOG_WARNING("no working area available, can't do block memory writes");
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 	}
 	;

-	retval = target_write_buffer(target, em357_info->write_algorithm->address,
+	retval = target_write_buffer(target, write_algorithm->address,
 			sizeof(em357_flash_write_code), (uint8_t *)em357_flash_write_code);
 	if (retval != ERROR_OK)
 		return retval;
@@ -512,20 +513,18 @@ static int em357_write_block(struct flash_bank *bank, uint8_t *buffer,
 	while (target_alloc_working_area_try(target, buffer_size, &source) != ERROR_OK) {
 		buffer_size /= 2;
 		if (buffer_size <= 256) {
-			/* if we already allocated the writing code, but failed to get a
+			/* we already allocated the writing code, but failed to get a
 			 * buffer, free the algorithm */
-			if (em357_info->write_algorithm)
-				target_free_working_area(target, em357_info->write_algorithm);
+			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 = ARMV7M_MODE_ANY;
+	armv7m_info.core_mode = ARM_MODE_THREAD;

 	init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
 	init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
@@ -546,7 +545,7 @@ static int em357_write_block(struct flash_bank *bank, uint8_t *buffer,
 		buf_set_u32(reg_params[3].value, 0, 32, 0);

 		retval = target_run_algorithm(target, 0, NULL, 4, reg_params,
-				em357_info->write_algorithm->address, 0, 10000, &armv7m_info);
+				write_algorithm->address, 0, 10000, &armv7m_info);
 		if (retval != ERROR_OK) {
 			LOG_ERROR("error executing em357 flash write algorithm");
 			break;
@@ -574,7 +573,7 @@ static int em357_write_block(struct flash_bank *bank, uint8_t *buffer,
 	}

 	target_free_working_area(target, source);
-	target_free_working_area(target, em357_info->write_algorithm);
+	target_free_working_area(target, write_algorithm);

 	destroy_reg_param(&reg_params[0]);
 	destroy_reg_param(&reg_params[1]);
@@ -612,6 +611,8 @@ static int em357_write(struct flash_bank *bank, uint8_t *buffer,
 	if (retval != ERROR_OK)
 		return retval;

+	target_write_u32(target, EM357_FPEC_CLK, 0x00000001);
+
 	/* multiple half words (2-byte) to be programmed? */
 	if (words_remaining > 0) {
 		/* try using a block write */
@@ -683,14 +684,40 @@ static int em357_probe(struct flash_bank *bank)

 	em357_info->probed = 0;

+	switch (bank->size) {
+		case 0x10000:
+			/* 64k -- 64 1k pages */
+			num_pages = 64;
+			page_size = 1024;
+			break;
+		case 0x20000:
+			/* 128k -- 128 1k pages */
+			num_pages = 128;
+			page_size = 1024;
+			break;
+		case 0x30000:
+			/* 192k -- 96 2k pages */
+			num_pages = 96;
+			page_size = 2048;
+			break;
+		case 0x40000:
+			/* 256k -- 128 2k pages */
+			num_pages = 128;
+			page_size = 2048;
+			break;
+		default:
+			LOG_WARNING("No size specified for em357 flash driver, assuming 192k!");
+			num_pages = 96;
+			page_size = 2048;
+			break;
+	}
+
 	/* Enable FPEC CLK */
 	int retval = target_write_u32(target, EM357_FPEC_CLK, 0x00000001);
 	if (retval != ERROR_OK)
 		return retval;

-	page_size = 2048;
 	em357_info->ppage_size = 4;
-	num_pages = 96;

 	LOG_INFO("flash size = %dkbytes", num_pages*page_size/1024);

@@ -816,6 +843,9 @@ static int em357_mass_erase(struct flash_bank *bank)
 		return ERROR_TARGET_NOT_HALTED;
 	}

+	/* Make sure the flash clock is on */
+	target_write_u32(target, EM357_FPEC_CLK, 0x00000001);
+
 	/* unlock option flash registers */
 	int retval = target_write_u32(target, EM357_FLASH_KEYR, KEY1);
 	if (retval != ERROR_OK)
--- a/src/flash/nor/fm3.c
+++ b/src/flash/nor/fm3.c
@@ -38,12 +38,17 @@ enum fm3_variant {
 	mb9bfxx4,
 	mb9bfxx5,
 	mb9bfxx6,
+	mb9bfxx7,
+	mb9bfxx8,
+
 	mb9afxx1,	/* Flash Type '2' */
 	mb9afxx2,
 	mb9afxx3,
 	mb9afxx4,
 	mb9afxx5,
-	mb9afxx6
+	mb9afxx6,
+	mb9afxx7,
+	mb9afxx8,
 };

 enum fm3_flash_type {
@@ -53,7 +58,6 @@ enum fm3_flash_type {
 };

 struct fm3_flash_bank {
-	struct working_area *write_algorithm;
 	enum fm3_variant variant;
 	enum fm3_flash_type flashtype;
 	int probed;
@@ -88,6 +92,12 @@ FLASH_BANK_COMMAND_HANDLER(fm3_flash_bank_command)
 	} else if (strcmp(CMD_ARGV[5], "mb9bfxx6.cpu") == 0) {
 		fm3_info->variant = mb9bfxx6;
 		fm3_info->flashtype = fm3_flash_type1;
+	} else if (strcmp(CMD_ARGV[5], "mb9bfxx7.cpu") == 0) {
+		fm3_info->variant = mb9bfxx7;
+		fm3_info->flashtype = fm3_flash_type1;
+	} else if (strcmp(CMD_ARGV[5], "mb9bfxx8.cpu") == 0) {
+		fm3_info->variant = mb9bfxx8;
+		fm3_info->flashtype = fm3_flash_type1;
 	} else if (strcmp(CMD_ARGV[5], "mb9afxx1.cpu") == 0) {	/* Flash type '2' */
 		fm3_info->variant = mb9afxx1;
 		fm3_info->flashtype = fm3_flash_type2;
@@ -106,6 +116,12 @@ FLASH_BANK_COMMAND_HANDLER(fm3_flash_bank_command)
 	} else if (strcmp(CMD_ARGV[5], "mb9afxx6.cpu") == 0) {
 		fm3_info->variant = mb9afxx6;
 		fm3_info->flashtype = fm3_flash_type2;
+	} else if (strcmp(CMD_ARGV[5], "mb9afxx7.cpu") == 0) {
+		fm3_info->variant = mb9afxx7;
+		fm3_info->flashtype = fm3_flash_type2;
+	} else if (strcmp(CMD_ARGV[5], "mb9afxx8.cpu") == 0) {
+		fm3_info->variant = mb9afxx8;
+		fm3_info->flashtype = fm3_flash_type2;
 	}

 	/* unknown Flash type */
@@ -115,7 +131,6 @@ FLASH_BANK_COMMAND_HANDLER(fm3_flash_bank_command)
 		return ERROR_FLASH_BANK_INVALID;
 	}

-	fm3_info->write_algorithm = NULL;
 	fm3_info->probed = 0;

 	return ERROR_OK;
@@ -282,6 +297,7 @@ static int fm3_write_block(struct flash_bank *bank, uint8_t *buffer,
 	struct fm3_flash_bank *fm3_info = bank->driver_priv;
 	struct target *target = bank->target;
 	uint32_t buffer_size = 2048;		/* 8192 for MB9Bxx6! */
+	struct working_area *write_algorithm;
 	struct working_area *source;
 	uint32_t address = bank->base + offset;
 	struct reg_param reg_params[6];
@@ -461,12 +477,12 @@ static int fm3_write_block(struct flash_bank *bank, uint8_t *buffer,

 	/* allocate working area with flash programming code */
 	if (target_alloc_working_area(target, sizeof(fm3_flash_write_code),
-			&fm3_info->write_algorithm) != ERROR_OK) {
+			&write_algorithm) != ERROR_OK) {
 		LOG_WARNING("no working area available, can't do block memory writes");
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 	}

-	retval = target_write_buffer(target, fm3_info->write_algorithm->address,
+	retval = target_write_buffer(target, write_algorithm->address,
 		sizeof(fm3_flash_write_code), fm3_flash_write_code);
 	if (retval != ERROR_OK)
 		return retval;
@@ -477,9 +493,8 @@ static int fm3_write_block(struct flash_bank *bank, uint8_t *buffer,
 	while (target_alloc_working_area(target, buffer_size, &source) != ERROR_OK) {
 		buffer_size /= 2;
 		if (buffer_size <= 256) {
-			/* free working area, if write algorithm already allocated */
-			if (fm3_info->write_algorithm)
-				target_free_working_area(target, fm3_info->write_algorithm);
+			/* 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;
@@ -487,7 +502,7 @@ static int fm3_write_block(struct flash_bank *bank, uint8_t *buffer,
 	}

 	armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
-	armv7m_info.core_mode = ARMV7M_MODE_ANY;
+	armv7m_info.core_mode = ARM_MODE_THREAD;

 	init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT); /* source start address */
 	init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT); /* target start address */
@@ -501,22 +516,22 @@ static int fm3_write_block(struct flash_bank *bank, uint8_t *buffer,
 	while (count > 0) {
 		uint32_t thisrun_count = (count > (buffer_size / 2)) ? (buffer_size / 2) : count;

-		retval = target_write_buffer(target, fm3_info->write_algorithm->address, 8,
+		retval = target_write_buffer(target, write_algorithm->address, 8,
 				fm3_flash_write_code);
 		if (retval != ERROR_OK)
 			break;

 		/* Patching 'local variable address' for different RAM addresses */
-		if (fm3_info->write_algorithm->address != 0x1FFF8008) {
+		if (write_algorithm->address != 0x1FFF8008) {
 			/* Algorithm: u32DummyRead: */
-			retval = target_write_u32(target, (fm3_info->write_algorithm->address)
-				+ sizeof(fm3_flash_write_code) - 8, (fm3_info->write_algorithm->address) - 8);
+			retval = target_write_u32(target, (write_algorithm->address)
+				+ sizeof(fm3_flash_write_code) - 8, (write_algorithm->address) - 8);
 			if (retval != ERROR_OK)
 				break;

 			/* Algorithm: u32FlashResult: */
-			retval = target_write_u32(target, (fm3_info->write_algorithm->address)
-				+ sizeof(fm3_flash_write_code) - 4, (fm3_info->write_algorithm->address) - 4);
+			retval = target_write_u32(target, (write_algorithm->address)
+				+ sizeof(fm3_flash_write_code) - 4, (write_algorithm->address) - 4);
 			if (retval != ERROR_OK)
 				break;
 		}
@@ -532,7 +547,7 @@ static int fm3_write_block(struct flash_bank *bank, uint8_t *buffer,
 		buf_set_u32(reg_params[4].value, 0, 32, u32FlashSeqAddress2);

 		retval = target_run_algorithm(target, 0, NULL, 6, reg_params,
-				fm3_info->write_algorithm->address, 0, 1000, &armv7m_info);
+				write_algorithm->address, 0, 1000, &armv7m_info);
 		if (retval != ERROR_OK) {
 			LOG_ERROR("Error executing fm3 Flash programming algorithm");
 			retval = ERROR_FLASH_OPERATION_FAILED;
@@ -552,7 +567,7 @@ static int fm3_write_block(struct flash_bank *bank, uint8_t *buffer,
 	}

 	target_free_working_area(target, source);
-	target_free_working_area(target, fm3_info->write_algorithm);
+	target_free_working_area(target, write_algorithm);

 	destroy_reg_param(&reg_params[0]);
 	destroy_reg_param(&reg_params[1]);
@@ -574,7 +589,22 @@ static int fm3_probe(struct flash_bank *bank)
 		return ERROR_TARGET_NOT_HALTED;
 	}

-	num_pages = 6;				/* max number of Flash pages for malloc */
+/*
+ -- page-- start -- blocksize - mpu - totalFlash --
+	page0 0x00000	16k
+	page1 0x04000	16k
+	page2 0x08000	96k		___ fxx3  128k Flash
+	page3 0x20000  128k		___ fxx4  256k Flash
+	page4 0x40000  128k		___ fxx5  384k Flash
+	page5 0x60000  128k		___ fxx6  512k Flash
+-----------------------
+	page6 0x80000  128k
+	page7 0xa0000  128k		___ fxx7  256k Flash
+	page8 0xc0000  128k
+	page9 0xe0000  128k		___ fxx8  256k Flash
+ */
+
+	num_pages = 10;				/* max number of Flash pages for malloc */
 	fm3_info->probed = 0;

 	bank->sectors = malloc(sizeof(struct flash_sector) * num_pages);
@@ -607,10 +637,14 @@ static int fm3_probe(struct flash_bank *bank)
 		|| (fm3_info->variant == mb9bfxx4)
 		|| (fm3_info->variant == mb9bfxx5)
 		|| (fm3_info->variant == mb9bfxx6)
+		|| (fm3_info->variant == mb9bfxx7)
+		|| (fm3_info->variant == mb9bfxx8)
 		|| (fm3_info->variant == mb9afxx2)
 		|| (fm3_info->variant == mb9afxx4)
 		|| (fm3_info->variant == mb9afxx5)
-		|| (fm3_info->variant == mb9afxx6)) {
+		|| (fm3_info->variant == mb9afxx6)
+		|| (fm3_info->variant == mb9afxx7)
+		|| (fm3_info->variant == mb9afxx8)) {
 		num_pages = 3;
 		bank->size = 128 * 1024; /* bytes */
 		bank->num_sectors = num_pages;
@@ -624,9 +658,13 @@ static int fm3_probe(struct flash_bank *bank)
 	if ((fm3_info->variant == mb9bfxx4)
 		|| (fm3_info->variant == mb9bfxx5)
 		|| (fm3_info->variant == mb9bfxx6)
+		|| (fm3_info->variant == mb9bfxx7)
+		|| (fm3_info->variant == mb9bfxx8)
 		|| (fm3_info->variant == mb9afxx4)
 		|| (fm3_info->variant == mb9afxx5)
-		|| (fm3_info->variant == mb9afxx6)) {
+		|| (fm3_info->variant == mb9afxx6)
+		|| (fm3_info->variant == mb9afxx7)
+		|| (fm3_info->variant == mb9afxx8)) {
 		num_pages = 4;
 		bank->size = 256 * 1024; /* bytes */
 		bank->num_sectors = num_pages;
@@ -639,8 +677,12 @@ static int fm3_probe(struct flash_bank *bank)

 	if ((fm3_info->variant == mb9bfxx5)
 		|| (fm3_info->variant == mb9bfxx6)
+		|| (fm3_info->variant == mb9bfxx7)
+		|| (fm3_info->variant == mb9bfxx8)
 		|| (fm3_info->variant == mb9afxx5)
-		|| (fm3_info->variant == mb9afxx6)) {
+		|| (fm3_info->variant == mb9afxx6)
+		|| (fm3_info->variant == mb9afxx7)
+		|| (fm3_info->variant == mb9afxx8)) {
 		num_pages = 5;
 		bank->size = 384 * 1024; /* bytes */
 		bank->num_sectors = num_pages;
@@ -652,7 +694,11 @@ static int fm3_probe(struct flash_bank *bank)
 	}

 	if ((fm3_info->variant == mb9bfxx6)
-		|| (fm3_info->variant == mb9afxx6)) {
+		|| (fm3_info->variant == mb9bfxx7)
+		|| (fm3_info->variant == mb9bfxx8)
+		|| (fm3_info->variant == mb9afxx6)
+		|| (fm3_info->variant == mb9afxx7)
+		|| (fm3_info->variant == mb9afxx8)) {
 		num_pages = 6;
 		bank->size = 512 * 1024; /* bytes */
 		bank->num_sectors = num_pages;
@@ -663,6 +709,42 @@ static int fm3_probe(struct flash_bank *bank)
 		bank->sectors[5].is_protected = -1;
 	}

+	if ((fm3_info->variant == mb9bfxx7)
+		|| (fm3_info->variant == mb9bfxx8)
+		|| (fm3_info->variant == mb9afxx7)
+		|| (fm3_info->variant == mb9afxx8)) {
+		num_pages = 8;
+		bank->size = 768 * 1024; /* bytes */
+		bank->num_sectors = num_pages;
+
+		bank->sectors[6].offset = 0x80000;
+		bank->sectors[6].size = 128 * 1024;
+		bank->sectors[6].is_erased = -1;
+		bank->sectors[6].is_protected = -1;
+
+		bank->sectors[7].offset = 0xa0000;
+		bank->sectors[7].size = 128 * 1024;
+		bank->sectors[7].is_erased = -1;
+		bank->sectors[7].is_protected = -1;
+	}
+
+	if ((fm3_info->variant == mb9bfxx8)
+		|| (fm3_info->variant == mb9afxx8)) {
+		num_pages = 10;
+		bank->size = 1024 * 1024; /* bytes */
+		bank->num_sectors = num_pages;
+
+		bank->sectors[8].offset = 0xc0000;
+		bank->sectors[8].size = 128 * 1024;
+		bank->sectors[8].is_erased = -1;
+		bank->sectors[8].is_protected = -1;
+
+		bank->sectors[9].offset = 0xe0000;
+		bank->sectors[9].size = 128 * 1024;
+		bank->sectors[9].is_erased = -1;
+		bank->sectors[9].is_protected = -1;
+	}
+
 	fm3_info->probed = 1;

 	return ERROR_OK;
--- a/src/flash/nor/kinetis.c
+++ b/src/flash/nor/kinetis.c
@@ -95,6 +95,24 @@ const struct {
 	{ 4<<10, 4<<10, 4 }
 };

+/* Addressess */
+#define FLEXRAM		0x14000000
+#define FTFx_FSTAT	0x40020000
+#define FTFx_FCNFG	0x40020001
+#define FTFx_FCCOB3	0x40020004
+#define FTFx_FPROT3	0x40020010
+#define SIM_SDID	0x40048024
+#define SIM_FCFG1	0x4004804c
+#define SIM_FCFG2	0x40048050
+
+/* Commands */
+#define FTFx_CMD_BLOCKSTAT 0x00
+#define FTFx_CMD_SECTSTAT 0x01
+#define FTFx_CMD_LWORDPROG 0x06
+#define FTFx_CMD_SECTERASE 0x09
+#define FTFx_CMD_SECTWRITE 0x0b
+#define FTFx_CMD_SETFLEXRAM 0x81
+
 struct kinetis_flash_bank {
 	unsigned granularity;
 	unsigned bank_ordinal;
@@ -160,8 +178,8 @@ static int kinetis_protect_check(struct flash_bank *bank)
 		uint32_t fprot, psec;
 		int i, b;

-		/* read protection register FTFx_FPROT */
-		result = target_read_memory(bank->target, 0x40020010, 1, 4, buffer);
+		/* read protection register */
+		result = target_read_memory(bank->target, FTFx_FPROT3, 1, 4, buffer);

 		if (result != ERROR_OK)
 			return result;
@@ -195,54 +213,55 @@ static int kinetis_protect_check(struct flash_bank *bank)
 	return ERROR_OK;
 }

-static int kinetis_ftfx_command(struct flash_bank *bank, uint32_t w0,
-				uint32_t w1, uint32_t w2, uint8_t *ftfx_fstat)
+static int kinetis_ftfx_command(struct flash_bank *bank, uint8_t fcmd, uint32_t faddr,
+				uint8_t fccob4, uint8_t fccob5, uint8_t fccob6, uint8_t fccob7,
+				uint8_t fccob8, uint8_t fccob9, uint8_t fccoba, uint8_t fccobb,
+				uint8_t *ftfx_fstat)
 {
-	uint8_t buffer[12];
+	uint8_t command[12] = {faddr & 0xff, (faddr >> 8) & 0xff, (faddr >> 16) & 0xff, fcmd,
+			       fccob7, fccob6, fccob5, fccob4,
+			       fccobb, fccoba, fccob9, fccob8};
 	int result, i;
+	uint8_t buffer;

 	/* wait for done */
 	for (i = 0; i < 50; i++) {
 		result =
-			target_read_memory(bank->target, 0x40020000, 1, 1, buffer);
+			target_read_memory(bank->target, FTFx_FSTAT, 1, 1, &buffer);

 		if (result != ERROR_OK)
 			return result;

-		if (buffer[0] & 0x80)
+		if (buffer & 0x80)
 			break;

-		buffer[0] = 0x00;
+		buffer = 0x00;
 	}

-	if (buffer[0] != 0x80) {
+	if (buffer != 0x80) {
 		/* reset error flags */
-		buffer[0] = 0x30;
+		buffer = 0x30;
 		result =
-			target_write_memory(bank->target, 0x40020000, 1, 1, buffer);
+			target_write_memory(bank->target, FTFx_FSTAT, 1, 1, &buffer);
 		if (result != ERROR_OK)
 			return result;
 	}

-	target_buffer_set_u32(bank->target, buffer, w0);
-	target_buffer_set_u32(bank->target, buffer + 4, w1);
-	target_buffer_set_u32(bank->target, buffer + 8, w2);
-
-	result = target_write_memory(bank->target, 0x40020004, 4, 3, buffer);
+	result = target_write_memory(bank->target, FTFx_FCCOB3, 4, 3, command);

 	if (result != ERROR_OK)
 		return result;

 	/* start command */
-	buffer[0] = 0x80;
-	result = target_write_memory(bank->target, 0x40020000, 1, 1, buffer);
+	buffer = 0x80;
+	result = target_write_memory(bank->target, FTFx_FSTAT, 1, 1, &buffer);
 	if (result != ERROR_OK)
 		return result;

 	/* wait for done */
 	for (i = 0; i < 50; i++) {
 		result =
-			target_read_memory(bank->target, 0x40020000, 1, 1, ftfx_fstat);
+			target_read_memory(bank->target, FTFx_FSTAT, 1, 1, ftfx_fstat);

 		if (result != ERROR_OK)
 			return result;
@@ -253,9 +272,10 @@ static int kinetis_ftfx_command(struct flash_bank *bank, uint32_t w0,

 	if ((*ftfx_fstat & 0xf0) != 0x80) {
 		LOG_ERROR
-			("ftfx command failed FSTAT: %02X W0: %08X W1: %08X W2: %08X",
-			 *ftfx_fstat, w0, w1, w2);
-
+			("ftfx command failed FSTAT: %02X FCCOB: %02X%02X%02X%02X %02X%02X%02X%02X %02X%02X%02X%02X",
+			 *ftfx_fstat, command[3], command[2], command[1], command[0],
+				      command[7], command[6], command[5], command[4],
+				      command[11], command[10], command[9], command[8]);
 		return ERROR_FLASH_OPERATION_FAILED;
 	}

@@ -265,7 +285,6 @@ static int kinetis_ftfx_command(struct flash_bank *bank, uint32_t w0,
 static int kinetis_erase(struct flash_bank *bank, int first, int last)
 {
 	int result, i;
-	uint32_t w0 = 0, w1 = 0, w2 = 0;

 	if (bank->target->state != TARGET_HALTED) {
 		LOG_ERROR("Target not halted");
@@ -283,9 +302,8 @@ static int kinetis_erase(struct flash_bank *bank, int first, int last)
 	for (i = first; i <= last; i++) {
 		uint8_t ftfx_fstat;
 		/* set command and sector address */
-		w0 = (0x09 << 24) | (bank->base + bank->sectors[i].offset);
-
-		result = kinetis_ftfx_command(bank, w0, w1, w2, &ftfx_fstat);
+		result = kinetis_ftfx_command(bank, FTFx_CMD_SECTERASE, bank->base + bank->sectors[i].offset,
+					      0, 0, 0, 0,  0, 0, 0, 0,  &ftfx_fstat);

 		if (result != ERROR_OK) {
 			LOG_WARNING("erase sector %d failed", i);
@@ -308,7 +326,7 @@ static int kinetis_write(struct flash_bank *bank, uint8_t *buffer,
 {
 	unsigned int i, result, fallback = 0;
 	uint8_t buf[8];
-	uint32_t wc, w0 = 0, w1 = 0, w2 = 0;
+	uint32_t wc;
 	struct kinetis_flash_bank *kinfo = bank->driver_priv;

 	if (bank->target->state != TARGET_HALTED) {
@@ -322,15 +340,13 @@ static int kinetis_write(struct flash_bank *bank, uint8_t *buffer,
 		LOG_DEBUG("flash write into FlexNVM @%08X", offset);

 		/* make flex ram available */
-		w0 = (0x81 << 24) | 0x00ff0000;
-
-		result = kinetis_ftfx_command(bank, w0, w1, w2, &ftfx_fstat);
+		result = kinetis_ftfx_command(bank, FTFx_CMD_SETFLEXRAM, 0x00ff0000, 0, 0, 0, 0,  0, 0, 0, 0,  &ftfx_fstat);

 		if (result != ERROR_OK)
 			return ERROR_FLASH_OPERATION_FAILED;

 		/* check if ram ready */
-		result = target_read_memory(bank->target, 0x40020001, 1, 1, buf);
+		result = target_read_memory(bank->target, FTFx_FCNFG, 1, 1, buf);

 		if (result != ERROR_OK)
 			return result;
@@ -349,23 +365,27 @@ static int kinetis_write(struct flash_bank *bank, uint8_t *buffer,

 	/* program section command */
 	if (fallback == 0) {
-		unsigned prog_section_bytes = kinfo->sector_size >> 8;
-		for (i = 0; i < count; i += kinfo->sector_size) {
-			/*
-			 * The largest possible Kinetis "section" is
-			 * 16 bytes.  A full Kinetis sector is always
-			 * 256 "section"s.
-			 */
+		/*
+		 * Kinetis uses different terms for the granularity of
+		 * sector writes, e.g. "phrase" or "128 bits".  We use
+		 * the generic term "chunk". The largest possible
+		 * Kinetis "chunk" is 16 bytes (128 bits).
+		 */
+		unsigned prog_section_chunk_bytes = kinfo->sector_size >> 8;
+		/* assume the NVM sector size is half the FlexRAM size */
+		unsigned prog_size_bytes = MIN(kinfo->sector_size,
+					       kinetis_flash_params[kinfo->granularity].nvm_sector_size_bytes);
+		for (i = 0; i < count; i += prog_size_bytes) {
 			uint8_t residual_buffer[16];
 			uint8_t ftfx_fstat;
-			uint32_t section_count = 256;
+			uint32_t section_count = prog_size_bytes / prog_section_chunk_bytes;
 			uint32_t residual_wc = 0;

 			/*
 			 * Assume the word count covers an entire
 			 * sector.
 			 */
-			wc = kinfo->sector_size / 4;
+			wc = prog_size_bytes / 4;

 			/*
 			 * If bytes to be programmed are less than the
@@ -374,29 +394,29 @@ static int kinetis_write(struct flash_bank *bank, uint8_t *buffer,
 			 * residual buffer so that a full "section"
 			 * may always be programmed.
 			 */
-			if ((count - i) < kinfo->sector_size) {
+			if ((count - i) < prog_size_bytes) {
 				/* number of bytes to program beyond full section */
-				unsigned residual_bc = (count-i) % prog_section_bytes;
+				unsigned residual_bc = (count-i) % prog_section_chunk_bytes;

 				/* number of complete words to copy directly from buffer */
 				wc = (count - i) / 4;

 				/* number of total sections to write, including residual */
-				section_count = DIV_ROUND_UP((count-i), prog_section_bytes);
+				section_count = DIV_ROUND_UP((count-i), prog_section_chunk_bytes);

 				/* any residual bytes delivers a whole residual section */
-				residual_wc = (residual_bc ? prog_section_bytes : 0)/4;
+				residual_wc = (residual_bc ? prog_section_chunk_bytes : 0)/4;

 				/* clear residual buffer then populate residual bytes */
-				(void) memset(residual_buffer, 0xff, prog_section_bytes);
+				(void) memset(residual_buffer, 0xff, prog_section_chunk_bytes);
 				(void) memcpy(residual_buffer, &buffer[i+4*wc], residual_bc);
 			}

 			LOG_DEBUG("write section @ %08X with length %d bytes",
-				  offset + i, (count - i));
+				  offset + i, wc*4);

 			/* write data to flexram as whole-words */
-			result = target_write_memory(bank->target, 0x14000000, 4, wc,
+			result = target_write_memory(bank->target, FLEXRAM, 4, wc,
 						     buffer + i);

 			if (result != ERROR_OK) {
@@ -407,7 +427,7 @@ static int kinetis_write(struct flash_bank *bank, uint8_t *buffer,
 			/* write the residual words to the flexram */
 			if (residual_wc) {
 				result = target_write_memory(bank->target,
-							     0x14000000+4*wc,
+							     FLEXRAM+4*wc,
 							     4, residual_wc,
 							     residual_buffer);

@@ -418,10 +438,9 @@ static int kinetis_write(struct flash_bank *bank, uint8_t *buffer,
 			}

 			/* execute section-write command */
-			w0 = (0x0b << 24) | (bank->base + offset + i);
-			w1 = section_count << 16;
-
-			result = kinetis_ftfx_command(bank, w0, w1, w2, &ftfx_fstat);
+			result = kinetis_ftfx_command(bank, FTFx_CMD_SECTWRITE, bank->base + offset + i,
+						      section_count>>8, section_count, 0, 0,
+						      0, 0, 0, 0,  &ftfx_fstat);

 			if (result != ERROR_OK)
 				return ERROR_FLASH_OPERATION_FAILED;
@@ -434,16 +453,11 @@ static int kinetis_write(struct flash_bank *bank, uint8_t *buffer,

 			LOG_DEBUG("write longword @ %08X", offset + i);

-			w0 = (0x06 << 24) | (bank->base + offset + i);
-			if (count - i < 4) {
-				uint32_t padding = 0xffffffff;
-				memcpy(&padding, buffer + i, count - i);
-				w1 = buf_get_u32(&padding, 0, 32);
-			} else {
-				w1 = buf_get_u32(buffer + i, 0, 32);
-			}
-
-			result = kinetis_ftfx_command(bank, w0, w1, w2, &ftfx_fstat);
+			uint8_t padding[4] = {0xff, 0xff, 0xff, 0xff};
+			memcpy(padding, buffer + i, MIN(4, count-i));
+			result = kinetis_ftfx_command(bank, FTFx_CMD_LWORDPROG, bank->base + offset + i,
+						      padding[3], padding[2], padding[1], padding[0],
+						      0, 0, 0, 0,  &ftfx_fstat);

 			if (result != ERROR_OK)
 				return ERROR_FLASH_OPERATION_FAILED;
@@ -467,16 +481,18 @@ static int kinetis_read_part_info(struct flash_bank *bank)
 		first_nvm_bank = 0, reassign = 0;
 	struct kinetis_flash_bank *kinfo = bank->driver_priv;

-	result = target_read_memory(bank->target, 0x40048024, 1, 4, buf);
+	result = target_read_memory(bank->target, SIM_SDID, 1, 4, buf);
 	if (result != ERROR_OK)
 		return result;
 	kinfo->sim_sdid = target_buffer_get_u32(bank->target, buf);
 	granularity = (kinfo->sim_sdid >> 7) & 0x03;
-	result = target_read_memory(bank->target, 0x4004804c, 1, 4, buf);
+
+	result = target_read_memory(bank->target, SIM_FCFG1, 1, 4, buf);
 	if (result != ERROR_OK)
 		return result;
 	kinfo->sim_fcfg1 = target_buffer_get_u32(bank->target, buf);
-	result = target_read_memory(bank->target, 0x40048050, 1, 4, buf);
+
+	result = target_read_memory(bank->target, SIM_FCFG2, 1, 4, buf);
 	if (result != ERROR_OK)
 		return result;
 	kinfo->sim_fcfg2 = target_buffer_get_u32(bank->target, buf);
@@ -625,7 +641,7 @@ static int kinetis_read_part_info(struct flash_bank *bank)
 				} else if (bank->size != ee_size) {
 					LOG_WARNING("FlexRAM size mismatch");
 					reassign = 1;
-				} else if (bank->base != 0x14000000) {
+				} else if (bank->base != FLEXRAM) {
 					LOG_WARNING("FlexRAM address mismatch");
 					reassign = 1;
 				} else if (kinfo->sector_size !=
@@ -744,14 +760,10 @@ static int kinetis_blank_check(struct flash_bank *bank)

 	if (kinfo->flash_class == FC_PFLASH) {
 		int result;
-		uint32_t w0 = 0, w1 = 0, w2 = 0;
 		uint8_t ftfx_fstat;

 		/* check if whole bank is blank */
-		w0 = (0x00 << 24) | bank->base;
-		w1 = 0; /* "normal margin" */
-
-		result = kinetis_ftfx_command(bank, w0, w1, w2, &ftfx_fstat);
+		result = kinetis_ftfx_command(bank, FTFx_CMD_BLOCKSTAT, bank->base, 0, 0, 0, 0,  0, 0, 0, 0, &ftfx_fstat);

 		if (result != ERROR_OK)
 			return result;
@@ -760,10 +772,9 @@ static int kinetis_blank_check(struct flash_bank *bank)
 			/* the whole bank is not erased, check sector-by-sector */
 			int i;
 			for (i = 0; i < bank->num_sectors; i++) {
-				w0 = (0x01 << 24) | (bank->base + bank->sectors[i].offset);
-				w1 = (0x100 << 16) | 0; /* normal margin */
-
-				result = kinetis_ftfx_command(bank, w0, w1, w2, &ftfx_fstat);
+				/* normal margin */
+				result = kinetis_ftfx_command(bank, FTFx_CMD_SECTSTAT, bank->base + bank->sectors[i].offset,
+						1, 0, 0, 0,  0, 0, 0, 0, &ftfx_fstat);

 				if (result == ERROR_OK) {
 					bank->sectors[i].is_erased = !(ftfx_fstat & 0x01);
--- a/src/flash/nor/lpc2000.c
+++ b/src/flash/nor/lpc2000.c
@@ -35,9 +35,8 @@
 * @file
 * flash programming support for NXP LPC17xx 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).
+ * @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).
 */
 /*
 * currently supported devices:
@@ -59,17 +58,21 @@
 * lpc1700:
 * - 175x
 * - 176x (tested with LPC1768)
+ *
+ * lpc4300 (also available as lpc1800 - alias)
+ * - 43x2 | 3 | 5 | 7 (tested with 4337)
+ * - 18x2 | 3 | 5 | 7
 */

 typedef enum {
 	lpc2000_v1,
 	lpc2000_v2,
-	lpc1700
+	lpc1700,
+	lpc4300,
 } lpc2000_variant;

 struct lpc2000_flash_bank {
 	lpc2000_variant variant;
-	struct working_area *iap_working_area;
 	uint32_t cclk;
 	int cmd51_dst_boundary;
 	int cmd51_can_256b;
@@ -77,6 +80,9 @@ struct lpc2000_flash_bank {
 	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;
 };

 enum lpc2000_status_codes {
@@ -99,13 +105,15 @@ enum lpc2000_status_codes {
 	LPC2000_INVALID_CODE = 16,
 	LPC2000_INVALID_BAUD_RATE = 17,
 	LPC2000_INVALID_STOP_BIT = 18,
-	LPC2000_CRP_ENABLED = 19
+	LPC2000_CRP_ENABLED = 19,
+	LPC2000_INVALID_FLASH_UNIT = 20,
+	LPC2000_USER_CODE_CHECKSUM = 21,
+	LCP2000_ERROR_SETTING_ACTIVE_PARTITION = 22,
 };

 static int lpc2000_build_sector_list(struct flash_bank *bank)
 {
 	struct lpc2000_flash_bank *lpc2000_info = bank->driver_priv;
-	int i;
 	uint32_t offset = 0;

 	/* default to a 4096 write buffer */
@@ -116,7 +124,7 @@ static int lpc2000_build_sector_list(struct flash_bank *bank)
 		if (bank->size == 128 * 1024) {
 			bank->num_sectors = 16;
 			bank->sectors = malloc(sizeof(struct flash_sector) * 16);
-			for (i = 0; i < 16; i++) {
+			for (int i = 0; i < 16; i++) {
 				bank->sectors[i].offset = offset;
 				bank->sectors[i].size = 8 * 1024;
 				offset += bank->sectors[i].size;
@@ -127,21 +135,21 @@ static int lpc2000_build_sector_list(struct flash_bank *bank)
 			bank->num_sectors = 18;
 			bank->sectors = malloc(sizeof(struct flash_sector) * 18);

-			for (i = 0; i < 8; i++) {
+			for (int i = 0; i < 8; i++) {
 				bank->sectors[i].offset = offset;
 				bank->sectors[i].size = 8 * 1024;
 				offset += bank->sectors[i].size;
 				bank->sectors[i].is_erased = -1;
 				bank->sectors[i].is_protected = 1;
 			}
-			for (i = 8; i < 10; i++) {
+			for (int i = 8; i < 10; i++) {
 				bank->sectors[i].offset = offset;
 				bank->sectors[i].size = 64 * 1024;
 				offset += bank->sectors[i].size;
 				bank->sectors[i].is_erased = -1;
 				bank->sectors[i].is_protected = 1;
 			}
-			for (i = 10; i < 18; i++) {
+			for (int i = 10; i < 18; i++) {
 				bank->sectors[i].offset = offset;
 				bank->sectors[i].size = 8 * 1024;
 				offset += bank->sectors[i].size;
@@ -193,7 +201,7 @@ static int lpc2000_build_sector_list(struct flash_bank *bank)

 		bank->sectors = malloc(sizeof(struct flash_sector) * bank->num_sectors);

-		for (i = 0; i < bank->num_sectors; i++) {
+		for (int i = 0; i < bank->num_sectors; i++) {
 			if (i < 8) {
 				bank->sectors[i].offset = offset;
 				bank->sectors[i].size = 4 * 1024;
@@ -238,15 +246,40 @@ static int lpc2000_build_sector_list(struct flash_bank *bank)

 		bank->sectors = malloc(sizeof(struct flash_sector) * bank->num_sectors);

-		for (i = 0; i < bank->num_sectors; i++) {
+		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 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) {
+		switch (bank->size) {
+			case 256 * 1024:
+				bank->num_sectors = 11;
+				break;
+			case 384 * 1024:
+				bank->num_sectors = 13;
+				break;
+			case 512 * 1024:
+				bank->num_sectors = 15;
+				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;
+			/* sectors 0-7 are 8kB-sized, 8 and above are 64kB-sized for LPC43xx devices */
+			bank->sectors[i].size = (i < 8) ? 8 * 1024 : 64 * 1024;
+			offset += bank->sectors[i].size;
+			bank->sectors[i].is_erased = -1;
+			bank->sectors[i].is_protected = 1;
+		}
 	} else {
 		LOG_ERROR("BUG: unknown lpc2000_info->variant encountered");
 		exit(-1);
@@ -255,69 +288,67 @@ static int lpc2000_build_sector_list(struct flash_bank *bank)
 	return ERROR_OK;
 }

-/* call LPC1700/LPC2000 IAP function
- * uses 180 bytes working area
+/* this function allocates and initializes working area used for IAP algorithm
+ * uses 52 + max IAP stack bytes working area
 * 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: stack (only 128b needed)
+ * 0x34 to 0xb3|0x104: stack (only 128b needed for lpc17xx/2000, 208 for lpc43xx)
 */
-static int lpc2000_iap_call(struct flash_bank *bank,
-	int code,
-	uint32_t param_table[5],
-	uint32_t result_table[4])
+
+static int lpc2000_iap_working_area_init(struct flash_bank *bank, struct working_area **iap_working_area)
+{
+	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) {
+		LOG_ERROR("no working area specified, can't write LPC2000 internal flash");
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	uint8_t jump_gate[8];
+
+	/* write IAP code to working area */
+	switch (lpc2000_info->variant) {
+		case lpc1700:
+		case lpc4300:
+			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;
+		case lpc2000_v1:
+		case lpc2000_v2:
+			target_buffer_set_u32(target, jump_gate, ARMV4_5_BX(12));
+			target_buffer_set_u32(target, jump_gate + 4, ARMV4_5_B(0xfffffe, 0));
+			break;
+		default:
+			LOG_ERROR("BUG: unknown lpc2000_info->variant encountered");
+			exit(-1);
+	}
+
+	int retval = target_write_memory(target, (*iap_working_area)->address, 4, 2, jump_gate);
+	if (retval != ERROR_OK)
+		LOG_ERROR("Write memory at address 0x%8.8" PRIx32 " failed (check work_area definition)",
+				(*iap_working_area)->address);
+
+	return retval;
+}
+
+/* call LPC1700/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])
 {
-	int retval;
 	struct lpc2000_flash_bank *lpc2000_info = bank->driver_priv;
 	struct target *target = bank->target;
-	struct mem_param mem_params[2];
-	struct reg_param reg_params[5];
+
 	struct arm_algorithm arm_algo;	/* for LPC2000 */
 	struct armv7m_algorithm armv7m_info;	/* for LPC1700 */
-	uint32_t status_code;
 	uint32_t iap_entry_point = 0;	/* to make compiler happier */

-	/* regrab previously allocated working_area, or allocate a new one */
-	if (!lpc2000_info->iap_working_area) {
-		uint8_t jump_gate[8];
-
-		/* make sure we have a working area */
-		if (target_alloc_working_area(target, 180,
-				&lpc2000_info->iap_working_area) != ERROR_OK) {
-			LOG_ERROR("no working area specified, can't write LPC2000 internal flash");
-			return ERROR_FLASH_OPERATION_FAILED;
-		}
-
-		/* write IAP code to working area */
-		switch (lpc2000_info->variant) {
-			case lpc1700:
-				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;
-			case lpc2000_v1:
-			case lpc2000_v2:
-				target_buffer_set_u32(target, jump_gate, ARMV4_5_BX(12));
-				target_buffer_set_u32(target, jump_gate + 4, ARMV4_5_B(0xfffffe, 0));
-				break;
-			default:
-				LOG_ERROR("BUG: unknown lpc2000_info->variant encountered");
-				exit(-1);
-		}
-
-		retval = target_write_memory(target,
-				lpc2000_info->iap_working_area->address, 4, 2, jump_gate);
-		if (retval != ERROR_OK) {
-			LOG_ERROR(
-				"Write memory at address 0x%8.8" PRIx32 " failed (check work_area definition)",
-				lpc2000_info->iap_working_area->address);
-			return retval;
-		}
-	}
-
 	switch (lpc2000_info->variant) {
 		case lpc1700:
 			armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
-			armv7m_info.core_mode = ARMV7M_MODE_ANY;
+			armv7m_info.core_mode = ARM_MODE_THREAD;
 			iap_entry_point = 0x1fff1ff1;
 			break;
 		case lpc2000_v1:
@@ -327,14 +358,21 @@ static int lpc2000_iap_call(struct flash_bank *bank,
 			arm_algo.core_state = ARM_STATE_ARM;
 			iap_entry_point = 0x7ffffff1;
 			break;
+		case lpc4300:
+			armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
+			armv7m_info.core_mode = ARM_MODE_THREAD;
+			/* read out IAP entry point from ROM driver table at 0x10400100 */
+			target_read_u32(target, 0x10400100, &iap_entry_point);
+			break;
 		default:
 			LOG_ERROR("BUG: unknown lpc2000->variant encountered");
 			exit(-1);
 	}

+	struct mem_param mem_params[2];
+
 	/* command parameter table */
-	init_mem_param(&mem_params[0], lpc2000_info->iap_working_area->address + 8, 6 * 4,
-		PARAM_OUT);
+	init_mem_param(&mem_params[0], iap_working_area->address + 8, 6 * 4, PARAM_OUT);
 	target_buffer_set_u32(target, mem_params[0].value, code);
 	target_buffer_set_u32(target, mem_params[0].value + 0x04, param_table[0]);
 	target_buffer_set_u32(target, mem_params[0].value + 0x08, param_table[1]);
@@ -342,17 +380,16 @@ static int lpc2000_iap_call(struct flash_bank *bank,
 	target_buffer_set_u32(target, mem_params[0].value + 0x10, param_table[3]);
 	target_buffer_set_u32(target, mem_params[0].value + 0x14, param_table[4]);

+	struct reg_param reg_params[5];
+
 	init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
-	buf_set_u32(reg_params[0].value, 0, 32, lpc2000_info->iap_working_area->address + 0x08);
+	buf_set_u32(reg_params[0].value, 0, 32, iap_working_area->address + 0x08);

 	/* command result table */
-	init_mem_param(&mem_params[1],
-		lpc2000_info->iap_working_area->address + 0x20,
-		5 * 4,
-		PARAM_IN);
+	init_mem_param(&mem_params[1], iap_working_area->address + 0x20, 5 * 4, PARAM_IN);

 	init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
-	buf_set_u32(reg_params[1].value, 0, 32, lpc2000_info->iap_working_area->address + 0x20);
+	buf_set_u32(reg_params[1].value, 0, 32, iap_working_area->address + 0x20);

 	/* IAP entry point */
 	init_reg_param(&reg_params[2], "r12", 32, PARAM_OUT);
@@ -360,53 +397,46 @@ static int lpc2000_iap_call(struct flash_bank *bank,

 	switch (lpc2000_info->variant) {
 		case lpc1700:
+		case lpc4300:
 			/* IAP stack */
 			init_reg_param(&reg_params[3], "sp", 32, PARAM_OUT);
-			buf_set_u32(reg_params[3].value, 0, 32,
-					lpc2000_info->iap_working_area->address + 0xb4);
+			buf_set_u32(reg_params[3].value, 0, 32, iap_working_area->address + lpc2000_info->cmd51_src_offset);

 			/* return address */
 			init_reg_param(&reg_params[4], "lr", 32, PARAM_OUT);
-			buf_set_u32(reg_params[4].value, 0, 32,
-					(lpc2000_info->iap_working_area->address + 0x04) | 1);
+			buf_set_u32(reg_params[4].value, 0, 32, (iap_working_area->address + 0x04) | 1);
 			/* bit0 of LR = 1 to return in Thumb mode */

-			target_run_algorithm(target, 2, mem_params, 5, reg_params,
-					lpc2000_info->iap_working_area->address, 0, 10000, &armv7m_info);
+			target_run_algorithm(target, 2, mem_params, 5, reg_params, iap_working_area->address, 0, 10000,
+					&armv7m_info);
 			break;
 		case lpc2000_v1:
 		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,
-					lpc2000_info->iap_working_area->address + 0xb4);
+			buf_set_u32(reg_params[3].value, 0, 32, iap_working_area->address + lpc2000_info->cmd51_src_offset);

 			/* return address */
 			init_reg_param(&reg_params[4], "lr_svc", 32, PARAM_OUT);
-			buf_set_u32(reg_params[4].value, 0, 32,
-					lpc2000_info->iap_working_area->address + 0x04);
+			buf_set_u32(reg_params[4].value, 0, 32, iap_working_area->address + 0x04);

-			target_run_algorithm(target, 2, mem_params, 5, reg_params,
-					lpc2000_info->iap_working_area->address,
-					lpc2000_info->iap_working_area->address + 0x4,
-					10000, &arm_algo);
+			target_run_algorithm(target, 2, mem_params, 5, reg_params, iap_working_area->address,
+					iap_working_area->address + 0x4, 10000, &arm_algo);
 			break;
 		default:
 			LOG_ERROR("BUG: unknown lpc2000->variant encountered");
 			exit(-1);
 	}

-	status_code = target_buffer_get_u32(target, mem_params[1].value);
+	int status_code = target_buffer_get_u32(target, mem_params[1].value);
 	result_table[0] = target_buffer_get_u32(target, mem_params[1].value + 0x04);
 	result_table[1] = target_buffer_get_u32(target, mem_params[1].value + 0x08);
 	result_table[2] = target_buffer_get_u32(target, mem_params[1].value + 0x0c);
 	result_table[3] = target_buffer_get_u32(target, mem_params[1].value + 0x10);

-	LOG_DEBUG("IAP command = %i (0x%8.8" PRIx32 ", 0x%8.8" PRIx32
-			", 0x%8.8" PRIx32 ", 0x%8.8" PRIx32 ", 0x%8.8"
-			PRIx32 ") completed with result = %8.8" PRIx32,
-			code, param_table[0], param_table[1], param_table[2],
-			param_table[3], param_table[4], status_code);
+	LOG_DEBUG("IAP command = %i (0x%8.8" PRIx32 ", 0x%8.8" PRIx32 ", 0x%8.8" PRIx32 ", 0x%8.8" PRIx32 ", 0x%8.8" PRIx32
+			") completed with result = %8.8" PRIx32,
+			code, param_table[0], param_table[1], param_table[2], param_table[3], param_table[4], status_code);

 	destroy_mem_param(&mem_params[0]);
 	destroy_mem_param(&mem_params[1]);
@@ -422,22 +452,31 @@ static int lpc2000_iap_call(struct flash_bank *bank,

 static int lpc2000_iap_blank_check(struct flash_bank *bank, int first, int last)
 {
-	uint32_t param_table[5] = {0};
-	uint32_t result_table[4];
-	int status_code;
-	int i;
-
 	if ((first < 0) || (last >= bank->num_sectors))
 		return ERROR_FLASH_SECTOR_INVALID;

-	for (i = first; i <= last; i++) {
+	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;
+
+	struct lpc2000_flash_bank *lpc2000_info = bank->driver_priv;
+	if (lpc2000_info->variant == lpc4300)
+		param_table[2] = lpc2000_info->lpc4300_bank;
+
+	for (int i = first; i <= last && retval == ERROR_OK; i++) {
 		/* check single sector */
 		param_table[0] = param_table[1] = i;
-		status_code = lpc2000_iap_call(bank, 53, param_table, result_table);
+		int status_code = lpc2000_iap_call(bank, iap_working_area, 53, param_table, result_table);

 		switch (status_code) {
 			case ERROR_FLASH_OPERATION_FAILED:
-				return ERROR_FLASH_OPERATION_FAILED;
+				retval = ERROR_FLASH_OPERATION_FAILED;
+				break;
 			case LPC2000_CMD_SUCCESS:
 				bank->sectors[i].is_erased = 1;
 				break;
@@ -448,7 +487,7 @@ static int lpc2000_iap_blank_check(struct flash_bank *bank, int first, int last)
 				bank->sectors[i].is_erased = 0;
 				break;
 			case LPC2000_BUSY:
-				return ERROR_FLASH_BUSY;
+				retval = ERROR_FLASH_BUSY;
 				break;
 			default:
 				LOG_ERROR("BUG: unknown LPC2000 status code %i", status_code);
@@ -456,7 +495,10 @@ static int lpc2000_iap_blank_check(struct flash_bank *bank, int first, int last)
 		}
 	}

-	return ERROR_OK;
+	struct target *target = bank->target;
+	target_free_working_area(target, iap_working_area);
+
+	return retval;
 }

 /*
@@ -464,12 +506,10 @@ static int lpc2000_iap_blank_check(struct flash_bank *bank, int first, int last)
 */
 FLASH_BANK_COMMAND_HANDLER(lpc2000_flash_bank_command)
 {
-	struct lpc2000_flash_bank *lpc2000_info;
-
 	if (CMD_ARGC < 8)
 		return ERROR_COMMAND_SYNTAX_ERROR;

-	lpc2000_info = malloc(sizeof(struct lpc2000_flash_bank));
+	struct lpc2000_flash_bank *lpc2000_info = malloc(sizeof(struct lpc2000_flash_bank));
 	bank->driver_priv = lpc2000_info;

 	if (strcmp(CMD_ARGV[6], "lpc2000_v1") == 0) {
@@ -478,29 +518,48 @@ FLASH_BANK_COMMAND_HANDLER(lpc2000_flash_bank_command)
 		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) {
 		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 {
 		LOG_ERROR("unknown LPC2000 variant: %s", CMD_ARGV[6]);
 		free(lpc2000_info);
 		return ERROR_FLASH_BANK_INVALID;
 	}

-	lpc2000_info->iap_working_area = NULL;
+	/* see lpc2000_iap_working_area_init() for the reason behind the 0x34 value */
+	lpc2000_info->cmd51_src_offset = 0x34 + lpc2000_info->iap_max_stack;
+
 	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);
+	if (temp_base >= 0x1B000000)
+		lpc2000_info->lpc4300_bank = 1; /* bank B */
+	else
+		lpc2000_info->lpc4300_bank = 0; /* bank A */
+
 	if (CMD_ARGC >= 9) {
 		if (strcmp(CMD_ARGV[8], "calc_checksum") == 0)
 			lpc2000_info->calc_checksum = 1;
@@ -511,51 +570,74 @@ FLASH_BANK_COMMAND_HANDLER(lpc2000_flash_bank_command)

 static int lpc2000_erase(struct flash_bank *bank, int first, int last)
 {
-	struct lpc2000_flash_bank *lpc2000_info = bank->driver_priv;
-	uint32_t param_table[5] = {0};
-	uint32_t result_table[4];
-	int status_code;
-
 	if (bank->target->state != TARGET_HALTED) {
 		LOG_ERROR("Target not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}

+	struct lpc2000_flash_bank *lpc2000_info = bank->driver_priv;
+	uint32_t param_table[5] = {0};
+
 	param_table[0] = first;
 	param_table[1] = last;
-	param_table[2] = lpc2000_info->cclk;
+
+	if (lpc2000_info->variant == lpc4300)
+		param_table[2] = lpc2000_info->lpc4300_bank;
+	else
+		param_table[2] = lpc2000_info->cclk;
+
+	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;

 	/* Prepare sectors */
-	status_code = lpc2000_iap_call(bank, 50, param_table, result_table);
+	int status_code = lpc2000_iap_call(bank, iap_working_area, 50, param_table, result_table);
 	switch (status_code) {
 		case ERROR_FLASH_OPERATION_FAILED:
-			return ERROR_FLASH_OPERATION_FAILED;
+			retval = ERROR_FLASH_OPERATION_FAILED;
+			break;
 		case LPC2000_CMD_SUCCESS:
 			break;
 		case LPC2000_INVALID_SECTOR:
-			return ERROR_FLASH_SECTOR_INVALID;
+			retval = ERROR_FLASH_SECTOR_INVALID;
 			break;
 		default:
 			LOG_WARNING("lpc2000 prepare sectors returned %i", status_code);
-			return ERROR_FLASH_OPERATION_FAILED;
+			retval = ERROR_FLASH_OPERATION_FAILED;
+			break;
 	}

-	/* Erase sectors */
-	status_code = lpc2000_iap_call(bank, 52, param_table, result_table);
-	switch (status_code) {
-		case ERROR_FLASH_OPERATION_FAILED:
-			return ERROR_FLASH_OPERATION_FAILED;
-		case LPC2000_CMD_SUCCESS:
-			break;
-		case LPC2000_INVALID_SECTOR:
-			return ERROR_FLASH_SECTOR_INVALID;
-			break;
-		default:
-			LOG_WARNING("lpc2000 erase sectors returned %i", status_code);
-			return ERROR_FLASH_OPERATION_FAILED;
+	if (retval == ERROR_OK) {
+		/* Erase sectors */
+		param_table[2] = lpc2000_info->cclk;
+		if (lpc2000_info->variant == lpc4300)
+			param_table[3] = lpc2000_info->lpc4300_bank;
+
+		status_code = lpc2000_iap_call(bank, iap_working_area, 52, param_table, result_table);
+		switch (status_code) {
+			case ERROR_FLASH_OPERATION_FAILED:
+				retval = ERROR_FLASH_OPERATION_FAILED;
+				break;
+			case LPC2000_CMD_SUCCESS:
+				break;
+			case LPC2000_INVALID_SECTOR:
+				retval = ERROR_FLASH_SECTOR_INVALID;
+				break;
+			default:
+				LOG_WARNING("lpc2000 erase sectors returned %i", status_code);
+				retval = ERROR_FLASH_OPERATION_FAILED;
+				break;
+		}
 	}

-	return ERROR_OK;
+	struct target *target = bank->target;
+	target_free_working_area(target, iap_working_area);
+
+	return retval;
 }

 static int lpc2000_protect(struct flash_bank *bank, int set, int first, int last)
@@ -566,19 +648,7 @@ static int lpc2000_protect(struct flash_bank *bank, int set, int first, int last

 static int lpc2000_write(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, uint32_t count)
 {
-	struct lpc2000_flash_bank *lpc2000_info = bank->driver_priv;
 	struct target *target = bank->target;
-	uint32_t dst_min_alignment;
-	uint32_t bytes_remaining = count;
-	uint32_t bytes_written = 0;
-	int first_sector = 0;
-	int last_sector = 0;
-	uint32_t param_table[5] = {0};
-	uint32_t result_table[4];
-	int status_code;
-	int i;
-	struct working_area *download_area;
-	int retval = ERROR_OK;

 	if (bank->target->state != TARGET_HALTED) {
 		LOG_ERROR("Target not halted");
@@ -588,20 +658,22 @@ static int lpc2000_write(struct flash_bank *bank, uint8_t *buffer, uint32_t offs
 	if (offset + count > bank->size)
 		return ERROR_FLASH_DST_OUT_OF_BANK;

-	dst_min_alignment = lpc2000_info->cmd51_dst_boundary;
+	struct lpc2000_flash_bank *lpc2000_info = bank->driver_priv;
+
+	uint32_t dst_min_alignment = lpc2000_info->cmd51_dst_boundary;

 	if (offset % dst_min_alignment) {
-		LOG_WARNING("offset 0x%" PRIx32 " breaks required alignment 0x%" PRIx32,
-			offset,
-			dst_min_alignment);
+		LOG_WARNING("offset 0x%" PRIx32 " breaks required alignment 0x%" PRIx32, offset, dst_min_alignment);
 		return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
 	}

-	for (i = 0; i < bank->num_sectors; i++) {
+	int first_sector = 0;
+	int last_sector = 0;
+
+	for (int i = 0; i < bank->num_sectors; i++) {
 		if (offset >= bank->sectors[i].offset)
 			first_sector = i;
-		if (offset + DIV_ROUND_UP(count, dst_min_alignment)
-				* dst_min_alignment > bank->sectors[i].offset)
+		if (offset + DIV_ROUND_UP(count, dst_min_alignment) * dst_min_alignment > bank->sectors[i].offset)
 			last_sector = i;
 	}

@@ -610,35 +682,46 @@ static int lpc2000_write(struct flash_bank *bank, uint8_t *buffer, uint32_t offs
 	/* check if exception vectors should be flashed */
 	if ((offset == 0) && (count >= 0x20) && lpc2000_info->calc_checksum) {
 		uint32_t checksum = 0;
-		for (i = 0; i < 8; i++) {
-			LOG_DEBUG("Vector 0x%2.2x: 0x%8.8" PRIx32, i * 4,
-				buf_get_u32(buffer + (i * 4), 0, 32));
+		for (int i = 0; i < 8; i++) {
+			LOG_DEBUG("Vector 0x%2.2x: 0x%8.8" PRIx32, i * 4, buf_get_u32(buffer + (i * 4), 0, 32));
 			if (i != lpc2000_info->checksum_vector)
 				checksum += buf_get_u32(buffer + (i * 4), 0, 32);
 		}
 		checksum = 0 - checksum;
 		LOG_DEBUG("checksum: 0x%8.8" PRIx32, checksum);

-		uint32_t original_value = buf_get_u32(buffer +
-				(lpc2000_info->checksum_vector * 4), 0, 32);
+		uint32_t original_value = buf_get_u32(buffer + (lpc2000_info->checksum_vector * 4), 0, 32);
 		if (original_value != checksum) {
-			LOG_WARNING("Verification will fail since checksum in image (0x%8.8" PRIx32 ") "
-					"to be written to flash is different from calculated vector "
-					"checksum (0x%8.8" PRIx32 ").", original_value, checksum);
-			LOG_WARNING("To remove this warning modify build tools on developer PC "
-					"to inject correct LPC vector checksum.");
+			LOG_WARNING("Verification will fail since checksum in image (0x%8.8" PRIx32 ") to be written to flash is "
+					"different from calculated vector checksum (0x%8.8" PRIx32 ").", original_value, checksum);
+			LOG_WARNING("To remove this warning modify build tools on developer PC to inject correct LPC vector "
+					"checksum.");
 		}

 		buf_set_u32(buffer + (lpc2000_info->checksum_vector * 4), 0, 32, checksum);
 	}

+	struct working_area *iap_working_area;
+
+	int retval = lpc2000_iap_working_area_init(bank, &iap_working_area);
+
+	if (retval != ERROR_OK)
+		return retval;
+
+	struct working_area *download_area;
+
 	/* allocate a working area */
-	if (target_alloc_working_area(target, lpc2000_info->cmd51_max_buffer,
-			&download_area) != ERROR_OK) {
+	if (target_alloc_working_area(target, lpc2000_info->cmd51_max_buffer, &download_area) != ERROR_OK) {
 		LOG_ERROR("no working area specified, can't write LPC2000 internal flash");
+		target_free_working_area(target, iap_working_area);
 		return ERROR_FLASH_OPERATION_FAILED;
 	}

+	uint32_t bytes_remaining = count;
+	uint32_t bytes_written = 0;
+	uint32_t param_table[5] = {0};
+	uint32_t result_table[4];
+
 	while (bytes_remaining > 0) {
 		uint32_t thisrun_bytes;
 		if (bytes_remaining >= lpc2000_info->cmd51_max_buffer)
@@ -653,7 +736,13 @@ static int lpc2000_write(struct flash_bank *bank, uint8_t *buffer, uint32_t offs
 		/* Prepare sectors */
 		param_table[0] = first_sector;
 		param_table[1] = last_sector;
-		status_code = lpc2000_iap_call(bank, 50, param_table, result_table);
+
+		if (lpc2000_info->variant == lpc4300)
+			param_table[2] = lpc2000_info->lpc4300_bank;
+		else
+			param_table[2] = lpc2000_info->cclk;
+
+		int status_code = lpc2000_iap_call(bank, iap_working_area, 50, param_table, result_table);
 		switch (status_code) {
 			case ERROR_FLASH_OPERATION_FAILED:
 				retval = ERROR_FLASH_OPERATION_FAILED;
@@ -674,8 +763,7 @@ static int lpc2000_write(struct flash_bank *bank, uint8_t *buffer, uint32_t offs
 			break;

 		if (bytes_remaining >= thisrun_bytes) {
-			retval = target_write_buffer(bank->target, download_area->address,
-					thisrun_bytes, buffer + bytes_written);
+			retval = target_write_buffer(bank->target, download_area->address, thisrun_bytes, buffer + bytes_written);
 			if (retval != ERROR_OK) {
 				retval = ERROR_FLASH_OPERATION_FAILED;
 				break;
@@ -683,25 +771,20 @@ static int lpc2000_write(struct flash_bank *bank, uint8_t *buffer, uint32_t offs
 		} else {
 			uint8_t *last_buffer = malloc(thisrun_bytes);
 			memcpy(last_buffer, buffer + bytes_written, bytes_remaining);
-			memset(last_buffer + bytes_remaining, 0xff, thisrun_bytes -
-				bytes_remaining);
-			target_write_buffer(bank->target,
-				download_area->address,
-				thisrun_bytes,
-				last_buffer);
+			memset(last_buffer + bytes_remaining, 0xff, thisrun_bytes - bytes_remaining);
+			target_write_buffer(bank->target, download_area->address, thisrun_bytes, last_buffer);
 			free(last_buffer);
 		}

-		LOG_DEBUG("writing 0x%" PRIx32 " bytes to address 0x%" PRIx32,
-			thisrun_bytes,
-			bank->base + offset + bytes_written);
+		LOG_DEBUG("writing 0x%" PRIx32 " bytes to address 0x%" PRIx32, thisrun_bytes,
+				bank->base + offset + bytes_written);

 		/* Write data */
 		param_table[0] = bank->base + offset + bytes_written;
 		param_table[1] = download_area->address;
 		param_table[2] = thisrun_bytes;
 		param_table[3] = lpc2000_info->cclk;
-		status_code = lpc2000_iap_call(bank, 51, param_table, result_table);
+		status_code = lpc2000_iap_call(bank, iap_working_area, 51, param_table, result_table);
 		switch (status_code) {
 			case ERROR_FLASH_OPERATION_FAILED:
 				retval = ERROR_FLASH_OPERATION_FAILED;
@@ -728,6 +811,7 @@ static int lpc2000_write(struct flash_bank *bank, uint8_t *buffer, uint32_t offs
 		bytes_written += thisrun_bytes;
 	}

+	target_free_working_area(target, iap_working_area);
 	target_free_working_area(target, download_area);

 	return retval;
@@ -735,9 +819,7 @@ static int lpc2000_write(struct flash_bank *bank, uint8_t *buffer, uint32_t offs

 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
-	 */
+	/* we can't probe on an lpc2000 if this is an lpc2xxx, it has the configured flash */
 	return ERROR_OK;
 }

@@ -761,21 +843,14 @@ static int get_lpc2000_info(struct flash_bank *bank, char *buf, int buf_size)
 {
 	struct lpc2000_flash_bank *lpc2000_info = bank->driver_priv;

-	snprintf(buf,
-		buf_size,
-		"lpc2000 flash driver variant: %i, clk: %" PRIi32 "kHz",
-		lpc2000_info->variant,
-		lpc2000_info->cclk);
+	snprintf(buf, buf_size, "lpc2000 flash driver variant: %i, clk: %" PRIi32 "kHz", lpc2000_info->variant,
+			lpc2000_info->cclk);

 	return ERROR_OK;
 }

 COMMAND_HANDLER(lpc2000_handle_part_id_command)
 {
-	uint32_t param_table[5] = {0};
-	uint32_t result_table[4];
-	int status_code;
-
 	if (CMD_ARGC < 1)
 		return ERROR_COMMAND_SYNTAX_ERROR;

@@ -789,18 +864,25 @@ COMMAND_HANDLER(lpc2000_handle_part_id_command)
 		return ERROR_TARGET_NOT_HALTED;
 	}

-	status_code = lpc2000_iap_call(bank, 54, param_table, result_table);
+	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);
 	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");
-			return ERROR_OK;
-		}
-		command_print(CMD_CTX, "lpc2000 IAP returned status code %i", status_code);
+			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]);

-	return ERROR_OK;
+	return retval;
 }

 static const struct command_registration lpc2000_exec_command_handlers[] = {
--- a/src/flash/nor/lpcspifi.c
+++ b/src/flash/nor/lpcspifi.c
@@ -0,0 +1,968 @@
+/***************************************************************************
+ *   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.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "imp.h"
+#include "spi.h"
+#include <jtag/jtag.h>
+#include <helper/time_support.h>
+#include <target/algorithm.h>
+#include <target/armv7m.h>
+
+/* Offsets from ssp_base into config & data registers */
+#define SSP_CR0		(0x00)  /* Control register 0 */
+#define SSP_CR1		(0x04)  /* Control register 1 */
+#define SSP_DATA	(0x08)  /* Data register (TX and RX) */
+#define SSP_SR		(0x0C)  /* Status register */
+#define SSP_CPSR	(0x10)  /* Clock prescale register */
+
+/* Status register fields */
+#define SSP_BSY		(0x00000010)
+
+/* Timeout in ms */
+#define SSP_CMD_TIMEOUT   (100)
+#define SSP_PROBE_TIMEOUT (100)
+#define SSP_MAX_TIMEOUT  (3000)
+
+struct lpcspifi_flash_bank {
+	int probed;
+	uint32_t ssp_base;
+	uint32_t io_base;
+	uint32_t ioconfig_base;
+	uint32_t bank_num;
+	uint32_t max_spi_clock_mhz;
+	struct flash_device *dev;
+};
+
+struct lpcspifi_target {
+	char *name;
+	uint32_t tap_idcode;
+	uint32_t spifi_base;
+	uint32_t ssp_base;
+	uint32_t io_base;
+	uint32_t ioconfig_base; /* base address for the port word pin registers */
+};
+
+static struct lpcspifi_target target_devices[] = {
+	/* name,          tap_idcode, spifi_base, ssp_base,   io_base,    ioconfig_base */
+	{ "LPC43xx/18xx", 0x4ba00477, 0x14000000, 0x40083000, 0x400F4000, 0x40086000 },
+	{ NULL,           0,          0,          0,          0,          0 }
+};
+
+/* flash_bank lpcspifi <base> <size> <chip_width> <bus_width> <target>
+ */
+FLASH_BANK_COMMAND_HANDLER(lpcspifi_flash_bank_command)
+{
+	struct lpcspifi_flash_bank *lpcspifi_info;
+
+	if (CMD_ARGC < 6)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	lpcspifi_info = malloc(sizeof(struct lpcspifi_flash_bank));
+	if (lpcspifi_info == NULL) {
+		LOG_ERROR("not enough memory");
+		return ERROR_FAIL;
+	}
+
+	bank->driver_priv = lpcspifi_info;
+	lpcspifi_info->probed = 0;
+
+	return ERROR_OK;
+}
+
+static inline int ioconfig_write_reg(struct target *target, uint32_t ioconfig_base, uint32_t offset, uint32_t value)
+{
+	return target_write_u32(target, ioconfig_base + offset, value);
+}
+
+static inline int ssp_write_reg(struct target *target, uint32_t ssp_base, uint32_t offset, uint32_t value)
+{
+	return target_write_u32(target, ssp_base + offset, value);
+}
+
+static inline int io_write_reg(struct target *target, uint32_t io_base, uint32_t offset, uint32_t value)
+{
+	return target_write_u32(target, io_base + offset, value);
+}
+
+static inline int ssp_read_reg(struct target *target, uint32_t ssp_base, uint32_t offset, uint32_t *value)
+{
+	return target_read_u32(target, ssp_base + offset, value);
+}
+
+static int ssp_setcs(struct target *target, uint32_t io_base, unsigned int value)
+{
+	return io_write_reg(target, io_base, 0x12ac, value ? 0xffffffff : 0x00000000);
+}
+
+/* Poll the SSP busy flag. When this comes back as 0, the transfer is complete
+ * and the controller is idle. */
+static int poll_ssp_busy(struct target *target, uint32_t ssp_base, int timeout)
+{
+	long long endtime;
+	uint32_t value;
+	int retval;
+
+	retval = ssp_read_reg(target, ssp_base, SSP_SR, &value);
+	if ((retval == ERROR_OK) && (value & SSP_BSY) == 0)
+		return ERROR_OK;
+	else if (retval != ERROR_OK)
+		return retval;
+
+	endtime = timeval_ms() + timeout;
+	do {
+		alive_sleep(1);
+		retval = ssp_read_reg(target, ssp_base, SSP_SR, &value);
+		if ((retval == ERROR_OK) && (value & SSP_BSY) == 0)
+			return ERROR_OK;
+		else if (retval != ERROR_OK)
+			return retval;
+	} while (timeval_ms() < endtime);
+
+	LOG_ERROR("Timeout while polling BSY");
+	return ERROR_FLASH_OPERATION_FAILED;
+}
+
+/* Un-initialize the ssp module and initialize the SPIFI module */
+static int lpcspifi_set_hw_mode(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	struct lpcspifi_flash_bank *lpcspifi_info = bank->driver_priv;
+	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];
+	int retval = ERROR_OK;
+
+	LOG_DEBUG("Uninitializing LPC43xx SSP");
+	/* Turn off the SSP module */
+	retval = ssp_write_reg(target, ssp_base, SSP_CR1, 0x00000000);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* see contrib/loaders/flash/lpcspifi_init.S for src */
+	static const uint8_t spifi_init_code[] = {
+		0x4f, 0xea, 0x00, 0x08, 0xa1, 0xb0, 0x00, 0xaf,
+		0x4f, 0xf4, 0xc0, 0x43, 0xc4, 0xf2, 0x08, 0x03,
+		0x4f, 0xf0, 0xf3, 0x02, 0xc3, 0xf8, 0x8c, 0x21,
+		0x4f, 0xf4, 0xc0, 0x43, 0xc4, 0xf2, 0x08, 0x03,
+		0x4f, 0xf4, 0xc0, 0x42, 0xc4, 0xf2, 0x08, 0x02,
+		0x4f, 0xf4, 0xc0, 0x41, 0xc4, 0xf2, 0x08, 0x01,
+		0x4f, 0xf4, 0xc0, 0x40, 0xc4, 0xf2, 0x08, 0x00,
+		0x4f, 0xf0, 0xd3, 0x04, 0xc0, 0xf8, 0x9c, 0x41,
+		0x20, 0x46, 0xc1, 0xf8, 0x98, 0x01, 0x01, 0x46,
+		0xc2, 0xf8, 0x94, 0x11, 0xc3, 0xf8, 0x90, 0x11,
+		0x4f, 0xf4, 0xc0, 0x43, 0xc4, 0xf2, 0x08, 0x03,
+		0x4f, 0xf0, 0x13, 0x02, 0xc3, 0xf8, 0xa0, 0x21,
+		0x40, 0xf2, 0x18, 0x13, 0xc1, 0xf2, 0x40, 0x03,
+		0x1b, 0x68, 0x1c, 0x68, 0x40, 0xf2, 0xb4, 0x30,
+		0xc1, 0xf2, 0x00, 0x00, 0x4f, 0xf0, 0x03, 0x01,
+		0x4f, 0xf0, 0xc0, 0x02, 0x4f, 0xea, 0x08, 0x03,
+		0xa0, 0x47, 0x00, 0xf0, 0x00, 0xb8, 0x00, 0xbe
+	};
+
+	armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
+	armv7m_info.core_mode = ARM_MODE_THREAD;
+
+
+	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);
+	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)
+		);
+
+		return retval;
+	}
+
+	LOG_DEBUG("Writing algorithm to working area at 0x%08x",
+		spifi_init_algorithm->address);
+	/* Write algorithm to working area */
+	retval = target_write_buffer(target,
+		spifi_init_algorithm->address,
+		sizeof(spifi_init_code),
+		spifi_init_code
+	);
+
+	if (retval != ERROR_OK) {
+		target_free_working_area(target, spifi_init_algorithm);
+		return retval;
+	}
+
+	init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);		/* spifi clk speed */
+
+	/* For now, the algorithm will set up the SPIFI module
+	 * @ the IRC clock speed. In the future, it could be made
+	 * a bit smarter to use other clock sources if the user has
+	 * already configured them in order to speed up memory-
+	 * mapped reads. */
+	buf_set_u32(reg_params[0].value, 0, 32, 12);
+
+	/* Run the algorithm */
+	LOG_DEBUG("Running SPIFI init algorithm");
+	retval = target_run_algorithm(target, 0 , NULL, 1, reg_params,
+		spifi_init_algorithm->address,
+		spifi_init_algorithm->address + sizeof(spifi_init_code) - 2,
+		1000, &armv7m_info);
+
+	if (retval != ERROR_OK)
+		LOG_ERROR("Error executing SPIFI init algorithm");
+
+	target_free_working_area(target, spifi_init_algorithm);
+
+	destroy_reg_param(&reg_params[0]);
+
+	return retval;
+}
+
+/* Initialize the ssp module */
+static int lpcspifi_set_sw_mode(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	struct lpcspifi_flash_bank *lpcspifi_info = bank->driver_priv;
+	uint32_t ssp_base = lpcspifi_info->ssp_base;
+	uint32_t io_base = lpcspifi_info->io_base;
+	uint32_t ioconfig_base = lpcspifi_info->ioconfig_base;
+	int retval = ERROR_OK;
+
+	/* Re-initialize SPIFI. There are a couple of errata on this, so this makes
+	sure that nothing's in an unhappy state. */
+	retval = lpcspifi_set_hw_mode(bank);
+
+	/* If we couldn't initialize hardware mode, don't even bother continuing */
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Initialize the pins */
+	retval = ioconfig_write_reg(target, ioconfig_base, 0x194, 0x00000040);
+	if (retval == ERROR_OK)
+		retval = ioconfig_write_reg(target, ioconfig_base, 0x1a0, 0x00000044);
+	if (retval == ERROR_OK)
+		retval = ioconfig_write_reg(target, ioconfig_base, 0x190, 0x00000040);
+	if (retval == ERROR_OK)
+		retval = ioconfig_write_reg(target, ioconfig_base, 0x19c, 0x000000ed);
+	if (retval == ERROR_OK)
+		retval = ioconfig_write_reg(target, ioconfig_base, 0x198, 0x000000ed);
+	if (retval == ERROR_OK)
+		retval = ioconfig_write_reg(target, ioconfig_base, 0x18c, 0x000000ea);
+
+	/* Set CS high & as an output */
+	if (retval == ERROR_OK)
+		retval = io_write_reg(target, io_base, 0x12ac, 0xffffffff);
+	if (retval == ERROR_OK)
+		retval = io_write_reg(target, io_base, 0x2014, 0x00000800);
+
+	/* Initialize the module */
+	if (retval == ERROR_OK)
+		retval = ssp_write_reg(target, ssp_base, SSP_CR0, 0x00000007);
+	if (retval == ERROR_OK)
+		retval = ssp_write_reg(target, ssp_base, SSP_CR1, 0x00000000);
+	if (retval == ERROR_OK)
+		retval = ssp_write_reg(target, ssp_base, SSP_CPSR, 0x00000008);
+	if (retval == ERROR_OK)
+		retval = ssp_write_reg(target, ssp_base, SSP_CR1, 0x00000002);
+
+	/* If something didn't work out, attempt to return SPIFI to HW mode */
+	if (retval != ERROR_OK)
+		lpcspifi_set_hw_mode(bank);
+
+	return retval;
+}
+
+/* Read the status register of the external SPI flash chip. */
+static int read_status_reg(struct flash_bank *bank, uint32_t *status)
+{
+	struct target *target = bank->target;
+	struct lpcspifi_flash_bank *lpcspifi_info = bank->driver_priv;
+	uint32_t ssp_base = lpcspifi_info->ssp_base;
+	uint32_t io_base = lpcspifi_info->io_base;
+	uint32_t value;
+	int retval = ERROR_OK;
+
+	retval = ssp_setcs(target, io_base, 0);
+	if (retval == ERROR_OK)
+		retval = ssp_write_reg(target, ssp_base, SSP_DATA, SPIFLASH_READ_STATUS);
+	if (retval == ERROR_OK)
+		retval = poll_ssp_busy(target, ssp_base, SSP_CMD_TIMEOUT);
+	if (retval == ERROR_OK)
+		retval = ssp_read_reg(target, ssp_base, SSP_DATA, &value);
+	/* Dummy write to clock in the register */
+	if (retval == ERROR_OK)
+		retval = ssp_write_reg(target, ssp_base, SSP_DATA, 0x00);
+	if (retval == ERROR_OK)
+		retval = poll_ssp_busy(target, ssp_base, SSP_CMD_TIMEOUT);
+	if (retval == ERROR_OK)
+		retval = ssp_setcs(target, io_base, 1);
+
+	if (retval == ERROR_OK)
+		retval = ssp_read_reg(target, ssp_base, SSP_DATA, &value);
+	if (retval == ERROR_OK)
+		*status = value;
+
+	return retval;
+}
+
+/* check for BSY bit in flash status register */
+/* timeout in ms */
+static int wait_till_ready(struct flash_bank *bank, int timeout)
+{
+	uint32_t status;
+	int retval;
+	long long endtime;
+
+	endtime = timeval_ms() + timeout;
+	do {
+		/* read flash status register */
+		retval = read_status_reg(bank, &status);
+		if (retval != ERROR_OK)
+			return retval;
+
+		if ((status & SPIFLASH_BSY_BIT) == 0)
+			return ERROR_OK;
+		alive_sleep(1);
+	} while (timeval_ms() < endtime);
+
+	LOG_ERROR("timeout waiting for flash to finish write/erase operation");
+	return ERROR_FAIL;
+}
+
+/* Send "write enable" command to SPI flash chip. */
+static int lpcspifi_write_enable(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	struct lpcspifi_flash_bank *lpcspifi_info = bank->driver_priv;
+	uint32_t ssp_base = lpcspifi_info->ssp_base;
+	uint32_t io_base = lpcspifi_info->io_base;
+	uint32_t status, value;
+	int retval = ERROR_OK;
+
+	retval = ssp_setcs(target, io_base, 0);
+	if (retval == ERROR_OK)
+		retval = ssp_write_reg(target, ssp_base, SSP_DATA, SPIFLASH_WRITE_ENABLE);
+	if (retval == ERROR_OK)
+		retval = poll_ssp_busy(target, ssp_base, SSP_CMD_TIMEOUT);
+	if (retval == ERROR_OK)
+		retval = ssp_read_reg(target, ssp_base, SSP_DATA, &value);
+	if (retval == ERROR_OK)
+		retval = ssp_setcs(target, io_base, 1);
+
+	/* read flash status register */
+	if (retval == ERROR_OK)
+		retval = read_status_reg(bank, &status);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Check write enabled */
+	if ((status & SPIFLASH_WE_BIT) == 0) {
+		LOG_ERROR("Cannot enable write to flash. Status=0x%08" PRIx32, status);
+		return ERROR_FAIL;
+	}
+
+	return retval;
+}
+
+static int lpcspifi_bulk_erase(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	struct lpcspifi_flash_bank *lpcspifi_info = bank->driver_priv;
+	uint32_t ssp_base = lpcspifi_info->ssp_base;
+	uint32_t io_base = lpcspifi_info->io_base;
+	uint32_t value;
+	int retval = ERROR_OK;
+
+	retval = lpcspifi_set_sw_mode(bank);
+
+	if (retval == ERROR_OK)
+		retval = lpcspifi_write_enable(bank);
+
+	/* send SPI command "bulk erase" */
+	if (retval == ERROR_OK)
+		ssp_setcs(target, io_base, 0);
+	if (retval == ERROR_OK)
+		retval = ssp_write_reg(target, ssp_base, SSP_DATA, lpcspifi_info->dev->chip_erase_cmd);
+	if (retval == ERROR_OK)
+		retval = poll_ssp_busy(target, ssp_base, SSP_CMD_TIMEOUT);
+	if (retval == ERROR_OK)
+		retval = ssp_read_reg(target, ssp_base, SSP_DATA, &value);
+	if (retval == ERROR_OK)
+		retval = ssp_setcs(target, io_base, 1);
+
+	/* poll flash BSY for self-timed bulk erase */
+	if (retval == ERROR_OK)
+		retval = wait_till_ready(bank, bank->num_sectors*SSP_MAX_TIMEOUT);
+
+	return retval;
+}
+
+static int lpcspifi_erase(struct flash_bank *bank, int first, int last)
+{
+	struct target *target = bank->target;
+	struct lpcspifi_flash_bank *lpcspifi_info = bank->driver_priv;
+	struct reg_param reg_params[4];
+	struct armv7m_algorithm armv7m_info;
+	struct working_area *erase_algorithm;
+	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 (!(lpcspifi_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)
+		&& lpcspifi_info->dev->chip_erase_cmd != lpcspifi_info->dev->erase_cmd) {
+		LOG_DEBUG("Chip supports the bulk erase command."\
+		" Will use bulk erase instead of sector-by-sector erase.");
+		retval = lpcspifi_bulk_erase(bank);
+
+		if (retval == ERROR_OK) {
+			retval = lpcspifi_set_hw_mode(bank);
+			return retval;
+		} else
+			LOG_WARNING("Bulk flash erase failed. Falling back to sector-by-sector erase.");
+	}
+
+	retval = lpcspifi_set_hw_mode(bank);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* see contrib/loaders/flash/lpcspifi_erase.S for src */
+	static const uint8_t lpcspifi_flash_erase_code[] = {
+		0x4f, 0xf4, 0xc0, 0x4a, 0xc4, 0xf2, 0x08, 0x0a,
+		0x4f, 0xf0, 0xea, 0x08, 0xca, 0xf8, 0x8c, 0x81,
+		0x4f, 0xf0, 0x40, 0x08, 0xca, 0xf8, 0x90, 0x81,
+		0x4f, 0xf0, 0x40, 0x08, 0xca, 0xf8, 0x94, 0x81,
+		0x4f, 0xf0, 0xed, 0x08, 0xca, 0xf8, 0x98, 0x81,
+		0x4f, 0xf0, 0xed, 0x08, 0xca, 0xf8, 0x9c, 0x81,
+		0x4f, 0xf0, 0x44, 0x08, 0xca, 0xf8, 0xa0, 0x81,
+		0x4f, 0xf4, 0xc0, 0x4a, 0xc4, 0xf2, 0x0f, 0x0a,
+		0x4f, 0xf4, 0x00, 0x68, 0xca, 0xf8, 0x14, 0x80,
+		0x4f, 0xf4, 0x80, 0x4a, 0xc4, 0xf2, 0x0f, 0x0a,
+		0x4f, 0xf0, 0xff, 0x08, 0xca, 0xf8, 0xab, 0x80,
+		0x4f, 0xf0, 0x00, 0x0a, 0xc4, 0xf2, 0x05, 0x0a,
+		0x4f, 0xf0, 0x00, 0x08, 0xc0, 0xf2, 0x00, 0x18,
+		0xca, 0xf8, 0x94, 0x80, 0x4f, 0xf4, 0x00, 0x5a,
+		0xc4, 0xf2, 0x05, 0x0a, 0x4f, 0xf0, 0x01, 0x08,
+		0xca, 0xf8, 0x00, 0x87, 0x4f, 0xf4, 0x40, 0x5a,
+		0xc4, 0xf2, 0x08, 0x0a, 0x4f, 0xf0, 0x07, 0x08,
+		0xca, 0xf8, 0x00, 0x80, 0x4f, 0xf0, 0x02, 0x08,
+		0xca, 0xf8, 0x10, 0x80, 0xca, 0xf8, 0x04, 0x80,
+		0x00, 0xf0, 0x52, 0xf8, 0x4f, 0xf0, 0x06, 0x09,
+		0x00, 0xf0, 0x3b, 0xf8, 0x00, 0xf0, 0x48, 0xf8,
+		0x00, 0xf0, 0x4a, 0xf8, 0x4f, 0xf0, 0x05, 0x09,
+		0x00, 0xf0, 0x33, 0xf8, 0x4f, 0xf0, 0x00, 0x09,
+		0x00, 0xf0, 0x2f, 0xf8, 0x00, 0xf0, 0x3c, 0xf8,
+		0x19, 0xf0, 0x02, 0x0f, 0x00, 0xf0, 0x45, 0x80,
+		0x00, 0xf0, 0x3a, 0xf8, 0x4f, 0xea, 0x02, 0x09,
+		0x00, 0xf0, 0x23, 0xf8, 0x4f, 0xea, 0x10, 0x49,
+		0x00, 0xf0, 0x1f, 0xf8, 0x4f, 0xea, 0x10, 0x29,
+		0x00, 0xf0, 0x1b, 0xf8, 0x4f, 0xea, 0x00, 0x09,
+		0x00, 0xf0, 0x17, 0xf8, 0x00, 0xf0, 0x24, 0xf8,
+		0x00, 0xf0, 0x26, 0xf8, 0x4f, 0xf0, 0x05, 0x09,
+		0x00, 0xf0, 0x0f, 0xf8, 0x4f, 0xf0, 0x00, 0x09,
+		0x00, 0xf0, 0x0b, 0xf8, 0x00, 0xf0, 0x18, 0xf8,
+		0x19, 0xf0, 0x01, 0x0f, 0x7f, 0xf4, 0xf0, 0xaf,
+		0x01, 0x39, 0xf9, 0xb1, 0x18, 0x44, 0xff, 0xf7,
+		0xbf, 0xbf, 0x4f, 0xf4, 0x40, 0x5a, 0xc4, 0xf2,
+		0x08, 0x0a, 0xca, 0xf8, 0x08, 0x90, 0xda, 0xf8,
+		0x0c, 0x90, 0x19, 0xf0, 0x10, 0x0f, 0x7f, 0xf4,
+		0xfa, 0xaf, 0xda, 0xf8, 0x08, 0x90, 0x70, 0x47,
+		0x4f, 0xf0, 0xff, 0x08, 0x00, 0xf0, 0x02, 0xb8,
+		0x4f, 0xf0, 0x00, 0x08, 0x4f, 0xf4, 0x80, 0x4a,
+		0xc4, 0xf2, 0x0f, 0x0a, 0xca, 0xf8, 0xab, 0x80,
+		0x70, 0x47, 0x00, 0x20, 0x00, 0xbe, 0xff, 0xff
+	};
+
+	armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
+	armv7m_info.core_mode = ARM_MODE_THREAD;
+
+
+	/* Get memory for spifi initialization algorithm */
+	retval = target_alloc_working_area(target, sizeof(lpcspifi_flash_erase_code),
+		&erase_algorithm);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("Insufficient working area. You must configure a working"\
+			" area of at least %zdB in order to erase SPIFI flash.",
+			sizeof(lpcspifi_flash_erase_code));
+		return retval;
+	}
+
+	/* Write algorithm to working area */
+	retval = target_write_buffer(target, erase_algorithm->address,
+		sizeof(lpcspifi_flash_erase_code), lpcspifi_flash_erase_code);
+	if (retval != ERROR_OK) {
+		target_free_working_area(target, erase_algorithm);
+		return retval;
+	}
+
+	init_reg_param(&reg_params[0], "r0", 32, PARAM_IN_OUT);	/* Start address */
+	init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);	/* Sector count */
+	init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);	/* Erase command */
+	init_reg_param(&reg_params[3], "r3", 32, PARAM_OUT);	/* Sector size */
+
+	buf_set_u32(reg_params[0].value, 0, 32, bank->sectors[first].offset);
+	buf_set_u32(reg_params[1].value, 0, 32, last - first + 1);
+	buf_set_u32(reg_params[2].value, 0, 32, lpcspifi_info->dev->erase_cmd);
+	buf_set_u32(reg_params[3].value, 0, 32, bank->sectors[first].size);
+
+	/* Run the algorithm */
+	retval = target_run_algorithm(target, 0 , NULL, 4, reg_params,
+		erase_algorithm->address,
+		erase_algorithm->address + sizeof(lpcspifi_flash_erase_code) - 4,
+		3000*(last - first + 1), &armv7m_info);
+
+	if (retval != ERROR_OK)
+		LOG_ERROR("Error executing flash erase algorithm");
+
+	target_free_working_area(target, erase_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]);
+
+	retval = lpcspifi_set_hw_mode(bank);
+
+	return retval;
+}
+
+static int lpcspifi_protect(struct flash_bank *bank, int set,
+	int first, int last)
+{
+	int sector;
+
+	for (sector = first; sector <= last; sector++)
+		bank->sectors[sector].is_protected = set;
+	return ERROR_OK;
+}
+
+static int lpcspifi_write(struct flash_bank *bank, uint8_t *buffer,
+	uint32_t offset, uint32_t count)
+{
+	struct target *target = bank->target;
+	struct lpcspifi_flash_bank *lpcspifi_info = bank->driver_priv;
+	uint32_t page_size, fifo_size;
+	struct working_area *fifo;
+	struct reg_param reg_params[5];
+	struct armv7m_algorithm armv7m_info;
+	struct working_area *write_algorithm;
+	int sector;
+	int retval = ERROR_OK;
+
+	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 > lpcspifi_info->dev->size_in_bytes) {
+		LOG_WARNING("Writes past end of flash. Extra data discarded.");
+		count = lpcspifi_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 = lpcspifi_info->dev->pagesize;
+
+	retval = lpcspifi_set_hw_mode(bank);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* see contrib/loaders/flash/lpcspifi_write.S for src */
+	static const uint8_t lpcspifi_flash_write_code[] = {
+		0x4f, 0xf4, 0xc0, 0x4a, 0xc4, 0xf2, 0x08, 0x0a,
+		0x4f, 0xf0, 0xea, 0x08, 0xca, 0xf8, 0x8c, 0x81,
+		0x4f, 0xf0, 0x40, 0x08, 0xca, 0xf8, 0x90, 0x81,
+		0x4f, 0xf0, 0x40, 0x08, 0xca, 0xf8, 0x94, 0x81,
+		0x4f, 0xf0, 0xed, 0x08, 0xca, 0xf8, 0x98, 0x81,
+		0x4f, 0xf0, 0xed, 0x08, 0xca, 0xf8, 0x9c, 0x81,
+		0x4f, 0xf0, 0x44, 0x08, 0xca, 0xf8, 0xa0, 0x81,
+		0x4f, 0xf4, 0xc0, 0x4a, 0xc4, 0xf2, 0x0f, 0x0a,
+		0x4f, 0xf4, 0x00, 0x68, 0xca, 0xf8, 0x14, 0x80,
+		0x4f, 0xf4, 0x80, 0x4a, 0xc4, 0xf2, 0x0f, 0x0a,
+		0x4f, 0xf0, 0xff, 0x08, 0xca, 0xf8, 0xab, 0x80,
+		0x4f, 0xf0, 0x00, 0x0a, 0xc4, 0xf2, 0x05, 0x0a,
+		0x4f, 0xf0, 0x00, 0x08, 0xc0, 0xf2, 0x00, 0x18,
+		0xca, 0xf8, 0x94, 0x80, 0x4f, 0xf4, 0x00, 0x5a,
+		0xc4, 0xf2, 0x05, 0x0a, 0x4f, 0xf0, 0x01, 0x08,
+		0xca, 0xf8, 0x00, 0x87, 0x4f, 0xf4, 0x40, 0x5a,
+		0xc4, 0xf2, 0x08, 0x0a, 0x4f, 0xf0, 0x07, 0x08,
+		0xca, 0xf8, 0x00, 0x80, 0x4f, 0xf0, 0x02, 0x08,
+		0xca, 0xf8, 0x10, 0x80, 0xca, 0xf8, 0x04, 0x80,
+		0x4f, 0xf0, 0x00, 0x0b, 0xa3, 0x44, 0x93, 0x45,
+		0x7f, 0xf6, 0xfc, 0xaf, 0x00, 0xf0, 0x6a, 0xf8,
+		0x4f, 0xf0, 0x06, 0x09, 0x00, 0xf0, 0x53, 0xf8,
+		0x00, 0xf0, 0x60, 0xf8, 0x00, 0xf0, 0x62, 0xf8,
+		0x4f, 0xf0, 0x05, 0x09, 0x00, 0xf0, 0x4b, 0xf8,
+		0x4f, 0xf0, 0x00, 0x09, 0x00, 0xf0, 0x47, 0xf8,
+		0x00, 0xf0, 0x54, 0xf8, 0x19, 0xf0, 0x02, 0x0f,
+		0x00, 0xf0, 0x5d, 0x80, 0x00, 0xf0, 0x52, 0xf8,
+		0x4f, 0xf0, 0x02, 0x09, 0x00, 0xf0, 0x3b, 0xf8,
+		0x4f, 0xea, 0x12, 0x49, 0x00, 0xf0, 0x37, 0xf8,
+		0x4f, 0xea, 0x12, 0x29, 0x00, 0xf0, 0x33, 0xf8,
+		0x4f, 0xea, 0x02, 0x09, 0x00, 0xf0, 0x2f, 0xf8,
+		0xd0, 0xf8, 0x00, 0x80, 0xb8, 0xf1, 0x00, 0x0f,
+		0x00, 0xf0, 0x47, 0x80, 0x47, 0x68, 0x47, 0x45,
+		0x3f, 0xf4, 0xf6, 0xaf, 0x17, 0xf8, 0x01, 0x9b,
+		0x00, 0xf0, 0x21, 0xf8, 0x8f, 0x42, 0x28, 0xbf,
+		0x00, 0xf1, 0x08, 0x07, 0x47, 0x60, 0x01, 0x3b,
+		0xbb, 0xb3, 0x02, 0xf1, 0x01, 0x02, 0x93, 0x45,
+		0x7f, 0xf4, 0xe6, 0xaf, 0x00, 0xf0, 0x22, 0xf8,
+		0xa3, 0x44, 0x00, 0xf0, 0x23, 0xf8, 0x4f, 0xf0,
+		0x05, 0x09, 0x00, 0xf0, 0x0c, 0xf8, 0x4f, 0xf0,
+		0x00, 0x09, 0x00, 0xf0, 0x08, 0xf8, 0x00, 0xf0,
+		0x15, 0xf8, 0x19, 0xf0, 0x01, 0x0f, 0x7f, 0xf4,
+		0xf0, 0xaf, 0xff, 0xf7, 0xa7, 0xbf, 0x4f, 0xf4,
+		0x40, 0x5a, 0xc4, 0xf2, 0x08, 0x0a, 0xca, 0xf8,
+		0x08, 0x90, 0xda, 0xf8, 0x0c, 0x90, 0x19, 0xf0,
+		0x10, 0x0f, 0x7f, 0xf4, 0xfa, 0xaf, 0xda, 0xf8,
+		0x08, 0x90, 0x70, 0x47, 0x4f, 0xf0, 0xff, 0x08,
+		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
+	};
+
+	if (target_alloc_working_area(target, sizeof(lpcspifi_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(lpcspifi_flash_write_code));
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	};
+
+	retval = target_write_buffer(target, write_algorithm->address,
+			sizeof(lpcspifi_flash_write_code),
+			lpcspifi_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(lpcspifi_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.",
+			sizeof(lpcspifi_flash_write_code) + page_size
+		);
+	else if (fifo_size > 0x2000) /* Beyond this point, we start to get diminishing returns */
+		fifo_size = 0x2000;
+
+	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 */
+
+	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);
+
+	retval = target_run_flash_async_algorithm(target, buffer, count, 1,
+			0, NULL,
+			5, 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]);
+
+	/* Switch to HW mode before return to prompt */
+	retval = lpcspifi_set_hw_mode(bank);
+	return retval;
+}
+
+/* Return ID of flash device */
+/* On exit, SW mode is kept */
+static int lpcspifi_read_flash_id(struct flash_bank *bank, uint32_t *id)
+{
+	struct target *target = bank->target;
+	struct lpcspifi_flash_bank *lpcspifi_info = bank->driver_priv;
+	uint32_t ssp_base = lpcspifi_info->ssp_base;
+	uint32_t io_base = lpcspifi_info->io_base;
+	uint32_t value;
+	int retval;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	LOG_DEBUG("Getting ID");
+	retval = lpcspifi_set_sw_mode(bank);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* poll WIP */
+	if (retval == ERROR_OK)
+		retval = wait_till_ready(bank, SSP_PROBE_TIMEOUT);
+
+	/* Send SPI command "read ID" */
+	if (retval == ERROR_OK)
+		retval = ssp_setcs(target, io_base, 0);
+	if (retval == ERROR_OK)
+		retval = ssp_write_reg(target, ssp_base, SSP_DATA, SPIFLASH_READ_ID);
+	if (retval == ERROR_OK)
+		retval = poll_ssp_busy(target, ssp_base, SSP_CMD_TIMEOUT);
+	if (retval == ERROR_OK)
+		retval = ssp_read_reg(target, ssp_base, SSP_DATA, &value);
+
+	/* Dummy write to clock in data */
+	if (retval == ERROR_OK)
+		retval = ssp_write_reg(target, ssp_base, SSP_DATA, 0x00);
+	if (retval == ERROR_OK)
+		retval = poll_ssp_busy(target, ssp_base, SSP_CMD_TIMEOUT);
+	if (retval == ERROR_OK)
+		retval = ssp_read_reg(target, ssp_base, SSP_DATA, &value);
+	if (retval == ERROR_OK)
+		((uint8_t *)id)[0] = value;
+
+	/* Dummy write to clock in data */
+	if (retval == ERROR_OK)
+		retval = ssp_write_reg(target, ssp_base, SSP_DATA, 0x00);
+	if (retval == ERROR_OK)
+		retval = poll_ssp_busy(target, ssp_base, SSP_CMD_TIMEOUT);
+	if (retval == ERROR_OK)
+		retval = ssp_read_reg(target, ssp_base, SSP_DATA, &value);
+	if (retval == ERROR_OK)
+		((uint8_t *)id)[1] = value;
+
+	/* Dummy write to clock in data */
+	if (retval == ERROR_OK)
+		retval = ssp_write_reg(target, ssp_base, SSP_DATA, 0x00);
+	if (retval == ERROR_OK)
+		retval = poll_ssp_busy(target, ssp_base, SSP_CMD_TIMEOUT);
+	if (retval == ERROR_OK)
+		retval = ssp_read_reg(target, ssp_base, SSP_DATA, &value);
+	if (retval == ERROR_OK)
+		((uint8_t *)id)[2] = value;
+
+	if (retval == ERROR_OK)
+		retval = ssp_setcs(target, io_base, 1);
+
+	return retval;
+}
+
+static int lpcspifi_probe(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	struct lpcspifi_flash_bank *lpcspifi_info = bank->driver_priv;
+	uint32_t ssp_base;
+	uint32_t io_base;
+	uint32_t ioconfig_base;
+	struct flash_sector *sectors;
+	uint32_t id = 0; /* silence uninitialized warning */
+	struct lpcspifi_target *target_device;
+	int retval;
+
+	/* If we've already probed, we should be fine to skip this time. */
+	if (lpcspifi_info->probed)
+		return ERROR_OK;
+	lpcspifi_info->probed = 0;
+
+	for (target_device = target_devices ; target_device->name ; ++target_device)
+		if (target_device->tap_idcode == target->tap->idcode)
+			break;
+	if (!target_device->name) {
+		LOG_ERROR("Device ID 0x%" PRIx32 " is not known as SPIFI capable",
+				target->tap->idcode);
+		return ERROR_FAIL;
+	}
+
+	ssp_base = target_device->ssp_base;
+	io_base = target_device->io_base;
+	ioconfig_base = target_device->ioconfig_base;
+	lpcspifi_info->ssp_base = ssp_base;
+	lpcspifi_info->io_base = io_base;
+	lpcspifi_info->ioconfig_base = ioconfig_base;
+	lpcspifi_info->bank_num = bank->bank_number;
+
+	LOG_DEBUG("Valid SPIFI on device %s at address 0x%" PRIx32,
+		target_device->name, bank->base);
+
+	/* read and decode flash ID; returns in SW mode */
+	retval = lpcspifi_read_flash_id(bank, &id);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = lpcspifi_set_hw_mode(bank);
+	if (retval != ERROR_OK)
+		return retval;
+
+	lpcspifi_info->dev = NULL;
+	for (struct flash_device *p = flash_devices; p->name ; p++)
+		if (p->device_id == id) {
+			lpcspifi_info->dev = p;
+			break;
+		}
+
+	if (!lpcspifi_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 ")",
+		lpcspifi_info->dev->name, lpcspifi_info->dev->device_id);
+
+	/* Set correct size value */
+	bank->size = lpcspifi_info->dev->size_in_bytes;
+
+	/* create and fill sectors array */
+	bank->num_sectors =
+		lpcspifi_info->dev->size_in_bytes / lpcspifi_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 * lpcspifi_info->dev->sectorsize;
+		sectors[sector].size = lpcspifi_info->dev->sectorsize;
+		sectors[sector].is_erased = -1;
+		sectors[sector].is_protected = 1;
+	}
+
+	bank->sectors = sectors;
+
+	lpcspifi_info->probed = 1;
+	return ERROR_OK;
+}
+
+static int lpcspifi_auto_probe(struct flash_bank *bank)
+{
+	struct lpcspifi_flash_bank *lpcspifi_info = bank->driver_priv;
+	if (lpcspifi_info->probed)
+		return ERROR_OK;
+	return lpcspifi_probe(bank);
+}
+
+static int lpcspifi_protect_check(struct flash_bank *bank)
+{
+	/* Nothing to do. Protection is only handled in SW. */
+	return ERROR_OK;
+}
+
+static int get_lpcspifi_info(struct flash_bank *bank, char *buf, int buf_size)
+{
+	struct lpcspifi_flash_bank *lpcspifi_info = bank->driver_priv;
+
+	if (!(lpcspifi_info->probed)) {
+		snprintf(buf, buf_size,
+			"\nSPIFI flash bank not probed yet\n");
+		return ERROR_OK;
+	}
+
+	snprintf(buf, buf_size, "\nSPIFI flash information:\n"
+		"  Device \'%s\' (ID 0x%08x)\n",
+		lpcspifi_info->dev->name, lpcspifi_info->dev->device_id);
+
+	return ERROR_OK;
+}
+
+struct flash_driver lpcspifi_flash = {
+	.name = "lpcspifi",
+	.flash_bank_command = lpcspifi_flash_bank_command,
+	.erase = lpcspifi_erase,
+	.protect = lpcspifi_protect,
+	.write = lpcspifi_write,
+	.read = default_flash_read,
+	.probe = lpcspifi_probe,
+	.auto_probe = lpcspifi_auto_probe,
+	.erase_check = default_flash_blank_check,
+	.protect_check = lpcspifi_protect_check,
+	.info = get_lpcspifi_info,
+};
--- a/src/flash/nor/pic32mx.c
+++ b/src/flash/nor/pic32mx.c
@@ -96,7 +96,6 @@
 #define MX_1_2			1	/* PIC32mx1xx/2xx */

 struct pic32mx_flash_bank {
-	struct working_area *write_algorithm;
 	int probed;
 	int dev_type;		/* Default 0. 1 for Pic32MX1XX/2XX variant */
 };
@@ -193,7 +192,6 @@ FLASH_BANK_COMMAND_HANDLER(pic32mx_flash_bank_command)
 	pic32mx_info = malloc(sizeof(struct pic32mx_flash_bank));
 	bank->driver_priv = pic32mx_info;

-	pic32mx_info->write_algorithm = NULL;
 	pic32mx_info->probed = 0;
 	pic32mx_info->dev_type = 0;

@@ -417,6 +415,7 @@ static int pic32mx_write_block(struct flash_bank *bank, uint8_t *buffer,
 {
 	struct target *target = bank->target;
 	uint32_t buffer_size = 16384;
+	struct working_area *write_algorithm;
 	struct working_area *source;
 	uint32_t address = bank->base + offset;
 	struct reg_param reg_params[3];
@@ -428,7 +427,7 @@ static int pic32mx_write_block(struct flash_bank *bank, uint8_t *buffer,

 	/* flash write code */
 	if (target_alloc_working_area(target, sizeof(pic32mx_flash_write_code),
-			&pic32mx_info->write_algorithm) != ERROR_OK) {
+			&write_algorithm) != ERROR_OK) {
 		LOG_WARNING("no working area available, can't do block memory writes");
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 	};
@@ -450,7 +449,7 @@ static int pic32mx_write_block(struct flash_bank *bank, uint8_t *buffer,
 		row_size = 512;
 	}

-	retval = target_write_buffer(target, pic32mx_info->write_algorithm->address,
+	retval = target_write_buffer(target, write_algorithm->address,
 			sizeof(pic32mx_flash_write_code), (uint8_t *)pic32mx_flash_write_code);
 	if (retval != ERROR_OK)
 		return retval;
@@ -459,10 +458,9 @@ static int pic32mx_write_block(struct flash_bank *bank, uint8_t *buffer,
 	while (target_alloc_working_area_try(target, buffer_size, &source) != ERROR_OK) {
 		buffer_size /= 2;
 		if (buffer_size <= 256) {
-			/* if we already allocated the writing code, but failed to get a
+			/* we already allocated the writing code, but failed to get a
 			 * buffer, free the algorithm */
-			if (pic32mx_info->write_algorithm)
-				target_free_working_area(target, pic32mx_info->write_algorithm);
+			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;
@@ -518,7 +516,7 @@ static int pic32mx_write_block(struct flash_bank *bank, uint8_t *buffer,
 		buf_set_u32(reg_params[2].value, 0, 32, thisrun_count + row_offset / 4);

 		retval = target_run_algorithm(target, 0, NULL, 3, reg_params,
-				pic32mx_info->write_algorithm->address,
+				write_algorithm->address,
 				0, 10000, &mips32_info);
 		if (retval != ERROR_OK) {
 			LOG_ERROR("error executing pic32mx flash write algorithm");
@@ -550,7 +548,7 @@ static int pic32mx_write_block(struct flash_bank *bank, uint8_t *buffer,
 	}

 	target_free_working_area(target, source);
-	target_free_working_area(target, pic32mx_info->write_algorithm);
+	target_free_working_area(target, write_algorithm);

 	destroy_reg_param(&reg_params[0]);
 	destroy_reg_param(&reg_params[1]);
--- a/src/flash/nor/spi.c
+++ b/src/flash/nor/spi.c
@@ -0,0 +1,71 @@
+/***************************************************************************
+ *   Copyright (C) 2012 by George Harris                                   *
+ *   george@luminairecoffee.com                                            *
+ *                                                                         *
+ *   Copyright (C) 2010 by Antonio Borneo                                  *
+ *   borneo.antonio@gmail.com                                              *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "imp.h"
+#include "spi.h"
+#include <jtag/jtag.h>
+
+ /* Shared table of known SPI flash devices for SPI-based flash drivers. Taken
+  * from device datasheets and Linux SPI flash drivers. */
+struct flash_device flash_devices[] = {
+	/* name, erase_cmd, chip_erase_cmd, device_id, pagesize, sectorsize, size_in_bytes */
+	FLASH_ID("st m25p05",      0xd8, 0xC7, 0x00102020, 0x80,  0x8000,  0x10000),
+	FLASH_ID("st m25p10",      0xd8, 0xC7, 0x00112020, 0x80,  0x8000,  0x20000),
+	FLASH_ID("st m25p20",      0xd8, 0xC7, 0x00122020, 0x100, 0x10000, 0x40000),
+	FLASH_ID("st m25p40",      0xd8, 0xC7, 0x00132020, 0x100, 0x10000, 0x80000),
+	FLASH_ID("st m25p80",      0xd8, 0xC7, 0x00142020, 0x100, 0x10000, 0x100000),
+	FLASH_ID("st m25p16",      0xd8, 0xC7, 0x00152020, 0x100, 0x10000, 0x200000),
+	FLASH_ID("st m25p32",      0xd8, 0xC7, 0x00162020, 0x100, 0x10000, 0x400000),
+	FLASH_ID("st m25p64",      0xd8, 0xC7, 0x00172020, 0x100, 0x10000, 0x800000),
+	FLASH_ID("st m25p128",     0xd8, 0xC7, 0x00182020, 0x100, 0x40000, 0x1000000),
+	FLASH_ID("st m45pe10",     0xd8, 0xd8, 0x00114020, 0x100, 0x10000, 0x20000),
+	FLASH_ID("st m45pe20",     0xd8, 0xd8, 0x00124020, 0x100, 0x10000, 0x40000),
+	FLASH_ID("st m45pe40",     0xd8, 0xd8, 0x00134020, 0x100, 0x10000, 0x80000),
+	FLASH_ID("st m45pe80",     0xd8, 0xd8, 0x00144020, 0x100, 0x10000, 0x100000),
+	FLASH_ID("sp s25fl004",    0xd8, 0xC7, 0x00120201, 0x100, 0x10000, 0x80000),
+	FLASH_ID("sp s25fl008",    0xd8, 0xC7, 0x00130201, 0x100, 0x10000, 0x100000),
+	FLASH_ID("sp s25fl016",    0xd8, 0xC7, 0x00140201, 0x100, 0x10000, 0x200000),
+	FLASH_ID("sp s25fl032",    0xd8, 0xC7, 0x00150201, 0x100, 0x10000, 0x400000),
+	FLASH_ID("sp s25fl064",    0xd8, 0xC7, 0x00160201, 0x100, 0x10000, 0x800000),
+	FLASH_ID("atmel 25f512",   0x52, 0xC7, 0x0065001f, 0x80,  0x8000,  0x10000),
+	FLASH_ID("atmel 25f1024",  0x52, 0x62, 0x0060001f, 0x100, 0x8000,  0x20000),
+	FLASH_ID("atmel 25f2048",  0x52, 0x62, 0x0063001f, 0x100, 0x10000, 0x40000),
+	FLASH_ID("atmel 25f4096",  0x52, 0x62, 0x0064001f, 0x100, 0x10000, 0x80000),
+	FLASH_ID("atmel 25fs040",  0xd7, 0xC7, 0x0004661f, 0x100, 0x10000, 0x80000),
+	FLASH_ID("mac 25l512",     0xd8, 0xC7, 0x001020c2, 0x010, 0x10000, 0x10000),
+	FLASH_ID("mac 25l1005",    0xd8, 0xd8, 0x001120c2, 0x010, 0x10000, 0x20000),
+	FLASH_ID("mac 25l2005",    0xd8, 0xC7, 0x001220c2, 0x010, 0x10000, 0x40000),
+	FLASH_ID("mac 25l4005",    0xd8, 0xC7, 0x001320c2, 0x010, 0x10000, 0x80000),
+	FLASH_ID("mac 25l8005",    0xd8, 0xC7, 0x001420c2, 0x010, 0x10000, 0x100000),
+	FLASH_ID("mac 25l1605",    0xd8, 0xC7, 0x001520c2, 0x100, 0x10000, 0x200000),
+	FLASH_ID("mac 25l3205",    0xd8, 0xC7, 0x001620c2, 0x100, 0x10000, 0x400000),
+	FLASH_ID("mac 25l6405",    0xd8, 0xC7, 0x001720c2, 0x100, 0x10000, 0x800000),
+	FLASH_ID("win w25q32dw",   0xd8, 0xC7, 0x001660ef, 0x100, 0x10000, 0x400000),
+	FLASH_ID("win w25q64cv",   0xd8, 0xC7, 0x001740ef, 0x100, 0x10000, 0x800000),
+	FLASH_ID(NULL,             0,    0,	   0,          0,     0,       0)
+};
--- a/src/flash/nor/spi.h
+++ b/src/flash/nor/spi.h
@@ -0,0 +1,58 @@
+/***************************************************************************
+ *   Copyright (C) 2012 by George Harris                                   *
+ *   george@luminairecoffee.com                                            *
+ *                                                                         *
+ *   Copyright (C) 2010 by Antonio Borneo                                  *
+ *   borneo.antonio@gmail.com                                              *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+
+/* data structure to maintain flash ids from different vendors */
+struct flash_device {
+	char *name;
+	uint8_t erase_cmd;
+	uint8_t chip_erase_cmd;
+	uint32_t device_id;
+	uint32_t pagesize;
+	unsigned long sectorsize;
+	unsigned long size_in_bytes;
+};
+
+#define FLASH_ID(n, es, ces, id, psize, ssize, size) \
+{	                        \
+	.name = n,              \
+	.erase_cmd = es,        \
+	.chip_erase_cmd = ces,  \
+	.device_id = id,        \
+	.pagesize = psize,      \
+	.sectorsize = ssize,    \
+	.size_in_bytes = size   \
+}
+
+extern struct flash_device flash_devices[];
+
+/* fields in SPI flash status register */
+#define SPIFLASH_BSY_BIT		0x00000001 /* WIP Bit of SPI SR on SMI SR */
+#define SPIFLASH_WE_BIT			0x00000002 /* WEL Bit of SPI SR on SMI SR */
+
+/* SPI Flash Commands */
+#define SPIFLASH_READ_ID		0x9F /* Read Flash Identification */
+#define SPIFLASH_READ_STATUS	0x05 /* Read Status Register */
+#define SPIFLASH_WRITE_ENABLE	0x06 /* Write Enable */
+#define SPIFLASH_PAGE_PROGRAM	0x02 /* Page Program */
+#define SPIFLASH_FAST_READ		0x0B /* Fast Read */
+#define SPIFLASH_READ			0x03 /* Normal Read */
--- a/src/flash/nor/stellaris.c
+++ b/src/flash/nor/stellaris.c
@@ -124,7 +124,7 @@ struct stellaris_flash_bank {
 };

 /* Autogenerated by contrib/gen-stellaris-part-header.pl */
-/* From Stellaris Firmware Development Package revision 8049 */
+/* From Stellaris Firmware Development Package revision 9453 */
 static struct {
 	uint8_t class;
 	uint8_t partno;
@@ -189,7 +189,6 @@ static struct {
 	{0x04, 0xC9, "LM3S1R26"},
 	{0x04, 0x30, "LM3S1W16"},
 	{0x04, 0x2F, "LM3S1Z16"},
-	{0x01, 0xD4, "LM3S2016"},
 	{0x01, 0x51, "LM3S2110"},
 	{0x01, 0x84, "LM3S2139"},
 	{0x03, 0x39, "LM3S2276"},
@@ -301,9 +300,7 @@ static struct {
 	{0x01, 0x8B, "LM3S6637"},
 	{0x01, 0xA3, "LM3S6730"},
 	{0x01, 0x77, "LM3S6753"},
-	{0x01, 0xD1, "LM3S6816"},
 	{0x01, 0xE9, "LM3S6911"},
-	{0x01, 0xD3, "LM3S6916"},
 	{0x01, 0xE8, "LM3S6918"},
 	{0x01, 0x89, "LM3S6938"},
 	{0x01, 0x72, "LM3S6950"},
@@ -350,11 +347,9 @@ static struct {
 	{0x04, 0x1E, "LM3S9BN5"},
 	{0x04, 0x1F, "LM3S9BN6"},
 	{0x06, 0x70, "LM3S9C97"},
-	{0x06, 0x7A, "LM3S9CN5"},
 	{0x06, 0xA9, "LM3S9D81"},
 	{0x06, 0x7E, "LM3S9D90"},
 	{0x06, 0x92, "LM3S9D92"},
-	{0x06, 0xC8, "LM3S9D95"},
 	{0x06, 0x9D, "LM3S9D96"},
 	{0x06, 0x7B, "LM3S9DN5"},
 	{0x06, 0x7C, "LM3S9DN6"},
@@ -365,7 +360,6 @@ static struct {
 	{0x06, 0xA8, "LM3S9U81"},
 	{0x06, 0x7D, "LM3S9U90"},
 	{0x06, 0x90, "LM3S9U92"},
-	{0x06, 0xB7, "LM3S9U95"},
 	{0x06, 0x9B, "LM3S9U96"},
 	{0x05, 0x18, "LM4F110B2QR"},
 	{0x05, 0x19, "LM4F110C4QR"},
@@ -401,6 +395,13 @@ static struct {
 	{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"},
@@ -410,8 +411,9 @@ static struct {
 	{0x05, 0xC1, "LM4F232H5QC"},
 	{0x05, 0xC5, "LM4F232H5QD"},
 	{0x05, 0xE5, "LM4FS1AH5BB"},
+	{0x05, 0xEA, "LM4FS1GH5BB"},
 	{0x05, 0xE4, "LM4FS99H5BB"},
-	{0x05, 0xE0, "LM4FSXAH5BB"},
+	{0x05, 0xE1, "LM4FSXLH5BB"},
 	{0xFF, 0x00, "Unknown Part"}
 };

@@ -1031,8 +1033,7 @@ static int stellaris_write_block(struct flash_bank *bank,
 	while (target_alloc_working_area_try(target, buffer_size, &source) != ERROR_OK) {
 		buffer_size /= 2;
 		if (buffer_size <= buf_min) {
-			if (write_algorithm)
-				target_free_working_area(target, write_algorithm);
+			target_free_working_area(target, write_algorithm);
 			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 		}
 		LOG_DEBUG("retry target_alloc_working_area(%s, size=%u)",
@@ -1044,7 +1045,7 @@ static int stellaris_write_block(struct flash_bank *bank,
 			(uint8_t *) stellaris_write_code);

 	armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
-	armv7m_info.core_mode = ARMV7M_MODE_ANY;
+	armv7m_info.core_mode = ARM_MODE_THREAD;

 	init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
 	init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
--- a/src/flash/nor/stm32f1x.c
+++ b/src/flash/nor/stm32f1x.c
@@ -102,25 +102,36 @@
 #define KEY1			0x45670123
 #define KEY2			0xCDEF89AB

+/* timeout values */
+
+#define FLASH_WRITE_TIMEOUT 10
+#define FLASH_ERASE_TIMEOUT 100
+
 struct stm32x_options {
 	uint16_t RDP;
 	uint16_t user_options;
+	uint16_t user_data;
 	uint16_t protection[4];
 };

 struct stm32x_flash_bank {
 	struct stm32x_options option_bytes;
-	struct working_area *write_algorithm;
 	int ppage_size;
 	int probed;

 	bool has_dual_banks;
 	/* used to access dual flash bank stm32xl */
 	uint32_t register_base;
+	uint16_t default_rdp;
+	int user_data_offset;
+	int option_offset;
+	uint32_t user_bank_size;
 };

 static int stm32x_mass_erase(struct flash_bank *bank);
 static int stm32x_get_device_id(struct flash_bank *bank, uint32_t *device_id);
+static int stm32x_write_block(struct flash_bank *bank, uint8_t *buffer,
+		uint32_t offset, uint32_t count);

 /* flash bank stm32x <base> <size> 0 0 <target#>
 */
@@ -134,10 +145,10 @@ FLASH_BANK_COMMAND_HANDLER(stm32x_flash_bank_command)
 	stm32x_info = malloc(sizeof(struct stm32x_flash_bank));

 	bank->driver_priv = stm32x_info;
-	stm32x_info->write_algorithm = NULL;
 	stm32x_info->probed = 0;
 	stm32x_info->has_dual_banks = false;
 	stm32x_info->register_base = FLASH_REG_BASE_B0;
+	stm32x_info->user_bank_size = bank->size;

 	return ERROR_OK;
 }
@@ -196,7 +207,7 @@ static int stm32x_wait_status_busy(struct flash_bank *bank, int timeout)
 	return retval;
 }

-int stm32x_check_operation_supported(struct flash_bank *bank)
+static int stm32x_check_operation_supported(struct flash_bank *bank)
 {
 	struct stm32x_flash_bank *stm32x_info = bank->driver_priv;

@@ -223,7 +234,8 @@ static int stm32x_read_options(struct flash_bank *bank)
 	if (retval != ERROR_OK)
 		return retval;

-	stm32x_info->option_bytes.user_options = (uint16_t)0xFFF8 | ((optiondata >> 2) & 0x07);
+	stm32x_info->option_bytes.user_options = (optiondata >> stm32x_info->option_offset >> 2) & 0xffff;
+	stm32x_info->option_bytes.user_data = (optiondata >> stm32x_info->user_data_offset) & 0xffff;
 	stm32x_info->option_bytes.RDP = (optiondata & (1 << OPT_READOUT)) ? 0xFFFF : 0x5AA5;

 	if (optiondata & (1 << OPT_READOUT))
@@ -277,13 +289,13 @@ static int stm32x_erase_options(struct flash_bank *bank)
 	if (retval != ERROR_OK)
 		return retval;

-	retval = stm32x_wait_status_busy(bank, 10);
+	retval = stm32x_wait_status_busy(bank, FLASH_ERASE_TIMEOUT);
 	if (retval != ERROR_OK)
 		return retval;

 	/* clear readout protection and complementary option bytes
 	 * this will also force a device unlock if set */
-	stm32x_info->option_bytes.RDP = 0x5AA5;
+	stm32x_info->option_bytes.RDP = stm32x_info->default_rdp;

 	return ERROR_OK;
 }
@@ -316,59 +328,24 @@ static int stm32x_write_options(struct flash_bank *bank)
 	if (retval != ERROR_OK)
 		return retval;

-	/* write user option byte */
-	retval = target_write_u16(target, STM32_OB_USER, stm32x_info->option_bytes.user_options);
-	if (retval != ERROR_OK)
-		return retval;
+	uint8_t opt_bytes[16];

-	retval = stm32x_wait_status_busy(bank, 10);
-	if (retval != ERROR_OK)
-		return retval;
+	target_buffer_set_u16(target, opt_bytes, stm32x_info->option_bytes.RDP);
+	target_buffer_set_u16(target, opt_bytes + 2, stm32x_info->option_bytes.user_options);
+	target_buffer_set_u16(target, opt_bytes + 4, stm32x_info->option_bytes.user_data & 0xff);
+	target_buffer_set_u16(target, opt_bytes + 6, (stm32x_info->option_bytes.user_data >> 8) & 0xff);
+	target_buffer_set_u16(target, opt_bytes + 8, stm32x_info->option_bytes.protection[0]);
+	target_buffer_set_u16(target, opt_bytes + 10, stm32x_info->option_bytes.protection[1]);
+	target_buffer_set_u16(target, opt_bytes + 12, stm32x_info->option_bytes.protection[2]);
+	target_buffer_set_u16(target, opt_bytes + 14, stm32x_info->option_bytes.protection[3]);

-	/* write protection byte 1 */
-	retval = target_write_u16(target, STM32_OB_WRP0, stm32x_info->option_bytes.protection[0]);
-	if (retval != ERROR_OK)
-		return retval;
-
-	retval = stm32x_wait_status_busy(bank, 10);
-	if (retval != ERROR_OK)
-		return retval;
-
-	/* write protection byte 2 */
-	retval = target_write_u16(target, STM32_OB_WRP1, stm32x_info->option_bytes.protection[1]);
-	if (retval != ERROR_OK)
-		return retval;
-
-	retval = stm32x_wait_status_busy(bank, 10);
-	if (retval != ERROR_OK)
-		return retval;
-
-	/* write protection byte 3 */
-	retval = target_write_u16(target, STM32_OB_WRP2, stm32x_info->option_bytes.protection[2]);
-	if (retval != ERROR_OK)
-		return retval;
-
-	retval = stm32x_wait_status_busy(bank, 10);
-	if (retval != ERROR_OK)
-		return retval;
-
-	/* write protection byte 4 */
-	retval = target_write_u16(target, STM32_OB_WRP3, stm32x_info->option_bytes.protection[3]);
-	if (retval != ERROR_OK)
-		return retval;
-
-	retval = stm32x_wait_status_busy(bank, 10);
-	if (retval != ERROR_OK)
-		return retval;
-
-	/* write readout protection bit */
-	retval = target_write_u16(target, STM32_OB_RDP, stm32x_info->option_bytes.RDP);
-	if (retval != ERROR_OK)
-		return retval;
-
-	retval = stm32x_wait_status_busy(bank, 10);
-	if (retval != ERROR_OK)
+	uint32_t offset = STM32_OB_RDP - bank->base;
+	retval = stm32x_write_block(bank, opt_bytes, offset, sizeof(opt_bytes) / 2);
+	if (retval != ERROR_OK) {
+		if (retval == ERROR_TARGET_RESOURCE_NOT_AVAILABLE)
+			LOG_ERROR("working area required to erase options bytes");
 		return retval;
+	}

 	retval = target_write_u32(target, STM32_FLASH_CR_B0, FLASH_LOCK);
 	if (retval != ERROR_OK)
@@ -481,7 +458,7 @@ static int stm32x_erase(struct flash_bank *bank, int first, int last)
 		if (retval != ERROR_OK)
 			return retval;

-		retval = stm32x_wait_status_busy(bank, 100);
+		retval = stm32x_wait_status_busy(bank, FLASH_ERASE_TIMEOUT);
 		if (retval != ERROR_OK)
 			return retval;

@@ -595,6 +572,7 @@ static int stm32x_write_block(struct flash_bank *bank, uint8_t *buffer,
 	struct stm32x_flash_bank *stm32x_info = bank->driver_priv;
 	struct target *target = bank->target;
 	uint32_t buffer_size = 16384;
+	struct working_area *write_algorithm;
 	struct working_area *source;
 	uint32_t address = bank->base + offset;
 	struct reg_param reg_params[5];
@@ -644,12 +622,12 @@ static int stm32x_write_block(struct flash_bank *bank, uint8_t *buffer,

 	/* flash write code */
 	if (target_alloc_working_area(target, sizeof(stm32x_flash_write_code),
-			&stm32x_info->write_algorithm) != ERROR_OK) {
+			&write_algorithm) != ERROR_OK) {
 		LOG_WARNING("no working area available, can't do block memory writes");
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 	};

-	retval = target_write_buffer(target, stm32x_info->write_algorithm->address,
+	retval = target_write_buffer(target, write_algorithm->address,
 			sizeof(stm32x_flash_write_code), (uint8_t *)stm32x_flash_write_code);
 	if (retval != ERROR_OK)
 		return retval;
@@ -659,10 +637,9 @@ static int stm32x_write_block(struct flash_bank *bank, uint8_t *buffer,
 		buffer_size /= 2;
 		buffer_size &= ~3UL; /* Make sure it's 4 byte aligned */
 		if (buffer_size <= 256) {
-			/* if we already allocated the writing code, but failed to get a
+			/* we already allocated the writing code, but failed to get a
 			 * buffer, free the algorithm */
-			if (stm32x_info->write_algorithm)
-				target_free_working_area(target, stm32x_info->write_algorithm);
+			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;
@@ -682,13 +659,13 @@ static int stm32x_write_block(struct flash_bank *bank, uint8_t *buffer,
 	buf_set_u32(reg_params[4].value, 0, 32, address);

 	armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
-	armv7m_info.core_mode = ARMV7M_MODE_ANY;
+	armv7m_info.core_mode = ARM_MODE_THREAD;

 	retval = target_run_flash_async_algorithm(target, buffer, count, 2,
 			0, NULL,
 			5, reg_params,
 			source->address, source->size,
-			stm32x_info->write_algorithm->address, 0,
+			write_algorithm->address, 0,
 			&armv7m_info);

 	if (retval == ERROR_FLASH_OPERATION_FAILED) {
@@ -709,7 +686,7 @@ static int stm32x_write_block(struct flash_bank *bank, uint8_t *buffer,
 	}

 	target_free_working_area(target, source);
-	target_free_working_area(target, stm32x_info->write_algorithm);
+	target_free_working_area(target, write_algorithm);

 	destroy_reg_param(&reg_params[0]);
 	destroy_reg_param(&reg_params[1]);
@@ -882,6 +859,11 @@ static int stm32x_probe(struct flash_bank *bank)

 	stm32x_info->probed = 0;
 	stm32x_info->register_base = FLASH_REG_BASE_B0;
+	stm32x_info->user_data_offset = 10;
+	stm32x_info->option_offset = 0;
+
+	/* default factory protection level */
+	stm32x_info->default_rdp = 0x5AA5;

 	/* read stm32 device id register */
 	int retval = stm32x_get_device_id(bank, &device_id);
@@ -921,6 +903,9 @@ static int stm32x_probe(struct flash_bank *bank)
 		page_size = 2048;
 		stm32x_info->ppage_size = 2;
 		max_flash_size_in_kb = 256;
+		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;
@@ -937,11 +922,17 @@ static int stm32x_probe(struct flash_bank *bank)
 		page_size = 2048;
 		stm32x_info->ppage_size = 2;
 		max_flash_size_in_kb = 256;
+		stm32x_info->user_data_offset = 16;
+		stm32x_info->option_offset = 6;
+		stm32x_info->default_rdp = 0x55AA;
 		break;
 	case 0x440: /* stm32f0x */
 		page_size = 1024;
 		stm32x_info->ppage_size = 4;
 		max_flash_size_in_kb = 64;
+		stm32x_info->user_data_offset = 16;
+		stm32x_info->option_offset = 6;
+		stm32x_info->default_rdp = 0x55AA;
 		break;
 	default:
 		LOG_WARNING("Cannot identify target as a STM32 family.");
@@ -972,6 +963,13 @@ static int stm32x_probe(struct flash_bank *bank)
 		}
 	}

+	/* 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 (stm32x_info->user_bank_size) {
+		LOG_INFO("ignoring flash probed value, using configured bank size");
+		flash_size_in_kb = stm32x_info->user_bank_size / 1024;
+	}
+
 	LOG_INFO("flash size = %dkbytes", flash_size_in_kb);

 	/* did we assign flash size? */
@@ -1131,7 +1129,15 @@ static int get_stm32x_info(struct flash_bank *bank, char *buf, int buf_size)

 		switch (device_id >> 16) {
 			case 0x1000:
-				snprintf(buf, buf_size, "1.0");
+				snprintf(buf, buf_size, "A");
+				break;
+
+			case 0x1001:
+				snprintf(buf, buf_size, "Z");
+				break;
+
+			case 0x2000:
+				snprintf(buf, buf_size, "B");
 				break;

 			default:
@@ -1177,7 +1183,11 @@ static int get_stm32x_info(struct flash_bank *bank, char *buf, int buf_size)

 		switch (device_id >> 16) {
 			case 0x1000:
-				snprintf(buf, buf_size, "1.0");
+				snprintf(buf, buf_size, "A");
+				break;
+
+			case 0x2000:
+				snprintf(buf, buf_size, "B");
 				break;

 			default:
@@ -1326,6 +1336,8 @@ COMMAND_HANDLER(stm32x_handle_options_read_command)
 		return retval;
 	command_print(CMD_CTX, "Option Byte: 0x%" PRIx32 "", optionbyte);

+	int user_data = optionbyte;
+
 	if (buf_get_u32((uint8_t *)&optionbyte, OPT_ERROR, 1))
 		command_print(CMD_CTX, "Option Byte Complement Error");

@@ -1334,6 +1346,9 @@ COMMAND_HANDLER(stm32x_handle_options_read_command)
 	else
 		command_print(CMD_CTX, "Readout Protection Off");

+	/* user option bytes are offset depending on variant */
+	optionbyte >>= stm32x_info->option_offset;
+
 	if (buf_get_u32((uint8_t *)&optionbyte, OPT_RDWDGSW, 1))
 		command_print(CMD_CTX, "Software Watchdog");
 	else
@@ -1356,6 +1371,11 @@ COMMAND_HANDLER(stm32x_handle_options_read_command)
 			command_print(CMD_CTX, "Boot: Bank 1");
 	}

+	command_print(CMD_CTX, "User Option0: 0x%02" PRIx8,
+			(user_data >> stm32x_info->user_data_offset) & 0xff);
+	command_print(CMD_CTX, "User Option1: 0x%02" PRIx8,
+			(user_data >> (stm32x_info->user_data_offset + 8)) & 0xff);
+
 	return ERROR_OK;
 }

@@ -1363,9 +1383,9 @@ COMMAND_HANDLER(stm32x_handle_options_write_command)
 {
 	struct target *target = NULL;
 	struct stm32x_flash_bank *stm32x_info = NULL;
-	uint16_t optionbyte = 0xF8;
+	uint16_t optionbyte;

-	if (CMD_ARGC < 4)
+	if (CMD_ARGC < 2)
 		return ERROR_COMMAND_SYNTAX_ERROR;

 	struct flash_bank *bank;
@@ -1386,34 +1406,41 @@ COMMAND_HANDLER(stm32x_handle_options_write_command)
 	if (ERROR_OK != retval)
 		return retval;

-	/* REVISIT: ignores some options which we will display...
-	 * and doesn't insist on the specified syntax.
-	 */
+	retval = stm32x_read_options(bank);
+	if (ERROR_OK != retval)
+		return retval;

-	/* OPT_RDWDGSW */
-	if (strcmp(CMD_ARGV[1], "SWWDG") == 0)
-		optionbyte |= (1 << 0);
-	else	/* REVISIT must be "HWWDG" then ... */
-		optionbyte &= ~(1 << 0);
+	/* start with current options */
+	optionbyte = stm32x_info->option_bytes.user_options;

-	/* OPT_RDRSTSTOP */
-	if (strcmp(CMD_ARGV[2], "NORSTSTOP") == 0)
-		optionbyte |= (1 << 1);
-	else	/* REVISIT must be "RSTSTNDBY" then ... */
-		optionbyte &= ~(1 << 1);
+	/* skip over flash bank */
+	CMD_ARGC--;
+	CMD_ARGV++;

-	/* OPT_RDRSTSTDBY */
-	if (strcmp(CMD_ARGV[3], "NORSTSTNDBY") == 0)
-		optionbyte |= (1 << 2);
-	else	/* REVISIT must be "RSTSTOP" then ... */
-		optionbyte &= ~(1 << 2);
-
-	if (CMD_ARGC > 4 && stm32x_info->has_dual_banks) {
-		/* OPT_BFB2 */
-		if (strcmp(CMD_ARGV[4], "BOOT0") == 0)
-			optionbyte |= (1 << 3);
-		else
-			optionbyte &= ~(1 << 3);
+	while (CMD_ARGC) {
+		if (strcmp("SWWDG", CMD_ARGV[0]) == 0)
+			optionbyte |= (1 << 0);
+		else if (strcmp("HWWDG", CMD_ARGV[0]) == 0)
+			optionbyte &= ~(1 << 0);
+		else if (strcmp("NORSTSTOP", CMD_ARGV[0]) == 0)
+			optionbyte &= ~(1 << 1);
+		else if (strcmp("RSTSTNDBY", CMD_ARGV[0]) == 0)
+			optionbyte &= ~(1 << 1);
+		else if (strcmp("NORSTSTNDBY", CMD_ARGV[0]) == 0)
+			optionbyte &= ~(1 << 2);
+		else if (strcmp("RSTSTOP", CMD_ARGV[0]) == 0)
+			optionbyte &= ~(1 << 2);
+		else if (stm32x_info->has_dual_banks) {
+			if (strcmp("BOOT0", CMD_ARGV[0]) == 0)
+				optionbyte |= (1 << 3);
+			else if (strcmp("BOOT1", CMD_ARGV[0]) == 0)
+				optionbyte &= ~(1 << 3);
+			else
+				return ERROR_COMMAND_SYNTAX_ERROR;
+		} else
+			return ERROR_COMMAND_SYNTAX_ERROR;
+		CMD_ARGC--;
+		CMD_ARGV++;
 	}

 	if (stm32x_erase_options(bank) != ERROR_OK) {
@@ -1461,7 +1488,7 @@ static int stm32x_mass_erase(struct flash_bank *bank)
 	if (retval != ERROR_OK)
 		return retval;

-	retval = stm32x_wait_status_busy(bank, 100);
+	retval = stm32x_wait_status_busy(bank, FLASH_ERASE_TIMEOUT);
 	if (retval != ERROR_OK)
 		return retval;

--- a/src/flash/nor/stm32f2x.c
+++ b/src/flash/nor/stm32f2x.c
@@ -91,69 +91,75 @@
 #define FLASH_ERASE_TIMEOUT 10000
 #define FLASH_WRITE_TIMEOUT 5

-#define STM32_FLASH_BASE	0x40023c00
-#define STM32_FLASH_ACR		0x40023c00
-#define STM32_FLASH_KEYR	0x40023c04
-#define STM32_FLASH_OPTKEYR	0x40023c08
-#define STM32_FLASH_SR		0x40023c0C
-#define STM32_FLASH_CR		0x40023c10
-#define STM32_FLASH_OPTCR	0x40023c14
-#define STM32_FLASH_OBR		0x40023c1C
-
-/* option byte location */
-
-#define STM32_OB_RDP		0x1FFFF800
-#define STM32_OB_USER		0x1FFFF802
-#define STM32_OB_DATA0		0x1FFFF804
-#define STM32_OB_DATA1		0x1FFFF806
-#define STM32_OB_WRP0		0x1FFFF808
-#define STM32_OB_WRP1		0x1FFFF80A
-#define STM32_OB_WRP2		0x1FFFF80C
-#define STM32_OB_WRP3		0x1FFFF80E
+#define STM32_FLASH_BASE    0x40023c00
+#define STM32_FLASH_ACR     0x40023c00
+#define STM32_FLASH_KEYR    0x40023c04
+#define STM32_FLASH_OPTKEYR 0x40023c08
+#define STM32_FLASH_SR      0x40023c0C
+#define STM32_FLASH_CR      0x40023c10
+#define STM32_FLASH_OPTCR   0x40023c14
+#define STM32_FLASH_OPTCR1  0x40023c18

 /* FLASH_CR register bits */

-#define FLASH_PG		(1 << 0)
-#define FLASH_SER		(1 << 1)
-#define FLASH_MER		(1 << 2)
-#define FLASH_STRT		(1 << 16)
-#define FLASH_PSIZE_8	(0 << 8)
-#define FLASH_PSIZE_16	(1 << 8)
-#define FLASH_PSIZE_32	(2 << 8)
-#define FLASH_PSIZE_64	(3 << 8)
-#define FLASH_SNB(a)	((a) << 3)
-#define FLASH_LOCK		(1 << 31)
+#define FLASH_PG       (1 << 0)
+#define FLASH_SER      (1 << 1)
+#define FLASH_MER      (1 << 2)
+#define FLASH_MER1     (1 << 15)
+#define FLASH_STRT     (1 << 16)
+#define FLASH_PSIZE_8  (0 << 8)
+#define FLASH_PSIZE_16 (1 << 8)
+#define FLASH_PSIZE_32 (2 << 8)
+#define FLASH_PSIZE_64 (3 << 8)
+#define FLASH_SNB(a)   ((a) << 3)
+#define FLASH_LOCK     (1 << 31)

 /* FLASH_SR register bits */

-#define FLASH_BSY		(1 << 16)
-#define FLASH_PGSERR	(1 << 7) /* Programming sequence error */
-#define FLASH_PGPERR	(1 << 6) /* Programming parallelism error */
-#define FLASH_PGAERR	(1 << 5) /* Programming alignment error */
-#define FLASH_WRPERR	(1 << 4) /* Write protection error */
-#define FLASH_OPERR		(1 << 1) /* Operation error */
+#define FLASH_BSY      (1 << 16)
+#define FLASH_PGSERR   (1 << 7) /* Programming sequence error */
+#define FLASH_PGPERR   (1 << 6) /* Programming parallelism error */
+#define FLASH_PGAERR   (1 << 5) /* Programming alignment error */
+#define FLASH_WRPERR   (1 << 4) /* Write protection error */
+#define FLASH_OPERR    (1 << 1) /* Operation error */

 #define FLASH_ERROR (FLASH_PGSERR | FLASH_PGPERR | FLASH_PGAERR | FLASH_WRPERR | FLASH_OPERR)

+/* STM32_FLASH_OPTCR register bits */
+
+#define OPT_LOCK      (1 << 0)
+#define OPT_START     (1 << 1)
+
 /* STM32_FLASH_OBR bit definitions (reading) */

-#define OPT_ERROR		0
-#define OPT_READOUT		1
-#define OPT_RDWDGSW		2
-#define OPT_RDRSTSTOP	3
-#define OPT_RDRSTSTDBY	4
-#define OPT_BFB2		5	/* dual flash bank only */
+#define OPT_ERROR      0
+#define OPT_READOUT    1
+#define OPT_RDWDGSW    2
+#define OPT_RDRSTSTOP  3
+#define OPT_RDRSTSTDBY 4
+#define OPT_BFB2       5	/* dual flash bank only */

 /* register unlock keys */

-#define KEY1			0x45670123
-#define KEY2			0xCDEF89AB
+#define KEY1           0x45670123
+#define KEY2           0xCDEF89AB

-struct stm32x_flash_bank {
-	struct working_area *write_algorithm;
-	int probed;
+/* option register unlock key */
+#define OPTKEY1        0x08192A3B
+#define OPTKEY2        0x4C5D6E7F
+
+struct stm32x_options {
+	uint8_t RDP;
+	uint8_t user_options;
+	uint32_t protection;
 };

+struct stm32x_flash_bank {
+	struct stm32x_options option_bytes;
+	int probed;
+	bool has_large_mem;		/* stm32f42x/stm32f43x family */
+	uint32_t user_bank_size;
+};

 /* flash bank stm32x <base> <size> 0 0 <target#>
 */
@@ -167,8 +173,8 @@ FLASH_BANK_COMMAND_HANDLER(stm32x_flash_bank_command)
 	stm32x_info = malloc(sizeof(struct stm32x_flash_bank));
 	bank->driver_priv = stm32x_info;

-	stm32x_info->write_algorithm = NULL;
 	stm32x_info->probed = 0;
+	stm32x_info->user_bank_size = bank->size;

 	return ERROR_OK;
 }
@@ -257,8 +263,144 @@ static int stm32x_unlock_reg(struct target *target)
 	return ERROR_OK;
 }

+static int stm32x_unlock_option_reg(struct target *target)
+{
+	uint32_t ctrl;
+
+	int retval = target_read_u32(target, STM32_FLASH_OPTCR, &ctrl);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if ((ctrl & OPT_LOCK) == 0)
+		return ERROR_OK;
+
+	/* unlock option registers */
+	retval = target_write_u32(target, STM32_FLASH_OPTKEYR, OPTKEY1);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = target_write_u32(target, STM32_FLASH_OPTKEYR, OPTKEY2);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = target_read_u32(target, STM32_FLASH_OPTCR, &ctrl);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (ctrl & OPT_LOCK) {
+		LOG_ERROR("options not unlocked STM32_FLASH_OPTCR: %x", ctrl);
+		return ERROR_TARGET_FAILURE;
+	}
+
+	return ERROR_OK;
+}
+
+static int stm32x_read_options(struct flash_bank *bank)
+{
+	uint32_t optiondata;
+	struct stm32x_flash_bank *stm32x_info = NULL;
+	struct target *target = bank->target;
+
+	stm32x_info = bank->driver_priv;
+
+	/* read current option bytes */
+	int retval = target_read_u32(target, STM32_FLASH_OPTCR, &optiondata);
+	if (retval != ERROR_OK)
+		return retval;
+
+	stm32x_info->option_bytes.user_options = optiondata & 0xec;
+	stm32x_info->option_bytes.RDP = (optiondata >> 8) & 0xff;
+	stm32x_info->option_bytes.protection = (optiondata >> 16) & 0xfff;
+
+	if (stm32x_info->has_large_mem) {
+
+		retval = target_read_u32(target, STM32_FLASH_OPTCR1, &optiondata);
+		if (retval != ERROR_OK)
+			return retval;
+
+		/* append protection bits */
+		stm32x_info->option_bytes.protection |= (optiondata >> 4) & 0x00fff000;
+	}
+
+	if (stm32x_info->option_bytes.RDP != 0xAA)
+		LOG_INFO("Device Security Bit Set");
+
+	return ERROR_OK;
+}
+
+static int stm32x_write_options(struct flash_bank *bank)
+{
+	struct stm32x_flash_bank *stm32x_info = NULL;
+	struct target *target = bank->target;
+	uint32_t optiondata;
+
+	stm32x_info = bank->driver_priv;
+
+	int retval = stm32x_unlock_option_reg(target);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* 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);
+
+	/* program options */
+	retval = target_write_u32(target, STM32_FLASH_OPTCR, optiondata);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (stm32x_info->has_large_mem) {
+
+		uint32_t optiondata2 = 0;
+		buf_set_u32(&optiondata2, 16, 12, stm32x_info->option_bytes.protection >> 12);
+		retval = target_write_u32(target, STM32_FLASH_OPTCR1, optiondata2);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	/* start programming cycle */
+	retval = target_write_u32(target, STM32_FLASH_OPTCR, optiondata | OPT_START);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* wait for completion */
+	retval = stm32x_wait_status_busy(bank, FLASH_ERASE_TIMEOUT);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* relock registers */
+	retval = target_write_u32(target, STM32_FLASH_OPTCR, OPT_LOCK);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return ERROR_OK;
+}
+
 static int stm32x_protect_check(struct flash_bank *bank)
 {
+	struct target *target = bank->target;
+	struct stm32x_flash_bank *stm32x_info = bank->driver_priv;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* read write protection settings */
+	int retval = stm32x_read_options(bank);
+	if (retval != ERROR_OK) {
+		LOG_DEBUG("unable to read option bytes");
+		return retval;
+	}
+
+	for (int i = 0; i < bank->num_sectors; i++) {
+		if (stm32x_info->option_bytes.protection & (1 << i))
+			bank->sectors[i].is_protected = 0;
+		else
+			bank->sectors[i].is_protected = 1;
+	}
+
 	return ERROR_OK;
 }

@@ -310,15 +452,42 @@ static int stm32x_erase(struct flash_bank *bank, int first, int last)

 static int stm32x_protect(struct flash_bank *bank, int set, int first, int last)
 {
+	struct target *target = bank->target;
+	struct stm32x_flash_bank *stm32x_info = bank->driver_priv;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* read protection settings */
+	int retval = stm32x_read_options(bank);
+	if (retval != ERROR_OK) {
+		LOG_DEBUG("unable to read option bytes");
+		return retval;
+	}
+
+	for (int i = first; i <= last; i++) {
+
+		if (set)
+			stm32x_info->option_bytes.protection &= ~(1 << i);
+		else
+			stm32x_info->option_bytes.protection |= (1 << i);
+	}
+
+	retval = stm32x_write_options(bank);
+	if (retval != ERROR_OK)
+		return retval;
+
 	return ERROR_OK;
 }

 static int stm32x_write_block(struct flash_bank *bank, uint8_t *buffer,
 		uint32_t offset, uint32_t count)
 {
-	struct stm32x_flash_bank *stm32x_info = bank->driver_priv;
 	struct target *target = bank->target;
 	uint32_t buffer_size = 16384;
+	struct working_area *write_algorithm;
 	struct working_area *source;
 	uint32_t address = bank->base + offset;
 	struct reg_param reg_params[5];
@@ -366,12 +535,12 @@ static int stm32x_write_block(struct flash_bank *bank, uint8_t *buffer,
 	};

 	if (target_alloc_working_area(target, sizeof(stm32x_flash_write_code),
-			&stm32x_info->write_algorithm) != ERROR_OK) {
+			&write_algorithm) != ERROR_OK) {
 		LOG_WARNING("no working area available, can't do block memory writes");
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 	};

-	retval = target_write_buffer(target, stm32x_info->write_algorithm->address,
+	retval = target_write_buffer(target, write_algorithm->address,
 			sizeof(stm32x_flash_write_code),
 			(uint8_t *)stm32x_flash_write_code);
 	if (retval != ERROR_OK)
@@ -381,10 +550,9 @@ static int stm32x_write_block(struct flash_bank *bank, uint8_t *buffer,
 	while (target_alloc_working_area_try(target, buffer_size, &source) != ERROR_OK) {
 		buffer_size /= 2;
 		if (buffer_size <= 256) {
-			/* if we already allocated the writing code, but failed to get a
+			/* we already allocated the writing code, but failed to get a
 			 * buffer, free the algorithm */
-			if (stm32x_info->write_algorithm)
-				target_free_working_area(target, stm32x_info->write_algorithm);
+			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;
@@ -392,7 +560,7 @@ static int stm32x_write_block(struct flash_bank *bank, uint8_t *buffer,
 	};

 	armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
-	armv7m_info.core_mode = ARMV7M_MODE_ANY;
+	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 */
@@ -410,7 +578,7 @@ static int stm32x_write_block(struct flash_bank *bank, uint8_t *buffer,
 			0, NULL,
 			5, reg_params,
 			source->address, source->size,
-			stm32x_info->write_algorithm->address, 0,
+			write_algorithm->address, 0,
 			&armv7m_info);

 	if (retval == ERROR_FLASH_OPERATION_FAILED) {
@@ -430,7 +598,7 @@ static int stm32x_write_block(struct flash_bank *bank, uint8_t *buffer,
 	}

 	target_free_working_area(target, source);
-	target_free_working_area(target, stm32x_info->write_algorithm);
+	target_free_working_area(target, write_algorithm);

 	destroy_reg_param(&reg_params[0]);
 	destroy_reg_param(&reg_params[1]);
@@ -591,6 +759,7 @@ static int stm32x_probe(struct flash_bank *bank)
 	uint32_t base_address = 0x08000000;

 	stm32x_info->probed = 0;
+	stm32x_info->has_large_mem = false;

 	/* read stm32 device id register */
 	int retval = stm32x_get_device_id(bank, &device_id);
@@ -604,6 +773,10 @@ static int stm32x_probe(struct flash_bank *bank)
 	case 0x413:
 		max_flash_size_in_kb = 1024;
 		break;
+	case 0x419:
+		max_flash_size_in_kb = 2048;
+		stm32x_info->has_large_mem = true;
+		break;
 	default:
 		LOG_WARNING("Cannot identify target as a STM32 family.");
 		return ERROR_FAIL;
@@ -620,6 +793,13 @@ static int stm32x_probe(struct flash_bank *bank)
 		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 (stm32x_info->user_bank_size) {
+		LOG_INFO("ignoring flash probed value, using configured bank size");
+		flash_size_in_kb = stm32x_info->user_bank_size / 1024;
+	}
+
 	LOG_INFO("flash size = %dkbytes", flash_size_in_kb);

 	/* did we assign flash size? */
@@ -628,6 +808,10 @@ static int stm32x_probe(struct flash_bank *bank)
 	/* calculate numbers of pages */
 	int num_pages = (flash_size_in_kb / 128) + 4;

+	/* check for larger 2048 bytes devices */
+	if (stm32x_info->has_large_mem)
+		num_pages += 4;
+
 	/* check that calculation result makes sense */
 	assert(num_pages > 0);

@@ -646,7 +830,17 @@ static int stm32x_probe(struct flash_bank *bank)
 	setup_sector(bank, 4, 1, 64 * 1024);

 	/* dynamic memory */
-	setup_sector(bank, 4 + 1, num_pages - 5, 128 * 1024);
+	setup_sector(bank, 4 + 1, MAX(12, num_pages) - 5, 128 * 1024);
+
+	if (stm32x_info->has_large_mem) {
+
+		/* fixed memory for larger devices */
+		setup_sector(bank, 12, 4, 16 * 1024);
+		setup_sector(bank, 16, 1, 64 * 1024);
+
+		/* dynamic memory for larger devices */
+		setup_sector(bank, 16 + 1, num_pages - 5 - 12, 128 * 1024);
+	}

 	for (i = 0; i < num_pages; i++) {
 		bank->sectors[i].is_erased = -1;
@@ -698,11 +892,16 @@ static int get_stm32x_info(struct flash_bank *bank, char *buf, int buf_size)
 				snprintf(buf, buf_size, "Y");
 				break;

+			case 0x2003:
+				snprintf(buf, buf_size, "X");
+				break;
+
 			default:
 				snprintf(buf, buf_size, "unknown");
 				break;
 		}
-	} else if ((device_id & 0xfff) == 0x413) {
+	} else if (((device_id & 0xfff) == 0x413) ||
+			((device_id & 0xfff) == 0x419)) {
 		printed = snprintf(buf, buf_size, "stm32f4x - Rev: ");
 		buf += printed;
 		buf_size -= printed;
@@ -728,22 +927,109 @@ static int get_stm32x_info(struct flash_bank *bank, char *buf, int buf_size)
 	return ERROR_OK;
 }

-static int stm32x_mass_erase(struct flash_bank *bank)
+COMMAND_HANDLER(stm32x_handle_lock_command)
 {
-	int retval;
-	struct target *target = bank->target;
+	struct target *target = NULL;
+	struct stm32x_flash_bank *stm32x_info = NULL;
+
+	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;
+
+	stm32x_info = bank->driver_priv;
+	target = bank->target;

 	if (target->state != TARGET_HALTED) {
 		LOG_ERROR("Target not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}

+	if (stm32x_read_options(bank) != ERROR_OK) {
+		command_print(CMD_CTX, "%s failed to read options", bank->driver->name);
+		return ERROR_OK;
+	}
+
+	/* set readout protection */
+	stm32x_info->option_bytes.RDP = 0;
+
+	if (stm32x_write_options(bank) != ERROR_OK) {
+		command_print(CMD_CTX, "%s failed to lock device", bank->driver->name);
+		return ERROR_OK;
+	}
+
+	command_print(CMD_CTX, "%s locked", bank->driver->name);
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(stm32x_handle_unlock_command)
+{
+	struct target *target = NULL;
+	struct stm32x_flash_bank *stm32x_info = NULL;
+
+	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;
+
+	stm32x_info = bank->driver_priv;
+	target = bank->target;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (stm32x_read_options(bank) != ERROR_OK) {
+		command_print(CMD_CTX, "%s failed to read options", bank->driver->name);
+		return ERROR_OK;
+	}
+
+	/* clear readout protection and complementary option bytes
+	 * this will also force a device unlock if set */
+	stm32x_info->option_bytes.RDP = 0xAA;
+
+	if (stm32x_write_options(bank) != ERROR_OK) {
+		command_print(CMD_CTX, "%s failed to unlock device", bank->driver->name);
+		return ERROR_OK;
+	}
+
+	command_print(CMD_CTX, "%s unlocked.\n"
+			"INFO: a reset or power cycle is required "
+			"for the new settings to take effect.", bank->driver->name);
+
+	return ERROR_OK;
+}
+
+static int stm32x_mass_erase(struct flash_bank *bank)
+{
+	int retval;
+	struct target *target = bank->target;
+	struct stm32x_flash_bank *stm32x_info = NULL;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	stm32x_info = bank->driver_priv;
+
 	retval = stm32x_unlock_reg(target);
 	if (retval != ERROR_OK)
 		return retval;

 	/* mass erase flash memory */
-	retval = target_write_u32(target, stm32x_get_flash_reg(bank, STM32_FLASH_CR), FLASH_MER);
+	if (stm32x_info->has_large_mem)
+		retval = target_write_u32(target, stm32x_get_flash_reg(bank, STM32_FLASH_CR), FLASH_MER | FLASH_MER1);
+	else
+		retval = target_write_u32(target, stm32x_get_flash_reg(bank, STM32_FLASH_CR), FLASH_MER);
 	if (retval != ERROR_OK)
 		return retval;
 	retval = target_write_u32(target, stm32x_get_flash_reg(bank, STM32_FLASH_CR),
@@ -791,6 +1077,20 @@ COMMAND_HANDLER(stm32x_handle_mass_erase_command)
 }

 static const struct command_registration stm32x_exec_command_handlers[] = {
+	{
+		.name = "lock",
+		.handler = stm32x_handle_lock_command,
+		.mode = COMMAND_EXEC,
+		.usage = "bank_id",
+		.help = "Lock entire flash device.",
+	},
+	{
+		.name = "unlock",
+		.handler = stm32x_handle_unlock_command,
+		.mode = COMMAND_EXEC,
+		.usage = "bank_id",
+		.help = "Unlock entire protected flash device.",
+	},
 	{
 		.name = "mass_erase",
 		.handler = stm32x_handle_mass_erase_command,
--- a/src/flash/nor/stm32lx.c
+++ b/src/flash/nor/stm32lx.c
@@ -32,6 +32,7 @@
 #include <helper/binarybuffer.h>
 #include <target/algorithm.h>
 #include <target/armv7m.h>
+#include <target/cortex_m.h>

 /* stm32lx flash register locations */

@@ -119,8 +120,9 @@ static int stm32lx_erase_sector(struct flash_bank *bank, int sector);
 static int stm32lx_wait_until_bsy_clear(struct flash_bank *bank);

 struct stm32lx_flash_bank {
-	struct working_area *write_algorithm;
 	int probed;
+	bool has_dual_banks;
+	uint32_t user_bank_size;
 };

 /* flash bank stm32lx <base> <size> 0 0 <target#>
@@ -142,8 +144,9 @@ FLASH_BANK_COMMAND_HANDLER(stm32lx_flash_bank_command)

 	bank->driver_priv = stm32lx_info;

-	stm32lx_info->write_algorithm = NULL;
 	stm32lx_info->probed = 0;
+	stm32lx_info->has_dual_banks = false;
+	stm32lx_info->user_bank_size = bank->size;

 	return ERROR_OK;
 }
@@ -213,45 +216,35 @@ static int stm32lx_protect(struct flash_bank *bank, int set, int first,
 static int stm32lx_write_half_pages(struct flash_bank *bank, uint8_t *buffer,
 		uint32_t offset, uint32_t count)
 {
-	struct stm32lx_flash_bank *stm32lx_info = bank->driver_priv;
 	struct target *target = bank->target;
-	uint32_t buffer_size = 4096 * 4;
+	uint32_t buffer_size = 16384;
+	struct working_area *write_algorithm;
 	struct working_area *source;
 	uint32_t address = bank->base + offset;

-	struct reg_param reg_params[5];
+	struct reg_param reg_params[3];
 	struct armv7m_algorithm armv7m_info;

 	int retval = ERROR_OK;
-	uint32_t reg32;

-	/* see contib/loaders/flash/stm32lx.s for src */
+	/* see contib/loaders/flash/stm32lx.S for src */

-	static const uint16_t stm32lx_flash_write_code_16[] = {
-	/*	00000000 <write_word-0x4>: */
-			0x2300, /* 0:	2300		movs	r3, #0 */
-			0xe004, /* 2:	e004		b.n	e <test_done> */
+	static const uint8_t stm32lx_flash_write_code[] = {
+		/* write_word: */
+		0x00, 0x23,             /* movs r3, #0 */
+		0x04, 0xe0,             /* b test_done */

-			/*	00000004 <write_word>: */
-			0xf851, 0xcb04, /* 4:	f851 cb04	ldr.w	ip, [r1], #4 */
-			0xf840, 0xcb04, /* 8:	f840 cb04	str.w	ip, [r0], #4 */
-			0x3301, /* c:	3301		adds	r3, #1 */
+		/* write_word: */
+		0x51, 0xf8, 0x04, 0xcb, /* ldr ip, [r1], #4 */
+		0x40, 0xf8, 0x04, 0xcb, /* str ip, [r0], #4 */
+		0x01, 0x33,             /* adds r3, #1 */

-			/*	0000000e <test_done>: */
-			0x4293, /* e:	4293		cmp	r3, r2 */
-			0xd3f8, /* 10:	d3f8		bcc.n	4 <write_word> */
-			0xbe00, /* 12:	be00		bkpt	0x0000 */
+		/* test_done: */
+		0x93, 0x42,             /* cmp r3, r2 */
+		0xf8, 0xd3,             /* bcc write_word */
+		0x00, 0xbe,             /* bkpt 0 */
+	};

-			};
-
-	/* Flip endian */
-	uint8_t stm32lx_flash_write_code[sizeof(stm32lx_flash_write_code_16)];
-	for (unsigned int i = 0; i < sizeof(stm32lx_flash_write_code_16) / 2; i++) {
-		stm32lx_flash_write_code[i * 2 + 0] = stm32lx_flash_write_code_16[i]
-				& 0xff;
-		stm32lx_flash_write_code[i * 2 + 1] = (stm32lx_flash_write_code_16[i]
-				>> 8) & 0xff;
-	}
 	/* Check if there is an even number of half pages (128bytes) */
 	if (count % 128) {
 		LOG_ERROR("there should be an even number "
@@ -259,74 +252,77 @@ static int stm32lx_write_half_pages(struct flash_bank *bank, uint8_t *buffer,
 		return ERROR_FAIL;
 	}

-	/* Allocate working area */
-	reg32 = sizeof(stm32lx_flash_write_code);
-	/* Add bytes to make 4byte aligned */
-	reg32 += (4 - (reg32 % 4)) % 4;
-	retval = target_alloc_working_area(target, reg32,
-			&stm32lx_info->write_algorithm);
-	if (retval != ERROR_OK)
-		return retval;
+	/* flash write code */
+	if (target_alloc_working_area(target, sizeof(stm32lx_flash_write_code),
+			&write_algorithm) != ERROR_OK) {
+		LOG_DEBUG("no working area for block memory writes");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	};

 	/* Write the flashing code */
 	retval = target_write_buffer(target,
-			stm32lx_info->write_algorithm->address,
+			write_algorithm->address,
 			sizeof(stm32lx_flash_write_code),
 			(uint8_t *)stm32lx_flash_write_code);
 	if (retval != ERROR_OK) {
-		target_free_working_area(target, stm32lx_info->write_algorithm);
+		target_free_working_area(target, write_algorithm);
 		return retval;
 	}

 	/* Allocate half pages memory */
-	while (target_alloc_working_area_try(target, buffer_size, &source)
-			!= ERROR_OK) {
+	while (target_alloc_working_area_try(target, buffer_size, &source) != ERROR_OK) {
 		if (buffer_size > 1024)
 			buffer_size -= 1024;
 		else
 			buffer_size /= 2;

 		if (buffer_size <= 256) {
-			/* if we already allocated the writing code, but failed to get a
+			/* we already allocated the writing code, but failed to get a
 			 * buffer, free the algorithm */
-			if (stm32lx_info->write_algorithm)
-				target_free_working_area(target, stm32lx_info->write_algorithm);
+			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;
 		}
 	}
-	LOG_DEBUG("allocated working area for data (%" PRIx32 " bytes)", buffer_size);

 	armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
-	armv7m_info.core_mode = ARMV7M_MODE_ANY;
+	armv7m_info.core_mode = ARM_MODE_THREAD;
 	init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
 	init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
 	init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);
-	init_reg_param(&reg_params[3], "r3", 32, PARAM_IN_OUT);
-	init_reg_param(&reg_params[4], "r4", 32, PARAM_OUT);

 	/* Enable half-page write */
 	retval = stm32lx_enable_write_half_page(bank);
 	if (retval != ERROR_OK) {
 		target_free_working_area(target, source);
-		target_free_working_area(target, stm32lx_info->write_algorithm);
+		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;
 	}

+	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");
+		return retval;
+	}
+
+	/* save any DEMCR flags and configure target to catch any Hard Faults */
+	uint32_t demcr_save = armv7m->demcr;
+	armv7m->demcr = VC_HARDERR;
+
 	/* Loop while there are bytes to write */
 	while (count > 0) {
 		uint32_t this_count;
 		this_count = (count > buffer_size) ? buffer_size : count;

 		/* Write the next half pages */
-		retval = target_write_buffer(target, source->address, this_count,
-				buffer);
+		retval = target_write_buffer(target, source->address, this_count, buffer);
 		if (retval != ERROR_OK)
 			break;

@@ -341,10 +337,14 @@ static int stm32lx_write_half_pages(struct flash_bank *bank, uint8_t *buffer,
 		/* 5: Execute the bunch of code */
 		retval = target_run_algorithm(target, 0, NULL, sizeof(reg_params)
 				/ sizeof(*reg_params), reg_params,
-				stm32lx_info->write_algorithm->address, 0, 20000, &armv7m_info);
+				write_algorithm->address, 0, 10000, &armv7m_info);
 		if (retval != ERROR_OK)
 			break;

+		/* check for Hard Fault */
+		if (armv7m->exception_number == 3)
+			break;
+
 		/* 6: Wait while busy */
 		retval = stm32lx_wait_until_bsy_clear(bank);
 		if (retval != ERROR_OK)
@@ -355,106 +355,163 @@ static int stm32lx_write_half_pages(struct flash_bank *bank, uint8_t *buffer,
 		count -= this_count;
 	}

+	/* restore previous flags */
+	armv7m->demcr = demcr_save;
+
+	if (armv7m->exception_number == 3) {
+
+		/* the stm32l15x devices seem to have an issue when blank.
+		 * if a ram loader is executed on a blank device it will
+		 * Hard Fault, this issue does not happen for a already programmed device.
+		 * A related issue is described in the stm32l151xx errata (Doc ID 17721 Rev 6 - 2.1.3).
+		 * The workaround of handling the Hard Fault exception does work, but makes the
+		 * loader more complicated, as a compromise we manually write the pages, programming time
+		 * is reduced by 50% using this slower method.
+		 */
+
+		LOG_WARNING("couldn't use loader, falling back to page memory writes");
+
+		while (count > 0) {
+			uint32_t this_count;
+			this_count = (count > 128) ? 128 : count;
+
+			/* Write the next half pages */
+			retval = target_write_buffer(target, address, this_count, buffer);
+			if (retval != ERROR_OK)
+				break;
+
+			/* Wait while busy */
+			retval = stm32lx_wait_until_bsy_clear(bank);
+			if (retval != ERROR_OK)
+				break;
+
+			buffer += this_count;
+			address += this_count;
+			count -= this_count;
+		}
+	}
+
 	if (retval == ERROR_OK)
 		retval = stm32lx_lock_program_memory(bank);

 	target_free_working_area(target, source);
-	target_free_working_area(target, stm32lx_info->write_algorithm);
+	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 stm32lx_write(struct flash_bank *bank, uint8_t *buffer,
 		uint32_t offset, uint32_t count)
 {
 	struct target *target = bank->target;

 	uint32_t halfpages_number;
-	uint32_t words_remaining;
-	uint32_t bytes_remaining;
+	uint32_t bytes_remaining = 0;
 	uint32_t address = bank->base + offset;
 	uint32_t bytes_written = 0;
-	int retval;
+	int retval, retval2;

 	if (bank->target->state != TARGET_HALTED) {
 		LOG_ERROR("Target not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}

-	if (offset & 0x1) {
-		LOG_ERROR("offset 0x%" PRIx32 " breaks required 2-byte alignment", offset);
+	if (offset & 0x3) {
+		LOG_ERROR("offset 0x%" PRIx32 " breaks required 4-byte alignment", offset);
 		return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
 	}

-	/* Check if there are some full half pages */
-	if (((offset % 128) == 0) && (count >= 128)) {
-		halfpages_number = count / 128;
-		words_remaining = (count - 128 * halfpages_number) / 4;
-		bytes_remaining = (count & 0x3);
-	} else {
-		halfpages_number = 0;
-		words_remaining = (count / 4);
-		bytes_remaining = (count & 0x3);
-	}
-
-	if (halfpages_number) {
-		retval = stm32lx_write_half_pages(bank, buffer, offset, 128
-				* halfpages_number);
-		if (retval != ERROR_OK)
-			return ERROR_FAIL;
-	}
-
-	bytes_written = 128 * halfpages_number;
-	address += bytes_written;
-
 	retval = stm32lx_unlock_program_memory(bank);
 	if (retval != ERROR_OK)
 		return retval;

-	while (words_remaining > 0) {
-		uint32_t value;
-		uint8_t *p = buffer + bytes_written;
+	/* first we need to write any unaligned head bytes upto
+	 * the next 128 byte page */

-		/* Prepare the word, Little endian conversion */
-		value = p[0] + (p[1] << 8) + (p[2] << 16) + (p[3] << 24);
+	if (offset % 128)
+		bytes_remaining = MIN(count, 128 - (offset % 128));

-		retval = target_write_u32(target, address, value);
+	while (bytes_remaining > 0) {
+		uint8_t value[4] = {0xff, 0xff, 0xff, 0xff};
+
+		/* copy remaining bytes into the write buffer */
+		uint32_t bytes_to_write = MIN(4, bytes_remaining);
+		memcpy(value, buffer + bytes_written, bytes_to_write);
+
+		retval = target_write_buffer(target, address, 4, value);
 		if (retval != ERROR_OK)
-			return retval;
+			goto reset_pg_and_lock;

-		bytes_written += 4;
-		words_remaining--;
+		bytes_written += bytes_to_write;
+		bytes_remaining -= bytes_to_write;
 		address += 4;

 		retval = stm32lx_wait_until_bsy_clear(bank);
 		if (retval != ERROR_OK)
-			return retval;
+			goto reset_pg_and_lock;
 	}

-	if (bytes_remaining) {
-		uint8_t last_word[4] = {0xff, 0xff, 0xff, 0xff};
+	offset += bytes_written;
+	count -= bytes_written;

-		/* copy the last remaining bytes into the write buffer */
-		memcpy(last_word, buffer+bytes_written, bytes_remaining);
+	/* this should always pass this check here */
+	assert((offset % 128) == 0);

-		retval = target_write_buffer(target, address, 4, last_word);
-		if (retval != ERROR_OK)
-			return retval;
+	/* calculate half pages */
+	halfpages_number = count / 128;

-		retval = stm32lx_wait_until_bsy_clear(bank);
-		if (retval != ERROR_OK)
-			return retval;
+	if (halfpages_number) {
+		retval = stm32lx_write_half_pages(bank, buffer + bytes_written, offset, 128 * 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");
+			halfpages_number = 0;
+		} else {
+			if (retval != ERROR_OK)
+				return ERROR_FAIL;
+		}
 	}

-	retval = stm32lx_lock_program_memory(bank);
+	/* write any remaining bytes */
+	uint32_t page_bytes_written = 128 * halfpages_number;
+	bytes_written += page_bytes_written;
+	address += page_bytes_written;
+	bytes_remaining = count - page_bytes_written;
+
+	retval = stm32lx_unlock_program_memory(bank);
 	if (retval != ERROR_OK)
 		return retval;

-	return ERROR_OK;
+	while (bytes_remaining > 0) {
+		uint8_t value[4] = {0xff, 0xff, 0xff, 0xff};
+
+		/* copy remaining bytes into the write buffer */
+		uint32_t bytes_to_write = MIN(4, bytes_remaining);
+		memcpy(value, buffer + bytes_written, bytes_to_write);
+
+		retval = target_write_buffer(target, address, 4, value);
+		if (retval != ERROR_OK)
+			goto reset_pg_and_lock;
+
+		bytes_written += bytes_to_write;
+		bytes_remaining -= bytes_to_write;
+		address += 4;
+
+		retval = stm32lx_wait_until_bsy_clear(bank);
+		if (retval != ERROR_OK)
+			goto reset_pg_and_lock;
+	}
+
+reset_pg_and_lock:
+	retval2 = stm32lx_lock_program_memory(bank);
+	if (retval == ERROR_OK)
+		retval = retval2;
+
+	return retval;
 }

 static int stm32lx_probe(struct flash_bank *bank)
@@ -465,6 +522,9 @@ static int stm32lx_probe(struct flash_bank *bank)
 	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;

@@ -480,23 +540,68 @@ static int stm32lx_probe(struct flash_bank *bank)
 	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;
 	default:
 		LOG_WARNING("Cannot identify target as a STM32L family.");
 		return ERROR_FAIL;
 	}

-	/* get flash size from target. */
+	/* Get the flash size from target. */
 	retval = target_read_u16(target, F_SIZE, &flash_size_in_kb);

-	/* failed reading flash size or flash size invalid (early silicon),
+	/* 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("STM32 flash size failed, probe inaccurate - assuming %dk flash",
+		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) {
+		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;
+	}
+
+	if (stm32lx_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) {
+			/* 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) {
+			/* This is the first bank */
+			flash_size_in_kb = first_bank_size_in_kb;
+		} else {
+			LOG_WARNING("STM32L flash bank base address config is incorrect. 0x%x but should rather be 0x%x or 0x%x",
+						bank->base, base_address, second_bank_base);
+			return ERROR_FAIL;
+		}
+		LOG_INFO("STM32L flash has dual banks. Bank (%d) size is %dkb, base address is 0x%x",
+				bank->bank_number, flash_size_in_kb, base_address);
+	} else {
+		LOG_INFO("STM32L flash size is %dkb, base address is 0x%x", flash_size_in_kb, base_address);
+	}
+
+	/* 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 (stm32lx_info->user_bank_size) {
+		flash_size_in_kb = stm32lx_info->user_bank_size / 1024;
+		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
@@ -504,15 +609,14 @@ static int stm32lx_probe(struct flash_bank *bank)

 	/* calculate numbers of sectors (4kB per sector) */
 	int num_sectors = (flash_size_in_kb * 1024) / FLASH_SECTOR_SIZE;
-	LOG_INFO("flash size = %dkbytes", flash_size_in_kb);

 	if (bank->sectors) {
 		free(bank->sectors);
 		bank->sectors = NULL;
 	}

-	bank->base = FLASH_BANK0_ADDRESS;
 	bank->size = flash_size_in_kb * 1024;
+	bank->base = base_address;
 	bank->num_sectors = num_sectors;
 	bank->sectors = malloc(sizeof(struct flash_sector) * num_sectors);
 	if (bank->sectors == NULL) {
@@ -649,6 +753,10 @@ static int stm32lx_get_info(struct flash_bank *bank, char *buf, int buf_size)
 				snprintf(buf, buf_size, "Z");
 				break;

+			case 0x1018:
+				snprintf(buf, buf_size, "Y");
+				break;
+
 			default:
 				snprintf(buf, buf_size, "unknown");
 				break;
@@ -703,6 +811,14 @@ static int stm32lx_unlock_program_memory(struct flash_bank *bank)
 	 * then by writing the 2 PRGKEY to the PRGKEYR register
 	 */

+	/* check flash is not already unlocked */
+	retval = target_read_u32(target, FLASH_PECR, &reg32);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if ((reg32 & FLASH_PECR__PRGLOCK) == 0)
+		return ERROR_OK;
+
 	/* To unlock the PECR write the 2 PEKEY to the PEKEYR register */
 	retval = target_write_u32(target, FLASH_PEKEYR, PEKEY1);
 	if (retval != ERROR_OK)
@@ -738,6 +854,7 @@ static int stm32lx_unlock_program_memory(struct flash_bank *bank)
 		LOG_ERROR("PRGLOCK is not cleared :(");
 		return ERROR_FLASH_OPERATION_FAILED;
 	}
+
 	return ERROR_OK;
 }

--- a/src/flash/nor/stmsmi.c
+++ b/src/flash/nor/stmsmi.c
@@ -39,6 +39,7 @@
 #endif

 #include "imp.h"
+#include "spi.h"
 #include <jtag/jtag.h>
 #include <helper/time_support.h>

@@ -105,8 +106,6 @@
 #define SMI_SEL_BANK3     0x00003000 /* Select Bank3 */

 /* fields in SMI_SR */
-#define SMI_WIP_BIT       0x00000001 /* WIP Bit of SPI SR on SMI SR */
-#define SMI_WEL_BIT       0x00000002 /* WEL Bit of SPI SR on SMI SR */
 #define SMI_TFF           0x00000100 /* Transfer Finished Flag */

 /* Commands */
@@ -124,65 +123,6 @@ struct stmsmi_flash_bank {
 	struct flash_device *dev;
 };

-/* data structure to maintain flash ids from different vendors */
-struct flash_device {
-	char *name;
-	uint8_t erase_cmd;
-	uint32_t device_id;
-	uint32_t pagesize;
-	unsigned long sectorsize;
-	unsigned long size_in_bytes;
-};
-
-#define FLASH_ID(n, es, id, psize, ssize, size) \
-{	                        \
-	.name = n,              \
-	.erase_cmd = es,        \
-	.device_id = id,        \
-	.pagesize = psize,      \
-	.sectorsize = ssize,    \
-	.size_in_bytes = size   \
-}
-
-/* List below is taken from Linux driver. It is not exhaustive of all the
- * possible SPI memories, nor exclusive for SMI. Could be shared with
- * other SPI drivers. */
-static struct flash_device flash_devices[] = {
-	/* name, erase_cmd, device_id, pagesize, sectorsize, size_in_bytes */
-	FLASH_ID("st m25p05",      0xd8, 0x00102020, 0x80,  0x8000,  0x10000),
-	FLASH_ID("st m25p10",      0xd8, 0x00112020, 0x80,  0x8000,  0x20000),
-	FLASH_ID("st m25p20",      0xd8, 0x00122020, 0x100, 0x10000, 0x40000),
-	FLASH_ID("st m25p40",      0xd8, 0x00132020, 0x100, 0x10000, 0x80000),
-	FLASH_ID("st m25p80",      0xd8, 0x00142020, 0x100, 0x10000, 0x100000),
-	FLASH_ID("st m25p16",      0xd8, 0x00152020, 0x100, 0x10000, 0x200000),
-	FLASH_ID("st m25p32",      0xd8, 0x00162020, 0x100, 0x10000, 0x400000),
-	FLASH_ID("st m25p64",      0xd8, 0x00172020, 0x100, 0x10000, 0x800000),
-	FLASH_ID("st m25p128",     0xd8, 0x00182020, 0x100, 0x40000, 0x1000000),
-	FLASH_ID("st m45pe10",     0xd8, 0x00114020, 0x100, 0x10000, 0x20000),
-	FLASH_ID("st m45pe20",     0xd8, 0x00124020, 0x100, 0x10000, 0x40000),
-	FLASH_ID("st m45pe40",     0xd8, 0x00134020, 0x100, 0x10000, 0x80000),
-	FLASH_ID("st m45pe80",     0xd8, 0x00144020, 0x100, 0x10000, 0x100000),
-	FLASH_ID("sp s25fl004",    0xd8, 0x00120201, 0x100, 0x10000, 0x80000),
-	FLASH_ID("sp s25fl008",    0xd8, 0x00130201, 0x100, 0x10000, 0x100000),
-	FLASH_ID("sp s25fl016",    0xd8, 0x00140201, 0x100, 0x10000, 0x200000),
-	FLASH_ID("sp s25fl032",    0xd8, 0x00150201, 0x100, 0x10000, 0x400000),
-	FLASH_ID("sp s25fl064",    0xd8, 0x00160201, 0x100, 0x10000, 0x800000),
-	FLASH_ID("atmel 25f512",   0x52, 0x0065001f, 0x80,  0x8000,  0x10000),
-	FLASH_ID("atmel 25f1024",  0x52, 0x0060001f, 0x100, 0x8000,  0x20000),
-	FLASH_ID("atmel 25f2048",  0x52, 0x0063001f, 0x100, 0x10000, 0x40000),
-	FLASH_ID("atmel 25f4096",  0x52, 0x0064001f, 0x100, 0x10000, 0x80000),
-	FLASH_ID("atmel 25fs040",  0xd7, 0x0004661f, 0x100, 0x10000, 0x80000),
-	FLASH_ID("mac 25l512",     0xd8, 0x001020c2, 0x010, 0x10000, 0x10000),
-	FLASH_ID("mac 25l1005",    0xd8, 0x001120c2, 0x010, 0x10000, 0x20000),
-	FLASH_ID("mac 25l2005",    0xd8, 0x001220c2, 0x010, 0x10000, 0x40000),
-	FLASH_ID("mac 25l4005",    0xd8, 0x001320c2, 0x010, 0x10000, 0x80000),
-	FLASH_ID("mac 25l8005",    0xd8, 0x001420c2, 0x010, 0x10000, 0x100000),
-	FLASH_ID("mac 25l1605",    0xd8, 0x001520c2, 0x100, 0x10000, 0x200000),
-	FLASH_ID("mac 25l3205",    0xd8, 0x001620c2, 0x100, 0x10000, 0x400000),
-	FLASH_ID("mac 25l6405",    0xd8, 0x001720c2, 0x100, 0x10000, 0x800000),
-	FLASH_ID(NULL,             0,    0,          0,     0,       0)
-};
-
 struct stmsmi_target {
 	char *name;
 	uint32_t tap_idcode;
@@ -282,7 +222,7 @@ static int wait_till_ready(struct flash_bank *bank, int timeout)
 		if (retval != ERROR_OK)
 			return retval;

-		if ((status & SMI_WIP_BIT) == 0)
+		if ((status & SPIFLASH_BSY_BIT) == 0)
 			return ERROR_OK;
 		alive_sleep(1);
 	} while (timeval_ms() < endtime);
@@ -320,7 +260,7 @@ static int smi_write_enable(struct flash_bank *bank)
 		return retval;

 	/* Check write enabled */
-	if ((status & SMI_WEL_BIT) == 0) {
+	if ((status & SPIFLASH_WE_BIT) == 0) {
 		LOG_ERROR("Cannot enable write to flash. Status=0x%08" PRIx32, status);
 		return ERROR_FAIL;
 	}
--- a/src/flash/nor/str7x.c
+++ b/src/flash/nor/str7x.c
@@ -90,7 +90,6 @@ struct str7x_flash_bank {
 	uint32_t disable_bit;
 	uint32_t busy_bits;
 	uint32_t register_base;
-	struct working_area *write_algorithm;
 };

 struct str7x_mem_layout {
@@ -227,8 +226,6 @@ FLASH_BANK_COMMAND_HANDLER(str7x_flash_bank_command)

 	str7x_build_block_list(bank);

-	str7x_info->write_algorithm = NULL;
-
 	return ERROR_OK;
 }

@@ -451,6 +448,7 @@ static int str7x_write_block(struct flash_bank *bank, uint8_t *buffer,
 	struct str7x_flash_bank *str7x_info = bank->driver_priv;
 	struct target *target = bank->target;
 	uint32_t buffer_size = 32768;
+	struct working_area *write_algorithm;
 	struct working_area *source;
 	uint32_t address = bank->base + offset;
 	struct reg_param reg_params[6];
@@ -487,11 +485,11 @@ static int str7x_write_block(struct flash_bank *bank, uint8_t *buffer,

 	/* flash write code */
 	if (target_alloc_working_area_try(target, sizeof(str7x_flash_write_code),
-			&str7x_info->write_algorithm) != ERROR_OK) {
+			&write_algorithm) != ERROR_OK) {
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 	};

-	target_write_buffer(target, str7x_info->write_algorithm->address,
+	target_write_buffer(target, write_algorithm->address,
 			sizeof(str7x_flash_write_code),
 			(uint8_t *)str7x_flash_write_code);

@@ -499,10 +497,9 @@ static int str7x_write_block(struct flash_bank *bank, uint8_t *buffer,
 	while (target_alloc_working_area_try(target, buffer_size, &source) != ERROR_OK) {
 		buffer_size /= 2;
 		if (buffer_size <= 256) {
-			/* if we already allocated the writing code, but failed to get a
+			/* we already allocated the writing code, but failed to get a
 			 * buffer, free the algorithm */
-			if (str7x_info->write_algorithm)
-				target_free_working_area(target, str7x_info->write_algorithm);
+			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;
@@ -532,8 +529,8 @@ static int str7x_write_block(struct flash_bank *bank, uint8_t *buffer,
 		buf_set_u32(reg_params[5].value, 0, 32, str7x_info->busy_bits);

 		retval = target_run_algorithm(target, 0, NULL, 6, reg_params,
-				str7x_info->write_algorithm->address,
-				str7x_info->write_algorithm->address + (sizeof(str7x_flash_write_code) - 4),
+				write_algorithm->address,
+				write_algorithm->address + (sizeof(str7x_flash_write_code) - 4),
 				10000, &arm_algo);
 		if (retval != ERROR_OK)
 			break;
@@ -549,7 +546,7 @@ static int str7x_write_block(struct flash_bank *bank, uint8_t *buffer,
 	}

 	target_free_working_area(target, source);
-	target_free_working_area(target, str7x_info->write_algorithm);
+	target_free_working_area(target, write_algorithm);

 	destroy_reg_param(&reg_params[0]);
 	destroy_reg_param(&reg_params[1]);
--- a/src/flash/nor/str9x.c
+++ b/src/flash/nor/str9x.c
@@ -46,7 +46,6 @@ struct str9x_flash_bank {
 	uint32_t *sector_bits;
 	int variant;
 	int bank1;
-	struct working_area *write_algorithm;
 };

 enum str9x_status_codes {
@@ -158,8 +157,6 @@ FLASH_BANK_COMMAND_HANDLER(str9x_flash_bank_command)

 	str9x_build_block_list(bank);

-	str9x_info->write_algorithm = NULL;
-
 	return ERROR_OK;
 }

@@ -352,9 +349,9 @@ static int str9x_protect(struct flash_bank *bank,
 static int str9x_write_block(struct flash_bank *bank,
 		uint8_t *buffer, uint32_t offset, uint32_t count)
 {
-	struct str9x_flash_bank *str9x_info = bank->driver_priv;
 	struct target *target = bank->target;
 	uint32_t buffer_size = 32768;
+	struct working_area *write_algorithm;
 	struct working_area *source;
 	uint32_t address = bank->base + offset;
 	struct reg_param reg_params[4];
@@ -390,12 +387,12 @@ static int str9x_write_block(struct flash_bank *bank,

 	/* flash write code */
 	if (target_alloc_working_area(target, sizeof(str9x_flash_write_code),
-			&str9x_info->write_algorithm) != ERROR_OK) {
+			&write_algorithm) != ERROR_OK) {
 		LOG_WARNING("no working area available, can't do block memory writes");
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 	};

-	target_write_buffer(target, str9x_info->write_algorithm->address,
+	target_write_buffer(target, write_algorithm->address,
 			sizeof(str9x_flash_write_code),
 			(uint8_t *)str9x_flash_write_code);

@@ -403,10 +400,9 @@ static int str9x_write_block(struct flash_bank *bank,
 	while (target_alloc_working_area_try(target, buffer_size, &source) != ERROR_OK) {
 		buffer_size /= 2;
 		if (buffer_size <= 256) {
-			/* if we already allocated the writing code, but failed to get a
+			/* we already allocated the writing code, but failed to get a
 			 * buffer, free the algorithm */
-			if (str9x_info->write_algorithm)
-				target_free_working_area(target, str9x_info->write_algorithm);
+			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;
@@ -432,7 +428,7 @@ static int str9x_write_block(struct flash_bank *bank,
 		buf_set_u32(reg_params[2].value, 0, 32, thisrun_count);

 		retval = target_run_algorithm(target, 0, NULL, 4, reg_params,
-				str9x_info->write_algorithm->address,
+				write_algorithm->address,
 				0, 10000, &arm_algo);
 		if (retval != ERROR_OK) {
 			LOG_ERROR("error executing str9x flash write algorithm");
@@ -451,7 +447,7 @@ static int str9x_write_block(struct flash_bank *bank,
 	}

 	target_free_working_area(target, source);
-	target_free_working_area(target, str9x_info->write_algorithm);
+	target_free_working_area(target, write_algorithm);

 	destroy_reg_param(&reg_params[0]);
 	destroy_reg_param(&reg_params[1]);
--- a/src/flash/startup.tcl
+++ b/src/flash/startup.tcl
@@ -1,5 +1,75 @@
 # Defines basic Tcl procs for OpenOCD flash module

+#
+# program utility proc
+# usage: program filename
+# optional args: verify, reset and address
+#
+
+proc program {filename args} {
+
+	foreach arg $args {
+		if {[string equal $arg "verify"]} {
+			set verify 1
+		} elseif {[string equal $arg "reset"]} {
+			set reset 1
+		} else {
+			set address $arg
+		}
+	}
+
+	# make sure init is called
+	if {[catch {init}] != 0} {
+		echo "** OpenOCD init Failed **"
+		shutdown
+		return
+	}
+
+	# reset target and call any init scripts
+	if {[catch {reset init}] != 0} {
+		echo "** Unable to reset target **"
+		shutdown
+		return
+	}
+
+	# start programming phase
+	echo "** Programming Started **"
+	if {[info exists address]} {
+		set flash_args "$filename $address"
+	} else {
+		set flash_args "$filename"
+	}
+
+	if {[catch {eval flash write_image erase $flash_args}] == 0} {
+		echo "** Programming Finished **"
+		if {[info exists verify]} {
+			# verify phase
+			echo "** Verify Started **"
+			if {[catch {eval verify_image $flash_args}] == 0} {
+				echo "** Verified OK **"
+			} else {
+				echo "** Verify Failed **"
+			}
+		}
+
+		if {[info exists reset]} {
+			# reset target if requested
+			# also disable target polling, we are shutting down anyway
+			poll off
+			echo "** Resetting Target **"
+			reset run
+		}
+	} else {
+		echo "** Programming Failed **"
+	}
+
+	# shutdown OpenOCD
+	shutdown
+}
+
+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\]"
+
 # ease migration to updated flash driver
 proc stm32x args {
 	echo "DEPRECATED! use 'stm32f1x $args' not 'stm32x $args'"
@@ -10,4 +80,3 @@ proc stm32f2xxx args {
 	echo "DEPRECATED! use 'stm32f2x $args' not 'stm32f2xxx $args'"
 	eval stm32f2x $args
 }
-
--- a/src/helper/binarybuffer.c
+++ b/src/helper/binarybuffer.c
@@ -370,3 +370,30 @@ void bit_copy_discard(struct bit_copy_queue *q)
 		free(qe);
 	}
 }
+
+int unhexify(char *bin, const char *hex, int count)
+{
+	int i, tmp;
+
+	for (i = 0; i < count; i++) {
+		if (sscanf(hex + (2 * i), "%02x", &tmp) != 1)
+			return i;
+		bin[i] = tmp;
+	}
+
+	return i;
+}
+
+int hexify(char *hex, const char *bin, int count, int out_maxlen)
+{
+	int i, cmd_len = 0;
+
+	/* May use a length, or a null-terminated string as input. */
+	if (count == 0)
+		count = strlen(bin);
+
+	for (i = 0; i < count; i++)
+		cmd_len += snprintf(hex + cmd_len, out_maxlen - cmd_len, "%02x", bin[i] & 0xff);
+
+	return cmd_len;
+}
--- a/src/helper/binarybuffer.h
+++ b/src/helper/binarybuffer.h
@@ -156,4 +156,9 @@ int bit_copy_queued(struct bit_copy_queue *q, uint8_t *dst, unsigned dst_offset,
 void bit_copy_execute(struct bit_copy_queue *q);
 void bit_copy_discard(struct bit_copy_queue *q);

+/* functions to convert to/from hex encoded buffer
+ * used in ti-icdi driver and gdb server */
+int unhexify(char *bin, const char *hex, int count);
+int hexify(char *hex, const char *bin, int count, int out_maxlen);
+
 #endif /* BINARYBUFFER_H */
--- a/src/helper/command.c
+++ b/src/helper/command.c
@@ -1313,6 +1313,8 @@ struct command_context *command_init(const char *startup_tcl, Jim_Interp *interp
 	HostOs = "ecos";
 #elif defined(__FreeBSD__)
 	HostOs = "freebsd";
+#elif defined(__NetBSD__)
+	HostOs = "netbsd";
 #elif defined(__OpenBSD__)
 	HostOs = "openbsd";
 #else
--- a/src/helper/command.h
+++ b/src/helper/command.h
@@ -24,7 +24,6 @@
 #ifndef COMMAND_H
 #define COMMAND_H

-#include <jim.h>
 #include <jim-nvp.h>

 /* To achieve C99 printf compatibility in MinGW, gnu_printf should be
--- a/src/helper/configuration.c
+++ b/src/helper/configuration.c
@@ -128,3 +128,47 @@ int parse_config_file(struct command_context *cmd_ctx)

 	return ERROR_OK;
 }
+
+#ifndef _WIN32
+#include <pwd.h>
+#endif
+
+char *get_home_dir(const char *append_path)
+{
+	char *home = getenv("HOME");
+
+	if (home == NULL) {
+
+#ifdef _WIN32
+		home = getenv("USERPROFILE");
+
+		if (home == NULL) {
+
+			char homepath[MAX_PATH];
+			char *drive = getenv("HOMEDRIVE");
+			char *path = getenv("HOMEPATH");
+			if (drive && path) {
+				snprintf(homepath, MAX_PATH, "%s/%s", drive, path);
+				home = homepath;
+			}
+		}
+#else
+		struct passwd *pwd = getpwuid(getuid());
+		if (pwd)
+			home = pwd->pw_dir;
+
+#endif
+	}
+
+	if (home == NULL)
+		return home;
+
+	char *home_path;
+
+	if (append_path)
+		home_path = alloc_printf("%s/%s", home, append_path);
+	else
+		home_path = alloc_printf("%s", home);
+
+	return home_path;
+}
--- a/src/helper/configuration.h
+++ b/src/helper/configuration.h
@@ -40,5 +40,6 @@ int configuration_output_handler(struct command_context *cmd_ctx,
 FILE *open_file_from_path(const char *file, const char *mode);

 char *find_file(const char *name);
+char *get_home_dir(const char *append_path);

 #endif	/* CONFIGURATION_H */
--- a/src/helper/ioutil.c
+++ b/src/helper/ioutil.c
@@ -137,6 +137,7 @@ COMMAND_HANDLER(handle_trunc_command)
 	return ERROR_OK;
 }

+#ifdef HAVE_MALLOC_H
 COMMAND_HANDLER(handle_meminfo_command)
 {
 	static int prev;
@@ -155,7 +156,7 @@ COMMAND_HANDLER(handle_meminfo_command)

 	return ERROR_OK;
 }
-
+#endif

 COMMAND_HANDLER(handle_append_command)
 {
@@ -487,6 +488,8 @@ static int ioutil_Jim_Command_ip(Jim_Interp *interp, int argc,
 	return JIM_OK;
 }

+#ifdef HAVE_SYS_IOCTL_H
+#ifdef SIOCGIFHWADDR
 /* not so pretty code to fish out eth0 mac address */
 static int ioutil_Jim_Command_mac(Jim_Interp *interp, int argc,
 	Jim_Obj *const *argv)
@@ -545,6 +548,8 @@ static int ioutil_Jim_Command_mac(Jim_Interp *interp, int argc,
 	return JIM_ERR;

 }
+#endif
+#endif

 static const struct command_registration ioutil_command_handlers[] = {
 	{
@@ -575,12 +580,14 @@ static const struct command_registration ioutil_command_handlers[] = {
 		.help = "append a variable number of strings to a file",
 		.usage = "file_name [<string1>, [<string2>, ...]]",
 	},
+#ifdef HAVE_MALLOC_H
 	{
 		.name = "meminfo",
 		.handler = handle_meminfo_command,
 		.mode = COMMAND_ANY,
 		.help = "display free heap space",
 	},
+#endif
 	{
 		.name = "rm",
 		.mode = COMMAND_ANY,
@@ -616,12 +623,16 @@ static const struct command_registration ioutil_command_handlers[] = {
 		.help = "show a listing of files",
 		.usage = "dirname",
 	},
+#ifdef HAVE_SYS_IOCTL_H
+#ifdef SIOCGIFHWADDR
 	{
 		.name = "mac",
 		.mode = COMMAND_ANY,
 		.jim_handler = ioutil_Jim_Command_mac,
 		.help = "show MAC address",
 	},
+#endif
+#endif
 	{
 		.name = "ip",
 		.jim_handler = ioutil_Jim_Command_ip,
--- a/src/helper/replacements.c
+++ b/src/helper/replacements.c
@@ -89,12 +89,9 @@ int gettimeofday(struct timeval *tv, struct timezone *tz)
 		GetSystemTimeAsFileTime(&ft);
 		li.LowPart  = ft.dwLowDateTime;
 		li.HighPart = ft.dwHighDateTime;
-		t  = li.QuadPart;					/* In 100-nanosecond
-									 *intervals */
-		t -= EPOCHFILETIME;					/* Offset to the Epoch time
-									 **/
-		t /= 10;							/* In microseconds
-										 **/
+		t  = li.QuadPart;					/* In 100-nanosecond intervals */
+		t -= EPOCHFILETIME;					/* Offset to the Epoch time */
+		t /= 10;							/* In microseconds */
 		tv->tv_sec  = (long)(t / 1000000);
 		tv->tv_usec = (long)(t % 1000000);
 	}
@@ -209,10 +206,11 @@ int win_select(int max_fd, fd_set *rfds, fd_set *wfds, fd_set *efds, struct time
 			aexcept = sock_except;

 			tvslice.tv_sec = 0;
-			tvslice.tv_usec = 100000;
+			tvslice.tv_usec = 1000;

 			retcode = select(sock_max_fd + 1, &aread, &awrite, &aexcept, &tvslice);
 		}
+
 		if (n_handles > 0) {
 			/* check handles */
 			DWORD wret;
@@ -220,7 +218,7 @@ int win_select(int max_fd, fd_set *rfds, fd_set *wfds, fd_set *efds, struct time
 			wret = MsgWaitForMultipleObjects(n_handles,
 					handles,
 					FALSE,
-					retcode > 0 ? 0 : 100,
+					retcode > 0 ? 0 : 1,
 					QS_ALLEVENTS);

 			if (wret == WAIT_TIMEOUT) {
@@ -244,16 +242,13 @@ int win_select(int max_fd, fd_set *rfds, fd_set *wfds, fd_set *efds, struct time

 							if (PeekNamedPipe((HANDLE)handle, NULL, 0,
 								    NULL, &dwBytes, NULL)) {
-								/* check to see if gdb pipe has data
-								 *available */
+								/* check to see if gdb pipe has data available */
 								if (dwBytes) {
-									FD_SET(handle_slot_to_fd[i],
-										&aread);
+									FD_SET(handle_slot_to_fd[i], &aread);
 									retcode++;
 								}
 							} else {
-								FD_SET(handle_slot_to_fd[i],
-									&aread);
+								FD_SET(handle_slot_to_fd[i], &aread);
 								retcode++;
 							}
 						}
--- a/src/jtag/Makefile.am
+++ b/src/jtag/Makefile.am
@@ -39,9 +39,9 @@ else
 MINIDRIVER_IMP_DIR = $(srcdir)/drivers
 DRIVERFILES += commands.c

-if STLINK
-SUBDIRS += stlink
-libjtag_la_LIBADD += $(top_builddir)/src/jtag/stlink/libocdstlink.la
+if HLADAPTER
+SUBDIRS += hla
+libjtag_la_LIBADD += $(top_builddir)/src/jtag/hla/libocdhla.la
 endif

 SUBDIRS += drivers
--- a/src/jtag/adapter.c
+++ b/src/jtag/adapter.c
@@ -274,7 +274,7 @@ COMMAND_HANDLER(handle_reset_config_command)
 			goto next;

 		/* srst_type (NOP without HAS_SRST) */
-		m |= RESET_SRST_PUSH_PULL;
+		m = RESET_SRST_PUSH_PULL;
 		if (strcmp(*CMD_ARGV, "srst_push_pull") == 0)
 			tmp |= RESET_SRST_PUSH_PULL;
 		else if (strcmp(*CMD_ARGV, "srst_open_drain") == 0)
@@ -289,6 +289,22 @@ COMMAND_HANDLER(handle_reset_config_command)
 		if (m)
 			goto next;

+		/* connect_type - only valid when srst_nogate */
+		m = RESET_CNCT_UNDER_SRST;
+		if (strcmp(*CMD_ARGV, "connect_assert_srst") == 0)
+			tmp |= RESET_CNCT_UNDER_SRST;
+		else if (strcmp(*CMD_ARGV, "connect_deassert_srst") == 0)
+			/* connect normally - default */;
+		else
+			m = 0;
+		if (mask & m) {
+			LOG_ERROR("extra reset_config %s spec (%s)",
+					"connect_type", *CMD_ARGV);
+			return ERROR_COMMAND_SYNTAX_ERROR;
+		}
+		if (m)
+			goto next;
+
 		/* caller provided nonsense; fail */
 		LOG_ERROR("unknown reset_config flag (%s)", *CMD_ARGV);
 		return ERROR_COMMAND_SYNTAX_ERROR;
@@ -314,7 +330,7 @@ next:
 	/*
 	 * Display the (now-)current reset mode
 	 */
-	char *modes[5];
+	char *modes[6];

 	/* minimal JTAG has neither SRST nor TRST (so that's the default) */
 	switch (new_cfg & (RESET_HAS_TRST | RESET_HAS_SRST)) {
@@ -368,14 +384,20 @@ next:
 			modes[4] = " srst_push_pull";
 		else
 			modes[4] = " srst_open_drain";
+
+		if (new_cfg & RESET_CNCT_UNDER_SRST)
+			modes[5] = " connect_assert_srst";
+		else
+			modes[5] = " connect_deassert_srst";
 	} else {
 		modes[2] = "";
 		modes[4] = "";
+		modes[5] = "";
 	}

-	command_print(CMD_CTX, "%s %s%s%s%s",
+	command_print(CMD_CTX, "%s %s%s%s%s%s",
 			modes[0], modes[1],
-			modes[2], modes[3], modes[4]);
+			modes[2], modes[3], modes[4], modes[5]);

 	return ERROR_OK;
 }
--- a/src/jtag/core.c
+++ b/src/jtag/core.c
@@ -1371,6 +1371,11 @@ int adapter_init(struct command_context *cmd_ctx)
 			return retval;
 	}

+	if (jtag->speed == NULL) {
+		LOG_INFO("This adapter doesn't support configurable speed");
+		return ERROR_OK;
+	}
+
 	if (CLOCK_MODE_UNSELECTED == clock_mode) {
 		LOG_ERROR("An adapter speed is not selected in the init script."
 			" Insert a call to adapter_khz or jtag_rclk to proceed.");
@@ -1549,7 +1554,17 @@ int jtag_init_reset(struct command_context *cmd_ctx)
 		if ((jtag_reset_config & RESET_SRST_PULLS_TRST) == 0)
 			jtag_add_reset(0, 1);
 	}
-	jtag_add_reset(0, 0);
+
+	/* some targets enable us to connect with srst asserted */
+	if (jtag_reset_config & RESET_CNCT_UNDER_SRST) {
+		if (jtag_reset_config & RESET_SRST_NO_GATING)
+			jtag_add_reset(0, 1);
+		else {
+			LOG_WARNING("\'srst_nogate\' reset_config option is required");
+			jtag_add_reset(0, 0);
+		}
+	} else
+		jtag_add_reset(0, 0);
 	retval = jtag_execute_queue();
 	if (retval != ERROR_OK)
 		return retval;
@@ -1572,6 +1587,14 @@ int jtag_init(struct command_context *cmd_ctx)

 	/* guard against oddball hardware: force resets to be inactive */
 	jtag_add_reset(0, 0);
+
+	/* some targets enable us to connect with srst asserted */
+	if (jtag_reset_config & RESET_CNCT_UNDER_SRST) {
+		if (jtag_reset_config & RESET_SRST_NO_GATING)
+			jtag_add_reset(0, 1);
+		else
+			LOG_WARNING("\'srst_nogate\' reset_config option is required");
+	}
 	retval = jtag_execute_queue();
 	if (retval != ERROR_OK)
 		return retval;
--- a/src/jtag/drivers/Makefile.am
+++ b/src/jtag/drivers/Makefile.am
@@ -98,8 +98,9 @@ endif
 if REMOTE_BITBANG
 DRIVERFILES += remote_bitbang.c
 endif
-if STLINK
+if HLADAPTER
 DRIVERFILES += stlink_usb.c
+DRIVERFILES += ti_icdi_usb.c
 endif
 if OSBDM
 DRIVERFILES += osbdm.c
@@ -107,6 +108,9 @@ endif
 if OPENDOUS
 DRIVERFILES += opendous.c
 endif
+if SYSFSGPIO
+DRIVERFILES += sysfsgpio.c
+endif

 noinst_HEADERS = \
 	bitbang.h \
--- a/src/jtag/drivers/amt_jtagaccel.c
+++ b/src/jtag/drivers/amt_jtagaccel.c
@@ -264,7 +264,10 @@ static void amt_jtagaccel_scan(bool ir_scan, enum scan_type type, uint8_t *buffe
 	else
 		amt_jtagaccel_end_state(TAP_DRSHIFT);

-	amt_jtagaccel_state_move();
+	/* Only move if we're not already there */
+	if (tap_get_state() != tap_get_end_state())
+		amt_jtagaccel_state_move();
+
 	amt_jtagaccel_end_state(saved_end_state);

 	/* handle unaligned bits at the beginning */
--- a/src/jtag/drivers/arm-jtag-ew.c
+++ b/src/jtag/drivers/arm-jtag-ew.c
@@ -355,7 +355,10 @@ static void armjtagew_scan(bool ir_scan,
 	/* Move to appropriate scan state */
 	armjtagew_end_state(ir_scan ? TAP_IRSHIFT : TAP_DRSHIFT);

-	armjtagew_state_move();
+	/* Only move if we're not already there */
+	if (tap_get_state() != tap_get_end_state())
+		armjtagew_state_move();
+
 	armjtagew_end_state(saved_end_state);

 	/* Scan */
--- a/src/jtag/drivers/at91rm9200.c
+++ b/src/jtag/drivers/at91rm9200.c
@@ -115,7 +115,6 @@ static int at91rm9200_read(void);
 static void at91rm9200_write(int tck, int tms, int tdi);
 static void at91rm9200_reset(int trst, int srst);

-static int at91rm9200_speed(int speed);
 static int at91rm9200_init(void);
 static int at91rm9200_quit(void);

@@ -163,12 +162,6 @@ static void at91rm9200_reset(int trst, int srst)
 		pio_base[device->SRST_PIO + PIO_CODR] = device->SRST_MASK;
 }

-static int at91rm9200_speed(int speed)
-{
-
-	return ERROR_OK;
-}
-
 COMMAND_HANDLER(at91rm9200_handle_device_command)
 {
 	if (CMD_ARGC == 0)
@@ -196,7 +189,6 @@ static const struct command_registration at91rm9200_command_handlers[] = {
 struct jtag_interface at91rm9200_interface = {
 	.name = "at91rm9200",
 	.execute_queue = bitbang_execute_queue,
-	.speed = at91rm9200_speed,
 	.commands = at91rm9200_command_handlers,
 	.init = at91rm9200_init,
 	.quit = at91rm9200_quit,
--- a/src/jtag/drivers/buspirate.c
+++ b/src/jtag/drivers/buspirate.c
@@ -32,8 +32,6 @@
 #undef DEBUG_SERIAL
 /*#define DEBUG_SERIAL */
 static int buspirate_execute_queue(void);
-static int buspirate_speed(int speed);
-static int buspirate_khz(int khz, int *jtag_speed);
 static int buspirate_init(void);
 static int buspirate_quit(void);

@@ -117,19 +115,6 @@ static int buspirate_serial_read(int fd, char *buf, int size);
 static void buspirate_serial_close(int fd);
 static void buspirate_print_buffer(char *buf, int size);

-static int buspirate_speed(int speed)
-{
-	/* TODO */
-	LOG_INFO("Want to set speed to %dkHz, but not implemented yet", speed);
-	return ERROR_OK;
-}
-
-static int buspirate_khz(int khz, int *jtag_speed)
-{
-	*jtag_speed = khz;
-	return ERROR_OK;
-}
-
 static int buspirate_execute_queue(void)
 {
 	/* currently processed command */
@@ -426,8 +411,6 @@ static const struct command_registration buspirate_command_handlers[] = {
 struct jtag_interface buspirate_interface = {
 	.name = "buspirate",
 	.execute_queue = buspirate_execute_queue,
-	.speed = buspirate_speed,
-	.khz = buspirate_khz,
 	.commands = buspirate_command_handlers,
 	.init = buspirate_init,
 	.quit = buspirate_quit
@@ -520,7 +503,10 @@ static void buspirate_scan(bool ir_scan, enum scan_type type,
 	saved_end_state = tap_get_end_state();

 	buspirate_end_state(ir_scan ? TAP_IRSHIFT : TAP_DRSHIFT);
-	buspirate_state_move();
+
+	/* Only move if we're not already there */
+	if (tap_get_state() != tap_get_end_state())
+		buspirate_state_move();

 	buspirate_tap_append_scan(scan_size, buffer, command);

--- a/src/jtag/drivers/ep93xx.c
+++ b/src/jtag/drivers/ep93xx.c
@@ -47,7 +47,6 @@ static int ep93xx_read(void);
 static void ep93xx_write(int tck, int tms, int tdi);
 static void ep93xx_reset(int trst, int srst);

-static int ep93xx_speed(int speed);
 static int ep93xx_init(void);
 static int ep93xx_quit(void);

@@ -59,7 +58,6 @@ struct jtag_interface ep93xx_interface = {
 	.supported = DEBUG_CAP_TMS_SEQ,
 	.execute_queue = bitbang_execute_queue,

-	.speed = ep93xx_speed,
 	.init = ep93xx_init,
 	.quit = ep93xx_quit,
 };
@@ -114,12 +112,6 @@ static void ep93xx_reset(int trst, int srst)
 	nanosleep(&ep93xx_zzzz, NULL);
 }

-static int ep93xx_speed(int speed)
-{
-
-	return ERROR_OK;
-}
-
 static int set_gonk_mode(void)
 {
 	void *syscon;
--- a/src/jtag/drivers/ft2232.c
+++ b/src/jtag/drivers/ft2232.c
@@ -135,7 +135,7 @@ enum ftdi_interface {
 #if BUILD_FT2232_FTD2XX == 1
 	enum { FT_DEVICE_2232H = 6, FT_DEVICE_4232H, FT_DEVICE_232H };
 #elif BUILD_FT2232_LIBFTDI == 1
-	enum { TYPE_2232H = 4, TYPE_4232H = 5, TYPE_232H = 6 };
+	enum ftdi_chip_type { TYPE_2232H = 4, TYPE_4232H = 5, TYPE_232H = 6 };
 #endif
 #endif

@@ -156,6 +156,7 @@ static char *ft2232_device_desc;
 static char *ft2232_serial;
 static uint8_t ft2232_latency = 2;
 static unsigned ft2232_max_tck = FTDI_2232C_MAX_TCK;
+static int ft2232_channel = INTERFACE_ANY;

 #define MAX_USB_IDS 8
 /* vid = pid = 0 marks the end of the list */
@@ -1060,7 +1061,8 @@ static void ft2232_add_scan(bool ir_scan, enum scan_type type, uint8_t *buffer,
 			/* LOG_DEBUG("added TDI bits (i %i)", bits_left - 1); */
 		}
 		buffer_write(0x0);
-		buffer_write(last_bit);
+		if (type != SCAN_IN)
+			buffer_write(last_bit);
 	} else {
 		int tms_bits;
 		int tms_count;
@@ -1106,6 +1108,11 @@ static int ft2232_large_scan(struct scan_command *cmd,
 	int retval;
 	int thisrun_read = 0;

+	if (!receive_buffer) {
+		LOG_ERROR("failed to allocate memory");
+		exit(-1);
+	}
+
 	if (cmd->ir_scan) {
 		LOG_ERROR("BUG: large IR scans are not supported");
 		exit(-1);
@@ -1271,6 +1278,8 @@ static int ft2232_large_scan(struct scan_command *cmd,
 			(int)bytes_read);
 	}

+	free(receive_buffer);
+
 	return ERROR_OK;
 }

@@ -2358,7 +2367,7 @@ static int ft2232_init(void)
 				more, &try_more);
 #elif BUILD_FT2232_LIBFTDI == 1
 		retval = ft2232_init_libftdi(ft2232_vid[i], ft2232_pid[i],
-				more, &try_more, layout->channel);
+				more, &try_more, ft2232_channel);
 #endif
 		if (retval >= 0)
 			break;
@@ -3132,9 +3141,8 @@ static void flossjtag_blink(void)
 static int ft2232_quit(void)
 {
 #if BUILD_FT2232_FTD2XX == 1
-	FT_STATUS status;

-	status = FT_Close(ftdih);
+	FT_Close(ftdih);
 #elif BUILD_FT2232_LIBFTDI == 1
 	ftdi_usb_close(&ftdic);

@@ -3203,6 +3211,7 @@ COMMAND_HANDLER(ft2232_handle_layout_command)
 	for (const struct ft2232_layout *l = ft2232_layouts; l->name; l++) {
 		if (strcmp(l->name, CMD_ARGV[0]) == 0) {
 			layout = l;
+			ft2232_channel = l->channel;
 			return ERROR_OK;
 		}
 	}
@@ -3251,6 +3260,18 @@ COMMAND_HANDLER(ft2232_handle_latency_command)
 	return ERROR_OK;
 }

+COMMAND_HANDLER(ft2232_handle_channel_command)
+{
+	if (CMD_ARGC == 1) {
+		ft2232_channel = atoi(CMD_ARGV[0]);
+		if (ft2232_channel < 0 || ft2232_channel > 4)
+			LOG_ERROR("ft2232_channel must be in the 0 to 4 range");
+	} else
+		LOG_ERROR("expected exactly one argument to ft2232_channel <ch>");
+
+	return ERROR_OK;
+}
+
 static int ft2232_stableclocks(int num_cycles, struct jtag_command *cmd)
 {
 	int retval = 0;
@@ -4258,6 +4279,13 @@ static const struct command_registration ft2232_command_handlers[] = {
 		.help = "set the FT2232 latency timer to a new value",
 		.usage = "value",
 	},
+	{
+		.name = "ft2232_channel",
+		.handler = &ft2232_handle_channel_command,
+		.mode = COMMAND_CONFIG,
+		.help = "set the FT2232 channel to a new value",
+		.usage = "value",
+	},
 	COMMAND_REGISTRATION_DONE
 };

--- a/src/jtag/drivers/ftd2xx_common.h
+++ b/src/jtag/drivers/ftd2xx_common.h
@@ -22,7 +22,7 @@
 #ifndef _FTD2XX_COMMON_H
 #define _FTD2XX_COMMON_H

-#if BUILD_FT2232_FTD2XX == 1
+#if ((BUILD_FT2232_FTD2XX == 1) || (BUILD_PRESTO_FTD2XX == 1) || (BUILD_USB_BLASTER_FTD2XX == 1))
 #include <ftd2xx.h>

 static const char *ftd2xx_status_string(FT_STATUS status)
--- a/src/jtag/drivers/ftdi.c
+++ b/src/jtag/drivers/ftdi.c
@@ -175,7 +175,7 @@ static int ftdi_set_signal(const struct signal *s, char value)

 	output = data ? output | s->data_mask : output & ~s->data_mask;
 	if (s->oe_mask == s->data_mask)
-		direction = oe ? output | s->oe_mask : output & ~s->oe_mask;
+		direction = oe ? direction | s->oe_mask : direction & ~s->oe_mask;
 	else
 		output = oe ? output | s->oe_mask : output & ~s->oe_mask;

@@ -247,6 +247,11 @@ static int ftdi_speed_div(int speed, int *khz)

 static int ftdi_khz(int khz, int *jtag_speed)
 {
+	if (khz == 0 && !mpsse_is_high_speed(mpsse_ctx)) {
+		LOG_DEBUG("RCLK not supported");
+		return ERROR_FAIL;
+	}
+
 	*jtag_speed = khz * 1000;
 	return ERROR_OK;
 }
@@ -638,7 +643,7 @@ static int ftdi_initialize(void)
 	if (retval == ERROR_OK)
 		retval = mpsse_set_data_bits_high_byte(mpsse_ctx, output >> 8, direction >> 8);
 	if (retval != ERROR_OK)	{
-		LOG_ERROR("couldn't initialize FTDI with 'JTAGkey' layout");
+		LOG_ERROR("couldn't initialize FTDI with configured layout");
 		return ERROR_JTAG_INIT_FAILED;
 	}

--- a/src/jtag/drivers/gw16012.c
+++ b/src/jtag/drivers/gw16012.c
@@ -146,12 +146,6 @@ static void gw16012_reset(int trst, int srst)
 		gw16012_control(0x0b);
 }

-static int gw16012_speed(int speed)
-{
-
-	return ERROR_OK;
-}
-
 static void gw16012_end_state(tap_state_t state)
 {
 	if (tap_is_state_stable(state))
@@ -547,6 +541,5 @@ struct jtag_interface gw16012_interface = {

 	.init = gw16012_init,
 	.quit = gw16012_quit,
-	.speed = gw16012_speed,
 	.execute_queue = gw16012_execute_queue,
 };
--- a/src/jtag/drivers/libusb0_common.c
+++ b/src/jtag/drivers/libusb0_common.c
@@ -28,7 +28,7 @@
 static bool jtag_libusb_match(struct jtag_libusb_device *dev,
 		const uint16_t vids[], const uint16_t pids[])
 {
-	for (unsigned i = 0; vids[i] && pids[i]; i++) {
+	for (unsigned i = 0; vids[i]; i++) {
 		if (dev->descriptor.idVendor == vids[i] &&
 			dev->descriptor.idProduct == pids[i]) {
 			return true;
@@ -67,6 +67,21 @@ void jtag_libusb_close(jtag_libusb_device_handle *dev)
 	usb_close(dev);
 }

+int jtag_libusb_control_transfer(jtag_libusb_device_handle *dev, uint8_t requestType,
+		uint8_t request, uint16_t wValue, uint16_t wIndex, char *bytes,
+		uint16_t size, unsigned int timeout)
+{
+	int transferred = 0;
+
+	transferred = usb_control_msg(dev, requestType, request, wValue, wIndex,
+				bytes, size, timeout);
+
+	if (transferred < 0)
+		transferred = 0;
+
+	return transferred;
+}
+
 int jtag_libusb_bulk_write(jtag_libusb_device_handle *dev, int ep, char *bytes,
 		int size, int timeout)
 {
--- a/src/jtag/drivers/libusb0_common.h
+++ b/src/jtag/drivers/libusb0_common.h
@@ -35,6 +35,12 @@
 #define jtag_libusb_reset_device(dev)		usb_reset(dev)
 #define jtag_libusb_get_device(devh)		usb_device(devh)

+/* make some defines compatible to libusb1 */
+#define LIBUSB_REQUEST_TYPE_VENDOR		USB_TYPE_VENDOR
+#define LIBUSB_RECIPIENT_DEVICE			USB_RECIP_DEVICE
+#define LIBUSB_ENDPOINT_OUT				USB_ENDPOINT_OUT
+#define LIBUSB_ENDPOINT_IN				USB_ENDPOINT_IN
+
 static inline int jtag_libusb_claim_interface(jtag_libusb_device_handle *devh,
 				       int iface)
 {
@@ -44,6 +50,9 @@ static inline int jtag_libusb_claim_interface(jtag_libusb_device_handle *devh,
 int jtag_libusb_open(const uint16_t vids[], const uint16_t pids[],
 		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,
+		uint8_t requestType, uint8_t request, uint16_t wValue,
+		uint16_t wIndex, char *bytes,	uint16_t size, unsigned int timeout);
 int jtag_libusb_bulk_write(struct jtag_libusb_device_handle *dev, int ep,
 		char *bytes, int size, int timeout);
 int jtag_libusb_bulk_read(struct jtag_libusb_device_handle *dev, int ep,
--- a/src/jtag/drivers/libusb1_common.c
+++ b/src/jtag/drivers/libusb1_common.c
@@ -33,7 +33,7 @@ static bool jtag_libusb_match(struct jtag_libusb_device *dev,
 {
 	struct libusb_device_descriptor dev_desc;

-	for (unsigned i = 0; vids[i] && pids[i]; i++) {
+	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])
@@ -51,8 +51,6 @@ int jtag_libusb_open(const uint16_t vids[], const uint16_t pids[],
 	if (libusb_init(&jtag_libusb_context) < 0)
 		return -ENODEV;

-	libusb_set_debug(jtag_libusb_context, 3);
-
 	cnt = libusb_get_device_list(jtag_libusb_context, &devs);

 	for (idx = 0; idx < cnt; idx++) {
@@ -79,6 +77,21 @@ void jtag_libusb_close(jtag_libusb_device_handle *dev)
 	libusb_exit(jtag_libusb_context);
 }

+int jtag_libusb_control_transfer(jtag_libusb_device_handle *dev, uint8_t requestType,
+		uint8_t request, uint16_t wValue, uint16_t wIndex, char *bytes,
+		uint16_t size, unsigned int timeout)
+{
+	int transferred = 0;
+
+	transferred = libusb_control_transfer(dev, requestType, request, wValue, wIndex,
+				(unsigned char *)bytes, size, timeout);
+
+	if (transferred < 0)
+		transferred = 0;
+
+	return transferred;
+}
+
 int jtag_libusb_bulk_write(jtag_libusb_device_handle *dev, int ep, char *bytes,
 		int size, int timeout)
 {
--- a/src/jtag/drivers/libusb1_common.h
+++ b/src/jtag/drivers/libusb1_common.h
@@ -44,6 +44,9 @@ static inline int jtag_libusb_claim_interface(jtag_libusb_device_handle *devh,
 int jtag_libusb_open(const uint16_t vids[], const uint16_t pids[],
 		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,
+		uint8_t requestType, uint8_t request, uint16_t wValue,
+		uint16_t wIndex, char *bytes,	uint16_t size, unsigned int timeout);
 int jtag_libusb_bulk_write(struct jtag_libusb_device_handle *dev, int ep,
 		char *bytes,	int size, int timeout);
 int jtag_libusb_bulk_read(struct jtag_libusb_device_handle *dev, int ep,
--- a/src/jtag/drivers/mpsse.c
+++ b/src/jtag/drivers/mpsse.c
@@ -779,11 +779,8 @@ int mpsse_flush(struct mpsse_ctx *ctx)
 	if (ctx->read_count) {
 		buffer_write_byte(ctx, 0x87); /* SEND_IMMEDIATE */
 		read_result.done = false;
-		read_transfer = libusb_alloc_transfer(0);
-		libusb_fill_bulk_transfer(read_transfer, ctx->usb_dev, ctx->in_ep, ctx->read_chunk,
-			ctx->read_chunk_size, read_cb, &read_result,
-			ctx->usb_read_timeout);
-		retval = libusb_submit_transfer(read_transfer);
+		/* delay read transaction to ensure the FTDI chip can support us with data
+		   immediately after processing the MPSSE commands in the write transaction */
 	}

 	struct transfer_result write_result = { .ctx = ctx, .done = false };
@@ -792,6 +789,14 @@ int mpsse_flush(struct mpsse_ctx *ctx)
 		ctx->write_count, write_cb, &write_result, ctx->usb_write_timeout);
 	retval = libusb_submit_transfer(write_transfer);

+	if (ctx->read_count) {
+		read_transfer = libusb_alloc_transfer(0);
+		libusb_fill_bulk_transfer(read_transfer, ctx->usb_dev, ctx->in_ep, ctx->read_chunk,
+			ctx->read_chunk_size, read_cb, &read_result,
+			ctx->usb_read_timeout);
+		retval = libusb_submit_transfer(read_transfer);
+	}
+
 	/* Polling loop, more or less taken from libftdi */
 	while (!write_result.done || !read_result.done) {
 		retval = libusb_handle_events(ctx->usb_ctx);
--- a/src/jtag/drivers/opendous.c
+++ b/src/jtag/drivers/opendous.c
@@ -39,39 +39,59 @@
 #include <sys/timeb.h>
 #include <time.h>

-#define ESTICK_VID 0x1781
-#define ESTICK_PID 0xC0C0
+#define OPENDOUS_MAX_VIDS_PIDS 4
+/* define some probes with similar interface */
+struct opendous_probe {
+	char *name;
+	uint16_t VID[OPENDOUS_MAX_VIDS_PIDS];
+	uint16_t PID[OPENDOUS_MAX_VIDS_PIDS];
+	uint8_t READ_EP;
+	uint8_t WRITE_EP;
+	uint8_t CONTROL_TRANSFER;
+	int BUFFERSIZE;
+};

-#define OPENDOUS_VID 0x03EB
-#define OPENDOUS_PID 0x204F
+static 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 },
+	{NULL,				{0x0000},				{0x0000},				0x00, 0x00, 0x00,   0 }
+};

-/* pid could be specified at runtime */
-static uint16_t vids[] = { OPENDOUS_VID, ESTICK_VID, 0 };
-static uint16_t pids[] = { OPENDOUS_PID, ESTICK_PID, 0 };
-
-#define OPENDOUS_WRITE_ENDPOINT   0x02
-#define OPENDOUS_READ_ENDPOINT    0x81
+#define OPENDOUS_WRITE_ENDPOINT   (opendous_probe->WRITE_EP)
+#define OPENDOUS_READ_ENDPOINT    (opendous_probe->READ_EP)

 static unsigned int opendous_hw_jtag_version = 1;

 #define OPENDOUS_USB_TIMEOUT      1000

-#define OPENDOUS_USB_BUFFER_SIZE  360
+#define OPENDOUS_USB_BUFFER_SIZE  (opendous_probe->BUFFERSIZE)
 #define OPENDOUS_IN_BUFFER_SIZE   (OPENDOUS_USB_BUFFER_SIZE)
 #define OPENDOUS_OUT_BUFFER_SIZE  (OPENDOUS_USB_BUFFER_SIZE)

 /* Global USB buffers */
-static uint8_t usb_in_buffer[OPENDOUS_IN_BUFFER_SIZE];
-static uint8_t usb_out_buffer[OPENDOUS_OUT_BUFFER_SIZE];
+static uint8_t *usb_in_buffer;
+static uint8_t *usb_out_buffer;

 /* Constants for OPENDOUS command */

 #define OPENDOUS_MAX_SPEED			66
-#define OPENDOUS_MAX_TAP_TRANSMIT	350	/* even number is easier to handle */
+#define OPENDOUS_MAX_TAP_TRANSMIT	((opendous_probe->BUFFERSIZE)-10)
 #define OPENDOUS_MAX_INPUT_DATA		(OPENDOUS_MAX_TAP_TRANSMIT*4)

+/* TAP */
 #define OPENDOUS_TAP_BUFFER_SIZE 65536

+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;
+};
+
+static int pending_scan_results_length;
+static struct pending_scan_result *pending_scan_results_buffer;
+
 #define MAX_PENDING_SCAN_RESULTS (OPENDOUS_MAX_INPUT_DATA)

 /* JTAG usb commands */
@@ -84,11 +104,16 @@ static uint8_t usb_out_buffer[OPENDOUS_OUT_BUFFER_SIZE];
 #define JTAG_CMD_SET_SRST_TRST	0x6
 #define JTAG_CMD_READ_CONFIG	0x7

+/* usbvlab control transfer */
+#define FUNC_START_BOOTLOADER 30
+#define FUNC_WRITE_DATA       0x50
+#define FUNC_READ_DATA        0x51
+
+static char *opendous_type;
+static struct opendous_probe *opendous_probe;
+
 /* External interface functions */
 static int opendous_execute_queue(void);
-static int opendous_speed(int speed);
-static int opendous_speed_div(int speed, int *khz);
-static int opendous_khz(int khz, int *jtag_speed);
 static int opendous_init(void);
 static int opendous_quit(void);

@@ -135,6 +160,22 @@ static struct opendous_jtag *opendous_jtag_handle;
 /***************************************************************************/
 /* External interface implementation */

+COMMAND_HANDLER(opendous_handle_opendous_type_command)
+{
+	if (CMD_ARGC == 0)
+		return ERROR_OK;
+
+	/* only if the cable name wasn't overwritten by cmdline */
+	if (opendous_type == NULL) {
+		/* REVISIT first verify that it's listed in cables[] ... */
+		opendous_type = strdup(CMD_ARGV[0]);
+	}
+
+	/* REVISIT it's probably worth returning the current value ... */
+
+	return ERROR_OK;
+}
+
 COMMAND_HANDLER(opendous_handle_opendous_info_command)
 {
 	if (opendous_get_version_info() == ERROR_OK) {
@@ -187,6 +228,13 @@ static const struct command_registration opendous_command_handlers[] = {
 		.help = "access opendous HW JTAG command version",
 		.usage = "[2|3]",
 	},
+	{
+		.name = "opendous_type",
+		.handler = &opendous_handle_opendous_type_command,
+		.mode = COMMAND_CONFIG,
+		.help = "set opendous type",
+		.usage = "[usbvlab|usbprog-jtag|opendous]",
+	},
 	COMMAND_REGISTRATION_DONE
 };

@@ -194,9 +242,6 @@ struct jtag_interface opendous_interface = {
 	.name = "opendous",
 	.commands = opendous_command_handlers,
 	.execute_queue = opendous_execute_queue,
-	.speed = opendous_speed,
-	.speed_div = opendous_speed_div,
-	.khz = opendous_khz,
 	.init = opendous_init,
 	.quit = opendous_quit,
 };
@@ -276,36 +321,37 @@ static int opendous_execute_queue(void)
 	return opendous_tap_execute();
 }

-/* Sets speed in kHz. */
-static int opendous_speed(int speed)
-{
-	if (speed <= OPENDOUS_MAX_SPEED) {
-		/* one day... */
-		return ERROR_OK;
-	} else
-		LOG_INFO("Requested speed %dkHz exceeds maximum of %dkHz, ignored", speed, OPENDOUS_MAX_SPEED);
-
-	return ERROR_OK;
-}
-
-static int opendous_speed_div(int speed, int *khz)
-{
-	*khz = speed;
-
-	return ERROR_OK;
-}
-
-static int opendous_khz(int khz, int *jtag_speed)
-{
-	*jtag_speed = khz;
-	/* TODO: convert this into delay value for opendous */
-
-	return ERROR_OK;
-}
-
 static int opendous_init(void)
 {
 	int check_cnt;
+	struct opendous_probe *cur_opendous_probe;
+
+	cur_opendous_probe = opendous_probes;
+
+	if (opendous_type == NULL) {
+		opendous_type = strdup("opendous");
+		LOG_WARNING("No opendous_type specified, using default 'opendous'");
+	}
+
+	while (cur_opendous_probe->name) {
+		if (strcmp(cur_opendous_probe->name, opendous_type) == 0) {
+			opendous_probe = cur_opendous_probe;
+			break;
+		}
+		cur_opendous_probe++;
+	}
+
+	if (!opendous_probe) {
+		LOG_ERROR("No matching cable found for %s", opendous_type);
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+
+	usb_in_buffer = malloc(opendous_probe->BUFFERSIZE);
+	usb_out_buffer = malloc(opendous_probe->BUFFERSIZE);
+
+	pending_scan_results_buffer = (struct pending_scan_result *)
+			malloc(MAX_PENDING_SCAN_RESULTS * sizeof(struct pending_scan_result));

 	opendous_jtag_handle = opendous_usb_open();

@@ -336,6 +382,27 @@ static int opendous_init(void)
 static int opendous_quit(void)
 {
 	opendous_usb_close(opendous_jtag_handle);
+
+	if (usb_out_buffer) {
+		free(usb_out_buffer);
+		usb_out_buffer = NULL;
+	}
+
+	if (usb_in_buffer) {
+		free(usb_in_buffer);
+		usb_in_buffer = NULL;
+	}
+
+	if (pending_scan_results_buffer) {
+		free(pending_scan_results_buffer);
+		pending_scan_results_buffer = NULL;
+	}
+
+	if (opendous_type) {
+		free(opendous_type);
+		opendous_type = NULL;
+	}
+
 	return ERROR_OK;
 }

@@ -422,7 +489,9 @@ void opendous_scan(int ir_scan, enum scan_type type, uint8_t *buffer, int scan_s
 	/* Move to appropriate scan state */
 	opendous_end_state(ir_scan ? TAP_IRSHIFT : TAP_DRSHIFT);

-	opendous_state_move();
+	if (tap_get_state() != tap_get_end_state())
+		opendous_state_move();
+
 	opendous_end_state(saved_end_state);

 	/* Scan */
@@ -492,16 +561,6 @@ static int tap_length;
 static uint8_t tms_buffer[OPENDOUS_TAP_BUFFER_SIZE];
 static uint8_t tdo_buffer[OPENDOUS_TAP_BUFFER_SIZE];

-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;
-};
-
-static int pending_scan_results_length;
-static struct pending_scan_result pending_scan_results_buffer[MAX_PENDING_SCAN_RESULTS];
-
 static int last_tms;

 void opendous_tap_init(void)
@@ -513,8 +572,9 @@ void opendous_tap_init(void)
 void opendous_tap_ensure_space(int scans, int bits)
 {
 	int available_scans = MAX_PENDING_SCAN_RESULTS - pending_scan_results_length;
+	int available_bits = OPENDOUS_TAP_BUFFER_SIZE / 2 - tap_length;

-	if (scans > available_scans)
+	if ((scans > available_scans) || (bits > available_bits))
 		opendous_tap_execute();
 }

@@ -524,6 +584,8 @@ void opendous_tap_append_step(int tms, int tdi)
 	unsigned char _tms = tms ? 1 : 0;
 	unsigned char _tdi = tdi ? 1 : 0;

+	opendous_tap_ensure_space(0, 1);
+
 	int tap_index =  tap_length / 4;
 	int bits  = (tap_length % 4) * 2;

@@ -650,7 +712,7 @@ struct opendous_jtag *opendous_usb_open(void)
 	struct opendous_jtag *result;

 	struct jtag_libusb_device_handle *devh;
-	if (jtag_libusb_open(vids, pids, &devh) != ERROR_OK)
+	if (jtag_libusb_open(opendous_probe->VID, opendous_probe->PID, &devh) != ERROR_OK)
 		return NULL;

 	jtag_libusb_set_configuration(devh, 0);
@@ -700,8 +762,14 @@ 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));
 #endif
-	result = jtag_libusb_bulk_write(opendous_jtag->usb_handle, OPENDOUS_WRITE_ENDPOINT, \
-		(char *)usb_out_buffer, out_length, OPENDOUS_USB_TIMEOUT);
+	if (opendous_probe->CONTROL_TRANSFER) {
+		result = jtag_libusb_control_transfer(opendous_jtag->usb_handle,
+			LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE | LIBUSB_ENDPOINT_OUT,
+			FUNC_WRITE_DATA, 0, 0, (char *) usb_out_buffer, out_length, OPENDOUS_USB_TIMEOUT);
+	} else {
+		result = jtag_libusb_bulk_write(opendous_jtag->usb_handle, OPENDOUS_WRITE_ENDPOINT, \
+			(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);
 #endif
@@ -720,8 +788,15 @@ int opendous_usb_read(struct opendous_jtag *opendous_jtag)
 #ifdef _DEBUG_USB_COMMS_
 	LOG_DEBUG("%s: USB read begin", opendous_get_time(time_str));
 #endif
-	int result = jtag_libusb_bulk_read(opendous_jtag->usb_handle, OPENDOUS_READ_ENDPOINT,
-		(char *)usb_in_buffer, OPENDOUS_IN_BUFFER_SIZE, OPENDOUS_USB_TIMEOUT);
+	int result;
+	if (opendous_probe->CONTROL_TRANSFER) {
+		result = jtag_libusb_control_transfer(opendous_jtag->usb_handle,
+			LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE | LIBUSB_ENDPOINT_IN,
+			FUNC_READ_DATA, 0, 0, (char *) usb_in_buffer, OPENDOUS_IN_BUFFER_SIZE, OPENDOUS_USB_TIMEOUT);
+	} else {
+		result = jtag_libusb_bulk_read(opendous_jtag->usb_handle, OPENDOUS_READ_ENDPOINT,
+			(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);
 #endif
--- a/src/jtag/drivers/osbdm.c
+++ b/src/jtag/drivers/osbdm.c
@@ -130,8 +130,8 @@ static struct queue *queue_alloc(void)

 /* Lists of valid VID/PID pairs
 */
-static const uint16_t osbdm_vid[] = { 0x15a2, 0x15a2, 0 };
-static const uint16_t osbdm_pid[] = { 0x0042, 0x0058, 0 };
+static const uint16_t osbdm_vid[] = { 0x15a2, 0x15a2, 0x15a2, 0 };
+static const uint16_t osbdm_pid[] = { 0x0042, 0x0058, 0x005e, 0 };

 struct osbdm {
 	struct jtag_libusb_device_handle *devh; /* USB handle */
@@ -690,33 +690,12 @@ static int osbdm_init(void)
 	return ERROR_OK;
 }

-static int osbdm_khz(int khz, int *speed)
-{
-	*speed = khz;
-	return ERROR_OK;
-}
-
-static int osbdm_speed(int speed)
-{
-	return ERROR_OK;
-}
-
-static int osbdm_speed_div(int speed, int *khz)
-{
-	*khz = speed;
-	return ERROR_OK;
-}
-
 struct jtag_interface osbdm_interface = {
 	.name = "osbdm",

 	.transports = jtag_only,
 	.execute_queue = osbdm_execute_queue,

-	.khz = osbdm_khz,
-	.speed = osbdm_speed,
-	.speed_div = osbdm_speed_div,
-
 	.init = osbdm_init,
 	.quit = osbdm_quit
 };
--- a/src/jtag/drivers/parport.c
+++ b/src/jtag/drivers/parport.c
@@ -268,8 +268,8 @@ static int parport_init(void)

 	cur_cable = cables;

-	if ((parport_cable == NULL) || (parport_cable[0] == 0)) {
-		parport_cable = "wiggler";
+	if (parport_cable == NULL) {
+		parport_cable = strdup("wiggler");
 		LOG_WARNING("No parport cable specified, using default 'wiggler'");
 	}

@@ -452,8 +452,13 @@ COMMAND_HANDLER(parport_handle_parport_toggling_time_command)

 		parport_toggling_time_ns = ns;
 		retval = jtag_get_speed(&wait_states);
-		if (retval != ERROR_OK)
-			return retval;
+		if (retval != ERROR_OK) {
+			/* if jtag_get_speed fails then the clock_mode
+			 * has not been configured, this happens if parport_toggling_time is
+			 * called before the adapter speed is set */
+			LOG_INFO("no parport speed set - defaulting to zero wait states");
+			wait_states = 0;
+		}
 	}

 	command_print(CMD_CTX, "parport toggling time = %" PRIu32 " ns",
--- a/src/jtag/drivers/remote_bitbang.c
+++ b/src/jtag/drivers/remote_bitbang.c
@@ -130,11 +130,6 @@ static struct bitbang_interface remote_bitbang_bitbang = {
 	.blink = &remote_bitbang_blink,
 };

-static int remote_bitbang_speed(int speed)
-{
-	return ERROR_OK;
-}
-
 static int remote_bitbang_init_tcp(void)
 {
 	LOG_INFO("Connecting to %s:%i", remote_bitbang_host, remote_bitbang_port);
@@ -235,19 +230,6 @@ static int remote_bitbang_init(void)
 	return remote_bitbang_init_tcp();
 }

-static int remote_bitbang_khz(int khz, int *jtag_speed)
-{
-	*jtag_speed = 0;
-	return ERROR_OK;
-}
-
-static int remote_bitbang_speed_div(int speed, int *khz)
-{
-	/* I don't think this really matters any. */
-	*khz = 1;
-	return ERROR_OK;
-}
-
 COMMAND_HANDLER(remote_bitbang_handle_remote_bitbang_port_command)
 {
 	if (CMD_ARGC == 1) {
@@ -290,10 +272,7 @@ static const struct command_registration remote_bitbang_command_handlers[] = {
 struct jtag_interface remote_bitbang_interface = {
 	.name = "remote_bitbang",
 	.execute_queue = &bitbang_execute_queue,
-	.speed = &remote_bitbang_speed,
 	.commands = remote_bitbang_command_handlers,
 	.init = &remote_bitbang_init,
 	.quit = &remote_bitbang_quit,
-	.khz = &remote_bitbang_khz,
-	.speed_div = &remote_bitbang_speed_div,
 };
--- a/src/jtag/drivers/rlink.c
+++ b/src/jtag/drivers/rlink.c
@@ -482,7 +482,7 @@ static int dtc_run_download(


 	/* Wait for DTC to finish running command buffer */
-	for (i = 10;; ) {
+	for (i = 50;; ) {
 		usb_err = ep1_generic_commandl(
 				pHDev_param, 4,

@@ -642,7 +642,7 @@ static int dtc_queue_run(void)

 	usb_err = dtc_run_download(pHDev,
 			dtc_queue.cmd_buffer, dtc_queue.cmd_index,
-			reply_buffer, dtc_queue.reply_index
+			reply_buffer, sizeof(reply_buffer)
 			);
 	if (usb_err < 0) {
 		LOG_ERROR("dtc_run_download: %s", usb_strerror());
--- a/src/jtag/drivers/stlink_usb.c
+++ b/src/jtag/drivers/stlink_usb.c
@@ -30,25 +30,25 @@
 /* project specific includes */
 #include <helper/binarybuffer.h>
 #include <jtag/interface.h>
-#include <jtag/stlink/stlink_layout.h>
-#include <jtag/stlink/stlink_transport.h>
-#include <jtag/stlink/stlink_interface.h>
+#include <jtag/hla/hla_layout.h>
+#include <jtag/hla/hla_transport.h>
+#include <jtag/hla/hla_interface.h>
 #include <target/target.h>

 #include <target/cortex_m.h>

 #include "libusb_common.h"

-#define ENDPOINT_IN	0x80
-#define ENDPOINT_OUT	0x00
+#define ENDPOINT_IN  0x80
+#define ENDPOINT_OUT 0x00

-#define STLINK_NULL_EP		0
-#define STLINK_RX_EP		(1|ENDPOINT_IN)
-#define STLINK_TX_EP		(2|ENDPOINT_OUT)
-#define STLINK_SG_SIZE		(31)
-#define STLINK_DATA_SIZE	(4*128)
-#define STLINK_CMD_SIZE_V2	(16)
-#define STLINK_CMD_SIZE_V1	(10)
+#define STLINK_NULL_EP     0
+#define STLINK_RX_EP       (1|ENDPOINT_IN)
+#define STLINK_TX_EP       (2|ENDPOINT_OUT)
+#define STLINK_SG_SIZE     (31)
+#define STLINK_DATA_SIZE   (4*128)
+#define STLINK_CMD_SIZE_V2 (16)
+#define STLINK_CMD_SIZE_V1 (10)

 enum stlink_jtag_api_version {
 	STLINK_JTAG_API_V1 = 1,
@@ -82,7 +82,7 @@ struct stlink_usb_handle_s {
 	/** */
 	uint8_t databuf[STLINK_DATA_SIZE];
 	/** */
-	enum stlink_transports transport;
+	enum hl_transports transport;
 	/** */
 	struct stlink_usb_version version;
 	/** */
@@ -93,73 +93,74 @@ struct stlink_usb_handle_s {
 	enum stlink_jtag_api_version jtag_api;
 };

-#define STLINK_DEBUG_ERR_OK			0x80
-#define STLINK_DEBUG_ERR_FAULT		0x81
-#define STLINK_SWD_AP_WAIT			0x10
-#define STLINK_SWD_DP_WAIT			0x14
+#define STLINK_DEBUG_ERR_OK            0x80
+#define STLINK_DEBUG_ERR_FAULT         0x81
+#define STLINK_SWD_AP_WAIT             0x10
+#define STLINK_SWD_DP_WAIT             0x14

-#define STLINK_CORE_RUNNING			0x80
-#define STLINK_CORE_HALTED			0x81
-#define STLINK_CORE_STAT_UNKNOWN		-1
+#define STLINK_CORE_RUNNING            0x80
+#define STLINK_CORE_HALTED             0x81
+#define STLINK_CORE_STAT_UNKNOWN       -1

-#define STLINK_GET_VERSION			0xF1
-#define STLINK_DEBUG_COMMAND			0xF2
-#define STLINK_DFU_COMMAND			0xF3
-#define STLINK_SWIM_COMMAND			0xF4
-#define STLINK_GET_CURRENT_MODE			0xF5
+#define STLINK_GET_VERSION             0xF1
+#define STLINK_DEBUG_COMMAND           0xF2
+#define STLINK_DFU_COMMAND             0xF3
+#define STLINK_SWIM_COMMAND            0xF4
+#define STLINK_GET_CURRENT_MODE        0xF5
+#define STLINK_GET_TARGET_VOLTAGE      0xF7

-#define STLINK_DEV_DFU_MODE			0x00
-#define STLINK_DEV_MASS_MODE			0x01
-#define STLINK_DEV_DEBUG_MODE			0x02
-#define STLINK_DEV_SWIM_MODE			0x03
-#define STLINK_DEV_BOOTLOADER_MODE		0x04
-#define STLINK_DEV_UNKNOWN_MODE			-1
+#define STLINK_DEV_DFU_MODE            0x00
+#define STLINK_DEV_MASS_MODE           0x01
+#define STLINK_DEV_DEBUG_MODE          0x02
+#define STLINK_DEV_SWIM_MODE           0x03
+#define STLINK_DEV_BOOTLOADER_MODE     0x04
+#define STLINK_DEV_UNKNOWN_MODE        -1

-#define STLINK_DFU_EXIT				0x07
+#define STLINK_DFU_EXIT                0x07

-#define STLINK_SWIM_ENTER			0x00
-#define STLINK_SWIM_EXIT			0x01
+#define STLINK_SWIM_ENTER              0x00
+#define STLINK_SWIM_EXIT               0x01

-#define STLINK_DEBUG_ENTER_JTAG			0x00
-#define STLINK_DEBUG_GETSTATUS			0x01
-#define STLINK_DEBUG_FORCEDEBUG			0x02
-#define STLINK_DEBUG_APIV1_RESETSYS		0x03
-#define STLINK_DEBUG_APIV1_READALLREGS		0x04
-#define STLINK_DEBUG_APIV1_READREG		0x05
-#define STLINK_DEBUG_APIV1_WRITEREG		0x06
-#define STLINK_DEBUG_READMEM_32BIT		0x07
-#define STLINK_DEBUG_WRITEMEM_32BIT		0x08
-#define STLINK_DEBUG_RUNCORE			0x09
-#define STLINK_DEBUG_STEPCORE			0x0a
-#define STLINK_DEBUG_APIV1_SETFP		0x0b
-#define STLINK_DEBUG_READMEM_8BIT		0x0c
-#define STLINK_DEBUG_WRITEMEM_8BIT		0x0d
-#define STLINK_DEBUG_APIV1_CLEARFP		0x0e
-#define STLINK_DEBUG_APIV1_WRITEDEBUGREG	0x0f
-#define STLINK_DEBUG_APIV1_SETWATCHPOINT	0x10
+#define STLINK_DEBUG_ENTER_JTAG            0x00
+#define STLINK_DEBUG_GETSTATUS             0x01
+#define STLINK_DEBUG_FORCEDEBUG            0x02
+#define STLINK_DEBUG_APIV1_RESETSYS        0x03
+#define STLINK_DEBUG_APIV1_READALLREGS     0x04
+#define STLINK_DEBUG_APIV1_READREG         0x05
+#define STLINK_DEBUG_APIV1_WRITEREG        0x06
+#define STLINK_DEBUG_READMEM_32BIT         0x07
+#define STLINK_DEBUG_WRITEMEM_32BIT        0x08
+#define STLINK_DEBUG_RUNCORE               0x09
+#define STLINK_DEBUG_STEPCORE              0x0a
+#define STLINK_DEBUG_APIV1_SETFP           0x0b
+#define STLINK_DEBUG_READMEM_8BIT          0x0c
+#define STLINK_DEBUG_WRITEMEM_8BIT         0x0d
+#define STLINK_DEBUG_APIV1_CLEARFP         0x0e
+#define STLINK_DEBUG_APIV1_WRITEDEBUGREG   0x0f
+#define STLINK_DEBUG_APIV1_SETWATCHPOINT   0x10

-#define STLINK_DEBUG_ENTER_JTAG			0x00
-#define STLINK_DEBUG_ENTER_SWD			0xa3
+#define STLINK_DEBUG_ENTER_JTAG            0x00
+#define STLINK_DEBUG_ENTER_SWD             0xa3

-#define STLINK_DEBUG_APIV1_ENTER		0x20
-#define STLINK_DEBUG_EXIT			0x21
-#define STLINK_DEBUG_READCOREID			0x22
+#define STLINK_DEBUG_APIV1_ENTER           0x20
+#define STLINK_DEBUG_EXIT                  0x21
+#define STLINK_DEBUG_READCOREID            0x22

-#define STLINK_DEBUG_APIV2_ENTER		0x30
-#define STLINK_DEBUG_APIV2_READ_IDCODES		0x31
-#define STLINK_DEBUG_APIV2_RESETSYS		0x32
-#define STLINK_DEBUG_APIV2_READREG		0x33
-#define STLINK_DEBUG_APIV2_WRITEREG		0x34
-#define STLINK_DEBUG_APIV2_WRITEDEBUGREG	0x35
-#define STLINK_DEBUG_APIV2_READDEBUGREG	0x36
+#define STLINK_DEBUG_APIV2_ENTER           0x30
+#define STLINK_DEBUG_APIV2_READ_IDCODES    0x31
+#define STLINK_DEBUG_APIV2_RESETSYS        0x32
+#define STLINK_DEBUG_APIV2_READREG         0x33
+#define STLINK_DEBUG_APIV2_WRITEREG        0x34
+#define STLINK_DEBUG_APIV2_WRITEDEBUGREG   0x35
+#define STLINK_DEBUG_APIV2_READDEBUGREG    0x36

-#define STLINK_DEBUG_APIV2_READALLREGS		0x3A
-#define STLINK_DEBUG_APIV2_GETLASTRWSTATUS	0x3B
-#define STLINK_DEBUG_APIV2_DRIVE_NRST		0x3C
+#define STLINK_DEBUG_APIV2_READALLREGS     0x3A
+#define STLINK_DEBUG_APIV2_GETLASTRWSTATUS 0x3B
+#define STLINK_DEBUG_APIV2_DRIVE_NRST      0x3C

-#define STLINK_DEBUG_APIV2_DRIVE_NRST_LOW	0x00
-#define STLINK_DEBUG_APIV2_DRIVE_NRST_HIGH	0x01
-#define STLINK_DEBUG_APIV2_DRIVE_NRST_PULSE	0x02
+#define STLINK_DEBUG_APIV2_DRIVE_NRST_LOW   0x00
+#define STLINK_DEBUG_APIV2_DRIVE_NRST_HIGH  0x01
+#define STLINK_DEBUG_APIV2_DRIVE_NRST_PULSE 0x02

 /** */
 enum stlink_mode {
@@ -171,8 +172,8 @@ enum stlink_mode {
 	STLINK_MODE_DEBUG_SWIM
 };

-#define REQUEST_SENSE		0x03
-#define REQUEST_SENSE_LENGTH	18
+#define REQUEST_SENSE        0x03
+#define REQUEST_SENSE_LENGTH 18

 static void stlink_usb_init_buffer(void *handle, uint8_t direction, uint32_t size);

@@ -413,7 +414,7 @@ static int stlink_usb_version(void *handle)
 	else
 		h->version.jtag_api_max = STLINK_JTAG_API_V1;

-	LOG_DEBUG("STLINK v%d JTAG v%d API v%d SWIM v%d VID 0x%04X PID 0x%04X",
+	LOG_INFO("STLINK v%d JTAG v%d API v%d SWIM v%d VID 0x%04X PID 0x%04X",
 		h->version.stlink,
 		h->version.jtag,
 		(h->version.jtag_api_max == STLINK_JTAG_API_V1) ? 1 : 2,
@@ -424,6 +425,40 @@ static int stlink_usb_version(void *handle)
 	return ERROR_OK;
 }

+static int stlink_usb_check_voltage(void *handle, float *target_voltage)
+{
+	struct stlink_usb_handle_s *h;
+	uint32_t adc_results[2];
+
+	h = (struct stlink_usb_handle_s *)handle;
+
+	/* only supported by stlink/v2 and for firmware >= 13 */
+	if (h->version.stlink == 1 || h->version.jtag < 13)
+		return ERROR_COMMAND_NOTFOUND;
+
+	stlink_usb_init_buffer(handle, STLINK_RX_EP, 8);
+
+	h->cmdbuf[h->cmdidx++] = STLINK_GET_TARGET_VOLTAGE;
+
+	int result = stlink_usb_xfer(handle, h->databuf, 8);
+
+	if (result != ERROR_OK)
+		return result;
+
+	/* convert result */
+	adc_results[0] = le_to_h_u32(h->databuf);
+	adc_results[1] = le_to_h_u32(h->databuf + 4);
+
+	*target_voltage = 0;
+
+	if (adc_results[0])
+		*target_voltage = 2 * ((float)adc_results[1]) * (float)(1.2 / adc_results[0]);
+
+	LOG_INFO("Target voltage: %f", (double)*target_voltage);
+
+	return ERROR_OK;
+}
+
 /** */
 static int stlink_usb_current_mode(void *handle, uint8_t *mode)
 {
@@ -544,8 +579,10 @@ static int stlink_usb_mode_leave(void *handle, enum stlink_mode type)
 	return ERROR_OK;
 }

+static int stlink_usb_assert_srst(void *handle, int srst);
+
 /** */
-static int stlink_usb_init_mode(void *handle)
+static int stlink_usb_init_mode(void *handle, bool connect_under_reset)
 {
 	int res;
 	uint8_t mode;
@@ -593,17 +630,40 @@ static int stlink_usb_init_mode(void *handle)
 	if (res != ERROR_OK)
 		return res;

+	/* we check the target voltage here as an aid to debugging connection problems.
+	 * the stlink requires the target Vdd to be connected for reliable debugging.
+	 * this cmd is supported in all modes except DFU
+	 */
+	if (mode != STLINK_DEV_DFU_MODE) {
+
+		float target_voltage;
+
+		/* check target voltage (if supported) */
+		res = stlink_usb_check_voltage(h, &target_voltage);
+
+		if (res != ERROR_OK) {
+			if (res != ERROR_COMMAND_NOTFOUND)
+				LOG_ERROR("voltage check failed");
+			/* attempt to continue as it is not a catastrophic failure */
+		} else {
+			/* check for a sensible target voltage, operating range is 1.65-5.5v
+			 * according to datasheet */
+			if (target_voltage < 1.5)
+				LOG_ERROR("target voltage may be too low for reliable debugging");
+		}
+	}
+
 	LOG_DEBUG("MODE: 0x%02X", mode);

 	/* set selected mode */
 	switch (h->transport) {
-		case STLINK_TRANSPORT_SWD:
+		case HL_TRANSPORT_SWD:
 			emode = STLINK_MODE_DEBUG_SWD;
 			break;
-		case STLINK_TRANSPORT_JTAG:
+		case HL_TRANSPORT_JTAG:
 			emode = STLINK_MODE_DEBUG_JTAG;
 			break;
-		case STLINK_TRANSPORT_SWIM:
+		case HL_TRANSPORT_SWIM:
 			emode = STLINK_MODE_DEBUG_SWIM;
 			break;
 		default:
@@ -616,6 +676,12 @@ static int stlink_usb_init_mode(void *handle)
 		return ERROR_FAIL;
 	}

+	if (connect_under_reset) {
+		res = stlink_usb_assert_srst(handle, 0);
+		if (res != ERROR_OK)
+			return res;
+	}
+
 	res = stlink_usb_mode_enter(handle, emode);

 	if (res != ERROR_OK)
@@ -1143,7 +1209,22 @@ static int stlink_usb_write_mem32(void *handle, uint32_t addr, uint16_t len,
 }

 /** */
-static int stlink_usb_open(struct stlink_interface_param_s *param, void **fd)
+static int stlink_usb_close(void *fd)
+{
+	struct stlink_usb_handle_s *h;
+
+	h = (struct stlink_usb_handle_s *)fd;
+
+	if (h->fd)
+		jtag_libusb_close(h->fd);
+
+	free(fd);
+
+	return ERROR_OK;
+}
+
+/** */
+static int stlink_usb_open(struct hl_interface_param_s *param, void **fd)
 {
 	int err;
 	struct stlink_usb_handle_s *h;
@@ -1151,7 +1232,7 @@ static int stlink_usb_open(struct stlink_interface_param_s *param, void **fd)

 	LOG_DEBUG("stlink_usb_open");

-	h = malloc(sizeof(struct stlink_usb_handle_s));
+	h = calloc(1, sizeof(struct stlink_usb_handle_s));

 	if (h == 0) {
 		LOG_DEBUG("malloc failed");
@@ -1160,6 +1241,9 @@ static int stlink_usb_open(struct stlink_interface_param_s *param, void **fd)

 	h->transport = param->transport;

+	/* set max read/write buffer size in bytes */
+	param->max_buffer = 512;
+
 	const uint16_t vids[] = { param->vid, 0 };
 	const uint16_t pids[] = { param->pid, 0 };

@@ -1168,14 +1252,14 @@ static int stlink_usb_open(struct stlink_interface_param_s *param, void **fd)

 	if (jtag_libusb_open(vids, pids, &h->fd) != ERROR_OK) {
 		LOG_ERROR("open failed");
-		return ERROR_FAIL;
+		goto error_open;
 	}

 	jtag_libusb_set_configuration(h->fd, 0);

 	if (jtag_libusb_claim_interface(h->fd, 0) != ERROR_OK) {
 		LOG_DEBUG("claim interface failed");
-		return ERROR_FAIL;
+		goto error_open;
 	}

 	/* wrap version for first read */
@@ -1193,9 +1277,7 @@ static int stlink_usb_open(struct stlink_interface_param_s *param, void **fd)

 	if (err != ERROR_OK) {
 		LOG_ERROR("read version failed");
-		jtag_libusb_close(h->fd);
-		free(h);
-		return err;
+		goto error_open;
 	}

 	/* compare usb vid/pid */
@@ -1208,12 +1290,12 @@ static int stlink_usb_open(struct stlink_interface_param_s *param, void **fd)
 	err = ERROR_OK;

 	switch (h->transport) {
-		case STLINK_TRANSPORT_SWD:
-		case STLINK_TRANSPORT_JTAG:
+		case HL_TRANSPORT_SWD:
+		case HL_TRANSPORT_JTAG:
 			if (h->version.jtag == 0)
 				err = ERROR_FAIL;
 			break;
-		case STLINK_TRANSPORT_SWIM:
+		case HL_TRANSPORT_SWIM:
 			if (h->version.swim == 0)
 				err = ERROR_FAIL;
 			break;
@@ -1224,9 +1306,7 @@ static int stlink_usb_open(struct stlink_interface_param_s *param, void **fd)

 	if (err != ERROR_OK) {
 		LOG_ERROR("mode (transport) not supported by device");
-		jtag_libusb_close(h->fd);
-		free(h);
-		return err;
+		goto error_open;
 	}

 	api = h->version.jtag_api_max;
@@ -1242,28 +1322,25 @@ static int stlink_usb_open(struct stlink_interface_param_s *param, void **fd)
 	h->jtag_api = api;

 	/* initialize the debug hardware */
-	err = stlink_usb_init_mode(h);
+	err = stlink_usb_init_mode(h, param->connect_under_reset);

 	if (err != ERROR_OK) {
 		LOG_ERROR("init mode failed");
-		jtag_libusb_close(h->fd);
-		free(h);
-		return err;
+		goto error_open;
 	}

 	*fd = h;

 	return ERROR_OK;
+
+error_open:
+	stlink_usb_close(h);
+
+	return ERROR_FAIL;
 }

 /** */
-static int stlink_usb_close(void *fd)
-{
-	return ERROR_OK;
-}
-
-/** */
-struct stlink_layout_api_s stlink_usb_layout_api = {
+struct hl_layout_api_s stlink_usb_layout_api = {
 	/** */
 	.open = stlink_usb_open,
 	/** */
--- a/src/jtag/drivers/sysfsgpio.c
+++ b/src/jtag/drivers/sysfsgpio.c
@@ -0,0 +1,499 @@
+/***************************************************************************
+ *   Copyright (C) 2012 by Creative Product Design, marc @ cpdesign.com.au *
+ *                                                                         *
+ *   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.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+/**
+ * @file
+ * This driver implements a bitbang jtag interface using gpio lines via
+ * sysfs.
+ * The aim of this driver implementation is use system GPIOs but avoid the
+ * need for a additional kernel driver.
+ * (Note memory mapped IO is another option, however it doesn't mix well with
+ * the kernel gpiolib driver - which makes sense I guess.)
+ *
+ * A gpio is required for tck, tms, tdi and tdo. One or both of srst and trst
+ * must be also be specified. The required jtag gpios are specified via the
+ * sysfsgpio_jtag_nums command or the relevant sysfsgpio_XXX_num commang.
+ * The srst and trst gpios are set via the sysfsgpio_srst_num and
+ * sysfsgpio_trst_num respectively. GPIO numbering follows the kernel
+ * convention of starting from 0.
+ *
+ * The gpios should not be in use by another entity, and must not be requested
+ * by a kernel driver without also being exported by it (otherwise they can't
+ * be exported by sysfs).
+ *
+ * The sysfs gpio interface can only manipulate one gpio at a time, so the
+ * bitbang write handler remembers the last state for tck, tms, tdi to avoid
+ * superfluous writes.
+ * For speed the sysfs "value" entry is opened at init and held open.
+ * This results in considerable gains over open-write-close (45s vs 900s)
+ *
+ * Further work could address:
+ *  -srst and trst open drain/ push pull
+ *  -configurable active high/low for srst & trst
+ */
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <jtag/interface.h>
+#include "bitbang.h"
+
+/*
+ * Helper func to determine if gpio number valid
+ *
+ * Assume here that there will be less than 1000 gpios on a system
+ */
+static int is_gpio_valid(int gpio)
+{
+	return gpio >= 0 && gpio < 1000;
+}
+
+/*
+ * Helper func to open, write to and close a file
+ * name and valstr must be null terminated.
+ *
+ * Returns negative on failure.
+ */
+static int open_write_close(const char *name, const char *valstr)
+{
+	int ret;
+	int fd = open(name, O_WRONLY);
+	if (fd < 0)
+		return fd;
+
+	ret = write(fd, valstr, strlen(valstr));
+	close(fd);
+
+	return ret;
+}
+
+/*
+ * Helper func to unexport gpio from sysfs
+ */
+static void unexport_sysfs_gpio(int gpio)
+{
+	char gpiostr[4];
+
+	if (!is_gpio_valid(gpio))
+		return;
+
+	snprintf(gpiostr, sizeof(gpiostr), "%d", gpio);
+	if (open_write_close("/sys/class/gpio/unexport", gpiostr) < 0)
+		LOG_ERROR("Couldn't unexport gpio %d", gpio);
+
+	return;
+}
+
+/*
+ * Exports and sets up direction for gpio.
+ * If the gpio is an output, it is initialized according to init_high,
+ * otherwise it is ignored.
+ *
+ * If the gpio is already exported we just show a warning and continue; if
+ * openocd happened to crash (or was killed by user) then the gpios will not
+ * have been cleaned up.
+ */
+static int setup_sysfs_gpio(int gpio, int is_output, int init_high)
+{
+	char buf[40];
+	char gpiostr[4];
+	int ret;
+
+	if (!is_gpio_valid(gpio))
+		return ERROR_OK;
+
+	snprintf(gpiostr, sizeof(gpiostr), "%d", gpio);
+	ret = open_write_close("/sys/class/gpio/export", gpiostr);
+	if (ret < 0) {
+		if (errno == EBUSY) {
+			LOG_WARNING("gpio %d is already exported", gpio);
+		} else {
+			LOG_ERROR("Couldn't export gpio %d", gpio);
+			perror("sysfsgpio: ");
+			return ERROR_FAIL;
+		}
+	}
+
+	snprintf(buf, sizeof(buf), "/sys/class/gpio/gpio%d/direction", gpio);
+	ret = open_write_close(buf, is_output ? (init_high ? "high" : "low") : "in");
+	if (ret < 0) {
+		LOG_ERROR("Couldn't set direction for gpio %d", gpio);
+		perror("sysfsgpio: ");
+		unexport_sysfs_gpio(gpio);
+		return ERROR_FAIL;
+	}
+
+	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)
+		unexport_sysfs_gpio(gpio);
+
+	return ret;
+}
+
+/*
+ * file descriptors for /sys/class/gpio/gpioXX/value
+ * Set up during init.
+ */
+static int tck_fd = -1;
+static int tms_fd = -1;
+static int tdi_fd = -1;
+static int tdo_fd = -1;
+static int trst_fd = -1;
+static int srst_fd = -1;
+
+/*
+ * Bitbang interface read of TDO
+ *
+ * The sysfs value will read back either '0' or '1'. The trick here is to call
+ * lseek to bypass buffering in the sysfs kernel driver.
+ */
+static int sysfsgpio_read(void)
+{
+	char buf[1];
+
+	/* important to seek to signal sysfs of new read */
+	lseek(tdo_fd, 0, SEEK_SET);
+	int ret = read(tdo_fd, &buf, sizeof(buf));
+
+	if (ret < 0) {
+		LOG_WARNING("reading tdo failed");
+		return 0;
+	}
+
+	return buf[0] == '1';
+}
+
+/*
+ * Bitbang interface write of TCK, TMS, TDI
+ *
+ * Seeing as this is the only function where the outputs are changed,
+ * we can cache the old value to avoid needlessly writing it.
+ */
+static void sysfsgpio_write(int tck, int tms, int tdi)
+{
+	const char one[] = "1";
+	const char zero[] = "0";
+
+	static int last_tck;
+	static int last_tms;
+	static int last_tdi;
+
+	static int first_time;
+	size_t bytes_written;
+
+	if (!first_time) {
+		last_tck = !tck;
+		last_tms = !tms;
+		last_tdi = !tdi;
+		first_time = 1;
+	}
+
+	if (tdi != last_tdi) {
+		bytes_written = write(tdi_fd, tdi ? &one : &zero, 1);
+		if (bytes_written != 1)
+			LOG_WARNING("writing tdi failed");
+	}
+
+	if (tms != last_tms) {
+		bytes_written = write(tms_fd, tms ? &one : &zero, 1);
+		if (bytes_written != 1)
+			LOG_WARNING("writing tms failed");
+	}
+
+	/* write clk last */
+	if (tck != last_tck) {
+		bytes_written = write(tck_fd, tck ? &one : &zero, 1);
+		if (bytes_written != 1)
+			LOG_WARNING("writing tck failed");
+	}
+
+	last_tdi = tdi;
+	last_tms = tms;
+	last_tck = tck;
+}
+
+/*
+ * Bitbang interface to manipulate reset lines SRST and TRST
+ *
+ * (1) assert or (0) deassert reset lines
+ */
+static void sysfsgpio_reset(int trst, int srst)
+{
+	const char one[] = "1";
+	const char zero[] = "0";
+	size_t bytes_written;
+
+	/* assume active low */
+	if (srst_fd >= 0) {
+		bytes_written = write(srst_fd, srst ? &zero : &one, 1);
+		if (bytes_written != 1)
+			LOG_WARNING("writing srst failed");
+	}
+
+	/* assume active low */
+	if (trst_fd >= 0) {
+		bytes_written = write(trst_fd, trst ? &zero : &one, 1);
+		if (bytes_written != 1)
+			LOG_WARNING("writing trst failed");
+	}
+}
+
+/* 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) {
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], tck_gpio);
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[1], tms_gpio);
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[2], tdi_gpio);
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[3], tdo_gpio);
+	} else if (CMD_ARGC != 0) {
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	command_print(CMD_CTX,
+			"SysfsGPIO nums: tck = %d, tms = %d, tdi = %d, tdi = %d",
+			tck_gpio, tms_gpio, tdi_gpio, tdo_gpio);
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(sysfsgpio_handle_jtag_gpionum_tck)
+{
+	if (CMD_ARGC == 1)
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], tck_gpio);
+
+	command_print(CMD_CTX, "SysfsGPIO num: tck = %d", tck_gpio);
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(sysfsgpio_handle_jtag_gpionum_tms)
+{
+	if (CMD_ARGC == 1)
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], tms_gpio);
+
+	command_print(CMD_CTX, "SysfsGPIO num: tms = %d", tms_gpio);
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(sysfsgpio_handle_jtag_gpionum_tdo)
+{
+	if (CMD_ARGC == 1)
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], tdo_gpio);
+
+	command_print(CMD_CTX, "SysfsGPIO num: tdo = %d", tdo_gpio);
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(sysfsgpio_handle_jtag_gpionum_tdi)
+{
+	if (CMD_ARGC == 1)
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], tdi_gpio);
+
+	command_print(CMD_CTX, "SysfsGPIO num: tdi = %d", tdi_gpio);
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(sysfsgpio_handle_jtag_gpionum_srst)
+{
+	if (CMD_ARGC == 1)
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], srst_gpio);
+
+	command_print(CMD_CTX, "SysfsGPIO num: srst = %d", srst_gpio);
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(sysfsgpio_handle_jtag_gpionum_trst)
+{
+	if (CMD_ARGC == 1)
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], trst_gpio);
+
+	command_print(CMD_CTX, "SysfsGPIO num: trst = %d", trst_gpio);
+	return ERROR_OK;
+}
+
+static const struct command_registration sysfsgpio_command_handlers[] = {
+	{
+		.name = "sysfsgpio_jtag_nums",
+		.handler = &sysfsgpio_handle_jtag_gpionums,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio numbers for tck, tms, tdi, tdo. (in that order)",
+		.usage = "(tck tms tdi tdo)* ",
+	},
+	{
+		.name = "sysfsgpio_tck_num",
+		.handler = &sysfsgpio_handle_jtag_gpionum_tck,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio number for tck.",
+	},
+	{
+		.name = "sysfsgpio_tms_num",
+		.handler = &sysfsgpio_handle_jtag_gpionum_tms,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio number for tms.",
+	},
+	{
+		.name = "sysfsgpio_tdo_num",
+		.handler = &sysfsgpio_handle_jtag_gpionum_tdo,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio number for tdo.",
+	},
+	{
+		.name = "sysfsgpio_tdi_num",
+		.handler = &sysfsgpio_handle_jtag_gpionum_tdi,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio number for tdi.",
+	},
+	{
+		.name = "sysfsgpio_srst_num",
+		.handler = &sysfsgpio_handle_jtag_gpionum_srst,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio number for srst.",
+	},
+	{
+		.name = "sysfsgpio_trst_num",
+		.handler = &sysfsgpio_handle_jtag_gpionum_trst,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio number for trst.",
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+static int sysfsgpio_init(void);
+static int sysfsgpio_quit(void);
+
+struct jtag_interface sysfsgpio_interface = {
+	.name = "sysfsgpio",
+	.supported = DEBUG_CAP_TMS_SEQ,
+	.execute_queue = bitbang_execute_queue,
+	.transports = jtag_only,
+	.commands = sysfsgpio_command_handlers,
+	.init = sysfsgpio_init,
+	.quit = sysfsgpio_quit,
+};
+
+static struct bitbang_interface sysfsgpio_bitbang = {
+	.read = sysfsgpio_read,
+	.write = sysfsgpio_write,
+	.reset = sysfsgpio_reset,
+	.blink = 0
+};
+
+/* helper func to close and cleanup files only if they were valid/ used */
+static void cleanup_fd(int fd, int gpio)
+{
+	if (gpio >= 0) {
+		if (fd >= 0)
+			close(fd);
+
+		unexport_sysfs_gpio(gpio);
+	}
+}
+
+static void cleanup_all_fds(void)
+{
+	cleanup_fd(tck_fd, tck_gpio);
+	cleanup_fd(tms_fd, tms_gpio);
+	cleanup_fd(tdi_fd, tdi_gpio);
+	cleanup_fd(tdo_fd, tdo_gpio);
+	cleanup_fd(trst_fd, trst_gpio);
+	cleanup_fd(srst_fd, srst_gpio);
+}
+
+static int sysfsgpio_init(void)
+{
+	bitbang_interface = &sysfsgpio_bitbang;
+
+	LOG_INFO("SysfsGPIO JTAG 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");
+		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.
+	 */
+	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;
+
+	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;
+
+	/* assume active low*/
+	trst_fd = setup_sysfs_gpio(trst_gpio, 1, 1);
+	if (trst_gpio > 0 && trst_fd < 0)
+		goto out_error;
+
+	/* assume active low*/
+	srst_fd = setup_sysfs_gpio(srst_gpio, 1, 1);
+	if (srst_gpio > 0 && srst_fd < 0)
+		goto out_error;
+
+	return ERROR_OK;
+
+out_error:
+	cleanup_all_fds();
+	return ERROR_JTAG_INIT_FAILED;
+}
+
+static int sysfsgpio_quit(void)
+{
+	cleanup_all_fds();
+	return ERROR_OK;
+}
+
--- a/src/jtag/drivers/ti_icdi_usb.c
+++ b/src/jtag/drivers/ti_icdi_usb.c
@@ -0,0 +1,740 @@
+/***************************************************************************
+ *                                                                         *
+ *   Copyright (C) 2012 by Spencer Oliver                                  *
+ *   spen@spen-soft.co.uk                                                  *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+/* project specific includes */
+#include <helper/binarybuffer.h>
+#include <jtag/interface.h>
+#include <jtag/hla/hla_layout.h>
+#include <jtag/hla/hla_transport.h>
+#include <jtag/hla/hla_interface.h>
+#include <target/target.h>
+
+#include <target/cortex_m.h>
+
+#include <libusb-1.0/libusb.h>
+
+#define ICDI_WRITE_ENDPOINT 0x02
+#define ICDI_READ_ENDPOINT 0x83
+
+#define ICDI_WRITE_TIMEOUT 1000
+#define ICDI_READ_TIMEOUT 1000
+#define ICDI_PACKET_SIZE 2048
+
+#define PACKET_START "$"
+#define PACKET_END "#"
+
+struct icdi_usb_handle_s {
+	libusb_context *usb_ctx;
+	libusb_device_handle *usb_dev;
+
+	char *read_buffer;
+	char *write_buffer;
+	int max_packet;
+	int read_count;
+};
+
+static int icdi_usb_read_mem32(void *handle, uint32_t addr, uint16_t len, uint8_t *buffer);
+static int icdi_usb_write_mem32(void *handle, uint32_t addr, uint16_t len, const uint8_t *buffer);
+
+static int remote_escape_output(const char *buffer, int len, char *out_buf, int *out_len, int out_maxlen)
+{
+	int input_index, output_index;
+
+	output_index = 0;
+
+	for (input_index = 0; input_index < len; input_index++) {
+
+		char b = buffer[input_index];
+
+		if (b == '$' || b == '#' || b == '}' || b == '*') {
+			/* These must be escaped.  */
+			if (output_index + 2 > out_maxlen)
+				break;
+			out_buf[output_index++] = '}';
+			out_buf[output_index++] = b ^ 0x20;
+		} else {
+			if (output_index + 1 > out_maxlen)
+				break;
+			out_buf[output_index++] = b;
+		}
+	}
+
+	*out_len = input_index;
+	return output_index;
+}
+
+static int remote_unescape_input(const char *buffer, int len, char *out_buf, int out_maxlen)
+{
+	int input_index, output_index;
+	int escaped;
+
+	output_index = 0;
+	escaped = 0;
+
+	for (input_index = 0; input_index < len; input_index++) {
+
+		char b = buffer[input_index];
+
+		if (output_index + 1 > out_maxlen)
+			LOG_ERROR("Received too much data from the target.");
+
+		if (escaped) {
+			out_buf[output_index++] = b ^ 0x20;
+			escaped = 0;
+		} else if (b == '}')
+			escaped = 1;
+		else
+			out_buf[output_index++] = b;
+	}
+
+	if (escaped)
+		LOG_ERROR("Unmatched escape character in target response.");
+
+	return output_index;
+}
+
+static int icdi_send_packet(void *handle, int len)
+{
+	unsigned char cksum = 0;
+	struct icdi_usb_handle_s *h;
+	int result, retry = 0;
+	int transferred = 0;
+
+	assert(handle != NULL);
+	h = (struct icdi_usb_handle_s *)handle;
+
+	/* check we have a large enough buffer for checksum "#00" */
+	if (len + 3 > h->max_packet) {
+		LOG_ERROR("packet buffer too small");
+		return ERROR_FAIL;
+	}
+
+	/* calculate checksum - offset start of packet */
+	for (int i = 1; i < len; i++)
+		cksum += h->write_buffer[i];
+
+	len += sprintf(&h->write_buffer[len], PACKET_END "%02x", cksum);
+
+#ifdef _DEBUG_USB_COMMS_
+	char buffer[50];
+	char ch = h->write_buffer[1];
+	if (ch == 'x' || ch == 'X')
+		LOG_DEBUG("writing packet: <binary>");
+	else {
+		memcpy(buffer, h->write_buffer, len >= 50 ? 50-1 : len);
+		buffer[len] = 0;
+		LOG_DEBUG("writing packet: %s", buffer);
+	}
+#endif
+
+	while (1) {
+
+		result = libusb_bulk_transfer(h->usb_dev, ICDI_WRITE_ENDPOINT, (unsigned char *)h->write_buffer, len,
+				&transferred, ICDI_WRITE_TIMEOUT);
+		if (result != 0 || transferred != len) {
+			LOG_DEBUG("Error TX Data %d", result);
+			return ERROR_FAIL;
+		}
+
+		/* check that the client got the message ok, or shall we resend */
+		result = libusb_bulk_transfer(h->usb_dev, ICDI_READ_ENDPOINT, (unsigned char *)h->read_buffer, h->max_packet,
+					&transferred, ICDI_READ_TIMEOUT);
+		if (result != 0 || transferred < 1) {
+			LOG_DEBUG("Error RX Data %d", result);
+			return ERROR_FAIL;
+		}
+
+#ifdef _DEBUG_USB_COMMS_
+		LOG_DEBUG("received reply: '%c' : count %d", h->read_buffer[0], transferred);
+#endif
+
+		if (h->read_buffer[0] == '-') {
+			LOG_DEBUG("Resending packet %d", ++retry);
+		} else {
+			if (h->read_buffer[0] != '+')
+				LOG_DEBUG("Unexpected Reply from ICDI: %c", h->read_buffer[0]);
+			break;
+		}
+
+		if (retry == 3) {
+			LOG_DEBUG("maximum nack retries attempted");
+			return ERROR_FAIL;
+		}
+	}
+
+	retry = 0;
+	h->read_count = transferred;
+
+	while (1) {
+
+		/* read reply from icdi */
+		result = libusb_bulk_transfer(h->usb_dev, ICDI_READ_ENDPOINT, (unsigned char *)h->read_buffer + h->read_count,
+				h->max_packet - h->read_count, &transferred, ICDI_READ_TIMEOUT);
+
+#ifdef _DEBUG_USB_COMMS_
+		LOG_DEBUG("received data: count %d", transferred);
+#endif
+
+		/* check for errors but retry for timeout */
+		if (result != 0) {
+
+			if (result == LIBUSB_ERROR_TIMEOUT) {
+				LOG_DEBUG("Error RX timeout %d", result);
+			} else {
+				LOG_DEBUG("Error RX Data %d", result);
+				return ERROR_FAIL;
+			}
+		}
+
+		h->read_count += transferred;
+
+		/* we need to make sure we have a full packet, including checksum */
+		if (h->read_count > 5) {
+
+			/* check that we have received an packet delimiter
+			 * we do not validate the checksum
+			 * reply should contain $...#AA - so we check for # */
+			if (h->read_buffer[h->read_count - 3] == '#')
+				return ERROR_OK;
+		}
+
+		if (retry++ == 3) {
+			LOG_DEBUG("maximum data retries attempted");
+			break;
+		}
+	}
+
+	return ERROR_FAIL;
+}
+
+static int icdi_send_cmd(void *handle, const char *cmd)
+{
+	struct icdi_usb_handle_s *h;
+	h = (struct icdi_usb_handle_s *)handle;
+
+	int cmd_len = snprintf(h->write_buffer, h->max_packet, PACKET_START "%s", cmd);
+	return icdi_send_packet(handle, cmd_len);
+}
+
+static int icdi_send_remote_cmd(void *handle, const char *data)
+{
+	struct icdi_usb_handle_s *h;
+	h = (struct icdi_usb_handle_s *)handle;
+
+	size_t cmd_len = sprintf(h->write_buffer, PACKET_START "qRcmd,");
+	cmd_len += hexify(h->write_buffer + cmd_len, data, 0, h->max_packet - cmd_len);
+
+	return icdi_send_packet(handle, cmd_len);
+}
+
+static int icdi_get_cmd_result(void *handle)
+{
+	struct icdi_usb_handle_s *h;
+	int offset = 0;
+	char ch;
+
+	assert(handle != NULL);
+	h = (struct icdi_usb_handle_s *)handle;
+
+	do {
+		ch = h->read_buffer[offset++];
+		if (offset > h->read_count)
+			return ERROR_FAIL;
+	} while (ch != '$');
+
+	if (memcmp("OK", h->read_buffer + offset, 2) == 0)
+		return ERROR_OK;
+
+	if (h->read_buffer[offset] == 'E') {
+		/* get error code */
+		char result;
+		if (unhexify(&result, h->read_buffer + offset + 1, 1) != 1)
+			return ERROR_FAIL;
+		return result;
+	}
+
+	/* for now we assume everything else is ok */
+	return ERROR_OK;
+}
+
+static int icdi_usb_idcode(void *handle, uint32_t *idcode)
+{
+	return ERROR_OK;
+}
+
+static int icdi_usb_write_debug_reg(void *handle, uint32_t addr, uint32_t val)
+{
+	return icdi_usb_write_mem32(handle, addr, 1, (uint8_t *)&val);
+}
+
+static enum target_state icdi_usb_state(void *handle)
+{
+	int result;
+	struct icdi_usb_handle_s *h;
+	uint32_t dhcsr;
+
+	h = (struct icdi_usb_handle_s *)handle;
+
+	result = icdi_usb_read_mem32(h, DCB_DHCSR, 1, (uint8_t *)&dhcsr);
+	if (result != ERROR_OK)
+		return TARGET_UNKNOWN;
+
+	if (dhcsr & S_HALT)
+		return TARGET_HALTED;
+
+	return TARGET_RUNNING;
+}
+
+static int icdi_usb_version(void *handle)
+{
+	struct icdi_usb_handle_s *h;
+	h = (struct icdi_usb_handle_s *)handle;
+
+	char version[20];
+
+	/* get info about icdi */
+	int result = icdi_send_remote_cmd(handle, "version");
+	if (result != ERROR_OK)
+		return result;
+
+	if (h->read_count < 8) {
+		LOG_ERROR("Invalid Reply Received");
+		return ERROR_FAIL;
+	}
+
+	/* convert reply */
+	if (unhexify(version, h->read_buffer + 2, 4) != 4) {
+		LOG_WARNING("unable to get ICDI version");
+		return ERROR_OK;
+	}
+
+	/* null terminate and print info */
+	version[4] = 0;
+
+	LOG_INFO("ICDI Firmware version: %s", version);
+
+	return ERROR_OK;
+}
+
+static int icdi_usb_query(void *handle)
+{
+	int result;
+
+	struct icdi_usb_handle_s *h;
+	h = (struct icdi_usb_handle_s *)handle;
+
+	result = icdi_send_cmd(handle, "qSupported");
+	if (result != ERROR_OK)
+		return result;
+
+	/* check result */
+	result = icdi_get_cmd_result(handle);
+	if (result != ERROR_OK) {
+		LOG_ERROR("query supported failed: 0x%x", result);
+		return ERROR_FAIL;
+	}
+
+	/* from this we can get the max packet supported */
+
+	/* query packet buffer size */
+	char *offset = strstr(h->read_buffer, "PacketSize");
+	if (offset) {
+		char *separator;
+		int max_packet;
+
+		max_packet = strtoul(offset + 11, &separator, 16);
+		if (!max_packet)
+			LOG_ERROR("invalid max packet, using defaults");
+		else
+			h->max_packet = max_packet;
+		LOG_DEBUG("max packet supported : %" PRIu32 " bytes", h->max_packet);
+	}
+
+
+	/* if required re allocate packet buffer */
+	if (h->max_packet != ICDI_PACKET_SIZE) {
+		h->read_buffer = realloc(h->read_buffer, h->max_packet);
+		h->write_buffer = realloc(h->write_buffer, h->max_packet);
+		if (h->read_buffer == 0 || h->write_buffer == 0) {
+			LOG_ERROR("unable to reallocate memory");
+			return ERROR_FAIL;
+		}
+	}
+
+	/* set extended mode */
+	result = icdi_send_cmd(handle, "!");
+	if (result != ERROR_OK)
+		return result;
+
+	/* check result */
+	result = icdi_get_cmd_result(handle);
+	if (result != ERROR_OK) {
+		LOG_ERROR("unable to enable extended mode: 0x%x", result);
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
+static int icdi_usb_reset(void *handle)
+{
+	/* we do this in hla_target.c */
+	return ERROR_OK;
+}
+
+static int icdi_usb_assert_srst(void *handle, int srst)
+{
+	/* TODO not supported yet */
+	return ERROR_COMMAND_NOTFOUND;
+}
+
+static int icdi_usb_run(void *handle)
+{
+	int result;
+
+	/* resume target at current address */
+	result = icdi_send_cmd(handle, "c");
+	if (result != ERROR_OK)
+		return result;
+
+	/* check result */
+	result = icdi_get_cmd_result(handle);
+	if (result != ERROR_OK) {
+		LOG_ERROR("continue failed: 0x%x", result);
+		return ERROR_FAIL;
+	}
+
+	return result;
+}
+
+static int icdi_usb_halt(void *handle)
+{
+	int result;
+
+	/* this query halts the target ?? */
+	result = icdi_send_cmd(handle, "?");
+	if (result != ERROR_OK)
+		return result;
+
+	/* check result */
+	result = icdi_get_cmd_result(handle);
+	if (result != ERROR_OK) {
+		LOG_ERROR("halt failed: 0x%x", result);
+		return ERROR_FAIL;
+	}
+
+	return result;
+}
+
+static int icdi_usb_step(void *handle)
+{
+	int result;
+
+	/* step target at current address */
+	result = icdi_send_cmd(handle, "s");
+	if (result != ERROR_OK)
+		return result;
+
+	/* check result */
+	result = icdi_get_cmd_result(handle);
+	if (result != ERROR_OK) {
+		LOG_ERROR("step failed: 0x%x", result);
+		return ERROR_FAIL;
+	}
+
+	return result;
+}
+
+static int icdi_usb_read_regs(void *handle)
+{
+	/* currently unsupported */
+	return ERROR_OK;
+}
+
+static int icdi_usb_read_reg(void *handle, int num, uint32_t *val)
+{
+	int result;
+	struct icdi_usb_handle_s *h;
+	char cmd[10];
+
+	h = (struct icdi_usb_handle_s *)handle;
+
+	snprintf(cmd, sizeof(cmd), "p%x", num);
+	result = icdi_send_cmd(handle, cmd);
+	if (result != ERROR_OK)
+		return result;
+
+	/* check result */
+	result = icdi_get_cmd_result(handle);
+	if (result != ERROR_OK) {
+		LOG_ERROR("register read failed: 0x%x", result);
+		return ERROR_FAIL;
+	}
+
+	/* convert result */
+	if (unhexify((char *)val, h->read_buffer + 2, 4) != 4) {
+		LOG_ERROR("failed to convert result");
+		return ERROR_FAIL;
+	}
+
+	return result;
+}
+
+static int icdi_usb_write_reg(void *handle, int num, uint32_t val)
+{
+	int result;
+	char cmd[20];
+
+	int cmd_len = snprintf(cmd, sizeof(cmd), "P%x=", num);
+	hexify(cmd + cmd_len, (char *)&val, 4, sizeof(cmd));
+
+	result = icdi_send_cmd(handle, cmd);
+	if (result != ERROR_OK)
+		return result;
+
+	/* check result */
+	result = icdi_get_cmd_result(handle);
+	if (result != ERROR_OK) {
+		LOG_ERROR("register write failed: 0x%x", result);
+		return ERROR_FAIL;
+	}
+
+	return result;
+}
+
+static int icdi_usb_read_mem(void *handle, uint32_t addr, uint32_t len, uint8_t *buffer)
+{
+	int result;
+	struct icdi_usb_handle_s *h;
+	char cmd[20];
+
+	h = (struct icdi_usb_handle_s *)handle;
+
+	snprintf(cmd, sizeof(cmd), "x%x,%x", addr, len);
+	result = icdi_send_cmd(handle, cmd);
+	if (result != ERROR_OK)
+		return result;
+
+	/* check result */
+	result = icdi_get_cmd_result(handle);
+	if (result != ERROR_OK) {
+		LOG_ERROR("memory read failed: 0x%x", result);
+		return ERROR_FAIL;
+	}
+
+	/* unescape input */
+	int read_len = remote_unescape_input(h->read_buffer + 5, h->read_count - 8, (char *)buffer, len);
+	if (read_len != (int)len) {
+		LOG_ERROR("read more bytes than expected: actual 0x%" PRIx32 " expected 0x%" PRIx32, read_len, len);
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
+static int icdi_usb_write_mem(void *handle, uint32_t addr, uint32_t len, const uint8_t *buffer)
+{
+	int result;
+	struct icdi_usb_handle_s *h;
+
+	h = (struct icdi_usb_handle_s *)handle;
+
+	size_t cmd_len = snprintf(h->write_buffer, h->max_packet, PACKET_START "X%x,%x:", addr, len);
+
+	int out_len;
+	cmd_len += remote_escape_output((char *)buffer, len, h->write_buffer + cmd_len,
+			&out_len, h->max_packet - cmd_len);
+
+	if (out_len < (int)len) {
+		/* for now issue a error as we have no way of allocating a larger buffer */
+		LOG_ERROR("memory buffer too small: requires 0x%" PRIx32 " actual 0x%" PRIx32, out_len, len);
+		return ERROR_FAIL;
+	}
+
+	result = icdi_send_packet(handle, cmd_len);
+	if (result != ERROR_OK)
+		return result;
+
+	/* check result */
+	result = icdi_get_cmd_result(handle);
+	if (result != ERROR_OK) {
+		LOG_ERROR("memory write failed: 0x%x", result);
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
+static int icdi_usb_read_mem8(void *handle, uint32_t addr, uint16_t len, uint8_t *buffer)
+{
+	return icdi_usb_read_mem(handle, addr, len, buffer);
+}
+
+static int icdi_usb_write_mem8(void *handle, uint32_t addr, uint16_t len, const uint8_t *buffer)
+{
+	return icdi_usb_write_mem(handle, addr, len, buffer);
+}
+
+static int icdi_usb_read_mem32(void *handle, uint32_t addr, uint16_t len, uint8_t *buffer)
+{
+	return icdi_usb_read_mem(handle, addr, len * 4, buffer);
+}
+
+static int icdi_usb_write_mem32(void *handle, uint32_t addr, uint16_t len, const uint8_t *buffer)
+{
+	return icdi_usb_write_mem(handle, addr, len * 4, buffer);
+}
+
+static int icdi_usb_close(void *handle)
+{
+	struct icdi_usb_handle_s *h;
+
+	h = (struct icdi_usb_handle_s *)handle;
+
+	if (h->usb_dev)
+		libusb_close(h->usb_dev);
+
+	if (h->usb_ctx)
+		libusb_exit(h->usb_ctx);
+
+	if (h->read_buffer)
+		free(h->read_buffer);
+
+	if (h->write_buffer)
+		free(h->write_buffer);
+
+	free(handle);
+
+	return ERROR_OK;
+}
+
+static int icdi_usb_open(struct hl_interface_param_s *param, void **fd)
+{
+	int retval;
+	struct icdi_usb_handle_s *h;
+
+	LOG_DEBUG("icdi_usb_open");
+
+	h = calloc(1, sizeof(struct icdi_usb_handle_s));
+
+	if (h == 0) {
+		LOG_ERROR("unable to allocate memory");
+		return ERROR_FAIL;
+	}
+
+	LOG_DEBUG("transport: %d vid: 0x%04x pid: 0x%04x", param->transport,
+		param->vid, param->pid);
+
+	if (libusb_init(&h->usb_ctx) != 0) {
+		LOG_ERROR("libusb init failed");
+		goto error_open;
+	}
+
+	h->usb_dev = libusb_open_device_with_vid_pid(h->usb_ctx, param->vid, param->pid);
+	if (!h->usb_dev) {
+		LOG_ERROR("open failed");
+		goto error_open;
+	}
+
+	if (libusb_claim_interface(h->usb_dev, 2)) {
+		LOG_DEBUG("claim interface failed");
+		goto error_open;
+	}
+
+	/* check if mode is supported */
+	retval = ERROR_OK;
+
+	switch (param->transport) {
+#if 0
+		/* TODO place holder as swd is not currently supported */
+		case HL_TRANSPORT_SWD:
+#endif
+		case HL_TRANSPORT_JTAG:
+			break;
+		default:
+			retval = ERROR_FAIL;
+			break;
+	}
+
+	if (retval != ERROR_OK) {
+		LOG_ERROR("mode (transport) not supported by device");
+		goto error_open;
+	}
+
+	/* allocate buffer */
+	h->read_buffer = malloc(ICDI_PACKET_SIZE);
+	h->write_buffer = malloc(ICDI_PACKET_SIZE);
+	h->max_packet = ICDI_PACKET_SIZE;
+
+	if (h->read_buffer == 0 || h->write_buffer == 0) {
+		LOG_DEBUG("malloc failed");
+		goto error_open;
+	}
+
+	/* query icdi version etc */
+	retval = icdi_usb_version(h);
+	if (retval != ERROR_OK)
+		goto error_open;
+
+	/* query icdi support */
+	retval = icdi_usb_query(h);
+	if (retval != ERROR_OK)
+		goto error_open;
+
+	*fd = h;
+
+	/* set the max target read/write buffer in bytes
+	 * as we are using gdb binary packets to transfer memory we have to
+	 * reserve half the buffer for any possible escape chars plus
+	 * at least 64 bytes for the gdb packet header */
+	param->max_buffer = (((h->max_packet - 64) / 4) * 4) / 2;
+
+	return ERROR_OK;
+
+error_open:
+	icdi_usb_close(h);
+
+	return ERROR_FAIL;
+}
+
+struct hl_layout_api_s icdi_usb_layout_api = {
+	.open = icdi_usb_open,
+	.close = icdi_usb_close,
+	.idcode = icdi_usb_idcode,
+	.state = icdi_usb_state,
+	.reset = icdi_usb_reset,
+	.assert_srst = icdi_usb_assert_srst,
+	.run = icdi_usb_run,
+	.halt = icdi_usb_halt,
+	.step = icdi_usb_step,
+	.read_regs = icdi_usb_read_regs,
+	.read_reg = icdi_usb_read_reg,
+	.write_reg = icdi_usb_write_reg,
+	.read_mem8 = icdi_usb_read_mem8,
+	.write_mem8 = icdi_usb_write_mem8,
+	.read_mem32 = icdi_usb_read_mem32,
+	.write_mem32 = icdi_usb_write_mem32,
+	.write_debug_reg = icdi_usb_write_debug_reg
+};
--- a/src/jtag/drivers/usb_blaster.c
+++ b/src/jtag/drivers/usb_blaster.c
@@ -297,27 +297,6 @@ static void usb_blaster_write(int tck, int tms, int tdi)
 	usb_blaster_addtowritebuffer(out_value, false);
 }

-static int usb_blaster_speed(int speed)
-{
-#if BUILD_USB_BLASTER_FTD2XX == 1
-	LOG_DEBUG("TODO: usb_blaster_speed() isn't implemented for libftd2xx!");
-#elif BUILD_USB_BLASTER_LIBFTDI == 1
-	LOG_DEBUG("TODO: usb_blaster_speed() isn't optimally implemented!");
-
-	/* TODO: libftdi's ftdi_set_baudrate chokes on high rates, use lowlevel
-	 * usb function instead! And additionally allow user to throttle.
-	 */
-	if (ftdi_set_baudrate(&ftdic, 3000000 / 4) < 0) {
-		LOG_ERROR("Can't set baud rate to max: %s",
-			ftdi_get_error_string(&ftdic));
-		return ERROR_JTAG_DEVICE_ERROR;
-	}
-	;
-#endif
-
-	return ERROR_OK;
-}
-
 static void usb_blaster_reset(int trst, int srst)
 {
 	LOG_DEBUG("TODO: usb_blaster_reset(%d,%d) isn't implemented!",
@@ -359,7 +338,7 @@ static int usb_blaster_init(void)
 	if (usb_blaster_device_desc == NULL) {
 		LOG_WARNING("no usb_blaster device description specified, "
 			"using default 'USB-Blaster'");
-		usb_blaster_device_desc = "USB-Blaster";
+		usb_blaster_device_desc = strdup("USB-Blaster");
 	}

 #if IS_WIN32 == 0
@@ -490,6 +469,11 @@ static int usb_blaster_quit(void)
 	ftdi_deinit(&ftdic);
 #endif

+	if (usb_blaster_device_desc) {
+		free(usb_blaster_device_desc);
+		usb_blaster_device_desc = NULL;
+	}
+
 	return ERROR_OK;
 }

@@ -588,7 +572,6 @@ struct jtag_interface usb_blaster_interface = {

 	.execute_queue = bitbang_execute_queue,

-	.speed = usb_blaster_speed,
 	.init = usb_blaster_init,
 	.quit = usb_blaster_quit,
 };
--- a/src/jtag/drivers/usbprog.c
+++ b/src/jtag/drivers/usbprog.c
@@ -96,11 +96,6 @@ static void usbprog_jtag_write_slice(struct usbprog_jtag *usbprog_jtag, unsigned
 static void usbprog_jtag_set_bit(struct usbprog_jtag *usbprog_jtag, int bit, int value);
 /* static int usbprog_jtag_get_bit(struct usbprog_jtag *usbprog_jtag, int bit); */

-static int usbprog_speed(int speed)
-{
-	return ERROR_OK;
-}
-
 static int usbprog_execute_queue(void)
 {
 	struct jtag_command *cmd = jtag_command_queue;	/* currently processed command */
@@ -621,7 +616,6 @@ struct jtag_interface usbprog_interface = {
 	.name = "usbprog",

 	.execute_queue = usbprog_execute_queue,
-	.speed = usbprog_speed,
 	.init = usbprog_init,
 	.quit = usbprog_quit
 };
--- a/src/jtag/drivers/vsllink.c
+++ b/src/jtag/drivers/vsllink.c
@@ -302,7 +302,7 @@ static int vsllink_init(void)
 	}

 	/* malloc buffer size for tap */
-	tap_buffer_size = versaloon_interface.usb_setting.buf_size - 32;
+	tap_buffer_size = versaloon_interface.usb_setting.buf_size / 2 - 32;
 	vsllink_free_buffer();
 	tdi_buffer = (uint8_t *)malloc(tap_buffer_size);
 	tdo_buffer = (uint8_t *)malloc(tap_buffer_size);
--- a/src/jtag/hla/Makefile.am
+++ b/src/jtag/hla/Makefile.am
@@ -0,0 +1,23 @@
+include $(top_srcdir)/common.mk
+
+noinst_LTLIBRARIES = libocdhla.la
+
+libocdhla_la_SOURCES = \
+	$(HLFILES)
+
+HLFILES =
+
+if HLADAPTER
+HLFILES += hla_transport.c
+HLFILES += hla_tcl.c
+HLFILES += hla_interface.c
+HLFILES += hla_layout.c
+endif
+
+noinst_HEADERS = \
+	hla_interface.h \
+	hla_layout.h \
+	hla_tcl.h \
+	hla_transport.h
+
+MAINTAINERCLEANFILES = $(srcdir)/Makefile.in
--- a/src/jtag/hla/hla_interface.c
+++ b/src/jtag/hla/hla_interface.c
@@ -0,0 +1,270 @@
+/***************************************************************************
+ *   Copyright (C) 2011 by Mathias Kuester                                 *
+ *   Mathias Kuester <kesmtp@freenet.de>                                   *
+ *                                                                         *
+ *   Copyright (C) 2012 by Spencer Oliver                                  *
+ *   spen@spen-soft.co.uk                                                  *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+/* project specific includes */
+#include <jtag/interface.h>
+#include <transport/transport.h>
+#include <helper/time_support.h>
+
+#include <jtag/hla/hla_tcl.h>
+#include <jtag/hla/hla_layout.h>
+#include <jtag/hla/hla_transport.h>
+#include <jtag/hla/hla_interface.h>
+
+#include <target/target.h>
+
+static struct hl_interface_s hl_if = { {0, 0, 0, 0, 0, HL_TRANSPORT_UNKNOWN, 0, false}, 0, 0 };
+
+int hl_interface_open(enum hl_transports tr)
+{
+	LOG_DEBUG("hl_interface_open");
+
+	enum reset_types jtag_reset_config = jtag_get_reset_config();
+
+	if (jtag_reset_config & RESET_CNCT_UNDER_SRST) {
+		if (jtag_reset_config & RESET_SRST_NO_GATING)
+			hl_if.param.connect_under_reset = true;
+		else
+			LOG_WARNING("\'srst_nogate\' reset_config option is required");
+	}
+
+	/* set transport mode */
+	hl_if.param.transport = tr;
+
+	int result = hl_if.layout->open(&hl_if);
+	if (result != ERROR_OK)
+		return result;
+
+	return hl_interface_init_reset();
+}
+
+int hl_interface_init_target(struct target *t)
+{
+	int res;
+
+	LOG_DEBUG("hl_interface_init_target");
+
+	/* this is the interface for the current target and we
+	 * can setup the private pointer in the tap structure
+	 * if the interface match the tap idcode
+	 */
+	res = hl_if.layout->api->idcode(hl_if.fd, &t->tap->idcode);
+
+	if (res != ERROR_OK)
+		return res;
+
+	unsigned ii, limit = t->tap->expected_ids_cnt;
+	int found = 0;
+
+	for (ii = 0; ii < limit; ii++) {
+		uint32_t expected = t->tap->expected_ids[ii];
+
+		/* treat "-expected-id 0" as a "don't-warn" wildcard */
+		if (!expected || (t->tap->idcode == expected)) {
+			found = 1;
+			break;
+		}
+	}
+
+	if (found == 0) {
+		LOG_ERROR("hl_interface_init_target: target not found: idcode: 0x%08x",
+				t->tap->idcode);
+		return ERROR_FAIL;
+	}
+
+	t->tap->priv = &hl_if;
+	t->tap->hasidcode = 1;
+
+	return ERROR_OK;
+}
+
+static int hl_interface_init(void)
+{
+	LOG_DEBUG("hl_interface_init");
+
+	/* here we can initialize the layout */
+	return hl_layout_init(&hl_if);
+}
+
+static int hl_interface_quit(void)
+{
+	LOG_DEBUG("hl_interface_quit");
+
+	return ERROR_OK;
+}
+
+static int hl_interface_execute_queue(void)
+{
+	LOG_DEBUG("hl_interface_execute_queue: ignored");
+
+	return ERROR_OK;
+}
+
+int hl_interface_init_reset(void)
+{
+	/* incase the adapter has not already handled asserting srst
+	 * we will attempt it again */
+	if (hl_if.param.connect_under_reset) {
+		jtag_add_reset(0, 1);
+		hl_if.layout->api->assert_srst(hl_if.fd, 0);
+	}
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(hl_interface_handle_device_desc_command)
+{
+	LOG_DEBUG("hl_interface_handle_device_desc_command");
+
+	if (CMD_ARGC == 1) {
+		hl_if.param.device_desc = strdup(CMD_ARGV[0]);
+	} else {
+		LOG_ERROR("expected exactly one argument to hl_device_desc <description>");
+	}
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(hl_interface_handle_serial_command)
+{
+	LOG_DEBUG("hl_interface_handle_serial_command");
+
+	if (CMD_ARGC == 1) {
+		hl_if.param.serial = strdup(CMD_ARGV[0]);
+	} else {
+		LOG_ERROR("expected exactly one argument to hl_serial <serial-number>");
+	}
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(hl_interface_handle_layout_command)
+{
+	LOG_DEBUG("hl_interface_handle_layout_command");
+
+	if (CMD_ARGC != 1) {
+		LOG_ERROR("Need exactly one argument to stlink_layout");
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	if (hl_if.layout) {
+		LOG_ERROR("already specified hl_layout %s",
+				hl_if.layout->name);
+		return (strcmp(hl_if.layout->name, CMD_ARGV[0]) != 0)
+		    ? ERROR_FAIL : ERROR_OK;
+	}
+
+	for (const struct hl_layout *l = hl_layout_get_list(); l->name;
+	     l++) {
+		if (strcmp(l->name, CMD_ARGV[0]) == 0) {
+			hl_if.layout = l;
+			return ERROR_OK;
+		}
+	}
+
+	LOG_ERROR("No adapter layout '%s' found", CMD_ARGV[0]);
+	return ERROR_FAIL;
+}
+
+COMMAND_HANDLER(hl_interface_handle_vid_pid_command)
+{
+	LOG_DEBUG("hl_interface_handle_vid_pid_command");
+
+	if (CMD_ARGC != 2) {
+		LOG_WARNING("ignoring extra IDs in hl_vid_pid (maximum is 1 pair)");
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	COMMAND_PARSE_NUMBER(u16, CMD_ARGV[0], hl_if.param.vid);
+	COMMAND_PARSE_NUMBER(u16, CMD_ARGV[1], hl_if.param.pid);
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(stlink_interface_handle_api_command)
+{
+	if (CMD_ARGC != 1)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	unsigned new_api;
+	COMMAND_PARSE_NUMBER(uint, CMD_ARGV[0], new_api);
+	if ((new_api == 0) || (new_api > 2))
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	hl_if.param.api = new_api;
+
+	return ERROR_OK;
+}
+
+static const struct command_registration hl_interface_command_handlers[] = {
+	{
+	 .name = "hla_device_desc",
+	 .handler = &hl_interface_handle_device_desc_command,
+	 .mode = COMMAND_CONFIG,
+	 .help = "set the a device description of the adapter",
+	 .usage = "description_string",
+	 },
+	{
+	 .name = "hla_serial",
+	 .handler = &hl_interface_handle_serial_command,
+	 .mode = COMMAND_CONFIG,
+	 .help = "set the serial number of the adapter",
+	 .usage = "serial_string",
+	 },
+	{
+	 .name = "hla_layout",
+	 .handler = &hl_interface_handle_layout_command,
+	 .mode = COMMAND_CONFIG,
+	 .help = "set the layout of the adapter",
+	 .usage = "layout_name",
+	 },
+	{
+	 .name = "hla_vid_pid",
+	 .handler = &hl_interface_handle_vid_pid_command,
+	 .mode = COMMAND_CONFIG,
+	 .help = "the vendor and product ID of the adapter",
+	 .usage = "(vid pid)* ",
+	 },
+	 {
+	 .name = "stlink_api",
+	 .handler = &stlink_interface_handle_api_command,
+	 .mode = COMMAND_CONFIG,
+	 .help = "set the desired stlink api level",
+	 .usage = "api version 1 or 2",
+	 },
+	COMMAND_REGISTRATION_DONE
+};
+
+struct jtag_interface hl_interface = {
+	.name = "hla",
+	.supported = 0,
+	.commands = hl_interface_command_handlers,
+	.transports = hl_transports,
+	.init = hl_interface_init,
+	.quit = hl_interface_quit,
+	.execute_queue = hl_interface_execute_queue,
+};
--- a/src/jtag/stlink/stlink_interface.h
+++ b/src/jtag/stlink/stlink_interface.h
@@ -2,6 +2,9 @@
 *   Copyright (C) 2011 by Mathias Kuester                                 *
 *   Mathias Kuester <kesmtp@freenet.de>                                   *
 *                                                                         *
+ *   Copyright (C) 2012 by Spencer Oliver                                  *
+ *   spen@spen-soft.co.uk                                                  *
+ *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
@@ -18,17 +21,17 @@
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/

-#ifndef _STLINK_INTERFACE_
-#define _STLINK_INTERFACE_
+#ifndef _HL_INTERFACE
+#define _HL_INTERFACE

 /** */
 struct target;
 /** */
-enum e_stlink_transports;
+enum e_hl_transports;
 /** */
-extern const char *stlink_transports[];
+extern const char *hl_transports[];

-struct stlink_interface_param_s {
+struct hl_interface_param_s {
 	/** */
 	char *device_desc;
 	/** */
@@ -40,21 +43,27 @@ struct stlink_interface_param_s {
 	/** */
 	unsigned api;
 	/** */
-	enum stlink_transports transport;
+	enum hl_transports transport;
+	/** */
+	int max_buffer;
+	/** */
+	bool connect_under_reset;
 };

-struct stlink_interface_s {
+struct hl_interface_s {
 	/** */
-	struct stlink_interface_param_s param;
+	struct hl_interface_param_s param;
 	/** */
-	const struct stlink_layout *layout;
+	const struct hl_layout *layout;
 	/** */
 	void *fd;
 };

 /** */
-int stlink_interface_open(enum stlink_transports tr);
+int hl_interface_open(enum hl_transports tr);
 /** */
-int stlink_interface_init_target(struct target *t);

-#endif
+int hl_interface_init_target(struct target *t);
+int hl_interface_init_reset(void);
+
+#endif /* _HL_INTERFACE */
--- a/src/jtag/stlink/stlink_layout.c
+++ b/src/jtag/stlink/stlink_layout.c
@@ -2,6 +2,9 @@
 *   Copyright (C) 2011 by Mathias Kuester                                 *
 *   Mathias Kuester <kesmtp@freenet.de>                                   *
 *                                                                         *
+ *   Copyright (C) 2012 by Spencer Oliver                                  *
+ *   spen@spen-soft.co.uk                                                  *
+ *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
@@ -27,67 +30,67 @@
 #include <transport/transport.h>
 #include <helper/time_support.h>

-#include <jtag/stlink/stlink_layout.h>
-#include <jtag/stlink/stlink_tcl.h>
-#include <jtag/stlink/stlink_transport.h>
-#include <jtag/stlink/stlink_interface.h>
+#include <jtag/hla/hla_layout.h>
+#include <jtag/hla/hla_tcl.h>
+#include <jtag/hla/hla_transport.h>
+#include <jtag/hla/hla_interface.h>

-#define STLINK_LAYOUT_UNKNOWN	0
-#define STLINK_LAYOUT_SG	1
-#define STLINK_LAYOUT_USB	2
-
-static int stlink_layout_open(struct stlink_interface_s *stlink_if)
+static int hl_layout_open(struct hl_interface_s *adapter)
 {
 	int res;

-	LOG_DEBUG("stlink_layout_open");
+	LOG_DEBUG("hl_layout_open");

-	stlink_if->fd = NULL;
+	adapter->fd = NULL;

-	res = stlink_if->layout->api->open(&stlink_if->param, &stlink_if->fd);
+	res = adapter->layout->api->open(&adapter->param, &adapter->fd);

 	if (res != ERROR_OK) {
 		LOG_DEBUG("failed");
 		return res;
 	}

+	/* make sure adapter has set the buffer size */
+	if (!adapter->param.max_buffer) {
+		LOG_ERROR("buffer size not set");
+		return ERROR_FAIL;
+	}
+
 	return ERROR_OK;
 }

-static int stlink_layout_close(struct stlink_interface_s *stlink_if)
+static int hl_layout_close(struct hl_interface_s *adapter)
 {
 	return ERROR_OK;
 }

-static const struct stlink_layout stlink_layouts[] = {
+static const struct hl_layout hl_layouts[] = {
 	{
-	 .name = "usb",
-	 .type = STLINK_LAYOUT_USB,
-	 .open = stlink_layout_open,
-	 .close = stlink_layout_close,
+	 .name = "stlink",
+	 .open = hl_layout_open,
+	 .close = hl_layout_close,
 	 .api = &stlink_usb_layout_api,
 	 },
 	{
-	 .name = "sg",
-	 .type = STLINK_LAYOUT_SG,
-	 .open = stlink_layout_open,
-	 .close = stlink_layout_close,
-	 .api = &stlink_usb_layout_api,
-	 },
+	 .name = "ti-icdi",
+	 .open = hl_layout_open,
+	 .close = hl_layout_close,
+	 .api = &icdi_usb_layout_api,
+	},
 	{.name = NULL, /* END OF TABLE */ },
 };

 /** */
-const struct stlink_layout *stlink_layout_get_list(void)
+const struct hl_layout *hl_layout_get_list(void)
 {
-	return stlink_layouts;
+	return hl_layouts;
 }

-int stlink_layout_init(struct stlink_interface_s *stlink_if)
+int hl_layout_init(struct hl_interface_s *adapter)
 {
-	LOG_DEBUG("stlink_layout_init");
+	LOG_DEBUG("hl_layout_init");

-	if (stlink_if->layout == NULL) {
+	if (adapter->layout == NULL) {
 		LOG_ERROR("no layout specified");
 		return ERROR_FAIL;
 	}
--- a/src/jtag/stlink/stlink_layout.h
+++ b/src/jtag/stlink/stlink_layout.h
@@ -2,6 +2,9 @@
 *   Copyright (C) 2011 by Mathias Kuester                                 *
 *   Mathias Kuester <kesmtp@freenet.de>                                   *
 *                                                                         *
+ *   Copyright (C) 2012 by Spencer Oliver                                  *
+ *   spen@spen-soft.co.uk                                                  *
+ *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
@@ -18,20 +21,21 @@
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/

-#ifndef _STLINK_LAYOUT_H_
-#define _STLINK_LAYOUT_H_
+#ifndef _HL_LAYOUT_H
+#define _HL_LAYOUT_H

 /** */
-struct stlink_interface_s;
-struct stlink_interface_param_s;
+struct hl_interface_s;
+struct hl_interface_param_s;

 /** */
-extern struct stlink_layout_api_s stlink_usb_layout_api;
+extern struct hl_layout_api_s stlink_usb_layout_api;
+extern struct hl_layout_api_s icdi_usb_layout_api;

 /** */
-struct stlink_layout_api_s {
+struct hl_layout_api_s {
 	/** */
-	int (*open) (struct stlink_interface_param_s *param, void **fd);
+	int (*open) (struct hl_interface_param_s *param, void **fd);
 	/** */
 	int (*close) (void *fd);
 	/** */
@@ -71,22 +75,20 @@ struct stlink_layout_api_s {
 };

 /** */
-struct stlink_layout {
+struct hl_layout {
 	/** */
 	char *name;
 	/** */
-	int type;
+	int (*open) (struct hl_interface_s *adapter);
 	/** */
-	int (*open) (struct stlink_interface_s *stlink_if);
+	int (*close) (struct hl_interface_s *adapter);
 	/** */
-	int (*close) (struct stlink_interface_s *stlink_if);
-	/** */
-	struct stlink_layout_api_s *api;
+	struct hl_layout_api_s *api;
 };

 /** */
-const struct stlink_layout *stlink_layout_get_list(void);
+const struct hl_layout *hl_layout_get_list(void);
 /** */
-int stlink_layout_init(struct stlink_interface_s *stlink_if);
+int hl_layout_init(struct hl_interface_s *adapter);

-#endif
+#endif /* _HL_LAYOUT_H */
--- a/src/jtag/stlink/stlink_tcl.c
+++ b/src/jtag/stlink/stlink_tcl.c
@@ -2,6 +2,9 @@
 *   Copyright (C) 2011 by Mathias Kuester                                 *
 *   Mathias Kuester <kesmtp@freenet.de>                                   *
 *                                                                         *
+ *   Copyright (C) 2012 by Spencer Oliver                                  *
+ *   spen@spen-soft.co.uk                                                  *
+ *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
@@ -58,7 +61,7 @@ static int jim_newtap_expected_id(Jim_Nvp *n, Jim_GetOptInfo *goi,

 #define NTAP_OPT_EXPECTED_ID 0

-static int jim_stlink_newtap_cmd(Jim_GetOptInfo *goi)
+static int jim_hl_newtap_cmd(Jim_GetOptInfo *goi)
 {
 	struct jtag_tap *pTap;
 	int x;
@@ -128,9 +131,9 @@ static int jim_stlink_newtap_cmd(Jim_GetOptInfo *goi)
 	return JIM_OK;
 }

-int jim_stlink_newtap(Jim_Interp *interp, int argc, Jim_Obj * const *argv)
+int jim_hl_newtap(Jim_Interp *interp, int argc, Jim_Obj * const *argv)
 {
 	Jim_GetOptInfo goi;
 	Jim_GetOpt_Setup(&goi, interp, argc - 1, argv + 1);
-	return jim_stlink_newtap_cmd(&goi);
+	return jim_hl_newtap_cmd(&goi);
 }
--- a/src/jtag/stlink/stlink_tcl.h
+++ b/src/jtag/stlink/stlink_tcl.h
@@ -2,6 +2,9 @@
 *   Copyright (C) 2011 by Mathias Kuester                                 *
 *   Mathias Kuester <kesmtp@freenet.de>                                   *
 *                                                                         *
+ *   Copyright (C) 2012 by Spencer Oliver                                  *
+ *   spen@spen-soft.co.uk                                                  *
+ *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
@@ -18,10 +21,10 @@
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/

-#ifndef _STLINK_TCL_
-#define _STLINK_TCL_
+#ifndef _HL_TCL_
+#define _HL_TCL_

 /** */
-int jim_stlink_newtap(Jim_Interp *interp, int argc, Jim_Obj * const *argv);
+int jim_hl_newtap(Jim_Interp *interp, int argc, Jim_Obj * const *argv);

-#endif
+#endif /* _HL_TCL_ */
--- a/src/jtag/stlink/stlink_transport.c
+++ b/src/jtag/stlink/stlink_transport.c
@@ -2,6 +2,9 @@
 *   Copyright (C) 2011 by Mathias Kuester                                 *
 *   Mathias Kuester <kesmtp@freenet.de>                                   *
 *                                                                         *
+ *   Copyright (C) 2012 by Spencer Oliver                                  *
+ *   spen@spen-soft.co.uk                                                  *
+ *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
@@ -28,23 +31,28 @@
 #include <transport/transport.h>
 #include <helper/time_support.h>
 #include <target/target.h>
-#include <jtag/stlink/stlink_tcl.h>
-#include <jtag/stlink/stlink_transport.h>
-#include <jtag/stlink/stlink_interface.h>
+#include <jtag/hla/hla_tcl.h>
+#include <jtag/hla/hla_transport.h>
+#include <jtag/hla/hla_interface.h>

-COMMAND_HANDLER(stlink_transport_jtag_command)
+COMMAND_HANDLER(hl_transport_jtag_command)
 {
-	LOG_DEBUG("stlink_transport_jtag_command");
+	LOG_DEBUG("hl_transport_jtag_command");

 	return ERROR_OK;
 }

+COMMAND_HANDLER(hl_transport_reset_command)
+{
+	return hl_interface_init_reset();
+}
+
 static const struct command_registration
-stlink_transport_stlink_subcommand_handlers[] = {
+hl_transport_stlink_subcommand_handlers[] = {
 	{
 	 .name = "newtap",
 	 .mode = COMMAND_CONFIG,
-	 .jim_handler = jim_stlink_newtap,
+	 .jim_handler = jim_hl_newtap,
 	 .help = "Create a new TAP instance named basename.tap_type, "
 	 "and appends it to the scan chain.",
 	 .usage = "basename tap_type '-irlen' count "
@@ -55,23 +63,23 @@ stlink_transport_stlink_subcommand_handlers[] = {
 };

 static const struct command_registration
-stlink_transport_jtag_subcommand_handlers[] = {
+hl_transport_jtag_subcommand_handlers[] = {
 	{
 	 .name = "init",
 	 .mode = COMMAND_ANY,
-	 .handler = stlink_transport_jtag_command,
+	 .handler = hl_transport_jtag_command,
 	 .usage = ""
 	 },
 	{
 	 .name = "arp_init",
 	 .mode = COMMAND_ANY,
-	 .handler = stlink_transport_jtag_command,
+	 .handler = hl_transport_jtag_command,
 	 .usage = ""
 	 },
 	{
 	 .name = "arp_init-reset",
 	 .mode = COMMAND_ANY,
-	 .handler = stlink_transport_jtag_command,
+	 .handler = hl_transport_reset_command,
 	 .usage = ""
 	 },
 	{
@@ -87,13 +95,13 @@ stlink_transport_jtag_subcommand_handlers[] = {
 	{
 	 .name = "tapdisable",
 	 .mode = COMMAND_EXEC,
-	 .handler = stlink_transport_jtag_command,
+	 .handler = hl_transport_jtag_command,
 	 .usage = "",
 	 },
 	{
 	 .name = "configure",
 	 .mode = COMMAND_EXEC,
-	 .handler = stlink_transport_jtag_command,
+	 .handler = hl_transport_jtag_command,
 	 .usage = "",
 	 },
 	{
@@ -104,7 +112,7 @@ stlink_transport_jtag_subcommand_handlers[] = {
 	{
 	 .name = "names",
 	 .mode = COMMAND_ANY,
-	 .handler = stlink_transport_jtag_command,
+	 .handler = hl_transport_jtag_command,
 	 .usage = "",
 	 },

@@ -114,33 +122,33 @@ stlink_transport_jtag_subcommand_handlers[] = {
 static const struct command_registration stlink_transport_command_handlers[] = {

 	{
-	 .name = "stlink",
+	 .name = "hla",
 	 .mode = COMMAND_ANY,
-	 .help = "perform stlink actions",
+	 .help = "perform hl adapter actions",
 	 .usage = "",
-	 .chain = stlink_transport_stlink_subcommand_handlers,
+	 .chain = hl_transport_stlink_subcommand_handlers,
 	 },
 	{
 	 .name = "jtag",
 	 .mode = COMMAND_ANY,
 	 .usage = "",
-	 .chain = stlink_transport_jtag_subcommand_handlers,
+	 .chain = hl_transport_jtag_subcommand_handlers,
 	 },
 	COMMAND_REGISTRATION_DONE
 };

-static int stlink_transport_register_commands(struct command_context *cmd_ctx)
+static int hl_transport_register_commands(struct command_context *cmd_ctx)
 {
 	return register_commands(cmd_ctx, NULL,
 				 stlink_transport_command_handlers);
 }

-static int stlink_transport_init(struct command_context *cmd_ctx)
+static int hl_transport_init(struct command_context *cmd_ctx)
 {
-	LOG_DEBUG("stlink_transport_init");
+	LOG_DEBUG("hl_transport_init");
 	struct target *t = get_current_target(cmd_ctx);
 	struct transport *transport;
-	enum stlink_transports tr;
+	enum hl_transports tr;

 	if (!t) {
 		LOG_ERROR("no current target");
@@ -157,26 +165,26 @@ static int stlink_transport_init(struct command_context *cmd_ctx)
 	LOG_DEBUG("current transport %s", transport->name);

 	/* get selected transport as enum */
-	tr = STLINK_TRANSPORT_UNKNOWN;
+	tr = HL_TRANSPORT_UNKNOWN;

-	if (strcmp(transport->name, "stlink_swd") == 0)
-		tr = STLINK_TRANSPORT_SWD;
-	else if (strcmp(transport->name, "stlink_jtag") == 0)
-		tr = STLINK_TRANSPORT_JTAG;
+	if (strcmp(transport->name, "hla_swd") == 0)
+		tr = HL_TRANSPORT_SWD;
+	else if (strcmp(transport->name, "hla_jtag") == 0)
+		tr = HL_TRANSPORT_JTAG;
 	else if (strcmp(transport->name, "stlink_swim") == 0)
-		tr = STLINK_TRANSPORT_SWIM;
+		tr = HL_TRANSPORT_SWIM;

-	int retval = stlink_interface_open(tr);
+	int retval = hl_interface_open(tr);

 	if (retval != ERROR_OK)
 		return retval;

-	return stlink_interface_init_target(t);
+	return hl_interface_init_target(t);
 }

-static int stlink_transport_select(struct command_context *ctx)
+static int hl_transport_select(struct command_context *ctx)
 {
-	LOG_DEBUG("stlink_transport_select");
+	LOG_DEBUG("hl_transport_select");

 	int retval;

@@ -184,7 +192,7 @@ static int stlink_transport_select(struct command_context *ctx)
 	 * That works with only C code ... no Tcl glue required.
 	 */

-	retval = stlink_transport_register_commands(ctx);
+	retval = hl_transport_register_commands(ctx);

 	if (retval != ERROR_OK)
 		return retval;
@@ -192,35 +200,30 @@ static int stlink_transport_select(struct command_context *ctx)
 	return ERROR_OK;
 }

-static struct transport stlink_swd_transport = {
-	.name = "stlink_swd",
-	.select = stlink_transport_select,
-	.init = stlink_transport_init,
+static struct transport hl_swd_transport = {
+	.name = "hla_swd",
+	.select = hl_transport_select,
+	.init = hl_transport_init,
 };

-static struct transport stlink_jtag_transport = {
-	.name = "stlink_jtag",
-	.select = stlink_transport_select,
-	.init = stlink_transport_init,
+static struct transport hl_jtag_transport = {
+	.name = "hla_jtag",
+	.select = hl_transport_select,
+	.init = hl_transport_init,
 };

 static struct transport stlink_swim_transport = {
 	.name = "stlink_swim",
-	.select = stlink_transport_select,
-	.init = stlink_transport_init,
+	.select = hl_transport_select,
+	.init = hl_transport_init,
 };

-const char *stlink_transports[] = { "stlink_swd", "stlink_jtag", "stlink_swim", NULL };
+const char *hl_transports[] = { "hla_swd", "hla_jtag", "stlink_swim", NULL };

-static void stlink_constructor(void) __attribute__ ((constructor));
-static void stlink_constructor(void)
+static void hl_constructor(void) __attribute__ ((constructor));
+static void hl_constructor(void)
 {
-	transport_register(&stlink_swd_transport);
-	transport_register(&stlink_jtag_transport);
+	transport_register(&hl_swd_transport);
+	transport_register(&hl_jtag_transport);
 	transport_register(&stlink_swim_transport);
 }
-
-bool transport_is_stlink(void)
-{
-	return get_current_transport() == &stlink_swd_transport;
-}
--- a/src/jtag/stlink/stlink_transport.h
+++ b/src/jtag/stlink/stlink_transport.h
@@ -2,6 +2,9 @@
 *   Copyright (C) 2011 by Mathias Kuester                                 *
 *   Mathias Kuester <kesmtp@freenet.de>                                   *
 *                                                                         *
+ *   Copyright (C) 2012 by Spencer Oliver                                  *
+ *   spen@spen-soft.co.uk                                                  *
+ *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
@@ -18,14 +21,14 @@
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/

-#ifndef _STLINK_TRANSPORT_
-#define _STLINK_TRANSPORT_
+#ifndef _HL_TRANSPORT
+#define _HL_TRANSPORT

-enum stlink_transports {
-	STLINK_TRANSPORT_UNKNOWN = 0,
-	STLINK_TRANSPORT_SWD,
-	STLINK_TRANSPORT_JTAG,
-	STLINK_TRANSPORT_SWIM
+enum hl_transports {
+	HL_TRANSPORT_UNKNOWN = 0,
+	HL_TRANSPORT_SWD,
+	HL_TRANSPORT_JTAG,
+	HL_TRANSPORT_SWIM
 };

-#endif
+#endif /* _HL_TRANSPORT */
--- a/src/jtag/interfaces.c
+++ b/src/jtag/interfaces.c
@@ -104,8 +104,8 @@ extern struct jtag_interface buspirate_interface;
 #if BUILD_REMOTE_BITBANG == 1
 extern struct jtag_interface remote_bitbang_interface;
 #endif
-#if BUILD_STLINK == 1
-extern struct jtag_interface stlink_interface;
+#if BUILD_HLADAPTER == 1
+extern struct jtag_interface hl_interface;
 #endif
 #if BUILD_OSBDM == 1
 extern struct jtag_interface osbdm_interface;
@@ -113,6 +113,9 @@ extern struct jtag_interface osbdm_interface;
 #if BUILD_OPENDOUS == 1
 extern struct jtag_interface opendous_interface;
 #endif
+#if BUILD_SYSFSGPIO == 1
+extern struct jtag_interface sysfsgpio_interface;
+#endif
 #endif /* standard drivers */

 /**
@@ -185,8 +188,8 @@ struct jtag_interface *jtag_interfaces[] = {
 #if BUILD_REMOTE_BITBANG == 1
 		&remote_bitbang_interface,
 #endif
-#if BUILD_STLINK == 1
-		&stlink_interface,
+#if BUILD_HLADAPTER == 1
+		&hl_interface,
 #endif
 #if BUILD_OSBDM == 1
 		&osbdm_interface,
@@ -194,6 +197,9 @@ struct jtag_interface *jtag_interfaces[] = {
 #if BUILD_OPENDOUS == 1
 		&opendous_interface,
 #endif
+#if BUILD_SYSFSGPIO == 1
+		&sysfsgpio_interface,
+#endif
 #endif /* standard drivers */
 		NULL,
 	};
--- a/src/jtag/jtag.h
+++ b/src/jtag/jtag.h
@@ -266,6 +266,7 @@ enum reset_types {
 	RESET_TRST_OPEN_DRAIN = 0x10,
 	RESET_SRST_PUSH_PULL  = 0x20,
 	RESET_SRST_NO_GATING  = 0x40,
+	RESET_CNCT_UNDER_SRST = 0x80
 };

 enum reset_types jtag_get_reset_config(void);
--- a/src/jtag/startup.tcl
+++ b/src/jtag/startup.tcl
@@ -106,4 +106,30 @@ proc jtag_nsrst_assert_width args {
 	eval adapter_nsrst_assert_width $args
 }

+# stlink migration helpers
+proc stlink_device_desc args {
+	echo "DEPRECATED! use 'hla_device_desc' not 'stlink_device_desc'"
+	eval hla_device_desc $args
+}
+
+proc stlink_serial args {
+	echo "DEPRECATED! use 'hla_serial' not 'stlink_serial'"
+	eval hla_serial $args
+}
+
+proc stlink_layout args {
+	echo "DEPRECATED! use 'hla_layout' not 'stlink_layout'"
+	eval hla_layout $args
+}
+
+proc stlink_vid_pid args {
+	echo "DEPRECATED! use 'hla_vid_pid' not 'stlink_vid_pid'"
+	eval hla_vid_pid $args
+}
+
+proc stlink args {
+	echo "DEPRECATED! use 'hla' not 'stlink'"
+	eval hla $args
+}
+
 # END MIGRATION AIDS
--- a/src/jtag/stlink/Makefile.am
+++ b/src/jtag/stlink/Makefile.am
@@ -1,23 +0,0 @@
-include $(top_srcdir)/common.mk
-
-noinst_LTLIBRARIES = libocdstlink.la
-
-libocdstlink_la_SOURCES = \
-	$(STLINKFILES)
-
-STLINKFILES =
-
-if STLINK
-STLINKFILES += stlink_transport.c
-STLINKFILES += stlink_tcl.c
-STLINKFILES += stlink_interface.c
-STLINKFILES += stlink_layout.c
-endif
-
-noinst_HEADERS = \
-	stlink_interface.h \
-	stlink_layout.h \
-	stlink_tcl.h \
-	stlink_transport.h
-
-MAINTAINERCLEANFILES = $(srcdir)/Makefile.in
--- a/src/jtag/stlink/stlink_interface.c
+++ b/src/jtag/stlink/stlink_interface.c
@@ -1,267 +0,0 @@
-/***************************************************************************
- *   Copyright (C) 2011 by Mathias Kuester                                 *
- *   Mathias Kuester <kesmtp@freenet.de>                                   *
- *                                                                         *
- *   This program is free software; you can redistribute it and/or modify  *
- *   it under the terms of the GNU General Public License as published by  *
- *   the Free Software Foundation; either version 2 of the License, or     *
- *   (at your option) any later version.                                   *
- *                                                                         *
- *   This program is distributed in the hope that it will be useful,       *
- *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
- *   GNU General Public License for more details.                          *
- *                                                                         *
- *   You should have received a copy of the GNU General Public License     *
- *   along with this program; if not, write to the                         *
- *   Free Software Foundation, Inc.,                                       *
- *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
- ***************************************************************************/
-
-#ifdef HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-/* project specific includes */
-#include <jtag/interface.h>
-#include <transport/transport.h>
-#include <helper/time_support.h>
-
-#include <jtag/stlink/stlink_tcl.h>
-#include <jtag/stlink/stlink_layout.h>
-#include <jtag/stlink/stlink_transport.h>
-#include <jtag/stlink/stlink_interface.h>
-
-#include <target/target.h>
-
-static struct stlink_interface_s stlink_if = { {0, 0, 0, 0, 0, 0}, 0, 0 };
-
-int stlink_interface_open(enum stlink_transports tr)
-{
-	LOG_DEBUG("stlink_interface_open");
-
-	/* set transport mode */
-	stlink_if.param.transport = tr;
-
-	return stlink_if.layout->open(&stlink_if);
-}
-
-int stlink_interface_init_target(struct target *t)
-{
-	int res;
-
-	LOG_DEBUG("stlink_interface_init_target");
-
-	/* this is the interface for the current target and we
-	 * can setup the private pointer in the tap structure
-	 * if the interface match the tap idcode
-	 */
-	res = stlink_if.layout->api->idcode(stlink_if.fd, &t->tap->idcode);
-
-	if (res != ERROR_OK)
-		return res;
-
-	unsigned ii, limit = t->tap->expected_ids_cnt;
-	int found = 0;
-
-	for (ii = 0; ii < limit; ii++) {
-		uint32_t expected = t->tap->expected_ids[ii];
-
-		/* treat "-expected-id 0" as a "don't-warn" wildcard */
-		if (!expected || (t->tap->idcode == expected)) {
-			found = 1;
-			break;
-		}
-	}
-
-	if (found == 0) {
-		LOG_ERROR("stlink_interface_init_target: target not found: idcode: 0x%08x",
-				t->tap->idcode);
-		return ERROR_FAIL;
-	}
-
-	t->tap->priv = &stlink_if;
-	t->tap->hasidcode = 1;
-
-	return ERROR_OK;
-}
-
-static int stlink_interface_init(void)
-{
-	LOG_DEBUG("stlink_interface_init");
-
-	/* here we can initialize the layout */
-	return stlink_layout_init(&stlink_if);
-}
-
-static int stlink_interface_quit(void)
-{
-	LOG_DEBUG("stlink_interface_quit");
-
-	return ERROR_OK;
-}
-
-static int stlink_interface_speed(int speed)
-{
-	LOG_DEBUG("stlink_interface_speed: ignore speed %d", speed);
-
-	return ERROR_OK;
-}
-
-static int stlink_speed_div(int speed, int *khz)
-{
-	*khz = speed;
-	return ERROR_OK;
-}
-
-static int stlink_khz(int khz, int *jtag_speed)
-{
-	*jtag_speed = khz;
-	return ERROR_OK;
-}
-
-static int stlink_interface_execute_queue(void)
-{
-	LOG_DEBUG("stlink_interface_execute_queue: ignored");
-
-	return ERROR_OK;
-}
-
-COMMAND_HANDLER(stlink_interface_handle_device_desc_command)
-{
-	LOG_DEBUG("stlink_interface_handle_device_desc_command");
-
-	if (CMD_ARGC == 1) {
-		stlink_if.param.device_desc = strdup(CMD_ARGV[0]);
-	} else {
-		LOG_ERROR
-		    ("expected exactly one argument to stlink_device_desc <description>");
-	}
-
-	return ERROR_OK;
-}
-
-COMMAND_HANDLER(stlink_interface_handle_serial_command)
-{
-	LOG_DEBUG("stlink_interface_handle_serial_command");
-
-	if (CMD_ARGC == 1) {
-		stlink_if.param.serial = strdup(CMD_ARGV[0]);
-	} else {
-		LOG_ERROR
-		    ("expected exactly one argument to stlink_serial <serial-number>");
-	}
-
-	return ERROR_OK;
-}
-
-COMMAND_HANDLER(stlink_interface_handle_layout_command)
-{
-	LOG_DEBUG("stlink_interface_handle_layout_command");
-
-	if (CMD_ARGC != 1) {
-		LOG_ERROR("Need exactly one argument to stlink_layout");
-		return ERROR_COMMAND_SYNTAX_ERROR;
-	}
-
-	if (stlink_if.layout) {
-		LOG_ERROR("already specified stlink_layout %s",
-			  stlink_if.layout->name);
-		return (strcmp(stlink_if.layout->name, CMD_ARGV[0]) != 0)
-		    ? ERROR_FAIL : ERROR_OK;
-	}
-
-	for (const struct stlink_layout *l = stlink_layout_get_list(); l->name;
-	     l++) {
-		if (strcmp(l->name, CMD_ARGV[0]) == 0) {
-			stlink_if.layout = l;
-			return ERROR_OK;
-		}
-	}
-
-	LOG_ERROR("No STLINK layout '%s' found", CMD_ARGV[0]);
-	return ERROR_FAIL;
-}
-
-COMMAND_HANDLER(stlink_interface_handle_vid_pid_command)
-{
-	LOG_DEBUG("stlink_interface_handle_vid_pid_command");
-
-	if (CMD_ARGC != 2) {
-		LOG_WARNING
-		    ("ignoring extra IDs in stlink_vid_pid (maximum is 1 pair)");
-		return ERROR_COMMAND_SYNTAX_ERROR;
-	}
-
-	COMMAND_PARSE_NUMBER(u16, CMD_ARGV[0], stlink_if.param.vid);
-	COMMAND_PARSE_NUMBER(u16, CMD_ARGV[1], stlink_if.param.pid);
-
-	return ERROR_OK;
-}
-
-COMMAND_HANDLER(stlink_interface_handle_api_command)
-{
-	if (CMD_ARGC != 1)
-		return ERROR_COMMAND_SYNTAX_ERROR;
-
-	unsigned new_api;
-	COMMAND_PARSE_NUMBER(uint, CMD_ARGV[0], new_api);
-	if ((new_api == 0) || (new_api > 2))
-		return ERROR_COMMAND_SYNTAX_ERROR;
-
-	stlink_if.param.api = new_api;
-
-	return ERROR_OK;
-}
-
-static const struct command_registration stlink_interface_command_handlers[] = {
-	{
-	 .name = "stlink_device_desc",
-	 .handler = &stlink_interface_handle_device_desc_command,
-	 .mode = COMMAND_CONFIG,
-	 .help = "set the stlink device description of the STLINK device",
-	 .usage = "description_string",
-	 },
-	{
-	 .name = "stlink_serial",
-	 .handler = &stlink_interface_handle_serial_command,
-	 .mode = COMMAND_CONFIG,
-	 .help = "set the serial number of the STLINK device",
-	 .usage = "serial_string",
-	 },
-	{
-	 .name = "stlink_layout",
-	 .handler = &stlink_interface_handle_layout_command,
-	 .mode = COMMAND_CONFIG,
-	 .help = "set the layout of the STLINK to usb or sg",
-	 .usage = "layout_name",
-	 },
-	{
-	 .name = "stlink_vid_pid",
-	 .handler = &stlink_interface_handle_vid_pid_command,
-	 .mode = COMMAND_CONFIG,
-	 .help = "the vendor and product ID of the STLINK device",
-	 .usage = "(vid pid)* ",
-	 },
-	 {
-	 .name = "stlink_api",
-	 .handler = &stlink_interface_handle_api_command,
-	 .mode = COMMAND_CONFIG,
-	 .help = "set the desired stlink api level",
-	 .usage = "api version 1 or 2",
-	 },
-	COMMAND_REGISTRATION_DONE
-};
-
-struct jtag_interface stlink_interface = {
-	.name = "stlink",
-	.supported = 0,
-	.commands = stlink_interface_command_handlers,
-	.transports = stlink_transports,
-	.init = stlink_interface_init,
-	.quit = stlink_interface_quit,
-	.speed = stlink_interface_speed,
-	.speed_div = stlink_speed_div,
-	.khz = stlink_khz,
-	.execute_queue = stlink_interface_execute_queue,
-};
--- a/src/rtos/ChibiOS.c
+++ b/src/rtos/ChibiOS.c
@@ -0,0 +1,557 @@
+/***************************************************************************
+ *   Copyright (C) 2012 by Matthias Blaicher                               *
+ *   Matthias Blaicher - matthias@blaicher.com                             *
+ *                                                                         *
+ *   Copyright (C) 2011 by Broadcom Corporation                            *
+ *   Evan Hunter - ehunter@broadcom.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.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <helper/time_support.h>
+#include <jtag/jtag.h>
+#include "target/target.h"
+#include "target/target_type.h"
+#include "target/armv7m.h"
+#include "target/cortex_m.h"
+#include "rtos.h"
+#include "helper/log.h"
+#include "helper/types.h"
+#include "rtos_chibios_stackings.h"
+
+
+/**
+ * @brief   ChibiOS/RT memory signature record.
+ *
+ * @details Definition copied from os/kernel/include/chregistry.h of ChibiOS/RT.
+ */
+struct ChibiOS_chdebug {
+	char      ch_identifier[4];       /**< @brief Always set to "main".       */
+	uint8_t   ch_zero;                /**< @brief Must be zero.               */
+	uint8_t   ch_size;                /**< @brief Size of this structure.     */
+	uint16_t  ch_version;             /**< @brief Encoded ChibiOS/RT version. */
+	uint8_t   ch_ptrsize;             /**< @brief Size of a pointer.          */
+	uint8_t   ch_timesize;            /**< @brief Size of a @p systime_t.     */
+	uint8_t   ch_threadsize;          /**< @brief Size of a @p Thread struct. */
+	uint8_t   cf_off_prio;            /**< @brief Offset of @p p_prio field.  */
+	uint8_t   cf_off_ctx;             /**< @brief Offset of @p p_ctx field.   */
+	uint8_t   cf_off_newer;           /**< @brief Offset of @p p_newer field. */
+	uint8_t   cf_off_older;           /**< @brief Offset of @p p_older field. */
+	uint8_t   cf_off_name;            /**< @brief Offset of @p p_name field.  */
+	uint8_t   cf_off_stklimit;        /**< @brief Offset of @p p_stklimit
+												field.                        */
+	uint8_t   cf_off_state;           /**< @brief Offset of @p p_state field. */
+	uint8_t   cf_off_flags;           /**< @brief Offset of @p p_flags field. */
+	uint8_t   cf_off_refs;            /**< @brief Offset of @p p_refs field.  */
+	uint8_t   cf_off_preempt;         /**< @brief Offset of @p p_preempt
+												field.                        */
+	uint8_t   cf_off_time;            /**< @brief Offset of @p p_time field.  */
+};
+
+#define GET_CH_KERNEL_MAJOR(codedVersion) ((codedVersion >> 11) & 0x1f)
+#define GET_CH_KERNEL_MINOR(codedVersion) ((codedVersion >> 6) & 0x1f)
+#define GET_CH_KERNEL_PATCH(codedVersion) ((codedVersion >> 0) & 0x3f)
+
+/**
+ * @brief ChibiOS thread states.
+ */
+const char *ChibiOS_thread_states[] = {
+	"READY", "CURRENT", "SUSPENDED", "WTSEM", "WTMTX", "WTCOND", "SLEEPING",
+	"WTEXIT", "WTOREVT", "WTANDEVT", "SNDMSGQ", "SNDMSG", "WTMSG", "WTQUEUE",
+	"FINAL"
+};
+
+#define CHIBIOS_NUM_STATES (sizeof(ChibiOS_thread_states)/sizeof(char *))
+
+/* Maximum ChibiOS thread name. There is no real limit set by ChibiOS but 64
+ * chars ought to be enough.
+ */
+#define CHIBIOS_THREAD_NAME_STR_SIZE (64)
+
+struct ChibiOS_params {
+	const char *target_name;
+
+	struct ChibiOS_chdebug *signature;
+	const struct rtos_register_stacking *stacking_info;
+};
+
+struct ChibiOS_params ChibiOS_params_list[] = {
+	{
+	"cortex_m",							/* target_name */
+	0,
+	NULL,									/* stacking_info */
+	},
+	{
+	"hla_target",							/* target_name */
+	0,
+	NULL,									/* stacking_info */
+	}
+};
+#define CHIBIOS_NUM_PARAMS ((int)(sizeof(ChibiOS_params_list)/sizeof(struct ChibiOS_params)))
+
+static int ChibiOS_detect_rtos(struct target *target);
+static int ChibiOS_create(struct target *target);
+static int ChibiOS_update_threads(struct rtos *rtos);
+static int ChibiOS_get_thread_reg_list(struct rtos *rtos, int64_t thread_id, char **hex_reg_list);
+static int ChibiOS_get_symbol_list_to_lookup(symbol_table_elem_t *symbol_list[]);
+
+struct rtos_type ChibiOS_rtos = {
+	.name = "ChibiOS",
+
+	.detect_rtos = ChibiOS_detect_rtos,
+	.create = ChibiOS_create,
+	.update_threads = ChibiOS_update_threads,
+	.get_thread_reg_list = ChibiOS_get_thread_reg_list,
+	.get_symbol_list_to_lookup = ChibiOS_get_symbol_list_to_lookup,
+};
+
+enum ChibiOS_symbol_values {
+	ChibiOS_VAL_rlist = 0,
+	ChibiOS_VAL_ch_debug = 1,
+	ChibiOS_VAL_chSysInit = 2
+};
+
+static char *ChibiOS_symbol_list[] = {
+	"rlist",		/* Thread ready list*/
+	"ch_debug",		/* Memory Signatur containing offsets of fields in rlist*/
+	"chSysInit",	/* Necessary part of API, used for ChibiOS detection*/
+	NULL
+};
+
+static int ChibiOS_update_memory_signature(struct rtos *rtos)
+{
+	int retval;
+	struct ChibiOS_params *param;
+	struct ChibiOS_chdebug *signature;
+
+	param = (struct ChibiOS_params *) rtos->rtos_specific_params;
+
+	/* Free existing memory description.*/
+	if (param->signature) {
+		free(param->signature);
+		param->signature = 0;
+	}
+
+	signature = malloc(sizeof(*signature));
+	if (!signature) {
+		LOG_ERROR("Could not allocate space for ChibiOS/RT memory signature");
+		return -1;
+	}
+
+	retval = target_read_buffer(rtos->target,
+								rtos->symbols[ChibiOS_VAL_ch_debug].address,
+								sizeof(*signature),
+								(uint8_t *) signature);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("Could not read ChibiOS/RT memory signature from target");
+		goto errfree;
+	}
+
+	if (strncmp(signature->ch_identifier, "main", 4) != 0) {
+		LOG_ERROR("Memory signature identifier does not contain magic bytes.");
+		goto errfree;
+	}
+
+	if (signature->ch_size < sizeof(*signature)) {
+		LOG_ERROR("ChibiOS/RT memory signature claims to be smaller "
+				"than expected");
+		goto errfree;
+	}
+
+	if (signature->ch_size > sizeof(*signature)) {
+		LOG_WARNING("ChibiOS/RT memory signature claims to be bigger than"
+					" expected. Assuming compatibility...");
+	}
+
+	/* Convert endianness of version field */
+	const uint8_t *versionTarget = (const uint8_t *)
+										&signature->ch_version;
+	signature->ch_version = rtos->target->endianness == TARGET_LITTLE_ENDIAN ?
+			le_to_h_u32(versionTarget) : be_to_h_u32(versionTarget);
+
+	const uint16_t ch_version = signature->ch_version;
+	LOG_INFO("Successfully loaded memory map of ChibiOS/RT target "
+			"running version %i.%i.%i", GET_CH_KERNEL_MAJOR(ch_version),
+			GET_CH_KERNEL_MINOR(ch_version), GET_CH_KERNEL_PATCH(ch_version));
+
+	/* Currently, we have the inherent assumption that all address pointers
+	 * are 32 bit wide. */
+	if (signature->ch_ptrsize != sizeof(uint32_t)) {
+		LOG_ERROR("ChibiOS/RT target memory signature claims an address"
+				  "width unequal to 32 bits!");
+		free(signature);
+		return -1;
+	}
+
+	param->signature = signature;
+	return 0;
+
+errfree:
+	/* Error reading the ChibiOS memory structure */
+	free(signature);
+	param->signature = 0;
+	return -1;
+}
+
+
+static int ChibiOS_update_stacking(struct rtos *rtos)
+{
+	/* Sometimes the stacking can not be determined only by looking at the
+	 * target name but only a runtime.
+	 *
+	 * For example, this is the case for cortex-m4 targets and ChibiOS which
+	 * only stack the FPU registers if it is enabled during ChibiOS build.
+	 *
+	 * Terminating which stacking is used is target depending.
+	 *
+	 * Assumptions:
+	 *  - Once ChibiOS is actually initialized, the stacking is fixed.
+	 *  - During startup code, the FPU might not be initialized and the
+	 *    detection might fail.
+	 *  - Since no threads are running during startup, the problem is solved
+	 *    by delaying stacking detection until there are more threads
+	 *    available than the current execution. In which case
+	 *    ChibiOS_get_thread_reg_list is called.
+	 */
+	int retval;
+
+	if (!rtos->rtos_specific_params)
+		return -1;
+
+	struct ChibiOS_params *param;
+	param = (struct ChibiOS_params *) rtos->rtos_specific_params;
+
+	/* Check for armv7m with *enabled* FPU, i.e. a Cortex M4  */
+	struct armv7m_common *armv7m_target = target_to_armv7m(rtos->target);
+	if (is_armv7m(armv7m_target)) {
+		if (armv7m_target->fp_feature == FPv4_SP) {
+			/* Found ARM v7m target which includes a FPU */
+			uint32_t cpacr;
+
+			retval = target_read_u32(rtos->target, FPU_CPACR, &cpacr);
+			if (retval != ERROR_OK) {
+				LOG_ERROR("Could not read CPACR register to check FPU state");
+				return -1;
+			}
+
+			/* Check if CP10 and CP11 are set to full access.
+			 * In ChibiOS this is done in ResetHandler() in crt0.c */
+			if (cpacr & 0x00F00000) {
+				/* Found target with enabled FPU */
+				/* FIXME: Need to figure out how to specify the FPU registers */
+				LOG_ERROR("ChibiOS ARM v7m targets with enabled FPU "
+						  " are NOT supported");
+				return -1;
+			}
+		}
+
+		/* Found ARM v7m target with no or disabled FPU */
+		param->stacking_info = &rtos_chibios_arm_v7m_stacking;
+		return 0;
+	}
+
+	return -1;
+}
+
+static int ChibiOS_update_threads(struct rtos *rtos)
+{
+	int retval;
+	const struct ChibiOS_params *param;
+	int tasks_found = 0;
+	int rtos_valid = -1;
+
+	if (!rtos->rtos_specific_params)
+		return -1;
+
+	if (!rtos->symbols) {
+		LOG_ERROR("No symbols for ChibiOS");
+		return -3;
+	}
+
+	param = (const struct ChibiOS_params *) rtos->rtos_specific_params;
+	/* Update the memory signature saved in the target memory */
+	if (!param->signature) {
+		retval = ChibiOS_update_memory_signature(rtos);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("Reading the memory signature of ChibiOS/RT failed");
+			return retval;
+		}
+	}
+
+	/* wipe out previous thread details if any */
+	int j;
+	if (rtos->thread_details) {
+		for (j = 0; j < rtos->thread_count; j++) {
+			struct thread_detail *current_thread = &rtos->thread_details[j];
+			if (current_thread->display_str != NULL)
+				free(current_thread->display_str);
+			if (current_thread->thread_name_str != NULL)
+				free(current_thread->thread_name_str);
+			if (current_thread->extra_info_str != NULL)
+				free(current_thread->extra_info_str);
+		}
+		free(rtos->thread_details);
+		rtos->thread_details = NULL;
+		rtos->thread_count = 0;
+	}
+	/* ChibiOS does not save the current thread count. We have to first
+	 * parse the double linked thread list to check for errors and the number of
+	 * threads. */
+	const uint32_t rlist = rtos->symbols[ChibiOS_VAL_rlist].address;
+	const struct ChibiOS_chdebug *signature = param->signature;
+	uint32_t current;
+	uint32_t previous;
+	uint32_t older;
+
+	current = rlist;
+	previous = rlist;
+	while (1) {
+		retval = target_read_u32(rtos->target,
+								 current + signature->cf_off_newer, &current);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("Could not read next ChibiOS thread");
+			return retval;
+		}
+		/* Could be NULL if the kernel is not initialized yet or if the
+		 * registry is corrupted. */
+		if (current == 0) {
+			LOG_ERROR("ChibiOS registry integrity check failed, NULL pointer");
+
+			rtos_valid = 0;
+			break;
+		}
+		/* Fetch previous thread in the list as a integrity check. */
+		retval = target_read_u32(rtos->target,
+								 current + signature->cf_off_older, &older);
+		if ((retval != ERROR_OK) || (older == 0) || (older != previous)) {
+			LOG_ERROR("ChibiOS registry integrity check failed, "
+						"double linked list violation");
+			rtos_valid = 0;
+			break;
+		}
+		/* Check for full iteration of the linked list. */
+		if (current == rlist)
+			break;
+		tasks_found++;
+		previous = current;
+	}
+	if (!rtos_valid) {
+		/* No RTOS, there is always at least the current execution, though */
+		LOG_INFO("Only showing current execution because of a broken "
+				"ChibiOS thread registry.");
+
+		const char tmp_thread_name[] = "Current Execution";
+		const char tmp_thread_extra_info[] = "No RTOS thread";
+
+		rtos->thread_details = (struct thread_detail *) malloc(
+				sizeof(struct thread_detail));
+		rtos->thread_details->threadid = 1;
+		rtos->thread_details->exists = true;
+		rtos->thread_details->display_str = NULL;
+
+		rtos->thread_details->extra_info_str = (char *) malloc(
+				sizeof(tmp_thread_extra_info));
+		strcpy(rtos->thread_details->extra_info_str, tmp_thread_extra_info);
+
+		rtos->thread_details->thread_name_str = (char *) malloc(
+				sizeof(tmp_thread_name));
+		strcpy(rtos->thread_details->thread_name_str, tmp_thread_name);
+
+		rtos->current_thread = 1;
+		rtos->thread_count = 1;
+		return ERROR_OK;
+	}
+
+	/* create space for new thread details */
+	rtos->thread_details = (struct thread_detail *) malloc(
+			sizeof(struct thread_detail) * tasks_found);
+	if (!rtos->thread_details) {
+		LOG_ERROR("Could not allocate space for thread details");
+		return -1;
+	}
+
+	rtos->thread_count = tasks_found;
+	/* Loop through linked list. */
+	struct thread_detail *curr_thrd_details = rtos->thread_details;
+	while (curr_thrd_details < rtos->thread_details + tasks_found) {
+		uint32_t name_ptr = 0;
+		char tmp_str[CHIBIOS_THREAD_NAME_STR_SIZE];
+
+		retval = target_read_u32(rtos->target,
+								 current + signature->cf_off_newer, &current);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("Could not read next ChibiOS thread");
+			return -6;
+		}
+
+		/* Check for full iteration of the linked list. */
+		if (current == rlist)
+			break;
+
+		/* Save the thread pointer */
+		curr_thrd_details->threadid = current;
+
+		/* read the name pointer */
+		retval = target_read_u32(rtos->target,
+								 current + signature->cf_off_name, &name_ptr);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("Could not read ChibiOS thread name pointer from target");
+			return retval;
+		}
+
+		/* Read the thread name */
+		retval = target_read_buffer(rtos->target, name_ptr,
+									CHIBIOS_THREAD_NAME_STR_SIZE,
+									(uint8_t *)&tmp_str);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("Error reading thread name from ChibiOS target");
+			return retval;
+		}
+		tmp_str[CHIBIOS_THREAD_NAME_STR_SIZE - 1] = '\x00';
+
+		if (tmp_str[0] == '\x00')
+			strcpy(tmp_str, "No Name");
+
+		curr_thrd_details->thread_name_str = (char *)malloc(
+				strlen(tmp_str) + 1);
+		strcpy(curr_thrd_details->thread_name_str, tmp_str);
+
+		/* State info */
+		uint8_t threadState;
+		const char *state_desc;
+
+		retval = target_read_u8(rtos->target,
+								current + signature->cf_off_state, &threadState);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("Error reading thread state from ChibiOS target");
+			return retval;
+		}
+
+
+		if (threadState < CHIBIOS_NUM_STATES)
+			state_desc = ChibiOS_thread_states[threadState];
+		else
+			state_desc = "Unknown state";
+
+		curr_thrd_details->extra_info_str = (char *)malloc(strlen(
+					state_desc)+1);
+		strcpy(curr_thrd_details->extra_info_str, state_desc);
+
+		curr_thrd_details->exists = true;
+		curr_thrd_details->display_str = NULL;
+
+		curr_thrd_details++;
+	}
+
+	uint32_t current_thrd;
+	/* NOTE: By design, cf_off_name equals readylist_current_offset */
+	retval = target_read_u32(rtos->target,
+							 rlist + signature->cf_off_name,
+							 &current_thrd);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("Could not read current Thread from ChibiOS target");
+		return retval;
+	}
+	rtos->current_thread = current_thrd;
+
+	return 0;
+}
+
+static int ChibiOS_get_thread_reg_list(struct rtos *rtos, int64_t thread_id, char **hex_reg_list)
+{
+	int retval;
+	const struct ChibiOS_params *param;
+	uint32_t stack_ptr = 0;
+
+	*hex_reg_list = NULL;
+	if ((rtos == NULL) || (thread_id == 0) ||
+			(rtos->rtos_specific_params == NULL))
+		return -1;
+
+	param = (const struct ChibiOS_params *) rtos->rtos_specific_params;
+
+	if (!param->signature)
+		return -1;
+
+	/* Update stacking if it can only be determined from runtime information */
+	if ((param->stacking_info == 0) &&
+		(ChibiOS_update_stacking(rtos) != ERROR_OK)) {
+		LOG_ERROR("Failed to determine exact stacking for the target type %s", rtos->target->type->name);
+		return -1;
+	}
+
+	/* Read the stack pointer */
+	retval = target_read_u32(rtos->target,
+							 thread_id + param->signature->cf_off_ctx, &stack_ptr);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("Error reading stack frame from ChibiOS thread");
+		return retval;
+	}
+
+	return rtos_generic_stack_read(rtos->target, param->stacking_info, stack_ptr, hex_reg_list);
+}
+
+static int ChibiOS_get_symbol_list_to_lookup(symbol_table_elem_t *symbol_list[])
+{
+	unsigned int i;
+	*symbol_list = (symbol_table_elem_t *) malloc(
+			sizeof(symbol_table_elem_t) * ARRAY_SIZE(ChibiOS_symbol_list));
+
+	for (i = 0; i < ARRAY_SIZE(ChibiOS_symbol_list); i++)
+		(*symbol_list)[i].symbol_name = ChibiOS_symbol_list[i];
+
+	return 0;
+}
+
+static int ChibiOS_detect_rtos(struct target *target)
+{
+	if ((target->rtos->symbols != NULL) &&
+			(target->rtos->symbols[ChibiOS_VAL_rlist].address != 0) &&
+			(target->rtos->symbols[ChibiOS_VAL_chSysInit].address != 0)) {
+
+		if (target->rtos->symbols[ChibiOS_VAL_ch_debug].address == 0) {
+			LOG_INFO("It looks like the target is running ChibiOS without "
+					"ch_debug.");
+			return 0;
+		}
+
+		/* looks like ChibiOS with memory map enabled.*/
+		return 1;
+	}
+
+	return 0;
+}
+
+static int ChibiOS_create(struct target *target)
+{
+	int i = 0;
+	while ((i < CHIBIOS_NUM_PARAMS) &&
+			(0 != strcmp(ChibiOS_params_list[i].target_name, target->type->name))) {
+		i++;
+	}
+	if (i >= CHIBIOS_NUM_PARAMS) {
+		LOG_WARNING("Could not find target \"%s\" in ChibiOS compatibility "
+				"list", target->type->name);
+		return -1;
+	}
+
+	target->rtos->rtos_specific_params = (void *) &ChibiOS_params_list[i];
+	return 0;
+}
--- a/src/rtos/FreeRTOS.c
+++ b/src/rtos/FreeRTOS.c
@@ -28,6 +28,7 @@
 #include "target/target_type.h"
 #include "rtos.h"
 #include "helper/log.h"
+#include "helper/types.h"
 #include "rtos_standard_stackings.h"

 #define FREERTOS_MAX_PRIORITIES	63
@@ -49,7 +50,19 @@ struct FreeRTOS_params {

 const struct FreeRTOS_params FreeRTOS_params_list[] = {
 	{
-	"cortex_m3",			/* target_name */
+	"cortex_m",			/* target_name */
+	4,						/* thread_count_width; */
+	4,						/* pointer_width; */
+	16,						/* list_next_offset; */
+	20,						/* list_width; */
+	8,						/* list_elem_next_offset; */
+	12,						/* list_elem_content_offset */
+	0,						/* thread_stack_offset; */
+	52,						/* thread_name_offset; */
+	&rtos_standard_Cortex_M3_stacking,	/* stacking_info */
+	},
+	{
+	"hla_target",			/* target_name */
 	4,						/* thread_count_width; */
 	4,						/* pointer_width; */
 	16,						/* list_next_offset; */
@@ -109,8 +122,6 @@ static char *FreeRTOS_symbol_list[] = {
 	NULL
 };

-#define FREERTOS_NUM_SYMBOLS (sizeof(FreeRTOS_symbol_list)/sizeof(char *))
-
 /* TODO: */
 /* this is not safe for little endian yet */
 /* may be problems reading if sizes are not 32 bit long integers. */
@@ -129,12 +140,12 @@ static int FreeRTOS_update_threads(struct rtos *rtos)
 	param = (const struct FreeRTOS_params *) rtos->rtos_specific_params;

 	if (rtos->symbols == NULL) {
-		LOG_OUTPUT("No symbols for FreeRTOS\r\n");
+		LOG_ERROR("No symbols for FreeRTOS");
 		return -3;
 	}

 	if (rtos->symbols[FreeRTOS_VAL_uxCurrentNumberOfTasks].address == 0) {
-		LOG_OUTPUT("Don't have the number of threads in FreeRTOS \r\n");
+		LOG_ERROR("Don't have the number of threads in FreeRTOS");
 		return -2;
 	}

@@ -145,7 +156,7 @@ static int FreeRTOS_update_threads(struct rtos *rtos)
 			(uint8_t *)&thread_list_size);

 	if (retval != ERROR_OK) {
-		LOG_OUTPUT("Could not read FreeRTOS thread count from target\r\n");
+		LOG_ERROR("Could not read FreeRTOS thread count from target");
 		return retval;
 	}

@@ -176,7 +187,7 @@ static int FreeRTOS_update_threads(struct rtos *rtos)
 			param->pointer_width,
 			(uint8_t *)&rtos->current_thread);
 	if (retval != ERROR_OK) {
-		LOG_OUTPUT("Error reading current thread in FreeRTOS thread list\r\n");
+		LOG_ERROR("Error reading current thread in FreeRTOS thread list");
 		return retval;
 	}

@@ -258,7 +269,7 @@ static int FreeRTOS_update_threads(struct rtos *rtos)
 				param->thread_count_width,
 				(uint8_t *)&list_thread_count);
 		if (retval != ERROR_OK) {
-			LOG_OUTPUT("Error reading number of threads in FreeRTOS thread list\r\n");
+			LOG_ERROR("Error reading number of threads in FreeRTOS thread list");
 			free(list_of_lists);
 			return retval;
 		}
@@ -274,7 +285,7 @@ static int FreeRTOS_update_threads(struct rtos *rtos)
 				param->pointer_width,
 				(uint8_t *)&list_elem_ptr);
 		if (retval != ERROR_OK) {
-			LOG_OUTPUT("Error reading first thread item location in FreeRTOS thread list\r\n");
+			LOG_ERROR("Error reading first thread item location in FreeRTOS thread list");
 			free(list_of_lists);
 			return retval;
 		}
@@ -289,7 +300,7 @@ static int FreeRTOS_update_threads(struct rtos *rtos)
 					param->pointer_width,
 					(uint8_t *)&(rtos->thread_details[tasks_found].threadid));
 			if (retval != ERROR_OK) {
-				LOG_OUTPUT("Error reading thread list item object in FreeRTOS thread list\r\n");
+				LOG_ERROR("Error reading thread list item object in FreeRTOS thread list");
 				free(list_of_lists);
 				return retval;
 			}
@@ -305,7 +316,7 @@ static int FreeRTOS_update_threads(struct rtos *rtos)
 					FREERTOS_THREAD_NAME_STR_SIZE,
 					(uint8_t *)&tmp_str);
 			if (retval != ERROR_OK) {
-				LOG_OUTPUT("Error reading first thread item location in FreeRTOS thread list\r\n");
+				LOG_ERROR("Error reading first thread item location in FreeRTOS thread list");
 				free(list_of_lists);
 				return retval;
 			}
@@ -339,7 +350,7 @@ static int FreeRTOS_update_threads(struct rtos *rtos)
 					param->pointer_width,
 					(uint8_t *)&list_elem_ptr);
 			if (retval != ERROR_OK) {
-				LOG_OUTPUT("Error reading next thread item location in FreeRTOS thread list\r\n");
+				LOG_ERROR("Error reading next thread item location in FreeRTOS thread list");
 				free(list_of_lists);
 				return retval;
 			}
@@ -375,21 +386,20 @@ static int FreeRTOS_get_thread_reg_list(struct rtos *rtos, int64_t thread_id, ch
 			param->pointer_width,
 			(uint8_t *)&stack_ptr);
 	if (retval != ERROR_OK) {
-		LOG_OUTPUT("Error reading stack frame from FreeRTOS thread\r\n");
+		LOG_ERROR("Error reading stack frame from FreeRTOS thread");
 		return retval;
 	}

 	return rtos_generic_stack_read(rtos->target, param->stacking_info, stack_ptr, hex_reg_list);
-
 }

 static int FreeRTOS_get_symbol_list_to_lookup(symbol_table_elem_t *symbol_list[])
 {
 	unsigned int i;
 	*symbol_list = (symbol_table_elem_t *) malloc(
-			sizeof(symbol_table_elem_t) * FREERTOS_NUM_SYMBOLS);
+			sizeof(symbol_table_elem_t) * ARRAY_SIZE(FreeRTOS_symbol_list));

-	for (i = 0; i < FREERTOS_NUM_SYMBOLS; i++)
+	for (i = 0; i < ARRAY_SIZE(FreeRTOS_symbol_list); i++)
 		(*symbol_list)[i].symbol_name = FreeRTOS_symbol_list[i];

 	return 0;
@@ -427,7 +437,7 @@ static int FreeRTOS_get_thread_ascii_info(struct rtos *rtos, threadid_t thread_i
 			FREERTOS_THREAD_NAME_STR_SIZE,
 			(uint8_t *)&tmp_str);
 	if (retval != ERROR_OK) {
-		LOG_OUTPUT("Error reading first thread item location in FreeRTOS thread list\r\n");
+		LOG_ERROR("Error reading first thread item location in FreeRTOS thread list");
 		return retval;
 	}
 	tmp_str[FREERTOS_THREAD_NAME_STR_SIZE-1] = '\x00';
@@ -460,7 +470,7 @@ static int FreeRTOS_create(struct target *target)
 		i++;
 	}
 	if (i >= FREERTOS_NUM_PARAMS) {
-		LOG_OUTPUT("Could not find target in FreeRTOS compatibility list\r\n");
+		LOG_ERROR("Could not find target in FreeRTOS compatibility list");
 		return -1;
 	}

--- a/src/rtos/Makefile.am
+++ b/src/rtos/Makefile.am
@@ -22,8 +22,8 @@ include $(top_srcdir)/common.mk

 METASOURCES = AUTO
 noinst_LTLIBRARIES = librtos.la
-noinst_HEADERS = rtos.h rtos_standard_stackings.h rtos_ecos_stackings.h linux_header.h
-librtos_la_SOURCES = rtos.c rtos_standard_stackings.c rtos_ecos_stackings.c FreeRTOS.c ThreadX.c eCos.c linux.c
+noinst_HEADERS = rtos.h rtos_standard_stackings.h rtos_ecos_stackings.h linux_header.h rtos_chibios_stackings.h
+librtos_la_SOURCES = rtos.c rtos_standard_stackings.c rtos_ecos_stackings.c rtos_chibios_stackings.c FreeRTOS.c ThreadX.c eCos.c linux.c ChibiOS.c

 librtos_la_CFLAGS =
 if IS_MINGW
--- a/src/rtos/ThreadX.c
+++ b/src/rtos/ThreadX.c
@@ -28,6 +28,7 @@
 #include "target/target_type.h"
 #include "rtos.h"
 #include "helper/log.h"
+#include "helper/types.h"
 #include "rtos_standard_stackings.h"

 static int ThreadX_detect_rtos(struct target *target);
@@ -72,14 +73,23 @@ struct ThreadX_params {

 const struct ThreadX_params ThreadX_params_list[] = {
 	{
-	"cortex_m3",				/* target_name */
+	"cortex_m",				/* target_name */
 	4,							/* pointer_width; */
 	8,							/* thread_stack_offset; */
 	40,							/* thread_name_offset; */
 	48,							/* thread_state_offset; */
 	136,						/* thread_next_offset */
 	&rtos_standard_Cortex_M3_stacking,	/* stacking_info */
-	}
+	},
+	{
+	"cortex_r4",				/* target_name */
+	4,							/* pointer_width; */
+	8,							/* thread_stack_offset; */
+	40,							/* thread_name_offset; */
+	48,							/* thread_state_offset; */
+	136,						/* thread_next_offset */
+	&rtos_standard_Cortex_R4_stacking,	/* stacking_info */
+	},
 };

 #define THREADX_NUM_PARAMS ((int)(sizeof(ThreadX_params_list)/sizeof(struct ThreadX_params)))
@@ -97,8 +107,6 @@ static char *ThreadX_symbol_list[] = {
 	NULL
 };

-#define THREADX_NUM_SYMBOLS (sizeof(ThreadX_symbol_list)/sizeof(char *))
-
 const struct rtos_type ThreadX_rtos = {
 	.name = "ThreadX",

@@ -126,12 +134,12 @@ static int ThreadX_update_threads(struct rtos *rtos)
 	param = (const struct ThreadX_params *) rtos->rtos_specific_params;

 	if (rtos->symbols == NULL) {
-		LOG_OUTPUT("No symbols for ThreadX\r\n");
+		LOG_ERROR("No symbols for ThreadX");
 		return -4;
 	}

 	if (rtos->symbols[ThreadX_VAL_tx_thread_created_count].address == 0) {
-		LOG_OUTPUT("Don't have the number of threads in ThreadX \r\n");
+		LOG_ERROR("Don't have the number of threads in ThreadX");
 		return -2;
 	}

@@ -142,7 +150,7 @@ static int ThreadX_update_threads(struct rtos *rtos)
 			(uint8_t *)&thread_list_size);

 	if (retval != ERROR_OK) {
-		LOG_OUTPUT("Could not read ThreadX thread count from target\r\n");
+		LOG_ERROR("Could not read ThreadX thread count from target");
 		return retval;
 	}

@@ -174,7 +182,7 @@ static int ThreadX_update_threads(struct rtos *rtos)
 			(uint8_t *)&rtos->current_thread);

 	if (retval != ERROR_OK) {
-		LOG_OUTPUT("Could not read ThreadX current thread from target\r\n");
+		LOG_ERROR("Could not read ThreadX current thread from target");
 		return retval;
 	}

@@ -211,7 +219,7 @@ static int ThreadX_update_threads(struct rtos *rtos)
 			param->pointer_width,
 			(uint8_t *)&thread_ptr);
 	if (retval != ERROR_OK) {
-		LOG_OUTPUT("Could not read ThreadX thread location from target\r\n");
+		LOG_ERROR("Could not read ThreadX thread location from target");
 		return retval;
 	}

@@ -233,7 +241,7 @@ static int ThreadX_update_threads(struct rtos *rtos)
 				param->pointer_width,
 				(uint8_t *)&name_ptr);
 		if (retval != ERROR_OK) {
-			LOG_OUTPUT("Could not read ThreadX thread name pointer from target\r\n");
+			LOG_ERROR("Could not read ThreadX thread name pointer from target");
 			return retval;
 		}

@@ -244,7 +252,7 @@ static int ThreadX_update_threads(struct rtos *rtos)
 				THREADX_THREAD_NAME_STR_SIZE,
 				(uint8_t *)&tmp_str);
 		if (retval != ERROR_OK) {
-			LOG_OUTPUT("Error reading thread name from ThreadX target\r\n");
+			LOG_ERROR("Error reading thread name from ThreadX target");
 			return retval;
 		}
 		tmp_str[THREADX_THREAD_NAME_STR_SIZE-1] = '\x00';
@@ -263,7 +271,7 @@ static int ThreadX_update_threads(struct rtos *rtos)
 				4,
 				(uint8_t *)&thread_status);
 		if (retval != ERROR_OK) {
-			LOG_OUTPUT("Error reading thread state from ThreadX target\r\n");
+			LOG_ERROR("Error reading thread state from ThreadX target");
 			return retval;
 		}

@@ -296,7 +304,7 @@ static int ThreadX_update_threads(struct rtos *rtos)
 				param->pointer_width,
 				(uint8_t *) &thread_ptr);
 		if (retval != ERROR_OK) {
-			LOG_OUTPUT("Error reading next thread pointer in ThreadX thread list\r\n");
+			LOG_ERROR("Error reading next thread pointer in ThreadX thread list");
 			return retval;
 		}
 	}
@@ -331,7 +339,7 @@ static int ThreadX_get_thread_reg_list(struct rtos *rtos, int64_t thread_id, cha
 			param->pointer_width,
 			(uint8_t *)&stack_ptr);
 	if (retval != ERROR_OK) {
-		LOG_OUTPUT("Error reading stack frame from ThreadX thread\r\n");
+		LOG_ERROR("Error reading stack frame from ThreadX thread");
 		return retval;
 	}

@@ -342,9 +350,9 @@ static int ThreadX_get_symbol_list_to_lookup(symbol_table_elem_t *symbol_list[])
 {
 	unsigned int i;
 	*symbol_list = (symbol_table_elem_t *) malloc(
-			sizeof(symbol_table_elem_t) * THREADX_NUM_SYMBOLS);
+			sizeof(symbol_table_elem_t) * ARRAY_SIZE(ThreadX_symbol_list));

-	for (i = 0; i < THREADX_NUM_SYMBOLS; i++)
+	for (i = 0; i < ARRAY_SIZE(ThreadX_symbol_list); i++)
 		(*symbol_list)[i].symbol_name = ThreadX_symbol_list[i];

 	return 0;
@@ -391,7 +399,7 @@ static int ThreadX_get_thread_detail(struct rtos *rtos,
 	param = (const struct ThreadX_params *) rtos->rtos_specific_params;

 	if (rtos->symbols == NULL) {
-		LOG_OUTPUT("No symbols for ThreadX\r\n");
+		LOG_ERROR("No symbols for ThreadX");
 		return -3;
 	}

@@ -404,7 +412,7 @@ static int ThreadX_get_thread_detail(struct rtos *rtos,
 			param->pointer_width,
 			(uint8_t *)&name_ptr);
 	if (retval != ERROR_OK) {
-		LOG_OUTPUT("Could not read ThreadX thread name pointer from target\r\n");
+		LOG_ERROR("Could not read ThreadX thread name pointer from target");
 		return retval;
 	}

@@ -414,7 +422,7 @@ static int ThreadX_get_thread_detail(struct rtos *rtos,
 			THREADX_THREAD_NAME_STR_SIZE,
 			(uint8_t *)&tmp_str);
 	if (retval != ERROR_OK) {
-		LOG_OUTPUT("Error reading thread name from ThreadX target\r\n");
+		LOG_ERROR("Error reading thread name from ThreadX target");
 		return retval;
 	}
 	tmp_str[THREADX_THREAD_NAME_STR_SIZE-1] = '\x00';
@@ -432,7 +440,7 @@ static int ThreadX_get_thread_detail(struct rtos *rtos,
 			4,
 			(uint8_t *)&thread_status);
 	if (retval != ERROR_OK) {
-		LOG_OUTPUT("Error reading thread state from ThreadX target\r\n");
+		LOG_ERROR("Error reading thread state from ThreadX target");
 		return retval;
 	}

@@ -466,7 +474,7 @@ static int ThreadX_create(struct target *target)
 		i++;
 	}
 	if (i >= THREADX_NUM_PARAMS) {
-		LOG_OUTPUT("Could not find target in ThreadX compatibility list\r\n");
+		LOG_ERROR("Could not find target in ThreadX compatibility list");
 		return -1;
 	}

--- a/src/rtos/eCos.c
+++ b/src/rtos/eCos.c
@@ -26,6 +26,7 @@
 #include "target/target_type.h"
 #include "rtos.h"
 #include "helper/log.h"
+#include "helper/types.h"
 #include "rtos_ecos_stackings.h"

 static int eCos_detect_rtos(struct target *target);
@@ -63,7 +64,7 @@ struct eCos_params {

 const struct eCos_params eCos_params_list[] = {
 	{
-	"cortex_m3",			/* target_name */
+	"cortex_m",			/* target_name */
 	4,						/* pointer_width; */
 	0x0c,					/* thread_stack_offset; */
 	0x9c,					/* thread_name_offset; */
@@ -87,8 +88,6 @@ static char *eCos_symbol_list[] = {
 	NULL
 };

-#define ECOS_NUM_SYMBOLS (sizeof(eCos_symbol_list)/sizeof(char *))
-
 const struct rtos_type eCos_rtos = {
 	.name = "eCos",

@@ -116,12 +115,12 @@ static int eCos_update_threads(struct rtos *rtos)
 	param = (const struct eCos_params *) rtos->rtos_specific_params;

 	if (rtos->symbols == NULL) {
-		LOG_OUTPUT("No symbols for eCos\r\n");
+		LOG_ERROR("No symbols for eCos");
 		return -4;
 	}

 	if (rtos->symbols[eCos_VAL_thread_list].address == 0) {
-		LOG_OUTPUT("Don't have the thread list head\r\n");
+		LOG_ERROR("Don't have the thread list head");
 		return -2;
 	}

@@ -178,7 +177,7 @@ static int eCos_update_threads(struct rtos *rtos)
 			2,
 			(uint8_t *)&rtos->current_thread);
 	if (retval != ERROR_OK) {
-		LOG_OUTPUT("Could not read eCos current thread from target\r\n");
+		LOG_ERROR("Could not read eCos current thread from target");
 		return retval;
 	}

@@ -225,7 +224,7 @@ static int eCos_update_threads(struct rtos *rtos)
 				2,
 				(uint8_t *)&thread_id);
 		if (retval != ERROR_OK) {
-			LOG_OUTPUT("Could not read eCos thread id from target\r\n");
+			LOG_ERROR("Could not read eCos thread id from target");
 			return retval;
 		}
 		rtos->thread_details[tasks_found].threadid = thread_id;
@@ -236,7 +235,7 @@ static int eCos_update_threads(struct rtos *rtos)
 				param->pointer_width,
 				(uint8_t *)&name_ptr);
 		if (retval != ERROR_OK) {
-			LOG_OUTPUT("Could not read eCos thread name pointer from target\r\n");
+			LOG_ERROR("Could not read eCos thread name pointer from target");
 			return retval;
 		}

@@ -247,7 +246,7 @@ static int eCos_update_threads(struct rtos *rtos)
 				ECOS_THREAD_NAME_STR_SIZE,
 				(uint8_t *)&tmp_str);
 		if (retval != ERROR_OK) {
-			LOG_OUTPUT("Error reading thread name from eCos target\r\n");
+			LOG_ERROR("Error reading thread name from eCos target");
 			return retval;
 		}
 		tmp_str[ECOS_THREAD_NAME_STR_SIZE-1] = '\x00';
@@ -266,7 +265,7 @@ static int eCos_update_threads(struct rtos *rtos)
 				4,
 				(uint8_t *)&thread_status);
 		if (retval != ERROR_OK) {
-			LOG_OUTPUT("Error reading thread state from eCos target\r\n");
+			LOG_ERROR("Error reading thread state from eCos target");
 			return retval;
 		}

@@ -300,7 +299,7 @@ static int eCos_update_threads(struct rtos *rtos)
 				param->pointer_width,
 				(uint8_t *) &thread_index);
 		if (retval != ERROR_OK) {
-			LOG_OUTPUT("Error reading next thread pointer in eCos thread list\r\n");
+			LOG_ERROR("Error reading next thread pointer in eCos thread list");
 			return retval;
 		}
 	} while (thread_index != first_thread);
@@ -340,7 +339,7 @@ static int eCos_get_thread_reg_list(struct rtos *rtos, int64_t thread_id, char *
 				2,
 				(uint8_t *)&id);
 		if (retval != ERROR_OK) {
-			LOG_OUTPUT("Error reading unique id from eCos thread\r\n");
+			LOG_ERROR("Error reading unique id from eCos thread");
 			return retval;
 		}

@@ -362,7 +361,7 @@ static int eCos_get_thread_reg_list(struct rtos *rtos, int64_t thread_id, char *
 				param->pointer_width,
 				(uint8_t *)&stack_ptr);
 		if (retval != ERROR_OK) {
-			LOG_OUTPUT("Error reading stack frame from eCos thread\r\n");
+			LOG_ERROR("Error reading stack frame from eCos thread");
 			return retval;
 		}

@@ -379,9 +378,9 @@ static int eCos_get_symbol_list_to_lookup(symbol_table_elem_t *symbol_list[])
 {
 	unsigned int i;
 	*symbol_list = (symbol_table_elem_t *) malloc(
-			sizeof(symbol_table_elem_t) * ECOS_NUM_SYMBOLS);
+			sizeof(symbol_table_elem_t) * ARRAY_SIZE(eCos_symbol_list));

-	for (i = 0; i < ECOS_NUM_SYMBOLS; i++)
+	for (i = 0; i < ARRAY_SIZE(eCos_symbol_list); i++)
 		(*symbol_list)[i].symbol_name = eCos_symbol_list[i];

 	return 0;
@@ -405,7 +404,7 @@ static int eCos_create(struct target *target)
 		i++;
 	}
 	if (i >= ECOS_NUM_PARAMS) {
-		LOG_OUTPUT("Could not find target in eCos compatibility list\r\n");
+		LOG_ERROR("Could not find target in eCos compatibility list");
 		return -1;
 	}

--- a/Show More
+++ b/Show More