sch_dsmark.c 10.8 KB
Newer Older
Linus Torvalds's avatar
Linus Torvalds committed
1 2 3 4 5 6 7 8 9 10 11 12
/* net/sched/sch_dsmark.c - Differentiated Services field marker */

/* Written 1998-2000 by Werner Almesberger, EPFL ICA */


#include <linux/module.h>
#include <linux/init.h>
#include <linux/types.h>
#include <linux/string.h>
#include <linux/errno.h>
#include <linux/skbuff.h>
#include <linux/rtnetlink.h>
13
#include <linux/bitops.h>
Linus Torvalds's avatar
Linus Torvalds committed
14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39
#include <net/pkt_sched.h>
#include <net/dsfield.h>
#include <net/inet_ecn.h>
#include <asm/byteorder.h>

/*
 * classid	class		marking
 * -------	-----		-------
 *   n/a	  0		n/a
 *   x:0	  1		use entry [0]
 *   ...	 ...		...
 *   x:y y>0	 y+1		use entry [y]
 *   ...	 ...		...
 * x:indices-1	indices		use entry [indices-1]
 *   ...	 ...		...
 *   x:y	 y+1		use entry [y & (indices-1)]
 *   ...	 ...		...
 * 0xffff	0x10000		use entry [indices-1]
 */


#define NO_DEFAULT_INDEX	(1 << 16)

struct dsmark_qdisc_data {
	struct Qdisc		*q;
	struct tcf_proto	*filter_list;
40 41 42 43
	u8			*mask;	/* "owns" the array */
	u8			*value;
	u16			indices;
	u32			default_index;	/* index range is 0...0xffff */
Linus Torvalds's avatar
Linus Torvalds committed
44 45 46
	int			set_tc_index;
};

47 48 49 50
static inline int dsmark_valid_index(struct dsmark_qdisc_data *p, u16 index)
{
	return (index <= p->indices && index > 0);
}
Linus Torvalds's avatar
Linus Torvalds committed
51 52 53

/* ------------------------- Class/flow operations ------------------------- */

54 55
static int dsmark_graft(struct Qdisc *sch, unsigned long arg,
			struct Qdisc *new, struct Qdisc **old)
Linus Torvalds's avatar
Linus Torvalds committed
56
{
57
	struct dsmark_qdisc_data *p = qdisc_priv(sch);
Linus Torvalds's avatar
Linus Torvalds committed
58

59
	pr_debug("dsmark_graft(sch %p,[qdisc %p],new %p,old %p)\n",
60
		sch, p, new, old);
61 62

	if (new == NULL) {
63 64
		new = qdisc_create_dflt(sch->dev, &pfifo_qdisc_ops,
					sch->handle);
65 66 67 68
		if (new == NULL)
			new = &noop_qdisc;
	}

Linus Torvalds's avatar
Linus Torvalds committed
69
	sch_tree_lock(sch);
70
	*old = xchg(&p->q, new);
71
	qdisc_tree_decrease_qlen(*old, (*old)->q.qlen);
72 73 74
	qdisc_reset(*old);
	sch_tree_unlock(sch);

75
	return 0;
Linus Torvalds's avatar
Linus Torvalds committed
76 77 78 79
}

static struct Qdisc *dsmark_leaf(struct Qdisc *sch, unsigned long arg)
{
80 81
	struct dsmark_qdisc_data *p = qdisc_priv(sch);
	return p->q;
Linus Torvalds's avatar
Linus Torvalds committed
82 83
}

84
static unsigned long dsmark_get(struct Qdisc *sch, u32 classid)
Linus Torvalds's avatar
Linus Torvalds committed
85
{
86 87
	pr_debug("dsmark_get(sch %p,[qdisc %p],classid %x)\n",
		sch, qdisc_priv(sch), classid);
Linus Torvalds's avatar
Linus Torvalds committed
88

89
	return TC_H_MIN(classid) + 1;
Linus Torvalds's avatar
Linus Torvalds committed
90 91 92
}

