fib_semantics.c

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

/* fib_semantics.c
 * [e-mail]
 * 2006-11-07
 */

#include "fib_semantics.h"
#include "log.h"
#include "fib_frontend.h"
#include "fib_lookup.h"
#include "devinet.h"

#include <linux/ip_mp_alg.h>
#include <linux/netdevice.h>
#include <linux/inetdevice.h>

static DEFINE_RWLOCK( myfib_info_lock );
static struct hlist_head *myfib_info_hash;
static struct hlist_head *myfib_info_laddrhash;
static unsigned int myfib_hash_size;
static unsigned int myfib_info_cnt;

#define DEVINDEX_HASHBITS 8
#define DEVINDEX_HASHSIZE (1U << DEVINDEX_HASHBITS)
static struct hlist_head myfib_info_devhash[DEVINDEX_HASHSIZE];

#ifdef CONFIG_IP_ROUTE_MULTIPATH

static DEFINE_SPINLOCK( myfib_multipath_lock );

#define for_nexthops(fi) { int nhsel; const struct fib_nh * nh; \
for (nhsel=0, nh = (fi)->fib_nh; nhsel < (fi)->fib_nhs; nh++, nhsel++)

#define change_nexthops(fi) { int nhsel; struct fib_nh * nh; \
for (nhsel=0, nh = (struct fib_nh*)((fi)->fib_nh); nhsel < (fi)->fib_nhs; nh++, nhsel++)

#else /* CONFIG_IP_ROUTE_MULTIPATH */

#define for_nexthops(fi) { int nhsel=0; const struct fib_nh * nh = (fi)->fib_nh; \
for (nhsel=0; nhsel < 1; nhsel++)

#define change_nexthops(fi) { int nhsel=0; struct fib_nh * nh = (struct fib_nh*)((fi)->fib_nh); \
for (nhsel=0; nhsel < 1; nhsel++)

#endif /* CONFIG_IP_ROUTE_MULTIPATH */

#define endfor_nexthops(fi) }

static const struct 
{
	int	error;
	u8	scope;
}myfib_props[RTA_MAX + 1] = {
	{
		.error	= 0,
		.scope	= RT_SCOPE_NOWHERE,
	},	/* RTN_UNSPEC */
	{
		.error	= 0,
		.scope	= RT_SCOPE_UNIVERSE,
	},	/* RTN_UNICAST */
	{
		.error	= 0,
		.scope	= RT_SCOPE_HOST,
	},	/* RTN_LOCAL */
	{
		.error	= 0,
		.scope	= RT_SCOPE_LINK,
	},	/* RTN_BROADCAST */
	{
		.error	= 0,
		.scope	= RT_SCOPE_LINK,
	},	/* RTN_ANYCAST */
	{
		.error	= 0,
		.scope	= RT_SCOPE_UNIVERSE,
	},	/* RTN_MULTICAST */
	{
		.error	= -EINVAL,
		.scope	= RT_SCOPE_UNIVERSE,
	},	/* RTN_BLACKHOLE */
	{
		.error	= -EHOSTUNREACH,
		.scope	= RT_SCOPE_UNIVERSE,
	},	/* RTN_UNREACHABLE */
	{
		.error	= -EACCES,
		.scope	= RT_SCOPE_UNIVERSE,
	},	/* RTN_PROHIBIT */
	{
		.error	= -EAGAIN,
		.scope	= RT_SCOPE_UNIVERSE,
	},	/* RTN_THROW */
	{
		.error	= -EINVAL,
		.scope	= RT_SCOPE_NOWHERE,
	},	/* RTN_NAT */
	{
		.error	= -EINVAL,
		.scope	= RT_SCOPE_NOWHERE,
	},	/* RTN_XRESOLVE */
};

u32 __myfib_res_prefsrc(struct fib_result *res)
{
	return myinet_select_addr( FIB_RES_DEV(*res), FIB_RES_GW(*res), res->scope );
}

void myfree_fib_info(struct fib_info *fi)
{
	if( fi->fib_dead == 0 ){
		PR_NOTICE( "Freeing alive fib_info %p\n", fi );
		return;
	}
	change_nexthops( fi ){
		if( nh->nh_dev )
			dev_put(nh->nh_dev);
		nh->nh_dev = NULL;
	}endfor_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;
}

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

	for_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++;
	}endfor_nexthops(fi);
	return 0;
}

static inline unsigned int myfib_info_hashfn(const struct fib_info *fi)
{
	unsigned int mask = ( myfib_hash_size - 1 );
	unsigned int val = fi->fib_nhs;
	val ^= fi->fib_protocol;
	val ^= fi->fib_prefsrc;
	val ^= fi->fib_priority;
	return (val ^ (val >> 7) ^ (val >> 12)) & mask;
}

static inline unsigned int myfib_laddr_hashfn(u32 val)
{
	unsigned int mask = ( myfib_hash_size - 1 );
	return (val ^ (val >> 7) ^ (val >> 14)) & mask;
}

static struct hlist_head *myfib_hash_alloc(int bytes)
{
	if( bytes <= PAGE_SIZE )
		return kmalloc( bytes, GFP_KERNEL );
	else
		return (struct hlist_head *)__get_free_pages(GFP_KERNEL, get_order(bytes));
}

