dev.c

一个基于linux的TCP/IP协议栈的实现

/* dev.c 
 * [e-mail]
 * 2006-09-18
 */

#include "dev_dummy.h"
#include "dev.h"
#include "log.h"

#include <linux/netpoll.h>
#include <linux/list.h>
#include <linux/rtnetlink.h>
#include <net/sch_generic.h>
#include <linux/delay.h>
#include <linux/seq_file.h>
#include <linux/proc_fs.h>
#include <net/sock.h>
#include <linux/inetdevice.h>

extern struct neigh_table myarp_tbl;
static gifconf_func_t * gifconf_list[NPROTO];
static struct notifier_block *mynetdev_chain;

static DEFINE_SPINLOCK(myptype_lock);
static struct list_head myptype_base[16];
static struct list_head myptype_all;

DEFINE_PER_CPU(struct softnet_data, mysoftnet_data) = { NULL };
DEFINE_PER_CPU(struct netif_rx_stats, mynetdev_rx_stat) = { 0, };

int mynetdev_max_backlog = 1000;
int mynetdev_budget = 300;
int myweight_p = 64;            /* old backlog weight */

int mynetdev_nit;

void mydev_add_pack(struct packet_type *pt)
{
	int hash;

	spin_lock_bh( &myptype_lock );

	if( pt->type == htons(ETH_P_ALL) ){
		mynetdev_nit++;
		list_add_rcu(&pt->list, &myptype_all);
	} else {
		hash = ntohs( pt->type ) & 15;
		list_add_rcu( &pt->list, &myptype_base[hash] );
	}

	spin_unlock_bh( &myptype_lock );
}

void __mydev_remove_pack(struct packet_type *pt)
{
	struct list_head *head;
	struct packet_type *pt1;

	spin_lock_bh( &myptype_lock );

	if( pt->type == htons(ETH_P_ALL) ){
		mynetdev_nit--;
		head = &myptype_all;
	} else
		head = &myptype_base[ntohs(pt->type) & 15];

	list_for_each_entry(pt1, head, list) {
		if( pt == pt1 ){
			list_del_rcu( &pt->list );
			goto out;
		}
	}

	PR_WARN( "dev_remove_pack: %p not found.\n", pt );
out:
	spin_unlock_bh( &myptype_lock );
}

void mydev_remove_pack(struct packet_type *pt)
{
	__mydev_remove_pack(pt);	
	synchronize_net();
}

int mydev_queue_xmit(struct sk_buff *skb)
{
	return 0;
}

static inline void __mynetif_rx_schedule(struct net_device *dev)
{
	unsigned long flags;

	local_irq_save( flags );
	dev_hold( dev );

	list_add_tail(&dev->poll_list, &__get_cpu_var( mysoftnet_data ).poll_list);
	if (dev->quota < 0)
		dev->quota += dev->weight;
	else
		dev->quota = dev->weight;
	__raise_softirq_irqoff( MYNET_RX_SOFTIRQ );

	local_irq_restore( flags );
}

static inline void mynetif_rx_schedule(struct net_device *dev)
{
	if (netif_rx_schedule_prep(dev))
		__mynetif_rx_schedule(dev);
}

int mynetif_rx( struct sk_buff *skb )
{
	struct softnet_data *queue;
	unsigned long flags;

	PR_DEBUG( "receive a packet!\n" );

	local_irq_save( flags );
	queue = &__get_cpu_var( mysoftnet_data );
	__get_cpu_var( mynetdev_rx_stat ).total++;

	if( queue->input_pkt_queue.qlen <= mynetdev_max_backlog ){
		if (queue->input_pkt_queue.qlen) {
enqueue:
			dev_hold(skb->dev);
			__skb_queue_tail( &queue->input_pkt_queue, skb );
			local_irq_restore( flags );
			return NET_RX_SUCCESS;
		}

		mynetif_rx_schedule( &queue->backlog_dev );
		goto enqueue;
	}

	return 0;	
}

static __inline__ int mydeliver_skb(struct sk_buff *skb,
				struct packet_type *pt_prev, struct net_device *orig_dev)
{
	atomic_inc( &skb->users );
	return pt_prev->func( skb, skb->dev, pt_prev, orig_dev );
}

static inline struct net_device *myskb_bond(struct sk_buff *skb)
{
	struct net_device *dev = skb->dev;

	if( dev->master )
		skb->dev = dev->master;

	return dev;
}

