tc_filter.c

radius协议源码÷The Radius Stack will connect to a Radius Server. This stack implementation is built upo

/*
 * tc_filter.c	Interface to Linux rsvp/rsvp6 packet classifier.
 *
 *		This program is free software; you can redistribute it and/or
 *		modify it under the terms of the GNU General Public License
 *		as published by the Free Software Foundation; either version
 *		2 of the License, or (at your option) any later version.
 *
 * Authors:	Alexey Kuznetsov, <[e-mail]>
 */

#include <stddef.h>
#include <sys/types.h>
#include <linux/types.h>

#include "rsvp_daemon.h"
#include "rapi_lib.h"		/* Define flowspec formats */
#include "rsvp_specs.h"		/* Flowspec descriptor format */
#include "rsvp_TCif.h"		/* Adaptation interface */

#include "libnetlink.h"
#include "tc_linux.h"
#include <linux/if_ether.h>

#ifndef IPPROTO_ESP
#define IPPROTO_ESP	50
#endif
#ifndef IPPROTO_AH
#define IPPROTO_AH	51
#endif

static void tc_del_filter_proto(int ifindex, Chandle classid, __u32 prio)
{
	char buf[256];
	struct nlmsghdr *n = (void*)buf;
	struct tcmsg *t;

	memset(n, 0, sizeof(*n));
	n->nlmsg_type = RTM_DELTFILTER;
	n->nlmsg_flags = NLM_F_REQUEST;
	n->nlmsg_len = NLMSG_LENGTH(sizeof(*t));
	t = NLMSG_DATA(n);
	memset(t, 0, sizeof(*t));
	t->tcm_parent = classid;
	t->tcm_ifindex = ifindex;
	t->tcm_info = prio;

	if (rtnl_tell(&tc_nl, n))
		log(LOG_ERR, errno, "deleting fhead %x %x@if%d\n", prio, classid, ifindex);
}

static int tc_add_filter_proto(int ifindex, Chandle classid,
			       rsvp_filter_head_t *fh)
{
	char buf[4096];
	struct nlmsghdr *n = (void*)buf;
	struct tcmsg *t;
	int err;

	memset(n, 0, sizeof(*n));
	n->nlmsg_type = RTM_NEWTFILTER;
	n->nlmsg_flags = NLM_F_REQUEST|NLM_F_CREATE|NLM_F_EXCL;
	n->nlmsg_len = NLMSG_LENGTH(sizeof(*t));
	t = NLMSG_DATA(n);
	memset(t, 0, sizeof(*t));
	t->tcm_parent = classid;
	t->tcm_ifindex = ifindex;
	t->tcm_info = TC_H_MAKE(fh->prio, fh->protocol);
	addattr_l(n, sizeof(buf), TCA_KIND, fh->kind, strlen(fh->kind)+1);

	err = rtnl_tell(&tc_nl, n);
	if (err) {
		int s_errno = errno;
		log(LOG_ERR, errno, "adding fhead %s/%x/%x, err=%d\n", fh->kind, t->tcm_info,
		    t->tcm_parent, err);
		errno = s_errno;
	}
	return err;
}

static rsvp_filter_head_t *
tc_find_fhead(rsvp_qdisc_t *qi, __u32 protocol)
{
	rsvp_filter_head_t *fh;

	for (fh = qi->filter_heads; fh; fh = fh->next)
		if (fh->protocol == protocol)
			return fh;
	return NULL;
}

void tc_kill_filters(rsvp_qdisc_t *qi, Chandle classid)
{
	tc_del_filter_proto(qi->link->ifl_index, classid, RSVP_DEF_FPRIO4);
	tc_del_filter_proto(qi->link->ifl_index, classid, RSVP_DEF_FPRIO6);
}

int tc_init_filters(rsvp_qdisc_t *qi, Chandle classid)
{
	rsvp_filter_head_t *fh;

	fh = malloc(sizeof(*fh));
	if (fh == NULL)
		return -1;
	memset(fh, 0, sizeof(*fh));
	fh->family = AF_INET;
	fh->protocol = htons(ETH_P_IP);
	fh->prio = RSVP_DEF_FPRIO4;
	strcpy(fh->kind, "rsvp");
	if (tc_add_filter_proto(qi->link->ifl_index, classid, fh)) {
		log(LOG_ERR, errno, "failed to add rsvp filter head\n");
		free(fh);
	} else {
		fh->next = qi->filter_heads;
		qi->filter_heads = fh;
	}

	fh = malloc(sizeof(*fh));
	if (fh == NULL)
		return 0;
	memset(fh, 0, sizeof(*fh));
	fh->family = AF_INET6;
	fh->protocol = htons(ETH_P_IPV6);
	fh->prio = RSVP_DEF_FPRIO6;
	strcpy(fh->kind, "rsvp6");
	if (tc_add_filter_proto(qi->link->ifl_index, classid, fh) < 0) {
		log(LOG_ERR, errno, "failed to add rsvp6 filter head\n");
		free(fh);
	} else {
		fh->next = qi->filter_heads;
		qi->filter_heads = fh;
	}

	return 0;
}

