Merge branch 'ft-dfu-code-loading'

24 files changed, 1221 insertions, 49 deletions

diff --git a/Makefile b/Makefile
index 57ed42c..e2159ed 100644
--- a/Makefile
+++ b/Makefile
@@ -47,6 +47,7 @@ export LIBS = $(MBED_DIR)/libstmf4.a $(RTOS_DIR)/librtos.a
 
 # linker script
 export LDSCRIPT = $(BOARD_DIR)/TOOLCHAIN_GCC_ARM/STM32F429BI.ld
+export BOOTLOADER_LDSCRIPT = $(BOARD_DIR)/TOOLCHAIN_GCC_ARM/STM32F429BI_bootloader.ld
 
 # board-specific objects, to link into every project
 export BOARD_OBJS = \
@@ -58,6 +59,7 @@ export BOARD_OBJS = \
 	$(TOPLEVEL)/stm-fpgacfg.o \
 	$(TOPLEVEL)/stm-keystore.o \
 	$(TOPLEVEL)/stm-sdram.o \
+	$(TOPLEVEL)/stm-flash.o \
 	$(TOPLEVEL)/syscalls.o \
 	$(BOARD_DIR)/TOOLCHAIN_GCC_ARM/startup_stm32f429xx.o \
 	$(BOARD_DIR)/system_stm32f4xx.o \
@@ -133,6 +135,9 @@ libhal-test: $(BOARD_OBJS) $(LIBS) $(LIBHAL_DIR)/libhal.a
 hsm: $(BOARD_OBJS) $(LIBS) $(LIBHAL_DIR)/libhal.a
 	$(MAKE) -C projects/hsm
 
+bootloader: $(BOARD_OBJS) $(LIBS)
+	$(MAKE) -C projects/bootloader
+
 # don't automatically delete objects, to avoid a lot of unnecessary rebuilding
 .SECONDARY: $(BOARD_OBJS)
 
diff --git a/README.md b/README.md
index aa259c1..6d813ef 100644
--- a/README.md
+++ b/README.md
@@ -1,9 +1,12 @@
-STM32 software for dev-bridge board
-===================================
+STM32 software for dev-bridge/Alpha board
+=========================================
 
 The dev-bridge board is a daughterboard for the Novena, which talks to the
 Novena's FPGA through the high-speed expansion connector.
 
+The Alpha board is a stand-alone board with an Artix-7 FPGA, a STM32 Cortex-M4
+microcontroller, two USB interfaces etc.
+
 See user/ft/stm32-dev-bridge/hardware/rev01 for schematics of the bridge
 board. There will be more information on the wiki shortly.
 
@@ -50,33 +53,58 @@ Do "bin/flash-target" from the top level directory (where this file is)
 to flash a built image into the microcontroller. See the section ST-LINK
 below for information about the actual hardware programming device needed.
 
+Example loading the bootloader and the led-test firmware to get some LEDs
+flashing:
+
+  $ make bootloader board-test
+  $ ./bin/flash-target projects/board-test/led-test
+  $ ./bin/flash-target projects/bootloader/bootloader
+
+At this point, the STM32 will reset into the bootloader which flashes the
+blue LED five times in one second, and then execution of the LED test
+firmware will begin. The LED test firmware will flash the green, yellow,
+red and blue LEDs in order until the end of time.
+
+Once the bootloader is installed, regular firmware can be loaded without
+an ST-LINK cable like this:
+
+  $ ./bin/dfu projects/board-test/led-test.bin
+
+Then reboot the Alpha board.
+
 
 ST-LINK
 =======
 To program the MCU, an ST-LINK adapter is used. The cheapest way to get
 one is to buy an evaluation board with an ST-LINK integrated, and pinouts
 to program external chips. This should work with any evaluation board from
-STM; we have tested with STM32F4DISCOVERY (with ST-LINK v2.0) and 
+STM; we have tested with STM32F4DISCOVERY (with ST-LINK v2.0) and
 NUCLEO-F411RE (with ST-LINK v2.1).
 
-The ST-LINK programming pins are the 1+4 throughole pads above the ARM
-on the circuit board. See the schematics for details, but the pinout
-from left to right (1, space, 4) of rev01 is
+The ST-LINK programming pins is called J1 and is near the CrypTech logo
+printed on the circuit board. The pin-outs is shown on the circuit board
+(follow the thin white line from J1 to the white box with STM32_SWD
+written in it). From left to right, the pins are
 
-  NRST, space, CLK, IO, GND, VCC
+  3V3, CLK, GND, I/O, NRST and N/C
+
+This matches the pin-out on the DISCO and NUCLEO boards we have tried.
 
 First remove the pair of ST-LINK jumpers (CN4 on the DISCO, CN2 on the
 NUCLEO). Then find the 6-pin SWD header on the left of the STM board (CN2
-on the DISCO, CN4 on the NUCLEO), and connect them to the dev-bridge
-board:
+on the DISCO, CN4 on the NUCLEO), and connect them to the Alpha board:
+
+    NUCLEO / DISCO       CRYPTECH ALPHA
+    --------------       --------------
+* 1 VDD_TARGET      <->  3V3
+* 2 SWCLK / T_JTCK  <->  CLK
+* 3 GND             <->  GND
+* 4 SWDIO / T_JTMS  <->  IO
+* 5 NRST / T_NRST   <->  NRST
 
-* 5 T_NRST <-> NRST
-* 2 T_JTCK <-> CLK
-* 4 T_JTMS <-> IO
-* 3 GND <-> GND
+N/C (pin 6) means Not Connected.
 
-The dev-bridge board should be connected to the Novena and powered on
-before attempting to flash it.
+The Alpha board should be powered on before attempting to flash it.
 
 
 Debugging the firmware
@@ -87,24 +115,13 @@ firmware in an STM32:
 
   http://fun-tech.se/stm32/OpenOCD/gdb.php
 
-I've only managed to get the most basic text line gdb to work,
-something along these lines:
-
-1) Start OpenOCD server (with a configuration file for your type of ST-LINK
-   adapter)
-
-   $ openocd -f /usr/share/openocd/scripts/board/stm32f4discovery.cfg
-
-2) Connect to the OpenOCD server and re-flash already compiled firmware:
-
-   $ telnet localhost 4444
-   reset halt
-   flash probe 0
-   stm32f2x mass_erase 0
-   flash write_bank 0 /path/to/main.bin 0
-   reset halt
+There is a shell script called 'bin/debug' that starts an OpenOCD server
+and GDB. Example:
 
-3) Start GDB and have it connect to the OpenOCD server:
+  $ ./bin/debug projects/board-test/led-test
 
-   $ arm-none-eabi-gdb --eval-command="target remote localhost:3333" main.elf
+Once in GDB, issue "monitor reset halt" to reset the STM32 before debugging.
 
+Remember that the first code to run will be the bootloader, but if you do
+e.g. "break main" and "continue" you will end up in led-test main() after
+the bootloader has jumped there.
diff --git a/bin/debug b/bin/debug
index 9309e66..f87a3b5 100755
--- a/bin/debug
+++ b/bin/debug
@@ -5,7 +5,9 @@ PROJ="${1?'project'}"
 OPENOCD=openocd
 OPENOCD_BOARD_DIR=/usr/share/openocd/scripts/board
 OPENOCD_PROC_FILE=stm32f4discovery.cfg
