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