Implemented lazy FPU, GP relaxation, and QEMU automation for GNU port in arch/risc-v32 (#513)

This PR adds three functional improvements to the RISC-V 32-bit GNU port.

Lazy FPU stacking (tx_thread_context_save.S, tx_thread_context_restore.S, tx_thread_schedule.S): FP register save/restore is now skipped whenmstatus.FS is Off, reducing context switch overhead for threads that do not use floating point.

GP relaxation (cmake/riscv32_gnu.cmake, entry.s, link.lds): Enables the -mrelax compiler flag and defines __global_pointer$ in the linker script.The entry stub initializes gp at startup. gp is not saved or restored during context switches.

WFI in idle loop (tx_thread_schedule.S): The scheduler issues wfi when no thread is ready, replacing busy-waiting with a low-power sleep.

A Python/QEMU/GDB functional test runner is added under ports/risc-v32/gnu/example_build/qemu_virt/test/. It validates context switching, FPUcontext preservation, timer interrupts, and preemption. To run:

cd ports/risc-v32/gnu/example_build/qemu_virt
make check-functional-riscv32

Tested on QEMU virt machine (rv32gc).

Co-authored-by: Wei-Chen Lai [email protected]
Co-authored-by: Frédéric Desbiens [email protected]
Co-authored-by: Copilot [email protected]

11 files changed, 410 insertions, 20 deletions

diff --git a/.gitignore b/.gitignore
index d2216ddb..349eaddc 100644
--- a/.gitignore
+++ b/.gitignore
@@ -4,6 +4,7 @@
 .tmp/
 _deps/
 build/
+build_qemu/
 Debug/
 CMakeFiles/
 CMakeScripts/
diff --git a/cmake/riscv32_gnu.cmake b/cmake/riscv32_gnu.cmake
index ec8f9857..6843423c 100644
--- a/cmake/riscv32_gnu.cmake
+++ b/cmake/riscv32_gnu.cmake
@@ -7,7 +7,7 @@ if(DEFINED SOFT_FLOAT)
     set(ARCH_FLAGS "-g -march=rv32ima_zicsr -mabi=ilp32 -mcmodel=medany")
     set(CACHE{SOFT_FLOAT} FORCE 1)
 else()
-    set(ARCH_FLAGS "-g -march=rv32gc -mabi=ilp32d -mcmodel=medany")
+    set(ARCH_FLAGS "-g -march=rv32gc -mabi=ilp32d -mcmodel=medany -mrelax")
 endif()
 set(CFLAGS "${ARCH_FLAGS}")
 set(ASFLAGS "${ARCH_FLAGS}")
diff --git a/ports/risc-v32/gnu/CMakeLists.txt b/ports/risc-v32/gnu/CMakeLists.txt
index 9357c697..7c6785bb 100644
--- a/ports/risc-v32/gnu/CMakeLists.txt
+++ b/ports/risc-v32/gnu/CMakeLists.txt
@@ -17,3 +17,7 @@ target_include_directories(${PROJECT_NAME}
     PUBLIC
     ${CMAKE_CURRENT_LIST_DIR}/inc
 )
+
+if(EXISTS ${CMAKE_CURRENT_LIST_DIR}/example_build/qemu_virt/CMakeLists.txt)
+    add_subdirectory(${CMAKE_CURRENT_LIST_DIR}/example_build/qemu_virt)
+endif()
diff --git a/ports/risc-v32/gnu/example_build/qemu_virt/CMakeLists.txt b/ports/risc-v32/gnu/example_build/qemu_virt/CMakeLists.txt
new file mode 100644
index 00000000..9e9ff722
--- /dev/null
+++ b/ports/risc-v32/gnu/example_build/qemu_virt/CMakeLists.txt
@@ -0,0 +1,45 @@
+set(QEMU_DEMO_DIR ${CMAKE_CURRENT_LIST_DIR})
+
+add_executable(kernel.elf EXCLUDE_FROM_ALL
+    ${QEMU_DEMO_DIR}/demo_threadx.c
+    ${QEMU_DEMO_DIR}/entry.s
+    ${QEMU_DEMO_DIR}/uart.c
+    ${QEMU_DEMO_DIR}/plic.c
+    ${QEMU_DEMO_DIR}/hwtimer.c
+    ${QEMU_DEMO_DIR}/trap.c
+    ${QEMU_DEMO_DIR}/board.c
+    ${QEMU_DEMO_DIR}/tx_initialize_low_level.S
+)
+
+target_link_libraries(kernel.elf PRIVATE threadx)
+
+target_include_directories(kernel.elf PRIVATE
+    ${CMAKE_SOURCE_DIR}/common/inc
+    ${CMAKE_SOURCE_DIR}/ports/${THREADX_ARCH}/${THREADX_TOOLCHAIN}/inc
+    ${QEMU_DEMO_DIR}
+)
+
+target_link_options(kernel.elf PRIVATE
+    -T${QEMU_DEMO_DIR}/link.lds
+    -nostartfiles
+    -Wl,-Map=kernel.map
+)
+
+# QEMU/GDB functional test runner.  Optional: skipped silently if the
+# host has no Python 3 interpreter on PATH.
+find_package(Python3 COMPONENTS Interpreter)
+if(Python3_FOUND)
+    add_custom_target(check-functional-riscv32
+        COMMAND ${Python3_EXECUTABLE}
+            ${QEMU_DEMO_DIR}/test/azrtos_test_tx_gnu_riscv32_qemu.py
+            --elf $<TARGET_FILE:kernel.elf>
+            --qemu qemu-system-riscv32
+            --gdb gdb
+        DEPENDS kernel.elf
+        WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}
+        COMMENT "Running RISC-V32 QEMU/GDB functional test runner..."
+    )
+else()
+    message(STATUS
+        "Python3 not found; check-functional-riscv32 target unavailable.")
+endif()
diff --git a/ports/risc-v32/gnu/example_build/qemu_virt/demo_threadx.c b/ports/risc-v32/gnu/example_build/qemu_virt/demo_threadx.c
index f21dbb26..6cf49014 100644
--- a/ports/risc-v32/gnu/example_build/qemu_virt/demo_threadx.c
+++ b/ports/risc-v32/gnu/example_build/qemu_virt/demo_threadx.c
@@ -9,6 +9,7 @@
 #define     DEMO_BYTE_POOL_SIZE     9120
 #define     DEMO_BLOCK_POOL_SIZE    100
 #define     DEMO_QUEUE_SIZE         100
