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