net/dsa/tag_8021q.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /* Copyright (c) 2019, Vladimir Oltean <[email protected]>
   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 conduit 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 *conduit;
 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         conduit = dsa_port_to_conduit(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 conduit's RX filter. */
 361         vlan_vid_add(conduit, 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 *conduit;
 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         conduit = dsa_port_to_conduit(dp);
 380
 381         dsa_port_tag_8021q_vlan_del(dp, vid, false);
 382
 383         vlan_vid_del(conduit, 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 *conduit,
 472                                                    int vbid)
 473 {
 474         struct dsa_port *cpu_dp = conduit->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->user;
 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);