Fix FPU stack order issue and Improve FPU checking flow

Author: TrongNguyenR

portable/CCRH/F1Kx/portasm.s

@@ -84,6 +84,8 @@ portSAVE_CONTEXT .macro
     stsr    FPEPC, r19
     pushsp  r18, r19
 
+    pushsp  r14, r15
+
     ; Get current TCB, the return value is stored in r10 (CCRH compiler)
     jarl    _pvPortGetCurrentTCB, lp
     st.w    sp, 0[r10]
@@ -101,14 +103,13 @@ portRESTORE_CONTEXT .macro
 
     ; Restore FPU registers if FPU is enabled
     mov     FPU_MSK, r19
-    stsr    PSW, r18
-    tst     r18, r19
-
-     ; Jump over next 3 instructions: stsr (4 bytes)*2 + popsp (4 bytes)
+    popsp	r14, r15
+    tst     r15, r19
+    ; Jump over next 3 instructions: stsr (4 bytes)*2 + popsp (4 bytes)
     bz      12
     popsp   r18, r19
-    ldsr    r18, FPEPC
-    ldsr    r19, FPSR
+    ldsr    r19, FPEPC
+    ldsr    r18, FPSR
 
     ;Restore general-purpose registers and EIPSW, EIPC, EIIC, CTPSW, CTPC
     popsp   r15, r19
@@ -146,30 +147,30 @@ SAVE_REGISTER .macro
     mov     ep, r15
     stsr    CTPSW, r14
     stsr    CTPC, r13
-    pushsp  r13, r19
+    pushsp  r13, r18
 
     mov     FPU_MSK, r16
     tst     r16, r19
-    bz      12
-    stsr    FPSR, r18
-    stsr    FPEPC, r19
-    pushsp  r18, r19
+    bz      8
+    stsr    FPSR, r17
+    stsr    FPEPC, r18
+
+    pushsp  r17, r19
 
 .endm
 ;------------------------------------------------------------------------------
 ; Restore used registers
 ;------------------------------------------------------------------------------
 RESTORE_REGISTER .macro
 
-    mov     FPU_MSK, r16
-    stsr    PSW, r18
-    tst     r18, r19
-    bz      12
-    popsp   r18, r19
+	mov     FPU_MSK, r15
+	popsp   r17, r19
+    tst     r19, r15
+    bz      8
     ldsr    r18, FPEPC
-    ldsr    r19, FPSR
+    ldsr    r17, FPSR
 
-    popsp   r13, r19
+    popsp   r13, r18
     ldsr    r13, CTPC
     ldsr    r14, CTPSW
     mov     r15, ep