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> | |
16 | #include <net/dsa.h> | |
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) | |
30 | goto out_free; | |
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; | |
44 | ||
45 | out_free: | |
46 | kfree_skb(skb); | |
47 | return NULL; | |
48 | } | |
49 | ||
a86d8bec FF |
50 | static struct sk_buff *mtk_tag_rcv(struct sk_buff *skb, struct net_device *dev, |
51 | struct packet_type *pt, | |
52 | struct net_device *orig_dev) | |
5cd8985a SW |
53 | { |
54 | struct dsa_switch_tree *dst = dev->dsa_ptr; | |
55 | struct dsa_switch *ds; | |
56 | int port; | |
57 | __be16 *phdr, hdr; | |
58 | ||
5cd8985a SW |
59 | if (unlikely(!pskb_may_pull(skb, MTK_HDR_LEN))) |
60 | goto out_drop; | |
61 | ||
62 | /* The MTK header is added by the switch between src addr | |
63 | * and ethertype at this point, skb->data points to 2 bytes | |
64 | * after src addr so header should be 2 bytes right before. | |
65 | */ | |
66 | phdr = (__be16 *)(skb->data - 2); | |
67 | hdr = ntohs(*phdr); | |
68 | ||
69 | /* Remove MTK tag and recalculate checksum. */ | |
70 | skb_pull_rcsum(skb, MTK_HDR_LEN); | |
71 | ||
72 | memmove(skb->data - ETH_HLEN, | |
73 | skb->data - ETH_HLEN - MTK_HDR_LEN, | |
74 | 2 * ETH_ALEN); | |
75 | ||
76 | /* This protocol doesn't support cascading multiple | |
77 | * switches so it's safe to assume the switch is first | |
78 | * in the tree. | |
79 | */ | |
80 | ds = dst->ds[0]; | |
81 | if (!ds) | |
82 | goto out_drop; | |
83 | ||
84 | /* Get source port information */ | |
85 | port = (hdr & MTK_HDR_RECV_SOURCE_PORT_MASK); | |
86 | if (!ds->ports[port].netdev) | |
87 | goto out_drop; | |
88 | ||
5cd8985a | 89 | skb->dev = ds->ports[port].netdev; |
5cd8985a | 90 | |
a86d8bec | 91 | return skb; |
5cd8985a SW |
92 | |
93 | out_drop: | |
a86d8bec | 94 | return NULL; |
5cd8985a SW |
95 | } |
96 | ||
97 | const struct dsa_device_ops mtk_netdev_ops = { | |
98 | .xmit = mtk_tag_xmit, | |
99 | .rcv = mtk_tag_rcv, | |
100 | }; |