net/dsa/tag_ksz.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2 /*
   3  * net/dsa/tag_ksz.c - Microchip KSZ Switch tag format handling
   4  * Copyright (c) 2017 Microchip Technology
   5  */
   6
   7 #include <linux/dsa/ksz_common.h>
   8 #include <linux/etherdevice.h>
   9 #include <linux/list.h>
  10 #include <linux/ptp_classify.h>
  11 #include <net/dsa.h>
  12
  13 #include "tag.h"
  14
  15 #define KSZ8795_NAME "ksz8795"
  16 #define KSZ9477_NAME "ksz9477"
  17 #define KSZ9893_NAME "ksz9893"
  18 #define LAN937X_NAME "lan937x"
  19
  20 /* Typically only one byte is used for tail tag. */
  21 #define KSZ_PTP_TAG_LEN                 4
  22 #define KSZ_EGRESS_TAG_LEN              1
  23 #define KSZ_INGRESS_TAG_LEN             1
  24
  25 #define KSZ_HWTS_EN  0
  26
  27 struct ksz_tagger_private {
  28         struct ksz_tagger_data data; /* Must be first */
  29         unsigned long state;
  30         struct kthread_worker *xmit_worker;
  31 };
  32
  33 static struct ksz_tagger_private *
  34 ksz_tagger_private(struct dsa_switch *ds)
  35 {
  36         return ds->tagger_data;
  37 }
  38
  39 static void ksz_hwtstamp_set_state(struct dsa_switch *ds, bool on)
  40 {
  41         struct ksz_tagger_private *priv = ksz_tagger_private(ds);
  42
  43         if (on)
  44                 set_bit(KSZ_HWTS_EN, &priv->state);
  45         else
  46                 clear_bit(KSZ_HWTS_EN, &priv->state);
  47 }
  48
  49 static void ksz_disconnect(struct dsa_switch *ds)
  50 {
  51         struct ksz_tagger_private *priv = ds->tagger_data;
  52
  53         kthread_destroy_worker(priv->xmit_worker);
  54         kfree(priv);
  55         ds->tagger_data = NULL;
  56 }
  57
  58 static int ksz_connect(struct dsa_switch *ds)
  59 {
  60         struct ksz_tagger_data *tagger_data;
  61         struct kthread_worker *xmit_worker;
  62         struct ksz_tagger_private *priv;
  63         int ret;
  64
  65         priv = kzalloc(sizeof(*priv), GFP_KERNEL);
  66         if (!priv)
  67                 return -ENOMEM;
  68
  69         xmit_worker = kthread_create_worker(0, "dsa%d:%d_xmit",
  70                                             ds->dst->index, ds->index);
  71         if (IS_ERR(xmit_worker)) {
  72                 ret = PTR_ERR(xmit_worker);
  73                 kfree(priv);
  74                 return ret;
  75         }
  76
  77         priv->xmit_worker = xmit_worker;
  78         /* Export functions for switch driver use */
  79         tagger_data = &priv->data;
  80         tagger_data->hwtstamp_set_state = ksz_hwtstamp_set_state;
  81         ds->tagger_data = priv;
  82
  83         return 0;
  84 }
  85
  86 static struct sk_buff *ksz_common_rcv(struct sk_buff *skb,
  87                                       struct net_device *dev,
  88                                       unsigned int port, unsigned int len)
  89 {
  90         skb->dev = dsa_conduit_find_user(dev, 0, port);
  91         if (!skb->dev)
  92                 return NULL;
  93
  94         if (pskb_trim_rcsum(skb, skb->len - len))
  95                 return NULL;
  96
  97         dsa_default_offload_fwd_mark(skb);
  98
  99         return skb;
 100 }
 101
 102 /*
 103  * For Ingress (Host -> KSZ8795), 1 byte is added before FCS.
 104  * ---------------------------------------------------------------------------
 105  * DA(6bytes)|SA(6bytes)|....|Data(nbytes)|tag(1byte)|FCS(4bytes)
 106  * ---------------------------------------------------------------------------
 107  * tag : each bit represents port (eg, 0x01=port1, 0x02=port2, 0x10=port5)
 108  *
 109  * For Egress (KSZ8795 -> Host), 1 byte is added before FCS.
 110  * ---------------------------------------------------------------------------
 111  * DA(6bytes)|SA(6bytes)|....|Data(nbytes)|tag0(1byte)|FCS(4bytes)
 112  * ---------------------------------------------------------------------------
 113  * tag0 : zero-based value represents port
 114  *        (eg, 0x0=port1, 0x2=port3, 0x3=port4)
 115  */
 116
 117 #define KSZ8795_TAIL_TAG_EG_PORT_M      GENMASK(1, 0)
 118 #define KSZ8795_TAIL_TAG_OVERRIDE       BIT(6)
 119 #define KSZ8795_TAIL_TAG_LOOKUP         BIT(7)
 120
 121 static struct sk_buff *ksz8795_xmit(struct sk_buff *skb, struct net_device *dev)
 122 {
 123         struct dsa_port *dp = dsa_user_to_port(dev);
 124         struct ethhdr *hdr;
 125         u8 *tag;
 126
 127         if (skb->ip_summed == CHECKSUM_PARTIAL && skb_checksum_help(skb))
 128                 return NULL;
 129
 130         /* Tag encoding */
 131         tag = skb_put(skb, KSZ_INGRESS_TAG_LEN);
 132         hdr = skb_eth_hdr(skb);
 133
 134         *tag = 1 << dp->index;
 135         if (is_link_local_ether_addr(hdr->h_dest))
 136                 *tag |= KSZ8795_TAIL_TAG_OVERRIDE;
 137
 138         return skb;
 139 }
 140
 141 static struct sk_buff *ksz8795_rcv(struct sk_buff *skb, struct net_device *dev)
 142 {
 143         u8 *tag = skb_tail_pointer(skb) - KSZ_EGRESS_TAG_LEN;
 144
 145         return ksz_common_rcv(skb, dev, tag[0] & KSZ8795_TAIL_TAG_EG_PORT_M,
 146                               KSZ_EGRESS_TAG_LEN);
 147 }
 148
 149 static const struct dsa_device_ops ksz8795_netdev_ops = {
 150         .name   = KSZ8795_NAME,
 151         .proto  = DSA_TAG_PROTO_KSZ8795,
 152         .xmit   = ksz8795_xmit,
 153         .rcv    = ksz8795_rcv,
 154         .needed_tailroom = KSZ_INGRESS_TAG_LEN,
 155 };
 156
 157 DSA_TAG_DRIVER(ksz8795_netdev_ops);
 158 MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_KSZ8795, KSZ8795_NAME);
 159
 160 /*
 161  * For Ingress (Host -> KSZ9477), 2/6 bytes are added before FCS.
 162  * ---------------------------------------------------------------------------
 163  * DA(6bytes)|SA(6bytes)|....|Data(nbytes)|ts(4bytes)|tag0(1byte)|tag1(1byte)|
 164  * FCS(4bytes)
 165  * ---------------------------------------------------------------------------
 166  * ts   : time stamp (Present only if PTP is enabled in the Hardware)
 167  * tag0 : Prioritization (not used now)
 168  * tag1 : each bit represents port (eg, 0x01=port1, 0x02=port2, 0x10=port5)
 169  *
 170  * For Egress (KSZ9477 -> Host), 1/5 bytes is added before FCS.
 171  * ---------------------------------------------------------------------------
 172  * DA(6bytes)|SA(6bytes)|....|Data(nbytes)|ts(4bytes)|tag0(1byte)|FCS(4bytes)
 173  * ---------------------------------------------------------------------------
 174  * ts   : time stamp (Present only if bit 7 of tag0 is set)
 175  * tag0 : zero-based value represents port
 176  *        (eg, 0x00=port1, 0x02=port3, 0x06=port7)
 177  */
 178
 179 #define KSZ9477_INGRESS_TAG_LEN         2
 180 #define KSZ9477_PTP_TAG_LEN             4
 181 #define KSZ9477_PTP_TAG_INDICATION      BIT(7)
 182
 183 #define KSZ9477_TAIL_TAG_EG_PORT_M      GENMASK(2, 0)
 184 #define KSZ9477_TAIL_TAG_PRIO           GENMASK(8, 7)
 185 #define KSZ9477_TAIL_TAG_OVERRIDE       BIT(9)
 186 #define KSZ9477_TAIL_TAG_LOOKUP         BIT(10)
 187
 188 static void ksz_rcv_timestamp(struct sk_buff *skb, u8 *tag)
 189 {
 190         u8 *tstamp_raw = tag - KSZ_PTP_TAG_LEN;
 191         ktime_t tstamp;
 192
 193         tstamp = ksz_decode_tstamp(get_unaligned_be32(tstamp_raw));
 194         KSZ_SKB_CB(skb)->tstamp = tstamp;
 195 }
 196
 197 /* Time stamp tag *needs* to be inserted if PTP is enabled in hardware.
 198  * Regardless of Whether it is a PTP frame or not.
 199  */
 200 static void ksz_xmit_timestamp(struct dsa_port *dp, struct sk_buff *skb)
 201 {
 202         struct ksz_tagger_private *priv;
 203         struct ptp_header *ptp_hdr;
 204         unsigned int ptp_type;
 205         u32 tstamp_raw = 0;
 206         s64 correction;
 207
 208         priv = ksz_tagger_private(dp->ds);
 209
 210         if (!test_bit(KSZ_HWTS_EN, &priv->state))
 211                 return;
 212
 213         if (!KSZ_SKB_CB(skb)->update_correction)
 214                 goto output_tag;
 215
 216         ptp_type = KSZ_SKB_CB(skb)->ptp_type;
 217
 218         ptp_hdr = ptp_parse_header(skb, ptp_type);
 219         if (!ptp_hdr)
 220                 goto output_tag;
 221
 222         correction = (s64)get_unaligned_be64(&ptp_hdr->correction);
 223
 224         if (correction < 0) {
 225                 struct timespec64 ts;
 226
 227                 ts = ns_to_timespec64(-correction >> 16);
 228                 tstamp_raw = ((ts.tv_sec & 3) << 30) | ts.tv_nsec;
 229
 230                 /* Set correction field to 0 and update UDP checksum */
 231                 ptp_header_update_correction(skb, ptp_type, ptp_hdr, 0);
 232         }
 233
 234 output_tag:
 235         put_unaligned_be32(tstamp_raw, skb_put(skb, KSZ_PTP_TAG_LEN));
 236 }
 237
 238 /* Defer transmit if waiting for egress time stamp is required.  */
 239 static struct sk_buff *ksz_defer_xmit(struct dsa_port *dp, struct sk_buff *skb)
 240 {
 241         struct ksz_tagger_data *tagger_data = ksz_tagger_data(dp->ds);
 242         struct ksz_tagger_private *priv = ksz_tagger_private(dp->ds);
 243         void (*xmit_work_fn)(struct kthread_work *work);
 244         struct sk_buff *clone = KSZ_SKB_CB(skb)->clone;
 245         struct ksz_deferred_xmit_work *xmit_work;
 246         struct kthread_worker *xmit_worker;
 247
 248         if (!clone)
 249                 return skb;  /* no deferred xmit for this packet */
 250
 251         xmit_work_fn = tagger_data->xmit_work_fn;
 252         xmit_worker = priv->xmit_worker;
 253
 254         if (!xmit_work_fn || !xmit_worker)
 255                 return NULL;
 256
 257         xmit_work = kzalloc(sizeof(*xmit_work), GFP_ATOMIC);
 258         if (!xmit_work)
 259                 return NULL;
 260
 261         kthread_init_work(&xmit_work->work, xmit_work_fn);
 262         /* Increase refcount so the kfree_skb in dsa_user_xmit
 263          * won't really free the packet.
 264          */
 265         xmit_work->dp = dp;
 266         xmit_work->skb = skb_get(skb);
 267
 268         kthread_queue_work(xmit_worker, &xmit_work->work);
 269
 270         return NULL;
 271 }
 272
 273 static struct sk_buff *ksz9477_xmit(struct sk_buff *skb,
 274                                     struct net_device *dev)
 275 {
 276         u16 queue_mapping = skb_get_queue_mapping(skb);
 277         u8 prio = netdev_txq_to_tc(dev, queue_mapping);
 278         struct dsa_port *dp = dsa_user_to_port(dev);
 279         struct ethhdr *hdr;
 280         __be16 *tag;
 281         u16 val;
 282
 283         if (skb->ip_summed == CHECKSUM_PARTIAL && skb_checksum_help(skb))
 284                 return NULL;
 285
 286         /* Tag encoding */
 287         ksz_xmit_timestamp(dp, skb);
 288
 289         tag = skb_put(skb, KSZ9477_INGRESS_TAG_LEN);
 290         hdr = skb_eth_hdr(skb);
 291
 292         val = BIT(dp->index);
 293
 294         val |= FIELD_PREP(KSZ9477_TAIL_TAG_PRIO, prio);
 295
 296         if (is_link_local_ether_addr(hdr->h_dest))
 297                 val |= KSZ9477_TAIL_TAG_OVERRIDE;
 298
 299         if (dev->features & NETIF_F_HW_HSR_DUP) {
 300                 struct net_device *hsr_dev = dp->hsr_dev;
 301                 struct dsa_port *other_dp;
 302
 303                 dsa_hsr_foreach_port(other_dp, dp->ds, hsr_dev)
 304                         val |= BIT(other_dp->index);
 305         }
 306
 307         *tag = cpu_to_be16(val);
 308
 309         return ksz_defer_xmit(dp, skb);
 310 }
 311
 312 static struct sk_buff *ksz9477_rcv(struct sk_buff *skb, struct net_device *dev)
 313 {
 314         /* Tag decoding */
 315         u8 *tag = skb_tail_pointer(skb) - KSZ_EGRESS_TAG_LEN;
 316         unsigned int port = tag[0] & KSZ9477_TAIL_TAG_EG_PORT_M;
 317         unsigned int len = KSZ_EGRESS_TAG_LEN;
 318
 319         /* Extra 4-bytes PTP timestamp */
 320         if (tag[0] & KSZ9477_PTP_TAG_INDICATION) {
 321                 ksz_rcv_timestamp(skb, tag);
 322                 len += KSZ_PTP_TAG_LEN;
 323         }
 324
 325         return ksz_common_rcv(skb, dev, port, len);
 326 }
 327
 328 static const struct dsa_device_ops ksz9477_netdev_ops = {
 329         .name   = KSZ9477_NAME,
 330         .proto  = DSA_TAG_PROTO_KSZ9477,
 331         .xmit   = ksz9477_xmit,
 332         .rcv    = ksz9477_rcv,
 333         .connect = ksz_connect,
 334         .disconnect = ksz_disconnect,
 335         .needed_tailroom = KSZ9477_INGRESS_TAG_LEN + KSZ_PTP_TAG_LEN,
 336 };
 337
 338 DSA_TAG_DRIVER(ksz9477_netdev_ops);
 339 MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_KSZ9477, KSZ9477_NAME);
 340
 341 #define KSZ9893_TAIL_TAG_PRIO           GENMASK(4, 3)
 342 #define KSZ9893_TAIL_TAG_OVERRIDE       BIT(5)
 343 #define KSZ9893_TAIL_TAG_LOOKUP         BIT(6)
 344
 345 static struct sk_buff *ksz9893_xmit(struct sk_buff *skb,
 346                                     struct net_device *dev)
 347 {
 348         u16 queue_mapping = skb_get_queue_mapping(skb);
 349         u8 prio = netdev_txq_to_tc(dev, queue_mapping);
 350         struct dsa_port *dp = dsa_user_to_port(dev);
 351         struct ethhdr *hdr;
 352         u8 *tag;
 353
 354         if (skb->ip_summed == CHECKSUM_PARTIAL && skb_checksum_help(skb))
 355                 return NULL;
 356
 357         /* Tag encoding */
 358         ksz_xmit_timestamp(dp, skb);
 359
 360         tag = skb_put(skb, KSZ_INGRESS_TAG_LEN);
 361         hdr = skb_eth_hdr(skb);
 362
 363         *tag = BIT(dp->index);
 364
 365         *tag |= FIELD_PREP(KSZ9893_TAIL_TAG_PRIO, prio);
 366
 367         if (is_link_local_ether_addr(hdr->h_dest))
 368                 *tag |= KSZ9893_TAIL_TAG_OVERRIDE;
 369
 370         return ksz_defer_xmit(dp, skb);
 371 }
 372
 373 static const struct dsa_device_ops ksz9893_netdev_ops = {
 374         .name   = KSZ9893_NAME,
 375         .proto  = DSA_TAG_PROTO_KSZ9893,
 376         .xmit   = ksz9893_xmit,
 377         .rcv    = ksz9477_rcv,
 378         .connect = ksz_connect,
 379         .disconnect = ksz_disconnect,
 380         .needed_tailroom = KSZ_INGRESS_TAG_LEN + KSZ_PTP_TAG_LEN,
 381 };
 382
 383 DSA_TAG_DRIVER(ksz9893_netdev_ops);
 384 MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_KSZ9893, KSZ9893_NAME);
 385
 386 /* For xmit, 2/6 bytes are added before FCS.
 387  * ---------------------------------------------------------------------------
 388  * DA(6bytes)|SA(6bytes)|....|Data(nbytes)|ts(4bytes)|tag0(1byte)|tag1(1byte)|
 389  * FCS(4bytes)
 390  * ---------------------------------------------------------------------------
 391  * ts   : time stamp (Present only if PTP is enabled in the Hardware)
 392  * tag0 : represents tag override, lookup and valid
 393  * tag1 : each bit represents port (eg, 0x01=port1, 0x02=port2, 0x80=port8)
 394  *
 395  * For rcv, 1/5 bytes is added before FCS.
 396  * ---------------------------------------------------------------------------
 397  * DA(6bytes)|SA(6bytes)|....|Data(nbytes)|ts(4bytes)|tag0(1byte)|FCS(4bytes)
 398  * ---------------------------------------------------------------------------
 399  * ts   : time stamp (Present only if bit 7 of tag0 is set)
 400  * tag0 : zero-based value represents port
 401  *        (eg, 0x00=port1, 0x02=port3, 0x07=port8)
 402  */
 403 #define LAN937X_EGRESS_TAG_LEN          2
 404
 405 #define LAN937X_TAIL_TAG_BLOCKING_OVERRIDE      BIT(11)
 406 #define LAN937X_TAIL_TAG_LOOKUP                 BIT(12)
 407 #define LAN937X_TAIL_TAG_VALID                  BIT(13)
 408 #define LAN937X_TAIL_TAG_PRIO                   GENMASK(10, 8)
 409 #define LAN937X_TAIL_TAG_PORT_MASK              7
 410
 411 static struct sk_buff *lan937x_xmit(struct sk_buff *skb,
 412                                     struct net_device *dev)
 413 {
 414         u16 queue_mapping = skb_get_queue_mapping(skb);
 415         u8 prio = netdev_txq_to_tc(dev, queue_mapping);
 416         struct dsa_port *dp = dsa_user_to_port(dev);
 417         const struct ethhdr *hdr = eth_hdr(skb);
 418         __be16 *tag;
 419         u16 val;
 420
 421         if (skb->ip_summed == CHECKSUM_PARTIAL && skb_checksum_help(skb))
 422                 return NULL;
 423
 424         ksz_xmit_timestamp(dp, skb);
 425
 426         tag = skb_put(skb, LAN937X_EGRESS_TAG_LEN);
 427
 428         val = BIT(dp->index);
 429
 430         val |= FIELD_PREP(LAN937X_TAIL_TAG_PRIO, prio);
 431
 432         if (is_link_local_ether_addr(hdr->h_dest))
 433                 val |= LAN937X_TAIL_TAG_BLOCKING_OVERRIDE;
 434
 435         /* Tail tag valid bit - This bit should always be set by the CPU */
 436         val |= LAN937X_TAIL_TAG_VALID;
 437
 438         put_unaligned_be16(val, tag);
 439
 440         return ksz_defer_xmit(dp, skb);
 441 }
 442
 443 static const struct dsa_device_ops lan937x_netdev_ops = {
 444         .name   = LAN937X_NAME,
 445         .proto  = DSA_TAG_PROTO_LAN937X,
 446         .xmit   = lan937x_xmit,
 447         .rcv    = ksz9477_rcv,
 448         .connect = ksz_connect,
 449         .disconnect = ksz_disconnect,
 450         .needed_tailroom = LAN937X_EGRESS_TAG_LEN + KSZ_PTP_TAG_LEN,
 451 };
 452
 453 DSA_TAG_DRIVER(lan937x_netdev_ops);
 454 MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_LAN937X, LAN937X_NAME);
 455
 456 static struct dsa_tag_driver *dsa_tag_driver_array[] = {
 457         &DSA_TAG_DRIVER_NAME(ksz8795_netdev_ops),
 458         &DSA_TAG_DRIVER_NAME(ksz9477_netdev_ops),
 459         &DSA_TAG_DRIVER_NAME(ksz9893_netdev_ops),
 460         &DSA_TAG_DRIVER_NAME(lan937x_netdev_ops),
 461 };
 462
 463 module_dsa_tag_drivers(dsa_tag_driver_array);
 464
 465 MODULE_DESCRIPTION("DSA tag driver for Microchip 8795/937x/9477/9893 families of switches");
 466 MODULE_LICENSE("GPL");