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 <mm/core_memprot.h> 10 #include <string.h> 11 #include <types_ext.h> 12 13 #define PM_FLAG_SUSPENDED BIT(0) 14 15 static struct pm_callback_handle *pm_cb_ref; 16 static size_t pm_cb_count; 17 18 static void verify_cb_args(struct pm_callback_handle *pm_hdl) 19 { 20 if (is_unpaged((void *)(vaddr_t)pm_change_state) && 21 (!is_unpaged((void *)(vaddr_t)pm_hdl->callback) || 22 (pm_hdl->handle && !is_unpaged(pm_hdl->handle)))) { 23 EMSG("PM callbacks mandates unpaged arguments: %p %p", 24 (void *)(vaddr_t)pm_hdl->callback, pm_hdl->handle); 25 panic(); 26 } 27 } 28 29 void register_pm_cb(struct pm_callback_handle *pm_hdl) 30 { 31 size_t count = pm_cb_count; 32 struct pm_callback_handle *ref; 33 34 verify_cb_args(pm_hdl); 35 36 ref = realloc(pm_cb_ref, sizeof(*ref) * (count + 1)); 37 if (!ref) 38 panic(); 39 40 ref[count] = *pm_hdl; 41 ref[count].flags = 0; 42 43 pm_cb_count = count + 1; 44 pm_cb_ref = ref; 45 } 46 47 static TEE_Result call_callbacks(enum pm_op op, uint32_t pm_hint, 48 enum pm_callback_order order) 49 { 50 struct pm_callback_handle *hdl; 51 size_t n; 52 TEE_Result res; 53 54 for (n = 0, hdl = pm_cb_ref; n < pm_cb_count; n++, hdl++) { 55 if (hdl->order != order || 56 (hdl->flags & PM_FLAG_SUSPENDED) == (op == PM_OP_SUSPEND)) 57 continue; 58 59 res = hdl->callback(op, pm_hint, hdl); 60 if (res) 61 return res; 62 63 if (op == PM_OP_SUSPEND) 64 hdl->flags |= PM_FLAG_SUSPENDED; 65 else 66 hdl->flags &= ~PM_FLAG_SUSPENDED; 67 } 68 69 return TEE_SUCCESS; 70 } 71 72 TEE_Result pm_change_state(enum pm_op op, uint32_t pm_hint) 73 { 74 enum pm_callback_order cnt; 75 TEE_Result res; 76 77 switch (op) { 78 case PM_OP_SUSPEND: 79 for (cnt = PM_CB_ORDER_DRIVER; cnt < PM_CB_ORDER_MAX; cnt++) { 80 res = call_callbacks(op, pm_hint, cnt); 81 if (res) 82 return res; 83 } 84 break; 85 case PM_OP_RESUME: 86 for (cnt = PM_CB_ORDER_MAX; cnt > PM_CB_ORDER_DRIVER; cnt--) { 87 res = call_callbacks(op, pm_hint, cnt - 1); 88 if (res) 89 return res; 90 } 91 break; 92 default: 93 panic(); 94 } 95 96 return TEE_SUCCESS; 97 } 98