qcom-geni-se : Add neareset frequency match support
Add nearest frequency match support for spi driver when
exact or exact multiple frequency is not available
Change-Id: If7e5bac2815967a279066abf6cc0a17d6de955eb
Signed-off-by: Prudhvi Yarlagadda <pyarlaga@codeaurora.org>
diff --git a/drivers/platform/msm/qcom-geni-se.c b/drivers/platform/msm/qcom-geni-se.c
index dedcb5f..75efdab 100644
--- a/drivers/platform/msm/qcom-geni-se.c
+++ b/drivers/platform/msm/qcom-geni-se.c
@@ -989,6 +989,9 @@
unsigned long *tbl;
int num_clk_levels;
int i;
+ unsigned long best_delta = 0;
+ unsigned long new_delta;
+ unsigned int divider;
num_clk_levels = geni_se_clk_tbl_get(rsc, &tbl);
if (num_clk_levels < 0)
@@ -998,17 +1001,21 @@
return -EFAULT;
*res_freq = 0;
- for (i = 0; i < num_clk_levels; i++) {
- if (!(tbl[i] % req_freq)) {
- *index = i;
- *res_freq = tbl[i];
- return 0;
- }
- if (!(*res_freq) || ((tbl[i] > *res_freq) &&
- (tbl[i] < req_freq))) {
+ for (i = 0; i < num_clk_levels; i++) {
+ divider = DIV_ROUND_UP(tbl[i], req_freq);
+ new_delta = req_freq - (tbl[i] / divider);
+
+ if (!best_delta || new_delta < best_delta) {
+ /* We have a new best! */
*index = i;
*res_freq = tbl[i];
+
+ /*If the new best is exact then we're done*/
+ if (new_delta == 0)
+ return 0;
+
+ best_delta = new_delta;
}
}
diff --git a/drivers/spi/spi-geni-qcom.c b/drivers/spi/spi-geni-qcom.c
index 8dfaea8..ec2b56a 100644
--- a/drivers/spi/spi-geni-qcom.c
+++ b/drivers/spi/spi-geni-qcom.c
@@ -170,27 +170,32 @@
int *clk_idx, int *clk_div)
{
unsigned long sclk_freq;
+ unsigned long res_freq;
struct se_geni_rsc *rsc = &mas->spi_rsc;
int ret = 0;
ret = geni_se_clk_freq_match(&mas->spi_rsc,
(speed_hz * mas->oversampling), clk_idx,
- &sclk_freq, true);
+ &sclk_freq, false);
if (ret) {
dev_err(mas->dev, "%s: Failed(%d) to find src clk for 0x%x\n",
__func__, ret, speed_hz);
return ret;
}
- *clk_div = ((sclk_freq / mas->oversampling) / speed_hz);
+ *clk_div = DIV_ROUND_UP(sclk_freq, (mas->oversampling*speed_hz));
+
if (!(*clk_div)) {
dev_err(mas->dev, "%s:Err:sclk:%lu oversampling:%d speed:%u\n",
__func__, sclk_freq, mas->oversampling, speed_hz);
return -EINVAL;
}
- dev_dbg(mas->dev, "%s: req %u sclk %lu, idx %d, div %d\n", __func__,
- speed_hz, sclk_freq, *clk_idx, *clk_div);
+ res_freq = (sclk_freq / (*clk_div));
+
+ dev_dbg(mas->dev, "%s: req %u resultant %lu sclk %lu, idx %d, div %d\n",
+ __func__, speed_hz, res_freq, sclk_freq, *clk_idx, *clk_div);
+
ret = clk_set_rate(rsc->se_clk, sclk_freq);
if (ret)
dev_err(mas->dev, "%s: clk_set_rate failed %d\n",