diff --git a/arch/arm/include/armv7-a/irq.h b/arch/arm/include/armv7-a/irq.h
index 2f90e74993..157542e230 100755
--- a/arch/arm/include/armv7-a/irq.h
+++ b/arch/arm/include/armv7-a/irq.h
@@ -1,7 +1,7 @@
 /****************************************************************************
  * arch/arm/include/armv7-a/irq.h
  *
- *   Copyright (C) 2013 Gregory Nutt. All rights reserved.
+ *   Copyright (C) 2013-2014 Gregory Nutt. All rights reserved.
  *   Author: Gregory Nutt <gnutt@nuttx.org>
  *
  * Redistribution and use in source and binary forms, with or without
@@ -277,6 +277,9 @@ static inline irqstate_t irqsave(void)
     (
       "\tmrs    %0, cpsr\n"
       "\tcpsid  i\n"
+#if defined(CONFIG_ARMV7A_DECODEFIQ)
+      "\tcpsid  f\n"
+#endif
       : "=r" (cpsr)
       :
       : "memory"
@@ -295,6 +298,9 @@ static inline irqstate_t irqenable(void)
     (
       "\tmrs    %0, cpsr\n"
       "\tcpsie  i\n"
+#if defined(CONFIG_ARMV7A_DECODEFIQ)
+      "\tcpsie  f\n"
+#endif
       : "=r" (cpsr)
       :
       : "memory"
diff --git a/arch/arm/include/sama5/irq.h b/arch/arm/include/sama5/irq.h
index db163de5bc..1cf5d453e4 100644
--- a/arch/arm/include/sama5/irq.h
+++ b/arch/arm/include/sama5/irq.h
@@ -73,7 +73,8 @@
 
 #ifdef __cplusplus
 #define EXTERN extern "C"
-extern "C" {
+extern "C"
+{
 #else
 #define EXTERN extern
 #endif
diff --git a/arch/arm/src/armv7-a/arm_vectors.S b/arch/arm/src/armv7-a/arm_vectors.S
index c3ea3063fe..ccdf235e2c 100644
--- a/arch/arm/src/armv7-a/arm_vectors.S
+++ b/arch/arm/src/armv7-a/arm_vectors.S
@@ -63,6 +63,11 @@ g_undeftmp:
 g_aborttmp:
 	.word	0		/* Saved lr */
 	.word	0		/* Saved spsr */
+#ifdef CONFIG_ARMV7A_DECODEFIQ
+g_fiqtmp:
+	.word	0		/* Saved lr */
+	.word	0		/* Saved spsr */
+#endif
 
 /************************************************************************************
  * Assembly Macros
@@ -86,6 +91,7 @@ g_aborttmp:
  *
  ************************************************************************************/
 
+	.globl	arm_decodeirq
 	.globl	arm_vectorirq
 	.type	arm_vectorirq, %function
 
@@ -153,7 +159,6 @@ arm_vectorirq:
 
 	/* Restore the CPSR, SVC mode registers and return */
 
