Rework internal API to save non-secure entry point info

This patch replaces the internal psci_save_ns_entry() API with a
psci_get_ns_ep_info() API. The new function splits the work done by the
previous one such that it populates and returns an 'entry_point_info_t'
structure with the information to enter the normal world upon completion
of the CPU_SUSPEND or CPU_ON call. This information is used to populate
the non-secure context structure separately.

This allows the new internal API `psci_get_ns_ep_info` to return error
and enable the code to return safely.

Change-Id: Ifd87430a4a3168eac0ebac712f59c93cbad1b231

diff --git a/services/std_svc/psci/psci_afflvl_on.c b/services/std_svc/psci/psci_afflvl_on.c
index 04529d2fc86a5f51490f0d76103b628ecc6af126..b778c5ee8f273fd9ac858c1f1b3e3fe2b6a7c7bb 100644 (file)
--- a/services/std_svc/psci/psci_afflvl_on.c
+++ b/services/std_svc/psci/psci_afflvl_on.c
@@ -40,9 +40,7 @@
 #include "psci_private.h"
 
 typedef int (*afflvl_on_handler_t)(unsigned long target_cpu,
-                                aff_map_node_t *node,
-                                unsigned long ns_entrypoint,
-                                unsigned long context_id);
+                                aff_map_node_t *node);
 
 /*******************************************************************************
  * This function checks whether a cpu which has been requested to be turned on
@@ -66,14 +64,9 @@ static int cpu_on_validate_state(unsigned int psci_state)
  * TODO: Split this code across separate handlers for each type of setup?
  ******************************************************************************/
 static int psci_afflvl0_on(unsigned long target_cpu,
-                          aff_map_node_t *cpu_node,
-                          unsigned long ns_entrypoint,
-                          unsigned long context_id)
+                          aff_map_node_t *cpu_node)
 {
        unsigned long psci_entrypoint;
-       uint32_t ns_scr_el3 = read_scr_el3();
-       uint32_t ns_sctlr_el1 = read_sctlr_el1();
-       int rc;
 
        /* Sanity check to safeguard against data corruption */
        assert(cpu_node->level == MPIDR_AFFLVL0);
@@ -86,16 +79,6 @@ static int psci_afflvl0_on(unsigned long target_cpu,
        if (psci_spd_pm && psci_spd_pm->svc_on)
                psci_spd_pm->svc_on(target_cpu);
 
-       /*
-        * Arch. management: Derive the re-entry information for
-        * the non-secure world from the non-secure state from
-        * where this call originated.
-        */
-       rc = psci_save_ns_entry(target_cpu, ns_entrypoint, context_id,
-                               ns_scr_el3, ns_sctlr_el1);
-       if (rc != PSCI_E_SUCCESS)
-               return rc;
-
        /* Set the secure world (EL3) re-entry point after BL1 */
        psci_entrypoint = (unsigned long) psci_aff_on_finish_entry;
 
@@ -119,9 +102,7 @@ static int psci_afflvl0_on(unsigned long target_cpu,
  * TODO: Split this code across separate handlers for each type of setup?
  ******************************************************************************/
 static int psci_afflvl1_on(unsigned long target_cpu,
-                          aff_map_node_t *cluster_node,
-                          unsigned long ns_entrypoint,
-                          unsigned long context_id)
+                          aff_map_node_t *cluster_node)
 {
        unsigned long psci_entrypoint;
 
@@ -155,9 +136,7 @@ static int psci_afflvl1_on(unsigned long target_cpu,
  * TODO: Split this code across separate handlers for each type of setup?
  ******************************************************************************/
 static int psci_afflvl2_on(unsigned long target_cpu,
-                          aff_map_node_t *system_node,
-                          unsigned long ns_entrypoint,
-                          unsigned long context_id)
+                          aff_map_node_t *system_node)
 {
        unsigned long psci_entrypoint;
 
@@ -201,9 +180,7 @@ static const afflvl_on_handler_t psci_afflvl_on_handlers[] = {
 static int psci_call_on_handlers(aff_map_node_t *target_cpu_nodes[],
                                 int start_afflvl,
                                 int end_afflvl,
-                                unsigned long target_cpu,
-                                unsigned long entrypoint,
-                                unsigned long context_id)
+                                unsigned long target_cpu)
 {
        int rc = PSCI_E_INVALID_PARAMS, level;
        aff_map_node_t *node;
@@ -219,9 +196,7 @@ static int psci_call_on_handlers(aff_map_node_t *target_cpu_nodes[],
                 * affinity levels.
                 */
                rc = psci_afflvl_on_handlers[level](target_cpu,
-                                                   node,
-                                                   entrypoint,
-                                                   context_id);
+                                                   node);
                if (rc != PSCI_E_SUCCESS)
                        break;
        }
@@ -246,8 +221,7 @@ static int psci_call_on_handlers(aff_map_node_t *target_cpu_nodes[],
  * first.
  ******************************************************************************/
 int psci_afflvl_on(unsigned long target_cpu,
-                  unsigned long entrypoint,
-                  unsigned long context_id,
+                  entry_point_info_t *ep,
                   int start_afflvl,
                   int end_afflvl)
 {
@@ -290,20 +264,23 @@ int psci_afflvl_on(unsigned long target_cpu,
        rc = psci_call_on_handlers(target_cpu_nodes,
                                   start_afflvl,
                                   end_afflvl,
-                                  target_cpu,
-                                  entrypoint,
-                                  context_id);
+                                  target_cpu);
 
        /*
         * This function updates the state of each affinity instance
         * corresponding to the mpidr in the range of affinity levels
         * specified.
         */
-       if (rc == PSCI_E_SUCCESS)
+       if (rc == PSCI_E_SUCCESS) {
                psci_do_afflvl_state_mgmt(start_afflvl,
                                          end_afflvl,
                                          target_cpu_nodes,
                                          PSCI_STATE_ON_PENDING);
+               /*
+                * Store the re-entry information for the non-secure world.
+                */
+               cm_init_context(target_cpu, ep);
+       }
 
 exit:
        /*

diff --git a/services/std_svc/psci/psci_afflvl_suspend.c b/services/std_svc/psci/psci_afflvl_suspend.c
index d30bf399bdb0be02517090c2fd8b01d4ec97547b..4988e677af28c5072229f9db685a9bde34ad0d07 100644 (file)
--- a/services/std_svc/psci/psci_afflvl_suspend.c
+++ b/services/std_svc/psci/psci_afflvl_suspend.c
@@ -41,8 +41,6 @@
 #include "psci_private.h"
 
 typedef int (*afflvl_suspend_handler_t)(aff_map_node_t *node,
-                                     unsigned long ns_entrypoint,
-                                     unsigned long context_id,
                                      unsigned int power_state);
 
 /*******************************************************************************
@@ -106,14 +104,9 @@ int psci_get_suspend_stateid_by_mpidr(unsigned long mpidr)
  * level which is called when that affinity level is about to be suspended.
  ******************************************************************************/
 static int psci_afflvl0_suspend(aff_map_node_t *cpu_node,
-                               unsigned long ns_entrypoint,
-                               unsigned long context_id,
                                unsigned int power_state)
 {
        unsigned long psci_entrypoint;
-       uint32_t ns_scr_el3 = read_scr_el3();
-       uint32_t ns_sctlr_el1 = read_sctlr_el1();
-       int rc;
 
        /* Sanity check to safeguard against data corruption */
        assert(cpu_node->level == MPIDR_AFFLVL0);
@@ -122,8 +115,7 @@ static int psci_afflvl0_suspend(aff_map_node_t *cpu_node,
        psci_set_suspend_power_state(power_state);
 
        /*
-        * Generic management: Store the re-entry information for the non-secure
-        * world and allow the secure world to suspend itself
+        * Generic management: Allow the Secure world to suspend itself
         */
 
        /*
@@ -134,14 +126,6 @@ static int psci_afflvl0_suspend(aff_map_node_t *cpu_node,
        if (psci_spd_pm && psci_spd_pm->svc_suspend)
                psci_spd_pm->svc_suspend(power_state);
 
-       /*
-        * Generic management: Store the re-entry information for the
-        * non-secure world
-        */
-       rc = psci_save_ns_entry(read_mpidr_el1(), ns_entrypoint, context_id,
-                               ns_scr_el3, ns_sctlr_el1);
-       if (rc != PSCI_E_SUCCESS)
-               return rc;
 
        /* Set the secure world (EL3) re-entry point after BL1 */
        psci_entrypoint = (unsigned long) psci_aff_suspend_finish_entry;
@@ -167,8 +151,6 @@ static int psci_afflvl0_suspend(aff_map_node_t *cpu_node,
 }
 
 static int psci_afflvl1_suspend(aff_map_node_t *cluster_node,
-                               unsigned long ns_entrypoint,
-                               unsigned long context_id,
                                unsigned int power_state)
 {
        unsigned int plat_state;
@@ -203,8 +185,6 @@ static int psci_afflvl1_suspend(aff_map_node_t *cluster_node,
 
 
 static int psci_afflvl2_suspend(aff_map_node_t *system_node,
-                               unsigned long ns_entrypoint,
-                               unsigned long context_id,
                                unsigned int power_state)
 {
        unsigned int plat_state;
@@ -259,8 +239,6 @@ static const afflvl_suspend_handler_t psci_afflvl_suspend_handlers[] = {
 static int psci_call_suspend_handlers(aff_map_node_t *mpidr_nodes[],
                                      int start_afflvl,
                                      int end_afflvl,
-                                     unsigned long entrypoint,
-                                     unsigned long context_id,
                                      unsigned int power_state)
 {
        int rc = PSCI_E_INVALID_PARAMS, level;
@@ -277,8 +255,6 @@ static int psci_call_suspend_handlers(aff_map_node_t *mpidr_nodes[],
                 * lower affinity levels.
                 */
                rc = psci_afflvl_suspend_handlers[level](node,
-                                                        entrypoint,
-                                                        context_id,
                                                         power_state);
                if (rc != PSCI_E_SUCCESS)
                        break;
@@ -306,8 +282,7 @@ static int psci_call_suspend_handlers(aff_map_node_t *mpidr_nodes[],
  * to turn off affinity level X it is neccesary to turn off affinity level X - 1
  * first.
  ******************************************************************************/
-int psci_afflvl_suspend(unsigned long entrypoint,
-                       unsigned long context_id,
+int psci_afflvl_suspend(entry_point_info_t *ep,
                        unsigned int power_state,
                        int start_afflvl,
                        int end_afflvl)
@@ -356,12 +331,15 @@ int psci_afflvl_suspend(unsigned long entrypoint,
        /* Stash the highest affinity level that will be turned off */
        psci_set_max_phys_off_afflvl(max_phys_off_afflvl);
 
+       /*
+        * Store the re-entry information for the non-secure world.
+        */
+       cm_init_context(read_mpidr_el1(), ep);
+
        /* Perform generic, architecture and platform specific handling */
        rc = psci_call_suspend_handlers(mpidr_nodes,
                                        start_afflvl,
                                        end_afflvl,
-                                       entrypoint,
-                                       context_id,
                                        power_state);
 
        /*

diff --git a/services/std_svc/psci/psci_common.c b/services/std_svc/psci/psci_common.c
index 0a1cdf9e54ec9421cf266884727aaa09a9c0338c..507b56eb5e0a58ca58e77b86d377698fba750fe5 100644 (file)
--- a/services/std_svc/psci/psci_common.c
+++ b/services/std_svc/psci/psci_common.c
@@ -290,15 +290,14 @@ int psci_validate_mpidr(unsigned long mpidr, int level)
 
 /*******************************************************************************
  * This function determines the full entrypoint information for the requested
- * PSCI entrypoint on power on/resume and saves this in the non-secure CPU
- * cpu_context, ready for when the core boots.
+ * PSCI entrypoint on power on/resume and returns it.
  ******************************************************************************/
-int psci_save_ns_entry(uint64_t mpidr,
-                      uint64_t entrypoint, uint64_t context_id,
-                      uint32_t ns_scr_el3, uint32_t ns_sctlr_el1)
+int psci_get_ns_ep_info(entry_point_info_t *ep,
+                      uint64_t entrypoint, uint64_t context_id)
 {
        uint32_t ep_attr, mode, sctlr, daif, ee;
-       entry_point_info_t ep;
+       uint32_t ns_scr_el3 = read_scr_el3();
+       uint32_t ns_sctlr_el1 = read_sctlr_el1();
 
        sctlr = ns_scr_el3 & SCR_HCE_BIT ? read_sctlr_el2() : ns_sctlr_el1;
        ee = 0;
@@ -308,11 +307,11 @@ int psci_save_ns_entry(uint64_t mpidr,
                ep_attr |= EP_EE_BIG;
                ee = 1;
        }
-       SET_PARAM_HEAD(&ep, PARAM_EP, VERSION_1, ep_attr);
+       SET_PARAM_HEAD(ep, PARAM_EP, VERSION_1, ep_attr);
 
-       ep.pc = entrypoint;
-       memset(&ep.args, 0, sizeof(ep.args));
-       ep.args.arg0 = context_id;
+       ep->pc = entrypoint;
+       memset(&ep->args, 0, sizeof(ep->args));
+       ep->args.arg0 = context_id;
 
        /*
         * Figure out whether the cpu enters the non-secure address space
@@ -329,7 +328,7 @@ int psci_save_ns_entry(uint64_t mpidr,
 
                mode = ns_scr_el3 & SCR_HCE_BIT ? MODE_EL2 : MODE_EL1;
 
-               ep.spsr = SPSR_64(mode, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
+               ep->spsr = SPSR_64(mode, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
        } else {
 
                mode = ns_scr_el3 & SCR_HCE_BIT ? MODE32_hyp : MODE32_svc;
@@ -340,12 +339,9 @@ int psci_save_ns_entry(uint64_t mpidr,
                 */
                daif = DAIF_ABT_BIT | DAIF_IRQ_BIT | DAIF_FIQ_BIT;
 
-               ep.spsr = SPSR_MODE32(mode, entrypoint & 0x1, ee, daif);
+               ep->spsr = SPSR_MODE32(mode, entrypoint & 0x1, ee, daif);
        }
 
-       /* initialise an entrypoint to set up the CPU context */
-       cm_init_context(mpidr, &ep);
-
        return PSCI_E_SUCCESS;
 }
 

diff --git a/services/std_svc/psci/psci_main.c b/services/std_svc/psci/psci_main.c
index 2e700e8ae11d6a52b083ff75bafd4057a103fae3..7fce5faf06b91ec699882957084ecf0b77f3c9ce 100644 (file)
--- a/services/std_svc/psci/psci_main.c
+++ b/services/std_svc/psci/psci_main.c
@@ -45,6 +45,7 @@ int psci_cpu_on(unsigned long target_cpu,
 {
        int rc;
        unsigned int start_afflvl, end_afflvl;
+       entry_point_info_t ep;
 
        /* Determine if the cpu exists of not */
        rc = psci_validate_mpidr(target_cpu, MPIDR_AFFLVL0);
@@ -52,6 +53,16 @@ int psci_cpu_on(unsigned long target_cpu,
                goto exit;
        }
 
+       /*
+        * Verify and derive the re-entry information for
+        * the non-secure world from the non-secure state from
+        * where this call originated.
+        */
+       rc = psci_get_ns_ep_info(&ep, entrypoint, context_id);
+       if (rc != PSCI_E_SUCCESS)
+               return rc;
+
+
        /*
         * To turn this cpu on, specify which affinity
         * levels need to be turned on
@@ -59,8 +70,7 @@ int psci_cpu_on(unsigned long target_cpu,
        start_afflvl = MPIDR_AFFLVL0;
        end_afflvl = get_max_afflvl();
        rc = psci_afflvl_on(target_cpu,
-                           entrypoint,
-                           context_id,
+                           &ep,
                            start_afflvl,
                            end_afflvl);
 
@@ -79,6 +89,7 @@ int psci_cpu_suspend(unsigned int power_state,
 {
        int rc;
        unsigned int target_afflvl, pstate_type;
+       entry_point_info_t ep;
 
        /* Check SBZ bits in power state are zero */
        if (psci_validate_power_state(power_state))
@@ -105,13 +116,21 @@ int psci_cpu_suspend(unsigned int power_state,
                return rc;
        }
 
+       /*
+        * Verify and derive the re-entry information for
+        * the non-secure world from the non-secure state from
+        * where this call originated.
+        */
+       rc = psci_get_ns_ep_info(&ep, entrypoint, context_id);
+       if (rc != PSCI_E_SUCCESS)
+               return rc;
+
        /*
         * Do what is needed to enter the power down state. Upon success,
         * enter the final wfi which will power down this cpu else return
         * an error.
         */
-       rc = psci_afflvl_suspend(entrypoint,
-                                context_id,
+       rc = psci_afflvl_suspend(&ep,
                                 power_state,
                                 MPIDR_AFFLVL0,
                                 target_afflvl);

diff --git a/services/std_svc/psci/psci_private.h b/services/std_svc/psci/psci_private.h
index 9a8ef73de4c12750156660247d09c179bce38586..3a4ee3c8d8acb71f9b215f91014ab37701bbddab 100644 (file)
--- a/services/std_svc/psci/psci_private.h
+++ b/services/std_svc/psci/psci_private.h
@@ -33,6 +33,7 @@
 
 #include <arch.h>
 #include <bakery_lock.h>
+#include <bl_common.h>
 #include <psci.h>
 
 /*
@@ -101,9 +102,8 @@ int get_power_on_target_afflvl(void);
 void psci_afflvl_power_on_finish(int,
                                int,
                                afflvl_power_on_finisher_t *);
-int psci_save_ns_entry(uint64_t mpidr,
-                      uint64_t entrypoint, uint64_t context_id,
-                      uint32_t caller_scr_el3, uint32_t caller_sctlr_el1);
+int psci_get_ns_ep_info(entry_point_info_t *ep,
+                      uint64_t entrypoint, uint64_t context_id);
 int psci_check_afflvl_range(int start_afflvl, int end_afflvl);
 void psci_do_afflvl_state_mgmt(uint32_t start_afflvl,
                               uint32_t end_afflvl,
@@ -129,21 +129,20 @@ int psci_get_aff_map_nodes(unsigned long mpidr,
 aff_map_node_t *psci_get_aff_map_node(unsigned long, int);
 
 /* Private exported functions from psci_affinity_on.c */
-int psci_afflvl_on(unsigned long,
-                       unsigned long,
-                       unsigned long,
-                       int,
-                       int);
+int psci_afflvl_on(unsigned long target_cpu,
+                  entry_point_info_t *ep,
+                  int start_afflvl,
+                  int end_afflvl);
 
 /* Private exported functions from psci_affinity_off.c */
 int psci_afflvl_off(int, int);
 
 /* Private exported functions from psci_affinity_suspend.c */
-int psci_afflvl_suspend(unsigned long,
-                       unsigned long,
-                       unsigned int,
-                       int,
-                       int);
+int psci_afflvl_suspend(entry_point_info_t *ep,
+                       unsigned int power_state,
+                       int start_afflvl,
+                       int end_afflvl);
+
 unsigned int psci_afflvl_suspend_finish(int, int);
 void psci_set_suspend_power_state(unsigned int power_state);