tc_qdisc.c

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

/*
 * tc_qdisc.c	Interface to Linux traffic control: qdisc & class manipulation/
 *
 *		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 "tc_linux.h"
#include "libnetlink.h"

struct rtnl_handle tc_nl;

extern struct rsvp_qdisc_ops cbq_qdisc_ops;

struct rsvp_qdisc_ops * QoS_aware_qdiscs[] = {
	&cbq_qdisc_ops,
#if 0
	&hfsc_qdisc_ops,
	&hpfq_qdisc_ops,
#endif
	NULL
};

static struct rsvp_qdisc_ops *
rsvp_find_qdisc(struct rtattr *kind)
{
	int i;
	
	for (i=0 ; QoS_aware_qdiscs[i]; i++) {
		if (strcmp(QoS_aware_qdiscs[i]->kind, RTA_DATA(kind)) == 0)
			return QoS_aware_qdiscs[i];
	}
	return NULL;
}

static int
store_qdiscinfo(struct sockaddr_nl *who, struct nlmsghdr *n, void *arg)
{
	int i;
	struct tcmsg *t = NLMSG_DATA(n);
	struct rtattr *rta[TCA_MAX+1];
	rsvp_qdisc_t *qi;
	struct rsvp_qdisc_ops *qops;

	if (n->nlmsg_type != RTM_NEWQDISC)
		return 0;
	if (n->nlmsg_len < NLMSG_LENGTH(sizeof(t)))
		return -1;
	if (parse_rtattr(rta, TCA_MAX, TCA_RTA(t), TCA_PAYLOAD(n)) < 0)
		return -1;

	for (i = 0; i<if_num; i++) {
		if (if_vec[i].if_index == t->tcm_ifindex)
			break;
	}
	if (i >= if_num)
		return 0;

	if (rta[TCA_KIND] == NULL)
		return -1;

	qops = rsvp_find_qdisc(rta[TCA_KIND]);
	if (qops == NULL)
		return 0;

	qi = ifl_vec[i].ifl_qdisc;

	if (t->tcm_parent != TC_H_ROOT && qi)
		return 0;

	if (qi==NULL) {
		if ((qi = malloc(sizeof(rsvp_qdisc_t))) == NULL)
			return -1;
		memset(qi, 0, sizeof(*qi));
	} else if (qi->qops->free)
		qi->qops->free(qi);

	ifl_vec[i].ifl_qdisc = qi;
	qi->link = &ifl_vec[i];
	qi->handle = t->tcm_handle;
	qi->parent = t->tcm_parent;
	qi->qops = qops;
	qi->data = NULL;

	if (qops->alloc(qi, t, rta)) {
		ifl_vec[i].ifl_qdisc = NULL;
		free(qi);
	}
	return 0;
}


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

	memset(n, 0, sizeof(*n));
	n->nlmsg_type = RTM_DELTCLASS;
	n->nlmsg_flags = NLM_F_REQUEST;
	n->nlmsg_len = NLMSG_LENGTH(sizeof(*t));
	t = NLMSG_DATA(n);

	memset(t, 0, sizeof(*t));
	t->tcm_handle = classid;
	t->tcm_ifindex = ifindex;

	return rtnl_tell(&tc_nl, n);
}

int tc_add_class(int ifindex, Chandle parent, Chandle *classid, struct rtattr *opt)
{
	int err;
	struct rtnl_dialog d;
	char buf[4096];
	struct nlmsghdr n, *h;
	struct tcmsg t;
	struct iovec iov[3];
	int ct = 0;

	rtnl_iov_set(&n, sizeof(n));
	memset(&n, 0, sizeof(n));
	n.nlmsg_type = RTM_NEWTCLASS;
	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));
	memset(&t, 0, sizeof(t));
	t.tcm_parent = parent;
	t.tcm_handle = *classid;
	t.tcm_ifindex = ifindex;

	if (opt)
		rtnl_iov_set(opt, opt->rta_len);

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

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

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

		*classid = tcm->tcm_handle;
	}
	if (err) {
		log(LOG_ERR, errno, "add_class");
	}
	return err;
}

int tc_chg_class(int ifindex, Chandle classid,
		 struct rtattr *opt, struct tc_estimator *est)
{
	char buf[4096];
	struct nlmsghdr *n = (void*)buf;
	struct tcmsg *t;
	struct iovec iov[2];
	int ct = 0;

	memset(n, 0, sizeof(*n));
	n->nlmsg_type = RTM_NEWTCLASS;
	n->nlmsg_len = NLMSG_LENGTH(sizeof(*t));
	t = NLMSG_DATA(n);

	memset(t, 0, sizeof(*t));
	t->tcm_handle = classid;
	t->tcm_ifindex = ifindex;
	if (est)
		addattr_l(n, sizeof(buf), TCA_RATE, est, sizeof(*est));
	rtnl_iov_set(n, n->nlmsg_len);

	if (opt)
		rtnl_iov_set(opt, opt->rta_len);
	else {
		struct rtattr opt_dummy;
		opt_dummy.rta_type = TCA_OPTIONS;
		opt_dummy.rta_len = RTA_LENGTH(0);
		rtnl_iov_set(&opt_dummy, opt_dummy.rta_len);
	} 

	return rtnl_tell_iov(&tc_nl, iov, ct);
}

int tc_get_stats(int ifindex, Chandle classid, struct tc_stats *st)
{
	int err;
	struct rtnl_dialog d;
	char buf[8192];
	struct nlmsghdr n, *h;
	struct tcmsg t;
	struct iovec iov[2];
	int ct = 0;

	rtnl_iov_set(&n, sizeof(n));
	memset(&n, 0, sizeof(n));
	n.nlmsg_type = RTM_GETTCLASS;
	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 = 0;
	t.tcm_handle = classid;
	t.tcm_ifindex = ifindex;

	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);
		struct rtattr *rta[TCA_MAX + 1];

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

		parse_rtattr(rta, TCA_MAX, TCA_RTA(tcm), TCA_PAYLOAD(h));
		if (rta[TCA_STATS])
			memcpy(st, RTA_DATA(rta[TCA_STATS]), sizeof(*st));
	}
	if (err) {
		log(LOG_ERR, errno, "get_stats");
	}
	return err;
}


int tc_collect_qdisc(void)
{
	if (rtnl_open(&tc_nl, 0) < 0) {
		log(LOG_ERR, 0, "cannot open rtnetlink\n");
		return -1;
	}

	/* Read QDISC list from kernel */

	if (rtnl_wilddump_request(&tc_nl, AF_UNSPEC, RTM_GETQDISC) < 0) {
		log(LOG_ERR, 0, "cannot send dump request");
		goto no_tc;
	}

	if (rtnl_dump_filter(&tc_nl, store_qdiscinfo, NULL) < 0) {
		log(LOG_ERR, 0, "dump terminated\n");
		goto no_tc;
	}
	return 0;

