PSCI: Introduce new platform and CM helper APIs

This patch introduces new platform APIs and context management helper APIs
to support the new topology framework based on linear core position. This
framework will be introduced in the follwoing patch and it removes the
assumption that the MPIDR based affinity levels map directly to levels
in a power domain tree. The new platforms APIs and context management
helpers based on core position are as described below:

* plat_my_core_pos() and plat_core_pos_by_mpidr()

These 2 new mandatory platform APIs are meant to replace the existing
'platform_get_core_pos()' API. The 'plat_my_core_pos()' API returns the
linear index of the calling core and 'plat_core_pos_by_mpidr()' returns
the linear index of a core specified by its MPIDR. The latter API will also
validate the MPIDR passed as an argument and will return an error code (-1)
if an invalid MPIDR is passed as the argument. This enables the caller to
safely convert an MPIDR of another core to its linear index without querying
the PSCI topology tree e.g. during a call to PSCI CPU_ON.

Since the 'plat_core_pos_by_mpidr()' API verifies an MPIDR, which is always
platform specific, it is no longer possible to maintain a default implementation
of this API. Also it might not be possible for a platform port to verify an
MPIDR before the C runtime has been setup or the topology has been initialized.
This would prevent 'plat_core_pos_by_mpidr()' from being callable prior to
topology setup. As a result, the generic Trusted Firmware code does not call
this API before the topology setup has been done.

The 'plat_my_core_pos' API should be able to run without a C runtime.
Since this API needs to return a core position which is equal to the one
returned by 'plat_core_pos_by_mpidr()' API for the corresponding MPIDR,
this too cannot have default implementation and is a mandatory API for
platform ports. These APIs will be implemented by the ARM reference platform
ports later in the patch stack.

* plat_get_my_stack() and plat_set_my_stack()

These APIs are the stack management APIs which set/return stack addresses
appropriate for the calling core. These replace the 'platform_get_stack()' and
'platform_set_stack()' APIs. A default weak MP version and a global UP version
of these APIs are provided for the platforms.

* Context management helpers based on linear core position

A set of new context management(CM) helpers viz cm_get_context_by_index(),
cm_set_context_by_index(), cm_init_my_context() and cm_init_context_by_index()
are defined which are meant to replace the old helpers which took MPIDR
as argument. The old CM helpers are implemented based on the new helpers to
allow for code consolidation and will be deprecated once the switch to the new
framework is done.

Change-Id: I89758632b370c2812973a4b2efdd9b81a41f9b69

diff --git a/bl31/context_mgmt.c b/bl31/context_mgmt.c
index 6f27176c1de25e4b4b9b71e208edbc3847a7e8c1..a0fd1b6541dd95074060293b78165c74cacaddbd 100644 (file)
--- a/bl31/context_mgmt.c
+++ b/bl31/context_mgmt.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2015, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -64,6 +64,32 @@ void cm_init(void)
         */
 }
 
+/*******************************************************************************
+ * This function returns a pointer to the most recent 'cpu_context' structure
+ * for the CPU identified by `cpu_idx` that was set as the context for the
+ * specified security state. NULL is returned if no such structure has been
+ * specified.
+ ******************************************************************************/
+void *cm_get_context_by_index(unsigned int cpu_idx,
+                               unsigned int security_state)
+{
+       assert(sec_state_is_valid(security_state));
+
+       return get_cpu_data_by_index(cpu_idx, cpu_context[security_state]);
+}
+
+/*******************************************************************************
+ * This function sets the pointer to the current 'cpu_context' structure for the
+ * specified security state for the CPU identified by CPU index.
+ ******************************************************************************/
+void cm_set_context_by_index(unsigned int cpu_idx, void *context,
+                               unsigned int security_state)
+{
+       assert(sec_state_is_valid(security_state));
+
+       set_cpu_data_by_index(cpu_idx, cpu_context[security_state], context);
+}
+
 /*******************************************************************************
  * This function returns a pointer to the most recent 'cpu_context' structure
  * for the CPU identified by MPIDR that was set as the context for the specified
@@ -73,7 +99,7 @@ void *cm_get_context_by_mpidr(uint64_t mpidr, uint32_t security_state)
 {
        assert(sec_state_is_valid(security_state));
 
-       return get_cpu_data_by_mpidr(mpidr, cpu_context[security_state]);
+       return cm_get_context_by_index(platform_get_core_pos(mpidr), security_state);
 }
 
 /*******************************************************************************
@@ -84,7 +110,8 @@ void cm_set_context_by_mpidr(uint64_t mpidr, void *context, uint32_t security_st
 {
        assert(sec_state_is_valid(security_state));
 
-       set_cpu_data_by_mpidr(mpidr, cpu_context[security_state], context);
+       cm_set_context_by_index(platform_get_core_pos(mpidr),
+                                                context, security_state);
 }
 
 /*******************************************************************************
@@ -114,7 +141,7 @@ static inline void cm_set_next_context(void *context)
 }
 
 /*******************************************************************************
- * The following function initializes a cpu_context for the current CPU for
+ * The following function initializes the cpu_context 'ctx' for
  * first use, and sets the initial entrypoint state as specified by the
  * entry_point_info structure.
  *
@@ -123,25 +150,24 @@ static inline void cm_set_next_context(void *context)
  * context and sets this as the next context to return to.
  *
  * The EE and ST attributes are used to configure the endianess and secure
- * timer availability for the new excution context.
+ * timer availability for the new execution context.
  *
  * To prepare the register state for entry call cm_prepare_el3_exit() and
  * el3_exit(). For Secure-EL1 cm_prepare_el3_exit() is equivalent to
  * cm_e1_sysreg_context_restore().
  ******************************************************************************/
