1 // SPDX-License-Identifier: BSD-2-Clause
2 /*
3 * Copyright (c) 2018, Linaro Limited
4 */
5
6 #include <keep.h>
7 #include <kernel/panic.h>
8 #include <kernel/pm.h>
9 #include <kernel/spinlock.h>
10 #include <malloc.h>
11 #include <mm/core_memprot.h>
12 #include <string.h>
13 #include <types_ext.h>
14
15 #define PM_FLAG_SUSPENDED BIT(0)
16
17 static unsigned int pm_list_lock = SPINLOCK_UNLOCK;
18 static struct pm_callback_handle *pm_cb_ref;
19 static size_t pm_cb_count;
20
21 static const char no_name[] = "no-name";
22 DECLARE_KEEP_PAGER(no_name);
23
verify_cb_args(struct pm_callback_handle * pm_hdl)24 static void verify_cb_args(struct pm_callback_handle *pm_hdl)
25 {
26 if (is_unpaged((void *)(vaddr_t)pm_change_state) &&
27 (!is_unpaged((void *)(vaddr_t)pm_hdl->callback) ||
28 (pm_hdl->handle && !is_unpaged(pm_hdl->handle)))) {
29 EMSG("PM callbacks mandates unpaged arguments: %p %p",
30 (void *)(vaddr_t)pm_hdl->callback, pm_hdl->handle);
31 panic();
32 }
33 }
34
register_pm_cb(struct pm_callback_handle * pm_hdl)35 void register_pm_cb(struct pm_callback_handle *pm_hdl)
36 {
37 struct pm_callback_handle *ref = NULL;
38 const char *name = pm_hdl->name;
39 size_t count = pm_cb_count;
40 uint32_t exceptions = 0;
41
42 verify_cb_args(pm_hdl);
43
44 if (!name)
45 name = no_name;
46
47 if (!is_unpaged((void *)name)) {
48 name = strdup(name);
49 if (!name)
50 panic();
51 }
52
53 exceptions = cpu_spin_lock_xsave(&pm_list_lock);
54
55 ref = realloc(pm_cb_ref, sizeof(*ref) * (count + 1));
56 if (!ref)
57 panic();
58
59 ref[count] = *pm_hdl;
60 ref[count].flags = 0;
61 ref[count].name = name;
62
63 pm_cb_count = count + 1;
64 pm_cb_ref = ref;
65
66 cpu_spin_unlock_xrestore(&pm_list_lock, exceptions);
67 }
68
unregister_pm_cb(struct pm_callback_handle * pm_hdl)69 void unregister_pm_cb(struct pm_callback_handle *pm_hdl)
70 {
71 uint32_t exceptions = 0;
72 size_t n = 0;
73
74 exceptions = cpu_spin_lock_xsave(&pm_list_lock);
75
76 for (n = 0; n < pm_cb_count; n++)
77 if (pm_cb_ref[n].callback == pm_hdl->callback &&
78 pm_cb_ref[n].handle == pm_hdl->handle &&
79 pm_cb_ref[n].order == pm_hdl->order)
80 break;
81
82 if (n < pm_cb_count) {
83 pm_cb_count--;
84 memmove(pm_cb_ref + n, pm_cb_ref + n + 1,
85 (pm_cb_count - n) * sizeof(*pm_cb_ref));
86 }
87
88 cpu_spin_unlock_xrestore(&pm_list_lock, exceptions);
89 }
90
do_pm_callback(enum pm_op op,uint32_t pm_hint,struct pm_callback_handle * hdl)91 static TEE_Result do_pm_callback(enum pm_op op, uint32_t pm_hint,
92 struct pm_callback_handle *hdl)
93 {
94 TEE_Result res = TEE_ERROR_GENERIC;
95 bool suspending = op == PM_OP_SUSPEND;
96
97 if (suspending == (bool)(hdl->flags & PM_FLAG_SUSPENDED))
98 return TEE_SUCCESS;
99
100 DMSG("%s %s", suspending ? "Suspend" : "Resume", hdl->name);
101
102 res = hdl->callback(op, pm_hint, hdl);
103 if (res) {
104 EMSG("%s %s (%p) failed: %#"PRIx32, suspending ? "Suspend" :
105 "Resume", hdl->name, (void *)(vaddr_t)hdl->callback, res);
106 return res;
107 }
108
109 if (suspending)
110 hdl->flags |= PM_FLAG_SUSPENDED;
111 else
112 hdl->flags &= ~PM_FLAG_SUSPENDED;
113
114 return TEE_SUCCESS;
115 }
116
call_callbacks(enum pm_op op,uint32_t pm_hint,enum pm_callback_order order)117 static TEE_Result call_callbacks(enum pm_op op, uint32_t pm_hint,
118 enum pm_callback_order order)
119 {
120 struct pm_callback_handle *hdl = NULL;
121 TEE_Result res = TEE_ERROR_GENERIC;
122 size_t n = 0;
123
124 /*
125 * Suspend first the last registered instances.
126 * Resume first the first registered instances.
127 */
128 if (op == PM_OP_SUSPEND)
129 hdl = pm_cb_ref + pm_cb_count - 1;
130 else
131 hdl = pm_cb_ref;
132
133 for (n = 0; n < pm_cb_count; n++) {
134 if (hdl->order == order) {
135 res = do_pm_callback(op, pm_hint, hdl);
136 if (res)
137 return res;
138 }
139
140 if (op == PM_OP_SUSPEND)
141 hdl--;
142 else
143 hdl++;
144 }
145
146 return TEE_SUCCESS;
147 }
148
pm_change_state(enum pm_op op,uint32_t pm_hint)149 TEE_Result pm_change_state(enum pm_op op, uint32_t pm_hint)
150 {
151 enum pm_callback_order cnt = PM_CB_ORDER_DRIVER;
152 TEE_Result res = TEE_ERROR_GENERIC;
153
154 switch (op) {
155 case PM_OP_SUSPEND:
156 for (cnt = PM_CB_ORDER_DRIVER; cnt < PM_CB_ORDER_MAX; cnt++) {
157 res = call_callbacks(op, pm_hint, cnt);
158 if (res)
159 return res;
160 }
161 break;
162 case PM_OP_RESUME:
163 for (cnt = PM_CB_ORDER_MAX; cnt > PM_CB_ORDER_DRIVER; cnt--) {
164 res = call_callbacks(op, pm_hint, cnt - 1);
165 if (res)
166 return res;
167 }
168 break;
169 default:
170 panic();
171 }
172
173 return TEE_SUCCESS;
174 }
175