myfib_semantics.c

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

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;

#ifdef CONFIG_IP_ROUTE_MULTIPATH

static DEFINE_SPINLOCK( myfib_multipath_lock );

#endif

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

#ifdef CONFIG_IP_ROUTE_MULTIPATH

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

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

#else

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

#endif 

#define myendfor_nexthops(fi) }

unsigned myinet_addr_type(u32 addr);

static const struct 
{
	int	error;
	u8	scope;
} fib_props[RTA_MAX + 1] = {
	{
		.error	= 0,
		.scope	= RT_SCOPE_NOWHERE,
	},
	{
		.error	= 0,
		.scope	= RT_SCOPE_UNIVERSE,
	},
	{
		.error	= 0,
		.scope	= RT_SCOPE_HOST,
	},
	{
		.error	= 0,
		.scope	= RT_SCOPE_LINK,
	},
	{
		.error	= 0,
		.scope	= RT_SCOPE_LINK,
	},
	{
		.error	= 0,
		.scope	= RT_SCOPE_UNIVERSE,
	},
	{
		.error	= -EINVAL,
		.scope	= RT_SCOPE_UNIVERSE,
	},
	{
		.error	= -EHOSTUNREACH,
		.scope	= RT_SCOPE_UNIVERSE,
	},
	{
		.error	= -EACCES,
		.scope	= RT_SCOPE_UNIVERSE,
	},
	{
		.error	= -EAGAIN,
		.scope	= RT_SCOPE_UNIVERSE,
	},
	{
		.error	= -EINVAL,
		.scope	= RT_SCOPE_NOWHERE,
	},
	{
		.error	= -EINVAL,
		.scope	= RT_SCOPE_NOWHERE,
	},
};

extern u32 myinet_select_addr(const struct net_device *dev, u32 dst, int scope);

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

int myfib_dump_info(struct sk_buff *skb, u32 pid, u32 seq, int event,
				u8 tb_id, u8 type, u8 scope, void *dst, int dst_len, u8 tos,
				struct fib_info *fi, unsigned int flags)
{
	return 0;
}

void myrtmsg_fib(int event, u32 key, struct fib_alias *fa,
	       int z, int tb_id,
	       struct nlmsghdr *n, struct netlink_skb_parms *req)
{
	struct sk_buff *skb;
	u32 pid = req ? req->pid : n->nlmsg_pid;
	int size = NLMSG_SPACE(sizeof(struct rtmsg)+256);

	skb = alloc_skb(size, GFP_KERNEL);
	if (!skb)
		return;

	if (myfib_dump_info(skb, pid, n->nlmsg_seq, event, tb_id,
			  fa->fa_type, fa->fa_scope, &key, z,
			  fa->fa_tos,
			  fa->fa_info, 0) < 0) {
		kfree_skb(skb);
		return;
	}
	NETLINK_CB(skb).dst_group = RTNLGRP_IPV4_ROUTE;
	if (n->nlmsg_flags&NLM_F_ECHO)
		atomic_inc(&skb->users);
	netlink_broadcast(rtnl, skb, pid, RTNLGRP_IPV4_ROUTE, GFP_KERNEL);
	if (n->nlmsg_flags&NLM_F_ECHO)
		netlink_unicast(rtnl, skb, pid, MSG_DONTWAIT);
}

int myfib_semantic_match(struct list_head *head, const struct flowi *flp,
				struct fib_result *res, __u32 zone, __u32 mask, 
				int prefixlen)
{
	struct fib_alias *fa;
	int nh_sel = 0;

	list_for_each_entry_rcu(fa, head, fa_list) {
		int err;

		if (fa->fa_tos &&
		    fa->fa_tos != flp->fl4_tos)
			continue;

		if (fa->fa_scope < flp->fl4_scope)
			continue;

		fa->fa_state |= FA_S_ACCESSED;

		err = fib_props[fa->fa_type].error;
		if (err == 0) {
			struct fib_info *fi = fa->fa_info;

			if (fi->fib_flags & RTNH_F_DEAD)
				continue;

			switch (fa->fa_type) {
			case RTN_UNICAST:
			case RTN_LOCAL:
			case RTN_BROADCAST:
			case RTN_ANYCAST:
			case RTN_MULTICAST:
				myfor_nexthops(fi) {
					if (nh->nh_flags&RTNH_F_DEAD)
						continue;
					if (!flp->oif || flp->oif == nh->nh_oif)
						break;
				}
#ifdef CONFIG_IP_ROUTE_MULTIPATH
				if (nhsel < fi->fib_nhs) {
					nh_sel = nhsel;
					goto out_fill_res;
				}
#else
				if (nhsel < 1) {
					goto out_fill_res;
				}
#endif
				myendfor_nexthops(fi);
				continue;

			default:
				printk(KERN_DEBUG "impossible 102\n");
				return -EINVAL;
			};
		}
		return err;
	}
	return 1;

out_fill_res:
	res->prefixlen = prefixlen;
	res->nh_sel = nh_sel;
	res->type = fa->fa_type;
	res->scope = fa->fa_scope;
	res->fi = fa->fa_info;
#ifdef CONFIG_IP_ROUTE_MULTIPATH_CACHED
	res->netmask = mask;
	res->network = zone &
		(0xFFFFFFFF >> (32 - prefixlen));
#endif
	atomic_inc(&res->fi->fib_clntref);
	return 0;
}

void free_fib_info(struct fib_info *fi)
{
}

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 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 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 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;
}

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);

	mychange_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);
	} myendfor_nexthops(fi);
	return 0;
}

extern int myfib_lookup(const struct flowi *flp, struct fib_result *res);

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;
			if( !(dev->flags & IFF_UP) )
				return -ENETDOWN;
			nh->nh_dev = dev;
			dev_hold(dev);