Enable FPU support for Link EC

With this CL, if CONFIG_FPU is defined (only for Link, ATM), the EC task
switcher will enable CONTROL.FPCA and expect all stack contexts to include
floating point state as well as normal state (an additional 18 words).

To support this, we need to increase the allocated stack space for each
task. The stack sizes are already chosen empirically, so I'm just rounding
them up a bit.

BUG=chrome-os-partner:14766
BRANCH=Link
TEST=manual

There should be no noticeable change. If you run the EC command "taskinfo"
you'll see the increased size each thread's stack, but everything that was
working before should continue to work just fine.

The additional overhead required to load and store another 18 words on each
context switch is not really measurable (I tried).

Change-Id: Ibaca7d7a2565285f049fda6906f32761e83207af
Signed-off-by: Bill Richardson <wfrichar@chromium.org>
Reviewed-on: https://gerrit.chromium.org/gerrit/34391
Reviewed-by: Vincent Palatin <vpalatin@chromium.org>

5 files changed, 27 insertions, 37 deletions

diff --git a/core/cortex-m/build.mk b/core/cortex-m/build.mk
index 7a2f16e4e9..35ad2d0517 100644
--- a/core/cortex-m/build.mk
+++ b/core/cortex-m/build.mk
@@ -14,5 +14,4 @@ CFLAGS_CPU=-mcpu=cortex-m4 -mthumb -Os -mno-sched-prolog
 CFLAGS_CPU+=$(CFLAGS_FPU-y)
 
 core-y=cpu.o init.o panic.o switch.o task.o timer.o
-core-$(CONFIG_FPU)+=fpu.o
 core-$(CONFIG_TASK_WATCHDOG)+=watchdog.o
diff --git a/core/cortex-m/fpu.c b/core/cortex-m/fpu.c
deleted file mode 100644
index 806b05438e..0000000000
--- a/core/cortex-m/fpu.c
+++ /dev/null
@@ -1,29 +0,0 @@
-/* Copyright (c) 2012 The Chromium OS Authors. All rights reserved.
- * Use of this source code is governed by a BSD-style license that can be
- * found in the LICENSE file.
- */
-
-/* FPU module for Chrome EC operating system */
-
-#include "task.h"
-
-void enable_fpu(void)
-{
-	interrupt_disable();
-	asm volatile("mrs r0, control;"
-		     "orr r0, r0, #(1 << 2);"
-		     "msr control, r0;"
-		     "isb;");
-}
-
-void disable_fpu(int32_t v)
-{
-	/* Optimization barrier to force compiler generate floating point
-	 * calculation code for 'v' before disabling FPU. */
-	asm volatile("" : : "r" (v) : "memory");
-	asm volatile("mrs r0, control;"
-		     "bic r0, r0, #(1 << 2);"
-		     "msr control, r0;"
-		     "isb;");
-	interrupt_enable();
-}
diff --git a/core/cortex-m/init.S b/core/cortex-m/init.S
index 22cece17cb..30883358f7 100644
--- a/core/cortex-m/init.S
+++ b/core/cortex-m/init.S
@@ -305,8 +305,12 @@ reset:
     /* Ensure we're in privileged mode with main stack.
      * Necessary if we've jumped directly here from another image
      * after task_start(). */
-    mov r0, #0
-    msr control, r0      @ use : priv. mode / main stack / no floating point
+#ifdef CONFIG_FPU
+    mov r0, #(1 << 2)    @ priv. mode / main stack / floating point on
+#else
+    mov r0, #0           @ priv. mode / main stack / no floating point
+#endif
+    msr control, r0
     isb                  @ ensure the write is done
     /* Set the vector table on our current code */
     ldr r1, =vectors
diff --git a/core/cortex-m/switch.S b/core/cortex-m/switch.S
index 1f94c15292..92c7e51247 100644
--- a/core/cortex-m/switch.S
+++ b/core/cortex-m/switch.S
@@ -35,8 +35,8 @@
 __switchto:
   mrs r3, psp            @ get the task stack where the context has been saved
   ldr r2, [r1]           @ get the new scheduled task stack pointer
-  stmdb r3!, {r4-r11}    @ save additional r4-r7 in the task stack
-  ldmia r2!, {r4-r11}    @ restore r4-r7 for the next task context
+  stmdb r3!, {r4-r11}    @ save additional r4-r11 in the task stack
+  ldmia r2!, {r4-r11}    @ restore r4-r11 for the next task context
   str r3, [r0]           @ save the task stack pointer in its context
   msr psp, r2            @ set the process stack pointer to exception context
   bx lr                  @ return from exception
@@ -49,13 +49,17 @@ __switchto:
 .thumb_func
 __task_start:
   ldr r2,=scratchpad     @ area used as dummy thread stack for the first switch
-  mov r3, #2
+#ifdef CONFIG_FPU
+  mov r3, #6             @ use : priv. mode / thread stack / floating point on
+#else
+  mov r3, #2             @ use : priv. mode / thread stack / no floating point
+#endif
   add r2, #17*4          @ put the pointer at the top of the stack
   mov r1, #0             @ __Schedule parameter : re-schedule nothing
   msr psp, r2            @ setup a thread stack up to the first context switch
   mov r2, #1
   isb                    @ ensure the write is done
-  msr control, r3        @ use : priv. mode / thread stack / no floating point
+  msr control, r3
   mov r3, r0
   mov r0, #0             @ __Schedule parameter : de-schedule nothing
   isb                    @ ensure the write is done
diff --git a/core/cortex-m/task.c b/core/cortex-m/task.c
index ba6e374501..7b4eb9ddfe 100644
--- a/core/cortex-m/task.c
+++ b/core/cortex-m/task.c
@@ -120,7 +120,11 @@ uint8_t task_stacks[0
 #undef TASK
 
 /* Reserve space to discard context on first context switch. */
+#ifdef CONFIG_FPU
+uint32_t scratchpad[17+18];
+#else
 uint32_t scratchpad[17];
+#endif
 
 static task_ *current_task = (task_ *)scratchpad;
 
@@ -573,8 +577,16 @@ int task_pre_init(void)
 
 		tasks[i].stack = stack_next;
 
-		/* Update stack used by first frame (16 uint32's) */
+		/*
+		 * Update stack used by first frame: 8 words for the normal
+		 * stack, plus 8 for R4-R11. With FP enabled, we need another
+		 * 18 words for S0-S15 and FPCSR and to align to 64-bit.
+		 */
+#ifdef CONFIG_FPU
+		sp = stack_next + ssize - 16 - 18;
+#else
 		sp = stack_next + ssize - 16;
+#endif
 		tasks[i].sp = (uint32_t)sp;
 
 		/* Initial context on stack (see __switchto()) */