[PATCH v5 3/5] clk: hisilicon: Add complex clock for Hi3798

[David Yang <mmyangfl@gmail.com>]

Complex clock allows manipulating multiple bits simultaneously.

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

diff --git a/drivers/clk/hisilicon/crg-hi3798.c b/drivers/clk/hisilicon/crg-hi3798.c
index 2f8f14e73..0b29c01c6 100644
--- a/drivers/clk/hisilicon/crg-hi3798.c
+++ b/drivers/clk/hisilicon/crg-hi3798.c
@@ -7,9 +7,11 @@
 
 #include <dt-bindings/clock/histb-clock.h>
 #include <linux/clk-provider.h>
+#include <linux/clkdev.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 +61,131 @@ 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(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 *clk;
+		struct clk_init_data init;
+
+		p_clk = kzalloc(sizeof(*p_clk), GFP_KERNEL);
+		if (!p_clk) {
+			ret = -ENOMEM;
+			goto err_kzalloc;
+		}
+
+		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;
+
+		clk = clk_register(NULL, &p_clk->hw);
+		if (IS_ERR(clk)) {
+			kfree(p_clk);
+err_kzalloc:
+			pr_err("%s: failed to register clock %s\n",
+			       __func__, clks[i].name);
+			goto err;
+		}
+
+		if (clks[i].alias)
+			clk_register_clkdev(clk, clks[i].alias, NULL);
+
+		data->clk_data.clks[clks[i].id] = clk;
+	}
+
+	return 0;
+
+err:
+	while (i--)
+		clk_unregister(data->clk_data.clks[clks[i].id]);
+
+	return ret;
+}
+
+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 +193,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 *hi3798_clk_register(
@@ -98,13 +227,19 @@ static struct hisi_clock_data *hi3798_clk_register(
 	if (ret)
 		goto unregister_mux;
 
+	ret = hi3798_clk_register_complex(clks->complex_clks, clks->complex_clks_nums, clk_data);
+	if (ret)
+		goto unregister_gate;
+
 	ret = of_clk_add_provider(pdev->dev.of_node,
 			of_clk_src_onecell_get, &clk_data->clk_data);
 	if (ret)
-		goto unregister_gate;
+		goto unregister_complex;
 
 	return clk_data;
 
+unregister_complex:
+	hi3798_clk_unregister_complex(clks->complex_clks, clks->complex_clks_nums, clk_data);
 unregister_gate:
 	hisi_clk_unregister_gate(clks->gate_clks, clks->gate_clks_nums, clk_data);
 unregister_mux:
@@ -123,6 +258,7 @@ static void hi3798_clk_unregister(
 
 	of_clk_del_provider(pdev->dev.of_node);
 
+	hi3798_clk_unregister_complex(clks->complex_clks, clks->complex_clks_nums, crg->clk_data);
 	hisi_clk_unregister_gate(clks->gate_clks, clks->gate_clks_nums, crg->clk_data);
 	hisi_clk_unregister_mux(clks->mux_clks, clks->mux_clks_nums, crg->clk_data);
 	hisi_clk_unregister_fixed_rate(hi3798_fixed_rate_clks,
-- 
2.39.2