myfib_hash.c

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

#define FZ_MAX_DIVISOR ((PAGE_SIZE<<MAX_ORDER) / sizeof(struct hlist_head))

static DEFINE_RWLOCK(myfib_hash_lock);
static unsigned int myfib_hash_genid;

static kmem_cache_t *myfn_hash_kmem __read_mostly;
static kmem_cache_t *myfn_alias_kmem __read_mostly;

struct fib_node {
	struct hlist_node	fn_hash;
	struct list_head	fn_alias;
	u32			fn_key;
};

struct fn_zone {
	struct fn_zone		*fz_next;
	struct hlist_head	*fz_hash;
	int			fz_nent;

	int			fz_divisor;
	u32			fz_hashmask;
#define FZ_HASHMASK(fz)		((fz)->fz_hashmask)

	int			fz_order;
	u32			fz_mask;
#define FZ_MASK(fz)		((fz)->fz_mask)
};

struct fn_hash {
	struct fn_zone	*fn_zones[33];
	struct fn_zone	*fn_zone_list;
};

void myrt_cache_flush(int delay);

static inline u32 myfz_key(u32 dst, struct fn_zone *fz)
{
	return dst & FZ_MASK(fz);
}

static inline u32 myfn_hash(u32 key, struct fn_zone *fz)
{
	u32 h = ntohl(key)>>(32 - fz->fz_order);
	h ^= (h>>20);
	h ^= (h>>10);
	h ^= (h>>5);
	h &= FZ_HASHMASK(fz);
	return h;
}

static int myfn_hash_lookup(struct fib_table *tb, 
							const struct flowi *flp, struct fib_result *res)
{
	int err;
	struct fn_zone *fz;
	struct fn_hash *t = (struct fn_hash*)tb->tb_data;

	read_lock(&myfib_hash_lock);
	for (fz = t->fn_zone_list; fz; fz = fz->fz_next) {
		struct hlist_head *head;
		struct hlist_node *node;
		struct fib_node *f;
		u32 k = myfz_key(flp->fl4_dst, fz);
		printk(KERN_INFO "dst: %u.%u.%u.%u\n", NIPQUAD(flp->fl4_dst) );
		head = &fz->fz_hash[myfn_hash(k, fz)];
		hlist_for_each_entry(f, node, head, fn_hash) {
				if (f->fn_key != k)
						continue;
				printk(KERN_INFO "the key ip: %u.%u.%u.%u\n",  NIPQUAD(k) );
				err = myfib_semantic_match(&f->fn_alias,
								flp, res,
								f->fn_key, fz->fz_mask,
								fz->fz_order);
				if (err <= 0)
						goto out;
		}
	}
	err = 1;
out:
	read_unlock(&myfib_hash_lock);
	return err;
}

static struct hlist_head *myfz_hash_alloc(int divisor)
{
	unsigned long size = divisor * sizeof(struct hlist_head);

	if (size <= PAGE_SIZE)
		return kmalloc(size, GFP_KERNEL);
	else
		return (struct hlist_head *)
			__get_free_pages(GFP_KERNEL, get_order(size));
}

static struct fn_zone* myfn_new_zone(struct fn_hash *table, int z)
{
	int i;
	struct fn_zone *fz = kmalloc(sizeof(struct fn_zone), GFP_KERNEL);
	if (!fz)
		return NULL;
	memset(fz, 0, sizeof(struct fn_zone));

	if( z )
		fz->fz_divisor = 16;
	else
		fz->fz_divisor = 1;

	fz->fz_hashmask = (fz->fz_divisor - 1);
	fz->fz_hash = myfz_hash_alloc(fz->fz_divisor);
	if (!fz->fz_hash) {
		kfree(fz);
		return NULL;
	}

	memset(fz->fz_hash, 0, fz->fz_divisor * sizeof(struct hlist_head *));
	fz->fz_order = z;
	fz->fz_mask = inet_make_mask(z);
	for( i = z+1; i <= 32; i++ )
		if( table->fn_zones[i] )
			break;

	write_lock_bh(&myfib_hash_lock);
	if( i>32 ){
		fz->fz_next = table->fn_zone_list;
		table->fn_zone_list = fz;
	}else{
		fz->fz_next = table->fn_zones[i]->fz_next;
		table->fn_zones[i]->fz_next = fz;
	}
	table->fn_zones[z] = fz;
	myfib_hash_genid++;
	write_unlock_bh(&myfib_hash_lock);
	return fz;

}

