From 0af1a534e2930da77ebbab6481adcd17069581ef Mon Sep 17 00:00:00 2001
From: jaseg <git-bigdata-wsl-arch@jaseg.de>
Date: Wed, 11 Mar 2020 13:57:22 +0100
Subject: Start with integration of everything

---
 controller/fw/Makefile                           |  94 +--
 controller/fw/musl_include_shims/bits/alltypes.h |  23 +
 controller/fw/musl_include_shims/endian.h        |  80 +++
 controller/fw/musl_include_shims/features.h      |  40 ++
 controller/fw/musl_include_shims/fp_arch.h       |   6 +
 controller/fw/musl_include_shims/libm.h          | 270 ++++++++
 controller/fw/src/adc.c                          |  93 +++
 controller/fw/src/adc.h                          |  10 +
 controller/fw/src/dsss_demod.c                   |  16 +-
 controller/fw/src/dsss_demod.h                   |   3 +
 controller/fw/src/freq_meas.c                    |   4 +-
 controller/fw/src/freq_meas.h                    |   2 +-
 controller/fw/src/main.c                         | 143 ++++-
 controller/fw/src/mspdebug_wrapper.c             |  18 +-
 controller/fw/src/protocol.c                     |   7 +
 controller/fw/src/spi_flash.c                    |  88 +--
 controller/fw/src/spi_flash.h                    |   6 +-
 controller/fw/src/sr_global.h                    |  10 +
 controller/fw/src/startup_stm32f407xx.s          | 521 ++++++++++++++++
 controller/fw/src/stm32f4_isr.h                  |  98 +++
 controller/fw/src/system_stm32f4xx.c             | 743 +++++++++++++++++++++++
 controller/fw/stm32f407.ld                       | 172 +++++-
 22 files changed, 2300 insertions(+), 147 deletions(-)
 create mode 100644 controller/fw/musl_include_shims/bits/alltypes.h
 create mode 100644 controller/fw/musl_include_shims/endian.h
 create mode 100644 controller/fw/musl_include_shims/features.h
 create mode 100644 controller/fw/musl_include_shims/fp_arch.h
 create mode 100644 controller/fw/musl_include_shims/libm.h
 create mode 100644 controller/fw/src/adc.c
 create mode 100644 controller/fw/src/adc.h
 create mode 100644 controller/fw/src/protocol.c
 create mode 100644 controller/fw/src/startup_stm32f407xx.s
 create mode 100644 controller/fw/src/stm32f4_isr.h
 create mode 100644 controller/fw/src/system_stm32f4xx.c

diff --git a/controller/fw/Makefile b/controller/fw/Makefile
index 85ab02f..df39fe4 100644
--- a/controller/fw/Makefile
+++ b/controller/fw/Makefile
@@ -1,12 +1,13 @@
 
-OPENCM3_DIR 	?= libopencm3
-CMSIS_DIR 	?= cmsis
+CUBE_DIR		?= STM32CubeF4
+CMSIS_DIR 		?= cmsis
 MSPDEBUG_DIR 	?= mspdebug
 LIBSODIUM_DIR	?= libsodium
-TINYAES_DIR	?= tinyaes
-MUSL_DIR	?= musl
+TINYAES_DIR		?= tinyaes
+MUSL_DIR		?= musl
 
-C_SOURCES		:= src/main.c src/mspdebug_wrapper.c src/spi_flash.c src/freq_meas.c src/dsss_demod.c
+C_SOURCES		:= src/main.c src/mspdebug_wrapper.c src/spi_flash.c src/freq_meas.c src/dsss_demod.c src/adc.c \
+					src/protocol.c
 C_SOURCES		+= $(MSPDEBUG_DIR)/drivers/jtaglib.c
 
 CMSIS_SOURCES		+= $(CMSIS_DIR)/CMSIS/DSP/Source/TransformFunctions/arm_rfft_fast_init_f32.c
@@ -23,6 +24,10 @@ MUSL_SOURCES		+= $(MUSL_DIR)/src/math/tanhf.c $(MUSL_DIR)/src/math/atanhf.c
 MUSL_SOURCES		+= $(MUSL_DIR)/src/math/expm1f.c $(MUSL_DIR)/src/math/log1pf.c
 MUSL_SOURCES		+= $(MUSL_DIR)/src/math/expf.c $(MUSL_DIR)/src/math/exp2f_data.c
 MUSL_SOURCES		+= $(MUSL_DIR)/src/math/powf.c
+MUSL_SOURCES		+= $(MUSL_DIR)/src/math/fabsf.c
+MUSL_SOURCES		+= $(MUSL_DIR)/src/stdlib/abs.c
+MUSL_SOURCES		+= $(MUSL_DIR)/src/string/memset.c
+MUSL_SOURCES		+= $(MUSL_DIR)/src/string/memcpy.c
 MUSL_SOURCES		+= $(MUSL_DIR)/src/math/__math_oflowf.c
 MUSL_SOURCES		+= $(MUSL_DIR)/src/math/__math_uflowf.c
 MUSL_SOURCES		+= $(MUSL_DIR)/src/math/__math_xflowf.c
@@ -36,7 +41,6 @@ CXX_SOURCES		+= src/ldpc_wrapper.cpp
 BUILDDIR 	?= build
 BINARY 		:= safetyreset.elf
 LDSCRIPT 	:= stm32f407.ld
-OPENCM3_LIB	:= opencm3_stm32f4
 
 PREFIX		?= arm-none-eabi-
 
@@ -47,7 +51,7 @@ FMEAS_FFT_LEN			?= 256
 FMEAS_FFT_WINDOW 		?= gaussian
 FMEAS_FFT_WINDOW_SIGMA 	?= 16.0
 # TODO: validate
-FMEAS_SAMPLING_RATE		?= 10.0
+FMEAS_SAMPLING_RATE		?= $(shell echo $(FMEAS_ADC_SAMPLING_RATE) / \($(FMEAS_FFT_LEN)/2\) | bc -l)
 
 DSSS_GOLD_CODE_NBITS 	?= 5
 DSSS_DECIMATION			?= 10
@@ -70,6 +74,8 @@ CXX			:= $(PREFIX)g++
 LD			:= $(PREFIX)gcc
 AR			:= $(PREFIX)ar
 AS			:= $(PREFIX)as
+SIZE		:= $(PREFIX)size
+NM			:= $(PREFIX)nm
 OBJCOPY		:= $(PREFIX)objcopy
 OBJDUMP		:= $(PREFIX)objdump
 GDB			:= $(PREFIX)gdb
@@ -84,25 +90,31 @@ HOST_OBJDUMP		?= $(HOST_PREFIX)objdump
 
 PYTHON3 ?= python3
 
-OPENCM3_DIR_ABS 	:= $(abspath $(OPENCM3_DIR))
 CMSIS_DIR_ABS 		:= $(abspath $(CMSIS_DIR))
 MSPDEBUG_DIR_ABS 	:= $(abspath $(MSPDEBUG_DIR))
 LIBSODIUM_DIR_ABS	:= $(abspath $(LIBSODIUM_DIR))
 TINYAES_DIR_ABS		:= $(abspath $(TINYAES_DIR))
 MUSL_DIR_ABS		:= $(abspath $(MUSL_DIR))
 
-COMMON_CFLAGS		+= -I$(OPENCM3_DIR_ABS)/include -Imspdebug/util -Imspdebug/drivers -Ilevmarq
+ARCH_FLAGS		?= -mthumb -mcpu=cortex-m4 -mfloat-abi=hard -mfpu=fpv4-sp-d16
+SYSTEM_FLAGS	?= -nostdlib -ffreestanding -nostartfiles
+
+COMMON_CFLAGS		+= -Imspdebug/util -Imspdebug/drivers -Ilevmarq
 COMMON_CFLAGS		+= -I$(CMSIS_DIR_ABS)/CMSIS/DSP/Include -I$(CMSIS_DIR_ABS)/CMSIS/Core/Include 
 CFLAGS				+= -I$(abspath musl_include_shims)
 COMMON_CFLAGS		+= -I$(BUILDDIR) -Isrc -Itinyaes
+CFLAGS				+= -I$(CUBE_DIR)/Drivers/CMSIS/Device/ST/STM32F4xx/Include
 
-COMMON_CFLAGS	+= -Os -std=gnu11 -g -DSTM32F4
-CFLAGS			+= -mthumb -mcpu=cortex-m4 -mfloat-abi=hard -mfpu=fpv4-sp-d16
+COMMON_CFLAGS	+= -Os -std=gnu11 -g -DSTM32F407xx -DSTM32F4
+CFLAGS			+= $(ARCH_FLAGS) $(SYSTEM_FLAGS)
 #SIM_CFLAGS		+= -mthumb -mcpu=cortex-m4 -mfloat-abi=soft
-# Note: libopencm3 requires some standard libc definitions from stdint.h and stdbool.h, so we don't pass -nostdinc here.
-CFLAGS			+= -nostdlib -ffreestanding
 CFLAGS			+= -fno-common -ffunction-sections -fdata-sections
-COMMON_CFLAGS	+= -DDSSS_GOLD_CODE_NBITS=$(DSSS_GOLD_CODE_NBITS) -DFMEAS_FFT_LEN=$(FMEAS_FFT_LEN) -DFMEAS_ADC_MAX=$(FMEAS_ADC_MAX)
+COMMON_CFLAGS 	+= -DARM_DSP_CONFIG_TABLES -DARM_FFT_ALLOW_TABLES \
+				   -DARM_TABLE_TWIDDLECOEF_F32_128 -DARM_TABLE_BITREVIDX_FLT_128 \
+				   -DARM_TABLE_TWIDDLECOEF_F32_128 -DARM_TABLE_TWIDDLECOEF_RFFT_F32_256
+COMMON_CFLAGS	+= -DDSSS_GOLD_CODE_NBITS=$(DSSS_GOLD_CODE_NBITS)
+COMMON_CFLAGS	+= -DFMEAS_FFT_LEN=$(FMEAS_FFT_LEN)
+COMMON_CFLAGS	+= -DFMEAS_ADC_MAX=$(FMEAS_ADC_MAX)
 COMMON_CFLAGS	+= -DFMEAS_ADC_SAMPLING_RATE=$(FMEAS_ADC_SAMPLING_RATE)
 COMMON_CFLAGS	+= -DFMEAS_FFT_WINDOW_SIGMA=$(FMEAS_FFT_WINDOW_SIGMA)
 COMMON_CFLAGS	+= -DDSSS_DECIMATION=$(DSSS_DECIMATION)
@@ -117,35 +129,43 @@ COMMON_CFLAGS	+= -DPRESIG_STORE_SIZE=$(PRESIG_STORE_SIZE)
 CFLAGS			+= -Dhidden=
 
 SIM_CFLAGS	+= -lm -DSIMULATION -fsanitize=address
-SIM_CFLAGS	+= -Wall -Wextra -Wpedantic -Wshadow -Wimplicit-function-declaration -Wundef
+SIM_CFLAGS	+= -Wall -Wextra -Wpedantic -Wshadow -Wimplicit-function-declaration -Wundef -Wno-unused-parameter
 
-INT_CFLAGS	+= -Wall -Wextra -Wpedantic -Wshadow -Wimplicit-function-declaration -Wundef
+INT_CFLAGS	+= -Wall -Wextra -Wpedantic -Wshadow -Wimplicit-function-declaration -Wundef -Wno-unused-parameter
 INT_CFLAGS	+= -Wredundant-decls -Wmissing-prototypes -Wstrict-prototypes
 
 CXXFLAGS	+= -Os -g
-CXXFLAGS	+= -nostdlib -ffreestanding
-CXXFLAGS	+= -mthumb -mcpu=cortex-m4 -mfloat-abi=hard -mfpu=fpv4-sp-d16
+CXXFLAGS	+= $(ARCH_FLAGS) $(SYSTEM_FLAGS)
 CXXFLAGS	+= -fno-common -ffunction-sections -fdata-sections
 CXXFLAGS	+= -Wall -Wextra -Wshadow -Wundef -Wredundant-decls
 CXXFLAGS	+= -I.
 
-COMMON_LDFLAGS	+= -mthumb -mcpu=cortex-m4 -mfloat-abi=hard -mfpu=fpv4-sp-d16 -g
-LDFLAGS			+= --static -nostartfiles
+LDFLAGS			+= $(ARCH_FLAGS) $(SYSTEM_FLAGS)
 LDFLAGS			+= -Wl,--cre
-#LDFLAGS		+= -Wl,--gc-sections
-LDFLAGS			+= -Wl,--start-group -lc -lgcc -lnosys -Wl,--end-group
-LDFLAGS			+= -L$(OPENCM3_DIR_ABS)/lib -l$(OPENCM3_LIB) $(shell $(CC) -print-libgcc-file-name)
+LDFLAGS			+= --trace
+
+LIBS		+= -lgcc
+LDFLAGS		+= -Wl,--gc-sections -Wl,--trace-symbol=twiddleCoef_4096 -Wl,--trace-symbol=arm_cfft_sR_f32_len4096
 
-all: $(BUILDDIR)/$(BINARY)
+all: binsize
+
+.PHONY: binsize
+binsize: $(BUILDDIR)/$(BINARY)
+	@echo "▐▬▬▬▌ SyMbOL sIzE HiGhScORe LiSt ▐▬▬▬▌"
+	$(NM) --print-size --size-sort --radix=d $< | tail -n 20
+	@echo
+	@echo "===== Binary size ====="
+	$(SIZE) $<
 
 src/dsss_demod.c: $(BUILDDIR)/generated/dsss_gold_code.h $(BUILDDIR)/generated/dsss_butter_filter.h
 
 $(BUILDDIR)/generated/dsss_gold_code.h: $(BUILDDIR)/generated/gold_code_$(DSSS_GOLD_CODE_NBITS).h
 	ln -srf $< $@
 
-.PRECIOUS: $(BUILDDIR)/generated/gold_code_%.c
-$(BUILDDIR)/generated/gold_code_%.c $(BUILDDIR)/generated/gold_code_%.h: | $(BUILDDIR)/generated
-	$(PYTHON3) tools/gold_code_header_gen.py -v dsss_gold_code_table -$(patsubst .%,%,$(suffix $@)) $* > $@
+.PRECIOUS: $(BUILDDIR)/generated/gold_code_%.c $(BUILDDIR)/generated/gold_code_%.h
+$(BUILDDIR)/generated/gold_code_%.c $(BUILDDIR)/generated/gold_code_%.h &: | $(BUILDDIR)/generated
+	$(PYTHON3) tools/gold_code_header_gen.py -v dsss_gold_code_table -c $* > $(BUILDDIR)/generated/gold_code_$*.c
+	$(PYTHON3) tools/gold_code_header_gen.py -v dsss_gold_code_table -h $* > $(BUILDDIR)/generated/gold_code_$*.h
 
 .PRECIOUS: $(BUILDDIR)/generated/fmeas_fft_window.c
 $(BUILDDIR)/generated/fmeas_fft_window.c: | $(BUILDDIR)/generated
@@ -172,7 +192,8 @@ $(BUILDDIR)/generated: ; mkdir -p $@
 OBJS := $(addprefix $(BUILDDIR)/,$(C_SOURCES:.c=.o) $(CXX_SOURCES:.cpp=.o))
 
 ALL_OBJS := $(OBJS)
-ALL_OBJS += $(OPENCM3_DIR)/lib/lib$(OPENCM3_LIB).a
+ALL_OBJS += $(BUILDDIR)/src/startup_stm32f407xx.o
+ALL_OBJS += $(BUILDDIR)/src/system_stm32f4xx.o
 ALL_OBJS += $(BUILDDIR)/libsodium/src/libsodium/.libs/libsodium.a
 ALL_OBJS += $(BUILDDIR)/tinyaes/aes.o
 ALL_OBJS += $(BUILDDIR)/levmarq/levmarq.o
@@ -180,8 +201,9 @@ ALL_OBJS += $(BUILDDIR)/generated/gold_code_$(DSSS_GOLD_CODE_NBITS).o
 ALL_OBJS += $(BUILDDIR)/generated/fmeas_fft_window.o
 ALL_OBJS += $(BUILDDIR)/generated/dsss_cwt_wavelet.o 
 
+.PRECIOUS: $(BUILDDIR)/$(BINARY) 
 $(BUILDDIR)/$(BINARY): $(ALL_OBJS)
-	$(LD) -T$(LDSCRIPT) $(COMMON_LDFLAGS) $(LDFLAGS) -o $@ -Wl,-Map=$(BUILDDIR)/src/$*.map $^
+	$(LD) -T$(LDSCRIPT) $(LDFLAGS) -o $@ -Wl,-Map=$(BUILDDIR)/src/$*.map $^ $(LIBS)
 
 tools: $(BUILDDIR)/tools/freq_meas_test
 $(BUILDDIR)/tools/freq_meas_test: tools/freq_meas_test.c src/freq_meas.c levmarq/levmarq.c $(BUILDDIR)/generated/fmeas_fft_window.c $(CMSIS_SOURCES)
