Commit | Line | Data |
---|---|---|
0ac2a08f DB |
1 | /* SPDX-License-Identifier: GPL-2.0 */ |
2 | /* | |
3 | * Copyright (c) 2023, Linaro Ltd. | |
4 | */ | |
5 | ||
6 | #include <linux/clk.h> | |
7 | #include <linux/device.h> | |
8 | #include <linux/interconnect-clk.h> | |
9 | #include <linux/interconnect-provider.h> | |
10 | ||
11 | struct icc_clk_node { | |
12 | struct clk *clk; | |
13 | bool enabled; | |
14 | }; | |
15 | ||
16 | struct icc_clk_provider { | |
17 | struct icc_provider provider; | |
18 | int num_clocks; | |
b1e0cdb0 | 19 | struct icc_clk_node clocks[] __counted_by(num_clocks); |
0ac2a08f DB |
20 | }; |
21 | ||
22 | #define to_icc_clk_provider(_provider) \ | |
23 | container_of(_provider, struct icc_clk_provider, provider) | |
24 | ||
25 | static int icc_clk_set(struct icc_node *src, struct icc_node *dst) | |
26 | { | |
27 | struct icc_clk_node *qn = src->data; | |
28 | int ret; | |
29 | ||
30 | if (!qn || !qn->clk) | |
31 | return 0; | |
32 | ||
33 | if (!src->peak_bw) { | |
34 | if (qn->enabled) | |
35 | clk_disable_unprepare(qn->clk); | |
36 | qn->enabled = false; | |
37 | ||
38 | return 0; | |
39 | } | |
40 | ||
41 | if (!qn->enabled) { | |
42 | ret = clk_prepare_enable(qn->clk); | |
43 | if (ret) | |
44 | return ret; | |
45 | qn->enabled = true; | |
46 | } | |
47 | ||
48 | return clk_set_rate(qn->clk, icc_units_to_bps(src->peak_bw)); | |
49 | } | |
50 | ||
51 | static int icc_clk_get_bw(struct icc_node *node, u32 *avg, u32 *peak) | |
52 | { | |
53 | struct icc_clk_node *qn = node->data; | |
54 | ||
55 | if (!qn || !qn->clk) | |
56 | *peak = INT_MAX; | |
57 | else | |
58 | *peak = Bps_to_icc(clk_get_rate(qn->clk)); | |
59 | ||
60 | return 0; | |
61 | } | |
62 | ||
63 | /** | |
64 | * icc_clk_register() - register a new clk-based interconnect provider | |
65 | * @dev: device supporting this provider | |
66 | * @first_id: an ID of the first provider's node | |
67 | * @num_clocks: number of instances of struct icc_clk_data | |
68 | * @data: data for the provider | |
69 | * | |
70 | * Registers and returns a clk-based interconnect provider. It is a simple | |
71 | * wrapper around COMMON_CLK framework, allowing other devices to vote on the | |
72 | * clock rate. | |
73 | * | |
74 | * Return: 0 on success, or an error code otherwise | |
75 | */ | |
76 | struct icc_provider *icc_clk_register(struct device *dev, | |
77 | unsigned int first_id, | |
78 | unsigned int num_clocks, | |
79 | const struct icc_clk_data *data) | |
80 | { | |
81 | struct icc_clk_provider *qp; | |
82 | struct icc_provider *provider; | |
83 | struct icc_onecell_data *onecell; | |
84 | struct icc_node *node; | |
85 | int ret, i, j; | |
86 | ||
87 | onecell = devm_kzalloc(dev, struct_size(onecell, nodes, 2 * num_clocks), GFP_KERNEL); | |
88 | if (!onecell) | |
89 | return ERR_PTR(-ENOMEM); | |
90 | ||
91 | qp = devm_kzalloc(dev, struct_size(qp, clocks, num_clocks), GFP_KERNEL); | |
92 | if (!qp) | |
93 | return ERR_PTR(-ENOMEM); | |
94 | ||
95 | qp->num_clocks = num_clocks; | |
96 | ||
97 | provider = &qp->provider; | |
98 | provider->dev = dev; | |
99 | provider->get_bw = icc_clk_get_bw; | |
100 | provider->set = icc_clk_set; | |
101 | provider->aggregate = icc_std_aggregate; | |
102 | provider->xlate = of_icc_xlate_onecell; | |
103 | INIT_LIST_HEAD(&provider->nodes); | |
104 | provider->data = onecell; | |
105 | ||
106 | icc_provider_init(provider); | |
107 | ||
108 | for (i = 0, j = 0; i < num_clocks; i++) { | |
109 | qp->clocks[i].clk = data[i].clk; | |
110 | ||
111 | node = icc_node_create(first_id + j); | |
112 | if (IS_ERR(node)) { | |
113 | ret = PTR_ERR(node); | |
114 | goto err; | |
115 | } | |
116 | ||
117 | node->name = devm_kasprintf(dev, GFP_KERNEL, "%s_master", data[i].name); | |
118 | node->data = &qp->clocks[i]; | |
119 | icc_node_add(node, provider); | |
120 | /* link to the next node, slave */ | |
121 | icc_link_create(node, first_id + j + 1); | |
122 | onecell->nodes[j++] = node; | |
123 | ||
124 | node = icc_node_create(first_id + j); | |
125 | if (IS_ERR(node)) { | |
126 | ret = PTR_ERR(node); | |
127 | goto err; | |
128 | } | |
129 | ||
130 | node->name = devm_kasprintf(dev, GFP_KERNEL, "%s_slave", data[i].name); | |
131 | /* no data for slave node */ | |
132 | icc_node_add(node, provider); | |
133 | onecell->nodes[j++] = node; | |
134 | } | |
135 | ||
136 | onecell->num_nodes = j; | |
137 | ||
138 | ret = icc_provider_register(provider); | |
139 | if (ret) | |
140 | goto err; | |
141 | ||
142 | return provider; | |
143 | ||
144 | err: | |
145 | icc_nodes_remove(provider); | |
146 | ||
147 | return ERR_PTR(ret); | |
148 | } | |
a1f7e721 | 149 | EXPORT_SYMBOL_GPL(icc_clk_register); |
0ac2a08f DB |
150 | |
151 | /** | |
152 | * icc_clk_unregister() - unregister a previously registered clk interconnect provider | |
153 | * @provider: provider returned by icc_clk_register() | |
154 | */ | |
155 | void icc_clk_unregister(struct icc_provider *provider) | |
156 | { | |
157 | struct icc_clk_provider *qp = container_of(provider, struct icc_clk_provider, provider); | |
158 | int i; | |
159 | ||
160 | icc_provider_deregister(&qp->provider); | |
161 | icc_nodes_remove(&qp->provider); | |
162 | ||
163 | for (i = 0; i < qp->num_clocks; i++) { | |
164 | struct icc_clk_node *qn = &qp->clocks[i]; | |
165 | ||
166 | if (qn->enabled) | |
167 | clk_disable_unprepare(qn->clk); | |
168 | } | |
169 | } | |
a1f7e721 DB |
170 | EXPORT_SYMBOL_GPL(icc_clk_unregister); |
171 | ||
172 | MODULE_LICENSE("GPL"); | |
173 | MODULE_DESCRIPTION("Interconnect wrapper for clocks"); | |
174 | MODULE_AUTHOR("Dmitry Baryshkov <dmitry.baryshkov@linaro.org>"); |