From faef5bdb31abe979276b79e7169e407e334031cf Mon Sep 17 00:00:00 2001
From: Giovanni Di Sirio <gdisirio@gmail.com>
Date: Fri, 6 May 2022 04:30:21 +0000
Subject: [PATCH] VRQs code.

git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@15620 27425a3e-05d8-49a3-a47f-9c15f0e5edd8
---
 .../startup/ARMCMx-SB/compilers/GCC/crt0.S    | 10 +++-
 os/sb/common/sbhdr.h                          |  6 ++-
 os/sb/host/sbvrq.c                            | 49 +++++++++++++++++++
 os/sb/host/sbvrq.h                            |  2 +-
 4 files changed, 64 insertions(+), 3 deletions(-)

diff --git a/os/common/startup/ARMCMx-SB/compilers/GCC/crt0.S b/os/common/startup/ARMCMx-SB/compilers/GCC/crt0.S
index 57a4aa969..84ca559d5 100644
--- a/os/common/startup/ARMCMx-SB/compilers/GCC/crt0.S
+++ b/os/common/startup/ARMCMx-SB/compilers/GCC/crt0.S
@@ -100,7 +100,7 @@ __sandbox:      .long       0xFE9154C0
                 .long       32
                 .long       __crt0_entry
                 .long       __crt0_exit
-                .long       0
+                .long       __crt0_vfq
                 .long       0
                 .long       0
 
@@ -119,6 +119,14 @@ environ:
 
                 .text
 
+                /* Default VFQ handler.*/
+                .align      2
+                .thumb_func
+                .weak       __crt0_vfq
+__crt0_vfq:
+                ldr         r0, =0xFFFFFFFF
+                /* Falls into __crt0_exit.*/
+
                 /* Default exit method, calling OS exit().*/
                 .align      2
                 .thumb_func
diff --git a/os/sb/common/sbhdr.h b/os/sb/common/sbhdr.h
index 457685095..da33c8386 100644
--- a/os/sb/common/sbhdr.h
+++ b/os/sb/common/sbhdr.h
@@ -76,10 +76,14 @@ typedef struct {
    * @brief   Exit vector.
    */
   uint32_t                      hdr_exit;
+  /**
+   * @brief   VFQ vector.
+   */
+  uint32_t                      hdr_vfq;
   /**
    * @brief   Used-defined parameters, defaulted to zero.
    */
-  uint32_t                      user[3];
+  uint32_t                      user[2];
 } sb_header_t;
 
 /*===========================================================================*/
diff --git a/os/sb/host/sbvrq.c b/os/sb/host/sbvrq.c
index ec1644a0c..0c384616d 100644
--- a/os/sb/host/sbvrq.c
+++ b/os/sb/host/sbvrq.c
@@ -25,6 +25,8 @@
  * @{
  */
 
+#include <string.h>
+
 #include "sb.h"
 
 #if (SB_CFG_ENABLE_VRQ == TRUE) || defined(__DOXYGEN__)
@@ -53,6 +55,53 @@
 /* Module exported functions.                                                */
 /*===========================================================================*/
 
+/**
+ * @brief   Activates VRQs on the specified sandbox.
+ *
+ * @param[in] sbp       pointer to a @p sb_class_t structure
+ * @param[in] vmask     mask of VRQs to be activated
+ * @return              The operation status.
+ * @retval false        if the activation has succeeded.
+ * @retval true         in case of sandbox stack overflow.
+ */
+bool sbVRQSignalMaskI(sb_class_t *sbp, sb_vrqmask_t vmask) {
+  struct port_extctx *ectxp;
+  const sb_header_t *sbhp;
+
+  /* This IRQ could have preempted the sandbox itself or some other thread,
+     handling is different.*/
+  if (sbp->tp->state == CH_STATE_CURRENT) {
+    /* Sandbox case, getting the current exception frame.*/
+    ectxp = (struct port_extctx *)__get_PSP() - 1;
+
+    /* Checking if the new frame is within the sandbox else failure.*/
+    if (!sb_is_valid_write_range(sbp,
+                                 (void *)ectxp,
+                                 sizeof (struct port_extctx))) {
+      return true;
+    }
+  }
+  else {
+    ectxp = sbp->tp->ctx.sp - 1;
+
+    /* Checking if the new frame is within the sandbox else failure.*/
+    if (!sb_is_valid_write_range(sbp,
+                                 (void *)ectxp,
+                                 sizeof (struct port_extctx))) {
+      return true;
+    }
+
+    /* Preventing leakage of information, clearing all register values, those
+       would come from outside the sandbox.*/
+    memset((void *)ectxp, 0, sizeof (struct port_extctx));
+  }
+
+  /* Header location.*/
+  sbhp = (const sb_header_t *)(void *)sbp->config->regions[sbp->config->code_region].area.base;
+
+  return false;
+}
+
 #endif /* SB_CFG_ENABLE_VRQ == TRUE */
 
 /** @} */
diff --git a/os/sb/host/sbvrq.h b/os/sb/host/sbvrq.h
index 83077a765..4dc73e4ad 100644
--- a/os/sb/host/sbvrq.h
+++ b/os/sb/host/sbvrq.h
@@ -62,7 +62,7 @@ typedef uint32_t sb_vrqmask_t;
 #ifdef __cplusplus
 extern "C" {
 #endif
-  void sbVRQPendI(sb_class_t *sbp, sb_vrqmask_t vmask);
+  bool sbVRQSignalMaskI(sb_class_t *sbp, sb_vrqmask_t vmask);
 #ifdef __cplusplus
 }
 #endif