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