static unsigned long dsmark_bind_filter(struct Qdisc *sch,
93
					unsigned long parent, u32 classid)
Linus Torvalds's avatar
Linus Torvalds committed
94
{
95
	return dsmark_get(sch, classid);
Linus Torvalds's avatar
Linus Torvalds committed
96 97 98 99 100 101 102
}

static void dsmark_put(struct Qdisc *sch, unsigned long cl)
{
}

static int dsmark_change(struct Qdisc *sch, u32 classid, u32 parent,
103
			 struct rtattr **tca, unsigned long *arg)
Linus Torvalds's avatar
Linus Torvalds committed
104
{
105
	struct dsmark_qdisc_data *p = qdisc_priv(sch);
Linus Torvalds's avatar
Linus Torvalds committed
106 107
	struct rtattr *opt = tca[TCA_OPTIONS-1];
	struct rtattr *tb[TCA_DSMARK_MAX];
108 109
	int err = -EINVAL;
	u8 mask = 0;
Linus Torvalds's avatar
Linus Torvalds committed
110

111
	pr_debug("dsmark_change(sch %p,[qdisc %p],classid %x,parent %x),"
112 113 114 115 116
		"arg 0x%lx\n", sch, p, classid, parent, *arg);

	if (!dsmark_valid_index(p, *arg)) {
		err = -ENOENT;
		goto rtattr_failure;
Linus Torvalds's avatar
Linus Torvalds committed
117 118
	}

119 120 121 122 123 124 125 126
	if (!opt || rtattr_parse_nested(tb, TCA_DSMARK_MAX, opt))
		goto rtattr_failure;

	if (tb[TCA_DSMARK_MASK-1])
		mask = RTA_GET_U8(tb[TCA_DSMARK_MASK-1]);

	if (tb[TCA_DSMARK_VALUE-1])
		p->value[*arg-1] = RTA_GET_U8(tb[TCA_DSMARK_VALUE-1]);
127

128 129 130 131 132 133 134 135
	if (tb[TCA_DSMARK_MASK-1])
		p->mask[*arg-1] = mask;

	err = 0;

rtattr_failure:
	return err;
}
Linus Torvalds's avatar
Linus Torvalds committed
136

137
static int dsmark_delete(struct Qdisc *sch, unsigned long arg)
Linus Torvalds's avatar
Linus Torvalds committed
138
{
139
	struct dsmark_qdisc_data *p = qdisc_priv(sch);
Linus Torvalds's avatar
Linus Torvalds committed
140

141
	if (!dsmark_valid_index(p, arg))
Linus Torvalds's avatar
Linus Torvalds committed
142
		return -EINVAL;
143

Linus Torvalds's avatar
Linus Torvalds committed
144 145
	p->mask[arg-1] = 0xff;
	p->value[arg-1] = 0;
146

Linus Torvalds's avatar
Linus Torvalds committed
147 148 149
	return 0;
}

150
static void dsmark_walk(struct Qdisc *sch, struct qdisc_walker *walker)
Linus Torvalds's avatar
Linus Torvalds committed
151
{
152
	struct dsmark_qdisc_data *p = qdisc_priv(sch);
Linus Torvalds's avatar
Linus Torvalds committed
153 154
	int i;

155
	pr_debug("dsmark_walk(sch %p,[qdisc %p],walker %p)\n", sch, p, walker);
156

Linus Torvalds's avatar
Linus Torvalds committed
157 158
	if (walker->stop)
		return;
159

Linus Torvalds's avatar
Linus Torvalds committed
160 161
	for (i = 0; i < p->indices; i++) {
		if (p->mask[i] == 0xff && !p->value[i])
162
			goto ignore;
Linus Torvalds's avatar
Linus Torvalds committed
163 164 165 166 167 168
		if (walker->count >= walker->skip) {
			if (walker->fn(sch, i+1, walker) < 0) {
				walker->stop = 1;
				break;
			}
		}
169
ignore:
170
		walker->count++;
171
	}
Linus Torvalds's avatar
Linus Torvalds committed
172 173
}

