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 "tag.h" 12 13 #define MTK_NAME "mtk" 14 15 #define MTK_HDR_LEN 4 16 #define MTK_HDR_XMIT_UNTAGGED 0 17 #define MTK_HDR_XMIT_TAGGED_TPID_8100 1 18 #define MTK_HDR_XMIT_TAGGED_TPID_88A8 2 19 #define MTK_HDR_RECV_SOURCE_PORT_MASK GENMASK(2, 0) 20 #define MTK_HDR_XMIT_DP_BIT_MASK GENMASK(5, 0) 21 #define MTK_HDR_XMIT_SA_DIS BIT(6) 22 23 static struct sk_buff *mtk_tag_xmit(struct sk_buff *skb, 24 struct net_device *dev) 25 { 26 struct dsa_port *dp = dsa_slave_to_port(dev); 27 u8 xmit_tpid; 28 u8 *mtk_tag; 29 30 skb_set_queue_mapping(skb, dp->index); 31 32 /* Build the special tag after the MAC Source Address. If VLAN header 33 * is present, it's required that VLAN header and special tag is 34 * being combined. Only in this way we can allow the switch can parse 35 * the both special and VLAN tag at the same time and then look up VLAN 36 * table with VID. 37 */ 38 switch (skb->protocol) { 39 case htons(ETH_P_8021Q): 40 xmit_tpid = MTK_HDR_XMIT_TAGGED_TPID_8100; 41 break; 42 case htons(ETH_P_8021AD): 43 xmit_tpid = MTK_HDR_XMIT_TAGGED_TPID_88A8; 44 break; 45 default: 46 xmit_tpid = MTK_HDR_XMIT_UNTAGGED; 47 skb_push(skb, MTK_HDR_LEN); 48 dsa_alloc_etype_header(skb, MTK_HDR_LEN); 49 } 50 51 mtk_tag = dsa_etype_header_pos_tx(skb); 52 53 /* Mark tag attribute on special tag insertion to notify hardware 54 * whether that's a combined special tag with 802.1Q header. 55 */ 56 mtk_tag[0] = xmit_tpid; 57 mtk_tag[1] = (1 << dp->index) & MTK_HDR_XMIT_DP_BIT_MASK; 58 59 /* Tag control information is kept for 802.1Q */ 60 if (xmit_tpid == MTK_HDR_XMIT_UNTAGGED) { 61 mtk_tag[2] = 0; 62 mtk_tag[3] = 0; 63 } 64 65 return skb; 66 } 67 68 static struct sk_buff *mtk_tag_rcv(struct sk_buff *skb, struct net_device *dev) 69 { 70 u16 hdr; 71 int port; 72 __be16 *phdr; 73 74 if (unlikely(!pskb_may_pull(skb, MTK_HDR_LEN))) 75 return NULL; 76 77 phdr = dsa_etype_header_pos_rx(skb); 78 hdr = ntohs(*phdr); 79 80 /* Remove MTK tag and recalculate checksum. */ 81 skb_pull_rcsum(skb, MTK_HDR_LEN); 82 83 dsa_strip_etype_header(skb, MTK_HDR_LEN); 84 85 /* Get source port information */ 86 port = (hdr & MTK_HDR_RECV_SOURCE_PORT_MASK); 87 88 skb->dev = dsa_master_find_slave(dev, 0, port); 89 if (!skb->dev) 90 return NULL; 91 92 dsa_default_offload_fwd_mark(skb); 93 94 return skb; 95 } 96 97 static const struct dsa_device_ops mtk_netdev_ops = { 98 .name = MTK_NAME, 99 .proto = DSA_TAG_PROTO_MTK, 100 .xmit = mtk_tag_xmit, 101 .rcv = mtk_tag_rcv, 102 .needed_headroom = MTK_HDR_LEN, 103 }; 104 105 MODULE_LICENSE("GPL"); 106 MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_MTK, MTK_NAME); 107 108 module_dsa_tag_driver(mtk_netdev_ops); 109