linux/net/dsa/tag_sja1105.c
<<
>>
Prefs
   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/* Similar to is_link_local_ether_addr(hdr->h_dest) but also covers PTP */
  11static inline bool sja1105_is_link_local(const struct sk_buff *skb)
  12{
  13        const struct ethhdr *hdr = eth_hdr(skb);
  14        u64 dmac = ether_addr_to_u64(hdr->h_dest);
  15
  16        if (ntohs(hdr->h_proto) == ETH_P_SJA1105_META)
  17                return false;
  18        if ((dmac & SJA1105_LINKLOCAL_FILTER_A_MASK) ==
  19                    SJA1105_LINKLOCAL_FILTER_A)
  20                return true;
  21        if ((dmac & SJA1105_LINKLOCAL_FILTER_B_MASK) ==
  22                    SJA1105_LINKLOCAL_FILTER_B)
  23                return true;
  24        return false;
  25}
  26
  27struct sja1105_meta {
  28        u64 tstamp;
  29        u64 dmac_byte_4;
  30        u64 dmac_byte_3;
  31        u64 source_port;
  32        u64 switch_id;
  33};
  34
  35static void sja1105_meta_unpack(const struct sk_buff *skb,
  36                                struct sja1105_meta *meta)
  37{
  38        u8 *buf = skb_mac_header(skb) + ETH_HLEN;
  39
  40        /* UM10944.pdf section 4.2.17 AVB Parameters:
  41         * Structure of the meta-data follow-up frame.
  42         * It is in network byte order, so there are no quirks
  43         * while unpacking the meta frame.
  44         *
  45         * Also SJA1105 E/T only populates bits 23:0 of the timestamp
  46         * whereas P/Q/R/S does 32 bits. Since the structure is the
  47         * same and the E/T puts zeroes in the high-order byte, use
  48         * a unified unpacking command for both device series.
  49         */
  50        packing(buf,     &meta->tstamp,     31, 0, 4, UNPACK, 0);
  51        packing(buf + 4, &meta->dmac_byte_4, 7, 0, 1, UNPACK, 0);
  52        packing(buf + 5, &meta->dmac_byte_3, 7, 0, 1, UNPACK, 0);
  53        packing(buf + 6, &meta->source_port, 7, 0, 1, UNPACK, 0);
  54        packing(buf + 7, &meta->switch_id,   7, 0, 1, UNPACK, 0);
  55}
  56
  57static inline bool sja1105_is_meta_frame(const struct sk_buff *skb)
  58{
  59        const struct ethhdr *hdr = eth_hdr(skb);
  60        u64 smac = ether_addr_to_u64(hdr->h_source);
  61        u64 dmac = ether_addr_to_u64(hdr->h_dest);
  62
  63        if (smac != SJA1105_META_SMAC)
  64                return false;
  65        if (dmac != SJA1105_META_DMAC)
  66                return false;
  67        if (ntohs(hdr->h_proto) != ETH_P_SJA1105_META)
  68                return false;
  69        return true;
  70}
  71
  72/* This is the first time the tagger sees the frame on RX.
  73 * Figure out if we can decode it.
  74 */
  75static bool sja1105_filter(const struct sk_buff *skb, struct net_device *dev)
  76{
  77        if (!dsa_port_is_vlan_filtering(dev->dsa_ptr))
  78                return true;
  79        if (sja1105_is_link_local(skb))
  80                return true;
  81        if (sja1105_is_meta_frame(skb))
  82                return true;
  83        return false;
  84}
  85
  86/* Calls sja1105_port_deferred_xmit in sja1105_main.c */
  87static struct sk_buff *sja1105_defer_xmit(struct sja1105_port *sp,
  88                                          struct sk_buff *skb)
  89{
  90        /* Increase refcount so the kfree_skb in dsa_slave_xmit
  91         * won't really free the packet.
  92         */
  93        skb_queue_tail(&sp->xmit_queue, skb_get(skb));
  94        kthread_queue_work(sp->xmit_worker, &sp->xmit_work);
  95
  96        return NULL;
  97}
  98
  99static struct sk_buff *sja1105_xmit(struct sk_buff *skb,
 100                                    struct net_device *netdev)
 101{
 102        struct dsa_port *dp = dsa_slave_to_port(netdev);
 103        u16 tx_vid = dsa_8021q_tx_vid(dp->ds, dp->index);
 104        u16 queue_mapping = skb_get_queue_mapping(skb);
 105        u8 pcp = netdev_txq_to_tc(netdev, queue_mapping);
 106
 107        /* Transmitting management traffic does not rely upon switch tagging,
 108         * but instead SPI-installed management routes. Part 2 of this
 109         * is the .port_deferred_xmit driver callback.
 110         */
 111        if (unlikely(sja1105_is_link_local(skb)))
 112                return sja1105_defer_xmit(dp->priv, skb);
 113
 114        /* If we are under a vlan_filtering bridge, IP termination on
 115         * switch ports based on 802.1Q tags is simply too brittle to
 116         * be passable. So just defer to the dsa_slave_notag_xmit
 117         * implementation.
 118         */
 119        if (dsa_port_is_vlan_filtering(dp))
 120                return skb;
 121
 122        return dsa_8021q_xmit(skb, netdev, ETH_P_SJA1105,
 123                             ((pcp << VLAN_PRIO_SHIFT) | tx_vid));
 124}
 125
 126static void sja1105_transfer_meta(struct sk_buff *skb,
 127                                  const struct sja1105_meta *meta)
 128{
 129        struct ethhdr *hdr = eth_hdr(skb);
 130
 131        hdr->h_dest[3] = meta->dmac_byte_3;
 132        hdr->h_dest[4] = meta->dmac_byte_4;
 133        SJA1105_SKB_CB(skb)->meta_tstamp = meta->tstamp;
 134}
 135
 136/* This is a simple state machine which follows the hardware mechanism of
 137 * generating RX timestamps:
 138 *
 139 * After each timestampable skb (all traffic for which send_meta1 and
 140 * send_meta0 is true, aka all MAC-filtered link-local traffic) a meta frame
 141 * containing a partial timestamp is immediately generated by the switch and
 142 * sent as a follow-up to the link-local frame on the CPU port.
 143 *
 144 * The meta frames have no unique identifier (such as sequence number) by which
 145 * one may pair them to the correct timestampable frame.
 146 * Instead, the switch has internal logic that ensures no frames are sent on
 147 * the CPU port between a link-local timestampable frame and its corresponding
 148 * meta follow-up. It also ensures strict ordering between ports (lower ports
 149 * have higher priority towards the CPU port). For this reason, a per-port
 150 * data structure is not needed/desirable.
 151 *
 152 * This function pairs the link-local frame with its partial timestamp from the
 153 * meta follow-up frame. The full timestamp will be reconstructed later in a
 154 * work queue.
 155 */
 156static struct sk_buff
 157*sja1105_rcv_meta_state_machine(struct sk_buff *skb,
 158                                struct sja1105_meta *meta,
 159                                bool is_link_local,
 160                                bool is_meta)
 161{
 162        struct sja1105_port *sp;
 163        struct dsa_port *dp;
 164
 165        dp = dsa_slave_to_port(skb->dev);
 166        sp = dp->priv;
 167
 168        /* Step 1: A timestampable frame was received.
 169         * Buffer it until we get its meta frame.
 170         */
 171        if (is_link_local) {
 172                if (!test_bit(SJA1105_HWTS_RX_EN, &sp->data->state))
 173                        /* Do normal processing. */
 174                        return skb;
 175
 176                spin_lock(&sp->data->meta_lock);
 177                /* Was this a link-local frame instead of the meta
 178                 * that we were expecting?
 179                 */
 180                if (sp->data->stampable_skb) {
 181                        dev_err_ratelimited(dp->ds->dev,
 182                                            "Expected meta frame, is %12llx "
 183                                            "in the DSA master multicast filter?\n",
 184                                            SJA1105_META_DMAC);
 185                        kfree_skb(sp->data->stampable_skb);
 186                }
 187
 188                /* Hold a reference to avoid dsa_switch_rcv
 189                 * from freeing the skb.
 190                 */
 191                sp->data->stampable_skb = skb_get(skb);
 192                spin_unlock(&sp->data->meta_lock);
 193
 194                /* Tell DSA we got nothing */
 195                return NULL;
 196
 197        /* Step 2: The meta frame arrived.
 198         * Time to take the stampable skb out of the closet, annotate it
 199         * with the partial timestamp, and pretend that we received it
 200         * just now (basically masquerade the buffered frame as the meta
 201         * frame, which serves no further purpose).
 202         */
 203        } else if (is_meta) {
 204                struct sk_buff *stampable_skb;
 205
 206                /* Drop the meta frame if we're not in the right state
 207                 * to process it.
 208                 */
 209                if (!test_bit(SJA1105_HWTS_RX_EN, &sp->data->state))
 210                        return NULL;
 211
 212                spin_lock(&sp->data->meta_lock);
 213
 214                stampable_skb = sp->data->stampable_skb;
 215                sp->data->stampable_skb = NULL;
 216
 217                /* Was this a meta frame instead of the link-local
 218                 * that we were expecting?
 219                 */
 220                if (!stampable_skb) {
 221                        dev_err_ratelimited(dp->ds->dev,
 222                                            "Unexpected meta frame\n");
 223                        spin_unlock(&sp->data->meta_lock);
 224                        return NULL;
 225                }
 226
 227                if (stampable_skb->dev != skb->dev) {
 228                        dev_err_ratelimited(dp->ds->dev,
 229                                            "Meta frame on wrong port\n");
 230                        spin_unlock(&sp->data->meta_lock);
 231                        return NULL;
 232                }
 233
 234                /* Free the meta frame and give DSA the buffered stampable_skb
 235                 * for further processing up the network stack.
 236                 */
 237                kfree_skb(skb);
 238                skb = stampable_skb;
 239                sja1105_transfer_meta(skb, meta);
 240
 241                spin_unlock(&sp->data->meta_lock);
 242        }
 243
 244        return skb;
 245}
 246
 247static struct sk_buff *sja1105_rcv(struct sk_buff *skb,
 248                                   struct net_device *netdev,
 249                                   struct packet_type *pt)
 250{
 251        struct sja1105_meta meta = {0};
 252        int source_port, switch_id;
 253        struct ethhdr *hdr;
 254        u16 tpid, vid, tci;
 255        bool is_link_local;
 256        bool is_tagged;
 257        bool is_meta;
 258
 259        hdr = eth_hdr(skb);
 260        tpid = ntohs(hdr->h_proto);
 261        is_tagged = (tpid == ETH_P_SJA1105);
 262        is_link_local = sja1105_is_link_local(skb);
 263        is_meta = sja1105_is_meta_frame(skb);
 264
 265        skb->offload_fwd_mark = 1;
 266
 267        if (is_tagged) {
 268                /* Normal traffic path. */
 269                skb_push_rcsum(skb, ETH_HLEN);
 270                __skb_vlan_pop(skb, &tci);
 271                skb_pull_rcsum(skb, ETH_HLEN);
 272                skb_reset_network_header(skb);
 273                skb_reset_transport_header(skb);
 274
 275                vid = tci & VLAN_VID_MASK;
 276                source_port = dsa_8021q_rx_source_port(vid);
 277                switch_id = dsa_8021q_rx_switch_id(vid);
 278                skb->priority = (tci & VLAN_PRIO_MASK) >> VLAN_PRIO_SHIFT;
 279        } else if (is_link_local) {
 280                /* Management traffic path. Switch embeds the switch ID and
 281                 * port ID into bytes of the destination MAC, courtesy of
 282                 * the incl_srcpt options.
 283                 */
 284                source_port = hdr->h_dest[3];
 285                switch_id = hdr->h_dest[4];
 286                /* Clear the DMAC bytes that were mangled by the switch */
 287                hdr->h_dest[3] = 0;
 288                hdr->h_dest[4] = 0;
 289        } else if (is_meta) {
 290                sja1105_meta_unpack(skb, &meta);
 291                source_port = meta.source_port;
 292                switch_id = meta.switch_id;
 293        } else {
 294                return NULL;
 295        }
 296
 297        skb->dev = dsa_master_find_slave(netdev, switch_id, source_port);
 298        if (!skb->dev) {
 299                netdev_warn(netdev, "Couldn't decode source port\n");
 300                return NULL;
 301        }
 302
 303        return sja1105_rcv_meta_state_machine(skb, &meta, is_link_local,
 304                                              is_meta);
 305}
 306
 307static struct dsa_device_ops sja1105_netdev_ops = {
 308        .name = "sja1105",
 309        .proto = DSA_TAG_PROTO_SJA1105,
 310        .xmit = sja1105_xmit,
 311        .rcv = sja1105_rcv,
 312        .filter = sja1105_filter,
 313        .overhead = VLAN_HLEN,
 314};
 315
 316MODULE_LICENSE("GPL v2");
 317MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_SJA1105);
 318
 319module_dsa_tag_driver(sja1105_netdev_ops);
 320