+float           fpu_test_val = 0.0f;
 
 char *_to_str(ULONG val)
 {
@@ -201,7 +202,7 @@ UINT    status;
         thread_0_counter++;
 
         /* Sleep for 10 ticks.  */
-        tx_thread_sleep(10);
+        tx_thread_sleep(1);
 
         /* Set event flag 0 to wakeup thread 5.  */
         status =  tx_event_flags_set(&event_flags_0, 0x1, TX_OR);
@@ -363,6 +364,8 @@ UINT    status;
         if (status != TX_SUCCESS)
             break;
 
+        /* FPU Test */
+        fpu_test_val += 1.1f;
         /* Get the mutex again with suspension.  This shows
            that an owning thread may retrieve the mutex it
            owns multiple times.  */
diff --git a/ports/risc-v32/gnu/example_build/qemu_virt/entry.s b/ports/risc-v32/gnu/example_build/qemu_virt/entry.s
index 9b202ca1..06cc2a2c 100644
--- a/ports/risc-v32/gnu/example_build/qemu_virt/entry.s
+++ b/ports/risc-v32/gnu/example_build/qemu_virt/entry.s
@@ -1,5 +1,5 @@
 
-.section .text
+.section .text.boot, "ax"
 .align 4
 .global _start
 .extern main
@@ -11,7 +11,10 @@ _start:
 	bne  t0, zero, 1f
 	li x1, 0
 	li x2, 0
-	li x3, 0
+.option push
+.option norelax
+	la gp, __global_pointer$        /* x3 = gp; norelax keeps this load absolute */
+.option pop
 	li x4, 0
 	li x5, 0
 	li x6, 0
diff --git a/ports/risc-v32/gnu/example_build/qemu_virt/link.lds b/ports/risc-v32/gnu/example_build/qemu_virt/link.lds
index 522f90d9..d2ef5f28 100644
--- a/ports/risc-v32/gnu/example_build/qemu_virt/link.lds
+++ b/ports/risc-v32/gnu/example_build/qemu_virt/link.lds
@@ -10,6 +10,7 @@ SECTIONS
   . = 0x80000000;
 
   .text : {
+    KEEP(*(.text.boot))   /* entry.s _start — must be first at 0x80000000 */
     *(.text .text.*)
     . = ALIGN(0x1000);
     PROVIDE(etext = .);
@@ -24,6 +25,9 @@ SECTIONS
 
   .data : {
     . = ALIGN(16);
+    /* Centre __global_pointer$ in the small-data window so the +/-2 KiB
+       reach of GP-relative addressing covers the .sdata/.sbss area. */
+    PROVIDE( __global_pointer$ = . + 0x800 );
     *(.sdata .sdata.*) /* do not need to distinguish this from .data */
     . = ALIGN(16);
     *(.data .data.*)
diff --git a/ports/risc-v32/gnu/example_build/qemu_virt/test/azrtos_test_tx_gnu_riscv32_qemu.py b/ports/risc-v32/gnu/example_build/qemu_virt/test/azrtos_test_tx_gnu_riscv32_qemu.py
new file mode 100644
index 00000000..858b96e6
--- /dev/null
+++ b/ports/risc-v32/gnu/example_build/qemu_virt/test/azrtos_test_tx_gnu_riscv32_qemu.py
@@ -0,0 +1,284 @@
+import subprocess
+import sys
+import os
+import argparse
+import socket
+import select
+
+def print_content(content):
+    """Prints content using os.write to handle non-blocking stdout robustly."""
+    try:
+        msg = f"{content}\n".encode('utf-8')
+        total_len = len(msg)
+        written = 0
+        fd = sys.stdout.fileno()
+        while written < total_len:
+            try:
+                n = os.write(fd, msg[written:])
+                written += n
+            except BlockingIOError:
+                select.select([], [fd], [])
+    except Exception:
+        pass
+
+def get_free_port():
+    """Finds a free TCP port."""
+    with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
+        s.bind(('', 0))
+        return s.getsockname()[1]
+
+def run_qemu_test(elf_path, qemu_bin, gdb_bin):
+    """
+    Runs a test cycle using QEMU and GDB.
+    """
+    print(f"Testing ELF: {elf_path}")
+    print(f"QEMU: {qemu_bin}")
+    print(f"GDB: {gdb_bin}")
+
+    # Find a free port for GDB connection
+    gdb_port = get_free_port()
+    print(f"Using GDB port: {gdb_port}")
+
+    # 1. Start QEMU in the background
+    qemu_cmd = [
+        qemu_bin,
+        "-M", "virt",
+        "-nographic",
+        "-bios", "none", # Disable default OpenSBI to avoid overlap at 0x80000000
+        "-kernel", elf_path,
+        "-gdb", f"tcp::{gdb_port}", "-S",
+        "-monitor", "none", # Disable monitor to avoid clutter
+        "-serial", "stdio"  # Redirect serial output to stdio so we can see it
+    ]
+    
+    print(f"Starting QEMU: {' '.join(qemu_cmd)}")
+    qemu_process = subprocess.Popen(
+        qemu_cmd,
+        stdout=subprocess.PIPE,
+        stderr=subprocess.PIPE,
+        text=True
+    )
+
+    if qemu_process.poll() is not None:
+        print("QEMU failed to start.")
+        print(qemu_process.stderr.read())
+        return False
+
+    # 2. Create a GDB command file
+    # We use a defined command for the timer interrupt to perform the check automatically
+    gdb_cmds = """
+file {elf}
+target remote :{port}
+set pagination off
+set confirm off
+
+# Setup Breakpoints
+break tx_application_define
+break thread_0_entry
+break thread_6_and_7_entry
+break _tx_timer_interrupt
+
+# Execute to Application Definition
+continue
+
+# Inspect mstatus once thread_0 has started (FS bits should be observable;
+# kept as a smoke check, the lazy-save logic itself is targeted by a
+# follow-up PR).
+continue
+print/x $mstatus
+
+# Verify FPU Logic and Register State exercised by thread_6/7
+continue
+finish
+step
+step
+step
+print/x $mstatus
+info registers float
+print fpu_test_val
+
+# Await Timer Interrupt
+continue
+print "Hit Timer Interrupt"
+
+# Verify MEPC Integrity - Save State
+print/x $mepc
+set $saved_pc = $mepc
+
+# Verify System Timer Before ISR
+set $clock_before = _tx_timer_system_clock
+print $clock_before
+
+# Configure Time-Slice Test Conditions
+set _tx_timer_time_slice = 1
+set _tx_timer_expired_time_slice = 0
+set $ts_handler_called = 0
+
+# Set Breakpoint at Time-Slice Handler with Auto-Continue
+tbreak _tx_thread_time_slice
+commands
+  set $ts_handler_called = 1
+  continue
+end
+
+# Set Breakpoint at ISR Return Address
+set $ret_addr = $ra
+tbreak *$ret_addr
+continue
+
+# Verify Time-Slice Handler Was Called
+if $ts_handler_called == 1
+  print "SUCCESS: Time-slice handler called."
+else
+  print "FAILURE: Time-slice handler NOT called."
+end
+
+# Verify System Timer Increment (Monotonicity)
+set $clock_after = _tx_timer_system_clock
+print $clock_after
+
+if $clock_after > $clock_before
+  print "SUCCESS: System timer incremented."
+else
+  print "FAILURE: System timer did not increment."
+end
+
+# Verify Preemption Logic (Thread Priority)
+#
+# We are now stopped at the return address from _tx_timer_interrupt,
+# after _tx_thread_time_slice has had a chance to update
+# _tx_thread_execute_ptr but before trap_handler returns into
+# _tx_thread_context_restore. At this point, a pending preemption is
+# observable directly by comparing current_ptr (interrupted thread)
+# and execute_ptr (thread chosen by the scheduler).
+set $curr_ptr = _tx_thread_current_ptr
+set $exec_ptr = _tx_thread_execute_ptr
+if $curr_ptr != 0 && $exec_ptr != 0
+  set $curr_prio = $curr_ptr->tx_thread_priority
+  set $exec_prio = $exec_ptr->tx_thread_priority
+  printf "PREEMPT_CHECK current_prio=%d execute_prio=%d\\n", $curr_prio, $exec_prio
+  if $exec_prio < $curr_prio
+    printf "PREEMPT_VERIFIED_OK\\n"
+  else
+    printf "PREEMPT_VERIFIED_FAIL_NOT_OBSERVED\\n"
+  end
+else
+  printf "PREEMPT_VERIFIED_FAIL_NULL\\n"
+end
+
+quit
+""".format(port=gdb_port, elf=elf_path)
+
+    gdb_cmd_file = "test_cmds.gdb"
+    with open(gdb_cmd_file, "w") as f:
+        f.write(gdb_cmds)
+
+    # 3. Run GDB
+    gdb_cmd = [
+        gdb_bin,
+        "--batch",
+        "-x", gdb_cmd_file
+    ]
+
+    print_content(f"Starting GDB: {' '.join(gdb_cmd)}")
+    
+    # Cap the GDB session to 30 s so a wedged batch script (e.g. a
+    # `continue` that never hits its breakpoint) cannot hang CI.
+    GDB_TIMEOUT_S = 30
+
+    try:
+        gdb_process = subprocess.run(
+            gdb_cmd,
+            stdout=subprocess.PIPE,
+            stderr=subprocess.PIPE,
+            text=True,
+            timeout=GDB_TIMEOUT_S,
+        )
+
+        print_content("GDB Output:")
+        print_content(gdb_process.stdout)
+        if gdb_process.stderr:
+            print_content("GDB Error Output:")
+            print_content(gdb_process.stderr)
+
+    except subprocess.TimeoutExpired as e:
+        print_content(
+            f"FAILURE: GDB session exceeded {GDB_TIMEOUT_S}s timeout; "
+            "likely stuck on a `continue` that never matched a breakpoint."
+        )
+        if e.stdout:
+            print_content("GDB Output (partial):")
+            print_content(e.stdout if isinstance(e.stdout, str)
+                          else e.stdout.decode(errors='replace'))
+        if e.stderr:
+            print_content("GDB Error Output (partial):")
+            print_content(e.stderr if isinstance(e.stderr, str)
+                          else e.stderr.decode(errors='replace'))
+        return False
+
+    except Exception as e:
+        print_content(f"An error occurred during test execution: {e}")
+        return False
+        
+    finally:
+        # 4. Clean up
+        print_content("Stopping QEMU...")
+        qemu_process.terminate()
+        try:
+            qemu_process.wait(timeout=2)
+        except subprocess.TimeoutExpired:
+            print_content("QEMU did not terminate gracefully, killing it forcefullly.")
+            qemu_process.kill()
+
+    # Verify results
+    stdout = gdb_process.stdout
+    timer_hit = "Breakpoint 4, _tx_timer_interrupt" in stdout
+    fpu_verified = False
+    preemption_verified = "PREEMPT_VERIFIED_OK" in stdout
+
+    if "Breakpoint 3, thread_6_and_7_entry" in stdout:
+        if "1.10" in stdout or "fpu_test_val" in stdout:
+             print_content("SUCCESS: FPU instructions executed and registers inspected.")
+             fpu_verified = True
+        else:
+             print_content("FAILURE: Hit thread, but failed to inspect FPU. Output does not contain expected value.")
+
+    if timer_hit:
+        print_content("SUCCESS: Timer Interrupt verified! Hit _tx_timer_interrupt.")
+    else:
+        print_content("FAILURE: Did not hit timer interrupt.")
+
+    if preemption_verified:
+        print_content("SUCCESS: Preemption verified (higher-priority thread "
+                      "preempted a lower-priority one).")
+    else:
+        if "PREEMPT_VERIFIED_FAIL_INVERTED" in stdout:
+            print_content("FAILURE: Preemption inverted -- lower priority "
+                          "thread scheduled over higher priority one.")
+        elif "PREEMPT_VERIFIED_FAIL_NULL" in stdout:
+            print_content("FAILURE: Preemption check saw NULL thread pointers.")
+        elif "PREEMPT_VERIFIED_FAIL_NOT_OBSERVED" in stdout:
+            print_content("FAILURE: Preemption was not observed within the "
+                          "loop budget.")
+        else:
+            print_content("FAILURE: Preemption check did not run to completion.")
+
+    if timer_hit and fpu_verified and preemption_verified:
+        return True
+    else:
+        return False
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(description="Run ThreadX QEMU/GDB Test")
+    parser.add_argument("--elf", required=True, help="Path to the kernel ELF file")
+    parser.add_argument("--qemu", default="qemu-system-riscv32", help="Path to QEMU binary")
+    parser.add_argument("--gdb", default="riscv-none-elf-gdb", help="Path to GDB binary")
+    
+    args = parser.parse_args()
+    
+    success = run_qemu_test(args.elf, args.qemu, args.gdb)
+    
+    if success:
+        sys.exit(0)
+    else:
+        sys.exit(1)
diff --git a/ports/risc-v32/gnu/src/tx_thread_context_restore.S b/ports/risc-v32/gnu/src/tx_thread_context_restore.S
index c20cbcdf..d3d449e4 100644
--- a/ports/risc-v32/gnu/src/tx_thread_context_restore.S
+++ b/ports/risc-v32/gnu/src/tx_thread_context_restore.S
@@ -29,6 +29,7 @@
 /*  AUTHOR                                                                */
 /*                                                                        */
 /*    Akif Ejaz, 10xEngineers                                             */
+/*    Wei-Chen Lai, National Cheng Kung University                        */
 /*                                                                        */
 /*  DESCRIPTION                                                           */
 /*                                                                        */
@@ -82,7 +83,13 @@ _tx_thread_context_restore:
     /* Just recover the saved registers and return to the point of
        interrupt.  */
 
-    /* Recover floating point registers.  */
+    /* Recover floating point registers only if saved mstatus.FS was not Off.  */
+#if defined(__riscv_float_abi_single) || defined(__riscv_float_abi_double)
+    lw      t1, 29*4(sp)                                // Pickup saved mstatus
+    srli    t1, t1, 13
+    andi    t1, t1, 0x3
+    beqz    t1, _tx_thread_skip_fp_restore              // Skip if FS was Off
+#endif
 #if defined(__riscv_float_abi_single)
     flw     f0,  31*4(sp)                               // Recover ft0
     flw     f1,  32*4(sp)                               // Recover ft1
@@ -130,6 +137,7 @@ _tx_thread_context_restore:
     lw      t0,  63*4(sp)                               // Recover fcsr
     csrw    fcsr, t0                                    // Restore fcsr
 #endif
+_tx_thread_skip_fp_restore:
 
     /* Recover standard registers.  */
 
@@ -222,7 +230,13 @@ _tx_thread_no_preempt_restore:
 
     lw      sp, 8(t1)                                   // Switch back to thread's stack
 
-    /* Recover floating point registers.  */
+    /* Recover floating point registers only if saved mstatus.FS was not Off.  */
+#if defined(__riscv_float_abi_single) || defined(__riscv_float_abi_double)
+    lw      t3, 29*4(sp)                                // Pickup saved mstatus
+    srli    t3, t3, 13
+    andi    t3, t3, 0x3
+    beqz    t3, _tx_thread_no_preempt_skip_fp_restore   // Skip if FS was Off
+#endif
 #if defined(__riscv_float_abi_single)
     flw     f0,  31*4(sp)                               // Recover ft0
     flw     f1,  32*4(sp)                               // Recover ft1
@@ -270,6 +284,7 @@ _tx_thread_no_preempt_restore:
     lw      t0,  63*4(sp)                               // Recover fcsr
     csrw    fcsr, t0                                    // Restore fcsr
 #endif
+_tx_thread_no_preempt_skip_fp_restore:
 
     /* Recover the saved context and return to the point of interrupt.  */
 
@@ -331,7 +346,13 @@ _tx_thread_preempt_restore:
     ori     t3, zero, 1                                 // Build interrupt stack type
     sw      t3, 0(t0)                                   // Store stack type
 
-    /* Store floating point preserved registers.  */
+    /* Store floating point preserved registers only if saved mstatus.FS was not Off.  */
+#if defined(__riscv_float_abi_single) || defined(__riscv_float_abi_double)
+    lw      t3, 29*4(t0)                                // Pickup saved mstatus
+    srli    t3, t3, 13
+    andi    t3, t3, 0x3
+    beqz    t3, _tx_thread_preempt_skip_fp_restore      // Skip if FS was Off
+#endif
 #ifdef __riscv_float_abi_single
     fsw     f8,  39*4(t0)                               // Store fs0
     fsw     f9,  40*4(t0)                               // Store fs1
@@ -359,6 +380,7 @@ _tx_thread_preempt_restore:
     fsd     f26, 57*4(t0)                               // Store fs10
     fsd     f27, 58*4(t0)                               // Store fs11
 #endif
+_tx_thread_preempt_skip_fp_restore:
 
     /* Store standard preserved registers.  */
 
diff --git a/ports/risc-v32/gnu/src/tx_thread_context_save.S b/ports/risc-v32/gnu/src/tx_thread_context_save.S
index 66402934..0a0c0c15 100644
--- a/ports/risc-v32/gnu/src/tx_thread_context_save.S
+++ b/ports/risc-v32/gnu/src/tx_thread_context_save.S
@@ -29,6 +29,7 @@
 /*  AUTHOR                                                                */
 /*                                                                        */
 /*    Akif Ejaz, 10xEngineers                                             */
+/*    Wei-Chen Lai, National Cheng Kung University                        */
 /*                                                                        */
 /*  DESCRIPTION                                                           */
 /*                                                                        */
@@ -96,6 +97,15 @@ _tx_thread_context_save:
     sw      t5, 14*4(sp)                         // Store t5
     sw      t6, 13*4(sp)                         // Store t6
 
+    /* Save mstatus and skip FP state if FS is Off.  */
+    csrr    t0, mstatus
+    sw      t0, 29*4(sp)
+#if defined(__riscv_float_abi_single) || defined(__riscv_float_abi_double)
+    srli    t1, t0, 13
+    andi    t1, t1, 0x3
+    beqz    t1, _tx_thread_skip_fpu_save
+#endif
+
     /* Save floating point registers.  */
 #if defined(__riscv_float_abi_single)
     fsw     f0,  31*4(sp)                        // Store ft0
@@ -144,14 +154,11 @@ _tx_thread_context_save:
     csrr    t0, fcsr
     sw      t0, 63*4(sp)                         // Store fcsr
 #endif
+_tx_thread_skip_fpu_save:
 
     csrr    t0, mepc
     sw      t0, 30*4(sp)                         // Save it on the stack
 
-    /* Save mstatus.  */
-    csrr    t0, mstatus
-    sw      t0, 29*4(sp)
-
     la      t1, _tx_thread_current_ptr           // Pickup address of current thread ptr
     lw      t2, 0(t1)                            // Pickup current thread pointer
     beqz    t2, _tx_thread_idle_system_save      // If NULL, idle system was interrupted
@@ -190,6 +197,15 @@ _tx_thread_nested_save:
     sw      t5, 14*4(sp)                         // Store t5
     sw      t6, 13*4(sp)                         // Store t6
 
+    /* Save mstatus and skip FP state if FS is Off.  */
+    csrr    t0, mstatus
+    sw      t0, 29*4(sp)
+#if defined(__riscv_float_abi_single) || defined(__riscv_float_abi_double)
+    srli    t1, t0, 13
+    andi    t1, t1, 0x3
+    beqz    t1, _tx_thread_skip_nested_fpu_save
+#endif
+
     /* Save floating point registers.  */
 #if defined(__riscv_float_abi_single)
     fsw     f0,  31*4(sp)                        // Store ft0
@@ -238,13 +254,11 @@ _tx_thread_nested_save:
     csrr    t0, fcsr
     sw      t0, 63*4(sp)                         // Store fcsr
 #endif
+_tx_thread_skip_nested_fpu_save:
 
     csrr    t0, mepc
     sw      t0, 30*4(sp)                         // Save it on stack
 
-    csrr    t0, mstatus
-    sw      t0, 29*4(sp)
-
     /* Call the ISR execution exit function if enabled.  */
 #ifdef TX_ENABLE_EXECUTION_CHANGE_NOTIFY
     call    _tx_execution_isr_enter              // Call the ISR execution enter function
diff --git a/ports/risc-v32/gnu/src/tx_thread_schedule.S b/ports/risc-v32/gnu/src/tx_thread_schedule.S
index 1c235a2d..213aa976 100644
--- a/ports/risc-v32/gnu/src/tx_thread_schedule.S
+++ b/ports/risc-v32/gnu/src/tx_thread_schedule.S
@@ -133,7 +133,12 @@ _tx_thread_ready_to_run:
     lw      t0, 0(sp)                                   // Pickup stack type
     beqz    t0, _tx_thread_solicited_return             // If 0, solicited return
 
-    /* Recover floating point registers.  */
+    /* Recover floating point registers only if saved mstatus.FS was not Off.  */
+#if defined(__riscv_float_abi_single) || defined(__riscv_float_abi_double)
+    lw      t1, 29*4(sp)                                // Pickup saved mstatus
+    srli    t1, t1, 13
+    andi    t1, t1, 0x3
+    beqz    t1, _tx_thread_schedule_skip_fp_restore     // Skip if FS was Off
 #if defined(__riscv_float_abi_single)
     flw     f0,  31*4(sp)                               // Recover ft0
     flw     f1,  32*4(sp)                               // Recover ft1
@@ -205,10 +210,10 @@ _tx_thread_ready_to_run:
     lw      t0,  63*4(sp)                               // Recover fcsr
     csrw    fcsr, t0                                    // Restore fcsr
 #endif
+_tx_thread_schedule_skip_fp_restore:
+#endif
 
     /* Recover standard registers.  */
-
-    lw      t0, 30*4(sp)                                // Recover mepc
     csrw    mepc, t0                                    // Setup mepc
 
     li      t0, 0x1880                                  // Prepare mstatus: MPP=Machine(0x1800) | MPIE(0x80)
@@ -256,7 +261,12 @@ _tx_thread_ready_to_run:
 
 _tx_thread_solicited_return:
 
-    /* Recover floating point registers.  */
+    /* Recover floating point registers only if saved mstatus.FS was not Off.  */
+#if defined(__riscv_float_abi_single) || defined(__riscv_float_abi_double)
+    lw      t1, 14*4(sp)                                // Pickup saved mstatus
+    srli    t1, t1, 13
+    andi    t1, t1, 0x3
+    beqz    t1, _tx_thread_schedule_solicited_skip_fp_restore   // Skip if FS was Off
 #if defined(__riscv_float_abi_single)
     flw     f8,  15*4(sp)                               // Recover fs0
     flw     f9,  16*4(sp)                               // Recover fs1
@@ -288,10 +298,10 @@ _tx_thread_solicited_return:
     lw      t0,  27*4(sp)                               // Recover fcsr
     csrw    fcsr, t0                                    // Restore fcsr
 #endif
+_tx_thread_schedule_solicited_skip_fp_restore:
+#endif
 
     /* Recover standard registers.  */
-
-    lw      t0, 14*4(sp)                                // Recover mstatus
     csrw    mstatus, t0                                 // Restore mstatus
 
     lw      ra,  13*4(sp)                               // Recover return address