static inline void myfn_rebuild_zone(struct fn_zone *fz,
				struct hlist_head *old_ht,
				int old_divisor)
{
	int i;

	for (i = 0; i < old_divisor; i++) {
		struct hlist_node *node, *n;
		struct fib_node *f;

		hlist_for_each_entry_safe(f, node, n, &old_ht[i], fn_hash) {
			struct hlist_head *new_head;

			hlist_del(&f->fn_hash);

			new_head = &fz->fz_hash[myfn_hash(f->fn_key, fz)];
			hlist_add_head(&f->fn_hash, new_head);
		}
	}
}

static void myfz_hash_free(struct hlist_head *hash, int divisor)
{
	unsigned long size = divisor * sizeof(struct hlist_head);

	if (size <= PAGE_SIZE)
		kfree(hash);
	else
		free_pages((unsigned long)hash, get_order(size));
}

static void myfn_rehash_zone(struct fn_zone *fz)
{
	struct hlist_head *ht, *old_ht;
	int old_divisor, new_divisor;
	u32 new_hashmask;
		
	old_divisor = fz->fz_divisor;

	switch (old_divisor) {
	case 16:
		new_divisor = 256;
		break;
	case 256:
		new_divisor = 1024;
		break;
	default:
		if ((old_divisor << 1) > FZ_MAX_DIVISOR) {
			printk(KERN_CRIT "route.c: bad divisor %d!\n", old_divisor);
			return;
		}
		new_divisor = (old_divisor << 1);
		break;
	}

	new_hashmask = (new_divisor - 1);

#if RT_CACHE_DEBUG >= 2
	printk("fn_rehash_zone: hash for zone %d grows from %d\n", fz->fz_order, old_divisor);
#endif

	ht = myfz_hash_alloc(new_divisor);

	if (ht)	{
		memset(ht, 0, new_divisor * sizeof(struct hlist_head));

		write_lock_bh( &myfib_hash_lock );
		old_ht = fz->fz_hash;
		fz->fz_hash = ht;
		fz->fz_hashmask = new_hashmask;
		fz->fz_divisor = new_divisor;
		myfn_rebuild_zone(fz, old_ht, old_divisor);
		myfib_hash_genid++;
		write_unlock_bh( &myfib_hash_lock );

		myfz_hash_free( old_ht, old_divisor );
	}
}

static struct fib_node *myfib_find_node(struct fn_zone *fz, u32 key)
{
	struct hlist_head *head = &fz->fz_hash[myfn_hash(key, fz)];
	struct hlist_node *node;
	struct fib_node *f;

	hlist_for_each_entry(f, node, head, fn_hash) {
		if (f->fn_key == key)
			return f;
	}

	return NULL;
}

struct fib_alias *myfib_find_alias(struct list_head *fah, u8 tos, u32 prio)
{
	if (fah) {
		struct fib_alias *fa;
		list_for_each_entry(fa, fah, fa_list) {
			if (fa->fa_tos > tos)
				continue;
			if (fa->fa_info->fib_priority >= prio ||
			    fa->fa_tos < tos)
				return fa;
		}
	}
	return NULL;
}

void myfib_release_info(struct fib_info *fi)
{
	write_lock( &myfib_info_lock );
	if (fi && --fi->fib_treeref == 0) {
		hlist_del(&fi->fib_hash);
		if (fi->fib_prefsrc)
			hlist_del(&fi->fib_lhash);
		mychange_nexthops(fi) {
			if (!nh->nh_dev)
				continue;
			hlist_del(&nh->nh_hash);
		} myendfor_nexthops(fi)
		fi->fib_dead = 1;
		fib_info_put(fi);
	}
	write_unlock( &myfib_info_lock );
}

static inline void myfib_insert_node(struct fn_zone *fz, struct fib_node *f)
{
	struct hlist_head *head = &fz->fz_hash[myfn_hash(f->fn_key, fz)];

	hlist_add_head(&f->fn_hash, head);
}

static int myfn_hash_insert(struct fib_table *tb, struct rtmsg *r, struct kern_rta *rta,
							struct nlmsghdr *n, struct netlink_skb_parms *req)
{
	struct fn_hash *table = (struct fn_hash *) tb->tb_data;
	struct fib_node *new_f, *f;
	struct fib_alias *fa, *new_fa;
	struct fn_zone *fz;
	struct fib_info *fi;
	int z = r->rtm_dst_len;
	int type = r->rtm_type;
	u8 tos = r->rtm_tos;
	u32 key;
	int err;

	if (z > 32)
		return -EINVAL;
	fz = table->fn_zones[z];
	if (!fz && !(fz = myfn_new_zone(table, z)))
		return -ENOBUFS;

