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