Vlan_net_card.rar_CARDVLAN_VLAN编程_vlan

#include <linux/module.h> #include <asm/uaccess.h> #include <asm/system.h> #include <linux/bitops.h> #include <linux/capability.h> #include <linux/types.h> #include <linux/kernel.h> #include <linux/mm.h> #include <linux/string.h> #include <linux/socket.h> #include <linux/sockios.h> #include <linux/errno.h> #include <linux/in.h> #include <linux/inet.h> #include <linux/inetdevice.h> #include <linux/netdevice.h> #include <linux/if_addr.h> #include <linux/if_arp.h> #include <linux/skbuff.h> #include <linux/netlink.h> #include <linux/init.h> #include <linux/list.h> #include <linux/if_addr.h> #include <linux/if_ether.h> #include <net/ip.h> #include <net/protocol.h> #include <net/tcp.h> #include <net/sock.h> #include <net/icmp.h> #include <net/arp.h> #include <net/ip_fib.h> #include "gsn_vlan.h" MODULE_LICENSE("Dual BSD/GPL"); /* ============================== */ #define VLAN_GRP_HASH_SHIFT 5 #define VLAN_GRP_HASH_SIZE (1 << VLAN_GRP_HASH_SHIFT) #define VLAN_GRP_HASH_MASK (VLAN_GRP_HASH_SIZE - 1) static int vlan_device_event(struct notifier_block *, unsigned long, void *); static int unregister_vlan_dev(struct net_device *, unsigned short); /* used for vlan visual device */ static struct notifier_block vlan_notifier_block = { .notifier_call = vlan_device_event, }; static struct packet_type vlan_packet_type = { .type = __constant_htons(ETH_P_8021Q), .func = vlan_skb_recv, }; /* Our listing of VLAN group(s) */ static struct hlist_head vlan_group_hash[VLAN_GRP_HASH_SIZE]; static struct lock_class_key vlan_netdev_xmit_lock_key; #define vlan_grp_hashfn(IDX) ((((IDX) >> VLAN_GRP_HASH_SHIFT) ^ (IDX)) & VLAN_GRP_HASH_MASK) /* ======== xxxx add the local route table ======= */ extern void fib_add_ifaddr(struct in_ifaddr *); /* ------ function ---------------- */ static inline u_int32_t make_mask(int mask_len) { if (mask_len) { return htonl(~((1<<(32 - mask_len)) - 1)); } return 0; } int vlan_do_skb(struct sk_buff *skb, struct net_device *dev, struct packet_type *ptype, struct net_device *orig_dev) { struct vlan_hdr *vhdr = (struct vlan_hdr *)(skb->data); unsigned short vid; struct net_device_stats *stats; unsigned short vlan_TCI; __be16 proto; vlan_TCI = ntohs(vhdr->h_vlan_TCI); vid = (vlan_TCI & VLAN_VID_MASK); rcu_read_lock(); skb->dev = __find_vlan_dev(dev, vid); /* from now, the skb->dev is the vlan dev*/ if (!skb->dev) { rcu_read_unlock(); /* XXX if we not register the VLAN, Should I deal with it ?*/ kfree_skb(skb); return -1; } skb->dev->last_rx = jiffies; stats = vlan_dev_get_stats(skb->dev); stats->rx_packets++; stats->rx_bytes += skb->len; skb_pull_rcsum(skb, VLAN_HLEN); if (dev != VLAN_DEV_INFO(skb->dev)->real_dev) { rcu_read_unlock(); printk("%s: dropping skb: %p because came in on wrong device, dev: %s real_dev: %s, skb_dev: %s\n", __FUNCTION__, skb, dev->name, VLAN_DEV_INFO(skb->dev)->real_dev->name, skb->dev->name); kfree_skb(skb); stats->rx_errors++; return -1; } skb->priority = vlan_get_ingress_priority(skb->dev, ntohs(vhdr->h_vlan_TCI)); switch (skb->pkt_type) { case PACKET_BROADCAST: break; case PACKET_MULTICAST: stats->multicast++; break; case PACKET_OTHERHOST: if (!compare_ether_addr(eth_hdr(skb)->h_dest, skb->dev->dev_addr)) { skb->pkt_type = PACKET_HOST; } break; } proto = vhdr->h_vlan_encapsulated_proto; skb->protocol = proto; rcu_read_unlock(); return 0; } static inline struct sk_buff *vlan_check_reorder_header(struct sk_buff *skb) { if (VLAN_DEV_INFO(skb->dev)->flags & 1) { if (skb_shared(skb) || skb_cloned(skb)) { struct sk_buff *nskb = skb_copy(skb, GFP_ATOMIC); kfree_skb(skb); skb = nskb; } if (skb) { /* the memmove is so good, when dest > src, the cope from last to first */ memmove(skb->data - ETH_HLEN, skb->data - VLAN_ETH_HLEN, 12); skb->mac.raw += VLAN_HLEN; } } return skb; } int vlan_skb_recv(struct sk_buff *skb, struct net_device *dev, struct packet_type *ptype, struct net_device *orig_dev) { if (vlan_do_skb(skb, dev, ptype, orig_dev) == -1) return -1; if (skb->protocol == __constant_htons(ETH_P_8021Q)) { printk("vlan_do_skb BUG!\n"); kfree(skb); return -1; } if (ntohs(skb->protocol) >= 1536) { skb = vlan_check_reorder_header(skb); if (skb) { netif_rx(skb); } else { /* stats->rx_errors++; */ printk("%s:skb error\n", __FUNCTION__); } return 0; } /* should alnayse more ??? */ printk("%s: the skb protocol no valid(should I deal with it ?), drop the skb\n", __FUNCTION__); kfree(skb); return -1; } static void route_add(void * parm, const char *eth_IF_name) { struct in_ifaddr * ifa; struct net_device *dev = dev_get_by_name(eth_IF_name); ifa = kmalloc(sizeof(struct in_ifaddr), GFP_ATOMIC); if (!ifa) { printk("error\n"); return ; } ifa->ifa_dev = in_dev_get(dev); ifa->ifa_mask = make_mask(24); ifa->ifa_local = 0; ifa->ifa_address = 255240384; ifa->ifa_prefixlen = 24; fib_add_ifaddr(ifa); in_dev_put(ifa->ifa_dev); dev_put(dev); kfree(ifa); return ; } static struct vlan_group * __vlan_find_group(int real_dev_ifindex) { struct vlan_group *grp; struct hlist_node *n; int hash = vlan_grp_hashfn(real_dev_ifindex); hlist_for_each_entry_rcu(grp, n, &vlan_group_hash[hash], hlist) { if (grp->real_dev_ifindex == real_dev_ifindex) return grp; } return NULL; } struct net_device * __find_vlan_dev(struct net_device * real_dev, unsigned short VID) { struct vlan_group * grp = __vlan_find_group(real_dev->ifindex); if (grp) return vlan_group_get_device(grp, VID); return NULL; } static void vlan_group_free(struct vlan_group *grp) { int i; for (i=0; i < VLAN_GROUP_ARRAY_SPLIT_PARTS; i++) kfree(grp->vlan_devices_arrays[i]); kfree(grp); } static void vlan_rcu_free(struct rcu_head *rcu) { vlan_group_free(container_of(rcu, struct vlan_group, rcu)); } static void vlan_setup(struct net_device *new_dev) { SET_MODULE_OWNER(new_dev); new_dev->get_stats = vlan_dev_get_stats; new_dev->priv_flags |= IFF_802_1Q_VLAN; new_dev->tx_queue_len = 0; new_dev->open = vlan_dev_open; new_dev->stop = vlan_dev_stop; new_dev->do_ioctl = vlan_dev_ioctl; new_dev->destructor = free_netdev; /* this should be called but I do it nothing */ new_dev->change_mtu = vlan_dev_change_mtu; new_dev->set_mac_address = vlan_dev_set_mac_address; new_dev->set_multicast_list = vlan_dev_set_multicast_list; } static void vlan_transfer_operstate(const struct net_device *dev, struct net_device *vlandev) { if (dev->operstate == IF_OPER_DORMANT) netif_dormant_on(vlandev); else netif_dormant_off(vlandev); if (netif_carrier_ok(dev)) { if (!netif_carrier_ok(vlandev)) netif_carrier_on(vlandev); } else { if (netif_carrier_ok(vlandev)) netif_carrier_off(vlandev); } } static int vlan_device_event(struct notifier_block *unused, unsigned long event, void *ptr) { struct net_device *dev = ptr; struct vlan_group *grp = __vlan_find_group(dev->ifindex); int i, flags; struct net_device *vlandev; if (!grp) goto out; switch(event) { case NETDEV_CHANGE: for (i = 0; i < VLAN_GROUP_ARRAY_PART_LEN; i++) { vlandev = vlan_group_get_device(grp, i); if (!vlandev) continue; vlan_transfer_operstate(dev, vlandev); } break; case NETDEV_DOWN: for (i = 0; i < VLAN_GROUP_ARRAY_PART_LEN; i++) { vlandev = vlan_group_get_device(grp, i); if (!vlandev) continue; flags = vlandev->flags; if (!(flags & IFF_UP)) continue; dev_change_flags(vlandev, flags & ~IFF_UP); } break; case NETDEV_UP: for (i = 0; i < VLAN_GROUP_ARRAY_PART_LEN; i++) { vlandev = vlan_group_get_device(grp, i); if (!vlandev) continue; flags = vlandev->flags; if (flags & IFF_UP) continue; dev_change_flags(vlandev, flags | IFF_UP); } break; case NETDEV_UNREGISTER: for (i = 0; i < VLAN_GROUP_ARRAY_LEN; i++) { int ret; vlandev