Merge pull request #508 from goodnorning/feature/rv64_rvv_support

Added rv64 rvv support

11 files changed, 393 insertions, 4 deletions

diff --git a/ports/risc-v64/gnu/example_build/qemu_virt/csr.h b/ports/risc-v64/gnu/example_build/qemu_virt/csr.h
index 92ca6644..83d05afd 100644
--- a/ports/risc-v64/gnu/example_build/qemu_virt/csr.h
+++ b/ports/risc-v64/gnu/example_build/qemu_virt/csr.h
@@ -22,6 +22,7 @@
 #define MSTATUS_MIE 		(1L << 3)    // machine-mode interrupt enable.
 #define MSTATUS_MPIE		(1L << 7)
 #define MSTATUS_FS		(1L << 13)
+#define MSTATUS_VS    (1L << 9)
 
 // Machine-mode Interrupt Enable
 #define MIE_MTIE		(1L << 7)
diff --git a/ports/risc-v64/gnu/example_build/qemu_virt/demo_threadx.c b/ports/risc-v64/gnu/example_build/qemu_virt/demo_threadx.c
index 229dcc68..9cbb33da 100644
--- a/ports/risc-v64/gnu/example_build/qemu_virt/demo_threadx.c
+++ b/ports/risc-v64/gnu/example_build/qemu_virt/demo_threadx.c
@@ -4,8 +4,13 @@
 
 #include "tx_api.h"
 #include "uart.h"
+#if defined(__riscv_vector)
+#define DEMO_STACK_SIZE         (1024 + 16448)      /* 16448 for RVV Extension */
+#define DEMO_BYTE_POOL_SIZE     (9180 + 148032)     /* 148032 for RVV Extension */
+#else
 #define DEMO_STACK_SIZE         1024
 #define DEMO_BYTE_POOL_SIZE     9180
+#endif
 #define DEMO_BLOCK_POOL_SIZE    100
 #define DEMO_QUEUE_SIZE         100
 
diff --git a/ports/risc-v64/gnu/example_build/qemu_virt/entry.s b/ports/risc-v64/gnu/example_build/qemu_virt/entry.s
index 9b202ca1..791d08eb 100644
--- a/ports/risc-v64/gnu/example_build/qemu_virt/entry.s
+++ b/ports/risc-v64/gnu/example_build/qemu_virt/entry.s
@@ -41,7 +41,11 @@ _start:
 	li x30, 0
 	li x31, 0
 	la t0, _sysstack_start
+#ifdef __riscv_vector
+	li t1, 0x5000
+#else
 	li t1, 0x1000
+#endif
 	add sp, t0, t1
 	la  t0, _bss_start
 	la  t1, _bss_end
diff --git a/ports/risc-v64/gnu/example_build/qemu_virt/link.lds b/ports/risc-v64/gnu/example_build/qemu_virt/link.lds
index 522f90d9..d0d8a6bc 100644
--- a/ports/risc-v64/gnu/example_build/qemu_virt/link.lds
+++ b/ports/risc-v64/gnu/example_build/qemu_virt/link.lds
@@ -42,6 +42,9 @@ SECTIONS
     . = ALIGN(4096);
     _sysstack_start = .;
     . += 0x1000;
+#ifdef __riscv_vector
+    . += 0x4000;
+#endif
     _sysstack_end = .;
   }
 
diff --git a/ports/risc-v64/gnu/example_build/qemu_virt/tx_initialize_low_level.S b/ports/risc-v64/gnu/example_build/qemu_virt/tx_initialize_low_level.S
index eba6c458..a9b2fda7 100644
--- a/ports/risc-v64/gnu/example_build/qemu_virt/tx_initialize_low_level.S
+++ b/ports/risc-v64/gnu/example_build/qemu_virt/tx_initialize_low_level.S
@@ -66,6 +66,13 @@
 #else
 	addi sp, sp, -256                 // Allocate space for all registers - without floating point enabled (32*8)
 #endif
+#if defined(__riscv_vector)
+    /* Allocate space for vector registers */
+    csrr    t4, vlenb
+    slli    t4, t4, 5
+    addi    t4, t4, 4*8
+    sub     sp, sp, t4
+#endif
 
 	sd   x1, 224(sp)                  // Store RA (28*8 = 224, because call will override ra [ra is a callee register in riscv])
 
