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 struct dsa_switch *ds = dp->ds; 200 201 if (dsa_port_is_dsa(dp) || dsa_port_is_cpu(dp)) 202 return true; 203 204 if (ds->dst->index == info->tree_index && ds->index == info->sw_index) 205 return dp->index == info->port; 206 207 return false; 208 } 209 210 int dsa_switch_tag_8021q_vlan_add(struct dsa_switch *ds, 211 struct dsa_notifier_tag_8021q_vlan_info *info) 212 { 213 struct dsa_port *dp; 214 int err; 215 216 /* Since we use dsa_broadcast(), there might be other switches in other 217 * trees which don't support tag_8021q, so don't return an error. 218 * Or they might even support tag_8021q but have not registered yet to 219 * use it (maybe they use another tagger currently). 220 */ 221 if (!ds->ops->tag_8021q_vlan_add || !ds->tag_8021q_ctx) 222 return 0; 223 224 dsa_switch_for_each_port(dp, ds) { 225 if (dsa_port_tag_8021q_vlan_match(dp, info)) { 226 u16 flags = 0; 227 228 if (dsa_port_is_user(dp)) 229 flags |= BRIDGE_VLAN_INFO_UNTAGGED | 230 BRIDGE_VLAN_INFO_PVID; 231 232 err = dsa_port_do_tag_8021q_vlan_add(dp, info->vid, 233 flags); 234 if (err) 235 return err; 236 } 237 } 238 239 return 0; 240 } 241 242 int dsa_switch_tag_8021q_vlan_del(struct dsa_switch *ds, 243 struct dsa_notifier_tag_8021q_vlan_info *info) 244 { 245 struct dsa_port *dp; 246 int err; 247 248 if (!ds->ops->tag_8021q_vlan_del || !ds->tag_8021q_ctx) 249 return 0; 250 251 dsa_switch_for_each_port(dp, ds) { 252 if (dsa_port_tag_8021q_vlan_match(dp, info)) { 253 err = dsa_port_do_tag_8021q_vlan_del(dp, info->vid); 254 if (err) 255 return err; 256 } 257 } 258 259 return 0; 260 } 261 262 /* There are 2 ways of offloading tag_8021q VLANs. 263 * 264 * One is to use a hardware TCAM to push the port's standalone VLAN into the 265 * frame when forwarding it to the CPU, as an egress modification rule on the 266 * CPU port. This is preferable because it has no side effects for the 267 * autonomous forwarding path, and accomplishes tag_8021q's primary goal of 268 * identifying the source port of each packet based on VLAN ID. 269 * 270 * The other is to commit the tag_8021q VLAN as a PVID to the VLAN table, and 271 * to configure the port as VLAN-unaware. This is less preferable because 272 * unique source port identification can only be done for standalone ports; 273 * under a VLAN-unaware bridge, all ports share the same tag_8021q VLAN as 274 * PVID, and under a VLAN-aware bridge, packets received by software will not 275 * have tag_8021q VLANs appended, just bridge VLANs. 276 * 277 * For tag_8021q implementations of the second type, this method is used to 278 * replace the standalone tag_8021q VLAN of a port with the tag_8021q VLAN to 279 * be used for VLAN-unaware bridging. 280 */ 281 int dsa_tag_8021q_bridge_join(struct dsa_switch *ds, int port, 282 struct dsa_bridge bridge) 283 { 284 struct dsa_port *dp = dsa_to_port(ds, port); 285 u16 standalone_vid, bridge_vid; 286 int err; 287 288 /* Delete the standalone VLAN of the port and replace it with a 289 * bridging VLAN 290 */ 291 standalone_vid = dsa_tag_8021q_standalone_vid(dp); 292 bridge_vid = dsa_tag_8021q_bridge_vid(bridge.num); 293 294 err = dsa_port_tag_8021q_vlan_add(dp, bridge_vid, true); 295 if (err) 296 return err; 297 298 dsa_port_tag_8021q_vlan_del(dp, standalone_vid, false); 299 300 return 0; 301 } 302 EXPORT_SYMBOL_GPL(dsa_tag_8021q_bridge_join); 303 304 void dsa_tag_8021q_bridge_leave(struct dsa_switch *ds, int port, 305 struct dsa_bridge bridge) 306 { 307 struct dsa_port *dp = dsa_to_port(ds, port); 308 u16 standalone_vid, bridge_vid; 309 int err; 310 311 /* Delete the bridging VLAN of the port and replace it with a 312 * standalone VLAN 313 */ 314 standalone_vid = dsa_tag_8021q_standalone_vid(dp); 315 bridge_vid = dsa_tag_8021q_bridge_vid(bridge.num); 316 317 err = dsa_port_tag_8021q_vlan_add(dp, standalone_vid, false); 318 if (err) { 319 dev_err(ds->dev, 320 "Failed to delete tag_8021q standalone VLAN %d from port %d: %pe\n", 321 standalone_vid, port, ERR_PTR(err)); 322 } 323 324 dsa_port_tag_8021q_vlan_del(dp, bridge_vid, true); 325 } 326 EXPORT_SYMBOL_GPL(dsa_tag_8021q_bridge_leave); 327 328 /* Set up a port's standalone tag_8021q VLAN */ 329 static int dsa_tag_8021q_port_setup(struct dsa_switch *ds, int port) 330 { 331 struct dsa_8021q_context *ctx = ds->tag_8021q_ctx; 332 struct dsa_port *dp = dsa_to_port(ds, port); 333 u16 vid = dsa_tag_8021q_standalone_vid(dp); 334 struct net_device *master; 335 int err; 336 337 /* The CPU port is implicitly configured by 338 * configuring the front-panel ports 339 */ 340 if (!dsa_port_is_user(dp)) 341 return 0; 342 343 master = dp->cpu_dp->master; 344 345 err = dsa_port_tag_8021q_vlan_add(dp, vid, false); 346 if (err) { 347 dev_err(ds->dev, 348 "Failed to apply standalone VID %d to port %d: %pe\n", 349 vid, port, ERR_PTR(err)); 350 return err; 351 } 352 353 /* Add the VLAN to the master's RX filter. */ 354 vlan_vid_add(master, ctx->proto, vid); 355 356 return err; 357 } 358 359 static void dsa_tag_8021q_port_teardown(struct dsa_switch *ds, int port) 360 { 361 struct dsa_8021q_context *ctx = ds->tag_8021q_ctx; 362 struct dsa_port *dp = dsa_to_port(ds, port); 363 u16 vid = dsa_tag_8021q_standalone_vid(dp); 364 struct net_device *master; 365 366 /* The CPU port is implicitly configured by 367 * configuring the front-panel ports 368 */ 369 if (!dsa_port_is_user(dp)) 370 return; 371 372 master = dp->cpu_dp->master; 373 374 dsa_port_tag_8021q_vlan_del(dp, vid, false); 375 376 vlan_vid_del(master, ctx->proto, vid); 377 } 378 379 static int dsa_tag_8021q_setup(struct dsa_switch *ds) 380 { 381 int err, port; 382 383 ASSERT_RTNL(); 384 385 for (port = 0; port < ds->num_ports; port++) { 386 err = dsa_tag_8021q_port_setup(ds, port); 387 if (err < 0) { 388 dev_err(ds->dev, 389 "Failed to setup VLAN tagging for port %d: %pe\n", 390 port, ERR_PTR(err)); 391 return err; 392 } 393 } 394 395 return 0; 396 } 397 398 static void dsa_tag_8021q_teardown(struct dsa_switch *ds) 399 { 400 int port; 401 402 ASSERT_RTNL(); 403 404 for (port = 0; port < ds->num_ports; port++) 405 dsa_tag_8021q_port_teardown(ds, port); 406 } 407 408 int dsa_tag_8021q_register(struct dsa_switch *ds, __be16 proto) 409 { 410 struct dsa_8021q_context *ctx; 411 412 ctx = kzalloc(sizeof(*ctx), GFP_KERNEL); 413 if (!ctx) 414 return -ENOMEM; 415 416 ctx->proto = proto; 417 ctx->ds = ds; 418 419 INIT_LIST_HEAD(&ctx->vlans); 420 421 ds->tag_8021q_ctx = ctx; 422 423 return dsa_tag_8021q_setup(ds); 424 } 425 EXPORT_SYMBOL_GPL(dsa_tag_8021q_register); 426 427 void dsa_tag_8021q_unregister(struct dsa_switch *ds) 428 { 429 struct dsa_8021q_context *ctx = ds->tag_8021q_ctx; 430 struct dsa_tag_8021q_vlan *v, *n; 431 432 dsa_tag_8021q_teardown(ds); 433 434 list_for_each_entry_safe(v, n, &ctx->vlans, list) { 435 list_del(&v->list); 436 kfree(v); 437 } 438 439 ds->tag_8021q_ctx = NULL; 440 441 kfree(ctx); 442 } 443 EXPORT_SYMBOL_GPL(dsa_tag_8021q_unregister); 444 445 struct sk_buff *dsa_8021q_xmit(struct sk_buff *skb, struct net_device *netdev, 446 u16 tpid, u16 tci) 447 { 448 /* skb->data points at skb_mac_header, which 449 * is fine for vlan_insert_tag. 450 */ 451 return vlan_insert_tag(skb, htons(tpid), tci); 452 } 453 EXPORT_SYMBOL_GPL(dsa_8021q_xmit); 454 455 struct net_device *dsa_tag_8021q_find_port_by_vbid(struct net_device *master, 456 int vbid) 457 { 458 struct dsa_port *cpu_dp = master->dsa_ptr; 459 struct dsa_switch_tree *dst = cpu_dp->dst; 460 struct dsa_port *dp; 461 462 if (WARN_ON(!vbid)) 463 return NULL; 464 465 dsa_tree_for_each_user_port(dp, dst) { 466 if (!dp->bridge) 467 continue; 468 469 if (dp->stp_state != BR_STATE_LEARNING && 470 dp->stp_state != BR_STATE_FORWARDING) 471 continue; 472 473 if (dp->cpu_dp != cpu_dp) 474 continue; 475 476 if (dsa_port_bridge_num_get(dp) == vbid) 477 return dp->slave; 478 } 479 480 return NULL; 481 } 482 EXPORT_SYMBOL_GPL(dsa_tag_8021q_find_port_by_vbid); 483 484 void dsa_8021q_rcv(struct sk_buff *skb, int *source_port, int *switch_id, 485 int *vbid) 486 { 487 u16 vid, tci; 488 489 if (skb_vlan_tag_present(skb)) { 490 tci = skb_vlan_tag_get(skb); 491 __vlan_hwaccel_clear_tag(skb); 492 } else { 493 skb_push_rcsum(skb, ETH_HLEN); 494 __skb_vlan_pop(skb, &tci); 495 skb_pull_rcsum(skb, ETH_HLEN); 496 } 497 498 vid = tci & VLAN_VID_MASK; 499 500 *source_port = dsa_8021q_rx_source_port(vid); 501 *switch_id = dsa_8021q_rx_switch_id(vid); 502 503 if (vbid) 504 *vbid = dsa_tag_8021q_rx_vbid(vid); 505 506 skb->priority = (tci & VLAN_PRIO_MASK) >> VLAN_PRIO_SHIFT; 507 } 508 EXPORT_SYMBOL_GPL(dsa_8021q_rcv); 509