arch/arm64/lib/psci.c


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118

/* SPDX-License-Identifier: GPL-2.0+ */
/*
 * psci.c - PSCI 0.2 at EL2, the calls a payload makes to control
 * cores and the system, per DEN 0022. the call arrives as an HVC
 * trap at EL2 (EC 0x16), x0 holds the function id, x1 to x3 the
 * arguments, the return value goes back in x0 and eret resumes the
 * caller at EL1.
 *
 * CPU_ON writes the spin gate of the target core and SEVs, the pen
 * from start.S does the release. CPU_OFF parks the calling core.
 * SYSTEM_OFF and SYSTEM_RESET drive the architecture reset domain.
 *
 * Copyright (C) 2026 Bradley Morgan <brads@mainlining.org>
 */

#include <asm/psci.h>
#include <debug.h>

extern void tb_system_reset(void);
extern void tb_system_off(void);

/* the gates and stamps from start.S, one per possible core */
extern unsigned long tb_spin_gates[8];
extern unsigned char tb_pen_stamps[8];

static uint64_t psci_cpu_on(uint64_t target, uint64_t entry,
			    uint64_t ctx)
{
	unsigned long mpidr;
	int cpu;

	/* affinity 0 only, our gate array indexes cores 0..7 */
	if (target > 7)
		return PSCI_RET_INVALID_PARAMS;

	asm volatile("mrs %0, mpidr_el1" : "=r" (mpidr));
	if ((mpidr & 0xff) == target)
		return PSCI_RET_ALREADY_ON;

	cpu = (int)target;

	/*
	 * the pen saves no context, CPU_ON per DEN 0022 passes an
	 * entry and a context id. the pen enters with x0 = ctx, the
	 * kernel secondary entry takes x0 as its context pointer.
	 * the gate holds the entry, the stamp array the ctx.
	 */
	tb_spin_gates[cpu] = entry;
	tb_pen_stamps[cpu] = (unsigned char)(ctx & 0xff);

	/* make the gate write visible before the wake, D1-2255 */
	asm volatile("dsb sy");
	asm volatile("sev");

	dprintf(ALWAYS, "psci: cpu_on %llu -> %llx\n",
		(unsigned long long)target,
		(unsigned long long)entry);

	return PSCI_RET_SUCCESS;
}

extern void park_ret(void);

static uint64_t psci_cpu_off(void)
{
	unsigned long mpidr;
	int cpu;

	asm volatile("mrs %0, mpidr_el1" : "=r" (mpidr));
	cpu = (int)(mpidr & 0xff);

	if (cpu > 7)
		return PSCI_RET_NOT_SUPPORTED;

	/* clear our own gate and go back to the pen */
	tb_spin_gates[cpu] = 0;

	dprintf(ALWAYS, "psci: cpu_off %d\n", cpu);

	asm volatile(
		"dsb	sy\n"
		"b	park_ret\n"
	);

	return PSCI_RET_INTERNAL_FAIL;	/* not reached */
}

/*
 * the asm vector calls this with the caller x0-x3 still in place,
 * function id in x0, arguments in x1-x3, the return lands in x0.
 */
uint64_t tb_psci_dispatch(uint64_t fn, uint64_t x1, uint64_t x2,
			   uint64_t x3)
{
	switch (fn) {
	case PSCI_FN_VERSION:
		return PSCI_VERSION_0_2;

	case PSCI_FN_CPU_ON:
		return psci_cpu_on(x1, x2, x3);

	case PSCI_FN_CPU_OFF:
		return psci_cpu_off();

	case PSCI_FN_SYSTEM_OFF:
		dprintf(ALWAYS, "psci: system off\n");
			tb_system_off();
		return PSCI_RET_SUCCESS;

	case PSCI_FN_SYSTEM_RESET:
		dprintf(ALWAYS, "psci: system reset\n");
		tb_system_reset();
		return PSCI_RET_INTERNAL_FAIL;	/* not reached */

	default:
		return PSCI_RET_NOT_SUPPORTED;
	}
}