2 * net/dsa/tag_qinq.c - QinQ tag format handling
3 * Copyright (c) 2010 Gabor Juhos <juhosg@openwrt.org>
5 * This file was based on:
6 * net/dsa/tag_edsa.c - Ethertype DSA tagging
7 * Copyright (c) 2008-2009 Marvell Semiconductor
9 * This program is free software; you can redistribute it and/or modify
10 * it under the terms of the GNU General Public License as published by
11 * the Free Software Foundation; either version 2 of the License, or
12 * (at your option) any later version.
15 #include <linux/etherdevice.h>
16 #include <linux/list.h>
17 #include <linux/netdevice.h>
18 #include <linux/if_vlan.h>
22 netdev_tx_t
qinq_xmit(struct sk_buff
*skb
, struct net_device
*dev
)
24 struct dsa_slave_priv
*p
= netdev_priv(dev
);
25 struct vlan_ethhdr
*veth
;
29 if (skb_cow_head(skb
, VLAN_HLEN
) < 0)
32 veth
= (struct vlan_ethhdr
*)skb_push(skb
, VLAN_HLEN
);
34 /* Move the mac addresses to the beginning of the new header. */
35 memmove(skb
->data
, skb
->data
+ VLAN_HLEN
, 2 * VLAN_ETH_ALEN
);
36 skb
->mac_header
-= VLAN_HLEN
;
38 /* setup VLAN header fields */
39 veth
->h_vlan_proto
= htons(ETH_P_QINQ
);
40 veth
->h_vlan_TCI
= htons(p
->port
);
43 skb
->protocol
= htons(ETH_P_QINQ
);
44 skb
->dev
= p
->parent
->dst
->master_netdev
;
46 ret
= dev_queue_xmit(skb
);
47 if (unlikely(ret
!= NET_XMIT_SUCCESS
))
50 dev
->stats
.tx_packets
++;
51 dev
->stats
.tx_bytes
+= len
;
58 dev
->stats
.tx_dropped
++;
62 static int qinq_rcv(struct sk_buff
*skb
, struct net_device
*dev
,
63 struct packet_type
*pt
, struct net_device
*orig_dev
)
65 struct dsa_switch_tree
*dst
;
66 struct dsa_switch
*ds
;
67 struct vlan_hdr
*vhdr
;
71 if (unlikely(dst
== NULL
))
75 skb
= skb_unshare(skb
, GFP_ATOMIC
);
79 if (unlikely(!pskb_may_pull(skb
, VLAN_HLEN
)))
82 vhdr
= (struct vlan_hdr
*)skb
->data
;
83 source_port
= ntohs(vhdr
->h_vlan_TCI
) & VLAN_VID_MASK
;
84 if (source_port
>= DSA_MAX_PORTS
|| ds
->ports
[source_port
] == NULL
)
87 /* Remove the outermost VLAN tag and update checksum. */
88 skb_pull_rcsum(skb
, VLAN_HLEN
);
89 memmove(skb
->data
- ETH_HLEN
,
90 skb
->data
- ETH_HLEN
- VLAN_HLEN
,
93 skb
->dev
= ds
->ports
[source_port
];
94 skb_push(skb
, ETH_HLEN
);
95 skb
->pkt_type
= PACKET_HOST
;
96 skb
->protocol
= eth_type_trans(skb
, skb
->dev
);
98 skb
->dev
->stats
.rx_packets
++;
99 skb
->dev
->stats
.rx_bytes
+= skb
->len
;
101 netif_receive_skb(skb
);
111 static struct packet_type qinq_packet_type __read_mostly
= {
112 .type
= cpu_to_be16(ETH_P_QINQ
),
116 static int __init
qinq_init_module(void)
118 dev_add_pack(&qinq_packet_type
);
121 module_init(qinq_init_module
);
123 static void __exit
qinq_cleanup_module(void)
125 dev_remove_pack(&qinq_packet_type
);
127 module_exit(qinq_cleanup_module
);