1 /*
2 * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
3 *
4 * SPDX-License-Identifier: BSD-3-Clause
5 */
6 #include <console.h>
7 #include <debug.h>
8 #include <libfdt.h>
9 #include <psci.h>
10 #include <string.h>
11 #include "qemu_private.h"
12
append_psci_compatible(void * fdt,int offs,const char * str)13 static int append_psci_compatible(void *fdt, int offs, const char *str)
14 {
15 return fdt_appendprop(fdt, offs, "compatible", str, strlen(str) + 1);
16 }
17
dt_add_psci_node(void * fdt)18 int dt_add_psci_node(void *fdt)
19 {
20 int offs;
21
22 if (fdt_path_offset(fdt, "/psci") >= 0) {
23 WARN("PSCI Device Tree node already exists!\n");
24 return 0;
25 }
26
27 offs = fdt_path_offset(fdt, "/");
28 if (offs < 0)
29 return -1;
30 offs = fdt_add_subnode(fdt, offs, "psci");
31 if (offs < 0)
32 return -1;
33 if (append_psci_compatible(fdt, offs, "arm,psci-1.0"))
34 return -1;
35 if (append_psci_compatible(fdt, offs, "arm,psci-0.2"))
36 return -1;
37 if (append_psci_compatible(fdt, offs, "arm,psci"))
38 return -1;
39 if (fdt_setprop_string(fdt, offs, "method", "smc"))
40 return -1;
41 if (fdt_setprop_u32(fdt, offs, "cpu_suspend", PSCI_CPU_SUSPEND_AARCH64))
42 return -1;
43 if (fdt_setprop_u32(fdt, offs, "cpu_off", PSCI_CPU_OFF))
44 return -1;
45 if (fdt_setprop_u32(fdt, offs, "cpu_on", PSCI_CPU_ON_AARCH64))
46 return -1;
47 if (fdt_setprop_u32(fdt, offs, "sys_poweroff", PSCI_SYSTEM_OFF))
48 return -1;
49 if (fdt_setprop_u32(fdt, offs, "sys_reset", PSCI_SYSTEM_RESET))
50 return -1;
51 return 0;
52 }
53
check_node_compat_prefix(void * fdt,int offs,const char * prefix)54 static int check_node_compat_prefix(void *fdt, int offs, const char *prefix)
55 {
56 const size_t prefix_len = strlen(prefix);
57 size_t l;
58 int plen;
59 const char *prop;
60
61 prop = fdt_getprop(fdt, offs, "compatible", &plen);
62 if (!prop)
63 return -1;
64
65 while (plen > 0) {
66 if (memcmp(prop, prefix, prefix_len) == 0)
67 return 0; /* match */
68
69 l = strlen(prop) + 1;
70 prop += l;
71 plen -= l;
72 }
73
74 return -1;
75 }
76
dt_add_psci_cpu_enable_methods(void * fdt)77 int dt_add_psci_cpu_enable_methods(void *fdt)
78 {
79 int offs = 0;
80
81 while (1) {
82 offs = fdt_next_node(fdt, offs, NULL);
83 if (offs < 0)
84 break;
85 if (fdt_getprop(fdt, offs, "enable-method", NULL))
86 continue; /* already set */
87 if (check_node_compat_prefix(fdt, offs, "arm,cortex-a"))
88 continue; /* no compatible */
89 if (fdt_setprop_string(fdt, offs, "enable-method", "psci"))
90 return -1;
91 /* Need to restart scanning as offsets may have changed */
92 offs = 0;
93 }
94 return 0;
95 }
96