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 master 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(dp) || dsa_port_is_dsa(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(&v->refcount); 158 return 0; 159 } 160 161 v = kzalloc(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(v); 168 return err; 169 } 170 171 v->vid = vid; 172 v->port = port; 173 refcount_set(&v->refcount, 1); 174 list_add_tail(&v->list, &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(dp) || dsa_port_is_dsa(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(&v->refcount)) 196 return 0; 197 198 err = ds->ops->tag_8021q_vlan_del(ds, port, vid); 199 if (err) { 200 refcount_inc(&v->refcount); 201 return err; 202 } 203 204 list_del(&v->list); 205 kfree(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(dp) || dsa_port_is_cpu(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, 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, 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, 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, bridge_vid, true); 302 if (err) 303 return err; 304 305 dsa_port_tag_8021q_vlan_del(dp, standalone_vid, 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, 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, standalone_vid, 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, bridge_vid, 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, port); 340 u16 vid = dsa_tag_8021q_standalone_vid(dp); 341 struct net_device *master; 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 master = dsa_port_to_master(dp); 351 352 err = dsa_port_tag_8021q_vlan_add(dp, vid, 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 master's RX filter. */ 361 vlan_vid_add(master, 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, port); 370 u16 vid = dsa_tag_8021q_standalone_vid(dp); 371 struct net_device *master; 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 master = dsa_port_to_master(dp); 380 381 dsa_port_tag_8021q_vlan_del(dp, vid, false); 382 383 vlan_vid_del(master, 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(sizeof(*ctx), GFP_KERNEL); 421 if (!ctx) 422 return -ENOMEM; 423 424 ctx->proto = proto; 425 ctx->ds = ds; 426 427 INIT_LIST_HEAD(&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(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(&v->list); 452 kfree(v); 453 } 454 455 ds->tag_8021q_ctx = NULL; 456 457 kfree(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), tci); 468 } 469 EXPORT_SYMBOL_GPL(dsa_8021q_xmit); 470 471 struct net_device *dsa_tag_8021q_find_port_by_vbid(struct net_device *master, 472 int vbid) 473 { 474 struct dsa_port *cpu_dp = master->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->slave; 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, &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