From 6003c5b30b8be9db2945f6025cf400de2008f98d Mon Sep 17 00:00:00 2001
From: Giovanni Di Sirio <gdisirio@gmail.com>
Date: Thu, 26 May 2022 13:13:43 +0000
Subject: [PATCH] Fixed a problem in SB VRQs preemption.

git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@15636 27425a3e-05d8-49a3-a47f-9c15f0e5edd8
---
 demos/various/SB-CLIENT-32k-08070000-4k-2001E000/main.c | 9 ++++++---
 os/sb/host/sbvrq.c                                      | 7 +++----
 2 files changed, 9 insertions(+), 7 deletions(-)

diff --git a/demos/various/SB-CLIENT-32k-08070000-4k-2001E000/main.c b/demos/various/SB-CLIENT-32k-08070000-4k-2001E000/main.c
index ca5f6e216..1b2bce4bb 100644
--- a/demos/various/SB-CLIENT-32k-08070000-4k-2001E000/main.c
+++ b/demos/various/SB-CLIENT-32k-08070000-4k-2001E000/main.c
@@ -23,6 +23,8 @@ void __crt0_vrq(int vrqn) {
 
   (void)vrqn;
 
+  printf("#1 Hello World (%u)!!\r\n", (unsigned)0);
+
   __sb_vrq_return();
 }
 
@@ -38,12 +40,13 @@ int main(void) {
   while (true) {
   }
 #endif
-  __sb_vrq_seten(2);
-  __sb_vrq_setwt(2);
+  __sb_vrq_seten(1);
+//  __sb_vrq_setwt(1);
+  sbSetAlarm(sbTimeMS2I(300), true);
 
   while (true) {
     msg_t msg = sbMsgWait();
-    printf("#1 Hello World (%u)!!\r\n", (unsigned)msg);
+//    printf("#1 Hello World (%u)!!\r\n", (unsigned)msg);
 //    sbFileWrite(1U, (const uint8_t *)"#1 Hello World!!\r\n", 15U);
     sbMsgReply(msg);
 //    sbSleepMilliseconds(500);
diff --git a/os/sb/host/sbvrq.c b/os/sb/host/sbvrq.c
index eeb7f1410..0578c647a 100644
--- a/os/sb/host/sbvrq.c
+++ b/os/sb/host/sbvrq.c
@@ -188,7 +188,7 @@ void sbVRQTriggerFromISR(sb_class_t *sbp, sb_vrqmask_t vmask) {
       if ((sbp->tp->state == CH_STATE_CURRENT)) {
         /* Sandbox case, getting the current exception frame.*/
         if ((__get_CONTROL() & 1U) == 0U) {
-          /* If preempted in privileged mode then getting the store U_PSP
+          /* If preempted in privileged mode then getting the stored U_PSP
              value.*/
           ectxp = (struct port_extctx *)sbp->tp->ctx.syscall.u_psp;
         }
@@ -213,9 +213,8 @@ void sbVRQTriggerFromISR(sb_class_t *sbp, sb_vrqmask_t vmask) {
         }
       }
       else {
-        /* Other thread case, getting the pointer from the context switch
-           structure.*/
-        ectxp   = sbp->tp->ctx.sp;
+        /* Getting the stored U_PSP value.*/
+        ectxp   = (struct port_extctx *)sbp->tp->ctx.syscall.u_psp;
         newctxp = ectxp - 1;
 
         /* Checking if the new frame is within the sandbox else failure.*/