freertos: Added experimental, optional FPU usage on level 1 ISR

components/freertos/Kconfig

@@ -420,4 +420,12 @@ menu "FreeRTOS"
         help
             Hidden option, gets selected by CONFIG_ESPxx_DEBUG_OCDAWARE
 
+    config FREERTOS_FPU_IN_ISR
+        bool "Allow use of float inside Level 1 ISR (EXPERIMENTAL)"
+        depends on IDF_TARGET_ESP32
+        default n
+        help
+            When enabled, the usage of float type is allowed inside Level 1
+            ISRs.
+
 endmenu

components/freertos/portasm.S

@@ -138,8 +138,24 @@ _frxt_int_enter:
 	mull	a2,  a4, a2
 	add     a1,  a1, a2					/* for current proc */
 
+    #ifdef CONFIG_FREERTOS_FPU_IN_ISR
+    #if XCHAL_CP_NUM > 0
+    rsr     a3, CPENABLE                /* Restore thread scope CPENABLE */
+    addi    sp, sp,-4                   /* ISR will manage FPU coprocessor by forcing */
+    s32i    a3, a1, 0                   /* its trigger */
+    #endif
+    #endif
+
 .Lnested:
 1:
+    #ifdef CONFIG_FREERTOS_FPU_IN_ISR
+    #if XCHAL_CP_NUM > 0
+    movi    a3,  0              /* whilst ISRs pending keep CPENABLE exception active */
+    wsr     a3,  CPENABLE
+    rsync               
+    #endif
+    #endif
+
     mov     a0,  a12                    /* restore return addr and return  */
     ret
 
@@ -176,6 +192,15 @@ _frxt_int_exit:
     s32i    a2,  a3, 0                  /* save nesting count              */
     bnez    a2,  .Lnesting              /* !=0 after decr so still nested  */
 
+    #ifdef CONFIG_FREERTOS_FPU_IN_ISR 
+    #if XCHAL_CP_NUM > 0
+    l32i    a3,  sp, 0                  /* Grab last CPENABLE before leave ISR */
+    addi    sp,  sp, 4                    
+    wsr     a3, CPENABLE
+    rsync                               /* ensure CPENABLE was modified */
+    #endif
+    #endif
+
     movi    a2,  pxCurrentTCB
 	addx4	a2,  a4, a2
     l32i    a2,  a2, 0                  /* a2 = current TCB                */
@@ -642,7 +667,6 @@ _frxt_task_coproc_state:
 	addx4	a15, a3, a15
     l32i    a15, a15, 0                 /* && pxCurrentTCB != 0) {                 */
 
-
     beqz    a15, 2f
     l32i    a15, a15, CP_TOPOFSTACK_OFFS
     ret

components/freertos/test/test_float_in_isr.c

@@ -14,6 +14,7 @@
 #include "math.h"
   
 #define SW_ISR_LEVEL_1  7
