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 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 46 uint32_t psci_version(void) 47 { 48 return PSCI_VERSION_1_0; 49 } 50 51 #ifdef CFG_BOOT_SECONDARY_REQUEST 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 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 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 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 128 void __noreturn psci_system_reset(void) 129 { 130 imx_wdog_restart(true); 131 } 132 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