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