174 175
static inline struct tcf_proto **dsmark_find_tcf(struct Qdisc *sch,
						 unsigned long cl)
Linus Torvalds's avatar
Linus Torvalds committed
176
{
177 178
	struct dsmark_qdisc_data *p = qdisc_priv(sch);
	return &p->filter_list;
Linus Torvalds's avatar
Linus Torvalds committed
179 180 181 182
}

/* --------------------------- Qdisc operations ---------------------------- */

183
static int dsmark_enqueue(struct sk_buff *skb, struct Qdisc *sch)
Linus Torvalds's avatar
Linus Torvalds committed
184
{
185
	struct dsmark_qdisc_data *p = qdisc_priv(sch);
186 187
	int err;

188
	pr_debug("dsmark_enqueue(skb %p,sch %p,[qdisc %p])\n", skb, sch, p);
Linus Torvalds's avatar
Linus Torvalds committed
189 190 191

	if (p->set_tc_index) {
		switch (skb->protocol) {
192 193 194
		case __constant_htons(ETH_P_IP):
			if (skb_cow_head(skb, sizeof(struct iphdr)))
				goto drop;
195

196 197 198
			skb->tc_index = ipv4_get_dsfield(ip_hdr(skb))
				& ~INET_ECN_MASK;
			break;
199

200 201 202
		case __constant_htons(ETH_P_IPV6):
			if (skb_cow_head(skb, sizeof(struct ipv6hdr)))
				goto drop;
203

204 205 206 207 208 209
			skb->tc_index = ipv6_get_dsfield(ipv6_hdr(skb))
				& ~INET_ECN_MASK;
			break;
		default:
			skb->tc_index = 0;
			break;
210
		}
Linus Torvalds's avatar
Linus Torvalds committed
211
	}
212 213

	if (TC_H_MAJ(skb->priority) == sch->handle)
Linus Torvalds's avatar
Linus Torvalds committed
214
		skb->tc_index = TC_H_MIN(skb->priority);
215 216 217 218
	else {
		struct tcf_result res;
		int result = tc_classify(skb, p->filter_list, &res);

219
		pr_debug("result %d class 0x%04x\n", result, res.classid);
220

Linus Torvalds's avatar
Linus Torvalds committed
221
		switch (result) {
222 223 224 225 226
#ifdef CONFIG_NET_CLS_ACT
		case TC_ACT_QUEUED:
		case TC_ACT_STOLEN:
			kfree_skb(skb);
			return NET_XMIT_SUCCESS;
227

228
		case TC_ACT_SHOT:
229
			goto drop;
Linus Torvalds's avatar
Linus Torvalds committed
230
#endif
231
		case TC_ACT_OK:
232 233
			skb->tc_index = TC_H_MIN(res.classid);
			break;
234

235 236 237 238
		default:
			if (p->default_index != NO_DEFAULT_INDEX)
				skb->tc_index = p->default_index;
			break;
239
		}
Linus Torvalds's avatar
Linus Torvalds committed
240 241
	}

242
	err = p->q->enqueue(skb, p->q);
243
	if (err != NET_XMIT_SUCCESS) {
Linus Torvalds's avatar
Linus Torvalds committed
244
		sch->qstats.drops++;
245
		return err;
Linus Torvalds's avatar
Linus Torvalds committed
246
	}
247

Linus Torvalds's avatar
Linus Torvalds committed
248 249 250 251
	sch->bstats.bytes += skb->len;
	sch->bstats.packets++;
	sch->q.qlen++;

252
	return NET_XMIT_SUCCESS;
253 254 255 256 257

drop:
	kfree_skb(skb);
	sch->qstats.drops++;
	return NET_XMIT_BYPASS;
258
}
Linus Torvalds's avatar
Linus Torvalds committed
259 260 261

