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); 124