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