1 /* 2 * This program is free software; you can redistribute it and/or modify 3 * it under the terms of the GNU General Public License version 2 as 4 * published by the Free Software Foundation. 5 * 6 * This program is distributed in the hope that it will be useful, 7 * but WITHOUT ANY WARRANTY; without even the implied warranty of 8 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 9 * GNU General Public License for more details. 10 * 11 * Copyright (C) 2013 ARM Limited 12 * 13 * Author: Will Deacon <will.deacon@arm.com> 14 */ 15 16 #define pr_fmt(fmt) "psci: " fmt 17 18 #include <linux/init.h> 19 #include <linux/of.h> 20 21 #include <asm/compiler.h> 22 #include <asm/errno.h> 23 #include <asm/psci.h> 24 25 struct psci_operations psci_ops; 26 27 static int (*invoke_psci_fn)(u64, u64, u64, u64); 28 29 enum psci_function { 30 PSCI_FN_CPU_SUSPEND, 31 PSCI_FN_CPU_ON, 32 PSCI_FN_CPU_OFF, 33 PSCI_FN_MIGRATE, 34 PSCI_FN_MAX, 35 }; 36 37 static u32 psci_function_id[PSCI_FN_MAX]; 38 39 #define PSCI_RET_SUCCESS 0 40 #define PSCI_RET_EOPNOTSUPP -1 41 #define PSCI_RET_EINVAL -2 42 #define PSCI_RET_EPERM -3 43 44 static int psci_to_linux_errno(int errno) 45 { 46 switch (errno) { 47 case PSCI_RET_SUCCESS: 48 return 0; 49 case PSCI_RET_EOPNOTSUPP: 50 return -EOPNOTSUPP; 51 case PSCI_RET_EINVAL: 52 return -EINVAL; 53 case PSCI_RET_EPERM: 54 return -EPERM; 55 }; 56 57 return -EINVAL; 58 } 59 60 #define PSCI_POWER_STATE_ID_MASK 0xffff 61 #define PSCI_POWER_STATE_ID_SHIFT 0 62 #define PSCI_POWER_STATE_TYPE_MASK 0x1 63 #define PSCI_POWER_STATE_TYPE_SHIFT 16 64 #define PSCI_POWER_STATE_AFFL_MASK 0x3 65 #define PSCI_POWER_STATE_AFFL_SHIFT 24 66 67 static u32 psci_power_state_pack(struct psci_power_state state) 68 { 69 return ((state.id & PSCI_POWER_STATE_ID_MASK) 70 << PSCI_POWER_STATE_ID_SHIFT) | 71 ((state.type & PSCI_POWER_STATE_TYPE_MASK) 72 << PSCI_POWER_STATE_TYPE_SHIFT) | 73 ((state.affinity_level & PSCI_POWER_STATE_AFFL_MASK) 74 << PSCI_POWER_STATE_AFFL_SHIFT); 75 } 76 77 /* 78 * The following two functions are invoked via the invoke_psci_fn pointer 79 * and will not be inlined, allowing us to piggyback on the AAPCS. 80 */ 81 static noinline int __invoke_psci_fn_hvc(u64 function_id, u64 arg0, u64 arg1, 82 u64 arg2) 83 { 84 asm volatile( 85 __asmeq("%0", "x0") 86 __asmeq("%1", "x1") 87 __asmeq("%2", "x2") 88 __asmeq("%3", "x3") 89 "hvc #0\n" 90 : "+r" (function_id) 91 : "r" (arg0), "r" (arg1), "r" (arg2)); 92 93 return function_id; 94 } 95 96 static noinline int __invoke_psci_fn_smc(u64 function_id, u64 arg0, u64 arg1, 97 u64 arg2) 98 { 99 asm volatile( 100 __asmeq("%0", "x0") 101 __asmeq("%1", "x1") 102 __asmeq("%2", "x2") 103 __asmeq("%3", "x3") 104 "smc #0\n" 105 : "+r" (function_id) 106 : "r" (arg0), "r" (arg1), "r" (arg2)); 107 108 return function_id; 109 } 110 111 static int psci_cpu_suspend(struct psci_power_state state, 112 unsigned long entry_point) 113 { 114 int err; 115 u32 fn, power_state; 116 117 fn = psci_function_id[PSCI_FN_CPU_SUSPEND]; 118 power_state = psci_power_state_pack(state); 119 err = invoke_psci_fn(fn, power_state, entry_point, 0); 120 return psci_to_linux_errno(err); 121 } 122 123 static int psci_cpu_off(struct psci_power_state state) 124 { 125 int err; 126 u32 fn, power_state; 127 128 fn = psci_function_id[PSCI_FN_CPU_OFF]; 129 power_state = psci_power_state_pack(state); 130 err = invoke_psci_fn(fn, power_state, 0, 0); 131 return psci_to_linux_errno(err); 132 } 133 134 static int psci_cpu_on(unsigned long cpuid, unsigned long entry_point) 135 { 136 int err; 137 u32 fn; 138 139 fn = psci_function_id[PSCI_FN_CPU_ON]; 140 err = invoke_psci_fn(fn, cpuid, entry_point, 0); 141 return psci_to_linux_errno(err); 142 } 143 144 static int psci_migrate(unsigned long cpuid) 145 { 146 int err; 147 u32 fn; 148 149 fn = psci_function_id[PSCI_FN_MIGRATE]; 150 err = invoke_psci_fn(fn, cpuid, 0, 0); 151 return psci_to_linux_errno(err); 152 } 153 154 static const struct of_device_id psci_of_match[] __initconst = { 155 { .compatible = "arm,psci", }, 156 {}, 157 }; 158 159 int __init psci_init(void) 160 { 161 struct device_node *np; 162 const char *method; 163 u32 id; 164 int err = 0; 165 166 np = of_find_matching_node(NULL, psci_of_match); 167 if (!np) 168 return -ENODEV; 169 170 pr_info("probing function IDs from device-tree\n"); 171 172 if (of_property_read_string(np, "method", &method)) { 173 pr_warning("missing \"method\" property\n"); 174 err = -ENXIO; 175 goto out_put_node; 176 } 177 178 if (!strcmp("hvc", method)) { 179 invoke_psci_fn = __invoke_psci_fn_hvc; 180 } else if (!strcmp("smc", method)) { 181 invoke_psci_fn = __invoke_psci_fn_smc; 182 } else { 183 pr_warning("invalid \"method\" property: %s\n", method); 184 err = -EINVAL; 185 goto out_put_node; 186 } 187 188 if (!of_property_read_u32(np, "cpu_suspend", &id)) { 189 psci_function_id[PSCI_FN_CPU_SUSPEND] = id; 190 psci_ops.cpu_suspend = psci_cpu_suspend; 191 } 192 193 if (!of_property_read_u32(np, "cpu_off", &id)) { 194 psci_function_id[PSCI_FN_CPU_OFF] = id; 195 psci_ops.cpu_off = psci_cpu_off; 196 } 197 198 if (!of_property_read_u32(np, "cpu_on", &id)) { 199 psci_function_id[PSCI_FN_CPU_ON] = id; 200 psci_ops.cpu_on = psci_cpu_on; 201 } 202 203 if (!of_property_read_u32(np, "migrate", &id)) { 204 psci_function_id[PSCI_FN_MIGRATE] = id; 205 psci_ops.migrate = psci_migrate; 206 } 207 208 out_put_node: 209 of_node_put(np); 210 return err; 211 } 212