-void cm_init_context(uint64_t mpidr, const entry_point_info_t *ep)
+static void cm_init_context_common(cpu_context_t *ctx, const entry_point_info_t *ep)
 {
-       uint32_t security_state;
-       cpu_context_t *ctx;
+       unsigned int security_state;
        uint32_t scr_el3;
        el3_state_t *state;
        gp_regs_t *gp_regs;
        unsigned long sctlr_elx;
 
-       security_state = GET_SECURITY_STATE(ep->h.attr);
-       ctx = cm_get_context_by_mpidr(mpidr, security_state);
        assert(ctx);
 
+       security_state = GET_SECURITY_STATE(ep->h.attr);
+
        /* Clear any residual register values from the context */
        memset(ctx, 0, sizeof(*ctx));
 
@@ -209,6 +235,45 @@ void cm_init_context(uint64_t mpidr, const entry_point_info_t *ep)
        memcpy(gp_regs, (void *)&ep->args, sizeof(aapcs64_params_t));
 }
 
+/*******************************************************************************
+ * The following function initializes the cpu_context for a CPU specified by
+ * its `cpu_idx` for first use, and sets the initial entrypoint state as
+ * specified by the entry_point_info structure.
+ ******************************************************************************/
+void cm_init_context_by_index(unsigned int cpu_idx,
+                             const entry_point_info_t *ep)
+{
+       cpu_context_t *ctx;
+       ctx = cm_get_context_by_index(cpu_idx, GET_SECURITY_STATE(ep->h.attr));
+       cm_init_context_common(ctx, ep);
+}
+
+/*******************************************************************************
+ * The following function initializes the cpu_context for the current CPU
+ * for first use, and sets the initial entrypoint state as specified by the
+ * entry_point_info structure.
+ ******************************************************************************/
+void cm_init_my_context(const entry_point_info_t *ep)
+{
+       cpu_context_t *ctx;
+       ctx = cm_get_context(GET_SECURITY_STATE(ep->h.attr));
+       cm_init_context_common(ctx, ep);
+}
+
+/*******************************************************************************
+ * The following function provides a compatibility function for SPDs using the
+ * existing cm library routines. This function is expected to be invoked for
+ * initializing the cpu_context for the CPU specified by MPIDR for first use.
+ ******************************************************************************/
+void cm_init_context(unsigned long mpidr, const entry_point_info_t *ep)
+{
+       if ((mpidr & MPIDR_AFFINITY_MASK) ==
+                       (read_mpidr_el1() & MPIDR_AFFINITY_MASK))
+               cm_init_my_context(ep);
+       else
+               cm_init_context_by_index(platform_get_core_pos(mpidr), ep);
+}
+
 /*******************************************************************************
  * Prepare the CPU system registers for first entry into secure or normal world
  *

diff --git a/include/bl31/context_mgmt.h b/include/bl31/context_mgmt.h
index 6e82fb70e879cd641b97a1297402e2244be9b848..7e9fe832c43c500e1e54f99eb939a760b2f3b793 100644 (file)
--- a/include/bl31/context_mgmt.h
+++ b/include/bl31/context_mgmt.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2015, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -48,8 +48,16 @@ static inline void *cm_get_context(uint32_t security_state);
 void cm_set_context_by_mpidr(uint64_t mpidr,
                             void *context,
                             uint32_t security_state);
+void *cm_get_context_by_index(unsigned int cpu_idx,
+                             unsigned int security_state);
+void cm_set_context_by_index(unsigned int cpu_idx,
+                            void *context,
+                            unsigned int security_state);
 static inline void cm_set_context(void *context, uint32_t security_state);
 void cm_init_context(uint64_t mpidr, const struct entry_point_info *ep);
+void cm_init_my_context(const struct entry_point_info *ep);
+void cm_init_context_by_index(unsigned int cpu_idx,
+                             const struct entry_point_info *ep);
 void cm_prepare_el3_exit(uint32_t security_state);
 void cm_el1_sysregs_context_save(uint32_t security_state);
 void cm_el1_sysregs_context_restore(uint32_t security_state);

diff --git a/include/common/asm_macros.S b/include/common/asm_macros.S
index 45058a60f30a07af8804e3e273db63a370c5613e..902127ec00e8b8d34dc1b36fec3fd3c3495e03fb 100644 (file)
--- a/include/common/asm_macros.S
+++ b/include/common/asm_macros.S
@@ -130,6 +130,20 @@
        madd x0, x0, x1, x2
        .endm
 
+       /*
+        * This macro calculates the base address of the current CPU's MP stack
+        * using the plat_my_core_pos() index, the name of the stack storage
+        * and the size of each stack
+        * Out: X0 = physical address of stack base
+        * Clobber: X30, X1, X2
+        */
+       .macro get_my_mp_stack _name, _size
+       bl  plat_my_core_pos
+       ldr x2, =(\_name + \_size)
+       mov x1, #\_size
+       madd x0, x0, x1, x2
+       .endm
+
        /*
         * This macro calculates the base address of a UP stack using the
         * name of the stack storage and the size of the stack

diff --git a/include/plat/common/psci1.0/platform.h b/include/plat/common/psci1.0/platform.h
index 50af4f85e6b63c2340569faf81175051b0398171..059320f10aea336ec277eb47eface68899b1ea85 100644 (file)
--- a/include/plat/common/psci1.0/platform.h
+++ b/include/plat/common/psci1.0/platform.h
@@ -59,6 +59,8 @@ int plat_get_image_source(unsigned int image_id,
                        uintptr_t *dev_handle,
                        uintptr_t *image_spec);
 unsigned long plat_get_ns_image_entrypoint(void);
+unsigned int plat_my_core_pos(void);
+int plat_core_pos_by_mpidr(unsigned long mpidr);
 
 /*******************************************************************************
  * Mandatory interrupt management functions
@@ -74,8 +76,7 @@ uint32_t plat_interrupt_type_to_line(uint32_t type,
 /*******************************************************************************
  * Optional common functions (may be overridden)
  ******************************************************************************/
