From 0738442a0c6aaefb7fa33355b2885a709c80d28f Mon Sep 17 00:00:00 2001
From: Giovanni Di Sirio <gdisirio@gmail.com>
Date: Wed, 14 Apr 2021 11:52:42 +0000
Subject: [PATCH] Shared registry in SMP mode, to be tested.

git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@14174 27425a3e-05d8-49a3-a47f-9c15f0e5edd8
---
 os/rt/include/chobjects.h  | 12 +++++++++++-
 os/rt/include/chregistry.h | 11 ++++++++++-
 os/rt/src/chregistry.c     |  4 ++--
 os/rt/src/chschd.c         |  4 ++--
 os/rt/src/chsys.c          | 28 +++++++++++++++++++++++-----
 5 files changed, 48 insertions(+), 11 deletions(-)

diff --git a/os/rt/include/chobjects.h b/os/rt/include/chobjects.h
index 14943dd55..a404932df 100644
--- a/os/rt/include/chobjects.h
+++ b/os/rt/include/chobjects.h
@@ -358,9 +358,11 @@ struct ch_os_instance {
    * @brief   Virtual timers delta list header.
    */
   virtual_timers_list_t vtlist;
-#if (CH_CFG_USE_REGISTRY == TRUE) || defined(__DOXYGEN__)
+#if ((CH_CFG_USE_REGISTRY == TRUE) && (CH_CFG_SMP_MODE == FALSE)) ||        \
+    defined(__DOXYGEN__)
   /**
    * @brief   Registry header.
+   * @note    This field is present only if the SMP mode is disabled.
    */
   ch_queue_t            reglist;
 #endif
@@ -414,6 +416,14 @@ typedef struct ch_system {
    * @brief   Initialized OS instances or @p NULL.
    */
   os_instance_t         *instances[PORT_CORES_NUMBER];
+#if ((CH_CFG_USE_REGISTRY == TRUE) && (CH_CFG_SMP_MODE == TRUE)) ||         \
+    defined(__DOXYGEN__)
+  /**
+   * @brief   Registry header.
+   * @note    This field is present only if the SMP mode is enabled.
+   */
+  ch_queue_t            reglist;
+#endif
 #if defined(PORT_SYSTEM_EXTRA_FIELDS) || defined(__DOXYGEN__)
   /* Extra fields from port layer.*/
   PORT_SYSTEM_EXTRA_FIELDS
diff --git a/os/rt/include/chregistry.h b/os/rt/include/chregistry.h
index a5c57efad..5a803844e 100644
--- a/os/rt/include/chregistry.h
+++ b/os/rt/include/chregistry.h
@@ -74,6 +74,15 @@ typedef struct {
 /* Module macros.                                                            */
 /*===========================================================================*/
 
+/**
+ * @brief   Access to the registry list header.
+ */
+#if (CH_CFG_SMP_MODE == TRUE) || defined(__DOXYGEN__)
+#define REG_HEADER(oip) (&ch_system.reglist)
+#else
+#define REG_HEADER(oip) (&(oip)->reglist)
+#endif
+
 /**
  * @brief   Removes a thread from the registry list.
  * @note    This macro is not meant for use in application code.
@@ -89,7 +98,7 @@ typedef struct {
  * @param[in] oip       pointer to the OS instance
  * @param[in] tp        thread to add to the registry
  */
-#define REG_INSERT(oip, tp) ch_queue_insert(&(tp)->rqueue, &(oip)->reglist)
+#define REG_INSERT(oip, tp) ch_queue_insert(&(tp)->rqueue, REG_HEADER(oip))
 
 /*===========================================================================*/
 /* External declarations.                                                    */
diff --git a/os/rt/src/chregistry.c b/os/rt/src/chregistry.c
index bf4ffc0fb..4da18e6c5 100644
--- a/os/rt/src/chregistry.c
+++ b/os/rt/src/chregistry.c
@@ -136,7 +136,7 @@ thread_t *chRegFirstThread(void) {
   uint8_t *p;
 
   chSysLock();
-  p = (uint8_t *)currcore->reglist.next;
+  p = (uint8_t *)REG_HEADER(currcore)->next;
   tp = (thread_t *)(p - offsetof(thread_t, rqueue));
 #if CH_CFG_USE_DYNAMIC == TRUE
   tp->refs++;
@@ -165,7 +165,7 @@ thread_t *chRegNextThread(thread_t *tp) {
 
   /* Next element in the registry queue.*/
   nqp = tp->rqueue.next;
-  if (nqp == &currcore->reglist) {
+  if (nqp == REG_HEADER(currcore)) {
     ntp = NULL;
   }
 #if CH_CFG_USE_DYNAMIC == TRUE
diff --git a/os/rt/src/chschd.c b/os/rt/src/chschd.c
index 374208bee..f1a620d35 100644
--- a/os/rt/src/chschd.c
+++ b/os/rt/src/chschd.c
@@ -313,8 +313,8 @@ void chSchObjectInit(os_instance_t *oip,
   /* Ready list initialization.*/
   ch_pqueue_init(&oip->rlist.pqueue);
 
-  /* Registry initialization.*/
-#if CH_CFG_USE_REGISTRY == TRUE
+#if (CH_CFG_USE_REGISTRY == TRUE) && (CH_CFG_SMP_MODE == FALSE)
+  /* Registry initialization when SMP mode is disabled.*/
   ch_queue_init(&oip->reglist);
 #endif
 
diff --git a/os/rt/src/chsys.c b/os/rt/src/chsys.c
index 4a72096ee..19b9aca22 100644
--- a/os/rt/src/chsys.c
+++ b/os/rt/src/chsys.c
@@ -91,6 +91,11 @@ void chSysInit(void) {
     ch_system.instances[i] = NULL;
   }
 
+#if (CH_CFG_USE_REGISTRY == TRUE) && (CH_CFG_SMP_MODE == TRUE)
+  /* Registry initialization when SMP mode is enabled.*/
+  ch_queue_init(&ch_system.reglist);
+#endif
+
   /* User system initialization hook.*/
   CH_CFG_SYSTEM_INIT_HOOK();
 
@@ -152,6 +157,15 @@ void chSysHalt(const char *reason) {
   /* Halt hook code, usually empty.*/
   CH_CFG_SYSTEM_HALT_HOOK(reason);
 
+#if defined(PORT_SYSTEM_HALT_HOOK)
+  /* Port-related actions, this could include halting other instances
+     via some inter-core messaging or other means.*/
+  PORT_SYSTEM_HALT_HOOK();
+#endif
+
+  /* Entering the halted state.*/
+  ch_system.state = ch_state_halted;
+
   /* Harmless infinite loop.*/
   while (true) {
   }
@@ -239,19 +253,23 @@ bool chSysIntegrityCheckI(unsigned testmask) {
 
 #if CH_CFG_USE_REGISTRY == TRUE
   if ((testmask & CH_INTEGRITY_REGISTRY) != 0U) {
-    ch_queue_t *qp;
+    ch_queue_t *qp, *rqp;
+
+    /* Registry header, access to this list depends on the current
+       kernel configuration.*/
+    rqp = REG_HEADER(oip);
 
     /* Scanning the ready list forward.*/
     n = (cnt_t)0;
-    qp = oip->reglist.next;
-    while (qp != &oip->reglist) {
+    qp = rqp->next;
+    while (qp != rqp) {
       n++;
       qp = qp->next;
     }
 
     /* Scanning the ready list backward.*/
-    qp = oip->reglist.prev;
-    while (qp != &oip->reglist) {
+    qp = rqp->prev;
+    while (qp != rqp) {
       n--;
       qp = qp->prev;
     }