diff --git a/plat/imx/common/imx_sip_handler.c b/plat/imx/common/imx_sip_handler.c index a414a44..22a7f2b 100644 --- a/plat/imx/common/imx_sip_handler.c +++ b/plat/imx/common/imx_sip_handler.c @@ -13,6 +13,12 @@ #include #include +#ifdef PLAT_IMX8QM +const static int ap_cluster_index[PLATFORM_CLUSTER_COUNT] = { + SC_R_A53, SC_R_A72, +}; +#endif + static int imx_srtc_set_time(uint32_t year_mon, unsigned long day_hour, unsigned long min_sec) @@ -42,3 +48,31 @@ SMC_RET1(handle, ret); } + +static void imx_cpufreq_set_target(uint32_t cluster_id, unsigned long freq) +{ + sc_pm_clock_rate_t rate = (sc_pm_clock_rate_t)freq; + +#ifdef PLAT_IMX8QM + sc_pm_set_clock_rate(ipc_handle, ap_cluster_index[cluster_id], SC_PM_CLK_CPU, &rate); +#endif +#ifdef PLAT_IMX8QX + sc_pm_set_clock_rate(ipc_handle, SC_R_A35, SC_PM_CLK_CPU, &rate); +#endif +} + +int imx_cpufreq_handler(uint32_t smc_fid, + u_register_t x1, + u_register_t x2, + u_register_t x3) +{ + switch (x1) { + case IMX_SIP_SET_CPUFREQ: + imx_cpufreq_set_target(x2, x3); + break; + default: + return SMC_UNK; + } + + return 0; +} diff --git a/plat/imx/common/imx_sip_svc.c b/plat/imx/common/imx_sip_svc.c index 23ff0bb..89b9df8 100644 --- a/plat/imx/common/imx_sip_svc.c +++ b/plat/imx/common/imx_sip_svc.c @@ -29,6 +29,9 @@ #if (defined(PLAT_IMX8QM) || defined(PLAT_IMX8QX)) case IMX_SIP_SRTC: return imx_srtc_handler(smc_fid, handle, x1, x2, x3, x4); + case IMX_SIP_CPUFREQ: + SMC_RET1(handle, imx_cpufreq_handler(smc_fid, x1, x2, x3)); + break; #endif default: WARN("Unimplemented i.MX SiP Service Call: 0x%x\n", smc_fid); diff --git a/plat/imx/common/include/imx_sip_svc.h b/plat/imx/common/include/imx_sip_svc.h index 0d21f75..4ba7b87 100644 --- a/plat/imx/common/include/imx_sip_svc.h +++ b/plat/imx/common/include/imx_sip_svc.h @@ -8,10 +8,15 @@ #define __IMX_SIP_SVC_H__ /* SMC function IDs for SiP Service queries */ +#define IMX_SIP_CPUFREQ 0xC2000001 +#define IMX_SIP_SET_CPUFREQ 0x00 + #define IMX_SIP_SRTC 0xC2000002 #define IMX_SIP_SRTC_SET_TIME 0x00 #if (defined(PLAT_IMX8QM) || defined(PLAT_IMX8QX)) +int imx_cpufreq_handler(uint32_t smc_fid, u_register_t x1, + u_register_t x2, u_register_t x3); int imx_srtc_handler(uint32_t smc_fid, void *handle, u_register_t x1, u_register_t x2, u_register_t x3, u_register_t x4); #endif