1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
|
// SPDX-License-Identifier: GPL-2.0
/*
* Copyright (c) 2019 Pengutronix, Oleksij Rempel <kernel@pengutronix.de>
*/
#include <linux/bitfield.h>
#include <linux/etherdevice.h>
#include "dsa_priv.h"
#define AR9331_HDR_LEN 2
#define AR9331_HDR_VERSION 1
#define AR9331_HDR_VERSION_MASK GENMASK(15, 14)
#define AR9331_HDR_PRIORITY_MASK GENMASK(13, 12)
#define AR9331_HDR_TYPE_MASK GENMASK(10, 8)
#define AR9331_HDR_BROADCAST BIT(7)
#define AR9331_HDR_FROM_CPU BIT(6)
/* AR9331_HDR_RESERVED - not used or may be version field.
* According to the AR8216 doc it should 0b10. On AR9331 it is 0b11 on RX path
* and should be set to 0b11 to make it work.
*/
#define AR9331_HDR_RESERVED_MASK GENMASK(5, 4)
#define AR9331_HDR_PORT_NUM_MASK GENMASK(3, 0)
static struct sk_buff *ar9331_tag_xmit(struct sk_buff *skb,
struct net_device *dev)
{
struct dsa_port *dp = dsa_slave_to_port(dev);
__le16 *phdr;
u16 hdr;
if (skb_cow_head(skb, AR9331_HDR_LEN) < 0)
return NULL;
phdr = skb_push(skb, AR9331_HDR_LEN);
hdr = FIELD_PREP(AR9331_HDR_VERSION_MASK, AR9331_HDR_VERSION);
hdr |= AR9331_HDR_FROM_CPU | dp->index;
/* 0b10 for AR8216 and 0b11 for AR9331 */
hdr |= AR9331_HDR_RESERVED_MASK;
phdr[0] = cpu_to_le16(hdr);
return skb;
}
static struct sk_buff *ar9331_tag_rcv(struct sk_buff *skb,
struct net_device *ndev,
struct packet_type *pt)
{
u8 ver, port;
u16 hdr;
if (unlikely(!pskb_may_pull(skb, AR9331_HDR_LEN)))
return NULL;
hdr = le16_to_cpu(*(__le16 *)skb_mac_header(skb));
ver = FIELD_GET(AR9331_HDR_VERSION_MASK, hdr);
if (unlikely(ver != AR9331_HDR_VERSION)) {
netdev_warn_once(ndev, "%s:%i wrong header version 0x%2x\n",
__func__, __LINE__, hdr);
return NULL;
}
if (unlikely(hdr & AR9331_HDR_FROM_CPU)) {
netdev_warn_once(ndev, "%s:%i packet should not be from cpu 0x%2x\n",
__func__, __LINE__, hdr);
return NULL;
}
skb_pull_rcsum(skb, AR9331_HDR_LEN);
/* Get source port information */
port = FIELD_GET(AR9331_HDR_PORT_NUM_MASK, hdr);
skb->dev = dsa_master_find_slave(ndev, 0, port);
if (!skb->dev)
return NULL;
return skb;
}
static const struct dsa_device_ops ar9331_netdev_ops = {
.name = "ar9331",
.proto = DSA_TAG_PROTO_AR9331,
.xmit = ar9331_tag_xmit,
.rcv = ar9331_tag_rcv,
.overhead = AR9331_HDR_LEN,
};
MODULE_LICENSE("GPL v2");
MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_AR9331);
module_dsa_tag_driver(ar9331_netdev_ops);
|
