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