PSCI: revert Allwinner changes

In their published ATF tree Allwinner changed some part of the
architectural PSCI code (under the services/std_svc/psci directory).
This is not only the wrong way to do, but also harmful, since they
swapped the order of "cleaning the cache" and the platform specific
"core power down" code.
With the removal of the arisc calling code, we were actually turning off
the cores in that part, so not having the caches cleaned before leads
to random data corruption and most likely crashes.
Revert that unnecessary change to this platform-agnostic code part,
also some other changes which we don't need anymore.
That fixes crashes when offlining cores, but still hangs the firmware
when the cores come back, because some locks are never released.
This will be fixed in the next commits.

Signed-off-by: Andre Przywara <andre.przywara@arm.com>

bl31/aarch64/bl31_arch_setup.c

@@ -33,10 +33,7 @@
 #include <assert.h>
 #include <bl_common.h>
 #include <bl31.h>
-#include <cpu_data.h>
 #include <platform.h>
-#include <debug.h>
-
 
 /*******************************************************************************
  * This duplicates what the primary cpu did after a cold boot in BL1. The same
@@ -50,8 +47,4 @@ void bl31_arch_setup(void)
 
 	/* Program the counter frequency */
 	write_cntfrq_el0(plat_get_syscnt_freq());
-
-	/* Initialize the cpu_ops pointer. */
-	init_cpu_ops();
-
 }

bl31/aarch64/bl31_entrypoint.S

@@ -161,6 +161,11 @@ code_start:
 	ldr	x1, =__COHERENT_RAM_UNALIGNED_SIZE__
 	bl	zeromem16
 
+	/* ---------------------------------------------
+	 * Initialize the cpu_ops pointer.
+	 * ---------------------------------------------
+	 */
+	bl      init_cpu_ops
 
 	/* ---------------------------------------------
 	 * Use SP_EL0 for the C runtime stack.

include/bl31/cpu_data.h

@@ -104,7 +104,6 @@ static inline struct cpu_data *_cpu_data(void)
  *************************************************************************/
 
 void init_cpu_data_ptr(void);
-void init_cpu_ops(void);
 
 #define get_cpu_data(_m)		   _cpu_data()->_m
 #define set_cpu_data(_m, _v)		   _cpu_data()->_m = _v

services/std_svc/psci/psci_afflvl_off.c

@@ -62,32 +62,35 @@ static int psci_afflvl0_off(aff_map_node_t *cpu_node)
 			return rc;
 	}
 
+	/*
+	 * Arch. management. Perform the necessary steps to flush all
+	 * cpu caches.
+	 */
+	psci_do_pwrdown_cache_maintenance(MPIDR_AFFLVL0);
+
 	if (!psci_plat_pm_ops->affinst_off)
 		return PSCI_E_SUCCESS;
 
 	/*
 	 * Plat. management: Perform platform specific actions to turn this
 	 * cpu off e.g. exit cpu coherency, program the power controller etc.
 	 */
-	rc = psci_plat_pm_ops->affinst_off(read_mpidr_el1(),
+	return psci_plat_pm_ops->affinst_off(read_mpidr_el1(),
 					     cpu_node->level,
 					     psci_get_phys_state(cpu_node));
-	/*
-	 * Arch. management. Perform the necessary steps to flush all
-	 * cpu caches.
-	 */
-	psci_do_pwrdown_cache_maintenance(MPIDR_AFFLVL0);
-
-	return rc;
 }
 
 static int psci_afflvl1_off(aff_map_node_t *cluster_node)
 {
-	int rc;
-
 	/* Sanity check the cluster level */
 	assert(cluster_node->level == MPIDR_AFFLVL1);
 
+	/*
+	 * Arch. Management. Flush all levels of caches to PoC if
+	 * the cluster is to be shutdown.
+	 */
+	psci_do_pwrdown_cache_maintenance(MPIDR_AFFLVL1);
+
 	if (!psci_plat_pm_ops->affinst_off)
 		return PSCI_E_SUCCESS;
 
@@ -96,23 +99,13 @@ static int psci_afflvl1_off(aff_map_node_t *cluster_node)
 	 * specific bookeeping e.g. turn off interconnect coherency,
 	 * program the power controller etc.
 	 */
-	rc = psci_plat_pm_ops->affinst_off(read_mpidr_el1(),
+	return psci_plat_pm_ops->affinst_off(read_mpidr_el1(),
 					     cluster_node->level,
 					     psci_get_phys_state(cluster_node));
-
-	/*
-	 * Arch. Management. Flush all levels of caches to PoC if
-	 * the cluster is to be shutdown.
-	 */
-	psci_do_pwrdown_cache_maintenance(MPIDR_AFFLVL1);
-
-	return rc;
 }
 
 static int psci_afflvl2_off(aff_map_node_t *system_node)
 {
-	int rc;
-
 	/* Cannot go beyond this level */
 	assert(system_node->level == MPIDR_AFFLVL2);
 
@@ -121,24 +114,22 @@ static int psci_afflvl2_off(aff_map_node_t *system_node)
 	 * action needs to be taken
 	 */
 
+	/*
+	 * Arch. Management. Flush all levels of caches to PoC if
+	 * the system is to be shutdown.
+	 */
+	psci_do_pwrdown_cache_maintenance(MPIDR_AFFLVL2);
+
 	if (!psci_plat_pm_ops->affinst_off)
 		return PSCI_E_SUCCESS;
 
 	/*
 	 * Plat. Management : Allow the platform to do its bookeeping
 	 * at this affinity level
 	 */
-	rc = psci_plat_pm_ops->affinst_off(read_mpidr_el1(),
+	return psci_plat_pm_ops->affinst_off(read_mpidr_el1(),
 					     system_node->level,
 					     psci_get_phys_state(system_node));
-
-	/*
-	 * Arch. Management. Flush all levels of caches to PoC if
-	 * the system is to be shutdown.
-	 */
-	psci_do_pwrdown_cache_maintenance(MPIDR_AFFLVL2);
-
-	return rc;
 }
 
 static const afflvl_off_handler_t psci_afflvl_off_handlers[] = {

services/std_svc/psci/psci_afflvl_on.c

@@ -38,7 +38,6 @@
 #include <runtime_svc.h>
 #include <stddef.h>
 #include "psci_private.h"
-#include <debug.h>
 
 typedef int (*afflvl_on_handler_t)(unsigned long,
 				 aff_map_node_t *,

services/std_svc/psci/psci_afflvl_suspend.c

@@ -38,7 +38,6 @@
 #include <platform.h>
 #include <runtime_svc.h>
 #include <stddef.h>
-#include <debug.h>
 #include "psci_private.h"
 
 typedef int (*afflvl_suspend_handler_t)(aff_map_node_t *,
@@ -147,6 +146,12 @@ static int psci_afflvl0_suspend(aff_map_node_t *cpu_node,
 	/* Set the secure world (EL3) re-entry point after BL1 */
 	psci_entrypoint = (unsigned long) psci_aff_suspend_finish_entry;
 
+	/*
+	 * Arch. management. Perform the necessary steps to flush all
+	 * cpu caches.
+	 */
+	psci_do_pwrdown_cache_maintenance(MPIDR_AFFLVL0);
+
 	if (!psci_plat_pm_ops->affinst_suspend)
 		return PSCI_E_SUCCESS;
 
@@ -156,19 +161,11 @@ static int psci_afflvl0_suspend(aff_map_node_t *cpu_node,
 	 * platform defined mailbox with the psci entrypoint,
 	 * program the power controller etc.
 	 */
-	rc = psci_plat_pm_ops->affinst_suspend(read_mpidr_el1(),
+	return psci_plat_pm_ops->affinst_suspend(read_mpidr_el1(),
 						 psci_entrypoint,
 						 ns_entrypoint,
 						 cpu_node->level,
 						 psci_get_phys_state(cpu_node));
-
-	/*
-	 * Arch. management. Perform the necessary steps to flush all
-	 * cpu caches.
-	 */
-	psci_do_pwrdown_cache_maintenance(MPIDR_AFFLVL0);
-
-	return rc;
 }
 
 static int psci_afflvl1_suspend(aff_map_node_t *cluster_node,
@@ -178,11 +175,16 @@ static int psci_afflvl1_suspend(aff_map_node_t *cluster_node,
 {
 	unsigned int plat_state;
 	unsigned long psci_entrypoint;
-	int rc;
 
 	/* Sanity check the cluster level */
 	assert(cluster_node->level == MPIDR_AFFLVL1);
 
+	/*
+	 * Arch. management: Flush all levels of caches to PoC if the
+	 * cluster is to be shutdown.
+	 */
+	psci_do_pwrdown_cache_maintenance(MPIDR_AFFLVL1);
+
 	if (!psci_plat_pm_ops->affinst_suspend)
 		return PSCI_E_SUCCESS;
 
@@ -196,18 +198,11 @@ static int psci_afflvl1_suspend(aff_map_node_t *cluster_node,
 	 */
 	plat_state = psci_get_phys_state(cluster_node);
 	psci_entrypoint = (unsigned long) psci_aff_suspend_finish_entry;
-	rc = psci_plat_pm_ops->affinst_suspend(read_mpidr_el1(),
+	return psci_plat_pm_ops->affinst_suspend(read_mpidr_el1(),
 						 psci_entrypoint,
 						 ns_entrypoint,
 						 cluster_node->level,
 						 plat_state);
-	/*
-	 * Arch. management: Flush all levels of caches to PoC if the
-	 * cluster is to be shutdown.
-	 */
-	psci_do_pwrdown_cache_maintenance(MPIDR_AFFLVL1);
-
-	return rc;
 }
 
 
@@ -218,7 +213,6 @@ static int psci_afflvl2_suspend(aff_map_node_t *system_node,
 {
 	unsigned int plat_state;
 	unsigned long psci_entrypoint;
-	int rc;
 
 	/* Cannot go beyond this */
 	assert(system_node->level == MPIDR_AFFLVL2);
@@ -229,6 +223,12 @@ static int psci_afflvl2_suspend(aff_map_node_t *system_node,
 	 */
 	plat_state = psci_get_phys_state(system_node);
 
+	/*
+	 * Arch. management: Flush all levels of caches to PoC if the
+	 * system is to be shutdown.
+	 */
+	psci_do_pwrdown_cache_maintenance(MPIDR_AFFLVL2);
+
 	/*
 	 * Plat. Management : Allow the platform to do its bookeeping
 	 * at this affinity level
@@ -244,18 +244,11 @@ static int psci_afflvl2_suspend(aff_map_node_t *system_node,
 	 */
 	plat_state = psci_get_phys_state(system_node);
 	psci_entrypoint = (unsigned long) psci_aff_suspend_finish_entry;
-	rc = psci_plat_pm_ops->affinst_suspend(read_mpidr_el1(),
+	return psci_plat_pm_ops->affinst_suspend(read_mpidr_el1(),
 						 psci_entrypoint,
 						 ns_entrypoint,
 						 system_node->level,
 						 plat_state);
-	/*
-	 * Arch. management: Flush all levels of caches to PoC if the
-	 * system is to be shutdown.
-	 */
-	psci_do_pwrdown_cache_maintenance(MPIDR_AFFLVL2);
-
-	return rc;
 }
 
 static const afflvl_suspend_handler_t psci_afflvl_suspend_handlers[] = {

services/std_svc/psci/psci_common.c

@@ -140,11 +140,7 @@ int get_power_on_target_afflvl()
 	 * Sanity check the state of the cpu. It should be either suspend or "on
 	 * pending"
 	 */
-	do
-	{
-		state = psci_get_state(node); //wait cpu0 set this state --by wangwei
-	}while(state != PSCI_STATE_SUSPEND && state != PSCI_STATE_ON_PENDING);
-
+	state = psci_get_state(node);
 	assert(state == PSCI_STATE_SUSPEND || state == PSCI_STATE_ON_PENDING);
 #endif
 

services/std_svc/psci/psci_entry.S

@@ -70,12 +70,19 @@ psci_aff_common_finish_entry:
 	msr	sctlr_el3, x0
 	isb
 #endif
+
 	/* ---------------------------------------------
 	 * Initialise the pcpu cache pointer for the CPU
 	 * ---------------------------------------------
 	 */
 	bl	init_cpu_data_ptr
 
+	/* ---------------------------------------------
+	 * Initialize the cpu_ops pointer.
+	 * ---------------------------------------------
+	 */
+	bl	init_cpu_ops
+
 	/* ---------------------------------------------
 	 * Set the exception vectors
 	 * ---------------------------------------------

services/std_svc/psci/psci_main.c

@@ -115,11 +115,8 @@ int psci_cpu_suspend(unsigned int power_state,
 				 power_state,
 				 MPIDR_AFFLVL0,
 				 target_afflvl);
-	if (rc == PSCI_E_SUCCESS) {
-		dcsw_op_all(DCCISW);
+	if (rc == PSCI_E_SUCCESS)
 		psci_power_down_wfi();
-	}
-
 	assert(rc == PSCI_E_INVALID_PARAMS);
 	return rc;
 }
@@ -142,11 +139,8 @@ int psci_cpu_off(void)
 	 * successfully completed. Enter a wfi loop which will allow the
 	 * power controller to physically power down this cpu.
 	 */
-	if (rc == PSCI_E_SUCCESS) {
-		dcsw_op_all(DCCISW);
+	if (rc == PSCI_E_SUCCESS)
 		psci_power_down_wfi();
-	}
-
 
 	/*
 	 * The only error cpu_off can return is E_DENIED. So check if that's

services/std_svc/psci/psci_setup.c

@@ -36,7 +36,6 @@
 #include <context_mgmt.h>
 #include <platform.h>
 #include <stddef.h>
-#include <debug.h>
 #include "psci_private.h"
 
 /*******************************************************************************
@@ -195,7 +194,6 @@ static void psci_init_aff_map_node(unsigned long mpidr,
 		 * instance through the context management library.
 		 */
 		linear_id = platform_get_core_pos(mpidr);
-		//NOTICE("linear_id %d, mpidr %d max %d\n",linear_id,mpidr,PLATFORM_CORE_COUNT );
 		assert(linear_id < PLATFORM_CORE_COUNT);
 
 		/* Invalidate the suspend context for the node */
@@ -212,8 +210,6 @@ static void psci_init_aff_map_node(unsigned long mpidr,
 				      psci_svc_cpu_data.max_phys_off_afflvl,
 				      PSCI_INVALID_DATA);
 
-		flush_cpu_data_by_index(linear_id, psci_svc_cpu_data);
-
 		cm_set_context_by_mpidr(mpidr,
 					(void *) &psci_ns_context[linear_id],
 					NON_SECURE);