static struct sk_buff *dsmark_dequeue(struct Qdisc *sch)
{
262
	struct dsmark_qdisc_data *p = qdisc_priv(sch);
Linus Torvalds's avatar
Linus Torvalds committed
263
	struct sk_buff *skb;
264 265
	u32 index;

266
	pr_debug("dsmark_dequeue(sch %p,[qdisc %p])\n", sch, p);
Linus Torvalds's avatar
Linus Torvalds committed
267 268

	skb = p->q->ops->dequeue(p->q);
269
	if (skb == NULL)
Linus Torvalds's avatar
Linus Torvalds committed
270
		return NULL;
271

Linus Torvalds's avatar
Linus Torvalds committed
272
	sch->q.qlen--;
273 274

	index = skb->tc_index & (p->indices - 1);
275
	pr_debug("index %d->%d\n", skb->tc_index, index);
276

Linus Torvalds's avatar
Linus Torvalds committed
277
	switch (skb->protocol) {
278 279 280
	case __constant_htons(ETH_P_IP):
		ipv4_change_dsfield(ip_hdr(skb), p->mask[index],
				    p->value[index]);
Linus Torvalds's avatar
Linus Torvalds committed
281
			break;
282 283 284
	case __constant_htons(ETH_P_IPV6):
		ipv6_change_dsfield(ipv6_hdr(skb), p->mask[index],
				    p->value[index]);
Linus Torvalds's avatar
Linus Torvalds committed
285
			break;
286 287 288 289 290 291 292 293 294 295 296
	default:
		/*
		 * Only complain if a change was actually attempted.
		 * This way, we can send non-IP traffic through dsmark
		 * and don't need yet another qdisc as a bypass.
		 */
		if (p->mask[index] != 0xff || p->value[index])
			printk(KERN_WARNING
			       "dsmark_dequeue: unsupported protocol %d\n",
			       ntohs(skb->protocol));
		break;
297
	}
298

Linus Torvalds's avatar
Linus Torvalds committed
299 300 301
	return skb;
}

302
static int dsmark_requeue(struct sk_buff *skb, struct Qdisc *sch)
Linus Torvalds's avatar
Linus Torvalds committed
303
{
304
	struct dsmark_qdisc_data *p = qdisc_priv(sch);
305
	int err;
Linus Torvalds's avatar
Linus Torvalds committed
306

307
	pr_debug("dsmark_requeue(skb %p,sch %p,[qdisc %p])\n", skb, sch, p);
308 309 310 311 312

	err = p->q->ops->requeue(skb, p->q);
	if (err != NET_XMIT_SUCCESS) {
		sch->qstats.drops++;
		return err;
Linus Torvalds's avatar
Linus Torvalds committed
313 314
	}

315 316 317 318 319
	sch->q.qlen++;
	sch->qstats.requeues++;

	return NET_XMIT_SUCCESS;
}
Linus Torvalds's avatar
Linus Torvalds committed
320 321 322

static unsigned int dsmark_drop(struct Qdisc *sch)
{
323
	struct dsmark_qdisc_data *p = qdisc_priv(sch);
Linus Torvalds's avatar
Linus Torvalds committed
324
	unsigned int len;
325

326
	pr_debug("dsmark_reset(sch %p,[qdisc %p])\n", sch, p);
327 328

	if (p->q->ops->drop == NULL)
Linus Torvalds's avatar
Linus Torvalds committed
329
		return 0;
330 331 332 333 334

	len = p->q->ops->drop(p->q);
	if (len)
		sch->q.qlen--;

Linus Torvalds's avatar
Linus Torvalds committed
335 336 337
	return len;
}

