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 rate = min_t(u64, rate, LONG_MAX);
72
73 if (qn->rate == rate)
74 return 0;
75
76 for (i = 0; i < qp->num_clks; i++) {
77 ret = clk_set_rate(qp->bus_clks[i].clk, rate);
78 if (ret) {
79 pr_err("%s clk_set_rate error: %d\n",
80 qp->bus_clks[i].id, ret);
81 return ret;
82 }
83 }
84
85 qn->rate = rate;
86
87 return 0;
88 }
89
qnoc_probe(struct platform_device * pdev,size_t cd_size,int cd_num,const struct clk_bulk_data * cd)90 int qnoc_probe(struct platform_device *pdev, size_t cd_size, int cd_num,
91 const struct clk_bulk_data *cd)
92 {
93 struct device *dev = &pdev->dev;
94 const struct qcom_icc_desc *desc;
95 struct icc_onecell_data *data;
96 struct icc_provider *provider;
97 struct qcom_icc_node **qnodes;
98 struct qcom_icc_provider *qp;
99 struct icc_node *node;
100 size_t num_nodes, i;
101 int ret;
102
103 /* wait for the RPM proxy */
104 if (!qcom_icc_rpm_smd_available())
105 return -EPROBE_DEFER;
106
107 desc = of_device_get_match_data(dev);
108 if (!desc)
109 return -EINVAL;
110
111 qnodes = desc->nodes;
112 num_nodes = desc->num_nodes;
113
114 qp = devm_kzalloc(dev, sizeof(*qp), GFP_KERNEL);
115 if (!qp)
116 return -ENOMEM;
117
118 data = devm_kzalloc(dev, struct_size(data, nodes, num_nodes),
119 GFP_KERNEL);
120 if (!data)
121 return -ENOMEM;
122
123 qp->bus_clks = devm_kmemdup(dev, cd, cd_size,
124 GFP_KERNEL);
125 if (!qp->bus_clks)
126 return -ENOMEM;
127
128 qp->num_clks = cd_num;
129 ret = devm_clk_bulk_get(dev, qp->num_clks, qp->bus_clks);
130 if (ret)
131 return ret;
132
133 ret = clk_bulk_prepare_enable(qp->num_clks, qp->bus_clks);
134 if (ret)
135 return ret;
136
137 provider = &qp->provider;
138 INIT_LIST_HEAD(&provider->nodes);
139 provider->dev = dev;
140 provider->set = qcom_icc_set;
141 provider->aggregate = icc_std_aggregate;
142 provider->xlate = of_icc_xlate_onecell;
143 provider->data = data;
144
145 ret = icc_provider_add(provider);
146 if (ret) {
147 dev_err(dev, "error adding interconnect provider: %d\n", ret);
148 clk_bulk_disable_unprepare(qp->num_clks, qp->bus_clks);
149 return ret;
150 }
151
152 for (i = 0; i < num_nodes; i++) {
153 size_t j;
154
155 node = icc_node_create(qnodes[i]->id);
156 if (IS_ERR(node)) {
157 ret = PTR_ERR(node);
158 goto err;
159 }
160
161 node->name = qnodes[i]->name;
162 node->data = qnodes[i];
163 icc_node_add(node, provider);
164
165 for (j = 0; j < qnodes[i]->num_links; j++)
166 icc_link_create(node, qnodes[i]->links[j]);
167
168 data->nodes[i] = node;
169 }
170 data->num_nodes = num_nodes;
171
172 platform_set_drvdata(pdev, qp);
173
174 return 0;
175 err:
176 icc_nodes_remove(provider);
177 clk_bulk_disable_unprepare(qp->num_clks, qp->bus_clks);
178 icc_provider_del(provider);
179
180 return ret;
181 }
182 EXPORT_SYMBOL(qnoc_probe);
183
qnoc_remove(struct platform_device * pdev)184 int qnoc_remove(struct platform_device *pdev)
185 {
186 struct qcom_icc_provider *qp = platform_get_drvdata(pdev);
187
188 icc_nodes_remove(&qp->provider);
189 clk_bulk_disable_unprepare(qp->num_clks, qp->bus_clks);
190 return icc_provider_del(&qp->provider);
191 }
192 EXPORT_SYMBOL(qnoc_remove);
193