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