Commit | Line | Data |
---|---|---|
2874c5fd | 1 | // SPDX-License-Identifier: GPL-2.0-or-later |
f2f23566 VD |
2 | /* |
3 | * Handling of a master device, switching frames via its switch fabric CPU port | |
4 | * | |
5 | * Copyright (c) 2017 Savoir-faire Linux Inc. | |
6 | * Vivien Didelot <vivien.didelot@savoirfairelinux.com> | |
f2f23566 VD |
7 | */ |
8 | ||
9 | #include "dsa_priv.h" | |
10 | ||
48e23311 VD |
11 | static int dsa_master_get_regs_len(struct net_device *dev) |
12 | { | |
13 | struct dsa_port *cpu_dp = dev->dsa_ptr; | |
14 | const struct ethtool_ops *ops = cpu_dp->orig_ethtool_ops; | |
15 | struct dsa_switch *ds = cpu_dp->ds; | |
16 | int port = cpu_dp->index; | |
17 | int ret = 0; | |
18 | int len; | |
19 | ||
20 | if (ops->get_regs_len) { | |
21 | len = ops->get_regs_len(dev); | |
22 | if (len < 0) | |
23 | return len; | |
24 | ret += len; | |
25 | } | |
26 | ||
27 | ret += sizeof(struct ethtool_drvinfo); | |
28 | ret += sizeof(struct ethtool_regs); | |
29 | ||
30 | if (ds->ops->get_regs_len) { | |
31 | len = ds->ops->get_regs_len(ds, port); | |
32 | if (len < 0) | |
33 | return len; | |
34 | ret += len; | |
35 | } | |
36 | ||
37 | return ret; | |
38 | } | |
39 | ||
40 | static void dsa_master_get_regs(struct net_device *dev, | |
41 | struct ethtool_regs *regs, void *data) | |
42 | { | |
43 | struct dsa_port *cpu_dp = dev->dsa_ptr; | |
44 | const struct ethtool_ops *ops = cpu_dp->orig_ethtool_ops; | |
45 | struct dsa_switch *ds = cpu_dp->ds; | |
46 | struct ethtool_drvinfo *cpu_info; | |
47 | struct ethtool_regs *cpu_regs; | |
48 | int port = cpu_dp->index; | |
49 | int len; | |
50 | ||
51 | if (ops->get_regs_len && ops->get_regs) { | |
52 | len = ops->get_regs_len(dev); | |
53 | if (len < 0) | |
54 | return; | |
55 | regs->len = len; | |
56 | ops->get_regs(dev, regs, data); | |
57 | data += regs->len; | |
58 | } | |
59 | ||
60 | cpu_info = (struct ethtool_drvinfo *)data; | |
61 | strlcpy(cpu_info->driver, "dsa", sizeof(cpu_info->driver)); | |
62 | data += sizeof(*cpu_info); | |
63 | cpu_regs = (struct ethtool_regs *)data; | |
64 | data += sizeof(*cpu_regs); | |
65 | ||
66 | if (ds->ops->get_regs_len && ds->ops->get_regs) { | |
67 | len = ds->ops->get_regs_len(ds, port); | |
68 | if (len < 0) | |
69 | return; | |
70 | cpu_regs->len = len; | |
71 | ds->ops->get_regs(ds, port, cpu_regs, data); | |
72 | } | |
73 | } | |
74 | ||
f2f23566 VD |
75 | static void dsa_master_get_ethtool_stats(struct net_device *dev, |
76 | struct ethtool_stats *stats, | |
77 | uint64_t *data) | |
78 | { | |
2f657a60 | 79 | struct dsa_port *cpu_dp = dev->dsa_ptr; |
7ec764ee VD |
80 | const struct ethtool_ops *ops = cpu_dp->orig_ethtool_ops; |
81 | struct dsa_switch *ds = cpu_dp->ds; | |
82 | int port = cpu_dp->index; | |
f2f23566 VD |
83 | int count = 0; |
84 | ||
1d1e79f1 | 85 | if (ops->get_sset_count && ops->get_ethtool_stats) { |
f2f23566 VD |
86 | count = ops->get_sset_count(dev, ETH_SS_STATS); |
87 | ops->get_ethtool_stats(dev, stats, data); | |
88 | } | |
89 | ||
90 | if (ds->ops->get_ethtool_stats) | |
7ec764ee | 91 | ds->ops->get_ethtool_stats(ds, port, data + count); |
f2f23566 VD |
92 | } |
93 | ||
cf963573 FF |
94 | static void dsa_master_get_ethtool_phy_stats(struct net_device *dev, |
95 | struct ethtool_stats *stats, | |
96 | uint64_t *data) | |
97 | { | |
98 | struct dsa_port *cpu_dp = dev->dsa_ptr; | |
99 | const struct ethtool_ops *ops = cpu_dp->orig_ethtool_ops; | |
100 | struct dsa_switch *ds = cpu_dp->ds; | |
101 | int port = cpu_dp->index; | |
102 | int count = 0; | |
103 | ||
104 | if (dev->phydev && !ops->get_ethtool_phy_stats) { | |
105 | count = phy_ethtool_get_sset_count(dev->phydev); | |
106 | if (count >= 0) | |
107 | phy_ethtool_get_stats(dev->phydev, stats, data); | |
108 | } else if (ops->get_sset_count && ops->get_ethtool_phy_stats) { | |
109 | count = ops->get_sset_count(dev, ETH_SS_PHY_STATS); | |
110 | ops->get_ethtool_phy_stats(dev, stats, data); | |
111 | } | |
112 | ||
113 | if (count < 0) | |
114 | count = 0; | |
115 | ||
116 | if (ds->ops->get_ethtool_phy_stats) | |
117 | ds->ops->get_ethtool_phy_stats(ds, port, data + count); | |
118 | } | |
119 | ||
f2f23566 VD |
120 | static int dsa_master_get_sset_count(struct net_device *dev, int sset) |
121 | { | |
2f657a60 | 122 | struct dsa_port *cpu_dp = dev->dsa_ptr; |
7ec764ee VD |
123 | const struct ethtool_ops *ops = cpu_dp->orig_ethtool_ops; |
124 | struct dsa_switch *ds = cpu_dp->ds; | |
f2f23566 VD |
125 | int count = 0; |
126 | ||
cf963573 FF |
127 | if (sset == ETH_SS_PHY_STATS && dev->phydev && |
128 | !ops->get_ethtool_phy_stats) | |
129 | count = phy_ethtool_get_sset_count(dev->phydev); | |
130 | else if (ops->get_sset_count) | |
89f09048 | 131 | count = ops->get_sset_count(dev, sset); |
cf963573 FF |
132 | |
133 | if (count < 0) | |
134 | count = 0; | |
f2f23566 | 135 | |
89f09048 FF |
136 | if (ds->ops->get_sset_count) |
137 | count += ds->ops->get_sset_count(ds, cpu_dp->index, sset); | |
f2f23566 VD |
138 | |
139 | return count; | |
140 | } | |
141 | ||
142 | static void dsa_master_get_strings(struct net_device *dev, uint32_t stringset, | |
143 | uint8_t *data) | |
144 | { | |
2f657a60 | 145 | struct dsa_port *cpu_dp = dev->dsa_ptr; |
7ec764ee VD |
146 | const struct ethtool_ops *ops = cpu_dp->orig_ethtool_ops; |
147 | struct dsa_switch *ds = cpu_dp->ds; | |
148 | int port = cpu_dp->index; | |
f2f23566 VD |
149 | int len = ETH_GSTRING_LEN; |
150 | int mcount = 0, count; | |
151 | unsigned int i; | |
152 | uint8_t pfx[4]; | |
153 | uint8_t *ndata; | |
154 | ||
7ec764ee | 155 | snprintf(pfx, sizeof(pfx), "p%.2d", port); |
f2f23566 VD |
156 | /* We do not want to be NULL-terminated, since this is a prefix */ |
157 | pfx[sizeof(pfx) - 1] = '_'; | |
158 | ||
cf963573 FF |
159 | if (stringset == ETH_SS_PHY_STATS && dev->phydev && |
160 | !ops->get_ethtool_phy_stats) { | |
161 | mcount = phy_ethtool_get_sset_count(dev->phydev); | |
162 | if (mcount < 0) | |
163 | mcount = 0; | |
164 | else | |
165 | phy_ethtool_get_strings(dev->phydev, data); | |
166 | } else if (ops->get_sset_count && ops->get_strings) { | |
89f09048 FF |
167 | mcount = ops->get_sset_count(dev, stringset); |
168 | if (mcount < 0) | |
169 | mcount = 0; | |
f2f23566 VD |
170 | ops->get_strings(dev, stringset, data); |
171 | } | |
172 | ||
89f09048 | 173 | if (ds->ops->get_strings) { |
f2f23566 VD |
174 | ndata = data + mcount * len; |
175 | /* This function copies ETH_GSTRINGS_LEN bytes, we will mangle | |
176 | * the output after to prepend our CPU port prefix we | |
177 | * constructed earlier | |
178 | */ | |
89f09048 FF |
179 | ds->ops->get_strings(ds, port, stringset, ndata); |
180 | count = ds->ops->get_sset_count(ds, port, stringset); | |
f2f23566 VD |
181 | for (i = 0; i < count; i++) { |
182 | memmove(ndata + (i * len + sizeof(pfx)), | |
183 | ndata + i * len, len - sizeof(pfx)); | |
184 | memcpy(ndata + i * len, pfx, sizeof(pfx)); | |
185 | } | |
186 | } | |
187 | } | |
188 | ||
da7b9e9b FF |
189 | static int dsa_master_get_phys_port_name(struct net_device *dev, |
190 | char *name, size_t len) | |
191 | { | |
192 | struct dsa_port *cpu_dp = dev->dsa_ptr; | |
193 | ||
194 | if (snprintf(name, len, "p%d", cpu_dp->index) >= len) | |
195 | return -EINVAL; | |
196 | ||
197 | return 0; | |
198 | } | |
199 | ||
17a22fcf | 200 | static int dsa_master_ethtool_setup(struct net_device *dev) |
f2f23566 | 201 | { |
2f657a60 | 202 | struct dsa_port *cpu_dp = dev->dsa_ptr; |
7ec764ee | 203 | struct dsa_switch *ds = cpu_dp->ds; |
f2f23566 VD |
204 | struct ethtool_ops *ops; |
205 | ||
206 | ops = devm_kzalloc(ds->dev, sizeof(*ops), GFP_KERNEL); | |
207 | if (!ops) | |
208 | return -ENOMEM; | |
209 | ||
7ec764ee VD |
210 | cpu_dp->orig_ethtool_ops = dev->ethtool_ops; |
211 | if (cpu_dp->orig_ethtool_ops) | |
212 | memcpy(ops, cpu_dp->orig_ethtool_ops, sizeof(*ops)); | |
f2f23566 | 213 | |
48e23311 VD |
214 | ops->get_regs_len = dsa_master_get_regs_len; |
215 | ops->get_regs = dsa_master_get_regs; | |
f2f23566 VD |
216 | ops->get_sset_count = dsa_master_get_sset_count; |
217 | ops->get_ethtool_stats = dsa_master_get_ethtool_stats; | |
218 | ops->get_strings = dsa_master_get_strings; | |
cf963573 | 219 | ops->get_ethtool_phy_stats = dsa_master_get_ethtool_phy_stats; |
f2f23566 VD |
220 | |
221 | dev->ethtool_ops = ops; | |
222 | ||
223 | return 0; | |
224 | } | |
225 | ||
17a22fcf | 226 | static void dsa_master_ethtool_teardown(struct net_device *dev) |
f2f23566 | 227 | { |
2f657a60 | 228 | struct dsa_port *cpu_dp = dev->dsa_ptr; |
f2f23566 | 229 | |
7ec764ee VD |
230 | dev->ethtool_ops = cpu_dp->orig_ethtool_ops; |
231 | cpu_dp->orig_ethtool_ops = NULL; | |
f2f23566 | 232 | } |
17a22fcf | 233 | |
da7b9e9b FF |
234 | static int dsa_master_ndo_setup(struct net_device *dev) |
235 | { | |
236 | struct dsa_port *cpu_dp = dev->dsa_ptr; | |
237 | struct dsa_switch *ds = cpu_dp->ds; | |
238 | struct net_device_ops *ops; | |
239 | ||
240 | if (dev->netdev_ops->ndo_get_phys_port_name) | |
241 | return 0; | |
242 | ||
243 | ops = devm_kzalloc(ds->dev, sizeof(*ops), GFP_KERNEL); | |
244 | if (!ops) | |
245 | return -ENOMEM; | |
246 | ||
247 | cpu_dp->orig_ndo_ops = dev->netdev_ops; | |
248 | if (cpu_dp->orig_ndo_ops) | |
249 | memcpy(ops, cpu_dp->orig_ndo_ops, sizeof(*ops)); | |
250 | ||
251 | ops->ndo_get_phys_port_name = dsa_master_get_phys_port_name; | |
252 | ||
253 | dev->netdev_ops = ops; | |
254 | ||
255 | return 0; | |
256 | } | |
257 | ||
258 | static void dsa_master_ndo_teardown(struct net_device *dev) | |
259 | { | |
260 | struct dsa_port *cpu_dp = dev->dsa_ptr; | |
261 | ||
262 | dev->netdev_ops = cpu_dp->orig_ndo_ops; | |
263 | cpu_dp->orig_ndo_ops = NULL; | |
264 | } | |
265 | ||
a3d7e01d FF |
266 | static ssize_t tagging_show(struct device *d, struct device_attribute *attr, |
267 | char *buf) | |
268 | { | |
269 | struct net_device *dev = to_net_dev(d); | |
270 | struct dsa_port *cpu_dp = dev->dsa_ptr; | |
271 | ||
272 | return sprintf(buf, "%s\n", | |
273 | dsa_tag_protocol_to_str(cpu_dp->tag_ops)); | |
274 | } | |
275 | static DEVICE_ATTR_RO(tagging); | |
276 | ||
277 | static struct attribute *dsa_slave_attrs[] = { | |
278 | &dev_attr_tagging.attr, | |
279 | NULL | |
280 | }; | |
281 | ||
282 | static const struct attribute_group dsa_group = { | |
283 | .name = "dsa", | |
284 | .attrs = dsa_slave_attrs, | |
285 | }; | |
286 | ||
a60956ed | 287 | static void dsa_master_set_mtu(struct net_device *dev, struct dsa_port *cpu_dp) |
dc0fe7d4 AL |
288 | { |
289 | unsigned int mtu = ETH_DATA_LEN + cpu_dp->tag_ops->overhead; | |
290 | int err; | |
291 | ||
292 | rtnl_lock(); | |
293 | if (mtu <= dev->max_mtu) { | |
294 | err = dev_set_mtu(dev, mtu); | |
295 | if (err) | |
296 | netdev_dbg(dev, "Unable to set MTU to include for DSA overheads\n"); | |
297 | } | |
298 | rtnl_unlock(); | |
299 | } | |
300 | ||
91ba4795 AL |
301 | static void dsa_master_reset_mtu(struct net_device *dev) |
302 | { | |
303 | int err; | |
304 | ||
305 | rtnl_lock(); | |
306 | err = dev_set_mtu(dev, ETH_DATA_LEN); | |
307 | if (err) | |
308 | netdev_dbg(dev, | |
309 | "Unable to reset MTU to exclude DSA overheads\n"); | |
310 | rtnl_unlock(); | |
311 | } | |
312 | ||
17a22fcf VD |
313 | int dsa_master_setup(struct net_device *dev, struct dsa_port *cpu_dp) |
314 | { | |
a3d7e01d FF |
315 | int ret; |
316 | ||
dc0fe7d4 AL |
317 | dsa_master_set_mtu(dev, cpu_dp); |
318 | ||
17a22fcf VD |
319 | /* If we use a tagging format that doesn't have an ethertype |
320 | * field, make sure that all packets from this point on get | |
321 | * sent to the tag format's receive function. | |
322 | */ | |
323 | wmb(); | |
324 | ||
325 | dev->dsa_ptr = cpu_dp; | |
a3d7e01d FF |
326 | ret = dsa_master_ethtool_setup(dev); |
327 | if (ret) | |
328 | return ret; | |
329 | ||
da7b9e9b FF |
330 | ret = dsa_master_ndo_setup(dev); |
331 | if (ret) | |
332 | goto out_err_ethtool_teardown; | |
333 | ||
a3d7e01d FF |
334 | ret = sysfs_create_group(&dev->dev.kobj, &dsa_group); |
335 | if (ret) | |
da7b9e9b FF |
336 | goto out_err_ndo_teardown; |
337 | ||
338 | return ret; | |
a3d7e01d | 339 | |
da7b9e9b FF |
340 | out_err_ndo_teardown: |
341 | dsa_master_ndo_teardown(dev); | |
342 | out_err_ethtool_teardown: | |
343 | dsa_master_ethtool_teardown(dev); | |
a3d7e01d | 344 | return ret; |
17a22fcf VD |
345 | } |
346 | ||
347 | void dsa_master_teardown(struct net_device *dev) | |
348 | { | |
a3d7e01d | 349 | sysfs_remove_group(&dev->dev.kobj, &dsa_group); |
da7b9e9b | 350 | dsa_master_ndo_teardown(dev); |
17a22fcf | 351 | dsa_master_ethtool_teardown(dev); |
91ba4795 | 352 | dsa_master_reset_mtu(dev); |
17a22fcf VD |
353 | |
354 | dev->dsa_ptr = NULL; | |
355 | ||
356 | /* If we used a tagging format that doesn't have an ethertype | |
357 | * field, make sure that all packets from this point get sent | |
358 | * without the tag and go through the regular receive path. | |
359 | */ | |
360 | wmb(); | |
361 | } |