@@ -198,6 +220,10 @@ $(BUILDDIR)/tools/crypto_test: tools/crypto_test.c src/crypto.c tinyaes/aes.c $(
 	mkdir -p $(@D)
 	$(HOST_CC) $(COMMON_CFLAGS) $(SIM_CFLAGS) -lsodium -o $@ $^
 
+$(BUILDDIR)/src/%.o: src/%.s
+	mkdir -p $(@D)
+	$(CC) $(COMMON_CFLAGS) $(CFLAGS) $(INT_CFLAGS) -o $@ -c $<
+
 $(BUILDDIR)/src/%.o: src/%.c
 	mkdir -p $(@D)
 	$(CC) $(COMMON_CFLAGS) $(CFLAGS) $(INT_CFLAGS) -o $@ -c $<
@@ -214,9 +240,6 @@ $(BUILDDIR)/%.o: %.c
 	mkdir -p $(@D)
 	$(CC) $(COMMON_CFLAGS) $(CFLAGS) $(EXT_CFLAGS) -o $@ -c $<
 
-$(OPENCM3_DIR)/lib/lib$(OPENCM3_LIB).a:
-	$(MAKE) -C $(OPENCM3_DIR) -j $(shell nproc)
-
 $(BUILDDIR)/libsodium/src/libsodium/.libs/libsodium.a:
 	mkdir -p $(BUILDDIR)/libsodium
 	cd $(BUILDDIR)/libsodium && CFLAGS="$(COMMON_CFLAGS) $(CFLAGS) -DDEV_MODE=1" $(LIBSODIUM_DIR_ABS)/configure --host=arm-none-eabi && $(MAKE) -j $(shell nproc)
@@ -225,9 +248,9 @@ $(BUILDDIR)/tinyaes/aes.o:
 	mkdir -p $(@D)
 	make -C $(@D) -f $(TINYAES_DIR_ABS)/Makefile VPATH=$(TINYAES_DIR_ABS) CFLAGS="$(COMMON_CFLAGS) $(CFLAGS) -c" CC=$(CC) LD=$(LD) AR=$(AR) aes.o
 
-$(BUILDDIR)/musl/lib/libc.a:
-	mkdir -p $(BUILDDIR)/musl
-	cd $(BUILDDIR)/musl && CFLAGS="$(SIM_CFLAGS) $(COMMON_CFLAGS)" CC=$(CC) LD=$(LD) AR=$(AR) $(MUSL_DIR_ABS)/configure && $(MAKE) TARGET=arm-linux-musleabihf GCC_CONFIG="-mcpu=cortex-m4 -mfloat-abi=soft" -j $(shell nproc)
+# $(BUILDDIR)/musl/lib/libc.a:
+# 	mkdir -p $(BUILDDIR)/musl
+# 	cd $(BUILDDIR)/musl && CFLAGS="$(SIM_CFLAGS) $(COMMON_CFLAGS)" CC=$(CC) LD=$(LD) AR=$(AR) $(MUSL_DIR_ABS)/configure && $(MAKE) TARGET=arm-linux-musleabihf GCC_CONFIG="-mcpu=cortex-m4 -mfloat-abi=soft" -j $(shell nproc)
 
 build/ldpc_decoder_test.so: src/ldpc_decoder.c
 	gcc -fPIC -shared -Wall -Wextra -Wpedantic -std=gnu11 -O0 -g -o $@ $^
@@ -240,7 +263,6 @@ clean:
 
 mrproper: clean
 	rm -rf build
-	-$(MAKE) -C $(OPENCM3_DIR) clean
 
 .PHONY: clean mrproper
 
diff --git a/controller/fw/musl_include_shims/bits/alltypes.h b/controller/fw/musl_include_shims/bits/alltypes.h
new file mode 100644
index 0000000..581ca85
--- /dev/null
+++ b/controller/fw/musl_include_shims/bits/alltypes.h
@@ -0,0 +1,23 @@
+
+/* shim file for musl */
+
+#ifndef __MUSL_SHIM_BITS_ALLTYPES_H__
+#define __MUSL_SHIM_BITS_ALLTYPES_H__
+
+#define _REDIR_TIME64 1
+#define _Addr int
+#define _Int64 long long
+#define _Reg int
+
+#define __BYTE_ORDER 1234
+
+#define __LONG_MAX 0x7fffffffL
+
+#ifndef __cplusplus
+typedef unsigned wchar_t;
+#endif
+
+typedef float float_t;
+typedef double double_t;
+
+#endif /* __MUSL_SHIM_BITS_ALLTYPES_H__ */
diff --git a/controller/fw/musl_include_shims/endian.h b/controller/fw/musl_include_shims/endian.h
new file mode 100644
index 0000000..172c432
--- /dev/null
+++ b/controller/fw/musl_include_shims/endian.h
@@ -0,0 +1,80 @@
+#ifndef _ENDIAN_H
+#define _ENDIAN_H
+
+#include <features.h>
+
+#define __NEED_uint16_t
+#define __NEED_uint32_t
+#define __NEED_uint64_t
+
+#include <bits/alltypes.h>
+
+#define __PDP_ENDIAN 3412
+
+#define BIG_ENDIAN __BIG_ENDIAN
+#define LITTLE_ENDIAN __LITTLE_ENDIAN
+#define PDP_ENDIAN __PDP_ENDIAN
+#define BYTE_ORDER __BYTE_ORDER
+
+static __inline uint16_t __bswap16(uint16_t __x)
+{
+	return __x<<8 | __x>>8;
+}
+
+static __inline uint32_t __bswap32(uint32_t __x)
+{
+	return __x>>24 | __x>>8&0xff00 | __x<<8&0xff0000 | __x<<24;
+}
+
+static __inline uint64_t __bswap64(uint64_t __x)
+{
+	return __bswap32(__x)+0ULL<<32 | __bswap32(__x>>32);
+}
+
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+#define htobe16(x) __bswap16(x)
+#define be16toh(x) __bswap16(x)
+#define htobe32(x) __bswap32(x)
+#define be32toh(x) __bswap32(x)
+#define htobe64(x) __bswap64(x)
+#define be64toh(x) __bswap64(x)
+#define htole16(x) (uint16_t)(x)
+#define le16toh(x) (uint16_t)(x)
+#define htole32(x) (uint32_t)(x)
+#define le32toh(x) (uint32_t)(x)
+#define htole64(x) (uint64_t)(x)
+#define le64toh(x) (uint64_t)(x)
+#else
+#define htobe16(x) (uint16_t)(x)
+#define be16toh(x) (uint16_t)(x)
+#define htobe32(x) (uint32_t)(x)
+#define be32toh(x) (uint32_t)(x)
+#define htobe64(x) (uint64_t)(x)
+#define be64toh(x) (uint64_t)(x)
+#define htole16(x) __bswap16(x)
+#define le16toh(x) __bswap16(x)
+#define htole32(x) __bswap32(x)
+#define le32toh(x) __bswap32(x)
+#define htole64(x) __bswap64(x)
+#define le64toh(x) __bswap64(x)
+#endif
+
+#if defined(_GNU_SOURCE) || defined(_BSD_SOURCE)
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+#define betoh16(x) __bswap16(x)
+#define betoh32(x) __bswap32(x)
+#define betoh64(x) __bswap64(x)
+#define letoh16(x) (uint16_t)(x)
+#define letoh32(x) (uint32_t)(x)
+#define letoh64(x) (uint64_t)(x)
+#else
+#define betoh16(x) (uint16_t)(x)
+#define betoh32(x) (uint32_t)(x)
+#define betoh64(x) (uint64_t)(x)
+#define letoh16(x) __bswap16(x)
+#define letoh32(x) __bswap32(x)
+#define letoh64(x) __bswap64(x)
+#endif
+#endif
+
+#endif
diff --git a/controller/fw/musl_include_shims/features.h b/controller/fw/musl_include_shims/features.h
new file mode 100644
index 0000000..85cfb72
--- /dev/null
+++ b/controller/fw/musl_include_shims/features.h
@@ -0,0 +1,40 @@
+#ifndef _FEATURES_H
+#define _FEATURES_H
+
+#if defined(_ALL_SOURCE) && !defined(_GNU_SOURCE)
+#define _GNU_SOURCE 1
+#endif
+
+#if defined(_DEFAULT_SOURCE) && !defined(_BSD_SOURCE)
+#define _BSD_SOURCE 1
+#endif
+
+#if !defined(_POSIX_SOURCE) && !defined(_POSIX_C_SOURCE) \
+ && !defined(_XOPEN_SOURCE) && !defined(_GNU_SOURCE) \
+ && !defined(_BSD_SOURCE) && !defined(__STRICT_ANSI__)
+#define _BSD_SOURCE 1
+#define _XOPEN_SOURCE 700
+#endif
+
+#if __STDC_VERSION__ >= 199901L
+#define __restrict restrict
+#elif !defined(__GNUC__)
+#define __restrict
+#endif
+
+#if __STDC_VERSION__ >= 199901L || defined(__cplusplus)
+#define __inline inline
+#elif !defined(__GNUC__)
+#define __inline
+#endif
+
+#if __STDC_VERSION__ >= 201112L
+#elif defined(__GNUC__)
+#define _Noreturn __attribute__((__noreturn__))
+#else
+#define _Noreturn
+#endif
+
+#define __REDIR(x,y) __typeof__(x) x __asm__(#y)
+
+#endif
diff --git a/controller/fw/musl_include_shims/fp_arch.h b/controller/fw/musl_include_shims/fp_arch.h
new file mode 100644
index 0000000..f5bab6d
--- /dev/null
+++ b/controller/fw/musl_include_shims/fp_arch.h
@@ -0,0 +1,6 @@
+#ifndef __MUSL_SHIM_FP_ARCH_H__
+#define __MUSL_SHIM_FP_ARCH_H__
+
+#define hidden
+
+#endif /* __MUSL_SHIM_FP_ARCH_H__ */
diff --git a/controller/fw/musl_include_shims/libm.h b/controller/fw/musl_include_shims/libm.h
new file mode 100644
index 0000000..d48135d
--- /dev/null
+++ b/controller/fw/musl_include_shims/libm.h
@@ -0,0 +1,270 @@
+#ifndef _LIBM_H
+#define _LIBM_H
+
+#include <stdint.h>
+#include <float.h>
+#include <math.h>
+#include <endian.h>
+#include "fp_arch.h"
+
+#if LDBL_MANT_DIG == 53 && LDBL_MAX_EXP == 1024
+#elif LDBL_MANT_DIG == 64 && LDBL_MAX_EXP == 16384 && __BYTE_ORDER == __LITTLE_ENDIAN
+union ldshape {
+	long double f;
+	struct {
+		uint64_t m;
+		uint16_t se;
+	} i;
+};
+#elif LDBL_MANT_DIG == 64 && LDBL_MAX_EXP == 16384 && __BYTE_ORDER == __BIG_ENDIAN
+/* This is the m68k variant of 80-bit long double, and this definition only works
+ * on archs where the alignment requirement of uint64_t is <= 4. */
+union ldshape {
+	long double f;
+	struct {
+		uint16_t se;
+		uint16_t pad;
+		uint64_t m;
+	} i;
+};
+#elif LDBL_MANT_DIG == 113 && LDBL_MAX_EXP == 16384 && __BYTE_ORDER == __LITTLE_ENDIAN
+union ldshape {
+	long double f;
+	struct {
+		uint64_t lo;
+		uint32_t mid;
+		uint16_t top;
+		uint16_t se;
+	} i;
+	struct {
+		uint64_t lo;
+		uint64_t hi;
+	} i2;
+};
+#elif LDBL_MANT_DIG == 113 && LDBL_MAX_EXP == 16384 && __BYTE_ORDER == __BIG_ENDIAN
+union ldshape {
+	long double f;
+	struct {
+		uint16_t se;
+		uint16_t top;
+		uint32_t mid;
+		uint64_t lo;
+	} i;
+	struct {
+		uint64_t hi;
+		uint64_t lo;
+	} i2;
+};
+#else
+#error Unsupported long double representation
+#endif
+
+/* Support non-nearest rounding mode.  */
+#define WANT_ROUNDING 1
+/* Support signaling NaNs.  */
+#define WANT_SNAN 0
+
+#if WANT_SNAN
+#error SNaN is unsupported
+#else
+#define issignalingf_inline(x) 0
+#define issignaling_inline(x) 0
+#endif
+
+#ifndef TOINT_INTRINSICS
+#define TOINT_INTRINSICS 0
+#endif
+
+#if TOINT_INTRINSICS
+/* Round x to nearest int in all rounding modes, ties have to be rounded
+   consistently with converttoint so the results match.  If the result
+   would be outside of [-2^31, 2^31-1] then the semantics is unspecified.  */
+static double_t roundtoint(double_t);
+
+/* Convert x to nearest int in all rounding modes, ties have to be rounded
+   consistently with roundtoint.  If the result is not representible in an
+   int32_t then the semantics is unspecified.  */
+static int32_t converttoint(double_t);
+#endif
+
+/* Helps static branch prediction so hot path can be better optimized.  */
+#ifdef __GNUC__
+#define predict_true(x) __builtin_expect(!!(x), 1)
+#define predict_false(x) __builtin_expect(x, 0)
+#else
+#define predict_true(x) (x)
+#define predict_false(x) (x)
+#endif
+
+/* Evaluate an expression as the specified type. With standard excess
+   precision handling a type cast or assignment is enough (with
+   -ffloat-store an assignment is required, in old compilers argument
+   passing and return statement may not drop excess precision).  */
+
+static inline float eval_as_float(float x)
+{
+	float y = x;
+	return y;
+}
+
+static inline double eval_as_double(double x)
+{
+	double y = x;
+	return y;
+}
+
+/* fp_barrier returns its input, but limits code transformations
+   as if it had a side-effect (e.g. observable io) and returned
+   an arbitrary value.  */
+
+#ifndef fp_barrierf
+#define fp_barrierf fp_barrierf
+static inline float fp_barrierf(float x)
+{
+	volatile float y = x;
+	return y;
+}
+#endif
+
+#ifndef fp_barrier
+#define fp_barrier fp_barrier
+static inline double fp_barrier(double x)
+{
+	volatile double y = x;
+	return y;
+}
+#endif
+
+#ifndef fp_barrierl
+#define fp_barrierl fp_barrierl
+static inline long double fp_barrierl(long double x)
+{
+	volatile long double y = x;
+	return y;
+}
+#endif
+
+/* fp_force_eval ensures that the input value is computed when that's
+   otherwise unused.  To prevent the constant folding of the input
+   expression, an additional fp_barrier may be needed or a compilation
+   mode that does so (e.g. -frounding-math in gcc). Then it can be
+   used to evaluate an expression for its fenv side-effects only.   */
+
+#ifndef fp_force_evalf
+#define fp_force_evalf fp_force_evalf
+static inline void fp_force_evalf(float x)
+{
+	volatile float y;
+	y = x;
+}
+#endif
+
+#ifndef fp_force_eval
+#define fp_force_eval fp_force_eval
+static inline void fp_force_eval(double x)
+{
+	volatile double y;
+	y = x;
+}
+#endif
+
+#ifndef fp_force_evall
+#define fp_force_evall fp_force_evall
+static inline void fp_force_evall(long double x)
+{
+	volatile long double y;
+	y = x;
+}
+#endif
+
+#define FORCE_EVAL(x) do {                        \
+	if (sizeof(x) == sizeof(float)) {         \
+		fp_force_evalf(x);                \
+	} else if (sizeof(x) == sizeof(double)) { \
+		fp_force_eval(x);                 \
+	} else {                                  \
+		fp_force_evall(x);                \
+	}                                         \
+} while(0)
+
+#define asuint(f) ((union{float _f; uint32_t _i;}){f})._i
+#define asfloat(i) ((union{uint32_t _i; float _f;}){i})._f
+#define asuint64(f) ((union{double _f; uint64_t _i;}){f})._i
+#define asdouble(i) ((union{uint64_t _i; double _f;}){i})._f
+
+#define EXTRACT_WORDS(hi,lo,d)                    \
+do {                                              \
+  uint64_t __u = asuint64(d);                     \
+  (hi) = __u >> 32;                               \
+  (lo) = (uint32_t)__u;                           \
+} while (0)
+
+#define GET_HIGH_WORD(hi,d)                       \
+do {                                              \
+  (hi) = asuint64(d) >> 32;                       \
+} while (0)
+
+#define GET_LOW_WORD(lo,d)                        \
+do {                                              \
+  (lo) = (uint32_t)asuint64(d);                   \
+} while (0)
+
+#define INSERT_WORDS(d,hi,lo)                     \
+do {                                              \
+  (d) = asdouble(((uint64_t)(hi)<<32) | (uint32_t)(lo)); \
+} while (0)
+
+#define SET_HIGH_WORD(d,hi)                       \
+  INSERT_WORDS(d, hi, (uint32_t)asuint64(d))
+
+#define SET_LOW_WORD(d,lo)                        \
+  INSERT_WORDS(d, asuint64(d)>>32, lo)
+
+#define GET_FLOAT_WORD(w,d)                       \
+do {                                              \
+  (w) = asuint(d);                                \
+} while (0)
+
+#define SET_FLOAT_WORD(d,w)                       \
+do {                                              \
+  (d) = asfloat(w);                               \
+} while (0)
+
+hidden int    __rem_pio2_large(double*,double*,int,int,int);
+
+hidden int    __rem_pio2(double,double*);
+hidden double __sin(double,double,int);
+hidden double __cos(double,double);
+hidden double __tan(double,double,int);
+hidden double __expo2(double);
+
+hidden int    __rem_pio2f(float,double*);
+hidden float  __sindf(double);
+hidden float  __cosdf(double);
+hidden float  __tandf(double,int);
+hidden float  __expo2f(float);
+
+hidden int __rem_pio2l(long double, long double *);
+hidden long double __sinl(long double, long double, int);
+hidden long double __cosl(long double, long double);
+hidden long double __tanl(long double, long double, int);
+
+hidden long double __polevll(long double, const long double *, int);
+hidden long double __p1evll(long double, const long double *, int);
+
+hidden double __lgamma_r(double, int *);
+hidden float __lgammaf_r(float, int *);
+
+/* error handling functions */
+hidden float __math_xflowf(uint32_t, float);
+hidden float __math_uflowf(uint32_t);
+hidden float __math_oflowf(uint32_t);
+hidden float __math_divzerof(uint32_t);
+hidden float __math_invalidf(float);
+hidden double __math_xflow(uint32_t, double);
+hidden double __math_uflow(uint32_t);
+hidden double __math_oflow(uint32_t);
+hidden double __math_divzero(uint32_t);
+hidden double __math_invalid(double);
+
+#endif
diff --git a/controller/fw/src/adc.c b/controller/fw/src/adc.c
new file mode 100644
index 0000000..5f6b995
--- /dev/null
+++ b/controller/fw/src/adc.c
@@ -0,0 +1,93 @@
+
+#include <stm32f407xx.h>
+#include <stm32f4_isr.h>
+
+#include "adc.h"
+#include "sr_global.h"
+
+uint16_t adc_fft_buf[2][FMEAS_FFT_LEN];
+volatile int adc_fft_buf_ready_idx = -1;
+
+static DMA_TypeDef *const adc_dma = DMA2;
+static DMA_Stream_TypeDef *const mem_stream = DMA2_Stream1;
+static DMA_Stream_TypeDef *const adc_stream = DMA2_Stream0;
+static const int dma_adc_channel = 0;
+
+/* Configure ADC1 to sample channel 0. Trigger from TIM1 CC0 every 1ms. Transfer readings into alternating buffers
+ * throug DMA. Enable DMA interrupts.
+ *
+ * We have two full FFT buffers. We always transfer data from the ADC to the back half of the active one, while a
+ * DMA memcpy'es the latter half of the inactive one to the front half of the active one. This means at the end of the
+ * ADC's DMA transfer, in the now-inactive buffer that the ADC results were just written to we have last half-period's
+ * data sitting in front of this half-period's data like so: [old_adc_data, new_adc_data]
+ *
+ * This means we can immediately start running an FFT on ADC DMA transfer complete interrupt.
+ */
+void adc_init() {
+    RCC->AHB1ENR |= RCC_AHB1ENR_DMA2EN;
+    RCC->APB2ENR |= RCC_APB2ENR_ADC1EN | RCC_APB2ENR_TIM1EN;
+
+    adc_dma->LIFCR |= 0x3f;
+    adc_stream->CR = 0; /* disable */
+    while (adc_stream->CR & DMA_SxCR_EN)
+        ; /* wait for stream to become available */
+    adc_stream->NDTR = FMEAS_FFT_LEN/2; 
+    adc_stream->PAR = ADC1->DR;
+    adc_stream->M0AR = (uint32_t) (adc_fft_buf[0] + FMEAS_FFT_LEN/2);
+    adc_stream->M1AR = (uint32_t) (adc_fft_buf[1] + FMEAS_FFT_LEN/2);
+    adc_stream->CR = (dma_adc_channel<<DMA_SxCR_CHSEL_Pos) | DMA_SxCR_DBM | (1<<DMA_SxCR_MSIZE_Pos)
+        | (1<<DMA_SxCR_PSIZE_Pos) | DMA_SxCR_MINC | DMA_SxCR_CIRC | DMA_SxCR_PFCTRL
+        | DMA_SxCR_TCIE | DMA_SxCR_TEIE | DMA_SxCR_DMEIE | DMA_SxCR_EN;
+    adc_stream->CR |= DMA_SxCR_EN;
+
+    NVIC_EnableIRQ(DMA2_Stream0_IRQn);
+    NVIC_SetPriority(DMA2_Stream0_IRQn, 128);
+
+    ADC1->CR1 = (0<<ADC_CR1_RES_Pos) | (0<<ADC_CR1_DISCNUM_Pos) | ADC_CR1_DISCEN | (0<<ADC_CR1_AWDCH_Pos);
+    ADC1->CR2 = ADC_CR2_EXTEN | (0<<ADC_CR2_EXTSEL_Pos) | ADC_CR2_DMA | ADC_CR2_ADON | ADC_CR2_DDS;
+
+    TIM1->CR2 = (2<<TIM_CR2_MMS_Pos); /* Enable update event on TRGO to provide a 1ms reference to rest of system */
+    TIM1->CR1 = TIM_CR1_CEN;
+    TIM1->CCMR1 = (6<<TIM_CCMR1_OC1M_Pos) | (0<<TIM_CCMR1_CC1S_Pos);
+    TIM1->CCER = TIM_CCER_CC1E;
+    TIM1->PSC = 84; /* 1us ticks @ f_APB2=84MHz */
+    TIM1->ARR = 1000; /* 1ms period */
+    TIM1->CCR1 = 1;
+    TIM1->EGR = TIM_EGR_UG;
+}
+
+void DMA2_Stream0_IRQHandler(void) {
+    uint8_t isr = (DMA2->LISR >> DMA_LISR_FEIF0_Pos) & 0x3f;
+
+    if (isr & DMA_LISR_TCIF0) { /* Transfer complete */
+        /* Check we're done processing the old buffer */
+        if (adc_fft_buf_ready_idx != -1)
+            panic();
+
+        /* Kickoff memory DMA into new buffer */
+        if (mem_stream->CR & DMA_SxCR_EN)
+            panic(); /* We should be long done by now. */
+
+        mem_stream->NDTR = FMEAS_FFT_LEN/2; 
+        int ct = !!(adc_stream->CR & DMA_SxCR_CT);
+        /* back half of old buffer (that was just written) */
+        mem_stream->PAR = (uint32_t)(adc_fft_buf[!ct]);
+        /* front half of current buffer (whose back half is being written now) */
+        mem_stream->M0AR = (uint32_t) (adc_fft_buf[ct] + 0);
+        mem_stream->CR = (1<<DMA_SxCR_MSIZE_Pos) | (1<<DMA_SxCR_PSIZE_Pos) | DMA_SxCR_MINC | DMA_SxCR_PINC 
+            | DMA_SxCR_TCIE | DMA_SxCR_TEIE | DMA_SxCR_DMEIE;
+        mem_stream->CR |= DMA_SxCR_EN;
+
+        /* Kickoff FFT */
+        adc_fft_buf_ready_idx = !ct;
+    }
+
+    if (isr & DMA_LISR_DMEIF0) /* Direct mode error */
+        panic();
+
+    if (isr & DMA_LISR_TEIF0) /* Transfer error */
+        panic();
+
+    /* clear all flags */
+    adc_dma->LIFCR = isr<<DMA_LISR_FEIF0_Pos;
+}
diff --git a/controller/fw/src/adc.h b/controller/fw/src/adc.h
new file mode 100644
index 0000000..e5611cb
--- /dev/null
+++ b/controller/fw/src/adc.h
@@ -0,0 +1,10 @@
+#ifndef __ADC_H__
+#define __ADC_H__
+
+void adc_init(void);
+
+extern uint16_t adc_fft_buf[2][FMEAS_FFT_LEN];
+/* set index of ready buffer in adc.c, reset to -1 in main.c after processing */
+extern volatile int adc_fft_buf_ready_idx;
+
+#endif /* __ADC_H__ */
diff --git a/controller/fw/src/dsss_demod.c b/controller/fw/src/dsss_demod.c
index c5a2070..0758525 100644
--- a/controller/fw/src/dsss_demod.c
+++ b/controller/fw/src/dsss_demod.c
@@ -23,12 +23,15 @@ struct iir_biquad cwt_filter_bq[DSSS_FILTER_CLEN] = {DSSS_FILTER_COEFF};
 void debug_print_vector(const char *name, size_t len, const float *data, size_t stride, bool index, bool debug);
 static float gold_correlate_step(const size_t ncode, const float a[DSSS_CORRELATION_LENGTH], size_t offx, bool debug);
 static float cwt_convolve_step(const float v[DSSS_WAVELET_LUT_SIZE], size_t offx);
