1 /*
2 * Copyright (c) 2025, Mediatek Inc. All rights reserved.
3 *
4 * SPDX-License-Identifier: BSD-3-Clause
5 */
6
7 #include <lib/libc/errno.h>
8 #include <stdbool.h>
9
10 #include <lib/mmio.h>
11
12 #include <lib/pm/mtk_pm.h>
13 #include <lpm_v2/mt_lpm_smc.h>
14 #include <mt_spm_cond.h>
15 #include <platform_def.h>
16
17 static struct plat_cond_info_t *cond_info;
18
mt_spm_cond_check(const struct mt_spm_cond_tables * src,const struct mt_spm_cond_tables * dest,struct mt_spm_cond_tables * res)19 uint64_t mt_spm_cond_check(const struct mt_spm_cond_tables *src,
20 const struct mt_spm_cond_tables *dest,
21 struct mt_spm_cond_tables *res)
22 {
23 uint64_t b_res = 0;
24 unsigned int i = 0;
25
26 if (!src || !dest)
27 return SPM_COND_CHECK_FAIL;
28
29 if (cond_info && src->table_cg && dest->table_cg) {
30 for (i = 0; i < cond_info->idle_cond_num; i++) {
31 if (res && res->table_cg) {
32 res->table_cg[i] =
33 (src->table_cg[i] & dest->table_cg[i]);
34
35 if (res->table_cg[i])
36 b_res |= (((uint64_t)1) << i);
37
38 } else if (src->table_cg[i] & dest->table_cg[i]) {
39 b_res |= (((uint64_t)1) << i);
40 break;
41 }
42 }
43 }
44
45 if (res) {
46 res->table_pll = (src->table_pll & dest->table_pll);
47
48 if (res->table_pll)
49 b_res |= (res->table_pll << SPM_COND_BLOCKED_PLL_IDX) |
50 SPM_COND_CHECK_BLOCKED_PLL;
51 } else if (src->table_pll & dest->table_pll)
52 b_res |= SPM_COND_CHECK_BLOCKED_PLL;
53
54 return b_res;
55 }
56
mt_spm_dump_all_pll(const struct mt_spm_cond_tables * src,const struct mt_spm_cond_tables * dest,struct mt_spm_cond_tables * res)57 uint64_t mt_spm_dump_all_pll(const struct mt_spm_cond_tables *src,
58 const struct mt_spm_cond_tables *dest,
59 struct mt_spm_cond_tables *res)
60 {
61 uint64_t b_res = 0;
62
63 if (res) {
64 res->table_pll = src->table_pll;
65 if (res->table_pll)
66 b_res |= (res->table_pll << SPM_COND_BLOCKED_PLL_IDX) |
67 SPM_COND_CHECK_BLOCKED_PLL;
68 } else if (src->table_pll & dest->table_pll)
69 b_res |= SPM_COND_CHECK_BLOCKED_PLL;
70
71 return b_res;
72 }
73
mt_spm_cond_update(struct mt_resource_constraint ** con,int num,int stateid,void * priv)74 int mt_spm_cond_update(struct mt_resource_constraint **con, int num,
75 int stateid, void *priv)
76 {
77 int i, res;
78 struct mt_resource_constraint *const *_con;
79 struct plat_idle_cond_info *cg_table;
80 struct plat_pll_cond_info *pll_table;
81 struct mt_spm_cond_tables *spm_cond;
82
83 if (!cond_info)
84 return -ENODEV;
85
86 cg_table = cond_info->idle_cond_table;
87 pll_table = cond_info->pll_cond_table;
88 spm_cond = &cond_info->cond_table_buf;
89
90 if (cg_table && spm_cond->table_cg) {
91 for (i = 0; i < cond_info->idle_cond_num; i++) {
92 spm_cond->table_cg[i] = 0;
93 /* check mtcmos, if off set idle_value and clk to 0 disable */
94 if (cond_info->spm_pwr_status_addr)
95 if (!(mmio_read_32(
96 cond_info->spm_pwr_status_addr) &
97 cg_table[i].pwr_status_mask))
98 continue;
99
100 if (cond_info->spm_pwr_status_2nd_addr)
101 if (!(mmio_read_32(
102 cond_info->spm_pwr_status_2nd_addr) &
103 cg_table[i].pwr_status_mask))
104 continue;
105 /* check clkmux */
106 if (cg_table[i].clkmux_addr)
107 if (mmio_read_32(cg_table[i].clkmux_addr) &
108 cg_table[i].clkmux_mask)
109 continue;
110
111 spm_cond->table_cg[i] =
112 cg_table[i].bBitflip ?
113 ~mmio_read_32(cg_table[i].cg_addr) :
114 mmio_read_32(cg_table[i].cg_addr);
115 }
116 }
117
118 if (pll_table) {
119 spm_cond->table_pll = 0;
120 for (i = 0; pll_table[i].pll_bit_set != PLL_BIT_MAX; i++) {
121 if (mmio_read_32(pll_table[i].pll_addr) &
122 pll_table[i].pll_mask) {
123 spm_cond->table_pll |= pll_table[i].pll_bit_set;
124 }
125 }
126 }
127
128 spm_cond->priv = priv;
129
130 for (i = 0, _con = con; *_con && (*_con)->update && (i < num);
131 _con++, i++) {
132 res = (*_con)->update(stateid, PLAT_RC_UPDATE_CONDITION,
133 (void const *)spm_cond);
134 if (res != MT_RM_STATUS_OK)
135 break;
136 }
137
138 return 0;
139 }
140
register_plat_cond_info(struct plat_cond_info_t * cond)141 int register_plat_cond_info(struct plat_cond_info_t *cond)
142 {
143 if (!cond)
144 return -EINVAL;
145
146 cond_info = cond;
147
148 return 0;
149 }
150