[PATCH 4/7] clk: imx: add imx_hw_clk_cpuv2 for i.MX7ULP

[peng . fan]

From: Peng Fan <peng.fan@xxxxxxx>

Add a clk api for i.MX7ULP ARM core clk usage.
imx_hw_clk_cpu could not be reused, because i.MX7ULP ARM core
clk has totally different design. To simplify ARM core clk
change logic, add a new clk api.

A draft picture to show the ARM core clock.
                                                      |-sirc
     |->   run(500MHz)    ->  div -> mux -------------|-firc
  ARM|                                                |
     |->   hsrun(720MHz)  ->  hs div -> hs mux -------|-spll pfd
                                                      |-....

Need to configure PMC when ARM core runs in HSRUN or RUN mode.

RUN and HSRUN related registers are not same, but their
mux has same clocks as input.

The API takes arm core, div, hs div, mux, hs mux, mux parent, pfd, step
as params for switch clk freq.

When set rate, need to switch mux to take firc as input, then
set spll pfd freq, then switch back mux to spll pfd as parent.

Per i.MX7ULP requirement, when clk runs in HSRUN mode, it could
only support arm core wfi idle, so add pm qos to support it.

Signed-off-by: Peng Fan <peng.fan@xxxxxxx>
---
 drivers/clk/imx/Makefile    |   1 +
 drivers/clk/imx/clk-cpuv2.c | 137 ++++++++++++++++++++++++++++++++++++++++++++
 drivers/clk/imx/clk.h       |   9 +++
 3 files changed, 147 insertions(+)
 create mode 100644 drivers/clk/imx/clk-cpuv2.c

diff --git a/drivers/clk/imx/Makefile b/drivers/clk/imx/Makefile
index 928f874c73d2..9707fef8da98 100644
--- a/drivers/clk/imx/Makefile
+++ b/drivers/clk/imx/Makefile
@@ -5,6 +5,7 @@ obj-$(CONFIG_MXC_CLK) += \
 	clk-busy.o \
 	clk-composite-8m.o \
 	clk-cpu.o \
+	clk-cpuv2.o \
 	clk-composite-7ulp.o \
 	clk-divider-gate.o \
 	clk-fixup-div.o \
