diff --git a/.gitmodules b/.gitmodules
index 62eac3c491..903fe453ef 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -7,3 +7,6 @@
 [submodule "vendor/cmock"]
 	path = vendor/cmock
 	url = https://github.com/throwtheswitch/cmock.git
+[submodule "vendor/CMSIS"]
+	path = vendor/CMSIS
+	url = https://github.com/ARM-software/CMSIS
diff --git a/Makefile b/Makefile
index cdc3b0e3d3..99bfe9b8dd 100644
--- a/Makefile
+++ b/Makefile
@@ -191,6 +191,9 @@ PROJ_OBJ += version.o FreeRTOS-openocd.o
 PROJ_OBJ_CF1 += configblockflash.o
 PROJ_OBJ_CF2 += configblockeeprom.o
 
+# Libs
+PROJ_OBJ_CF2 += libarm_math.a
+
 OBJ = $(FREERTOS_OBJ) $(PORT_OBJ) $(ST_OBJ) $(PROJ_OBJ)
 ifeq ($(PLATFORM), CF1)
 OBJ += $(CRT0_CF1) $(ST_OBJ_CF1) $(PROJ_OBJ_CF1)
@@ -214,7 +217,7 @@ GDB = $(CROSS_COMPILE)gdb
 INCLUDES  = -I$(FREERTOS)/include -I$(PORT) -Isrc
 INCLUDES += -Isrc/config -Isrc/hal/interface -Isrc/modules/interface
 INCLUDES += -Isrc/utils/interface -Isrc/drivers/interface -Isrc/platform
-INCLUDES += -I$(STLIB)/CMSIS/Include
+INCLUDES += -Ivendor/CMSIS/CMSIS/Include
 
 INCLUDES_CF1 += -I$(STLIB)/STM32F10x_StdPeriph_Driver/inc
 INCLUDES_CF1 += -I$(STLIB)/CMSIS/Core/CM3
@@ -319,6 +322,9 @@ all: check_submodules build
 build: clean_version compile print_version size
 compile: clean_version $(PROG).hex $(PROG).bin $(PROG).dfu
 
+libarm_math.a:
+	+$(MAKE) -C tools/make/cmsis_dsp/ V=$(V)
+
 clean_version:
 ifeq ($(SHELL),/bin/sh)
 	@echo "  CLEAN_VERSION"