@@ -150,6 +157,10 @@ _tx_initialize_low_level:
     csrrs   zero, mstatus, t0			           // set MSTATUS_FS bit to open f/d isa in riscv
     fscsr   x0
 #endif
+#ifdef __riscv_vector
+    li      t0, MSTATUS_VS
+    csrrs   zero, mstatus, t0			           // set MSTATUS_VS bit to open vector isa in riscv
+#endif
     addi    sp, sp, -8
     sd	    ra, 0(sp)
     call    board_init
diff --git a/ports/risc-v64/gnu/inc/tx_port.h b/ports/risc-v64/gnu/inc/tx_port.h
index ae617b39..c8f5b312 100644
--- a/ports/risc-v64/gnu/inc/tx_port.h
+++ b/ports/risc-v64/gnu/inc/tx_port.h
@@ -100,7 +100,11 @@ typedef unsigned short                          USHORT;
    thread creation is less than this value, the thread create call will return an error.  */
 
 #ifndef TX_MINIMUM_STACK
-#define TX_MINIMUM_STACK                        1024        /* Minimum stack size for this port  */
+#if defined(__riscv_vector)
+#define TX_MINIMUM_STACK                        (1024 + 16448)        /* Minimum stack size for this port  */
+#else
+#define TX_MINIMUM_STACK                        1024                  /* Minimum stack size for this port  */
+#endif
 #endif
 
 
@@ -108,7 +112,11 @@ typedef unsigned short                          USHORT;
    if TX_TIMER_PROCESS_IN_ISR is not defined.  */
 
 #ifndef TX_TIMER_THREAD_STACK_SIZE
-#define TX_TIMER_THREAD_STACK_SIZE              1024        /* Default timer thread stack size  */
+#if defined(__riscv_vector)
+#define TX_TIMER_THREAD_STACK_SIZE              (1024 + 16448)        /* Default timer thread stack size  */
+#else
+#define TX_TIMER_THREAD_STACK_SIZE              1024                  /* Default timer thread stack size  */
+#endif
 #endif
 
 #ifndef TX_TIMER_THREAD_PRIORITY
diff --git a/ports/risc-v64/gnu/src/tx_thread_context_restore.S b/ports/risc-v64/gnu/src/tx_thread_context_restore.S
index 6aae3981..4473a39b 100644
--- a/ports/risc-v64/gnu/src/tx_thread_context_restore.S
+++ b/ports/risc-v64/gnu/src/tx_thread_context_restore.S
@@ -133,6 +133,34 @@ _tx_thread_context_restore:
     csrw    fcsr, t0
 #endif
 
+#if defined(__riscv_vector)
+    /* Recover vector registers v0-v31 */
+#if defined(__riscv_float_abi_single) || defined(__riscv_float_abi_double)
+    addi    t1, sp, 64*8
+#else
+    addi    t1, sp, 31*8
+#endif
+    addi    t2, t1, 4*8
+    vsetvli t3, zero, e8, m8, ta, ma
+    vle8.v  v0, (t2)                                    // Recover v0 ~ v7
+    add     t2, t2, t3
+    vle8.v  v8, (t2)                                    // Recover v8 ~ v15
+    add     t2, t2, t3
+    vle8.v  v16, (t2)                                   // Recover v16 ~ v23
+    add     t2, t2, t3
+    vle8.v  v24, (t2)                                   // Recover v24 ~ v31
+    add     t2, t2, t3
+
+    /* Recover vector CSRs */
+    ld      t2, 0*8(t1)
+    ld      t3, 1*8(t1)
+    ld      t4, 2*8(t1)
+    vsetvl  zero, t4, t3
+    csrw    vstart, t2
+    ld      t4, 3*8(t1)
+    csrw    vcsr, t4
+#endif
+
     /* Recover standard registers.  */
 
     /* Restore registers,
@@ -164,6 +192,10 @@ _tx_thread_context_restore:
     li      t0, 0x2000                                  // Set FS bits (bits 14:13 to 01) for FP state
     or      t1, t1, t0
 #endif
+#if defined(__riscv_vector)
+    li      t0, 0x0200                                  // Set VS bits (bits 10:9 to 01) for vector state
+    or      t1, t1, t0
+#endif
     csrw    mstatus, t1                                 // Update mstatus safely
 
     ld      ra,  28*8(sp)                               // Recover return address
@@ -189,6 +221,21 @@ _tx_thread_context_restore:
 #else
     addi    sp, sp, 32*8                                // Recover stack frame - without floating point enabled
 #endif
+
+#if defined(__riscv_vector)
+#if defined(__riscv_float_abi_single) || defined(__riscv_float_abi_double)
+    addi    t0, sp, -65*8
+#else
+    addi    t0, sp, -32*8
+#endif
+    csrr    t1, vlenb                                   // Get vector register byte length
+    slli    t1, t1, 5                                   // Multiply by 32 (number of vector registers)
+    addi    t1, t1, 4*8                                 // Add vector CSR space: vstart, vtype, vl, vcsr
+    add     sp, sp, t1                                  // Recover vector stack frame
+
+    ld      t1, 18*8(t0)                                // Recover t1
+    ld      t0, 19*8(t0)                                // Recover t0
+#endif
     mret                                                // Return to point of interrupt
 
     /* }  */
