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