Index: kernel/arch/arm32/Makefile.inc
===================================================================
--- kernel/arch/arm32/Makefile.inc	(revision b9f72b977e060712cf7645e04ee94db0127dd34e)
+++ kernel/arch/arm32/Makefile.inc	(revision 7e87436b47fe77e9b7b3c41d0b98ac95f883aaec)
@@ -34,4 +34,8 @@
 
 GCC_CFLAGS += -fno-omit-frame-pointer -mapcs-frame -march=$(subst _,-,$(PROCESSOR)) -mno-unaligned-access
+
+ifeq ($(MACHINE),beagleboardxm)
+GCC_CFLAGS += -mcpu=cortex-a8
+endif
 
 ifeq ($(CONFIG_FPU),y)
Index: kernel/arch/arm32/include/security_ext.h
===================================================================
--- kernel/arch/arm32/include/security_ext.h	(revision b9f72b977e060712cf7645e04ee94db0127dd34e)
+++ kernel/arch/arm32/include/security_ext.h	(revision 7e87436b47fe77e9b7b3c41d0b98ac95f883aaec)
@@ -49,10 +49,25 @@
 }
 
+static inline bool sec_ext_is_monitor_mode()
+{
+	return (current_status_reg_read() & MODE_MASK) == MONITOR_MODE;
+}
+
 static inline bool sec_ext_is_secure()
 {
 	return sec_ext_is_implemented()
-	    && ((current_status_reg_read() & MODE_MASK) == MONITOR_MODE
-	        || !(SCR_read() & SCR_NS_FLAG));
+	    && (sec_ext_is_monitor_mode() || !(SCR_read() & SCR_NS_FLAG));
 }
+
+typedef enum {
+	SECURITY_CALL_ENABLE_CP10_11 = 0xaaaa
+} sec_ext_call_t;
+
+static inline void sec_ext_call(sec_ext_call_t call)
+{
+	asm volatile ("mov r0, %0\nsmc #0" ::"r"(call));
+}
+
+int sec_ext_handle_call(sec_ext_call_t call);
 
 #endif
Index: kernel/arch/arm32/src/fpu_context.c
===================================================================
--- kernel/arch/arm32/src/fpu_context.c	(revision b9f72b977e060712cf7645e04ee94db0127dd34e)
+++ kernel/arch/arm32/src/fpu_context.c	(revision 7e87436b47fe77e9b7b3c41d0b98ac95f883aaec)
@@ -38,4 +38,5 @@
 #include <arch/types.h>
 #include <arch/security_ext.h>
+#include <arch/cp15.h>
 #include <cpu.h>
 
@@ -113,4 +114,15 @@
 static void (*restore_context)(fpu_context_t *ctx);
 
+int sec_ext_handle_call(sec_ext_call_t call)
+{
+	printf("Handling secure call %x in %s context (%s mode-%x)\n",
+		call, sec_ext_is_secure() ? "secure" : "unsecure",
+		sec_ext_is_monitor_mode() ? "monitor" : "other",
+		current_status_reg_read());
+	if (sec_ext_is_monitor_mode() && call == SECURITY_CALL_ENABLE_CP10_11)
+		return 1;
+	return 0;
+}
+
 static int fpu_have_coprocessor_access()
 {
@@ -118,15 +130,15 @@
  * rely on user decision to use CONFIG_FPU.
  */
-#ifndef PROCESSOR_armv7_a
-	return 1;
-#endif
+#ifdef PROCESSOR_armv7_a
 	const uint32_t cpacr = CPACR_read();
 	/* FPU needs access to coprocessor 10 and 11.
 	 * Moreover they need to have same access enabledd */
-	if (((cpacr & CPACR_CP_MASK(10)) == CPACR_CP_FULL_ACCESS(10)) &&
-	   ((cpacr & CPACR_CP_MASK(11)) == CPACR_CP_FULL_ACCESS(11)))
-		return 1;
-	printf("No sccess to CP10 and CP11: %" PRIx32 "\n", cpacr);
-	return 0;
+	if (((cpacr & CPACR_CP_MASK(10)) != CPACR_CP_FULL_ACCESS(10)) &&
+	   ((cpacr & CPACR_CP_MASK(11)) != CPACR_CP_FULL_ACCESS(11))) {
+		printf("No access to CP10 and CP11: %" PRIx32 "\n", cpacr);
+		return 0;
+	}
+#endif
+	return 1;
 }
 
@@ -150,22 +162,19 @@
 	return;
 #endif
-#if 0
-	uint32_t cpr;
-	asm volatile("MRC p15, 0, %0, c1, c1, 0" : "=r" (cpr)::);
-	if (cpr & 1)
-		printf("We are in unsecure state, we can't change access\n");
-
-	/* Allow non-secure access */
-	uint32_t nsacr;
-	asm volatile ("mrc p15, 0, %0, c1, c1, 2" :"=r" (nsacr)::);
-	/* FPU needs access to coprocessor 10 and 11.
-	 * Moreover, they need to have same access enabled */
-	nsacr |= NSACR_CP10_FLAG | NSACR_CP11_FLAG;
-	asm volatile ("mcr p15, 0, %0, c1, c1, 2" :"=r" (nsacr)::);
-
-#ifdef MACHINE_beagleboardxm
-	asm volatile ("isb" ::: "memory" );
-#endif
-#endif
+	if (sec_ext_is_implemented()) {
+		printf("Enabling FPU in %s context (%x)\n",
+			sec_ext_is_secure() ? "secure" : "unsecure",
+			SCR_read());
+		if (!sec_ext_is_secure()) {
+			sec_ext_call(SECURITY_CALL_ENABLE_CP10_11);
+		} else {
+			uint32_t nsacr = NSACR_read();
+			nsacr |= NSACR_CP_FLAG(10) | NSACR_CP_FLAG(11);
+			NSACR_write(nsacr);
+			printf("NSACR: %x => %x\n", nsacr, NSACR_read());
+			smc_coherence(0);
+		}
+	}
+
 	/* Allow coprocessor access */
 	uint32_t cpacr = CPACR_read();
@@ -184,8 +193,5 @@
 void fpu_init(void)
 {
-	/* Enable coprocessor access*/
-	fpu_enable_coprocessor_access();
-
-	/* Check if we succeeded */
+	/* Check if we have access */
 	if (!fpu_have_coprocessor_access())
 		return;
@@ -202,5 +208,8 @@
 void fpu_setup(void)
 {
-	/* Check if we have access */
+	/* Enable coprocessor access*/
+	fpu_enable_coprocessor_access();
+
+	/* Check if we succeeded */
 	if (!fpu_have_coprocessor_access())
 		return;