X-Git-Url: http://git.rot13.org/?p=bcm963xx.git;a=blobdiff_plain;f=kernel%2Flinux%2Fnet%2Fatm%2Fbr2684.c;h=43a1a0553879b25e228c9d3ca83743b9f208929f;hp=8b9f822763d3865c0a5af6c3b44440761f0bec1c;hb=57a096f051259ceaefd5977f30d269884e1dd248;hpb=9887430fc6b7c0f8eb8e81de2bfe3bba12d8d4a1 diff --git a/kernel/linux/net/atm/br2684.c b/kernel/linux/net/atm/br2684.c index 8b9f8227..43a1a055 100755 --- 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 */ +//#define VLAN_DEBUG +//#define SKB_DEBUG #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; +#ifdef SUPPORT_VLAN + unsigned short vlan_id; /* vlan id (0-4096) */ +#endif // SUPPORT_VLAN + + #endif }; @@ -155,6 +162,70 @@ static struct net_device *br2684_find_dev(const struct br2684_if_spec *s) return NULL; } +#ifdef SUPPORT_VLAN +#include +/** + * 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, @@ -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; + #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 - 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); @@ -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; - 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); - brdev->stats.tx_dropped++; 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 @@ -472,7 +581,6 @@ static void br2684_push(struct atm_vcc *atmvcc, struct sk_buff *skb) //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) { @@ -502,6 +610,28 @@ static void br2684_push(struct atm_vcc *atmvcc, struct sk_buff *skb) 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, @@ -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); - brdev->stats.rx_dropped++; 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)); - skb_debug(skb); + 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)); + 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; +#ifdef SUPPORT_VLAN + brvcc->vlan_id = be.vlan_id; +#endif // SUPPORT_VLAN #endif barrier(); atmvcc->push = br2684_push;