ar71xx: add QinQ tagging format for the DSA driver

SVN-Revision: 19926

diff --git a/target/linux/ar71xx/config-2.6.32 b/target/linux/ar71xx/config-2.6.32
index 1c193b27776ede8a1eb06f2172f9add58a3cca4b..f4b1184a83616ecbde618b0c47cfdd56caba68da 100644 (file)
--- a/target/linux/ar71xx/config-2.6.32
+++ b/target/linux/ar71xx/config-2.6.32
@@ -173,6 +173,7 @@ CONFIG_NET_DSA_MV88E6063=y
 # CONFIG_NET_DSA_MV88E6XXX_NEED_PPU is not set
 # CONFIG_NET_DSA_TAG_DSA is not set
 # CONFIG_NET_DSA_TAG_EDSA is not set
 # CONFIG_NET_DSA_MV88E6XXX_NEED_PPU is not set
 # CONFIG_NET_DSA_TAG_DSA is not set
 # CONFIG_NET_DSA_TAG_EDSA is not set

+CONFIG_NET_DSA_TAG_QINQ=y

 CONFIG_NET_DSA_TAG_TRAILER=y
 # CONFIG_NO_IOPORT is not set
 # CONFIG_NXP_STB220 is not set
 CONFIG_NET_DSA_TAG_TRAILER=y
 # CONFIG_NO_IOPORT is not set
 # CONFIG_NXP_STB220 is not set

diff --git a/target/linux/ar71xx/files/net/dsa/tag_qinq.c b/target/linux/ar71xx/files/net/dsa/tag_qinq.c
new file mode 100644 (file)
index 0000000..68bcc38
--- /dev/null
+++ b/target/linux/ar71xx/files/net/dsa/tag_qinq.c
@@ -0,0 +1,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);

diff --git a/target/linux/ar71xx/patches-2.6.32/122-dsa-add-qinq-tagging-format.patch b/target/linux/ar71xx/patches-2.6.32/122-dsa-add-qinq-tagging-format.patch
new file mode 100644 (file)
index 0000000..dc03fd6
--- /dev/null
+++ b/target/linux/ar71xx/patches-2.6.32/122-dsa-add-qinq-tagging-format.patch
@@ -0,0 +1,79 @@
+--- a/include/linux/if_ether.h
++++ b/include/linux/if_ether.h
+@@ -81,6 +81,7 @@
+ #define ETH_P_1588    0x88F7          /* IEEE 1588 Timesync */
+ #define ETH_P_FCOE    0x8906          /* Fibre Channel over Ethernet  */
+ #define ETH_P_FIP     0x8914          /* FCoE Initialization Protocol */
++#define ETH_P_QINQ    0x9100          /* QinQ VLAN Stacking Protocol */
+ #define ETH_P_EDSA    0xDADA          /* Ethertype DSA [ NOT AN OFFICIALLY REGISTERED ID ] */
+ 
+ /*
+--- a/net/dsa/dsa_priv.h
++++ b/net/dsa/dsa_priv.h
+@@ -174,6 +174,9 @@ netdev_tx_t dsa_xmit(struct sk_buff *skb
+ /* tag_edsa.c */
+ netdev_tx_t edsa_xmit(struct sk_buff *skb, struct net_device *dev);
+ 
++/* tag_qinq.c */
++netdev_tx_t qinq_xmit(struct sk_buff *skb, struct net_device *dev);
++
+ /* tag_trailer.c */
+ netdev_tx_t trailer_xmit(struct sk_buff *skb, struct net_device *dev);
+ 
+--- a/net/dsa/Kconfig
++++ b/net/dsa/Kconfig
+@@ -23,6 +23,10 @@ config NET_DSA_TAG_TRAILER
+       bool
+       default n
+ 
++config NET_DSA_TAG_QINQ
++      bool
++      default y
++
+ 
+ # switch drivers
+ config NET_DSA_MV88E6XXX
+--- a/net/dsa/Makefile
++++ b/net/dsa/Makefile
+@@ -1,6 +1,7 @@
+ # tagging formats
+ obj-$(CONFIG_NET_DSA_TAG_DSA) += tag_dsa.o
+ obj-$(CONFIG_NET_DSA_TAG_EDSA) += tag_edsa.o
++obj-$(CONFIG_NET_DSA_TAG_QINQ) += tag_qinq.o
+ obj-$(CONFIG_NET_DSA_TAG_TRAILER) += tag_trailer.o
+ 
+ # switch drivers
+--- a/net/dsa/slave.c
++++ b/net/dsa/slave.c
+@@ -321,6 +321,19 @@ static const struct net_device_ops edsa_
+       .ndo_do_ioctl           = dsa_slave_ioctl,
+ };
+ #endif
++#ifdef CONFIG_NET_DSA_TAG_QINQ
++static const struct net_device_ops qinq_netdev_ops = {
++      .ndo_init               = dsa_slave_init,
++      .ndo_open               = dsa_slave_open,
++      .ndo_stop               = dsa_slave_close,
++      .ndo_start_xmit         = qinq_xmit,
++      .ndo_change_rx_flags    = dsa_slave_change_rx_flags,
++      .ndo_set_rx_mode        = dsa_slave_set_rx_mode,
++      .ndo_set_multicast_list = dsa_slave_set_rx_mode,
++      .ndo_set_mac_address    = dsa_slave_set_mac_address,
++      .ndo_do_ioctl           = dsa_slave_ioctl,
++};
++#endif
+ #ifdef CONFIG_NET_DSA_TAG_TRAILER
+ static const struct net_device_ops trailer_netdev_ops = {
+       .ndo_init               = dsa_slave_init,
+@@ -366,6 +379,11 @@ dsa_slave_create(struct dsa_switch *ds, 
+               slave_dev->netdev_ops = &edsa_netdev_ops;
+               break;
+ #endif
++#ifdef CONFIG_NET_DSA_TAG_QINQ
++      case htons(ETH_P_QINQ):
++              slave_dev->netdev_ops = &qinq_netdev_ops;
++              break;
++#endif
+ #ifdef CONFIG_NET_DSA_TAG_TRAILER
+       case htons(ETH_P_TRAILER):
+               slave_dev->netdev_ops = &trailer_netdev_ops;