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
|
// SPDX-License-Identifier: BSD-2-Clause
/*
* Copyright (c) 2019, Arm Limited
*/
#include <arm.h>
#include <ffa.h>
#include <initcall.h>
#include <keep.h>
#include <kernel/boot.h>
#include <kernel/interrupt.h>
#include <kernel/misc.h>
#include <kernel/panic.h>
#include <mm/core_memprot.h>
#include <mm/core_mmu.h>
#include <platform_config.h>
#include <sm/psci.h>
#include <stdint.h>
#include <string.h>
#include <trace.h>
/*
* Lookup table of core and cluster affinities on the FVP. In the absence of a
* DT that provides the same information, this table is used to initialise
* OP-TEE on secondary cores.
*/
static const uint64_t core_clus_aff_array[] = {
0x0000, /* Cluster 0 Cpu 0 */
0x0001, /* Cluster 0 Cpu 1 */
0x0002, /* Cluster 0 Cpu 2 */
0x0003, /* Cluster 0 Cpu 3 */
0x0100, /* Cluster 1 Cpu 0 */
0x0101, /* Cluster 1 Cpu 1 */
0x0102, /* Cluster 1 Cpu 2 */
0x0103, /* Cluster 1 Cpu 3 */
};
static uint32_t get_cpu_on_fid(void)
{
#ifdef ARM64
return PSCI_CPU_ON_SMC64;
#endif
#ifdef ARM32
return PSCI_CPU_ON;
#endif
}
void ffa_secondary_cpu_boot_req(vaddr_t secondary_ep, uint64_t cookie)
{
unsigned long mpidr = read_mpidr();
unsigned int aff_shift = 0;
unsigned long a1 = 0;
unsigned int cnt = 0;
if (mpidr & MPIDR_MT_MASK)
aff_shift = MPIDR_CLUSTER_SHIFT;
for (cnt = 0; cnt < ARRAY_SIZE(core_clus_aff_array); cnt++) {
int32_t ret = 0;
/* Clear out the affinity fields until level 2 */
a1 = mpidr & ~(unsigned long)MPIDR_AARCH32_AFF_MASK;
/* Create an mpidr from core_clus_aff_array */
a1 |= core_clus_aff_array[cnt] << aff_shift;
/* Ignore current cpu */
if (a1 == mpidr)
continue;
DMSG("PSCI_CPU_ON op on mpidr 0x%lx", a1);
/* Invoke the PSCI_CPU_ON function */
ret = thread_smc(get_cpu_on_fid(), a1, secondary_ep, cookie);
if (ret != PSCI_RET_SUCCESS)
EMSG("PSCI_CPU_ON op on mpidr 0x%lx failed %"PRId32,
a1, ret);
else
DMSG("PSCI_CPU_ON op on mpidr 0x%lx done", a1);
}
}
|
