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, bool *tx_fwd_offload,
 290                               struct netlink_ext_ack *extack)
 291 {
 292         struct dsa_port *dp = dsa_to_port(ds, port);
 293         u16 standalone_vid, bridge_vid;
 294         int err;
 295
 296         /* Delete the standalone VLAN of the port and replace it with a
 297          * bridging VLAN
 298          */
 299         standalone_vid = dsa_tag_8021q_standalone_vid(dp);
 300         bridge_vid = dsa_tag_8021q_bridge_vid(bridge.num);
 301
 302         err = dsa_port_tag_8021q_vlan_add(dp, bridge_vid, true);
 303         if (err)
 304                 return err;
 305
 306         dsa_port_tag_8021q_vlan_del(dp, standalone_vid, false);
 307
 308         *tx_fwd_offload = true;
 309
 310         return 0;
 311 }
 312 EXPORT_SYMBOL_GPL(dsa_tag_8021q_bridge_join);
 313
 314 void dsa_tag_8021q_bridge_leave(struct dsa_switch *ds, int port,
 315                                 struct dsa_bridge bridge)
 316 {
 317         struct dsa_port *dp = dsa_to_port(ds, port);
 318         u16 standalone_vid, bridge_vid;
 319         int err;
 320
 321         /* Delete the bridging VLAN of the port and replace it with a
 322          * standalone VLAN
 323          */
 324         standalone_vid = dsa_tag_8021q_standalone_vid(dp);
 325         bridge_vid = dsa_tag_8021q_bridge_vid(bridge.num);
 326
 327         err = dsa_port_tag_8021q_vlan_add(dp, standalone_vid, false);
 328         if (err) {
 329                 dev_err(ds->dev,
 330                         "Failed to delete tag_8021q standalone VLAN %d from port %d: %pe\n",
 331                         standalone_vid, port, ERR_PTR(err));
 332         }
 333
 334         dsa_port_tag_8021q_vlan_del(dp, bridge_vid, true);
 335 }
 336 EXPORT_SYMBOL_GPL(dsa_tag_8021q_bridge_leave);
 337
 338 /* Set up a port's standalone tag_8021q VLAN */
 339 static int dsa_tag_8021q_port_setup(struct dsa_switch *ds, int port)
 340 {
 341         struct dsa_8021q_context *ctx = ds->tag_8021q_ctx;
 342         struct dsa_port *dp = dsa_to_port(ds, port);
 343         u16 vid = dsa_tag_8021q_standalone_vid(dp);
 344         struct net_device *conduit;
 345         int err;
 346
 347         /* The CPU port is implicitly configured by
 348          * configuring the front-panel ports
 349          */
 350         if (!dsa_port_is_user(dp))
 351                 return 0;
 352
 353         conduit = dsa_port_to_conduit(dp);
 354
 355         err = dsa_port_tag_8021q_vlan_add(dp, vid, false);
 356         if (err) {
 357                 dev_err(ds->dev,
 358                         "Failed to apply standalone VID %d to port %d: %pe\n",
 359                         vid, port, ERR_PTR(err));
 360                 return err;
 361         }
 362
 363         /* Add the VLAN to the conduit's RX filter. */
 364         vlan_vid_add(conduit, ctx->proto, vid);
 365
 366         return err;
 367 }
 368
 369 static void dsa_tag_8021q_port_teardown(struct dsa_switch *ds, int port)
 370 {
 371         struct dsa_8021q_context *ctx = ds->tag_8021q_ctx;
 372         struct dsa_port *dp = dsa_to_port(ds, port);
 373         u16 vid = dsa_tag_8021q_standalone_vid(dp);
 374         struct net_device *conduit;
 375
 376         /* The CPU port is implicitly configured by
 377          * configuring the front-panel ports
 378          */
 379         if (!dsa_port_is_user(dp))
 380                 return;
 381
 382         conduit = dsa_port_to_conduit(dp);
 383
 384         dsa_port_tag_8021q_vlan_del(dp, vid, false);
 385
 386         vlan_vid_del(conduit, ctx->proto, vid);
 387 }
 388
 389 static int dsa_tag_8021q_setup(struct dsa_switch *ds)
 390 {
 391         int err, port;
 392
 393         ASSERT_RTNL();
 394
 395         for (port = 0; port < ds->num_ports; port++) {
 396                 err = dsa_tag_8021q_port_setup(ds, port);
 397                 if (err < 0) {
 398                         dev_err(ds->dev,
 399                                 "Failed to setup VLAN tagging for port %d: %pe\n",
 400                                 port, ERR_PTR(err));
 401                         return err;
 402                 }
 403         }
 404
 405         return 0;
 406 }
 407
 408 static void dsa_tag_8021q_teardown(struct dsa_switch *ds)
 409 {
 410         int port;
 411
 412         ASSERT_RTNL();
 413
 414         for (port = 0; port < ds->num_ports; port++)
 415                 dsa_tag_8021q_port_teardown(ds, port);
 416 }
 417
 418 int dsa_tag_8021q_register(struct dsa_switch *ds, __be16 proto)
 419 {
 420         struct dsa_8021q_context *ctx;
 421         int err;
 422
 423         ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
 424         if (!ctx)
 425                 return -ENOMEM;
 426
 427         ctx->proto = proto;
 428         ctx->ds = ds;
 429
 430         INIT_LIST_HEAD(&ctx->vlans);
 431
 432         ds->tag_8021q_ctx = ctx;
 433
 434         err = dsa_tag_8021q_setup(ds);
 435         if (err)
 436                 goto err_free;
 437
 438         return 0;
 439
 440 err_free:
 441         kfree(ctx);
 442         return err;
 443 }
 444 EXPORT_SYMBOL_GPL(dsa_tag_8021q_register);
 445
 446 void dsa_tag_8021q_unregister(struct dsa_switch *ds)
 447 {
 448         struct dsa_8021q_context *ctx = ds->tag_8021q_ctx;
 449         struct dsa_tag_8021q_vlan *v, *n;
 450
 451         dsa_tag_8021q_teardown(ds);
 452
 453         list_for_each_entry_safe(v, n, &ctx->vlans, list) {
 454                 list_del(&v->list);
 455                 kfree(v);
 456         }
 457
 458         ds->tag_8021q_ctx = NULL;
 459
 460         kfree(ctx);
 461 }
 462 EXPORT_SYMBOL_GPL(dsa_tag_8021q_unregister);
 463
 464 struct sk_buff *dsa_8021q_xmit(struct sk_buff *skb, struct net_device *netdev,
 465                                u16 tpid, u16 tci)
 466 {
 467         /* skb->data points at the MAC header, which is fine
 468          * for vlan_insert_tag().
 469          */
 470         return vlan_insert_tag(skb, htons(tpid), tci);
 471 }
 472 EXPORT_SYMBOL_GPL(dsa_8021q_xmit);
 473
 474 static struct net_device *
 475 dsa_tag_8021q_find_port_by_vbid(struct net_device *conduit, int vbid)
 476 {
 477         struct dsa_port *cpu_dp = conduit->dsa_ptr;
 478         struct dsa_switch_tree *dst = cpu_dp->dst;
 479         struct dsa_port *dp;
 480
 481         if (WARN_ON(!vbid))
 482                 return NULL;
 483
 484         dsa_tree_for_each_user_port(dp, dst) {
 485                 if (!dp->bridge)
 486                         continue;
 487
 488                 if (dp->stp_state != BR_STATE_LEARNING &&
 489                     dp->stp_state != BR_STATE_FORWARDING)
 490                         continue;
 491
 492                 if (dp->cpu_dp != cpu_dp)
 493                         continue;
 494
 495                 if (dsa_port_bridge_num_get(dp) == vbid)
 496                         return dp->user;
 497         }
 498
 499         return NULL;
 500 }
 501
 502 struct net_device *dsa_tag_8021q_find_user(struct net_device *conduit,
 503                                            int source_port, int switch_id,
 504                                            int vid, int vbid)
 505 {
 506         /* Always prefer precise source port information, if available */
 507         if (source_port != -1 && switch_id != -1)
 508                 return dsa_conduit_find_user(conduit, switch_id, source_port);
 509         else if (vbid >= 1)
 510                 return dsa_tag_8021q_find_port_by_vbid(conduit, vbid);
 511
 512         return dsa_find_designated_bridge_port_by_vid(conduit, vid);
 513 }
 514 EXPORT_SYMBOL_GPL(dsa_tag_8021q_find_user);
 515
 516 /**
 517  * dsa_8021q_rcv - Decode source information from tag_8021q header
 518  * @skb: RX socket buffer
 519  * @source_port: pointer to storage for precise source port information.
 520  *      If this is known already from outside tag_8021q, the pre-initialized
 521  *      value is preserved. If not known, pass -1.
 522  * @switch_id: similar to source_port.
 523  * @vbid: pointer to storage for imprecise bridge ID. Must be pre-initialized
 524  *      with -1. If a positive value is returned, the source_port and switch_id
 525  *      are invalid.
 526  * @vid: pointer to storage for original VID, in case tag_8021q decoding failed.
 527  *
 528  * If the packet has a tag_8021q header, decode it and set @source_port,
 529  * @switch_id and @vbid, and strip the header. Otherwise set @vid and keep the
 530  * header in the hwaccel area of the packet.
 531  */
 532 void dsa_8021q_rcv(struct sk_buff *skb, int *source_port, int *switch_id,
 533                    int *vbid, int *vid)
 534 {
 535         int tmp_source_port, tmp_switch_id, tmp_vbid;
 536         __be16 vlan_proto;
 537         u16 tmp_vid, tci;
 538
 539         if (skb_vlan_tag_present(skb)) {
 540                 vlan_proto = skb->vlan_proto;
 541                 tci = skb_vlan_tag_get(skb);
 542                 __vlan_hwaccel_clear_tag(skb);
 543         } else {
 544                 struct vlan_ethhdr *hdr = vlan_eth_hdr(skb);
 545
 546                 vlan_proto = hdr->h_vlan_proto;
 547                 skb_push_rcsum(skb, ETH_HLEN);
 548                 __skb_vlan_pop(skb, &tci);
 549                 skb_pull_rcsum(skb, ETH_HLEN);
 550         }
 551
 552         tmp_vid = tci & VLAN_VID_MASK;
 553         if (!vid_is_dsa_8021q(tmp_vid)) {
 554                 /* Not a tag_8021q frame, so return the VID to the
 555                  * caller for further processing, and put the tag back
 556                  */
 557                 if (vid)
 558                         *vid = tmp_vid;
 559
 560                 __vlan_hwaccel_put_tag(skb, vlan_proto, tci);
 561
 562                 return;
 563         }
 564
 565         tmp_source_port = dsa_8021q_rx_source_port(tmp_vid);
 566         tmp_switch_id = dsa_8021q_rx_switch_id(tmp_vid);
 567         tmp_vbid = dsa_tag_8021q_rx_vbid(tmp_vid);
 568
 569         /* Precise source port information is unknown when receiving from a
 570          * VLAN-unaware bridging domain, and tmp_source_port and tmp_switch_id
 571          * are zeroes in this case.
 572          *
 573          * Preserve the source information from hardware-specific mechanisms,
 574          * if available. This allows us to not overwrite a valid source port
 575          * and switch ID with less precise values.
 576          */
 577         if (tmp_vbid == 0 && *source_port == -1)
 578                 *source_port = tmp_source_port;
 579         if (tmp_vbid == 0 && *switch_id == -1)
 580                 *switch_id = tmp_switch_id;
 581
 582         if (vbid)
 583                 *vbid = tmp_vbid;
 584
 585         skb->priority = (tci & VLAN_PRIO_MASK) >> VLAN_PRIO_SHIFT;
 586         return;
 587 }
 588 EXPORT_SYMBOL_GPL(dsa_8021q_rcv);