1 // SPDX-License-Identifier: GPL-2.0 2 /* 3 * Mediatek DSA Tag support 4 * Copyright (C) 2017 Landen Chao <landen.chao@mediatek.com> 5 * Sean Wang <sean.wang@mediatek.com> 6 */ 7 8 #include <linux/etherdevice.h> 9 #include <linux/if_vlan.h> 10 11 #include "dsa_priv.h" 12 13 #define MTK_HDR_LEN 4 14 #define MTK_HDR_XMIT_UNTAGGED 0 15 #define MTK_HDR_XMIT_TAGGED_TPID_8100 1 16 #define MTK_HDR_RECV_SOURCE_PORT_MASK GENMASK(2, 0) 17 #define MTK_HDR_XMIT_DP_BIT_MASK GENMASK(5, 0) 18 19 static struct sk_buff *mtk_tag_xmit(struct sk_buff *skb, 20 struct net_device *dev) 21 { 22 struct dsa_port *dp = dsa_slave_to_port(dev); 23 u8 *mtk_tag; 24 bool is_vlan_skb = true; 25 26 /* Build the special tag after the MAC Source Address. If VLAN header 27 * is present, it's required that VLAN header and special tag is 28 * being combined. Only in this way we can allow the switch can parse 29 * the both special and VLAN tag at the same time and then look up VLAN 30 * table with VID. 31 */ 32 if (!skb_vlan_tagged(skb)) { 33 if (skb_cow_head(skb, MTK_HDR_LEN) < 0) 34 return NULL; 35 36 skb_push(skb, MTK_HDR_LEN); 37 memmove(skb->data, skb->data + MTK_HDR_LEN, 2 * ETH_ALEN); 38 is_vlan_skb = false; 39 } 40 41 mtk_tag = skb->data + 2 * ETH_ALEN; 42 43 /* Mark tag attribute on special tag insertion to notify hardware 44 * whether that's a combined special tag with 802.1Q header. 45 */ 46 mtk_tag[0] = is_vlan_skb ? MTK_HDR_XMIT_TAGGED_TPID_8100 : 47 MTK_HDR_XMIT_UNTAGGED; 48 mtk_tag[1] = (1 << dp->index) & MTK_HDR_XMIT_DP_BIT_MASK; 49 50 /* Tag control information is kept for 802.1Q */ 51 if (!is_vlan_skb) { 52 mtk_tag[2] = 0; 53 mtk_tag[3] = 0; 54 } 55 56 return skb; 57 } 58 59 static struct sk_buff *mtk_tag_rcv(struct sk_buff *skb, struct net_device *dev, 60 struct packet_type *pt) 61 { 62 int port; 63 __be16 *phdr, hdr; 64 65 if (unlikely(!pskb_may_pull(skb, MTK_HDR_LEN))) 66 return NULL; 67 68 /* The MTK header is added by the switch between src addr 69 * and ethertype at this point, skb->data points to 2 bytes 70 * after src addr so header should be 2 bytes right before. 71 */ 72 phdr = (__be16 *)(skb->data - 2); 73 hdr = ntohs(*phdr); 74 75 /* Remove MTK tag and recalculate checksum. */ 76 skb_pull_rcsum(skb, MTK_HDR_LEN); 77 78 memmove(skb->data - ETH_HLEN, 79 skb->data - ETH_HLEN - MTK_HDR_LEN, 80 2 * ETH_ALEN); 81 82 /* Get source port information */ 83 port = (hdr & MTK_HDR_RECV_SOURCE_PORT_MASK); 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 mtk_tag_flow_dissect(const struct sk_buff *skb, __be16 *proto, 93 int *offset) 94 { 95 *offset = 4; 96 *proto = ((__be16 *)skb->data)[1]; 97 98 return 0; 99 } 100 101 static const struct dsa_device_ops mtk_netdev_ops = { 102 .name = "mtk", 103 .proto = DSA_TAG_PROTO_MTK, 104 .xmit = mtk_tag_xmit, 105 .rcv = mtk_tag_rcv, 106 .flow_dissect = mtk_tag_flow_dissect, 107 .overhead = MTK_HDR_LEN, 108 }; 109 110 MODULE_LICENSE("GPL"); 111 MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_MTK); 112 113 module_dsa_tag_driver(mtk_netdev_ops); 114