+#if 0
 static float run_iir(const float x, const int order, const struct iir_biquad q[order], struct iir_biquad_state st[order]);
 static float run_biquad(float x, const struct iir_biquad *const q, struct iir_biquad_state *const restrict st);
+#endif
 static void matcher_init(struct matcher_state states[static DSSS_MATCHER_CACHE_SIZE]);
 static void matcher_tick(struct matcher_state states[static DSSS_MATCHER_CACHE_SIZE],
         uint64_t ts, int peak_ch, float peak_ampl);
 static void group_received(struct dsss_demod_state *st);
+static uint8_t decode_peak(int peak_ch, float peak_ampl);
 
 #ifdef SIMULATION
 void debug_print_vector(const char *name, size_t len, const float *data, size_t stride, bool index, bool debug) {
@@ -48,7 +51,8 @@ void debug_print_vector(const char *name, size_t len, const float *data, size_t
     DEBUG_PRINTN("]\n");
 }
 #else 
-void debug_print_vector(const char *name, size_t len, const float *data, size_t stride, bool index, bool debug) {}
+void debug_print_vector(unused_a const char *name, unused_a size_t len, unused_a const float *data,
+        unused_a size_t stride, unused_a bool index, unused_a bool debug) {}
 #endif
 
 void dsss_demod_init(struct dsss_demod_state *st) {
@@ -74,7 +78,7 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value, uint64_t ts)
 
     float avg = 0.0f;
     for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++)
-        avg += fabs(cwt[i]);
+        avg += fabsf(cwt[i]);
     avg /= (float)DSSS_GOLD_CODE_COUNT;
     /* FIXME fix this filter */
     //avg = run_iir(avg, ARRAY_LENGTH(cwt_filter_bq), cwt_filter_bq, st->cwt_filter.st);