void tc_del_filter(rsvp_filter_t *f)
{
	char buf[1024];
	struct nlmsghdr *n = (void*)buf;
	struct tcmsg *t;

	memset(n, 0, sizeof(*n));
	n->nlmsg_type = RTM_DELTFILTER;
	n->nlmsg_flags = NLM_F_REQUEST;
	n->nlmsg_len = NLMSG_LENGTH(sizeof(*t));
	t = NLMSG_DATA(n);
	memset(t, 0, sizeof(*t));
	log(LOG_ERR, 0, "delfilter %08x\n", f->handle);
	t->tcm_parent = f->flow->qi->handle;
	t->tcm_handle = f->handle;
	t->tcm_ifindex = f->flow->qi->link->ifl_index;
	t->tcm_info = f->fhead->prio;

	if (f->handle)
		rtnl_tell(&tc_nl, n);
}

int tc_add_filter(rsvp_filter_t *f, FilterSpec *fs, Session *s)
{
	int err;
	struct rtnl_dialog d;
	char buf[4096];
	struct nlmsghdr n, *h;
	struct tcmsg t;
	struct iovec iov[3];
	int ct = 0;
	struct tc_rsvp_pinfo pinfo;
	__u32  dst[4];
	__u32  src[4];
	int dlen = 0, slen = 0;
	struct rtattr *rta = (void*)buf;

	memset(&t, 0, sizeof(t));
	memset(&pinfo, 0, sizeof(pinfo));

	switch (Obj_CType(s->d_session)) {
	case ctype_SESSION_ipv4:
		/* Encode destination port to DPI */
		pinfo.dpi.key = hton32((__u32)ntohs(s->d_session->sess4_port));
		if (pinfo.dpi.key)
			pinfo.dpi.mask = hton32(0xFFFF);
		/* Fall through... */
	case ctype_SESSION_ipv4GPI:
		/* Encode destination IPv4 address */
		dst[0] = s->d_session->sess4_addr.s_addr;
		dlen = 4;
		pinfo.protocol = s->d_session->sess4_prot;
		if ((f->fhead = tc_find_fhead(f->flow->qi, htons(ETH_P_IP))) == NULL) {
			log(LOG_ERR, 0, "bad filter protocol\n");
			rsvp_errno = Set_Errno(RSVP_Err_UNKNOWN_CTYPE,
					       Obj_CType(&s->d_session));
			return -1;
		}
		break;
#ifdef USE_IPV6
	case ctype_SESSION_ipv6:
		/* Encode destination port to DPI */
		pinfo.dpi.key = hton32((__u32)ntohs(s->d_session->sess6_port));
		if (pinfo.dpi.key)
			pinfo.dpi.mask = hton32(0xFFFF);
		/* Fall through... */
	case ctype_SESSION_ipv6GPI:
		/* Encode destination IPv6 address */
		memcpy(dst, &s->d_session->sess6_addr, 16);
		dlen = 16;
		pinfo.protocol = s->d_session->sess6_prot;
		if ((f->fhead = tc_find_fhead(f->flow->qi, htons(ETH_P_IPV6))) == NULL) {
			rsvp_errno = Set_Errno(RSVP_Err_UNKNOWN_CTYPE,
					       Obj_CType(s->d_session));
			return -1;
		}
#endif
		break;
	default:	
		log(LOG_ERR, 0, "bad filter session type\n");
		rsvp_errno = Set_Errno(RSVP_Err_UNKNOWN_CTYPE,
				       Obj_CType(s->d_session));
		return -1;
	}

	/* If flowspec is NULL, it is wildcard.
	   Otherwise, fill source part of filter.
	 */
	if (fs != NULL) {
		switch (Obj_CType(fs)) {
		case ctype_FILTER_SPEC_ipv4GPI:
			/* Encode GPI to SPI */
			pinfo.spi.key = fs->filtgpi4_srcgpi;
			pinfo.spi.mask = hton32(0xFFFFFFFF);
			/* IP-AH needs special treatment */
			if (s->d_session->sess4_prot == IPPROTO_AH)
				pinfo.spi.offset = 4;
			goto set_src4;
		case ctype_FILTER_SPEC_ipv4:
			/* Encode source port to SPI */
			if (fs->filt4_srcaddr.s_addr) {
				pinfo.spi.key = hton32(((__u32)ntohs(fs->filt4_srcport))<<16);
				if (pinfo.spi.key)
					pinfo.spi.mask = hton32(0xFFFF0000);
			}

set_src4:
			if (fs->filt4_srcaddr.s_addr) {
				slen = 4;
				src[0] = fs->filt4_srcaddr.s_addr;
			}
			break;
#ifdef USE_IPV6
/* No flow labels in rel4.2a4 
		case ctype_FILTER_SPEC_ipv6FL:
			pinfo.spi.key = fs->filtfl6_srcflowlabel;
			pinfo.spi.mask = hton32(0x000FFFFF);
			pinfo.spi.offset = -40;
			goto set_src6;
*/
		case ctype_FILTER_SPEC_ipv6GPI:
			pinfo.spi.key = fs->filtgpi6_srcgpi;
			pinfo.spi.mask = hton32(0xFFFFFFFF);
			/* IP-AH needs special treatment */
			if (s->d_session->sess6_prot == IPPROTO_AH)
				pinfo.spi.offset = 4;
			goto set_src6;
		case ctype_FILTER_SPEC_ipv6:
			pinfo.spi.key = hton32(((__u32)ntohs(fs->filt6_srcport))<<16);
			if (pinfo.spi.key)
				pinfo.spi.mask = hton32(0xFFFF0000);

set_src6:
			slen = 16;
			memcpy(src, &fs->filt6_srcaddr, 16);
#endif
			break;
		default:
			log(LOG_ERR, 0, "bad filter spec type\n");
			rsvp_errno = Set_Errno(RSVP_Err_UNKNOWN_CTYPE, Obj_CType(fs));
			return -1;
		}
	}

	rtnl_iov_set(&n, sizeof(n));
	memset(&n, 0, sizeof(n));
	n.nlmsg_type = RTM_NEWTFILTER;
	n.nlmsg_flags = NLM_F_ECHO|NLM_F_EXCL|NLM_F_CREATE;
	n.nlmsg_len = NLMSG_LENGTH(sizeof(t));

	rtnl_iov_set(&t, sizeof(t));
	t.tcm_parent = f->flow->qi->handle;
	t.tcm_handle = 0;
	f->handle = 0;
	t.tcm_ifindex = f->flow->qi->link->ifl_index;
	t.tcm_info = TC_H_MAKE(f->fhead->prio, f->fhead->protocol);

	addattr_l(&n, sizeof(buf), TCA_KIND, f->fhead->kind, IFNAMSIZ);

	rta->rta_type = TCA_OPTIONS;
	rta->rta_len = RTA_LENGTH(0);

	rta_addattr_l(rta, sizeof(buf), TCA_RSVP_DST, dst, dlen);
	if (slen)
		rta_addattr_l(rta, sizeof(buf), TCA_RSVP_SRC, src, slen);
	rta_addattr_l(rta, sizeof(buf), TCA_RSVP_PINFO, &pinfo, sizeof(pinfo));
	rta_addattr_l(rta, sizeof(buf), TCA_RSVP_CLASSID, &f->flow->classid, 4);
	if (f->flow->policer.action == TC_POLICE_RECLASSIFY)
		tca_add_policing(rta, sizeof(buf), TCA_RSVP_POLICE, f->flow->qi, &f->flow->policer);
	rtnl_iov_set(rta, rta->rta_len);

	err = rtnl_ask(&tc_nl, iov, ct, &d, buf, sizeof(buf));
	if (err)
		return err;

	while ((h = rtnl_wait(&tc_nl, &d, &err)) != NULL) {
		struct tcmsg *tcm = NLMSG_DATA(h);

		if (h->nlmsg_type != RTM_NEWTFILTER)
			continue;
		if (!f->handle || f->handle == tcm->tcm_handle) {
			f->handle = tcm->tcm_handle;
			if (f->flow->policer.action == TC_POLICE_RECLASSIFY &&
			    f->flow->policer.index == 0)
				f->flow->policer.index = tca_policing_index(h);
		}
	}
	if (err) {
		log(LOG_ERR, errno, "add_filter");
	}
	return err;
}