-unsigned int platform_get_core_pos(unsigned long mpidr);
-unsigned long platform_get_stack(unsigned long mpidr);
+unsigned long plat_get_my_stack(void);
 void plat_report_exception(unsigned long);
 int plat_crash_console_init(void);
 int plat_crash_console_putc(int c);

diff --git a/plat/common/aarch64/platform_mp_stack.S b/plat/common/aarch64/platform_mp_stack.S
index 7a98d290dd366cc49be1775e0a43a8d36a279381..0cea9ac35753a069b7c1723e9578cf02324d89f9 100644 (file)
--- a/plat/common/aarch64/platform_mp_stack.S
+++ b/plat/common/aarch64/platform_mp_stack.S
@@ -36,7 +36,8 @@
        .local  platform_normal_stacks
        .weak   platform_set_stack
        .weak   platform_get_stack
-
+       .weak   plat_get_my_stack
+       .weak   plat_set_my_stack
 
        /* -----------------------------------------------------
         * unsigned long platform_get_stack (unsigned long mpidr)
@@ -65,6 +66,33 @@ func platform_set_stack
        ret x9
 endfunc platform_set_stack
 
+       /* -----------------------------------------------------
+        * unsigned long plat_get_my_stack ()
+        *
+        * For the current CPU, this function returns the stack
+        * pointer for a stack allocated in device memory.
+        * -----------------------------------------------------
+        */
+func plat_get_my_stack
+       mov     x10, x30 // lr
+       get_my_mp_stack platform_normal_stacks, PLATFORM_STACK_SIZE
+       ret     x10
+endfunc plat_get_my_stack
+
+       /* -----------------------------------------------------
+        * void plat_set_my_stack ()
+        *
+        * For the current CPU, this function sets the stack
+        * pointer to a stack allocated in normal memory.
+        * -----------------------------------------------------
+        */
+func plat_set_my_stack
+       mov     x9, x30 // lr
+       bl      plat_get_my_stack
+       mov     sp, x0
+       ret     x9
+endfunc plat_set_my_stack
+
        /* -----------------------------------------------------
         * Per-cpu stacks in normal memory. Each cpu gets a
         * stack of PLATFORM_STACK_SIZE bytes.

diff --git a/plat/common/aarch64/platform_up_stack.S b/plat/common/aarch64/platform_up_stack.S
index ea6641a7eaf62beb7b28c2bfda35d8578bf3e3fb..d6d6c6e22368a3aa856ef116a4795fa4053e7b3f 100644 (file)
--- a/plat/common/aarch64/platform_up_stack.S
+++ b/plat/common/aarch64/platform_up_stack.S
@@ -34,10 +34,13 @@
 
 
        .local  platform_normal_stacks
+       .globl  plat_set_my_stack
+       .globl  plat_get_my_stack
        .globl  platform_set_stack
        .globl  platform_get_stack
 
        /* -----------------------------------------------------
+        * unsigned long plat_get_my_stack ()
         * unsigned long platform_get_stack (unsigned long)
         *
         * For cold-boot BL images, only the primary CPU needs a
@@ -45,12 +48,14 @@
         * stack allocated in device memory.
         * -----------------------------------------------------
         */
