From 8e22134773f28f6e800fd2befd54ce8ed56dd014 Mon Sep 17 00:00:00 2001 From: Gabor Juhos Date: Mon, 1 Mar 2010 07:34:37 +0000 Subject: [PATCH] ar71xx: add QinQ tagging format for the DSA driver SVN-Revision: 19926 --- target/linux/ar71xx/config-2.6.32 | 1 + target/linux/ar71xx/files/net/dsa/tag_qinq.c | 127 ++++++++++++++++++ .../122-dsa-add-qinq-tagging-format.patch | 79 +++++++++++ 3 files changed, 207 insertions(+) create mode 100644 target/linux/ar71xx/files/net/dsa/tag_qinq.c create mode 100644 target/linux/ar71xx/patches-2.6.32/122-dsa-add-qinq-tagging-format.patch diff --git a/target/linux/ar71xx/config-2.6.32 b/target/linux/ar71xx/config-2.6.32 index 1c193b2777..f4b1184a83 100644 --- 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_TAG_QINQ=y 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 index 0000000000..68bcc38287 --- /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 + * + * 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 +#include +#include +#include + +#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 index 0000000000..dc03fd6eb4 --- /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; -- 2.30.2