1 // SPDX-License-Identifier: GPL-2.0
2 /*
3 * Copyright (C) 2020 Linaro Ltd
4 */
5
6 #include <linux/clk.h>
7 #include <linux/device.h>
8 #include <linux/interconnect-provider.h>
9 #include <linux/io.h>
10 #include <linux/module.h>
11 #include <linux/of_device.h>
12 #include <linux/of_platform.h>
13 #include <linux/platform_device.h>
14 #include <linux/slab.h>
15
16 #include "smd-rpm.h"
17 #include "icc-rpm.h"
18
qcom_icc_set(struct icc_node * src,struct icc_node * dst)19 static int qcom_icc_set(struct icc_node *src, struct icc_node *dst)
20 {
21 struct qcom_icc_provider *qp;
22 struct qcom_icc_node *qn;
23 struct icc_provider *provider;
24 struct icc_node *n;
25 u64 sum_bw;
26 u64 max_peak_bw;
27 u64 rate;
28 u32 agg_avg = 0;
29 u32 agg_peak = 0;
30 int ret, i;
31
32 qn = src->data;
33 provider = src->provider;
34 qp = to_qcom_provider(provider);
35
36 list_for_each_entry(n, &provider->nodes, node_list)
37 provider->aggregate(n, 0, n->avg_bw, n->peak_bw,
38 &agg_avg, &agg_peak);
39
40 sum_bw = icc_units_to_bps(agg_avg);
41 max_peak_bw = icc_units_to_bps(agg_peak);
42
43 /* send bandwidth request message to the RPM processor */
44 if (qn->mas_rpm_id != -1) {
45 ret = qcom_icc_rpm_smd_send(QCOM_SMD_RPM_ACTIVE_STATE,
46 RPM_BUS_MASTER_REQ,
47 qn->mas_rpm_id,
48 sum_bw);
49 if (ret) {
50 pr_err("qcom_icc_rpm_smd_send mas %d error %d\n",
51 qn->mas_rpm_id, ret);
52 return ret;
53 }
54 }
55
56 if (qn->slv_rpm_id != -1) {
57 ret = qcom_icc_rpm_smd_send(QCOM_SMD_RPM_ACTIVE_STATE,
58 RPM_BUS_SLAVE_REQ,
59 qn->slv_rpm_id,
60 sum_bw);
61 if (ret) {
62 pr_err("qcom_icc_rpm_smd_send slv %d error %d\n",
63 qn->slv_rpm_id, ret);
64 return ret;
65 }
66 }
67
68 rate = max(sum_bw, max_peak_bw);
69
70 do_div(rate, qn->buswidth);
71
72 if (qn->rate == rate)
73 return 0;
74
75 for (i = 0; i < qp->num_clks; i++) {
76 ret = clk_set_rate(qp->bus_clks[i].clk, rate);
77 if (ret) {
78 pr_err("%s clk_set_rate error: %d\n",
79 qp->bus_clks[i].id, ret);
80 return ret;
81 }
82 }
83
84 qn->rate = rate;
85
86 return 0;
87 }
88
qnoc_probe(struct platform_device * pdev,size_t cd_size,int cd_num,const struct clk_bulk_data * cd)89 int qnoc_probe(struct platform_device *pdev, size_t cd_size, int cd_num,
90 const struct clk_bulk_data *cd)
91 {
92 struct device *dev = &pdev->dev;
93 const struct qcom_icc_desc *desc;
94 struct icc_onecell_data *data;
95 struct icc_provider *provider;
96 struct qcom_icc_node **qnodes;
97 struct qcom_icc_provider *qp;
98 struct icc_node *node;
99 size_t num_nodes, i;
100 int ret;
101
102 /* wait for the RPM proxy */
103 if (!qcom_icc_rpm_smd_available())
104 return -EPROBE_DEFER;
105
106 desc = of_device_get_match_data(dev);
107 if (!desc)
108 return -EINVAL;
109
110 qnodes = desc->nodes;
111 num_nodes = desc->num_nodes;
112
113 qp = devm_kzalloc(dev, sizeof(*qp), GFP_KERNEL);
114 if (!qp)
115 return -ENOMEM;
116
117 data = devm_kzalloc(dev, struct_size(data, nodes, num_nodes),
118 GFP_KERNEL);
119 if (!data)
120 return -ENOMEM;
121
122 qp->bus_clks = devm_kmemdup(dev, cd, cd_size,
123 GFP_KERNEL);
124 if (!qp->bus_clks)
125 return -ENOMEM;
126
127 qp->num_clks = cd_num;
128 ret = devm_clk_bulk_get(dev, qp->num_clks, qp->bus_clks);
129 if (ret)
130 return ret;
131
132 ret = clk_bulk_prepare_enable(qp->num_clks, qp->bus_clks);
133 if (ret)
134 return ret;
135
136 provider = &qp->provider;
137 INIT_LIST_HEAD(&provider->nodes);
138 provider->dev = dev;
139 provider->set = qcom_icc_set;
140 provider->aggregate = icc_std_aggregate;
141 provider->xlate = of_icc_xlate_onecell;
142 provider->data = data;
143
144 ret = icc_provider_add(provider);
145 if (ret) {
146 dev_err(dev, "error adding interconnect provider: %d\n", ret);
147 clk_bulk_disable_unprepare(qp->num_clks, qp->bus_clks);
148 return ret;
149 }
150
151 for (i = 0; i < num_nodes; i++) {
152 size_t j;
153
154 node = icc_node_create(qnodes[i]->id);
155 if (IS_ERR(node)) {
156 ret = PTR_ERR(node);
157 goto err;
158 }
159
160 node->name = qnodes[i]->name;
161 node->data = qnodes[i];
162 icc_node_add(node, provider);
163
164 for (j = 0; j < qnodes[i]->num_links; j++)
165 icc_link_create(node, qnodes[i]->links[j]);
166
167 data->nodes[i] = node;
168 }
169 data->num_nodes = num_nodes;
170
171 platform_set_drvdata(pdev, qp);
172
173 return 0;
174 err:
175 icc_nodes_remove(provider);
176 clk_bulk_disable_unprepare(qp->num_clks, qp->bus_clks);
177 icc_provider_del(provider);
178
179 return ret;
180 }
181 EXPORT_SYMBOL(qnoc_probe);
182
qnoc_remove(struct platform_device * pdev)183 int qnoc_remove(struct platform_device *pdev)
184 {
185 struct qcom_icc_provider *qp = platform_get_drvdata(pdev);
186
187 icc_nodes_remove(&qp->provider);
188 clk_bulk_disable_unprepare(qp->num_clks, qp->bus_clks);
189 return icc_provider_del(&qp->provider);
190 }
191 EXPORT_SYMBOL(qnoc_remove);
192