1 /*
2 * Copyright (c) 2022, Mediatek Inc. All rights reserved.
3 *
4 * SPDX-License-Identifier: BSD-3-Clause
5 */
6
7 #include <assert.h>
8 #include <errno.h>
9
10 #include <common/debug.h>
11 #include <drivers/arm/gicv3.h>
12 #include <lib/psci/psci.h>
13 #include <lib/utils.h>
14 #ifdef MTK_PUBEVENT_ENABLE
15 #include <vendor_pubsub_events.h>
16 #endif
17 #include <plat/arm/common/plat_arm.h>
18 #include <plat/common/platform.h>
19
20 #include <dfd.h>
21 #include <lib/mtk_init/mtk_init.h>
22 #include <lib/pm/mtk_pm.h>
23 #include <mt_gic_v3.h>
24 #include <platform_def.h>
25
26 #define IS_AFFLV_PUBEVENT(_pstate) \
27 ((_pstate & (MT_CPUPM_PWR_DOMAIN_MCUSYS | MT_CPUPM_PWR_DOMAIN_CLUSTER)) != 0)
28
29 #ifdef MTK_PUBEVENT_ENABLE
30 #define MT_CPUPM_EVENT_PWR_ON(x) ({ \
31 PUBLISH_EVENT_ARG(mt_cpupm_publish_pwr_on, (const void *)(x)); })
32
33 #define MT_CPUPM_EVENT_PWR_OFF(x) ({ \
34 PUBLISH_EVENT_ARG(mt_cpupm_publish_pwr_off, (const void *)(x)); })
35
36 #define MT_CPUPM_EVENT_AFFLV_PWR_ON(x) ({ \
37 PUBLISH_EVENT_ARG(mt_cpupm_publish_afflv_pwr_on, (const void *)(x)); })
38
39 #define MT_CPUPM_EVENT_AFFLV_PWR_OFF(x) ({ \
40 PUBLISH_EVENT_ARG(mt_cpupm_publish_afflv_pwr_off, (const void *)(x)); })
41
42 #else
43 #define MT_CPUPM_EVENT_PWR_ON(x) ({ (void)x; })
44 #define MT_CPUPM_EVENT_PWR_OFF(x) ({ (void)x; })
45 #define MT_CPUPM_EVENT_AFFLV_PWR_ON(x) ({ (void)x; })
46 #define MT_CPUPM_EVENT_AFFLV_PWR_OFF(x) ({ (void)x; })
47 #endif
48
49 /*
50 * The cpu require to cluster power stattus
51 * [0] : The cpu require cluster power down
52 * [1] : The cpu require cluster power on
53 */
54 #define coordinate_cluster(onoff) write_clusterpwrdn_el1(onoff)
55 #define coordinate_cluster_pwron() coordinate_cluster(1)
56 #define coordinate_cluster_pwroff() coordinate_cluster(0)
57
58 /* defaultly disable all functions */
59 #define MTK_CPUPM_FN_MASK_DEFAULT (0)
60
61 struct mtk_cpu_pwr_ctrl {
62 unsigned int fn_mask;
63 struct mtk_cpu_pm_ops *ops;
64 struct mtk_cpu_smp_ops *smp;
65 };
66
67 static struct mtk_cpu_pwr_ctrl mtk_cpu_pwr = {
68 .fn_mask = MTK_CPUPM_FN_MASK_DEFAULT,
69 .ops = NULL,
70 };
71
72 #define IS_CPUIDLE_FN_ENABLE(x) ((mtk_cpu_pwr.ops != NULL) && ((mtk_cpu_pwr.fn_mask & x) != 0))
73 #define IS_CPUSMP_FN_ENABLE(x) ((mtk_cpu_pwr.smp != NULL) && ((mtk_cpu_pwr.fn_mask & x) != 0))
74
75 /* per-cpu power state */
76 static unsigned int armv8_2_power_state[PLATFORM_CORE_COUNT];
77
78 #define armv8_2_get_pwr_stateid(cpu) psci_get_pstate_id(armv8_2_power_state[cpu])
79
get_mediatek_pstate(unsigned int domain,unsigned int psci_state,struct mtk_cpupm_pwrstate * state)80 static unsigned int get_mediatek_pstate(unsigned int domain, unsigned int psci_state,
81 struct mtk_cpupm_pwrstate *state)
82 {
83 if (IS_CPUIDLE_FN_ENABLE(MTK_CPUPM_FN_CPUPM_GET_PWR_STATE)) {
84 return mtk_cpu_pwr.ops->get_pstate(domain, psci_state, state);
85 }
86
87 return 0;
88 }
89
armv8_2_get_pwr_afflv(const psci_power_state_t * state_info)90 unsigned int armv8_2_get_pwr_afflv(const psci_power_state_t *state_info)
91 {
92 int i;
93
94 for (i = (int)PLAT_MAX_PWR_LVL; i >= (int)PSCI_CPU_PWR_LVL; i--) {
95 if (is_local_state_run(state_info->pwr_domain_state[i]) == 0) {
96 return (unsigned int) i;
97 }
98 }
99
100 return PSCI_INVALID_PWR_LVL;
101 }
102
103 /* MediaTek mcusys power on control interface */
armv8_2_mcusys_pwr_on_common(const struct mtk_cpupm_pwrstate * state)104 static void armv8_2_mcusys_pwr_on_common(const struct mtk_cpupm_pwrstate *state)
105 {
106 gicv3_distif_init();
107 mt_gic_distif_restore();
108 gic_sgi_restore_all();
109
110 dfd_resume();
111
112 /* Add code here that behavior before system enter mcusys'on */
113 if (IS_CPUIDLE_FN_ENABLE(MTK_CPUPM_FN_RESUME_MCUSYS)) {
114 mtk_cpu_pwr.ops->mcusys_resume(state);
115 }
116 }
117
118 /* MediaTek mcusys power down control interface */
armv8_2_mcusys_pwr_dwn_common(const struct mtk_cpupm_pwrstate * state)119 static void armv8_2_mcusys_pwr_dwn_common(const struct mtk_cpupm_pwrstate *state)
120 {
121 mt_gic_distif_save();
122 gic_sgi_save_all();
123
124 /* Add code here that behaves before entering mcusys off */
125 if (IS_CPUIDLE_FN_ENABLE(MTK_CPUPM_FN_SUSPEND_MCUSYS)) {
126 mtk_cpu_pwr.ops->mcusys_suspend(state);
127 }
128 }
129
130 /* MediaTek Cluster power on control interface */
armv8_2_cluster_pwr_on_common(const struct mtk_cpupm_pwrstate * state)131 static void armv8_2_cluster_pwr_on_common(const struct mtk_cpupm_pwrstate *state)
132 {
133 /* Add code here that behavior before system enter cluster'on */
134 #if defined(MTK_CM_MGR) && !defined(MTK_FPGA_EARLY_PORTING)
135 /* init cpu stall counter */
136 init_cpu_stall_counter_all();
137 #endif
138
139 if (IS_CPUIDLE_FN_ENABLE(MTK_CPUPM_FN_RESUME_CLUSTER)) {
140 mtk_cpu_pwr.ops->cluster_resume(state);
141 }
142 }
143
144 /* MediaTek Cluster power down control interface */
armv8_2_cluster_pwr_dwn_common(const struct mtk_cpupm_pwrstate * state)145 static void armv8_2_cluster_pwr_dwn_common(const struct mtk_cpupm_pwrstate *state)
146 {
147 if (IS_CPUIDLE_FN_ENABLE(MTK_CPUPM_FN_SUSPEND_CLUSTER)) {
148 mtk_cpu_pwr.ops->cluster_suspend(state);
149 }
150 }
151
152 /* MediaTek CPU power on control interface */
armv8_2_cpu_pwr_on_common(const struct mtk_cpupm_pwrstate * state,unsigned int pstate)153 static void armv8_2_cpu_pwr_on_common(const struct mtk_cpupm_pwrstate *state, unsigned int pstate)
154 {
155 coordinate_cluster_pwron();
156
157 gicv3_rdistif_init(plat_my_core_pos());
158 gicv3_cpuif_enable(plat_my_core_pos());
159
160 /* If MCUSYS has been powered down then restore GIC redistributor for all CPUs. */
161 if (IS_PLAT_SYSTEM_RETENTION(state->pwr.afflv)) {
162 mt_gic_rdistif_restore_all();
163 } else {
164 mt_gic_rdistif_restore();
165 }
166 }
167
168 /* MediaTek CPU power down control interface */
armv8_2_cpu_pwr_dwn_common(const struct mtk_cpupm_pwrstate * state,unsigned int pstate)169 static void armv8_2_cpu_pwr_dwn_common(const struct mtk_cpupm_pwrstate *state, unsigned int pstate)
170 {
171 if ((pstate & MT_CPUPM_PWR_DOMAIN_PERCORE_DSU) != 0) {
172 coordinate_cluster_pwroff();
173 }
174
175 mt_gic_rdistif_save();
176 gicv3_cpuif_disable(plat_my_core_pos());
177 gicv3_rdistif_off(plat_my_core_pos());
178 }
179
armv8_2_cpu_pwr_resume(const struct mtk_cpupm_pwrstate * state,unsigned int pstate)180 static void armv8_2_cpu_pwr_resume(const struct mtk_cpupm_pwrstate *state, unsigned int pstate)
181 {
182 armv8_2_cpu_pwr_on_common(state, pstate);
183 if (IS_CPUIDLE_FN_ENABLE(MTK_CPUPM_FN_RESUME_CORE)) {
184 mtk_cpu_pwr.ops->cpu_resume(state);
185 }
186 }
187
armv8_2_cpu_pwr_suspend(const struct mtk_cpupm_pwrstate * state,unsigned int pstate)188 static void armv8_2_cpu_pwr_suspend(const struct mtk_cpupm_pwrstate *state, unsigned int pstate)
189 {
190 if (IS_CPUIDLE_FN_ENABLE(MTK_CPUPM_FN_SUSPEND_CORE)) {
191 mtk_cpu_pwr.ops->cpu_suspend(state);
192 }
193 armv8_2_cpu_pwr_dwn_common(state, pstate);
194 }
195
armv8_2_cpu_pwr_on(const struct mtk_cpupm_pwrstate * state,unsigned int pstate)196 static void armv8_2_cpu_pwr_on(const struct mtk_cpupm_pwrstate *state, unsigned int pstate)
197 {
198 armv8_2_cpu_pwr_on_common(state, pstate);
199
200 if (IS_CPUSMP_FN_ENABLE(MTK_CPUPM_FN_SMP_CORE_ON)) {
201 mtk_cpu_pwr.smp->cpu_on(state);
202 }
203 }
204
armv8_2_cpu_pwr_off(const struct mtk_cpupm_pwrstate * state,unsigned int pstate)205 static void armv8_2_cpu_pwr_off(const struct mtk_cpupm_pwrstate *state, unsigned int pstate)
206 {
207 if (IS_CPUSMP_FN_ENABLE(MTK_CPUPM_FN_SMP_CORE_OFF)) {
208 mtk_cpu_pwr.smp->cpu_off(state);
209 }
210 armv8_2_cpu_pwr_dwn_common(state, pstate);
211 }
212
213 /* MediaTek PSCI power domain */
armv8_2_power_domain_on(u_register_t mpidr)214 static int armv8_2_power_domain_on(u_register_t mpidr)
215 {
216 int ret = PSCI_E_SUCCESS;
217 int cpu = plat_core_pos_by_mpidr(mpidr);
218 uintptr_t entry = plat_pm_get_warm_entry();
219
220 if (IS_CPUSMP_FN_ENABLE(MTK_CPUPM_FN_PWR_ON_CORE_PREPARE)) {
221 if (mtk_cpu_pwr.smp->cpu_pwr_on_prepare(cpu, entry) != 0) {
222 ret = PSCI_E_DENIED;
223 }
224 }
225 INFO("CPU %u power domain prepare on\n", cpu);
226 return ret;
227 }
228
229 /* MediaTek PSCI power domain */
armv8_2_power_domain_on_finish(const psci_power_state_t * state)230 static void armv8_2_power_domain_on_finish(const psci_power_state_t *state)
231 {
232 struct mt_cpupm_event_data nb;
233 unsigned int pstate = (MT_CPUPM_PWR_DOMAIN_CORE | MT_CPUPM_PWR_DOMAIN_PERCORE_DSU);
234 struct mtk_cpupm_pwrstate pm_state = {
235 .info = {
236 .cpuid = plat_my_core_pos(),
237 .mode = MTK_CPU_PM_SMP,
238 },
239 .pwr = {
240 .afflv = armv8_2_get_pwr_afflv(state),
241 .state_id = 0x0,
242 },
243 };
244
245 armv8_2_cpu_pwr_on(&pm_state, pstate);
246
247 nb.cpuid = pm_state.info.cpuid;
248 nb.pwr_domain = pstate;
249 MT_CPUPM_EVENT_PWR_ON(&nb);
250
251 INFO("CPU %u power domain on finished\n", pm_state.info.cpuid);
252 }
253
254 /* MediaTek PSCI power domain */
armv8_2_power_domain_off(const psci_power_state_t * state)255 static void armv8_2_power_domain_off(const psci_power_state_t *state)
256 {
257 struct mt_cpupm_event_data nb;
258 unsigned int pstate = (MT_CPUPM_PWR_DOMAIN_CORE | MT_CPUPM_PWR_DOMAIN_PERCORE_DSU);
259 struct mtk_cpupm_pwrstate pm_state = {
260 .info = {
261 .cpuid = plat_my_core_pos(),
262 .mode = MTK_CPU_PM_SMP,
263 },
264 .pwr = {
265 .afflv = armv8_2_get_pwr_afflv(state),
266 .state_id = 0x0,
267 },
268 };
269 armv8_2_cpu_pwr_off(&pm_state, pstate);
270
271 nb.cpuid = pm_state.info.cpuid;
272 nb.pwr_domain = pstate;
273 MT_CPUPM_EVENT_PWR_OFF(&nb);
274
275 INFO("CPU %u power domain off\n", pm_state.info.cpuid);
276 }
277
278 /* MediaTek PSCI power domain */
armv8_2_power_domain_suspend(const psci_power_state_t * state)279 static void armv8_2_power_domain_suspend(const psci_power_state_t *state)
280 {
281 unsigned int pstate = 0;
282 struct mt_cpupm_event_data nb;
283 struct mtk_cpupm_pwrstate pm_state = {
284 .info = {
285 .cpuid = plat_my_core_pos(),
286 .mode = MTK_CPU_PM_CPUIDLE,
287 },
288 };
289
290 pm_state.pwr.state_id = armv8_2_get_pwr_stateid(pm_state.info.cpuid);
291 pm_state.pwr.afflv = armv8_2_get_pwr_afflv(state);
292 pm_state.pwr.raw = state;
293
294 pstate = get_mediatek_pstate(CPUPM_PWR_OFF,
295 armv8_2_power_state[pm_state.info.cpuid], &pm_state);
296
297 armv8_2_cpu_pwr_suspend(&pm_state, pstate);
298
299 if ((pstate & MT_CPUPM_PWR_DOMAIN_CLUSTER) != 0) {
300 armv8_2_cluster_pwr_dwn_common(&pm_state);
301 }
302
303 if ((pstate & MT_CPUPM_PWR_DOMAIN_MCUSYS) != 0) {
304 armv8_2_mcusys_pwr_dwn_common(&pm_state);
305 }
306
307 nb.cpuid = pm_state.info.cpuid;
308 nb.pwr_domain = pstate;
309 MT_CPUPM_EVENT_PWR_OFF(&nb);
310
311 if (IS_AFFLV_PUBEVENT(pstate)) {
312 MT_CPUPM_EVENT_AFFLV_PWR_OFF(&nb);
313 }
314 }
315
316 /* MediaTek PSCI power domain */
armv8_2_power_domain_suspend_finish(const psci_power_state_t * state)317 static void armv8_2_power_domain_suspend_finish(const psci_power_state_t *state)
318 {
319 unsigned int pstate = 0;
320 struct mt_cpupm_event_data nb;
321 struct mtk_cpupm_pwrstate pm_state = {
322 .info = {
323 .cpuid = plat_my_core_pos(),
324 .mode = MTK_CPU_PM_CPUIDLE,
325 },
326 };
327
328 pm_state.pwr.state_id = armv8_2_get_pwr_stateid(pm_state.info.cpuid);
329 pm_state.pwr.afflv = armv8_2_get_pwr_afflv(state);
330 pm_state.pwr.raw = state;
331
332 pstate = get_mediatek_pstate(CPUPM_PWR_ON,
333 armv8_2_power_state[pm_state.info.cpuid], &pm_state);
334
335 if ((pstate & MT_CPUPM_PWR_DOMAIN_MCUSYS) != 0) {
336 armv8_2_mcusys_pwr_on_common(&pm_state);
337 }
338
339 if ((pstate & MT_CPUPM_PWR_DOMAIN_CLUSTER) != 0) {
340 armv8_2_cluster_pwr_on_common(&pm_state);
341 }
342
343 armv8_2_cpu_pwr_resume(&pm_state, pstate);
344
345 nb.cpuid = pm_state.info.cpuid;
346 nb.pwr_domain = pstate;
347 MT_CPUPM_EVENT_PWR_ON(&nb);
348
349 if (IS_AFFLV_PUBEVENT(pstate)) {
350 MT_CPUPM_EVENT_AFFLV_PWR_ON(&nb);
351 }
352 }
353
354 /* MediaTek PSCI power domain */
armv8_2_validate_power_state(unsigned int power_state,psci_power_state_t * req_state)355 static int armv8_2_validate_power_state(unsigned int power_state, psci_power_state_t *req_state)
356 {
357 unsigned int i;
358 unsigned int pstate = psci_get_pstate_type(power_state);
359 unsigned int aff_lvl = psci_get_pstate_pwrlvl(power_state);
360 unsigned int my_core_pos = plat_my_core_pos();
361
362 if (mtk_cpu_pwr.ops == NULL) {
363 return PSCI_E_INVALID_PARAMS;
364 }
365
366 if (IS_CPUIDLE_FN_ENABLE(MTK_CPUPM_FN_PWR_STATE_VALID)) {
367 if (mtk_cpu_pwr.ops->pwr_state_valid(aff_lvl, pstate) != 0) {
368 return PSCI_E_INVALID_PARAMS;
369 }
370 }
371
372 if (pstate == PSTATE_TYPE_STANDBY) {
373 req_state->pwr_domain_state[0] = PLAT_MAX_RET_STATE;
374 } else {
375 for (i = PSCI_CPU_PWR_LVL; i <= aff_lvl; i++) {
376 req_state->pwr_domain_state[i] = PLAT_MAX_OFF_STATE;
377 }
378 }
379 armv8_2_power_state[my_core_pos] = power_state;
380
381 return PSCI_E_SUCCESS;
382 }
383
384 /* MediaTek PSCI power domain */
385 #if CONFIG_MTK_SUPPORT_SYSTEM_SUSPEND
armv8_2_get_sys_suspend_power_state(psci_power_state_t * req_state)386 static void armv8_2_get_sys_suspend_power_state(psci_power_state_t *req_state)
387 {
388 unsigned int i;
389 int ret;
390 unsigned int power_state;
391 unsigned int my_core_pos = plat_my_core_pos();
392
393 ret = mtk_cpu_pwr.ops->pwr_state_valid(PLAT_MAX_PWR_LVL,
394 PSTATE_TYPE_POWERDOWN);
395
396 if (ret != MTK_CPUPM_E_OK) {
397 /* Avoid suspend due to platform is not ready. */
398 req_state->pwr_domain_state[PSCI_CPU_PWR_LVL] =
399 PLAT_MAX_RET_STATE;
400 for (i = PSCI_CPU_PWR_LVL + 1; i <= PLAT_MAX_PWR_LVL; i++) {
401 req_state->pwr_domain_state[i] = PSCI_LOCAL_STATE_RUN;
402 }
403
404 power_state = psci_make_powerstate(0, PSTATE_TYPE_STANDBY, PSCI_CPU_PWR_LVL);
405 } else {
406 for (i = PSCI_CPU_PWR_LVL; i <= PLAT_MAX_PWR_LVL; i++) {
407 req_state->pwr_domain_state[i] = PLAT_MAX_OFF_STATE;
408 }
409
410 power_state = psci_make_powerstate(MT_PLAT_PWR_STATE_SUSPEND,
411 PSTATE_TYPE_POWERDOWN, PLAT_MAX_PWR_LVL);
412 }
413
414 armv8_2_power_state[my_core_pos] = power_state;
415 flush_dcache_range((uintptr_t)&armv8_2_power_state[my_core_pos],
416 sizeof(armv8_2_power_state[my_core_pos]));
417 }
418 #endif
armv8_2_pm_smp_init(unsigned int cpu_id,uintptr_t entry_point)419 static void armv8_2_pm_smp_init(unsigned int cpu_id, uintptr_t entry_point)
420 {
421 if (entry_point == 0) {
422 ERROR("%s, warm_entry_point is null\n", __func__);
423 panic();
424 }
425 if (IS_CPUSMP_FN_ENABLE(MTK_CPUPM_FN_SMP_INIT)) {
426 mtk_cpu_pwr.smp->init(cpu_id, entry_point);
427 }
428 INFO("[%s:%d] - Initialize finished\n", __func__, __LINE__);
429 }
430
431 static struct plat_pm_pwr_ctrl armv8_2_pwr_ops = {
432 .pwr_domain_suspend = armv8_2_power_domain_suspend,
433 .pwr_domain_suspend_finish = armv8_2_power_domain_suspend_finish,
434 .validate_power_state = armv8_2_validate_power_state,
435 #if CONFIG_MTK_SUPPORT_SYSTEM_SUSPEND
436 .get_sys_suspend_power_state = armv8_2_get_sys_suspend_power_state,
437 #endif
438 };
439
440 struct plat_pm_smp_ctrl armv8_2_smp_ops = {
441 .init = armv8_2_pm_smp_init,
442 .pwr_domain_on = armv8_2_power_domain_on,
443 .pwr_domain_off = armv8_2_power_domain_off,
444 .pwr_domain_on_finish = armv8_2_power_domain_on_finish,
445 };
446
447 #define ISSUE_CPU_PM_REG_FAIL(_success) ({ _success = false; assert(0); })
448
449 #define CPM_PM_FN_CHECK(_fns, _ops, _id, _func, _result, _flag) ({ \
450 if ((_fns & _id)) { \
451 if (_ops->_func) \
452 _flag |= _id; \
453 else { \
454 ISSUE_CPU_PM_REG_FAIL(_result); \
455 } \
456 } })
457
register_cpu_pm_ops(unsigned int fn_flags,struct mtk_cpu_pm_ops * ops)458 int register_cpu_pm_ops(unsigned int fn_flags, struct mtk_cpu_pm_ops *ops)
459 {
460 bool success = true;
461 unsigned int fns = 0;
462
463 if ((ops == NULL) || (mtk_cpu_pwr.ops != NULL)) {
464 ERROR("[%s:%d] register cpu_pm fail !!\n", __FILE__, __LINE__);
465 return MTK_CPUPM_E_ERR;
466 }
467
468 CPM_PM_FN_CHECK(fn_flags, ops, MTK_CPUPM_FN_RESUME_CORE,
469 cpu_resume, success, fns);
470
471 CPM_PM_FN_CHECK(fn_flags, ops, MTK_CPUPM_FN_SUSPEND_CORE,
472 cpu_suspend, success, fns);
473
474 CPM_PM_FN_CHECK(fn_flags, ops, MTK_CPUPM_FN_RESUME_CLUSTER,
475 cluster_resume, success, fns);
476
477 CPM_PM_FN_CHECK(fn_flags, ops, MTK_CPUPM_FN_SUSPEND_CLUSTER,
478 cluster_suspend, success, fns);
479
480 CPM_PM_FN_CHECK(fn_flags, ops, MTK_CPUPM_FN_RESUME_MCUSYS,
481 mcusys_resume, success, fns);
482
483 CPM_PM_FN_CHECK(fn_flags, ops, MTK_CPUPM_FN_SUSPEND_MCUSYS,
484 mcusys_suspend, success, fns);
485
486 CPM_PM_FN_CHECK(fn_flags, ops, MTK_CPUPM_FN_CPUPM_GET_PWR_STATE,
487 get_pstate, success, fns);
488
489 CPM_PM_FN_CHECK(fn_flags, ops, MTK_CPUPM_FN_PWR_STATE_VALID,
490 pwr_state_valid, success, fns);
491
492 CPM_PM_FN_CHECK(fn_flags, ops, MTK_CPUPM_FN_INIT,
493 init, success, fns);
494
495 if (success) {
496 mtk_cpu_pwr.ops = ops;
497 mtk_cpu_pwr.fn_mask |= fns;
498 plat_pm_ops_setup_pwr(&armv8_2_pwr_ops);
499 INFO("[%s:%d] CPU pwr ops register success, support:0x%x\n",
500 __func__, __LINE__, fns);
501 } else {
502 ERROR("[%s:%d] register cpu_pm ops fail !, fn:0x%x\n",
503 __func__, __LINE__, fn_flags);
504 assert(0);
505 }
506 return MTK_CPUPM_E_OK;
507 }
508
register_cpu_smp_ops(unsigned int fn_flags,struct mtk_cpu_smp_ops * ops)509 int register_cpu_smp_ops(unsigned int fn_flags, struct mtk_cpu_smp_ops *ops)
510 {
511 bool success = true;
512 unsigned int fns = 0;
513
514 if ((ops == NULL) || (mtk_cpu_pwr.smp != NULL)) {
515 ERROR("[%s:%d] register cpu_smp fail !!\n", __FILE__, __LINE__);
516 return MTK_CPUPM_E_ERR;
517 }
518
519 CPM_PM_FN_CHECK(fn_flags, ops, MTK_CPUPM_FN_SMP_INIT,
520 init, success, fns);
521
522 CPM_PM_FN_CHECK(fn_flags, ops, MTK_CPUPM_FN_PWR_ON_CORE_PREPARE,
523 cpu_pwr_on_prepare, success, fns);
524
525 CPM_PM_FN_CHECK(fn_flags, ops, MTK_CPUPM_FN_SMP_CORE_ON,
526 cpu_on, success, fns);
527
528 CPM_PM_FN_CHECK(fn_flags, ops, MTK_CPUPM_FN_SMP_CORE_OFF,
529 cpu_off, success, fns);
530
531 if (success == true) {
532 mtk_cpu_pwr.smp = ops;
533 mtk_cpu_pwr.fn_mask |= fns;
534 plat_pm_ops_setup_smp(&armv8_2_smp_ops);
535 INFO("[%s:%d] CPU smp ops register success, support:0x%x\n",
536 __func__, __LINE__, fns);
537 } else {
538 ERROR("[%s:%d] register cpu_smp ops fail !, fn:0x%x\n",
539 __func__, __LINE__, fn_flags);
540 assert(0);
541 }
542 return MTK_CPUPM_E_OK;
543 }
544