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(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 }
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(&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 }
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