clk: msm: clock-osm: Request Nominal Fmax frequency after OSM enable

Index 0 LUT entry must be selected before enabling OSM. The final
desired boot rate should be selected once OSM is enabled.
Fix this.

CRs-Fixed: 1082804
Change-Id: I7901df15ec03273304c5e4d417f272497ad1523c
Signed-off-by: Osvaldo Banuelos <osvaldob@codeaurora.org>

1 files changed, 31 insertions, 17 deletions

diff --git a/drivers/clk/msm/clock-osm.c b/drivers/clk/msm/clock-osm.c
index b1c9a24c4087..5391ef456aae 100644
--- a/drivers/clk/msm/clock-osm.c
+++ b/drivers/clk/msm/clock-osm.c
@@ -2748,33 +2748,26 @@ static int cpu_clock_osm_driver_probe(struct platform_device *pdev)
 	}
 	clk_prepare_enable(&sys_apcsaux_clk_gcc.c);
 
-	/* Set boot rate */
-	rc = clk_set_rate(&pwrcl_clk.c, msmcobalt_v1 ?
-			  MSMCOBALTV1_PWRCL_BOOT_RATE :
-			  MSMCOBALTV2_PWRCL_BOOT_RATE);
+	rc = clk_set_rate(&osm_clk_src.c, osm_clk_init_rate);
 	if (rc) {
-		dev_err(&pdev->dev, "Unable to set boot rate on pwr cluster, rc=%d\n",
+		dev_err(&pdev->dev, "Unable to set init rate on osm_clk, rc=%d\n",
 			rc);
-		clk_disable_unprepare(&sys_apcsaux_clk_gcc.c);
-		return rc;
+		goto exit2;
 	}
 
-	rc = clk_set_rate(&perfcl_clk.c, msmcobalt_v1 ?
-			  MSMCOBALTV1_PERFCL_BOOT_RATE :
-			  MSMCOBALTV2_PERFCL_BOOT_RATE);
+	/* Make sure index zero is selected */
+	rc = clk_set_rate(&pwrcl_clk.c, init_rate);
 	if (rc) {
-		dev_err(&pdev->dev, "Unable to set boot rate on perf cluster, rc=%d\n",
+		dev_err(&pdev->dev, "Unable to set init rate on pwr cluster, rc=%d\n",
 			rc);
-		clk_disable_unprepare(&sys_apcsaux_clk_gcc.c);
-		return rc;
+		goto exit2;
 	}
 
-	rc = clk_set_rate(&osm_clk_src.c, osm_clk_init_rate);
+	rc = clk_set_rate(&perfcl_clk.c, init_rate);
 	if (rc) {
-		dev_err(&pdev->dev, "Unable to set init rate on osm_clk, rc=%d\n",
+		dev_err(&pdev->dev, "Unable to set init rate on perf cluster, rc=%d\n",
 			rc);
-		clk_disable_unprepare(&sys_apcsaux_clk_gcc.c);
-		return rc;
+		goto exit2;
 	}
 
 	get_online_cpus();
@@ -2785,6 +2778,25 @@ static int cpu_clock_osm_driver_probe(struct platform_device *pdev)
 		     "Failed to enable clock for cpu %d\n", cpu);
 	}
 
+	/* Set final boot rate */
+	rc = clk_set_rate(&pwrcl_clk.c, msmcobalt_v1 ?
+			  MSMCOBALTV1_PWRCL_BOOT_RATE :
+			  MSMCOBALTV2_PWRCL_BOOT_RATE);
+	if (rc) {
+		dev_err(&pdev->dev, "Unable to set boot rate on pwr cluster, rc=%d\n",
+			rc);
+		goto exit2;
+	}
+
+	rc = clk_set_rate(&perfcl_clk.c, msmcobalt_v1 ?
+			  MSMCOBALTV1_PERFCL_BOOT_RATE :
+			  MSMCOBALTV2_PERFCL_BOOT_RATE);
+	if (rc) {
+		dev_err(&pdev->dev, "Unable to set boot rate on perf cluster, rc=%d\n",
+			rc);
+		goto exit2;
+	}
+
 	pwrcl_clk.version = clk_osm_read_reg(&pwrcl_clk, VERSION_REG);
 	perfcl_clk.version = clk_osm_read_reg(&perfcl_clk, VERSION_REG);
 
@@ -2801,6 +2813,8 @@ static int cpu_clock_osm_driver_probe(struct platform_device *pdev)
 
 	return 0;
 
+exit2:
+	clk_disable_unprepare(&sys_apcsaux_clk_gcc.c);
 exit:
 	dev_err(&pdev->dev, "OSM driver failed to initialize, rc=%d\n",
 		rc);