Andrew Lunn | dfedd3b | 2019-04-28 19:37:10 +0200 | [diff] [blame] | 1 | // SPDX-License-Identifier: GPL-2.0 |
Sean Wang | 5cd8985 | 2017-04-07 16:45:06 +0800 | [diff] [blame] | 2 | /* |
| 3 | * Mediatek DSA Tag support |
| 4 | * Copyright (C) 2017 Landen Chao <landen.chao@mediatek.com> |
| 5 | * Sean Wang <sean.wang@mediatek.com> |
Sean Wang | 5cd8985 | 2017-04-07 16:45:06 +0800 | [diff] [blame] | 6 | */ |
| 7 | |
| 8 | #include <linux/etherdevice.h> |
Sean Wang | f0af343 | 2017-12-15 12:47:01 +0800 | [diff] [blame] | 9 | #include <linux/if_vlan.h> |
Vivien Didelot | ea5dd34 | 2017-05-17 15:46:03 -0400 | [diff] [blame] | 10 | |
Sean Wang | 5cd8985 | 2017-04-07 16:45:06 +0800 | [diff] [blame] | 11 | #include "dsa_priv.h" |
| 12 | |
| 13 | #define MTK_HDR_LEN 4 |
Sean Wang | f0af343 | 2017-12-15 12:47:01 +0800 | [diff] [blame] | 14 | #define MTK_HDR_XMIT_UNTAGGED 0 |
| 15 | #define MTK_HDR_XMIT_TAGGED_TPID_8100 1 |
Sean Wang | 5cd8985 | 2017-04-07 16:45:06 +0800 | [diff] [blame] | 16 | #define MTK_HDR_RECV_SOURCE_PORT_MASK GENMASK(2, 0) |
| 17 | #define MTK_HDR_XMIT_DP_BIT_MASK GENMASK(5, 0) |
| 18 | |
| 19 | static struct sk_buff *mtk_tag_xmit(struct sk_buff *skb, |
| 20 | struct net_device *dev) |
| 21 | { |
Vivien Didelot | d945097 | 2017-10-16 11:12:15 -0400 | [diff] [blame] | 22 | struct dsa_port *dp = dsa_slave_to_port(dev); |
Sean Wang | 5cd8985 | 2017-04-07 16:45:06 +0800 | [diff] [blame] | 23 | u8 *mtk_tag; |
Sean Wang | f0af343 | 2017-12-15 12:47:01 +0800 | [diff] [blame] | 24 | bool is_vlan_skb = true; |
Sean Wang | 5cd8985 | 2017-04-07 16:45:06 +0800 | [diff] [blame] | 25 | |
Sean Wang | f0af343 | 2017-12-15 12:47:01 +0800 | [diff] [blame] | 26 | /* Build the special tag after the MAC Source Address. If VLAN header |
| 27 | * is present, it's required that VLAN header and special tag is |
| 28 | * being combined. Only in this way we can allow the switch can parse |
| 29 | * the both special and VLAN tag at the same time and then look up VLAN |
| 30 | * table with VID. |
| 31 | */ |
| 32 | if (!skb_vlan_tagged(skb)) { |
| 33 | if (skb_cow_head(skb, MTK_HDR_LEN) < 0) |
| 34 | return NULL; |
Sean Wang | 5cd8985 | 2017-04-07 16:45:06 +0800 | [diff] [blame] | 35 | |
Sean Wang | f0af343 | 2017-12-15 12:47:01 +0800 | [diff] [blame] | 36 | skb_push(skb, MTK_HDR_LEN); |
| 37 | memmove(skb->data, skb->data + MTK_HDR_LEN, 2 * ETH_ALEN); |
| 38 | is_vlan_skb = false; |
| 39 | } |
Sean Wang | 5cd8985 | 2017-04-07 16:45:06 +0800 | [diff] [blame] | 40 | |
Sean Wang | 5cd8985 | 2017-04-07 16:45:06 +0800 | [diff] [blame] | 41 | mtk_tag = skb->data + 2 * ETH_ALEN; |
Sean Wang | f0af343 | 2017-12-15 12:47:01 +0800 | [diff] [blame] | 42 | |
| 43 | /* Mark tag attribute on special tag insertion to notify hardware |
| 44 | * whether that's a combined special tag with 802.1Q header. |
| 45 | */ |
| 46 | mtk_tag[0] = is_vlan_skb ? MTK_HDR_XMIT_TAGGED_TPID_8100 : |
| 47 | MTK_HDR_XMIT_UNTAGGED; |
Vivien Didelot | d945097 | 2017-10-16 11:12:15 -0400 | [diff] [blame] | 48 | mtk_tag[1] = (1 << dp->index) & MTK_HDR_XMIT_DP_BIT_MASK; |
Sean Wang | f0af343 | 2017-12-15 12:47:01 +0800 | [diff] [blame] | 49 | |
| 50 | /* Tag control information is kept for 802.1Q */ |
| 51 | if (!is_vlan_skb) { |
| 52 | mtk_tag[2] = 0; |
| 53 | mtk_tag[3] = 0; |
| 54 | } |
Sean Wang | 5cd8985 | 2017-04-07 16:45:06 +0800 | [diff] [blame] | 55 | |
| 56 | return skb; |
Sean Wang | 5cd8985 | 2017-04-07 16:45:06 +0800 | [diff] [blame] | 57 | } |
| 58 | |
Florian Fainelli | a86d8be | 2017-04-08 08:55:23 -0700 | [diff] [blame] | 59 | static struct sk_buff *mtk_tag_rcv(struct sk_buff *skb, struct net_device *dev, |
Florian Westphal | 89e4950 | 2017-08-17 16:47:00 +0200 | [diff] [blame] | 60 | struct packet_type *pt) |
Sean Wang | 5cd8985 | 2017-04-07 16:45:06 +0800 | [diff] [blame] | 61 | { |
Sean Wang | 5cd8985 | 2017-04-07 16:45:06 +0800 | [diff] [blame] | 62 | int port; |
| 63 | __be16 *phdr, hdr; |
| 64 | |
Sean Wang | 5cd8985 | 2017-04-07 16:45:06 +0800 | [diff] [blame] | 65 | if (unlikely(!pskb_may_pull(skb, MTK_HDR_LEN))) |
Vivien Didelot | 5470979 | 2017-06-01 16:07:14 -0400 | [diff] [blame] | 66 | return NULL; |
Sean Wang | 5cd8985 | 2017-04-07 16:45:06 +0800 | [diff] [blame] | 67 | |
| 68 | /* The MTK header is added by the switch between src addr |
| 69 | * and ethertype at this point, skb->data points to 2 bytes |
| 70 | * after src addr so header should be 2 bytes right before. |
| 71 | */ |
| 72 | phdr = (__be16 *)(skb->data - 2); |
| 73 | hdr = ntohs(*phdr); |
| 74 | |
| 75 | /* Remove MTK tag and recalculate checksum. */ |
| 76 | skb_pull_rcsum(skb, MTK_HDR_LEN); |
| 77 | |
| 78 | memmove(skb->data - ETH_HLEN, |
| 79 | skb->data - ETH_HLEN - MTK_HDR_LEN, |
| 80 | 2 * ETH_ALEN); |
| 81 | |
Sean Wang | 5cd8985 | 2017-04-07 16:45:06 +0800 | [diff] [blame] | 82 | /* Get source port information */ |
| 83 | port = (hdr & MTK_HDR_RECV_SOURCE_PORT_MASK); |
Sean Wang | 5cd8985 | 2017-04-07 16:45:06 +0800 | [diff] [blame] | 84 | |
Vivien Didelot | 2231c43 | 2017-10-16 11:12:17 -0400 | [diff] [blame] | 85 | skb->dev = dsa_master_find_slave(dev, 0, port); |
Vivien Didelot | 3775b1b | 2017-09-29 17:19:15 -0400 | [diff] [blame] | 86 | if (!skb->dev) |
| 87 | return NULL; |
Sean Wang | 5cd8985 | 2017-04-07 16:45:06 +0800 | [diff] [blame] | 88 | |
Florian Fainelli | a86d8be | 2017-04-08 08:55:23 -0700 | [diff] [blame] | 89 | return skb; |
Sean Wang | 5cd8985 | 2017-04-07 16:45:06 +0800 | [diff] [blame] | 90 | } |
| 91 | |
John Crispin | 2dd592b | 2017-08-09 14:41:18 +0200 | [diff] [blame] | 92 | static int mtk_tag_flow_dissect(const struct sk_buff *skb, __be16 *proto, |
| 93 | int *offset) |
| 94 | { |
| 95 | *offset = 4; |
| 96 | *proto = ((__be16 *)skb->data)[1]; |
| 97 | |
| 98 | return 0; |
| 99 | } |
| 100 | |
Andrew Lunn | f81a43e | 2019-04-28 19:37:21 +0200 | [diff] [blame] | 101 | static const struct dsa_device_ops mtk_netdev_ops = { |
Andrew Lunn | 875138f | 2019-04-28 19:37:11 +0200 | [diff] [blame] | 102 | .name = "mtk", |
Andrew Lunn | 056eed2 | 2019-04-28 19:37:14 +0200 | [diff] [blame] | 103 | .proto = DSA_TAG_PROTO_MTK, |
John Crispin | 2dd592b | 2017-08-09 14:41:18 +0200 | [diff] [blame] | 104 | .xmit = mtk_tag_xmit, |
| 105 | .rcv = mtk_tag_rcv, |
| 106 | .flow_dissect = mtk_tag_flow_dissect, |
Andrew Lunn | a5dd308 | 2018-12-06 11:36:04 +0100 | [diff] [blame] | 107 | .overhead = MTK_HDR_LEN, |
Sean Wang | 5cd8985 | 2017-04-07 16:45:06 +0800 | [diff] [blame] | 108 | }; |
Andrew Lunn | 0b42f03 | 2019-04-28 19:37:12 +0200 | [diff] [blame] | 109 | |
Andrew Lunn | f18bba5 | 2019-04-28 19:37:13 +0200 | [diff] [blame] | 110 | MODULE_LICENSE("GPL"); |
Andrew Lunn | 0b42f03 | 2019-04-28 19:37:12 +0200 | [diff] [blame] | 111 | MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_MTK); |
Andrew Lunn | d3b8c04 | 2019-04-28 19:37:15 +0200 | [diff] [blame] | 112 | |
| 113 | module_dsa_tag_driver(mtk_netdev_ops); |