diff --git a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/include/tfm_svcalls.h b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/include/tfm_svcalls.h
index c64ce74889..fdbc4a9ce4 100644
--- a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/include/tfm_svcalls.h
+++ b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/include/tfm_svcalls.h
@@ -58,6 +58,7 @@ psa_handle_t tfm_svcall_psa_connect(uint32_t *args, int32_t ns_caller);
  *                              handle, in_vec, in_len, out_vec, out_len.
  * \param[in] ns_caller         If 'non-zero', call from non-secure client.
  *                              Or from secure client.
+ * \param[in] lr                Link register to be stored
  *
  * \retval >=0                  RoT Service-specific status value.
  * \retval <0                   RoT Service-specific error code.
@@ -73,7 +74,7 @@ psa_handle_t tfm_svcall_psa_connect(uint32_t *args, int32_t ns_caller);
  * \arg                           The message is unrecognized by the RoT
  *                                Service or incorrectly formatted.
  */
-psa_status_t tfm_svcall_psa_call(uint32_t *args, int32_t ns_caller);
+psa_status_t tfm_svcall_psa_call(uint32_t *args, int32_t ns_caller, uint32_t lr);
 
 /**
  * \brief SVC handler for \ref psa_close.
@@ -96,10 +97,11 @@ void tfm_svcall_psa_close(uint32_t *args, int32_t ns_caller);
  *
  * \param[in] svc_num           SVC number
  * \param[in] ctx               Argument context
+ * \param[in] lr                Link register to be stored
  *
  * \returns                     Return values from those who has,
  *                              or PSA_SUCCESS.
  */
-int32_t SVC_Handler_IPC(tfm_svc_number_t svc_num, uint32_t *ctx);
+int32_t SVC_Handler_IPC(tfm_svc_number_t svc_num, uint32_t *ctx, uint32_t lr);
 
 #endif
diff --git a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/tfm_svcalls.c b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/tfm_svcalls.c
index 68d244af5b..a3fab65fea 100644
--- a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/tfm_svcalls.c
+++ b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/tfm_svcalls.c
@@ -107,7 +107,7 @@ psa_handle_t tfm_svcall_psa_connect(uint32_t *args, int32_t ns_caller)
     return PSA_NULL_HANDLE;
 }
 
-psa_status_t tfm_svcall_psa_call(uint32_t *args, int32_t ns_caller)
+psa_status_t tfm_svcall_psa_call(uint32_t *args, int32_t ns_caller, uint32_t lr)
 {
     psa_handle_t handle;
     psa_invec *inptr, invecs[PSA_MAX_IOVEC];
@@ -124,14 +124,17 @@ psa_status_t tfm_svcall_psa_call(uint32_t *args, int32_t ns_caller)
         in_num = (size_t)args[2];
         outptr = (psa_outvec *)args[3];
         /*
-         * FixMe: 5th parameter is pushed at stack top before SVC; plus
-         * exception stacked contents, 5th parameter is now at 8th position
-         * in SVC handler. However, if thread mode applies FloatPoint, then
-         * FloatPoint context is pushed into stack and then 5th parameter
-         * will not be args[8].
-         * Will refine it later.
+         * 5th parameter is pushed at stack top before SVC; plus exception stacked contents,
+         * 5th parameter is now at 8th position in SVC handler.
+         * However, if thread mode applies FloatPoint, then FloatPoint context is pushed into
+         * stack and then 5th parameter will be args[26].
          */
-         out_num = (size_t)args[8];
+        if (lr & EXC_RETURN_FPU_FRAME_BASIC) {
+            out_num = (size_t)args[8];
+        }
+        else {
+            out_num = (size_t)args[26];
+        }
     } else {
         /*
          * FixMe: From non-secure caller, vec and len are composed into a new
@@ -926,7 +929,7 @@ static void tfm_svcall_psa_eoi(uint32_t *args)
     /* FixMe: re-enable interrupt */
 }
 
-int32_t SVC_Handler_IPC(tfm_svc_number_t svc_num, uint32_t *ctx)
+int32_t SVC_Handler_IPC(tfm_svc_number_t svc_num, uint32_t *ctx, uint32_t lr)
 {
     switch (svc_num) {
     case TFM_SVC_SCHEDULE:
@@ -939,7 +942,7 @@ int32_t SVC_Handler_IPC(tfm_svc_number_t svc_num, uint32_t *ctx)
     case TFM_SVC_PSA_CONNECT:
         return tfm_svcall_psa_connect(ctx, 0);
     case TFM_SVC_PSA_CALL:
-        return tfm_svcall_psa_call(ctx, 0);
+        return tfm_svcall_psa_call(ctx, 0, lr);
     case TFM_SVC_PSA_CLOSE:
         tfm_svcall_psa_close(ctx, 0);
         break;
diff --git a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_handler.c b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_handler.c
index 58e012322d..be77fcd3df 100644
--- a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_handler.c
+++ b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_handler.c
@@ -205,7 +205,7 @@ uint32_t SVCHandler_main(uint32_t *svc_args, uint32_t lr)
     case TFM_SVC_PSA_NOTIFY:
     case TFM_SVC_PSA_CLEAR:
     case TFM_SVC_PSA_EOI:
-        svc_args[0] = SVC_Handler_IPC(svc_number, svc_args);
+        svc_args[0] = SVC_Handler_IPC(svc_number, svc_args, lr);
         break;
 #endif
     default: