Commit | Line | Data |
---|---|---|
5cd8985a SW |
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> | |
ea5dd34b | 16 | |
5cd8985a SW |
17 | #include "dsa_priv.h" |
18 | ||
19 | #define MTK_HDR_LEN 4 | |
20 | #define MTK_HDR_RECV_SOURCE_PORT_MASK GENMASK(2, 0) | |
21 | #define MTK_HDR_XMIT_DP_BIT_MASK GENMASK(5, 0) | |
22 | ||
23 | static struct sk_buff *mtk_tag_xmit(struct sk_buff *skb, | |
24 | struct net_device *dev) | |
25 | { | |
26 | struct dsa_slave_priv *p = netdev_priv(dev); | |
27 | u8 *mtk_tag; | |
28 | ||
29 | if (skb_cow_head(skb, MTK_HDR_LEN) < 0) | |
fe47d563 | 30 | return NULL; |
5cd8985a SW |
31 | |
32 | skb_push(skb, MTK_HDR_LEN); | |
33 | ||
34 | memmove(skb->data, skb->data + MTK_HDR_LEN, 2 * ETH_ALEN); | |
35 | ||
36 | /* Build the tag after the MAC Source Address */ | |
37 | mtk_tag = skb->data + 2 * ETH_ALEN; | |
38 | mtk_tag[0] = 0; | |
39 | mtk_tag[1] = (1 << p->dp->index) & MTK_HDR_XMIT_DP_BIT_MASK; | |
40 | mtk_tag[2] = 0; | |
41 | mtk_tag[3] = 0; | |
42 | ||
43 | return skb; | |
5cd8985a SW |
44 | } |
45 | ||
a86d8bec | 46 | static struct sk_buff *mtk_tag_rcv(struct sk_buff *skb, struct net_device *dev, |
89e49506 | 47 | struct packet_type *pt) |
5cd8985a SW |
48 | { |
49 | struct dsa_switch_tree *dst = dev->dsa_ptr; | |
50 | struct dsa_switch *ds; | |
51 | int port; | |
52 | __be16 *phdr, hdr; | |
53 | ||
5cd8985a | 54 | if (unlikely(!pskb_may_pull(skb, MTK_HDR_LEN))) |
54709795 | 55 | return NULL; |
5cd8985a SW |
56 | |
57 | /* The MTK header is added by the switch between src addr | |
58 | * and ethertype at this point, skb->data points to 2 bytes | |
59 | * after src addr so header should be 2 bytes right before. | |
60 | */ | |
61 | phdr = (__be16 *)(skb->data - 2); | |
62 | hdr = ntohs(*phdr); | |
63 | ||
64 | /* Remove MTK tag and recalculate checksum. */ | |
65 | skb_pull_rcsum(skb, MTK_HDR_LEN); | |
66 | ||
67 | memmove(skb->data - ETH_HLEN, | |
68 | skb->data - ETH_HLEN - MTK_HDR_LEN, | |
69 | 2 * ETH_ALEN); | |
70 | ||
71 | /* This protocol doesn't support cascading multiple | |
72 | * switches so it's safe to assume the switch is first | |
73 | * in the tree. | |
74 | */ | |
75 | ds = dst->ds[0]; | |
76 | if (!ds) | |
54709795 | 77 | return NULL; |
5cd8985a SW |
78 | |
79 | /* Get source port information */ | |
80 | port = (hdr & MTK_HDR_RECV_SOURCE_PORT_MASK); | |
81 | if (!ds->ports[port].netdev) | |
54709795 | 82 | return NULL; |
5cd8985a | 83 | |
5cd8985a | 84 | skb->dev = ds->ports[port].netdev; |
5cd8985a | 85 | |
a86d8bec | 86 | return skb; |
5cd8985a SW |
87 | } |
88 | ||
2dd592b2 JC |
89 | static int mtk_tag_flow_dissect(const struct sk_buff *skb, __be16 *proto, |
90 | int *offset) | |
91 | { | |
92 | *offset = 4; | |
93 | *proto = ((__be16 *)skb->data)[1]; | |
94 | ||
95 | return 0; | |
96 | } | |
97 | ||
5cd8985a | 98 | const struct dsa_device_ops mtk_netdev_ops = { |
2dd592b2 JC |
99 | .xmit = mtk_tag_xmit, |
100 | .rcv = mtk_tag_rcv, | |
101 | .flow_dissect = mtk_tag_flow_dissect, | |
5cd8985a | 102 | }; |