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