Add data fields and ops to support voting for gate.
Signed-off-by: Guangjie Song <guangjie.song@xxxxxxxxxxxx>
---
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
