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_v2/mt_lpm_dispatch.h> 10 11 /* 12 * Notice, this data don't link to bss section. 13 * It means data structure won't be set as zero. 14 * Please make sure the member will be initialized. 15 */ 16 struct mt_dispatch_ctrl mt_dispatcher 17 __section("mt_lpm_s") = { 18 .enable = 0, 19 }; 20 21 struct mt_dispatch_ctrl mt_secure_dispatcher 22 __section("mt_secure_lpm_s") = { 23 .enable = 0, 24 }; 25 26 u_register_t invoke_mt_lpm_dispatch(u_register_t x1, u_register_t x2, 27 u_register_t x3, u_register_t x4, 28 void *handle, struct smccc_res *smccc_ret) 29 { 30 uint64_t res = 0; 31 uint32_t user; 32 33 if (!IS_MT_LPM_SMC(x1)) 34 return res; 35 36 user = MT_LPM_SMC_USER(x1); 37 if ((user < MT_LPM_SMC_USER_MAX) && 38 (mt_dispatcher.enable & (1 << user))) { 39 res = mt_dispatcher.fn[user](MT_LPM_SMC_USER_ID(x1), 40 x2, x3, x4, 41 handle, smccc_ret); 42 } 43 44 return res; 45 } 46 DECLARE_SMC_HANDLER(MTK_SIP_MTK_LPM_CONTROL, invoke_mt_lpm_dispatch); 47 48 u_register_t invoke_mt_secure_lpm_dispatch(u_register_t x1, u_register_t x2, 49 u_register_t x3, u_register_t x4, 50 void *handle, 51 struct smccc_res *smccc_ret) 52 { 53 uint64_t res = 0; 54 unsigned int user; 55 56 if (!IS_MT_LPM_SMC(x1)) 57 return res; 58 59 user = MT_LPM_SMC_USER(x1); 60 if (mt_secure_dispatcher.enable & (1 << user)) { 61 res = mt_secure_dispatcher.fn[user](MT_LPM_SMC_USER_ID(x1), x2, 62 x3, x4, handle, smccc_ret); 63 } 64 65 return res; 66 } 67 DECLARE_SMC_HANDLER(MTK_SIP_BL_LPM_CONTROL, invoke_mt_secure_lpm_dispatch); 68 69 /* Check lpm smc user number at compile time */ 70 CASSERT(MT_LPM_SMC_USER_MAX <= MTK_DISPATCH_ID_MAX, 71 lpm_smc_user_declare_too_large); 72 73 void mt_lpm_dispatcher_registry(unsigned int id, mt_lpm_dispatch_fn fn) 74 { 75 if (id >= MT_LPM_SMC_USER_MAX) 76 return; 77 78 mt_dispatcher.enable |= BIT(id); 79 mt_dispatcher.fn[id] = fn; 80 } 81 82 void mt_secure_lpm_dispatcher_registry(unsigned int id, mt_lpm_dispatch_fn fn) 83 { 84 if (id >= MT_LPM_SMC_USER_MAX) 85 return; 86 87 mt_secure_dispatcher.enable |= BIT(id); 88 mt_secure_dispatcher.fn[id] = fn; 89 } 90