338
static int dsmark_init(struct Qdisc *sch, struct rtattr *opt)
Linus Torvalds's avatar
Linus Torvalds committed
339
{
340
	struct dsmark_qdisc_data *p = qdisc_priv(sch);
Linus Torvalds's avatar
Linus Torvalds committed
341
	struct rtattr *tb[TCA_DSMARK_MAX];
342 343 344 345 346
	int err = -EINVAL;
	u32 default_index = NO_DEFAULT_INDEX;
	u16 indices;
	u8 *mask;

347
	pr_debug("dsmark_init(sch %p,[qdisc %p],opt %p)\n", sch, p, opt);
348 349 350 351 352

	if (!opt || rtattr_parse_nested(tb, TCA_DSMARK_MAX, opt) < 0)
		goto errout;

	indices = RTA_GET_U16(tb[TCA_DSMARK_INDICES-1]);
353 354

	if (hweight32(indices) != 1)
355 356 357 358 359 360 361 362 363
		goto errout;

	if (tb[TCA_DSMARK_DEFAULT_INDEX-1])
		default_index = RTA_GET_U16(tb[TCA_DSMARK_DEFAULT_INDEX-1]);

	mask = kmalloc(indices * 2, GFP_KERNEL);
	if (mask == NULL) {
		err = -ENOMEM;
		goto errout;
Linus Torvalds's avatar
Linus Torvalds committed
364
	}
365 366 367 368 369 370 371 372 373 374 375

	p->mask = mask;
	memset(p->mask, 0xff, indices);

	p->value = p->mask + indices;
	memset(p->value, 0, indices);

	p->indices = indices;
	p->default_index = default_index;
	p->set_tc_index = RTA_GET_FLAG(tb[TCA_DSMARK_SET_TC_INDEX-1]);

376
	p->q = qdisc_create_dflt(sch->dev, &pfifo_qdisc_ops, sch->handle);
377
	if (p->q == NULL)
Linus Torvalds's avatar
Linus Torvalds committed
378
		p->q = &noop_qdisc;
379

380
	pr_debug("dsmark_init: qdisc %p\n", p->q);
381 382 383 384 385

	err = 0;
errout:
rtattr_failure:
	return err;
Linus Torvalds's avatar
Linus Torvalds committed
386 387 388 389
}

static void dsmark_reset(struct Qdisc *sch)
{
390
	struct dsmark_qdisc_data *p = qdisc_priv(sch);
Linus Torvalds's avatar
Linus Torvalds committed
391

392
	pr_debug("dsmark_reset(sch %p,[qdisc %p])\n", sch, p);
Linus Torvalds's avatar
Linus Torvalds committed
393 394 395 396 397 398
	qdisc_reset(p->q);
	sch->q.qlen = 0;
}

static void dsmark_destroy(struct Qdisc *sch)
{
399
	struct dsmark_qdisc_data *p = qdisc_priv(sch);
Linus Torvalds's avatar
Linus Torvalds committed
400

401
	pr_debug("dsmark_destroy(sch %p,[qdisc %p])\n", sch, p);
402

403
	tcf_destroy_chain(p->filter_list);
Linus Torvalds's avatar
Linus Torvalds committed
404 405 406 407 408
	qdisc_destroy(p->q);
	kfree(p->mask);
}

static int dsmark_dump_class(struct Qdisc *sch, unsigned long cl,
409
			     struct sk_buff *skb, struct tcmsg *tcm)