diff --git a/bin/vendor/readme.txt b/bin/vendor/readme.txt
new file mode 100644
index 0000000000..36bee15683
--- /dev/null
+++ b/bin/vendor/readme.txt
@@ -0,0 +1 @@
+Here goes the vendor object files (used for compiling the ARM DSP lib)
diff --git a/tools/make/cmsis_dsp/Makefile b/tools/make/cmsis_dsp/Makefile
new file mode 100644
index 0000000000..75a9123c17
--- /dev/null
+++ b/tools/make/cmsis_dsp/Makefile
@@ -0,0 +1,43 @@
+include obj.mk
+
+PROJ_ROOT=../../..
+BIN=$(PROJ_ROOT)/bin/vendor
+PROJ_BIN=$(PROJ_ROOT)/bin
+DSP_SRC=$(PROJ_ROOT)/vendor/CMSIS/CMSIS/DSP_Lib/Source
+DSP_INC=$(PROJ_ROOT)/vendor/CMSIS/CMSIS/Include
+
+VPATH += $(BIN)
+VPATH += $(DSP_SRC)/BasicMathFunctions/
+VPATH += $(DSP_SRC)/CommonTables/
+VPATH += $(DSP_SRC)/ComplexMathFunctions/
+VPATH += $(DSP_SRC)/ControllerFunctions/
+VPATH += $(DSP_SRC)/FastMathFunctions/
+VPATH += $(DSP_SRC)/FilteringFunctions/
+VPATH += $(DSP_SRC)/MatrixFunctions/
+VPATH += $(DSP_SRC)/StatisticsFunctions/
+VPATH += $(DSP_SRC)/SupportFunctions/
+VPATH += $(DSP_SRC)/TransformFunctions/
+
+
+LIB=$(PROJ_BIN)/libarm_math.a
+
+CSFLAGS = -O2 -I$(DSP_INC) -std=c11 -DARM_MATH_CM4 -D__FPU_PRESENT=1 -D__TARGET_FPU_VFP
+CSFLAGS += -mcpu=cortex-m4 -mthumb -ffunction-sections -fdata-sections
+CFLAGS=$(CSFLAGS) -include stdint.h
+
+CROSS_COMPILE     ?= arm-none-eabi-
+AS = $(CROSS_COMPILE)as
+CC = $(CROSS_COMPILE)gcc
+LD = $(CROSS_COMPILE)gcc
+AR = $(CROSS_COMPILE)ar
+SIZE = $(CROSS_COMPILE)size
+OBJCOPY = $(CROSS_COMPILE)objcopy
+GDB = $(CROSS_COMPILE)gdb
+
+all: $(LIB)
+
+$(LIB): $(DSP_OBJ) #$(foreach o,$(DSP_OBJ),$(BIN)/$(o))
+	@$(AR) rcs $@ $(foreach o,$(DSP_OBJ),$(BIN)/$(o))
+	@echo "  AR    libarm_math.a"
+
+include ../targets.mk
diff --git a/tools/make/cmsis_dsp/obj.mk b/tools/make/cmsis_dsp/obj.mk
new file mode 100644
index 0000000000..bc30bd6f66
--- /dev/null
+++ b/tools/make/cmsis_dsp/obj.mk
@@ -0,0 +1,273 @@
+DSP_OBJ += arm_iir_lattice_init_q31.o
+DSP_OBJ += arm_fir_interpolate_init_q31.o
+DSP_OBJ += arm_fir_q7.o
+DSP_OBJ += arm_fir_lattice_init_q15.o
+DSP_OBJ += arm_lms_norm_q15.o
+DSP_OBJ += arm_biquad_cascade_df1_32x64_init_q31.o
+DSP_OBJ += arm_biquad_cascade_df1_init_q15.o
+DSP_OBJ += arm_biquad_cascade_df2T_init_f32.o
+DSP_OBJ += arm_correlate_q15.o
+DSP_OBJ += arm_conv_partial_q31.o
+DSP_OBJ += arm_biquad_cascade_stereo_df2T_f32.o
+DSP_OBJ += arm_fir_decimate_init_q31.o
+DSP_OBJ += arm_correlate_opt_q7.o
+DSP_OBJ += arm_fir_sparse_q15.o
+DSP_OBJ += arm_conv_partial_q15.o
+DSP_OBJ += arm_conv_q7.o
+DSP_OBJ += arm_biquad_cascade_df1_init_q31.o
+DSP_OBJ += arm_biquad_cascade_df1_q31.o
+DSP_OBJ += arm_fir_lattice_f32.o
+DSP_OBJ += arm_fir_decimate_fast_q31.o
+DSP_OBJ += arm_conv_partial_fast_q15.o
+DSP_OBJ += arm_conv_partial_opt_q7.o
+DSP_OBJ += arm_fir_interpolate_q15.o
+DSP_OBJ += arm_correlate_q31.o
+DSP_OBJ += arm_fir_decimate_q15.o
+DSP_OBJ += arm_fir_fast_q15.o
+DSP_OBJ += arm_fir_sparse_q31.o
+DSP_OBJ += arm_conv_q15.o
+DSP_OBJ += arm_conv_opt_q15.o
+DSP_OBJ += arm_fir_decimate_init_q15.o
+DSP_OBJ += arm_fir_sparse_init_q31.o
+DSP_OBJ += arm_conv_partial_fast_opt_q15.o
+DSP_OBJ += arm_iir_lattice_q15.o
+DSP_OBJ += arm_lms_init_f32.o
+DSP_OBJ += arm_correlate_fast_opt_q15.o
+DSP_OBJ += arm_lms_norm_f32.o
+DSP_OBJ += arm_conv_partial_q7.o
+DSP_OBJ += arm_fir_sparse_init_q15.o
+DSP_OBJ += arm_lms_init_q31.o
+DSP_OBJ += arm_iir_lattice_init_q15.o
+DSP_OBJ += arm_fir_q31.o
+DSP_OBJ += arm_correlate_fast_q15.o
+DSP_OBJ += arm_biquad_cascade_stereo_df2T_init_f32.o
+DSP_OBJ += arm_fir_sparse_init_q7.o
+DSP_OBJ += arm_fir_sparse_q7.o
+DSP_OBJ += arm_conv_q31.o
+DSP_OBJ += arm_biquad_cascade_df1_fast_q31.o
+DSP_OBJ += arm_fir_interpolate_q31.o
+DSP_OBJ += arm_biquad_cascade_df1_32x64_q31.o
+DSP_OBJ += arm_fir_lattice_init_f32.o
+DSP_OBJ += arm_lms_norm_init_q31.o
+DSP_OBJ += arm_fir_init_q7.o
+DSP_OBJ += arm_biquad_cascade_df2T_init_f64.o
+DSP_OBJ += arm_fir_lattice_init_q31.o
+DSP_OBJ += arm_biquad_cascade_df1_f32.o
+DSP_OBJ += arm_correlate_f32.o
+DSP_OBJ += arm_fir_decimate_init_f32.o
+DSP_OBJ += arm_biquad_cascade_df1_init_f32.o
+DSP_OBJ += arm_lms_q31.o
+DSP_OBJ += arm_fir_lattice_q15.o
+DSP_OBJ += arm_lms_f32.o
+DSP_OBJ += arm_biquad_cascade_df2T_f32.o
+DSP_OBJ += arm_conv_fast_q15.o
+DSP_OBJ += arm_iir_lattice_init_f32.o
+DSP_OBJ += arm_correlate_q7.o
+DSP_OBJ += arm_fir_decimate_fast_q15.o
+DSP_OBJ += arm_correlate_fast_q31.o
+DSP_OBJ += arm_biquad_cascade_df2T_f64.o
+DSP_OBJ += arm_iir_lattice_q31.o
+DSP_OBJ += arm_conv_partial_opt_q15.o
+DSP_OBJ += arm_lms_q15.o
+DSP_OBJ += arm_fir_init_f32.o
+DSP_OBJ += arm_conv_partial_fast_q31.o
+DSP_OBJ += arm_fir_sparse_f32.o
+DSP_OBJ += arm_lms_norm_q31.o
+DSP_OBJ += arm_fir_f32.o
+DSP_OBJ += arm_iir_lattice_f32.o
+DSP_OBJ += arm_correlate_opt_q15.o
+DSP_OBJ += arm_fir_interpolate_init_f32.o
+DSP_OBJ += arm_lms_norm_init_f32.o
+DSP_OBJ += arm_conv_f32.o
+DSP_OBJ += arm_fir_decimate_q31.o
+DSP_OBJ += arm_fir_decimate_f32.o
+DSP_OBJ += arm_biquad_cascade_df1_fast_q15.o
+DSP_OBJ += arm_biquad_cascade_df1_q15.o
+DSP_OBJ += arm_lms_norm_init_q15.o
+DSP_OBJ += arm_fir_interpolate_f32.o
+DSP_OBJ += arm_conv_opt_q7.o
+DSP_OBJ += arm_conv_partial_f32.o
+DSP_OBJ += arm_conv_fast_opt_q15.o
+DSP_OBJ += arm_fir_sparse_init_f32.o
+DSP_OBJ += arm_fir_init_q15.o
+DSP_OBJ += arm_fir_lattice_q31.o
+DSP_OBJ += arm_fir_fast_q31.o
+DSP_OBJ += arm_fir_q15.o
+DSP_OBJ += arm_lms_init_q15.o
+DSP_OBJ += arm_fir_interpolate_init_q15.o
+DSP_OBJ += arm_fir_init_q31.o
+DSP_OBJ += arm_conv_fast_q31.o
+DSP_OBJ += arm_cmplx_mult_real_q15.o
+DSP_OBJ += arm_cmplx_mag_squared_q15.o
+DSP_OBJ += arm_cmplx_mult_cmplx_q15.o
+DSP_OBJ += arm_cmplx_mag_q31.o
+DSP_OBJ += arm_cmplx_mag_squared_q31.o
+DSP_OBJ += arm_cmplx_mult_cmplx_f32.o
+DSP_OBJ += arm_cmplx_conj_q31.o
+DSP_OBJ += arm_cmplx_dot_prod_q15.o
+DSP_OBJ += arm_cmplx_conj_q15.o
+DSP_OBJ += arm_cmplx_mag_q15.o
+DSP_OBJ += arm_cmplx_conj_f32.o
+DSP_OBJ += arm_cmplx_dot_prod_f32.o
+DSP_OBJ += arm_cmplx_mag_f32.o
+DSP_OBJ += arm_cmplx_mult_cmplx_q31.o
+DSP_OBJ += arm_cmplx_dot_prod_q31.o
+DSP_OBJ += arm_cmplx_mult_real_q31.o
+DSP_OBJ += arm_cmplx_mag_squared_f32.o
+DSP_OBJ += arm_cmplx_mult_real_f32.o
+DSP_OBJ += arm_shift_q7.o
+DSP_OBJ += arm_offset_q15.o
+DSP_OBJ += arm_negate_q31.o
+DSP_OBJ += arm_offset_f32.o
+DSP_OBJ += arm_abs_f32.o
+DSP_OBJ += arm_sub_q15.o
+DSP_OBJ += arm_scale_q31.o
+DSP_OBJ += arm_negate_q7.o
+DSP_OBJ += arm_negate_q15.o
+DSP_OBJ += arm_offset_q31.o
+DSP_OBJ += arm_add_q31.o
+DSP_OBJ += arm_abs_q7.o
+DSP_OBJ += arm_abs_q31.o
+DSP_OBJ += arm_shift_q15.o
+DSP_OBJ += arm_shift_q31.o
+DSP_OBJ += arm_dot_prod_q15.o
+DSP_OBJ += arm_mult_q7.o
+DSP_OBJ += arm_scale_q15.o
+DSP_OBJ += arm_dot_prod_f32.o
+DSP_OBJ += arm_offset_q7.o
+DSP_OBJ += arm_sub_f32.o
+DSP_OBJ += arm_mult_q31.o
+DSP_OBJ += arm_dot_prod_q7.o
+DSP_OBJ += arm_mult_f32.o
+DSP_OBJ += arm_scale_q7.o
+DSP_OBJ += arm_scale_f32.o
+DSP_OBJ += arm_add_q15.o
+DSP_OBJ += arm_sub_q31.o
+DSP_OBJ += arm_negate_f32.o
+DSP_OBJ += arm_mult_q15.o
+DSP_OBJ += arm_add_q7.o
+DSP_OBJ += arm_sub_q7.o
+DSP_OBJ += arm_abs_q15.o
+DSP_OBJ += arm_dot_prod_q31.o
+DSP_OBJ += arm_add_f32.o
+DSP_OBJ += arm_dct4_q15.o
+DSP_OBJ += arm_cfft_radix4_init_f32.o
+DSP_OBJ += arm_rfft_f32.o
+DSP_OBJ += arm_cfft_radix4_init_q15.o
+DSP_OBJ += arm_cfft_radix8_f32.o
+DSP_OBJ += arm_cfft_radix4_q31.o
+DSP_OBJ += arm_dct4_init_q15.o
+DSP_OBJ += arm_cfft_radix2_init_q15.o
+DSP_OBJ += arm_cfft_q31.o
+DSP_OBJ += arm_cfft_radix2_q15.o
+DSP_OBJ += arm_dct4_init_f32.o
+DSP_OBJ += arm_dct4_init_q31.o
+DSP_OBJ += arm_cfft_radix4_q15.o
+DSP_OBJ += arm_rfft_init_q31.o
+DSP_OBJ += arm_cfft_f32.o
+DSP_OBJ += arm_cfft_radix2_init_q31.o
+DSP_OBJ += arm_cfft_q15.o
+DSP_OBJ += arm_rfft_q15.o
+DSP_OBJ += arm_cfft_radix2_init_f32.o
+DSP_OBJ += arm_cfft_radix2_f32.o
+DSP_OBJ += arm_dct4_f32.o
+DSP_OBJ += arm_rfft_fast_init_f32.o
+DSP_OBJ += arm_cfft_radix4_f32.o
+DSP_OBJ += arm_rfft_init_q15.o
+DSP_OBJ += arm_cfft_radix2_q31.o
+DSP_OBJ += arm_rfft_init_f32.o
+DSP_OBJ += arm_bitreversal.o
+DSP_OBJ += arm_rfft_fast_f32.o
+DSP_OBJ += arm_dct4_q31.o
+DSP_OBJ += arm_rfft_q31.o
+DSP_OBJ += arm_cfft_radix4_init_q31.o
+DSP_OBJ += arm_std_q31.o
+DSP_OBJ += arm_mean_q7.o
+DSP_OBJ += arm_mean_f32.o
+DSP_OBJ += arm_max_q7.o
+DSP_OBJ += arm_mean_q15.o
+DSP_OBJ += arm_var_q15.o
+DSP_OBJ += arm_power_f32.o
+DSP_OBJ += arm_min_f32.o
+DSP_OBJ += arm_power_q7.o
+DSP_OBJ += arm_var_f32.o
+DSP_OBJ += arm_max_q15.o
+DSP_OBJ += arm_max_q31.o
+DSP_OBJ += arm_max_f32.o
+DSP_OBJ += arm_min_q7.o
+DSP_OBJ += arm_std_f32.o
+DSP_OBJ += arm_power_q15.o
+DSP_OBJ += arm_rms_q15.o
+DSP_OBJ += arm_power_q31.o
+DSP_OBJ += arm_rms_f32.o
+DSP_OBJ += arm_mean_q31.o
+DSP_OBJ += arm_var_q31.o
+DSP_OBJ += arm_rms_q31.o
+DSP_OBJ += arm_min_q31.o
+DSP_OBJ += arm_min_q15.o
+DSP_OBJ += arm_std_q15.o
+DSP_OBJ += arm_sin_cos_f32.o
+DSP_OBJ += arm_pid_reset_f32.o
+DSP_OBJ += arm_pid_reset_q31.o
+DSP_OBJ += arm_pid_init_q15.o
+DSP_OBJ += arm_pid_init_q31.o
+DSP_OBJ += arm_pid_reset_q15.o
+DSP_OBJ += arm_pid_init_f32.o
+DSP_OBJ += arm_sin_cos_q31.o
+DSP_OBJ += arm_common_tables.o
+DSP_OBJ += arm_const_structs.o
+DSP_OBJ += arm_copy_f32.o
+DSP_OBJ += arm_fill_f32.o
+DSP_OBJ += arm_q15_to_float.o
+DSP_OBJ += arm_float_to_q15.o
+DSP_OBJ += arm_q31_to_q7.o
+DSP_OBJ += arm_copy_q31.o
+DSP_OBJ += arm_float_to_q31.o
+DSP_OBJ += arm_float_to_q7.o
+DSP_OBJ += arm_fill_q7.o
+DSP_OBJ += arm_fill_q15.o
+DSP_OBJ += arm_q7_to_q31.o
+DSP_OBJ += arm_q31_to_float.o
+DSP_OBJ += arm_fill_q31.o
+DSP_OBJ += arm_q7_to_q15.o
+DSP_OBJ += arm_copy_q7.o
+DSP_OBJ += arm_q31_to_q15.o
+DSP_OBJ += arm_copy_q15.o
+DSP_OBJ += arm_q7_to_float.o
+DSP_OBJ += arm_q15_to_q7.o
+DSP_OBJ += arm_q15_to_q31.o
+DSP_OBJ += arm_mat_mult_fast_q31.o
+DSP_OBJ += arm_mat_init_f32.o
+DSP_OBJ += arm_mat_mult_q31.o
+DSP_OBJ += arm_mat_add_q15.o
+DSP_OBJ += arm_mat_cmplx_mult_f32.o
+DSP_OBJ += arm_mat_add_f32.o
+DSP_OBJ += arm_mat_init_q31.o
+DSP_OBJ += arm_mat_trans_q15.o
+DSP_OBJ += arm_mat_cmplx_mult_q15.o
+DSP_OBJ += arm_mat_add_q31.o
+DSP_OBJ += arm_mat_mult_q15.o
+DSP_OBJ += arm_mat_sub_q15.o
+DSP_OBJ += arm_mat_sub_q31.o
+DSP_OBJ += arm_mat_init_q15.o
+DSP_OBJ += arm_mat_mult_f32.o
+DSP_OBJ += arm_mat_scale_f32.o
+DSP_OBJ += arm_mat_sub_f32.o
+DSP_OBJ += arm_mat_inverse_f32.o
+DSP_OBJ += arm_mat_cmplx_mult_q31.o
+DSP_OBJ += arm_mat_inverse_f64.o
+DSP_OBJ += arm_mat_scale_q31.o
+DSP_OBJ += arm_mat_scale_q15.o
+DSP_OBJ += arm_mat_trans_q31.o
+DSP_OBJ += arm_mat_mult_fast_q15.o
+DSP_OBJ += arm_mat_trans_f32.o
+DSP_OBJ += arm_sin_q31.o
+DSP_OBJ += arm_sqrt_q15.o
+DSP_OBJ += arm_sqrt_q31.o
+DSP_OBJ += arm_cos_q15.o
+DSP_OBJ += arm_cos_f32.o
+DSP_OBJ += arm_cos_q31.o
+DSP_OBJ += arm_sin_f32.o
+DSP_OBJ += arm_sin_q15.o
+
+DSP_OBJ += arm_bitreversal2.o
diff --git a/tools/make/targets.mk b/tools/make/targets.mk
index 4fb89b9764..1d0f693717 100644
--- a/tools/make/targets.mk
+++ b/tools/make/targets.mk
@@ -22,10 +22,16 @@ VTMPL_COMMAND_SILENT="  VTMPL $@"
 
 CC_COMMAND=$(CC) $(CFLAGS) -c $< -o $(BIN)/$@
 CC_COMMAND_SILENT="  CC    $@"
