1
2
3
4
5
6
7
8
9#include <linux/if_bridge.h>
10#include <linux/if_vlan.h>
11#include <linux/dsa/8021q.h>
12
13#include "dsa_priv.h"
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42#define DSA_8021Q_DIR_SHIFT 10
43#define DSA_8021Q_DIR_MASK GENMASK(11, 10)
44#define DSA_8021Q_DIR(x) (((x) << DSA_8021Q_DIR_SHIFT) & \
45 DSA_8021Q_DIR_MASK)
46#define DSA_8021Q_DIR_RX DSA_8021Q_DIR(1)
47#define DSA_8021Q_DIR_TX DSA_8021Q_DIR(2)
48
49#define DSA_8021Q_SWITCH_ID_SHIFT 6
50#define DSA_8021Q_SWITCH_ID_MASK GENMASK(8, 6)
51#define DSA_8021Q_SWITCH_ID(x) (((x) << DSA_8021Q_SWITCH_ID_SHIFT) & \
52 DSA_8021Q_SWITCH_ID_MASK)
53
54#define DSA_8021Q_VBID_HI_SHIFT 9
55#define DSA_8021Q_VBID_HI_MASK GENMASK(9, 9)
56#define DSA_8021Q_VBID_LO_SHIFT 4
57#define DSA_8021Q_VBID_LO_MASK GENMASK(5, 4)
58#define DSA_8021Q_VBID_HI(x) (((x) & GENMASK(2, 2)) >> 2)
59#define DSA_8021Q_VBID_LO(x) ((x) & GENMASK(1, 0))
60#define DSA_8021Q_VBID(x) \
61 (((DSA_8021Q_VBID_LO(x) << DSA_8021Q_VBID_LO_SHIFT) & \
62 DSA_8021Q_VBID_LO_MASK) | \
63 ((DSA_8021Q_VBID_HI(x) << DSA_8021Q_VBID_HI_SHIFT) & \
64 DSA_8021Q_VBID_HI_MASK))
65
66#define DSA_8021Q_PORT_SHIFT 0
67#define DSA_8021Q_PORT_MASK GENMASK(3, 0)
68#define DSA_8021Q_PORT(x) (((x) << DSA_8021Q_PORT_SHIFT) & \
69 DSA_8021Q_PORT_MASK)
70
71u16 dsa_8021q_bridge_tx_fwd_offload_vid(int bridge_num)
72{
73
74 return DSA_8021Q_DIR_TX | DSA_8021Q_VBID(bridge_num + 1);
75}
76EXPORT_SYMBOL_GPL(dsa_8021q_bridge_tx_fwd_offload_vid);
77
78
79
80
81u16 dsa_8021q_tx_vid(struct dsa_switch *ds, int port)
82{
83 return DSA_8021Q_DIR_TX | DSA_8021Q_SWITCH_ID(ds->index) |
84 DSA_8021Q_PORT(port);
85}
86EXPORT_SYMBOL_GPL(dsa_8021q_tx_vid);
87
88
89
90
91u16 dsa_8021q_rx_vid(struct dsa_switch *ds, int port)
92{
93 return DSA_8021Q_DIR_RX | DSA_8021Q_SWITCH_ID(ds->index) |
94 DSA_8021Q_PORT(port);
95}
96EXPORT_SYMBOL_GPL(dsa_8021q_rx_vid);
97
98
99int dsa_8021q_rx_switch_id(u16 vid)
100{
101 return (vid & DSA_8021Q_SWITCH_ID_MASK) >> DSA_8021Q_SWITCH_ID_SHIFT;
102}
103EXPORT_SYMBOL_GPL(dsa_8021q_rx_switch_id);
104
105
106int dsa_8021q_rx_source_port(u16 vid)
107{
108 return (vid & DSA_8021Q_PORT_MASK) >> DSA_8021Q_PORT_SHIFT;
109}
110EXPORT_SYMBOL_GPL(dsa_8021q_rx_source_port);
111
112bool vid_is_dsa_8021q_rxvlan(u16 vid)
113{
114 return (vid & DSA_8021Q_DIR_MASK) == DSA_8021Q_DIR_RX;
115}
116EXPORT_SYMBOL_GPL(vid_is_dsa_8021q_rxvlan);
117
118bool vid_is_dsa_8021q_txvlan(u16 vid)
119{
120 return (vid & DSA_8021Q_DIR_MASK) == DSA_8021Q_DIR_TX;
121}
122EXPORT_SYMBOL_GPL(vid_is_dsa_8021q_txvlan);
123
124bool vid_is_dsa_8021q(u16 vid)
125{
126 return vid_is_dsa_8021q_rxvlan(vid) || vid_is_dsa_8021q_txvlan(vid);
127}
128EXPORT_SYMBOL_GPL(vid_is_dsa_8021q);
129
130static struct dsa_tag_8021q_vlan *
131dsa_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
142static int dsa_switch_do_tag_8021q_vlan_add(struct dsa_switch *ds, int port,
143 u16 vid, u16 flags)
144{
145 struct dsa_8021q_context *ctx = ds->tag_8021q_ctx;
146 struct dsa_port *dp = dsa_to_port(ds, port);
147 struct dsa_tag_8021q_vlan *v;
148 int err;
149
150
151 if (!(dsa_port_is_cpu(dp) || dsa_port_is_dsa(dp)))
152 return ds->ops->tag_8021q_vlan_add(ds, port, vid, flags);
153
154 v = dsa_tag_8021q_vlan_find(ctx, port, vid);
155 if (v) {
156 refcount_inc(&v->refcount);
157 return 0;
158 }
159
160 v = kzalloc(sizeof(*v), GFP_KERNEL);
161 if (!v)
162 return -ENOMEM;
163
164 err = ds->ops->tag_8021q_vlan_add(ds, port, vid, flags);
165 if (err) {
166 kfree(v);
167 return err;
168 }
169
170 v->vid = vid;
171 v->port = port;
172 refcount_set(&v->refcount, 1);
173 list_add_tail(&v->list, &ctx->vlans);
174
175 return 0;
176}
177
178static int dsa_switch_do_tag_8021q_vlan_del(struct dsa_switch *ds, int port,
179 u16 vid)
180{
181 struct dsa_8021q_context *ctx = ds->tag_8021q_ctx;
182 struct dsa_port *dp = dsa_to_port(ds, port);
183 struct dsa_tag_8021q_vlan *v;
184 int err;
185
186
187 if (!(dsa_port_is_cpu(dp) || dsa_port_is_dsa(dp)))
188 return ds->ops->tag_8021q_vlan_del(ds, port, vid);
189
190 v = dsa_tag_8021q_vlan_find(ctx, port, vid);
191 if (!v)
192 return -ENOENT;
193
194 if (!refcount_dec_and_test(&v->refcount))
195 return 0;
196
197 err = ds->ops->tag_8021q_vlan_del(ds, port, vid);
198 if (err) {
199 refcount_inc(&v->refcount);
200 return err;
201 }
202
203 list_del(&v->list);
204 kfree(v);
205
206 return 0;
207}
208
209static bool
210dsa_switch_tag_8021q_vlan_match(struct dsa_switch *ds, int port,
211 struct dsa_notifier_tag_8021q_vlan_info *info)
212{
213 if (dsa_is_dsa_port(ds, port) || dsa_is_cpu_port(ds, port))
214 return true;
215
216 if (ds->dst->index == info->tree_index && ds->index == info->sw_index)
217 return port == info->port;
218
219 return false;
220}
221
222int dsa_switch_tag_8021q_vlan_add(struct dsa_switch *ds,
223 struct dsa_notifier_tag_8021q_vlan_info *info)
224{
225 int port, err;
226
227
228
229
230
231
232 if (!ds->ops->tag_8021q_vlan_add || !ds->tag_8021q_ctx)
233 return 0;
234
235 for (port = 0; port < ds->num_ports; port++) {
236 if (dsa_switch_tag_8021q_vlan_match(ds, port, info)) {
237 u16 flags = 0;
238
239 if (dsa_is_user_port(ds, port))
240 flags |= BRIDGE_VLAN_INFO_UNTAGGED;
241
242 if (vid_is_dsa_8021q_rxvlan(info->vid) &&
243 dsa_8021q_rx_switch_id(info->vid) == ds->index &&
244 dsa_8021q_rx_source_port(info->vid) == port)
245 flags |= BRIDGE_VLAN_INFO_PVID;
246
247 err = dsa_switch_do_tag_8021q_vlan_add(ds, port,
248 info->vid,
249 flags);
250 if (err)
251 return err;
252 }
253 }
254
255 return 0;
256}
257
258int dsa_switch_tag_8021q_vlan_del(struct dsa_switch *ds,
259 struct dsa_notifier_tag_8021q_vlan_info *info)
260{
261 int port, err;
262
263 if (!ds->ops->tag_8021q_vlan_del || !ds->tag_8021q_ctx)
264 return 0;
265
266 for (port = 0; port < ds->num_ports; port++) {
267 if (dsa_switch_tag_8021q_vlan_match(ds, port, info)) {
268 err = dsa_switch_do_tag_8021q_vlan_del(ds, port,
269 info->vid);
270 if (err)
271 return err;
272 }
273 }
274
275 return 0;
276}
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325static bool dsa_tag_8021q_bridge_match(struct dsa_switch *ds, int port,
326 struct dsa_notifier_bridge_info *info)
327{
328 struct dsa_port *dp = dsa_to_port(ds, port);
329
330
331 if (ds->dst->index == info->tree_index &&
332 ds->index == info->sw_index &&
333 port == info->port)
334 return false;
335
336 if (dsa_port_is_user(dp))
337 return dp->bridge_dev == info->br;
338
339 return false;
340}
341
342int dsa_tag_8021q_bridge_join(struct dsa_switch *ds,
343 struct dsa_notifier_bridge_info *info)
344{
345 struct dsa_switch *targeted_ds;
346 struct dsa_port *targeted_dp;
347 u16 targeted_rx_vid;
348 int err, port;
349
350 if (!ds->tag_8021q_ctx)
351 return 0;
352
353 targeted_ds = dsa_switch_find(info->tree_index, info->sw_index);
354 targeted_dp = dsa_to_port(targeted_ds, info->port);
355 targeted_rx_vid = dsa_8021q_rx_vid(targeted_ds, info->port);
356
357 for (port = 0; port < ds->num_ports; port++) {
358 struct dsa_port *dp = dsa_to_port(ds, port);
359 u16 rx_vid = dsa_8021q_rx_vid(ds, port);
360
361 if (!dsa_tag_8021q_bridge_match(ds, port, info))
362 continue;
363
364
365 err = dsa_port_tag_8021q_vlan_add(dp, targeted_rx_vid, true);
366 if (err)
367 return err;
368
369
370 err = dsa_port_tag_8021q_vlan_add(targeted_dp, rx_vid, true);
371 if (err)
372 return err;
373 }
374
375 return 0;
376}
377
378int dsa_tag_8021q_bridge_leave(struct dsa_switch *ds,
379 struct dsa_notifier_bridge_info *info)
380{
381 struct dsa_switch *targeted_ds;
382 struct dsa_port *targeted_dp;
383 u16 targeted_rx_vid;
384 int port;
385
386 if (!ds->tag_8021q_ctx)
387 return 0;
388
389 targeted_ds = dsa_switch_find(info->tree_index, info->sw_index);
390 targeted_dp = dsa_to_port(targeted_ds, info->port);
391 targeted_rx_vid = dsa_8021q_rx_vid(targeted_ds, info->port);
392
393 for (port = 0; port < ds->num_ports; port++) {
394 struct dsa_port *dp = dsa_to_port(ds, port);
395 u16 rx_vid = dsa_8021q_rx_vid(ds, port);
396
397 if (!dsa_tag_8021q_bridge_match(ds, port, info))
398 continue;
399
400
401 dsa_port_tag_8021q_vlan_del(dp, targeted_rx_vid, true);
402
403
404 dsa_port_tag_8021q_vlan_del(targeted_dp, rx_vid, true);
405 }
406
407 return 0;
408}
409
410int dsa_tag_8021q_bridge_tx_fwd_offload(struct dsa_switch *ds, int port,
411 struct net_device *br,
412 int bridge_num)
413{
414 u16 tx_vid = dsa_8021q_bridge_tx_fwd_offload_vid(bridge_num);
415
416 return dsa_port_tag_8021q_vlan_add(dsa_to_port(ds, port), tx_vid,
417 true);
418}
419EXPORT_SYMBOL_GPL(dsa_tag_8021q_bridge_tx_fwd_offload);
420
421void dsa_tag_8021q_bridge_tx_fwd_unoffload(struct dsa_switch *ds, int port,
422 struct net_device *br,
423 int bridge_num)
424{
425 u16 tx_vid = dsa_8021q_bridge_tx_fwd_offload_vid(bridge_num);
426
427 dsa_port_tag_8021q_vlan_del(dsa_to_port(ds, port), tx_vid, true);
428}
429EXPORT_SYMBOL_GPL(dsa_tag_8021q_bridge_tx_fwd_unoffload);
430
431
432static int dsa_tag_8021q_port_setup(struct dsa_switch *ds, int port)
433{
434 struct dsa_8021q_context *ctx = ds->tag_8021q_ctx;
435 struct dsa_port *dp = dsa_to_port(ds, port);
436 u16 rx_vid = dsa_8021q_rx_vid(ds, port);
437 u16 tx_vid = dsa_8021q_tx_vid(ds, port);
438 struct net_device *master;
439 int err;
440
441
442
443
444 if (!dsa_port_is_user(dp))
445 return 0;
446
447 master = dp->cpu_dp->master;
448
449
450
451
452
453
454 err = dsa_port_tag_8021q_vlan_add(dp, rx_vid, false);
455 if (err) {
456 dev_err(ds->dev,
457 "Failed to apply RX VID %d to port %d: %pe\n",
458 rx_vid, port, ERR_PTR(err));
459 return err;
460 }
461
462
463 vlan_vid_add(master, ctx->proto, rx_vid);
464
465
466 err = dsa_port_tag_8021q_vlan_add(dp, tx_vid, false);
467 if (err) {
468 dev_err(ds->dev,
469 "Failed to apply TX VID %d on port %d: %pe\n",
470 tx_vid, port, ERR_PTR(err));
471 return err;
472 }
473
474 return err;
475}
476
477static void dsa_tag_8021q_port_teardown(struct dsa_switch *ds, int port)
478{
479 struct dsa_8021q_context *ctx = ds->tag_8021q_ctx;
480 struct dsa_port *dp = dsa_to_port(ds, port);
481 u16 rx_vid = dsa_8021q_rx_vid(ds, port);
482 u16 tx_vid = dsa_8021q_tx_vid(ds, port);
483 struct net_device *master;
484
485
486
487
488 if (!dsa_port_is_user(dp))
489 return;
490
491 master = dp->cpu_dp->master;
492
493 dsa_port_tag_8021q_vlan_del(dp, rx_vid, false);
494
495 vlan_vid_del(master, ctx->proto, rx_vid);
496
497 dsa_port_tag_8021q_vlan_del(dp, tx_vid, false);
498}
499
500static int dsa_tag_8021q_setup(struct dsa_switch *ds)
501{
502 int err, port;
503
504 ASSERT_RTNL();
505
506 for (port = 0; port < ds->num_ports; port++) {
507 err = dsa_tag_8021q_port_setup(ds, port);
508 if (err < 0) {
509 dev_err(ds->dev,
510 "Failed to setup VLAN tagging for port %d: %pe\n",
511 port, ERR_PTR(err));
512 return err;
513 }
514 }
515
516 return 0;
517}
518
519static void dsa_tag_8021q_teardown(struct dsa_switch *ds)
520{
521 int port;
522
523 ASSERT_RTNL();
524
525 for (port = 0; port < ds->num_ports; port++)
526 dsa_tag_8021q_port_teardown(ds, port);
527}
528
529int dsa_tag_8021q_register(struct dsa_switch *ds, __be16 proto)
530{
531 struct dsa_8021q_context *ctx;
532
533 ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
534 if (!ctx)
535 return -ENOMEM;
536
537 ctx->proto = proto;
538 ctx->ds = ds;
539
540 INIT_LIST_HEAD(&ctx->vlans);
541
542 ds->tag_8021q_ctx = ctx;
543
544 return dsa_tag_8021q_setup(ds);
545}
546EXPORT_SYMBOL_GPL(dsa_tag_8021q_register);
547
548void dsa_tag_8021q_unregister(struct dsa_switch *ds)
549{
550 struct dsa_8021q_context *ctx = ds->tag_8021q_ctx;
551 struct dsa_tag_8021q_vlan *v, *n;
552
553 dsa_tag_8021q_teardown(ds);
554
555 list_for_each_entry_safe(v, n, &ctx->vlans, list) {
556 list_del(&v->list);
557 kfree(v);
558 }
559
560 ds->tag_8021q_ctx = NULL;
561
562 kfree(ctx);
563}
564EXPORT_SYMBOL_GPL(dsa_tag_8021q_unregister);
565
566struct sk_buff *dsa_8021q_xmit(struct sk_buff *skb, struct net_device *netdev,
567 u16 tpid, u16 tci)
568{
569
570
571
572 return vlan_insert_tag(skb, htons(tpid), tci);
573}
574EXPORT_SYMBOL_GPL(dsa_8021q_xmit);
575
576void dsa_8021q_rcv(struct sk_buff *skb, int *source_port, int *switch_id)
577{
578 u16 vid, tci;
579
580 skb_push_rcsum(skb, ETH_HLEN);
581 if (skb_vlan_tag_present(skb)) {
582 tci = skb_vlan_tag_get(skb);
583 __vlan_hwaccel_clear_tag(skb);
584 } else {
585 __skb_vlan_pop(skb, &tci);
586 }
587 skb_pull_rcsum(skb, ETH_HLEN);
588
589 vid = tci & VLAN_VID_MASK;
590
591 *source_port = dsa_8021q_rx_source_port(vid);
592 *switch_id = dsa_8021q_rx_switch_id(vid);
593 skb->priority = (tci & VLAN_PRIO_MASK) >> VLAN_PRIO_SHIFT;
594}
595EXPORT_SYMBOL_GPL(dsa_8021q_rcv);
596