clk: at91: clk-master: add notifier for divider

SAMA7G5 supports DVFS by changing cpuck. On SAMA7G5 mck0 shares the same
parent with cpuck as seen in the following clock tree:

                       +----------> cpuck
                       |
FRAC PLL ---> DIV PLL -+-> DIV ---> mck0

mck0 could go b/w 32KHz and 200MHz on SAMA7G5. To avoid mck0 overclocking
while changing FRAC PLL or DIV PLL the commit implements a notifier for
mck0 which applies a safe divider to register (maximum value of the divider
which is 5) on PRE_RATE_CHANGE events (such that changes on PLL to not
overclock mck0) and sets the maximum allowed rate on POST_RATE_CHANGE
events.

Signed-off-by: Claudiu Beznea <claudiu.beznea@microchip.com>
Link: https://lore.kernel.org/r/20211011112719.3951784-13-claudiu.beznea@microchip.com
Acked-by: Nicolas Ferre <nicolas.ferre@microchip.com>
Signed-off-by: Stephen Boyd <sboyd@kernel.org>

diff --git a/drivers/clk/at91/at91rm9200.c b/drivers/clk/at91/at91rm9200.c
index 428a6f4b9ebc56e054601e6f1a079fa01272aab2..fff4fdda974fc469c349cd8ef77a75fdd45e3c17 100644 (file)
--- a/drivers/clk/at91/at91rm9200.c
+++ b/drivers/clk/at91/at91rm9200.c
@@ -152,7 +152,7 @@ static void __init at91rm9200_pmc_setup(struct device_node *np)
                                          "masterck_pres",
                                          &at91rm9200_master_layout,
                                          &rm9200_mck_characteristics,
-                                         &rm9200_mck_lock, CLK_SET_RATE_GATE);
+                                         &rm9200_mck_lock, CLK_SET_RATE_GATE, 0);
        if (IS_ERR(hw))
                goto err_free;
 

diff --git a/drivers/clk/at91/at91sam9260.c b/drivers/clk/at91/at91sam9260.c
index b29843bea278b3a04dc7e1a42e8abf4ad9385533..79802f864ee55a4ed47d17133d9bd052a0000ffb 100644 (file)
--- a/drivers/clk/at91/at91sam9260.c
+++ b/drivers/clk/at91/at91sam9260.c
@@ -429,7 +429,7 @@ static void __init at91sam926x_pmc_setup(struct device_node *np,
                                          &at91rm9200_master_layout,
                                          data->mck_characteristics,
                                          &at91sam9260_mck_lock,
-                                         CLK_SET_RATE_GATE);
+                                         CLK_SET_RATE_GATE, 0);
        if (IS_ERR(hw))
                goto err_free;
 

diff --git a/drivers/clk/at91/at91sam9g45.c b/drivers/clk/at91/at91sam9g45.c
index 15da0dfe3ef25fe2f59e1ca9a0c60df4dc5ffdfa..7ed984f8058c4702ac3d54ba207874895d17878c 100644 (file)
--- a/drivers/clk/at91/at91sam9g45.c
+++ b/drivers/clk/at91/at91sam9g45.c
@@ -164,7 +164,7 @@ static void __init at91sam9g45_pmc_setup(struct device_node *np)
                                          &at91rm9200_master_layout,
                                          &mck_characteristics,
                                          &at91sam9g45_mck_lock,
-                                         CLK_SET_RATE_GATE);
+                                         CLK_SET_RATE_GATE, 0);
        if (IS_ERR(hw))
                goto err_free;
 

diff --git a/drivers/clk/at91/at91sam9n12.c b/drivers/clk/at91/at91sam9n12.c
index 7fe435f4b46b71bfca8949377cf32111c6814930..63cc58944b0019f6ae531685992b988350c236c5 100644 (file)
--- a/drivers/clk/at91/at91sam9n12.c
+++ b/drivers/clk/at91/at91sam9n12.c
@@ -191,7 +191,7 @@ static void __init at91sam9n12_pmc_setup(struct device_node *np)
                                          &at91sam9x5_master_layout,
                                          &mck_characteristics,
                                          &at91sam9n12_mck_lock,
