3 files changed, 32 insertions, 5 deletions
diff --git a/services/spd/tspd/tspd_main.c b/services/spd/tspd/tspd_main.c
index f4cfa43..ec28773 100644
--- a/services/spd/tspd/tspd_main.c
+++ b/services/spd/tspd/tspd_main.c
@@ -44,7 +44,6 @@
 #include <arch_helpers.h>
 #include <console.h>
 #include <platform.h>
-#include <psci_private.h>
 #include <context_mgmt.h>
 #include <runtime_svc.h>
 #include <bl31.h>
@@ -64,6 +63,10 @@ entry_info *tsp_entry_info;
  ******************************************************************************/
 tsp_context tspd_sp_context[TSPD_CORE_COUNT];
 
+
+int32_t tspd_init(meminfo *bl32_meminfo);
+
+
 /*******************************************************************************
  * Secure Payload Dispatcher setup. The SPD finds out the SP entrypoint and type
  * (aarch32/aarch64) if not already known and initialises the context for entry
@@ -87,6 +90,14 @@ int32_t tspd_setup(void)
 	assert(image_info);
 
 	/*
+	 * If there's no valid entry point for SP, we return a non-zero value
+	 * signalling failure initializing the service. We bail out without
+	 * registering any handlers
+	 */
+	if (!image_info->entrypoint)
+		return 1;
+
+	/*
 	 * We could inspect the SP image and determine it's execution
 	 * state i.e whether AArch32 or AArch64. Assuming it's AArch64
 	 * for the time being.
@@ -97,6 +108,12 @@ int32_t tspd_setup(void)
 				     &tspd_sp_context[linear_id]);
 	assert(rc == 0);
 
+	/*
+	 * All TSPD initialization done. Now register our init function with
+	 * BL31 for deferred invocation
+	 */
+	bl31_register_bl32_init(&tspd_init);
+
 	return rc;
 }
 
@@ -110,7 +127,7 @@ int32_t tspd_setup(void)
  * back to this routine through a SMC. It also passes the extents of memory made
  * available to BL32 by BL31.
  ******************************************************************************/
-int32_t bl32_init(meminfo *bl32_meminfo)
+int32_t tspd_init(meminfo *bl32_meminfo)
 {
 	uint64_t mpidr = read_mpidr();
 	uint32_t linear_id = platform_get_core_pos(mpidr);
@@ -135,12 +152,20 @@ int32_t bl32_init(meminfo *bl32_meminfo)
 	 */
 	rc = tspd_synchronous_sp_entry(tsp_ctx);
 	assert(rc != 0);
-	if (rc)
+	if (rc) {
 		tsp_ctx->state = TSP_STATE_ON;
 
+		/*
+		 * TSP has been successfully initialized. Register power
+		 * managemnt hooks with PSCI
+		 */
+		psci_register_spd_pm_hook(&tspd_pm);
+	}
+
 	return rc;
 }
 
+
 /*******************************************************************************
  * This function is responsible for handling all SMCs in the Trusted OS/App
  * range from the non-secure state as defined in the SMC Calling Convention
diff --git a/services/spd/tspd/tspd_pm.c b/services/spd/tspd/tspd_pm.c
index 9e2f6c2..4ebafc7 100644
--- a/services/spd/tspd/tspd_pm.c
+++ b/services/spd/tspd/tspd_pm.c
@@ -34,7 +34,6 @@
 #include <arch_helpers.h>
 #include <console.h>
 #include <platform.h>
-#include <psci_private.h>
 #include <context_mgmt.h>
 #include <runtime_svc.h>
 #include <bl31.h>
@@ -199,7 +198,7 @@ static int32_t tspd_cpu_migrate_info(uint64_t *resident_cpu)
  * Structure populated by the TSP Dispatcher to be given a chance to perform any
  * TSP bookkeeping before PSCI executes a power mgmt.  operation.
  ******************************************************************************/
-const spd_pm_ops spd_pm = {
+const spd_pm_ops tspd_pm = {
 	tspd_cpu_on_handler,
 	tspd_cpu_off_handler,
 	tspd_cpu_suspend_handler,
diff --git a/services/spd/tspd/tspd_private.h b/services/spd/tspd/tspd_private.h
index 2bc35c1..69cf199 100644
--- a/services/spd/tspd/tspd_private.h
+++ b/services/spd/tspd/tspd_private.h
@@ -116,6 +116,9 @@ typedef struct {
 	cpu_context cpu_ctx;
 } tsp_context;
 
+/* TSPD power management handlers */
+extern const spd_pm_ops tspd_pm;
+
 /*******************************************************************************
  * Function & Data prototypes
  ******************************************************************************/