/* * Mediatek DSA Tag support * Copyright (C) 2017 Landen Chao <landen.chao@mediatek.com> * Sean Wang <sean.wang@mediatek.com> * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License version 2 and * only version 2 as published by the Free Software Foundation. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. */ #include <linux/etherdevice.h> #include <linux/if_vlan.h> #include "dsa_priv.h" #define MTK_HDR_LEN 4 #define MTK_HDR_XMIT_UNTAGGED 0 #define MTK_HDR_XMIT_TAGGED_TPID_8100 1 #define MTK_HDR_RECV_SOURCE_PORT_MASK GENMASK(2, 0) #define MTK_HDR_XMIT_DP_BIT_MASK GENMASK(5, 0) static struct sk_buff *mtk_tag_xmit(struct sk_buff *skb, struct net_device *dev) { struct dsa_port *dp = dsa_slave_to_port(dev); u8 *mtk_tag; bool is_vlan_skb = true; /* Build the special tag after the MAC Source Address. If VLAN header * is present, it's required that VLAN header and special tag is * being combined. Only in this way we can allow the switch can parse * the both special and VLAN tag at the same time and then look up VLAN * table with VID. */ if (!skb_vlan_tagged(skb)) { if (skb_cow_head(skb, MTK_HDR_LEN) < 0) return NULL; skb_push(skb, MTK_HDR_LEN); memmove(skb->data, skb->data + MTK_HDR_LEN, 2 * ETH_ALEN); is_vlan_skb = false; } mtk_tag = skb->data + 2 * ETH_ALEN; /* Mark tag attribute on special tag insertion to notify hardware * whether that's a combined special tag with 802.1Q header. */ mtk_tag[0] = is_vlan_skb ? MTK_HDR_XMIT_TAGGED_TPID_8100 : MTK_HDR_XMIT_UNTAGGED; mtk_tag[1] = (1 << dp->index) & MTK_HDR_XMIT_DP_BIT_MASK; /* Tag control information is kept for 802.1Q */ if (!is_vlan_skb) { mtk_tag[2] = 0; mtk_tag[3] = 0; } return skb; } static struct sk_buff *mtk_tag_rcv(struct sk_buff *skb, struct net_device *dev, struct packet_type *pt) { int port; __be16 *phdr, hdr; if (unlikely(!pskb_may_pull(skb, MTK_HDR_LEN))) return NULL; /* The MTK header is added by the switch between src addr * and ethertype at this point, skb->data points to 2 bytes * after src addr so header should be 2 bytes right before. */ phdr = (__be16 *)(skb->data - 2); hdr = ntohs(*phdr); /* Remove MTK tag and recalculate checksum. */ skb_pull_rcsum(skb, MTK_HDR_LEN); memmove(skb->data - ETH_HLEN, skb->data - ETH_HLEN - MTK_HDR_LEN, 2 * ETH_ALEN); /* Get source port information */ port = (hdr & MTK_HDR_RECV_SOURCE_PORT_MASK); skb->dev = dsa_master_find_slave(dev, 0, port); if (!skb->dev) return NULL; return skb; } static int mtk_tag_flow_dissect(const struct sk_buff *skb, __be16 *proto, int *offset) { *offset = 4; *proto = ((__be16 *)skb->data)[1]; return 0; } const struct dsa_device_ops mtk_netdev_ops = { .xmit = mtk_tag_xmit, .rcv = mtk_tag_rcv, .flow_dissect = mtk_tag_flow_dissect, };