no_tc:
	rtnl_close(&tc_nl);
	return -1;
}



int tc_add_shaper(rsvp_qdisc_t *qi, struct tc_police *p, Chandle classid)
{
	int err;
	char buf[4096];
	struct nlmsghdr *n = (void*)buf;
	struct tcmsg *t;
	struct tc_tbf_qopt opt;
	__u32 rtab[256];
	__u32 ptab[256];
	struct rtattr *tail;

	memset(n, 0, sizeof(*n));
	n->nlmsg_type = RTM_NEWQDISC;
	n->nlmsg_len = NLMSG_LENGTH(sizeof(*t));
	n->nlmsg_flags = NLM_F_CREATE;
	t = NLMSG_DATA(n);

	memset(t, 0, sizeof(*t));
	t->tcm_parent = classid;
	t->tcm_ifindex = qi->link->ifl_index;

	addattr_l(n, sizeof(buf), TCA_KIND, "tbf", 4);
	opt.rate = p->rate;
	opt.peakrate = p->peakrate;
	opt.limit = p->limit;
	opt.buffer = tc_calc_xmittime(opt.rate.rate, p->burst);
	opt.mtu = tc_calc_xmittime(opt.peakrate.rate, p->mtu);
	opt.rate.cell_log = tc_calc_rtable(opt.rate.rate, rtab, -1, p->mtu, p->rate.mpu);
	if (opt.peakrate.rate)
		opt.peakrate.cell_log = tc_calc_rtable(opt.peakrate.rate, ptab, -1, p->mtu, p->peakrate.mpu);

	tail = (struct rtattr*)(((void*)n)+NLMSG_ALIGN(n->nlmsg_len));
	addattr_l(n, sizeof(buf), TCA_OPTIONS, NULL, 0);
	addattr_l(n, sizeof(buf), TCA_TBF_PARMS, &opt, sizeof(opt));
	addattr_l(n, sizeof(buf), TCA_TBF_RTAB, rtab, 1024);
	if (opt.peakrate.rate)
		addattr_l(n, sizeof(buf), TCA_TBF_PTAB, ptab, 1024);
	tail->rta_len = (((void*)n)+NLMSG_ALIGN(n->nlmsg_len)) - (void*)tail;

	err = rtnl_tell(&tc_nl, n);
	if (err) {
		int s_errno = errno;
		log(LOG_ERR, errno, "failed to set TBF on %x@%s\n", classid,
		    qi->link->ifl_name);
		errno = s_errno;
	}
	return err;
}

int tc_del_shaper(rsvp_qdisc_t *qi, struct tc_police *p, Chandle classid)
{
	int err;
	char buf[4096];
	struct nlmsghdr *n = (void*)buf;
	struct tcmsg *t;
	struct tc_fifo_qopt opt;

	memset(n, 0, sizeof(*n));
	n->nlmsg_type = RTM_NEWQDISC;
	n->nlmsg_len = NLMSG_LENGTH(sizeof(*t));
	n->nlmsg_flags = NLM_F_CREATE;
	t = NLMSG_DATA(n);

	memset(t, 0, sizeof(*t));
	t->tcm_parent = classid;
	t->tcm_ifindex = qi->link->ifl_index;

	addattr_l(n, sizeof(buf), TCA_KIND, "bfifo", 4);
	opt.limit = p->limit;
	if (opt.limit < p->mtu)
		opt.limit = p->mtu;

	addattr_l(n, sizeof(buf), TCA_OPTIONS, &opt, sizeof(opt));

	err = rtnl_tell(&tc_nl, n);
	if (err) {
		int s_errno = errno;
		log(LOG_ERR, errno, "failed to set BFIFO on %x@%s\n", classid,
		    qi->link->ifl_name);
		errno = s_errno;
	}
	return err;
}