int mynetif_receive_skb(struct sk_buff *skb)
{
	struct packet_type *ptype, *pt_prev;
	struct net_device *orig_dev;
	int ret = 0;
	unsigned short type;

	if( !skb->input_dev )
		skb->input_dev = skb->dev;

	PR_DEBUG( "mynetif_receive_skb\n" );
	__get_cpu_var(mynetdev_rx_stat).total++;

	orig_dev = myskb_bond(skb);

	pt_prev = NULL;
	rcu_read_lock();
/*	
	list_for_each_entry_rcu(ptype, &myptype_all, list) {
		if (!ptype->dev || ptype->dev == skb->dev) {
			if (pt_prev) 
				ret = mydeliver_skb(skb, pt_prev, orig_dev);
			pt_prev = ptype;
		}
	}
*/
	type = skb->protocol;
	list_for_each_entry_rcu( ptype, &myptype_base[ntohs(type)&15], list ){
		if( ptype->type == type && (!ptype->dev || ptype->dev == skb->dev) ){
			if( pt_prev ) 
				ret = mydeliver_skb( skb, pt_prev, orig_dev );
			pt_prev = ptype;
		}
	}

	if (pt_prev) {
		ret = pt_prev->func( skb, skb->dev, pt_prev, orig_dev );
	}else{
		kfree_skb( skb );
		ret = NET_RX_DROP;
	}
	rcu_read_unlock();
	return ret;
}

static int myprocess_backlog(struct net_device *backlog_dev, int *budget)
{
	int work = 0;
	int quota = min(backlog_dev->quota, *budget);
	struct softnet_data *queue = &__get_cpu_var( mysoftnet_data );
	unsigned long start_time = jiffies;

	backlog_dev->weight = myweight_p;
	for (;;) {
		struct sk_buff *skb;
		struct net_device *dev;

		local_irq_disable();
		skb = __skb_dequeue( &queue->input_pkt_queue );
		if (!skb)
			goto job_done;
		local_irq_enable();
		dev = skb->dev;
		mynetif_receive_skb(skb);
		dev_put(dev);
		work++;
		if (work >= quota || jiffies - start_time > 1)
			break;
	}

	backlog_dev->quota -= work;
	*budget -= work;
	return -1;

job_done:
	backlog_dev->quota -= work;
	*budget -= work;

	list_del( &backlog_dev->poll_list );
	smp_mb__before_clear_bit();
	netif_poll_enable(backlog_dev);

	local_irq_enable();
	return 0;
}

static void mynet_tx_action(struct softirq_action *h)
{

}

static void mynet_rx_action(struct softirq_action *h)
{
	struct softnet_data *queue = &__get_cpu_var( mysoftnet_data );
	unsigned long start_time = jiffies;
	int budget = mynetdev_budget;
	void *have;

	PR_DEBUG( "receive a packet!\n" );

	local_irq_disable();

	while( !list_empty(&queue->poll_list) ){
		struct net_device *dev;
		if (budget <= 0 || jiffies - start_time > 1)
			goto softnet_break;
		local_irq_enable();
		dev = list_entry(queue->poll_list.next, struct net_device, poll_list);
		have = netpoll_poll_lock(dev);

		if (dev->quota <= 0 || dev->poll(dev, &budget)) {
			netpoll_poll_unlock(have);
			local_irq_disable();
			list_del(&dev->poll_list);
			list_add_tail(&dev->poll_list, &queue->poll_list);
			if (dev->quota < 0)
				dev->quota += dev->weight;
			else
				dev->quota = dev->weight;
		} else {
			netpoll_poll_unlock(have);
			dev_put(dev);
			local_irq_disable();
		}
	}
out:
	local_irq_enable();
	return;

softnet_break:
	__get_cpu_var( mynetdev_rx_stat ).time_squeeze++;
	__raise_softirq_irqoff( MYNET_RX_SOFTIRQ );
	goto out;
}

int myregister_gifconf( unsigned int family, gifconf_func_t * gifconf )
{
	if (family >= NPROTO)
		return -EINVAL;
	gifconf_list[family] = gifconf;
	return 0;
}

int myregister_netdevice_notifier( struct notifier_block *nb )
{
	struct net_device *dev;
	int err;

	rtnl_lock();
	err = notifier_chain_register( &mynetdev_chain, nb );
	if( !err ){
		for( dev = dev_base; dev; dev = dev->next ){
			if( strstr( dev->name, "my" ) == NULL )
				continue;
			nb->notifier_call(nb, NETDEV_REGISTER, dev);

			if (dev->flags & IFF_UP) 
				nb->notifier_call(nb, NETDEV_UP, dev);
		}
	}
	rtnl_unlock();

	return err;
}

int myunregister_netdevice_notifier( struct notifier_block *nb )
{
	return notifier_chain_unregister( &mynetdev_chain, nb );
}

int mydev_close(struct net_device *dev)
{
	if( !(dev->flags & IFF_UP) )
		return 0;

	notifier_call_chain( &mynetdev_chain, NETDEV_GOING_DOWN, dev );
	//dev_deactivate(dev);
	clear_bit(__LINK_STATE_START, &dev->state);

	smp_mb__after_clear_bit();
	while( test_bit(__LINK_STATE_RX_SCHED, &dev->state) ){
		msleep(1);
	}
	if( dev->stop )
		dev->stop( dev );
	dev->flags &= ~IFF_UP;
	notifier_call_chain( &mynetdev_chain, NETDEV_DOWN, dev );
	return 0;
}

