Branch data Line data Source code
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) 2012 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/opcodes-sec.h>
24 : : #include <asm/opcodes-virt.h>
25 : : #include <asm/psci.h>
26 : :
27 : : struct psci_operations psci_ops;
28 : :
29 : : static int (*invoke_psci_fn)(u32, u32, u32, u32);
30 : :
31 : : enum psci_function {
32 : : PSCI_FN_CPU_SUSPEND,
33 : : PSCI_FN_CPU_ON,
34 : : PSCI_FN_CPU_OFF,
35 : : PSCI_FN_MIGRATE,
36 : : PSCI_FN_MAX,
37 : : };
38 : :
39 : : static u32 psci_function_id[PSCI_FN_MAX];
40 : :
41 : : #define PSCI_RET_SUCCESS 0
42 : : #define PSCI_RET_EOPNOTSUPP -1
43 : : #define PSCI_RET_EINVAL -2
44 : : #define PSCI_RET_EPERM -3
45 : : #define PSCI_RET_EALREADYON -4
46 : :
47 : : static int psci_to_linux_errno(int errno)
48 : : {
49 [ # # ][ # # ]: 0 : switch (errno) {
[ # # ][ # # ]
50 : : case PSCI_RET_SUCCESS:
51 : : return 0;
52 : : case PSCI_RET_EOPNOTSUPP:
53 : : return -EOPNOTSUPP;
54 : : case PSCI_RET_EINVAL:
55 : : return -EINVAL;
56 : : case PSCI_RET_EPERM:
57 : : return -EPERM;
58 : : case PSCI_RET_EALREADYON:
59 : : return -EAGAIN;
60 : : };
61 : :
62 : : return -EINVAL;
63 : : }
64 : :
65 : : #define PSCI_POWER_STATE_ID_MASK 0xffff
66 : : #define PSCI_POWER_STATE_ID_SHIFT 0
67 : : #define PSCI_POWER_STATE_TYPE_MASK 0x1
68 : : #define PSCI_POWER_STATE_TYPE_SHIFT 16
69 : : #define PSCI_POWER_STATE_AFFL_MASK 0x3
70 : : #define PSCI_POWER_STATE_AFFL_SHIFT 24
71 : :
72 : : static u32 psci_power_state_pack(struct psci_power_state state)
73 : : {
74 : 0 : return ((state.id & PSCI_POWER_STATE_ID_MASK)
75 : 0 : << PSCI_POWER_STATE_ID_SHIFT) |
76 : 0 : ((state.type & PSCI_POWER_STATE_TYPE_MASK)
77 : 0 : << PSCI_POWER_STATE_TYPE_SHIFT) |
78 : 0 : ((state.affinity_level & PSCI_POWER_STATE_AFFL_MASK)
79 : 0 : << PSCI_POWER_STATE_AFFL_SHIFT);
80 : : }
81 : :
82 : : /*
83 : : * The following two functions are invoked via the invoke_psci_fn pointer
84 : : * and will not be inlined, allowing us to piggyback on the AAPCS.
85 : : */
86 : 0 : static noinline int __invoke_psci_fn_hvc(u32 function_id, u32 arg0, u32 arg1,
87 : : u32 arg2)
88 : : {
89 : 0 : asm volatile(
90 : : __asmeq("%0", "r0")
91 : : __asmeq("%1", "r1")
92 : : __asmeq("%2", "r2")
93 : : __asmeq("%3", "r3")
94 : : __HVC(0)
95 : : : "+r" (function_id)
96 : : : "r" (arg0), "r" (arg1), "r" (arg2));
97 : :
98 : 0 : return function_id;
99 : : }
100 : :
101 : 0 : static noinline int __invoke_psci_fn_smc(u32 function_id, u32 arg0, u32 arg1,
102 : : u32 arg2)
103 : : {
104 : 0 : asm volatile(
105 : : __asmeq("%0", "r0")
106 : : __asmeq("%1", "r1")
107 : : __asmeq("%2", "r2")
108 : : __asmeq("%3", "r3")
109 : : __SMC(0)
110 : : : "+r" (function_id)
111 : : : "r" (arg0), "r" (arg1), "r" (arg2));
112 : :
113 : 0 : return function_id;
114 : : }
115 : :
116 : 0 : static int psci_cpu_suspend(struct psci_power_state state,
117 : : unsigned long entry_point)
118 : : {
119 : : int err;
120 : : u32 fn, power_state;
121 : :
122 : 0 : fn = psci_function_id[PSCI_FN_CPU_SUSPEND];
123 : : power_state = psci_power_state_pack(state);
124 : 0 : err = invoke_psci_fn(fn, power_state, entry_point, 0);
125 : 0 : return psci_to_linux_errno(err);
126 : : }
127 : :
128 : 0 : static int psci_cpu_off(struct psci_power_state state)
129 : : {
130 : : int err;
131 : : u32 fn, power_state;
132 : :
133 : 0 : fn = psci_function_id[PSCI_FN_CPU_OFF];
134 : : power_state = psci_power_state_pack(state);
135 : 0 : err = invoke_psci_fn(fn, power_state, 0, 0);
136 : 0 : return psci_to_linux_errno(err);
137 : : }
138 : :
139 : 0 : static int psci_cpu_on(unsigned long cpuid, unsigned long entry_point)
140 : : {
141 : : int err;
142 : : u32 fn;
143 : :
144 : 0 : fn = psci_function_id[PSCI_FN_CPU_ON];
145 : 0 : err = invoke_psci_fn(fn, cpuid, entry_point, 0);
146 : 0 : return psci_to_linux_errno(err);
147 : : }
148 : :
149 : 0 : static int psci_migrate(unsigned long cpuid)
150 : : {
151 : : int err;
152 : : u32 fn;
153 : :
154 : 0 : fn = psci_function_id[PSCI_FN_MIGRATE];
155 : 0 : err = invoke_psci_fn(fn, cpuid, 0, 0);
156 : 0 : return psci_to_linux_errno(err);
157 : : }
158 : :
159 : : static const struct of_device_id psci_of_match[] = {
160 : : { .compatible = "arm,psci", },
161 : : {},
162 : : };
163 : :
164 : 0 : void __init psci_init(void)
165 : : {
166 : : struct device_node *np;
167 : : const char *method;
168 : : u32 id;
169 : :
170 : : np = of_find_matching_node(NULL, psci_of_match);
171 [ # # ]: 0 : if (!np)
172 : : return;
173 : :
174 : 0 : pr_info("probing function IDs from device-tree\n");
175 : :
176 [ # # ]: 0 : if (of_property_read_string(np, "method", &method)) {
177 : 0 : pr_warning("missing \"method\" property\n");
178 : 0 : goto out_put_node;
179 : : }
180 : :
181 [ # # ]: 0 : if (!strcmp("hvc", method)) {
182 : 0 : invoke_psci_fn = __invoke_psci_fn_hvc;
183 [ # # ]: 0 : } else if (!strcmp("smc", method)) {
184 : 0 : invoke_psci_fn = __invoke_psci_fn_smc;
185 : : } else {
186 : 0 : pr_warning("invalid \"method\" property: %s\n", method);
187 : 0 : goto out_put_node;
188 : : }
189 : :
190 [ # # ]: 0 : if (!of_property_read_u32(np, "cpu_suspend", &id)) {
191 : 0 : psci_function_id[PSCI_FN_CPU_SUSPEND] = id;
192 : 0 : psci_ops.cpu_suspend = psci_cpu_suspend;
193 : : }
194 : :
195 [ # # ]: 0 : if (!of_property_read_u32(np, "cpu_off", &id)) {
196 : 0 : psci_function_id[PSCI_FN_CPU_OFF] = id;
197 : 0 : psci_ops.cpu_off = psci_cpu_off;
198 : : }
199 : :
200 [ # # ]: 0 : if (!of_property_read_u32(np, "cpu_on", &id)) {
201 : 0 : psci_function_id[PSCI_FN_CPU_ON] = id;
202 : 0 : psci_ops.cpu_on = psci_cpu_on;
203 : : }
204 : :
205 [ # # ]: 0 : if (!of_property_read_u32(np, "migrate", &id)) {
206 : 0 : psci_function_id[PSCI_FN_MIGRATE] = id;
207 : 0 : psci_ops.migrate = psci_migrate;
208 : : }
209 : :
210 : : out_put_node:
211 : : of_node_put(np);
212 : : return;
213 : : }
214 : :
215 : 0 : int psci_probe(void)
216 : : {
217 : : struct device_node *np;
218 : : int ret = -ENODEV;
219 : :
220 : : np = of_find_matching_node(NULL, psci_of_match);
221 [ # # ]: 0 : if (np)
222 : : ret = 0;
223 : :
224 : : of_node_put(np);
225 : 0 : return ret;
226 : : }
|