diff --git a/drivers/clk/imx/clk-cpuv2.c b/drivers/clk/imx/clk-cpuv2.c
new file mode 100644
index 000000000000..a73d97a782aa
--- /dev/null
+++ b/drivers/clk/imx/clk-cpuv2.c
@@ -0,0 +1,137 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright 2020 NXP
+ *
+ * Peng Fan <peng.fan@xxxxxxx>
+ */
+
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/slab.h>
+#include <linux/pm_qos.h>
+#include "clk.h"
+
+static struct pm_qos_request pm_qos_hsrun;
+
+#define MAX_NORMAL_RUN_FREQ	528000000
+
+struct clk_cpu {
+	struct clk_hw	hw;
+	struct clk_hw	*core;
+	struct clk_hw	*div_nor;
+	struct clk_hw	*div_hs;
+	struct clk_hw	*mux_nor;
+	struct clk_hw	*mux_hs;
+	struct clk_hw	*mux_parent;
+	struct clk_hw	*pfd;
+	struct clk_hw	*step;
+};
+
+static inline struct clk_cpu *to_clk_cpu(struct clk_hw *hw)
+{
+	return container_of(hw, struct clk_cpu, hw);
+}
+
+static unsigned long clk_cpu_recalc_rate(struct clk_hw *hw,
+					 unsigned long parent_rate)
+{
+	struct clk_cpu *cpu = to_clk_cpu(hw);
+
+	return clk_hw_get_rate(cpu->core);
+}
+
+static long clk_cpu_round_rate(struct clk_hw *hw, unsigned long rate,
+			       unsigned long *prate)
+{
+	return rate;
+}
+
+static int clk_cpu_set_rate(struct clk_hw *hw, unsigned long rate,
+			    unsigned long parent_rate)
+{
+	struct clk_cpu *cpu = to_clk_cpu(hw);
+	int ret;
+	struct clk_hw *div, *mux_now;
+	unsigned long old_rate = clk_hw_get_rate(cpu->core);
+
+	div = clk_hw_get_parent(cpu->core);
+
+	if (div == cpu->div_nor)
+		mux_now = cpu->mux_nor;
+	else
+		mux_now = cpu->mux_hs;
+
+	ret = clk_hw_set_parent(mux_now, cpu->step);
+	if (ret)
+		return ret;
+
+	ret = clk_set_rate(cpu->pfd->clk, rate);
+	if (ret) {
+		clk_hw_set_parent(mux_now, cpu->mux_parent);
+		return ret;
+	}
+
+	if (rate > MAX_NORMAL_RUN_FREQ) {
+		pm_qos_add_request(&pm_qos_hsrun, PM_QOS_CPU_DMA_LATENCY, 0);
+		clk_hw_set_parent(cpu->mux_hs, cpu->mux_parent);
+		clk_hw_set_parent(cpu->core, cpu->div_hs);
+	} else {
+		clk_hw_set_parent(cpu->mux_nor, cpu->mux_parent);
+		clk_hw_set_parent(cpu->core, cpu->div_nor);
+		if (old_rate > MAX_NORMAL_RUN_FREQ)
+			pm_qos_remove_request(&pm_qos_hsrun);
+	}
+
+	return 0;
+}
+
+static const struct clk_ops clk_cpu_ops = {
+	.recalc_rate	= clk_cpu_recalc_rate,
+	.round_rate	= clk_cpu_round_rate,
+	.set_rate	= clk_cpu_set_rate,
+};
+
+struct clk_hw *imx_clk_hw_cpuv2(const char *name, const char *parent_names,
+				struct clk_hw *core,
+				struct clk_hw *div_nor, struct clk_hw *div_hs,
+				struct clk_hw *mux_nor, struct clk_hw *mux_hs,
+				struct clk_hw *mux_parent,
+				struct clk_hw *pfd, struct clk_hw *step,
+				unsigned long flags,
+				unsigned long plat_flags)
+{
+	struct clk_cpu *cpu;
+	struct clk_hw *hw;
+	struct clk_init_data init;
+	int ret;
+
+	cpu = kzalloc(sizeof(*cpu), GFP_KERNEL);
+	if (!cpu)
+		return ERR_PTR(-ENOMEM);
+
+	cpu->core = core;
+	cpu->div_nor = div_nor;
+	cpu->div_hs = div_hs;
+	cpu->mux_nor = mux_nor;
+	cpu->mux_hs = mux_hs;
+	cpu->mux_parent = mux_parent;
+	cpu->pfd = pfd;
+	cpu->step = step;
+
+	init.name = name;
+	init.ops = &clk_cpu_ops;
+	init.flags = flags;
+	init.parent_names = &parent_names;
+	init.num_parents = 1;
+
+	cpu->hw.init = &init;
+	hw = &cpu->hw;
+
+	ret = clk_hw_register(NULL, hw);
+	if (ret) {
+		kfree(cpu);
+		return ERR_PTR(ret);
+	}
+
+	return hw;
+}
diff --git a/drivers/clk/imx/clk.h b/drivers/clk/imx/clk.h
index f074dd8ec42e..7deaba2e525c 100644
--- a/drivers/clk/imx/clk.h
+++ b/drivers/clk/imx/clk.h
@@ -521,4 +521,13 @@ struct clk_hw *imx_clk_hw_divider_gate(const char *name, const char *parent_name
 		unsigned long flags, void __iomem *reg, u8 shift, u8 width,
 		u8 clk_divider_flags, const struct clk_div_table *table,
 		spinlock_t *lock);
+
+struct clk_hw *imx_clk_hw_cpuv2(const char *name, const char *parent_names,
+				struct clk_hw *core,
+				struct clk_hw *div_nor, struct clk_hw *div_hs,
+				struct clk_hw *mux_nor, struct clk_hw *mux_hs,
+				struct clk_hw *mux_parent,
+				struct clk_hw *pfd, struct clk_hw *step,
+				unsigned long flags,
+				unsigned long plat_flags);
 #endif
-- 
2.16.4