www.usr.com/support/gpl/USR9107_release.1.4.tar.gz

[bcm963xx.git] / kernel / linux / net / atm / br2684.c

diff --git a/kernel/linux/net/atm/br2684.c b/kernel/linux/net/atm/br2684.c
index 8b9f822..43a1a05 100755 (executable)
--- a/kernel/linux/net/atm/br2684.c
+++ b/kernel/linux/net/atm/br2684.c
@@ -34,6 +34,8 @@ Author: Marcell GAL, 2000, XDSL Ltd, Hungary
  * if we need space for the header
  */
 /* #define FASTER_VERSION */
  * if we need space for the header
  */
 /* #define FASTER_VERSION */

+//#define VLAN_DEBUG
+//#define SKB_DEBUG

 
 #ifdef DEBUG
 #define DPRINTK(format, args...) printk(KERN_DEBUG "br2684: " format, ##args)
 
 #ifdef DEBUG
 #define DPRINTK(format, args...) printk(KERN_DEBUG "br2684: " format, ##args)


@@ -84,6 +86,11 @@ struct br2684_vcc {
           When turned on, all non-PPPoE traffic will be dropped
           on this PVC */  
         int proto_filter;
           When turned on, all non-PPPoE traffic will be dropped
           on this PVC */  
         int proto_filter;

+#ifdef SUPPORT_VLAN
+        unsigned short vlan_id;        /* vlan id (0-4096) */
+#endif  // SUPPORT_VLAN
+
+

 #endif
 };
 
 #endif
 };
 


@@ -155,6 +162,70 @@ static struct net_device *br2684_find_dev(const struct br2684_if_spec *s)
        return NULL;
 }
 
        return NULL;
 }
 

+#ifdef SUPPORT_VLAN
+#include <linux/if_vlan.h>
+/**
+ * vlan_tag_insert - regular VLAN tag inserting
+ * @skb: skbuff to tag
+ * @tag: VLAN tag to insert
+ *
+ * Inserts the VLAN tag into @skb as part of the payload
+ * Returns a VLAN tagged skb. If a new skb is created, @skb is freed.
+ * 
+ * Following the skb_unshare() example, in case of error, the calling function
+ * doesn't have to worry about freeing the original skb.
+ */
+#define ATM_HEADER_LEN     10
+#define ATM_AND_MAC_LEN    ((ATM_HEADER_LEN) + (2 * VLAN_ETH_ALEN))
+
+struct atm_vlan_ethhdr {
+   unsigned char        h_atm_stuff[ATM_HEADER_LEN]; /* atm stuff 10 bytes */
+   unsigned char       h_dest[ETH_ALEN];          /* destination eth addr      */
+   unsigned char       h_source[ETH_ALEN];        /* source ether addr */
+   unsigned short       h_vlan_proto;              /* Should always be 0x8100 */
+   unsigned short       h_vlan_TCI;                /* Encapsulates priority and VLAN ID */
+   unsigned short      h_vlan_encapsulated_proto; /* packet type ID field (or len) */
+};
+
+static inline struct sk_buff *vlan_tag_insert(struct sk_buff *skb, unsigned short tag)
+{
+       struct atm_vlan_ethhdr *veth;
+
+       if (skb_headroom(skb) < VLAN_HLEN) {
+               struct sk_buff *sk_tmp = skb;
+               skb = skb_realloc_headroom(sk_tmp, VLAN_HLEN);
+               kfree_skb(sk_tmp);
+               if (!skb) {
+                       printk(KERN_ERR "vlan: failed to realloc headroom\n");
+                       return NULL;
+               }
+       } else {
+               skb = skb_unshare(skb, GFP_ATOMIC);
+               if (!skb) {
+                       printk(KERN_ERR "vlan: failed to unshare skbuff\n");
+                       return NULL;
+               }
+       }
+
+       veth = (struct atm_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) + ATM_HEADER_LEN);
+
+       /* first, the ethernet type */
+       veth->h_vlan_proto = __constant_htons(ETH_P_8021Q);
+
+       /* now, the tag */
+       veth->h_vlan_TCI = htons(tag);
+
+       skb->protocol = __constant_htons(ETH_P_8021Q);
+       skb->mac.raw -= VLAN_HLEN;
+       skb->nh.raw -= VLAN_HLEN;
+
+       return skb;
+}
+#endif  // SUPPORT_VLAN
+

 /*
  * Send a packet out a particular vcc.  Not to useful right now, but paves
  * the way for multiple vcc's per itf.  Returns true if we can send,
 /*
  * Send a packet out a particular vcc.  Not to useful right now, but paves
  * the way for multiple vcc's per itf.  Returns true if we can send,


@@ -164,6 +235,7 @@ static int br2684_xmit_vcc(struct sk_buff *skb, struct br2684_dev *brdev,
        struct br2684_vcc *brvcc)
 {
        struct atm_vcc *atmvcc;
        struct br2684_vcc *brvcc)
 {
        struct atm_vcc *atmvcc;

+

 #ifdef FASTER_VERSION
        if (brvcc->encaps == e_llc)
                memcpy(skb_push(skb, 8), llc_oui_pid_pad, 8);
 #ifdef FASTER_VERSION
        if (brvcc->encaps == e_llc)
                memcpy(skb_push(skb, 8), llc_oui_pid_pad, 8);


@@ -204,7 +276,40 @@ static int br2684_xmit_vcc(struct sk_buff *skb, struct br2684_dev *brdev,
        }
 #endif
 
        }
 #endif
 

-       skb_debug(skb);
+#ifdef SUPPORT_VLAN
+#ifdef VLAN_DEBUG
+        printk("=====> br2684_xmit_vcc  bef add vlan tag, skb->len=0x%04x\n", skb->len);
+        skb_debug(skb);
+#endif // VLAN_DEBUG
+
+       /* Construct the second two bytes. This field looks something
+        * like:
+        * usr_priority: 3 bits  (high bits)
+        * CFI           1 bit
+        * VLAN ID       12 bits (low bits)
+        */
+       //brvcc->vlan_id |= vlan_dev_get_egress_qos_mask(dev, skb);
+        /* bit 3-0 of the 32-bit nfmark is the atm priority, set by iptables
+         * bit 7-4 is the Ethernet switch physical port number, set by lan port drivers.
+         * bit 8-11 is the wanVlan priority bits
+         */
+       if (brvcc->vlan_id != 0xffff) {
+               brvcc->vlan_id &= 0xffff0fff;   // clear the priority bits first
+               // if bit 8-11 is set (none zeros), add in the priority bits
+               if (skb->nfmark & 0x0000f000) {
+                       brvcc->vlan_id |= (skb->nfmark & 0x0000f000);
+               }
+               skb = vlan_tag_insert(skb, brvcc->vlan_id);
+               if (!skb) {
+                       brdev->stats.tx_dropped++;
+                       return 1;
+               }
+#ifdef VLAN_DEBUG
+               printk("=====> br2684_xmit_vcc  aft add vlan tag, skb->len=%d\n", skb->len);
+               skb_debug(skb);
+#endif // VLAN_DEBUG
+       }
+#endif  // SUPPORT_VLAN

 
        ATM_SKB(skb)->vcc = atmvcc = brvcc->atmvcc;
        DPRINTK("atm_skb(%p)->vcc(%p)->dev(%p)\n", skb, atmvcc, atmvcc->dev);
 
        ATM_SKB(skb)->vcc = atmvcc = brvcc->atmvcc;
        DPRINTK("atm_skb(%p)->vcc(%p)->dev(%p)\n", skb, atmvcc, atmvcc->dev);