@@ -86,12 +90,12 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value, uint64_t ts)
     for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++) {
         float val = cwt[i] / avg;
 
-        if (fabs(val) > fabs(max_val)) {
+        if (fabsf(val) > fabsf(max_val)) {
             max_val = val;
             max_ch = i;
             max_ts = ts;
 
-            if (fabs(val) > DSSS_THESHOLD_FACTOR)
+            if (fabsf(val) > DSSS_THESHOLD_FACTOR)
                 found = true;
         }
     }
@@ -155,7 +159,7 @@ void matcher_tick(struct matcher_state states[static DSSS_MATCHER_CACHE_SIZE], u
 
         if (current_phase == states[i].last_phase) {
             /* Skip sampling */
-            float score = fabs(peak_ampl) * (1.0f - skip_sampling_depreciation);
+            float score = fabsf(peak_ampl) * (1.0f - skip_sampling_depreciation);
             if (score > states[i].candidate_score) {
                 /* We win, update candidate */
                 assert(i < DSSS_MATCHER_CACHE_SIZE);
@@ -272,6 +276,7 @@ void group_received(struct dsss_demod_state *st) {
     }
 }
 
+#if 0
 float run_iir(const float x, const int order, const struct iir_biquad q[order], struct iir_biquad_state st[order]) {
     float intermediate = x;
     for (int i=0; i<(order+1)/2; i++)
@@ -287,6 +292,7 @@ float run_biquad(float x, const struct iir_biquad *const q, struct iir_biquad_st
     st->reg[0] = intermediate;
     return out;
 }
+#endif
 
 float cwt_convolve_step(const float v[DSSS_WAVELET_LUT_SIZE], size_t offx) {
     float sum = 0.0f;
diff --git a/controller/fw/src/dsss_demod.h b/controller/fw/src/dsss_demod.h
index 98cf590..448abb4 100644
--- a/controller/fw/src/dsss_demod.h
+++ b/controller/fw/src/dsss_demod.h
@@ -1,6 +1,9 @@
 #ifndef __DSSS_DEMOD_H__
 #define __DSSS_DEMOD_H__
 
+#include <stdint.h>
+#include <unistd.h>
+
 #define DSSS_GOLD_CODE_LENGTH   ((1<<DSSS_GOLD_CODE_NBITS) - 1)
 #define DSSS_GOLD_CODE_COUNT    ((1<<DSSS_GOLD_CODE_NBITS) + 1)
 #define DSSS_CORRELATION_LENGTH (DSSS_GOLD_CODE_LENGTH * DSSS_DECIMATION)
diff --git a/controller/fw/src/freq_meas.c b/controller/fw/src/freq_meas.c
index ba7801e..83a330c 100644
--- a/controller/fw/src/freq_meas.c
+++ b/controller/fw/src/freq_meas.c
@@ -30,7 +30,7 @@ extern arm_status arm_rfft_4096_fast_init_f32(arm_rfft_fast_instance_f32 * S);
 void func_gauss_grad(float *out, float *params, int x, void *userdata);
 float func_gauss(float *params, int x, void *userdata);
 
-int adc_buf_measure_freq(int16_t adc_buf[FMEAS_FFT_LEN], float *out) {
+int adc_buf_measure_freq(uint16_t adc_buf[FMEAS_FFT_LEN], float *out) {
     int rc;
     float in_buf[FMEAS_FFT_LEN];
     float out_buf[FMEAS_FFT_LEN];
@@ -42,7 +42,7 @@ int adc_buf_measure_freq(int16_t adc_buf[FMEAS_FFT_LEN], float *out) {
     */
     //DEBUG_PRINT("Applying window function");
     for (size_t i=0; i<FMEAS_FFT_LEN; i++)
-        in_buf[i] = (float)adc_buf[i] / (float)FMEAS_ADC_MAX * fmeas_fft_window_table[i];
+        in_buf[i] = ((float)adc_buf[i] / (float)FMEAS_ADC_MAX - 0.5) * fmeas_fft_window_table[i];
 
     //DEBUG_PRINT("Running FFT");
     arm_rfft_fast_instance_f32 fft_inst;
diff --git a/controller/fw/src/freq_meas.h b/controller/fw/src/freq_meas.h
index 4786c0e..1c083f8 100644
--- a/controller/fw/src/freq_meas.h
+++ b/controller/fw/src/freq_meas.h
@@ -2,6 +2,6 @@
 #ifndef __FREQ_MEAS_H__
 #define __FREQ_MEAS_H__
 
-int adc_buf_measure_freq(int16_t adc_buf[FMEAS_FFT_LEN], float *out);
+int adc_buf_measure_freq(uint16_t adc_buf[FMEAS_FFT_LEN], float *out);
 
 #endif /* __FREQ_MEAS_H__ */
diff --git a/controller/fw/src/main.c b/controller/fw/src/main.c
index 3131df1..58977f7 100644
--- a/controller/fw/src/main.c
+++ b/controller/fw/src/main.c
@@ -1,45 +1,126 @@
 
 #include <stdbool.h>
+#include <stdint.h>
+#include <assert.h>
 
-#include <libopencm3/stm32/rcc.h>
-#include <libopencm3/stm32/spi.h>
-#include <libopencm3/stm32/gpio.h>
+#include <stm32f407xx.h>
 
+#include "sr_global.h"
+#include "adc.h"
 #include "spi_flash.h"
+#include "freq_meas.h"
+#include "dsss_demod.h"
 
 static struct spi_flash_if spif;
 
+
+void __libc_init_array(void) { /* we don't need this. */ }
+void __assert_func (unused_a const char *file, unused_a int line, unused_a const char *function, unused_a const char *expr) {
+    asm volatile ("bkpt");
+    while(1) {}
+}
+
 static void clock_setup(void)
 {
-    rcc_clock_setup_pll(&rcc_hse_8mhz_3v3[RCC_CLOCK_3V3_168MHZ]);
-    rcc_periph_clock_enable(RCC_GPIOA);
-    rcc_periph_clock_enable(RCC_GPIOB);
-    rcc_periph_clock_enable(RCC_SPI1);
+    /* 8MHz HSE clock as PLL source.
+     * 
+     * Divide by 8 -> 1 MHz */
+#define PLL_M 8
+    /* Multiply by 336 -> 336 MHz VCO frequency */
+#define PLL_N 336
+    /* Divide by 2 -> 168 MHz (max freq for our chip) */
+#define PLL_P 2
+    /* Aux clock for USB OTG, SDIO, RNG: divide VCO frequency (336 MHz) by 7 -> 48 MHz (required by USB OTG) */
+#define PLL_Q 7
+
+    RCC->CR |= RCC_CR_HSEON;
+    while(!(RCC->CR & RCC_CR_HSERDY));
+
+    RCC->APB1ENR |= RCC_APB1ENR_PWREN;
+
+    /* set voltage scale to 1 for max frequency
+     * (0b0) scale 2 for fCLK <= 144 Mhz
+     * (0b1) scale 1 for 144 Mhz < fCLK <= 168 Mhz
+     */
+    PWR->CR |= PWR_CR_VOS;
+
+    /* set AHB prescaler to /1 (CFGR:bits 7:4) */
+    RCC->CFGR |= (0 << RCC_CFGR_HPRE_Pos);
+    /* set ABP1 prescaler to 4 */
+    RCC->CFGR |= (5 << RCC_CFGR_PPRE1_Pos);
+    /* set ABP2 prescaler to 2 */
+    RCC->CFGR |= (0x4 << RCC_CFGR_PPRE2_Pos);
+
+    /* Configure PLL */
+    static_assert(PLL_P % 2 == 0);
+    static_assert(PLL_P >= 2 && PLL_P <= 8);
+    static_assert(PLL_N >= 50 && PLL_N <= 432);
+    static_assert(PLL_M >= 2 && PLL_M <= 63);
+    static_assert(PLL_Q >= 2 && PLL_Q <= 15);
+    RCC->PLLCFGR = (PLL_M<<RCC_PLLCFGR_PLLM_Pos)
+        | (PLL_N << RCC_PLLCFGR_PLLM_Pos)
+        | ((PLL_P/2 - 1) << RCC_PLLCFGR_PLLP_Pos)
+        | (PLL_Q << RCC_PLLCFGR_PLLQ_Pos)
+        | RCC_PLLCFGR_PLLSRC; /* select HSE as PLL source */
+    RCC->CR |= RCC_CR_PLLON;
+
+    /* Wait for main PLL */
+    while(!(RCC->CR & RCC_CR_PLLRDY))
+        ;
+
+    /* Configure Flash: enable prefetch, insn cache, data cache; set latency = 5 wait states
+     * See reference manual (RM0090), Section 3.5.1, Table 10 (p. 80)
+     */
+    FLASH->ACR = FLASH_ACR_PRFTEN | FLASH_ACR_ICEN | FLASH_ACR_DCEN | (5<<FLASH_ACR_LATENCY);
+
+    /* Select PLL as system clock source */
+    RCC->CFGR &= ~RCC_CFGR_SW_Msk;
+    RCC->CFGR |= 2 << RCC_CFGR_SW_Pos;
+
+    /* Wait for clock to switch over */
+    while ((RCC->CFGR & RCC_CFGR_SWS_Msk)>>RCC_CFGR_SWS_Pos != 2)
+        ;
 }
 
 static void led_setup(void)
 {
-    gpio_mode_setup(GPIOA, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO6 | GPIO7);
+    GPIOA->MODER |= (1<<GPIO_MODER_MODER6_Pos) | (1<<GPIO_MODER_MODER7_Pos);
 }
 
 static void spi_flash_if_set_cs(bool val) {
     if (val)
-        gpio_set(GPIOB, GPIO0);
+        GPIOB->BSRR = 1<<0;
     else
-        gpio_clear(GPIOB, GPIO0);
+        GPIOB->BSRR = 1<<16;
 }
 
 static void spi_flash_setup(void)
 {
-    gpio_mode_setup(GPIOB, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO3 | GPIO4 | GPIO5); /* SPI flash SCK/MISO/MOSI */
-    gpio_mode_setup(GPIOB, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO0); /* SPI flash CS */
-    gpio_set_output_options(GPIOB, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, GPIO0 | GPIO3 | GPIO4 | GPIO5);
-    gpio_set_af(GPIOB, 5, GPIO3 | GPIO4 | GPIO5);
+    GPIOB->MODER &= ~GPIO_MODER_MODER3_Msk & ~GPIO_MODER_MODER4_Msk & ~GPIO_MODER_MODER5_Msk & ~GPIO_MODER_MODER0_Msk;
+    GPIOB->MODER |= (2<<GPIO_MODER_MODER3_Pos) /* SCK */
+        | (2<<GPIO_MODER_MODER4_Pos) /* MISO */
+        | (2<<GPIO_MODER_MODER5_Pos) /* MOSI */
+        | (1<<GPIO_MODER_MODER0_Pos); /* CS */
+
+    GPIOB->OSPEEDR &= ~GPIO_OSPEEDR_OSPEED3_Msk & ~GPIO_OSPEEDR_OSPEED4_Msk
+        & ~GPIO_OSPEEDR_OSPEED5_Msk & ~GPIO_OSPEEDR_OSPEED0_Msk;
+    GPIOB->OSPEEDR |= (2<<GPIO_OSPEEDR_OSPEED3_Pos) /* SCK */
+        | (2<<GPIO_OSPEEDR_OSPEED4_Pos) /* MISO */
+        | (2<<GPIO_OSPEEDR_OSPEED5_Pos) /* MOSI */
+        | (2<<GPIO_OSPEEDR_OSPEED0_Pos); /* CS */
+
+    GPIOB->AFR[0] &= ~GPIO_AFRL_AFSEL3_Msk & ~GPIO_AFRL_AFSEL4_Msk & ~GPIO_AFRL_AFSEL5_Msk;
+    GPIOB->AFR[0] |= (5<<GPIO_AFRL_AFSEL3_Pos) | (5<<GPIO_AFRL_AFSEL4_Pos) | (5<<GPIO_AFRL_AFSEL5_Pos);
+
+    RCC->APB2ENR |= RCC_APB2ENR_SPI1EN;
+    RCC->APB2RSTR |= RCC_APB2RSTR_SPI1RST;
+    RCC->APB2RSTR &= RCC_APB2RSTR_SPI1RST;
 
     spif_init(&spif, 256, SPI1, &spi_flash_if_set_cs);
 }
 
-/*
+/* SPI flash test routine to be called from gdb */
+#ifdef SPI_FLASH_TEST
 void spi_flash_test(void) {
     spif_clear_mem(&spif);
 
@@ -48,7 +129,7 @@ void spi_flash_test(void) {
         for (size_t i=0; i<sizeof(buf); i+= sizeof(buf[0]))
             buf[i/sizeof(buf[0])] = addr + i;
 
-            spif_write(&spif, addr, sizeof(buf), (char *)buf);
+        spif_write(&spif, addr, sizeof(buf), (char *)buf);
     }
 
     for (size_t i=0; i<sizeof(buf)/sizeof(buf[0]); i++)
@@ -56,21 +137,35 @@ void spi_flash_test(void) {
     spif_read(&spif, 0x1030, sizeof(buf), (char *)buf);
     asm volatile ("bkpt");
 }
-*/
+#endif
+
+static unsigned int measurement_errors = 0;
+static struct dsss_demod_state demod_state;
+static uint32_t freq_sample_ts = 0;
 
 int main(void)
 {
     clock_setup();
     led_setup();
     spi_flash_setup();
-
-    gpio_set(GPIOA, GPIO6);
-
-    while (1) {
-        gpio_toggle(GPIOA, GPIO6 | GPIO7);
-        for (int i = 0; i < 6000000; i++)
-            __asm__("nop");
+    adc_init();
+
+    while (23) {
+        if (adc_fft_buf_ready_idx != -1) {
+            float out;
+            if (adc_buf_measure_freq(adc_fft_buf[adc_fft_buf_ready_idx], &out)) {
+                measurement_errors++;
+                continue;
+            }
+
+            dsss_demod_init(&demod_state);
+            dsss_demod_step(&demod_state, out, freq_sample_ts);
+
+            freq_sample_ts++; /* TODO: also increase in case of freq measurement error? */
+            adc_fft_buf_ready_idx = -1;
+        }
     }
 
     return 0;
 }
+
diff --git a/controller/fw/src/mspdebug_wrapper.c b/controller/fw/src/mspdebug_wrapper.c
index 3a6d526..eda91cc 100644
--- a/controller/fw/src/mspdebug_wrapper.c
+++ b/controller/fw/src/mspdebug_wrapper.c
@@ -2,14 +2,14 @@
 #include <unistd.h>
 #include <errno.h>
 
-#include <libopencm3/stm32/gpio.h>
-
 #include "output.h"
 #include "jtaglib.h"
 
 #include "sr_global.h"
 #include "mspdebug_wrapper.h"
 
+#include <stm32f407xx.h>
+
 #define BLOCK_SIZE 512 /* bytes */
 
 
@@ -101,7 +101,7 @@ enum sr_gpio_types {
 };
 
 struct {
-	uint32_t port;
+    GPIO_TypeDef *gpio;
 	uint16_t num;
 } gpios[SR_NUM_GPIOS] = {
 	[SR_GPIO_TCK] = {GPIOD, 8},
@@ -114,9 +114,9 @@ struct {
 
 static void sr_gpio_write(int num, int out) {
 	if (out)
-		gpio_set(gpios[num].port, gpios[num].num);
+        gpios[num].gpio->BSRR = 1<<gpios[num].num;
 	else
-		gpio_clear(gpios[num].port, gpios[num].num);
+        gpios[num].gpio->BSRR = 1<<gpios[num].num<<16;
 }
 
 static void sr_jtdev_tck(struct jtdev *p, int out) {
@@ -146,7 +146,7 @@ static void sr_jtdev_tst(struct jtdev *p, int out) {
 
 static int sr_jtdev_tdo_get(struct jtdev *p) {
     UNUSED(p);
-	return gpio_get(gpios[SR_GPIO_TST].port, gpios[SR_GPIO_TST].num);
+	return !!(gpios[SR_GPIO_TST].gpio->IDR & (1<<gpios[SR_GPIO_TST].num));
 }
 
 static void sr_jtdev_tclk(struct jtdev *p, int out) {
@@ -156,14 +156,14 @@ static void sr_jtdev_tclk(struct jtdev *p, int out) {
 
 static int sr_jtdev_tclk_get(struct jtdev *p) {
     UNUSED(p);
-	return gpio_get(gpios[SR_GPIO_TDI].port, gpios[SR_GPIO_TDI].num);
+	return !!(gpios[SR_GPIO_TDI].gpio->IDR & (1<<gpios[SR_GPIO_TDI].num));
 }
 
 static void sr_jtdev_tclk_strobe(struct jtdev *p, unsigned int count) {
     UNUSED(p);
 	while (count--) {
-		gpio_set(gpios[SR_GPIO_TDI].port, gpios[SR_GPIO_TDI].num);
-		gpio_clear(gpios[SR_GPIO_TDI].port, gpios[SR_GPIO_TDI].num);
+        gpios[SR_GPIO_TDI].gpio->BSRR = 1<<gpios[SR_GPIO_TDI].num;
+        gpios[SR_GPIO_TDI].gpio->BSRR = 1<<gpios[SR_GPIO_TDI].num<<16;
 	}
 }
 
diff --git a/controller/fw/src/protocol.c b/controller/fw/src/protocol.c
new file mode 100644
index 0000000..17020bd
--- /dev/null
+++ b/controller/fw/src/protocol.c
@@ -0,0 +1,7 @@
+
+#include "sr_global.h"
+#include "dsss_demod.h"
+
+void handle_dsss_received(uint8_t unused_a data[static TRANSMISSION_SYMBOLS]) {
+    asm volatile ("bkpt");
+}
diff --git a/controller/fw/src/spi_flash.c b/controller/fw/src/spi_flash.c
index 98c773f..305be8f 100644
--- a/controller/fw/src/spi_flash.c
+++ b/controller/fw/src/spi_flash.c
@@ -22,7 +22,6 @@
  */
 
 #include "spi_flash.h"
-#include <libopencm3/stm32/spi.h>
 
 enum {
     WRITE_ENABLE                = 0x06,
@@ -49,6 +48,10 @@ enum {
 };
 
 
+static uint8_t spi_xfer(volatile SPI_TypeDef *spi, uint8_t b);
+static uint8_t spi_read(struct spi_flash_if *spif);
+static void spi_write(struct spi_flash_if *spif, uint8_t b);
+
 static void spif_write_page(struct spi_flash_if *spif, size_t addr, size_t len, const char* data);
 static uint8_t spif_read_status(struct spi_flash_if *spif);
 static void spif_enable_write(struct spi_flash_if *spif);
@@ -58,32 +61,38 @@ static void spif_wait_for_write(struct spi_flash_if *spif);
 #define mid_byte(x) ((x>>8)&0xff)
 #define high_byte(x) ((x>>16)&0xff)
 
+uint8_t spi_xfer(volatile SPI_TypeDef *spi, uint8_t b) {
+    while (!(spi->SR & SPI_SR_TXE))
+        ;
+    (void) spi->DR; /* perform dummy read to clear RXNE flag */
+    spi->DR = b;
+    while (!(spi->SR & SPI_SR_RXNE))
+        ;
+    return spi->DR;
+}
 
-void spif_init(struct spi_flash_if *spif, size_t page_size, uint32_t spi_base, void (*cs)(bool val)) {
+uint8_t spi_read(struct spi_flash_if *spif) {
+    return spi_xfer(spif->spi, 0);
+}
+
+void spi_write(struct spi_flash_if *spif, uint8_t b) {
+    (void)spi_xfer(spif->spi, b);
+}
 
-    spif->spi_base = spi_base;
+void spif_init(struct spi_flash_if *spif, size_t page_size, SPI_TypeDef *spi, void (*cs)(bool val)) {
+
+    spif->spi = spi;
     spif->page_size = page_size;
     spif->cs = cs;
     spif->cs(1);
 
-    spi_reset(spif->spi_base);
-    spi_init_master(spif->spi_base,
-            SPI_CR1_BAUDRATE_FPCLK_DIV_2,
-            SPI_CR1_CPOL_CLK_TO_1_WHEN_IDLE,
-            SPI_CR1_CPHA_CLK_TRANSITION_2,
-            SPI_CR1_DFF_8BIT,
-            SPI_CR1_MSBFIRST);
-
-    spi_enable_software_slave_management(spif->spi_base);
-    spi_set_nss_high(spif->spi_base);
-
-    spi_enable(spif->spi_base);
+    spi->CR1 = (0<<SPI_CR1_BR_Pos) | SPI_CR1_CPOL | SPI_CR1_CPHA | SPI_CR1_SSM | SPI_CR1_SSI | SPI_CR1_SPE | SPI_CR1_MSTR;
 
     spif->cs(0);
-    spi_send(spif->spi_base, READ_IDENTIFICATION);
-    spif->id.mfg_id = spi_xfer(spif->spi_base, 0);
-    spif->id.type = spi_xfer(spif->spi_base, 0);
-    spif->id.size = 1<<spi_xfer(spif->spi_base, 0);
+    spi_write(spif, READ_IDENTIFICATION);
+    spif->id.mfg_id = spi_read(spif);
+    spif->id.type = spi_read(spif);
+    spif->id.size = 1<<spi_read(spif);
     spif->cs(1);
 }
 
@@ -91,13 +100,13 @@ void spif_read(struct spi_flash_if *spif, size_t addr, size_t len, char* data) {
     spif_enable_write(spif);
 
     spif->cs(0);
-    spi_xfer(spif->spi_base, READ_DATA);
-    spi_xfer(spif->spi_base, high_byte(addr));
-    spi_xfer(spif->spi_base, mid_byte(addr));
-    spi_xfer(spif->spi_base, low_byte(addr));
+    spi_write(spif, READ_DATA);
+    spi_write(spif, high_byte(addr));
+    spi_write(spif, mid_byte(addr));
+    spi_write(spif, low_byte(addr));
 
     for (size_t i = 0; i < len; i++)
-        data[i] = spi_xfer(spif->spi_base, 0);
+        data[i] = spi_read(spif);
 
     spif->cs(1);
 }
@@ -117,8 +126,8 @@ void spif_write(struct spi_flash_if *spif, size_t addr, size_t len, const char*
 
 static uint8_t spif_read_status(struct spi_flash_if *spif) {
     spif->cs(0);
-    spi_xfer(spif->spi_base, READ_STATUS);
-    uint8_t status = spi_xfer(spif->spi_base, 0);
+    spi_write(spif, READ_STATUS);
+    uint8_t status = spi_read(spif);
     spif->cs(1);
 
     return status;
@@ -128,10 +137,10 @@ void spif_clear_sector(struct spi_flash_if *spif, size_t addr) {
     spif_enable_write(spif);
 
     spif->cs(0);
-    spi_xfer(spif->spi_base, SECTOR_ERASE);
-    spi_xfer(spif->spi_base, high_byte(addr));
-    spi_xfer(spif->spi_base, mid_byte(addr));
-    spi_xfer(spif->spi_base, low_byte(addr));
+    spi_write(spif, SECTOR_ERASE);
+    spi_write(spif, high_byte(addr));
+    spi_write(spif, mid_byte(addr));
+    spi_write(spif, low_byte(addr));
     spif->cs(1);
 
     spif_wait_for_write(spif);
@@ -141,7 +150,7 @@ void spif_clear_mem(struct spi_flash_if *spif) {
     spif_enable_write(spif);
 
     spif->cs(0);
-    spi_xfer(spif->spi_base, BULK_ERASE);
+    spi_write(spif, BULK_ERASE);
     spif->cs(1);
 
     spif_wait_for_write(spif);
@@ -151,13 +160,13 @@ static void spif_write_page(struct spi_flash_if *spif, size_t addr, size_t len,
     spif_enable_write(spif);
 
     spif->cs(0);
-    spi_xfer(spif->spi_base, PAGE_PROGRAM);
-    spi_xfer(spif->spi_base, high_byte(addr));
-    spi_xfer(spif->spi_base, mid_byte(addr));
-    spi_xfer(spif->spi_base, low_byte(addr));
+    spi_write(spif, PAGE_PROGRAM);
+    spi_write(spif, high_byte(addr));
+    spi_write(spif, mid_byte(addr));
+    spi_write(spif, low_byte(addr));
 
     for (size_t i = 0; i < len; i++) {
-        spi_xfer(spif->spi_base, data[i]);
+        spi_write(spif, data[i]);
     }
 
     spif->cs(1);
@@ -166,7 +175,7 @@ static void spif_write_page(struct spi_flash_if *spif, size_t addr, size_t len,
 
 static void spif_enable_write(struct spi_flash_if *spif) {
     spif->cs(0);
-    spi_xfer(spif->spi_base, WRITE_ENABLE);
+    spi_write(spif, WRITE_ENABLE);
     spif->cs(1);
 }
 
@@ -178,12 +187,13 @@ static void spif_wait_for_write(struct spi_flash_if *spif) {
 
 void spif_deep_power_down(struct spi_flash_if *spif) {
     spif->cs(0);
-    spi_xfer(spif->spi_base, DEEP_POWER_DOWN);
+    spi_write(spif, DEEP_POWER_DOWN);
     spif->cs(1);
 }
 
 void spif_wakeup(struct spi_flash_if *spif) {
     spif->cs(0);
-    spi_xfer(spif->spi_base, DEEP_POWER_DOWN_RELEASE);
+    spi_write(spif, DEEP_POWER_DOWN_RELEASE);
     spif->cs(1);
 }
+
diff --git a/controller/fw/src/spi_flash.h b/controller/fw/src/spi_flash.h
index e647c6a..2be8933 100644
--- a/controller/fw/src/spi_flash.h
+++ b/controller/fw/src/spi_flash.h
@@ -4,6 +4,8 @@
 #include <stdbool.h>
 #include <unistd.h>
 
+#include <stm32f407xx.h>
+
 struct spi_mem_id {
     size_t size;
     uint8_t mfg_id;
@@ -12,12 +14,12 @@ struct spi_mem_id {
 
 struct spi_flash_if {
     struct spi_mem_id id;
-    uint32_t spi_base;
+    volatile SPI_TypeDef *spi;
     size_t page_size;
     void (*cs)(bool val);
 };
 
-void spif_init(struct spi_flash_if *spif, size_t page_size, uint32_t spi_base, void (*cs)(bool val));
+void spif_init(struct spi_flash_if *spif, size_t page_size, SPI_TypeDef *spi, void (*cs)(bool val));
 
 void spif_write(struct spi_flash_if *spif, size_t addr, size_t len, const char* data);
 void spif_read(struct spi_flash_if *spif, size_t addr, size_t len, char* data);
diff --git a/controller/fw/src/sr_global.h b/controller/fw/src/sr_global.h
index abbad93..ac77359 100644
--- a/controller/fw/src/sr_global.h
+++ b/controller/fw/src/sr_global.h
@@ -1,9 +1,19 @@
 #ifndef __SR_GLOBAL_H__
 #define __SR_GLOBAL_H__
 
+#include <stdint.h>
+
 #define UNUSED(x) ((void) x)
 #define ARRAY_LENGTH(x) (sizeof(x) / sizeof(x[0]))
 
+#define unused_a __attribute__((unused))
+
 static inline uint16_t htole(uint16_t val) { return val; }
 
+void __libc_init_array(void);
+
+static inline void panic(void) {
+    asm volatile ("bkpt");
+}
+
 #endif /* __SR_GLOBAL_H__ */
diff --git a/controller/fw/src/startup_stm32f407xx.s b/controller/fw/src/startup_stm32f407xx.s
new file mode 100644
index 0000000..aeeeb22
--- /dev/null
+++ b/controller/fw/src/startup_stm32f407xx.s
@@ -0,0 +1,521 @@
+/**
+  ******************************************************************************
+  * @file      startup_stm32f407xx.s
+  * @author    MCD Application Team
+  * @brief     STM32F407xx Devices vector table for GCC based toolchains. 
+  *            This module performs:
+  *                - Set the initial SP
+  *                - Set the initial PC == Reset_Handler,
+  *                - Set the vector table entries with the exceptions ISR address
+  *                - Branches to main in the C library (which eventually
+  *                  calls main()).
+  *            After Reset the Cortex-M4 processor is in Thread mode,
+  *            priority is Privileged, and the Stack is set to Main.
+  ******************************************************************************
+  * @attention
+  *
+  * <h2><center>&copy; COPYRIGHT 2017 STMicroelectronics</center></h2>
+  *
+  * Redistribution and use in source and binary forms, with or without modification,
+  * are permitted provided that the following conditions are met:
+  *   1. Redistributions of source code must retain the above copyright notice,
+  *      this list of conditions and the following disclaimer.
+  *   2. 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.
+  *   3. Neither the name of STMicroelectronics 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.
+  *
+  ******************************************************************************
+  */
+    
+  .syntax unified
+  .cpu cortex-m4
+  .fpu softvfp
+  .thumb
+
+.global  g_pfnVectors
+.global  Default_Handler
+
+/* start address for the initialization values of the .data section. 
+defined in linker script */
+.word  _sidata
+/* start address for the .data section. defined in linker script */  
+.word  _sdata
+/* end address for the .data section. defined in linker script */
+.word  _edata
+/* start address for the .bss section. defined in linker script */
+.word  _sbss
+/* end address for the .bss section. defined in linker script */
+.word  _ebss
+/* stack used for SystemInit_ExtMemCtl; always internal RAM used */
+
+/**
+ * @brief  This is the code that gets called when the processor first
+ *          starts execution following a reset event. Only the absolutely
+ *          necessary set is performed, after which the application
+ *          supplied main() routine is called. 
+ * @param  None
+ * @retval : None
+*/
+
+    .section  .text.Reset_Handler
+  .weak  Reset_Handler
+  .type  Reset_Handler, %function
+Reset_Handler:  
+  ldr   sp, =_estack     /* set stack pointer */
+
+/* Copy the data segment initializers from flash to SRAM */  
+  movs  r1, #0
+  b  LoopCopyDataInit
+
+CopyDataInit:
+  ldr  r3, =_sidata
+  ldr  r3, [r3, r1]
+  str  r3, [r0, r1]
+  adds  r1, r1, #4
+    
+LoopCopyDataInit:
+  ldr  r0, =_sdata
+  ldr  r3, =_edata
+  adds  r2, r0, r1
+  cmp  r2, r3
+  bcc  CopyDataInit
+  ldr  r2, =_sbss
+  b  LoopFillZerobss
+/* Zero fill the bss segment. */  
+FillZerobss:
+  movs  r3, #0
+  str  r3, [r2], #4
+    
+LoopFillZerobss:
+  ldr  r3, = _ebss
+  cmp  r2, r3
+  bcc  FillZerobss
+
+/* Call the clock system intitialization function.*/
+  bl  SystemInit   
+/* Call static constructors */
+    bl __libc_init_array
+/* Call the application's entry point.*/
+  bl  main
+  bx  lr    
+.size  Reset_Handler, .-Reset_Handler
+
+/**
+ * @brief  This is the code that gets called when the processor receives an 
+ *         unexpected interrupt.  This simply enters an infinite loop, preserving
+ *         the system state for examination by a debugger.
+ * @param  None     
+ * @retval None       
+*/
+    .section  .text.Default_Handler,"ax",%progbits
+Default_Handler:
+Infinite_Loop:
+  b  Infinite_Loop
+  .size  Default_Handler, .-Default_Handler
+/******************************************************************************
+*
+* The minimal vector table for a Cortex M3. Note that the proper constructs
+* must be placed on this to ensure that it ends up at physical address
+* 0x0000.0000.
+* 
+*******************************************************************************/
+   .section  .isr_vector,"a",%progbits
+  .type  g_pfnVectors, %object
+  .size  g_pfnVectors, .-g_pfnVectors
+    
+    
+g_pfnVectors:
+  .word  _estack
+  .word  Reset_Handler
+  .word  NMI_Handler
+  .word  HardFault_Handler
+  .word  MemManage_Handler
+  .word  BusFault_Handler
+  .word  UsageFault_Handler
+  .word  0
+  .word  0
+  .word  0
+  .word  0
+  .word  SVC_Handler
+  .word  DebugMon_Handler
+  .word  0
+  .word  PendSV_Handler
+  .word  SysTick_Handler
+  
+  /* External Interrupts */
+  .word     WWDG_IRQHandler                   /* Window WatchDog              */                                        
+  .word     PVD_IRQHandler                    /* PVD through EXTI Line detection */                        
+  .word     TAMP_STAMP_IRQHandler             /* Tamper and TimeStamps through the EXTI line */            
+  .word     RTC_WKUP_IRQHandler               /* RTC Wakeup through the EXTI line */                      
+  .word     FLASH_IRQHandler                  /* FLASH                        */                                          
+  .word     RCC_IRQHandler                    /* RCC                          */                                            
+  .word     EXTI0_IRQHandler                  /* EXTI Line0                   */                        
+  .word     EXTI1_IRQHandler                  /* EXTI Line1                   */                          
+  .word     EXTI2_IRQHandler                  /* EXTI Line2                   */                          
+  .word     EXTI3_IRQHandler                  /* EXTI Line3                   */                          
+  .word     EXTI4_IRQHandler                  /* EXTI Line4                   */                          
+  .word     DMA1_Stream0_IRQHandler           /* DMA1 Stream 0                */                  
+  .word     DMA1_Stream1_IRQHandler           /* DMA1 Stream 1                */                   
+  .word     DMA1_Stream2_IRQHandler           /* DMA1 Stream 2                */                   
+  .word     DMA1_Stream3_IRQHandler           /* DMA1 Stream 3                */                   
+  .word     DMA1_Stream4_IRQHandler           /* DMA1 Stream 4                */                   
+  .word     DMA1_Stream5_IRQHandler           /* DMA1 Stream 5                */                   
+  .word     DMA1_Stream6_IRQHandler           /* DMA1 Stream 6                */                   
+  .word     ADC_IRQHandler                    /* ADC1, ADC2 and ADC3s         */                   
+  .word     CAN1_TX_IRQHandler                /* CAN1 TX                      */                         
+  .word     CAN1_RX0_IRQHandler               /* CAN1 RX0                     */                          
+  .word     CAN1_RX1_IRQHandler               /* CAN1 RX1                     */                          
+  .word     CAN1_SCE_IRQHandler               /* CAN1 SCE                     */                          
+  .word     EXTI9_5_IRQHandler                /* External Line[9:5]s          */                          
+  .word     TIM1_BRK_TIM9_IRQHandler          /* TIM1 Break and TIM9          */         
+  .word     TIM1_UP_TIM10_IRQHandler          /* TIM1 Update and TIM10        */         
+  .word     TIM1_TRG_COM_TIM11_IRQHandler     /* TIM1 Trigger and Commutation and TIM11 */
+  .word     TIM1_CC_IRQHandler                /* TIM1 Capture Compare         */                          
+  .word     TIM2_IRQHandler                   /* TIM2                         */                   
+  .word     TIM3_IRQHandler                   /* TIM3                         */                   
+  .word     TIM4_IRQHandler                   /* TIM4                         */                   
+  .word     I2C1_EV_IRQHandler                /* I2C1 Event                   */                          
+  .word     I2C1_ER_IRQHandler                /* I2C1 Error                   */                          
+  .word     I2C2_EV_IRQHandler                /* I2C2 Event                   */                          
+  .word     I2C2_ER_IRQHandler                /* I2C2 Error                   */                            
+  .word     SPI1_IRQHandler                   /* SPI1                         */                   
+  .word     SPI2_IRQHandler                   /* SPI2                         */                   
+  .word     USART1_IRQHandler                 /* USART1                       */                   
+  .word     USART2_IRQHandler                 /* USART2                       */                   
+  .word     USART3_IRQHandler                 /* USART3                       */                   
+  .word     EXTI15_10_IRQHandler              /* External Line[15:10]s        */                          
+  .word     RTC_Alarm_IRQHandler              /* RTC Alarm (A and B) through EXTI Line */                 
+  .word     OTG_FS_WKUP_IRQHandler            /* USB OTG FS Wakeup through EXTI line */                       
+  .word     TIM8_BRK_TIM12_IRQHandler         /* TIM8 Break and TIM12         */         
+  .word     TIM8_UP_TIM13_IRQHandler          /* TIM8 Update and TIM13        */         
+  .word     TIM8_TRG_COM_TIM14_IRQHandler     /* TIM8 Trigger and Commutation and TIM14 */
+  .word     TIM8_CC_IRQHandler                /* TIM8 Capture Compare         */                          
+  .word     DMA1_Stream7_IRQHandler           /* DMA1 Stream7                 */                          
+  .word     FSMC_IRQHandler                   /* FSMC                         */                   
+  .word     SDIO_IRQHandler                   /* SDIO                         */                   
+  .word     TIM5_IRQHandler                   /* TIM5                         */                   
+  .word     SPI3_IRQHandler                   /* SPI3                         */                   
+  .word     UART4_IRQHandler                  /* UART4                        */                   
+  .word     UART5_IRQHandler                  /* UART5                        */                   
+  .word     TIM6_DAC_IRQHandler               /* TIM6 and DAC1&2 underrun errors */                   
+  .word     TIM7_IRQHandler                   /* TIM7                         */
+  .word     DMA2_Stream0_IRQHandler           /* DMA2 Stream 0                */                   
+  .word     DMA2_Stream1_IRQHandler           /* DMA2 Stream 1                */                   
+  .word     DMA2_Stream2_IRQHandler           /* DMA2 Stream 2                */                   
+  .word     DMA2_Stream3_IRQHandler           /* DMA2 Stream 3                */                   
+  .word     DMA2_Stream4_IRQHandler           /* DMA2 Stream 4                */                   
+  .word     ETH_IRQHandler                    /* Ethernet                     */                   
+  .word     ETH_WKUP_IRQHandler               /* Ethernet Wakeup through EXTI line */                     
+  .word     CAN2_TX_IRQHandler                /* CAN2 TX                      */                          
+  .word     CAN2_RX0_IRQHandler               /* CAN2 RX0                     */                          
+  .word     CAN2_RX1_IRQHandler               /* CAN2 RX1                     */                          
+  .word     CAN2_SCE_IRQHandler               /* CAN2 SCE                     */                          
+  .word     OTG_FS_IRQHandler                 /* USB OTG FS                   */                   
+  .word     DMA2_Stream5_IRQHandler           /* DMA2 Stream 5                */                   
+  .word     DMA2_Stream6_IRQHandler           /* DMA2 Stream 6                */                   
+  .word     DMA2_Stream7_IRQHandler           /* DMA2 Stream 7                */                   
+  .word     USART6_IRQHandler                 /* USART6                       */                    
+  .word     I2C3_EV_IRQHandler                /* I2C3 event                   */                          
+  .word     I2C3_ER_IRQHandler                /* I2C3 error                   */                          
+  .word     OTG_HS_EP1_OUT_IRQHandler         /* USB OTG HS End Point 1 Out   */                   
+  .word     OTG_HS_EP1_IN_IRQHandler          /* USB OTG HS End Point 1 In    */                   
+  .word     OTG_HS_WKUP_IRQHandler            /* USB OTG HS Wakeup through EXTI */                         
+  .word     OTG_HS_IRQHandler                 /* USB OTG HS                   */                   
+  .word     DCMI_IRQHandler                   /* DCMI                         */                   
+  .word     0                                 /* CRYP crypto                  */                   
+  .word     HASH_RNG_IRQHandler               /* Hash and Rng                 */
+  .word     FPU_IRQHandler                    /* FPU                          */
+                         
+                         
+/*******************************************************************************
+*
+* Provide weak aliases for each Exception handler to the Default_Handler. 
+* As they are weak aliases, any function with the same name will override 
+* this definition.
+* 
+*******************************************************************************/
+   .weak      NMI_Handler
+   .thumb_set NMI_Handler,Default_Handler
+  
+   .weak      HardFault_Handler
+   .thumb_set HardFault_Handler,Default_Handler
+  
+   .weak      MemManage_Handler
+   .thumb_set MemManage_Handler,Default_Handler
+  
+   .weak      BusFault_Handler
+   .thumb_set BusFault_Handler,Default_Handler
+
+   .weak      UsageFault_Handler
+   .thumb_set UsageFault_Handler,Default_Handler
+
+   .weak      SVC_Handler
+   .thumb_set SVC_Handler,Default_Handler
+
+   .weak      DebugMon_Handler
+   .thumb_set DebugMon_Handler,Default_Handler
+
+   .weak      PendSV_Handler
+   .thumb_set PendSV_Handler,Default_Handler
+
+   .weak      SysTick_Handler
+   .thumb_set SysTick_Handler,Default_Handler              
+  
+   .weak      WWDG_IRQHandler                   
+   .thumb_set WWDG_IRQHandler,Default_Handler      
+                  
+   .weak      PVD_IRQHandler      
+   .thumb_set PVD_IRQHandler,Default_Handler
+               
+   .weak      TAMP_STAMP_IRQHandler            
+   .thumb_set TAMP_STAMP_IRQHandler,Default_Handler
+            
+   .weak      RTC_WKUP_IRQHandler                  
+   .thumb_set RTC_WKUP_IRQHandler,Default_Handler
+            
+   .weak      FLASH_IRQHandler         
+   .thumb_set FLASH_IRQHandler,Default_Handler
+                  
+   .weak      RCC_IRQHandler      
+   .thumb_set RCC_IRQHandler,Default_Handler
+                  
+   .weak      EXTI0_IRQHandler         
+   .thumb_set EXTI0_IRQHandler,Default_Handler
+                  
+   .weak      EXTI1_IRQHandler         
+   .thumb_set EXTI1_IRQHandler,Default_Handler
+                     
+   .weak      EXTI2_IRQHandler         
+   .thumb_set EXTI2_IRQHandler,Default_Handler 
+                 
+   .weak      EXTI3_IRQHandler         
+   .thumb_set EXTI3_IRQHandler,Default_Handler
+                        
+   .weak      EXTI4_IRQHandler         
+   .thumb_set EXTI4_IRQHandler,Default_Handler
+                  
+   .weak      DMA1_Stream0_IRQHandler               
+   .thumb_set DMA1_Stream0_IRQHandler,Default_Handler
+         
+   .weak      DMA1_Stream1_IRQHandler               
+   .thumb_set DMA1_Stream1_IRQHandler,Default_Handler
+                  
+   .weak      DMA1_Stream2_IRQHandler               
+   .thumb_set DMA1_Stream2_IRQHandler,Default_Handler
+                  
+   .weak      DMA1_Stream3_IRQHandler               
+   .thumb_set DMA1_Stream3_IRQHandler,Default_Handler 
+                 
+   .weak      DMA1_Stream4_IRQHandler              
+   .thumb_set DMA1_Stream4_IRQHandler,Default_Handler
+                  
+   .weak      DMA1_Stream5_IRQHandler               
+   .thumb_set DMA1_Stream5_IRQHandler,Default_Handler
+                  
+   .weak      DMA1_Stream6_IRQHandler               
+   .thumb_set DMA1_Stream6_IRQHandler,Default_Handler
+                  
+   .weak      ADC_IRQHandler      
+   .thumb_set ADC_IRQHandler,Default_Handler
+               
+   .weak      CAN1_TX_IRQHandler   
+   .thumb_set CAN1_TX_IRQHandler,Default_Handler
+            
+   .weak      CAN1_RX0_IRQHandler                  
+   .thumb_set CAN1_RX0_IRQHandler,Default_Handler
+                           
+   .weak      CAN1_RX1_IRQHandler                  
+   .thumb_set CAN1_RX1_IRQHandler,Default_Handler
+            
+   .weak      CAN1_SCE_IRQHandler                  
+   .thumb_set CAN1_SCE_IRQHandler,Default_Handler
+            
+   .weak      EXTI9_5_IRQHandler   
+   .thumb_set EXTI9_5_IRQHandler,Default_Handler
+            
+   .weak      TIM1_BRK_TIM9_IRQHandler            
+   .thumb_set TIM1_BRK_TIM9_IRQHandler,Default_Handler
+            
+   .weak      TIM1_UP_TIM10_IRQHandler            
+   .thumb_set TIM1_UP_TIM10_IRQHandler,Default_Handler
+      
+   .weak      TIM1_TRG_COM_TIM11_IRQHandler      
+   .thumb_set TIM1_TRG_COM_TIM11_IRQHandler,Default_Handler
+      
+   .weak      TIM1_CC_IRQHandler   
+   .thumb_set TIM1_CC_IRQHandler,Default_Handler
+                  
+   .weak      TIM2_IRQHandler            
+   .thumb_set TIM2_IRQHandler,Default_Handler
+                  
+   .weak      TIM3_IRQHandler            
+   .thumb_set TIM3_IRQHandler,Default_Handler
+                  
+   .weak      TIM4_IRQHandler            
+   .thumb_set TIM4_IRQHandler,Default_Handler
+                  
+   .weak      I2C1_EV_IRQHandler   
+   .thumb_set I2C1_EV_IRQHandler,Default_Handler
+                     
+   .weak      I2C1_ER_IRQHandler   
+   .thumb_set I2C1_ER_IRQHandler,Default_Handler
+                     
+   .weak      I2C2_EV_IRQHandler   
+   .thumb_set I2C2_EV_IRQHandler,Default_Handler
+                  
+   .weak      I2C2_ER_IRQHandler   
+   .thumb_set I2C2_ER_IRQHandler,Default_Handler
+                           
+   .weak      SPI1_IRQHandler            
+   .thumb_set SPI1_IRQHandler,Default_Handler
+                        
+   .weak      SPI2_IRQHandler            
+   .thumb_set SPI2_IRQHandler,Default_Handler
+                  
+   .weak      USART1_IRQHandler      
+   .thumb_set USART1_IRQHandler,Default_Handler
+                     
+   .weak      USART2_IRQHandler      
+   .thumb_set USART2_IRQHandler,Default_Handler
+                     
+   .weak      USART3_IRQHandler      
+   .thumb_set USART3_IRQHandler,Default_Handler
+                  
+   .weak      EXTI15_10_IRQHandler               
+   .thumb_set EXTI15_10_IRQHandler,Default_Handler
+               
+   .weak      RTC_Alarm_IRQHandler               
+   .thumb_set RTC_Alarm_IRQHandler,Default_Handler
+            
+   .weak      OTG_FS_WKUP_IRQHandler         
+   .thumb_set OTG_FS_WKUP_IRQHandler,Default_Handler
+            
+   .weak      TIM8_BRK_TIM12_IRQHandler         
+   .thumb_set TIM8_BRK_TIM12_IRQHandler,Default_Handler
+         
+   .weak      TIM8_UP_TIM13_IRQHandler            
+   .thumb_set TIM8_UP_TIM13_IRQHandler,Default_Handler
+         
+   .weak      TIM8_TRG_COM_TIM14_IRQHandler      
+   .thumb_set TIM8_TRG_COM_TIM14_IRQHandler,Default_Handler
+      
+   .weak      TIM8_CC_IRQHandler   
+   .thumb_set TIM8_CC_IRQHandler,Default_Handler
+                  
+   .weak      DMA1_Stream7_IRQHandler               
+   .thumb_set DMA1_Stream7_IRQHandler,Default_Handler
+                     
+   .weak      FSMC_IRQHandler            
+   .thumb_set FSMC_IRQHandler,Default_Handler
+                     
+   .weak      SDIO_IRQHandler            
+   .thumb_set SDIO_IRQHandler,Default_Handler
+                     
+   .weak      TIM5_IRQHandler            
+   .thumb_set TIM5_IRQHandler,Default_Handler
+                     
+   .weak      SPI3_IRQHandler            
+   .thumb_set SPI3_IRQHandler,Default_Handler
+                     
+   .weak      UART4_IRQHandler         
+   .thumb_set UART4_IRQHandler,Default_Handler
+                  
+   .weak      UART5_IRQHandler         
+   .thumb_set UART5_IRQHandler,Default_Handler
+                  
+   .weak      TIM6_DAC_IRQHandler                  
+   .thumb_set TIM6_DAC_IRQHandler,Default_Handler
+               
+   .weak      TIM7_IRQHandler            
+   .thumb_set TIM7_IRQHandler,Default_Handler
+         
+   .weak      DMA2_Stream0_IRQHandler               
+   .thumb_set DMA2_Stream0_IRQHandler,Default_Handler
+               
+   .weak      DMA2_Stream1_IRQHandler               
+   .thumb_set DMA2_Stream1_IRQHandler,Default_Handler
+                  
+   .weak      DMA2_Stream2_IRQHandler               
+   .thumb_set DMA2_Stream2_IRQHandler,Default_Handler
+            
+   .weak      DMA2_Stream3_IRQHandler               
+   .thumb_set DMA2_Stream3_IRQHandler,Default_Handler
+            
+   .weak      DMA2_Stream4_IRQHandler               
+   .thumb_set DMA2_Stream4_IRQHandler,Default_Handler
+            
+   .weak      ETH_IRQHandler      
+   .thumb_set ETH_IRQHandler,Default_Handler
+                  
+   .weak      ETH_WKUP_IRQHandler                  
+   .thumb_set ETH_WKUP_IRQHandler,Default_Handler
+            
+   .weak      CAN2_TX_IRQHandler   
+   .thumb_set CAN2_TX_IRQHandler,Default_Handler
+                           
+   .weak      CAN2_RX0_IRQHandler                  
+   .thumb_set CAN2_RX0_IRQHandler,Default_Handler
+                           
+   .weak      CAN2_RX1_IRQHandler                  
+   .thumb_set CAN2_RX1_IRQHandler,Default_Handler
+                           
+   .weak      CAN2_SCE_IRQHandler                  
+   .thumb_set CAN2_SCE_IRQHandler,Default_Handler
+                           
+   .weak      OTG_FS_IRQHandler      
+   .thumb_set OTG_FS_IRQHandler,Default_Handler
+                     
+   .weak      DMA2_Stream5_IRQHandler               
+   .thumb_set DMA2_Stream5_IRQHandler,Default_Handler
+                  
+   .weak      DMA2_Stream6_IRQHandler               
+   .thumb_set DMA2_Stream6_IRQHandler,Default_Handler
+                  
+   .weak      DMA2_Stream7_IRQHandler               
+   .thumb_set DMA2_Stream7_IRQHandler,Default_Handler
+                  
+   .weak      USART6_IRQHandler      
+   .thumb_set USART6_IRQHandler,Default_Handler
+                        
+   .weak      I2C3_EV_IRQHandler   
+   .thumb_set I2C3_EV_IRQHandler,Default_Handler
+                        
+   .weak      I2C3_ER_IRQHandler   
+   .thumb_set I2C3_ER_IRQHandler,Default_Handler
+                        
+   .weak      OTG_HS_EP1_OUT_IRQHandler         
+   .thumb_set OTG_HS_EP1_OUT_IRQHandler,Default_Handler
+               
+   .weak      OTG_HS_EP1_IN_IRQHandler            
+   .thumb_set OTG_HS_EP1_IN_IRQHandler,Default_Handler
+               
+   .weak      OTG_HS_WKUP_IRQHandler         
+   .thumb_set OTG_HS_WKUP_IRQHandler,Default_Handler
+            
+   .weak      OTG_HS_IRQHandler      
+   .thumb_set OTG_HS_IRQHandler,Default_Handler
+                  
+   .weak      DCMI_IRQHandler            
+   .thumb_set DCMI_IRQHandler,Default_Handler
+                                   
+   .weak      HASH_RNG_IRQHandler                  
+   .thumb_set HASH_RNG_IRQHandler,Default_Handler   
+
+   .weak      FPU_IRQHandler                  
+   .thumb_set FPU_IRQHandler,Default_Handler  
+
+/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
diff --git a/controller/fw/src/stm32f4_isr.h b/controller/fw/src/stm32f4_isr.h
new file mode 100644
index 0000000..bc16421
--- /dev/null
+++ b/controller/fw/src/stm32f4_isr.h
@@ -0,0 +1,98 @@
+
+#ifndef __STM32F4_ISR_H__
+#define __STM32F4_ISR_H__
+
+void Reset_Handler(void);
+void NMI_Handler(void);
+void HardFault_Handler(void);
+void MemManage_Handler(void);
+void BusFault_Handler(void);
+void UsageFault_Handler(void);
+void SVC_Handler(void);
+void DebugMon_Handler(void);
+void PendSV_Handler(void);
+void SysTick_Handler(void);
+
+void WWDG_IRQHandler(void);
+void PVD_IRQHandler(void);
+void TAMP_STAMP_IRQHandler(void);
+void RTC_WKUP_IRQHandler(void);
+void FLASH_IRQHandler(void);
+void RCC_IRQHandler(void);
+void EXTI0_IRQHandler(void);
+void EXTI1_IRQHandler(void);
+void EXTI2_IRQHandler(void);
+void EXTI3_IRQHandler(void);
+void EXTI4_IRQHandler(void);
+void DMA1_Stream0_IRQHandler(void);
+void DMA1_Stream1_IRQHandler(void);
+void DMA1_Stream2_IRQHandler(void);
+void DMA1_Stream3_IRQHandler(void);
+void DMA1_Stream4_IRQHandler(void);
+void DMA1_Stream5_IRQHandler(void);
+void DMA1_Stream6_IRQHandler(void);
+void ADC_IRQHandler(void);
+void CAN1_TX_IRQHandler(void);
+void CAN1_RX0_IRQHandler(void);
+void CAN1_RX1_IRQHandler(void);
+void CAN1_SCE_IRQHandler(void);
+void EXTI9_5_IRQHandler(void);
+void TIM1_BRK_TIM9_IRQHandler(void);
+void TIM1_UP_TIM10_IRQHandler(void);
+void TIM1_TRG_COM_TIM11_IRQHandler(void);
+void TIM1_CC_IRQHandler(void);
+void TIM2_IRQHandler(void);
+void TIM3_IRQHandler(void);
+void TIM4_IRQHandler(void);
+void I2C1_EV_IRQHandler(void);
+void I2C1_ER_IRQHandler(void);
+void I2C2_EV_IRQHandler(void);
+void I2C2_ER_IRQHandler(void);
+void SPI1_IRQHandler(void);
+void SPI2_IRQHandler(void);
+void USART1_IRQHandler(void);
+void USART2_IRQHandler(void);
+void USART3_IRQHandler(void);
+void EXTI15_10_IRQHandler(void);
+void RTC_Alarm_IRQHandler(void);
+void OTG_FS_WKUP_IRQHandler(void);
+void TIM8_BRK_TIM12_IRQHandler(void);
+void TIM8_UP_TIM13_IRQHandler(void);
+void TIM8_TRG_COM_TIM14_IRQHandler(void);
+void TIM8_CC_IRQHandler(void);
+void DMA1_Stream7_IRQHandler(void);
+void FSMC_IRQHandler(void);
+void SDIO_IRQHandler(void);
+void TIM5_IRQHandler(void);
+void SPI3_IRQHandler(void);
+void UART4_IRQHandler(void);
+void UART5_IRQHandler(void);
+void TIM6_DAC_IRQHandler(void);
+void TIM7_IRQHandler(void);
+void DMA2_Stream0_IRQHandler(void);
+void DMA2_Stream1_IRQHandler(void);
+void DMA2_Stream2_IRQHandler(void);
+void DMA2_Stream3_IRQHandler(void);
+void DMA2_Stream4_IRQHandler(void);
+void ETH_IRQHandler(void);
+void ETH_WKUP_IRQHandler(void);
+void CAN2_TX_IRQHandler(void);
+void CAN2_RX0_IRQHandler(void);
+void CAN2_RX1_IRQHandler(void);
+void CAN2_SCE_IRQHandler(void);
+void OTG_FS_IRQHandler(void);
+void DMA2_Stream5_IRQHandler(void);
+void DMA2_Stream6_IRQHandler(void);
+void DMA2_Stream7_IRQHandler(void);
+void USART6_IRQHandler(void);
+void I2C3_EV_IRQHandler(void);
+void I2C3_ER_IRQHandler(void);
+void OTG_HS_EP1_OUT_IRQHandler(void);
+void OTG_HS_EP1_IN_IRQHandler(void);
+void OTG_HS_WKUP_IRQHandler(void);
+void OTG_HS_IRQHandler(void);
+void DCMI_IRQHandler(void);
+void HASH_RNG_IRQHandler(void);
+void FPU_IRQHandler(void);
+
+#endif /* __STM32F4_ISR_H__ */
diff --git a/controller/fw/src/system_stm32f4xx.c b/controller/fw/src/system_stm32f4xx.c
new file mode 100644
index 0000000..9037604
--- /dev/null
+++ b/controller/fw/src/system_stm32f4xx.c
@@ -0,0 +1,743 @@
+/**
+  ******************************************************************************
+  * @file    system_stm32f4xx.c
+  * @author  MCD Application Team
+  * @brief   CMSIS Cortex-M4 Device Peripheral Access Layer System Source File.
+  *
+  *   This file provides two functions and one global variable to be called from 
+  *   user application:
+  *      - SystemInit(): This function is called at startup just after reset and 
+  *                      before branch to main program. This call is made inside
+  *                      the "startup_stm32f4xx.s" file.
+  *
+  *      - SystemCoreClock variable: Contains the core clock (HCLK), it can be used
+  *                                  by the user application to setup the SysTick 
+  *                                  timer or configure other parameters.
+  *                                     
+  *      - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must
+  *                                 be called whenever the core clock is changed
+  *                                 during program execution.
+  *
+  *
+  ******************************************************************************
+  * @attention
+  *
+  * <h2><center>&copy; COPYRIGHT 2017 STMicroelectronics</center></h2>
+  *
+  * Redistribution and use in source and binary forms, with or without modification,
+  * are permitted provided that the following conditions are met:
+  *   1. Redistributions of source code must retain the above copyright notice,
+  *      this list of conditions and the following disclaimer.
+  *   2. 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.
+  *   3. Neither the name of STMicroelectronics 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.
+  *
+  ******************************************************************************
+  */
+
+/** @addtogroup CMSIS
+  * @{
+  */
+
+/** @addtogroup stm32f4xx_system
+  * @{
+  */  
+  
+/** @addtogroup STM32F4xx_System_Private_Includes
+  * @{
+  */
+
+
+#include "stm32f4xx.h"
+
+#if !defined  (HSE_VALUE) 
+  #define HSE_VALUE    ((uint32_t)25000000) /*!< Default value of the External oscillator in Hz */
+#endif /* HSE_VALUE */
+
+#if !defined  (HSI_VALUE)
+  #define HSI_VALUE    ((uint32_t)16000000) /*!< Value of the Internal oscillator in Hz*/
+#endif /* HSI_VALUE */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32F4xx_System_Private_TypesDefinitions
+  * @{
+  */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32F4xx_System_Private_Defines
+  * @{
+  */
+
+/************************* Miscellaneous Configuration ************************/
+/*!< Uncomment the following line if you need to use external SRAM or SDRAM as data memory  */
+#if defined(STM32F405xx) || defined(STM32F415xx) || defined(STM32F407xx) || defined(STM32F417xx)\
+ || defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)\
+ || defined(STM32F469xx) || defined(STM32F479xx) || defined(STM32F412Zx) || defined(STM32F412Vx)
+/* #define DATA_IN_ExtSRAM */
+#endif /* STM32F40xxx || STM32F41xxx || STM32F42xxx || STM32F43xxx || STM32F469xx || STM32F479xx ||\
+          STM32F412Zx || STM32F412Vx */
+ 
+#if defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)\
+ || defined(STM32F446xx) || defined(STM32F469xx) || defined(STM32F479xx)
+/* #define DATA_IN_ExtSDRAM */
+#endif /* STM32F427xx || STM32F437xx || STM32F429xx || STM32F439xx || STM32F446xx || STM32F469xx ||\
+          STM32F479xx */
+
+/*!< Uncomment the following line if you need to relocate your vector Table in
+     Internal SRAM. */
+/* #define VECT_TAB_SRAM */
+#define VECT_TAB_OFFSET  0x00 /*!< Vector Table base offset field. 
+                                   This value must be a multiple of 0x200. */
+/******************************************************************************/
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32F4xx_System_Private_Macros
+  * @{
+  */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32F4xx_System_Private_Variables
+  * @{
+  */
+  /* This variable is updated in three ways:
+      1) by calling CMSIS function SystemCoreClockUpdate()
+      2) by calling HAL API function HAL_RCC_GetHCLKFreq()
+      3) each time HAL_RCC_ClockConfig() is called to configure the system clock frequency 
+         Note: If you use this function to configure the system clock; then there
+               is no need to call the 2 first functions listed above, since SystemCoreClock
+               variable is updated automatically.
+  */
+uint32_t SystemCoreClock = 16000000;
+const uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9};
+const uint8_t APBPrescTable[8]  = {0, 0, 0, 0, 1, 2, 3, 4};
+/**
+  * @}
+  */
+
+/** @addtogroup STM32F4xx_System_Private_FunctionPrototypes
+  * @{
+  */
+
+#if defined (DATA_IN_ExtSRAM) || defined (DATA_IN_ExtSDRAM)
+  static void SystemInit_ExtMemCtl(void); 
+#endif /* DATA_IN_ExtSRAM || DATA_IN_ExtSDRAM */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32F4xx_System_Private_Functions
+  * @{
+  */
+
+/**
+  * @brief  Setup the microcontroller system
+  *         Initialize the FPU setting, vector table location and External memory 
+  *         configuration.
+  * @param  None
+  * @retval None
+  */
+void SystemInit(void)
+{
+  /* FPU settings ------------------------------------------------------------*/
+  #if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
+    SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2));  /* set CP10 and CP11 Full Access */
+  #endif
+
+#if defined (DATA_IN_ExtSRAM) || defined (DATA_IN_ExtSDRAM)
+  SystemInit_ExtMemCtl(); 
+#endif /* DATA_IN_ExtSRAM || DATA_IN_ExtSDRAM */
+
+  /* Configure the Vector Table location add offset address ------------------*/
+#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 */
+#endif
+}
+
+/**
+   * @brief  Update SystemCoreClock variable according to Clock Register Values.
+  *         The SystemCoreClock variable contains the core clock (HCLK), it can
+  *         be used by the user application to setup the SysTick timer or configure
+  *         other parameters.
+  *           
+  * @note   Each time the core clock (HCLK) changes, this function must be called
+  *         to update SystemCoreClock variable value. Otherwise, any configuration
+  *         based on this variable will be incorrect.         
+  *     
+  * @note   - The system frequency computed by this function is not the real 
+  *           frequency in the chip. It is calculated based on the predefined 
+  *           constant and the selected clock source:
+  *             
+  *           - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(*)
+  *                                              
+  *           - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(**)
+  *                          
+  *           - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(**) 
+  *             or HSI_VALUE(*) multiplied/divided by the PLL factors.
+  *         
+  *         (*) HSI_VALUE is a constant defined in stm32f4xx_hal_conf.h file (default value
+  *             16 MHz) but the real value may vary depending on the variations
+  *             in voltage and temperature.   
+  *    
+  *         (**) HSE_VALUE is a constant defined in stm32f4xx_hal_conf.h file (its value
+  *              depends on the application requirements), user has to ensure that HSE_VALUE
+  *              is same as the real frequency of the crystal used. Otherwise, this function
+  *              may have wrong result.
+  *                
+  *         - The result of this function could be not correct when using fractional
+  *           value for HSE crystal.
+  *     
+  * @param  None
+  * @retval None
+  */
+void SystemCoreClockUpdate(void)
+{
+  uint32_t tmp = 0, pllvco = 0, pllp = 2, pllsource = 0, pllm = 2;
+  
+  /* Get SYSCLK source -------------------------------------------------------*/
+  tmp = RCC->CFGR & RCC_CFGR_SWS;
+
+  switch (tmp)
+  {
+    case 0x00:  /* HSI used as system clock source */
+      SystemCoreClock = HSI_VALUE;
+      break;
+    case 0x04:  /* HSE used as system clock source */
+      SystemCoreClock = HSE_VALUE;
+      break;
+    case 0x08:  /* PLL used as system clock source */
+
+      /* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLL_M) * PLL_N
+         SYSCLK = PLL_VCO / PLL_P
+         */    
+      pllsource = (RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC) >> 22;
+      pllm = RCC->PLLCFGR & RCC_PLLCFGR_PLLM;
+      
+      if (pllsource != 0)
+      {
+        /* HSE used as PLL clock source */
+        pllvco = (HSE_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6);
+      }
+      else
+      {
+        /* HSI used as PLL clock source */
+        pllvco = (HSI_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6);
+      }
+
+      pllp = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLP) >>16) + 1 ) *2;
+      SystemCoreClock = pllvco/pllp;
+      break;
+    default:
+      SystemCoreClock = HSI_VALUE;
+      break;
+  }
+  /* Compute HCLK frequency --------------------------------------------------*/
+  /* Get HCLK prescaler */
+  tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)];
+  /* HCLK frequency */
+  SystemCoreClock >>= tmp;
+}
+
+#if defined (DATA_IN_ExtSRAM) && defined (DATA_IN_ExtSDRAM)
+#if defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)\
+ || defined(STM32F469xx) || defined(STM32F479xx)
+/**
+  * @brief  Setup the external memory controller.
+  *         Called in startup_stm32f4xx.s before jump to main.
+  *         This function configures the external memories (SRAM/SDRAM)
+  *         This SRAM/SDRAM will be used as program data memory (including heap and stack).
+  * @param  None
+  * @retval None
+  */
+void SystemInit_ExtMemCtl(void)
+{
+  __IO uint32_t tmp = 0x00;
+
+  register uint32_t tmpreg = 0, timeout = 0xFFFF;
+  register __IO uint32_t index;
+
+  /* Enable GPIOC, GPIOD, GPIOE, GPIOF, GPIOG, GPIOH and GPIOI interface clock */
+  RCC->AHB1ENR |= 0x000001F8;
+
+  /* Delay after an RCC peripheral clock enabling */
+  tmp = READ_BIT(RCC->AHB1ENR, RCC_AHB1ENR_GPIOCEN);
+  
+  /* Connect PDx pins to FMC Alternate function */
+  GPIOD->AFR[0]  = 0x00CCC0CC;
+  GPIOD->AFR[1]  = 0xCCCCCCCC;
+  /* Configure PDx pins in Alternate function mode */  
+  GPIOD->MODER   = 0xAAAA0A8A;
+  /* Configure PDx pins speed to 100 MHz */  
+  GPIOD->OSPEEDR = 0xFFFF0FCF;
+  /* Configure PDx pins Output type to push-pull */  
+  GPIOD->OTYPER  = 0x00000000;
+  /* No pull-up, pull-down for PDx pins */ 
+  GPIOD->PUPDR   = 0x00000000;
+
+  /* Connect PEx pins to FMC Alternate function */
+  GPIOE->AFR[0]  = 0xC00CC0CC;
+  GPIOE->AFR[1]  = 0xCCCCCCCC;
+  /* Configure PEx pins in Alternate function mode */ 
+  GPIOE->MODER   = 0xAAAA828A;
+  /* Configure PEx pins speed to 100 MHz */ 
+  GPIOE->OSPEEDR = 0xFFFFC3CF;
+  /* Configure PEx pins Output type to push-pull */  
+  GPIOE->OTYPER  = 0x00000000;
+  /* No pull-up, pull-down for PEx pins */ 
+  GPIOE->PUPDR   = 0x00000000;
+  
+  /* Connect PFx pins to FMC Alternate function */
+  GPIOF->AFR[0]  = 0xCCCCCCCC;
+  GPIOF->AFR[1]  = 0xCCCCCCCC;
+  /* Configure PFx pins in Alternate function mode */   
+  GPIOF->MODER   = 0xAA800AAA;
+  /* Configure PFx pins speed to 50 MHz */ 
+  GPIOF->OSPEEDR = 0xAA800AAA;
+  /* Configure PFx pins Output type to push-pull */  
+  GPIOF->OTYPER  = 0x00000000;
+  /* No pull-up, pull-down for PFx pins */ 
+  GPIOF->PUPDR   = 0x00000000;
+
+  /* Connect PGx pins to FMC Alternate function */
+  GPIOG->AFR[0]  = 0xCCCCCCCC;
+  GPIOG->AFR[1]  = 0xCCCCCCCC;
+  /* Configure PGx pins in Alternate function mode */ 
+  GPIOG->MODER   = 0xAAAAAAAA;
+  /* Configure PGx pins speed to 50 MHz */ 
+  GPIOG->OSPEEDR = 0xAAAAAAAA;
+  /* Configure PGx pins Output type to push-pull */  
+  GPIOG->OTYPER  = 0x00000000;
+  /* No pull-up, pull-down for PGx pins */ 
+  GPIOG->PUPDR   = 0x00000000;
+  
+  /* Connect PHx pins to FMC Alternate function */
+  GPIOH->AFR[0]  = 0x00C0CC00;
+  GPIOH->AFR[1]  = 0xCCCCCCCC;
+  /* Configure PHx pins in Alternate function mode */ 
+  GPIOH->MODER   = 0xAAAA08A0;
+  /* Configure PHx pins speed to 50 MHz */ 
+  GPIOH->OSPEEDR = 0xAAAA08A0;
+  /* Configure PHx pins Output type to push-pull */  
+  GPIOH->OTYPER  = 0x00000000;
+  /* No pull-up, pull-down for PHx pins */ 
+  GPIOH->PUPDR   = 0x00000000;
+  
+  /* Connect PIx pins to FMC Alternate function */
+  GPIOI->AFR[0]  = 0xCCCCCCCC;
+  GPIOI->AFR[1]  = 0x00000CC0;
+  /* Configure PIx pins in Alternate function mode */ 
+  GPIOI->MODER   = 0x0028AAAA;
+  /* Configure PIx pins speed to 50 MHz */ 
+  GPIOI->OSPEEDR = 0x0028AAAA;
+  /* Configure PIx pins Output type to push-pull */  
+  GPIOI->OTYPER  = 0x00000000;
+  /* No pull-up, pull-down for PIx pins */ 
+  GPIOI->PUPDR   = 0x00000000;
+  
+/*-- FMC Configuration -------------------------------------------------------*/
+  /* Enable the FMC interface clock */
+  RCC->AHB3ENR |= 0x00000001;
+  /* Delay after an RCC peripheral clock enabling */
+  tmp = READ_BIT(RCC->AHB3ENR, RCC_AHB3ENR_FMCEN);
+
+  FMC_Bank5_6->SDCR[0] = 0x000019E4;
+  FMC_Bank5_6->SDTR[0] = 0x01115351;      
+  
+  /* SDRAM initialization sequence */
+  /* Clock enable command */
+  FMC_Bank5_6->SDCMR = 0x00000011; 
+  tmpreg = FMC_Bank5_6->SDSR & 0x00000020; 
+  while((tmpreg != 0) && (timeout-- > 0))
+  {
+    tmpreg = FMC_Bank5_6->SDSR & 0x00000020; 
+  }
+
+  /* Delay */
+  for (index = 0; index<1000; index++);
+  
+  /* PALL command */
+  FMC_Bank5_6->SDCMR = 0x00000012;           
+  timeout = 0xFFFF;
+  while((tmpreg != 0) && (timeout-- > 0))
+  {
+    tmpreg = FMC_Bank5_6->SDSR & 0x00000020; 
+  }
+  
+  /* Auto refresh command */
+  FMC_Bank5_6->SDCMR = 0x00000073;
+  timeout = 0xFFFF;
+  while((tmpreg != 0) && (timeout-- > 0))
+  {
+    tmpreg = FMC_Bank5_6->SDSR & 0x00000020; 
+  }
+ 
+  /* MRD register program */
+  FMC_Bank5_6->SDCMR = 0x00046014;
+  timeout = 0xFFFF;
+  while((tmpreg != 0) && (timeout-- > 0))
+  {
+    tmpreg = FMC_Bank5_6->SDSR & 0x00000020; 
+  } 
+  
+  /* Set refresh count */
+  tmpreg = FMC_Bank5_6->SDRTR;
+  FMC_Bank5_6->SDRTR = (tmpreg | (0x0000027C<<1));
+  
+  /* Disable write protection */
+  tmpreg = FMC_Bank5_6->SDCR[0]; 
+  FMC_Bank5_6->SDCR[0] = (tmpreg & 0xFFFFFDFF);
+
+#if defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)
+  /* Configure and enable Bank1_SRAM2 */
+  FMC_Bank1->BTCR[2]  = 0x00001011;
+  FMC_Bank1->BTCR[3]  = 0x00000201;
+  FMC_Bank1E->BWTR[2] = 0x0fffffff;
+#endif /* STM32F427xx || STM32F437xx || STM32F429xx || STM32F439xx */ 
+#if defined(STM32F469xx) || defined(STM32F479xx)
+  /* Configure and enable Bank1_SRAM2 */
+  FMC_Bank1->BTCR[2]  = 0x00001091;
+  FMC_Bank1->BTCR[3]  = 0x00110212;
+  FMC_Bank1E->BWTR[2] = 0x0fffffff;
+#endif /* STM32F469xx || STM32F479xx */
+
+  (void)(tmp); 
+}
+#endif /* STM32F427xx || STM32F437xx || STM32F429xx || STM32F439xx || STM32F469xx || STM32F479xx */
+#elif defined (DATA_IN_ExtSRAM) || defined (DATA_IN_ExtSDRAM)
+/**
+  * @brief  Setup the external memory controller.
+  *         Called in startup_stm32f4xx.s before jump to main.
+  *         This function configures the external memories (SRAM/SDRAM)
+  *         This SRAM/SDRAM will be used as program data memory (including heap and stack).
+  * @param  None
+  * @retval None
+  */
+void SystemInit_ExtMemCtl(void)
+{
+  __IO uint32_t tmp = 0x00;
+#if defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)\
+ || defined(STM32F446xx) || defined(STM32F469xx) || defined(STM32F479xx)
+#if defined (DATA_IN_ExtSDRAM)
+  register uint32_t tmpreg = 0, timeout = 0xFFFF;
+  register __IO uint32_t index;
+
+#if defined(STM32F446xx)
+  /* Enable GPIOA, GPIOC, GPIOD, GPIOE, GPIOF, GPIOG interface
+      clock */
+  RCC->AHB1ENR |= 0x0000007D;
+#else
+  /* Enable GPIOC, GPIOD, GPIOE, GPIOF, GPIOG, GPIOH and GPIOI interface 
+      clock */
+  RCC->AHB1ENR |= 0x000001F8;
+#endif /* STM32F446xx */  
+  /* Delay after an RCC peripheral clock enabling */
+  tmp = READ_BIT(RCC->AHB1ENR, RCC_AHB1ENR_GPIOCEN);
+  
+#if defined(STM32F446xx)
+  /* Connect PAx pins to FMC Alternate function */
+  GPIOA->AFR[0]  |= 0xC0000000;
+  GPIOA->AFR[1]  |= 0x00000000;
+  /* Configure PDx pins in Alternate function mode */
+  GPIOA->MODER   |= 0x00008000;
+  /* Configure PDx pins speed to 50 MHz */
+  GPIOA->OSPEEDR |= 0x00008000;
+  /* Configure PDx pins Output type to push-pull */
+  GPIOA->OTYPER  |= 0x00000000;
+  /* No pull-up, pull-down for PDx pins */
+  GPIOA->PUPDR   |= 0x00000000;
+
+  /* Connect PCx pins to FMC Alternate function */
+  GPIOC->AFR[0]  |= 0x00CC0000;
+  GPIOC->AFR[1]  |= 0x00000000;
+  /* Configure PDx pins in Alternate function mode */
+  GPIOC->MODER   |= 0x00000A00;
+  /* Configure PDx pins speed to 50 MHz */
+  GPIOC->OSPEEDR |= 0x00000A00;
+  /* Configure PDx pins Output type to push-pull */
+  GPIOC->OTYPER  |= 0x00000000;
+  /* No pull-up, pull-down for PDx pins */
+  GPIOC->PUPDR   |= 0x00000000;
+#endif /* STM32F446xx */
+
+  /* Connect PDx pins to FMC Alternate function */
+  GPIOD->AFR[0]  = 0x000000CC;
+  GPIOD->AFR[1]  = 0xCC000CCC;
+  /* Configure PDx pins in Alternate function mode */  
+  GPIOD->MODER   = 0xA02A000A;
+  /* Configure PDx pins speed to 50 MHz */  
+  GPIOD->OSPEEDR = 0xA02A000A;
+  /* Configure PDx pins Output type to push-pull */  
+  GPIOD->OTYPER  = 0x00000000;
+  /* No pull-up, pull-down for PDx pins */ 
+  GPIOD->PUPDR   = 0x00000000;
+
+  /* Connect PEx pins to FMC Alternate function */
+  GPIOE->AFR[0]  = 0xC00000CC;
+  GPIOE->AFR[1]  = 0xCCCCCCCC;
+  /* Configure PEx pins in Alternate function mode */ 
+  GPIOE->MODER   = 0xAAAA800A;
+  /* Configure PEx pins speed to 50 MHz */ 
+  GPIOE->OSPEEDR = 0xAAAA800A;
+  /* Configure PEx pins Output type to push-pull */  
+  GPIOE->OTYPER  = 0x00000000;
+  /* No pull-up, pull-down for PEx pins */ 
+  GPIOE->PUPDR   = 0x00000000;
+
+  /* Connect PFx pins to FMC Alternate function */
+  GPIOF->AFR[0]  = 0xCCCCCCCC;
+  GPIOF->AFR[1]  = 0xCCCCCCCC;
+  /* Configure PFx pins in Alternate function mode */   
+  GPIOF->MODER   = 0xAA800AAA;
+  /* Configure PFx pins speed to 50 MHz */ 
+  GPIOF->OSPEEDR = 0xAA800AAA;
+  /* Configure PFx pins Output type to push-pull */  
+  GPIOF->OTYPER  = 0x00000000;
+  /* No pull-up, pull-down for PFx pins */ 
+  GPIOF->PUPDR   = 0x00000000;
+
+  /* Connect PGx pins to FMC Alternate function */
+  GPIOG->AFR[0]  = 0xCCCCCCCC;
+  GPIOG->AFR[1]  = 0xCCCCCCCC;
+  /* Configure PGx pins in Alternate function mode */ 
+  GPIOG->MODER   = 0xAAAAAAAA;
+  /* Configure PGx pins speed to 50 MHz */ 
+  GPIOG->OSPEEDR = 0xAAAAAAAA;
+  /* Configure PGx pins Output type to push-pull */  
+  GPIOG->OTYPER  = 0x00000000;
+  /* No pull-up, pull-down for PGx pins */ 
+  GPIOG->PUPDR   = 0x00000000;
+
+#if defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)\
+ || defined(STM32F469xx) || defined(STM32F479xx)  
+  /* Connect PHx pins to FMC Alternate function */
+  GPIOH->AFR[0]  = 0x00C0CC00;
+  GPIOH->AFR[1]  = 0xCCCCCCCC;
+  /* Configure PHx pins in Alternate function mode */ 
+  GPIOH->MODER   = 0xAAAA08A0;
+  /* Configure PHx pins speed to 50 MHz */ 
+  GPIOH->OSPEEDR = 0xAAAA08A0;
+  /* Configure PHx pins Output type to push-pull */  
+  GPIOH->OTYPER  = 0x00000000;
+  /* No pull-up, pull-down for PHx pins */ 
+  GPIOH->PUPDR   = 0x00000000;
+  
+  /* Connect PIx pins to FMC Alternate function */
+  GPIOI->AFR[0]  = 0xCCCCCCCC;
+  GPIOI->AFR[1]  = 0x00000CC0;
+  /* Configure PIx pins in Alternate function mode */ 
+  GPIOI->MODER   = 0x0028AAAA;
+  /* Configure PIx pins speed to 50 MHz */ 
+  GPIOI->OSPEEDR = 0x0028AAAA;
+  /* Configure PIx pins Output type to push-pull */  
+  GPIOI->OTYPER  = 0x00000000;
+  /* No pull-up, pull-down for PIx pins */ 
+  GPIOI->PUPDR   = 0x00000000;
+#endif /* STM32F427xx || STM32F437xx || STM32F429xx || STM32F439xx || STM32F469xx || STM32F479xx */
+  
+/*-- FMC Configuration -------------------------------------------------------*/
+  /* Enable the FMC interface clock */
+  RCC->AHB3ENR |= 0x00000001;
+  /* Delay after an RCC peripheral clock enabling */
+  tmp = READ_BIT(RCC->AHB3ENR, RCC_AHB3ENR_FMCEN);
+
+  /* Configure and enable SDRAM bank1 */
+#if defined(STM32F446xx)
+  FMC_Bank5_6->SDCR[0] = 0x00001954;
+#else  
+  FMC_Bank5_6->SDCR[0] = 0x000019E4;
+#endif /* STM32F446xx */
+  FMC_Bank5_6->SDTR[0] = 0x01115351;      
+  
+  /* SDRAM initialization sequence */
+  /* Clock enable command */
+  FMC_Bank5_6->SDCMR = 0x00000011; 
+  tmpreg = FMC_Bank5_6->SDSR & 0x00000020; 
+  while((tmpreg != 0) && (timeout-- > 0))
+  {
+    tmpreg = FMC_Bank5_6->SDSR & 0x00000020; 
+  }
+
+  /* Delay */
+  for (index = 0; index<1000; index++);
+  
+  /* PALL command */
+  FMC_Bank5_6->SDCMR = 0x00000012;           
+  timeout = 0xFFFF;
+  while((tmpreg != 0) && (timeout-- > 0))
+  {
+    tmpreg = FMC_Bank5_6->SDSR & 0x00000020; 
+  }
+  
+  /* Auto refresh command */
+#if defined(STM32F446xx)
+  FMC_Bank5_6->SDCMR = 0x000000F3;
+#else  
+  FMC_Bank5_6->SDCMR = 0x00000073;
+#endif /* STM32F446xx */
+  timeout = 0xFFFF;
+  while((tmpreg != 0) && (timeout-- > 0))
+  {
+    tmpreg = FMC_Bank5_6->SDSR & 0x00000020; 
+  }
+ 
+  /* MRD register program */
+#if defined(STM32F446xx)
+  FMC_Bank5_6->SDCMR = 0x00044014;
+#else  
+  FMC_Bank5_6->SDCMR = 0x00046014;
+#endif /* STM32F446xx */
+  timeout = 0xFFFF;
+  while((tmpreg != 0) && (timeout-- > 0))
+  {
+    tmpreg = FMC_Bank5_6->SDSR & 0x00000020; 
+  } 
+  
+  /* Set refresh count */
+  tmpreg = FMC_Bank5_6->SDRTR;
+#if defined(STM32F446xx)
+  FMC_Bank5_6->SDRTR = (tmpreg | (0x0000050C<<1));
+#else    
+  FMC_Bank5_6->SDRTR = (tmpreg | (0x0000027C<<1));
+#endif /* STM32F446xx */
+  
+  /* Disable write protection */
+  tmpreg = FMC_Bank5_6->SDCR[0]; 
+  FMC_Bank5_6->SDCR[0] = (tmpreg & 0xFFFFFDFF);
+#endif /* DATA_IN_ExtSDRAM */
+#endif /* STM32F427xx || STM32F437xx || STM32F429xx || STM32F439xx || STM32F446xx || STM32F469xx || STM32F479xx */
+
+#if defined(STM32F405xx) || defined(STM32F415xx) || defined(STM32F407xx) || defined(STM32F417xx)\
+ || defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)\
+ || defined(STM32F469xx) || defined(STM32F479xx) || defined(STM32F412Zx) || defined(STM32F412Vx)
+
+#if defined(DATA_IN_ExtSRAM)
+/*-- GPIOs Configuration -----------------------------------------------------*/
+   /* Enable GPIOD, GPIOE, GPIOF and GPIOG interface clock */
+  RCC->AHB1ENR   |= 0x00000078;
+  /* Delay after an RCC peripheral clock enabling */
+  tmp = READ_BIT(RCC->AHB1ENR, RCC_AHB1ENR_GPIODEN);
+  
+  /* Connect PDx pins to FMC Alternate function */
+  GPIOD->AFR[0]  = 0x00CCC0CC;
+  GPIOD->AFR[1]  = 0xCCCCCCCC;
+  /* Configure PDx pins in Alternate function mode */  
+  GPIOD->MODER   = 0xAAAA0A8A;
+  /* Configure PDx pins speed to 100 MHz */  
+  GPIOD->OSPEEDR = 0xFFFF0FCF;
+  /* Configure PDx pins Output type to push-pull */  
+  GPIOD->OTYPER  = 0x00000000;
+  /* No pull-up, pull-down for PDx pins */ 
+  GPIOD->PUPDR   = 0x00000000;
+
+  /* Connect PEx pins to FMC Alternate function */
+  GPIOE->AFR[0]  = 0xC00CC0CC;
+  GPIOE->AFR[1]  = 0xCCCCCCCC;
+  /* Configure PEx pins in Alternate function mode */ 
+  GPIOE->MODER   = 0xAAAA828A;
+  /* Configure PEx pins speed to 100 MHz */ 
+  GPIOE->OSPEEDR = 0xFFFFC3CF;
+  /* Configure PEx pins Output type to push-pull */  
+  GPIOE->OTYPER  = 0x00000000;
+  /* No pull-up, pull-down for PEx pins */ 
+  GPIOE->PUPDR   = 0x00000000;
+
+  /* Connect PFx pins to FMC Alternate function */
+  GPIOF->AFR[0]  = 0x00CCCCCC;
+  GPIOF->AFR[1]  = 0xCCCC0000;
+  /* Configure PFx pins in Alternate function mode */   
+  GPIOF->MODER   = 0xAA000AAA;
+  /* Configure PFx pins speed to 100 MHz */ 
+  GPIOF->OSPEEDR = 0xFF000FFF;
+  /* Configure PFx pins Output type to push-pull */  
+  GPIOF->OTYPER  = 0x00000000;
+  /* No pull-up, pull-down for PFx pins */ 
+  GPIOF->PUPDR   = 0x00000000;
+
+  /* Connect PGx pins to FMC Alternate function */
+  GPIOG->AFR[0]  = 0x00CCCCCC;
+  GPIOG->AFR[1]  = 0x000000C0;
+  /* Configure PGx pins in Alternate function mode */ 
+  GPIOG->MODER   = 0x00085AAA;
+  /* Configure PGx pins speed to 100 MHz */ 
+  GPIOG->OSPEEDR = 0x000CAFFF;
+  /* Configure PGx pins Output type to push-pull */  
+  GPIOG->OTYPER  = 0x00000000;
+  /* No pull-up, pull-down for PGx pins */ 
+  GPIOG->PUPDR   = 0x00000000;
+  
+/*-- FMC/FSMC Configuration --------------------------------------------------*/
+  /* Enable the FMC/FSMC interface clock */
+  RCC->AHB3ENR         |= 0x00000001;
+
+#if defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)
+  /* Delay after an RCC peripheral clock enabling */
+  tmp = READ_BIT(RCC->AHB3ENR, RCC_AHB3ENR_FMCEN);
+  /* Configure and enable Bank1_SRAM2 */
+  FMC_Bank1->BTCR[2]  = 0x00001011;
+  FMC_Bank1->BTCR[3]  = 0x00000201;
+  FMC_Bank1E->BWTR[2] = 0x0fffffff;
+#endif /* STM32F427xx || STM32F437xx || STM32F429xx || STM32F439xx */ 
+#if defined(STM32F469xx) || defined(STM32F479xx)
+  /* Delay after an RCC peripheral clock enabling */
+  tmp = READ_BIT(RCC->AHB3ENR, RCC_AHB3ENR_FMCEN);
+  /* Configure and enable Bank1_SRAM2 */
+  FMC_Bank1->BTCR[2]  = 0x00001091;
+  FMC_Bank1->BTCR[3]  = 0x00110212;
+  FMC_Bank1E->BWTR[2] = 0x0fffffff;
+#endif /* STM32F469xx || STM32F479xx */
+#if defined(STM32F405xx) || defined(STM32F415xx) || defined(STM32F407xx)|| defined(STM32F417xx)\
+   || defined(STM32F412Zx) || defined(STM32F412Vx)
+  /* Delay after an RCC peripheral clock enabling */
+  tmp = READ_BIT(RCC->AHB3ENR, RCC_AHB3ENR_FSMCEN);
+  /* Configure and enable Bank1_SRAM2 */
+  FSMC_Bank1->BTCR[2]  = 0x00001011;
+  FSMC_Bank1->BTCR[3]  = 0x00000201;
+  FSMC_Bank1E->BWTR[2] = 0x0FFFFFFF;
+#endif /* STM32F405xx || STM32F415xx || STM32F407xx || STM32F417xx || STM32F412Zx || STM32F412Vx */
+
+#endif /* DATA_IN_ExtSRAM */
+#endif /* STM32F405xx || STM32F415xx || STM32F407xx || STM32F417xx || STM32F427xx || STM32F437xx ||\
+          STM32F429xx || STM32F439xx || STM32F469xx || STM32F479xx || STM32F412Zx || STM32F412Vx  */ 
+  (void)(tmp); 
+}
+#endif /* DATA_IN_ExtSRAM && DATA_IN_ExtSDRAM */
+/**
+  * @}
+  */
+
+/**
+  * @}
+  */
+
+/**
+  * @}
+  */
+/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
diff --git a/controller/fw/stm32f407.ld b/controller/fw/stm32f407.ld
index 764ed6b..f998b4a 100644
--- a/controller/fw/stm32f407.ld
+++ b/controller/fw/stm32f407.ld
@@ -1,34 +1,148 @@
-/*
- * This file is part of the libopencm3 project.
- *
- * Copyright (C) 2009 Uwe Hermann <uwe@hermann-uwe.de>
- * Copyright (C) 2011 Stephen Caudle <scaudle@doceme.com>
- *
- * This library is free software: you can redistribute it and/or modify
- * it under the terms of the GNU Lesser General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This library 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 Lesser General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public License
- * along with this library.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-/* Linker script for ST STM32F4DISCOVERY (STM32F407VG, 1024K flash, 128K RAM). */
-
-/* Define memory regions. */
 MEMORY
 {
-	rom (rx) : ORIGIN = 0x08000000, LENGTH = 1024K
-	ram (rwx) : ORIGIN = 0x20000000, LENGTH = 128K
-	ccm (rwx) : ORIGIN = 0x10000000, LENGTH = 64K
-	bkp (rwx) : ORIGIN = 0x40024000, LENGTH = 4K
+	FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 512K
+	RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K
+	CCMRAM (rwx) : ORIGIN = 0x10000000, LENGTH = 64K
+	BACKUP (rwx) : ORIGIN = 0x40024000, LENGTH = 4K
+}
+
+/* Entry Point */
+ENTRY(Reset_Handler)
+
+/* Highest address of the user mode stack */
+_estack = 0x20020000;    /* end of RAM */
+/* Generate a link error if heap and stack don't fit into RAM */
+_Min_Heap_Size = 0x200;;      /* required amount of heap  */
+_Min_Stack_Size = 0x400;; /* required amount of stack */
+
+/* Define output sections */
+SECTIONS
+{
+  /* The startup code goes first into FLASH */
+  .isr_vector :
+  {
+    . = ALIGN(4);
+    KEEP(*(.isr_vector)) /* Startup code */
+    . = ALIGN(4);
+  } >FLASH
+
+  /* The program code and other data goes into FLASH */
+  .text :
+  {
+    . = ALIGN(4);
+    *(.text)           /* .text sections (code) */
+    *(.text*)          /* .text* sections (code) */
+    *(.glue_7)         /* glue arm to thumb code */
+    *(.glue_7t)        /* glue thumb to arm code */
+    *(.eh_frame)
+
+    KEEP (*(.init))
+    KEEP (*(.fini))
+
+    . = ALIGN(4);
+    _etext = .;        /* define a global symbols at end of code */
+  } >FLASH
+
+  /* Constant data goes into FLASH */
+  .rodata :
+  {
+    . = ALIGN(4);
+    *(.rodata)         /* .rodata sections (constants, strings, etc.) */
+    *(.rodata*)        /* .rodata* sections (constants, strings, etc.) */
+    . = ALIGN(4);
+  } >FLASH
+
+  .ARM.extab   : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH
+  .ARM : {
+    __exidx_start = .;
+    *(.ARM.exidx*)
+    __exidx_end = .;
+  } >FLASH
+
+  .preinit_array     :
+  {
+    PROVIDE_HIDDEN (__preinit_array_start = .);
+    KEEP (*(.preinit_array*))
+    PROVIDE_HIDDEN (__preinit_array_end = .);
+  } >FLASH
+  .init_array :
+  {
+    PROVIDE_HIDDEN (__init_array_start = .);
+    KEEP (*(SORT(.init_array.*)))
+    KEEP (*(.init_array*))
+    PROVIDE_HIDDEN (__init_array_end = .);
+  } >FLASH
+  .fini_array :
+  {
+    PROVIDE_HIDDEN (__fini_array_start = .);
+    KEEP (*(SORT(.fini_array.*)))
+    KEEP (*(.fini_array*))
+    PROVIDE_HIDDEN (__fini_array_end = .);
+  } >FLASH
+
+  /* used by the startup to initialize data */
+  _sidata = LOADADDR(.data);
+
+  /* Initialized data sections goes into RAM, load LMA copy after code */
+  .data : 
+  {
+    . = ALIGN(4);
+    _sdata = .;        /* create a global symbol at data start */
+    *(.data)           /* .data sections */
+    *(.data*)          /* .data* sections */
+
+    . = ALIGN(4);
+    _edata = .;        /* define a global symbol at data end */
+  } >RAM AT> FLASH
+
+  _siccmram = LOADADDR(.ccmram);
+
+  /* CCM-RAM section 
+  * 
+  * IMPORTANT NOTE! 
+  * If initialized variables will be placed in this section,
+  * the startup code needs to be modified to copy the init-values.  
+  */
+  .ccmram :
+  {
+    . = ALIGN(4);
+    _sccmram = .;       /* create a global symbol at ccmram start */
+    *(.ccmram)
+    *(.ccmram*)
+    
+    . = ALIGN(4);
+    _eccmram = .;       /* create a global symbol at ccmram end */
+  } >CCMRAM AT> FLASH
+
+  
+  /* Uninitialized data section */
+  . = ALIGN(4);
+  .bss :
+  {
+    /* This is used by the startup in order to initialize the .bss secion */
+    _sbss = .;         /* define a global symbol at bss start */
+    __bss_start__ = _sbss;
+    *(.bss)
+    *(.bss*)
+    *(COMMON)
+
+    . = ALIGN(4);
+    _ebss = .;         /* define a global symbol at bss end */
+    __bss_end__ = _ebss;
+  } >RAM
+
+  /* User_heap_stack section, used to check that there is enough RAM left */
+  ._user_heap_stack :
+  {
+    . = ALIGN(4);
+    PROVIDE ( end = . );
+    PROVIDE ( _end = . );
+    . = . + _Min_Heap_Size;
+    . = . + _Min_Stack_Size;
+    . = ALIGN(4);
+  } >RAM
+
+  .ARM.attributes 0 : { *(.ARM.attributes) }
 }
 
-/* Include the common ld script. */
-INCLUDE cortex-m-generic.ld
 
-- 
cgit