	key = 0;
	if( rta->rta_dst ){
		u32 dst;
		memcpy(&dst, rta->rta_dst, 4);
		if (dst & ~FZ_MASK(fz))
			return -EINVAL;
		key = myfz_key(dst, fz);
	}
	printk(KERN_INFO "in insert hash, the key: %u.%u.%u.%u\n", NIPQUAD(key) );
	if( (fi = myfib_create_info(r, rta, n, &err)) == NULL )
		return err;

	if( fz->fz_nent > ( fz->fz_divisor << 1 ) &&
					fz->fz_divisor < FZ_MAX_DIVISOR &&
					(z == 32 || (1<<z) > fz->fz_divisor))
		myfn_rehash_zone( fz );

	f = myfib_find_node(fz, key);
	if (!f)
		fa = NULL;
	else
		fa = myfib_find_alias(&f->fn_alias, tos, fi->fib_priority);

	if (fa && fa->fa_tos == tos &&
	    fa->fa_info->fib_priority == fi->fib_priority) {
		struct fib_alias *fa_orig;

		err = -EEXIST;
		if (n->nlmsg_flags & NLM_F_EXCL)
			goto out;

		if (n->nlmsg_flags & NLM_F_REPLACE) {
			struct fib_info *fi_drop;
			u8 state;

			write_lock_bh( &myfib_hash_lock );
			fi_drop = fa->fa_info;
			fa->fa_info = fi;
			fa->fa_type = type;
			fa->fa_scope = r->rtm_scope;
			state = fa->fa_state;
			fa->fa_state &= ~FA_S_ACCESSED;
			myfib_hash_genid++;
			write_unlock_bh( &myfib_hash_lock );

			myfib_release_info(fi_drop);
			if (state & FA_S_ACCESSED)
				myrt_cache_flush(-1);
			return 0;
		}

		fa_orig = fa;
		fa = list_entry(fa->fa_list.prev, struct fib_alias, fa_list);
		list_for_each_entry_continue(fa, &f->fn_alias, fa_list) {
			if (fa->fa_tos != tos)
				break;
			if (fa->fa_info->fib_priority != fi->fib_priority)
				break;
			if (fa->fa_type == type &&
			    fa->fa_scope == r->rtm_scope &&
			    fa->fa_info == fi)
				goto out;
		}
		if (!(n->nlmsg_flags & NLM_F_APPEND))
			fa = fa_orig;
	}

	err = -ENOENT;
	if( !( n->nlmsg_flags & NLM_F_CREATE ) )
		goto out;

	err = -ENOBUFS;
	new_fa = kmem_cache_alloc(myfn_alias_kmem, SLAB_KERNEL);
	if (new_fa == NULL)
		goto out;

	new_f = NULL;
	if (!f) {
		new_f = kmem_cache_alloc(myfn_hash_kmem, SLAB_KERNEL);
		if (new_f == NULL)
			goto out_free_new_fa;

		INIT_HLIST_NODE(&new_f->fn_hash);
		INIT_LIST_HEAD(&new_f->fn_alias);
		new_f->fn_key = key;
		f = new_f;
	}

	new_fa->fa_info = fi;
	new_fa->fa_tos = tos;
	new_fa->fa_type = type;
	new_fa->fa_scope = r->rtm_scope;
	new_fa->fa_state = 0;

	write_lock_bh( &myfib_hash_lock );
	if (new_f)
		myfib_insert_node(fz, new_f);
	list_add_tail(&new_fa->fa_list,
		 (fa ? &fa->fa_list : &f->fn_alias));
	myfib_hash_genid++;
	write_unlock_bh( &myfib_hash_lock );

	if (new_f)
		fz->fz_nent++;
	myrt_cache_flush(-1);

	myrtmsg_fib(RTM_NEWROUTE, key, new_fa, z, tb->tb_id, n, req);
	return 0;

out_free_new_fa:
	kmem_cache_free(myfn_alias_kmem, new_fa);
out:
	myfib_release_info(fi);
	return err;
}

static int myfn_hash_delete(struct fib_table *tb, struct rtmsg *r, struct kern_rta *rta,
				struct nlmsghdr *n, struct netlink_skb_parms *req)
{
	return 0;
}

static int myfn_hash_flush(struct fib_table *tb)
{
	return 0;
}

static void myfn_hash_select_default(struct fib_table *tb, 
				const struct flowi *flp, struct fib_result *res)

{

}

static int myfn_hash_dump(struct fib_table *tb, 
				struct sk_buff *skb, struct netlink_callback *cb)
{
	return 0;
}