1 | // SPDX-License-Identifier: GPL-2.0-only |
2 | /* |
3 | * Copyright (C) 2016 Rafał Miłecki <rafal@milecki.pl> |
4 | */ |
5 | |
6 | #include <linux/clk-provider.h> |
7 | #include <linux/err.h> |
8 | #include <linux/io.h> |
9 | #include <linux/mfd/syscon.h> |
10 | #include <linux/of.h> |
11 | #include <linux/of_address.h> |
12 | #include <linux/regmap.h> |
13 | #include <linux/slab.h> |
14 | |
15 | #define PMU_XTAL_FREQ_RATIO 0x66c |
16 | #define XTAL_ALP_PER_4ILP 0x00001fff |
17 | #define XTAL_CTL_EN 0x80000000 |
18 | #define PMU_SLOW_CLK_PERIOD 0x6dc |
19 | |
20 | struct bcm53573_ilp { |
21 | struct clk_hw hw; |
22 | struct regmap *regmap; |
23 | }; |
24 | |
25 | static int bcm53573_ilp_enable(struct clk_hw *hw) |
26 | { |
27 | struct bcm53573_ilp *ilp = container_of(hw, struct bcm53573_ilp, hw); |
28 | |
29 | regmap_write(map: ilp->regmap, PMU_SLOW_CLK_PERIOD, val: 0x10199); |
30 | regmap_write(map: ilp->regmap, reg: 0x674, val: 0x10000); |
31 | |
32 | return 0; |
33 | } |
34 | |
35 | static void bcm53573_ilp_disable(struct clk_hw *hw) |
36 | { |
37 | struct bcm53573_ilp *ilp = container_of(hw, struct bcm53573_ilp, hw); |
38 | |
39 | regmap_write(map: ilp->regmap, PMU_SLOW_CLK_PERIOD, val: 0); |
40 | regmap_write(map: ilp->regmap, reg: 0x674, val: 0); |
41 | } |
42 | |
43 | static unsigned long bcm53573_ilp_recalc_rate(struct clk_hw *hw, |
44 | unsigned long parent_rate) |
45 | { |
46 | struct bcm53573_ilp *ilp = container_of(hw, struct bcm53573_ilp, hw); |
47 | struct regmap *regmap = ilp->regmap; |
48 | u32 last_val, cur_val; |
49 | int sum = 0, num = 0, loop_num = 0; |
50 | int avg; |
51 | |
52 | /* Enable measurement */ |
53 | regmap_write(map: regmap, PMU_XTAL_FREQ_RATIO, XTAL_CTL_EN); |
54 | |
55 | /* Read initial value */ |
56 | regmap_read(map: regmap, PMU_XTAL_FREQ_RATIO, val: &last_val); |
57 | last_val &= XTAL_ALP_PER_4ILP; |
58 | |
59 | /* |
60 | * At minimum we should loop for a bit to let hardware do the |
61 | * measurement. This isn't very accurate however, so for a better |
62 | * precision lets try getting 20 different values for and use average. |
63 | */ |
64 | while (num < 20) { |
65 | regmap_read(map: regmap, PMU_XTAL_FREQ_RATIO, val: &cur_val); |
66 | cur_val &= XTAL_ALP_PER_4ILP; |
67 | |
68 | if (cur_val != last_val) { |
69 | /* Got different value, use it */ |
70 | sum += cur_val; |
71 | num++; |
72 | loop_num = 0; |
73 | last_val = cur_val; |
74 | } else if (++loop_num > 5000) { |
75 | /* Same value over and over, give up */ |
76 | sum += cur_val; |
77 | num++; |
78 | break; |
79 | } |
80 | |
81 | cpu_relax(); |
82 | } |
83 | |
84 | /* Disable measurement to save power */ |
85 | regmap_write(map: regmap, PMU_XTAL_FREQ_RATIO, val: 0x0); |
86 | |
87 | avg = sum / num; |
88 | |
89 | return parent_rate * 4 / avg; |
90 | } |
91 | |
92 | static const struct clk_ops bcm53573_ilp_clk_ops = { |
93 | .enable = bcm53573_ilp_enable, |
94 | .disable = bcm53573_ilp_disable, |
95 | .recalc_rate = bcm53573_ilp_recalc_rate, |
96 | }; |
97 | |
98 | static void bcm53573_ilp_init(struct device_node *np) |
99 | { |
100 | struct bcm53573_ilp *ilp; |
101 | struct clk_init_data init = { }; |
102 | const char *parent_name; |
103 | int err; |
104 | |
105 | ilp = kzalloc(size: sizeof(*ilp), GFP_KERNEL); |
106 | if (!ilp) |
107 | return; |
108 | |
109 | parent_name = of_clk_get_parent_name(np, index: 0); |
110 | if (!parent_name) { |
111 | err = -ENOENT; |
112 | goto err_free_ilp; |
113 | } |
114 | |
115 | ilp->regmap = syscon_node_to_regmap(np: of_get_parent(node: np)); |
116 | if (IS_ERR(ptr: ilp->regmap)) { |
117 | err = PTR_ERR(ptr: ilp->regmap); |
118 | goto err_free_ilp; |
119 | } |
120 | |
121 | init.name = np->name; |
122 | init.ops = &bcm53573_ilp_clk_ops; |
123 | init.parent_names = &parent_name; |
124 | init.num_parents = 1; |
125 | |
126 | ilp->hw.init = &init; |
127 | err = clk_hw_register(NULL, hw: &ilp->hw); |
128 | if (err) |
129 | goto err_free_ilp; |
130 | |
131 | err = of_clk_add_hw_provider(np, get: of_clk_hw_simple_get, data: &ilp->hw); |
132 | if (err) |
133 | goto err_clk_hw_unregister; |
134 | |
135 | return; |
136 | |
137 | err_clk_hw_unregister: |
138 | clk_hw_unregister(hw: &ilp->hw); |
139 | err_free_ilp: |
140 | kfree(objp: ilp); |
141 | pr_err("Failed to init ILP clock: %d\n" , err); |
142 | } |
143 | |
144 | /* We need it very early for arch code, before device model gets ready */ |
145 | CLK_OF_DECLARE(bcm53573_ilp_clk, "brcm,bcm53573-ilp" , bcm53573_ilp_init); |
146 | |