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