[PATCH 04/26] clk: mediatek: Support voting for gate

[Guangjie Song <guangjie.song@mediatek.com>]

Add data fields and ops to support voting for gate.

Signed-off-by: Guangjie Song <guangjie.song@mediatek.com>
---
 drivers/clk/mediatek/clk-gate.c | 230 +++++++++++++++++++++++++++++++-
 drivers/clk/mediatek/clk-gate.h |   5 +
 2 files changed, 228 insertions(+), 7 deletions(-)

diff --git a/drivers/clk/mediatek/clk-gate.c b/drivers/clk/mediatek/clk-gate.c
index 67d9e741c5e7..be6cf4a6d246 100644
--- a/drivers/clk/mediatek/clk-gate.c
+++ b/drivers/clk/mediatek/clk-gate.c
@@ -12,14 +12,19 @@
 #include <linux/slab.h>
 #include <linux/types.h>
 
+#include "clk-mtk.h"
 #include "clk-gate.h"
 
 struct mtk_clk_gate {
 	struct clk_hw	hw;
 	struct regmap	*regmap;
+	struct regmap	*vote_regmap;
 	int		set_ofs;
 	int		clr_ofs;
 	int		sta_ofs;
+	int		vote_set_ofs;
+	int		vote_clr_ofs;
+	int		vote_sta_ofs;
 	u8		bit;
 };
 
@@ -100,6 +105,143 @@ static void mtk_cg_disable_inv(struct clk_hw *hw)
 	mtk_cg_clr_bit(hw);
 }
 
