68bcc382872224a96591bc8afdbd3067c6f6a8ec
[openwrt/svn-archive/archive.git] / target / linux / ar71xx / files / net / dsa / tag_qinq.c
1 /*
2 * net/dsa/tag_qinq.c - QinQ tag format handling
3 * Copyright (c) 2010 Gabor Juhos <juhosg@openwrt.org>
4 *
5 * This file was based on:
6 * net/dsa/tag_edsa.c - Ethertype DSA tagging
7 * Copyright (c) 2008-2009 Marvell Semiconductor
8 *
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.
13 */
14
15 #include <linux/etherdevice.h>
16 #include <linux/list.h>
17 #include <linux/netdevice.h>
18 #include <linux/if_vlan.h>
19
20 #include "dsa_priv.h"
21
22 netdev_tx_t qinq_xmit(struct sk_buff *skb, struct net_device *dev)
23 {
24 struct dsa_slave_priv *p = netdev_priv(dev);
25 struct vlan_ethhdr *veth;
26 unsigned int len;
27 int ret;
28
29 if (skb_cow_head(skb, VLAN_HLEN) < 0)
30 goto out_free_skb;
31
32 veth = (struct vlan_ethhdr *)skb_push(skb, VLAN_HLEN);
33
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;
37
38 /* setup VLAN header fields */
39 veth->h_vlan_proto = htons(ETH_P_QINQ);
40 veth->h_vlan_TCI = htons(p->port);
41
42 len = skb->len;
43 skb->protocol = htons(ETH_P_QINQ);
44 skb->dev = p->parent->dst->master_netdev;
45
46 ret = dev_queue_xmit(skb);
47 if (unlikely(ret != NET_XMIT_SUCCESS))
48 goto out_dropped;
49
50 dev->stats.tx_packets++;
51 dev->stats.tx_bytes += len;
52
53 return NETDEV_TX_OK;
54
55 out_free_skb:
56 kfree_skb(skb);
57 out_dropped:
58 dev->stats.tx_dropped++;
59 return NETDEV_TX_OK;
60 }
61
62 static int qinq_rcv(struct sk_buff *skb, struct net_device *dev,
63 struct packet_type *pt, struct net_device *orig_dev)
64 {
65 struct dsa_switch_tree *dst;
66 struct dsa_switch *ds;
67 struct vlan_hdr *vhdr;
68 int source_port;
69
70 dst = dev->dsa_ptr;
71 if (unlikely(dst == NULL))
72 goto out_drop;
73 ds = dst->ds[0];
74
75 skb = skb_unshare(skb, GFP_ATOMIC);
76 if (skb == NULL)
77 goto out;
78
79 if (unlikely(!pskb_may_pull(skb, VLAN_HLEN)))
80 goto out_drop;
81
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)
85 goto out_drop;
86
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,
91 2 * ETH_ALEN);
92
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);
97
98 skb->dev->stats.rx_packets++;
99 skb->dev->stats.rx_bytes += skb->len;
100
101 netif_receive_skb(skb);
102
103 return 0;
104
105 out_drop:
106 kfree_skb(skb);
107 out:
108 return 0;
109 }
110
111 static struct packet_type qinq_packet_type __read_mostly = {
112 .type = cpu_to_be16(ETH_P_QINQ),
113 .func = qinq_rcv,
114 };
115
116 static int __init qinq_init_module(void)
117 {
118 dev_add_pack(&qinq_packet_type);
119 return 0;
120 }
121 module_init(qinq_init_module);
122
123 static void __exit qinq_cleanup_module(void)
124 {
125 dev_remove_pack(&qinq_packet_type);
126 }
127 module_exit(qinq_cleanup_module);