int tc_mod_filter(rsvp_filter_t *f)
{
	int err;
	struct rtnl_dialog d;
	char buf[8192];
	struct nlmsghdr n, *h;
	struct tcmsg t;
	struct iovec iov[3];
	int ct = 0;
	struct rtattr *rta = (void*)buf;

	if (f->handle == 0)
		return -1;

	rtnl_iov_set(&n, sizeof(n));
	memset(&n, 0, sizeof(n));
	n.nlmsg_type = RTM_NEWTFILTER;
	n.nlmsg_flags = NLM_F_ECHO;
	n.nlmsg_len = NLMSG_LENGTH(sizeof(t));

	rtnl_iov_set(&t, sizeof(t));
	memset(&t, 0, sizeof(t));
	t.tcm_parent = f->flow->qi->handle;
	t.tcm_handle = f->handle;
	t.tcm_ifindex = f->flow->qi->link->ifl_index;
	t.tcm_info = f->fhead->prio;

	rta->rta_type = TCA_OPTIONS;
	rta->rta_len = RTA_LENGTH(0);

	rta_addattr_l(rta, sizeof(buf), TCA_RSVP_CLASSID, &f->flow->classid, 4);
	if (f->flow->policer.action == TC_POLICE_RECLASSIFY)
		tca_add_policing(rta, sizeof(buf), TCA_RSVP_POLICE, f->flow->qi, &f->flow->policer);
	rtnl_iov_set(rta, rta->rta_len);

	err = rtnl_ask(&tc_nl, iov, ct, &d, buf, sizeof(buf));
	if (err)
		return err;

	while ((h = rtnl_wait(&tc_nl, &d, &err)) != NULL) {
		struct tcmsg *tcm = NLMSG_DATA(h);

		if (h->nlmsg_type != RTM_NEWTFILTER)
			continue;

		if (f->handle == tcm->tcm_handle) {
			if (f->flow->policer.action == TC_POLICE_RECLASSIFY)
				f->flow->policer.index = tca_policing_index(h);
		}
	}
	if (err) {
		log(LOG_ERR, errno, "mod_filter");
	}
	return err;
}