-.c.o: 
+.c.o:
 	@$(if $(QUIET), ,echo $(CC_COMMAND$(VERBOSE)) )
 	@$(CC_COMMAND)
 
+CCS_COMMAND=$(CC) $(CSFLAGS) -c $< -o $(BIN)/$@
+CCS_COMMAND_SILENT="  CCS   $@"
+.S.o:
+	@$(if $(QUIET), ,echo $(CCS_COMMAND$(VERBOSE)) )
+	@$(CCS_COMMAND)
+
 LD_COMMAND=$(LD) $(LDFLAGS) $(foreach o,$(OBJ),$(BIN)/$(o)) -lm -o $@
 LD_COMMAND_SILENT="  LD    $@"
 $(PROG).elf: $(OBJ)
@@ -68,10 +74,8 @@ clean:
 	@$(if $(QUIET), ,echo $(CLEAN_COMMAND$(VERBOSE)) )
 	@$(CLEAN_COMMAND)
 
-MRPROPER_COMMAND=rm -f *~ hal/src/*~ hal/interface/*~ tasks/src/*~ tasks/inc/*~ utils/src/*~ utils/inc/*~ tools/make/*~; rm -rf bin/dep
+MRPROPER_COMMAND=rm -f *~ hal/src/*~ hal/interface/*~ tasks/src/*~ tasks/inc/*~ utils/src/*~ utils/inc/*~ tools/make/*~; rm -rf bin/dep/*.d $(BIN)/*.a $(BIN)/vendor/*.o
 MRPROPER_COMMAND_SILENT="  MRPROPER"
 mrproper: clean
 	@$(if $(QUIET), ,echo $(MRPROPER_COMMAND$(VERBOSE)) )
 	@$(MRPROPER_COMMAND)
-
-
diff --git a/vendor/CMSIS b/vendor/CMSIS
new file mode 160000
index 0000000000..0156c6f40e
--- /dev/null
+++ b/vendor/CMSIS
@@ -0,0 +1 @@
+Subproject commit 0156c6f40ec9556bf46b2d4a947ae3ef872cd1bd