net/dsa/tag_sja1105.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /* Copyright (c) 2019, Vladimir Oltean <olteanv@gmail.com>
   3  */
   4 #include <linux/if_vlan.h>
   5 #include <linux/dsa/sja1105.h>
   6 #include <linux/dsa/8021q.h>
   7 #include <linux/packing.h>
   8
   9 #include "tag.h"
  10 #include "tag_8021q.h"
  11
  12 #define SJA1105_NAME                            "sja1105"
  13 #define SJA1110_NAME                            "sja1110"
  14
  15 /* Is this a TX or an RX header? */
  16 #define SJA1110_HEADER_HOST_TO_SWITCH           BIT(15)
  17
  18 /* RX header */
  19 #define SJA1110_RX_HEADER_IS_METADATA           BIT(14)
  20 #define SJA1110_RX_HEADER_HOST_ONLY             BIT(13)
  21 #define SJA1110_RX_HEADER_HAS_TRAILER           BIT(12)
  22
  23 /* Trap-to-host format (no trailer present) */
  24 #define SJA1110_RX_HEADER_SRC_PORT(x)           (((x) & GENMASK(7, 4)) >> 4)
  25 #define SJA1110_RX_HEADER_SWITCH_ID(x)          ((x) & GENMASK(3, 0))
  26
  27 /* Timestamp format (trailer present) */
  28 #define SJA1110_RX_HEADER_TRAILER_POS(x)        ((x) & GENMASK(11, 0))
  29
  30 #define SJA1110_RX_TRAILER_SWITCH_ID(x)         (((x) & GENMASK(7, 4)) >> 4)
  31 #define SJA1110_RX_TRAILER_SRC_PORT(x)          ((x) & GENMASK(3, 0))
  32
  33 /* Meta frame format (for 2-step TX timestamps) */
  34 #define SJA1110_RX_HEADER_N_TS(x)               (((x) & GENMASK(8, 4)) >> 4)
  35
  36 /* TX header */
  37 #define SJA1110_TX_HEADER_UPDATE_TC             BIT(14)
  38 #define SJA1110_TX_HEADER_TAKE_TS               BIT(13)
  39 #define SJA1110_TX_HEADER_TAKE_TS_CASC          BIT(12)
  40 #define SJA1110_TX_HEADER_HAS_TRAILER           BIT(11)
  41
  42 /* Only valid if SJA1110_TX_HEADER_HAS_TRAILER is false */
  43 #define SJA1110_TX_HEADER_PRIO(x)               (((x) << 7) & GENMASK(10, 7))
  44 #define SJA1110_TX_HEADER_TSTAMP_ID(x)          ((x) & GENMASK(7, 0))
  45
  46 /* Only valid if SJA1110_TX_HEADER_HAS_TRAILER is true */
  47 #define SJA1110_TX_HEADER_TRAILER_POS(x)        ((x) & GENMASK(10, 0))
  48
  49 #define SJA1110_TX_TRAILER_TSTAMP_ID(x)         (((x) << 24) & GENMASK(31, 24))
  50 #define SJA1110_TX_TRAILER_PRIO(x)              (((x) << 21) & GENMASK(23, 21))
  51 #define SJA1110_TX_TRAILER_SWITCHID(x)          (((x) << 12) & GENMASK(15, 12))
  52 #define SJA1110_TX_TRAILER_DESTPORTS(x)         (((x) << 1) & GENMASK(11, 1))
  53
  54 #define SJA1110_META_TSTAMP_SIZE                10
  55
  56 #define SJA1110_HEADER_LEN                      4
  57 #define SJA1110_RX_TRAILER_LEN                  13
  58 #define SJA1110_TX_TRAILER_LEN                  4
  59 #define SJA1110_MAX_PADDING_LEN                 15
  60
  61 struct sja1105_tagger_private {
  62         struct sja1105_tagger_data data; /* Must be first */
  63         /* Protects concurrent access to the meta state machine
  64          * from taggers running on multiple ports on SMP systems
  65          */
  66         spinlock_t meta_lock;
  67         struct sk_buff *stampable_skb;
  68         struct kthread_worker *xmit_worker;
  69 };
  70
  71 static struct sja1105_tagger_private *
  72 sja1105_tagger_private(struct dsa_switch *ds)
  73 {
  74         return ds->tagger_data;
  75 }
  76
  77 /* Similar to is_link_local_ether_addr(hdr->h_dest) but also covers PTP */
  78 static inline bool sja1105_is_link_local(const struct sk_buff *skb)
  79 {
  80         const struct ethhdr *hdr = eth_hdr(skb);
  81         u64 dmac = ether_addr_to_u64(hdr->h_dest);
  82
  83         if (ntohs(hdr->h_proto) == ETH_P_SJA1105_META)
  84                 return false;
  85         if ((dmac & SJA1105_LINKLOCAL_FILTER_A_MASK) ==
  86                     SJA1105_LINKLOCAL_FILTER_A)
  87                 return true;
  88         if ((dmac & SJA1105_LINKLOCAL_FILTER_B_MASK) ==
  89                     SJA1105_LINKLOCAL_FILTER_B)
  90                 return true;
  91         return false;
  92 }
  93
  94 struct sja1105_meta {
  95         u64 tstamp;
  96         u64 dmac_byte_4;
  97         u64 dmac_byte_3;
  98         u64 source_port;
  99         u64 switch_id;
 100 };
 101
 102 static void sja1105_meta_unpack(const struct sk_buff *skb,
 103                                 struct sja1105_meta *meta)
 104 {
 105         u8 *buf = skb_mac_header(skb) + ETH_HLEN;
 106
 107         /* UM10944.pdf section 4.2.17 AVB Parameters:
 108          * Structure of the meta-data follow-up frame.
 109          * It is in network byte order, so there are no quirks
 110          * while unpacking the meta frame.
 111          *
 112          * Also SJA1105 E/T only populates bits 23:0 of the timestamp
 113          * whereas P/Q/R/S does 32 bits. Since the structure is the
 114          * same and the E/T puts zeroes in the high-order byte, use
 115          * a unified unpacking command for both device series.
 116          */
 117         packing(buf,     &meta->tstamp,     31, 0, 4, UNPACK, 0);
 118         packing(buf + 4, &meta->dmac_byte_3, 7, 0, 1, UNPACK, 0);
 119         packing(buf + 5, &meta->dmac_byte_4, 7, 0, 1, UNPACK, 0);
 120         packing(buf + 6, &meta->source_port, 7, 0, 1, UNPACK, 0);
 121         packing(buf + 7, &meta->switch_id,   7, 0, 1, UNPACK, 0);
 122 }
 123
 124 static inline bool sja1105_is_meta_frame(const struct sk_buff *skb)
 125 {
 126         const struct ethhdr *hdr = eth_hdr(skb);
 127         u64 smac = ether_addr_to_u64(hdr->h_source);
 128         u64 dmac = ether_addr_to_u64(hdr->h_dest);
 129
 130         if (smac != SJA1105_META_SMAC)
 131                 return false;
 132         if (dmac != SJA1105_META_DMAC)
 133                 return false;
 134         if (ntohs(hdr->h_proto) != ETH_P_SJA1105_META)
 135                 return false;
 136         return true;
 137 }
 138
 139 /* Calls sja1105_port_deferred_xmit in sja1105_main.c */
 140 static struct sk_buff *sja1105_defer_xmit(struct dsa_port *dp,
 141                                           struct sk_buff *skb)
 142 {
 143         struct sja1105_tagger_data *tagger_data = sja1105_tagger_data(dp->ds);
 144         struct sja1105_tagger_private *priv = sja1105_tagger_private(dp->ds);
 145         void (*xmit_work_fn)(struct kthread_work *work);
 146         struct sja1105_deferred_xmit_work *xmit_work;
 147         struct kthread_worker *xmit_worker;
 148
 149         xmit_work_fn = tagger_data->xmit_work_fn;
 150         xmit_worker = priv->xmit_worker;
 151
 152         if (!xmit_work_fn || !xmit_worker)
 153                 return NULL;
 154
 155         xmit_work = kzalloc(sizeof(*xmit_work), GFP_ATOMIC);
 156         if (!xmit_work)
 157                 return NULL;
 158
 159         kthread_init_work(&xmit_work->work, xmit_work_fn);
 160         /* Increase refcount so the kfree_skb in dsa_slave_xmit
 161          * won't really free the packet.
 162          */
 163         xmit_work->dp = dp;
 164         xmit_work->skb = skb_get(skb);
 165
 166         kthread_queue_work(xmit_worker, &xmit_work->work);
 167
 168         return NULL;
 169 }
 170
 171 /* Send VLAN tags with a TPID that blends in with whatever VLAN protocol a
 172  * bridge spanning ports of this switch might have.
 173  */
 174 static u16 sja1105_xmit_tpid(struct dsa_port *dp)
 175 {
 176         struct dsa_switch *ds = dp->ds;
 177         struct dsa_port *other_dp;
 178         u16 proto;
 179
 180         /* Since VLAN awareness is global, then if this port is VLAN-unaware,
 181          * all ports are. Use the VLAN-unaware TPID used for tag_8021q.
 182          */
 183         if (!dsa_port_is_vlan_filtering(dp))
 184                 return ETH_P_SJA1105;
 185
 186         /* Port is VLAN-aware, so there is a bridge somewhere (a single one,
 187          * we're sure about that). It may not be on this port though, so we
 188          * need to find it.
 189          */
 190         dsa_switch_for_each_port(other_dp, ds) {
 191                 struct net_device *br = dsa_port_bridge_dev_get(other_dp);
 192
 193                 if (!br)
 194                         continue;
 195
 196                 /* Error is returned only if CONFIG_BRIDGE_VLAN_FILTERING,
 197                  * which seems pointless to handle, as our port cannot become
 198                  * VLAN-aware in that case.
 199                  */
 200                 br_vlan_get_proto(br, &proto);
 201
 202                 return proto;
 203         }
 204
 205         WARN_ONCE(1, "Port is VLAN-aware but cannot find associated bridge!\n");
 206
 207         return ETH_P_SJA1105;
 208 }
 209
 210 static struct sk_buff *sja1105_imprecise_xmit(struct sk_buff *skb,
 211                                               struct net_device *netdev)
 212 {
 213         struct dsa_port *dp = dsa_slave_to_port(netdev);
 214         unsigned int bridge_num = dsa_port_bridge_num_get(dp);
 215         struct net_device *br = dsa_port_bridge_dev_get(dp);
 216         u16 tx_vid;
 217
 218         /* If the port is under a VLAN-aware bridge, just slide the
 219          * VLAN-tagged packet into the FDB and hope for the best.
 220          * This works because we support a single VLAN-aware bridge
 221          * across the entire dst, and its VLANs cannot be shared with
 222          * any standalone port.
 223          */
 224         if (br_vlan_enabled(br))
 225                 return skb;
 226
 227         /* If the port is under a VLAN-unaware bridge, use an imprecise
 228          * TX VLAN that targets the bridge's entire broadcast domain,
 229          * instead of just the specific port.
 230          */
 231         tx_vid = dsa_tag_8021q_bridge_vid(bridge_num);
 232
 233         return dsa_8021q_xmit(skb, netdev, sja1105_xmit_tpid(dp), tx_vid);
 234 }
 235
 236 /* Transform untagged control packets into pvid-tagged control packets so that
 237  * all packets sent by this tagger are VLAN-tagged and we can configure the
 238  * switch to drop untagged packets coming from the DSA master.
 239  */
 240 static struct sk_buff *sja1105_pvid_tag_control_pkt(struct dsa_port *dp,
 241                                                     struct sk_buff *skb, u8 pcp)
 242 {
 243         __be16 xmit_tpid = htons(sja1105_xmit_tpid(dp));
 244         struct vlan_ethhdr *hdr;
 245
 246         /* If VLAN tag is in hwaccel area, move it to the payload
 247          * to deal with both cases uniformly and to ensure that
 248          * the VLANs are added in the right order.
 249          */
 250         if (unlikely(skb_vlan_tag_present(skb))) {
 251                 skb = __vlan_hwaccel_push_inside(skb);
 252                 if (!skb)
 253                         return NULL;
 254         }
 255
 256         hdr = skb_vlan_eth_hdr(skb);
 257
 258         /* If skb is already VLAN-tagged, leave that VLAN ID in place */
 259         if (hdr->h_vlan_proto == xmit_tpid)
 260                 return skb;
 261
 262         return vlan_insert_tag(skb, xmit_tpid, (pcp << VLAN_PRIO_SHIFT) |
 263                                SJA1105_DEFAULT_VLAN);
 264 }
 265
 266 static struct sk_buff *sja1105_xmit(struct sk_buff *skb,
 267                                     struct net_device *netdev)
 268 {
 269         struct dsa_port *dp = dsa_slave_to_port(netdev);
 270         u16 queue_mapping = skb_get_queue_mapping(skb);
 271         u8 pcp = netdev_txq_to_tc(netdev, queue_mapping);
 272         u16 tx_vid = dsa_tag_8021q_standalone_vid(dp);
 273
 274         if (skb->offload_fwd_mark)
 275                 return sja1105_imprecise_xmit(skb, netdev);
 276
 277         /* Transmitting management traffic does not rely upon switch tagging,
 278          * but instead SPI-installed management routes. Part 2 of this
 279          * is the .port_deferred_xmit driver callback.
 280          */
 281         if (unlikely(sja1105_is_link_local(skb))) {
 282                 skb = sja1105_pvid_tag_control_pkt(dp, skb, pcp);
 283                 if (!skb)
 284                         return NULL;
 285
 286                 return sja1105_defer_xmit(dp, skb);
 287         }
 288
 289         return dsa_8021q_xmit(skb, netdev, sja1105_xmit_tpid(dp),
 290                              ((pcp << VLAN_PRIO_SHIFT) | tx_vid));
 291 }
 292
 293 static struct sk_buff *sja1110_xmit(struct sk_buff *skb,
 294                                     struct net_device *netdev)
 295 {
 296         struct sk_buff *clone = SJA1105_SKB_CB(skb)->clone;
 297         struct dsa_port *dp = dsa_slave_to_port(netdev);
 298         u16 queue_mapping = skb_get_queue_mapping(skb);
 299         u8 pcp = netdev_txq_to_tc(netdev, queue_mapping);
 300         u16 tx_vid = dsa_tag_8021q_standalone_vid(dp);
 301         __be32 *tx_trailer;
 302         __be16 *tx_header;
 303         int trailer_pos;
 304
 305         if (skb->offload_fwd_mark)
 306                 return sja1105_imprecise_xmit(skb, netdev);
 307
 308         /* Transmitting control packets is done using in-band control
 309          * extensions, while data packets are transmitted using
 310          * tag_8021q TX VLANs.
 311          */
 312         if (likely(!sja1105_is_link_local(skb)))
 313                 return dsa_8021q_xmit(skb, netdev, sja1105_xmit_tpid(dp),
 314                                      ((pcp << VLAN_PRIO_SHIFT) | tx_vid));
 315
 316         skb = sja1105_pvid_tag_control_pkt(dp, skb, pcp);
 317         if (!skb)
 318                 return NULL;
 319
 320         skb_push(skb, SJA1110_HEADER_LEN);
 321
 322         dsa_alloc_etype_header(skb, SJA1110_HEADER_LEN);
 323
 324         trailer_pos = skb->len;
 325
 326         tx_header = dsa_etype_header_pos_tx(skb);
 327         tx_trailer = skb_put(skb, SJA1110_TX_TRAILER_LEN);
 328
 329         tx_header[0] = htons(ETH_P_SJA1110);
 330         tx_header[1] = htons(SJA1110_HEADER_HOST_TO_SWITCH |
 331                              SJA1110_TX_HEADER_HAS_TRAILER |
 332                              SJA1110_TX_HEADER_TRAILER_POS(trailer_pos));
 333         *tx_trailer = cpu_to_be32(SJA1110_TX_TRAILER_PRIO(pcp) |
 334                                   SJA1110_TX_TRAILER_SWITCHID(dp->ds->index) |
 335                                   SJA1110_TX_TRAILER_DESTPORTS(BIT(dp->index)));
 336         if (clone) {
 337                 u8 ts_id = SJA1105_SKB_CB(clone)->ts_id;
 338
 339                 tx_header[1] |= htons(SJA1110_TX_HEADER_TAKE_TS);
 340                 *tx_trailer |= cpu_to_be32(SJA1110_TX_TRAILER_TSTAMP_ID(ts_id));
 341         }
 342
 343         return skb;
 344 }
 345
 346 static void sja1105_transfer_meta(struct sk_buff *skb,
 347                                   const struct sja1105_meta *meta)
 348 {
 349         struct ethhdr *hdr = eth_hdr(skb);
 350
 351         hdr->h_dest[3] = meta->dmac_byte_3;
 352         hdr->h_dest[4] = meta->dmac_byte_4;
 353         SJA1105_SKB_CB(skb)->tstamp = meta->tstamp;
 354 }
 355
 356 /* This is a simple state machine which follows the hardware mechanism of
 357  * generating RX timestamps:
 358  *
 359  * After each timestampable skb (all traffic for which send_meta1 and
 360  * send_meta0 is true, aka all MAC-filtered link-local traffic) a meta frame
 361  * containing a partial timestamp is immediately generated by the switch and
 362  * sent as a follow-up to the link-local frame on the CPU port.
 363  *
 364  * The meta frames have no unique identifier (such as sequence number) by which
 365  * one may pair them to the correct timestampable frame.
 366  * Instead, the switch has internal logic that ensures no frames are sent on
 367  * the CPU port between a link-local timestampable frame and its corresponding
 368  * meta follow-up. It also ensures strict ordering between ports (lower ports
 369  * have higher priority towards the CPU port). For this reason, a per-port
 370  * data structure is not needed/desirable.
 371  *
 372  * This function pairs the link-local frame with its partial timestamp from the
 373  * meta follow-up frame. The full timestamp will be reconstructed later in a
 374  * work queue.
 375  */
 376 static struct sk_buff
 377 *sja1105_rcv_meta_state_machine(struct sk_buff *skb,
 378                                 struct sja1105_meta *meta,
 379                                 bool is_link_local,
 380                                 bool is_meta)
 381 {
 382         /* Step 1: A timestampable frame was received.
 383          * Buffer it until we get its meta frame.
 384          */
 385         if (is_link_local) {
 386                 struct dsa_port *dp = dsa_slave_to_port(skb->dev);
 387                 struct sja1105_tagger_private *priv;
 388                 struct dsa_switch *ds = dp->ds;
 389
 390                 priv = sja1105_tagger_private(ds);
 391
 392                 spin_lock(&priv->meta_lock);
 393                 /* Was this a link-local frame instead of the meta
 394                  * that we were expecting?
 395                  */
 396                 if (priv->stampable_skb) {
 397                         dev_err_ratelimited(ds->dev,
 398                                             "Expected meta frame, is %12llx "
 399                                             "in the DSA master multicast filter?\n",
 400                                             SJA1105_META_DMAC);
 401                         kfree_skb(priv->stampable_skb);
 402                 }
 403
 404                 /* Hold a reference to avoid dsa_switch_rcv
 405                  * from freeing the skb.
 406                  */
 407                 priv->stampable_skb = skb_get(skb);
 408                 spin_unlock(&priv->meta_lock);
 409
 410                 /* Tell DSA we got nothing */
 411                 return NULL;
 412
 413         /* Step 2: The meta frame arrived.
 414          * Time to take the stampable skb out of the closet, annotate it
 415          * with the partial timestamp, and pretend that we received it
 416          * just now (basically masquerade the buffered frame as the meta
 417          * frame, which serves no further purpose).
 418          */
 419         } else if (is_meta) {
 420                 struct dsa_port *dp = dsa_slave_to_port(skb->dev);
 421                 struct sja1105_tagger_private *priv;
 422                 struct dsa_switch *ds = dp->ds;
 423                 struct sk_buff *stampable_skb;
 424
 425                 priv = sja1105_tagger_private(ds);
 426
 427                 spin_lock(&priv->meta_lock);
 428
 429                 stampable_skb = priv->stampable_skb;
 430                 priv->stampable_skb = NULL;
 431
 432                 /* Was this a meta frame instead of the link-local
 433                  * that we were expecting?
 434                  */
 435                 if (!stampable_skb) {
 436                         dev_err_ratelimited(ds->dev,
 437                                             "Unexpected meta frame\n");
 438                         spin_unlock(&priv->meta_lock);
 439                         return NULL;
 440                 }
 441
 442                 if (stampable_skb->dev != skb->dev) {
 443                         dev_err_ratelimited(ds->dev,
 444                                             "Meta frame on wrong port\n");
 445                         spin_unlock(&priv->meta_lock);
 446                         return NULL;
 447                 }
 448
 449                 /* Free the meta frame and give DSA the buffered stampable_skb
 450                  * for further processing up the network stack.
 451                  */
 452                 kfree_skb(skb);
 453                 skb = stampable_skb;
 454                 sja1105_transfer_meta(skb, meta);
 455
 456                 spin_unlock(&priv->meta_lock);
 457         }
 458
 459         return skb;
 460 }
 461
 462 static bool sja1105_skb_has_tag_8021q(const struct sk_buff *skb)
 463 {
 464         u16 tpid = ntohs(eth_hdr(skb)->h_proto);
 465
 466         return tpid == ETH_P_SJA1105 || tpid == ETH_P_8021Q ||
 467                skb_vlan_tag_present(skb);
 468 }
 469
 470 static bool sja1110_skb_has_inband_control_extension(const struct sk_buff *skb)
 471 {
 472         return ntohs(eth_hdr(skb)->h_proto) == ETH_P_SJA1110;
 473 }
 474
 475 /* If the VLAN in the packet is a tag_8021q one, set @source_port and
 476  * @switch_id and strip the header. Otherwise set @vid and keep it in the
 477  * packet.
 478  */
 479 static void sja1105_vlan_rcv(struct sk_buff *skb, int *source_port,
 480                              int *switch_id, int *vbid, u16 *vid)
 481 {
 482         struct vlan_ethhdr *hdr = vlan_eth_hdr(skb);
 483         u16 vlan_tci;
 484
 485         if (skb_vlan_tag_present(skb))
 486                 vlan_tci = skb_vlan_tag_get(skb);
 487         else
 488                 vlan_tci = ntohs(hdr->h_vlan_TCI);
 489
 490         if (vid_is_dsa_8021q(vlan_tci & VLAN_VID_MASK))
 491                 return dsa_8021q_rcv(skb, source_port, switch_id, vbid);
 492
 493         /* Try our best with imprecise RX */
 494         *vid = vlan_tci & VLAN_VID_MASK;
 495 }
 496
 497 static struct sk_buff *sja1105_rcv(struct sk_buff *skb,
 498                                    struct net_device *netdev)
 499 {
 500         int source_port = -1, switch_id = -1, vbid = -1;
 501         struct sja1105_meta meta = {0};
 502         struct ethhdr *hdr;
 503         bool is_link_local;
 504         bool is_meta;
 505         u16 vid;
 506
 507         hdr = eth_hdr(skb);
 508         is_link_local = sja1105_is_link_local(skb);
 509         is_meta = sja1105_is_meta_frame(skb);
 510
 511         if (is_link_local) {
 512                 /* Management traffic path. Switch embeds the switch ID and
 513                  * port ID into bytes of the destination MAC, courtesy of
 514                  * the incl_srcpt options.
 515                  */
 516                 source_port = hdr->h_dest[3];
 517                 switch_id = hdr->h_dest[4];
 518         } else if (is_meta) {
 519                 sja1105_meta_unpack(skb, &meta);
 520                 source_port = meta.source_port;
 521                 switch_id = meta.switch_id;
 522         }
 523
 524         /* Normal data plane traffic and link-local frames are tagged with
 525          * a tag_8021q VLAN which we have to strip
 526          */
 527         if (sja1105_skb_has_tag_8021q(skb)) {
 528                 int tmp_source_port = -1, tmp_switch_id = -1;
 529
 530                 sja1105_vlan_rcv(skb, &tmp_source_port, &tmp_switch_id, &vbid,
 531                                  &vid);
 532                 /* Preserve the source information from the INCL_SRCPT option,
 533                  * if available. This allows us to not overwrite a valid source
 534                  * port and switch ID with zeroes when receiving link-local
 535                  * frames from a VLAN-unaware bridged port (non-zero vbid) or a
 536                  * VLAN-aware bridged port (non-zero vid). Furthermore, the
 537                  * tag_8021q source port information is only of trust when the
 538                  * vbid is 0 (precise port). Otherwise, tmp_source_port and
 539                  * tmp_switch_id will be zeroes.
 540                  */
 541                 if (vbid == 0 && source_port == -1)
 542                         source_port = tmp_source_port;
 543                 if (vbid == 0 && switch_id == -1)
 544                         switch_id = tmp_switch_id;
 545         } else if (source_port == -1 && switch_id == -1) {
 546                 /* Packets with no source information have no chance of
 547                  * getting accepted, drop them straight away.
 548                  */
 549                 return NULL;
 550         }
 551
 552         if (source_port != -1 && switch_id != -1)
 553                 skb->dev = dsa_master_find_slave(netdev, switch_id, source_port);
 554         else if (vbid >= 1)
 555                 skb->dev = dsa_tag_8021q_find_port_by_vbid(netdev, vbid);
 556         else
 557                 skb->dev = dsa_find_designated_bridge_port_by_vid(netdev, vid);
 558         if (!skb->dev) {
 559                 netdev_warn(netdev, "Couldn't decode source port\n");
 560                 return NULL;
 561         }
 562
 563         if (!is_link_local)
 564                 dsa_default_offload_fwd_mark(skb);
 565
 566         return sja1105_rcv_meta_state_machine(skb, &meta, is_link_local,
 567                                               is_meta);
 568 }
 569
 570 static struct sk_buff *sja1110_rcv_meta(struct sk_buff *skb, u16 rx_header)
 571 {
 572         u8 *buf = dsa_etype_header_pos_rx(skb) + SJA1110_HEADER_LEN;
 573         int switch_id = SJA1110_RX_HEADER_SWITCH_ID(rx_header);
 574         int n_ts = SJA1110_RX_HEADER_N_TS(rx_header);
 575         struct sja1105_tagger_data *tagger_data;
 576         struct net_device *master = skb->dev;
 577         struct dsa_port *cpu_dp;
 578         struct dsa_switch *ds;
 579         int i;
 580
 581         cpu_dp = master->dsa_ptr;
 582         ds = dsa_switch_find(cpu_dp->dst->index, switch_id);
 583         if (!ds) {
 584                 net_err_ratelimited("%s: cannot find switch id %d\n",
 585                                     master->name, switch_id);
 586                 return NULL;
 587         }
 588
 589         tagger_data = sja1105_tagger_data(ds);
 590         if (!tagger_data->meta_tstamp_handler)
 591                 return NULL;
 592
 593         for (i = 0; i <= n_ts; i++) {
 594                 u8 ts_id, source_port, dir;
 595                 u64 tstamp;
 596
 597                 ts_id = buf[0];
 598                 source_port = (buf[1] & GENMASK(7, 4)) >> 4;
 599                 dir = (buf[1] & BIT(3)) >> 3;
 600                 tstamp = be64_to_cpu(*(__be64 *)(buf + 2));
 601
 602                 tagger_data->meta_tstamp_handler(ds, source_port, ts_id, dir,
 603                                                  tstamp);
 604
 605                 buf += SJA1110_META_TSTAMP_SIZE;
 606         }
 607
 608         /* Discard the meta frame, we've consumed the timestamps it contained */
 609         return NULL;
 610 }
 611
 612 static struct sk_buff *sja1110_rcv_inband_control_extension(struct sk_buff *skb,
 613                                                             int *source_port,
 614                                                             int *switch_id,
 615                                                             bool *host_only)
 616 {
 617         u16 rx_header;
 618
 619         if (unlikely(!pskb_may_pull(skb, SJA1110_HEADER_LEN)))
 620                 return NULL;
 621
 622         /* skb->data points to skb_mac_header(skb) + ETH_HLEN, which is exactly
 623          * what we need because the caller has checked the EtherType (which is
 624          * located 2 bytes back) and we just need a pointer to the header that
 625          * comes afterwards.
 626          */
 627         rx_header = ntohs(*(__be16 *)skb->data);
 628
 629         if (rx_header & SJA1110_RX_HEADER_HOST_ONLY)
 630                 *host_only = true;
 631
 632         if (rx_header & SJA1110_RX_HEADER_IS_METADATA)
 633                 return sja1110_rcv_meta(skb, rx_header);
 634
 635         /* Timestamp frame, we have a trailer */
 636         if (rx_header & SJA1110_RX_HEADER_HAS_TRAILER) {
 637                 int start_of_padding = SJA1110_RX_HEADER_TRAILER_POS(rx_header);
 638                 u8 *rx_trailer = skb_tail_pointer(skb) - SJA1110_RX_TRAILER_LEN;
 639                 u64 *tstamp = &SJA1105_SKB_CB(skb)->tstamp;
 640                 u8 last_byte = rx_trailer[12];
 641
 642                 /* The timestamp is unaligned, so we need to use packing()
 643                  * to get it
 644                  */
 645                 packing(rx_trailer, tstamp, 63, 0, 8, UNPACK, 0);
 646
 647                 *source_port = SJA1110_RX_TRAILER_SRC_PORT(last_byte);
 648                 *switch_id = SJA1110_RX_TRAILER_SWITCH_ID(last_byte);
 649
 650                 /* skb->len counts from skb->data, while start_of_padding
 651                  * counts from the destination MAC address. Right now skb->data
 652                  * is still as set by the DSA master, so to trim away the
 653                  * padding and trailer we need to account for the fact that
 654                  * skb->data points to skb_mac_header(skb) + ETH_HLEN.
 655                  */
 656                 if (pskb_trim_rcsum(skb, start_of_padding - ETH_HLEN))
 657                         return NULL;
 658         /* Trap-to-host frame, no timestamp trailer */
 659         } else {
 660                 *source_port = SJA1110_RX_HEADER_SRC_PORT(rx_header);
 661                 *switch_id = SJA1110_RX_HEADER_SWITCH_ID(rx_header);
 662         }
 663
 664         /* Advance skb->data past the DSA header */
 665         skb_pull_rcsum(skb, SJA1110_HEADER_LEN);
 666
 667         dsa_strip_etype_header(skb, SJA1110_HEADER_LEN);
 668
 669         /* With skb->data in its final place, update the MAC header
 670          * so that eth_hdr() continues to works properly.
 671          */
 672         skb_set_mac_header(skb, -ETH_HLEN);
 673
 674         return skb;
 675 }
 676
 677 static struct sk_buff *sja1110_rcv(struct sk_buff *skb,
 678                                    struct net_device *netdev)
 679 {
 680         int source_port = -1, switch_id = -1, vbid = -1;
 681         bool host_only = false;
 682         u16 vid = 0;
 683
 684         if (sja1110_skb_has_inband_control_extension(skb)) {
 685                 skb = sja1110_rcv_inband_control_extension(skb, &source_port,
 686                                                            &switch_id,
 687                                                            &host_only);
 688                 if (!skb)
 689                         return NULL;
 690         }
 691
 692         /* Packets with in-band control extensions might still have RX VLANs */
 693         if (likely(sja1105_skb_has_tag_8021q(skb)))
 694                 sja1105_vlan_rcv(skb, &source_port, &switch_id, &vbid, &vid);
 695
 696         if (vbid >= 1)
 697                 skb->dev = dsa_tag_8021q_find_port_by_vbid(netdev, vbid);
 698         else if (source_port == -1 || switch_id == -1)
 699                 skb->dev = dsa_find_designated_bridge_port_by_vid(netdev, vid);
 700         else
 701                 skb->dev = dsa_master_find_slave(netdev, switch_id, source_port);
 702         if (!skb->dev) {
 703                 netdev_warn(netdev, "Couldn't decode source port\n");
 704                 return NULL;
 705         }
 706
 707         if (!host_only)
 708                 dsa_default_offload_fwd_mark(skb);
 709
 710         return skb;
 711 }
 712
 713 static void sja1105_flow_dissect(const struct sk_buff *skb, __be16 *proto,
 714                                  int *offset)
 715 {
 716         /* No tag added for management frames, all ok */
 717         if (unlikely(sja1105_is_link_local(skb)))
 718                 return;
 719
 720         dsa_tag_generic_flow_dissect(skb, proto, offset);
 721 }
 722
 723 static void sja1110_flow_dissect(const struct sk_buff *skb, __be16 *proto,
 724                                  int *offset)
 725 {
 726         /* Management frames have 2 DSA tags on RX, so the needed_headroom we
 727          * declared is fine for the generic dissector adjustment procedure.
 728          */
 729         if (unlikely(sja1105_is_link_local(skb)))
 730                 return dsa_tag_generic_flow_dissect(skb, proto, offset);
 731
 732         /* For the rest, there is a single DSA tag, the tag_8021q one */
 733         *offset = VLAN_HLEN;
 734         *proto = ((__be16 *)skb->data)[(VLAN_HLEN / 2) - 1];
 735 }
 736
 737 static void sja1105_disconnect(struct dsa_switch *ds)
 738 {
 739         struct sja1105_tagger_private *priv = ds->tagger_data;
 740
 741         kthread_destroy_worker(priv->xmit_worker);
 742         kfree(priv);
 743         ds->tagger_data = NULL;
 744 }
 745
 746 static int sja1105_connect(struct dsa_switch *ds)
 747 {
 748         struct sja1105_tagger_private *priv;
 749         struct kthread_worker *xmit_worker;
 750         int err;
 751
 752         priv = kzalloc(sizeof(*priv), GFP_KERNEL);
 753         if (!priv)
 754                 return -ENOMEM;
 755
 756         spin_lock_init(&priv->meta_lock);
 757
 758         xmit_worker = kthread_create_worker(0, "dsa%d:%d_xmit",
 759                                             ds->dst->index, ds->index);
 760         if (IS_ERR(xmit_worker)) {
 761                 err = PTR_ERR(xmit_worker);
 762                 kfree(priv);
 763                 return err;
 764         }
 765
 766         priv->xmit_worker = xmit_worker;
 767         ds->tagger_data = priv;
 768
 769         return 0;
 770 }
 771
 772 static const struct dsa_device_ops sja1105_netdev_ops = {
 773         .name = SJA1105_NAME,
 774         .proto = DSA_TAG_PROTO_SJA1105,
 775         .xmit = sja1105_xmit,
 776         .rcv = sja1105_rcv,
 777         .connect = sja1105_connect,
 778         .disconnect = sja1105_disconnect,
 779         .needed_headroom = VLAN_HLEN,
 780         .flow_dissect = sja1105_flow_dissect,
 781         .promisc_on_master = true,
 782 };
 783
 784 DSA_TAG_DRIVER(sja1105_netdev_ops);
 785 MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_SJA1105, SJA1105_NAME);
 786
 787 static const struct dsa_device_ops sja1110_netdev_ops = {
 788         .name = SJA1110_NAME,
 789         .proto = DSA_TAG_PROTO_SJA1110,
 790         .xmit = sja1110_xmit,
 791         .rcv = sja1110_rcv,
 792         .connect = sja1105_connect,
 793         .disconnect = sja1105_disconnect,
 794         .flow_dissect = sja1110_flow_dissect,
 795         .needed_headroom = SJA1110_HEADER_LEN + VLAN_HLEN,
 796         .needed_tailroom = SJA1110_RX_TRAILER_LEN + SJA1110_MAX_PADDING_LEN,
 797 };
 798
 799 DSA_TAG_DRIVER(sja1110_netdev_ops);
 800 MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_SJA1110, SJA1110_NAME);
 801
 802 static struct dsa_tag_driver *sja1105_tag_driver_array[] = {
 803         &DSA_TAG_DRIVER_NAME(sja1105_netdev_ops),
 804         &DSA_TAG_DRIVER_NAME(sja1110_netdev_ops),
 805 };
 806
 807 module_dsa_tag_drivers(sja1105_tag_driver_array);
 808
 809 MODULE_LICENSE("GPL v2");