diff --git a/os/sb/host/sbapi.c b/os/sb/host/sbapi.c
index 2e0b97ded..c431e66b0 100644
--- a/os/sb/host/sbapi.c
+++ b/os/sb/host/sbapi.c
@@ -1216,19 +1216,11 @@ void __sb_abort(msg_t msg) {
   chSysHalt("zombies");
 }
 
-void __port_do_syscall_entry(struct port_extctx *ectxpxx,
+void __port_do_syscall_entry(struct port_extctx *ectxp,
                              uint32_t n) {
   extern void __sb_dispatch_syscall(struct port_extctx *ctxp, uint32_t n);
   thread_t *tp = __sch_get_currthread();
-  struct port_extctx *ectxp, *newctxp;
-  uint32_t u_psp;
-
-(void)ectxpxx;
-
-  /* Caller context in unprivileged memory.*/
-  u_psp = __get_PSP();
-  ectxp = (struct port_extctx *)u_psp;
-  __port_syscall_set_u_psp(tp, u_psp);
+  struct port_extctx *newctxp;
 
   /* Return context for change in privileged mode.*/
   newctxp = ((struct port_extctx *)__port_syscall_get_s_psp(tp)) - 1;
@@ -1242,6 +1234,9 @@ void __port_do_syscall_entry(struct port_extctx *ectxpxx,
   newctxp->fpscr = FPU->FPDSCR;
 #endif
 
+  /* Caller context in unprivileged memory.*/
+  __port_syscall_set_u_psp(tp, (uint32_t)ectxp);
+
   /* Switching PSP to the privileged mode PSP.*/
   __set_PSP((uint32_t)newctxp);
 }
diff --git a/os/sb/host/sbvrq.c b/os/sb/host/sbvrq.c
index 11da74c27..2042ebe3a 100644
--- a/os/sb/host/sbvrq.c
+++ b/os/sb/host/sbvrq.c
@@ -371,15 +371,23 @@ void sb_api_vrq_getisr(struct port_extctx *ectxp) {
 
 void sb_api_vrq_return(struct port_extctx *ectxp) {
   sb_class_t *sbp = (sb_class_t *)chThdGetSelfX()->ctx.syscall.p;
-
-  /* Returning from VRQ, enabling VRQs globally again.*/
-  sbp->vrq_isr = 0U;
+  sb_vrqmask_t active_mask;
 
   /* Discarding the return current context, returning on the previous one.
      TODO: Check for overflows????*/
   ectxp++;
 
-  __sb_vrq_check_pending(sbp, ectxp);
+  active_mask = sbp->vrq_wtmask & sbp->vrq_enmask;
+  if (active_mask != 0U) {
+    sbp->vrq_isr = 0U;
+
+    /* Creating a return context.*/
+    vrq_pushctx2(ectxp, sbp, active_mask);
+  }
+  else {
+    sbp->vrq_isr = 0U;
+    __set_PSP((uint32_t)ectxp);
+  }
 }
 
 /**