-#OPENOCD_PROC_FILE=st_nucleo_f4.cfg
+if [ "x`lsusb -d 0483:374b`" != "x" ]; then
+    OPENOCD_PROC_FILE=st_nucleo_f4.cfg
+fi
 $OPENOCD -f $OPENOCD_BOARD_DIR/$OPENOCD_PROC_FILE &
 
 GDB=arm-none-eabi-gdb
diff --git a/bin/dfu b/bin/dfu
new file mode 100755
index 0000000..e270438
--- /dev/null
+++ b/bin/dfu
@@ -0,0 +1,179 @@
+#!/usr/bin/python
+#
+# Copyright (c) 2016, NORDUnet A/S All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are
+# met:
+# - Redistributions of source code must retain the above copyright notice,
+#   this list of conditions and the following disclaimer.
+#
+# - Redistributions in binary form must reproduce the above copyright
+#   notice, this list of conditions and the following disclaimer in the
+#   documentation and/or other materials provided with the distribution.
+#
+# - Neither the name of the NORDUnet nor the names of its contributors may
+#   be used to endorse or promote products derived from this software
+#   without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+# IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+# TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+# PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+# HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+# TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+# PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+# LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+# NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+"""
+Utility to transfer a new firmware to the bootloader.
+
+Example usage:
+
+   ./bin/dfu path/to/firmware.bin
+
+Then reset the Cryptech Alpha board.
+"""
+import os
+import sys
+import time
+import struct
+import serial
+import argparse
+
+from binascii import crc32
+
+CHUNK_SIZE = 4096
+
+
+def parse_args():
+    """
+    Parse the command line arguments
+    """
+    parser = argparse.ArgumentParser(description = "Device firmware upgrader",
+                                     add_help = True,
+                                     formatter_class = argparse.ArgumentDefaultsHelpFormatter,
+                                     )
+
+    parser.add_argument('--verbose',
+                        dest='verbose',
+                        action='store_true', default=False,
+                        help='Verbose operation',
+                        )
+
+    parser.add_argument('--device',
+                        dest='device',
+                        default='/dev/ttyUSB0',
+                        help='Name of management port USB serial device',
+                        )
+
+    # positional argument(s)
+    parser.add_argument('filename')
+
+    return parser.parse_args()
+
+
+def _write(dst, data):
+    for i in range(len(data)):
+        dst.write(data[i])
+        time.sleep(0.1)
+    if len(data) == 4:
+        print("Wrote 0x{:02x}{:02x}{:02x}{:02x}".format(ord(data[0]), ord(data[1]), ord(data[2]), ord(data[3])))
+    else:
+        print("Wrote {!r}".format(data))
+
+
+def _read(dst, verbose=False):
+    res = ''
+    while True:
+        x = dst.read(1)
+        if not x:
+            break
+        res += x
+    if res and verbose:
+        print ("Read {!r}".format(res))
+    return res
+
+
+def send_file(filename, args):
+    s = os.stat(filename)
+    size = s.st_size
+    src = open(filename, 'rb')
+    chunk_size = CHUNK_SIZE
+
+    print("Waiting for contact with the bootloader on device {!s}, reset the CrypTech Alpha...".format(args.device))
+
+    while True:
+        try:
+            dst = serial.Serial(args.device, 921600, timeout=0.1)
+        except serial.SerialException:
+            time.sleep(0.2)
+            continue
+        dst.write('\r')
+        response = _read(dst, args.verbose)
+        if 'OK' in response:
+            dst.timeout=2
+            break
+
+    print('\nUploading firmware\n')
+
+    crc = 0
+    counter = 0
+    # 1. Write size of file (4 bytes)
+    _write(dst, struct.pack('<I', size))
+    _read(dst, args.verbose)
+    # 2. Write file contents while calculating CRC-32
+    while True:
+        data = src.read(chunk_size)
+        if not data:
+            break
+        dst.write(data)
+        print("Wrote {!s} bytes (chunk {!s}/{!s})".format(len(data), counter, int(size / chunk_size)))
+        # read ACK (a counter of number of 4k chunks received)
+        while True:
+            ack_bytes = dst.read(4)
+            if len(ack_bytes) == 4:
+                break
+            print('ERROR: Did not receive an ACK, got {!r}'.format(ack_bytes))
+            dst.write('\r')  # eventually get back to the CLI prompt
+        ack = struct.unpack('<I', ack_bytes)[0]
+        if ack != counter + 1:
+            print('ERROR: Did not receive the expected counter as ACK (got {!r}/{!r}, not {!r})'.format(ack, ack_bytes, counter))
+            flush = dst.read(100)
+            print('FLUSH data: {!r}'.format(flush))
+            return False
+        counter += 1
+
+        crc = crc32(data, crc) & 0xffffffff
+
+    _read(dst, args.verbose)
+
+    # 3. Write CRC-32 (4 bytes)
+    _write(dst, struct.pack('<I', crc))
+
+    if args.verbose:
+        print('\nFinished - this command might hang now depending on the firmware loaded')
+        _read(dst, args.verbose)
+    else:
+        print('\nFinished uploading firmware')
+
+    src.close()
+    dst.close()
+    return True
+
+
+def main(args):
+    send_file(args.filename, args)
+    return True
+
+if __name__ == '__main__':
+    try:
+        args = parse_args()
+        if main(args):
+            sys.exit(0)
+        sys.exit(1)
+    except KeyboardInterrupt:
+        pass
+
diff --git a/bin/flash-target b/bin/flash-target
index aa77a99..0d60c85 100755
--- a/bin/flash-target
+++ b/bin/flash-target
@@ -1,7 +1,6 @@
 #!/bin/sh
 
 PROJ="${1?'project'}"
-
 OPENOCD=openocd
 
 # location of OpenOCD Board .cfg files (only used with 'make flash-target')
@@ -22,6 +21,8 @@ OPENOCD_BOARD_DIR=/usr/share/openocd/scripts/board
 # the OPENOCD_BOARD_DIR directory.
 #
 OPENOCD_PROC_FILE=stm32f4discovery.cfg
-#OPENOCD_PROC_FILE=st_nucleo_f4.cfg
+if [ "x`lsusb -d 0483:374b`" != "x" ]; then
+    OPENOCD_PROC_FILE=st_nucleo_f4.cfg
+fi
 
 $OPENOCD -f $OPENOCD_BOARD_DIR/$OPENOCD_PROC_FILE -c "program $PROJ.elf verify reset exit"
diff --git a/bin/reset b/bin/reset
index 4461cf1..1c57f97 100755
--- a/bin/reset
+++ b/bin/reset
@@ -20,6 +20,8 @@ OPENOCD_BOARD_DIR=/usr/share/openocd/scripts/board
 # the OPENOCD_BOARD_DIR directory.
 #
 OPENOCD_PROC_FILE=stm32f4discovery.cfg
-#OPENOCD_PROC_FILE=st_nucleo_f4.cfg
+if [ "x`lsusb -d 0483:374b`" != "x" ]; then
+    OPENOCD_PROC_FILE=st_nucleo_f4.cfg
+fi
 
 $OPENOCD -f $OPENOCD_BOARD_DIR/$OPENOCD_PROC_FILE -c "init" -c "reset run" -c "exit"
diff --git a/libraries/mbed/targets/cmsis/TARGET_STM/TARGET_STM32F4/TARGET_CRYPTECH_ALPHA/TOOLCHAIN_GCC_ARM/STM32F429BI.ld b/libraries/mbed/targets/cmsis/TARGET_STM/TARGET_STM32F4/TARGET_CRYPTECH_ALPHA/TOOLCHAIN_GCC_ARM/STM32F429BI.ld
index c3aa304..ad7ddaf 100644
--- a/libraries/mbed/targets/cmsis/TARGET_STM/TARGET_STM32F4/TARGET_CRYPTECH_ALPHA/TOOLCHAIN_GCC_ARM/STM32F429BI.ld
+++ b/libraries/mbed/targets/cmsis/TARGET_STM/TARGET_STM32F4/TARGET_CRYPTECH_ALPHA/TOOLCHAIN_GCC_ARM/STM32F429BI.ld
@@ -1,16 +1,24 @@
-/* Linker script to configure memory regions. */
+/* Linker script to configure memory regions.
+ *
+ * This is the script for the firmware (meaning any application except the bootloader).
+ * It should be placed 128 KB from the start of the STM32 internal flash.
+ *
+ */
 MEMORY
-{ 
-  FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 2048k
-  CCM (rwx) : ORIGIN = 0x10000000, LENGTH = 64K
-  RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 192k
+{
+  /* FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 2048k */
+  BOOTLOADER (rx) : ORIGIN = 0x08000000, LENGTH = 128K
+  FIRMWARE (rx)   : ORIGIN = 0x08000000 + 128K, LENGTH = 2048K - 128K
+  FLASH (rx)      : ORIGIN = 0x08000000 + 128K, LENGTH = 2048K - 128K
+  RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 192K - 4
 }
+/* original:  FLASH (rx)   : ORIGIN = 0x08000000, LENGTH = 2048K */
 
 /* Linker script to place sections and symbol values. Should be used together
  * with other linker script that defines memory regions FLASH and RAM.
  * It references following symbols, which must be defined in code:
  *   Reset_Handler : Entry of reset handler
- * 
+ *
  * It defines following symbols, which code can use without definition:
  *   __exidx_start
  *   __exidx_end
@@ -149,6 +157,17 @@ SECTIONS
     __StackLimit = __StackTop - SIZEOF(.stack_dummy);
     PROVIDE(__stack = __StackTop);
 
+    /* The DFU code needs to know where the firmware lives */
+    CRYPTECH_BOOTLOADER_START = ORIGIN(BOOTLOADER);
+    CRYPTECH_BOOTLOADER_END = ORIGIN(BOOTLOADER) + LENGTH(BOOTLOADER) - 1;
+    CRYPTECH_FIRMWARE_START = ORIGIN(FIRMWARE);
+    CRYPTECH_FIRMWARE_END = ORIGIN(FIRMWARE) + LENGTH(FIRMWARE) - 1;
+    /* The last 4 bytes of RAM is used to control the DFU reset+jumping.
+     * Have to be reserved in here to not get overwritten by the Reset_Handler
+     * that zeros memory. Maybe there is a better way?
+     */
+    CRYPTECH_DFU_CONTROL = ORIGIN(RAM) + LENGTH(RAM);
+
     /* Check if data + heap + stack exceeds RAM limit */
     ASSERT(__StackLimit >= __HeapLimit, "region RAM overflowed with stack")
 }
diff --git a/libraries/mbed/targets/cmsis/TARGET_STM/TARGET_STM32F4/TARGET_CRYPTECH_ALPHA/TOOLCHAIN_GCC_ARM/STM32F429BI_bootloader.ld b/libraries/mbed/targets/cmsis/TARGET_STM/TARGET_STM32F4/TARGET_CRYPTECH_ALPHA/TOOLCHAIN_GCC_ARM/STM32F429BI_bootloader.ld
new file mode 100644
index 0000000..2a5de56
--- /dev/null
+++ b/libraries/mbed/targets/cmsis/TARGET_STM/TARGET_STM32F4/TARGET_CRYPTECH_ALPHA/TOOLCHAIN_GCC_ARM/STM32F429BI_bootloader.ld
@@ -0,0 +1,172 @@
+/* Linker script to configure memory regions.
+ *
+ * This one is for the bootloader, so FLASH points at the real beginning of the
+ * STM32 internal flash memory (0x08000000).
+ *
+ */
+MEMORY
+{
+  /* FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 2048k */
+  BOOTLOADER (rx) : ORIGIN = 0x08000000, LENGTH = 128K
+  FIRMWARE (rx)   : ORIGIN = 0x08000000 + 128K, LENGTH = 2048K - 128K
+  FLASH (rx)      : ORIGIN = 0x08000000, LENGTH = 2048K
+  RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 192K - 4
+}
+
+/* Linker script to place sections and symbol values. Should be used together
+ * with other linker script that defines memory regions FLASH and RAM.
+ * It references following symbols, which must be defined in code:
+ *   Reset_Handler : Entry of reset handler
+ *
+ * It defines following symbols, which code can use without definition:
+ *   __exidx_start
+ *   __exidx_end
+ *   __etext
+ *   __data_start__
+ *   __preinit_array_start
+ *   __preinit_array_end
+ *   __init_array_start
+ *   __init_array_end
+ *   __fini_array_start
+ *   __fini_array_end
+ *   __data_end__
+ *   __bss_start__
+ *   __bss_end__
+ *   __end__
+ *   end
+ *   __HeapLimit
+ *   __StackLimit
+ *   __StackTop
+ *   __stack
+ *   _estack
+ */
+ENTRY(Reset_Handler)
+
+SECTIONS
+{
+    .text :
+    {
+        KEEP(*(.isr_vector))
+        *(.text*)
+        KEEP(*(.init))
+        KEEP(*(.fini))
+
+        /* .ctors */
+        *crtbegin.o(.ctors)
+        *crtbegin?.o(.ctors)
+        *(EXCLUDE_FILE(*crtend?.o *crtend.o) .ctors)
+        *(SORT(.ctors.*))
+        *(.ctors)
+
+        /* .dtors */
+        *crtbegin.o(.dtors)
+        *crtbegin?.o(.dtors)
+        *(EXCLUDE_FILE(*crtend?.o *crtend.o) .dtors)
+        *(SORT(.dtors.*))
+        *(.dtors)
+
+        *(.rodata*)
+
+        KEEP(*(.eh_frame*))
+    } > FLASH
+
+    .ARM.extab :
+    {
+        *(.ARM.extab* .gnu.linkonce.armextab.*)
+    } > FLASH
+
+    __exidx_start = .;
+    .ARM.exidx :
+    {
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+    } > FLASH
+    __exidx_end = .;
+
+    __etext = .;
+    _sidata = .;
+
+    .data : AT (__etext)
+    {
+        __data_start__ = .;
+        _sdata = .;
+        *(vtable)
+        *(.data*)
+
+        . = ALIGN(4);
+        /* preinit data */
+        PROVIDE_HIDDEN (__preinit_array_start = .);
+        KEEP(*(.preinit_array))
+        PROVIDE_HIDDEN (__preinit_array_end = .);
+
+        . = ALIGN(4);
+        /* init data */
+        PROVIDE_HIDDEN (__init_array_start = .);
+        KEEP(*(SORT(.init_array.*)))
+        KEEP(*(.init_array))
+        PROVIDE_HIDDEN (__init_array_end = .);
+
+
+        . = ALIGN(4);
+        /* finit data */
+        PROVIDE_HIDDEN (__fini_array_start = .);
+        KEEP(*(SORT(.fini_array.*)))
+        KEEP(*(.fini_array))
+        PROVIDE_HIDDEN (__fini_array_end = .);
+
+        KEEP(*(.jcr*))
+        . = ALIGN(4);
+        /* All data end */
+        __data_end__ = .;
+        _edata = .;
+
+    } > RAM
+
+    .bss :
+    {
+        . = ALIGN(4);
+        __bss_start__ = .;
+        _sbss = .;
+        *(.bss*)
+        *(COMMON)
+        . = ALIGN(4);
+        __bss_end__ = .;
+        _ebss = .;
+    } > RAM
+
+    .heap (COPY):
+    {
+        __end__ = .;
+        end = __end__;
+        *(.heap*)
+        __HeapLimit = .;
+    } > RAM
+
+    /* .stack_dummy section doesn't contains any symbols. It is only
+     * used for linker to calculate size of stack sections, and assign
+     * values to stack symbols later */
+    .stack_dummy (COPY):
+    {
+        *(.stack*)
+    } > RAM
+
+    /* Set stack top to end of RAM, and stack limit move down by
+     * size of stack_dummy section */
+    __StackTop = ORIGIN(RAM) + LENGTH(RAM);
+    _estack = __StackTop;
+    __StackLimit = __StackTop - SIZEOF(.stack_dummy);
+    PROVIDE(__stack = __StackTop);
+
+    /* The DFU code needs to know where the firmware lives */
+    CRYPTECH_BOOTLOADER_START = ORIGIN(BOOTLOADER);
+    CRYPTECH_BOOTLOADER_END = ORIGIN(BOOTLOADER) + LENGTH(BOOTLOADER) - 1;
+    CRYPTECH_FIRMWARE_START = ORIGIN(FIRMWARE);
+    CRYPTECH_FIRMWARE_END = ORIGIN(FIRMWARE) + LENGTH(FIRMWARE) - 1;
+    /* The last 4 bytes of RAM is used to control the DFU reset+jumping.
+     * Have to be reserved in here to not get overwritten by the Reset_Handler
+     * that zeros memory. Maybe there is a better way?
+     */
+    CRYPTECH_DFU_CONTROL = ORIGIN(RAM) + LENGTH(RAM);
+
+    /* Check if data + heap + stack exceeds RAM limit */
+    ASSERT(__StackLimit >= __HeapLimit, "region RAM overflowed with stack")
+}
diff --git a/libraries/mbed/targets/cmsis/TARGET_STM/TARGET_STM32F4/TARGET_CRYPTECH_ALPHA/system_stm32f4xx.c b/libraries/mbed/targets/cmsis/TARGET_STM/TARGET_STM32F4/TARGET_CRYPTECH_ALPHA/system_stm32f4xx.c
index c2816d0..2824fd6 100644
--- a/libraries/mbed/targets/cmsis/TARGET_STM/TARGET_STM32F4/TARGET_CRYPTECH_ALPHA/system_stm32f4xx.c
+++ b/libraries/mbed/targets/cmsis/TARGET_STM/TARGET_STM32F4/TARGET_CRYPTECH_ALPHA/system_stm32f4xx.c
@@ -197,10 +197,14 @@ void SystemInit(void)
 #endif /* DATA_IN_ExtSRAM || DATA_IN_ExtSDRAM */
 
   /* Configure the Vector Table location add offset address ------------------*/
+  /* cryptech: Don't change VTOR if it is already set up by the bootloader */
 #ifdef VECT_TAB_SRAM
   SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM */
 #else
-  SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH */
+  /* Set up VTOR unless it has already been set by the bootloader. */
+  if (! SCB->VTOR) {
+      SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH */
+  }
 #endif
 
   /* Configure the Cube driver */
diff --git a/projects/bootloader/Makefile b/projects/bootloader/Makefile
new file mode 100644
index 0000000..4eef758
--- /dev/null
+++ b/projects/bootloader/Makefile
@@ -0,0 +1,20 @@
+PROG = bootloader
+
+OBJS = crc32.o dfu.o
+
+all: $(PROG:=.elf)
+
+%.elf: %.o $(BOARD_OBJS) $(OBJS) $(LIBS)
+	$(CC) $(CFLAGS) $^ -o $@ -T$(BOOTLOADER_LDSCRIPT) -g -Wl,-Map=$*.map
+	$(OBJCOPY) -O ihex $*.elf $*.hex
+	$(OBJCOPY) -O binary $*.elf $*.bin
+	$(OBJDUMP) -St $*.elf >$*.lst
+	$(SIZE) $*.elf
+
+clean:
+	rm -f *.o
+	rm -f *.elf
+	rm -f *.hex
+	rm -f *.bin
+	rm -f *.map
+	rm -f *.lst
diff --git a/projects/bootloader/bootloader.c b/projects/bootloader/bootloader.c
new file mode 100644
index 0000000..ab3c1d9
--- /dev/null
+++ b/projects/bootloader/bootloader.c
@@ -0,0 +1,126 @@
+/*
+ * bootloader.c
+ * ------------
+ * Bootloader to either install new firmware received from the MGMT UART,
+ * or jump to previously installed firmware.
+ *
+ * Copyright (c) 2016, NORDUnet A/S All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are
+ * met:
+ * - Redistributions of source code must retain the above copyright notice,
+ *   this list of conditions and the following disclaimer.
+ *
+ * - Redistributions in binary form must reproduce the above copyright
+ *   notice, this list of conditions and the following disclaimer in the
+ *   documentation and/or other materials provided with the distribution.
+ *
+ * - Neither the name of the NORDUnet nor the names of its contributors may
+ *   be used to endorse or promote products derived from this software
+ *   without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+ * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+ * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+ * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+ * TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+#include "stm-init.h"
+#include "stm-led.h"
+#include "stm-uart.h"
+#include "dfu.h"
+
+/* Linker symbols are strange in C. Make regular pointers for sanity. */
+__IO uint32_t *dfu_control = &CRYPTECH_DFU_CONTROL;
+__IO uint32_t *dfu_firmware = &CRYPTECH_FIRMWARE_START;
+/* The first word in the firmware is an address to the stack (msp) */
+__IO uint32_t *dfu_msp_ptr = &CRYPTECH_FIRMWARE_START;
+/* The second word in the firmware is a pointer to the code
+ * (points at the Reset_Handler from the linker script).
+ */
+__IO uint32_t *dfu_code_ptr = &CRYPTECH_FIRMWARE_START + 1;
+
+typedef  void (*pFunction)(void);
+
+/* This is it's own function to make it more convenient to set a breakpoint at it in gdb */
+void do_early_dfu_jump(void)
+{
+    pFunction loaded_app = (pFunction) *dfu_code_ptr;
+    /* Set the stack pointer to the correct one for the firmware */
+    __set_MSP(*dfu_msp_ptr);
+    /* Set the Vector Table Offset Register */
+    SCB->VTOR = (uint32_t) dfu_firmware;
+    loaded_app();
+    while (1);
+}
+
+int should_dfu()
+{
+    int i;
+    uint8_t rx = 0;
+
+    /* While blinking the blue LED for one second, see if we receive a CR on the MGMT UART.
+     * We've discussed also requiring one or both of the FPGA config jumpers installed
+     * before allowing DFU of the STM32 - that check could be done here.
+     */
+    led_on(LED_BLUE);
+    for (i = 0; i < 10; i++) {
+	HAL_Delay(100);
+	led_toggle(LED_BLUE);
+	if (uart_recv_char2(STM_UART_MGMT, &rx, 0) == HAL_OK) {
+	    if (rx == 13) return 1;
+	}
+    }
+    return 0;
+}
+
+int
+main()
+{
+    int status;
+
+    /* Check if we've just rebooted in order to jump to the firmware. */
+    if (*dfu_control == HARDWARE_EARLY_DFU_JUMP) {
+	*dfu_control = 0;
+	do_early_dfu_jump();
+    }
+
+    stm_init();
+
+    uart_send_string2(STM_UART_MGMT, (char *) "\r\n\r\nThis is the bootloader speaking...");
+
+    if (should_dfu()) {
+	led_off(LED_BLUE);
+	if ((status = dfu_receive_firmware()) != 0) {
+	    /* Upload of new firmware failed, reboot after lighting the red LED
+	     * for three seconds.
+	     */
+	    led_off(LED_BLUE);
+	    led_on(LED_RED);
+	    uart_send_string2(STM_UART_MGMT, (char *) "dfu_receive_firmware failed: ");
+	    uart_send_number2(STM_UART_MGMT, status, 3, 16);
+	    uart_send_string2(STM_UART_MGMT, (char *) "\r\n\r\nRebooting in three seconds\r\n");
+	    HAL_Delay(3000);
+	    HAL_NVIC_SystemReset();
+	    while (1) {};
+	}
+    }
+
+    /* Set dfu_control to the magic value that will cause the us to call do_early_dfu_jump
+     * after rebooting back into this main() function.
+     */
+    *dfu_control = HARDWARE_EARLY_DFU_JUMP;
+
+    uart_send_string2(STM_UART_MGMT, (char *) "loading firmware\r\n\r\n");
+
+    /* De-initialize hardware by rebooting */
+    HAL_NVIC_SystemReset();
+    while (1) {};
+}
diff --git a/projects/bootloader/crc32.c b/projects/bootloader/crc32.c
new file mode 100644
index 0000000..4d1a0bc
--- /dev/null
+++ b/projects/bootloader/crc32.c
@@ -0,0 +1,62 @@
+/* Reference code from RFC1952. Not meant to be used outside test code. */
+
+#include "stm32f4xx_hal.h"
+
+
+/* Table of CRCs of all 8-bit messages. */
+unsigned long crc_table[256];
+
+/* Flag: has the table been computed? Initially false. */
+int crc_table_computed = 0;
+
+/* Make the table for a fast CRC. */
+void make_crc_table(void)
+{
+    unsigned long c;
+
+    int n, k;
+    for (n = 0; n < 256; n++) {
+	c = (unsigned long) n;
+	for (k = 0; k < 8; k++) {
+	    if (c & 1) {
+		c = 0xedb88320L ^ (c >> 1);
+	    } else {
+		c = c >> 1;
+	    }
+	}
+	crc_table[n] = c;
+    }
+    crc_table_computed = 1;
+}
+
+/*
+  Update a running crc with the bytes buf[0..len-1] and return
+  the updated crc. The crc should be initialized to zero. Pre- and
+  post-conditioning (one's complement) is performed within this
+  function so it shouldn't be done by the caller. Usage example:
+
+  unsigned long crc = 0L;
+
+  while (read_buffer(buffer, length) != EOF) {
+  crc = update_crc(crc, buffer, length);
+  }
+  if (crc != original_crc) error();
+*/
+uint32_t update_crc(uint32_t crc, uint8_t *buf, int len)
+{
+    unsigned long c = crc ^ 0xffffffffL;
+    int n;
+
+    if (!crc_table_computed)
+	make_crc_table();
+    for (n = 0; n < len; n++) {
+	c = crc_table[(c ^ buf[n]) & 0xff] ^ (c >> 8);
+    }
+    return c ^ 0xffffffffL;
+}
+
+/* Return the CRC of the bytes buf[0..len-1]. */
+unsigned long crc(unsigned char *buf, int len)
+{
+    return update_crc(0L, buf, len);
+}
diff --git a/projects/bootloader/dfu.c b/projects/bootloader/dfu.c
new file mode 100644
index 0000000..231e388
--- /dev/null
+++ b/projects/bootloader/dfu.c
@@ -0,0 +1,106 @@
+/*
+ * dfu.c
+ * ------------
+ * Receive new firmware from MGMT UART and write it to STM32 internal flash.
+ *
+ * Copyright (c) 2016, NORDUnet A/S All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are
+ * met:
+ * - Redistributions of source code must retain the above copyright notice,
+ *   this list of conditions and the following disclaimer.
+ *
+ * - Redistributions in binary form must reproduce the above copyright
+ *   notice, this list of conditions and the following disclaimer in the
+ *   documentation and/or other materials provided with the distribution.
+ *
+ * - Neither the name of the NORDUnet nor the names of its contributors may
+ *   be used to endorse or promote products derived from this software
+ *   without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+ * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+ * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+ * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+ * TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+#include "dfu.h"
+#include "stm-led.h"
+#include "stm-uart.h"
+#include "stm-flash.h"
+
+#include <string.h>
+
+extern uint32_t update_crc(uint32_t crc, uint8_t *buf, int len);
+
+
+int dfu_receive_firmware(void)
+{
+    uint32_t filesize = 0, crc = 0, my_crc = 0, counter = 0;
+    uint32_t offset = DFU_FIRMWARE_ADDR, n = DFU_UPLOAD_CHUNK_SIZE;
+    uint32_t buf[DFU_UPLOAD_CHUNK_SIZE / 4];
+
+    uart_send_string2(STM_UART_MGMT, (char *) "\r\nOK, bootloader waiting for new firmware\r\n");
+
+    /* Read file size (4 bytes) */
+    uart_receive_bytes(STM_UART_MGMT, (void *) &filesize, 4, 1000);
+    if (filesize < 512 || filesize > DFU_FIRMWARE_END_ADDR - DFU_FIRMWARE_ADDR) {
+	return -1;
+    }
+
+    HAL_FLASH_Unlock();
+
+    while (filesize) {
+	/* By initializing buf to the same value that erased flash has (0xff), we don't
+	 * have to try and be smart when writing the last page of data to the memory.
+	 */
+	memset(buf, 0xffffffff, sizeof(buf));
+
+	if (filesize < n) {
+	    n = filesize;
+	}
+
+	if (uart_receive_bytes(STM_UART_MGMT, (void *) &buf, n, 1000) != HAL_OK) {
+	    return -2;
+	}
+	filesize -= n;
+
+	/* After reception of a chunk but before ACKing we have "all" the time in the world to
+	 * calculate CRC and write it to flash.
+	 */
+	my_crc = update_crc(my_crc, (uint8_t *) buf, n);
+	stm_flash_write32(offset, buf, sizeof(buf) / 4);
+	offset += DFU_UPLOAD_CHUNK_SIZE;
+
+	/* ACK this chunk by sending the current chunk counter (4 bytes) */
+	counter++;
+	uart_send_bytes(STM_UART_MGMT, (void *) &counter, 4);
+	led_toggle(LED_BLUE);
+    }
+
+    HAL_FLASH_Lock();
+
+    /* The sending side will now send it's calculated CRC-32 */
+    uart_receive_bytes(STM_UART_MGMT, (void *) &crc, 4, 1000);
+    if (crc == my_crc) {
+	uart_send_string2(STM_UART_MGMT, (char *) "\r\nSuccess\r\n");
+	return 0;
+    }
+
+    led_on(LED_RED);
+    led_on(LED_YELLOW);
+
+    /* Better to erase the known bad firmware */
+    stm_flash_erase_sectors(DFU_FIRMWARE_ADDR, DFU_FIRMWARE_END_ADDR);
+
+    led_off(LED_YELLOW);
+
+    return 0;
+}
diff --git a/projects/bootloader/dfu.h b/projects/bootloader/dfu.h
new file mode 100644
index 0000000..8dfed9d
--- /dev/null
+++ b/projects/bootloader/dfu.h
@@ -0,0 +1,62 @@
+/*
+ * dfu.h
+ * ---------
+ * Device Firmware Upgrade defines and prototypes.
+ *
+ * Copyright (c) 2016, NORDUnet A/S All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are
+ * met:
+ * - Redistributions of source code must retain the above copyright notice,
+ *   this list of conditions and the following disclaimer.
+ *
+ * - Redistributions in binary form must reproduce the above copyright
+ *   notice, this list of conditions and the following disclaimer in the
+ *   documentation and/or other materials provided with the distribution.
+ *
+ * - Neither the name of the NORDUnet nor the names of its contributors may
+ *   be used to endorse or promote products derived from this software
+ *   without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+ * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+ * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+ * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+ * TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __STM32_BOOTLOADER_DFU_H
+#define __STM32_BOOTLOADER_DFU_H
+
+#include "stm-init.h"
+
+/* symbols defined in the linker script (STM32F429BI_bootloader.ld) */
+extern uint32_t CRYPTECH_FIRMWARE_START;
+extern uint32_t CRYPTECH_FIRMWARE_END;
+extern uint32_t CRYPTECH_DFU_CONTROL;
+
+#define DFU_FIRMWARE_ADDR         ((uint32_t) &CRYPTECH_FIRMWARE_START)
+#define DFU_FIRMWARE_END_ADDR     ((uint32_t) &CRYPTECH_FIRMWARE_END)
+#define DFU_UPLOAD_CHUNK_SIZE     4096
+
+/* Magic bytes to signal the bootloader it should jump to the firmware
+ * instead of trying to receive a new firmware using the MGMT UART.
+ */
+#define HARDWARE_EARLY_DFU_JUMP   0xBADABADA
+
+extern __IO uint32_t *dfu_control;
+extern __IO uint32_t *dfu_firmware;
+extern __IO uint32_t *dfu_msp_ptr;
+extern __IO uint32_t *dfu_code_ptr;
+
+extern int dfu_receive_firmware(void);
+
+
+#endif /* __STM32_BOOTLOADER_DFU_H */
diff --git a/projects/cli-test/Makefile b/projects/cli-test/Makefile
index 7737e13..39619fc 100644
--- a/projects/cli-test/Makefile
+++ b/projects/cli-test/Makefile
@@ -1,6 +1,6 @@
 TEST = cli-test
 
-OBJS = crc32.o mgmt-cli.o test_sdram.o
+OBJS = crc32.o mgmt-cli.o test_sdram.o mgmt-dfu.o
 
 CFLAGS += -I$(LIBCLI_DIR)
 LIBS += $(LIBCLI_DIR)/libcli.a
diff --git a/projects/cli-test/cli-test.c b/projects/cli-test/cli-test.c
index 41d7365..84c268b 100644
--- a/projects/cli-test/cli-test.c
+++ b/projects/cli-test/cli-test.c
@@ -39,6 +39,7 @@
 #include "stm-keystore.h"
 #include "stm-sdram.h"
 #include "mgmt-cli.h"
+#include "mgmt-dfu.h"
 #include "test_sdram.h"
 
 #include <string.h>
@@ -396,7 +397,7 @@ void configure_cli_test(struct cli_def *cli)
     cli_command_node(test, sdram, "Run SDRAM tests");
 }
 
-void configure_cli_misc(struct cli_def *cli)
+static void configure_cli_misc(struct cli_def *cli)
 {
     /* filetransfer */
     cli_command_root_node(filetransfer, "Test file transfering");
@@ -404,11 +405,32 @@ void configure_cli_misc(struct cli_def *cli)
     cli_command_root_node(reboot, "Reboot the STM32");
 }
 
+typedef  void (*pFunction)(void);
+
+/* This is it's own function to make it more convenient to set a breakpoint at it in gdb */
+void do_early_dfu_jump(void)
+{
+    pFunction loaded_app = (pFunction) *dfu_code_ptr;
+    /* Set the stack pointer to the correct one for the firmware */
+    __set_MSP(*dfu_msp_ptr);
+    /* Set the Vector Table Offset Register */
+    SCB->VTOR = (uint32_t) dfu_firmware;
+    loaded_app();
+    while (1);
+}
+
+
 int
 main()
 {
     static struct cli_def cli;
 
+    /* Check if we've just rebooted in order to jump to the firmware. */
+    if (*dfu_control == HARDWARE_EARLY_DFU_JUMP) {
+	*dfu_control = 0;
+	do_early_dfu_jump();
+    }
+
     stm_init();
 
     led_on(LED_RED);
@@ -420,6 +442,7 @@ main()
     configure_cli_fpga(&cli);
     configure_cli_test(&cli);
     configure_cli_misc(&cli);
+    configure_cli_dfu(&cli);
 
     led_off(LED_RED);
     led_on(LED_GREEN);
diff --git a/projects/cli-test/filetransfer b/projects/cli-test/filetransfer
index 2b74570..025a6ac 100755
--- a/projects/cli-test/filetransfer
+++ b/projects/cli-test/filetransfer
@@ -40,6 +40,7 @@ import argparse
 from binascii import crc32
 
 CHUNK_SIZE = 256
+DFU_CHUNK_SIZE = 256
 FPGA_CHUNK_SIZE = 4096
 
 
@@ -57,6 +58,11 @@ def parse_args():
                         action='store_true', default=False,
                         help='Perform FPGA bitstream upload',
                         )
+    parser.add_argument('--dfu',
+                        dest='dfu',
+                        action='store_true', default=False,
+                        help='Perform DFU application upload',
+                        )
 
     parser.add_argument('--device',
                         dest='device',
@@ -117,6 +123,9 @@ def send_file(filename, args, dst):
         src.read(0x64)
         chunk_size = FPGA_CHUNK_SIZE
         response = _execute(dst, 'fpga bitstream upload')
+    elif args.dfu:
+        chunk_size = DFU_CHUNK_SIZE
+        response = _execute(dst, 'dfu upload')
     else:
         chunk_size = CHUNK_SIZE
         response = _execute(dst, 'filetransfer')
diff --git a/projects/cli-test/mgmt-cli.c b/projects/cli-test/mgmt-cli.c
index faaafda..46faae8 100644
--- a/projects/cli-test/mgmt-cli.c
+++ b/projects/cli-test/mgmt-cli.c
@@ -31,7 +31,6 @@
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  */
-#include "stm32f4xx_hal.h"
 #include "stm-init.h"
 #include "stm-uart.h"
 #include "mgmt-cli.h"
diff --git a/projects/cli-test/mgmt-cli.h b/projects/cli-test/mgmt-cli.h
index e6780a3..dd6a58b 100644
--- a/projects/cli-test/mgmt-cli.h
+++ b/projects/cli-test/mgmt-cli.h
@@ -35,7 +35,7 @@
 #ifndef __STM32_MGMT_CLI_H
 #define __STM32_MGMT_CLI_H
 
-#include "stm32f4xx_hal.h"
+#include "stm-init.h"
 #include <libcli.h>
 
 
@@ -64,7 +64,7 @@
 
 /* ROOT NODE is a label without a parent, but with a command associated with it */
 #define cli_command_root_node(name, help)				\
-    _cli_cmd_struct(name, name, NULL, (char *) help);			\
+    _cli_cmd_struct(name, name, cmd_##name, (char *) help);		\
     cli_register_command2(cli, &cmd_##name##_s, NULL)
 
 
diff --git a/projects/cli-test/mgmt-dfu.c b/projects/cli-test/mgmt-dfu.c
new file mode 100644
index 0000000..27fd722
--- /dev/null
+++ b/projects/cli-test/mgmt-dfu.c
@@ -0,0 +1,120 @@
+/*
+ * mgmt-dfu.c
+ * ---------
+ * CLI code for looking at, jumping to or erasing the loaded firmware.
+ *
+ * Copyright (c) 2016, NORDUnet A/S All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are
+ * met:
+ * - Redistributions of source code must retain the above copyright notice,
+ *   this list of conditions and the following disclaimer.
+ *
+ * - Redistributions in binary form must reproduce the above copyright
+ *   notice, this list of conditions and the following disclaimer in the
+ *   documentation and/or other materials provided with the distribution.
+ *
+ * - Neither the name of the NORDUnet nor the names of its contributors may
+ *   be used to endorse or promote products derived from this software
+ *   without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+ * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+ * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+ * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+ * TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "stm-init.h"
+#include "mgmt-cli.h"
+#include "stm-uart.h"
+#include "stm-flash.h"
+#include "mgmt-dfu.h"
+
+#include <string.h>
+
+extern uint32_t update_crc(uint32_t crc, uint8_t *buf, int len);
+
+/* Linker symbols are strange in C. Make regular pointers for sanity. */
+__IO uint32_t *dfu_control = &CRYPTECH_DFU_CONTROL;
+__IO uint32_t *dfu_firmware = &CRYPTECH_FIRMWARE_START;
+__IO uint32_t *dfu_firmware_end = &CRYPTECH_FIRMWARE_END;
+/* The first word in the firmware is an address to the stack (msp) */
+__IO uint32_t *dfu_msp_ptr = &CRYPTECH_FIRMWARE_START;
+/* The second word in the firmware is a pointer to the code
+ * (points at the Reset_Handler from the linker script).
+ */
+__IO uint32_t *dfu_code_ptr = &CRYPTECH_FIRMWARE_START + 1;
+
+
+
+int cmd_dfu_dump(struct cli_def *cli, const char *command, char *argv[], int argc)
+{
+    cli_print(cli, "First 256 bytes from DFU application address %p:\r\n", dfu_firmware);
+
+    uart_send_hexdump(STM_UART_MGMT, (uint8_t *) dfu_firmware, 0, 0xff);
+    uart_send_string2(STM_UART_MGMT, (char *) "\r\n\r\n");
+
+    return CLI_OK;
+}
+
+int cmd_dfu_erase(struct cli_def *cli, const char *command, char *argv[], int argc)
+{
+    int status;
+
+    cli_print(cli, "Erasing flash sectors %i to %i (address %p to %p) - expect the CLI to crash now",
+	      stm_flash_sector_num((uint32_t) dfu_firmware),
+	      stm_flash_sector_num((uint32_t) dfu_firmware_end),
+	      dfu_firmware,
+	      dfu_firmware_end);
+
+    if ((status = stm_flash_erase_sectors((uint32_t) dfu_firmware, (uint32_t) dfu_firmware_end)) != 0) {
+	cli_print(cli, "Failed erasing flash sectors (%i)", status);
+    }
+
+    return CLI_OK;
+}
+
+int cmd_dfu_jump(struct cli_def *cli, const char *command, char *argv[], int argc)
+{
+    uint32_t i;
+    /* Load first byte from the DFU_FIRMWARE_PTR to verify it contains an IVT before
+     * jumping there.
+     */
+    cli_print(cli, "Checking for application at %p", dfu_firmware);
+
+    i = *dfu_msp_ptr & 0xFF000000;
+    /* 'new_msp' is supposed to be a pointer to the new applications stack, it should
+     * point either at RAM (0x20000000) or at the CCM memory (0x10000000).
+     */
+    if (i == 0x20000000 || i == 0x10000000) {
+	/* Set dfu_control to the magic value that will cause the us to jump to the
+	 * firmware from the CLI main() function after rebooting.
+	 */
+	*dfu_control = HARDWARE_EARLY_DFU_JUMP;
+	cli_print(cli, "Making the leap");
+	HAL_NVIC_SystemReset();
+	while (1) { ; }
+    } else {
+	cli_print(cli, "No loaded application found at %p (read 0x%x)",
+		  dfu_firmware, (unsigned int) *dfu_msp_ptr);
+    }
+
+    return CLI_OK;
+}
+
+void configure_cli_dfu(struct cli_def *cli)
+{
+    cli_command_root(dfu);
+
+    cli_command_node(dfu, dump, "Show the first 256 bytes of the loaded firmware");
+    cli_command_node(dfu, jump, "Jump to the loaded firmware");
+    cli_command_node(dfu, erase, "Erase the firmware memory (will crash the CLI)");
+}
diff --git a/projects/cli-test/mgmt-dfu.h b/projects/cli-test/mgmt-dfu.h
new file mode 100644
index 0000000..ac6589c
--- /dev/null
+++ b/projects/cli-test/mgmt-dfu.h
@@ -0,0 +1,59 @@
+/*
+ * mgmt-dfu.h
+ * ---------
+ * Management CLI Device Firmware Upgrade code.
+ *
+ * Copyright (c) 2016, NORDUnet A/S All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are
+ * met:
+ * - Redistributions of source code must retain the above copyright notice,
+ *   this list of conditions and the following disclaimer.
+ *
+ * - Redistributions in binary form must reproduce the above copyright
+ *   notice, this list of conditions and the following disclaimer in the
+ *   documentation and/or other materials provided with the distribution.
+ *
+ * - Neither the name of the NORDUnet nor the names of its contributors may
+ *   be used to endorse or promote products derived from this software
+ *   without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+ * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+ * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+ * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+ * TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __STM32_CLI_MGMT_DFU_H
+#define __STM32_CLI_MGMT_DFU_H
+
+#include "stm-init.h"
+#include <libcli.h>
+
+/* symbols defined in the linker script (STM32F429BI.ld) */
+extern uint32_t CRYPTECH_FIRMWARE_START;
+extern uint32_t CRYPTECH_FIRMWARE_END;
+extern uint32_t CRYPTECH_DFU_CONTROL;
+
+#define DFU_FIRMWARE_ADDR         ((uint32_t) &CRYPTECH_FIRMWARE_START)
+#define DFU_FIRMWARE_END_ADDR     ((uint32_t) &CRYPTECH_FIRMWARE_END)
+#define DFU_UPLOAD_CHUNK_SIZE     256
+#define HARDWARE_EARLY_DFU_JUMP   0xBADABADA
+
+extern __IO uint32_t *dfu_control;
+extern __IO uint32_t *dfu_firmware;
+extern __IO uint32_t *dfu_msp_ptr;
+extern __IO uint32_t *dfu_code_ptr;
+
+
+extern void configure_cli_dfu(struct cli_def *cli);
+
+#endif /* __STM32_CLI_MGMT_DFU_H */
diff --git a/projects/cli-test/test_sdram.h b/projects/cli-test/test_sdram.h
index b848d18..3076aa1 100644
--- a/projects/cli-test/test_sdram.h
+++ b/projects/cli-test/test_sdram.h
@@ -31,6 +31,9 @@
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  */
+#ifndef __STM32_CLI_TEST_SDRAM_H
+#define __STM32_CLI_TEST_SDRAM_H
+
 extern uint32_t lfsr1;
 extern uint32_t lfsr2;
 
@@ -40,3 +43,5 @@ extern int test_sdrams_interleaved(uint32_t *base_addr1, uint32_t *base_addr2);
 
 extern uint32_t lfsr_next_32(uint32_t lfsr);
 extern uint32_t lfsr_next_24(uint32_t lfsr);
+
+#endif /* __STM32_CLI_TEST_SDRAM_H */
diff --git a/stm-flash.c b/stm-flash.c
new file mode 100644
index 0000000..991379b
--- /dev/null
+++ b/stm-flash.c
@@ -0,0 +1,138 @@
+/*
+ * stm-flash.c
+ * -----------
+ * Functions for writing/erasing the STM32 internal flash memory.
+ * The flash is memory mapped, so no code is needed here to read it.
+ *
+ * Copyright (c) 2016, NORDUnet A/S All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are
+ * met:
+ * - Redistributions of source code must retain the above copyright notice,
+ *   this list of conditions and the following disclaimer.
+ *
+ * - Redistributions in binary form must reproduce the above copyright
+ *   notice, this list of conditions and the following disclaimer in the
+ *   documentation and/or other materials provided with the distribution.
+ *
+ * - Neither the name of the NORDUnet nor the names of its contributors may
+ *   be used to endorse or promote products derived from this software
+ *   without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+ * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+ * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+ * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+ * TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "stm32f4xx_hal.h"
+#include "stm-flash.h"
+#include "stm-init.h"
+
+
+/* Flash sector offsets from RM0090, Table 6. Flash module - 2 Mbyte dual bank organization */
+#define FLASH_NUM_SECTORS 24
+uint32_t flash_sector_offsets[FLASH_NUM_SECTORS + 1] = {
+    /* Bank 1 */
+    0x08000000, /* #0,  16 KBytes */
+    0x08004000, /* #1,  16 Kbytes */
+    0x08008000, /* #2,  16 Kbytes */
+    0x0800C000, /* #3,  16 Kbytes */
+    0x08010000, /* #4,  64 Kbytes */
+    0x08020000, /* #5,  128 Kbytes */
+    0x08040000, /* #6,  128 Kbytes */
+    0x08060000, /* #7,  128 Kbytes */
+    0x08080000, /* #8,  128 Kbytes */
+    0x080A0000, /* #9,  128 Kbytes */
+    0x080C0000, /* #10, 128 Kbytes */
+    0x080E0000, /* #11, 128 Kbytes */
+    /* Bank 2 */
+    0x08100000, /* #12,  16 Kbytes */
+    0x08104000, /* #13,  16 Kbytes */
+    0x08108000, /* #14,  16 Kbytes */
+    0x0810C000, /* #15,  16 Kbytes */
+    0x08110000, /* #16,  64 Kbytes */
+    0x08120000, /* #17, 128 Kbytes */
+    0x08140000, /* #18, 128 Kbytes */
+    0x08160000, /* #19, 128 Kbytes */
+    0x08180000, /* #20, 128 Kbytes */
+    0x081A0000, /* #21, 128 Kbytes */
+    0x081C0000, /* #22, 128 Kbytes */
+    0x081E0000, /* #23, 128 Kbytes */
+    0x08200000  /* first address *after* flash */
+};
+
+int stm_flash_sector_num(const uint32_t offset)
+{
+    int i = FLASH_NUM_SECTORS;
+
+    while (i-- >= 0) {
+	if (offset >= flash_sector_offsets[i] &&
+	    offset < flash_sector_offsets[i + 1]) {
+	    return i;
+	}
+    }
+    return -1;
+}
+
+int stm_flash_erase_sectors(const uint32_t start_offset, const uint32_t end_offset)
+{
+    uint32_t start_sector = stm_flash_sector_num(start_offset);
+    uint32_t end_sector = stm_flash_sector_num(end_offset);
+    FLASH_EraseInitTypeDef FLASH_EraseInitStruct;
+    uint32_t SectorError = 0;
+
+    if (start_sector > end_sector) return -1;
+    if (start_sector < 0 || end_sector > FLASH_NUM_SECTORS) return -2;
+
+    FLASH_EraseInitStruct.Sector = start_sector;
+    FLASH_EraseInitStruct.NbSectors = (end_sector - start_sector) + 1;
+    FLASH_EraseInitStruct.TypeErase = TYPEERASE_SECTORS;
+    FLASH_EraseInitStruct.VoltageRange = VOLTAGE_RANGE_3;
+
+    HAL_FLASH_Unlock();
+
+    if (HAL_FLASHEx_Erase(&FLASH_EraseInitStruct, &SectorError) != HAL_OK) {
+	return -3;
+    }
+
+    HAL_FLASH_Lock();
+
+    if (SectorError == 0xFFFFFFFF) return 0;
+
+    return -3;
+}
+
+int stm_flash_write32(uint32_t offset, const uint32_t *buf, const uint32_t elements)
+{
+    uint32_t sector = stm_flash_sector_num(offset);
+    uint32_t i, j;
+
+    if (offset == flash_sector_offsets[sector]) {
+	/* Request to write to beginning of a flash sector, erase it first. */
+	if (stm_flash_erase_sectors(offset, offset) != 0) {
+	    return -1;
+	}
+    }
+
+    HAL_FLASH_Unlock();
+
+    for (i = 0; i < elements; i++) {
+	if ((j = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, offset, buf[i])) != HAL_OK) {
+	    return -2;
+	}
+	offset += 4;
+    }
+
+    HAL_FLASH_Lock();
+
+    return 1;
+}
diff --git a/stm-flash.h b/stm-flash.h
new file mode 100644
index 0000000..a9cf7db
--- /dev/null
+++ b/stm-flash.h
@@ -0,0 +1,42 @@
+/*
+ * stm-flash.h
+ * -----------
+ * Functions and defines for accessing the flash memory.
+ *
+ * Copyright (c) 2016, NORDUnet A/S All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are
+ * met:
+ * - Redistributions of source code must retain the above copyright notice,
+ *   this list of conditions and the following disclaimer.
+ *
+ * - Redistributions in binary form must reproduce the above copyright
+ *   notice, this list of conditions and the following disclaimer in the
+ *   documentation and/or other materials provided with the distribution.
+ *
+ * - Neither the name of the NORDUnet nor the names of its contributors may
+ *   be used to endorse or promote products derived from this software
+ *   without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+ * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+ * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+ * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+ * TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __STM32_FLASH_H
+#define __STM32_FLASH_H
+
+extern int stm_flash_sector_num(const uint32_t offset);
+extern int stm_flash_erase_sectors(const uint32_t start_offset, const uint32_t end_offset);
+extern int stm_flash_write32(const uint32_t offset, const uint32_t *buf, const uint32_t elements);
+
+#endif /* __STM32_FLASH_H */