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
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)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
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)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
mt_lpm_dispatcher_registry(unsigned int id,mt_lpm_dispatch_fn fn)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
mt_secure_lpm_dispatcher_registry(unsigned int id,mt_lpm_dispatch_fn fn)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