1 | // SPDX-License-Identifier: GPL-2.0 |
2 | /* Copyright (c) 2019, Vladimir Oltean <olteanv@gmail.com> |
3 | * |
4 | * This module is not a complete tagger implementation. It only provides |
5 | * primitives for taggers that rely on 802.1Q VLAN tags to use. |
6 | */ |
7 | #include <linux/if_vlan.h> |
8 | #include <linux/dsa/8021q.h> |
9 | |
10 | #include "port.h" |
11 | #include "switch.h" |
12 | #include "tag.h" |
13 | #include "tag_8021q.h" |
14 | |
15 | /* Binary structure of the fake 12-bit VID field (when the TPID is |
16 | * ETH_P_DSA_8021Q): |
17 | * |
18 | * | 11 | 10 | 9 | 8 | 7 | 6 | 5 | 4 | 3 | 2 | 1 | 0 | |
19 | * +-----------+-----+-----------------+-----------+-----------------------+ |
20 | * | RSV | VBID| SWITCH_ID | VBID | PORT | |
21 | * +-----------+-----+-----------------+-----------+-----------------------+ |
22 | * |
23 | * RSV - VID[11:10]: |
24 | * Reserved. Must be set to 3 (0b11). |
25 | * |
26 | * SWITCH_ID - VID[8:6]: |
27 | * Index of switch within DSA tree. Must be between 0 and 7. |
28 | * |
29 | * VBID - { VID[9], VID[5:4] }: |
30 | * Virtual bridge ID. If between 1 and 7, packet targets the broadcast |
31 | * domain of a bridge. If transmitted as zero, packet targets a single |
32 | * port. |
33 | * |
34 | * PORT - VID[3:0]: |
35 | * Index of switch port. Must be between 0 and 15. |
36 | */ |
37 | |
38 | #define DSA_8021Q_RSV_VAL 3 |
39 | #define DSA_8021Q_RSV_SHIFT 10 |
40 | #define DSA_8021Q_RSV_MASK GENMASK(11, 10) |
41 | #define DSA_8021Q_RSV ((DSA_8021Q_RSV_VAL << DSA_8021Q_RSV_SHIFT) & \ |
42 | DSA_8021Q_RSV_MASK) |
43 | |
44 | #define DSA_8021Q_SWITCH_ID_SHIFT 6 |
45 | #define DSA_8021Q_SWITCH_ID_MASK GENMASK(8, 6) |
46 | #define DSA_8021Q_SWITCH_ID(x) (((x) << DSA_8021Q_SWITCH_ID_SHIFT) & \ |
47 | DSA_8021Q_SWITCH_ID_MASK) |
48 | |
49 | #define DSA_8021Q_VBID_HI_SHIFT 9 |
50 | #define DSA_8021Q_VBID_HI_MASK GENMASK(9, 9) |
51 | #define DSA_8021Q_VBID_LO_SHIFT 4 |
52 | #define DSA_8021Q_VBID_LO_MASK GENMASK(5, 4) |
53 | #define DSA_8021Q_VBID_HI(x) (((x) & GENMASK(2, 2)) >> 2) |
54 | #define DSA_8021Q_VBID_LO(x) ((x) & GENMASK(1, 0)) |
55 | #define DSA_8021Q_VBID(x) \ |
56 | (((DSA_8021Q_VBID_LO(x) << DSA_8021Q_VBID_LO_SHIFT) & \ |
57 | DSA_8021Q_VBID_LO_MASK) | \ |
58 | ((DSA_8021Q_VBID_HI(x) << DSA_8021Q_VBID_HI_SHIFT) & \ |
59 | DSA_8021Q_VBID_HI_MASK)) |
60 | |
61 | #define DSA_8021Q_PORT_SHIFT 0 |
62 | #define DSA_8021Q_PORT_MASK GENMASK(3, 0) |
63 | #define DSA_8021Q_PORT(x) (((x) << DSA_8021Q_PORT_SHIFT) & \ |
64 | DSA_8021Q_PORT_MASK) |
65 | |
66 | struct dsa_tag_8021q_vlan { |
67 | struct list_head list; |
68 | int port; |
69 | u16 vid; |
70 | refcount_t refcount; |
71 | }; |
72 | |
73 | struct dsa_8021q_context { |
74 | struct dsa_switch *ds; |
75 | struct list_head vlans; |
76 | /* EtherType of RX VID, used for filtering on conduit interface */ |
77 | __be16 proto; |
78 | }; |
79 | |
80 | u16 dsa_tag_8021q_bridge_vid(unsigned int bridge_num) |
81 | { |
82 | /* The VBID value of 0 is reserved for precise TX, but it is also |
83 | * reserved/invalid for the bridge_num, so all is well. |
84 | */ |
85 | return DSA_8021Q_RSV | DSA_8021Q_VBID(bridge_num); |
86 | } |
87 | EXPORT_SYMBOL_GPL(dsa_tag_8021q_bridge_vid); |
88 | |
89 | /* Returns the VID that will be installed as pvid for this switch port, sent as |
90 | * tagged egress towards the CPU port and decoded by the rcv function. |
91 | */ |
92 | u16 dsa_tag_8021q_standalone_vid(const struct dsa_port *dp) |
93 | { |
94 | return DSA_8021Q_RSV | DSA_8021Q_SWITCH_ID(dp->ds->index) | |
95 | DSA_8021Q_PORT(dp->index); |
96 | } |
97 | EXPORT_SYMBOL_GPL(dsa_tag_8021q_standalone_vid); |
98 | |
99 | /* Returns the decoded switch ID from the RX VID. */ |
100 | int dsa_8021q_rx_switch_id(u16 vid) |
101 | { |
102 | return (vid & DSA_8021Q_SWITCH_ID_MASK) >> DSA_8021Q_SWITCH_ID_SHIFT; |
103 | } |
104 | EXPORT_SYMBOL_GPL(dsa_8021q_rx_switch_id); |
105 | |
106 | /* Returns the decoded port ID from the RX VID. */ |
107 | int dsa_8021q_rx_source_port(u16 vid) |
108 | { |
109 | return (vid & DSA_8021Q_PORT_MASK) >> DSA_8021Q_PORT_SHIFT; |
110 | } |
111 | EXPORT_SYMBOL_GPL(dsa_8021q_rx_source_port); |
112 | |
113 | /* Returns the decoded VBID from the RX VID. */ |
114 | static int dsa_tag_8021q_rx_vbid(u16 vid) |
115 | { |
116 | u16 vbid_hi = (vid & DSA_8021Q_VBID_HI_MASK) >> DSA_8021Q_VBID_HI_SHIFT; |
117 | u16 vbid_lo = (vid & DSA_8021Q_VBID_LO_MASK) >> DSA_8021Q_VBID_LO_SHIFT; |
118 | |
119 | return (vbid_hi << 2) | vbid_lo; |
120 | } |
121 | |
122 | bool vid_is_dsa_8021q(u16 vid) |
123 | { |
124 | u16 rsv = (vid & DSA_8021Q_RSV_MASK) >> DSA_8021Q_RSV_SHIFT; |
125 | |
126 | return rsv == DSA_8021Q_RSV_VAL; |
127 | } |
128 | EXPORT_SYMBOL_GPL(vid_is_dsa_8021q); |
129 | |
130 | static struct dsa_tag_8021q_vlan * |
131 | dsa_tag_8021q_vlan_find(struct dsa_8021q_context *ctx, int port, u16 vid) |
132 | { |
133 | struct dsa_tag_8021q_vlan *v; |
134 | |
135 | list_for_each_entry(v, &ctx->vlans, list) |
136 | if (v->vid == vid && v->port == port) |
137 | return v; |
138 | |
139 | return NULL; |
140 | } |
141 | |
142 | static int dsa_port_do_tag_8021q_vlan_add(struct dsa_port *dp, u16 vid, |
143 | u16 flags) |
144 | { |
145 | struct dsa_8021q_context *ctx = dp->ds->tag_8021q_ctx; |
146 | struct dsa_switch *ds = dp->ds; |
147 | struct dsa_tag_8021q_vlan *v; |
148 | int port = dp->index; |
149 | int err; |
150 | |
151 | /* No need to bother with refcounting for user ports */ |
152 | if (!(dsa_port_is_cpu(port: dp) || dsa_port_is_dsa(port: dp))) |
153 | return ds->ops->tag_8021q_vlan_add(ds, port, vid, flags); |
154 | |
155 | v = dsa_tag_8021q_vlan_find(ctx, port, vid); |
156 | if (v) { |
157 | refcount_inc(r: &v->refcount); |
158 | return 0; |
159 | } |
160 | |
161 | v = kzalloc(size: sizeof(*v), GFP_KERNEL); |
162 | if (!v) |
163 | return -ENOMEM; |
164 | |
165 | err = ds->ops->tag_8021q_vlan_add(ds, port, vid, flags); |
166 | if (err) { |
167 | kfree(objp: v); |
168 | return err; |
169 | } |
170 | |
171 | v->vid = vid; |
172 | v->port = port; |
173 | refcount_set(r: &v->refcount, n: 1); |
174 | list_add_tail(new: &v->list, head: &ctx->vlans); |
175 | |
176 | return 0; |
177 | } |
178 | |
179 | static int dsa_port_do_tag_8021q_vlan_del(struct dsa_port *dp, u16 vid) |
180 | { |
181 | struct dsa_8021q_context *ctx = dp->ds->tag_8021q_ctx; |
182 | struct dsa_switch *ds = dp->ds; |
183 | struct dsa_tag_8021q_vlan *v; |
184 | int port = dp->index; |
185 | int err; |
186 | |
187 | /* No need to bother with refcounting for user ports */ |
188 | if (!(dsa_port_is_cpu(port: dp) || dsa_port_is_dsa(port: dp))) |
189 | return ds->ops->tag_8021q_vlan_del(ds, port, vid); |
190 | |
191 | v = dsa_tag_8021q_vlan_find(ctx, port, vid); |
192 | if (!v) |
193 | return -ENOENT; |
194 | |
195 | if (!refcount_dec_and_test(r: &v->refcount)) |
196 | return 0; |
197 | |
198 | err = ds->ops->tag_8021q_vlan_del(ds, port, vid); |
199 | if (err) { |
200 | refcount_inc(r: &v->refcount); |
201 | return err; |
202 | } |
203 | |
204 | list_del(entry: &v->list); |
205 | kfree(objp: v); |
206 | |
207 | return 0; |
208 | } |
209 | |
210 | static bool |
211 | dsa_port_tag_8021q_vlan_match(struct dsa_port *dp, |
212 | struct dsa_notifier_tag_8021q_vlan_info *info) |
213 | { |
214 | return dsa_port_is_dsa(port: dp) || dsa_port_is_cpu(port: dp) || dp == info->dp; |
215 | } |
216 | |
217 | int dsa_switch_tag_8021q_vlan_add(struct dsa_switch *ds, |
218 | struct dsa_notifier_tag_8021q_vlan_info *info) |
219 | { |
220 | struct dsa_port *dp; |
221 | int err; |
222 | |
223 | /* Since we use dsa_broadcast(), there might be other switches in other |
224 | * trees which don't support tag_8021q, so don't return an error. |
225 | * Or they might even support tag_8021q but have not registered yet to |
226 | * use it (maybe they use another tagger currently). |
227 | */ |
228 | if (!ds->ops->tag_8021q_vlan_add || !ds->tag_8021q_ctx) |
229 | return 0; |
230 | |
231 | dsa_switch_for_each_port(dp, ds) { |
232 | if (dsa_port_tag_8021q_vlan_match(dp, info)) { |
233 | u16 flags = 0; |
234 | |
235 | if (dsa_port_is_user(dp)) |
236 | flags |= BRIDGE_VLAN_INFO_UNTAGGED | |
237 | BRIDGE_VLAN_INFO_PVID; |
238 | |
239 | err = dsa_port_do_tag_8021q_vlan_add(dp, vid: info->vid, |
240 | flags); |
241 | if (err) |
242 | return err; |
243 | } |
244 | } |
245 | |
246 | return 0; |
247 | } |
248 | |
249 | int dsa_switch_tag_8021q_vlan_del(struct dsa_switch *ds, |
250 | struct dsa_notifier_tag_8021q_vlan_info *info) |
251 | { |
252 | struct dsa_port *dp; |
253 | int err; |
254 | |
255 | if (!ds->ops->tag_8021q_vlan_del || !ds->tag_8021q_ctx) |
256 | return 0; |
257 | |
258 | dsa_switch_for_each_port(dp, ds) { |
259 | if (dsa_port_tag_8021q_vlan_match(dp, info)) { |
260 | err = dsa_port_do_tag_8021q_vlan_del(dp, vid: info->vid); |
261 | if (err) |
262 | return err; |
263 | } |
264 | } |
265 | |
266 | return 0; |
267 | } |
268 | |
269 | /* There are 2 ways of offloading tag_8021q VLANs. |
270 | * |
271 | * One is to use a hardware TCAM to push the port's standalone VLAN into the |
272 | * frame when forwarding it to the CPU, as an egress modification rule on the |
273 | * CPU port. This is preferable because it has no side effects for the |
274 | * autonomous forwarding path, and accomplishes tag_8021q's primary goal of |
275 | * identifying the source port of each packet based on VLAN ID. |
276 | * |
277 | * The other is to commit the tag_8021q VLAN as a PVID to the VLAN table, and |
278 | * to configure the port as VLAN-unaware. This is less preferable because |
279 | * unique source port identification can only be done for standalone ports; |
280 | * under a VLAN-unaware bridge, all ports share the same tag_8021q VLAN as |
281 | * PVID, and under a VLAN-aware bridge, packets received by software will not |
282 | * have tag_8021q VLANs appended, just bridge VLANs. |
283 | * |
284 | * For tag_8021q implementations of the second type, this method is used to |
285 | * replace the standalone tag_8021q VLAN of a port with the tag_8021q VLAN to |
286 | * be used for VLAN-unaware bridging. |
287 | */ |
288 | int dsa_tag_8021q_bridge_join(struct dsa_switch *ds, int port, |
289 | struct dsa_bridge bridge) |
290 | { |
291 | struct dsa_port *dp = dsa_to_port(ds, p: port); |
292 | u16 standalone_vid, bridge_vid; |
293 | int err; |
294 | |
295 | /* Delete the standalone VLAN of the port and replace it with a |
296 | * bridging VLAN |
297 | */ |
298 | standalone_vid = dsa_tag_8021q_standalone_vid(dp); |
299 | bridge_vid = dsa_tag_8021q_bridge_vid(bridge.num); |
300 | |
301 | err = dsa_port_tag_8021q_vlan_add(dp, vid: bridge_vid, broadcast: true); |
302 | if (err) |
303 | return err; |
304 | |
305 | dsa_port_tag_8021q_vlan_del(dp, vid: standalone_vid, broadcast: false); |
306 | |
307 | return 0; |
308 | } |
309 | EXPORT_SYMBOL_GPL(dsa_tag_8021q_bridge_join); |
310 | |
311 | void dsa_tag_8021q_bridge_leave(struct dsa_switch *ds, int port, |
312 | struct dsa_bridge bridge) |
313 | { |
314 | struct dsa_port *dp = dsa_to_port(ds, p: port); |
315 | u16 standalone_vid, bridge_vid; |
316 | int err; |
317 | |
318 | /* Delete the bridging VLAN of the port and replace it with a |
319 | * standalone VLAN |
320 | */ |
321 | standalone_vid = dsa_tag_8021q_standalone_vid(dp); |
322 | bridge_vid = dsa_tag_8021q_bridge_vid(bridge.num); |
323 | |
324 | err = dsa_port_tag_8021q_vlan_add(dp, vid: standalone_vid, broadcast: false); |
325 | if (err) { |
326 | dev_err(ds->dev, |
327 | "Failed to delete tag_8021q standalone VLAN %d from port %d: %pe\n" , |
328 | standalone_vid, port, ERR_PTR(err)); |
329 | } |
330 | |
331 | dsa_port_tag_8021q_vlan_del(dp, vid: bridge_vid, broadcast: true); |
332 | } |
333 | EXPORT_SYMBOL_GPL(dsa_tag_8021q_bridge_leave); |
334 | |
335 | /* Set up a port's standalone tag_8021q VLAN */ |
336 | static int dsa_tag_8021q_port_setup(struct dsa_switch *ds, int port) |
337 | { |
338 | struct dsa_8021q_context *ctx = ds->tag_8021q_ctx; |
339 | struct dsa_port *dp = dsa_to_port(ds, p: port); |
340 | u16 vid = dsa_tag_8021q_standalone_vid(dp); |
341 | struct net_device *conduit; |
342 | int err; |
343 | |
344 | /* The CPU port is implicitly configured by |
345 | * configuring the front-panel ports |
346 | */ |
347 | if (!dsa_port_is_user(dp)) |
348 | return 0; |
349 | |
350 | conduit = dsa_port_to_conduit(dp); |
351 | |
352 | err = dsa_port_tag_8021q_vlan_add(dp, vid, broadcast: false); |
353 | if (err) { |
354 | dev_err(ds->dev, |
355 | "Failed to apply standalone VID %d to port %d: %pe\n" , |
356 | vid, port, ERR_PTR(err)); |
357 | return err; |
358 | } |
359 | |
360 | /* Add the VLAN to the conduit's RX filter. */ |
361 | vlan_vid_add(dev: conduit, proto: ctx->proto, vid); |
362 | |
363 | return err; |
364 | } |
365 | |
366 | static void dsa_tag_8021q_port_teardown(struct dsa_switch *ds, int port) |
367 | { |
368 | struct dsa_8021q_context *ctx = ds->tag_8021q_ctx; |
369 | struct dsa_port *dp = dsa_to_port(ds, p: port); |
370 | u16 vid = dsa_tag_8021q_standalone_vid(dp); |
371 | struct net_device *conduit; |
372 | |
373 | /* The CPU port is implicitly configured by |
374 | * configuring the front-panel ports |
375 | */ |
376 | if (!dsa_port_is_user(dp)) |
377 | return; |
378 | |
379 | conduit = dsa_port_to_conduit(dp); |
380 | |
381 | dsa_port_tag_8021q_vlan_del(dp, vid, broadcast: false); |
382 | |
383 | vlan_vid_del(dev: conduit, proto: ctx->proto, vid); |
384 | } |
385 | |
386 | static int dsa_tag_8021q_setup(struct dsa_switch *ds) |
387 | { |
388 | int err, port; |
389 | |
390 | ASSERT_RTNL(); |
391 | |
392 | for (port = 0; port < ds->num_ports; port++) { |
393 | err = dsa_tag_8021q_port_setup(ds, port); |
394 | if (err < 0) { |
395 | dev_err(ds->dev, |
396 | "Failed to setup VLAN tagging for port %d: %pe\n" , |
397 | port, ERR_PTR(err)); |
398 | return err; |
399 | } |
400 | } |
401 | |
402 | return 0; |
403 | } |
404 | |
405 | static void dsa_tag_8021q_teardown(struct dsa_switch *ds) |
406 | { |
407 | int port; |
408 | |
409 | ASSERT_RTNL(); |
410 | |
411 | for (port = 0; port < ds->num_ports; port++) |
412 | dsa_tag_8021q_port_teardown(ds, port); |
413 | } |
414 | |
415 | int dsa_tag_8021q_register(struct dsa_switch *ds, __be16 proto) |
416 | { |
417 | struct dsa_8021q_context *ctx; |
418 | int err; |
419 | |
420 | ctx = kzalloc(size: sizeof(*ctx), GFP_KERNEL); |
421 | if (!ctx) |
422 | return -ENOMEM; |
423 | |
424 | ctx->proto = proto; |
425 | ctx->ds = ds; |
426 | |
427 | INIT_LIST_HEAD(list: &ctx->vlans); |
428 | |
429 | ds->tag_8021q_ctx = ctx; |
430 | |
431 | err = dsa_tag_8021q_setup(ds); |
432 | if (err) |
433 | goto err_free; |
434 | |
435 | return 0; |
436 | |
437 | err_free: |
438 | kfree(objp: ctx); |
439 | return err; |
440 | } |
441 | EXPORT_SYMBOL_GPL(dsa_tag_8021q_register); |
442 | |
443 | void dsa_tag_8021q_unregister(struct dsa_switch *ds) |
444 | { |
445 | struct dsa_8021q_context *ctx = ds->tag_8021q_ctx; |
446 | struct dsa_tag_8021q_vlan *v, *n; |
447 | |
448 | dsa_tag_8021q_teardown(ds); |
449 | |
450 | list_for_each_entry_safe(v, n, &ctx->vlans, list) { |
451 | list_del(entry: &v->list); |
452 | kfree(objp: v); |
453 | } |
454 | |
455 | ds->tag_8021q_ctx = NULL; |
456 | |
457 | kfree(objp: ctx); |
458 | } |
459 | EXPORT_SYMBOL_GPL(dsa_tag_8021q_unregister); |
460 | |
461 | struct sk_buff *dsa_8021q_xmit(struct sk_buff *skb, struct net_device *netdev, |
462 | u16 tpid, u16 tci) |
463 | { |
464 | /* skb->data points at the MAC header, which is fine |
465 | * for vlan_insert_tag(). |
466 | */ |
467 | return vlan_insert_tag(skb, htons(tpid), vlan_tci: tci); |
468 | } |
469 | EXPORT_SYMBOL_GPL(dsa_8021q_xmit); |
470 | |
471 | struct net_device *dsa_tag_8021q_find_port_by_vbid(struct net_device *conduit, |
472 | int vbid) |
473 | { |
474 | struct dsa_port *cpu_dp = conduit->dsa_ptr; |
475 | struct dsa_switch_tree *dst = cpu_dp->dst; |
476 | struct dsa_port *dp; |
477 | |
478 | if (WARN_ON(!vbid)) |
479 | return NULL; |
480 | |
481 | dsa_tree_for_each_user_port(dp, dst) { |
482 | if (!dp->bridge) |
483 | continue; |
484 | |
485 | if (dp->stp_state != BR_STATE_LEARNING && |
486 | dp->stp_state != BR_STATE_FORWARDING) |
487 | continue; |
488 | |
489 | if (dp->cpu_dp != cpu_dp) |
490 | continue; |
491 | |
492 | if (dsa_port_bridge_num_get(dp) == vbid) |
493 | return dp->user; |
494 | } |
495 | |
496 | return NULL; |
497 | } |
498 | EXPORT_SYMBOL_GPL(dsa_tag_8021q_find_port_by_vbid); |
499 | |
500 | void dsa_8021q_rcv(struct sk_buff *skb, int *source_port, int *switch_id, |
501 | int *vbid) |
502 | { |
503 | u16 vid, tci; |
504 | |
505 | if (skb_vlan_tag_present(skb)) { |
506 | tci = skb_vlan_tag_get(skb); |
507 | __vlan_hwaccel_clear_tag(skb); |
508 | } else { |
509 | skb_push_rcsum(skb, ETH_HLEN); |
510 | __skb_vlan_pop(skb, vlan_tci: &tci); |
511 | skb_pull_rcsum(skb, ETH_HLEN); |
512 | } |
513 | |
514 | vid = tci & VLAN_VID_MASK; |
515 | |
516 | *source_port = dsa_8021q_rx_source_port(vid); |
517 | *switch_id = dsa_8021q_rx_switch_id(vid); |
518 | |
519 | if (vbid) |
520 | *vbid = dsa_tag_8021q_rx_vbid(vid); |
521 | |
522 | skb->priority = (tci & VLAN_PRIO_MASK) >> VLAN_PRIO_SHIFT; |
523 | } |
524 | EXPORT_SYMBOL_GPL(dsa_8021q_rcv); |
525 | |