[PATCH v6 4/6] clk: hisilicon: Add complex clock for Hi3798

[David Yang]

Complex clock allows manipulating multiple bits simultaneously.

Signed-off-by: David Yang <mmyangfl@xxxxxxxxx>
---
 drivers/clk/hisilicon/crg-hi3798.c | 126 +++++++++++++++++++++++++++++
 1 file changed, 126 insertions(+)

diff --git a/drivers/clk/hisilicon/crg-hi3798.c b/drivers/clk/hisilicon/crg-hi3798.c
index d05151d0e..78915bd26 100644
--- a/drivers/clk/hisilicon/crg-hi3798.c
+++ b/drivers/clk/hisilicon/crg-hi3798.c
@@ -7,9 +7,13 @@
 
 #include <dt-bindings/clock/histb-clock.h>
 #include <linux/clk-provider.h>
+#include <linux/clkdev.h>
+#include <linux/device.h>
+#include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/of_device.h>
 #include <linux/platform_device.h>
+#include <linux/slab.h>
 #include "clk.h"
 #include "crg.h"
 #include "reset.h"
@@ -59,6 +63,121 @@ static const struct hisi_fixed_rate_clock hi3798_fixed_rate_clks[] = {
 	{ HI3798_FIXED_250M, "250m", NULL, 0, 250000000, },
 };
 
+struct hi3798_complex_clock {
+	unsigned int	id;
+	const char	*name;
+	const char	*parent_name;
+	unsigned long	flags;
+	unsigned long	offset;
+	u32		mask;
+	u32		value;
+	const char	*alias;
+};
+
+struct hi3798_clk_complex {
+	struct clk_hw	hw;
+	void __iomem	*reg;
+	u32		mask;
+	u32		value;
+};
+
+#define to_complex_clk(_hw) container_of(_hw, struct hi3798_clk_complex, hw)
+
+static int hi3798_clk_complex_prepare(struct clk_hw *hw)
+{
+	struct hi3798_clk_complex *clk = to_complex_clk(hw);
+	u32 val;
+
+	val = readl_relaxed(clk->reg);
+	val &= ~(clk->mask);
+	val |= clk->value;
+	writel_relaxed(val, clk->reg);
+
+	return 0;
+}
+
+static void hi3798_clk_complex_unprepare(struct clk_hw *hw)
+{
+	struct hi3798_clk_complex *clk = to_complex_clk(hw);
+	u32 val;
+
+	val = readl_relaxed(clk->reg);
+	val &= ~(clk->mask);
+	writel_relaxed(val, clk->reg);
+}
+
+static int hi3798_clk_complex_is_prepared(struct clk_hw *hw)
+{
+	struct hi3798_clk_complex *clk = to_complex_clk(hw);
+	u32 val;
+
+	val = readl_relaxed(clk->reg);
+	return (val & clk->mask) == clk->value;
+}
+
+static const struct clk_ops hi3798_clk_complex_ops = {
+	.prepare = hi3798_clk_complex_prepare,
+	.unprepare = hi3798_clk_complex_unprepare,
+	.is_prepared = hi3798_clk_complex_is_prepared,
+};
+
+static int hi3798_clk_register_complex(struct device *dev,
+				       const struct hi3798_complex_clock *clks, int nums,
+				       struct hisi_clock_data *data)
+{
+	void __iomem *base = data->base;
+	int i;
+	int ret;
+
+	for (i = 0; i < nums; i++) {
+		struct hi3798_clk_complex *p_clk;
+		struct clk_init_data init;
+
+		p_clk = devm_kzalloc(dev, sizeof(*p_clk), GFP_KERNEL);
+		if (!p_clk)
+			return -ENOMEM;
+
+		init.name = clks[i].name;
+		init.ops = &hi3798_clk_complex_ops;
+
+		init.flags = 0;
+		init.parent_names =
+			(clks[i].parent_name ? &clks[i].parent_name : NULL);
+		init.num_parents = (clks[i].parent_name ? 1 : 0);
+
+		p_clk->reg = base + clks[i].offset;
+		p_clk->mask = clks[i].mask;
+		p_clk->value = clks[i].value;
+		p_clk->hw.init = &init;
+
+		ret = devm_clk_hw_register(dev, &p_clk->hw);
+		if (ret) {
+			pr_err("%s: failed to register clock %s\n",
+			       __func__, clks[i].name);
+			return ret;
+		}
+
+		if (clks[i].alias)
+			clk_register_clkdev(clk, clks[i].alias, NULL);
+
+		data->clk_data->hws[clks[i].id] = &p_clk->hw;
+	}
+
+	return 0;
+}
+
+static void hi3798_clk_unregister_complex(const struct hi3798_complex_clock *clks, int nums,
+					  struct hisi_clock_data *data)
+{
+	struct clk **clocks = data->clk_data.clks;
+	int i;
+
+	for (i = 0; i < nums; i++) {
+		if (clocks[clks[i].id])
+			clk_unregister(clocks[clks[i].id]);
+	}
+}
+
 struct hi3798_clks {
 	const struct hisi_gate_clock *gate_clks;
 	int gate_clks_nums;
@@ -66,6 +185,8 @@ struct hi3798_clks {
 	int mux_clks_nums;
 	const struct hisi_phase_clock *phase_clks;
 	int phase_clks_nums;
+	const struct hi3798_complex_clock *complex_clks;
+	int complex_clks_nums;
 };
 
 static struct hisi_clock_data *
@@ -100,6 +221,11 @@ hi3798_clk_register(struct platform_device *pdev,
 	if (ret)
 		return ERR_PTR(ret);
 
+	ret = hi3798_clk_register_complex(dev, clks->complex_clks,
+					  clks->complex_clks_nums, clk_data);
+	if (ret)
+		return ERR_PTR(ret);
+
 	ret = devm_of_clk_add_hw_provider(dev, of_clk_hw_onecell_get,
 					  clk_data->clk_data);
 	if (ret)
-- 
2.39.2