@@ -268,6 +315,34 @@ _tx_thread_no_preempt_restore:
     csrw    fcsr, t0                                    // Restore fcsr
 #endif
 
+#if defined(__riscv_vector)
+    /* Recover vector registers v0-v31 */
+#if defined(__riscv_float_abi_single) || defined(__riscv_float_abi_double)
+    addi    t1, sp, 64*8
+#else
+    addi    t1, sp, 31*8
+#endif
+    addi    t2, t1, 4*8
+    vsetvli t3, zero, e8, m8, ta, ma
+    vle8.v  v0, (t2)                                    // Recover v0 ~ v7
+    add     t2, t2, t3
+    vle8.v  v8, (t2)                                    // Recover v8 ~ v15
+    add     t2, t2, t3
+    vle8.v  v16, (t2)                                   // Recover v16 ~ v23
+    add     t2, t2, t3
+    vle8.v  v24, (t2)                                   // Recover v24 ~ v31
+    add     t2, t2, t3
+
+    /* Recover vector CSRs */
+    ld      t2, 0*8(t1)
+    ld      t3, 1*8(t1)
+    ld      t4, 2*8(t1)
+    vsetvl  zero, t4, t3
+    csrw    vstart, t2
+    ld      t4, 3*8(t1)
+    csrw    vcsr, t4
+#endif
+
     /* Recover the saved context and return to the point of interrupt.  */
 
     /* Recover standard registers.  */
@@ -290,6 +365,10 @@ _tx_thread_no_preempt_restore:
     li      t0, 0x2000                                  // Set FS bits for FP state
     or      t1, t1, t0
 #endif
+#if defined(__riscv_vector)
+    li      t0, 0x0200                                  // Set VS bits (bits 10:9 to 01) for vector state
+    or      t1, t1, t0
+#endif
     csrw    mstatus, t1                                 // Update mstatus safely
 
     ld      ra,  28*8(sp)                               // Recover return address
@@ -315,6 +394,21 @@ _tx_thread_no_preempt_restore:
 #else
     addi    sp, sp, 32*8                                // Recover stack frame - without floating point enabled
 #endif
