]> git.kernelconcepts.de Git - karo-tx-linux.git/blob - net/sched/cls_flower.c
net/sched: cls_flower: Add offload support using egress Hardware device
[karo-tx-linux.git] / net / sched / cls_flower.c
1 /*
2  * net/sched/cls_flower.c               Flower classifier
3  *
4  * Copyright (c) 2015 Jiri Pirko <jiri@resnulli.us>
5  *
6  * This program is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  */
11
12 #include <linux/kernel.h>
13 #include <linux/init.h>
14 #include <linux/module.h>
15 #include <linux/rhashtable.h>
16
17 #include <linux/if_ether.h>
18 #include <linux/in6.h>
19 #include <linux/ip.h>
20
21 #include <net/sch_generic.h>
22 #include <net/pkt_cls.h>
23 #include <net/ip.h>
24 #include <net/flow_dissector.h>
25
26 #include <net/dst.h>
27 #include <net/dst_metadata.h>
28
29 struct fl_flow_key {
30         int     indev_ifindex;
31         struct flow_dissector_key_control control;
32         struct flow_dissector_key_control enc_control;
33         struct flow_dissector_key_basic basic;
34         struct flow_dissector_key_eth_addrs eth;
35         struct flow_dissector_key_vlan vlan;
36         union {
37                 struct flow_dissector_key_ipv4_addrs ipv4;
38                 struct flow_dissector_key_ipv6_addrs ipv6;
39         };
40         struct flow_dissector_key_ports tp;
41         struct flow_dissector_key_keyid enc_key_id;
42         union {
43                 struct flow_dissector_key_ipv4_addrs enc_ipv4;
44                 struct flow_dissector_key_ipv6_addrs enc_ipv6;
45         };
46         struct flow_dissector_key_ports enc_tp;
47 } __aligned(BITS_PER_LONG / 8); /* Ensure that we can do comparisons as longs. */
48
49 struct fl_flow_mask_range {
50         unsigned short int start;
51         unsigned short int end;
52 };
53
54 struct fl_flow_mask {
55         struct fl_flow_key key;
56         struct fl_flow_mask_range range;
57         struct rcu_head rcu;
58 };
59
60 struct cls_fl_head {
61         struct rhashtable ht;
62         struct fl_flow_mask mask;
63         struct flow_dissector dissector;
64         u32 hgen;
65         bool mask_assigned;
66         struct list_head filters;
67         struct rhashtable_params ht_params;
68         struct rcu_head rcu;
69 };
70
71 struct cls_fl_filter {
72         struct rhash_head ht_node;
73         struct fl_flow_key mkey;
74         struct tcf_exts exts;
75         struct tcf_result res;
76         struct fl_flow_key key;
77         struct list_head list;
78         u32 handle;
79         u32 flags;
80         struct rcu_head rcu;
81         struct tc_to_netdev tc;
82         struct net_device *hw_dev;
83 };
84
85 static unsigned short int fl_mask_range(const struct fl_flow_mask *mask)
86 {
87         return mask->range.end - mask->range.start;
88 }
89
90 static void fl_mask_update_range(struct fl_flow_mask *mask)
91 {
92         const u8 *bytes = (const u8 *) &mask->key;
93         size_t size = sizeof(mask->key);
94         size_t i, first = 0, last = size - 1;
95
96         for (i = 0; i < sizeof(mask->key); i++) {
97                 if (bytes[i]) {
98                         if (!first && i)
99                                 first = i;
100                         last = i;
101                 }
102         }
103         mask->range.start = rounddown(first, sizeof(long));
104         mask->range.end = roundup(last + 1, sizeof(long));
105 }
106
107 static void *fl_key_get_start(struct fl_flow_key *key,
108                               const struct fl_flow_mask *mask)
109 {
110         return (u8 *) key + mask->range.start;
111 }
112
113 static void fl_set_masked_key(struct fl_flow_key *mkey, struct fl_flow_key *key,
114                               struct fl_flow_mask *mask)
115 {
116         const long *lkey = fl_key_get_start(key, mask);
117         const long *lmask = fl_key_get_start(&mask->key, mask);
118         long *lmkey = fl_key_get_start(mkey, mask);
119         int i;
120
121         for (i = 0; i < fl_mask_range(mask); i += sizeof(long))
122                 *lmkey++ = *lkey++ & *lmask++;
123 }
124
125 static void fl_clear_masked_range(struct fl_flow_key *key,
126                                   struct fl_flow_mask *mask)
127 {
128         memset(fl_key_get_start(key, mask), 0, fl_mask_range(mask));
129 }
130
131 static int fl_classify(struct sk_buff *skb, const struct tcf_proto *tp,
132                        struct tcf_result *res)
133 {
134         struct cls_fl_head *head = rcu_dereference_bh(tp->root);
135         struct cls_fl_filter *f;
136         struct fl_flow_key skb_key;
137         struct fl_flow_key skb_mkey;
138         struct ip_tunnel_info *info;
139
140         if (!atomic_read(&head->ht.nelems))
141                 return -1;
142
143         fl_clear_masked_range(&skb_key, &head->mask);
144
145         info = skb_tunnel_info(skb);
146         if (info) {
147                 struct ip_tunnel_key *key = &info->key;
148
149                 switch (ip_tunnel_info_af(info)) {
150                 case AF_INET:
151                         skb_key.enc_ipv4.src = key->u.ipv4.src;
152                         skb_key.enc_ipv4.dst = key->u.ipv4.dst;
153                         break;
154                 case AF_INET6:
155                         skb_key.enc_ipv6.src = key->u.ipv6.src;
156                         skb_key.enc_ipv6.dst = key->u.ipv6.dst;
157                         break;
158                 }
159
160                 skb_key.enc_key_id.keyid = tunnel_id_to_key32(key->tun_id);
161                 skb_key.enc_tp.src = key->tp_src;
162                 skb_key.enc_tp.dst = key->tp_dst;
163         }
164
165         skb_key.indev_ifindex = skb->skb_iif;
166         /* skb_flow_dissect() does not set n_proto in case an unknown protocol,
167          * so do it rather here.
168          */
169         skb_key.basic.n_proto = skb->protocol;
170         skb_flow_dissect(skb, &head->dissector, &skb_key, 0);
171
172         fl_set_masked_key(&skb_mkey, &skb_key, &head->mask);
173
174         f = rhashtable_lookup_fast(&head->ht,
175                                    fl_key_get_start(&skb_mkey, &head->mask),
176                                    head->ht_params);
177         if (f && !tc_skip_sw(f->flags)) {
178                 *res = f->res;
179                 return tcf_exts_exec(skb, &f->exts, res);
180         }
181         return -1;
182 }
183
184 static int fl_init(struct tcf_proto *tp)
185 {
186         struct cls_fl_head *head;
187
188         head = kzalloc(sizeof(*head), GFP_KERNEL);
189         if (!head)
190                 return -ENOBUFS;
191
192         INIT_LIST_HEAD_RCU(&head->filters);
193         rcu_assign_pointer(tp->root, head);
194
195         return 0;
196 }
197
198 static void fl_destroy_filter(struct rcu_head *head)
199 {
200         struct cls_fl_filter *f = container_of(head, struct cls_fl_filter, rcu);
201
202         tcf_exts_destroy(&f->exts);
203         kfree(f);
204 }
205
206 static void fl_hw_destroy_filter(struct tcf_proto *tp, struct cls_fl_filter *f)
207 {
208         struct tc_cls_flower_offload offload = {0};
209         struct net_device *dev = f->hw_dev;
210         struct tc_to_netdev *tc = &f->tc;
211
212         if (!tc_can_offload(dev, tp))
213                 return;
214
215         offload.command = TC_CLSFLOWER_DESTROY;
216         offload.cookie = (unsigned long)f;
217
218         tc->type = TC_SETUP_CLSFLOWER;
219         tc->cls_flower = &offload;
220
221         dev->netdev_ops->ndo_setup_tc(dev, tp->q->handle, tp->protocol, tc);
222 }
223
224 static int fl_hw_replace_filter(struct tcf_proto *tp,
225                                 struct flow_dissector *dissector,
226                                 struct fl_flow_key *mask,
227                                 struct cls_fl_filter *f)
228 {
229         struct net_device *dev = tp->q->dev_queue->dev;
230         struct tc_cls_flower_offload offload = {0};
231         struct tc_to_netdev *tc = &f->tc;
232         int err;
233
234         if (!tc_can_offload(dev, tp)) {
235                 if (tcf_exts_get_dev(dev, &f->exts, &f->hw_dev))
236                         return tc_skip_sw(f->flags) ? -EINVAL : 0;
237                 dev = f->hw_dev;
238                 tc->egress_dev = true;
239         } else {
240                 f->hw_dev = dev;
241         }
242
243         offload.command = TC_CLSFLOWER_REPLACE;
244         offload.cookie = (unsigned long)f;
245         offload.dissector = dissector;
246         offload.mask = mask;
247         offload.key = &f->key;
248         offload.exts = &f->exts;
249
250         tc->type = TC_SETUP_CLSFLOWER;
251         tc->cls_flower = &offload;
252
253         err = dev->netdev_ops->ndo_setup_tc(dev, tp->q->handle, tp->protocol,
254                                             tc);
255
256         if (tc_skip_sw(f->flags))
257                 return err;
258         return 0;
259 }
260
261 static void fl_hw_update_stats(struct tcf_proto *tp, struct cls_fl_filter *f)
262 {
263         struct tc_cls_flower_offload offload = {0};
264         struct net_device *dev = f->hw_dev;
265         struct tc_to_netdev *tc = &f->tc;
266
267         if (!tc_can_offload(dev, tp))
268                 return;
269
270         offload.command = TC_CLSFLOWER_STATS;
271         offload.cookie = (unsigned long)f;
272         offload.exts = &f->exts;
273
274         tc->type = TC_SETUP_CLSFLOWER;
275         tc->cls_flower = &offload;
276
277         dev->netdev_ops->ndo_setup_tc(dev, tp->q->handle, tp->protocol, tc);
278 }
279
280 static void __fl_delete(struct tcf_proto *tp, struct cls_fl_filter *f)
281 {
282         list_del_rcu(&f->list);
283         if (!tc_skip_hw(f->flags))
284                 fl_hw_destroy_filter(tp, f);
285         tcf_unbind_filter(tp, &f->res);
286         call_rcu(&f->rcu, fl_destroy_filter);
287 }
288
289 static bool fl_destroy(struct tcf_proto *tp, bool force)
290 {
291         struct cls_fl_head *head = rtnl_dereference(tp->root);
292         struct cls_fl_filter *f, *next;
293
294         if (!force && !list_empty(&head->filters))
295                 return false;
296
297         list_for_each_entry_safe(f, next, &head->filters, list)
298                 __fl_delete(tp, f);
299         RCU_INIT_POINTER(tp->root, NULL);
300         if (head->mask_assigned)
301                 rhashtable_destroy(&head->ht);
302         kfree_rcu(head, rcu);
303         return true;
304 }
305
306 static unsigned long fl_get(struct tcf_proto *tp, u32 handle)
307 {
308         struct cls_fl_head *head = rtnl_dereference(tp->root);
309         struct cls_fl_filter *f;
310
311         list_for_each_entry(f, &head->filters, list)
312                 if (f->handle == handle)
313                         return (unsigned long) f;
314         return 0;
315 }
316
317 static const struct nla_policy fl_policy[TCA_FLOWER_MAX + 1] = {
318         [TCA_FLOWER_UNSPEC]             = { .type = NLA_UNSPEC },
319         [TCA_FLOWER_CLASSID]            = { .type = NLA_U32 },
320         [TCA_FLOWER_INDEV]              = { .type = NLA_STRING,
321                                             .len = IFNAMSIZ },
322         [TCA_FLOWER_KEY_ETH_DST]        = { .len = ETH_ALEN },
323         [TCA_FLOWER_KEY_ETH_DST_MASK]   = { .len = ETH_ALEN },
324         [TCA_FLOWER_KEY_ETH_SRC]        = { .len = ETH_ALEN },
325         [TCA_FLOWER_KEY_ETH_SRC_MASK]   = { .len = ETH_ALEN },
326         [TCA_FLOWER_KEY_ETH_TYPE]       = { .type = NLA_U16 },
327         [TCA_FLOWER_KEY_IP_PROTO]       = { .type = NLA_U8 },
328         [TCA_FLOWER_KEY_IPV4_SRC]       = { .type = NLA_U32 },
329         [TCA_FLOWER_KEY_IPV4_SRC_MASK]  = { .type = NLA_U32 },
330         [TCA_FLOWER_KEY_IPV4_DST]       = { .type = NLA_U32 },
331         [TCA_FLOWER_KEY_IPV4_DST_MASK]  = { .type = NLA_U32 },
332         [TCA_FLOWER_KEY_IPV6_SRC]       = { .len = sizeof(struct in6_addr) },
333         [TCA_FLOWER_KEY_IPV6_SRC_MASK]  = { .len = sizeof(struct in6_addr) },
334         [TCA_FLOWER_KEY_IPV6_DST]       = { .len = sizeof(struct in6_addr) },
335         [TCA_FLOWER_KEY_IPV6_DST_MASK]  = { .len = sizeof(struct in6_addr) },
336         [TCA_FLOWER_KEY_TCP_SRC]        = { .type = NLA_U16 },
337         [TCA_FLOWER_KEY_TCP_DST]        = { .type = NLA_U16 },
338         [TCA_FLOWER_KEY_UDP_SRC]        = { .type = NLA_U16 },
339         [TCA_FLOWER_KEY_UDP_DST]        = { .type = NLA_U16 },
340         [TCA_FLOWER_KEY_VLAN_ID]        = { .type = NLA_U16 },
341         [TCA_FLOWER_KEY_VLAN_PRIO]      = { .type = NLA_U8 },
342         [TCA_FLOWER_KEY_VLAN_ETH_TYPE]  = { .type = NLA_U16 },
343         [TCA_FLOWER_KEY_ENC_KEY_ID]     = { .type = NLA_U32 },
344         [TCA_FLOWER_KEY_ENC_IPV4_SRC]   = { .type = NLA_U32 },
345         [TCA_FLOWER_KEY_ENC_IPV4_SRC_MASK] = { .type = NLA_U32 },
346         [TCA_FLOWER_KEY_ENC_IPV4_DST]   = { .type = NLA_U32 },
347         [TCA_FLOWER_KEY_ENC_IPV4_DST_MASK] = { .type = NLA_U32 },
348         [TCA_FLOWER_KEY_ENC_IPV6_SRC]   = { .len = sizeof(struct in6_addr) },
349         [TCA_FLOWER_KEY_ENC_IPV6_SRC_MASK] = { .len = sizeof(struct in6_addr) },
350         [TCA_FLOWER_KEY_ENC_IPV6_DST]   = { .len = sizeof(struct in6_addr) },
351         [TCA_FLOWER_KEY_ENC_IPV6_DST_MASK] = { .len = sizeof(struct in6_addr) },
352         [TCA_FLOWER_KEY_TCP_SRC_MASK]   = { .type = NLA_U16 },
353         [TCA_FLOWER_KEY_TCP_DST_MASK]   = { .type = NLA_U16 },
354         [TCA_FLOWER_KEY_UDP_SRC_MASK]   = { .type = NLA_U16 },
355         [TCA_FLOWER_KEY_UDP_DST_MASK]   = { .type = NLA_U16 },
356         [TCA_FLOWER_KEY_SCTP_SRC_MASK]  = { .type = NLA_U16 },
357         [TCA_FLOWER_KEY_SCTP_DST_MASK]  = { .type = NLA_U16 },
358         [TCA_FLOWER_KEY_SCTP_SRC]       = { .type = NLA_U16 },
359         [TCA_FLOWER_KEY_SCTP_DST]       = { .type = NLA_U16 },
360         [TCA_FLOWER_KEY_ENC_UDP_SRC_PORT]       = { .type = NLA_U16 },
361         [TCA_FLOWER_KEY_ENC_UDP_SRC_PORT_MASK]  = { .type = NLA_U16 },
362         [TCA_FLOWER_KEY_ENC_UDP_DST_PORT]       = { .type = NLA_U16 },
363         [TCA_FLOWER_KEY_ENC_UDP_DST_PORT_MASK]  = { .type = NLA_U16 },
364 };
365
366 static void fl_set_key_val(struct nlattr **tb,
367                            void *val, int val_type,
368                            void *mask, int mask_type, int len)
369 {
370         if (!tb[val_type])
371                 return;
372         memcpy(val, nla_data(tb[val_type]), len);
373         if (mask_type == TCA_FLOWER_UNSPEC || !tb[mask_type])
374                 memset(mask, 0xff, len);
375         else
376                 memcpy(mask, nla_data(tb[mask_type]), len);
377 }
378
379 static void fl_set_key_vlan(struct nlattr **tb,
380                             struct flow_dissector_key_vlan *key_val,
381                             struct flow_dissector_key_vlan *key_mask)
382 {
383 #define VLAN_PRIORITY_MASK      0x7
384
385         if (tb[TCA_FLOWER_KEY_VLAN_ID]) {
386                 key_val->vlan_id =
387                         nla_get_u16(tb[TCA_FLOWER_KEY_VLAN_ID]) & VLAN_VID_MASK;
388                 key_mask->vlan_id = VLAN_VID_MASK;
389         }
390         if (tb[TCA_FLOWER_KEY_VLAN_PRIO]) {
391                 key_val->vlan_priority =
392                         nla_get_u8(tb[TCA_FLOWER_KEY_VLAN_PRIO]) &
393                         VLAN_PRIORITY_MASK;
394                 key_mask->vlan_priority = VLAN_PRIORITY_MASK;
395         }
396 }
397
398 static int fl_set_key(struct net *net, struct nlattr **tb,
399                       struct fl_flow_key *key, struct fl_flow_key *mask)
400 {
401         __be16 ethertype;
402 #ifdef CONFIG_NET_CLS_IND
403         if (tb[TCA_FLOWER_INDEV]) {
404                 int err = tcf_change_indev(net, tb[TCA_FLOWER_INDEV]);
405                 if (err < 0)
406                         return err;
407                 key->indev_ifindex = err;
408                 mask->indev_ifindex = 0xffffffff;
409         }
410 #endif
411
412         fl_set_key_val(tb, key->eth.dst, TCA_FLOWER_KEY_ETH_DST,
413                        mask->eth.dst, TCA_FLOWER_KEY_ETH_DST_MASK,
414                        sizeof(key->eth.dst));
415         fl_set_key_val(tb, key->eth.src, TCA_FLOWER_KEY_ETH_SRC,
416                        mask->eth.src, TCA_FLOWER_KEY_ETH_SRC_MASK,
417                        sizeof(key->eth.src));
418
419         if (tb[TCA_FLOWER_KEY_ETH_TYPE]) {
420                 ethertype = nla_get_be16(tb[TCA_FLOWER_KEY_ETH_TYPE]);
421
422                 if (ethertype == htons(ETH_P_8021Q)) {
423                         fl_set_key_vlan(tb, &key->vlan, &mask->vlan);
424                         fl_set_key_val(tb, &key->basic.n_proto,
425                                        TCA_FLOWER_KEY_VLAN_ETH_TYPE,
426                                        &mask->basic.n_proto, TCA_FLOWER_UNSPEC,
427                                        sizeof(key->basic.n_proto));
428                 } else {
429                         key->basic.n_proto = ethertype;
430                         mask->basic.n_proto = cpu_to_be16(~0);
431                 }
432         }
433
434         if (key->basic.n_proto == htons(ETH_P_IP) ||
435             key->basic.n_proto == htons(ETH_P_IPV6)) {
436                 fl_set_key_val(tb, &key->basic.ip_proto, TCA_FLOWER_KEY_IP_PROTO,
437                                &mask->basic.ip_proto, TCA_FLOWER_UNSPEC,
438                                sizeof(key->basic.ip_proto));
439         }
440
441         if (tb[TCA_FLOWER_KEY_IPV4_SRC] || tb[TCA_FLOWER_KEY_IPV4_DST]) {
442                 key->control.addr_type = FLOW_DISSECTOR_KEY_IPV4_ADDRS;
443                 fl_set_key_val(tb, &key->ipv4.src, TCA_FLOWER_KEY_IPV4_SRC,
444                                &mask->ipv4.src, TCA_FLOWER_KEY_IPV4_SRC_MASK,
445                                sizeof(key->ipv4.src));
446                 fl_set_key_val(tb, &key->ipv4.dst, TCA_FLOWER_KEY_IPV4_DST,
447                                &mask->ipv4.dst, TCA_FLOWER_KEY_IPV4_DST_MASK,
448                                sizeof(key->ipv4.dst));
449         } else if (tb[TCA_FLOWER_KEY_IPV6_SRC] || tb[TCA_FLOWER_KEY_IPV6_DST]) {
450                 key->control.addr_type = FLOW_DISSECTOR_KEY_IPV6_ADDRS;
451                 fl_set_key_val(tb, &key->ipv6.src, TCA_FLOWER_KEY_IPV6_SRC,
452                                &mask->ipv6.src, TCA_FLOWER_KEY_IPV6_SRC_MASK,
453                                sizeof(key->ipv6.src));
454                 fl_set_key_val(tb, &key->ipv6.dst, TCA_FLOWER_KEY_IPV6_DST,
455                                &mask->ipv6.dst, TCA_FLOWER_KEY_IPV6_DST_MASK,
456                                sizeof(key->ipv6.dst));
457         }
458
459         if (key->basic.ip_proto == IPPROTO_TCP) {
460                 fl_set_key_val(tb, &key->tp.src, TCA_FLOWER_KEY_TCP_SRC,
461                                &mask->tp.src, TCA_FLOWER_KEY_TCP_SRC_MASK,
462                                sizeof(key->tp.src));
463                 fl_set_key_val(tb, &key->tp.dst, TCA_FLOWER_KEY_TCP_DST,
464                                &mask->tp.dst, TCA_FLOWER_KEY_TCP_DST_MASK,
465                                sizeof(key->tp.dst));
466         } else if (key->basic.ip_proto == IPPROTO_UDP) {
467                 fl_set_key_val(tb, &key->tp.src, TCA_FLOWER_KEY_UDP_SRC,
468                                &mask->tp.src, TCA_FLOWER_KEY_UDP_SRC_MASK,
469                                sizeof(key->tp.src));
470                 fl_set_key_val(tb, &key->tp.dst, TCA_FLOWER_KEY_UDP_DST,
471                                &mask->tp.dst, TCA_FLOWER_KEY_UDP_DST_MASK,
472                                sizeof(key->tp.dst));
473         } else if (key->basic.ip_proto == IPPROTO_SCTP) {
474                 fl_set_key_val(tb, &key->tp.src, TCA_FLOWER_KEY_SCTP_SRC,
475                                &mask->tp.src, TCA_FLOWER_KEY_SCTP_SRC_MASK,
476                                sizeof(key->tp.src));
477                 fl_set_key_val(tb, &key->tp.dst, TCA_FLOWER_KEY_SCTP_DST,
478                                &mask->tp.dst, TCA_FLOWER_KEY_SCTP_DST_MASK,
479                                sizeof(key->tp.dst));
480         }
481
482         if (tb[TCA_FLOWER_KEY_ENC_IPV4_SRC] ||
483             tb[TCA_FLOWER_KEY_ENC_IPV4_DST]) {
484                 key->enc_control.addr_type = FLOW_DISSECTOR_KEY_IPV4_ADDRS;
485                 fl_set_key_val(tb, &key->enc_ipv4.src,
486                                TCA_FLOWER_KEY_ENC_IPV4_SRC,
487                                &mask->enc_ipv4.src,
488                                TCA_FLOWER_KEY_ENC_IPV4_SRC_MASK,
489                                sizeof(key->enc_ipv4.src));
490                 fl_set_key_val(tb, &key->enc_ipv4.dst,
491                                TCA_FLOWER_KEY_ENC_IPV4_DST,
492                                &mask->enc_ipv4.dst,
493                                TCA_FLOWER_KEY_ENC_IPV4_DST_MASK,
494                                sizeof(key->enc_ipv4.dst));
495         }
496
497         if (tb[TCA_FLOWER_KEY_ENC_IPV6_SRC] ||
498             tb[TCA_FLOWER_KEY_ENC_IPV6_DST]) {
499                 key->enc_control.addr_type = FLOW_DISSECTOR_KEY_IPV6_ADDRS;
500                 fl_set_key_val(tb, &key->enc_ipv6.src,
501                                TCA_FLOWER_KEY_ENC_IPV6_SRC,
502                                &mask->enc_ipv6.src,
503                                TCA_FLOWER_KEY_ENC_IPV6_SRC_MASK,
504                                sizeof(key->enc_ipv6.src));
505                 fl_set_key_val(tb, &key->enc_ipv6.dst,
506                                TCA_FLOWER_KEY_ENC_IPV6_DST,
507                                &mask->enc_ipv6.dst,
508                                TCA_FLOWER_KEY_ENC_IPV6_DST_MASK,
509                                sizeof(key->enc_ipv6.dst));
510         }
511
512         fl_set_key_val(tb, &key->enc_key_id.keyid, TCA_FLOWER_KEY_ENC_KEY_ID,
513                        &mask->enc_key_id.keyid, TCA_FLOWER_UNSPEC,
514                        sizeof(key->enc_key_id.keyid));
515
516         fl_set_key_val(tb, &key->enc_tp.src, TCA_FLOWER_KEY_ENC_UDP_SRC_PORT,
517                        &mask->enc_tp.src, TCA_FLOWER_KEY_ENC_UDP_SRC_PORT_MASK,
518                        sizeof(key->enc_tp.src));
519
520         fl_set_key_val(tb, &key->enc_tp.dst, TCA_FLOWER_KEY_ENC_UDP_DST_PORT,
521                        &mask->enc_tp.dst, TCA_FLOWER_KEY_ENC_UDP_DST_PORT_MASK,
522                        sizeof(key->enc_tp.dst));
523
524         return 0;
525 }
526
527 static bool fl_mask_eq(struct fl_flow_mask *mask1,
528                        struct fl_flow_mask *mask2)
529 {
530         const long *lmask1 = fl_key_get_start(&mask1->key, mask1);
531         const long *lmask2 = fl_key_get_start(&mask2->key, mask2);
532
533         return !memcmp(&mask1->range, &mask2->range, sizeof(mask1->range)) &&
534                !memcmp(lmask1, lmask2, fl_mask_range(mask1));
535 }
536
537 static const struct rhashtable_params fl_ht_params = {
538         .key_offset = offsetof(struct cls_fl_filter, mkey), /* base offset */
539         .head_offset = offsetof(struct cls_fl_filter, ht_node),
540         .automatic_shrinking = true,
541 };
542
543 static int fl_init_hashtable(struct cls_fl_head *head,
544                              struct fl_flow_mask *mask)
545 {
546         head->ht_params = fl_ht_params;
547         head->ht_params.key_len = fl_mask_range(mask);
548         head->ht_params.key_offset += mask->range.start;
549
550         return rhashtable_init(&head->ht, &head->ht_params);
551 }
552
553 #define FL_KEY_MEMBER_OFFSET(member) offsetof(struct fl_flow_key, member)
554 #define FL_KEY_MEMBER_SIZE(member) (sizeof(((struct fl_flow_key *) 0)->member))
555
556 #define FL_KEY_IS_MASKED(mask, member)                                          \
557         memchr_inv(((char *)mask) + FL_KEY_MEMBER_OFFSET(member),               \
558                    0, FL_KEY_MEMBER_SIZE(member))                               \
559
560 #define FL_KEY_SET(keys, cnt, id, member)                                       \
561         do {                                                                    \
562                 keys[cnt].key_id = id;                                          \
563                 keys[cnt].offset = FL_KEY_MEMBER_OFFSET(member);                \
564                 cnt++;                                                          \
565         } while(0);
566
567 #define FL_KEY_SET_IF_MASKED(mask, keys, cnt, id, member)                       \
568         do {                                                                    \
569                 if (FL_KEY_IS_MASKED(mask, member))                             \
570                         FL_KEY_SET(keys, cnt, id, member);                      \
571         } while(0);
572
573 static void fl_init_dissector(struct cls_fl_head *head,
574                               struct fl_flow_mask *mask)
575 {
576         struct flow_dissector_key keys[FLOW_DISSECTOR_KEY_MAX];
577         size_t cnt = 0;
578
579         FL_KEY_SET(keys, cnt, FLOW_DISSECTOR_KEY_CONTROL, control);
580         FL_KEY_SET(keys, cnt, FLOW_DISSECTOR_KEY_BASIC, basic);
581         FL_KEY_SET_IF_MASKED(&mask->key, keys, cnt,
582                              FLOW_DISSECTOR_KEY_ETH_ADDRS, eth);
583         FL_KEY_SET_IF_MASKED(&mask->key, keys, cnt,
584                              FLOW_DISSECTOR_KEY_IPV4_ADDRS, ipv4);
585         FL_KEY_SET_IF_MASKED(&mask->key, keys, cnt,
586                              FLOW_DISSECTOR_KEY_IPV6_ADDRS, ipv6);
587         FL_KEY_SET_IF_MASKED(&mask->key, keys, cnt,
588                              FLOW_DISSECTOR_KEY_PORTS, tp);
589         FL_KEY_SET_IF_MASKED(&mask->key, keys, cnt,
590                              FLOW_DISSECTOR_KEY_VLAN, vlan);
591         FL_KEY_SET_IF_MASKED(&mask->key, keys, cnt,
592                              FLOW_DISSECTOR_KEY_ENC_KEYID, enc_key_id);
593         FL_KEY_SET_IF_MASKED(&mask->key, keys, cnt,
594                              FLOW_DISSECTOR_KEY_ENC_IPV4_ADDRS, enc_ipv4);
595         FL_KEY_SET_IF_MASKED(&mask->key, keys, cnt,
596                              FLOW_DISSECTOR_KEY_ENC_IPV6_ADDRS, enc_ipv6);
597         if (FL_KEY_IS_MASKED(&mask->key, enc_ipv4) ||
598             FL_KEY_IS_MASKED(&mask->key, enc_ipv6))
599                 FL_KEY_SET(keys, cnt, FLOW_DISSECTOR_KEY_ENC_CONTROL,
600                            enc_control);
601         FL_KEY_SET_IF_MASKED(&mask->key, keys, cnt,
602                              FLOW_DISSECTOR_KEY_ENC_PORTS, enc_tp);
603
604         skb_flow_dissector_init(&head->dissector, keys, cnt);
605 }
606
607 static int fl_check_assign_mask(struct cls_fl_head *head,
608                                 struct fl_flow_mask *mask)
609 {
610         int err;
611
612         if (head->mask_assigned) {
613                 if (!fl_mask_eq(&head->mask, mask))
614                         return -EINVAL;
615                 else
616                         return 0;
617         }
618
619         /* Mask is not assigned yet. So assign it and init hashtable
620          * according to that.
621          */
622         err = fl_init_hashtable(head, mask);
623         if (err)
624                 return err;
625         memcpy(&head->mask, mask, sizeof(head->mask));
626         head->mask_assigned = true;
627
628         fl_init_dissector(head, mask);
629
630         return 0;
631 }
632
633 static int fl_set_parms(struct net *net, struct tcf_proto *tp,
634                         struct cls_fl_filter *f, struct fl_flow_mask *mask,
635                         unsigned long base, struct nlattr **tb,
636                         struct nlattr *est, bool ovr)
637 {
638         struct tcf_exts e;
639         int err;
640
641         err = tcf_exts_init(&e, TCA_FLOWER_ACT, 0);
642         if (err < 0)
643                 return err;
644         err = tcf_exts_validate(net, tp, tb, est, &e, ovr);
645         if (err < 0)
646                 goto errout;
647
648         if (tb[TCA_FLOWER_CLASSID]) {
649                 f->res.classid = nla_get_u32(tb[TCA_FLOWER_CLASSID]);
650                 tcf_bind_filter(tp, &f->res, base);
651         }
652
653         err = fl_set_key(net, tb, &f->key, &mask->key);
654         if (err)
655                 goto errout;
656
657         fl_mask_update_range(mask);
658         fl_set_masked_key(&f->mkey, &f->key, mask);
659
660         tcf_exts_change(tp, &f->exts, &e);
661
662         return 0;
663 errout:
664         tcf_exts_destroy(&e);
665         return err;
666 }
667
668 static u32 fl_grab_new_handle(struct tcf_proto *tp,
669                               struct cls_fl_head *head)
670 {
671         unsigned int i = 0x80000000;
672         u32 handle;
673
674         do {
675                 if (++head->hgen == 0x7FFFFFFF)
676                         head->hgen = 1;
677         } while (--i > 0 && fl_get(tp, head->hgen));
678
679         if (unlikely(i == 0)) {
680                 pr_err("Insufficient number of handles\n");
681                 handle = 0;
682         } else {
683                 handle = head->hgen;
684         }
685
686         return handle;
687 }
688
689 static int fl_change(struct net *net, struct sk_buff *in_skb,
690                      struct tcf_proto *tp, unsigned long base,
691                      u32 handle, struct nlattr **tca,
692                      unsigned long *arg, bool ovr)
693 {
694         struct cls_fl_head *head = rtnl_dereference(tp->root);
695         struct cls_fl_filter *fold = (struct cls_fl_filter *) *arg;
696         struct cls_fl_filter *fnew;
697         struct nlattr *tb[TCA_FLOWER_MAX + 1];
698         struct fl_flow_mask mask = {};
699         int err;
700
701         if (!tca[TCA_OPTIONS])
702                 return -EINVAL;
703
704         err = nla_parse_nested(tb, TCA_FLOWER_MAX, tca[TCA_OPTIONS], fl_policy);
705         if (err < 0)
706                 return err;
707
708         if (fold && handle && fold->handle != handle)
709                 return -EINVAL;
710
711         fnew = kzalloc(sizeof(*fnew), GFP_KERNEL);
712         if (!fnew)
713                 return -ENOBUFS;
714
715         err = tcf_exts_init(&fnew->exts, TCA_FLOWER_ACT, 0);
716         if (err < 0)
717                 goto errout;
718
719         if (!handle) {
720                 handle = fl_grab_new_handle(tp, head);
721                 if (!handle) {
722                         err = -EINVAL;
723                         goto errout;
724                 }
725         }
726         fnew->handle = handle;
727
728         if (tb[TCA_FLOWER_FLAGS]) {
729                 fnew->flags = nla_get_u32(tb[TCA_FLOWER_FLAGS]);
730
731                 if (!tc_flags_valid(fnew->flags)) {
732                         err = -EINVAL;
733                         goto errout;
734                 }
735         }
736
737         err = fl_set_parms(net, tp, fnew, &mask, base, tb, tca[TCA_RATE], ovr);
738         if (err)
739                 goto errout;
740
741         err = fl_check_assign_mask(head, &mask);
742         if (err)
743                 goto errout;
744
745         if (!tc_skip_sw(fnew->flags)) {
746                 err = rhashtable_insert_fast(&head->ht, &fnew->ht_node,
747                                              head->ht_params);
748                 if (err)
749                         goto errout;
750         }
751
752         if (!tc_skip_hw(fnew->flags)) {
753                 err = fl_hw_replace_filter(tp,
754                                            &head->dissector,
755                                            &mask.key,
756                                            fnew);
757                 if (err)
758                         goto errout;
759         }
760
761         if (fold) {
762                 rhashtable_remove_fast(&head->ht, &fold->ht_node,
763                                        head->ht_params);
764                 if (!tc_skip_hw(fold->flags))
765                         fl_hw_destroy_filter(tp, fold);
766         }
767
768         *arg = (unsigned long) fnew;
769
770         if (fold) {
771                 list_replace_rcu(&fold->list, &fnew->list);
772                 tcf_unbind_filter(tp, &fold->res);
773                 call_rcu(&fold->rcu, fl_destroy_filter);
774         } else {
775                 list_add_tail_rcu(&fnew->list, &head->filters);
776         }
777
778         return 0;
779
780 errout:
781         tcf_exts_destroy(&fnew->exts);
782         kfree(fnew);
783         return err;
784 }
785
786 static int fl_delete(struct tcf_proto *tp, unsigned long arg)
787 {
788         struct cls_fl_head *head = rtnl_dereference(tp->root);
789         struct cls_fl_filter *f = (struct cls_fl_filter *) arg;
790
791         rhashtable_remove_fast(&head->ht, &f->ht_node,
792                                head->ht_params);
793         __fl_delete(tp, f);
794         return 0;
795 }
796
797 static void fl_walk(struct tcf_proto *tp, struct tcf_walker *arg)
798 {
799         struct cls_fl_head *head = rtnl_dereference(tp->root);
800         struct cls_fl_filter *f;
801
802         list_for_each_entry_rcu(f, &head->filters, list) {
803                 if (arg->count < arg->skip)
804                         goto skip;
805                 if (arg->fn(tp, (unsigned long) f, arg) < 0) {
806                         arg->stop = 1;
807                         break;
808                 }
809 skip:
810                 arg->count++;
811         }
812 }
813
814 static int fl_dump_key_val(struct sk_buff *skb,
815                            void *val, int val_type,
816                            void *mask, int mask_type, int len)
817 {
818         int err;
819
820         if (!memchr_inv(mask, 0, len))
821                 return 0;
822         err = nla_put(skb, val_type, len, val);
823         if (err)
824                 return err;
825         if (mask_type != TCA_FLOWER_UNSPEC) {
826                 err = nla_put(skb, mask_type, len, mask);
827                 if (err)
828                         return err;
829         }
830         return 0;
831 }
832
833 static int fl_dump_key_vlan(struct sk_buff *skb,
834                             struct flow_dissector_key_vlan *vlan_key,
835                             struct flow_dissector_key_vlan *vlan_mask)
836 {
837         int err;
838
839         if (!memchr_inv(vlan_mask, 0, sizeof(*vlan_mask)))
840                 return 0;
841         if (vlan_mask->vlan_id) {
842                 err = nla_put_u16(skb, TCA_FLOWER_KEY_VLAN_ID,
843                                   vlan_key->vlan_id);
844                 if (err)
845                         return err;
846         }
847         if (vlan_mask->vlan_priority) {
848                 err = nla_put_u8(skb, TCA_FLOWER_KEY_VLAN_PRIO,
849                                  vlan_key->vlan_priority);
850                 if (err)
851                         return err;
852         }
853         return 0;
854 }
855
856 static int fl_dump(struct net *net, struct tcf_proto *tp, unsigned long fh,
857                    struct sk_buff *skb, struct tcmsg *t)
858 {
859         struct cls_fl_head *head = rtnl_dereference(tp->root);
860         struct cls_fl_filter *f = (struct cls_fl_filter *) fh;
861         struct nlattr *nest;
862         struct fl_flow_key *key, *mask;
863
864         if (!f)
865                 return skb->len;
866
867         t->tcm_handle = f->handle;
868
869         nest = nla_nest_start(skb, TCA_OPTIONS);
870         if (!nest)
871                 goto nla_put_failure;
872
873         if (f->res.classid &&
874             nla_put_u32(skb, TCA_FLOWER_CLASSID, f->res.classid))
875                 goto nla_put_failure;
876
877         key = &f->key;
878         mask = &head->mask.key;
879
880         if (mask->indev_ifindex) {
881                 struct net_device *dev;
882
883                 dev = __dev_get_by_index(net, key->indev_ifindex);
884                 if (dev && nla_put_string(skb, TCA_FLOWER_INDEV, dev->name))
885                         goto nla_put_failure;
886         }
887
888         if (!tc_skip_hw(f->flags))
889                 fl_hw_update_stats(tp, f);
890
891         if (fl_dump_key_val(skb, key->eth.dst, TCA_FLOWER_KEY_ETH_DST,
892                             mask->eth.dst, TCA_FLOWER_KEY_ETH_DST_MASK,
893                             sizeof(key->eth.dst)) ||
894             fl_dump_key_val(skb, key->eth.src, TCA_FLOWER_KEY_ETH_SRC,
895                             mask->eth.src, TCA_FLOWER_KEY_ETH_SRC_MASK,
896                             sizeof(key->eth.src)) ||
897             fl_dump_key_val(skb, &key->basic.n_proto, TCA_FLOWER_KEY_ETH_TYPE,
898                             &mask->basic.n_proto, TCA_FLOWER_UNSPEC,
899                             sizeof(key->basic.n_proto)))
900                 goto nla_put_failure;
901
902         if (fl_dump_key_vlan(skb, &key->vlan, &mask->vlan))
903                 goto nla_put_failure;
904
905         if ((key->basic.n_proto == htons(ETH_P_IP) ||
906              key->basic.n_proto == htons(ETH_P_IPV6)) &&
907             fl_dump_key_val(skb, &key->basic.ip_proto, TCA_FLOWER_KEY_IP_PROTO,
908                             &mask->basic.ip_proto, TCA_FLOWER_UNSPEC,
909                             sizeof(key->basic.ip_proto)))
910                 goto nla_put_failure;
911
912         if (key->control.addr_type == FLOW_DISSECTOR_KEY_IPV4_ADDRS &&
913             (fl_dump_key_val(skb, &key->ipv4.src, TCA_FLOWER_KEY_IPV4_SRC,
914                              &mask->ipv4.src, TCA_FLOWER_KEY_IPV4_SRC_MASK,
915                              sizeof(key->ipv4.src)) ||
916              fl_dump_key_val(skb, &key->ipv4.dst, TCA_FLOWER_KEY_IPV4_DST,
917                              &mask->ipv4.dst, TCA_FLOWER_KEY_IPV4_DST_MASK,
918                              sizeof(key->ipv4.dst))))
919                 goto nla_put_failure;
920         else if (key->control.addr_type == FLOW_DISSECTOR_KEY_IPV6_ADDRS &&
921                  (fl_dump_key_val(skb, &key->ipv6.src, TCA_FLOWER_KEY_IPV6_SRC,
922                                   &mask->ipv6.src, TCA_FLOWER_KEY_IPV6_SRC_MASK,
923                                   sizeof(key->ipv6.src)) ||
924                   fl_dump_key_val(skb, &key->ipv6.dst, TCA_FLOWER_KEY_IPV6_DST,
925                                   &mask->ipv6.dst, TCA_FLOWER_KEY_IPV6_DST_MASK,
926                                   sizeof(key->ipv6.dst))))
927                 goto nla_put_failure;
928
929         if (key->basic.ip_proto == IPPROTO_TCP &&
930             (fl_dump_key_val(skb, &key->tp.src, TCA_FLOWER_KEY_TCP_SRC,
931                              &mask->tp.src, TCA_FLOWER_KEY_TCP_SRC_MASK,
932                              sizeof(key->tp.src)) ||
933              fl_dump_key_val(skb, &key->tp.dst, TCA_FLOWER_KEY_TCP_DST,
934                              &mask->tp.dst, TCA_FLOWER_KEY_TCP_DST_MASK,
935                              sizeof(key->tp.dst))))
936                 goto nla_put_failure;
937         else if (key->basic.ip_proto == IPPROTO_UDP &&
938                  (fl_dump_key_val(skb, &key->tp.src, TCA_FLOWER_KEY_UDP_SRC,
939                                   &mask->tp.src, TCA_FLOWER_KEY_UDP_SRC_MASK,
940                                   sizeof(key->tp.src)) ||
941                   fl_dump_key_val(skb, &key->tp.dst, TCA_FLOWER_KEY_UDP_DST,
942                                   &mask->tp.dst, TCA_FLOWER_KEY_UDP_DST_MASK,
943                                   sizeof(key->tp.dst))))
944                 goto nla_put_failure;
945         else if (key->basic.ip_proto == IPPROTO_SCTP &&
946                  (fl_dump_key_val(skb, &key->tp.src, TCA_FLOWER_KEY_SCTP_SRC,
947                                   &mask->tp.src, TCA_FLOWER_KEY_SCTP_SRC_MASK,
948                                   sizeof(key->tp.src)) ||
949                   fl_dump_key_val(skb, &key->tp.dst, TCA_FLOWER_KEY_SCTP_DST,
950                                   &mask->tp.dst, TCA_FLOWER_KEY_SCTP_DST_MASK,
951                                   sizeof(key->tp.dst))))
952                 goto nla_put_failure;
953
954         if (key->enc_control.addr_type == FLOW_DISSECTOR_KEY_IPV4_ADDRS &&
955             (fl_dump_key_val(skb, &key->enc_ipv4.src,
956                             TCA_FLOWER_KEY_ENC_IPV4_SRC, &mask->enc_ipv4.src,
957                             TCA_FLOWER_KEY_ENC_IPV4_SRC_MASK,
958                             sizeof(key->enc_ipv4.src)) ||
959              fl_dump_key_val(skb, &key->enc_ipv4.dst,
960                              TCA_FLOWER_KEY_ENC_IPV4_DST, &mask->enc_ipv4.dst,
961                              TCA_FLOWER_KEY_ENC_IPV4_DST_MASK,
962                              sizeof(key->enc_ipv4.dst))))
963                 goto nla_put_failure;
964         else if (key->enc_control.addr_type == FLOW_DISSECTOR_KEY_IPV6_ADDRS &&
965                  (fl_dump_key_val(skb, &key->enc_ipv6.src,
966                             TCA_FLOWER_KEY_ENC_IPV6_SRC, &mask->enc_ipv6.src,
967                             TCA_FLOWER_KEY_ENC_IPV6_SRC_MASK,
968                             sizeof(key->enc_ipv6.src)) ||
969                  fl_dump_key_val(skb, &key->enc_ipv6.dst,
970                                  TCA_FLOWER_KEY_ENC_IPV6_DST,
971                                  &mask->enc_ipv6.dst,
972                                  TCA_FLOWER_KEY_ENC_IPV6_DST_MASK,
973                             sizeof(key->enc_ipv6.dst))))
974                 goto nla_put_failure;
975
976         if (fl_dump_key_val(skb, &key->enc_key_id, TCA_FLOWER_KEY_ENC_KEY_ID,
977                             &mask->enc_key_id, TCA_FLOWER_UNSPEC,
978                             sizeof(key->enc_key_id)) ||
979             fl_dump_key_val(skb, &key->enc_tp.src,
980                             TCA_FLOWER_KEY_ENC_UDP_SRC_PORT,
981                             &mask->enc_tp.src,
982                             TCA_FLOWER_KEY_ENC_UDP_SRC_PORT_MASK,
983                             sizeof(key->enc_tp.src)) ||
984             fl_dump_key_val(skb, &key->enc_tp.dst,
985                             TCA_FLOWER_KEY_ENC_UDP_DST_PORT,
986                             &mask->enc_tp.dst,
987                             TCA_FLOWER_KEY_ENC_UDP_DST_PORT_MASK,
988                             sizeof(key->enc_tp.dst)))
989                 goto nla_put_failure;
990
991         nla_put_u32(skb, TCA_FLOWER_FLAGS, f->flags);
992
993         if (tcf_exts_dump(skb, &f->exts))
994                 goto nla_put_failure;
995
996         nla_nest_end(skb, nest);
997
998         if (tcf_exts_dump_stats(skb, &f->exts) < 0)
999                 goto nla_put_failure;
1000
1001         return skb->len;
1002
1003 nla_put_failure:
1004         nla_nest_cancel(skb, nest);
1005         return -1;
1006 }
1007
1008 static struct tcf_proto_ops cls_fl_ops __read_mostly = {
1009         .kind           = "flower",
1010         .classify       = fl_classify,
1011         .init           = fl_init,
1012         .destroy        = fl_destroy,
1013         .get            = fl_get,
1014         .change         = fl_change,
1015         .delete         = fl_delete,
1016         .walk           = fl_walk,
1017         .dump           = fl_dump,
1018         .owner          = THIS_MODULE,
1019 };
1020
1021 static int __init cls_fl_init(void)
1022 {
1023         return register_tcf_proto_ops(&cls_fl_ops);
1024 }
1025
1026 static void __exit cls_fl_exit(void)
1027 {
1028         unregister_tcf_proto_ops(&cls_fl_ops);
1029 }
1030
1031 module_init(cls_fl_init);
1032 module_exit(cls_fl_exit);
1033
1034 MODULE_AUTHOR("Jiri Pirko <jiri@resnulli.us>");
1035 MODULE_DESCRIPTION("Flower classifier");
1036 MODULE_LICENSE("GPL v2");