product/module/*_system: Subscribe to SDS & SCMI notifications

Change-Id: Id37f8a4b415dd51ea359caef0904e41dffa62d84
Signed-off-by: Nicola Mazzucato <nicola.mazzucato@arm.com>

1 files changed, 67 insertions, 21 deletions

diff --git a/product/rdn1e1/module/rdn1e1_system/src/mod_rdn1e1_system.c b/product/rdn1e1/module/rdn1e1_system/src/mod_rdn1e1_system.c
index 80de3f17..5409350b 100644
--- a/product/rdn1e1/module/rdn1e1_system/src/mod_rdn1e1_system.c
+++ b/product/rdn1e1/module/rdn1e1_system/src/mod_rdn1e1_system.c
@@ -20,6 +20,8 @@
 #include <mod_clock.h>
 #include <mod_rdn1e1_system.h>
 #include <mod_log.h>
+#include <mod_scmi.h>
+#include <mod_sds.h>
 #include <mod_system_power.h>
 #include <mod_power_domain.h>
 #include <mod_ppu_v1.h>
@@ -27,8 +29,15 @@
 #include <rdn1e1_pik_scp.h>
 #include <scp_rdn1e1_irq.h>
 #include <scp_rdn1e1_mmap.h>
+#include <scp_rdn1e1_scmi.h>
 #include <config_clock.h>
 
+/* SCMI services required to enable the messaging stack */
+static unsigned int scmi_notification_table[] = {
+    SCP_RDN1E1_SCMI_SERVICE_IDX_PSCI,
+    SCP_RDN1E1_SCMI_SERVICE_IDX_OSPM,
+};
+
 /* Module context */
 struct rdn1e1_system_ctx {
     /* Pointer to the SCP PIK registers */
@@ -202,6 +211,7 @@ static int rdn1e1_system_process_bind_request(fwk_id_t requester_id,
 static int rdn1e1_system_start(fwk_id_t id)
 {
     int status;
+    unsigned int i;
 
     status = fwk_notification_subscribe(
         mod_clock_notification_id_state_changed,
@@ -213,6 +223,33 @@ static int rdn1e1_system_start(fwk_id_t id)
     rdn1e1_system_ctx.log_api->log(MOD_LOG_GROUP_DEBUG,
         "[RDN1E1 SYSTEM] Requesting SYSTOP initialization...\n");
 
+    /*
+     * Subscribe to these SCMI channels in order to know when they have all
+     * initialized.
+     * At that point we can consider the SCMI stack to be initialized from
+     * the point of view of the PSCI agent.
+     */
+    for (i = 0; i < FWK_ARRAY_SIZE(scmi_notification_table); i++) {
+        status = fwk_notification_subscribe(
+            mod_scmi_notification_id_initialized,
+            fwk_id_build_element_id(fwk_module_id_scmi,
+                scmi_notification_table[i]),
+            id);
+        if (status != FWK_SUCCESS)
+            return status;
+    }
+
+    /*
+     * Subscribe to the SDS initialized notification so we can correctly let the
+     * PSCI agent know that the SCMI stack is initialized.
+     */
+    status = fwk_notification_subscribe(
+        mod_sds_notification_id_initialized,
+        fwk_module_id_sds,
+        id);
+    if (status != FWK_SUCCESS)
+        return status;
+
     return
         rdn1e1_system_ctx.mod_pd_restricted_api->set_composite_state_async(
             FWK_ID_ELEMENT(FWK_MODULE_IDX_POWER_DOMAIN, 0), false,
@@ -227,35 +264,44 @@ int rdn1e1_system_process_notification(const struct fwk_event *event,
     struct clock_notification_params *params;
     struct mod_pd_restricted_api *mod_pd_restricted_api;
 
-    assert(fwk_id_is_equal(event->id, mod_clock_notification_id_state_changed));
     assert(fwk_id_is_type(event->target_id, FWK_ID_TYPE_MODULE));
 
-    params = (struct clock_notification_params *)event->params;
+    if (fwk_id_is_equal(event->id, mod_clock_notification_id_state_changed)) {
+        params = (struct clock_notification_params *)event->params;
 
-    /*
-     * Initialize primary core when the system is initialized for the first time
-     * only
-     */
-    if (params->new_state == MOD_CLOCK_STATE_RUNNING) {
-        rdn1e1_system_ctx.log_api->log(MOD_LOG_GROUP_DEBUG,
-            "[RDN1E1 SYSTEM] Initializing the primary core...\n");
+        /*
+         * Initialize primary core when the system is initialized for the first
+         * time only
+         */
+        if (params->new_state == MOD_CLOCK_STATE_RUNNING) {
+            rdn1e1_system_ctx.log_api->log(MOD_LOG_GROUP_DEBUG,
+                "[RDN1E1 SYSTEM] Initializing the primary core...\n");
 
-        mod_pd_restricted_api = rdn1e1_system_ctx.mod_pd_restricted_api;
+            mod_pd_restricted_api = rdn1e1_system_ctx.mod_pd_restricted_api;
 
-        status =  mod_pd_restricted_api->set_composite_state_async(
-            FWK_ID_ELEMENT(FWK_MODULE_IDX_POWER_DOMAIN, 0),
-            false,
-            MOD_PD_COMPOSITE_STATE(MOD_PD_LEVEL_2, 0, MOD_PD_STATE_ON,
-                MOD_PD_STATE_ON, MOD_PD_STATE_ON));
-        if (status != FWK_SUCCESS)
-            return status;
+            status =  mod_pd_restricted_api->set_composite_state_async(
+                FWK_ID_ELEMENT(FWK_MODULE_IDX_POWER_DOMAIN, 0),
+                false,
+                MOD_PD_COMPOSITE_STATE(MOD_PD_LEVEL_2, 0, MOD_PD_STATE_ON,
+                    MOD_PD_STATE_ON, MOD_PD_STATE_ON));
+            if (status != FWK_SUCCESS)
+                return status;
+
+            /* Unsubscribe to the notification */
+            return fwk_notification_unsubscribe(event->id, event->source_id,
+                                                event->target_id);
+        }
 
-        /* Unsubscribe to the notification */
-        return fwk_notification_unsubscribe(event->id, event->source_id,
-                                            event->target_id);
+        return FWK_SUCCESS;
+    } else if (fwk_id_is_equal(event->id,
+                               mod_scmi_notification_id_initialized)) {
+        return FWK_SUCCESS;
+    } else if (fwk_id_is_equal(event->id,
+                               mod_sds_notification_id_initialized)) {
+        return FWK_SUCCESS;
     }
 
-    return FWK_SUCCESS;
+    return FWK_E_PARAM;
 }
 
 const struct fwk_module module_rdn1e1_system = {