1 // SPDX-License-Identifier: BSD-2-Clause
2 /*
3 * Copyright (C) 2016 Freescale Semiconductor, Inc.
4 * Copyright 2023 NXP
5 *
6 * Peng Fan <peng.fan@nxp.com>
7 */
8
9 #include <drivers/imx_snvs.h>
10 #include <drivers/imx_wdog.h>
11 #include <io.h>
12 #include <imx.h>
13 #include <kernel/boot.h>
14 #include <kernel/misc.h>
15 #include <mm/core_mmu.h>
16 #include <mm/core_memprot.h>
17 #include <stdint.h>
18 #include <sm/psci.h>
19 #include <sm/std_smc.h>
20
21 #include "local.h"
22
23 #define IOMUXC_GPR5_OFFSET 0x14
24 #define ARM_WFI_STAT_MASK(n) BIT(n)
25
psci_features(uint32_t psci_fid)26 int psci_features(uint32_t psci_fid)
27 {
28 switch (psci_fid) {
29 case ARM_SMCCC_VERSION:
30 case PSCI_PSCI_FEATURES:
31 case PSCI_VERSION:
32 case PSCI_CPU_OFF:
33 #ifdef CFG_BOOT_SECONDARY_REQUEST
34 case PSCI_CPU_ON:
35 #endif
36 case PSCI_AFFINITY_INFO:
37 case PSCI_SYSTEM_OFF:
38 case PSCI_SYSTEM_RESET:
39 case PSCI_SYSTEM_RESET2:
40 return PSCI_RET_SUCCESS;
41 default:
42 return PSCI_RET_NOT_SUPPORTED;
43 }
44 }
45
psci_version(void)46 uint32_t psci_version(void)
47 {
48 return PSCI_VERSION_1_0;
49 }
50
51 #ifdef CFG_BOOT_SECONDARY_REQUEST
psci_cpu_on(uint32_t core_idx,uint32_t entry,uint32_t context_id)52 int psci_cpu_on(uint32_t core_idx, uint32_t entry, uint32_t context_id)
53 {
54 if (core_idx == 0 || core_idx >= CFG_TEE_CORE_NB_CORE)
55 return PSCI_RET_INVALID_PARAMETERS;
56
57 /* set secondary cores' NS entry addresses */
58 boot_set_core_ns_entry(core_idx, entry, context_id);
59 imx_set_src_gpr_entry(core_idx, virt_to_phys((void *)TEE_LOAD_ADDR));
60
61 #ifdef CFG_MX7
62 imx_gpcv2_set_core1_pup_by_software();
63 imx_src_release_secondary_core(core_idx);
64 #else
65 imx_src_release_secondary_core(core_idx);
66 imx_set_src_gpr_arg(core_idx, 0);
67 #endif /* CFG_MX7 */
68
69 IMSG("psci on ok");
70
71 return PSCI_RET_SUCCESS;
72 }
73
psci_cpu_off(void)74 int __noreturn psci_cpu_off(void)
75 {
76 uint32_t core_id = get_core_pos();
77
78 IMSG("core_id: %" PRIu32, core_id);
79
80 psci_armv7_cpu_off();
81
82 imx_set_src_gpr_arg(core_id, UINT32_MAX);
83
84 thread_mask_exceptions(THREAD_EXCP_ALL);
85
86 while (true)
87 wfi();
88 }
89
psci_affinity_info(uint32_t affinity,uint32_t lowest_affnity_level __unused)90 int psci_affinity_info(uint32_t affinity,
91 uint32_t lowest_affnity_level __unused)
92 {
93 vaddr_t base = core_mmu_get_va(IOMUXC_BASE, MEM_AREA_IO_SEC,
94 IOMUXC_SIZE);
95 uint32_t cpu = affinity;
96 bool wfi = true;
97
98 if (!soc_is_imx7ds())
99 wfi = io_read32(base + IOMUXC_GPR5_OFFSET) &
100 ARM_WFI_STAT_MASK(cpu);
101
102 if (imx_get_src_gpr_arg(cpu) == 0 || !wfi)
103 return PSCI_AFFINITY_LEVEL_ON;
104
105 DMSG("cpu: %" PRIu32 "GPR: %" PRIx32, cpu, imx_get_src_gpr_arg(cpu));
106
107 while (imx_get_src_gpr_arg(cpu) != UINT_MAX)
108 ;
109
110 imx_src_shutdown_core(cpu);
111 imx_set_src_gpr_arg(cpu, 0);
112
113 return PSCI_AFFINITY_LEVEL_OFF;
114 }
115 #endif
116
psci_system_off(void)117 void __noreturn psci_system_off(void)
118 {
119 #ifndef CFG_MX7ULP
120 imx_snvs_shutdown();
121 #endif
122 dsb();
123
124 while (1)
125 ;
126 }
127
psci_system_reset(void)128 void __noreturn psci_system_reset(void)
129 {
130 imx_wdog_restart(true);
131 }
132
psci_system_reset2(uint32_t reset_type __unused,uint32_t cookie __unused)133 int __noreturn psci_system_reset2(uint32_t reset_type __unused,
134 uint32_t cookie __unused)
135 {
136 /* force WDOG reset */
137 imx_wdog_restart(false);
138 }
139