MLK-22879-02 soc: imx: Update busfreq to support different frequncy setpoint

On i.MX8M SOC family, we can support LPDDR4, DDR4 or DDR3L, we may need
to support different setpoint for audio & low bus mode on different DDR type,
So update the code to get all the supported setpoint info from ATF.
The maximum setpoints that can be supported by hardware is 4, if the drate
for a setpoint is '0', that means this setpoint is not enabled. We can use
these info to find out the lowest drate setpoint for audio & low bus mode.

BuildInfo:
    - ATF 59fe78cfe7

Signed-off-by: Jacky Bai <ping.bai@nxp.com>
Reviewed-by: Anson Huang <anson.huang@nxp.com>

1 files changed, 85 insertions, 117 deletions

diff --git a/drivers/soc/imx/busfreq-imx8mq.c b/drivers/soc/imx/busfreq-imx8mq.c
index 441d5229f5c3..28fd6cb4fd46 100644
--- a/drivers/soc/imx/busfreq-imx8mq.c
+++ b/drivers/soc/imx/busfreq-imx8mq.c
@@ -41,6 +41,7 @@
 #define LOW_BUS_FREQ_100MTS	0x2
 #define LOW_BUS_FREQ_667MTS	0x1
 #define WAIT_BUS_FREQ_DONE	0xf
+#define DLL_ON_DRATE		667
 
 static struct device *busfreq_dev;
 static int low_bus_freq_mode;
@@ -53,6 +54,12 @@ static int cur_bus_freq_mode;
 static int busfreq_suspended;
 static bool cancel_reduce_bus_freq;
 
+static unsigned int fsp_table[4];
+static unsigned long origin_noc_rate;
+static int low_bus_mode_fsp_index;
+/* no bypass or dll off mode support if lowest fsp > 667mts */
+static bool bypass_support = true;
+
 static struct clk *dram_pll_clk;
 static struct clk *sys1_pll_800m;
 static struct clk *sys1_pll_400m;