Linus Torvalds's avatar
Linus Torvalds committed
410
{
411
	struct dsmark_qdisc_data *p = qdisc_priv(sch);
412
	struct rtattr *opts = NULL;
Linus Torvalds's avatar
Linus Torvalds committed
413

414
	pr_debug("dsmark_dump_class(sch %p,[qdisc %p],class %ld\n", sch, p, cl);
415 416

	if (!dsmark_valid_index(p, cl))
Linus Torvalds's avatar
Linus Torvalds committed
417
		return -EINVAL;
418 419

	tcm->tcm_handle = TC_H_MAKE(TC_H_MAJ(sch->handle), cl-1);
420
	tcm->tcm_info = p->q->handle;
421 422

	opts = RTA_NEST(skb, TCA_OPTIONS);
423 424
	RTA_PUT_U8(skb, TCA_DSMARK_MASK, p->mask[cl-1]);
	RTA_PUT_U8(skb, TCA_DSMARK_VALUE, p->value[cl-1]);
425 426

	return RTA_NEST_END(skb, opts);
Linus Torvalds's avatar
Linus Torvalds committed
427 428

rtattr_failure:
429
	return RTA_NEST_CANCEL(skb, opts);
Linus Torvalds's avatar
Linus Torvalds committed
430 431 432 433
}

static int dsmark_dump(struct Qdisc *sch, struct sk_buff *skb)
{
434
	struct dsmark_qdisc_data *p = qdisc_priv(sch);
435
	struct rtattr *opts = NULL;
Linus Torvalds's avatar
Linus Torvalds committed
436

437 438 439 440 441
	opts = RTA_NEST(skb, TCA_OPTIONS);
	RTA_PUT_U16(skb, TCA_DSMARK_INDICES, p->indices);

	if (p->default_index != NO_DEFAULT_INDEX)
		RTA_PUT_U16(skb, TCA_DSMARK_DEFAULT_INDEX, p->default_index);
Linus Torvalds's avatar
Linus Torvalds committed
442 443

	if (p->set_tc_index)
444 445 446
		RTA_PUT_FLAG(skb, TCA_DSMARK_SET_TC_INDEX);

	return RTA_NEST_END(skb, opts);
Linus Torvalds's avatar
Linus Torvalds committed
447 448

rtattr_failure:
449
	return RTA_NEST_CANCEL(skb, opts);
Linus Torvalds's avatar
Linus Torvalds committed
450 451
}

452
static const struct Qdisc_class_ops dsmark_class_ops = {
Linus Torvalds's avatar
Linus Torvalds committed
453 454 455 456 457 458 459 460 461 462 463 464 465
	.graft		=	dsmark_graft,
	.leaf		=	dsmark_leaf,
	.get		=	dsmark_get,
	.put		=	dsmark_put,
	.change		=	dsmark_change,
	.delete		=	dsmark_delete,
	.walk		=	dsmark_walk,
	.tcf_chain	=	dsmark_find_tcf,
	.bind_tcf	=	dsmark_bind_filter,
	.unbind_tcf	=	dsmark_put,
	.dump		=	dsmark_dump_class,
};

466
static struct Qdisc_ops dsmark_qdisc_ops __read_mostly = {
Linus Torvalds's avatar
Linus Torvalds committed
467 468 469 470 471 472 473 474 475 476 477 478 479 480 481 482 483 484 485 486
	.next		=	NULL,
	.cl_ops		=	&dsmark_class_ops,
	.id		=	"dsmark",
	.priv_size	=	sizeof(struct dsmark_qdisc_data),
	.enqueue	=	dsmark_enqueue,
	.dequeue	=	dsmark_dequeue,
	.requeue	=	dsmark_requeue,
	.drop		=	dsmark_drop,
	.init		=	dsmark_init,
	.reset		=	dsmark_reset,
	.destroy	=	dsmark_destroy,
	.change		=	NULL,
	.dump		=	dsmark_dump,
	.owner		=	THIS_MODULE,
};

static int __init dsmark_module_init(void)
{
	return register_qdisc(&dsmark_qdisc_ops);
}
487

488
static void __exit dsmark_module_exit(void)
Linus Torvalds's avatar
Linus Torvalds committed
489 490 491
{
	unregister_qdisc(&dsmark_qdisc_ops);
}
492

Linus Torvalds's avatar
Linus Torvalds committed
493 494
module_init(dsmark_module_init)
module_exit(dsmark_module_exit)
495

Linus Torvalds's avatar
Linus Torvalds committed
496
MODULE_LICENSE("GPL");