SAM3/4: Leverage some start-up logic from STM32

1 files changed, 166 insertions, 9 deletions

diff --git a/nuttx/arch/arm/src/sam34/sam_start.c b/nuttx/arch/arm/src/sam34/sam_start.c
index 0a51927a5..f8adc967b 100644
--- a/nuttx/arch/arm/src/sam34/sam_start.c
+++ b/nuttx/arch/arm/src/sam34/sam_start.c
@@ -1,7 +1,7 @@
 /****************************************************************************
  * arch/arm/src/sam34/sam_start.c
  *
- *   Copyright (C) 2009-2010, 2012-2013 Gregory Nutt. All rights reserved.
+ *   Copyright (C) 2009-2010, 2012-2013, 2015 Gregory Nutt. All rights reserved.
  *   Author: Gregory Nutt <gnutt@nuttx.org>
  *
  * Redistribution and use in source and binary forms, with or without
@@ -54,17 +54,27 @@
 #include "sam_cmcc.h"
 #include "sam_userspace.h"
 
-/****************************************************************************
- * Pre-processor Definitions
- ****************************************************************************/
+#ifdef CONFIG_ARCH_FPU
+#  include "nvic.h"
+#endif
 
 /****************************************************************************
- * Private Data
+ * Private Function prototypes
  ****************************************************************************/
 
-/****************************************************************************
- * Public Data
- ****************************************************************************/
+#ifdef CONFIG_ARCH_FPU
+static inline void sam_fpuconfig(void);
+#endif
+#ifdef CONFIG_STACK_COLORATION
+static void go_os_start(void *pv, unsigned int nbytes)
+  __attribute__ ((naked,no_instrument_function,noreturn));
+#endif
+
+#ifdef CONFIG_ARMV7M_STACKCHECK
+/* We need to get r10 set before we can allow instrumentation calls */
+
+void __start(void) __attribute__ ((no_instrument_function));
+#endif
 
 /****************************************************************************
  * Private Functions
@@ -85,6 +95,136 @@
 #endif
 
 /****************************************************************************
+ * Name: sam_fpuconfig
+ *
+ * Description:
+ *   Configure the FPU.  Relative bit settings:
+ *
+ *     CPACR:  Enables access to CP10 and CP11
+ *     CONTROL.FPCA: Determines whether the FP extension is active in the
+ *       current context:
+ *     FPCCR.ASPEN:  Enables automatic FP state preservation, then the
+ *       processor sets this bit to 1 on successful completion of any FP
+ *       instruction.
+ *     FPCCR.LSPEN:  Enables lazy context save of FP state. When this is
+ *       done, the processor reserves space on the stack for the FP state,
+ *       but does not save that state information to the stack.
+ *
+ *  Software must not change the value of the ASPEN bit or LSPEN bit while either:
+ *   - the CPACR permits access to CP10 and CP11, that give access to the FP
+ *     extension, or
+ *   - the CONTROL.FPCA bit is set to 1
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_ARCH_FPU
+#ifdef CONFIG_ARMV7M_CMNVECTOR
+
+static inline void sam_fpuconfig(void)
+{
+  uint32_t regval;
+
+  /* Set CONTROL.FPCA so that we always get the extended context frame
+   * with the volatile FP registers stacked above the basic context.
+   */
+
+  regval = getcontrol();
+  regval |= (1 << 2);
+  setcontrol(regval);
+
+  /* Ensure that FPCCR.LSPEN is disabled, so that we don't have to contend
+   * with the lazy FP context save behaviour.  Clear FPCCR.ASPEN since we
+   * are going to turn on CONTROL.FPCA for all contexts.
+   */
+
+  regval = getreg32(NVIC_FPCCR);
+  regval &= ~((1 << 31) | (1 << 30));
+  putreg32(regval, NVIC_FPCCR);
+
+  /* Enable full access to CP10 and CP11 */
+
+  regval = getreg32(NVIC_CPACR);
+  regval |= ((3 << (2*10)) | (3 << (2*11)));
+  putreg32(regval, NVIC_CPACR);
+}
+
+#else
+
+static inline void sam_fpuconfig(void)
+{
+  uint32_t regval;
+
+  /* Clear CONTROL.FPCA so that we do not get the extended context frame
+   * with the volatile FP registers stacked in the saved context.
+   */
+
+  regval = getcontrol();
+  regval &= ~(1 << 2);
+  setcontrol(regval);
+
+  /* Ensure that FPCCR.LSPEN is disabled, so that we don't have to contend
+   * with the lazy FP context save behaviour.  Clear FPCCR.ASPEN since we
+   * are going to keep CONTROL.FPCA off for all contexts.
+   */
+
+  regval = getreg32(NVIC_FPCCR);
+  regval &= ~((1 << 31) | (1 << 30));
+  putreg32(regval, NVIC_FPCCR);
+
+  /* Enable full access to CP10 and CP11 */
+
+  regval = getreg32(NVIC_CPACR);
+  regval |= ((3 << (2*10)) | (3 << (2*11)));
+  putreg32(regval, NVIC_CPACR);
+}
+
+#endif
+
+#else
+#  define sam_fpuconfig()
+#endif
+
+/****************************************************************************
+ * Name: go_os_start
+ *
+ * Description:
+ *   Set the IDLE stack to the
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_STACK_COLORATION
+static void go_os_start(void *pv, unsigned int nbytes)
+{
+  /* Set the IDLE stack to the stack coloration value then jump to
+   * os_start().  We take extreme care here because were currently
+   * executing on this stack.
+   *
+   * We want to avoid sneak stack access generated by the compiler.
+   */
+
+  __asm__ __volatile__
+  (
+    "\tmov  r1, r1, lsr #2\n"   /* R1 = nwords = nbytes >> 2 */
+    "\tbeq  2f\n"               /* (should not happen) */
+
+    "\tbic  r0, r0, #3\n"       /* R0 = Aligned stackptr */
+    "\tmovw r2, #0xbeef\n"      /* R2 = STACK_COLOR = 0xdeadbeef */
+    "\tmovt r2, #0xdead\n"
+
+    "1:\n"                      /* Top of the loop */
+    "\tsub  r1, r1, #1\n"       /* R1 nwords-- */
+    "\tcmp  r1, #0\n"           /* Check (nwords == 0) */
+    "\tstr  r2, [r0], #4\n"     /* Save stack color word, increment stackptr */
+    "\tbne  1b\n"               /* Bottom of the loop */
+
+    "2:\n"
+    "\tmov  r14, #0\n"          /* LR = return address (none) */
+    "\tb    os_start\n"         /* Branch to os_start */
+  );
+}
+#endif
+
+/****************************************************************************
  * Public Functions
  ****************************************************************************/
 