@@ -115,50 +122,14 @@ static void reduce_bus_freq(void)
 	 */
 	if (audio_bus_count) {
 		if (cur_bus_freq_mode == BUS_FREQ_HIGH) {
-
-			if (of_machine_is_compatible("fsl,imx8mq")) {
-				if (imx8_get_soc_revision() < IMX_CHIP_REVISION_2_1) {
-					update_bus_freq(LOW_BUS_FREQ_667MTS);
-
-					/*
-					 * the dram_apb and dram_core clk rate is changed
-					 * in ATF side, below two lines of code is just used
-					 * to upate the clock tree info in kernel side.
-					 */
-					clk_set_rate(dram_apb_pre_div, 160000000);
-					clk_get_rate(dram_pll_clk);
-				} else {
-					/* prepare the necessary clk before frequency change */
-					clk_prepare_enable(sys1_pll_40m);
-					clk_prepare_enable(dram_alt_root);
-					clk_prepare_enable(sys1_pll_100m);
-
-					update_bus_freq(LOW_BUS_FREQ_100MTS);
-
-					clk_set_parent(dram_alt_src, sys1_pll_100m);
-					clk_set_parent(dram_core_clk, dram_alt_root);
-					clk_set_parent(dram_apb_src, sys1_pll_40m);
-					clk_set_rate(dram_apb_pre_div, 20000000);
-					clk_disable_unprepare(sys1_pll_100m);
-					clk_disable_unprepare(sys1_pll_40m);
-					clk_disable_unprepare(dram_alt_root);
-				}
-				/* reduce the NOC & bus clock */
-				rate = clk_get_rate(noc_div);
-				if (rate == 0) {
-					WARN_ON(1);
-					return;
-				}
-				clk_set_rate(noc_div, rate / 8);
-			} else {
+			if (bypass_support) {
 				/* prepare the necessary clk before frequency change */
 				clk_prepare_enable(sys1_pll_40m);
 				clk_prepare_enable(dram_alt_root);
 				clk_prepare_enable(sys1_pll_100m);
 
-				update_bus_freq(LOW_BUS_FREQ_100MTS);
+				update_bus_freq(low_bus_mode_fsp_index);
 
-				/* correct the clock tree info */
 				clk_set_parent(dram_alt_src, sys1_pll_100m);
 				clk_set_parent(dram_core_clk, dram_alt_root);
 				clk_set_parent(dram_apb_src, sys1_pll_40m);
@@ -166,15 +137,22 @@ static void reduce_bus_freq(void)
 				clk_disable_unprepare(sys1_pll_100m);
 				clk_disable_unprepare(sys1_pll_40m);
 				clk_disable_unprepare(dram_alt_root);
-
-				/* change the NOC rate */
-				rate = clk_get_rate(noc_div);
-				if (rate == 0) {
-					WARN_ON(1);
-					return;
-				}
-				clk_set_rate(noc_div, rate / 5);
+			} else {
+				update_bus_freq(low_bus_mode_fsp_index);
+				/*
+				 * the dram_apb and dram_core clk rate is changed
+				 * in ATF side, below two lines of code is just used
+				 * to update the clock tree info in kernel side.
+				 */
+				clk_set_rate(dram_apb_pre_div, 160000000);
+				clk_get_rate(dram_pll_clk);
 			}
+			/* change the NOC rate */
+			if (of_machine_is_compatible("fsl,imx8mq"))
+				clk_set_rate(noc_div, origin_noc_rate / 8);
+			else
+				clk_set_rate(noc_div, origin_noc_rate / 5);
+
 			rate = clk_get_rate(ahb_div);
 			if (rate == 0) {
 				WARN_ON(1);
@@ -189,49 +167,14 @@ static void reduce_bus_freq(void)
 		cur_bus_freq_mode = BUS_FREQ_AUDIO;
 	} else {
 		if (cur_bus_freq_mode == BUS_FREQ_HIGH) {
-			if (of_machine_is_compatible("fsl,imx8mq")) {
-				if (imx8_get_soc_revision() < IMX_CHIP_REVISION_2_1) {
-					update_bus_freq(LOW_BUS_FREQ_667MTS);
-
-					/*
-					 * the dram_apb and dram_core clk rate is changed
-					 * in ATF side, below two lines of code is just used
-					 * to upate the clock tree info in kernel side.
-					 */
-					clk_set_rate(dram_apb_pre_div, 160000000);
-					clk_get_rate(dram_pll_clk);
-				} else {
-					/* prepare the necessary clk before frequency change */
-					clk_prepare_enable(sys1_pll_40m);
-					clk_prepare_enable(dram_alt_root);
-					clk_prepare_enable(sys1_pll_100m);
-
-					update_bus_freq(LOW_BUS_FREQ_100MTS);
-
-					clk_set_parent(dram_alt_src, sys1_pll_100m);
-					clk_set_parent(dram_core_clk, dram_alt_root);
-					clk_set_parent(dram_apb_src, sys1_pll_40m);
-					clk_set_rate(dram_apb_pre_div, 20000000);
-					clk_disable_unprepare(sys1_pll_100m);
-					clk_disable_unprepare(sys1_pll_40m);
-					clk_disable_unprepare(dram_alt_root);
-				}
-				/* reduce the NOC & bus clock */
-				rate = clk_get_rate(noc_div);
-				if (rate == 0) {
-					WARN_ON(1);
-					return;
-				}
-				clk_set_rate(noc_div, rate / 8);
-			} else {
+			if (bypass_support) {
 				/* prepare the necessary clk before frequency change */
 				clk_prepare_enable(sys1_pll_40m);
 				clk_prepare_enable(dram_alt_root);
 				clk_prepare_enable(sys1_pll_100m);
 
-				update_bus_freq(LOW_BUS_FREQ_100MTS);
+				update_bus_freq(low_bus_mode_fsp_index);
 
-				/* correct the clock tree info */
 				clk_set_parent(dram_alt_src, sys1_pll_100m);
 				clk_set_parent(dram_core_clk, dram_alt_root);
 				clk_set_parent(dram_apb_src, sys1_pll_40m);
@@ -239,16 +182,23 @@ static void reduce_bus_freq(void)
 				clk_disable_unprepare(sys1_pll_100m);
 				clk_disable_unprepare(sys1_pll_40m);
 				clk_disable_unprepare(dram_alt_root);
-
-				/* change the NOC clock rate  */
-				rate = clk_get_rate(noc_div);
-				if (rate == 0) {
-					WARN_ON(1);
-					return;
-				}
-				clk_set_rate(noc_div, rate / 5);
+			} else {
+				update_bus_freq(low_bus_mode_fsp_index);
+				/*
+				 * the dram_apb and dram_core clk rate is changed
+				 * in ATF side, below two lines of code is just used
+				 * to update the clock tree info in kernel side.
+				 */
+				clk_set_rate(dram_apb_pre_div, 160000000);
+				clk_get_rate(dram_pll_clk);
 			}
 
+			/* change the NOC rate */
+			if (of_machine_is_compatible("fsl,imx8mq"))
+				clk_set_rate(noc_div, origin_noc_rate / 8);
+			else
+				clk_set_rate(noc_div, origin_noc_rate / 5);
+
 			rate = clk_get_rate(ahb_div);
 			if (rate == 0) {
 				WARN_ON(1);
@@ -323,30 +273,7 @@ static int set_high_bus_freq(int high_bus_freq)
 	if (high_bus_freq_mode)
 		return 0;
 
-	if (of_machine_is_compatible("fsl,imx8mq")) {
-		if (imx8_get_soc_revision() < IMX_CHIP_REVISION_2_1) {
-			/* switch the DDR freqeuncy */
-			update_bus_freq(HIGH_FREQ_3200MTS);
-
-			clk_set_rate(dram_apb_pre_div, 200000000);
-			clk_get_rate(dram_pll_clk);
-		} else {
-			/*  enable the clks needed in frequency */
-			clk_prepare_enable(sys1_pll_800m);
-			clk_prepare_enable(dram_pll_clk);
-
-			/* switch the DDR freqeuncy */
-			update_bus_freq(HIGH_FREQ_3200MTS);
-
-			/* correct the clock tree info */
-			clk_set_parent(dram_apb_src, sys1_pll_800m);
-			clk_set_rate(dram_apb_pre_div, 160000000);
-			clk_set_parent(dram_core_clk, dram_pll_clk);
-			clk_disable_unprepare(sys1_pll_800m);
-			clk_disable_unprepare(dram_pll_clk);
-		}
-		clk_set_rate(noc_div, 800000000);
-	} else {
+	if (bypass_support) {
 		/*  enable the clks needed in frequency */
 		clk_prepare_enable(sys1_pll_800m);
 		clk_prepare_enable(dram_pll_clk);
@@ -360,9 +287,15 @@ static int set_high_bus_freq(int high_bus_freq)
 		clk_set_parent(dram_core_clk, dram_pll_clk);
 		clk_disable_unprepare(sys1_pll_800m);
 		clk_disable_unprepare(dram_pll_clk);
-		clk_set_rate(noc_div, 750000000);
+	} else {
+		/* switch the DDR freqeuncy */
+		update_bus_freq(HIGH_FREQ_3200MTS);
+
+		clk_set_rate(dram_apb_pre_div, 200000000);
+		clk_get_rate(dram_pll_clk);
 	}
 
+	clk_set_rate(noc_div, origin_noc_rate);
 	clk_set_rate(ahb_div, 133333333);
 	clk_set_parent(main_axi_src, sys2_pll_333m);
 
@@ -651,9 +584,11 @@ static int imx8mm_init_busfreq_clk(struct platform_device *pdev)
  * @return         The function returns 0 on success
  *
  */
+
 static int busfreq_probe(struct platform_device *pdev)
 {
-	int err;
+	int i, err;
+	struct arm_smccc_res res;
 
 	busfreq_dev = &pdev->dev;
 
@@ -668,6 +603,39 @@ static int busfreq_probe(struct platform_device *pdev)
 		return err;
 	}
 
+	origin_noc_rate = clk_get_rate(noc_div);
+	if (origin_noc_rate == 0) {
+		WARN_ON(1);
+		return -EINVAL;
+	}
+
+	/*
+	 * Get the supported frequency, normally the lowest frequency point
+	 * is used for low bus & audio bus mode.
+	 */
+	for (i = 0; i < 4; i++) {
+		arm_smccc_smc(FSL_SIP_DDR_DVFS, 0x11, i, 0, 0, 0, 0, 0, &res);
+		err = res.a0;
+		if (err < 0)
+			return -EINVAL;
+
+		fsp_table[i] = res.a0;
+	}
+
+	/* get the lowest fsp index */
+	for (i = 0; i < 4; i++)
+		if (fsp_table[i] == 0)
+			break;
+
+	low_bus_mode_fsp_index = i - 1;
+
+	/*
+	 * if lowest fsp data rate higher than 666mts, then no dll off mode or
+	 * bypass mode support.
+	 */
+	if (fsp_table[low_bus_mode_fsp_index] >= DLL_ON_DRATE)
+		bypass_support = false;
+
 	/* init the irq used for ddr frequency change */
 	err = init_busfreq_irq(pdev);
 	if (err) {