4 files changed, 30 insertions, 14 deletions

diff --git a/src/soc/qualcomm/common/clock.c b/src/soc/qualcomm/common/clock.c
index e06a954f3ca0..09484b76f41e 100644
--- a/src/soc/qualcomm/common/clock.c
+++ b/src/soc/qualcomm/common/clock.c
@@ -90,15 +90,20 @@ static void clock_configure_mnd(struct clock_rcg *clk, uint32_t m, uint32_t n,
 
 /* Clock Root clock Generator Operations */
 enum cb_err clock_configure(struct clock_rcg *clk,
-			struct clock_freq_config *clk_cfg, uint32_t hz,
-			uint32_t num_perfs)
+			    struct clock_freq_config *clk_cfg, uint32_t hz,
+			    uint32_t num_perfs)
 {
 	uint32_t reg_val, idx;
 
 	for (idx = 0; idx < num_perfs; idx++)
-		if (hz <= clk_cfg[idx].hz)
+		if (hz == clk_cfg[idx].hz)
 			break;
 
+	/* Verify we matched an entry.  If not, throw error. */
+	if (idx >= num_perfs)
+		die("Failed to find a matching clock frequency (%d hz) for %p!\n",
+		    hz, clk);
+
 	reg_val = (clk_cfg[idx].src << CLK_CTL_CFG_SRC_SEL_SHFT) |
 			(clk_cfg[idx].div << CLK_CTL_CFG_SRC_DIV_SHFT);
 
diff --git a/src/soc/qualcomm/common/include/soc/clock_common.h b/src/soc/qualcomm/common/include/soc/clock_common.h
index 091182714916..b08d39bdd8ac 100644
--- a/src/soc/qualcomm/common/include/soc/clock_common.h
+++ b/src/soc/qualcomm/common/include/soc/clock_common.h
@@ -145,8 +145,17 @@ enum cb_err enable_and_poll_gdsc_status(void *gdscr_addr);
 
 void clock_reset_bcr(void *bcr_addr, bool assert);
 
+/*
+ * clock_configure(): Configure the clock at the given clock speed (hz).  If hz
+ * does not match any entries in the clk_cfg array, will throw and error and die().
+ *
+ * @param clk          struct clock_rcg pointer (root clock generator)
+ * @param clk_cfg      Array with possible clock configurations
+ * @param hz           frequency of clock to set
+ * @param num_perfs    size of clock array
+ */
 enum cb_err clock_configure(struct clock_rcg *clk, struct clock_freq_config *clk_cfg,
-		uint32_t hz, uint32_t num_perfs);
+			    uint32_t hz, uint32_t num_perfs);
 
 void clock_configure_dfsr_table(int qup, struct clock_freq_config *clk_cfg,
 		uint32_t num_perfs);
diff --git a/src/soc/qualcomm/sc7180/clock.c b/src/soc/qualcomm/sc7180/clock.c
index 5741c54bce68..cd9c3f5869d2 100644
--- a/src/soc/qualcomm/sc7180/clock.c
+++ b/src/soc/qualcomm/sc7180/clock.c
@@ -215,7 +215,7 @@ enum cb_err mdss_clock_configure(enum mdss_clock clk_type, uint32_t source,
 	mdss_clk_cfg.d_2 = d_2;
 
 	return clock_configure((struct clock_rcg *)mdss_clock[clk_type],
-			&mdss_clk_cfg, 0, 0);
+			       &mdss_clk_cfg, 0, 1);
 }
 
 int mdss_clock_enable(enum mdss_clock clk_type)
diff --git a/src/soc/qualcomm/sc7280/clock.c b/src/soc/qualcomm/sc7280/clock.c
index e321e8da9e9f..1ad0b1a4f8af 100644
--- a/src/soc/qualcomm/sc7280/clock.c
+++ b/src/soc/qualcomm/sc7280/clock.c
@@ -403,31 +403,33 @@ enum cb_err mdss_clock_configure(enum clk_mdss clk_type, uint32_t hz,
 
 	/* Initialize it with received arguments */
 	mdss_clk_cfg.div = divider ?  QCOM_CLOCK_DIV(divider) : 0;
+	mdss_clk_cfg.src = source;
+	mdss_clk_cfg.m = m;
+	mdss_clk_cfg.n = n;
+	mdss_clk_cfg.d_2 = d_2;
+	mdss_clk_cfg.hz = hz;
 
 	if (clk_type == MDSS_CLK_MDP) {
 		for (idx = 0; idx < ARRAY_SIZE(mdss_mdp_cfg); idx++) {
 			if (hz <= mdss_mdp_cfg[idx].hz) {
-				source = mdss_mdp_cfg[idx].src;
+				mdss_clk_cfg.src = mdss_mdp_cfg[idx].src;
 				mdss_clk_cfg.div = mdss_mdp_cfg[idx].div;
-				m = 0;
+				mdss_clk_cfg.hz = mdss_mdp_cfg[idx].hz;
+				mdss_clk_cfg.m = 0;
 				break;
 			}
 		}
 	}
-	mdss_clk_cfg.src = source;
-	mdss_clk_cfg.m = m;
-	mdss_clk_cfg.n = n;
-	mdss_clk_cfg.d_2 = d_2;
 
 	switch (clk_type) {
 	case MDSS_CLK_EDP_PIXEL:
 	case MDSS_CLK_PCLK0:
 		return clock_configure((struct clock_rcg *)
-				mdss_clock_mnd[clk_type],
-				&mdss_clk_cfg, hz, 0);
+				       mdss_clock_mnd[clk_type],
+				       &mdss_clk_cfg, mdss_clk_cfg.hz, 1);
 	default:
 		return clock_configure(mdss_clock[clk_type],
-				&mdss_clk_cfg, hz, 0);
+				       &mdss_clk_cfg, mdss_clk_cfg.hz, 1);
 	}
 }