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