+
+#if defined(__riscv_vector)
+#if defined(__riscv_float_abi_single) || defined(__riscv_float_abi_double)
+    addi    t0, sp, -65*8
+#else
+    addi    t0, sp, -32*8
+#endif
+    csrr    t1, vlenb                                   // Get vector register byte length
+    slli    t1, t1, 5                                   // Multiply by 32 (number of vector registers)
+    addi    t1, t1, 4*8                                 // Add vector CSR space: vstart, vtype, vl, vcsr
+    add     sp, sp, t1                                  // Recover vector stack frame
+
+    ld      t1, 18*8(t0)                                // Recover t1
+    ld      t0, 19*8(t0)                                // Recover t0
+#endif
     mret                                                // Return to point of interrupt
 
     /* }
@@ -357,6 +451,36 @@ _tx_thread_preempt_restore:
     fsd     f27, 58*8(t0)                               // Store fs11
 #endif
 
+#if defined(__riscv_vector)
+    /* Store vector registers and CSRs */
+#if defined(__riscv_float_abi_single) || defined(__riscv_float_abi_double)
+    addi    t1, t0, 64*8
+#else
+    addi    t1, t0, 31*8
+#endif
+    /* Store vector CSRs */
+    csrr    t2, vstart                                  // Store vstart
+    sd      t2, 0*8(t1)
+    csrr    t2, vtype                                   // Store vtype
+    sd      t2, 1*8(t1)
+    csrr    t2, vl                                      // Store vl
+    sd      t2, 2*8(t1)
+    csrr    t2, vcsr                                    // Store vcsr
+    sd      t2, 3*8(t1)
+
+    /* Store vector registers v0-v31 */
+    addi    t2, t1, 4*8
+    vsetvli t3, zero, e8, m8, ta, ma
+    vse8.v  v0, 0(t2)                                   // Store v0 ~ v7
+    add     t2, t2, t3
+    vse8.v  v8, 0(t2)                                   // Store v8 ~ v15
+    add     t2, t2, t3
+    vse8.v  v16, 0(t2)                                  // Store v16 ~ v23
+    add     t2, t2, t3
+    vse8.v  v24, 0(t2)                                  // Store v24 ~ v31
+    add     t2, t2, t3
+#endif
+
     /* Store standard preserved registers.  */
 
     sd      x9,  11*8(t0)                               // Store s1
diff --git a/ports/risc-v64/gnu/src/tx_thread_context_save.S b/ports/risc-v64/gnu/src/tx_thread_context_save.S
index df6f2c9a..1a638990 100644
--- a/ports/risc-v64/gnu/src/tx_thread_context_save.S
+++ b/ports/risc-v64/gnu/src/tx_thread_context_save.S
@@ -146,6 +146,36 @@ _tx_thread_context_save:
     sd      t0, 63*8(sp)                                // Store fcsr
 #endif
 
+#if defined(__riscv_vector)
+    /* Store vector registers and CSRs */
+#if defined(__riscv_float_abi_single) || defined(__riscv_float_abi_double)
+    addi    t1, sp, 64*8
+#else
+    addi    t1, sp, 31*8
+#endif
+    /* Store vector CSRs */
+    csrr    t2, vstart                                  // Store vstart
+    sd      t2, 0*8(t1)
+    csrr    t2, vtype                                   // Store vtype
+    sd      t2, 1*8(t1)
+    csrr    t2, vl                                      // Store vl
+    sd      t2, 2*8(t1)
+    csrr    t2, vcsr                                    // Store vcsr
+    sd      t2, 3*8(t1)
+
+    /* Store vector registers v0-v31 */
+    addi    t2, t1, 4*8
+    vsetvli t3, zero, e8, m8, ta, ma
+    vse8.v  v0, 0(t2)                                   // Store v0 ~ v7
+    add     t2, t2, t3
+    vse8.v  v8, 0(t2)                                   // Store v8 ~ v15
+    add     t2, t2, t3
+    vse8.v  v16, 0(t2)                                  // Store v16 ~ v23
+    add     t2, t2, t3
+    vse8.v  v24, 0(t2)                                  // Store v24 ~ v31
+    add     t2, t2, t3
+#endif
+
 #ifdef TX_ENABLE_EXECUTION_CHANGE_NOTIFY
     call    _tx_execution_isr_enter                     // Call the ISR execution enter function
 #endif
@@ -236,6 +266,36 @@ _tx_thread_not_nested_save:
     sd      t0, 63*8(sp)                                // Store fcsr
 #endif
 
