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