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