net/sched: cls_flower: add support for matching on ip tos and ttl
[sfrench/cifs-2.6.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 #include <linux/workqueue.h>
17
18 #include <linux/if_ether.h>
19 #include <linux/in6.h>
20 #include <linux/ip.h>
21 #include <linux/mpls.h>
22
23 #include <net/sch_generic.h>
24 #include <net/pkt_cls.h>
25 #include <net/ip.h>
26 #include <net/flow_dissector.h>
27
28 #include <net/dst.h>
29 #include <net/dst_metadata.h>
30
31 struct fl_flow_key {
32         int     indev_ifindex;
33         struct flow_dissector_key_control control;
34         struct flow_dissector_key_control enc_control;
35         struct flow_dissector_key_basic basic;
36         struct flow_dissector_key_eth_addrs eth;
37         struct flow_dissector_key_vlan vlan;
38         union {
39                 struct flow_dissector_key_ipv4_addrs ipv4;
40                 struct flow_dissector_key_ipv6_addrs ipv6;
41         };
42         struct flow_dissector_key_ports tp;
43         struct flow_dissector_key_icmp icmp;
44         struct flow_dissector_key_arp arp;
45         struct flow_dissector_key_keyid enc_key_id;
46         union {
47                 struct flow_dissector_key_ipv4_addrs enc_ipv4;
48                 struct flow_dissector_key_ipv6_addrs enc_ipv6;
49         };
50         struct flow_dissector_key_ports enc_tp;
51         struct flow_dissector_key_mpls mpls;
52         struct flow_dissector_key_tcp tcp;
53         struct flow_dissector_key_ip ip;
54 } __aligned(BITS_PER_LONG / 8); /* Ensure that we can do comparisons as longs. */
55
56 struct fl_flow_mask_range {
57         unsigned short int start;
58         unsigned short int end;
59 };
60
61 struct fl_flow_mask {
62         struct fl_flow_key key;
63         struct fl_flow_mask_range range;
64         struct rcu_head rcu;
65 };
66
67 struct cls_fl_head {
68         struct rhashtable ht;
69         struct fl_flow_mask mask;
70         struct flow_dissector dissector;
71         u32 hgen;
72         bool mask_assigned;
73         struct list_head filters;
74         struct rhashtable_params ht_params;
75         union {
76                 struct work_struct work;
77                 struct rcu_head rcu;
78         };
79 };
80
81 struct cls_fl_filter {
82         struct rhash_head ht_node;
83         struct fl_flow_key mkey;
84         struct tcf_exts exts;
85         struct tcf_result res;
86         struct fl_flow_key key;
87         struct list_head list;
88         u32 handle;
89         u32 flags;
90         struct rcu_head rcu;
91         struct tc_to_netdev tc;
92         struct net_device *hw_dev;
93 };
94
95 static unsigned short int fl_mask_range(const struct fl_flow_mask *mask)
96 {
97         return mask->range.end - mask->range.start;
98 }
99
100 static void fl_mask_update_range(struct fl_flow_mask *mask)
101 {
102         const u8 *bytes = (const u8 *) &mask->key;
103         size_t size = sizeof(mask->key);
104         size_t i, first = 0, last = size - 1;
105
106         for (i = 0; i < sizeof(mask->key); i++) {
107                 if (bytes[i]) {
108                         if (!first && i)
109                                 first = i;
110                         last = i;
111                 }
112         }
113         mask->range.start = rounddown(first, sizeof(long));
114         mask->range.end = roundup(last + 1, sizeof(long));
115 }
116
117 static void *fl_key_get_start(struct fl_flow_key *key,
118                               const struct fl_flow_mask *mask)
119 {
120         return (u8 *) key + mask->range.start;
121 }
122
123 static void fl_set_masked_key(struct fl_flow_key *mkey, struct fl_flow_key *key,
124                               struct fl_flow_mask *mask)
125 {
126         const long *lkey = fl_key_get_start(key, mask);
127         const long *lmask = fl_key_get_start(&mask->key, mask);
128         long *lmkey = fl_key_get_start(mkey, mask);
129         int i;
130
131         for (i = 0; i < fl_mask_range(mask); i += sizeof(long))
132                 *lmkey++ = *lkey++ & *lmask++;
133 }
134
135 static void fl_clear_masked_range(struct fl_flow_key *key,
136                                   struct fl_flow_mask *mask)
137 {
138         memset(fl_key_get_start(key, mask), 0, fl_mask_range(mask));
139 }
140
141 static struct cls_fl_filter *fl_lookup(struct cls_fl_head *head,
142                                        struct fl_flow_key *mkey)
143 {
144         return rhashtable_lookup_fast(&head->ht,
145                                       fl_key_get_start(mkey, &head->mask),
146                                       head->ht_params);
147 }
148
149 static int fl_classify(struct sk_buff *skb, const struct tcf_proto *tp,
150                        struct tcf_result *res)
151 {
152         struct cls_fl_head *head = rcu_dereference_bh(tp->root);
153         struct cls_fl_filter *f;
154         struct fl_flow_key skb_key;
155         struct fl_flow_key skb_mkey;
156         struct ip_tunnel_info *info;
157
158         if (!atomic_read(&head->ht.nelems))
159                 return -1;
160
161         fl_clear_masked_range(&skb_key, &head->mask);
162
163         info = skb_tunnel_info(skb);
164         if (info) {
165                 struct ip_tunnel_key *key = &info->key;
166
167                 switch (ip_tunnel_info_af(info)) {
168                 case AF_INET:
169                         skb_key.enc_control.addr_type =
170                                 FLOW_DISSECTOR_KEY_IPV4_ADDRS;
171                         skb_key.enc_ipv4.src = key->u.ipv4.src;
172                         skb_key.enc_ipv4.dst = key->u.ipv4.dst;
173                         break;
174                 case AF_INET6:
175                         skb_key.enc_control.addr_type =
176                                 FLOW_DISSECTOR_KEY_IPV6_ADDRS;
177                         skb_key.enc_ipv6.src = key->u.ipv6.src;
178                         skb_key.enc_ipv6.dst = key->u.ipv6.dst;
179                         break;
180                 }
181
182                 skb_key.enc_key_id.keyid = tunnel_id_to_key32(key->tun_id);
183                 skb_key.enc_tp.src = key->tp_src;
184                 skb_key.enc_tp.dst = key->tp_dst;
185         }
186
187         skb_key.indev_ifindex = skb->skb_iif;
188         /* skb_flow_dissect() does not set n_proto in case an unknown protocol,
189          * so do it rather here.
190          */
191         skb_key.basic.n_proto = skb->protocol;
192         skb_flow_dissect(skb, &head->dissector, &skb_key, 0);
193
194         fl_set_masked_key(&skb_mkey, &skb_key, &head->mask);
195
196         f = fl_lookup(head, &skb_mkey);
197         if (f && !tc_skip_sw(f->flags)) {
198                 *res = f->res;
199                 return tcf_exts_exec(skb, &f->exts, res);
200         }
201         return -1;
202 }
203
204 static int fl_init(struct tcf_proto *tp)
205 {
206         struct cls_fl_head *head;
207
208         head = kzalloc(sizeof(*head), GFP_KERNEL);
209         if (!head)
210                 return -ENOBUFS;
211
212         INIT_LIST_HEAD_RCU(&head->filters);
213         rcu_assign_pointer(tp->root, head);
214
215         return 0;
216 }
217
218 static void fl_destroy_filter(struct rcu_head *head)
219 {
220         struct cls_fl_filter *f = container_of(head, struct cls_fl_filter, rcu);
221
222         tcf_exts_destroy(&f->exts);
223         kfree(f);
224 }
225
226 static void fl_hw_destroy_filter(struct tcf_proto *tp, struct cls_fl_filter *f)
227 {
228         struct tc_cls_flower_offload offload = {0};
229         struct net_device *dev = f->hw_dev;
230         struct tc_to_netdev *tc = &f->tc;
231
232         if (!tc_can_offload(dev, tp))
233                 return;
234
235         offload.command = TC_CLSFLOWER_DESTROY;
236         offload.prio = tp->prio;
237         offload.cookie = (unsigned long)f;
238
239         tc->type = TC_SETUP_CLSFLOWER;
240         tc->cls_flower = &offload;
241
242         dev->netdev_ops->ndo_setup_tc(dev, tp->q->handle, tp->protocol, tc);
243 }
244
245 static int fl_hw_replace_filter(struct tcf_proto *tp,
246                                 struct flow_dissector *dissector,
247                                 struct fl_flow_key *mask,
248                                 struct cls_fl_filter *f)
249 {
250         struct net_device *dev = tp->q->dev_queue->dev;
251         struct tc_cls_flower_offload offload = {0};
252         struct tc_to_netdev *tc = &f->tc;
253         int err;
254
255         if (!tc_can_offload(dev, tp)) {
256                 if (tcf_exts_get_dev(dev, &f->exts, &f->hw_dev) ||
257                     (f->hw_dev && !tc_can_offload(f->hw_dev, tp))) {
258                         f->hw_dev = dev;
259                         return tc_skip_sw(f->flags) ? -EINVAL : 0;
260                 }
261                 dev = f->hw_dev;
262                 tc->egress_dev = true;
263         } else {
264                 f->hw_dev = dev;
265         }
266
267         offload.command = TC_CLSFLOWER_REPLACE;
268         offload.prio = tp->prio;
269         offload.cookie = (unsigned long)f;
270         offload.dissector = dissector;
271         offload.mask = mask;
272         offload.key = &f->mkey;
273         offload.exts = &f->exts;
274
275         tc->type = TC_SETUP_CLSFLOWER;
276         tc->cls_flower = &offload;
277
278         err = dev->netdev_ops->ndo_setup_tc(dev, tp->q->handle, tp->protocol,
279                                             tc);
280         if (!err)
281                 f->flags |= TCA_CLS_FLAGS_IN_HW;
282
283         if (tc_skip_sw(f->flags))
284                 return err;
285         return 0;
286 }
287
288 static void fl_hw_update_stats(struct tcf_proto *tp, struct cls_fl_filter *f)
289 {
290         struct tc_cls_flower_offload offload = {0};
291         struct net_device *dev = f->hw_dev;
292         struct tc_to_netdev *tc = &f->tc;
293
294         if (!tc_can_offload(dev, tp))
295                 return;
296
297         offload.command = TC_CLSFLOWER_STATS;
298         offload.prio = tp->prio;
299         offload.cookie = (unsigned long)f;
300         offload.exts = &f->exts;
301
302         tc->type = TC_SETUP_CLSFLOWER;
303         tc->cls_flower = &offload;
304
305         dev->netdev_ops->ndo_setup_tc(dev, tp->q->handle, tp->protocol, tc);
306 }
307
308 static void __fl_delete(struct tcf_proto *tp, struct cls_fl_filter *f)
309 {
310         list_del_rcu(&f->list);
311         if (!tc_skip_hw(f->flags))
312                 fl_hw_destroy_filter(tp, f);
313         tcf_unbind_filter(tp, &f->res);
314         call_rcu(&f->rcu, fl_destroy_filter);
315 }
316
317 static void fl_destroy_sleepable(struct work_struct *work)
318 {
319         struct cls_fl_head *head = container_of(work, struct cls_fl_head,
320                                                 work);
321         if (head->mask_assigned)
322                 rhashtable_destroy(&head->ht);
323         kfree(head);
324         module_put(THIS_MODULE);
325 }
326
327 static void fl_destroy_rcu(struct rcu_head *rcu)
328 {
329         struct cls_fl_head *head = container_of(rcu, struct cls_fl_head, rcu);
330
331         INIT_WORK(&head->work, fl_destroy_sleepable);
332         schedule_work(&head->work);
333 }
334
335 static void fl_destroy(struct tcf_proto *tp)
336 {
337         struct cls_fl_head *head = rtnl_dereference(tp->root);
338         struct cls_fl_filter *f, *next;
339
340         list_for_each_entry_safe(f, next, &head->filters, list)
341                 __fl_delete(tp, f);
342
343         __module_get(THIS_MODULE);
344         call_rcu(&head->rcu, fl_destroy_rcu);
345 }
346
347 static unsigned long fl_get(struct tcf_proto *tp, u32 handle)
348 {
349         struct cls_fl_head *head = rtnl_dereference(tp->root);
350         struct cls_fl_filter *f;
351
352         list_for_each_entry(f, &head->filters, list)
353                 if (f->handle == handle)
354                         return (unsigned long) f;
355         return 0;
356 }
357
358 static const struct nla_policy fl_policy[TCA_FLOWER_MAX + 1] = {
359         [TCA_FLOWER_UNSPEC]             = { .type = NLA_UNSPEC },
360         [TCA_FLOWER_CLASSID]            = { .type = NLA_U32 },
361         [TCA_FLOWER_INDEV]              = { .type = NLA_STRING,
362                                             .len = IFNAMSIZ },
363         [TCA_FLOWER_KEY_ETH_DST]        = { .len = ETH_ALEN },
364         [TCA_FLOWER_KEY_ETH_DST_MASK]   = { .len = ETH_ALEN },
365         [TCA_FLOWER_KEY_ETH_SRC]        = { .len = ETH_ALEN },
366         [TCA_FLOWER_KEY_ETH_SRC_MASK]   = { .len = ETH_ALEN },
367         [TCA_FLOWER_KEY_ETH_TYPE]       = { .type = NLA_U16 },
368         [TCA_FLOWER_KEY_IP_PROTO]       = { .type = NLA_U8 },
369         [TCA_FLOWER_KEY_IPV4_SRC]       = { .type = NLA_U32 },
370         [TCA_FLOWER_KEY_IPV4_SRC_MASK]  = { .type = NLA_U32 },
371         [TCA_FLOWER_KEY_IPV4_DST]       = { .type = NLA_U32 },
372         [TCA_FLOWER_KEY_IPV4_DST_MASK]  = { .type = NLA_U32 },
373         [TCA_FLOWER_KEY_IPV6_SRC]       = { .len = sizeof(struct in6_addr) },
374         [TCA_FLOWER_KEY_IPV6_SRC_MASK]  = { .len = sizeof(struct in6_addr) },
375         [TCA_FLOWER_KEY_IPV6_DST]       = { .len = sizeof(struct in6_addr) },
376         [TCA_FLOWER_KEY_IPV6_DST_MASK]  = { .len = sizeof(struct in6_addr) },
377         [TCA_FLOWER_KEY_TCP_SRC]        = { .type = NLA_U16 },
378         [TCA_FLOWER_KEY_TCP_DST]        = { .type = NLA_U16 },
379         [TCA_FLOWER_KEY_UDP_SRC]        = { .type = NLA_U16 },
380         [TCA_FLOWER_KEY_UDP_DST]        = { .type = NLA_U16 },
381         [TCA_FLOWER_KEY_VLAN_ID]        = { .type = NLA_U16 },
382         [TCA_FLOWER_KEY_VLAN_PRIO]      = { .type = NLA_U8 },
383         [TCA_FLOWER_KEY_VLAN_ETH_TYPE]  = { .type = NLA_U16 },
384         [TCA_FLOWER_KEY_ENC_KEY_ID]     = { .type = NLA_U32 },
385         [TCA_FLOWER_KEY_ENC_IPV4_SRC]   = { .type = NLA_U32 },
386         [TCA_FLOWER_KEY_ENC_IPV4_SRC_MASK] = { .type = NLA_U32 },
387         [TCA_FLOWER_KEY_ENC_IPV4_DST]   = { .type = NLA_U32 },
388         [TCA_FLOWER_KEY_ENC_IPV4_DST_MASK] = { .type = NLA_U32 },
389         [TCA_FLOWER_KEY_ENC_IPV6_SRC]   = { .len = sizeof(struct in6_addr) },
390         [TCA_FLOWER_KEY_ENC_IPV6_SRC_MASK] = { .len = sizeof(struct in6_addr) },
391         [TCA_FLOWER_KEY_ENC_IPV6_DST]   = { .len = sizeof(struct in6_addr) },
392         [TCA_FLOWER_KEY_ENC_IPV6_DST_MASK] = { .len = sizeof(struct in6_addr) },
393         [TCA_FLOWER_KEY_TCP_SRC_MASK]   = { .type = NLA_U16 },
394         [TCA_FLOWER_KEY_TCP_DST_MASK]   = { .type = NLA_U16 },
395         [TCA_FLOWER_KEY_UDP_SRC_MASK]   = { .type = NLA_U16 },
396         [TCA_FLOWER_KEY_UDP_DST_MASK]   = { .type = NLA_U16 },
397         [TCA_FLOWER_KEY_SCTP_SRC_MASK]  = { .type = NLA_U16 },
398         [TCA_FLOWER_KEY_SCTP_DST_MASK]  = { .type = NLA_U16 },
399         [TCA_FLOWER_KEY_SCTP_SRC]       = { .type = NLA_U16 },
400         [TCA_FLOWER_KEY_SCTP_DST]       = { .type = NLA_U16 },
401         [TCA_FLOWER_KEY_ENC_UDP_SRC_PORT]       = { .type = NLA_U16 },
402         [TCA_FLOWER_KEY_ENC_UDP_SRC_PORT_MASK]  = { .type = NLA_U16 },
403         [TCA_FLOWER_KEY_ENC_UDP_DST_PORT]       = { .type = NLA_U16 },
404         [TCA_FLOWER_KEY_ENC_UDP_DST_PORT_MASK]  = { .type = NLA_U16 },
405         [TCA_FLOWER_KEY_FLAGS]          = { .type = NLA_U32 },
406         [TCA_FLOWER_KEY_FLAGS_MASK]     = { .type = NLA_U32 },
407         [TCA_FLOWER_KEY_ICMPV4_TYPE]    = { .type = NLA_U8 },
408         [TCA_FLOWER_KEY_ICMPV4_TYPE_MASK] = { .type = NLA_U8 },
409         [TCA_FLOWER_KEY_ICMPV4_CODE]    = { .type = NLA_U8 },
410         [TCA_FLOWER_KEY_ICMPV4_CODE_MASK] = { .type = NLA_U8 },
411         [TCA_FLOWER_KEY_ICMPV6_TYPE]    = { .type = NLA_U8 },
412         [TCA_FLOWER_KEY_ICMPV6_TYPE_MASK] = { .type = NLA_U8 },
413         [TCA_FLOWER_KEY_ICMPV6_CODE]    = { .type = NLA_U8 },
414         [TCA_FLOWER_KEY_ICMPV6_CODE_MASK] = { .type = NLA_U8 },
415         [TCA_FLOWER_KEY_ARP_SIP]        = { .type = NLA_U32 },
416         [TCA_FLOWER_KEY_ARP_SIP_MASK]   = { .type = NLA_U32 },
417         [TCA_FLOWER_KEY_ARP_TIP]        = { .type = NLA_U32 },
418         [TCA_FLOWER_KEY_ARP_TIP_MASK]   = { .type = NLA_U32 },
419         [TCA_FLOWER_KEY_ARP_OP]         = { .type = NLA_U8 },
420         [TCA_FLOWER_KEY_ARP_OP_MASK]    = { .type = NLA_U8 },
421         [TCA_FLOWER_KEY_ARP_SHA]        = { .len = ETH_ALEN },
422         [TCA_FLOWER_KEY_ARP_SHA_MASK]   = { .len = ETH_ALEN },
423         [TCA_FLOWER_KEY_ARP_THA]        = { .len = ETH_ALEN },
424         [TCA_FLOWER_KEY_ARP_THA_MASK]   = { .len = ETH_ALEN },
425         [TCA_FLOWER_KEY_MPLS_TTL]       = { .type = NLA_U8 },
426         [TCA_FLOWER_KEY_MPLS_BOS]       = { .type = NLA_U8 },
427         [TCA_FLOWER_KEY_MPLS_TC]        = { .type = NLA_U8 },
428         [TCA_FLOWER_KEY_MPLS_LABEL]     = { .type = NLA_U32 },
429         [TCA_FLOWER_KEY_TCP_FLAGS]      = { .type = NLA_U16 },
430         [TCA_FLOWER_KEY_TCP_FLAGS_MASK] = { .type = NLA_U16 },
431         [TCA_FLOWER_KEY_IP_TOS]         = { .type = NLA_U8 },
432         [TCA_FLOWER_KEY_IP_TOS_MASK]    = { .type = NLA_U8 },
433         [TCA_FLOWER_KEY_IP_TTL]         = { .type = NLA_U8 },
434         [TCA_FLOWER_KEY_IP_TTL_MASK]    = { .type = NLA_U8 },
435 };
436
437 static void fl_set_key_val(struct nlattr **tb,
438                            void *val, int val_type,
439                            void *mask, int mask_type, int len)
440 {
441         if (!tb[val_type])
442                 return;
443         memcpy(val, nla_data(tb[val_type]), len);
444         if (mask_type == TCA_FLOWER_UNSPEC || !tb[mask_type])
445                 memset(mask, 0xff, len);
446         else
447                 memcpy(mask, nla_data(tb[mask_type]), len);
448 }
449
450 static int fl_set_key_mpls(struct nlattr **tb,
451                            struct flow_dissector_key_mpls *key_val,
452                            struct flow_dissector_key_mpls *key_mask)
453 {
454         if (tb[TCA_FLOWER_KEY_MPLS_TTL]) {
455                 key_val->mpls_ttl = nla_get_u8(tb[TCA_FLOWER_KEY_MPLS_TTL]);
456                 key_mask->mpls_ttl = MPLS_TTL_MASK;
457         }
458         if (tb[TCA_FLOWER_KEY_MPLS_BOS]) {
459                 u8 bos = nla_get_u8(tb[TCA_FLOWER_KEY_MPLS_BOS]);
460
461                 if (bos & ~MPLS_BOS_MASK)
462                         return -EINVAL;
463                 key_val->mpls_bos = bos;
464                 key_mask->mpls_bos = MPLS_BOS_MASK;
465         }
466         if (tb[TCA_FLOWER_KEY_MPLS_TC]) {
467                 u8 tc = nla_get_u8(tb[TCA_FLOWER_KEY_MPLS_TC]);
468
469                 if (tc & ~MPLS_TC_MASK)
470                         return -EINVAL;
471                 key_val->mpls_tc = tc;
472                 key_mask->mpls_tc = MPLS_TC_MASK;
473         }
474         if (tb[TCA_FLOWER_KEY_MPLS_LABEL]) {
475                 u32 label = nla_get_u32(tb[TCA_FLOWER_KEY_MPLS_LABEL]);
476
477                 if (label & ~MPLS_LABEL_MASK)
478                         return -EINVAL;
479                 key_val->mpls_label = label;
480                 key_mask->mpls_label = MPLS_LABEL_MASK;
481         }
482         return 0;
483 }
484
485 static void fl_set_key_vlan(struct nlattr **tb,
486                             struct flow_dissector_key_vlan *key_val,
487                             struct flow_dissector_key_vlan *key_mask)
488 {
489 #define VLAN_PRIORITY_MASK      0x7
490
491         if (tb[TCA_FLOWER_KEY_VLAN_ID]) {
492                 key_val->vlan_id =
493                         nla_get_u16(tb[TCA_FLOWER_KEY_VLAN_ID]) & VLAN_VID_MASK;
494                 key_mask->vlan_id = VLAN_VID_MASK;
495         }
496         if (tb[TCA_FLOWER_KEY_VLAN_PRIO]) {
497                 key_val->vlan_priority =
498                         nla_get_u8(tb[TCA_FLOWER_KEY_VLAN_PRIO]) &
499                         VLAN_PRIORITY_MASK;
500                 key_mask->vlan_priority = VLAN_PRIORITY_MASK;
501         }
502 }
503
504 static void fl_set_key_flag(u32 flower_key, u32 flower_mask,
505                             u32 *dissector_key, u32 *dissector_mask,
506                             u32 flower_flag_bit, u32 dissector_flag_bit)
507 {
508         if (flower_mask & flower_flag_bit) {
509                 *dissector_mask |= dissector_flag_bit;
510                 if (flower_key & flower_flag_bit)
511                         *dissector_key |= dissector_flag_bit;
512         }
513 }
514
515 static int fl_set_key_flags(struct nlattr **tb,
516                             u32 *flags_key, u32 *flags_mask)
517 {
518         u32 key, mask;
519
520         /* mask is mandatory for flags */
521         if (!tb[TCA_FLOWER_KEY_FLAGS_MASK])
522                 return -EINVAL;
523
524         key = be32_to_cpu(nla_get_u32(tb[TCA_FLOWER_KEY_FLAGS]));
525         mask = be32_to_cpu(nla_get_u32(tb[TCA_FLOWER_KEY_FLAGS_MASK]));
526
527         *flags_key  = 0;
528         *flags_mask = 0;
529
530         fl_set_key_flag(key, mask, flags_key, flags_mask,
531                         TCA_FLOWER_KEY_FLAGS_IS_FRAGMENT, FLOW_DIS_IS_FRAGMENT);
532
533         return 0;
534 }
535
536 static void fl_set_key_ip(struct nlattr **tb,
537                           struct flow_dissector_key_ip *key,
538                           struct flow_dissector_key_ip *mask)
539 {
540                 fl_set_key_val(tb, &key->tos, TCA_FLOWER_KEY_IP_TOS,
541                                &mask->tos, TCA_FLOWER_KEY_IP_TOS_MASK,
542                                sizeof(key->tos));
543
544                 fl_set_key_val(tb, &key->ttl, TCA_FLOWER_KEY_IP_TTL,
545                                &mask->ttl, TCA_FLOWER_KEY_IP_TTL_MASK,
546                                sizeof(key->ttl));
547 }
548
549 static int fl_set_key(struct net *net, struct nlattr **tb,
550                       struct fl_flow_key *key, struct fl_flow_key *mask)
551 {
552         __be16 ethertype;
553         int ret = 0;
554 #ifdef CONFIG_NET_CLS_IND
555         if (tb[TCA_FLOWER_INDEV]) {
556                 int err = tcf_change_indev(net, tb[TCA_FLOWER_INDEV]);
557                 if (err < 0)
558                         return err;
559                 key->indev_ifindex = err;
560                 mask->indev_ifindex = 0xffffffff;
561         }
562 #endif
563
564         fl_set_key_val(tb, key->eth.dst, TCA_FLOWER_KEY_ETH_DST,
565                        mask->eth.dst, TCA_FLOWER_KEY_ETH_DST_MASK,
566                        sizeof(key->eth.dst));
567         fl_set_key_val(tb, key->eth.src, TCA_FLOWER_KEY_ETH_SRC,
568                        mask->eth.src, TCA_FLOWER_KEY_ETH_SRC_MASK,
569                        sizeof(key->eth.src));
570
571         if (tb[TCA_FLOWER_KEY_ETH_TYPE]) {
572                 ethertype = nla_get_be16(tb[TCA_FLOWER_KEY_ETH_TYPE]);
573
574                 if (ethertype == htons(ETH_P_8021Q)) {
575                         fl_set_key_vlan(tb, &key->vlan, &mask->vlan);
576                         fl_set_key_val(tb, &key->basic.n_proto,
577                                        TCA_FLOWER_KEY_VLAN_ETH_TYPE,
578                                        &mask->basic.n_proto, TCA_FLOWER_UNSPEC,
579                                        sizeof(key->basic.n_proto));
580                 } else {
581                         key->basic.n_proto = ethertype;
582                         mask->basic.n_proto = cpu_to_be16(~0);
583                 }
584         }
585
586         if (key->basic.n_proto == htons(ETH_P_IP) ||
587             key->basic.n_proto == htons(ETH_P_IPV6)) {
588                 fl_set_key_val(tb, &key->basic.ip_proto, TCA_FLOWER_KEY_IP_PROTO,
589                                &mask->basic.ip_proto, TCA_FLOWER_UNSPEC,
590                                sizeof(key->basic.ip_proto));
591                 fl_set_key_ip(tb, &key->ip, &mask->ip);
592         }
593
594         if (tb[TCA_FLOWER_KEY_IPV4_SRC] || tb[TCA_FLOWER_KEY_IPV4_DST]) {
595                 key->control.addr_type = FLOW_DISSECTOR_KEY_IPV4_ADDRS;
596                 mask->control.addr_type = ~0;
597                 fl_set_key_val(tb, &key->ipv4.src, TCA_FLOWER_KEY_IPV4_SRC,
598                                &mask->ipv4.src, TCA_FLOWER_KEY_IPV4_SRC_MASK,
599                                sizeof(key->ipv4.src));
600                 fl_set_key_val(tb, &key->ipv4.dst, TCA_FLOWER_KEY_IPV4_DST,
601                                &mask->ipv4.dst, TCA_FLOWER_KEY_IPV4_DST_MASK,
602                                sizeof(key->ipv4.dst));
603         } else if (tb[TCA_FLOWER_KEY_IPV6_SRC] || tb[TCA_FLOWER_KEY_IPV6_DST]) {
604                 key->control.addr_type = FLOW_DISSECTOR_KEY_IPV6_ADDRS;
605                 mask->control.addr_type = ~0;
606                 fl_set_key_val(tb, &key->ipv6.src, TCA_FLOWER_KEY_IPV6_SRC,
607                                &mask->ipv6.src, TCA_FLOWER_KEY_IPV6_SRC_MASK,
608                                sizeof(key->ipv6.src));
609                 fl_set_key_val(tb, &key->ipv6.dst, TCA_FLOWER_KEY_IPV6_DST,
610                                &mask->ipv6.dst, TCA_FLOWER_KEY_IPV6_DST_MASK,
611                                sizeof(key->ipv6.dst));
612         }
613
614         if (key->basic.ip_proto == IPPROTO_TCP) {
615                 fl_set_key_val(tb, &key->tp.src, TCA_FLOWER_KEY_TCP_SRC,
616                                &mask->tp.src, TCA_FLOWER_KEY_TCP_SRC_MASK,
617                                sizeof(key->tp.src));
618                 fl_set_key_val(tb, &key->tp.dst, TCA_FLOWER_KEY_TCP_DST,
619                                &mask->tp.dst, TCA_FLOWER_KEY_TCP_DST_MASK,
620                                sizeof(key->tp.dst));
621                 fl_set_key_val(tb, &key->tcp.flags, TCA_FLOWER_KEY_TCP_FLAGS,
622                                &mask->tcp.flags, TCA_FLOWER_KEY_TCP_FLAGS_MASK,
623                                sizeof(key->tcp.flags));
624         } else if (key->basic.ip_proto == IPPROTO_UDP) {
625                 fl_set_key_val(tb, &key->tp.src, TCA_FLOWER_KEY_UDP_SRC,
626                                &mask->tp.src, TCA_FLOWER_KEY_UDP_SRC_MASK,
627                                sizeof(key->tp.src));
628                 fl_set_key_val(tb, &key->tp.dst, TCA_FLOWER_KEY_UDP_DST,
629                                &mask->tp.dst, TCA_FLOWER_KEY_UDP_DST_MASK,
630                                sizeof(key->tp.dst));
631         } else if (key->basic.ip_proto == IPPROTO_SCTP) {
632                 fl_set_key_val(tb, &key->tp.src, TCA_FLOWER_KEY_SCTP_SRC,
633                                &mask->tp.src, TCA_FLOWER_KEY_SCTP_SRC_MASK,
634                                sizeof(key->tp.src));
635                 fl_set_key_val(tb, &key->tp.dst, TCA_FLOWER_KEY_SCTP_DST,
636                                &mask->tp.dst, TCA_FLOWER_KEY_SCTP_DST_MASK,
637                                sizeof(key->tp.dst));
638         } else if (key->basic.n_proto == htons(ETH_P_IP) &&
639                    key->basic.ip_proto == IPPROTO_ICMP) {
640                 fl_set_key_val(tb, &key->icmp.type, TCA_FLOWER_KEY_ICMPV4_TYPE,
641                                &mask->icmp.type,
642                                TCA_FLOWER_KEY_ICMPV4_TYPE_MASK,
643                                sizeof(key->icmp.type));
644                 fl_set_key_val(tb, &key->icmp.code, TCA_FLOWER_KEY_ICMPV4_CODE,
645                                &mask->icmp.code,
646                                TCA_FLOWER_KEY_ICMPV4_CODE_MASK,
647                                sizeof(key->icmp.code));
648         } else if (key->basic.n_proto == htons(ETH_P_IPV6) &&
649                    key->basic.ip_proto == IPPROTO_ICMPV6) {
650                 fl_set_key_val(tb, &key->icmp.type, TCA_FLOWER_KEY_ICMPV6_TYPE,
651                                &mask->icmp.type,
652                                TCA_FLOWER_KEY_ICMPV6_TYPE_MASK,
653                                sizeof(key->icmp.type));
654                 fl_set_key_val(tb, &key->icmp.code, TCA_FLOWER_KEY_ICMPV6_CODE,
655                                &mask->icmp.code,
656                                TCA_FLOWER_KEY_ICMPV6_CODE_MASK,
657                                sizeof(key->icmp.code));
658         } else if (key->basic.n_proto == htons(ETH_P_MPLS_UC) ||
659                    key->basic.n_proto == htons(ETH_P_MPLS_MC)) {
660                 ret = fl_set_key_mpls(tb, &key->mpls, &mask->mpls);
661                 if (ret)
662                         return ret;
663         } else if (key->basic.n_proto == htons(ETH_P_ARP) ||
664                    key->basic.n_proto == htons(ETH_P_RARP)) {
665                 fl_set_key_val(tb, &key->arp.sip, TCA_FLOWER_KEY_ARP_SIP,
666                                &mask->arp.sip, TCA_FLOWER_KEY_ARP_SIP_MASK,
667                                sizeof(key->arp.sip));
668                 fl_set_key_val(tb, &key->arp.tip, TCA_FLOWER_KEY_ARP_TIP,
669                                &mask->arp.tip, TCA_FLOWER_KEY_ARP_TIP_MASK,
670                                sizeof(key->arp.tip));
671                 fl_set_key_val(tb, &key->arp.op, TCA_FLOWER_KEY_ARP_OP,
672                                &mask->arp.op, TCA_FLOWER_KEY_ARP_OP_MASK,
673                                sizeof(key->arp.op));
674                 fl_set_key_val(tb, key->arp.sha, TCA_FLOWER_KEY_ARP_SHA,
675                                mask->arp.sha, TCA_FLOWER_KEY_ARP_SHA_MASK,
676                                sizeof(key->arp.sha));
677                 fl_set_key_val(tb, key->arp.tha, TCA_FLOWER_KEY_ARP_THA,
678                                mask->arp.tha, TCA_FLOWER_KEY_ARP_THA_MASK,
679                                sizeof(key->arp.tha));
680         }
681
682         if (tb[TCA_FLOWER_KEY_ENC_IPV4_SRC] ||
683             tb[TCA_FLOWER_KEY_ENC_IPV4_DST]) {
684                 key->enc_control.addr_type = FLOW_DISSECTOR_KEY_IPV4_ADDRS;
685                 mask->enc_control.addr_type = ~0;
686                 fl_set_key_val(tb, &key->enc_ipv4.src,
687                                TCA_FLOWER_KEY_ENC_IPV4_SRC,
688                                &mask->enc_ipv4.src,
689                                TCA_FLOWER_KEY_ENC_IPV4_SRC_MASK,
690                                sizeof(key->enc_ipv4.src));
691                 fl_set_key_val(tb, &key->enc_ipv4.dst,
692                                TCA_FLOWER_KEY_ENC_IPV4_DST,
693                                &mask->enc_ipv4.dst,
694                                TCA_FLOWER_KEY_ENC_IPV4_DST_MASK,
695                                sizeof(key->enc_ipv4.dst));
696         }
697
698         if (tb[TCA_FLOWER_KEY_ENC_IPV6_SRC] ||
699             tb[TCA_FLOWER_KEY_ENC_IPV6_DST]) {
700                 key->enc_control.addr_type = FLOW_DISSECTOR_KEY_IPV6_ADDRS;
701                 mask->enc_control.addr_type = ~0;
702                 fl_set_key_val(tb, &key->enc_ipv6.src,
703                                TCA_FLOWER_KEY_ENC_IPV6_SRC,
704                                &mask->enc_ipv6.src,
705                                TCA_FLOWER_KEY_ENC_IPV6_SRC_MASK,
706                                sizeof(key->enc_ipv6.src));
707                 fl_set_key_val(tb, &key->enc_ipv6.dst,
708                                TCA_FLOWER_KEY_ENC_IPV6_DST,
709                                &mask->enc_ipv6.dst,
710                                TCA_FLOWER_KEY_ENC_IPV6_DST_MASK,
711                                sizeof(key->enc_ipv6.dst));
712         }
713
714         fl_set_key_val(tb, &key->enc_key_id.keyid, TCA_FLOWER_KEY_ENC_KEY_ID,
715                        &mask->enc_key_id.keyid, TCA_FLOWER_UNSPEC,
716                        sizeof(key->enc_key_id.keyid));
717
718         fl_set_key_val(tb, &key->enc_tp.src, TCA_FLOWER_KEY_ENC_UDP_SRC_PORT,
719                        &mask->enc_tp.src, TCA_FLOWER_KEY_ENC_UDP_SRC_PORT_MASK,
720                        sizeof(key->enc_tp.src));
721
722         fl_set_key_val(tb, &key->enc_tp.dst, TCA_FLOWER_KEY_ENC_UDP_DST_PORT,
723                        &mask->enc_tp.dst, TCA_FLOWER_KEY_ENC_UDP_DST_PORT_MASK,
724                        sizeof(key->enc_tp.dst));
725
726         if (tb[TCA_FLOWER_KEY_FLAGS])
727                 ret = fl_set_key_flags(tb, &key->control.flags, &mask->control.flags);
728
729         return ret;
730 }
731
732 static bool fl_mask_eq(struct fl_flow_mask *mask1,
733                        struct fl_flow_mask *mask2)
734 {
735         const long *lmask1 = fl_key_get_start(&mask1->key, mask1);
736         const long *lmask2 = fl_key_get_start(&mask2->key, mask2);
737
738         return !memcmp(&mask1->range, &mask2->range, sizeof(mask1->range)) &&
739                !memcmp(lmask1, lmask2, fl_mask_range(mask1));
740 }
741
742 static const struct rhashtable_params fl_ht_params = {
743         .key_offset = offsetof(struct cls_fl_filter, mkey), /* base offset */
744         .head_offset = offsetof(struct cls_fl_filter, ht_node),
745         .automatic_shrinking = true,
746 };
747
748 static int fl_init_hashtable(struct cls_fl_head *head,
749                              struct fl_flow_mask *mask)
750 {
751         head->ht_params = fl_ht_params;
752         head->ht_params.key_len = fl_mask_range(mask);
753         head->ht_params.key_offset += mask->range.start;
754
755         return rhashtable_init(&head->ht, &head->ht_params);
756 }
757
758 #define FL_KEY_MEMBER_OFFSET(member) offsetof(struct fl_flow_key, member)
759 #define FL_KEY_MEMBER_SIZE(member) (sizeof(((struct fl_flow_key *) 0)->member))
760
761 #define FL_KEY_IS_MASKED(mask, member)                                          \
762         memchr_inv(((char *)mask) + FL_KEY_MEMBER_OFFSET(member),               \
763                    0, FL_KEY_MEMBER_SIZE(member))                               \
764
765 #define FL_KEY_SET(keys, cnt, id, member)                                       \
766         do {                                                                    \
767                 keys[cnt].key_id = id;                                          \
768                 keys[cnt].offset = FL_KEY_MEMBER_OFFSET(member);                \
769                 cnt++;                                                          \
770         } while(0);
771
772 #define FL_KEY_SET_IF_MASKED(mask, keys, cnt, id, member)                       \
773         do {                                                                    \
774                 if (FL_KEY_IS_MASKED(mask, member))                             \
775                         FL_KEY_SET(keys, cnt, id, member);                      \
776         } while(0);
777
778 static void fl_init_dissector(struct cls_fl_head *head,
779                               struct fl_flow_mask *mask)
780 {
781         struct flow_dissector_key keys[FLOW_DISSECTOR_KEY_MAX];
782         size_t cnt = 0;
783
784         FL_KEY_SET(keys, cnt, FLOW_DISSECTOR_KEY_CONTROL, control);
785         FL_KEY_SET(keys, cnt, FLOW_DISSECTOR_KEY_BASIC, basic);
786         FL_KEY_SET_IF_MASKED(&mask->key, keys, cnt,
787                              FLOW_DISSECTOR_KEY_ETH_ADDRS, eth);
788         FL_KEY_SET_IF_MASKED(&mask->key, keys, cnt,
789                              FLOW_DISSECTOR_KEY_IPV4_ADDRS, ipv4);
790         FL_KEY_SET_IF_MASKED(&mask->key, keys, cnt,
791                              FLOW_DISSECTOR_KEY_IPV6_ADDRS, ipv6);
792         FL_KEY_SET_IF_MASKED(&mask->key, keys, cnt,
793                              FLOW_DISSECTOR_KEY_PORTS, tp);
794         FL_KEY_SET_IF_MASKED(&mask->key, keys, cnt,
795                              FLOW_DISSECTOR_KEY_IP, ip);
796         FL_KEY_SET_IF_MASKED(&mask->key, keys, cnt,
797                              FLOW_DISSECTOR_KEY_TCP, tcp);
798         FL_KEY_SET_IF_MASKED(&mask->key, keys, cnt,
799                              FLOW_DISSECTOR_KEY_ICMP, icmp);
800         FL_KEY_SET_IF_MASKED(&mask->key, keys, cnt,
801                              FLOW_DISSECTOR_KEY_ARP, arp);
802         FL_KEY_SET_IF_MASKED(&mask->key, keys, cnt,
803                              FLOW_DISSECTOR_KEY_MPLS, mpls);
804         FL_KEY_SET_IF_MASKED(&mask->key, keys, cnt,
805                              FLOW_DISSECTOR_KEY_VLAN, vlan);
806         FL_KEY_SET_IF_MASKED(&mask->key, keys, cnt,
807                              FLOW_DISSECTOR_KEY_ENC_KEYID, enc_key_id);
808         FL_KEY_SET_IF_MASKED(&mask->key, keys, cnt,
809                              FLOW_DISSECTOR_KEY_ENC_IPV4_ADDRS, enc_ipv4);
810         FL_KEY_SET_IF_MASKED(&mask->key, keys, cnt,
811                              FLOW_DISSECTOR_KEY_ENC_IPV6_ADDRS, enc_ipv6);
812         if (FL_KEY_IS_MASKED(&mask->key, enc_ipv4) ||
813             FL_KEY_IS_MASKED(&mask->key, enc_ipv6))
814                 FL_KEY_SET(keys, cnt, FLOW_DISSECTOR_KEY_ENC_CONTROL,
815                            enc_control);
816         FL_KEY_SET_IF_MASKED(&mask->key, keys, cnt,
817                              FLOW_DISSECTOR_KEY_ENC_PORTS, enc_tp);
818
819         skb_flow_dissector_init(&head->dissector, keys, cnt);
820 }
821
822 static int fl_check_assign_mask(struct cls_fl_head *head,
823                                 struct fl_flow_mask *mask)
824 {
825         int err;
826
827         if (head->mask_assigned) {
828                 if (!fl_mask_eq(&head->mask, mask))
829                         return -EINVAL;
830                 else
831                         return 0;
832         }
833
834         /* Mask is not assigned yet. So assign it and init hashtable
835          * according to that.
836          */
837         err = fl_init_hashtable(head, mask);
838         if (err)
839                 return err;
840         memcpy(&head->mask, mask, sizeof(head->mask));
841         head->mask_assigned = true;
842
843         fl_init_dissector(head, mask);
844
845         return 0;
846 }
847
848 static int fl_set_parms(struct net *net, struct tcf_proto *tp,
849                         struct cls_fl_filter *f, struct fl_flow_mask *mask,
850                         unsigned long base, struct nlattr **tb,
851                         struct nlattr *est, bool ovr)
852 {
853         struct tcf_exts e;
854         int err;
855
856         err = tcf_exts_init(&e, TCA_FLOWER_ACT, 0);
857         if (err < 0)
858                 return err;
859         err = tcf_exts_validate(net, tp, tb, est, &e, ovr);
860         if (err < 0)
861                 goto errout;
862
863         if (tb[TCA_FLOWER_CLASSID]) {
864                 f->res.classid = nla_get_u32(tb[TCA_FLOWER_CLASSID]);
865                 tcf_bind_filter(tp, &f->res, base);
866         }
867
868         err = fl_set_key(net, tb, &f->key, &mask->key);
869         if (err)
870                 goto errout;
871
872         fl_mask_update_range(mask);
873         fl_set_masked_key(&f->mkey, &f->key, mask);
874
875         tcf_exts_change(tp, &f->exts, &e);
876
877         return 0;
878 errout:
879         tcf_exts_destroy(&e);
880         return err;
881 }
882
883 static u32 fl_grab_new_handle(struct tcf_proto *tp,
884                               struct cls_fl_head *head)
885 {
886         unsigned int i = 0x80000000;
887         u32 handle;
888
889         do {
890                 if (++head->hgen == 0x7FFFFFFF)
891                         head->hgen = 1;
892         } while (--i > 0 && fl_get(tp, head->hgen));
893
894         if (unlikely(i == 0)) {
895                 pr_err("Insufficient number of handles\n");
896                 handle = 0;
897         } else {
898                 handle = head->hgen;
899         }
900
901         return handle;
902 }
903
904 static int fl_change(struct net *net, struct sk_buff *in_skb,
905                      struct tcf_proto *tp, unsigned long base,
906                      u32 handle, struct nlattr **tca,
907                      unsigned long *arg, bool ovr)
908 {
909         struct cls_fl_head *head = rtnl_dereference(tp->root);
910         struct cls_fl_filter *fold = (struct cls_fl_filter *) *arg;
911         struct cls_fl_filter *fnew;
912         struct nlattr **tb;
913         struct fl_flow_mask mask = {};
914         int err;
915
916         if (!tca[TCA_OPTIONS])
917                 return -EINVAL;
918
919         tb = kcalloc(TCA_FLOWER_MAX + 1, sizeof(struct nlattr *), GFP_KERNEL);
920         if (!tb)
921                 return -ENOBUFS;
922
923         err = nla_parse_nested(tb, TCA_FLOWER_MAX, tca[TCA_OPTIONS],
924                                fl_policy, NULL);
925         if (err < 0)
926                 goto errout_tb;
927
928         if (fold && handle && fold->handle != handle) {
929                 err = -EINVAL;
930                 goto errout_tb;
931         }
932
933         fnew = kzalloc(sizeof(*fnew), GFP_KERNEL);
934         if (!fnew) {
935                 err = -ENOBUFS;
936                 goto errout_tb;
937         }
938
939         err = tcf_exts_init(&fnew->exts, TCA_FLOWER_ACT, 0);
940         if (err < 0)
941                 goto errout;
942
943         if (!handle) {
944                 handle = fl_grab_new_handle(tp, head);
945                 if (!handle) {
946                         err = -EINVAL;
947                         goto errout;
948                 }
949         }
950         fnew->handle = handle;
951
952         if (tb[TCA_FLOWER_FLAGS]) {
953                 fnew->flags = nla_get_u32(tb[TCA_FLOWER_FLAGS]);
954
955                 if (!tc_flags_valid(fnew->flags)) {
956                         err = -EINVAL;
957                         goto errout;
958                 }
959         }
960
961         err = fl_set_parms(net, tp, fnew, &mask, base, tb, tca[TCA_RATE], ovr);
962         if (err)
963                 goto errout;
964
965         err = fl_check_assign_mask(head, &mask);
966         if (err)
967                 goto errout;
968
969         if (!tc_skip_sw(fnew->flags)) {
970                 if (!fold && fl_lookup(head, &fnew->mkey)) {
971                         err = -EEXIST;
972                         goto errout;
973                 }
974
975                 err = rhashtable_insert_fast(&head->ht, &fnew->ht_node,
976                                              head->ht_params);
977                 if (err)
978                         goto errout;
979         }
980
981         if (!tc_skip_hw(fnew->flags)) {
982                 err = fl_hw_replace_filter(tp,
983                                            &head->dissector,
984                                            &mask.key,
985                                            fnew);
986                 if (err)
987                         goto errout;
988         }
989
990         if (!tc_in_hw(fnew->flags))
991                 fnew->flags |= TCA_CLS_FLAGS_NOT_IN_HW;
992
993         if (fold) {
994                 if (!tc_skip_sw(fold->flags))
995                         rhashtable_remove_fast(&head->ht, &fold->ht_node,
996                                                head->ht_params);
997                 if (!tc_skip_hw(fold->flags))
998                         fl_hw_destroy_filter(tp, fold);
999         }
1000
1001         *arg = (unsigned long) fnew;
1002
1003         if (fold) {
1004                 list_replace_rcu(&fold->list, &fnew->list);
1005                 tcf_unbind_filter(tp, &fold->res);
1006                 call_rcu(&fold->rcu, fl_destroy_filter);
1007         } else {
1008                 list_add_tail_rcu(&fnew->list, &head->filters);
1009         }
1010
1011         kfree(tb);
1012         return 0;
1013
1014 errout:
1015         tcf_exts_destroy(&fnew->exts);
1016         kfree(fnew);
1017 errout_tb:
1018         kfree(tb);
1019         return err;
1020 }
1021
1022 static int fl_delete(struct tcf_proto *tp, unsigned long arg, bool *last)
1023 {
1024         struct cls_fl_head *head = rtnl_dereference(tp->root);
1025         struct cls_fl_filter *f = (struct cls_fl_filter *) arg;
1026
1027         if (!tc_skip_sw(f->flags))
1028                 rhashtable_remove_fast(&head->ht, &f->ht_node,
1029                                        head->ht_params);
1030         __fl_delete(tp, f);
1031         *last = list_empty(&head->filters);
1032         return 0;
1033 }
1034
1035 static void fl_walk(struct tcf_proto *tp, struct tcf_walker *arg)
1036 {
1037         struct cls_fl_head *head = rtnl_dereference(tp->root);
1038         struct cls_fl_filter *f;
1039
1040         list_for_each_entry_rcu(f, &head->filters, list) {
1041                 if (arg->count < arg->skip)
1042                         goto skip;
1043                 if (arg->fn(tp, (unsigned long) f, arg) < 0) {
1044                         arg->stop = 1;
1045                         break;
1046                 }
1047 skip:
1048                 arg->count++;
1049         }
1050 }
1051
1052 static int fl_dump_key_val(struct sk_buff *skb,
1053                            void *val, int val_type,
1054                            void *mask, int mask_type, int len)
1055 {
1056         int err;
1057
1058         if (!memchr_inv(mask, 0, len))
1059                 return 0;
1060         err = nla_put(skb, val_type, len, val);
1061         if (err)
1062                 return err;
1063         if (mask_type != TCA_FLOWER_UNSPEC) {
1064                 err = nla_put(skb, mask_type, len, mask);
1065                 if (err)
1066                         return err;
1067         }
1068         return 0;
1069 }
1070
1071 static int fl_dump_key_mpls(struct sk_buff *skb,
1072                             struct flow_dissector_key_mpls *mpls_key,
1073                             struct flow_dissector_key_mpls *mpls_mask)
1074 {
1075         int err;
1076
1077         if (!memchr_inv(mpls_mask, 0, sizeof(*mpls_mask)))
1078                 return 0;
1079         if (mpls_mask->mpls_ttl) {
1080                 err = nla_put_u8(skb, TCA_FLOWER_KEY_MPLS_TTL,
1081                                  mpls_key->mpls_ttl);
1082                 if (err)
1083                         return err;
1084         }
1085         if (mpls_mask->mpls_tc) {
1086                 err = nla_put_u8(skb, TCA_FLOWER_KEY_MPLS_TC,
1087                                  mpls_key->mpls_tc);
1088                 if (err)
1089                         return err;
1090         }
1091         if (mpls_mask->mpls_label) {
1092                 err = nla_put_u32(skb, TCA_FLOWER_KEY_MPLS_LABEL,
1093                                   mpls_key->mpls_label);
1094                 if (err)
1095                         return err;
1096         }
1097         if (mpls_mask->mpls_bos) {
1098                 err = nla_put_u8(skb, TCA_FLOWER_KEY_MPLS_BOS,
1099                                  mpls_key->mpls_bos);
1100                 if (err)
1101                         return err;
1102         }
1103         return 0;
1104 }
1105
1106 static int fl_dump_key_ip(struct sk_buff *skb,
1107                           struct flow_dissector_key_ip *key,
1108                           struct flow_dissector_key_ip *mask)
1109 {
1110         if (fl_dump_key_val(skb, &key->tos, TCA_FLOWER_KEY_IP_TOS, &mask->tos,
1111                             TCA_FLOWER_KEY_IP_TOS_MASK, sizeof(key->tos)) ||
1112             fl_dump_key_val(skb, &key->ttl, TCA_FLOWER_KEY_IP_TTL, &mask->ttl,
1113                             TCA_FLOWER_KEY_IP_TTL_MASK, sizeof(key->ttl)))
1114                 return -1;
1115
1116         return 0;
1117 }
1118
1119 static int fl_dump_key_vlan(struct sk_buff *skb,
1120                             struct flow_dissector_key_vlan *vlan_key,
1121                             struct flow_dissector_key_vlan *vlan_mask)
1122 {
1123         int err;
1124
1125         if (!memchr_inv(vlan_mask, 0, sizeof(*vlan_mask)))
1126                 return 0;
1127         if (vlan_mask->vlan_id) {
1128                 err = nla_put_u16(skb, TCA_FLOWER_KEY_VLAN_ID,
1129                                   vlan_key->vlan_id);
1130                 if (err)
1131                         return err;
1132         }
1133         if (vlan_mask->vlan_priority) {
1134                 err = nla_put_u8(skb, TCA_FLOWER_KEY_VLAN_PRIO,
1135                                  vlan_key->vlan_priority);
1136                 if (err)
1137                         return err;
1138         }
1139         return 0;
1140 }
1141
1142 static void fl_get_key_flag(u32 dissector_key, u32 dissector_mask,
1143                             u32 *flower_key, u32 *flower_mask,
1144                             u32 flower_flag_bit, u32 dissector_flag_bit)
1145 {
1146         if (dissector_mask & dissector_flag_bit) {
1147                 *flower_mask |= flower_flag_bit;
1148                 if (dissector_key & dissector_flag_bit)
1149                         *flower_key |= flower_flag_bit;
1150         }
1151 }
1152
1153 static int fl_dump_key_flags(struct sk_buff *skb, u32 flags_key, u32 flags_mask)
1154 {
1155         u32 key, mask;
1156         __be32 _key, _mask;
1157         int err;
1158
1159         if (!memchr_inv(&flags_mask, 0, sizeof(flags_mask)))
1160                 return 0;
1161
1162         key = 0;
1163         mask = 0;
1164
1165         fl_get_key_flag(flags_key, flags_mask, &key, &mask,
1166                         TCA_FLOWER_KEY_FLAGS_IS_FRAGMENT, FLOW_DIS_IS_FRAGMENT);
1167
1168         _key = cpu_to_be32(key);
1169         _mask = cpu_to_be32(mask);
1170
1171         err = nla_put(skb, TCA_FLOWER_KEY_FLAGS, 4, &_key);
1172         if (err)
1173                 return err;
1174
1175         return nla_put(skb, TCA_FLOWER_KEY_FLAGS_MASK, 4, &_mask);
1176 }
1177
1178 static int fl_dump(struct net *net, struct tcf_proto *tp, unsigned long fh,
1179                    struct sk_buff *skb, struct tcmsg *t)
1180 {
1181         struct cls_fl_head *head = rtnl_dereference(tp->root);
1182         struct cls_fl_filter *f = (struct cls_fl_filter *) fh;
1183         struct nlattr *nest;
1184         struct fl_flow_key *key, *mask;
1185
1186         if (!f)
1187                 return skb->len;
1188
1189         t->tcm_handle = f->handle;
1190
1191         nest = nla_nest_start(skb, TCA_OPTIONS);
1192         if (!nest)
1193                 goto nla_put_failure;
1194
1195         if (f->res.classid &&
1196             nla_put_u32(skb, TCA_FLOWER_CLASSID, f->res.classid))
1197                 goto nla_put_failure;
1198
1199         key = &f->key;
1200         mask = &head->mask.key;
1201
1202         if (mask->indev_ifindex) {
1203                 struct net_device *dev;
1204
1205                 dev = __dev_get_by_index(net, key->indev_ifindex);
1206                 if (dev && nla_put_string(skb, TCA_FLOWER_INDEV, dev->name))
1207                         goto nla_put_failure;
1208         }
1209
1210         if (!tc_skip_hw(f->flags))
1211                 fl_hw_update_stats(tp, f);
1212
1213         if (fl_dump_key_val(skb, key->eth.dst, TCA_FLOWER_KEY_ETH_DST,
1214                             mask->eth.dst, TCA_FLOWER_KEY_ETH_DST_MASK,
1215                             sizeof(key->eth.dst)) ||
1216             fl_dump_key_val(skb, key->eth.src, TCA_FLOWER_KEY_ETH_SRC,
1217                             mask->eth.src, TCA_FLOWER_KEY_ETH_SRC_MASK,
1218                             sizeof(key->eth.src)) ||
1219             fl_dump_key_val(skb, &key->basic.n_proto, TCA_FLOWER_KEY_ETH_TYPE,
1220                             &mask->basic.n_proto, TCA_FLOWER_UNSPEC,
1221                             sizeof(key->basic.n_proto)))
1222                 goto nla_put_failure;
1223
1224         if (fl_dump_key_mpls(skb, &key->mpls, &mask->mpls))
1225                 goto nla_put_failure;
1226
1227         if (fl_dump_key_vlan(skb, &key->vlan, &mask->vlan))
1228                 goto nla_put_failure;
1229
1230         if ((key->basic.n_proto == htons(ETH_P_IP) ||
1231              key->basic.n_proto == htons(ETH_P_IPV6)) &&
1232             (fl_dump_key_val(skb, &key->basic.ip_proto, TCA_FLOWER_KEY_IP_PROTO,
1233                             &mask->basic.ip_proto, TCA_FLOWER_UNSPEC,
1234                             sizeof(key->basic.ip_proto)) ||
1235             fl_dump_key_ip(skb, &key->ip, &mask->ip)))
1236                 goto nla_put_failure;
1237
1238         if (key->control.addr_type == FLOW_DISSECTOR_KEY_IPV4_ADDRS &&
1239             (fl_dump_key_val(skb, &key->ipv4.src, TCA_FLOWER_KEY_IPV4_SRC,
1240                              &mask->ipv4.src, TCA_FLOWER_KEY_IPV4_SRC_MASK,
1241                              sizeof(key->ipv4.src)) ||
1242              fl_dump_key_val(skb, &key->ipv4.dst, TCA_FLOWER_KEY_IPV4_DST,
1243                              &mask->ipv4.dst, TCA_FLOWER_KEY_IPV4_DST_MASK,
1244                              sizeof(key->ipv4.dst))))
1245                 goto nla_put_failure;
1246         else if (key->control.addr_type == FLOW_DISSECTOR_KEY_IPV6_ADDRS &&
1247                  (fl_dump_key_val(skb, &key->ipv6.src, TCA_FLOWER_KEY_IPV6_SRC,
1248                                   &mask->ipv6.src, TCA_FLOWER_KEY_IPV6_SRC_MASK,
1249                                   sizeof(key->ipv6.src)) ||
1250                   fl_dump_key_val(skb, &key->ipv6.dst, TCA_FLOWER_KEY_IPV6_DST,
1251                                   &mask->ipv6.dst, TCA_FLOWER_KEY_IPV6_DST_MASK,
1252                                   sizeof(key->ipv6.dst))))
1253                 goto nla_put_failure;
1254
1255         if (key->basic.ip_proto == IPPROTO_TCP &&
1256             (fl_dump_key_val(skb, &key->tp.src, TCA_FLOWER_KEY_TCP_SRC,
1257                              &mask->tp.src, TCA_FLOWER_KEY_TCP_SRC_MASK,
1258                              sizeof(key->tp.src)) ||
1259              fl_dump_key_val(skb, &key->tp.dst, TCA_FLOWER_KEY_TCP_DST,
1260                              &mask->tp.dst, TCA_FLOWER_KEY_TCP_DST_MASK,
1261                              sizeof(key->tp.dst)) ||
1262              fl_dump_key_val(skb, &key->tcp.flags, TCA_FLOWER_KEY_TCP_FLAGS,
1263                              &mask->tcp.flags, TCA_FLOWER_KEY_TCP_FLAGS_MASK,
1264                              sizeof(key->tcp.flags))))
1265                 goto nla_put_failure;
1266         else if (key->basic.ip_proto == IPPROTO_UDP &&
1267                  (fl_dump_key_val(skb, &key->tp.src, TCA_FLOWER_KEY_UDP_SRC,
1268                                   &mask->tp.src, TCA_FLOWER_KEY_UDP_SRC_MASK,
1269                                   sizeof(key->tp.src)) ||
1270                   fl_dump_key_val(skb, &key->tp.dst, TCA_FLOWER_KEY_UDP_DST,
1271                                   &mask->tp.dst, TCA_FLOWER_KEY_UDP_DST_MASK,
1272                                   sizeof(key->tp.dst))))
1273                 goto nla_put_failure;
1274         else if (key->basic.ip_proto == IPPROTO_SCTP &&
1275                  (fl_dump_key_val(skb, &key->tp.src, TCA_FLOWER_KEY_SCTP_SRC,
1276                                   &mask->tp.src, TCA_FLOWER_KEY_SCTP_SRC_MASK,
1277                                   sizeof(key->tp.src)) ||
1278                   fl_dump_key_val(skb, &key->tp.dst, TCA_FLOWER_KEY_SCTP_DST,
1279                                   &mask->tp.dst, TCA_FLOWER_KEY_SCTP_DST_MASK,
1280                                   sizeof(key->tp.dst))))
1281                 goto nla_put_failure;
1282         else if (key->basic.n_proto == htons(ETH_P_IP) &&
1283                  key->basic.ip_proto == IPPROTO_ICMP &&
1284                  (fl_dump_key_val(skb, &key->icmp.type,
1285                                   TCA_FLOWER_KEY_ICMPV4_TYPE, &mask->icmp.type,
1286                                   TCA_FLOWER_KEY_ICMPV4_TYPE_MASK,
1287                                   sizeof(key->icmp.type)) ||
1288                   fl_dump_key_val(skb, &key->icmp.code,
1289                                   TCA_FLOWER_KEY_ICMPV4_CODE, &mask->icmp.code,
1290                                   TCA_FLOWER_KEY_ICMPV4_CODE_MASK,
1291                                   sizeof(key->icmp.code))))
1292                 goto nla_put_failure;
1293         else if (key->basic.n_proto == htons(ETH_P_IPV6) &&
1294                  key->basic.ip_proto == IPPROTO_ICMPV6 &&
1295                  (fl_dump_key_val(skb, &key->icmp.type,
1296                                   TCA_FLOWER_KEY_ICMPV6_TYPE, &mask->icmp.type,
1297                                   TCA_FLOWER_KEY_ICMPV6_TYPE_MASK,
1298                                   sizeof(key->icmp.type)) ||
1299                   fl_dump_key_val(skb, &key->icmp.code,
1300                                   TCA_FLOWER_KEY_ICMPV6_CODE, &mask->icmp.code,
1301                                   TCA_FLOWER_KEY_ICMPV6_CODE_MASK,
1302                                   sizeof(key->icmp.code))))
1303                 goto nla_put_failure;
1304         else if ((key->basic.n_proto == htons(ETH_P_ARP) ||
1305                   key->basic.n_proto == htons(ETH_P_RARP)) &&
1306                  (fl_dump_key_val(skb, &key->arp.sip,
1307                                   TCA_FLOWER_KEY_ARP_SIP, &mask->arp.sip,
1308                                   TCA_FLOWER_KEY_ARP_SIP_MASK,
1309                                   sizeof(key->arp.sip)) ||
1310                   fl_dump_key_val(skb, &key->arp.tip,
1311                                   TCA_FLOWER_KEY_ARP_TIP, &mask->arp.tip,
1312                                   TCA_FLOWER_KEY_ARP_TIP_MASK,
1313                                   sizeof(key->arp.tip)) ||
1314                   fl_dump_key_val(skb, &key->arp.op,
1315                                   TCA_FLOWER_KEY_ARP_OP, &mask->arp.op,
1316                                   TCA_FLOWER_KEY_ARP_OP_MASK,
1317                                   sizeof(key->arp.op)) ||
1318                   fl_dump_key_val(skb, key->arp.sha, TCA_FLOWER_KEY_ARP_SHA,
1319                                   mask->arp.sha, TCA_FLOWER_KEY_ARP_SHA_MASK,
1320                                   sizeof(key->arp.sha)) ||
1321                   fl_dump_key_val(skb, key->arp.tha, TCA_FLOWER_KEY_ARP_THA,
1322                                   mask->arp.tha, TCA_FLOWER_KEY_ARP_THA_MASK,
1323                                   sizeof(key->arp.tha))))
1324                 goto nla_put_failure;
1325
1326         if (key->enc_control.addr_type == FLOW_DISSECTOR_KEY_IPV4_ADDRS &&
1327             (fl_dump_key_val(skb, &key->enc_ipv4.src,
1328                             TCA_FLOWER_KEY_ENC_IPV4_SRC, &mask->enc_ipv4.src,
1329                             TCA_FLOWER_KEY_ENC_IPV4_SRC_MASK,
1330                             sizeof(key->enc_ipv4.src)) ||
1331              fl_dump_key_val(skb, &key->enc_ipv4.dst,
1332                              TCA_FLOWER_KEY_ENC_IPV4_DST, &mask->enc_ipv4.dst,
1333                              TCA_FLOWER_KEY_ENC_IPV4_DST_MASK,
1334                              sizeof(key->enc_ipv4.dst))))
1335                 goto nla_put_failure;
1336         else if (key->enc_control.addr_type == FLOW_DISSECTOR_KEY_IPV6_ADDRS &&
1337                  (fl_dump_key_val(skb, &key->enc_ipv6.src,
1338                             TCA_FLOWER_KEY_ENC_IPV6_SRC, &mask->enc_ipv6.src,
1339                             TCA_FLOWER_KEY_ENC_IPV6_SRC_MASK,
1340                             sizeof(key->enc_ipv6.src)) ||
1341                  fl_dump_key_val(skb, &key->enc_ipv6.dst,
1342                                  TCA_FLOWER_KEY_ENC_IPV6_DST,
1343                                  &mask->enc_ipv6.dst,
1344                                  TCA_FLOWER_KEY_ENC_IPV6_DST_MASK,
1345                             sizeof(key->enc_ipv6.dst))))
1346                 goto nla_put_failure;
1347
1348         if (fl_dump_key_val(skb, &key->enc_key_id, TCA_FLOWER_KEY_ENC_KEY_ID,
1349                             &mask->enc_key_id, TCA_FLOWER_UNSPEC,
1350                             sizeof(key->enc_key_id)) ||
1351             fl_dump_key_val(skb, &key->enc_tp.src,
1352                             TCA_FLOWER_KEY_ENC_UDP_SRC_PORT,
1353                             &mask->enc_tp.src,
1354                             TCA_FLOWER_KEY_ENC_UDP_SRC_PORT_MASK,
1355                             sizeof(key->enc_tp.src)) ||
1356             fl_dump_key_val(skb, &key->enc_tp.dst,
1357                             TCA_FLOWER_KEY_ENC_UDP_DST_PORT,
1358                             &mask->enc_tp.dst,
1359                             TCA_FLOWER_KEY_ENC_UDP_DST_PORT_MASK,
1360                             sizeof(key->enc_tp.dst)))
1361                 goto nla_put_failure;
1362
1363         if (fl_dump_key_flags(skb, key->control.flags, mask->control.flags))
1364                 goto nla_put_failure;
1365
1366         if (f->flags && nla_put_u32(skb, TCA_FLOWER_FLAGS, f->flags))
1367                 goto nla_put_failure;
1368
1369         if (tcf_exts_dump(skb, &f->exts))
1370                 goto nla_put_failure;
1371
1372         nla_nest_end(skb, nest);
1373
1374         if (tcf_exts_dump_stats(skb, &f->exts) < 0)
1375                 goto nla_put_failure;
1376
1377         return skb->len;
1378
1379 nla_put_failure:
1380         nla_nest_cancel(skb, nest);
1381         return -1;
1382 }
1383
1384 static struct tcf_proto_ops cls_fl_ops __read_mostly = {
1385         .kind           = "flower",
1386         .classify       = fl_classify,
1387         .init           = fl_init,
1388         .destroy        = fl_destroy,
1389         .get            = fl_get,
1390         .change         = fl_change,
1391         .delete         = fl_delete,
1392         .walk           = fl_walk,
1393         .dump           = fl_dump,
1394         .owner          = THIS_MODULE,
1395 };
1396
1397 static int __init cls_fl_init(void)
1398 {
1399         return register_tcf_proto_ops(&cls_fl_ops);
1400 }
1401
1402 static void __exit cls_fl_exit(void)
1403 {
1404         unregister_tcf_proto_ops(&cls_fl_ops);
1405 }
1406
1407 module_init(cls_fl_init);
1408 module_exit(cls_fl_exit);
1409
1410 MODULE_AUTHOR("Jiri Pirko <jiri@resnulli.us>");
1411 MODULE_DESCRIPTION("Flower classifier");
1412 MODULE_LICENSE("GPL v2");