net/dsa/tag_qca.c

0001 // SPDX-License-Identifier: GPL-2.0
0002 /*
0003  * Copyright (c) 2015, The Linux Foundation. All rights reserved.
0004  */
0005
0006 #include <linux/etherdevice.h>
0007 #include <linux/bitfield.h>
0008 #include <net/dsa.h>
0009 #include <linux/dsa/tag_qca.h>
0010
0011 #include "dsa_priv.h"
0012
0013 static struct sk_buff *qca_tag_xmit(struct sk_buff *skb, struct net_device *dev)
0014 {
0015     struct dsa_port *dp = dsa_slave_to_port(dev);
0016     __be16 *phdr;
0017     u16 hdr;
0018
0019     skb_push(skb, QCA_HDR_LEN);
0020
0021     dsa_alloc_etype_header(skb, QCA_HDR_LEN);
0022     phdr = dsa_etype_header_pos_tx(skb);
0023
0024     /* Set the version field, and set destination port information */
0025     hdr = FIELD_PREP(QCA_HDR_XMIT_VERSION, QCA_HDR_VERSION);
0026     hdr |= QCA_HDR_XMIT_FROM_CPU;
0027     hdr |= FIELD_PREP(QCA_HDR_XMIT_DP_BIT, BIT(dp->index));
0028
0029     *phdr = htons(hdr);
0030
0031     return skb;
0032 }
0033
0034 static struct sk_buff *qca_tag_rcv(struct sk_buff *skb, struct net_device *dev)
0035 {
0036     struct qca_tagger_data *tagger_data;
0037     struct dsa_port *dp = dev->dsa_ptr;
0038     struct dsa_switch *ds = dp->ds;
0039     u8 ver, pk_type;
0040     __be16 *phdr;
0041     int port;
0042     u16 hdr;
0043
0044     BUILD_BUG_ON(sizeof(struct qca_mgmt_ethhdr) != QCA_HDR_MGMT_HEADER_LEN + QCA_HDR_LEN);
0045
0046     tagger_data = ds->tagger_data;
0047
0048     if (unlikely(!pskb_may_pull(skb, QCA_HDR_LEN)))
0049         return NULL;
0050
0051     phdr = dsa_etype_header_pos_rx(skb);
0052     hdr = ntohs(*phdr);
0053
0054     /* Make sure the version is correct */
0055     ver = FIELD_GET(QCA_HDR_RECV_VERSION, hdr);
0056     if (unlikely(ver != QCA_HDR_VERSION))
0057         return NULL;
0058
0059     /* Get pk type */
0060     pk_type = FIELD_GET(QCA_HDR_RECV_TYPE, hdr);
0061
0062     /* Ethernet mgmt read/write packet */
0063     if (pk_type == QCA_HDR_RECV_TYPE_RW_REG_ACK) {
0064         if (likely(tagger_data->rw_reg_ack_handler))
0065             tagger_data->rw_reg_ack_handler(ds, skb);
0066         return NULL;
0067     }
0068
0069     /* Ethernet MIB counter packet */
0070     if (pk_type == QCA_HDR_RECV_TYPE_MIB) {
0071         if (likely(tagger_data->mib_autocast_handler))
0072             tagger_data->mib_autocast_handler(ds, skb);
0073         return NULL;
0074     }
0075
0076     /* Remove QCA tag and recalculate checksum */
0077     skb_pull_rcsum(skb, QCA_HDR_LEN);
0078     dsa_strip_etype_header(skb, QCA_HDR_LEN);
0079
0080     /* Get source port information */
0081     port = FIELD_GET(QCA_HDR_RECV_SOURCE_PORT, hdr);
0082
0083     skb->dev = dsa_master_find_slave(dev, 0, port);
0084     if (!skb->dev)
0085         return NULL;
0086
0087     return skb;
0088 }
0089
0090 static int qca_tag_connect(struct dsa_switch *ds)
0091 {
0092     struct qca_tagger_data *tagger_data;
0093
0094     tagger_data = kzalloc(sizeof(*tagger_data), GFP_KERNEL);
0095     if (!tagger_data)
0096         return -ENOMEM;
0097
0098     ds->tagger_data = tagger_data;
0099
0100     return 0;
0101 }
0102
0103 static void qca_tag_disconnect(struct dsa_switch *ds)
0104 {
0105     kfree(ds->tagger_data);
0106     ds->tagger_data = NULL;
0107 }
0108
0109 static const struct dsa_device_ops qca_netdev_ops = {
0110     .name   = "qca",
0111     .proto  = DSA_TAG_PROTO_QCA,
0112     .connect = qca_tag_connect,
0113     .disconnect = qca_tag_disconnect,
0114     .xmit   = qca_tag_xmit,
0115     .rcv    = qca_tag_rcv,
0116     .needed_headroom = QCA_HDR_LEN,
0117     .promisc_on_master = true,
0118 };
0119
0120 MODULE_LICENSE("GPL");
0121 MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_QCA);
0122
0123 module_dsa_tag_driver(qca_netdev_ops);