1
2
3
4
5
6
7
8
9
10
11
12
13
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
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69struct rsvp_head {
70 u32 tmap[256/32];
71 u32 hgenerator;
72 u8 tgenerator;
73 struct rsvp_session __rcu *ht[256];
74 struct rcu_head rcu;
75};
76
77struct rsvp_session {
78 struct rsvp_session __rcu *next;
79 __be32 dst[RSVP_DST_LEN];
80 struct tc_rsvp_gpi dpi;
81 u8 protocol;
82 u8 tunnelid;
83
84 struct rsvp_filter __rcu *ht[16 + 1];
85 struct rcu_head rcu;
86};
87
88
89struct rsvp_filter {
90 struct rsvp_filter __rcu *next;
91 __be32 src[RSVP_DST_LEN];
92 struct tc_rsvp_gpi spi;
93 u8 tunnelhdr;
94
95 struct tcf_result res;
96 struct tcf_exts exts;
97
98 u32 handle;
99 struct rsvp_session *sess;
100 struct rcu_work rwork;
101};
102
103static inline unsigned int hash_dst(__be32 *dst, u8 protocol, u8 tunnelid)
104{
105 unsigned int h = (__force __u32)dst[RSVP_DST_LEN - 1];
106
107 h ^= h>>16;
108 h ^= h>>8;
109 return (h ^ protocol ^ tunnelid) & 0xFF;
110}
111
112static inline unsigned int hash_src(__be32 *src)
113{
114 unsigned int h = (__force __u32)src[RSVP_DST_LEN-1];
115
116 h ^= h>>16;
117 h ^= h>>8;
118 h ^= h>>4;
119 return h & 0xF;
120}
121
122#define RSVP_APPLY_RESULT() \
123{ \
124 int r = tcf_exts_exec(skb, &f->exts, res); \
125 if (r < 0) \
126 continue; \
127 else if (r > 0) \
128 return r; \
129}
130
131static int rsvp_classify(struct sk_buff *skb, const struct tcf_proto *tp,
132 struct tcf_result *res)
133{
134 struct rsvp_head *head = rcu_dereference_bh(tp->root);
135 struct rsvp_session *s;
136 struct rsvp_filter *f;
137 unsigned int h1, h2;
138 __be32 *dst, *src;
139 u8 protocol;
140 u8 tunnelid = 0;
141 u8 *xprt;
142#if RSVP_DST_LEN == 4
143 struct ipv6hdr *nhptr;
144
145 if (!pskb_network_may_pull(skb, sizeof(*nhptr)))
146 return -1;
147 nhptr = ipv6_hdr(skb);
148#else
149 struct iphdr *nhptr;
150
151 if (!pskb_network_may_pull(skb, sizeof(*nhptr)))
152 return -1;
153 nhptr = ip_hdr(skb);
154#endif
155restart:
156
157#if RSVP_DST_LEN == 4
158 src = &nhptr->saddr.s6_addr32[0];
159 dst = &nhptr->daddr.s6_addr32[0];
160 protocol = nhptr->nexthdr;
161 xprt = ((u8 *)nhptr) + sizeof(struct ipv6hdr);
162#else
163 src = &nhptr->saddr;
164 dst = &nhptr->daddr;
165 protocol = nhptr->protocol;
166 xprt = ((u8 *)nhptr) + (nhptr->ihl<<2);
167 if (ip_is_fragment(nhptr))
168 return -1;
169#endif
170
171 h1 = hash_dst(dst, protocol, tunnelid);
172 h2 = hash_src(src);
173
174 for (s = rcu_dereference_bh(head->ht[h1]); s;
175 s = rcu_dereference_bh(s->next)) {
176 if (dst[RSVP_DST_LEN-1] == s->dst[RSVP_DST_LEN - 1] &&
177 protocol == s->protocol &&
178 !(s->dpi.mask &
179 (*(u32 *)(xprt + s->dpi.offset) ^ s->dpi.key)) &&
180#if RSVP_DST_LEN == 4
181 dst[0] == s->dst[0] &&
182 dst[1] == s->dst[1] &&
183 dst[2] == s->dst[2] &&
184#endif
185 tunnelid == s->tunnelid) {
186
187 for (f = rcu_dereference_bh(s->ht[h2]); f;
188 f = rcu_dereference_bh(f->next)) {
189 if (src[RSVP_DST_LEN-1] == f->src[RSVP_DST_LEN - 1] &&
190 !(f->spi.mask & (*(u32 *)(xprt + f->spi.offset) ^ f->spi.key))
191#if RSVP_DST_LEN == 4
192 &&
193 src[0] == f->src[0] &&
194 src[1] == f->src[1] &&
195 src[2] == f->src[2]
196#endif
197 ) {
198 *res = f->res;
199 RSVP_APPLY_RESULT();
200
201matched:
202 if (f->tunnelhdr == 0)
203 return 0;
204
205 tunnelid = f->res.classid;
206 nhptr = (void *)(xprt + f->tunnelhdr - sizeof(*nhptr));
207 goto restart;
208 }
209 }
210
211
212 for (f = rcu_dereference_bh(s->ht[16]); f;
213 f = rcu_dereference_bh(f->next)) {
214 *res = f->res;
215 RSVP_APPLY_RESULT();
216 goto matched;
217 }
218 return -1;
219 }
220 }
221 return -1;
222}
223
224static void rsvp_replace(struct tcf_proto *tp, struct rsvp_filter *n, u32 h)
225{
226 struct rsvp_head *head = rtnl_dereference(tp->root);
227 struct rsvp_session *s;
228 struct rsvp_filter __rcu **ins;
229 struct rsvp_filter *pins;
230 unsigned int h1 = h & 0xFF;
231 unsigned int h2 = (h >> 8) & 0xFF;
232
233 for (s = rtnl_dereference(head->ht[h1]); s;
234 s = rtnl_dereference(s->next)) {
235 for (ins = &s->ht[h2], pins = rtnl_dereference(*ins); ;
236 ins = &pins->next, pins = rtnl_dereference(*ins)) {
237 if (pins->handle == h) {
238 RCU_INIT_POINTER(n->next, pins->next);
239 rcu_assign_pointer(*ins, n);
240 return;
241 }
242 }
243 }
244
245
246
247
248 BUG_ON(1);
249}
250
251static void *rsvp_get(struct tcf_proto *tp, u32 handle)
252{
253 struct rsvp_head *head = rtnl_dereference(tp->root);
254 struct rsvp_session *s;
255 struct rsvp_filter *f;
256 unsigned int h1 = handle & 0xFF;
257 unsigned int h2 = (handle >> 8) & 0xFF;
258
259 if (h2 > 16)
260 return NULL;
261
262 for (s = rtnl_dereference(head->ht[h1]); s;
263 s = rtnl_dereference(s->next)) {
264 for (f = rtnl_dereference(s->ht[h2]); f;
265 f = rtnl_dereference(f->next)) {
266 if (f->handle == handle)
267 return f;
268 }
269 }
270 return NULL;
271}
272
273static int rsvp_init(struct tcf_proto *tp)
274{
275 struct rsvp_head *data;
276
277 data = kzalloc(sizeof(struct rsvp_head), GFP_KERNEL);
278 if (data) {
279 rcu_assign_pointer(tp->root, data);
280 return 0;
281 }
282 return -ENOBUFS;
283}
284
285static void __rsvp_delete_filter(struct rsvp_filter *f)
286{
287 tcf_exts_destroy(&f->exts);
288 tcf_exts_put_net(&f->exts);
289 kfree(f);
290}
291
292static void rsvp_delete_filter_work(struct work_struct *work)
293{
294 struct rsvp_filter *f = container_of(to_rcu_work(work),
295 struct rsvp_filter,
296 rwork);
297 rtnl_lock();
298 __rsvp_delete_filter(f);
299 rtnl_unlock();
300}
301
302static void rsvp_delete_filter(struct tcf_proto *tp, struct rsvp_filter *f)
303{
304 tcf_unbind_filter(tp, &f->res);
305
306
307
308
309 if (tcf_exts_get_net(&f->exts))
310 tcf_queue_work(&f->rwork, rsvp_delete_filter_work);
311 else
312 __rsvp_delete_filter(f);
313}
314
315static void rsvp_destroy(struct tcf_proto *tp, struct netlink_ext_ack *extack)
316{
317 struct rsvp_head *data = rtnl_dereference(tp->root);
318 int h1, h2;
319
320 if (data == NULL)
321 return;
322
323 for (h1 = 0; h1 < 256; h1++) {
324 struct rsvp_session *s;
325
326 while ((s = rtnl_dereference(data->ht[h1])) != NULL) {
327 RCU_INIT_POINTER(data->ht[h1], s->next);
328
329 for (h2 = 0; h2 <= 16; h2++) {
330 struct rsvp_filter *f;
331
332 while ((f = rtnl_dereference(s->ht[h2])) != NULL) {
333 rcu_assign_pointer(s->ht[h2], f->next);
334 rsvp_delete_filter(tp, f);
335 }
336 }
337 kfree_rcu(s, rcu);
338 }
339 }
340 kfree_rcu(data, rcu);
341}
342
343static int rsvp_delete(struct tcf_proto *tp, void *arg, bool *last,
344 struct netlink_ext_ack *extack)
345{
346 struct rsvp_head *head = rtnl_dereference(tp->root);
347 struct rsvp_filter *nfp, *f = arg;
348 struct rsvp_filter __rcu **fp;
349 unsigned int h = f->handle;
350 struct rsvp_session __rcu **sp;
351 struct rsvp_session *nsp, *s = f->sess;
352 int i, h1;
353
354 fp = &s->ht[(h >> 8) & 0xFF];
355 for (nfp = rtnl_dereference(*fp); nfp;
356 fp = &nfp->next, nfp = rtnl_dereference(*fp)) {
357 if (nfp == f) {
358 RCU_INIT_POINTER(*fp, f->next);
359 rsvp_delete_filter(tp, f);
360
361
362
363 for (i = 0; i <= 16; i++)
364 if (s->ht[i])
365 goto out;
366
367
368 sp = &head->ht[h & 0xFF];
369 for (nsp = rtnl_dereference(*sp); nsp;
370 sp = &nsp->next, nsp = rtnl_dereference(*sp)) {
371 if (nsp == s) {
372 RCU_INIT_POINTER(*sp, s->next);
373 kfree_rcu(s, rcu);
374 goto out;
375 }
376 }
377
378 break;
379 }
380 }
381
382out:
383 *last = true;
384 for (h1 = 0; h1 < 256; h1++) {
385 if (rcu_access_pointer(head->ht[h1])) {
386 *last = false;
387 break;
388 }
389 }
390
391 return 0;
392}
393
394static unsigned int gen_handle(struct tcf_proto *tp, unsigned salt)
395{
396 struct rsvp_head *data = rtnl_dereference(tp->root);
397 int i = 0xFFFF;
398
399 while (i-- > 0) {
400 u32 h;
401
402 if ((data->hgenerator += 0x10000) == 0)
403 data->hgenerator = 0x10000;
404 h = data->hgenerator|salt;
405 if (!rsvp_get(tp, h))
406 return h;
407 }
408 return 0;
409}
410
411static int tunnel_bts(struct rsvp_head *data)
412{
413 int n = data->tgenerator >> 5;
414 u32 b = 1 << (data->tgenerator & 0x1F);
415
416 if (data->tmap[n] & b)
417 return 0;
418 data->tmap[n] |= b;
419 return 1;
420}
421
422static void tunnel_recycle(struct rsvp_head *data)
423{
424 struct rsvp_session __rcu **sht = data->ht;
425 u32 tmap[256/32];
426 int h1, h2;
427
428 memset(tmap, 0, sizeof(tmap));
429
430 for (h1 = 0; h1 < 256; h1++) {
431 struct rsvp_session *s;
432 for (s = rtnl_dereference(sht[h1]); s;
433 s = rtnl_dereference(s->next)) {
434 for (h2 = 0; h2 <= 16; h2++) {
435 struct rsvp_filter *f;
436
437 for (f = rtnl_dereference(s->ht[h2]); f;
438 f = rtnl_dereference(f->next)) {
439 if (f->tunnelhdr == 0)
440 continue;
441 data->tgenerator = f->res.classid;
442 tunnel_bts(data);
443 }
444 }
445 }
446 }
447
448 memcpy(data->tmap, tmap, sizeof(tmap));
449}
450
451static u32 gen_tunnel(struct rsvp_head *data)
452{
453 int i, k;
454
455 for (k = 0; k < 2; k++) {
456 for (i = 255; i > 0; i--) {
457 if (++data->tgenerator == 0)
458 data->tgenerator = 1;
459 if (tunnel_bts(data))
460 return data->tgenerator;
461 }
462 tunnel_recycle(data);
463 }
464 return 0;
465}
466
467static const struct nla_policy rsvp_policy[TCA_RSVP_MAX + 1] = {
468 [TCA_RSVP_CLASSID] = { .type = NLA_U32 },
469 [TCA_RSVP_DST] = { .type = NLA_BINARY,
470 .len = RSVP_DST_LEN * sizeof(u32) },
471 [TCA_RSVP_SRC] = { .type = NLA_BINARY,
472 .len = RSVP_DST_LEN * sizeof(u32) },
473 [TCA_RSVP_PINFO] = { .len = sizeof(struct tc_rsvp_pinfo) },
474};
475
476static int rsvp_change(struct net *net, struct sk_buff *in_skb,
477 struct tcf_proto *tp, unsigned long base,
478 u32 handle,
479 struct nlattr **tca,
480 void **arg, bool ovr, struct netlink_ext_ack *extack)
481{
482 struct rsvp_head *data = rtnl_dereference(tp->root);
483 struct rsvp_filter *f, *nfp;
484 struct rsvp_filter __rcu **fp;
485 struct rsvp_session *nsp, *s;
486 struct rsvp_session __rcu **sp;
487 struct tc_rsvp_pinfo *pinfo = NULL;
488 struct nlattr *opt = tca[TCA_OPTIONS];
489 struct nlattr *tb[TCA_RSVP_MAX + 1];
490 struct tcf_exts e;
491 unsigned int h1, h2;
492 __be32 *dst;
493 int err;
494
495 if (opt == NULL)
496 return handle ? -EINVAL : 0;
497
498 err = nla_parse_nested(tb, TCA_RSVP_MAX, opt, rsvp_policy, NULL);
499 if (err < 0)
500 return err;
501
502 err = tcf_exts_init(&e, TCA_RSVP_ACT, TCA_RSVP_POLICE);
503 if (err < 0)
504 return err;
505 err = tcf_exts_validate(net, tp, tb, tca[TCA_RATE], &e, ovr, extack);
506 if (err < 0)
507 goto errout2;
508
509 f = *arg;
510 if (f) {
511
512 struct rsvp_filter *n;
513
514 if (f->handle != handle && handle)
515 goto errout2;
516
517 n = kmemdup(f, sizeof(*f), GFP_KERNEL);
518 if (!n) {
519 err = -ENOMEM;
520 goto errout2;
521 }
522
523 err = tcf_exts_init(&n->exts, TCA_RSVP_ACT, TCA_RSVP_POLICE);
524 if (err < 0) {
525 kfree(n);
526 goto errout2;
527 }
528
529 if (tb[TCA_RSVP_CLASSID]) {
530 n->res.classid = nla_get_u32(tb[TCA_RSVP_CLASSID]);
531 tcf_bind_filter(tp, &n->res, base);
532 }
533
534 tcf_exts_change(&n->exts, &e);
535 rsvp_replace(tp, n, handle);
536 return 0;
537 }
538
539
540 err = -EINVAL;
541 if (handle)
542 goto errout2;
543 if (tb[TCA_RSVP_DST] == NULL)
544 goto errout2;
545
546 err = -ENOBUFS;
547 f = kzalloc(sizeof(struct rsvp_filter), GFP_KERNEL);
548 if (f == NULL)
549 goto errout2;
550
551 err = tcf_exts_init(&f->exts, TCA_RSVP_ACT, TCA_RSVP_POLICE);
552 if (err < 0)
553 goto errout;
554 h2 = 16;
555 if (tb[TCA_RSVP_SRC]) {
556 memcpy(f->src, nla_data(tb[TCA_RSVP_SRC]), sizeof(f->src));
557 h2 = hash_src(f->src);
558 }
559 if (tb[TCA_RSVP_PINFO]) {
560 pinfo = nla_data(tb[TCA_RSVP_PINFO]);
561 f->spi = pinfo->spi;
562 f->tunnelhdr = pinfo->tunnelhdr;
563 }
564 if (tb[TCA_RSVP_CLASSID])
565 f->res.classid = nla_get_u32(tb[TCA_RSVP_CLASSID]);
566
567 dst = nla_data(tb[TCA_RSVP_DST]);
568 h1 = hash_dst(dst, pinfo ? pinfo->protocol : 0, pinfo ? pinfo->tunnelid : 0);
569
570 err = -ENOMEM;
571 if ((f->handle = gen_handle(tp, h1 | (h2<<8))) == 0)
572 goto errout;
573
574 if (f->tunnelhdr) {
575 err = -EINVAL;
576 if (f->res.classid > 255)
577 goto errout;
578
579 err = -ENOMEM;
580 if (f->res.classid == 0 &&
581 (f->res.classid = gen_tunnel(data)) == 0)
582 goto errout;
583 }
584
585 for (sp = &data->ht[h1];
586 (s = rtnl_dereference(*sp)) != NULL;
587 sp = &s->next) {
588 if (dst[RSVP_DST_LEN-1] == s->dst[RSVP_DST_LEN-1] &&
589 pinfo && pinfo->protocol == s->protocol &&
590 memcmp(&pinfo->dpi, &s->dpi, sizeof(s->dpi)) == 0 &&
591#if RSVP_DST_LEN == 4
592 dst[0] == s->dst[0] &&
593 dst[1] == s->dst[1] &&
594 dst[2] == s->dst[2] &&
595#endif
596 pinfo->tunnelid == s->tunnelid) {
597
598insert:
599
600
601 fp = &s->ht[h2];
602
603 f->sess = s;
604 if (f->tunnelhdr == 0)
605 tcf_bind_filter(tp, &f->res, base);
606
607 tcf_exts_change(&f->exts, &e);
608
609 fp = &s->ht[h2];
610 for (nfp = rtnl_dereference(*fp); nfp;
611 fp = &nfp->next, nfp = rtnl_dereference(*fp)) {
612 __u32 mask = nfp->spi.mask & f->spi.mask;
613
614 if (mask != f->spi.mask)
615 break;
616 }
617 RCU_INIT_POINTER(f->next, nfp);
618 rcu_assign_pointer(*fp, f);
619
620 *arg = f;
621 return 0;
622 }
623 }
624
625
626
627 err = -ENOBUFS;
628 s = kzalloc(sizeof(struct rsvp_session), GFP_KERNEL);
629 if (s == NULL)
630 goto errout;
631 memcpy(s->dst, dst, sizeof(s->dst));
632
633 if (pinfo) {
634 s->dpi = pinfo->dpi;
635 s->protocol = pinfo->protocol;
636 s->tunnelid = pinfo->tunnelid;
637 }
638 sp = &data->ht[h1];
639 for (nsp = rtnl_dereference(*sp); nsp;
640 sp = &nsp->next, nsp = rtnl_dereference(*sp)) {
641 if ((nsp->dpi.mask & s->dpi.mask) != s->dpi.mask)
642 break;
643 }
644 RCU_INIT_POINTER(s->next, nsp);
645 rcu_assign_pointer(*sp, s);
646
647 goto insert;
648
649errout:
650 tcf_exts_destroy(&f->exts);
651 kfree(f);
652errout2:
653 tcf_exts_destroy(&e);
654 return err;
655}
656
657static void rsvp_walk(struct tcf_proto *tp, struct tcf_walker *arg)
658{
659 struct rsvp_head *head = rtnl_dereference(tp->root);
660 unsigned int h, h1;
661
662 if (arg->stop)
663 return;
664
665 for (h = 0; h < 256; h++) {
666 struct rsvp_session *s;
667
668 for (s = rtnl_dereference(head->ht[h]); s;
669 s = rtnl_dereference(s->next)) {
670 for (h1 = 0; h1 <= 16; h1++) {
671 struct rsvp_filter *f;
672
673 for (f = rtnl_dereference(s->ht[h1]); f;
674 f = rtnl_dereference(f->next)) {
675 if (arg->count < arg->skip) {
676 arg->count++;
677 continue;
678 }
679 if (arg->fn(tp, f, arg) < 0) {
680 arg->stop = 1;
681 return;
682 }
683 arg->count++;
684 }
685 }
686 }
687 }
688}
689
690static int rsvp_dump(struct net *net, struct tcf_proto *tp, void *fh,
691 struct sk_buff *skb, struct tcmsg *t)
692{
693 struct rsvp_filter *f = fh;
694 struct rsvp_session *s;
695 struct nlattr *nest;
696 struct tc_rsvp_pinfo pinfo;
697
698 if (f == NULL)
699 return skb->len;
700 s = f->sess;
701
702 t->tcm_handle = f->handle;
703
704 nest = nla_nest_start(skb, TCA_OPTIONS);
705 if (nest == NULL)
706 goto nla_put_failure;
707
708 if (nla_put(skb, TCA_RSVP_DST, sizeof(s->dst), &s->dst))
709 goto nla_put_failure;
710 pinfo.dpi = s->dpi;
711 pinfo.spi = f->spi;
712 pinfo.protocol = s->protocol;
713 pinfo.tunnelid = s->tunnelid;
714 pinfo.tunnelhdr = f->tunnelhdr;
715 pinfo.pad = 0;
716 if (nla_put(skb, TCA_RSVP_PINFO, sizeof(pinfo), &pinfo))
717 goto nla_put_failure;
718 if (f->res.classid &&
719 nla_put_u32(skb, TCA_RSVP_CLASSID, f->res.classid))
720 goto nla_put_failure;
721 if (((f->handle >> 8) & 0xFF) != 16 &&
722 nla_put(skb, TCA_RSVP_SRC, sizeof(f->src), f->src))
723 goto nla_put_failure;
724
725 if (tcf_exts_dump(skb, &f->exts) < 0)
726 goto nla_put_failure;
727
728 nla_nest_end(skb, nest);
729
730 if (tcf_exts_dump_stats(skb, &f->exts) < 0)
731 goto nla_put_failure;
732 return skb->len;
733
734nla_put_failure:
735 nla_nest_cancel(skb, nest);
736 return -1;
737}
738
739static void rsvp_bind_class(void *fh, u32 classid, unsigned long cl)
740{
741 struct rsvp_filter *f = fh;
742
743 if (f && f->res.classid == classid)
744 f->res.class = cl;
745}
746
747static struct tcf_proto_ops RSVP_OPS __read_mostly = {
748 .kind = RSVP_ID,
749 .classify = rsvp_classify,
750 .init = rsvp_init,
751 .destroy = rsvp_destroy,
752 .get = rsvp_get,
753 .change = rsvp_change,
754 .delete = rsvp_delete,
755 .walk = rsvp_walk,
756 .dump = rsvp_dump,
757 .bind_class = rsvp_bind_class,
758 .owner = THIS_MODULE,
759};
760
761static int __init init_rsvp(void)
762{
763 return register_tcf_proto_ops(&RSVP_OPS);
764}
765
766static void __exit exit_rsvp(void)
767{
768 unregister_tcf_proto_ops(&RSVP_OPS);
769}
770
771module_init(init_rsvp)
772module_exit(exit_rsvp)
773