[thirdparty/u-boot.git] / arch / arm / mach-stm32mp / psci.c

// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
/*
 * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
 */

#include <config.h>
#include <common.h>
#include <asm/armv7.h>
#include <asm/cache.h>
#include <asm/gic.h>
#include <asm/io.h>
#include <asm/psci.h>
#include <asm/secure.h>

#define BOOT_API_A7_CORE0_MAGIC_NUMBER	0xCA7FACE0
#define BOOT_API_A7_CORE1_MAGIC_NUMBER	0xCA7FACE1

#define MPIDR_AFF0			GENMASK(7, 0)

#define RCC_MP_GRSTCSETR		(STM32_RCC_BASE + 0x0404)
#define RCC_MP_GRSTCSETR_MPUP1RST	BIT(5)
#define RCC_MP_GRSTCSETR_MPUP0RST	BIT(4)
#define RCC_MP_GRSTCSETR_MPSYSRST	BIT(0)

#define STM32MP1_PSCI_NR_CPUS		2
#if STM32MP1_PSCI_NR_CPUS > CONFIG_ARMV7_PSCI_NR_CPUS
#error "invalid value for CONFIG_ARMV7_PSCI_NR_CPUS"
#endif

u8 psci_state[STM32MP1_PSCI_NR_CPUS] __secure_data = {
	 PSCI_AFFINITY_LEVEL_ON,
	 PSCI_AFFINITY_LEVEL_OFF};

static u32 __secure_data cntfrq;

static u32 __secure cp15_read_cntfrq(void)
{
	u32 frq;

	asm volatile("mrc p15, 0, %0, c14, c0, 0" : "=r" (frq));

	return frq;
}

static void __secure cp15_write_cntfrq(u32 frq)
{
	asm volatile ("mcr p15, 0, %0, c14, c0, 0" : : "r" (frq));
}

static inline void psci_set_state(int cpu, u8 state)
{
	psci_state[cpu] = state;
	dsb();
	isb();
}

static u32 __secure stm32mp_get_gicd_base_address(void)
{
	u32 periphbase;

	/* get the GIC base address from the CBAR register */
	asm("mrc p15, 4, %0, c15, c0, 0\n" : "=r" (periphbase));

	return (periphbase & CBAR_MASK) + GIC_DIST_OFFSET;
}

static void __secure stm32mp_raise_sgi0(int cpu)
{
	u32 gic_dist_addr;

	gic_dist_addr = stm32mp_get_gicd_base_address();

	/* ask cpu with SGI0 */
	writel((BIT(cpu) << 16), gic_dist_addr + GICD_SGIR);
}

void __secure psci_arch_cpu_entry(void)
{
	u32 cpu = psci_get_cpu_id();

	psci_set_state(cpu, PSCI_AFFINITY_LEVEL_ON);

	/* write the saved cntfrq */
	cp15_write_cntfrq(cntfrq);

	/* reset magic in TAMP register */
	writel(0xFFFFFFFF, TAMP_BACKUP_MAGIC_NUMBER);
}

s32 __secure psci_features(u32 function_id, u32 psci_fid)
{
	switch (psci_fid) {
	case ARM_PSCI_0_2_FN_PSCI_VERSION:
	case ARM_PSCI_0_2_FN_CPU_OFF:
	case ARM_PSCI_0_2_FN_CPU_ON:
	case ARM_PSCI_0_2_FN_AFFINITY_INFO:
	case ARM_PSCI_0_2_FN_MIGRATE_INFO_TYPE:
	case ARM_PSCI_0_2_FN_SYSTEM_OFF:
	case ARM_PSCI_0_2_FN_SYSTEM_RESET:
		return 0x0;
	}
	return ARM_PSCI_RET_NI;
}

u32 __secure psci_version(void)
{
	return ARM_PSCI_VER_1_0;
}

s32 __secure psci_affinity_info(u32 function_id, u32 target_affinity,
				u32  lowest_affinity_level)
{
	u32 cpu = target_affinity & MPIDR_AFF0;

	if (lowest_affinity_level > 0)
		return ARM_PSCI_RET_INVAL;

	if (target_affinity & ~MPIDR_AFF0)
		return ARM_PSCI_RET_INVAL;

	if (cpu >= STM32MP1_PSCI_NR_CPUS)
		return ARM_PSCI_RET_INVAL;

	return psci_state[cpu];
}

u32 __secure psci_migrate_info_type(void)
{
	/*
	 * in Power_State_Coordination_Interface_PDD_v1_1_DEN0022D.pdf
	 * return 2 = Trusted OS is either not present or does not require
	 * migration, system of this type does not require the caller
	 * to use the MIGRATE function.
	 * MIGRATE function calls return NOT_SUPPORTED.
	 */
	return 2;
}

s32 __secure psci_cpu_on(u32 function_id, u32 target_cpu, u32 pc,
			 u32 context_id)
{
	u32 cpu = target_cpu & MPIDR_AFF0;

	if (target_cpu & ~MPIDR_AFF0)
		return ARM_PSCI_RET_INVAL;

	if (cpu >= STM32MP1_PSCI_NR_CPUS)
		return ARM_PSCI_RET_INVAL;

	if (psci_state[cpu] == PSCI_AFFINITY_LEVEL_ON)
		return ARM_PSCI_RET_ALREADY_ON;

	/* read and save cntfrq of current cpu to write on target cpu  */
	cntfrq = cp15_read_cntfrq();

	/* reset magic in TAMP register */
	if (readl(TAMP_BACKUP_MAGIC_NUMBER))
		writel(0xFFFFFFFF, TAMP_BACKUP_MAGIC_NUMBER);
	/*
	 * ROM code need a first SGI0 after core reset
	 * core is ready when magic is set to 0 in ROM code
	 */
	while (readl(TAMP_BACKUP_MAGIC_NUMBER))
		stm32mp_raise_sgi0(cpu);

	/* store target PC and context id*/
	psci_save(cpu, pc, context_id);

	/* write entrypoint in backup RAM register */
	writel((u32)&psci_cpu_entry, TAMP_BACKUP_BRANCH_ADDRESS);
	psci_set_state(cpu, PSCI_AFFINITY_LEVEL_ON_PENDING);

	/* write magic number in backup register */
	if (cpu == 0x01)
		writel(BOOT_API_A7_CORE1_MAGIC_NUMBER,
		       TAMP_BACKUP_MAGIC_NUMBER);
	else
		writel(BOOT_API_A7_CORE0_MAGIC_NUMBER,
		       TAMP_BACKUP_MAGIC_NUMBER);

	/* Generate an IT to start the core */
	stm32mp_raise_sgi0(cpu);

	return ARM_PSCI_RET_SUCCESS;
}

s32 __secure psci_cpu_off(void)
{
	u32 cpu;

	cpu = psci_get_cpu_id();

	psci_cpu_off_common();
	psci_set_state(cpu, PSCI_AFFINITY_LEVEL_OFF);

	/* reset core: wfi is managed by BootRom */
	if (cpu == 0x01)
		writel(RCC_MP_GRSTCSETR_MPUP1RST, RCC_MP_GRSTCSETR);
	else
		writel(RCC_MP_GRSTCSETR_MPUP0RST, RCC_MP_GRSTCSETR);

	/* just waiting reset */
	while (1)
		wfi();
}

void __secure psci_system_reset(void)
{
	/* System reset */
	writel(RCC_MP_GRSTCSETR_MPSYSRST, RCC_MP_GRSTCSETR);
	/* just waiting reset */
	while (1)
		wfi();
}

void __secure psci_system_off(void)
{
	/* System Off is not managed, waiting user power off
	 * TODO: handle I2C write in PMIC Main Control register bit 0 = SWOFF
	 */
	while (1)
		wfi();
}
Commit	Line	Data
41c79775 PD	1	// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
	2	/*
	3	* Copyright (C) 2018, STMicroelectronics - All Rights Reserved
	4	*/
	5
	6	#include <config.h>
	7	#include <common.h>
	8	#include <asm/armv7.h>
90526e9f	9	#include <asm/cache.h>
41c79775 PD	10	#include <asm/gic.h>
	11	#include <asm/io.h>
	12	#include <asm/psci.h>
	13	#include <asm/secure.h>
	14
	15	#define BOOT_API_A7_CORE0_MAGIC_NUMBER 0xCA7FACE0
	16	#define BOOT_API_A7_CORE1_MAGIC_NUMBER 0xCA7FACE1
	17
	18	#define MPIDR_AFF0 GENMASK(7, 0)
	19
	20	#define RCC_MP_GRSTCSETR (STM32_RCC_BASE + 0x0404)
	21	#define RCC_MP_GRSTCSETR_MPUP1RST BIT(5)
	22	#define RCC_MP_GRSTCSETR_MPUP0RST BIT(4)
	23	#define RCC_MP_GRSTCSETR_MPSYSRST BIT(0)
	24
	25	#define STM32MP1_PSCI_NR_CPUS 2
	26	#if STM32MP1_PSCI_NR_CPUS > CONFIG_ARMV7_PSCI_NR_CPUS
	27	#error "invalid value for CONFIG_ARMV7_PSCI_NR_CPUS"
	28	#endif
	29
	30	u8 psci_state[STM32MP1_PSCI_NR_CPUS] __secure_data = {
	31	PSCI_AFFINITY_LEVEL_ON,
	32	PSCI_AFFINITY_LEVEL_OFF};
	33
40e70ab8 LB	34	static u32 __secure_data cntfrq;
	35
	36	static u32 __secure cp15_read_cntfrq(void)
	37	{
	38	u32 frq;
	39
	40	asm volatile("mrc p15, 0, %0, c14, c0, 0" : "=r" (frq));
	41
	42	return frq;
	43	}
	44
	45	static void __secure cp15_write_cntfrq(u32 frq)
	46	{
	47	asm volatile ("mcr p15, 0, %0, c14, c0, 0" : : "r" (frq));
	48	}
	49
e21e3ffd	50	static inline void psci_set_state(int cpu, u8 state)
41c79775 PD	51	{
	52	psci_state[cpu] = state;
	53	dsb();
	54	isb();
	55	}
	56
	57	static u32 __secure stm32mp_get_gicd_base_address(void)
	58	{
	59	u32 periphbase;
	60
	61	/* get the GIC base address from the CBAR register */
	62	asm("mrc p15, 4, %0, c15, c0, 0\n" : "=r" (periphbase));
	63
	64	return (periphbase & CBAR_MASK) + GIC_DIST_OFFSET;
	65	}
	66
bb7288ef	67	static void __secure stm32mp_raise_sgi0(int cpu)
41c79775 PD	68	{
	69	u32 gic_dist_addr;
	70
	71	gic_dist_addr = stm32mp_get_gicd_base_address();
	72
bb7288ef PD	73	/* ask cpu with SGI0 */
bb7288ef PD	74	writel((BIT(cpu) << 16), gic_dist_addr + GICD_SGIR);
41c79775 PD	75	}
	76
	77	void __secure psci_arch_cpu_entry(void)
	78	{
	79	u32 cpu = psci_get_cpu_id();
	80
	81	psci_set_state(cpu, PSCI_AFFINITY_LEVEL_ON);
bb7288ef	82
40e70ab8 LB	83	/* write the saved cntfrq */
	84	cp15_write_cntfrq(cntfrq);
	85
bb7288ef PD	86	/* reset magic in TAMP register */
bb7288ef PD	87	writel(0xFFFFFFFF, TAMP_BACKUP_MAGIC_NUMBER);
41c79775 PD	88	}
41c79775 PD	89
e21e3ffd	90	s32 __secure psci_features(u32 function_id, u32 psci_fid)
41c79775 PD	91	{
	92	switch (psci_fid) {
	93	case ARM_PSCI_0_2_FN_PSCI_VERSION:
	94	case ARM_PSCI_0_2_FN_CPU_OFF:
	95	case ARM_PSCI_0_2_FN_CPU_ON:
	96	case ARM_PSCI_0_2_FN_AFFINITY_INFO:
	97	case ARM_PSCI_0_2_FN_MIGRATE_INFO_TYPE:
	98	case ARM_PSCI_0_2_FN_SYSTEM_OFF:
	99	case ARM_PSCI_0_2_FN_SYSTEM_RESET:
	100	return 0x0;
	101	}
	102	return ARM_PSCI_RET_NI;
	103	}
	104
e21e3ffd	105	u32 __secure psci_version(void)
41c79775 PD	106	{
	107	return ARM_PSCI_VER_1_0;
	108	}
	109
e21e3ffd	110	s32 __secure psci_affinity_info(u32 function_id, u32 target_affinity,
41c79775 PD	111	u32 lowest_affinity_level)
	112	{
	113	u32 cpu = target_affinity & MPIDR_AFF0;
	114
	115	if (lowest_affinity_level > 0)
	116	return ARM_PSCI_RET_INVAL;
	117
	118	if (target_affinity & ~MPIDR_AFF0)
	119	return ARM_PSCI_RET_INVAL;
	120
	121	if (cpu >= STM32MP1_PSCI_NR_CPUS)
	122	return ARM_PSCI_RET_INVAL;
	123
	124	return psci_state[cpu];
	125	}
	126
e21e3ffd	127	u32 __secure psci_migrate_info_type(void)
41c79775	128	{
b496eec6 PD	129	/*
	130	* in Power_State_Coordination_Interface_PDD_v1_1_DEN0022D.pdf
	131	* return 2 = Trusted OS is either not present or does not require
	132	* migration, system of this type does not require the caller
	133	* to use the MIGRATE function.
	134	* MIGRATE function calls return NOT_SUPPORTED.
	135	*/
41c79775 PD	136	return 2;
	137	}
	138
e21e3ffd	139	s32 __secure psci_cpu_on(u32 function_id, u32 target_cpu, u32 pc,
41c79775 PD	140	u32 context_id)
	141	{
	142	u32 cpu = target_cpu & MPIDR_AFF0;
	143
	144	if (target_cpu & ~MPIDR_AFF0)
	145	return ARM_PSCI_RET_INVAL;
	146
	147	if (cpu >= STM32MP1_PSCI_NR_CPUS)
	148	return ARM_PSCI_RET_INVAL;
	149
	150	if (psci_state[cpu] == PSCI_AFFINITY_LEVEL_ON)
	151	return ARM_PSCI_RET_ALREADY_ON;
	152
40e70ab8 LB	153	/* read and save cntfrq of current cpu to write on target cpu */
	154	cntfrq = cp15_read_cntfrq();
	155
bb7288ef PD	156	/* reset magic in TAMP register */
	157	if (readl(TAMP_BACKUP_MAGIC_NUMBER))
	158	writel(0xFFFFFFFF, TAMP_BACKUP_MAGIC_NUMBER);
	159	/*
	160	* ROM code need a first SGI0 after core reset
	161	* core is ready when magic is set to 0 in ROM code
	162	*/
	163	while (readl(TAMP_BACKUP_MAGIC_NUMBER))
	164	stm32mp_raise_sgi0(cpu);
	165
41c79775 PD	166	/* store target PC and context id*/
	167	psci_save(cpu, pc, context_id);
	168
	169	/* write entrypoint in backup RAM register */
	170	writel((u32)&psci_cpu_entry, TAMP_BACKUP_BRANCH_ADDRESS);
	171	psci_set_state(cpu, PSCI_AFFINITY_LEVEL_ON_PENDING);
	172
	173	/* write magic number in backup register */
	174	if (cpu == 0x01)
	175	writel(BOOT_API_A7_CORE1_MAGIC_NUMBER,
	176	TAMP_BACKUP_MAGIC_NUMBER);
	177	else
	178	writel(BOOT_API_A7_CORE0_MAGIC_NUMBER,
	179	TAMP_BACKUP_MAGIC_NUMBER);
	180
bb7288ef PD	181	/* Generate an IT to start the core */
bb7288ef PD	182	stm32mp_raise_sgi0(cpu);
41c79775 PD	183
	184	return ARM_PSCI_RET_SUCCESS;
	185	}
	186
e21e3ffd	187	s32 __secure psci_cpu_off(void)
41c79775 PD	188	{
	189	u32 cpu;
	190
	191	cpu = psci_get_cpu_id();
	192
	193	psci_cpu_off_common();
	194	psci_set_state(cpu, PSCI_AFFINITY_LEVEL_OFF);
	195
	196	/* reset core: wfi is managed by BootRom */
	197	if (cpu == 0x01)
	198	writel(RCC_MP_GRSTCSETR_MPUP1RST, RCC_MP_GRSTCSETR);
	199	else
	200	writel(RCC_MP_GRSTCSETR_MPUP0RST, RCC_MP_GRSTCSETR);
	201
	202	/* just waiting reset */
	203	while (1)
	204	wfi();
	205	}
	206
e21e3ffd	207	void __secure psci_system_reset(void)
41c79775 PD	208	{
	209	/* System reset */
	210	writel(RCC_MP_GRSTCSETR_MPSYSRST, RCC_MP_GRSTCSETR);
	211	/* just waiting reset */
	212	while (1)
	213	wfi();
	214	}
	215
e21e3ffd	216	void __secure psci_system_off(void)
41c79775 PD	217	{
	218	/* System Off is not managed, waiting user power off
	219	* TODO: handle I2C write in PMIC Main Control register bit 0 = SWOFF
	220	*/
	221	while (1)
	222	wfi();
	223	}