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