-func platform_get_stack
+func plat_get_my_stack
+platform_get_stack:
        get_up_stack platform_normal_stacks, PLATFORM_STACK_SIZE
        ret
-endfunc platform_get_stack
+endfunc plat_get_my_stack
 
        /* -----------------------------------------------------
+        * void plat_set_my_stack ()
         * void platform_set_stack (unsigned long)
         *
         * For cold-boot BL images, only the primary CPU needs a
@@ -58,11 +63,12 @@ endfunc platform_get_stack
         * allocated in normal memory.
         * -----------------------------------------------------
         */
-func platform_set_stack
+func plat_set_my_stack
+platform_set_stack:
        get_up_stack platform_normal_stacks, PLATFORM_STACK_SIZE
        mov sp, x0
        ret
-endfunc platform_set_stack
+endfunc plat_set_my_stack
 
        /* -----------------------------------------------------
         * Single cpu stack in normal memory.

diff --git a/services/std_svc/psci1.0/psci_common.c b/services/std_svc/psci1.0/psci_common.c
index 55578379dcf38b59a278656b1e4025fa6ef730d5..d9e9ccee0f7bda53512cebaa51c2111475d6751c 100644 (file)
--- a/services/std_svc/psci1.0/psci_common.c
+++ b/services/std_svc/psci1.0/psci_common.c
@@ -264,18 +264,14 @@ void psci_release_pwr_domain_locks(int start_pwrlvl,
 }
 
 /*******************************************************************************
- * Simple routine to determine whether an power domain instance at a given
- * level in an mpidr exists or not.
+ * Simple routine to determine whether a mpidr is valid or not.
  ******************************************************************************/
-int psci_validate_mpidr(unsigned long mpidr, int level)
+int psci_validate_mpidr(unsigned long mpidr)
 {
-       pwr_map_node_t *node;
-
-       node = psci_get_pwr_map_node(mpidr, level);
-       if (node && (node->state & PSCI_PWR_DOMAIN_PRESENT))
-               return PSCI_E_SUCCESS;
-       else
+       if (plat_core_pos_by_mpidr(mpidr) < 0)
                return PSCI_E_INVALID_PARAMS;
+
+       return PSCI_E_SUCCESS;
 }
 
 /*******************************************************************************

diff --git a/services/std_svc/psci1.0/psci_helpers.S b/services/std_svc/psci1.0/psci_helpers.S
index 07fb889389a3e8600e3631ca451906e643b2bdc9..7ef4cdd64e0736f3e6512d333af4009a3d707f6f 100644 (file)
--- a/services/std_svc/psci1.0/psci_helpers.S
+++ b/services/std_svc/psci1.0/psci_helpers.S
@@ -82,8 +82,7 @@ do_core_pwr_dwn:
         * ---------------------------------------------
         */
 do_stack_maintenance:
