net/dsa/tag_8021q.c

   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         int err;
 402
 403         ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
 404         if (!ctx)
 405                 return -ENOMEM;
 406
 407         ctx->proto = proto;
 408         ctx->ds = ds;
 409
 410         INIT_LIST_HEAD(&ctx->vlans);
 411
 412         ds->tag_8021q_ctx = ctx;
 413
 414         err = dsa_tag_8021q_setup(ds);
 415         if (err)
 416                 goto err_free;
 417
 418         return 0;
 419
 420 err_free:
 421         kfree(ctx);
 422         return err;
 423 }
 424 EXPORT_SYMBOL_GPL(dsa_tag_8021q_register);
 425
 426 void dsa_tag_8021q_unregister(struct dsa_switch *ds)
 427 {
 428         struct dsa_8021q_context *ctx = ds->tag_8021q_ctx;
 429         struct dsa_tag_8021q_vlan *v, *n;
 430
 431         dsa_tag_8021q_teardown(ds);
 432
 433         list_for_each_entry_safe(v, n, &ctx->vlans, list) {
 434                 list_del(&v->list);
 435                 kfree(v);
 436         }
 437
 438         ds->tag_8021q_ctx = NULL;
 439
 440         kfree(ctx);
 441 }
 442 EXPORT_SYMBOL_GPL(dsa_tag_8021q_unregister);
 443
 444 struct sk_buff *dsa_8021q_xmit(struct sk_buff *skb, struct net_device *netdev,
 445                                u16 tpid, u16 tci)
 446 {
 447         /* skb->data points at skb_mac_header, which
 448          * is fine for vlan_insert_tag.
 449          */
 450         return vlan_insert_tag(skb, htons(tpid), tci);
 451 }
 452 EXPORT_SYMBOL_GPL(dsa_8021q_xmit);
 453
 454 struct net_device *dsa_tag_8021q_find_port_by_vbid(struct net_device *master,
 455                                                    int vbid)
 456 {
 457         struct dsa_port *cpu_dp = master->dsa_ptr;
 458         struct dsa_switch_tree *dst = cpu_dp->dst;
 459         struct dsa_port *dp;
 460
 461         if (WARN_ON(!vbid))
 462                 return NULL;
 463
 464         dsa_tree_for_each_user_port(dp, dst) {
 465                 if (!dp->bridge)
 466                         continue;
 467
 468                 if (dp->stp_state != BR_STATE_LEARNING &&
 469                     dp->stp_state != BR_STATE_FORWARDING)
 470                         continue;
 471
 472                 if (dp->cpu_dp != cpu_dp)
 473                         continue;
 474
 475                 if (dsa_port_bridge_num_get(dp) == vbid)
 476                         return dp->slave;
 477         }
 478
 479         return NULL;
 480 }
 481 EXPORT_SYMBOL_GPL(dsa_tag_8021q_find_port_by_vbid);
 482
 483 void dsa_8021q_rcv(struct sk_buff *skb, int *source_port, int *switch_id,
 484                    int *vbid)
 485 {
 486         u16 vid, tci;
 487
 488         if (skb_vlan_tag_present(skb)) {
 489                 tci = skb_vlan_tag_get(skb);
 490                 __vlan_hwaccel_clear_tag(skb);
 491         } else {
 492                 skb_push_rcsum(skb, ETH_HLEN);
 493                 __skb_vlan_pop(skb, &tci);
 494                 skb_pull_rcsum(skb, ETH_HLEN);
 495         }
 496
 497         vid = tci & VLAN_VID_MASK;
 498
 499         *source_port = dsa_8021q_rx_source_port(vid);
 500         *switch_id = dsa_8021q_rx_switch_id(vid);
 501
 502         if (vbid)
 503                 *vbid = dsa_tag_8021q_rx_vbid(vid);
 504
 505         skb->priority = (tci & VLAN_PRIO_MASK) >> VLAN_PRIO_SHIFT;
 506 }
 507 EXPORT_SYMBOL_GPL(dsa_8021q_rcv);