-.Lnoirqset:
 	ldr		r1, [r0, #(4*REG_CPSR)]	/* Setup the SVC mode SPSR */
 	msr		spsr, r1
 	ldmia	r0, {r0-r15}^			/* Return */
@@ -175,6 +180,7 @@ arm_vectorirq:
  *
  ************************************************************************************/
 
+	.globl	arm_syscall
 	.globl	arm_vectorsvc
 	.type	arm_vectorsvc, %function
 
@@ -235,6 +241,7 @@ arm_vectorsvc:
  *
  ************************************************************************************/
 
+	.globl	arm_dataabort
 	.globl	arm_vectordata
 	.type	arm_vectordata, %function
 
@@ -317,6 +324,7 @@ arm_vectordata:
  *
  ************************************************************************************/
 
+	.globl	arm_prefetchabort
 	.globl	arm_vectorprefetch
 	.type	arm_vectorprefetch, %function
 
@@ -397,6 +405,7 @@ arm_vectorprefetch:
  *
  ************************************************************************************/
 
+	.globl	arm_undefinedinsn
 	.globl	arm_vectorundefinsn
 	.type	arm_vectorundefinsn, %function
 
@@ -474,12 +483,86 @@ arm_vectorundefinsn:
  *
  ************************************************************************************/
 
+#ifdef CONFIG_ARMV7A_DECODEFIQ
+	.globl	arm_decodefiq
+#endif
 	.globl	arm_vectorfiq
 	.type	arm_vectorfiq, %function
 
 arm_vectorfiq:
 #ifdef CONFIG_ARMV7A_DECODEFIQ
-#  error Missing logic
+	/* On entry we are free to use the FIQ mode registers r8 through r14 */
+
+	ldr		r13, .Lfiqtmp			/* Points to temp storage */
+	sub		lr, lr, #4				/* Fixup return */
+	str		lr, [r13]				/* Save in temp storage */
+	mrs		lr, spsr				/* Get SPSR_fiq */
+	str		lr, [r13, #4]			/* Save in temp storage */
+
+	/* Then switch back to SVC mode */
+
+	bic		lr, lr, #PSR_MODE_MASK	/* Keep F and T bits */
+	orr		lr, lr, #(PSR_MODE_SVC | PSR_I_BIT)
+	msr		cpsr_c, lr				/* Switch to SVC mode */
+
+	/* Create a context structure.  First set aside a stack frame
+	 * and store r0-r12 into the frame.
+	 */
+
+	sub		sp, sp, #XCPTCONTEXT_SIZE
+	stmia	sp, {r0-r12}			/* Save the SVC mode regs */
+
+	/* Get the correct values of r13(sp) and r14(lr) in r1 and r2 */
+
+	add		r1, sp, #XCPTCONTEXT_SIZE
+	mov		r2, r14
+
+	/* Get the values for r15(pc) and CPSR in r3 and r4 */
+
+	ldr		r0, .Lfiqtmp			/* Points to temp storage */
+	ldmia	r0, {r3, r4}			/* Recover r1=lr_IRQ, r2=spsr_IRQ */
+
+	add		r0, sp, #(4*REG_SP)		/* Offset to pc, cpsr storage */
+	stmia	r0, {r1-r4}
+
+	/* Then call the IRQ handler with interrupts disabled. */
+
+	mov		fp, #0					/* Init frame pointer */
+	mov		r0, sp					/* Get r0=xcp */
+
+#if CONFIG_ARCH_INTERRUPTSTACK > 3
+	ldr		sp, .Lfiqstackbase		/* SP = interrupt stack base */
+	str		r0, [sp]				/* Save the user stack pointer */
+	mov		r4, sp					/* Save the SP in a preserved register */
+	bic		sp, sp, #7				/* Force 8-byte alignement */
+	bl		arm_decodefiq			/* Call the handler */
+	ldr		sp, [r4]				/* Restore the user stack pointer */
+#else
+	mov		r4, sp					/* Save the SP in a preserved register */
+	bic		sp, sp, #7				/* Force 8-byte alignement */
+	bl		arm_decodefiq			/* Call the handler */
+	mov		sp, r4					/* Restore the possibly unaligned stack pointer */
+#endif
+
+	/* Upon return from arm_decodefiq, r0 holds the pointer to the register
+	 * state save area to use to restore the registers.  This may or may not
+	 * be the same value that was passed to arm_decodefiq:  It will differ if a
+	 * context switch is required.
+	 */
+
+	/* Restore the CPSR, SVC mode registers and return */
+
+	ldr		r1, [r0, #(4*REG_CPSR)]	/* Setup the SVC mode SPSR */
+	msr		spsr, r1
+	ldmia	r0, {r0-r15}^			/* Return */
+
+.Lfiqtmp:
+	.word	g_fiqtmp
+#if CONFIG_ARCH_INTERRUPTSTACK > 3
+.Lfiqstackbase:
+	.word	g_fiqstackbase
+#endif
+
 #else
 	subs	pc, lr, #4
 #endif
@@ -492,15 +575,33 @@ arm_vectorfiq:
 #if CONFIG_ARCH_INTERRUPTSTACK > 3
 	.bss
 	.align	4
+
 	.globl	g_intstackalloc
 	.type	g_intstackalloc, object
 	.globl	g_intstackbase
 	.type	g_intstackbase, object
+
 g_intstackalloc:
 	.skip	((CONFIG_ARCH_INTERRUPTSTACK & ~3) - 4)
 g_intstackbase:
 	.skip	4
 	.size	g_intstackbase, 4
 	.size	g_intstackalloc, (CONFIG_ARCH_INTERRUPTSTACK & ~3)
-#endif
+
+#ifdef CONFIG_ARMV7A_DECODEFIQ
+
+	.globl	g_fiqstackalloc
+	.type	g_fiqstackalloc, object
+	.globl	g_fiqstackbase
+	.type	g_fiqstackbase, object
+
+g_fiqstackalloc:
+	.skip	((CONFIG_ARCH_INTERRUPTSTACK & ~3) - 4)
+g_fiqstackbase:
+	.skip	4
+	.size	g_fiqstackbase, 4
+	.size	g_fiqstackalloc, (CONFIG_ARCH_INTERRUPTSTACK & ~3)
+
+#endif /* CONFIG_ARMV7A_DECODEFIQ */
+#endif /* CONFIG_ARCH_INTERRUPTSTACK > 3 */
 	.end
diff --git a/arch/arm/src/sama5/Kconfig b/arch/arm/src/sama5/Kconfig
index c1e8d10e55..c98379d927 100644
--- a/arch/arm/src/sama5/Kconfig
+++ b/arch/arm/src/sama5/Kconfig
@@ -80,6 +80,7 @@ config SAMA5_HAVE_HSMCI2
 config SAMA5_HAVE_SAIC
 	bool
 	default n
+	select ARMV7A_DECODEFIQ
 
 config SAMA5_HAVE_SBM
 	bool
@@ -531,7 +532,8 @@ config SAMA5_SECURE
 	bool "Enable secure features"
 	default n
 	depends on SAMA5_HAVE_TRUSTZONE && EXPERIMENTAL
-	select ARMV7A_DECODEFIQ if SAMA5_HAVE_SAIC
+	---help---
+		Does not do anything now, but may in the future
 
 config SAMA5_PIO_IRQ
 	bool "PIO pin interrupts"
diff --git a/arch/arm/src/sama5/sam_irq.c b/arch/arm/src/sama5/sam_irq.c
index d8cfdfd8ab..78d791fd91 100644
--- a/arch/arm/src/sama5/sam_irq.c
+++ b/arch/arm/src/sama5/sam_irq.c
@@ -136,7 +136,7 @@ static void sam_dumpaic(const char *msg, uintptr_t base, int irq)
 
   /* SAMA5D4 does not have the FFSR register */
 
-#ifdef SAM_AIC_FFSR
+#if defined(SAM_AIC_FFSR)
   lldbg("  IMR: %08x CISR: %08x  SPU: %08x FFSR: %08x\n",
         getreg32(base + SAM_AIC_IMR_OFFSET),
         getreg32(base + SAM_AIC_CISR_OFFSET),
@@ -253,7 +253,7 @@ static uint32_t *sam_fiqhandler(int irq, uint32_t *regs)
  *
  ****************************************************************************/
 
-#if defined(CONFIG_SAMA5_SECURE) && defined(CONFIG_SAMA5_HAVE_SAIC)
+#if defined(CONFIG_SAMA5_HAVE_SAIC)
 static inline bool sam_aic_ish64mx(uint32_t irq)
 {
   return (irq == SAM_IRQ_ARM    ||
@@ -280,7 +280,7 @@ static inline bool sam_aic_ish64mx(uint32_t irq)
  *
  ****************************************************************************/
 
-#if defined(CONFIG_SAMA5_SECURE) && defined(CONFIG_SAMA5_HAVE_SAIC)
+#if defined(CONFIG_SAMA5_HAVE_SAIC)
 static bool sam_aic_issecure(uint32_t irq)
 {
   uintptr_t regaddr;
@@ -418,7 +418,7 @@ void up_irqinitialize(void)
 
   sam_aic_initialize(SAM_AIC_VBASE);
 
-#if defined(CONFIG_SAMA5_SECURE) && defined(CONFIG_SAMA5_HAVE_SAIC)
+#if defined(CONFIG_SAMA5_HAVE_SAIC)
   /* Initialize the Secure Advanced Interrupt Controller (SAIC) */
 
   sam_aic_initialize(SAM_SAIC_VBASE);
@@ -621,10 +621,10 @@ uint32_t *arm_decodeirq(uint32_t *regs)
   return sam_decodeirq(SAM_AIC_VBASE, regs);
 }
 
-#if defined(CONFIG_SAMA5_SECURE) && defined(CONFIG_SAMA5_HAVE_SAIC)
+#if defined(CONFIG_SAMA5_HAVE_SAIC)
 /* This is the entry point from the ARM FIQ vector handler */
 
-uint32_t *arm_decodefiq(uint32_t *regs)
+uint32_t *arm_decodefiq(FAR uint32_t *regs)
 {
   /* In order to distinguish a FIQ from a true secure interrupt we need to
    * check the state of the FIQ line in the SAIC_CISR register.
@@ -632,7 +632,7 @@ uint32_t *arm_decodefiq(uint32_t *regs)
 
   if ((getreg32(SAM_SAIC_CISR) & AIC_CISR_NFIQ) != 0)
     {
-      return sam_fiqhandler(SAM_IRQ_FIQ, regs);
+      return arm_doirq(SAM_IRQ_FIQ, regs);
     }
   else
     {
@@ -681,7 +681,7 @@ static void sam_disable_irq(uintptr_t base, int irq)
 
 void up_disable_irq(int irq)
 {
-#if defined(CONFIG_SAMA5_SECURE) && defined(CONFIG_SAMA5_HAVE_SAIC)
+#if defined(CONFIG_SAMA5_HAVE_SAIC)
   if (sam_aic_issecure(irq))
     {
       sam_disable_irq(SAM_SAIC_VBASE, irq);
@@ -733,7 +733,7 @@ static void sam_enable_irq(uintptr_t base, int irq)
 
 void up_enable_irq(int irq)
 {
-#if defined(CONFIG_SAMA5_SECURE) && defined(CONFIG_SAMA5_HAVE_SAIC)
+#if defined(CONFIG_SAMA5_HAVE_SAIC)
   if (sam_aic_issecure(irq))
     {
       sam_enable_irq(SAM_SAIC_VBASE, irq);
@@ -809,7 +809,7 @@ static int sam_prioritize_irq(uint32_t base, int irq, int priority)
 
 int up_prioritize_irq(int irq, int priority)
 {
-#if defined(CONFIG_SAMA5_SECURE) && defined(CONFIG_SAMA5_HAVE_SAIC)
+#if defined(CONFIG_SAMA5_HAVE_SAIC)
   if (sam_aic_issecure(irq))
     {
       sam_prioritize_irq(SAM_SAIC_VBASE, irq, priority);
@@ -867,7 +867,7 @@ static void _sam_irq_srctype(uintptr_t base, int irq,
 
 void sam_irq_srctype(int irq, enum sam_srctype_e srctype)
 {
-#if defined(CONFIG_SAMA5_SECURE) && defined(CONFIG_SAMA5_HAVE_SAIC)
+#if defined(CONFIG_SAMA5_HAVE_SAIC)
   if (sam_aic_issecure(irq))
     {
       _sam_irq_srctype(SAM_SAIC_VBASE, irq, srctype);
diff --git a/configs/sama5d4-ek/ramtest/defconfig b/configs/sama5d4-ek/ramtest/defconfig
index 02b909b98f..3a08e5fdde 100644
--- a/configs/sama5d4-ek/ramtest/defconfig
+++ b/configs/sama5d4-ek/ramtest/defconfig
@@ -118,7 +118,7 @@ CONFIG_ARMV7A_TOOLCHAIN_CODESOURCERYW=y
 # CONFIG_ARMV7A_TOOLCHAIN_GNU_EABIL is not set
 # CONFIG_ARMV7A_TOOLCHAIN_GNU_EABIW is not set
 # CONFIG_ARMV7A_TOOLCHAIN_GNU_OABI is not set
-# CONFIG_ARMV7A_DECODEFIQ is not set
+CONFIG_ARMV7A_DECODEFIQ=y
 
 #
 # SAMA5 Configuration Options
@@ -213,7 +213,6 @@ CONFIG_SAMA5_USART3=y
 # CONFIG_SAMA5_FUSE is not set
 CONFIG_SAMA5_MPDDRC=y
 # CONFIG_SAMA5_VDEC is not set
-# CONFIG_SAMA5_SECURE is not set
 # CONFIG_SAMA5_PIO_IRQ is not set
 
 #