clk: at91: clk-peripheral: add support for changeable parent rate

Some peripheral clocks on SAMA7G5 supports requesting parent to change
its rate (image related clocks: csi, csi2dc, isc). Add support
so that if registered with this option the clock rate to be
requested from parent.

Signed-off-by: Claudiu Beznea <claudiu.beznea@microchip.com>
Link: https://lore.kernel.org/r/1595403506-8209-14-git-send-email-claudiu.beznea@microchip.com
Signed-off-by: Stephen Boyd <sboyd@kernel.org>

diff --git a/drivers/clk/at91/at91sam9n12.c b/drivers/clk/at91/at91sam9n12.c
index 4aa97e6..aaf4da3 100644 (file)
--- a/drivers/clk/at91/at91sam9n12.c
+++ b/drivers/clk/at91/at91sam9n12.c
@@ -222,7 +222,7 @@ static void __init at91sam9n12_pmc_setup(struct device_node *np)
                                                         at91sam9n12_periphck[i].n,
                                                         "masterck",
                                                         at91sam9n12_periphck[i].id,
-                                                        &range);
+                                                        &range, INT_MIN);
                if (IS_ERR(hw))
                        goto err_free;
 

diff --git a/drivers/clk/at91/at91sam9x5.c b/drivers/clk/at91/at91sam9x5.c
index 0ce3da0..52a9d2f 100644 (file)
--- a/drivers/clk/at91/at91sam9x5.c
+++ b/drivers/clk/at91/at91sam9x5.c
@@ -257,7 +257,7 @@ static void __init at91sam9x5_pmc_setup(struct device_node *np,
                                                         at91sam9x5_periphck[i].n,
                                                         "masterck",
                                                         at91sam9x5_periphck[i].id,
-                                                        &range);
+                                                        &range, INT_MIN);
                if (IS_ERR(hw))
                        goto err_free;
 
@@ -270,7 +270,7 @@ static void __init at91sam9x5_pmc_setup(struct device_node *np,
                                                         extra_pcks[i].n,
                                                         "masterck",
                                                         extra_pcks[i].id,
-                                                        &range);
+                                                        &range, INT_MIN);
                if (IS_ERR(hw))
                        goto err_free;
 

diff --git a/drivers/clk/at91/clk-peripheral.c b/drivers/clk/at91/clk-peripheral.c
index 4c9a414..7867eaf 100644 (file)
--- a/drivers/clk/at91/clk-peripheral.c
+++ b/drivers/clk/at91/clk-peripheral.c
@@ -38,6 +38,7 @@ struct clk_sam9x5_peripheral {
        u32 div;
        const struct clk_pcr_layout *layout;
        bool auto_div;
+       int chg_pid;
 };
 
 #define to_clk_sam9x5_peripheral(hw) \
@@ -238,6 +239,87 @@ clk_sam9x5_peripheral_recalc_rate(struct clk_hw *hw,
        return parent_rate >> periph->div;
 }
 
+static void clk_sam9x5_peripheral_best_diff(struct clk_rate_request *req,
+                                           struct clk_hw *parent,
+                                           unsigned long parent_rate,
+                                           u32 shift, long *best_diff,
+                                           long *best_rate)
+{
+       unsigned long tmp_rate = parent_rate >> shift;
+       unsigned long tmp_diff = abs(req->rate - tmp_rate);
+
+       if (*best_diff < 0 || *best_diff >= tmp_diff) {
+               *best_rate = tmp_rate;
+               *best_diff = tmp_diff;
+               req->best_parent_rate = parent_rate;
+               req->best_parent_hw = parent;
+       }
+}
+
+static int clk_sam9x5_peripheral_determine_rate(struct clk_hw *hw,
+                                               struct clk_rate_request *req)
+{
+       struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
+       struct clk_hw *parent = clk_hw_get_parent(hw);
+       struct clk_rate_request req_parent = *req;
+       unsigned long parent_rate = clk_hw_get_rate(parent);
+       unsigned long tmp_rate;
+       long best_rate = LONG_MIN;
+       long best_diff = LONG_MIN;
+       u32 shift;
+
+       if (periph->id < PERIPHERAL_ID_MIN || !periph->range.max)
+               return parent_rate;
+
+       /* Fist step: check the available dividers. */
+       for (shift = 0; shift <= PERIPHERAL_MAX_SHIFT; shift++) {
+               tmp_rate = parent_rate >> shift;
+
+               if (periph->range.max && tmp_rate > periph->range.max)
+                       continue;
+
+               clk_sam9x5_peripheral_best_diff(req, parent, parent_rate,
+                                               shift, &best_diff, &best_rate);
+
+               if (!best_diff || best_rate <= req->rate)
+                       break;
+       }
+
+       if (periph->chg_pid < 0)
+               goto end;
+
+       /* Step two: try to request rate from parent. */
+       parent = clk_hw_get_parent_by_index(hw, periph->chg_pid);
+       if (!parent)
+               goto end;
+
+       for (shift = 0; shift <= PERIPHERAL_MAX_SHIFT; shift++) {
+               req_parent.rate = req->rate << shift;
+
+               if (__clk_determine_rate(parent, &req_parent))
+                       continue;
+
+               clk_sam9x5_peripheral_best_diff(req, parent, req_parent.rate,
+                                               shift, &best_diff, &best_rate);
+
+               if (!best_diff)
+                       break;
+       }
+end:
+       if (best_rate < 0 ||
+           (periph->range.max && best_rate > periph->range.max))
+               return -EINVAL;
+
+       pr_debug("PCK: %s, best_rate = %ld, parent clk: %s @ %ld\n",
+                __func__, best_rate,
+                __clk_get_name((req->best_parent_hw)->clk),
+                req->best_parent_rate);
+
+       req->rate = best_rate;
+
+       return 0;
+}
+
 static long clk_sam9x5_peripheral_round_rate(struct clk_hw *hw,
                                             unsigned long rate,
                                             unsigned long *parent_rate)
@@ -320,11 +402,21 @@ static const struct clk_ops sam9x5_peripheral_ops = {
        .set_rate = clk_sam9x5_peripheral_set_rate,
 };
 
+static const struct clk_ops sam9x5_peripheral_chg_ops = {
+       .enable = clk_sam9x5_peripheral_enable,
+       .disable = clk_sam9x5_peripheral_disable,
+       .is_enabled = clk_sam9x5_peripheral_is_enabled,
+       .recalc_rate = clk_sam9x5_peripheral_recalc_rate,
+       .determine_rate = clk_sam9x5_peripheral_determine_rate,
+       .set_rate = clk_sam9x5_peripheral_set_rate,
+};
+
 struct clk_hw * __init
 at91_clk_register_sam9x5_peripheral(struct regmap *regmap, spinlock_t *lock,
                                    const struct clk_pcr_layout *layout,
                                    const char *name, const char *parent_name,
-                                   u32 id, const struct clk_range *range)
+                                   u32 id, const struct clk_range *range,
+                                   int chg_pid)
 {
        struct clk_sam9x5_peripheral *periph;
        struct clk_init_data init;
@@ -339,10 +431,16 @@ at91_clk_register_sam9x5_peripheral(struct regmap *regmap, spinlock_t *lock,
                return ERR_PTR(-ENOMEM);
 
        init.name = name;
-       init.ops = &sam9x5_peripheral_ops;
-       init.parent_names = (parent_name ? &parent_name : NULL);
-       init.num_parents = (parent_name ? 1 : 0);
-       init.flags = 0;
+       init.parent_names = &parent_name;
+       init.num_parents = 1;
+       if (chg_pid < 0) {
+               init.flags = 0;
+               init.ops = &sam9x5_peripheral_ops;
+       } else {
+               init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE |
+                            CLK_SET_RATE_PARENT;
+               init.ops = &sam9x5_peripheral_chg_ops;
+       }
 
        periph->id = id;
        periph->hw.init = &init;
@@ -353,6 +451,7 @@ at91_clk_register_sam9x5_peripheral(struct regmap *regmap, spinlock_t *lock,
                periph->auto_div = true;
        periph->layout = layout;
        periph->range = *range;
+       periph->chg_pid = chg_pid;
 
        hw = &periph->hw;
        ret = clk_hw_register(NULL, &periph->hw);

diff --git a/drivers/clk/at91/dt-compat.c b/drivers/clk/at91/dt-compat.c
index cc95d42..1b90c4f 100644 (file)
--- a/drivers/clk/at91/dt-compat.c
+++ b/drivers/clk/at91/dt-compat.c
@@ -463,7 +463,8 @@ of_at91_clk_periph_setup(struct device_node *np, u8 type)
                                                                 &dt_pcr_layout,
                                                                 name,
                                                                 parent_name,
-                                                                id, &range);
+                                                                id, &range,
+                                                                INT_MIN);
                }
 
                if (IS_ERR(hw))

diff --git a/drivers/clk/at91/pmc.h b/drivers/clk/at91/pmc.h
index 29d150f..34c9506 100644 (file)
--- a/drivers/clk/at91/pmc.h
+++ b/drivers/clk/at91/pmc.h
@@ -168,7 +168,8 @@ struct clk_hw * __init
 at91_clk_register_sam9x5_peripheral(struct regmap *regmap, spinlock_t *lock,
                                    const struct clk_pcr_layout *layout,
                                    const char *name, const char *parent_name,
-                                   u32 id, const struct clk_range *range);
+                                   u32 id, const struct clk_range *range,
+                                   int chg_pid);
 
 struct clk_hw * __init
 at91_clk_register_pll(struct regmap *regmap, const char *name,

diff --git a/drivers/clk/at91/sam9x60.c b/drivers/clk/at91/sam9x60.c
index 197ffc7..f090518 100644 (file)
--- a/drivers/clk/at91/sam9x60.c
+++ b/drivers/clk/at91/sam9x60.c
@@ -277,7 +277,7 @@ static void __init sam9x60_pmc_setup(struct device_node *np)
                                                         sam9x60_periphck[i].n,
                                                         "masterck",
                                                         sam9x60_periphck[i].id,
-                                                        &range);
+                                                        &range, INT_MIN);
                if (IS_ERR(hw))
                        goto err_free;
 

diff --git a/drivers/clk/at91/sama5d2.c b/drivers/clk/at91/sama5d2.c
index 6a685d0..c7765b6 100644 (file)
--- a/drivers/clk/at91/sama5d2.c
+++ b/drivers/clk/at91/sama5d2.c
@@ -291,7 +291,7 @@ static void __init sama5d2_pmc_setup(struct device_node *np)
                                                         sama5d2_periphck[i].n,
                                                         "masterck",
                                                         sama5d2_periphck[i].id,
-                                                        &range);
+                                                        &range, INT_MIN);
                if (IS_ERR(hw))
                        goto err_free;
 
@@ -304,7 +304,8 @@ static void __init sama5d2_pmc_setup(struct device_node *np)
                                                         sama5d2_periph32ck[i].n,
                                                         "h32mxck",
                                                         sama5d2_periph32ck[i].id,
-                                                        &sama5d2_periph32ck[i].r);
+                                                        &sama5d2_periph32ck[i].r,
+                                                        INT_MIN);
                if (IS_ERR(hw))
                        goto err_free;
 

diff --git a/drivers/clk/at91/sama5d3.c b/drivers/clk/at91/sama5d3.c
index 5609b04..49ec596 100644 (file)
--- a/drivers/clk/at91/sama5d3.c
+++ b/drivers/clk/at91/sama5d3.c
@@ -223,7 +223,8 @@ static void __init sama5d3_pmc_setup(struct device_node *np)
                                                         sama5d3_periphck[i].n,
                                                         "masterck",
                                                         sama5d3_periphck[i].id,
-                                                        &sama5d3_periphck[i].r);
+                                                        &sama5d3_periphck[i].r,
+                                                        INT_MIN);
                if (IS_ERR(hw))
                        goto err_free;
 

diff --git a/drivers/clk/at91/sama5d4.c b/drivers/clk/at91/sama5d4.c
index 662ff5f..fa12189 100644 (file)
--- a/drivers/clk/at91/sama5d4.c
+++ b/drivers/clk/at91/sama5d4.c
@@ -246,7 +246,7 @@ static void __init sama5d4_pmc_setup(struct device_node *np)
                                                         sama5d4_periphck[i].n,
                                                         "masterck",
                                                         sama5d4_periphck[i].id,
-                                                        &range);
+                                                        &range, INT_MIN);
                if (IS_ERR(hw))
                        goto err_free;
 
@@ -259,7 +259,7 @@ static void __init sama5d4_pmc_setup(struct device_node *np)
                                                         sama5d4_periph32ck[i].n,
                                                         "h32mxck",
                                                         sama5d4_periph32ck[i].id,
-                                                        &range);
+                                                        &range, INT_MIN);
                if (IS_ERR(hw))
                        goto err_free;