+#if defined(__riscv_vector)
+    /* Store vector registers and CSRs */
+#if defined(__riscv_float_abi_single) || defined(__riscv_float_abi_double)
+    addi    t1, sp, 64*8
+#else
+    addi    t1, sp, 31*8
+#endif
+    /* Store vector CSRs */
+    csrr    t2, vstart                                  // Store vstart
+    sd      t2, 0*8(t1)
+    csrr    t2, vtype                                   // Store vtype
+    sd      t2, 1*8(t1)
+    csrr    t2, vl                                      // Store vl
+    sd      t2, 2*8(t1)
+    csrr    t2, vcsr                                    // Store vcsr
+    sd      t2, 3*8(t1)
+
+    /* Store vector registers v0-v31 */
+    addi    t2, t1, 4*8
+    vsetvli t3, zero, e8, m8, ta, ma
+    vse8.v  v0, 0(t2)                                   // Store v0 ~ v7
+    add     t2, t2, t3
+    vse8.v  v8, 0(t2)                                   // Store v8 ~ v15
+    add     t2, t2, t3
+    vse8.v  v16, 0(t2)                                  // Store v16 ~ v23
+    add     t2, t2, t3
+    vse8.v  v24, 0(t2)                                  // Store v24 ~ v31
+    add     t2, t2, t3
+#endif
+
     /* Save the current stack pointer in the thread's control block.  */
     /* _tx_thread_current_ptr -> tx_thread_stack_ptr =  sp;  */
 
@@ -275,4 +335,19 @@ _tx_thread_idle_system_save:
 #else
     addi    sp, sp, 32*8                                // Recover the reserved stack space
 #endif
+
+#if defined(__riscv_vector)
+#if defined(__riscv_float_abi_single) || defined(__riscv_float_abi_double)
+    addi    t0, sp, -65*8
+#else
+    addi    t0, sp, -32*8
+#endif
+    csrr    t1, vlenb                                   // Get vector register byte length
+    slli    t1, t1, 5                                   // Multiply by 32 (number of vector registers)
+    addi    t1, t1, 4*8                                 // Add vector CSR space: vstart, vtype, vl, vcsr
+    add     sp, sp, t1                                  // Recover vector stack frame
+
+    ld      t1, 18*8(t0)                                // Recover t1
+    ld      t0, 19*8(t0)                                // Recover t0
+#endif
     ret                                                 // Return to calling ISR
diff --git a/ports/risc-v64/gnu/src/tx_thread_schedule.S b/ports/risc-v64/gnu/src/tx_thread_schedule.S
index d3332896..85fc9488 100644
--- a/ports/risc-v64/gnu/src/tx_thread_schedule.S
+++ b/ports/risc-v64/gnu/src/tx_thread_schedule.S
@@ -206,6 +206,34 @@ _tx_thread_schedule_loop:
     csrw    fcsr, t0                                    // Restore fcsr
 #endif
 
+#if defined(__riscv_vector)
+    /* Recover vector registers v0-v31 */
+#if defined(__riscv_float_abi_single) || defined(__riscv_float_abi_double)
+    addi    t1, sp, 64*8
+#else
+    addi    t1, sp, 31*8
+#endif
+    addi    t2, t1, 4*8
+    vsetvli t3, zero, e8, m8, ta, ma
+    vle8.v  v0, (t2)                                    // Recover v0 ~ v7
+    add     t2, t2, t3
+    vle8.v  v8, (t2)                                    // Recover v8 ~ v15
+    add     t2, t2, t3
+    vle8.v  v16, (t2)                                   // Recover v16 ~ v23
+    add     t2, t2, t3
+    vle8.v  v24, (t2)                                   // Recover v24 ~ v31
+    add     t2, t2, t3
+
+    /* Recover vector CSRs */
+    ld      t2, 0*8(t1)
+    ld      t3, 1*8(t1)
+    ld      t4, 2*8(t1)
+    vsetvl  zero, t4, t3
+    csrw    vstart, t2
+    ld      t4, 3*8(t1)
+    csrw    vcsr, t4
+#endif
+
     /* Recover standard registers.  */
 
     ld      t0, 30*8(sp)                                // Recover mepc
@@ -215,6 +243,10 @@ _tx_thread_schedule_loop:
     li      t1, 0x2000                                  // Set FS bits for FP state
     or      t0, t0, t1
 #endif
+#if defined(__riscv_vector)
+    li      t1, 0x0200                                  // Set VS bits (bits 10:9 to 01) for vector state
+    or      t0, t0, t1
+#endif
     csrw    mstatus, t0                                 // Set mstatus
 
     ld      ra,  28*8(sp)                               // Recover return address
@@ -251,6 +283,20 @@ _tx_thread_schedule_loop:
 #else
     addi    sp, sp, 32*8                                // Recover stack frame - without floating point registers
 #endif