-       mrs     x0, mpidr_el1
-       bl      platform_get_stack
+       bl      plat_get_my_stack
 
        /* ---------------------------------------------
         * Calculate and store the size of the used
@@ -136,8 +135,7 @@ func psci_do_pwrup_cache_maintenance
         * stack base address in x0.
         * ---------------------------------------------
         */
-       mrs     x0, mpidr_el1
-       bl      platform_get_stack
+       bl      plat_get_my_stack
        mov     x1, sp
        sub     x1, x0, x1
        mov     x0, sp

diff --git a/services/std_svc/psci1.0/psci_main.c b/services/std_svc/psci1.0/psci_main.c
index 0421b15f8295dd67bae57aaa55504fe1fa897cd3..f87fd52e7ca1c14201c71011987b6eaf0448ae02 100644 (file)
--- a/services/std_svc/psci1.0/psci_main.c
+++ b/services/std_svc/psci1.0/psci_main.c
@@ -50,10 +50,9 @@ int psci_cpu_on(unsigned long target_cpu,
        entry_point_info_t ep;
 
        /* Determine if the cpu exists of not */
-       rc = psci_validate_mpidr(target_cpu, MPIDR_AFFLVL0);
-       if (rc != PSCI_E_SUCCESS) {
+       rc = psci_validate_mpidr(target_cpu);
+       if (rc != PSCI_E_SUCCESS)
                return PSCI_E_INVALID_PARAMS;
-       }
 
        /* Validate the entrypoint using platform pm_ops */
        if (psci_plat_pm_ops->validate_ns_entrypoint) {
@@ -287,7 +286,7 @@ int psci_migrate(unsigned long target_cpu)
                return PSCI_E_NOT_PRESENT;
 
        /* Check the validity of the specified target cpu */
-       rc = psci_validate_mpidr(target_cpu, MPIDR_AFFLVL0);
+       rc = psci_validate_mpidr(target_cpu);
        if (rc != PSCI_E_SUCCESS)
                return PSCI_E_INVALID_PARAMS;
 

diff --git a/services/std_svc/psci1.0/psci_on.c b/services/std_svc/psci1.0/psci_on.c
index 43f58f9b9338a6b2c4f4b681a55c048c870d5fc4..c78e119a6c9d38cd2fd6392d0662be06e3210ced 100644 (file)
--- a/services/std_svc/psci1.0/psci_on.c
+++ b/services/std_svc/psci1.0/psci_on.c
@@ -147,13 +147,14 @@ int psci_cpu_on_start(unsigned long target_cpu,
 
        if (rc == PSCI_E_SUCCESS)
                /* Store the re-entry information for the non-secure world. */
-               cm_init_context(target_cpu, ep);
+               cm_init_context_by_index(target_idx, ep);
        else
                /* Restore the state on error. */
                psci_do_state_coordination(MPIDR_AFFLVL0,
                                          end_pwrlvl,
                                          target_cpu_nodes,
                                          PSCI_STATE_OFF);
+
 exit:
        /*
         * This loop releases the lock corresponding to each power level

diff --git a/services/std_svc/psci1.0/psci_private.h b/services/std_svc/psci1.0/psci_private.h
index e56d848bce51e0e903c6c70434d6d46eaf663695..79909a862996a60230d9402f4dfff764d85b6311 100644 (file)
--- a/services/std_svc/psci1.0/psci_private.h
+++ b/services/std_svc/psci1.0/psci_private.h
@@ -123,7 +123,7 @@ unsigned short psci_get_state(pwr_map_node_t *node);
 unsigned short psci_get_phys_state(pwr_map_node_t *node);
 void psci_set_state(pwr_map_node_t *node, unsigned short state);
 unsigned long mpidr_set_pwr_domain_inst(unsigned long, unsigned char, int);
-int psci_validate_mpidr(unsigned long, int);
+int psci_validate_mpidr(unsigned long mpidr);
 int get_power_on_target_pwrlvl(void);
 void psci_power_up_finish(int end_pwrlvl,
                                 pwrlvl_power_on_finisher_t pon_handler);

diff --git a/services/std_svc/psci1.0/psci_setup.c b/services/std_svc/psci1.0/psci_setup.c
index 002e220e6e5b0b9c7340a1feded6de0cd20f85ff..b1eef69f75d3e11c0b72a078e4d69221b44ae04e 100644 (file)
--- a/services/std_svc/psci1.0/psci_setup.c
+++ b/services/std_svc/psci1.0/psci_setup.c
@@ -193,36 +193,39 @@ static void psci_init_pwr_map_node(unsigned long mpidr,
        state = plat_get_pwr_domain_state(level, mpidr);
        psci_pwr_domain_map[idx].state = state;
 
-       if (level == MPIDR_AFFLVL0) {
+       /*
+        * Check if this is a CPU node and is present in which case certain
+        * other initialisations are required.
+        */
+       if (level != MPIDR_AFFLVL0)
+               return;
 
-               /*
-                * Mark the cpu as OFF. Higher power level reference counts
-                * have already been memset to 0
-                */
-               if (state & PSCI_PWR_DOMAIN_PRESENT)
-                       psci_set_state(&psci_pwr_domain_map[idx],
-                                               PSCI_STATE_OFF);
+       if (!(state & PSCI_PWR_DOMAIN_PRESENT))
+               return;
 
-               /*
-                * Associate a non-secure context with this power
-                * instance through the context management library.
-                */
-               linear_id = platform_get_core_pos(mpidr);
-               assert(linear_id < PLATFORM_CORE_COUNT);
+       /*
+        * Mark the cpu as OFF. Higher power level reference counts
+        * have already been memset to 0
+        */
+       psci_set_state(&psci_pwr_domain_map[idx], PSCI_STATE_OFF);
 
-               /* Invalidate the suspend context for the node */
-               set_cpu_data_by_index(linear_id,
-                                     psci_svc_cpu_data.power_state,
-                                     PSCI_INVALID_DATA);
+       /*
+        * Associate a non-secure context with this power
+        * instance through the context management library.
+        */
+       linear_id = plat_core_pos_by_mpidr(mpidr);
+       assert(linear_id < PLATFORM_CORE_COUNT);
 
-               flush_cpu_data_by_index(linear_id, psci_svc_cpu_data);
+       /* Invalidate the suspend context for the node */
+       set_cpu_data_by_index(linear_id,
+                             psci_svc_cpu_data.power_state,
+                             PSCI_INVALID_DATA);
 
-               cm_set_context_by_mpidr(mpidr,
-                                       (void *) &psci_ns_context[linear_id],
-                                       NON_SECURE);
-       }
+       flush_cpu_data_by_index(linear_id, psci_svc_cpu_data);
 
-       return;
+       cm_set_context_by_index(linear_id,
+                               (void *) &psci_ns_context[linear_id],
+                               NON_SECURE);
 }
 
 /*******************************************************************************

diff --git a/services/std_svc/psci1.0/psci_suspend.c b/services/std_svc/psci1.0/psci_suspend.c
index 7832b82b61fee6c82e7a52510977e0cbbb345b35..3d1bf09e85eb4504f5779d754b09ec49fc8bc36c 100644 (file)
--- a/services/std_svc/psci1.0/psci_suspend.c
+++ b/services/std_svc/psci1.0/psci_suspend.c
@@ -90,7 +90,7 @@ int psci_get_suspend_stateid_by_mpidr(unsigned long mpidr)
 {
        unsigned int power_state;
 
-       power_state = get_cpu_data_by_mpidr(mpidr,
+       power_state = get_cpu_data_by_index(plat_core_pos_by_mpidr(mpidr),
                                            psci_svc_cpu_data.power_state);
 
        return ((power_state == PSCI_INVALID_DATA) ?
@@ -185,7 +185,7 @@ void psci_cpu_suspend_start(entry_point_info_t *ep,
        /*
         * Store the re-entry information for the non-secure world.
         */
-       cm_init_context(read_mpidr_el1(), ep);
+       cm_init_my_context(ep);
 
        /* Set the secure world (EL3) re-entry point after BL1 */
        psci_entrypoint = (unsigned long) psci_cpu_suspend_finish_entry;