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