#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
