myfib_semantics.c

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

			nh->nh_scope = RT_SCOPE_LINK;
			return 0;
		}
		{
			struct flowi fl = { .nl_u = { .ip4_u =
					{ .daddr = nh->nh_gw,
							.scope = r->rtm_scope + 1 } },
						 .oif = nh->nh_oif };

			if (fl.fl4_scope < RT_SCOPE_LINK)
				fl.fl4_scope = RT_SCOPE_LINK;
			if ((err = myfib_lookup(&fl, &res)) != 0)
				return err;
		}
		err = -EINVAL;
		if( res.type != RTN_UNICAST && res.type != RTN_LOCAL )
			goto out;
		nh->nh_scope = res.scope;
		nh->nh_oif = FIB_RES_OIF(res);
		if( (nh->nh_dev = FIB_RES_DEV(res)) == NULL )
			goto out;
		dev_hold( nh->nh_dev );
		err = -ENETDOWN;
		if( !(nh->nh_dev->flags & IFF_UP) )
			goto out;
		err = 0;
out:
		fib_res_put(&res);
		return err;
	} else {
		struct in_device *in_dev;

		if( nh->nh_flags & (RTNH_F_PERVASIVE|RTNH_F_ONLINK) )
			return -EINVAL;

		in_dev = inetdev_by_index( nh->nh_oif );
		if (in_dev == NULL)
			return -ENODEV;
		if( !(in_dev->dev->flags & IFF_UP) ){
			in_dev_put(in_dev);
			return -ENETDOWN;
		}
		nh->nh_dev = in_dev->dev;
		dev_hold(nh->nh_dev);
		nh->nh_scope = RT_SCOPE_HOST;
		in_dev_put(in_dev);
	}
	return 0;
}

static __inline__ int mynh_comp(const struct fib_info *fi, const struct fib_info *ofi)
{
	const struct fib_nh *onh = ofi->fib_nh;

	myfor_nexthops(fi) {
		if (nh->nh_oif != onh->nh_oif ||
		    nh->nh_gw  != onh->nh_gw ||
		    nh->nh_scope != onh->nh_scope ||
#ifdef CONFIG_IP_ROUTE_MULTIPATH
		    nh->nh_weight != onh->nh_weight ||
#endif
#ifdef CONFIG_NET_CLS_ROUTE
		    nh->nh_tclassid != onh->nh_tclassid ||
#endif
		    ((nh->nh_flags^onh->nh_flags)&~RTNH_F_DEAD))
			return -1;
		onh++;
	} myendfor_nexthops(fi);
	return 0;
}

static struct fib_info *myfib_find_info(const struct fib_info *nfi)
{
	struct hlist_head *head;
	struct hlist_node *node;
	struct fib_info *fi;
	unsigned int hash;

	hash = myfib_info_hashfn(nfi);
	head = &myfib_info_hash[hash];

	hlist_for_each_entry(fi, node, head, fib_hash) {
		if (fi->fib_nhs != nfi->fib_nhs)
			continue;
		if (nfi->fib_protocol == fi->fib_protocol &&
		    nfi->fib_prefsrc == fi->fib_prefsrc &&
		    nfi->fib_priority == fi->fib_priority &&
		    memcmp(nfi->fib_metrics, fi->fib_metrics,
			   sizeof(fi->fib_metrics)) == 0 &&
			((nfi->fib_flags^fi->fib_flags)&~RTNH_F_DEAD) == 0 &&
			(nfi->fib_nhs == 0 || mynh_comp(fi, nfi) == 0))
			return fi;
	}

	return NULL;
}

void myfree_fib_info(struct fib_info *fi)
{
	if (fi->fib_dead == 0) {
		printk("Freeing alive fib_info %p\n", fi);
		return;
	}
	mychange_nexthops(fi) {
		if (nh->nh_dev)
			dev_put(nh->nh_dev);
		nh->nh_dev = NULL;
	} myendfor_nexthops(fi);
	myfib_info_cnt--;
	kfree(fi);
}

static inline unsigned int myfib_devindex_hashfn(unsigned int val)
{
	unsigned int mask = DEVINDEX_HASHSIZE - 1;

	return (val ^
		(val >> DEVINDEX_HASHBITS) ^
		(val >> (DEVINDEX_HASHBITS * 2))) & mask;
}

