1 files changed, 60 insertions, 1 deletions

diff --git a/drivers/clk/qcom/apss-ipq6018.c b/drivers/clk/qcom/apss-ipq6018.c
index f2f502e2d..e6295b832 100644
--- a/drivers/clk/qcom/apss-ipq6018.c
+++ b/drivers/clk/qcom/apss-ipq6018.c
@@ -9,8 +9,11 @@
 #include <linux/clk-provider.h>
 #include <linux/regmap.h>
 #include <linux/module.h>
+#include <linux/clk.h>
+#include <linux/soc/qcom/smem.h>
 
 #include <dt-bindings/clock/qcom,apss-ipq.h>
+#include <dt-bindings/arm/qcom,ids.h>
 
 #include "common.h"
 #include "clk-regmap.h"
@@ -20,16 +23,19 @@
 
 enum {
 	P_XO,
+	P_GPLL0,
 	P_APSS_PLL_EARLY,
 };
 
 static const struct clk_parent_data parents_apcs_alias0_clk_src[] = {
 	{ .fw_name = "xo" },
+	{ .fw_name = "gpll0" },
 	{ .fw_name = "pll" },
 };
 
 static const struct parent_map parents_apcs_alias0_clk_src_map[] = {
 	{ P_XO, 0 },
+	{ P_GPLL0, 4 },
 	{ P_APSS_PLL_EARLY, 5 },
 };
 
@@ -81,15 +87,68 @@ static const struct qcom_cc_desc apss_ipq6018_desc = {
 	.num_clks = ARRAY_SIZE(apss_ipq6018_clks),
 };
 
+static int cpu_clk_notifier_fn(struct notifier_block *nb, unsigned long action,
+				void *data)
+{
+	struct clk_hw *hw;
+	u8 index;
+	int err;
+
+	if (action == PRE_RATE_CHANGE)
+		index = P_GPLL0;
+	else if (action == POST_RATE_CHANGE || action == ABORT_RATE_CHANGE)
+		index = P_APSS_PLL_EARLY;
+	else
+		return NOTIFY_OK;
+
+	hw = &apcs_alias0_clk_src.clkr.hw;
+	err = clk_rcg2_mux_closest_ops.set_parent(hw, index);
+
+	return notifier_from_errno(err);
+}
+
 static int apss_ipq6018_probe(struct platform_device *pdev)
 {
+	struct clk_hw *hw = &apcs_alias0_clk_src.clkr.hw;
+	struct notifier_block *cpu_clk_notifier;
 	struct regmap *regmap;
+	u32 soc_id;
+	int ret;
+
+	ret = qcom_smem_get_soc_id(&soc_id);
+	if (ret)
+		return ret;
 
 	regmap = dev_get_regmap(pdev->dev.parent, NULL);
 	if (!regmap)
 		return -ENODEV;
 
-	return qcom_cc_really_probe(pdev, &apss_ipq6018_desc, regmap);
+	ret = qcom_cc_really_probe(pdev, &apss_ipq6018_desc, regmap);
+	if (ret)
+		return ret;
+
+	switch (soc_id) {
+	/* Only below variants of IPQ53xx support scaling */
+	case QCOM_ID_IPQ5332:
+	case QCOM_ID_IPQ5322:
+	case QCOM_ID_IPQ5300:
+		cpu_clk_notifier = devm_kzalloc(&pdev->dev,
+						sizeof(*cpu_clk_notifier),
+						GFP_KERNEL);
+		if (!cpu_clk_notifier)
+			return -ENOMEM;
+
+		cpu_clk_notifier->notifier_call = cpu_clk_notifier_fn;
+
+		ret = devm_clk_notifier_register(&pdev->dev, hw->clk, cpu_clk_notifier);
+		if (ret)
+			return ret;
+		break;
+	default:
+		break;
+	}
+
+	return 0;
 }
 
 static struct platform_driver apss_ipq6018_driver = {