int tca_add_policing(struct rtattr *rta, int rlen, int type, 
		     rsvp_qdisc_t *qi, struct tc_police *pol)
{
	struct tc_police p = *pol;
	__u32 rtab[256];
	__u32 ptab[256];
	struct rtattr *tail;

	p.rate.cell_log = tc_calc_rtable(p.rate.rate, rtab, -1, p.mtu, p.rate.mpu);
	p.burst = tc_calc_xmittime(p.rate.rate, p.burst);
	if (p.peakrate.rate)
		p.peakrate.cell_log = tc_calc_rtable(p.peakrate.rate, ptab, -1, p.mtu, p.peakrate.mpu);
	p.action = TC_POLICE_RECLASSIFY;

	tail = (struct rtattr *)(((char*)rta) + RTA_ALIGN(rta->rta_len));
	rta_addattr_l(rta, rlen, type, NULL, 0);
	rlen -= RTA_ALIGN(rta->rta_len);
	rta_addattr_l(tail, rlen, TCA_POLICE_TBF, &p, sizeof(p));
	rta_addattr_l(tail, rlen, TCA_POLICE_RATE, rtab, 1024);
	if (p.peakrate.rate)
		rta_addattr_l(tail, rlen, TCA_POLICE_PEAKRATE, ptab, 1024);
	rta->rta_len += RTA_ALIGN(tail->rta_len);
	return 0;
}

__u32 tca_policing_index(struct nlmsghdr *n)
{
	struct rtattr *rta[TCA_MAX+1];
	struct rtattr *tb[TCA_RSVP_MAX+1];

	if (parse_rtattr(rta, TCA_MAX, TCA_RTA(NLMSG_DATA(n)), TCA_PAYLOAD(n))) {
		log(LOG_ERR, 0, "tca_policing_index: unparsable attributes\n");
	}
	if (rta[TCA_OPTIONS] == NULL) {
		log(LOG_ERR, 0, "Mmm, no options?\n");
		return 0;
	}
	parse_rtattr(tb, TCA_RSVP_MAX, RTA_DATA(rta[TCA_OPTIONS]),
		     RTA_PAYLOAD(rta[TCA_OPTIONS]));
	if (tb[TCA_RSVP_POLICE] == NULL) {
		log(LOG_ERR, 0, "Mmm, no policing info?\n");
		return 0;
	}
	return ((struct tc_police*)RTA_DATA(tb[TCA_RSVP_POLICE]))->index;
}


#if 0
/* not used */
int TC_verf_sender(SESSION *s, FILTER_SPEC *sender, unsigned tmo)
{
#ifndef USE_IPV6
	return 0;
#else
	if (!sender || Obj_CType(sender) != ctype_FILTER_SPEC_ipv6FL)
		return 0;

	return ipv6_verf_flowlabel(sender->filtfl6_srcflowlabel, &s->sess6_addr, tmo);
#endif
}
#endif