1 /* 2 * Copyright (c) 2025, MediaTek Inc. All rights reserved. 3 * 4 * SPDX-License-Identifier: BSD-3-Clause 5 */ 6 7 #include <cdefs.h> 8 9 #include <lpm/mt_lpm_dispatch.h> 10 11 struct mt_dispatch_ctrl mt_dispatcher __section("mt_lpm_s") = { 12 .enable = 0, 13 }; 14 15 struct mt_dispatch_ctrl mt_secure_dispatcher __section("mt_secure_lpm_s") = { 16 .enable = 0, 17 }; 18 19 u_register_t invoke_mt_lpm_dispatch(u_register_t x1, 20 u_register_t x2, 21 u_register_t x3, 22 u_register_t x4, 23 void *handle, 24 struct smccc_res *smccc_ret) 25 { 26 uint64_t res = 0; 27 uint32_t user; 28 29 if (!IS_MT_LPM_SMC(x1)) 30 return 0; 31 32 user = MT_LPM_SMC_USER(x1); 33 if ((user < MT_LPM_SMC_USER_MAX) && 34 (mt_dispatcher.enable & (BIT(user)))) { 35 res = mt_dispatcher.fn[user](MT_LPM_SMC_USER_ID(x1), 36 x2, 37 x3, 38 x4, 39 handle, 40 smccc_ret); 41 } 42 43 return res; 44 } 45 DECLARE_SMC_HANDLER(MTK_SIP_MTK_LPM_CONTROL, invoke_mt_lpm_dispatch); 46 47 u_register_t invoke_mt_secure_lpm_dispatch(u_register_t x1, 48 u_register_t x2, 49 u_register_t x3, 50 u_register_t x4, 51 void *handle, 52 struct smccc_res *smccc_ret) 53 { 54 uint64_t res = 0; 55 uint32_t user; 56 57 if (!IS_MT_LPM_SMC(x1)) 58 return 0; 59 60 user = MT_LPM_SMC_USER(x1); 61 if (mt_secure_dispatcher.enable & (BIT(user))) { 62 res = mt_secure_dispatcher.fn[user](MT_LPM_SMC_USER_ID(x1), 63 x2, 64 x3, 65 x4, 66 handle, 67 smccc_ret); 68 } 69 70 return res; 71 } 72 DECLARE_SMC_HANDLER(MTK_SIP_BL_LPM_CONTROL, invoke_mt_secure_lpm_dispatch); 73 74 /* Check lpm smc user number at compile time */ 75 CASSERT(MT_LPM_SMC_USER_MAX <= MTK_DISPATCH_ID_MAX, 76 lpm_smc_user_declare_too_large); 77 78 void mt_lpm_dispatcher_registry(unsigned int id, mt_lpm_dispatch_fn fn) 79 { 80 if (id >= MT_LPM_SMC_USER_MAX) 81 return; 82 83 mt_dispatcher.enable |= BIT(id); 84 mt_dispatcher.fn[id] = fn; 85 } 86 87 void mt_secure_lpm_dispatcher_registry(unsigned int id, mt_lpm_dispatch_fn fn) 88 { 89 if (id >= MT_LPM_SMC_USER_MAX) 90 return; 91 92 mt_secure_dispatcher.enable |= BIT(id); 93 mt_secure_dispatcher.fn[id] = fn; 94 } 95