+#ifdef CONFIG_FREERTOS_FPU_IN_ISR
 
 struct fp_test_context {
     SemaphoreHandle_t sync;
@@ -27,8 +28,8 @@ static void software_isr(void *arg) {
 
     struct fp_test_context *ctx = (struct fp_test_context *)arg;
 
-    for(int i = 0; i < 10; i++) {
+    for(int i = 0; i < 16; i++) {
-        ctx->expected = ctx->expected * 2.0f;
+        ctx->expected = ctx->expected * 2.0f * cosf(0.0f);
     }
 
     xSemaphoreGiveFromISR(ctx->sync, &yield);
@@ -38,9 +39,10 @@ static void software_isr(void *arg) {
 }
 
 
-TEST_CASE("Floating point usage in ISR test", "[freertos] [ignore]")
+TEST_CASE("Floating point usage in ISR test", "[freertos]" "[fp]")
 {
     struct fp_test_context ctx;
+    float  fp_math_operation_result = 0.0f;
 
     intr_handle_t handle;
     esp_err_t err = esp_intr_alloc(ETS_INTERNAL_SW0_INTR_SOURCE, ESP_INTR_FLAG_LEVEL1, &software_isr, &ctx, &handle);
@@ -50,11 +52,16 @@ TEST_CASE("Floating point usage in ISR test", "[freertos] [ignore]")
     TEST_ASSERT(ctx.sync != NULL);
     ctx.expected = 1.0f;
 
+    fp_math_operation_result = cosf(0.0f);
+
     xt_set_intset(1 << SW_ISR_LEVEL_1);
     xSemaphoreTake(ctx.sync, portMAX_DELAY);
 
     esp_intr_free(handle);
     vSemaphoreDelete(ctx.sync);
 
-    TEST_ASSERT_FLOAT_WITHIN(0.1f, ctx.expected, 1024.0f);
+    printf("FP math isr result: %f \n", ctx.expected);
+    TEST_ASSERT_FLOAT_WITHIN(0.1f, ctx.expected, fp_math_operation_result * 65536.0f);
 }
+
+#endif

components/freertos/xtensa_vectors.S

@@ -955,7 +955,12 @@ _xt_coproc_exc:
 
     /* Get co-processor state save area of new owner thread. */
     call0   XT_RTOS_CP_STATE                /* a15 = new owner's save area */
-    beqz    a15, .L_goto_invalid            /* not in a thread (invalid) */
+
+    #ifndef CONFIG_FREERTOS_FPU_IN_ISR     
+    beqz    a15, .L_goto_invalid
+    #endif
+    
+    /*When FPU in ISR is enabled we could deal with zeroed a15 */
 
     /* Enable the co-processor's bit in CPENABLE. */
     movi    a0, _xt_coproc_mask
@@ -997,7 +1002,13 @@ locking.
     rsync                                    /* ensure wsr.CPENABLE is complete */
 
     /* Only need to context switch if new owner != old owner. */
+    /* If float is necessary on ISR, we need to remove this check */
+    /* below, because on restoring from ISR we may have new == old condition used
+     * to force cp restore to next thread
+     */
+    #ifndef CONFIG_FREERTOS_FPU_IN_ISR     
     beq     a15, a2, .L_goto_done           /* new owner == old, we're done */
+    #endif
 
     /* If no old owner then nothing to save. */
     beqz    a2, .L_check_new
@@ -1039,6 +1050,7 @@ locking.
 .L_check_new:
     /* Check if any state has to be restored for new owner. */
     /* NOTE: a15 = new owner's save area, cannot be zero when we get here. */
+    beqz    a15, .L_xt_coproc_done
 
     l16ui   a3,  a15, XT_CPSTORED           /* a3 = new owner's CPSTORED */
     movi    a4, _xt_coproc_sa_offset
@@ -1133,7 +1145,6 @@ _xt_lowint1:
     #endif
     #endif
 
-
     /* Save rest of interrupt context and enter RTOS. */
     call0   XT_RTOS_INT_ENTER               /* common RTOS interrupt entry */
 

docs/en/api-guides/freertos-smp.rst

@@ -409,8 +409,9 @@ the state of a core's FPU registers are not immediately saved when a context
 switch occurs. Therefore, tasks that utilize ``float`` must be pinned to a
 particular core upon creation. If not, ESP-IDF FreeRTOS will automatically pin
 the task in question to whichever core the task was running on upon the task's 
-first use of ``float``. Likewise due to Lazy Context Switching, interrupt service 
+first use of ``float``. Likewise due to Lazy Context Switching, only interrupt
-routines must also not use ``float``.
+service routines of lowest priority (that is it the Level 1) can use ``float``, 
+higher priority interrupts do not support FPU usage.
 
 ESP32 does not support hardware acceleration for double precision floating point
 arithmetic (``double``). Instead ``double`` is implemented via software hence the