net/dsa/tag_qca.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Copyright (c) 2015, The Linux Foundation. All rights reserved.
   4  */
   5
   6 #include <linux/etherdevice.h>
   7 #include <linux/bitfield.h>
   8 #include <net/dsa.h>
   9 #include <linux/dsa/tag_qca.h>
  10
  11 #include "dsa_priv.h"
  12
  13 static struct sk_buff *qca_tag_xmit(struct sk_buff *skb, struct net_device *dev)
  14 {
  15         struct dsa_port *dp = dsa_slave_to_port(dev);
  16         __be16 *phdr;
  17         u16 hdr;
  18
  19         skb_push(skb, QCA_HDR_LEN);
  20
  21         dsa_alloc_etype_header(skb, QCA_HDR_LEN);
  22         phdr = dsa_etype_header_pos_tx(skb);
  23
  24         /* Set the version field, and set destination port information */
  25         hdr = FIELD_PREP(QCA_HDR_XMIT_VERSION, QCA_HDR_VERSION);
  26         hdr |= QCA_HDR_XMIT_FROM_CPU;
  27         hdr |= FIELD_PREP(QCA_HDR_XMIT_DP_BIT, BIT(dp->index));
  28
  29         *phdr = htons(hdr);
  30
  31         return skb;
  32 }
  33
  34 static struct sk_buff *qca_tag_rcv(struct sk_buff *skb, struct net_device *dev)
  35 {
  36         struct qca_tagger_data *tagger_data;
  37         struct dsa_port *dp = dev->dsa_ptr;
  38         struct dsa_switch *ds = dp->ds;
  39         u8 ver, pk_type;
  40         __be16 *phdr;
  41         int port;
  42         u16 hdr;
  43
  44         BUILD_BUG_ON(sizeof(struct qca_mgmt_ethhdr) != QCA_HDR_MGMT_HEADER_LEN + QCA_HDR_LEN);
  45
  46         tagger_data = ds->tagger_data;
  47
  48         if (unlikely(!pskb_may_pull(skb, QCA_HDR_LEN)))
  49                 return NULL;
  50
  51         phdr = dsa_etype_header_pos_rx(skb);
  52         hdr = ntohs(*phdr);
  53
  54         /* Make sure the version is correct */
  55         ver = FIELD_GET(QCA_HDR_RECV_VERSION, hdr);
  56         if (unlikely(ver != QCA_HDR_VERSION))
  57                 return NULL;
  58
  59         /* Get pk type */
  60         pk_type = FIELD_GET(QCA_HDR_RECV_TYPE, hdr);
  61
  62         /* Ethernet mgmt read/write packet */
  63         if (pk_type == QCA_HDR_RECV_TYPE_RW_REG_ACK) {
  64                 if (likely(tagger_data->rw_reg_ack_handler))
  65                         tagger_data->rw_reg_ack_handler(ds, skb);
  66                 return NULL;
  67         }
  68
  69         /* Ethernet MIB counter packet */
  70         if (pk_type == QCA_HDR_RECV_TYPE_MIB) {
  71                 if (likely(tagger_data->mib_autocast_handler))
  72                         tagger_data->mib_autocast_handler(ds, skb);
  73                 return NULL;
  74         }
  75
  76         /* Remove QCA tag and recalculate checksum */
  77         skb_pull_rcsum(skb, QCA_HDR_LEN);
  78         dsa_strip_etype_header(skb, QCA_HDR_LEN);
  79
  80         /* Get source port information */
  81         port = FIELD_GET(QCA_HDR_RECV_SOURCE_PORT, hdr);
  82
  83         skb->dev = dsa_master_find_slave(dev, 0, port);
  84         if (!skb->dev)
  85                 return NULL;
  86
  87         return skb;
  88 }
  89
  90 static int qca_tag_connect(struct dsa_switch *ds)
  91 {
  92         struct qca_tagger_data *tagger_data;
  93
  94         tagger_data = kzalloc(sizeof(*tagger_data), GFP_KERNEL);
  95         if (!tagger_data)
  96                 return -ENOMEM;
  97
  98         ds->tagger_data = tagger_data;
  99
 100         return 0;
 101 }
 102
 103 static void qca_tag_disconnect(struct dsa_switch *ds)
 104 {
 105         kfree(ds->tagger_data);
 106         ds->tagger_data = NULL;
 107 }
 108
 109 static const struct dsa_device_ops qca_netdev_ops = {
 110         .name   = "qca",
 111         .proto  = DSA_TAG_PROTO_QCA,
 112         .connect = qca_tag_connect,
 113         .disconnect = qca_tag_disconnect,
 114         .xmit   = qca_tag_xmit,
 115         .rcv    = qca_tag_rcv,
 116         .needed_headroom = QCA_HDR_LEN,
 117         .promisc_on_master = true,
 118 };
 119
 120 MODULE_LICENSE("GPL");
 121 MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_QCA);
 122
 123 module_dsa_tag_driver(qca_netdev_ops);