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