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; |
19 | struct icc_clk_node clocks[] __counted_by(num_clocks); |
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(clk: qn->clk); |
36 | qn->enabled = false; |
37 | |
38 | return 0; |
39 | } |
40 | |
41 | if (!qn->enabled) { |
42 | ret = clk_prepare_enable(clk: qn->clk); |
43 | if (ret) |
44 | return ret; |
45 | qn->enabled = true; |
46 | } |
47 | |
48 | return clk_set_rate(clk: 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(error: -ENOMEM); |
90 | |
91 | qp = devm_kzalloc(dev, struct_size(qp, clocks, num_clocks), GFP_KERNEL); |
92 | if (!qp) |
93 | return ERR_PTR(error: -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(list: &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(id: first_id + j); |
112 | if (IS_ERR(ptr: node)) { |
113 | ret = PTR_ERR(ptr: node); |
114 | goto err; |
115 | } |
116 | |
117 | node->name = devm_kasprintf(dev, GFP_KERNEL, fmt: "%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, dst_id: first_id + j + 1); |
122 | onecell->nodes[j++] = node; |
123 | |
124 | node = icc_node_create(id: first_id + j); |
125 | if (IS_ERR(ptr: node)) { |
126 | ret = PTR_ERR(ptr: node); |
127 | goto err; |
128 | } |
129 | |
130 | node->name = devm_kasprintf(dev, GFP_KERNEL, fmt: "%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(error: ret); |
148 | } |
149 | EXPORT_SYMBOL_GPL(icc_clk_register); |
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(provider: &qp->provider); |
161 | icc_nodes_remove(provider: &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(clk: qn->clk); |
168 | } |
169 | } |
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>" ); |
175 | |