+static int mtk_cg_vote_is_set(struct clk_hw *hw)
+{
+	struct mtk_clk_gate *cg = to_mtk_clk_gate(hw);
+	u32 val = 0;
+
+	regmap_read(cg->vote_regmap, cg->vote_set_ofs, &val);
+
+	val &= BIT(cg->bit);
+
+	return val != 0;
+}
+
+static int mtk_cg_vote_is_done(struct clk_hw *hw)
+{
+	struct mtk_clk_gate *cg = to_mtk_clk_gate(hw);
+	u32 val = 0;
+
+	regmap_read(cg->vote_regmap, cg->vote_sta_ofs, &val);
+
+	val &= BIT(cg->bit);
+
+	return val != 0;
+}
+
+static int __cg_vote_enable(struct clk_hw *hw, bool inv)
+{
+	struct mtk_clk_gate *cg = to_mtk_clk_gate(hw);
+	u32 val = 0, val2 = 0;
+	bool is_done = false;
+	int i = 0;
+
+	regmap_write(cg->vote_regmap, cg->vote_set_ofs, BIT(cg->bit));
+
+	while (!mtk_cg_vote_is_set(hw)) {
+		if (i < MTK_WAIT_VOTE_PREPARE_CNT) {
+			udelay(MTK_WAIT_VOTE_PREPARE_US);
+		} else {
+			pr_err("%s cg prepare timeout\n", clk_hw_get_name(hw));
+			return -EBUSY;
+		}
+
+		i++;
+	}
+
+	i = 0;
+
+	while (1) {
+		if (!is_done)
+			regmap_read(cg->vote_regmap, cg->vote_sta_ofs, &val);
+
+		if ((val & BIT(cg->bit)) != 0)
+			is_done = true;
+
+		if (is_done) {
+			regmap_read(cg->regmap, cg->sta_ofs, &val2);
+			if ((inv && (val2 & BIT(cg->bit)) != 0) ||
+			    (!inv && (val2 & BIT(cg->bit)) == 0))
+				break;
+		}
+
+		if (i < MTK_WAIT_VOTE_DONE_CNT) {
+			udelay(MTK_WAIT_VOTE_DONE_US);
+		} else {
+			pr_err("%s cg enable timeout(%x %x)\n", clk_hw_get_name(hw), val, val2);
+
+			if (inv)
+				regmap_write(cg->regmap, cg->set_ofs, BIT(cg->bit));
+			else
+				regmap_write(cg->regmap, cg->clr_ofs, BIT(cg->bit));
+
+			return -EBUSY;
+		}
+
+		i++;
+	}
+
+	return 0;
+}
+
+static int mtk_cg_vote_enable(struct clk_hw *hw)
+{
+	return __cg_vote_enable(hw, false);
+}
+
+static int mtk_cg_vote_enable_inv(struct clk_hw *hw)
+{
+	return __cg_vote_enable(hw, true);
+}
+
+static void mtk_cg_vote_disable(struct clk_hw *hw)
+{
+	struct mtk_clk_gate *cg = to_mtk_clk_gate(hw);
+	u32 val;
+	int i = 0;
+
+	/* dummy read to clr idle signal of hw voter bus */
+	regmap_read(cg->vote_regmap, cg->vote_clr_ofs, &val);
+
+	regmap_write(cg->vote_regmap, cg->vote_clr_ofs, BIT(cg->bit));
+
+	while (mtk_cg_vote_is_set(hw)) {
+		if (i < MTK_WAIT_VOTE_PREPARE_CNT) {
+			udelay(MTK_WAIT_VOTE_PREPARE_US);
+		} else {
+			pr_err("%s cg unprepare timeout\n", clk_hw_get_name(hw));
+			return;
+		}
+
+		i++;
+	}
+
+	i = 0;
+
+	while (!mtk_cg_vote_is_done(hw)) {
+		if (i < MTK_WAIT_VOTE_DONE_CNT) {
+			udelay(MTK_WAIT_VOTE_DONE_US);
+		} else {
+			pr_err("%s cg disable timeout\n", clk_hw_get_name(hw));
+			return;
+		}
+
+		i++;
+	}
+}
+
+static void mtk_cg_vote_disable_unused(struct clk_hw *hw)
+{
+	mtk_cg_vote_enable(hw);
+	mtk_cg_vote_disable(hw);
+}
+
+static void mtk_cg_vote_disable_unused_inv(struct clk_hw *hw)
+{
+	mtk_cg_vote_enable_inv(hw);
+	mtk_cg_vote_disable(hw);
+}
+
 static int mtk_cg_enable_no_setclr(struct clk_hw *hw)
 {
 	mtk_cg_clr_bit_no_setclr(hw);
@@ -138,6 +280,22 @@ const struct clk_ops mtk_clk_gate_ops_setclr_inv = {
 };
 EXPORT_SYMBOL_GPL(mtk_clk_gate_ops_setclr_inv);
 
+const struct clk_ops mtk_clk_gate_ops_vote = {
+	.is_enabled	= mtk_cg_bit_is_cleared,
+	.enable		= mtk_cg_vote_enable,
+	.disable	= mtk_cg_vote_disable,
+	.disable_unused	= mtk_cg_vote_disable_unused,
+};
+EXPORT_SYMBOL_GPL(mtk_clk_gate_ops_vote);
+
+const struct clk_ops mtk_clk_gate_ops_vote_inv = {
+	.is_enabled	= mtk_cg_bit_is_set,
+	.enable		= mtk_cg_vote_enable_inv,
+	.disable	= mtk_cg_vote_disable,
+	.disable_unused	= mtk_cg_vote_disable_unused_inv,
+};
+EXPORT_SYMBOL_GPL(mtk_clk_gate_ops_vote_inv);
+
 const struct clk_ops mtk_clk_gate_ops_no_setclr = {
 	.is_enabled	= mtk_cg_bit_is_cleared,
 	.enable		= mtk_cg_enable_no_setclr,
@@ -190,6 +348,53 @@ static struct clk_hw *mtk_clk_register_gate(struct device *dev, const char *name
 	return &cg->hw;
 }
 
+static struct clk_hw *mtk_clk_register_gate_vote(struct device *dev,
+						 const struct mtk_gate *gate,
+						 struct regmap *regmap,
+						 struct regmap *vote_regmap)
+{
+	struct mtk_clk_gate *cg;
+	int ret;
+	struct clk_init_data init = {};
+
+	cg = kzalloc(sizeof(*cg), GFP_KERNEL);
+	if (!cg)
+		return ERR_PTR(-ENOMEM);
+
+	init.name = gate->name;
+	init.flags = gate->flags | CLK_SET_RATE_PARENT;
+	init.parent_names = gate->parent_name ? &gate->parent_name : NULL;
+	init.num_parents = gate->parent_name ? 1 : 0;
+	if (vote_regmap)
+		init.ops = gate->ops;
+	else
+		init.ops = gate->dma_ops;
+
+	cg->regmap = regmap;
+	cg->vote_regmap = vote_regmap;
+	if (gate->regs) {
+		cg->set_ofs = gate->regs->set_ofs;
+		cg->clr_ofs = gate->regs->clr_ofs;
+		cg->sta_ofs = gate->regs->sta_ofs;
+	}
+	if (gate->vote_regs) {
+		cg->vote_set_ofs = gate->vote_regs->set_ofs;
+		cg->vote_clr_ofs = gate->vote_regs->clr_ofs;
+		cg->vote_sta_ofs = gate->vote_regs->sta_ofs;
+	}
+	cg->bit = gate->shift;
+
+	cg->hw.init = &init;
+
+	ret = clk_hw_register(dev, &cg->hw);
+	if (ret) {
+		kfree(cg);
+		return ERR_PTR(ret);
+	}
+
+	return &cg->hw;
+}
+
 static void mtk_clk_unregister_gate(struct clk_hw *hw)
 {
 	struct mtk_clk_gate *cg;
@@ -209,6 +414,7 @@ int mtk_clk_register_gates(struct device *dev, struct device_node *node,
 	int i;
 	struct clk_hw *hw;
 	struct regmap *regmap;
+	struct regmap  *vote_regmap = NULL;
 
 	if (!clk_data)
 		return -ENOMEM;
@@ -228,13 +434,23 @@ int mtk_clk_register_gates(struct device *dev, struct device_node *node,
 			continue;
 		}
 
-		hw = mtk_clk_register_gate(dev, gate->name, gate->parent_name,
-					    regmap,
-					    gate->regs->set_ofs,
-					    gate->regs->clr_ofs,
-					    gate->regs->sta_ofs,
-					    gate->shift, gate->ops,
-					    gate->flags);
+		if (gate->flags & CLK_USE_VOTE) {
+			if (gate->vote_comp) {
+				vote_regmap = syscon_regmap_lookup_by_phandle(node, gate->vote_comp);
+				if (IS_ERR(vote_regmap))
+					vote_regmap = NULL;
+			}
+
+			hw = mtk_clk_register_gate_vote(dev, gate, regmap, vote_regmap);
+		} else {
+			hw = mtk_clk_register_gate(dev, gate->name, gate->parent_name,
+						   regmap,
+						   gate->regs->set_ofs,
+						   gate->regs->clr_ofs,
+						   gate->regs->sta_ofs,
+						   gate->shift, gate->ops,
+						   gate->flags);
+		}
 
 		if (IS_ERR(hw)) {
 			pr_err("Failed to register clk %s: %pe\n", gate->name,
diff --git a/drivers/clk/mediatek/clk-gate.h b/drivers/clk/mediatek/clk-gate.h
index 1a46b4c56fc5..7d93fc84ad51 100644
--- a/drivers/clk/mediatek/clk-gate.h
+++ b/drivers/clk/mediatek/clk-gate.h
@@ -19,6 +19,8 @@ extern const struct clk_ops mtk_clk_gate_ops_setclr;
 extern const struct clk_ops mtk_clk_gate_ops_setclr_inv;
 extern const struct clk_ops mtk_clk_gate_ops_no_setclr;
 extern const struct clk_ops mtk_clk_gate_ops_no_setclr_inv;
+extern const struct clk_ops mtk_clk_gate_ops_vote;
+extern const struct clk_ops mtk_clk_gate_ops_vote_inv;
 
 struct mtk_gate_regs {
 	u32 sta_ofs;
@@ -30,9 +32,12 @@ struct mtk_gate {
 	int id;
 	const char *name;
 	const char *parent_name;
+	const char *vote_comp;
 	const struct mtk_gate_regs *regs;
+	const struct mtk_gate_regs *vote_regs;
 	int shift;
 	const struct clk_ops *ops;
+	const struct clk_ops *dma_ops;
 	unsigned long flags;
 };
 
-- 
2.45.2