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 /* Remove QCA tag and recalculate checksum */ 79 skb_pull_rcsum(skb, QCA_HDR_LEN); 80 dsa_strip_etype_header(skb, QCA_HDR_LEN); 81 82 /* Get source port information */ 83 port = FIELD_GET(QCA_HDR_RECV_SOURCE_PORT, hdr); 84 85 skb->dev = dsa_master_find_slave(dev, 0, port); 86 if (!skb->dev) 87 return NULL; 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); 126