+#if defined(__riscv_vector)
+#if defined(__riscv_float_abi_single) || defined(__riscv_float_abi_double)
+    addi    t0, sp, -65*8
+#else
+    addi    t0, sp, -32*8
+#endif
+    csrr    t1, vlenb                                   // Get vector register byte length
+    slli    t1, t1, 5                                   // Multiply by 32 (number of vector registers)
+    addi    t1, t1, 4*8                                 // Add vector CSR space: vstart, vtype, vl, vcsr
+    add     sp, sp, t1                                  // Recover vector stack frame
+
+    ld      t1, 18*8(t0)                                // Recover t1
+    ld      t0, 19*8(t0)                                // Recover t0
+#endif
     mret                                                // Return to point of interrupt
 
 _tx_thread_synch_return:
@@ -287,6 +333,34 @@ _tx_thread_synch_return:
     csrw    fcsr, t0                                    //
 #endif
 
+#if defined(__riscv_vector)
+    /* Recover vector registers v0-v31 */
+#if defined(__riscv_float_abi_single) || defined(__riscv_float_abi_double)
+    addi    t1, sp, 28*8
+#else
+    addi    t1, sp, 15*8
+#endif
+    addi    t2, t1, 4*8
+    vsetvli t3, zero, e8, m8, ta, ma
+    vle8.v  v0, (t2)                                    // Recover v0 ~ v7
+    add     t2, t2, t3
+    vle8.v  v8, (t2)                                    // Recover v8 ~ v15
+    add     t2, t2, t3
+    vle8.v  v16, (t2)                                   // Recover v16 ~ v23
+    add     t2, t2, t3
+    vle8.v  v24, (t2)                                   // Recover v24 ~ v31
+    add     t2, t2, t3
+
+    /* Recover vector CSRs */
+    ld      t2, 0*8(t1)
+    ld      t3, 1*8(t1)
+    ld      t4, 2*8(t1)
+    vsetvl  zero, t4, t3
+    csrw    vstart, t2
+    ld      t4, 3*8(t1)
+    csrw    vcsr, t4
+#endif
+
     /* Recover standard preserved registers.  */
     /* Recover standard registers.  */
 
@@ -310,6 +384,12 @@ _tx_thread_synch_return:
 #else
     addi    sp, sp, 16*8                                // Recover stack frame
 #endif
+#if defined(__riscv_vector)
+    csrr    t1, vlenb                                   // Get vector register byte length
+    slli    t1, t1, 5                                   // Multiply by 32 (number of vector registers)
+    addi    t1, t1, 4*8                                 // Add vector CSR space: vstart, vtype, vl, vcsr
+    add     sp, sp, t1                                  // Recover vector stack frame
+#endif
     ret                                                 // Return to thread
 
 /* }  */
diff --git a/ports/risc-v64/gnu/src/tx_thread_stack_build.S b/ports/risc-v64/gnu/src/tx_thread_stack_build.S
index 8663a6f1..410f6cc8 100644
--- a/ports/risc-v64/gnu/src/tx_thread_stack_build.S
+++ b/ports/risc-v64/gnu/src/tx_thread_stack_build.S
@@ -128,6 +128,15 @@ If floating point support:
                     f30     61      Inital ft10
                     f31     62      Inital ft11
                     fscr    63      Inital fscr
+If vector extension support:
+                    vstart  64      Initial vstart
+                    vtype   65      Initial vtype
+                    vl      66      Initial vl
+                    vcsr    67      Initial vcsr
+                    v0      68      Initial v0
+                    v1      69      Initial v1
+                    ...
+                    v31     99      Initial v31
 
     Stack Bottom: (higher memory address)  */
 
@@ -142,6 +151,17 @@ If floating point support:
 #else
     addi    t0, t0, -32*8                               // Allocate space for the stack frame
 #endif
+
+#if defined(__riscv_vector)
+    /* Vector extension support: calculate space based on vlenb */
+    csrr    t4, vlenb                                   // Get vector register byte length
+    slli    t4, t4, 5                                   // Multiply by 32 (number of vector registers)
+    addi    t4, t4, 4*8                                 // Add vector CSR space: vstart, vtype, vl, vcsr
+    sub     t0, t0, t4                                  // Allocate vector space for the stack frame
+#else
+    li      t4, 0
+#endif
+
     li      t1, 1                                       // Build stack type
     sd      t1,  0*8(t0)                                // Place stack type on the top
     sd      zero,  1*8(t0)                              // Initial s11