-                                         CLK_SET_RATE_GATE);
+                                         CLK_SET_RATE_GATE, 0);
        if (IS_ERR(hw))
                goto err_free;
 

diff --git a/drivers/clk/at91/at91sam9rl.c b/drivers/clk/at91/at91sam9rl.c
index ecbabf5162bd6a715b65fb258a0ae6f0da52d147..4d4faf6c61d8af72cdf9668c0f71a243922ca5d0 100644 (file)
--- a/drivers/clk/at91/at91sam9rl.c
+++ b/drivers/clk/at91/at91sam9rl.c
@@ -132,7 +132,7 @@ static void __init at91sam9rl_pmc_setup(struct device_node *np)
                                          "masterck_pres",
                                          &at91rm9200_master_layout,
                                          &sam9rl_mck_characteristics,
-                                         &sam9rl_mck_lock, CLK_SET_RATE_GATE);
+                                         &sam9rl_mck_lock, CLK_SET_RATE_GATE, 0);
        if (IS_ERR(hw))
                goto err_free;
 

diff --git a/drivers/clk/at91/at91sam9x5.c b/drivers/clk/at91/at91sam9x5.c
index 5cce48c64ea2d334403305b0855c7a82b158cb3a..bd8007b4f3e02a362c123ae773062d07e6b0b819 100644 (file)
--- a/drivers/clk/at91/at91sam9x5.c
+++ b/drivers/clk/at91/at91sam9x5.c
@@ -210,7 +210,7 @@ static void __init at91sam9x5_pmc_setup(struct device_node *np,
                                          "masterck_pres",
                                          &at91sam9x5_master_layout,
                                          &mck_characteristics, &mck_lock,
-                                         CLK_SET_RATE_GATE);
+                                         CLK_SET_RATE_GATE, 0);
        if (IS_ERR(hw))
                goto err_free;
 

diff --git a/drivers/clk/at91/clk-master.c b/drivers/clk/at91/clk-master.c
index e67bcd03a827e9efdb23e14db966bef1c473c003..b2d0a7f4f7f9bb9a8b3f1713facdaf76d2d756b6 100644 (file)
--- a/drivers/clk/at91/clk-master.c
+++ b/drivers/clk/at91/clk-master.c
@@ -5,6 +5,7 @@
 
 #include <linux/clk-provider.h>
 #include <linux/clkdev.h>
+#include <linux/clk.h>
 #include <linux/clk/at91_pmc.h>
 #include <linux/of.h>
 #include <linux/mfd/syscon.h>
@@ -36,8 +37,12 @@ struct clk_master {
        u8 id;
        u8 parent;
        u8 div;
+       u32 safe_div;
 };
 
+/* MCK div reference to be used by notifier. */
+static struct clk_master *master_div;
+
 static inline bool clk_master_ready(struct clk_master *master)
 {
        unsigned int bit = master->id ? AT91_PMC_MCKXRDY : AT91_PMC_MCKRDY;
@@ -153,107 +158,81 @@ static const struct clk_ops master_div_ops = {
        .restore_context = clk_master_div_restore_context,
 };
 
-static int clk_master_div_set_rate(struct clk_hw *hw, unsigned long rate,
-                                  unsigned long parent_rate)
+/* This function must be called with lock acquired. */
+static int clk_master_div_set(struct clk_master *master,
+                             unsigned long parent_rate, int div)
 {
-       struct clk_master *master = to_clk_master(hw);
        const struct clk_master_characteristics *characteristics =
                                                master->characteristics;
-       unsigned long flags;
-       unsigned int mckr, tmp;
-       int div, i;
+       unsigned long rate = parent_rate;
+       unsigned int max_div = 0, div_index = 0, max_div_index = 0;
+       unsigned int i, mckr, tmp;
        int ret;
 
-       div = DIV_ROUND_CLOSEST(parent_rate, rate);
-       if (div > ARRAY_SIZE(characteristics->divisors))
-               return -EINVAL;
-
        for (i = 0; i < ARRAY_SIZE(characteristics->divisors); i++) {
                if (!characteristics->divisors[i])
                        break;
 
-               if (div == characteristics->divisors[i]) {
-                       div = i;
-                       break;
+               if (div == characteristics->divisors[i])
+                       div_index = i;
+
+               if (max_div < characteristics->divisors[i]) {
+                       max_div = characteristics->divisors[i];
+                       max_div_index = i;
                }
        }
 
-       if (i == ARRAY_SIZE(characteristics->divisors))
-               return -EINVAL;
+       if (div > max_div)
+               div_index = max_div_index;
 
-       spin_lock_irqsave(master->lock, flags);
        ret = regmap_read(master->regmap, master->layout->offset, &mckr);
        if (ret)
-               goto unlock;
+               return ret;
 
        mckr &= master->layout->mask;
        tmp = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK;
-       if (tmp == div)
-               goto unlock;
+       if (tmp == div_index)
+               return 0;
+
+       rate /= characteristics->divisors[div_index];
+       if (rate < characteristics->output.min)
+               pr_warn("master clk div is underclocked");
+       else if (rate > characteristics->output.max)
+               pr_warn("master clk div is overclocked");
 
        mckr &= ~(MASTER_DIV_MASK << MASTER_DIV_SHIFT);
-       mckr |= (div << MASTER_DIV_SHIFT);
+       mckr |= (div_index << MASTER_DIV_SHIFT);
        ret = regmap_write(master->regmap, master->layout->offset, mckr);
        if (ret)
-               goto unlock;
+               return ret;
 
        while (!clk_master_ready(master))
                cpu_relax();
-unlock:
-       spin_unlock_irqrestore(master->lock, flags);
+
+       master->div = characteristics->divisors[div_index];
 
        return 0;
 }
 
-static int clk_master_div_determine_rate(struct clk_hw *hw,
-                                        struct clk_rate_request *req)
+static unsigned long clk_master_div_recalc_rate_chg(struct clk_hw *hw,
+                                                   unsigned long parent_rate)
 {
        struct clk_master *master = to_clk_master(hw);
-       const struct clk_master_characteristics *characteristics =
-                                               master->characteristics;
-       struct clk_hw *parent;
-       unsigned long parent_rate, tmp_rate, best_rate = 0;
-       int i, best_diff = INT_MIN, tmp_diff;
-
-       parent = clk_hw_get_parent(hw);
-       if (!parent)
-               return -EINVAL;
-
-       parent_rate = clk_hw_get_rate(parent);
-       if (!parent_rate)
-               return -EINVAL;
-
-       for (i = 0; i < ARRAY_SIZE(characteristics->divisors); i++) {
-               if (!characteristics->divisors[i])
-                       break;
 
-               tmp_rate = DIV_ROUND_CLOSEST_ULL(parent_rate,
-                                                characteristics->divisors[i]);
-               tmp_diff = abs(tmp_rate - req->rate);
-
-               if (!best_rate || best_diff > tmp_diff) {
-                       best_diff = tmp_diff;
-                       best_rate = tmp_rate;
-               }
-
-               if (!best_diff)
-                       break;
-       }
-
-       req->best_parent_rate = best_rate;
-       req->best_parent_hw = parent;
-       req->rate = best_rate;
-
-       return 0;
+       return DIV_ROUND_CLOSEST_ULL(parent_rate, master->div);
 }
 
 static void clk_master_div_restore_context_chg(struct clk_hw *hw)
 {
        struct clk_master *master = to_clk_master(hw);
+       unsigned long flags;
        int ret;
 
-       ret = clk_master_div_set_rate(hw, master->pms.rate,
-                                     master->pms.parent_rate);
+       spin_lock_irqsave(master->lock, flags);
+       ret = clk_master_div_set(master, master->pms.parent_rate,
+                                DIV_ROUND_CLOSEST(master->pms.parent_rate,
+                                                  master->pms.rate));
+       spin_unlock_irqrestore(master->lock, flags);
        if (ret)
                pr_warn("Failed to restore MCK DIV clock\n");
 }
@@ -261,13 +240,116 @@ static void clk_master_div_restore_context_chg(struct clk_hw *hw)
 static const struct clk_ops master_div_ops_chg = {
        .prepare = clk_master_prepare,
        .is_prepared = clk_master_is_prepared,
-       .recalc_rate = clk_master_div_recalc_rate,
-       .determine_rate = clk_master_div_determine_rate,
-       .set_rate = clk_master_div_set_rate,
+       .recalc_rate = clk_master_div_recalc_rate_chg,
        .save_context = clk_master_div_save_context,
        .restore_context = clk_master_div_restore_context_chg,
 };
 
+static int clk_master_div_notifier_fn(struct notifier_block *notifier,
+                                     unsigned long code, void *data)
+{
+       const struct clk_master_characteristics *characteristics =
+                                               master_div->characteristics;
+       struct clk_notifier_data *cnd = data;
+       unsigned long flags, new_parent_rate, new_rate;
+       unsigned int mckr, div, new_div = 0;
+       int ret, i;
+       long tmp_diff;
+       long best_diff = -1;
+
+       spin_lock_irqsave(master_div->lock, flags);
+       switch (code) {
+       case PRE_RATE_CHANGE:
+               /*
+                * We want to avoid any overclocking of MCK DIV domain. To do
+                * this we set a safe divider (the underclocking is not of
+                * interest as we can go as low as 32KHz). The relation
+                * b/w this clock and its parents are as follows:
+                *
+                * FRAC PLL -> DIV PLL -> MCK DIV
+                *
+                * With the proper safe divider we should be good even with FRAC
+                * PLL at its maximum value.
+                */
+               ret = regmap_read(master_div->regmap, master_div->layout->offset,
+                                 &mckr);
+               if (ret) {
+                       ret = NOTIFY_STOP_MASK;
+                       goto unlock;
+               }
+
+               mckr &= master_div->layout->mask;
+               div = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK;
+
+               /* Switch to safe divider. */
+               clk_master_div_set(master_div,
+                                  cnd->old_rate * characteristics->divisors[div],
+                                  master_div->safe_div);
+               break;
+
+       case POST_RATE_CHANGE:
+               /*
+                * At this point we want to restore MCK DIV domain to its maximum
+                * allowed rate.
+                */
+               ret = regmap_read(master_div->regmap, master_div->layout->offset,
+                                 &mckr);
+               if (ret) {
+                       ret = NOTIFY_STOP_MASK;
+                       goto unlock;
+               }
+
+               mckr &= master_div->layout->mask;
+               div = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK;
+               new_parent_rate = cnd->new_rate * characteristics->divisors[div];
+
+               for (i = 0; i < ARRAY_SIZE(characteristics->divisors); i++) {
+                       if (!characteristics->divisors[i])
+                               break;
+
+                       new_rate = DIV_ROUND_CLOSEST_ULL(new_parent_rate,
+                                                        characteristics->divisors[i]);
+
+                       tmp_diff = characteristics->output.max - new_rate;
+                       if (tmp_diff < 0)
+                               continue;
+
+                       if (best_diff < 0 || best_diff > tmp_diff) {
+                               new_div = characteristics->divisors[i];
+                               best_diff = tmp_diff;
+                       }
+
+                       if (!tmp_diff)
+                               break;
+               }
+
+               if (!new_div) {
+                       ret = NOTIFY_STOP_MASK;
+                       goto unlock;
+               }
+
+               /* Update the div to preserve MCK DIV clock rate. */
+               clk_master_div_set(master_div, new_parent_rate,
+                                  new_div);
+
+               ret = NOTIFY_OK;
+               break;
+
+       default:
+               ret = NOTIFY_DONE;
+               break;
+       }
+
+unlock:
+       spin_unlock_irqrestore(master_div->lock, flags);
+
+       return ret;
+}
+
+static struct notifier_block clk_master_div_notifier = {
+       .notifier_call = clk_master_div_notifier_fn,
+};
+
 static void clk_sama7g5_master_best_diff(struct clk_rate_request *req,
                                         struct clk_hw *parent,
                                         unsigned long parent_rate,
@@ -496,6 +578,8 @@ at91_clk_register_master_internal(struct regmap *regmap,
        struct clk_master *master;
        struct clk_init_data init;
        struct clk_hw *hw;
+       unsigned int mckr;
+       unsigned long irqflags;
        int ret;
 
        if (!name || !num_parents || !parent_names || !lock)
@@ -518,6 +602,16 @@ at91_clk_register_master_internal(struct regmap *regmap,
        master->chg_pid = chg_pid;
        master->lock = lock;
 
+       if (ops == &master_div_ops_chg) {
+               spin_lock_irqsave(master->lock, irqflags);
+               regmap_read(master->regmap, master->layout->offset, &mckr);
+               spin_unlock_irqrestore(master->lock, irqflags);
+
+               mckr &= layout->mask;
+               mckr = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK;
+               master->div = characteristics->divisors[mckr];
+       }
+
        hw = &master->hw;
        ret = clk_hw_register(NULL, &master->hw);
        if (ret) {
@@ -554,19 +648,29 @@ at91_clk_register_master_div(struct regmap *regmap,
                const char *name, const char *parent_name,
                const struct clk_master_layout *layout,
                const struct clk_master_characteristics *characteristics,
-               spinlock_t *lock, u32 flags)
+               spinlock_t *lock, u32 flags, u32 safe_div)
 {
        const struct clk_ops *ops;
+       struct clk_hw *hw;
 
        if (flags & CLK_SET_RATE_GATE)
                ops = &master_div_ops;
        else
                ops = &master_div_ops_chg;
 
-       return at91_clk_register_master_internal(regmap, name, 1,
-                                                &parent_name, layout,
-                                                characteristics, ops,
-                                                lock, flags, -EINVAL);
+       hw = at91_clk_register_master_internal(regmap, name, 1,
+                                              &parent_name, layout,
+                                              characteristics, ops,
+                                              lock, flags, -EINVAL);
+
+       if (!IS_ERR(hw) && safe_div) {
+               master_div = to_clk_master(hw);
+               master_div->safe_div = safe_div;
+               clk_notifier_register(hw->clk,
+                                     &clk_master_div_notifier);
+       }
+
+       return hw;
 }
 
 static unsigned long

diff --git a/drivers/clk/at91/dt-compat.c b/drivers/clk/at91/dt-compat.c
index a97b99c2dc127fd98636a707a81a8a20deb8ca7b..ca2dbb65b9df04c9aba8a1f2193496e92b95ed43 100644 (file)
--- a/drivers/clk/at91/dt-compat.c
+++ b/drivers/clk/at91/dt-compat.c
@@ -399,7 +399,7 @@ of_at91_clk_master_setup(struct device_node *np,
 
        hw = at91_clk_register_master_div(regmap, name, "masterck_pres",
                                          layout, characteristics,
-                                         &mck_lock, CLK_SET_RATE_GATE);
+                                         &mck_lock, CLK_SET_RATE_GATE, 0);
        if (IS_ERR(hw))
                goto out_free_characteristics;
 

diff --git a/drivers/clk/at91/pmc.h b/drivers/clk/at91/pmc.h
index 207ecccef29f30300c5258ce11f76953e4a328d8..3a1bf6194c287d09483640f4756f95f49c074fea 100644 (file)
--- a/drivers/clk/at91/pmc.h
+++ b/drivers/clk/at91/pmc.h
@@ -182,7 +182,7 @@ at91_clk_register_master_div(struct regmap *regmap, const char *name,
                             const char *parent_names,
                             const struct clk_master_layout *layout,
                             const struct clk_master_characteristics *characteristics,
-                            spinlock_t *lock, u32 flags);
+                            spinlock_t *lock, u32 flags, u32 safe_div);
 
 struct clk_hw * __init
 at91_clk_sama7g5_register_master(struct regmap *regmap,

diff --git a/drivers/clk/at91/sama5d2.c b/drivers/clk/at91/sama5d2.c
index 3d1f78176c3e594db643bfbae4ef2331b6c77f4f..d027294a008980b9906fb8a5f787df90b9ddb212 100644 (file)
--- a/drivers/clk/at91/sama5d2.c
+++ b/drivers/clk/at91/sama5d2.c
@@ -249,7 +249,7 @@ static void __init sama5d2_pmc_setup(struct device_node *np)
                                          "masterck_pres",
                                          &at91sam9x5_master_layout,
                                          &mck_characteristics, &mck_lock,
-                                         CLK_SET_RATE_GATE);
+                                         CLK_SET_RATE_GATE, 0);
        if (IS_ERR(hw))
                goto err_free;
 

diff --git a/drivers/clk/at91/sama5d3.c b/drivers/clk/at91/sama5d3.c
index d376257807d26281e1877ec8abed113a9bd23c36..339d0f382ff0d5b62c11c05850b3c9f369752823 100644 (file)
--- a/drivers/clk/at91/sama5d3.c
+++ b/drivers/clk/at91/sama5d3.c
@@ -184,7 +184,7 @@ static void __init sama5d3_pmc_setup(struct device_node *np)
                                          "masterck_pres",
                                          &at91sam9x5_master_layout,
                                          &mck_characteristics, &mck_lock,
-                                         CLK_SET_RATE_GATE);
+                                         CLK_SET_RATE_GATE, 0);
        if (IS_ERR(hw))
                goto err_free;
 

diff --git a/drivers/clk/at91/sama5d4.c b/drivers/clk/at91/sama5d4.c
index 5cbaac68da447edd4a4a1b8f4e9507d446fc1d69..4af75b1e39e980babe0877f026c10acde692448d 100644 (file)
--- a/drivers/clk/at91/sama5d4.c
+++ b/drivers/clk/at91/sama5d4.c
@@ -199,7 +199,7 @@ static void __init sama5d4_pmc_setup(struct device_node *np)
                                          "masterck_pres",
                                          &at91sam9x5_master_layout,
                                          &mck_characteristics, &mck_lock,
-                                         CLK_SET_RATE_GATE);
+                                         CLK_SET_RATE_GATE, 0);
        if (IS_ERR(hw))
                goto err_free;
 

diff --git a/drivers/clk/at91/sama7g5.c b/drivers/clk/at91/sama7g5.c
index ae52c10af0408ebf062e72f3a38a8b22eadcc220..c66bde6f7b472b25ebf9ab4d72c24cf1bc6f081a 100644 (file)
--- a/drivers/clk/at91/sama7g5.c
+++ b/drivers/clk/at91/sama7g5.c
@@ -1003,7 +1003,7 @@ static void __init sama7g5_pmc_setup(struct device_node *np)
 
        hw = at91_clk_register_master_div(regmap, "mck0", "cpuck",
                                          &mck0_layout, &mck0_characteristics,
-                                         &pmc_mck0_lock, 0);
+                                         &pmc_mck0_lock, CLK_GET_RATE_NOCACHE, 5);
        if (IS_ERR(hw))
                goto err_free;