Commit | Line | Data |
---|---|---|
dfedd3b6 | 1 | // SPDX-License-Identifier: GPL-2.0 |
5cd8985a SW |
2 | /* |
3 | * Mediatek DSA Tag support | |
4 | * Copyright (C) 2017 Landen Chao <landen.chao@mediatek.com> | |
5 | * Sean Wang <sean.wang@mediatek.com> | |
5cd8985a SW |
6 | */ |
7 | ||
8 | #include <linux/etherdevice.h> | |
f0af3431 | 9 | #include <linux/if_vlan.h> |
ea5dd34b | 10 | |
5cd8985a SW |
11 | #include "dsa_priv.h" |
12 | ||
13 | #define MTK_HDR_LEN 4 | |
f0af3431 SW |
14 | #define MTK_HDR_XMIT_UNTAGGED 0 |
15 | #define MTK_HDR_XMIT_TAGGED_TPID_8100 1 | |
5cd8985a SW |
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 | { | |
d945097b | 22 | struct dsa_port *dp = dsa_slave_to_port(dev); |
5cd8985a | 23 | u8 *mtk_tag; |
f0af3431 | 24 | bool is_vlan_skb = true; |
5cd8985a | 25 | |
f0af3431 SW |
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; | |
5cd8985a | 35 | |
f0af3431 SW |
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 | } | |
5cd8985a | 40 | |
5cd8985a | 41 | mtk_tag = skb->data + 2 * ETH_ALEN; |
f0af3431 SW |
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; | |
d945097b | 48 | mtk_tag[1] = (1 << dp->index) & MTK_HDR_XMIT_DP_BIT_MASK; |
f0af3431 SW |
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 | } | |
5cd8985a SW |
55 | |
56 | return skb; | |
5cd8985a SW |
57 | } |
58 | ||
a86d8bec | 59 | static struct sk_buff *mtk_tag_rcv(struct sk_buff *skb, struct net_device *dev, |
89e49506 | 60 | struct packet_type *pt) |
5cd8985a | 61 | { |
5cd8985a SW |
62 | int port; |
63 | __be16 *phdr, hdr; | |
64 | ||
5cd8985a | 65 | if (unlikely(!pskb_may_pull(skb, MTK_HDR_LEN))) |
54709795 | 66 | return NULL; |
5cd8985a SW |
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 | ||
5cd8985a SW |
82 | /* Get source port information */ |
83 | port = (hdr & MTK_HDR_RECV_SOURCE_PORT_MASK); | |
5cd8985a | 84 | |
2231c43b | 85 | skb->dev = dsa_master_find_slave(dev, 0, port); |
3775b1b7 VD |
86 | if (!skb->dev) |
87 | return NULL; | |
5cd8985a | 88 | |
a86d8bec | 89 | return skb; |
5cd8985a SW |
90 | } |
91 | ||
2dd592b2 JC |
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 | ||
f81a43e8 | 101 | static const struct dsa_device_ops mtk_netdev_ops = { |
875138f8 | 102 | .name = "mtk", |
056eed2f | 103 | .proto = DSA_TAG_PROTO_MTK, |
2dd592b2 JC |
104 | .xmit = mtk_tag_xmit, |
105 | .rcv = mtk_tag_rcv, | |
106 | .flow_dissect = mtk_tag_flow_dissect, | |
a5dd3087 | 107 | .overhead = MTK_HDR_LEN, |
5cd8985a | 108 | }; |
0b42f033 | 109 | |
f18bba50 | 110 | MODULE_LICENSE("GPL"); |
0b42f033 | 111 | MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_MTK); |
d3b8c049 AL |
112 | |
113 | module_dsa_tag_driver(mtk_netdev_ops); |