int mydev_open(struct net_device *dev)
{
	int ret = 0;
	if( dev->flags & IFF_UP )
		return 0;
	if( !netif_device_present(dev) )
		return -ENODEV;

	set_bit(__LINK_STATE_START, &dev->state);
	if (dev->open) {
		ret = dev->open(dev);
		if (ret)
			clear_bit(__LINK_STATE_START, &dev->state);
	}

	if (!ret) {
		dev->flags |= IFF_UP;
		mydev_mc_upload(dev);
		//dev_activate(dev);
		notifier_call_chain( &mynetdev_chain, NETDEV_UP, dev );
	}
	return ret;
}

void myunregister_netdev(struct net_device *dev)
{
	struct in_device *in_dev = __in_dev_get_rtnl( dev );
	neigh_parms_release( &myarp_tbl, in_dev->arp_parms );
	unregister_netdev( dev );
}

int myregister_netdev( struct net_device *dev )
{
	int rc = register_netdev( dev );
	rtnl_lock();
	notifier_call_chain( &mynetdev_chain, NETDEV_REGISTER, dev );
	rtnl_unlock();
	return rc;
}

void mydev_load(const char *name)
{
	struct net_device *dev;  

	read_lock(&dev_base_lock);
	dev = __dev_get_by_name(name);
	read_unlock(&dev_base_lock);

	if (!dev && capable(CAP_SYS_MODULE))
		request_module("%s", name);
}

unsigned mydev_get_flags(const struct net_device *dev)
{
	unsigned flags;

	flags = (dev->flags & ~(IFF_PROMISC | IFF_ALLMULTI | IFF_RUNNING)) | 
			(dev->gflags & (IFF_PROMISC | IFF_ALLMULTI));

	if( netif_running(dev) && netif_carrier_ok(dev) )
		flags |= IFF_RUNNING;

	return flags;
}

void mydev_set_promiscuity(struct net_device *dev, int inc)
{
	unsigned short old_flags = dev->flags;

	if( (dev->promiscuity += inc) == 0 )
		dev->flags &= ~IFF_PROMISC;
	else
		dev->flags |= IFF_PROMISC;
	if( dev->flags != old_flags ){
		mydev_mc_upload(dev);
		PR_NOTICE( "device %s %s promiscuous mode\n",
						dev->name, (dev->flags & IFF_PROMISC) ? "entered" : "left");
	}
}

void mydev_set_allmulti(struct net_device *dev, int inc)
{
	unsigned short old_flags = dev->flags;

	dev->flags |= IFF_ALLMULTI;
	if( (dev->allmulti += inc) == 0 )
		dev->flags &= ~IFF_ALLMULTI;
	if( dev->flags ^ old_flags )
		mydev_mc_upload(dev);
}

int mydev_change_flags(struct net_device *dev, unsigned flags)
{
	int ret;
	int old_flags = dev->flags;

	dev->flags = (flags & (IFF_DEBUG | IFF_NOTRAILERS | IFF_NOARP |
							IFF_DYNAMIC | IFF_MULTICAST | IFF_PORTSEL |
							IFF_AUTOMEDIA)) |
			(dev->flags & (IFF_UP | IFF_VOLATILE | IFF_PROMISC |
						   IFF_ALLMULTI));
	
	mydev_mc_upload(dev);

	ret = 0;
	if( (old_flags ^ flags) & IFF_UP ){
		ret = ( (old_flags & IFF_UP) ? mydev_close : mydev_open)(dev);
		if( !ret )
			mydev_mc_upload(dev);
	}

	if( dev->flags & IFF_UP && ((old_flags ^ dev->flags) 
							& ~(IFF_UP | IFF_PROMISC | IFF_ALLMULTI |
									IFF_VOLATILE)))
		notifier_call_chain( &mynetdev_chain, NETDEV_CHANGE, dev );

	if( (flags ^ dev->gflags) & IFF_PROMISC ){
		int inc = (flags & IFF_PROMISC) ? +1 : -1;
		dev->gflags ^= IFF_PROMISC;
		mydev_set_promiscuity(dev, inc);
	}
	if( (flags ^ dev->gflags) & IFF_ALLMULTI ){
		int inc = (flags & IFF_ALLMULTI) ? +1 : -1;
		dev->gflags ^= IFF_ALLMULTI;
		mydev_set_allmulti( dev, inc );
	}
	//if( old_flags ^ dev->flags )
	//	rtmsg_ifinfo(RTM_NEWLINK, dev, old_flags ^ dev->flags);
	return ret;
}

int mydev_set_mtu(struct net_device *dev, int new_mtu)
{
	int err;
	if (new_mtu == dev->mtu)