@@ -208,9 +228,31 @@ If floating point support:
     sd      zero, 62*8(t0)                              // Initial ft11
     csrr    a1, fcsr                                    // Read fcsr for initial value
     sd      a1, 63*8(t0)                                // Initial fcsr
-    sd      zero, 64*8(t0)                              // Reserved word (0)
+#endif
+
+#if defined(__riscv_vector)
+    /* Clear vector register space */
+#if defined(__riscv_float_abi_single) || defined(__riscv_float_abi_double)
+    addi    t2, t0, 64*8                                // t2 = start of vector registers
+#else
+    addi    t2, t0, 31*8                                // t2 = start of vector registers
+#endif
+    add     t3, t2, t4                                  // t3 = end of vector registers
+
+vector_clear_loop:
+    beq     t2, t3, vector_clear_done                   // Done if reached end
+    sd      zero, 0(t2)                                 // Clear 8 bytes
+    addi    t2, t2, 8                                   // Move to next 8 bytes
+    j       vector_clear_loop
+
+vector_clear_done:
+#endif
+
+    add     t2, t0, t4
+#if defined(__riscv_float_abi_single) || defined(__riscv_float_abi_double)
+    sd      zero, 64*8(t2)                              // Reserved word (0)
 #else
-    sd      zero, 31*8(t0)                              // Reserved word (0)
+    sd      zero, 31*8(t2)                              // Reserved word (0)
 #endif
 
     /* Setup stack pointer.  */
diff --git a/ports/risc-v64/gnu/src/tx_thread_system_return.S b/ports/risc-v64/gnu/src/tx_thread_system_return.S
index 4e954ebe..61babae3 100644
--- a/ports/risc-v64/gnu/src/tx_thread_system_return.S
+++ b/ports/risc-v64/gnu/src/tx_thread_system_return.S
@@ -68,6 +68,12 @@ _tx_thread_system_return:
 #else
     addi    sp, sp, -16*8                               // Allocate space on the stack - without floating point enabled
 #endif
+#if defined(__riscv_vector)
+    csrr    t1, vlenb                                   // Get vector register byte length
+    slli    t1, t1, 5                                   // Multiply by 32 (number of vector registers)
+    addi    t1, t1, 4*8                                 // Add vector CSR space: vstart, vtype, vl, vcsr
+    sub     sp, sp, t1                                  // Allocate vector space on the stack
+#endif
 
     /* Store floating point preserved registers.  */
 #if defined(__riscv_float_abi_single)
@@ -102,6 +108,36 @@ _tx_thread_system_return:
     sd      t0, 27*8(sp)                                // Store fcsr
 #endif
 
+#if defined(__riscv_vector)
+    /* Store vector registers and CSRs */
+#if defined(__riscv_float_abi_single) || defined(__riscv_float_abi_double)
+    addi    t1, sp, 28*8
+#else
+    addi    t1, sp, 15*8
+#endif
+    /* Store vector CSRs */
+    csrr    t2, vstart                                  // Store vstart
+    sd      t2, 0*8(t1)
+    csrr    t2, vtype                                   // Store vtype
+    sd      t2, 1*8(t1)
+    csrr    t2, vl                                      // Store vl
+    sd      t2, 2*8(t1)
+    csrr    t2, vcsr                                    // Store vcsr
+    sd      t2, 3*8(t1)
+
+    /* Store vector registers v0-v31 */
+    addi    t2, t1, 4*8
+    vsetvli t3, zero, e8, m8, ta, ma
+    vse8.v  v0, 0(t2)                                   // Store v0 ~ v7
+    add     t2, t2, t3
+    vse8.v  v8, 0(t2)                                   // Store v8 ~ v15
+    add     t2, t2, t3
+    vse8.v  v16, 0(t2)                                  // Store v16 ~ v23
+    add     t2, t2, t3
+    vse8.v  v24, 0(t2)                                  // Store v24 ~ v31
+    add     t2, t2, t3
+#endif
+
     sd      zero,  0(sp)                                // Solicited stack type
     sd      ra,  13*8(sp)                               // Save return address
     sd      s0,  12*8(sp)                               // Save s0