struct fib_info *myfib_create_info(const struct rtmsg *r, struct kern_rta *rta,
				const struct nlmsghdr *nlh, int *errp)
{
	int err;
	struct fib_info *fi = NULL;
	struct fib_info *ofi;
#ifdef CONFIG_IP_ROUTE_MULTIPATH
	int nhs = 1;
#else
	const int nhs = 1;
#endif
#ifdef CONFIG_IP_ROUTE_MULTIPATH_CACHED
	u32 mp_alg = IP_MP_ALG_NONE;
#endif
	if (fib_props[r->rtm_type].scope > r->rtm_scope)
		goto err_inval;
#ifdef CONFIG_IP_ROUTE_MULTIPATH
	if (rta->rta_mp) {
		nhs = myfib_count_nexthops(rta->rta_mp);
		if (nhs == 0)
			goto err_inval;
	}
#endif
#ifdef CONFIG_IP_ROUTE_MULTIPATH_CACHED
	if (rta->rta_mp_alg) {
		mp_alg = *rta->rta_mp_alg;

		if (mp_alg < IP_MP_ALG_NONE ||
						mp_alg > IP_MP_ALG_MAX)
			goto err_inval;
	}
#endif

	err = -ENOBUFS;
	if( myfib_info_cnt >= myfib_hash_size ){
		unsigned int new_size = myfib_hash_size << 1;
		struct hlist_head *new_info_hash;
		struct hlist_head *new_laddrhash;
		unsigned int bytes;

		if (!new_size)
			new_size = 1;
		bytes = new_size * sizeof(struct hlist_head *);
		new_info_hash = myfib_hash_alloc(bytes);
		new_laddrhash = myfib_hash_alloc(bytes);
		if( !new_info_hash || !new_laddrhash ){
			myfib_hash_free(new_info_hash, bytes);
			myfib_hash_free(new_laddrhash, bytes);
		}else{
			memset(new_info_hash, 0, bytes);
			memset(new_laddrhash, 0, bytes);

			myfib_hash_move(new_info_hash, new_laddrhash, new_size);
		}
		if (!myfib_hash_size)
			goto failure;
	}
	fi = kmalloc(sizeof(*fi)+nhs*sizeof(struct fib_nh), GFP_KERNEL);
	if (fi == NULL)
		goto failure;
	myfib_info_cnt++;
	memset(fi, 0, sizeof(*fi)+nhs*sizeof(struct fib_nh));

	fi->fib_protocol = r->rtm_protocol;
	fi->fib_nhs = nhs;

	mychange_nexthops(fi) {
		nh->nh_parent = fi;
	}myendfor_nexthops(fi)

	fi->fib_flags = r->rtm_flags;
	if( rta->rta_priority )
		fi->fib_priority = *rta->rta_priority;
	if (rta->rta_mx) {
		int attrlen = RTA_PAYLOAD(rta->rta_mx);
		struct rtattr *attr = RTA_DATA(rta->rta_mx);

		while (RTA_OK(attr, attrlen)) {
			unsigned flavor = attr->rta_type;
			if (flavor) {
				if (flavor > RTAX_MAX)
					goto err_inval;
				fi->fib_metrics[flavor-1] = *(unsigned*)RTA_DATA(attr);
			}
			attr = RTA_NEXT(attr, attrlen);
		}
	}
	if (rta->rta_prefsrc)
		memcpy(&fi->fib_prefsrc, rta->rta_prefsrc, 4);

	if( rta->rta_mp ){
#ifdef CONFIG_IP_ROUTE_MULTIPATH
		if ((err = myfib_get_nhs(fi, rta->rta_mp, r)) != 0)
			goto failure;
		if (rta->rta_oif && fi->fib_nh->nh_oif != *rta->rta_oif)
			goto err_inval;
		if (rta->rta_gw && memcmp(&fi->fib_nh->nh_gw, rta->rta_gw, 4))
			goto err_inval;
#ifdef CONFIG_NET_CLS_ROUTE
		if (rta->rta_flow && memcmp(&fi->fib_nh->nh_tclassid, rta->rta_flow, 4))
			goto err_inval;
#endif
#else
		goto err_inval;
#endif
	}else{
		struct fib_nh *nh = fi->fib_nh;
		if (rta->rta_oif)
			nh->nh_oif = *rta->rta_oif;
		if (rta->rta_gw)
			memcpy(&nh->nh_gw, rta->rta_gw, 4);
#ifdef CONFIG_NET_CLS_ROUTE
		if (rta->rta_flow)
			memcpy(&nh->nh_tclassid, rta->rta_flow, 4);
#endif
		nh->nh_flags = r->rtm_flags;
#ifdef CONFIG_IP_ROUTE_MULTIPATH
		nh->nh_weight = 1;
#endif
	}

#ifdef CONFIG_IP_ROUTE_MULTIPATH_CACHED
	fi->fib_mp_alg = mp_alg;
#endif

	if (fib_props[r->rtm_type].error) {
		if (rta->rta_gw || rta->rta_oif || rta->rta_mp)
			goto err_inval;
		goto link_it;
	}

	if (r->rtm_scope > RT_SCOPE_HOST)
		goto err_inval;

	if (r->rtm_scope == RT_SCOPE_HOST) {
		struct fib_nh *nh = fi->fib_nh;

		if (nhs != 1 || nh->nh_gw)
			goto err_inval;
		nh->nh_scope = RT_SCOPE_NOWHERE;
		nh->nh_dev = dev_get_by_index(fi->fib_nh->nh_oif);
		err = -ENODEV;
		if (nh->nh_dev == NULL)
			goto failure;
	} else {
		mychange_nexthops(fi) {
			if ((err = myfib_check_nh(r, fi, nh)) != 0)
				goto failure;
		} myendfor_nexthops(fi)
	}

	if( fi->fib_prefsrc ){
		if( r->rtm_type != RTN_LOCAL || rta->rta_dst == NULL ||
		    memcmp(&fi->fib_prefsrc, rta->rta_dst, 4) )
			if( myinet_addr_type(fi->fib_prefsrc) != RTN_LOCAL )
				goto err_inval;
	}

link_it:
	if ((ofi = myfib_find_info(fi)) != NULL) {
		fi->fib_dead = 1;
		myfree_fib_info(fi);
		ofi->fib_treeref++;
		return ofi;
	}
	fi->fib_treeref++;
	atomic_inc( &fi->fib_clntref );

	write_lock( &myfib_info_lock );
	hlist_add_head(&fi->fib_hash, &myfib_info_hash[myfib_info_hashfn(fi)]);
	if (fi->fib_prefsrc) {
		struct hlist_head *head;
		head = &myfib_info_laddrhash[myfib_laddr_hashfn(fi->fib_prefsrc)];
		hlist_add_head(&fi->fib_lhash, head);
	}

	mychange_nexthops(fi) {
		struct hlist_head *head;
		unsigned int hash;
		if (!nh->nh_dev)
			continue;
		hash = myfib_devindex_hashfn(nh->nh_dev->ifindex);
		head = &myfib_info_devhash[hash];
		hlist_add_head(&nh->nh_hash, head);
	} myendfor_nexthops(fi)
	write_unlock( &myfib_info_lock );

	return fi;

err_inval:
	err = -EINVAL;
failure:
	*errp = err;
	if (fi) {
		fi->fib_dead = 1;
		myfree_fib_info(fi);
	}
	return NULL;
}

