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);