3 files changed, 42 insertions, 0 deletions
diff --git a/plat/imx/common/imx_sip_handler.c b/plat/imx/common/imx_sip_handler.c
index a414a4467..22a7f2b83 100644
--- a/plat/imx/common/imx_sip_handler.c
+++ b/plat/imx/common/imx_sip_handler.c
@@ -13,6 +13,12 @@
 #include <imx_sip_svc.h>
 #include <sci/sci.h>
 
+#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 @@ int imx_srtc_handler(uint32_t smc_fid,
 
 	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 23ff0bbef..89b9df8c5 100644
--- a/plat/imx/common/imx_sip_svc.c
+++ b/plat/imx/common/imx_sip_svc.c
@@ -29,6 +29,9 @@ static uintptr_t imx_sip_handler(unsigned int smc_fid,
 #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 0d21f7562..4ba7b87ac 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