#ifdef CONFIG_IP_ROUTE_MULTIPATH

void myfib_select_multipath(const struct flowi *flp, struct fib_result *res)
{
	struct fib_info *fi = res->fi;
	int w;

	spin_lock_bh( &myfib_multipath_lock );
	if (fi->fib_power <= 0) {
		int power = 0;
		mychange_nexthops(fi) {
			if( !(nh->nh_flags&RTNH_F_DEAD) ){
				power += nh->nh_weight;
				nh->nh_power = nh->nh_weight;
			}
		} myendfor_nexthops(fi);
		fi->fib_power = power;
		if (power <= 0) {
			spin_unlock_bh(&myfib_multipath_lock);
			res->nh_sel = 0;
			return;
		}
	}
	w = jiffies % fi->fib_power;

	mychange_nexthops(fi) {
		if (!(nh->nh_flags&RTNH_F_DEAD) && nh->nh_power) {
			if ((w -= nh->nh_power) <= 0) {
				nh->nh_power--;
				fi->fib_power--;
				res->nh_sel = nhsel;
				spin_unlock_bh( &myfib_multipath_lock );
				return;
			}
		}
	} myendfor_nexthops(fi);

	res->nh_sel = 0;
	spin_unlock_bh( &myfib_multipath_lock );
}

#endif

void __exit myfib_info_delete(void)
{
	struct hlist_head *head;
	struct hlist_node *node;
	struct hlist_node *tmp;
	struct fib_info *f;

	head = myfib_info_hash;
	hlist_for_each_entry_safe( f, node, tmp, head, fib_hash ){
		hlist_del( node );
	}

	f = NULL;
	head = myfib_info_laddrhash;
	hlist_for_each_entry_safe( f, node, tmp, head, fib_lhash ){
		hlist_del( node );
		kfree( f );
	}

}