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