From a84ef983632f7c257d79b2d35f43825a43ae2097 Mon Sep 17 00:00:00 2001
From: Giovanni Di Sirio <gdisirio@gmail.com>
Date: Fri, 30 Sep 2022 10:01:27 +0000
Subject: [PATCH] IRQ-on-VRQ routing concept, to be completed.

git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@15806 27425a3e-05d8-49a3-a47f-9c15f0e5edd8
---
 os/sb/host/sb.h    |  8 ++++++++
 os/sb/host/sbvrq.c | 34 +++++++++++++++-------------------
 2 files changed, 23 insertions(+), 19 deletions(-)

diff --git a/os/sb/host/sb.h b/os/sb/host/sb.h
index 9c1a02132..d828a65c0 100644
--- a/os/sb/host/sb.h
+++ b/os/sb/host/sb.h
@@ -325,6 +325,14 @@ struct sb_class {
    * @brief   Reference to sh SB thread while waiting for VRQs.
    */
   thread_reference_t            vrq_trp;
+  /**
+   * @brief   NVIC ISER to be updated on VRQ exit or NULL.
+   */
+  uint32_t                      *vrq_nvic_iser;
+  /**
+   * @brief   NVIC ISER mask to be applied on VRQ exit.
+   */
+  uint32_t                      vrq_nvic_mask;
 #endif
 #if (SB_CFG_ENABLE_VFS == TRUE) || defined(__DOXYGEN__)
   /**
diff --git a/os/sb/host/sbvrq.c b/os/sb/host/sbvrq.c
index 1724d1101..886a7a031 100644
--- a/os/sb/host/sbvrq.c
+++ b/os/sb/host/sbvrq.c
@@ -78,9 +78,9 @@ static inline void vrq_makectx(struct port_extctx *newctxp,
 }
 
 CC_NO_INLINE
-static struct port_extctx * vrq_writectx(struct port_extctx *ectxp,
-                                         sb_class_t *sbp,
-                                         sb_vrqmask_t active_mask) {
+static struct port_extctx *vrq_writectx(struct port_extctx *ectxp,
+                                        sb_class_t *sbp,
+                                        sb_vrqmask_t active_mask) {
 
   /* Checking if the new frame is within the sandbox else failure.*/
   if (!sb_is_valid_write_range(sbp,
@@ -99,6 +99,9 @@ static struct port_extctx * vrq_writectx(struct port_extctx *ectxp,
   return ectxp;
 }
 
+/* Note, this function may look an useless duplication of vrq_writectx() but
+   the included __set_PSP() makes it a viable tail-call candidate for the
+   compiler, this is a significant performance gain is several places.*/
 CC_NO_INLINE
 static void vrq_pushctx(struct port_extctx *ectxp,
                         sb_class_t *sbp,
@@ -159,16 +162,9 @@ void sbVRQTriggerS(sb_class_t *sbp, sb_vrqnum_t nvrq) {
       /* Checking if it is running in unprivileged mode, in this case we
          need to build a return context in its current PSP.*/
       if ((__get_CONTROL() & 1U) != 0U) {
-        struct port_extctx *ectxp;
-
-        /* Getting the current PSP, it is U_PSP.*/
-        ectxp = (struct port_extctx *)__get_PSP();
 
         /* Creating a return context.*/
-        ectxp = vrq_writectx(ectxp, sbp, active_mask);
-
-        /* Updating PSP position.*/
-        __set_PSP((uint32_t)ectxp);
+        vrq_pushctx((struct port_extctx *)__get_PSP(), sbp, active_mask);
       }
       else {
         /* It is in privileged mode so it will check for pending VRQs
@@ -210,16 +206,9 @@ void sbVRQTriggerI(sb_class_t *sbp, sb_vrqnum_t nvrq) {
         /* Checking if it is running in unprivileged mode, in this case we
            need to build a return context in its current PSP.*/
         if ((__get_CONTROL() & 1U) != 0U) {
-          struct port_extctx *ectxp;
-
-          /* Getting the current PSP, it is U_PSP.*/
-          ectxp = (struct port_extctx *)__get_PSP();
 
           /* Creating a return context.*/
-          ectxp = vrq_writectx(ectxp, sbp, active_mask);
-
-          /* Updating PSP position.*/
-          __set_PSP((uint32_t)ectxp);
+          vrq_pushctx((struct port_extctx *)__get_PSP(), sbp, active_mask);
         }
         else {
           /* It is in privileged mode so it will check for pending VRQs
@@ -376,12 +365,19 @@ void sb_fastc_vrq_return(struct port_extctx *ectxp) {
   sb_class_t *sbp = (sb_class_t *)chThdGetSelfX()->ctx.syscall.p;
   sb_vrqmask_t active_mask;
 
+  /* Checking if there are IRQs to be re-enabled on VRQ exit.*/
+  if (sbp->vrq_nvic_iser != NULL) {
+    *sbp->vrq_nvic_iser = sbp->vrq_nvic_mask;
+  }
+
   /* Discarding the return current context, returning on the previous one.
      TODO: Check for overflows????*/
   ectxp++;
 
   active_mask = sbp->vrq_wtmask & sbp->vrq_enmask;
   if (active_mask != 0U) {
+
+    /* Re-enabling VRQs globally.*/
     sbp->vrq_isr = 0U;
 
     /* Creating a return context.*/