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
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
|
/*
* net/dsa/tag_qinq.c - QinQ tag format handling
* Copyright (c) 2010 Gabor Juhos <juhosg@openwrt.org>
*
* This file was based on:
* net/dsa/tag_edsa.c - Ethertype DSA tagging
* Copyright (c) 2008-2009 Marvell Semiconductor
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*/
#include <linux/etherdevice.h>
#include <linux/list.h>
#include <linux/netdevice.h>
#include <linux/if_vlan.h>
#include "dsa_priv.h"
netdev_tx_t qinq_xmit(struct sk_buff *skb, struct net_device *dev)
{
struct dsa_slave_priv *p = netdev_priv(dev);
struct vlan_ethhdr *veth;
unsigned int len;
int ret;
if (skb_cow_head(skb, VLAN_HLEN) < 0)
goto out_free_skb;
veth = (struct vlan_ethhdr *)skb_push(skb, VLAN_HLEN);
/* Move the mac addresses to the beginning of the new header. */
memmove(skb->data, skb->data + VLAN_HLEN, 2 * VLAN_ETH_ALEN);
skb->mac_header -= VLAN_HLEN;
/* setup VLAN header fields */
veth->h_vlan_proto = htons(ETH_P_QINQ);
veth->h_vlan_TCI = htons(p->port);
len = skb->len;
skb->protocol = htons(ETH_P_QINQ);
skb->dev = p->parent->dst->master_netdev;
ret = dev_queue_xmit(skb);
if (unlikely(ret != NET_XMIT_SUCCESS))
goto out_dropped;
dev->stats.tx_packets++;
dev->stats.tx_bytes += len;
return NETDEV_TX_OK;
out_free_skb:
kfree_skb(skb);
out_dropped:
dev->stats.tx_dropped++;
return NETDEV_TX_OK;
}
static int qinq_rcv(struct sk_buff *skb, struct net_device *dev,
struct packet_type *pt, struct net_device *orig_dev)
{
struct dsa_switch_tree *dst;
struct dsa_switch *ds;
struct vlan_hdr *vhdr;
int source_port;
dst = dev->dsa_ptr;
if (unlikely(dst == NULL))
goto out_drop;
ds = dst->ds[0];
skb = skb_unshare(skb, GFP_ATOMIC);
if (skb == NULL)
goto out;
if (unlikely(!pskb_may_pull(skb, VLAN_HLEN)))
goto out_drop;
vhdr = (struct vlan_hdr *)skb->data;
source_port = ntohs(vhdr->h_vlan_TCI) & VLAN_VID_MASK;
if (source_port >= DSA_MAX_PORTS || ds->ports[source_port] == NULL)
goto out_drop;
/* Remove the outermost VLAN tag and update checksum. */
skb_pull_rcsum(skb, VLAN_HLEN);
memmove(skb->data - ETH_HLEN,
skb->data - ETH_HLEN - VLAN_HLEN,
2 * ETH_ALEN);
skb->dev = ds->ports[source_port];
skb_push(skb, ETH_HLEN);
skb->pkt_type = PACKET_HOST;
skb->protocol = eth_type_trans(skb, skb->dev);
skb->dev->stats.rx_packets++;
skb->dev->stats.rx_bytes += skb->len;
netif_receive_skb(skb);
return 0;
out_drop:
kfree_skb(skb);
out:
return 0;
}
static struct packet_type qinq_packet_type __read_mostly = {
.type = cpu_to_be16(ETH_P_QINQ),
.func = qinq_rcv,
};
static int __init qinq_init_module(void)
{
dev_add_pack(&qinq_packet_type);
return 0;
}
module_init(qinq_init_module);
static void __exit qinq_cleanup_module(void)
{
dev_remove_pack(&qinq_packet_type);
}
module_exit(qinq_cleanup_module);
|
