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, bool rtnl_held,
316 struct netlink_ext_ack *extack)
317{
318 struct rsvp_head *data = rtnl_dereference(tp->root);
319 int h1, h2;
320
321 if (data == NULL)
322 return;
323
324 for (h1 = 0; h1 < 256; h1++) {
325 struct rsvp_session *s;
326
327 while ((s = rtnl_dereference(data->ht[h1])) != NULL) {
328 RCU_INIT_POINTER(data->ht[h1], s->next);
329
330 for (h2 = 0; h2 <= 16; h2++) {
331 struct rsvp_filter *f;
332
333 while ((f = rtnl_dereference(s->ht[h2])) != NULL) {
334 rcu_assign_pointer(s->ht[h2], f->next);
335 rsvp_delete_filter(tp, f);
336 }
337 }
338 kfree_rcu(s, rcu);
339 }
340 }
341 kfree_rcu(data, rcu);
342}
343
344static int rsvp_delete(struct tcf_proto *tp, void *arg, bool *last,
345 bool rtnl_held, struct netlink_ext_ack *extack)
346{
347 struct rsvp_head *head = rtnl_dereference(tp->root);
348 struct rsvp_filter *nfp, *f = arg;
349 struct rsvp_filter __rcu **fp;
350 unsigned int h = f->handle;
351 struct rsvp_session __rcu **sp;
352 struct rsvp_session *nsp, *s = f->sess;
353 int i, h1;
354
355 fp = &s->ht[(h >> 8) & 0xFF];
356 for (nfp = rtnl_dereference(*fp); nfp;
357 fp = &nfp->next, nfp = rtnl_dereference(*fp)) {
358 if (nfp == f) {
359 RCU_INIT_POINTER(*fp, f->next);
360 rsvp_delete_filter(tp, f);
361
362
363
364 for (i = 0; i <= 16; i++)
365 if (s->ht[i])
366 goto out;
367
368
369 sp = &head->ht[h & 0xFF];
370 for (nsp = rtnl_dereference(*sp); nsp;
371 sp = &nsp->next, nsp = rtnl_dereference(*sp)) {
372 if (nsp == s) {
373 RCU_INIT_POINTER(*sp, s->next);
374 kfree_rcu(s, rcu);
375 goto out;
376 }
377 }
378
379 break;
380 }
381 }
382
383out:
384 *last = true;
385 for (h1 = 0; h1 < 256; h1++) {
386 if (rcu_access_pointer(head->ht[h1])) {
387 *last = false;
388 break;
389 }
390 }
391
392 return 0;
393}
394
395static unsigned int gen_handle(struct tcf_proto *tp, unsigned salt)
396{
397 struct rsvp_head *data = rtnl_dereference(tp->root);
398 int i = 0xFFFF;
399
400 while (i-- > 0) {
401 u32 h;
402
403 if ((data->hgenerator += 0x10000) == 0)
404 data->hgenerator = 0x10000;
405 h = data->hgenerator|salt;
406 if (!rsvp_get(tp, h))
407 return h;
408 }
409 return 0;
410}
411
412static int tunnel_bts(struct rsvp_head *data)
413{
414 int n = data->tgenerator >> 5;
415 u32 b = 1 << (data->tgenerator & 0x1F);
416
417 if (data->tmap[n] & b)
418 return 0;
419 data->tmap[n] |= b;
420 return 1;
421}
422
423static void tunnel_recycle(struct rsvp_head *data)
424{
425 struct rsvp_session __rcu **sht = data->ht;
426 u32 tmap[256/32];
427 int h1, h2;
428
429 memset(tmap, 0, sizeof(tmap));
430
431 for (h1 = 0; h1 < 256; h1++) {
432 struct rsvp_session *s;
433 for (s = rtnl_dereference(sht[h1]); s;
434 s = rtnl_dereference(s->next)) {
435 for (h2 = 0; h2 <= 16; h2++) {
436 struct rsvp_filter *f;
437
438 for (f = rtnl_dereference(s->ht[h2]); f;
439 f = rtnl_dereference(f->next)) {
440 if (f->tunnelhdr == 0)
441 continue;
442 data->tgenerator = f->res.classid;
443 tunnel_bts(data);
444 }
445 }
446 }
447 }
448
449 memcpy(data->tmap, tmap, sizeof(tmap));
450}
451
452static u32 gen_tunnel(struct rsvp_head *data)
453{
454 int i, k;
455
456 for (k = 0; k < 2; k++) {
457 for (i = 255; i > 0; i--) {
458 if (++data->tgenerator == 0)
459 data->tgenerator = 1;
460 if (tunnel_bts(data))
461 return data->tgenerator;
462 }
463 tunnel_recycle(data);
464 }
465 return 0;
466}
467
468static const struct nla_policy rsvp_policy[TCA_RSVP_MAX + 1] = {
469 [TCA_RSVP_CLASSID] = { .type = NLA_U32 },
470 [TCA_RSVP_DST] = { .len = RSVP_DST_LEN * sizeof(u32) },
471 [TCA_RSVP_SRC] = { .len = RSVP_DST_LEN * sizeof(u32) },
472 [TCA_RSVP_PINFO] = { .len = sizeof(struct tc_rsvp_pinfo) },
473};
474
475static int rsvp_change(struct net *net, struct sk_buff *in_skb,
476 struct tcf_proto *tp, unsigned long base,
477 u32 handle,
478 struct nlattr **tca,
479 void **arg, bool ovr, bool rtnl_held,
480 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_deprecated(tb, TCA_RSVP_MAX, opt, rsvp_policy,
499 NULL);
500 if (err < 0)
501 return err;
502
503 err = tcf_exts_init(&e, net, TCA_RSVP_ACT, TCA_RSVP_POLICE);
504 if (err < 0)
505 return err;
506 err = tcf_exts_validate(net, tp, tb, tca[TCA_RATE], &e, ovr, true,
507 extack);
508 if (err < 0)
509 goto errout2;
510
511 f = *arg;
512 if (f) {
513
514 struct rsvp_filter *n;
515
516 if (f->handle != handle && handle)
517 goto errout2;
518
519 n = kmemdup(f, sizeof(*f), GFP_KERNEL);
520 if (!n) {
521 err = -ENOMEM;
522 goto errout2;
523 }
524
525 err = tcf_exts_init(&n->exts, net, TCA_RSVP_ACT,
526 TCA_RSVP_POLICE);
527 if (err < 0) {
528 kfree(n);
529 goto errout2;
530 }
531
532 if (tb[TCA_RSVP_CLASSID]) {
533 n->res.classid = nla_get_u32(tb[TCA_RSVP_CLASSID]);
534 tcf_bind_filter(tp, &n->res, base);
535 }
536
537 tcf_exts_change(&n->exts, &e);
538 rsvp_replace(tp, n, handle);
539 return 0;
540 }
541
542
543 err = -EINVAL;
544 if (handle)
545 goto errout2;
546 if (tb[TCA_RSVP_DST] == NULL)
547 goto errout2;
548
549 err = -ENOBUFS;
550 f = kzalloc(sizeof(struct rsvp_filter), GFP_KERNEL);
551 if (f == NULL)
552 goto errout2;
553
554 err = tcf_exts_init(&f->exts, net, TCA_RSVP_ACT, TCA_RSVP_POLICE);
555 if (err < 0)
556 goto errout;
557 h2 = 16;
558 if (tb[TCA_RSVP_SRC]) {
559 memcpy(f->src, nla_data(tb[TCA_RSVP_SRC]), sizeof(f->src));
560 h2 = hash_src(f->src);
561 }
562 if (tb[TCA_RSVP_PINFO]) {
563 pinfo = nla_data(tb[TCA_RSVP_PINFO]);
564 f->spi = pinfo->spi;
565 f->tunnelhdr = pinfo->tunnelhdr;
566 }
567 if (tb[TCA_RSVP_CLASSID])
568 f->res.classid = nla_get_u32(tb[TCA_RSVP_CLASSID]);
569
570 dst = nla_data(tb[TCA_RSVP_DST]);
571 h1 = hash_dst(dst, pinfo ? pinfo->protocol : 0, pinfo ? pinfo->tunnelid : 0);
572
573 err = -ENOMEM;
574 if ((f->handle = gen_handle(tp, h1 | (h2<<8))) == 0)
575 goto errout;
576
577 if (f->tunnelhdr) {
578 err = -EINVAL;
579 if (f->res.classid > 255)
580 goto errout;
581
582 err = -ENOMEM;
583 if (f->res.classid == 0 &&
584 (f->res.classid = gen_tunnel(data)) == 0)
585 goto errout;
586 }
587
588 for (sp = &data->ht[h1];
589 (s = rtnl_dereference(*sp)) != NULL;
590 sp = &s->next) {
591 if (dst[RSVP_DST_LEN-1] == s->dst[RSVP_DST_LEN-1] &&
592 pinfo && pinfo->protocol == s->protocol &&
593 memcmp(&pinfo->dpi, &s->dpi, sizeof(s->dpi)) == 0 &&
594#if RSVP_DST_LEN == 4
595 dst[0] == s->dst[0] &&
596 dst[1] == s->dst[1] &&
597 dst[2] == s->dst[2] &&
598#endif
599 pinfo->tunnelid == s->tunnelid) {
600
601insert:
602
603
604 fp = &s->ht[h2];
605
606 f->sess = s;
607 if (f->tunnelhdr == 0)
608 tcf_bind_filter(tp, &f->res, base);
609
610 tcf_exts_change(&f->exts, &e);
611
612 fp = &s->ht[h2];
613 for (nfp = rtnl_dereference(*fp); nfp;
614 fp = &nfp->next, nfp = rtnl_dereference(*fp)) {
615 __u32 mask = nfp->spi.mask & f->spi.mask;
616
617 if (mask != f->spi.mask)
618 break;
619 }
620 RCU_INIT_POINTER(f->next, nfp);
621 rcu_assign_pointer(*fp, f);
622
623 *arg = f;
624 return 0;
625 }
626 }
627
628
629
630 err = -ENOBUFS;
631 s = kzalloc(sizeof(struct rsvp_session), GFP_KERNEL);
632 if (s == NULL)
633 goto errout;
634 memcpy(s->dst, dst, sizeof(s->dst));
635
636 if (pinfo) {
637 s->dpi = pinfo->dpi;
638 s->protocol = pinfo->protocol;
639 s->tunnelid = pinfo->tunnelid;
640 }
641 sp = &data->ht[h1];
642 for (nsp = rtnl_dereference(*sp); nsp;
643 sp = &nsp->next, nsp = rtnl_dereference(*sp)) {
644 if ((nsp->dpi.mask & s->dpi.mask) != s->dpi.mask)
645 break;
646 }
647 RCU_INIT_POINTER(s->next, nsp);
648 rcu_assign_pointer(*sp, s);
649
650 goto insert;
651
652errout:
653 tcf_exts_destroy(&f->exts);
654 kfree(f);
655errout2:
656 tcf_exts_destroy(&e);
657 return err;
658}
659
660static void rsvp_walk(struct tcf_proto *tp, struct tcf_walker *arg,
661 bool rtnl_held)
662{
663 struct rsvp_head *head = rtnl_dereference(tp->root);
664 unsigned int h, h1;
665
666 if (arg->stop)
667 return;
668
669 for (h = 0; h < 256; h++) {
670 struct rsvp_session *s;
671
672 for (s = rtnl_dereference(head->ht[h]); s;
673 s = rtnl_dereference(s->next)) {
674 for (h1 = 0; h1 <= 16; h1++) {
675 struct rsvp_filter *f;
676
677 for (f = rtnl_dereference(s->ht[h1]); f;
678 f = rtnl_dereference(f->next)) {
679 if (arg->count < arg->skip) {
680 arg->count++;
681 continue;
682 }
683 if (arg->fn(tp, f, arg) < 0) {
684 arg->stop = 1;
685 return;
686 }
687 arg->count++;
688 }
689 }
690 }
691 }
692}
693
694static int rsvp_dump(struct net *net, struct tcf_proto *tp, void *fh,
695 struct sk_buff *skb, struct tcmsg *t, bool rtnl_held)
696{
697 struct rsvp_filter *f = fh;
698 struct rsvp_session *s;
699 struct nlattr *nest;
700 struct tc_rsvp_pinfo pinfo;
701
702 if (f == NULL)
703 return skb->len;
704 s = f->sess;
705
706 t->tcm_handle = f->handle;
707
708 nest = nla_nest_start_noflag(skb, TCA_OPTIONS);
709 if (nest == NULL)
710 goto nla_put_failure;
711
712 if (nla_put(skb, TCA_RSVP_DST, sizeof(s->dst), &s->dst))
713 goto nla_put_failure;
714 pinfo.dpi = s->dpi;
715 pinfo.spi = f->spi;
716 pinfo.protocol = s->protocol;
717 pinfo.tunnelid = s->tunnelid;
718 pinfo.tunnelhdr = f->tunnelhdr;
719 pinfo.pad = 0;
720 if (nla_put(skb, TCA_RSVP_PINFO, sizeof(pinfo), &pinfo))
721 goto nla_put_failure;
722 if (f->res.classid &&
723 nla_put_u32(skb, TCA_RSVP_CLASSID, f->res.classid))
724 goto nla_put_failure;
725 if (((f->handle >> 8) & 0xFF) != 16 &&
726 nla_put(skb, TCA_RSVP_SRC, sizeof(f->src), f->src))
727 goto nla_put_failure;
728
729 if (tcf_exts_dump(skb, &f->exts) < 0)
730 goto nla_put_failure;
731
732 nla_nest_end(skb, nest);
733
734 if (tcf_exts_dump_stats(skb, &f->exts) < 0)
735 goto nla_put_failure;
736 return skb->len;
737
738nla_put_failure:
739 nla_nest_cancel(skb, nest);
740 return -1;
741}
742
743static void rsvp_bind_class(void *fh, u32 classid, unsigned long cl, void *q,
744 unsigned long base)
745{
746 struct rsvp_filter *f = fh;
747
748 if (f && f->res.classid == classid) {
749 if (cl)
750 __tcf_bind_filter(q, &f->res, base);
751 else
752 __tcf_unbind_filter(q, &f->res);
753 }
754}
755
756static struct tcf_proto_ops RSVP_OPS __read_mostly = {
757 .kind = RSVP_ID,
758 .classify = rsvp_classify,
759 .init = rsvp_init,
760 .destroy = rsvp_destroy,
761 .get = rsvp_get,
762 .change = rsvp_change,
763 .delete = rsvp_delete,
764 .walk = rsvp_walk,
765 .dump = rsvp_dump,
766 .bind_class = rsvp_bind_class,
767 .owner = THIS_MODULE,
768};
769
770static int __init init_rsvp(void)
771{
772 return register_tcf_proto_ops(&RSVP_OPS);
773}
774
775static void __exit exit_rsvp(void)
776{
777 unregister_tcf_proto_ops(&RSVP_OPS);
778}
779
780module_init(init_rsvp)
781module_exit(exit_rsvp)
782