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 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 */ 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. */ 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. */ 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. */ 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 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 * 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 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 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 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 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 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 */ 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 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 */ 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 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 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 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 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 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 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 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 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