static void myfib_hash_free(struct hlist_head *hash, int bytes)
{
	if (!hash) return;
	if (bytes <= PAGE_SIZE)
		kfree( hash );
	else
		free_pages( (unsigned long) hash, get_order(bytes) );
}

static void myfib_hash_move(struct hlist_head *new_info_hash,
				struct hlist_head *new_laddrhash, unsigned int new_size)
{
	struct hlist_head *old_info_hash, *old_laddrhash;
	unsigned int old_size = myfib_hash_size;
	unsigned int i, bytes;

	write_lock( &myfib_info_lock );
	old_info_hash = myfib_info_hash;
	old_laddrhash = myfib_info_laddrhash;
	myfib_hash_size = new_size;

	for( i = 0; i < old_size; i++ ){
		struct hlist_head *head = &myfib_info_hash[i];
		struct hlist_node *node, *n;
		struct fib_info *fi;

		hlist_for_each_entry_safe(fi, node, n, head, fib_hash) {
			struct hlist_head *dest;
			unsigned int new_hash;
			hlist_del(&fi->fib_hash);
			new_hash = myfib_info_hashfn(fi);
			dest = &new_info_hash[new_hash];
			hlist_add_head(&fi->fib_hash, dest);
		}
	}
	myfib_info_hash = new_info_hash;

	for (i = 0; i < old_size; i++) {
		struct hlist_head *lhead = &myfib_info_laddrhash[i];
		struct hlist_node *node, *n;
		struct fib_info *fi;

		hlist_for_each_entry_safe(fi, node, n, lhead, fib_lhash) {
			struct hlist_head *ldest;
			unsigned int new_hash;
			hlist_del(&fi->fib_lhash);
			new_hash = myfib_laddr_hashfn(fi->fib_prefsrc);
			ldest = &new_laddrhash[new_hash];
			hlist_add_head(&fi->fib_lhash, ldest);
		}
	}
	myfib_info_laddrhash = new_laddrhash;

	write_unlock( &myfib_info_lock );

	bytes = old_size * sizeof(struct hlist_head *);
	myfib_hash_free( old_info_hash, bytes );
	myfib_hash_free( old_laddrhash, bytes );
}

static u32 myfib_get_attr32(struct rtattr *attr, int attrlen, int type)
{
	while (RTA_OK(attr,attrlen)) {
		if (attr->rta_type == type)
			return *(u32*)RTA_DATA(attr);
		attr = RTA_NEXT(attr, attrlen);
	}
	return 0;
}

#ifdef CONFIG_IP_ROUTE_MULTIPATH

static int myfib_count_nexthops( struct rtattr *rta )
{
	int nhs = 0;
	struct rtnexthop *nhp = RTA_DATA(rta);
	int nhlen = RTA_PAYLOAD(rta);

	while( nhlen >= (int)sizeof(struct rtnexthop) ){
		if( (nhlen -= nhp->rtnh_len) < 0 )
			return 0;
		nhs++;
		nhp = RTNH_NEXT(nhp);
	};
	return nhs;
}

static int myfib_get_nhs(struct fib_info *fi, const struct rtattr *rta, const struct rtmsg *r)
{
	struct rtnexthop *nhp = RTA_DATA(rta);
	int nhlen = RTA_PAYLOAD(rta);

	change_nexthops(fi) {
		int attrlen = nhlen - sizeof(struct rtnexthop);
		if( attrlen < 0 || (nhlen -= nhp->rtnh_len) < 0 )
			return -EINVAL;
		nh->nh_flags = (r->rtm_flags & ~0xFF) | nhp->rtnh_flags;
		nh->nh_oif = nhp->rtnh_ifindex;
		nh->nh_weight = nhp->rtnh_hops + 1;
		if (attrlen) {
			nh->nh_gw = myfib_get_attr32(RTNH_DATA(nhp), attrlen, RTA_GATEWAY);
#ifdef CONFIG_NET_CLS_ROUTE
			nh->nh_tclassid = myfib_get_attr32(RTNH_DATA(nhp), attrlen, RTA_FLOW);
#endif
		}
		nhp = RTNH_NEXT(nhp);
	} endfor_nexthops(fi);
	return 0;
}

#endif

static int myfib_check_nh(const struct rtmsg *r, struct fib_info *fi, struct fib_nh *nh)
{
	int err;
	if (nh->nh_gw) {
		struct fib_result res;
#ifdef CONFIG_IP_ROUTE_PERVASIVE
		if( nh->nh_flags & RTNH_F_PERVASIVE )
			return 0;
#endif
		if( nh->nh_flags & RTNH_F_ONLINK ){
			struct net_device *dev;
			if( r->rtm_scope >= RT_SCOPE_LINK )
				return -EINVAL;
			if( myinet_addr_type(nh->nh_gw) != RTN_UNICAST )
				return -EINVAL;
			if( (dev = __dev_get_by_index(nh->nh_oif)) == NULL )
				return -ENODEV;
			PR_DEBUG( "nh->nh_dev: ref: %d\n", atomic_read(&(dev->refcnt)));
			if( !(dev->flags & IFF_UP) )
				return -ENETDOWN;
			nh->nh_dev = dev;
			dev_hold(dev);
			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 };

			/* It is not necessary, but requires a bit of thinking */
			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:
		myfib_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 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;
}

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( myfib_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;
	change_nexthops(fi){
		nh->nh_parent = fi;
	}endfor_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 )