@@ -241,7 +346,6 @@ static int br2684_start_xmit(struct sk_buff *skb, struct net_device *dev)
 {
        struct br2684_dev *brdev = BRPRIV(dev);
        struct br2684_vcc *brvcc;
 {
        struct br2684_dev *brdev = BRPRIV(dev);
        struct br2684_vcc *brvcc;

-

        DPRINTK("br2684_start_xmit, skb->dst=%p\n", skb->dst);
        read_lock(&devs_lock);
        brvcc = pick_outgoing_vcc(skb, brdev);
        DPRINTK("br2684_start_xmit, skb->dst=%p\n", skb->dst);
        read_lock(&devs_lock);
        brvcc = pick_outgoing_vcc(skb, brdev);


@@ -259,13 +363,18 @@ static int br2684_start_xmit(struct sk_buff *skb, struct net_device *dev)
        if (brvcc->proto_filter & FILTER_PPPOE) {
                if ((skb->protocol != htons(ETH_P_PPP_DISC)) && (skb->protocol != htons(ETH_P_PPP_SES))) {
                        DPRINTK("non-PPPOE packet dropped on TX dev %s\n", dev->name);
        if (brvcc->proto_filter & FILTER_PPPOE) {
                if ((skb->protocol != htons(ETH_P_PPP_DISC)) && (skb->protocol != htons(ETH_P_PPP_SES))) {
                        DPRINTK("non-PPPOE packet dropped on TX dev %s\n", dev->name);

-                       brdev->stats.tx_dropped++;

                        dev_kfree_skb(skb);
                        read_unlock(&devs_lock);
                        return 0;
                }
        }
 #endif 
                        dev_kfree_skb(skb);
                        read_unlock(&devs_lock);
                        return 0;
                }
        }
 #endif 

+
+#ifdef VLAN_DEBUG
+        if (brvcc->vlan_id != 0xffff)
+                printk("=====> br2684_start_xmit  vlan_id=0x%04x\n", brvcc->vlan_id);
+#endif // VLAN_DEBUG
+

        if (!br2684_xmit_vcc(skb, brdev, brvcc)) {
                /*
                 * We should probably use netif_*_queue() here, but that
        if (!br2684_xmit_vcc(skb, brdev, brvcc)) {
                /*
                 * We should probably use netif_*_queue() here, but that


@@ -472,7 +581,6 @@ static void br2684_push(struct atm_vcc *atmvcc, struct sk_buff *skb)
        //skb->__unused=FROM_WAN;
 #endif 
 
        //skb->__unused=FROM_WAN;
 #endif 
 

-       skb_debug(skb);

        atm_return(atmvcc, skb->truesize);
        DPRINTK("skb from brdev %p\n", brdev);
        if (brvcc->encaps == e_llc) {
        atm_return(atmvcc, skb->truesize);
        DPRINTK("skb from brdev %p\n", brdev);
        if (brvcc->encaps == e_llc) {


@@ -502,6 +610,28 @@ static void br2684_push(struct atm_vcc *atmvcc, struct sk_buff *skb)
                dev_kfree_skb(skb);     /* dev_ not needed? */
                return;
        }
                dev_kfree_skb(skb);     /* dev_ not needed? */
                return;
        }

+#ifdef SUPPORT_VLAN
+   if (brvcc->vlan_id != 0xffff) { /*  Vcc was configured to be vlan tagged*/
+               unsigned tmp[ATM_AND_MAC_LEN];
+#ifdef VLAN_DEBUG
+               printk("=====> before removing vlan id\n");
+               skb_debug(skb);
+#endif
+/*
+** There exist a situation where we tag vlan id upstream. But DSLAM sends untagged frame downstream.  So we need to check this situation before we move data around
+*/
+//eddie added if {}
+           if ( skb->data[22] == 0x81 && skb->data[23] == 0x0) {
+               memcpy(tmp, skb->data, ATM_AND_MAC_LEN);
+               skb_pull(skb, VLAN_HLEN);
+               memcpy(skb->data, tmp, ATM_AND_MAC_LEN);
+           }
+#ifdef VLAN_DEBUG
+               printk("=====> after removing vlan id\n");
+               skb_debug(skb);
+#endif
+       }
+#endif // SUPPORT_VLAN

 
 #ifdef FASTER_VERSION
        /* FIXME: tcpdump shows that pointer to mac header is 2 bytes earlier,
 
 #ifdef FASTER_VERSION
        /* FIXME: tcpdump shows that pointer to mac header is 2 bytes earlier,


@@ -529,16 +659,16 @@ static void br2684_push(struct atm_vcc *atmvcc, struct sk_buff *skb)
        if (brvcc->proto_filter & FILTER_PPPOE) {
                if ((skb->protocol != htons(ETH_P_PPP_DISC)) && (skb->protocol != htons(ETH_P_PPP_SES))) {
                        DPRINTK("non-PPPOE packet dropped on RX dev %s\n", net_dev->name);
        if (brvcc->proto_filter & FILTER_PPPOE) {
                if ((skb->protocol != htons(ETH_P_PPP_DISC)) && (skb->protocol != htons(ETH_P_PPP_SES))) {
                        DPRINTK("non-PPPOE packet dropped on RX dev %s\n", net_dev->name);

-                       brdev->stats.rx_dropped++;

                        dev_kfree_skb(skb);
                        return;
                }
        }
 #endif
                        dev_kfree_skb(skb);
                        return;
                }
        }
 #endif

-       skb->dev = net_dev;
+
+        skb->dev = net_dev;

        ATM_SKB(skb)->vcc = atmvcc;     /* needed ? */
        DPRINTK("received packet's protocol: %x\n", ntohs(skb->protocol));
        ATM_SKB(skb)->vcc = atmvcc;     /* needed ? */
        DPRINTK("received packet's protocol: %x\n", ntohs(skb->protocol));

-       skb_debug(skb);
+

        if (unlikely(!(net_dev->flags & IFF_UP))) {
                /* sigh, interface is down */
                brdev->stats.rx_dropped++;
        if (unlikely(!(net_dev->flags & IFF_UP))) {
                /* sigh, interface is down */
                brdev->stats.rx_dropped++;


@@ -548,6 +678,7 @@ static void br2684_push(struct atm_vcc *atmvcc, struct sk_buff *skb)
        brdev->stats.rx_packets++;
        brdev->stats.rx_bytes += skb->len;
        memset(ATM_SKB(skb), 0, sizeof(struct atm_skb_data));
        brdev->stats.rx_packets++;
        brdev->stats.rx_bytes += skb->len;
        memset(ATM_SKB(skb), 0, sizeof(struct atm_skb_data));

+

        netif_rx(skb);
 }
 
        netif_rx(skb);
 }
 


@@ -613,6 +744,9 @@ Note: we do not have explicit unassign, but look at _push()
        brvcc->old_push = atmvcc->push;
 #if defined(CONFIG_MIPS_BRCM)
        brvcc->proto_filter |= be.proto_filter;
        brvcc->old_push = atmvcc->push;
 #if defined(CONFIG_MIPS_BRCM)
        brvcc->proto_filter |= be.proto_filter;

+#ifdef SUPPORT_VLAN
+        brvcc->vlan_id = be.vlan_id;
+#endif // SUPPORT_VLAN

 #endif 
        barrier();
        atmvcc->push = br2684_push;
 #endif 
        barrier();
        atmvcc->push = br2684_push;