From b3309a03a3561cf63c0a9aa5eb7bcce0bed2fae6 Mon Sep 17 00:00:00 2001
From: Jeroen Domburg <git@j0h.nl>
Date: Wed, 28 Sep 2016 17:02:44 +0800
Subject: [PATCH] Automatically pin no-cpu-affinity task to a core when FPU is
 used

---
 components/freertos/include/freertos/task.h |  6 ++++++
 components/freertos/tasks.c                 | 12 ++++++++++++
 components/freertos/xtensa_vectors.S        | 15 +++++++++++++++
 3 files changed, 33 insertions(+)

diff --git a/components/freertos/include/freertos/task.h b/components/freertos/include/freertos/task.h
index ddf7a7589d..9f3f3d659e 100644
--- a/components/freertos/include/freertos/task.h
+++ b/components/freertos/include/freertos/task.h
@@ -1979,6 +1979,12 @@ BaseType_t xTaskGenericCreate( TaskFunction_t pxTaskCode, const char * const pcN
  */
 UBaseType_t uxTaskGetTaskNumber( TaskHandle_t xTask ) PRIVILEGED_FUNCTION;
 
+
+/*
+ * Get the current core affinity of a task
+ */
+BaseType_t xTaskGetAffinity( TaskHandle_t xTask ) PRIVILEGED_FUNCTION;
+
 /*
  * Set the uxTaskNumber of the task referenced by the xTask parameter to
  * uxHandle.
diff --git a/components/freertos/tasks.c b/components/freertos/tasks.c
index b9035bda0f..bdf3712b7d 100644
--- a/components/freertos/tasks.c
+++ b/components/freertos/tasks.c
@@ -3337,6 +3337,18 @@ TCB_t *pxNewTCB;
 }
 /*-----------------------------------------------------------*/
 
+BaseType_t xTaskGetAffinity( TaskHandle_t xTask )
+{
+	TCB_t *pxTCB;
+	UBaseType_t uxReturn;
+
+	pxTCB = prvGetTCBFromHandle( xTask );
+
+	return pxTCB->xCoreID;
+}
+/*-----------------------------------------------------------*/
+
+
 #if ( configUSE_TRACE_FACILITY == 1 )
 
 	static UBaseType_t prvListTaskWithinSingleList( TaskStatus_t *pxTaskStatusArray, List_t *pxList, eTaskState eState )
diff --git a/components/freertos/xtensa_vectors.S b/components/freertos/xtensa_vectors.S
index 0ff6c4c19b..57bfbe9f1e 100644
--- a/components/freertos/xtensa_vectors.S
+++ b/components/freertos/xtensa_vectors.S
@@ -92,6 +92,7 @@ SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 
 #include "xtensa_rtos.h"
 
+.extern pxCurrentTCB
 
 /* Enable stack backtrace across exception/interrupt - see below */
 #define XT_DEBUG_BACKTRACE    0
@@ -892,6 +893,20 @@ _xt_coproc_exc:
     addx4   a0, a5, a0                      /* a0 = &_xt_coproc_mask[n] */
     l32i    a0, a0, 0                       /* a0 = (n << 16) | (1 << n) */
 
+    /* TODO: Remove this as soon as coprocessor state moving works across cores - JD */
+    /* FPU operations are incompatible with non-pinned tasks. If we have a FPU operation
+       here, to keep the entire thing from crashing, it's better to pin the task to whatever
+       core we're running on now. */
+    movi    a2, pxCurrentTCB
+    getcoreid a3
+    slli    a3,  a3, 2
+    add     a2,  a2, a3
+    l32i    a2, a2, 0                       /* a2 = start of pxCurrentTCB[cpuid] */
+    addi    a2, a2, (0x3C+configMAX_TASK_NAME_LEN+3)&~3 /* offset to xCoreID in tcb struct */
+    getcoreid a3
+    s32i    a3, a2, 0                       /* store current cpuid */
+
+	/* Grab correct xt_coproc_owner_sa for this core */
 	getcoreid a2
 	movi    a3, XCHAL_CP_MAX << 2
 	mull    a2, a2, a3