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
invoke_mt_lpm_dispatch(u_register_t x1,u_register_t x2,u_register_t x3,u_register_t x4,void * handle,struct smccc_res * smccc_ret)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
invoke_mt_secure_lpm_dispatch(u_register_t x1,u_register_t x2,u_register_t x3,u_register_t x4,void * handle,struct smccc_res * smccc_ret)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
mt_lpm_dispatcher_registry(unsigned int id,mt_lpm_dispatch_fn fn)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
mt_secure_lpm_dispatcher_registry(unsigned int id,mt_lpm_dispatch_fn fn)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