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
|
// SPDX-License-Identifier: BSD-2-Clause
/*
* Copyright (c) 2017, Schneider Electric
* Copyright (c) 2020, Linaro Limited
*/
#include <arm.h>
#include <io.h>
#include <kernel/boot.h>
#include <kernel/misc.h>
#include <mm/core_memprot.h>
#include <platform_config.h>
#include <sm/psci.h>
#include <sm/std_smc.h>
#define SYSCTRL_REG_RSTEN (SYSCTRL_BASE + 0x120)
#define SYSCTRL_REG_RSTCTRL (SYSCTRL_BASE + 0x198)
#define SYSCTRL_BOOTADDR_REG (SYSCTRL_BASE + 0x204)
#define SYSCTRL_REG_RSTEN_MRESET_EN BIT(0)
#define SYSCTRL_REG_RSTEN_SWRST_EN BIT(6)
#define SYSCTRL_REG_RSTCTRL_SWRST_REQ BIT(6)
int psci_features(uint32_t psci_fid)
{
switch (psci_fid) {
case PSCI_PSCI_FEATURES:
case PSCI_VERSION:
case PSCI_CPU_ON:
case PSCI_CPU_OFF:
case PSCI_SYSTEM_RESET:
return PSCI_RET_SUCCESS;
default:
return PSCI_RET_NOT_SUPPORTED;
}
}
uint32_t psci_version(void)
{
return PSCI_VERSION_1_0;
}
int psci_cpu_on(uint32_t core_id, uint32_t entry, uint32_t context_id)
{
vaddr_t sctl_va = core_mmu_get_va(SYSCTRL_BOOTADDR_REG,
MEM_AREA_IO_SEC,
sizeof(uint32_t));
if (core_id == 0 || core_id >= CFG_TEE_CORE_NB_CORE)
return PSCI_RET_INVALID_PARAMETERS;
DMSG("core_id: %" PRIu32, core_id);
boot_set_core_ns_entry(core_id, entry, context_id);
io_write32(sctl_va, TEE_LOAD_ADDR);
dsb();
sev();
return PSCI_RET_SUCCESS;
}
int __noreturn psci_cpu_off(void)
{
DMSG("core_id: %" PRIu32, get_core_pos());
psci_armv7_cpu_off();
thread_mask_exceptions(THREAD_EXCP_ALL);
while (1)
wfi();
}
void psci_system_reset(void)
{
/* Enable software reset */
io_setbits32(core_mmu_get_va(SYSCTRL_REG_RSTEN, MEM_AREA_IO_SEC,
sizeof(uint32_t)),
SYSCTRL_REG_RSTEN_SWRST_EN | SYSCTRL_REG_RSTEN_MRESET_EN);
/* Trigger software reset */
io_setbits32(core_mmu_get_va(SYSCTRL_REG_RSTCTRL, MEM_AREA_IO_SEC,
sizeof(uint32_t)),
SYSCTRL_REG_RSTCTRL_SWRST_REQ);
dsb();
}
|
