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 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 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 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 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 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 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