Index: kernel/arch/arm32/src/cpu/cpu.c
===================================================================
--- kernel/arch/arm32/src/cpu/cpu.c	(revision 8316547f916ae25dffe4d30a1c706bfb38c39897)
+++ kernel/arch/arm32/src/cpu/cpu.c	(revision d238aa99f969085f93fcf0f5b17242da6c119dfc)
@@ -38,4 +38,5 @@
 #include <arch.h>
 #include <print.h>
+#include <fpu_context.h>
 
 /** Number of indexes left out in the #imp_data array */
@@ -138,4 +139,54 @@
 }
 
+void fpu_init(void)
+{
+	//TODO: Identify FPU unit
+	//and set correct functions to save/restore ctx
+}
+
+void fpu_enable(void)
+{
+	/* Enable FPU instructions */
+	asm volatile (
+		"ldr r1, =0x40000000\n"
+		"vmsr fpexc, r1\n"
+		::: "r1"
+	);
+}
+
+void fpu_disable(void)
+{
+	/* Disable FPU instructions */
+	asm volatile (
+		"ldr r1, =0x00000000\n"
+		"vmsr fpexc, r1\n"
+		::: "r1"
+	);
+}
+
+void fpu_context_save(fpu_context_t *ctx)
+{
+	// TODO check and complete. What about fpexc?
+	asm volatile (
+		"vmrs r1, fpscr\n"
+//		"vmrs r2, fpexc\n"
+		"stm %0, {r1, r2}\n"
+		"vstm %0, {d0, d1, d2, d3, d4, d5, d6, d7, d8, d9, d10, d11, d12, d13, d14, d15}\n"
+		::"r" (ctx): "r1","r2","memory"
+	);
+}
+
+void fpu_context_restore(fpu_context_t *ctx)
+{
+	// TODO check and complete. What about fpexc?
+	asm volatile (
+		"ldm %0, {r1, r2}\n"
+		"vmsr fpscr, r1\n"
+//		"vmsr fpexc, r2\n"
+		"vldm %0, {d0, d1, d2, d3, d4, d5, d6, d7, d8, d9, d10, d11, d12, d13, d14, d15}\n"
+		::"r" (ctx): "r1","r2"
+	);
+}
+
 /** Retrieves processor identification and stores it to #CPU.arch */
 void cpu_identify(void)