From a5969cabbb4660eab42b6ef0412cbbd1200cf14d Mon Sep 17 00:00:00 2001
From: hc <hc@nodka.com>
Date: Sat, 12 Oct 2024 07:10:09 +0000
Subject: [PATCH] 修改led为gpio
---
kernel/drivers/clk/qcom/clk-rcg2.c | 313 ++++++++++++++++++++++++++++++++++++++++++++++++++--
1 files changed, 300 insertions(+), 13 deletions(-)
diff --git a/kernel/drivers/clk/qcom/clk-rcg2.c b/kernel/drivers/clk/qcom/clk-rcg2.c
index f420f0c..71a0d30 100644
--- a/kernel/drivers/clk/qcom/clk-rcg2.c
+++ b/kernel/drivers/clk/qcom/clk-rcg2.c
@@ -10,8 +10,10 @@
#include <linux/export.h>
#include <linux/clk-provider.h>
#include <linux/delay.h>
+#include <linux/rational.h>
#include <linux/regmap.h>
#include <linux/math64.h>
+#include <linux/slab.h>
#include <asm/div64.h>
@@ -40,6 +42,19 @@
#define N_REG 0xc
#define D_REG 0x10
+#define RCG_CFG_OFFSET(rcg) ((rcg)->cmd_rcgr + (rcg)->cfg_off + CFG_REG)
+#define RCG_M_OFFSET(rcg) ((rcg)->cmd_rcgr + (rcg)->cfg_off + M_REG)
+#define RCG_N_OFFSET(rcg) ((rcg)->cmd_rcgr + (rcg)->cfg_off + N_REG)
+#define RCG_D_OFFSET(rcg) ((rcg)->cmd_rcgr + (rcg)->cfg_off + D_REG)
+
+/* Dynamic Frequency Scaling */
+#define MAX_PERF_LEVEL 8
+#define SE_CMD_DFSR_OFFSET 0x14
+#define SE_CMD_DFS_EN BIT(0)
+#define SE_PERF_DFSR(level) (0x1c + 0x4 * (level))
+#define SE_PERF_M_DFSR(level) (0x5c + 0x4 * (level))
+#define SE_PERF_N_DFSR(level) (0x9c + 0x4 * (level))
+
enum freq_policy {
FLOOR,
CEIL,
@@ -65,7 +80,7 @@
u32 cfg;
int i, ret;
- ret = regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &cfg);
+ ret = regmap_read(rcg->clkr.regmap, RCG_CFG_OFFSET(rcg), &cfg);
if (ret)
goto err;
@@ -114,7 +129,7 @@
int ret;
u32 cfg = rcg->parent_map[index].cfg << CFG_SRC_SEL_SHIFT;
- ret = regmap_update_bits(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG,
+ ret = regmap_update_bits(rcg->clkr.regmap, RCG_CFG_OFFSET(rcg),
CFG_SRC_SEL_MASK, cfg);
if (ret)
return ret;
@@ -153,13 +168,13 @@
struct clk_rcg2 *rcg = to_clk_rcg2(hw);
u32 cfg, hid_div, m = 0, n = 0, mode = 0, mask;
- regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &cfg);
+ regmap_read(rcg->clkr.regmap, RCG_CFG_OFFSET(rcg), &cfg);
if (rcg->mnd_width) {
mask = BIT(rcg->mnd_width) - 1;
- regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + M_REG, &m);
+ regmap_read(rcg->clkr.regmap, RCG_M_OFFSET(rcg), &m);
m &= mask;
- regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + N_REG, &n);
+ regmap_read(rcg->clkr.regmap, RCG_N_OFFSET(rcg), &n);
n = ~n;
n &= mask;
n += m;
@@ -192,7 +207,7 @@
break;
default:
return -EINVAL;
- };
+ }
if (!f)
return -EINVAL;
@@ -249,7 +264,7 @@
static int __clk_rcg2_configure(struct clk_rcg2 *rcg, const struct freq_tbl *f)
{
- u32 cfg, mask;
+ u32 cfg, mask, d_val, not2d_val, n_minus_m;
struct clk_hw *hw = &rcg->clkr.hw;
int ret, index = qcom_find_src_index(hw, rcg->parent_map, f->src);
@@ -259,17 +274,26 @@
if (rcg->mnd_width && f->n) {
mask = BIT(rcg->mnd_width) - 1;
ret = regmap_update_bits(rcg->clkr.regmap,
- rcg->cmd_rcgr + M_REG, mask, f->m);
+ RCG_M_OFFSET(rcg), mask, f->m);
if (ret)
return ret;
ret = regmap_update_bits(rcg->clkr.regmap,
- rcg->cmd_rcgr + N_REG, mask, ~(f->n - f->m));
+ RCG_N_OFFSET(rcg), mask, ~(f->n - f->m));
if (ret)
return ret;
+ /* Calculate 2d value */
+ d_val = f->n;
+
+ n_minus_m = f->n - f->m;
+ n_minus_m *= 2;
+
+ d_val = clamp_t(u32, d_val, f->m, n_minus_m);
+ not2d_val = ~d_val & mask;
+
ret = regmap_update_bits(rcg->clkr.regmap,
- rcg->cmd_rcgr + D_REG, mask, ~f->n);
+ RCG_D_OFFSET(rcg), mask, not2d_val);
if (ret)
return ret;
}
@@ -280,8 +304,7 @@
cfg |= rcg->parent_map[index].cfg << CFG_SRC_SEL_SHIFT;
if (rcg->mnd_width && f->n && (f->m != f->n))
cfg |= CFG_MODE_DUAL_EDGE;
-
- return regmap_update_bits(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG,
+ return regmap_update_bits(rcg->clkr.regmap, RCG_CFG_OFFSET(rcg),
mask, cfg);
}
@@ -311,7 +334,7 @@
break;
default:
return -EINVAL;
- };
+ }
if (!f)
return -EINVAL;
@@ -625,6 +648,7 @@
{ 2, 9 },
{ 4, 9 },
{ 1, 1 },
+ { 2, 3 },
{ }
};
@@ -934,3 +958,266 @@
.set_rate_and_parent = clk_rcg2_shared_set_rate_and_parent,
};
EXPORT_SYMBOL_GPL(clk_rcg2_shared_ops);
+
+/* Common APIs to be used for DFS based RCGR */
+static void clk_rcg2_dfs_populate_freq(struct clk_hw *hw, unsigned int l,
+ struct freq_tbl *f)
+{
+ struct clk_rcg2 *rcg = to_clk_rcg2(hw);
+ struct clk_hw *p;
+ unsigned long prate = 0;
+ u32 val, mask, cfg, mode, src;
+ int i, num_parents;
+
+ regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + SE_PERF_DFSR(l), &cfg);
+
+ mask = BIT(rcg->hid_width) - 1;
+ f->pre_div = 1;
+ if (cfg & mask)
+ f->pre_div = cfg & mask;
+
+ src = cfg & CFG_SRC_SEL_MASK;
+ src >>= CFG_SRC_SEL_SHIFT;
+
+ num_parents = clk_hw_get_num_parents(hw);
+ for (i = 0; i < num_parents; i++) {
+ if (src == rcg->parent_map[i].cfg) {
+ f->src = rcg->parent_map[i].src;
+ p = clk_hw_get_parent_by_index(&rcg->clkr.hw, i);
+ prate = clk_hw_get_rate(p);
+ }
+ }
+
+ mode = cfg & CFG_MODE_MASK;
+ mode >>= CFG_MODE_SHIFT;
+ if (mode) {
+ mask = BIT(rcg->mnd_width) - 1;
+ regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + SE_PERF_M_DFSR(l),
+ &val);
+ val &= mask;
+ f->m = val;
+
+ regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + SE_PERF_N_DFSR(l),
+ &val);
+ val = ~val;
+ val &= mask;
+ val += f->m;
+ f->n = val;
+ }
+
+ f->freq = calc_rate(prate, f->m, f->n, mode, f->pre_div);
+}
+
+static int clk_rcg2_dfs_populate_freq_table(struct clk_rcg2 *rcg)
+{
+ struct freq_tbl *freq_tbl;
+ int i;
+
+ /* Allocate space for 1 extra since table is NULL terminated */
+ freq_tbl = kcalloc(MAX_PERF_LEVEL + 1, sizeof(*freq_tbl), GFP_KERNEL);
+ if (!freq_tbl)
+ return -ENOMEM;
+ rcg->freq_tbl = freq_tbl;
+
+ for (i = 0; i < MAX_PERF_LEVEL; i++)
+ clk_rcg2_dfs_populate_freq(&rcg->clkr.hw, i, freq_tbl + i);
+
+ return 0;
+}
+
+static int clk_rcg2_dfs_determine_rate(struct clk_hw *hw,
+ struct clk_rate_request *req)
+{
+ struct clk_rcg2 *rcg = to_clk_rcg2(hw);
+ int ret;
+
+ if (!rcg->freq_tbl) {
+ ret = clk_rcg2_dfs_populate_freq_table(rcg);
+ if (ret) {
+ pr_err("Failed to update DFS tables for %s\n",
+ clk_hw_get_name(hw));
+ return ret;
+ }
+ }
+
+ return clk_rcg2_determine_rate(hw, req);
+}
+
+static unsigned long
+clk_rcg2_dfs_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
+{
+ struct clk_rcg2 *rcg = to_clk_rcg2(hw);
+ u32 level, mask, cfg, m = 0, n = 0, mode, pre_div;
+
+ regmap_read(rcg->clkr.regmap,
+ rcg->cmd_rcgr + SE_CMD_DFSR_OFFSET, &level);
+ level &= GENMASK(4, 1);
+ level >>= 1;
+
+ if (rcg->freq_tbl)
+ return rcg->freq_tbl[level].freq;
+
+ /*
+ * Assume that parent_rate is actually the parent because
+ * we can't do any better at figuring it out when the table
+ * hasn't been populated yet. We only populate the table
+ * in determine_rate because we can't guarantee the parents
+ * will be registered with the framework until then.
+ */
+ regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + SE_PERF_DFSR(level),
+ &cfg);
+
+ mask = BIT(rcg->hid_width) - 1;
+ pre_div = 1;
+ if (cfg & mask)
+ pre_div = cfg & mask;
+
+ mode = cfg & CFG_MODE_MASK;
+ mode >>= CFG_MODE_SHIFT;
+ if (mode) {
+ mask = BIT(rcg->mnd_width) - 1;
+ regmap_read(rcg->clkr.regmap,
+ rcg->cmd_rcgr + SE_PERF_M_DFSR(level), &m);
+ m &= mask;
+
+ regmap_read(rcg->clkr.regmap,
+ rcg->cmd_rcgr + SE_PERF_N_DFSR(level), &n);
+ n = ~n;
+ n &= mask;
+ n += m;
+ }
+
+ return calc_rate(parent_rate, m, n, mode, pre_div);
+}
+
+static const struct clk_ops clk_rcg2_dfs_ops = {
+ .is_enabled = clk_rcg2_is_enabled,
+ .get_parent = clk_rcg2_get_parent,
+ .determine_rate = clk_rcg2_dfs_determine_rate,
+ .recalc_rate = clk_rcg2_dfs_recalc_rate,
+};
+
+static int clk_rcg2_enable_dfs(const struct clk_rcg_dfs_data *data,
+ struct regmap *regmap)
+{
+ struct clk_rcg2 *rcg = data->rcg;
+ struct clk_init_data *init = data->init;
+ u32 val;
+ int ret;
+
+ ret = regmap_read(regmap, rcg->cmd_rcgr + SE_CMD_DFSR_OFFSET, &val);
+ if (ret)
+ return -EINVAL;
+
+ if (!(val & SE_CMD_DFS_EN))
+ return 0;
+
+ /*
+ * Rate changes with consumer writing a register in
+ * their own I/O region
+ */
+ init->flags |= CLK_GET_RATE_NOCACHE;
+ init->ops = &clk_rcg2_dfs_ops;
+
+ rcg->freq_tbl = NULL;
+
+ return 0;
+}
+
+int qcom_cc_register_rcg_dfs(struct regmap *regmap,
+ const struct clk_rcg_dfs_data *rcgs, size_t len)
+{
+ int i, ret;
+
+ for (i = 0; i < len; i++) {
+ ret = clk_rcg2_enable_dfs(&rcgs[i], regmap);
+ if (ret)
+ return ret;
+ }
+
+ return 0;
+}
+EXPORT_SYMBOL_GPL(qcom_cc_register_rcg_dfs);
+
+static int clk_rcg2_dp_set_rate(struct clk_hw *hw, unsigned long rate,
+ unsigned long parent_rate)
+{
+ struct clk_rcg2 *rcg = to_clk_rcg2(hw);
+ struct freq_tbl f = { 0 };
+ u32 mask = BIT(rcg->hid_width) - 1;
+ u32 hid_div, cfg;
+ int i, num_parents = clk_hw_get_num_parents(hw);
+ unsigned long num, den;
+
+ rational_best_approximation(parent_rate, rate,
+ GENMASK(rcg->mnd_width - 1, 0),
+ GENMASK(rcg->mnd_width - 1, 0), &den, &num);
+
+ if (!num || !den)
+ return -EINVAL;
+
+ regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &cfg);
+ hid_div = cfg;
+ cfg &= CFG_SRC_SEL_MASK;
+ cfg >>= CFG_SRC_SEL_SHIFT;
+
+ for (i = 0; i < num_parents; i++) {
+ if (cfg == rcg->parent_map[i].cfg) {
+ f.src = rcg->parent_map[i].src;
+ break;
+ }
+ }
+
+ f.pre_div = hid_div;
+ f.pre_div >>= CFG_SRC_DIV_SHIFT;
+ f.pre_div &= mask;
+
+ if (num != den) {
+ f.m = num;
+ f.n = den;
+ } else {
+ f.m = 0;
+ f.n = 0;
+ }
+
+ return clk_rcg2_configure(rcg, &f);
+}
+
+static int clk_rcg2_dp_set_rate_and_parent(struct clk_hw *hw,
+ unsigned long rate, unsigned long parent_rate, u8 index)
+{
+ return clk_rcg2_dp_set_rate(hw, rate, parent_rate);
+}
+
+static int clk_rcg2_dp_determine_rate(struct clk_hw *hw,
+ struct clk_rate_request *req)
+{
+ struct clk_rcg2 *rcg = to_clk_rcg2(hw);
+ unsigned long num, den;
+ u64 tmp;
+
+ /* Parent rate is a fixed phy link rate */
+ rational_best_approximation(req->best_parent_rate, req->rate,
+ GENMASK(rcg->mnd_width - 1, 0),
+ GENMASK(rcg->mnd_width - 1, 0), &den, &num);
+
+ if (!num || !den)
+ return -EINVAL;
+
+ tmp = req->best_parent_rate * num;
+ do_div(tmp, den);
+ req->rate = tmp;
+
+ return 0;
+}
+
+const struct clk_ops clk_dp_ops = {
+ .is_enabled = clk_rcg2_is_enabled,
+ .get_parent = clk_rcg2_get_parent,
+ .set_parent = clk_rcg2_set_parent,
+ .recalc_rate = clk_rcg2_recalc_rate,
+ .set_rate = clk_rcg2_dp_set_rate,
+ .set_rate_and_parent = clk_rcg2_dp_set_rate_and_parent,
+ .determine_rate = clk_rcg2_dp_determine_rate,
+};
+EXPORT_SYMBOL_GPL(clk_dp_ops);
--
Gitblit v1.6.2