@@ -101,6 +241,12 @@ void __start(void)
   const uint32_t *src;
   uint32_t *dest;
 
+#ifdef CONFIG_ARMV7M_STACKCHECK
+  /* Set the stack limit before we attempt to call any functions */
+
+  __asm__ volatile ("sub r10, sp, %0" : : "r" (CONFIG_IDLETHREAD_STACKSIZE - 64) : );
+#endif
+
   /* Clear .bss.  We'll do this inline (vs. calling memset) just to be
    * certain that there are no issues with the state of global variables.
    */
@@ -135,9 +281,10 @@ void __start(void)
     }
 #endif
 
-  /* Configure the uart so that we can get debug output as soon as possible */
+  /* Configure the UART so that we can get debug output as soon as possible */
 
   sam_clockconfig();
+  sam_fpuconfig();
   sam_lowsetup();
   showprogress('A');
 
@@ -179,9 +326,19 @@ void __start(void)
 
   showprogress('\r');
   showprogress('\n');
+
+#ifdef CONFIG_STACK_COLORATION
+  /* Set the IDLE stack to the coloration value and jump into os_start() */
+
+  go_os_start((FAR void *)&_ebss, CONFIG_IDLETHREAD_STACKSIZE);
+
+#else
+  /* Call os_start() */
+
   os_start();
 
   /* Shouldn't get here */
 
   for (;;);
+#endif
 }