Index: kernel/arch/arm32/src/cpu/cpu.c
===================================================================
--- kernel/arch/arm32/src/cpu/cpu.c	(revision e32720fffe177c9b6bf6489727240c59ac6a953b)
+++ kernel/arch/arm32/src/cpu/cpu.c	(revision 22a0ee80f97fa3cec4bd4ef790bfe46c8aa94130)
@@ -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)
Index: kernel/arch/arm32/src/dummy.S
===================================================================
--- kernel/arch/arm32/src/dummy.S	(revision e32720fffe177c9b6bf6489727240c59ac6a953b)
+++ kernel/arch/arm32/src/dummy.S	(revision 22a0ee80f97fa3cec4bd4ef790bfe46c8aa94130)
@@ -32,9 +32,4 @@
 .global asm_delay_loop
 
-.global fpu_context_restore
-.global fpu_context_save
-.global fpu_enable
-.global fpu_init
-
 .global sys_tls_set
 .global dummy
@@ -46,16 +41,4 @@
 	mov	pc, lr
 
-fpu_context_restore:
-	mov	pc, lr
-    
-fpu_context_save:
-	mov	pc, lr
-    
-fpu_enable:
-	mov	pc, lr
-
-fpu_init:
-	mov	pc, lr
-    
 # not used on ARM
 sys_tls_set: