3 files changed, 61 insertions, 0 deletions

diff --git a/drivers/clk/qcom/clk-krait.c b/drivers/clk/qcom/clk-krait.c
index 7ede744c6a14..59f1af415b58 100644
--- a/drivers/clk/qcom/clk-krait.c
+++ b/drivers/clk/qcom/clk-krait.c
@@ -50,6 +50,8 @@ static int krait_mux_set_parent(struct clk_hw *hw, u8 index)
         if (__clk_is_enabled(hw->clk))
                 __krait_mux_set_sel(mux, sel);
 
+        mux->reparent = true;
+
         return 0;
 }
 
diff --git a/drivers/clk/qcom/clk-krait.h b/drivers/clk/qcom/clk-krait.h
index 441ba1e18b81..9120bd2f5297 100644
--- a/drivers/clk/qcom/clk-krait.h
+++ b/drivers/clk/qcom/clk-krait.h
@@ -12,6 +12,9 @@ struct krait_mux_clk {
         u32             shift;
         u32             en_mask;
         bool            lpl;
+        u8              safe_sel;
+        u8              old_index;
+        bool            reparent;
 
         struct clk_hw   hw;
         struct notifier_block   clk_nb;
diff --git a/drivers/clk/qcom/krait-cc.c b/drivers/clk/qcom/krait-cc.c
index 7c9dfb032065..4d4b657d33c3 100644
--- a/drivers/clk/qcom/krait-cc.c
+++ b/drivers/clk/qcom/krait-cc.c
@@ -26,6 +26,49 @@ static unsigned int pri_mux_map[] = {
         0,
 };
 
+/*
+ * Notifier function for switching the muxes to safe parent
+ * while the hfpll is getting reprogrammed.
+ */
+static int krait_notifier_cb(struct notifier_block *nb,
+                             unsigned long event,
+                             void *data)
+{
+        int ret = 0;
+        struct krait_mux_clk *mux = container_of(nb, struct krait_mux_clk,
+                                                 clk_nb);
+        /* Switch to safe parent */
+        if (event == PRE_RATE_CHANGE) {
+                mux->old_index = krait_mux_clk_ops.get_parent(&mux->hw);
+                ret = krait_mux_clk_ops.set_parent(&mux->hw, mux->safe_sel);
+                mux->reparent = false;
+        /*
+         * By the time POST_RATE_CHANGE notifier is called,
+         * clk framework itself would have changed the parent for the new rate.
+         * Only otherwise, put back to the old parent.
+         */
+        } else if (event == POST_RATE_CHANGE) {
+                if (!mux->reparent)
+                        ret = krait_mux_clk_ops.set_parent(&mux->hw,
+                                                           mux->old_index);
+        }
+
+        return notifier_from_errno(ret);
+}
+
+static int krait_notifier_register(struct device *dev, struct clk *clk,
+                                   struct krait_mux_clk *mux)
+{
+        int ret = 0;
+
+        mux->clk_nb.notifier_call = krait_notifier_cb;
+        ret = clk_notifier_register(clk, &mux->clk_nb);
+        if (ret)
+                dev_err(dev, "failed to register clock notifier: %d\n", ret);
+
+        return ret;
+}
+
 static int
 krait_add_div(struct device *dev, int id, const char *s, unsigned int offset)
 {
@@ -70,6 +113,7 @@ static int
 krait_add_sec_mux(struct device *dev, int id, const char *s,
                   unsigned int offset, bool unique_aux)
 {
+        int ret;
         struct krait_mux_clk *mux;
         static const char *sec_mux_list[] = {
                 "acpu_aux",
@@ -93,6 +137,7 @@ krait_add_sec_mux(struct device *dev, int id, const char *s,
         mux->shift = 2;
         mux->parent_map = sec_mux_map;
         mux->hw.init = &init;
+        mux->safe_sel = 0;
 
         init.name = kasprintf(GFP_KERNEL, "krait%s_sec_mux", s);
         if (!init.name)
@@ -108,6 +153,11 @@ krait_add_sec_mux(struct device *dev, int id, const char *s,
 
         clk = devm_clk_register(dev, &mux->hw);
 
+        ret = krait_notifier_register(dev, clk, mux);
+        if (ret)
+                goto unique_aux;
+
+unique_aux:
         if (unique_aux)
                 kfree(sec_mux_list[0]);
 err_aux:
@@ -119,6 +169,7 @@ static struct clk *
 krait_add_pri_mux(struct device *dev, int id, const char *s,
                   unsigned int offset)
 {
+        int ret;
         struct krait_mux_clk *mux;
         const char *p_names[3];
         struct clk_init_data init = {
@@ -139,6 +190,7 @@ krait_add_pri_mux(struct device *dev, int id, const char *s,
         mux->lpl = id >= 0;
         mux->parent_map = pri_mux_map;
         mux->hw.init = &init;
+        mux->safe_sel = 2;
 
         init.name = kasprintf(GFP_KERNEL, "krait%s_pri_mux", s);
         if (!init.name)
@@ -164,6 +216,10 @@ krait_add_pri_mux(struct device *dev, int id, const char *s,
 
         clk = devm_clk_register(dev, &mux->hw);
 
+        ret = krait_notifier_register(dev, clk, mux);
+        if (ret)
+                goto err_p3;
+err_p3:
         kfree(p_names[2]);
 err_p2:
         kfree(p_names[1]);