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 #define MTK_HDR_XMIT_SA_DIS BIT(6) 19 20 static struct sk_buff *mtk_tag_xmit(struct sk_buff *skb, 21 struct net_device *dev) 22 { 23 struct dsa_port *dp = dsa_slave_to_port(dev); 24 u8 *mtk_tag; 25 bool is_vlan_skb = true; 26 unsigned char *dest = eth_hdr(skb)->h_dest; 27 bool is_multicast_skb = is_multicast_ether_addr(dest) && 28 !is_broadcast_ether_addr(dest); 29 30 /* Build the special tag after the MAC Source Address. If VLAN header 31 * is present, it's required that VLAN header and special tag is 32 * being combined. Only in this way we can allow the switch can parse 33 * the both special and VLAN tag at the same time and then look up VLAN 34 * table with VID. 35 */ 36 if (!skb_vlan_tagged(skb)) { 37 skb_push(skb, MTK_HDR_LEN); 38 memmove(skb->data, skb->data + MTK_HDR_LEN, 2 * ETH_ALEN); 39 is_vlan_skb = false; 40 } 41 42 mtk_tag = skb->data + 2 * ETH_ALEN; 43 44 /* Mark tag attribute on special tag insertion to notify hardware 45 * whether that's a combined special tag with 802.1Q header. 46 */ 47 mtk_tag[0] = is_vlan_skb ? MTK_HDR_XMIT_TAGGED_TPID_8100 : 48 MTK_HDR_XMIT_UNTAGGED; 49 mtk_tag[1] = (1 << dp->index) & MTK_HDR_XMIT_DP_BIT_MASK; 50 51 /* Disable SA learning for multicast frames */ 52 if (unlikely(is_multicast_skb)) 53 mtk_tag[1] |= MTK_HDR_XMIT_SA_DIS; 54 55 /* Tag control information is kept for 802.1Q */ 56 if (!is_vlan_skb) { 57 mtk_tag[2] = 0; 58 mtk_tag[3] = 0; 59 } 60 61 return skb; 62 } 63 64 static struct sk_buff *mtk_tag_rcv(struct sk_buff *skb, struct net_device *dev, 65 struct packet_type *pt) 66 { 67 u16 hdr; 68 int port; 69 __be16 *phdr; 70 unsigned char *dest = eth_hdr(skb)->h_dest; 71 bool is_multicast_skb = is_multicast_ether_addr(dest) && 72 !is_broadcast_ether_addr(dest); 73 74 if (unlikely(!pskb_may_pull(skb, MTK_HDR_LEN))) 75 return NULL; 76 77 /* The MTK header is added by the switch between src addr 78 * and ethertype at this point, skb->data points to 2 bytes 79 * after src addr so header should be 2 bytes right before. 80 */ 81 phdr = (__be16 *)(skb->data - 2); 82 hdr = ntohs(*phdr); 83 84 /* Remove MTK tag and recalculate checksum. */ 85 skb_pull_rcsum(skb, MTK_HDR_LEN); 86 87 memmove(skb->data - ETH_HLEN, 88 skb->data - ETH_HLEN - MTK_HDR_LEN, 89 2 * ETH_ALEN); 90 91 /* Get source port information */ 92 port = (hdr & MTK_HDR_RECV_SOURCE_PORT_MASK); 93 94 skb->dev = dsa_master_find_slave(dev, 0, port); 95 if (!skb->dev) 96 return NULL; 97 98 /* Only unicast or broadcast frames are offloaded */ 99 if (likely(!is_multicast_skb)) 100 skb->offload_fwd_mark = 1; 101 102 return skb; 103 } 104 105 static const struct dsa_device_ops mtk_netdev_ops = { 106 .name = "mtk", 107 .proto = DSA_TAG_PROTO_MTK, 108 .xmit = mtk_tag_xmit, 109 .rcv = mtk_tag_rcv, 110 .overhead = MTK_HDR_LEN, 111 }; 112 113 MODULE_LICENSE("GPL"); 114 MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_MTK); 115 116 module_dsa_tag_driver(mtk_netdev_ops); 117