xref: /linux/drivers/net/amt.c (revision 314f14abdeca78de6b16f97d796a9966ce4b90ae)
1 // SPDX-License-Identifier: GPL-2.0-or-later
2 /* Copyright (c) 2021 Taehee Yoo <ap420073@gmail.com> */
3 
4 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
5 
6 #include <linux/module.h>
7 #include <linux/skbuff.h>
8 #include <linux/udp.h>
9 #include <linux/jhash.h>
10 #include <linux/if_tunnel.h>
11 #include <linux/net.h>
12 #include <linux/igmp.h>
13 #include <linux/workqueue.h>
14 #include <net/net_namespace.h>
15 #include <net/protocol.h>
16 #include <net/ip.h>
17 #include <net/udp.h>
18 #include <net/udp_tunnel.h>
19 #include <net/icmp.h>
20 #include <net/mld.h>
21 #include <net/amt.h>
22 #include <uapi/linux/amt.h>
23 #include <linux/security.h>
24 #include <net/gro_cells.h>
25 #include <net/ipv6.h>
26 #include <net/protocol.h>
27 #include <net/if_inet6.h>
28 #include <net/ndisc.h>
29 #include <net/addrconf.h>
30 #include <net/ip6_route.h>
31 #include <net/inet_common.h>
32 #include <net/ip6_checksum.h>
33 
34 static struct workqueue_struct *amt_wq;
35 
36 static HLIST_HEAD(source_gc_list);
37 /* Lock for source_gc_list */
38 static spinlock_t source_gc_lock;
39 static struct delayed_work source_gc_wq;
40 static char *status_str[] = {
41 	"AMT_STATUS_INIT",
42 	"AMT_STATUS_SENT_DISCOVERY",
43 	"AMT_STATUS_RECEIVED_DISCOVERY",
44 	"AMT_STATUS_SENT_ADVERTISEMENT",
45 	"AMT_STATUS_RECEIVED_ADVERTISEMENT",
46 	"AMT_STATUS_SENT_REQUEST",
47 	"AMT_STATUS_RECEIVED_REQUEST",
48 	"AMT_STATUS_SENT_QUERY",
49 	"AMT_STATUS_RECEIVED_QUERY",
50 	"AMT_STATUS_SENT_UPDATE",
51 	"AMT_STATUS_RECEIVED_UPDATE",
52 };
53 
54 static char *type_str[] = {
55 	"AMT_MSG_DISCOVERY",
56 	"AMT_MSG_ADVERTISEMENT",
57 	"AMT_MSG_REQUEST",
58 	"AMT_MSG_MEMBERSHIP_QUERY",
59 	"AMT_MSG_MEMBERSHIP_UPDATE",
60 	"AMT_MSG_MULTICAST_DATA",
61 	"AMT_MSG_TEARDOWM",
62 };
63 
64 static char *action_str[] = {
65 	"AMT_ACT_GMI",
66 	"AMT_ACT_GMI_ZERO",
67 	"AMT_ACT_GT",
68 	"AMT_ACT_STATUS_FWD_NEW",
69 	"AMT_ACT_STATUS_D_FWD_NEW",
70 	"AMT_ACT_STATUS_NONE_NEW",
71 };
72 
73 static struct igmpv3_grec igmpv3_zero_grec;
74 
75 #if IS_ENABLED(CONFIG_IPV6)
76 #define MLD2_ALL_NODE_INIT { { { 0xff, 0x02, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0x01 } } }
77 static struct in6_addr mld2_all_node = MLD2_ALL_NODE_INIT;
78 static struct mld2_grec mldv2_zero_grec;
79 #endif
80 
81 static struct amt_skb_cb *amt_skb_cb(struct sk_buff *skb)
82 {
83 	BUILD_BUG_ON(sizeof(struct amt_skb_cb) + sizeof(struct qdisc_skb_cb) >
84 		     sizeof_field(struct sk_buff, cb));
85 
86 	return (struct amt_skb_cb *)((void *)skb->cb +
87 		sizeof(struct qdisc_skb_cb));
88 }
89 
90 static void __amt_source_gc_work(void)
91 {
92 	struct amt_source_node *snode;
93 	struct hlist_head gc_list;
94 	struct hlist_node *t;
95 
96 	spin_lock_bh(&source_gc_lock);
97 	hlist_move_list(&source_gc_list, &gc_list);
98 	spin_unlock_bh(&source_gc_lock);
99 
100 	hlist_for_each_entry_safe(snode, t, &gc_list, node) {
101 		hlist_del_rcu(&snode->node);
102 		kfree_rcu(snode, rcu);
103 	}
104 }
105 
106 static void amt_source_gc_work(struct work_struct *work)
107 {
108 	__amt_source_gc_work();
109 
110 	spin_lock_bh(&source_gc_lock);
111 	mod_delayed_work(amt_wq, &source_gc_wq,
112 			 msecs_to_jiffies(AMT_GC_INTERVAL));
113 	spin_unlock_bh(&source_gc_lock);
114 }
115 
116 static bool amt_addr_equal(union amt_addr *a, union amt_addr *b)
117 {
118 	return !memcmp(a, b, sizeof(union amt_addr));
119 }
120 
121 static u32 amt_source_hash(struct amt_tunnel_list *tunnel, union amt_addr *src)
122 {
123 	u32 hash = jhash(src, sizeof(*src), tunnel->amt->hash_seed);
124 
125 	return reciprocal_scale(hash, tunnel->amt->hash_buckets);
126 }
127 
128 static bool amt_status_filter(struct amt_source_node *snode,
129 			      enum amt_filter filter)
130 {
131 	bool rc = false;
132 
133 	switch (filter) {
134 	case AMT_FILTER_FWD:
135 		if (snode->status == AMT_SOURCE_STATUS_FWD &&
136 		    snode->flags == AMT_SOURCE_OLD)
137 			rc = true;
138 		break;
139 	case AMT_FILTER_D_FWD:
140 		if (snode->status == AMT_SOURCE_STATUS_D_FWD &&
141 		    snode->flags == AMT_SOURCE_OLD)
142 			rc = true;
143 		break;
144 	case AMT_FILTER_FWD_NEW:
145 		if (snode->status == AMT_SOURCE_STATUS_FWD &&
146 		    snode->flags == AMT_SOURCE_NEW)
147 			rc = true;
148 		break;
149 	case AMT_FILTER_D_FWD_NEW:
150 		if (snode->status == AMT_SOURCE_STATUS_D_FWD &&
151 		    snode->flags == AMT_SOURCE_NEW)
152 			rc = true;
153 		break;
154 	case AMT_FILTER_ALL:
155 		rc = true;
156 		break;
157 	case AMT_FILTER_NONE_NEW:
158 		if (snode->status == AMT_SOURCE_STATUS_NONE &&
159 		    snode->flags == AMT_SOURCE_NEW)
160 			rc = true;
161 		break;
162 	case AMT_FILTER_BOTH:
163 		if ((snode->status == AMT_SOURCE_STATUS_D_FWD ||
164 		     snode->status == AMT_SOURCE_STATUS_FWD) &&
165 		    snode->flags == AMT_SOURCE_OLD)
166 			rc = true;
167 		break;
168 	case AMT_FILTER_BOTH_NEW:
169 		if ((snode->status == AMT_SOURCE_STATUS_D_FWD ||
170 		     snode->status == AMT_SOURCE_STATUS_FWD) &&
171 		    snode->flags == AMT_SOURCE_NEW)
172 			rc = true;
173 		break;
174 	default:
175 		WARN_ON_ONCE(1);
176 		break;
177 	}
178 
179 	return rc;
180 }
181 
182 static struct amt_source_node *amt_lookup_src(struct amt_tunnel_list *tunnel,
183 					      struct amt_group_node *gnode,
184 					      enum amt_filter filter,
185 					      union amt_addr *src)
186 {
187 	u32 hash = amt_source_hash(tunnel, src);
188 	struct amt_source_node *snode;
189 
190 	hlist_for_each_entry_rcu(snode, &gnode->sources[hash], node)
191 		if (amt_status_filter(snode, filter) &&
192 		    amt_addr_equal(&snode->source_addr, src))
193 			return snode;
194 
195 	return NULL;
196 }
197 
198 static u32 amt_group_hash(struct amt_tunnel_list *tunnel, union amt_addr *group)
199 {
200 	u32 hash = jhash(group, sizeof(*group), tunnel->amt->hash_seed);
201 
202 	return reciprocal_scale(hash, tunnel->amt->hash_buckets);
203 }
204 
205 static struct amt_group_node *amt_lookup_group(struct amt_tunnel_list *tunnel,
206 					       union amt_addr *group,
207 					       union amt_addr *host,
208 					       bool v6)
209 {
210 	u32 hash = amt_group_hash(tunnel, group);
211 	struct amt_group_node *gnode;
212 
213 	hlist_for_each_entry_rcu(gnode, &tunnel->groups[hash], node) {
214 		if (amt_addr_equal(&gnode->group_addr, group) &&
215 		    amt_addr_equal(&gnode->host_addr, host) &&
216 		    gnode->v6 == v6)
217 			return gnode;
218 	}
219 
220 	return NULL;
221 }
222 
223 static void amt_destroy_source(struct amt_source_node *snode)
224 {
225 	struct amt_group_node *gnode = snode->gnode;
226 	struct amt_tunnel_list *tunnel;
227 
228 	tunnel = gnode->tunnel_list;
229 
230 	if (!gnode->v6) {
231 		netdev_dbg(snode->gnode->amt->dev,
232 			   "Delete source %pI4 from %pI4\n",
233 			   &snode->source_addr.ip4,
234 			   &gnode->group_addr.ip4);
235 #if IS_ENABLED(CONFIG_IPV6)
236 	} else {
237 		netdev_dbg(snode->gnode->amt->dev,
238 			   "Delete source %pI6 from %pI6\n",
239 			   &snode->source_addr.ip6,
240 			   &gnode->group_addr.ip6);
241 #endif
242 	}
243 
244 	cancel_delayed_work(&snode->source_timer);
245 	hlist_del_init_rcu(&snode->node);
246 	tunnel->nr_sources--;
247 	gnode->nr_sources--;
248 	spin_lock_bh(&source_gc_lock);
249 	hlist_add_head_rcu(&snode->node, &source_gc_list);
250 	spin_unlock_bh(&source_gc_lock);
251 }
252 
253 static void amt_del_group(struct amt_dev *amt, struct amt_group_node *gnode)
254 {
255 	struct amt_source_node *snode;
256 	struct hlist_node *t;
257 	int i;
258 
259 	if (cancel_delayed_work(&gnode->group_timer))
260 		dev_put(amt->dev);
261 	hlist_del_rcu(&gnode->node);
262 	gnode->tunnel_list->nr_groups--;
263 
264 	if (!gnode->v6)
265 		netdev_dbg(amt->dev, "Leave group %pI4\n",
266 			   &gnode->group_addr.ip4);
267 #if IS_ENABLED(CONFIG_IPV6)
268 	else
269 		netdev_dbg(amt->dev, "Leave group %pI6\n",
270 			   &gnode->group_addr.ip6);
271 #endif
272 	for (i = 0; i < amt->hash_buckets; i++)
273 		hlist_for_each_entry_safe(snode, t, &gnode->sources[i], node)
274 			amt_destroy_source(snode);
275 
276 	/* tunnel->lock was acquired outside of amt_del_group()
277 	 * But rcu_read_lock() was acquired too so It's safe.
278 	 */
279 	kfree_rcu(gnode, rcu);
280 }
281 
282 /* If a source timer expires with a router filter-mode for the group of
283  * INCLUDE, the router concludes that traffic from this particular
284  * source is no longer desired on the attached network, and deletes the
285  * associated source record.
286  */
287 static void amt_source_work(struct work_struct *work)
288 {
289 	struct amt_source_node *snode = container_of(to_delayed_work(work),
290 						     struct amt_source_node,
291 						     source_timer);
292 	struct amt_group_node *gnode = snode->gnode;
293 	struct amt_dev *amt = gnode->amt;
294 	struct amt_tunnel_list *tunnel;
295 
296 	tunnel = gnode->tunnel_list;
297 	spin_lock_bh(&tunnel->lock);
298 	rcu_read_lock();
299 	if (gnode->filter_mode == MCAST_INCLUDE) {
300 		amt_destroy_source(snode);
301 		if (!gnode->nr_sources)
302 			amt_del_group(amt, gnode);
303 	} else {
304 		/* When a router filter-mode for a group is EXCLUDE,
305 		 * source records are only deleted when the group timer expires
306 		 */
307 		snode->status = AMT_SOURCE_STATUS_D_FWD;
308 	}
309 	rcu_read_unlock();
310 	spin_unlock_bh(&tunnel->lock);
311 }
312 
313 static void amt_act_src(struct amt_tunnel_list *tunnel,
314 			struct amt_group_node *gnode,
315 			struct amt_source_node *snode,
316 			enum amt_act act)
317 {
318 	struct amt_dev *amt = tunnel->amt;
319 
320 	switch (act) {
321 	case AMT_ACT_GMI:
322 		mod_delayed_work(amt_wq, &snode->source_timer,
323 				 msecs_to_jiffies(amt_gmi(amt)));
324 		break;
325 	case AMT_ACT_GMI_ZERO:
326 		cancel_delayed_work(&snode->source_timer);
327 		break;
328 	case AMT_ACT_GT:
329 		mod_delayed_work(amt_wq, &snode->source_timer,
330 				 gnode->group_timer.timer.expires);
331 		break;
332 	case AMT_ACT_STATUS_FWD_NEW:
333 		snode->status = AMT_SOURCE_STATUS_FWD;
334 		snode->flags = AMT_SOURCE_NEW;
335 		break;
336 	case AMT_ACT_STATUS_D_FWD_NEW:
337 		snode->status = AMT_SOURCE_STATUS_D_FWD;
338 		snode->flags = AMT_SOURCE_NEW;
339 		break;
340 	case AMT_ACT_STATUS_NONE_NEW:
341 		cancel_delayed_work(&snode->source_timer);
342 		snode->status = AMT_SOURCE_STATUS_NONE;
343 		snode->flags = AMT_SOURCE_NEW;
344 		break;
345 	default:
346 		WARN_ON_ONCE(1);
347 		return;
348 	}
349 
350 	if (!gnode->v6)
351 		netdev_dbg(amt->dev, "Source %pI4 from %pI4 Acted %s\n",
352 			   &snode->source_addr.ip4,
353 			   &gnode->group_addr.ip4,
354 			   action_str[act]);
355 #if IS_ENABLED(CONFIG_IPV6)
356 	else
357 		netdev_dbg(amt->dev, "Source %pI6 from %pI6 Acted %s\n",
358 			   &snode->source_addr.ip6,
359 			   &gnode->group_addr.ip6,
360 			   action_str[act]);
361 #endif
362 }
363 
364 static struct amt_source_node *amt_alloc_snode(struct amt_group_node *gnode,
365 					       union amt_addr *src)
366 {
367 	struct amt_source_node *snode;
368 
369 	snode = kzalloc(sizeof(*snode), GFP_ATOMIC);
370 	if (!snode)
371 		return NULL;
372 
373 	memcpy(&snode->source_addr, src, sizeof(union amt_addr));
374 	snode->gnode = gnode;
375 	snode->status = AMT_SOURCE_STATUS_NONE;
376 	snode->flags = AMT_SOURCE_NEW;
377 	INIT_HLIST_NODE(&snode->node);
378 	INIT_DELAYED_WORK(&snode->source_timer, amt_source_work);
379 
380 	return snode;
381 }
382 
383 /* RFC 3810 - 7.2.2.  Definition of Filter Timers
384  *
385  *  Router Mode          Filter Timer         Actions/Comments
386  *  -----------       -----------------       ----------------
387  *
388  *    INCLUDE             Not Used            All listeners in
389  *                                            INCLUDE mode.
390  *
391  *    EXCLUDE             Timer > 0           At least one listener
392  *                                            in EXCLUDE mode.
393  *
394  *    EXCLUDE             Timer == 0          No more listeners in
395  *                                            EXCLUDE mode for the
396  *                                            multicast address.
397  *                                            If the Requested List
398  *                                            is empty, delete
399  *                                            Multicast Address
400  *                                            Record.  If not, switch
401  *                                            to INCLUDE filter mode;
402  *                                            the sources in the
403  *                                            Requested List are
404  *                                            moved to the Include
405  *                                            List, and the Exclude
406  *                                            List is deleted.
407  */
408 static void amt_group_work(struct work_struct *work)
409 {
410 	struct amt_group_node *gnode = container_of(to_delayed_work(work),
411 						    struct amt_group_node,
412 						    group_timer);
413 	struct amt_tunnel_list *tunnel = gnode->tunnel_list;
414 	struct amt_dev *amt = gnode->amt;
415 	struct amt_source_node *snode;
416 	bool delete_group = true;
417 	struct hlist_node *t;
418 	int i, buckets;
419 
420 	buckets = amt->hash_buckets;
421 
422 	spin_lock_bh(&tunnel->lock);
423 	if (gnode->filter_mode == MCAST_INCLUDE) {
424 		/* Not Used */
425 		spin_unlock_bh(&tunnel->lock);
426 		goto out;
427 	}
428 
429 	rcu_read_lock();
430 	for (i = 0; i < buckets; i++) {
431 		hlist_for_each_entry_safe(snode, t,
432 					  &gnode->sources[i], node) {
433 			if (!delayed_work_pending(&snode->source_timer) ||
434 			    snode->status == AMT_SOURCE_STATUS_D_FWD) {
435 				amt_destroy_source(snode);
436 			} else {
437 				delete_group = false;
438 				snode->status = AMT_SOURCE_STATUS_FWD;
439 			}
440 		}
441 	}
442 	if (delete_group)
443 		amt_del_group(amt, gnode);
444 	else
445 		gnode->filter_mode = MCAST_INCLUDE;
446 	rcu_read_unlock();
447 	spin_unlock_bh(&tunnel->lock);
448 out:
449 	dev_put(amt->dev);
450 }
451 
452 /* Non-existant group is created as INCLUDE {empty}:
453  *
454  * RFC 3376 - 5.1. Action on Change of Interface State
455  *
456  * If no interface state existed for that multicast address before
457  * the change (i.e., the change consisted of creating a new
458  * per-interface record), or if no state exists after the change
459  * (i.e., the change consisted of deleting a per-interface record),
460  * then the "non-existent" state is considered to have a filter mode
461  * of INCLUDE and an empty source list.
462  */
463 static struct amt_group_node *amt_add_group(struct amt_dev *amt,
464 					    struct amt_tunnel_list *tunnel,
465 					    union amt_addr *group,
466 					    union amt_addr *host,
467 					    bool v6)
468 {
469 	struct amt_group_node *gnode;
470 	u32 hash;
471 	int i;
472 
473 	if (tunnel->nr_groups >= amt->max_groups)
474 		return ERR_PTR(-ENOSPC);
475 
476 	gnode = kzalloc(sizeof(*gnode) +
477 			(sizeof(struct hlist_head) * amt->hash_buckets),
478 			GFP_ATOMIC);
479 	if (unlikely(!gnode))
480 		return ERR_PTR(-ENOMEM);
481 
482 	gnode->amt = amt;
483 	gnode->group_addr = *group;
484 	gnode->host_addr = *host;
485 	gnode->v6 = v6;
486 	gnode->tunnel_list = tunnel;
487 	gnode->filter_mode = MCAST_INCLUDE;
488 	INIT_HLIST_NODE(&gnode->node);
489 	INIT_DELAYED_WORK(&gnode->group_timer, amt_group_work);
490 	for (i = 0; i < amt->hash_buckets; i++)
491 		INIT_HLIST_HEAD(&gnode->sources[i]);
492 
493 	hash = amt_group_hash(tunnel, group);
494 	hlist_add_head_rcu(&gnode->node, &tunnel->groups[hash]);
495 	tunnel->nr_groups++;
496 
497 	if (!gnode->v6)
498 		netdev_dbg(amt->dev, "Join group %pI4\n",
499 			   &gnode->group_addr.ip4);
500 #if IS_ENABLED(CONFIG_IPV6)
501 	else
502 		netdev_dbg(amt->dev, "Join group %pI6\n",
503 			   &gnode->group_addr.ip6);
504 #endif
505 
506 	return gnode;
507 }
508 
509 static struct sk_buff *amt_build_igmp_gq(struct amt_dev *amt)
510 {
511 	u8 ra[AMT_IPHDR_OPTS] = { IPOPT_RA, 4, 0, 0 };
512 	int hlen = LL_RESERVED_SPACE(amt->dev);
513 	int tlen = amt->dev->needed_tailroom;
514 	struct igmpv3_query *ihv3;
515 	void *csum_start = NULL;
516 	__sum16 *csum = NULL;
517 	struct sk_buff *skb;
518 	struct ethhdr *eth;
519 	struct iphdr *iph;
520 	unsigned int len;
521 	int offset;
522 
523 	len = hlen + tlen + sizeof(*iph) + AMT_IPHDR_OPTS + sizeof(*ihv3);
524 	skb = netdev_alloc_skb_ip_align(amt->dev, len);
525 	if (!skb)
526 		return NULL;
527 
528 	skb_reserve(skb, hlen);
529 	skb_push(skb, sizeof(*eth));
530 	skb->protocol = htons(ETH_P_IP);
531 	skb_reset_mac_header(skb);
532 	skb->priority = TC_PRIO_CONTROL;
533 	skb_put(skb, sizeof(*iph));
534 	skb_put_data(skb, ra, sizeof(ra));
535 	skb_put(skb, sizeof(*ihv3));
536 	skb_pull(skb, sizeof(*eth));
537 	skb_reset_network_header(skb);
538 
539 	iph		= ip_hdr(skb);
540 	iph->version	= 4;
541 	iph->ihl	= (sizeof(struct iphdr) + AMT_IPHDR_OPTS) >> 2;
542 	iph->tos	= AMT_TOS;
543 	iph->tot_len	= htons(sizeof(*iph) + AMT_IPHDR_OPTS + sizeof(*ihv3));
544 	iph->frag_off	= htons(IP_DF);
545 	iph->ttl	= 1;
546 	iph->id		= 0;
547 	iph->protocol	= IPPROTO_IGMP;
548 	iph->daddr	= htonl(INADDR_ALLHOSTS_GROUP);
549 	iph->saddr	= htonl(INADDR_ANY);
550 	ip_send_check(iph);
551 
552 	eth = eth_hdr(skb);
553 	ether_addr_copy(eth->h_source, amt->dev->dev_addr);
554 	ip_eth_mc_map(htonl(INADDR_ALLHOSTS_GROUP), eth->h_dest);
555 	eth->h_proto = htons(ETH_P_IP);
556 
557 	ihv3		= skb_pull(skb, sizeof(*iph) + AMT_IPHDR_OPTS);
558 	skb_reset_transport_header(skb);
559 	ihv3->type	= IGMP_HOST_MEMBERSHIP_QUERY;
560 	ihv3->code	= 1;
561 	ihv3->group	= 0;
562 	ihv3->qqic	= amt->qi;
563 	ihv3->nsrcs	= 0;
564 	ihv3->resv	= 0;
565 	ihv3->suppress	= false;
566 	ihv3->qrv	= amt->net->ipv4.sysctl_igmp_qrv;
567 	ihv3->csum	= 0;
568 	csum		= &ihv3->csum;
569 	csum_start	= (void *)ihv3;
570 	*csum		= ip_compute_csum(csum_start, sizeof(*ihv3));
571 	offset		= skb_transport_offset(skb);
572 	skb->csum	= skb_checksum(skb, offset, skb->len - offset, 0);
573 	skb->ip_summed	= CHECKSUM_NONE;
574 
575 	skb_push(skb, sizeof(*eth) + sizeof(*iph) + AMT_IPHDR_OPTS);
576 
577 	return skb;
578 }
579 
580 static void __amt_update_gw_status(struct amt_dev *amt, enum amt_status status,
581 				   bool validate)
582 {
583 	if (validate && amt->status >= status)
584 		return;
585 	netdev_dbg(amt->dev, "Update GW status %s -> %s",
586 		   status_str[amt->status], status_str[status]);
587 	amt->status = status;
588 }
589 
590 static void __amt_update_relay_status(struct amt_tunnel_list *tunnel,
591 				      enum amt_status status,
592 				      bool validate)
593 {
594 	if (validate && tunnel->status >= status)
595 		return;
596 	netdev_dbg(tunnel->amt->dev,
597 		   "Update Tunnel(IP = %pI4, PORT = %u) status %s -> %s",
598 		   &tunnel->ip4, ntohs(tunnel->source_port),
599 		   status_str[tunnel->status], status_str[status]);
600 	tunnel->status = status;
601 }
602 
603 static void amt_update_gw_status(struct amt_dev *amt, enum amt_status status,
604 				 bool validate)
605 {
606 	spin_lock_bh(&amt->lock);
607 	__amt_update_gw_status(amt, status, validate);
608 	spin_unlock_bh(&amt->lock);
609 }
610 
611 static void amt_update_relay_status(struct amt_tunnel_list *tunnel,
612 				    enum amt_status status, bool validate)
613 {
614 	spin_lock_bh(&tunnel->lock);
615 	__amt_update_relay_status(tunnel, status, validate);
616 	spin_unlock_bh(&tunnel->lock);
617 }
618 
619 static void amt_send_discovery(struct amt_dev *amt)
620 {
621 	struct amt_header_discovery *amtd;
622 	int hlen, tlen, offset;
623 	struct socket *sock;
624 	struct udphdr *udph;
625 	struct sk_buff *skb;
626 	struct iphdr *iph;
627 	struct rtable *rt;
628 	struct flowi4 fl4;
629 	u32 len;
630 	int err;
631 
632 	rcu_read_lock();
633 	sock = rcu_dereference(amt->sock);
634 	if (!sock)
635 		goto out;
636 
637 	if (!netif_running(amt->stream_dev) || !netif_running(amt->dev))
638 		goto out;
639 
640 	rt = ip_route_output_ports(amt->net, &fl4, sock->sk,
641 				   amt->discovery_ip, amt->local_ip,
642 				   amt->gw_port, amt->relay_port,
643 				   IPPROTO_UDP, 0,
644 				   amt->stream_dev->ifindex);
645 	if (IS_ERR(rt)) {
646 		amt->dev->stats.tx_errors++;
647 		goto out;
648 	}
649 
650 	hlen = LL_RESERVED_SPACE(amt->dev);
651 	tlen = amt->dev->needed_tailroom;
652 	len = hlen + tlen + sizeof(*iph) + sizeof(*udph) + sizeof(*amtd);
653 	skb = netdev_alloc_skb_ip_align(amt->dev, len);
654 	if (!skb) {
655 		ip_rt_put(rt);
656 		amt->dev->stats.tx_errors++;
657 		goto out;
658 	}
659 
660 	skb->priority = TC_PRIO_CONTROL;
661 	skb_dst_set(skb, &rt->dst);
662 
663 	len = sizeof(*iph) + sizeof(*udph) + sizeof(*amtd);
664 	skb_reset_network_header(skb);
665 	skb_put(skb, len);
666 	amtd = skb_pull(skb, sizeof(*iph) + sizeof(*udph));
667 	amtd->version	= 0;
668 	amtd->type	= AMT_MSG_DISCOVERY;
669 	amtd->reserved	= 0;
670 	amtd->nonce	= amt->nonce;
671 	skb_push(skb, sizeof(*udph));
672 	skb_reset_transport_header(skb);
673 	udph		= udp_hdr(skb);
674 	udph->source	= amt->gw_port;
675 	udph->dest	= amt->relay_port;
676 	udph->len	= htons(sizeof(*udph) + sizeof(*amtd));
677 	udph->check	= 0;
678 	offset = skb_transport_offset(skb);
679 	skb->csum = skb_checksum(skb, offset, skb->len - offset, 0);
680 	udph->check = csum_tcpudp_magic(amt->local_ip, amt->discovery_ip,
681 					sizeof(*udph) + sizeof(*amtd),
682 					IPPROTO_UDP, skb->csum);
683 
684 	skb_push(skb, sizeof(*iph));
685 	iph		= ip_hdr(skb);
686 	iph->version	= 4;
687 	iph->ihl	= (sizeof(struct iphdr)) >> 2;
688 	iph->tos	= AMT_TOS;
689 	iph->frag_off	= 0;
690 	iph->ttl	= ip4_dst_hoplimit(&rt->dst);
691 	iph->daddr	= amt->discovery_ip;
692 	iph->saddr	= amt->local_ip;
693 	iph->protocol	= IPPROTO_UDP;
694 	iph->tot_len	= htons(len);
695 
696 	skb->ip_summed = CHECKSUM_NONE;
697 	ip_select_ident(amt->net, skb, NULL);
698 	ip_send_check(iph);
699 	err = ip_local_out(amt->net, sock->sk, skb);
700 	if (unlikely(net_xmit_eval(err)))
701 		amt->dev->stats.tx_errors++;
702 
703 	spin_lock_bh(&amt->lock);
704 	__amt_update_gw_status(amt, AMT_STATUS_SENT_DISCOVERY, true);
705 	spin_unlock_bh(&amt->lock);
706 out:
707 	rcu_read_unlock();
708 }
709 
710 static void amt_send_request(struct amt_dev *amt, bool v6)
711 {
712 	struct amt_header_request *amtrh;
713 	int hlen, tlen, offset;
714 	struct socket *sock;
715 	struct udphdr *udph;
716 	struct sk_buff *skb;
717 	struct iphdr *iph;
718 	struct rtable *rt;
719 	struct flowi4 fl4;
720 	u32 len;
721 	int err;
722 
723 	rcu_read_lock();
724 	sock = rcu_dereference(amt->sock);
725 	if (!sock)
726 		goto out;
727 
728 	if (!netif_running(amt->stream_dev) || !netif_running(amt->dev))
729 		goto out;
730 
731 	rt = ip_route_output_ports(amt->net, &fl4, sock->sk,
732 				   amt->remote_ip, amt->local_ip,
733 				   amt->gw_port, amt->relay_port,
734 				   IPPROTO_UDP, 0,
735 				   amt->stream_dev->ifindex);
736 	if (IS_ERR(rt)) {
737 		amt->dev->stats.tx_errors++;
738 		goto out;
739 	}
740 
741 	hlen = LL_RESERVED_SPACE(amt->dev);
742 	tlen = amt->dev->needed_tailroom;
743 	len = hlen + tlen + sizeof(*iph) + sizeof(*udph) + sizeof(*amtrh);
744 	skb = netdev_alloc_skb_ip_align(amt->dev, len);
745 	if (!skb) {
746 		ip_rt_put(rt);
747 		amt->dev->stats.tx_errors++;
748 		goto out;
749 	}
750 
751 	skb->priority = TC_PRIO_CONTROL;
752 	skb_dst_set(skb, &rt->dst);
753 
754 	len = sizeof(*iph) + sizeof(*udph) + sizeof(*amtrh);
755 	skb_reset_network_header(skb);
756 	skb_put(skb, len);
757 	amtrh = skb_pull(skb, sizeof(*iph) + sizeof(*udph));
758 	amtrh->version	 = 0;
759 	amtrh->type	 = AMT_MSG_REQUEST;
760 	amtrh->reserved1 = 0;
761 	amtrh->p	 = v6;
762 	amtrh->reserved2 = 0;
763 	amtrh->nonce	 = amt->nonce;
764 	skb_push(skb, sizeof(*udph));
765 	skb_reset_transport_header(skb);
766 	udph		= udp_hdr(skb);
767 	udph->source	= amt->gw_port;
768 	udph->dest	= amt->relay_port;
769 	udph->len	= htons(sizeof(*amtrh) + sizeof(*udph));
770 	udph->check	= 0;
771 	offset = skb_transport_offset(skb);
772 	skb->csum = skb_checksum(skb, offset, skb->len - offset, 0);
773 	udph->check = csum_tcpudp_magic(amt->local_ip, amt->remote_ip,
774 					sizeof(*udph) + sizeof(*amtrh),
775 					IPPROTO_UDP, skb->csum);
776 
777 	skb_push(skb, sizeof(*iph));
778 	iph		= ip_hdr(skb);
779 	iph->version	= 4;
780 	iph->ihl	= (sizeof(struct iphdr)) >> 2;
781 	iph->tos	= AMT_TOS;
782 	iph->frag_off	= 0;
783 	iph->ttl	= ip4_dst_hoplimit(&rt->dst);
784 	iph->daddr	= amt->remote_ip;
785 	iph->saddr	= amt->local_ip;
786 	iph->protocol	= IPPROTO_UDP;
787 	iph->tot_len	= htons(len);
788 
789 	skb->ip_summed = CHECKSUM_NONE;
790 	ip_select_ident(amt->net, skb, NULL);
791 	ip_send_check(iph);
792 	err = ip_local_out(amt->net, sock->sk, skb);
793 	if (unlikely(net_xmit_eval(err)))
794 		amt->dev->stats.tx_errors++;
795 
796 out:
797 	rcu_read_unlock();
798 }
799 
800 static void amt_send_igmp_gq(struct amt_dev *amt,
801 			     struct amt_tunnel_list *tunnel)
802 {
803 	struct sk_buff *skb;
804 
805 	skb = amt_build_igmp_gq(amt);
806 	if (!skb)
807 		return;
808 
809 	amt_skb_cb(skb)->tunnel = tunnel;
810 	dev_queue_xmit(skb);
811 }
812 
813 #if IS_ENABLED(CONFIG_IPV6)
814 static struct sk_buff *amt_build_mld_gq(struct amt_dev *amt)
815 {
816 	u8 ra[AMT_IP6HDR_OPTS] = { IPPROTO_ICMPV6, 0, IPV6_TLV_ROUTERALERT,
817 				   2, 0, 0, IPV6_TLV_PAD1, IPV6_TLV_PAD1 };
818 	int hlen = LL_RESERVED_SPACE(amt->dev);
819 	int tlen = amt->dev->needed_tailroom;
820 	struct mld2_query *mld2q;
821 	void *csum_start = NULL;
822 	struct ipv6hdr *ip6h;
823 	struct sk_buff *skb;
824 	struct ethhdr *eth;
825 	u32 len;
826 
827 	len = hlen + tlen + sizeof(*ip6h) + sizeof(ra) + sizeof(*mld2q);
828 	skb = netdev_alloc_skb_ip_align(amt->dev, len);
829 	if (!skb)
830 		return NULL;
831 
832 	skb_reserve(skb, hlen);
833 	skb_push(skb, sizeof(*eth));
834 	skb_reset_mac_header(skb);
835 	eth = eth_hdr(skb);
836 	skb->priority = TC_PRIO_CONTROL;
837 	skb->protocol = htons(ETH_P_IPV6);
838 	skb_put_zero(skb, sizeof(*ip6h));
839 	skb_put_data(skb, ra, sizeof(ra));
840 	skb_put_zero(skb, sizeof(*mld2q));
841 	skb_pull(skb, sizeof(*eth));
842 	skb_reset_network_header(skb);
843 	ip6h			= ipv6_hdr(skb);
844 	ip6h->payload_len	= htons(sizeof(ra) + sizeof(*mld2q));
845 	ip6h->nexthdr		= NEXTHDR_HOP;
846 	ip6h->hop_limit		= 1;
847 	ip6h->daddr		= mld2_all_node;
848 	ip6_flow_hdr(ip6h, 0, 0);
849 
850 	if (ipv6_dev_get_saddr(amt->net, amt->dev, &ip6h->daddr, 0,
851 			       &ip6h->saddr)) {
852 		amt->dev->stats.tx_errors++;
853 		kfree_skb(skb);
854 		return NULL;
855 	}
856 
857 	eth->h_proto = htons(ETH_P_IPV6);
858 	ether_addr_copy(eth->h_source, amt->dev->dev_addr);
859 	ipv6_eth_mc_map(&mld2_all_node, eth->h_dest);
860 
861 	skb_pull(skb, sizeof(*ip6h) + sizeof(ra));
862 	skb_reset_transport_header(skb);
863 	mld2q			= (struct mld2_query *)icmp6_hdr(skb);
864 	mld2q->mld2q_mrc	= htons(1);
865 	mld2q->mld2q_type	= ICMPV6_MGM_QUERY;
866 	mld2q->mld2q_code	= 0;
867 	mld2q->mld2q_cksum	= 0;
868 	mld2q->mld2q_resv1	= 0;
869 	mld2q->mld2q_resv2	= 0;
870 	mld2q->mld2q_suppress	= 0;
871 	mld2q->mld2q_qrv	= amt->qrv;
872 	mld2q->mld2q_nsrcs	= 0;
873 	mld2q->mld2q_qqic	= amt->qi;
874 	csum_start		= (void *)mld2q;
875 	mld2q->mld2q_cksum = csum_ipv6_magic(&ip6h->saddr, &ip6h->daddr,
876 					     sizeof(*mld2q),
877 					     IPPROTO_ICMPV6,
878 					     csum_partial(csum_start,
879 							  sizeof(*mld2q), 0));
880 
881 	skb->ip_summed = CHECKSUM_NONE;
882 	skb_push(skb, sizeof(*eth) + sizeof(*ip6h) + sizeof(ra));
883 	return skb;
884 }
885 
886 static void amt_send_mld_gq(struct amt_dev *amt, struct amt_tunnel_list *tunnel)
887 {
888 	struct sk_buff *skb;
889 
890 	skb = amt_build_mld_gq(amt);
891 	if (!skb)
892 		return;
893 
894 	amt_skb_cb(skb)->tunnel = tunnel;
895 	dev_queue_xmit(skb);
896 }
897 #else
898 static void amt_send_mld_gq(struct amt_dev *amt, struct amt_tunnel_list *tunnel)
899 {
900 }
901 #endif
902 
903 static void amt_secret_work(struct work_struct *work)
904 {
905 	struct amt_dev *amt = container_of(to_delayed_work(work),
906 					   struct amt_dev,
907 					   secret_wq);
908 
909 	spin_lock_bh(&amt->lock);
910 	get_random_bytes(&amt->key, sizeof(siphash_key_t));
911 	spin_unlock_bh(&amt->lock);
912 	mod_delayed_work(amt_wq, &amt->secret_wq,
913 			 msecs_to_jiffies(AMT_SECRET_TIMEOUT));
914 }
915 
916 static void amt_discovery_work(struct work_struct *work)
917 {
918 	struct amt_dev *amt = container_of(to_delayed_work(work),
919 					   struct amt_dev,
920 					   discovery_wq);
921 
922 	spin_lock_bh(&amt->lock);
923 	if (amt->status > AMT_STATUS_SENT_DISCOVERY)
924 		goto out;
925 	get_random_bytes(&amt->nonce, sizeof(__be32));
926 	spin_unlock_bh(&amt->lock);
927 
928 	amt_send_discovery(amt);
929 	spin_lock_bh(&amt->lock);
930 out:
931 	mod_delayed_work(amt_wq, &amt->discovery_wq,
932 			 msecs_to_jiffies(AMT_DISCOVERY_TIMEOUT));
933 	spin_unlock_bh(&amt->lock);
934 }
935 
936 static void amt_req_work(struct work_struct *work)
937 {
938 	struct amt_dev *amt = container_of(to_delayed_work(work),
939 					   struct amt_dev,
940 					   req_wq);
941 	u32 exp;
942 
943 	spin_lock_bh(&amt->lock);
944 	if (amt->status < AMT_STATUS_RECEIVED_ADVERTISEMENT)
945 		goto out;
946 
947 	if (amt->req_cnt++ > AMT_MAX_REQ_COUNT) {
948 		netdev_dbg(amt->dev, "Gateway is not ready");
949 		amt->qi = AMT_INIT_REQ_TIMEOUT;
950 		amt->ready4 = false;
951 		amt->ready6 = false;
952 		amt->remote_ip = 0;
953 		__amt_update_gw_status(amt, AMT_STATUS_INIT, false);
954 		amt->req_cnt = 0;
955 	}
956 	spin_unlock_bh(&amt->lock);
957 
958 	amt_send_request(amt, false);
959 	amt_send_request(amt, true);
960 	amt_update_gw_status(amt, AMT_STATUS_SENT_REQUEST, true);
961 	spin_lock_bh(&amt->lock);
962 out:
963 	exp = min_t(u32, (1 * (1 << amt->req_cnt)), AMT_MAX_REQ_TIMEOUT);
964 	mod_delayed_work(amt_wq, &amt->req_wq, msecs_to_jiffies(exp * 1000));
965 	spin_unlock_bh(&amt->lock);
966 }
967 
968 static bool amt_send_membership_update(struct amt_dev *amt,
969 				       struct sk_buff *skb,
970 				       bool v6)
971 {
972 	struct amt_header_membership_update *amtmu;
973 	struct socket *sock;
974 	struct iphdr *iph;
975 	struct flowi4 fl4;
976 	struct rtable *rt;
977 	int err;
978 
979 	sock = rcu_dereference_bh(amt->sock);
980 	if (!sock)
981 		return true;
982 
983 	err = skb_cow_head(skb, LL_RESERVED_SPACE(amt->dev) + sizeof(*amtmu) +
984 			   sizeof(*iph) + sizeof(struct udphdr));
985 	if (err)
986 		return true;
987 
988 	skb_reset_inner_headers(skb);
989 	memset(&fl4, 0, sizeof(struct flowi4));
990 	fl4.flowi4_oif         = amt->stream_dev->ifindex;
991 	fl4.daddr              = amt->remote_ip;
992 	fl4.saddr              = amt->local_ip;
993 	fl4.flowi4_tos         = AMT_TOS;
994 	fl4.flowi4_proto       = IPPROTO_UDP;
995 	rt = ip_route_output_key(amt->net, &fl4);
996 	if (IS_ERR(rt)) {
997 		netdev_dbg(amt->dev, "no route to %pI4\n", &amt->remote_ip);
998 		return true;
999 	}
1000 
1001 	amtmu			= skb_push(skb, sizeof(*amtmu));
1002 	amtmu->version		= 0;
1003 	amtmu->type		= AMT_MSG_MEMBERSHIP_UPDATE;
1004 	amtmu->reserved		= 0;
1005 	amtmu->nonce		= amt->nonce;
1006 	amtmu->response_mac	= amt->mac;
1007 
1008 	if (!v6)
1009 		skb_set_inner_protocol(skb, htons(ETH_P_IP));
1010 	else
1011 		skb_set_inner_protocol(skb, htons(ETH_P_IPV6));
1012 	udp_tunnel_xmit_skb(rt, sock->sk, skb,
1013 			    fl4.saddr,
1014 			    fl4.daddr,
1015 			    AMT_TOS,
1016 			    ip4_dst_hoplimit(&rt->dst),
1017 			    0,
1018 			    amt->gw_port,
1019 			    amt->relay_port,
1020 			    false,
1021 			    false);
1022 	amt_update_gw_status(amt, AMT_STATUS_SENT_UPDATE, true);
1023 	return false;
1024 }
1025 
1026 static void amt_send_multicast_data(struct amt_dev *amt,
1027 				    const struct sk_buff *oskb,
1028 				    struct amt_tunnel_list *tunnel,
1029 				    bool v6)
1030 {
1031 	struct amt_header_mcast_data *amtmd;
1032 	struct socket *sock;
1033 	struct sk_buff *skb;
1034 	struct iphdr *iph;
1035 	struct flowi4 fl4;
1036 	struct rtable *rt;
1037 
1038 	sock = rcu_dereference_bh(amt->sock);
1039 	if (!sock)
1040 		return;
1041 
1042 	skb = skb_copy_expand(oskb, sizeof(*amtmd) + sizeof(*iph) +
1043 			      sizeof(struct udphdr), 0, GFP_ATOMIC);
1044 	if (!skb)
1045 		return;
1046 
1047 	skb_reset_inner_headers(skb);
1048 	memset(&fl4, 0, sizeof(struct flowi4));
1049 	fl4.flowi4_oif         = amt->stream_dev->ifindex;
1050 	fl4.daddr              = tunnel->ip4;
1051 	fl4.saddr              = amt->local_ip;
1052 	fl4.flowi4_proto       = IPPROTO_UDP;
1053 	rt = ip_route_output_key(amt->net, &fl4);
1054 	if (IS_ERR(rt)) {
1055 		netdev_dbg(amt->dev, "no route to %pI4\n", &tunnel->ip4);
1056 		kfree_skb(skb);
1057 		return;
1058 	}
1059 
1060 	amtmd = skb_push(skb, sizeof(*amtmd));
1061 	amtmd->version = 0;
1062 	amtmd->reserved = 0;
1063 	amtmd->type = AMT_MSG_MULTICAST_DATA;
1064 
1065 	if (!v6)
1066 		skb_set_inner_protocol(skb, htons(ETH_P_IP));
1067 	else
1068 		skb_set_inner_protocol(skb, htons(ETH_P_IPV6));
1069 	udp_tunnel_xmit_skb(rt, sock->sk, skb,
1070 			    fl4.saddr,
1071 			    fl4.daddr,
1072 			    AMT_TOS,
1073 			    ip4_dst_hoplimit(&rt->dst),
1074 			    0,
1075 			    amt->relay_port,
1076 			    tunnel->source_port,
1077 			    false,
1078 			    false);
1079 }
1080 
1081 static bool amt_send_membership_query(struct amt_dev *amt,
1082 				      struct sk_buff *skb,
1083 				      struct amt_tunnel_list *tunnel,
1084 				      bool v6)
1085 {
1086 	struct amt_header_membership_query *amtmq;
1087 	struct socket *sock;
1088 	struct rtable *rt;
1089 	struct flowi4 fl4;
1090 	int err;
1091 
1092 	sock = rcu_dereference_bh(amt->sock);
1093 	if (!sock)
1094 		return true;
1095 
1096 	err = skb_cow_head(skb, LL_RESERVED_SPACE(amt->dev) + sizeof(*amtmq) +
1097 			   sizeof(struct iphdr) + sizeof(struct udphdr));
1098 	if (err)
1099 		return true;
1100 
1101 	skb_reset_inner_headers(skb);
1102 	memset(&fl4, 0, sizeof(struct flowi4));
1103 	fl4.flowi4_oif         = amt->stream_dev->ifindex;
1104 	fl4.daddr              = tunnel->ip4;
1105 	fl4.saddr              = amt->local_ip;
1106 	fl4.flowi4_tos         = AMT_TOS;
1107 	fl4.flowi4_proto       = IPPROTO_UDP;
1108 	rt = ip_route_output_key(amt->net, &fl4);
1109 	if (IS_ERR(rt)) {
1110 		netdev_dbg(amt->dev, "no route to %pI4\n", &tunnel->ip4);
1111 		return -1;
1112 	}
1113 
1114 	amtmq		= skb_push(skb, sizeof(*amtmq));
1115 	amtmq->version	= 0;
1116 	amtmq->type	= AMT_MSG_MEMBERSHIP_QUERY;
1117 	amtmq->reserved = 0;
1118 	amtmq->l	= 0;
1119 	amtmq->g	= 0;
1120 	amtmq->nonce	= tunnel->nonce;
1121 	amtmq->response_mac = tunnel->mac;
1122 
1123 	if (!v6)
1124 		skb_set_inner_protocol(skb, htons(ETH_P_IP));
1125 	else
1126 		skb_set_inner_protocol(skb, htons(ETH_P_IPV6));
1127 	udp_tunnel_xmit_skb(rt, sock->sk, skb,
1128 			    fl4.saddr,
1129 			    fl4.daddr,
1130 			    AMT_TOS,
1131 			    ip4_dst_hoplimit(&rt->dst),
1132 			    0,
1133 			    amt->relay_port,
1134 			    tunnel->source_port,
1135 			    false,
1136 			    false);
1137 	amt_update_relay_status(tunnel, AMT_STATUS_SENT_QUERY, true);
1138 	return false;
1139 }
1140 
1141 static netdev_tx_t amt_dev_xmit(struct sk_buff *skb, struct net_device *dev)
1142 {
1143 	struct amt_dev *amt = netdev_priv(dev);
1144 	struct amt_tunnel_list *tunnel;
1145 	struct amt_group_node *gnode;
1146 	union amt_addr group = {0,};
1147 #if IS_ENABLED(CONFIG_IPV6)
1148 	struct ipv6hdr *ip6h;
1149 	struct mld_msg *mld;
1150 #endif
1151 	bool report = false;
1152 	struct igmphdr *ih;
1153 	bool query = false;
1154 	struct iphdr *iph;
1155 	bool data = false;
1156 	bool v6 = false;
1157 	u32 hash;
1158 
1159 	iph = ip_hdr(skb);
1160 	if (iph->version == 4) {
1161 		if (!ipv4_is_multicast(iph->daddr))
1162 			goto free;
1163 
1164 		if (!ip_mc_check_igmp(skb)) {
1165 			ih = igmp_hdr(skb);
1166 			switch (ih->type) {
1167 			case IGMPV3_HOST_MEMBERSHIP_REPORT:
1168 			case IGMP_HOST_MEMBERSHIP_REPORT:
1169 				report = true;
1170 				break;
1171 			case IGMP_HOST_MEMBERSHIP_QUERY:
1172 				query = true;
1173 				break;
1174 			default:
1175 				goto free;
1176 			}
1177 		} else {
1178 			data = true;
1179 		}
1180 		v6 = false;
1181 		group.ip4 = iph->daddr;
1182 #if IS_ENABLED(CONFIG_IPV6)
1183 	} else if (iph->version == 6) {
1184 		ip6h = ipv6_hdr(skb);
1185 		if (!ipv6_addr_is_multicast(&ip6h->daddr))
1186 			goto free;
1187 
1188 		if (!ipv6_mc_check_mld(skb)) {
1189 			mld = (struct mld_msg *)skb_transport_header(skb);
1190 			switch (mld->mld_type) {
1191 			case ICMPV6_MGM_REPORT:
1192 			case ICMPV6_MLD2_REPORT:
1193 				report = true;
1194 				break;
1195 			case ICMPV6_MGM_QUERY:
1196 				query = true;
1197 				break;
1198 			default:
1199 				goto free;
1200 			}
1201 		} else {
1202 			data = true;
1203 		}
1204 		v6 = true;
1205 		group.ip6 = ip6h->daddr;
1206 #endif
1207 	} else {
1208 		dev->stats.tx_errors++;
1209 		goto free;
1210 	}
1211 
1212 	if (!pskb_may_pull(skb, sizeof(struct ethhdr)))
1213 		goto free;
1214 
1215 	skb_pull(skb, sizeof(struct ethhdr));
1216 
1217 	if (amt->mode == AMT_MODE_GATEWAY) {
1218 		/* Gateway only passes IGMP/MLD packets */
1219 		if (!report)
1220 			goto free;
1221 		if ((!v6 && !amt->ready4) || (v6 && !amt->ready6))
1222 			goto free;
1223 		if (amt_send_membership_update(amt, skb,  v6))
1224 			goto free;
1225 		goto unlock;
1226 	} else if (amt->mode == AMT_MODE_RELAY) {
1227 		if (query) {
1228 			tunnel = amt_skb_cb(skb)->tunnel;
1229 			if (!tunnel) {
1230 				WARN_ON(1);
1231 				goto free;
1232 			}
1233 
1234 			/* Do not forward unexpected query */
1235 			if (amt_send_membership_query(amt, skb, tunnel, v6))
1236 				goto free;
1237 			goto unlock;
1238 		}
1239 
1240 		if (!data)
1241 			goto free;
1242 		list_for_each_entry_rcu(tunnel, &amt->tunnel_list, list) {
1243 			hash = amt_group_hash(tunnel, &group);
1244 			hlist_for_each_entry_rcu(gnode, &tunnel->groups[hash],
1245 						 node) {
1246 				if (!v6) {
1247 					if (gnode->group_addr.ip4 == iph->daddr)
1248 						goto found;
1249 #if IS_ENABLED(CONFIG_IPV6)
1250 				} else {
1251 					if (ipv6_addr_equal(&gnode->group_addr.ip6,
1252 							    &ip6h->daddr))
1253 						goto found;
1254 #endif
1255 				}
1256 			}
1257 			continue;
1258 found:
1259 			amt_send_multicast_data(amt, skb, tunnel, v6);
1260 		}
1261 	}
1262 
1263 	dev_kfree_skb(skb);
1264 	return NETDEV_TX_OK;
1265 free:
1266 	dev_kfree_skb(skb);
1267 unlock:
1268 	dev->stats.tx_dropped++;
1269 	return NETDEV_TX_OK;
1270 }
1271 
1272 static int amt_parse_type(struct sk_buff *skb)
1273 {
1274 	struct amt_header *amth;
1275 
1276 	if (!pskb_may_pull(skb, sizeof(struct udphdr) +
1277 			   sizeof(struct amt_header)))
1278 		return -1;
1279 
1280 	amth = (struct amt_header *)(udp_hdr(skb) + 1);
1281 
1282 	if (amth->version != 0)
1283 		return -1;
1284 
1285 	if (amth->type >= __AMT_MSG_MAX || !amth->type)
1286 		return -1;
1287 	return amth->type;
1288 }
1289 
1290 static void amt_clear_groups(struct amt_tunnel_list *tunnel)
1291 {
1292 	struct amt_dev *amt = tunnel->amt;
1293 	struct amt_group_node *gnode;
1294 	struct hlist_node *t;
1295 	int i;
1296 
1297 	spin_lock_bh(&tunnel->lock);
1298 	rcu_read_lock();
1299 	for (i = 0; i < amt->hash_buckets; i++)
1300 		hlist_for_each_entry_safe(gnode, t, &tunnel->groups[i], node)
1301 			amt_del_group(amt, gnode);
1302 	rcu_read_unlock();
1303 	spin_unlock_bh(&tunnel->lock);
1304 }
1305 
1306 static void amt_tunnel_expire(struct work_struct *work)
1307 {
1308 	struct amt_tunnel_list *tunnel = container_of(to_delayed_work(work),
1309 						      struct amt_tunnel_list,
1310 						      gc_wq);
1311 	struct amt_dev *amt = tunnel->amt;
1312 
1313 	spin_lock_bh(&amt->lock);
1314 	rcu_read_lock();
1315 	list_del_rcu(&tunnel->list);
1316 	amt->nr_tunnels--;
1317 	amt_clear_groups(tunnel);
1318 	rcu_read_unlock();
1319 	spin_unlock_bh(&amt->lock);
1320 	kfree_rcu(tunnel, rcu);
1321 }
1322 
1323 static void amt_cleanup_srcs(struct amt_dev *amt,
1324 			     struct amt_tunnel_list *tunnel,
1325 			     struct amt_group_node *gnode)
1326 {
1327 	struct amt_source_node *snode;
1328 	struct hlist_node *t;
1329 	int i;
1330 
1331 	/* Delete old sources */
1332 	for (i = 0; i < amt->hash_buckets; i++) {
1333 		hlist_for_each_entry_safe(snode, t, &gnode->sources[i], node) {
1334 			if (snode->flags == AMT_SOURCE_OLD)
1335 				amt_destroy_source(snode);
1336 		}
1337 	}
1338 
1339 	/* switch from new to old */
1340 	for (i = 0; i < amt->hash_buckets; i++)  {
1341 		hlist_for_each_entry_rcu(snode, &gnode->sources[i], node) {
1342 			snode->flags = AMT_SOURCE_OLD;
1343 			if (!gnode->v6)
1344 				netdev_dbg(snode->gnode->amt->dev,
1345 					   "Add source as OLD %pI4 from %pI4\n",
1346 					   &snode->source_addr.ip4,
1347 					   &gnode->group_addr.ip4);
1348 #if IS_ENABLED(CONFIG_IPV6)
1349 			else
1350 				netdev_dbg(snode->gnode->amt->dev,
1351 					   "Add source as OLD %pI6 from %pI6\n",
1352 					   &snode->source_addr.ip6,
1353 					   &gnode->group_addr.ip6);
1354 #endif
1355 		}
1356 	}
1357 }
1358 
1359 static void amt_add_srcs(struct amt_dev *amt, struct amt_tunnel_list *tunnel,
1360 			 struct amt_group_node *gnode, void *grec,
1361 			 bool v6)
1362 {
1363 	struct igmpv3_grec *igmp_grec;
1364 	struct amt_source_node *snode;
1365 #if IS_ENABLED(CONFIG_IPV6)
1366 	struct mld2_grec *mld_grec;
1367 #endif
1368 	union amt_addr src = {0,};
1369 	u16 nsrcs;
1370 	u32 hash;
1371 	int i;
1372 
1373 	if (!v6) {
1374 		igmp_grec = (struct igmpv3_grec *)grec;
1375 		nsrcs = ntohs(igmp_grec->grec_nsrcs);
1376 	} else {
1377 #if IS_ENABLED(CONFIG_IPV6)
1378 		mld_grec = (struct mld2_grec *)grec;
1379 		nsrcs = ntohs(mld_grec->grec_nsrcs);
1380 #else
1381 	return;
1382 #endif
1383 	}
1384 	for (i = 0; i < nsrcs; i++) {
1385 		if (tunnel->nr_sources >= amt->max_sources)
1386 			return;
1387 		if (!v6)
1388 			src.ip4 = igmp_grec->grec_src[i];
1389 #if IS_ENABLED(CONFIG_IPV6)
1390 		else
1391 			memcpy(&src.ip6, &mld_grec->grec_src[i],
1392 			       sizeof(struct in6_addr));
1393 #endif
1394 		if (amt_lookup_src(tunnel, gnode, AMT_FILTER_ALL, &src))
1395 			continue;
1396 
1397 		snode = amt_alloc_snode(gnode, &src);
1398 		if (snode) {
1399 			hash = amt_source_hash(tunnel, &snode->source_addr);
1400 			hlist_add_head_rcu(&snode->node, &gnode->sources[hash]);
1401 			tunnel->nr_sources++;
1402 			gnode->nr_sources++;
1403 
1404 			if (!gnode->v6)
1405 				netdev_dbg(snode->gnode->amt->dev,
1406 					   "Add source as NEW %pI4 from %pI4\n",
1407 					   &snode->source_addr.ip4,
1408 					   &gnode->group_addr.ip4);
1409 #if IS_ENABLED(CONFIG_IPV6)
1410 			else
1411 				netdev_dbg(snode->gnode->amt->dev,
1412 					   "Add source as NEW %pI6 from %pI6\n",
1413 					   &snode->source_addr.ip6,
1414 					   &gnode->group_addr.ip6);
1415 #endif
1416 		}
1417 	}
1418 }
1419 
1420 /* Router State   Report Rec'd New Router State
1421  * ------------   ------------ ----------------
1422  * EXCLUDE (X,Y)  IS_IN (A)    EXCLUDE (X+A,Y-A)
1423  *
1424  * -----------+-----------+-----------+
1425  *            |    OLD    |    NEW    |
1426  * -----------+-----------+-----------+
1427  *    FWD     |     X     |    X+A    |
1428  * -----------+-----------+-----------+
1429  *    D_FWD   |     Y     |    Y-A    |
1430  * -----------+-----------+-----------+
1431  *    NONE    |           |     A     |
1432  * -----------+-----------+-----------+
1433  *
1434  * a) Received sources are NONE/NEW
1435  * b) All NONE will be deleted by amt_cleanup_srcs().
1436  * c) All OLD will be deleted by amt_cleanup_srcs().
1437  * d) After delete, NEW source will be switched to OLD.
1438  */
1439 static void amt_lookup_act_srcs(struct amt_tunnel_list *tunnel,
1440 				struct amt_group_node *gnode,
1441 				void *grec,
1442 				enum amt_ops ops,
1443 				enum amt_filter filter,
1444 				enum amt_act act,
1445 				bool v6)
1446 {
1447 	struct amt_dev *amt = tunnel->amt;
1448 	struct amt_source_node *snode;
1449 	struct igmpv3_grec *igmp_grec;
1450 #if IS_ENABLED(CONFIG_IPV6)
1451 	struct mld2_grec *mld_grec;
1452 #endif
1453 	union amt_addr src = {0,};
1454 	struct hlist_node *t;
1455 	u16 nsrcs;
1456 	int i, j;
1457 
1458 	if (!v6) {
1459 		igmp_grec = (struct igmpv3_grec *)grec;
1460 		nsrcs = ntohs(igmp_grec->grec_nsrcs);
1461 	} else {
1462 #if IS_ENABLED(CONFIG_IPV6)
1463 		mld_grec = (struct mld2_grec *)grec;
1464 		nsrcs = ntohs(mld_grec->grec_nsrcs);
1465 #else
1466 	return;
1467 #endif
1468 	}
1469 
1470 	memset(&src, 0, sizeof(union amt_addr));
1471 	switch (ops) {
1472 	case AMT_OPS_INT:
1473 		/* A*B */
1474 		for (i = 0; i < nsrcs; i++) {
1475 			if (!v6)
1476 				src.ip4 = igmp_grec->grec_src[i];
1477 #if IS_ENABLED(CONFIG_IPV6)
1478 			else
1479 				memcpy(&src.ip6, &mld_grec->grec_src[i],
1480 				       sizeof(struct in6_addr));
1481 #endif
1482 			snode = amt_lookup_src(tunnel, gnode, filter, &src);
1483 			if (!snode)
1484 				continue;
1485 			amt_act_src(tunnel, gnode, snode, act);
1486 		}
1487 		break;
1488 	case AMT_OPS_UNI:
1489 		/* A+B */
1490 		for (i = 0; i < amt->hash_buckets; i++) {
1491 			hlist_for_each_entry_safe(snode, t, &gnode->sources[i],
1492 						  node) {
1493 				if (amt_status_filter(snode, filter))
1494 					amt_act_src(tunnel, gnode, snode, act);
1495 			}
1496 		}
1497 		for (i = 0; i < nsrcs; i++) {
1498 			if (!v6)
1499 				src.ip4 = igmp_grec->grec_src[i];
1500 #if IS_ENABLED(CONFIG_IPV6)
1501 			else
1502 				memcpy(&src.ip6, &mld_grec->grec_src[i],
1503 				       sizeof(struct in6_addr));
1504 #endif
1505 			snode = amt_lookup_src(tunnel, gnode, filter, &src);
1506 			if (!snode)
1507 				continue;
1508 			amt_act_src(tunnel, gnode, snode, act);
1509 		}
1510 		break;
1511 	case AMT_OPS_SUB:
1512 		/* A-B */
1513 		for (i = 0; i < amt->hash_buckets; i++) {
1514 			hlist_for_each_entry_safe(snode, t, &gnode->sources[i],
1515 						  node) {
1516 				if (!amt_status_filter(snode, filter))
1517 					continue;
1518 				for (j = 0; j < nsrcs; j++) {
1519 					if (!v6)
1520 						src.ip4 = igmp_grec->grec_src[j];
1521 #if IS_ENABLED(CONFIG_IPV6)
1522 					else
1523 						memcpy(&src.ip6,
1524 						       &mld_grec->grec_src[j],
1525 						       sizeof(struct in6_addr));
1526 #endif
1527 					if (amt_addr_equal(&snode->source_addr,
1528 							   &src))
1529 						goto out_sub;
1530 				}
1531 				amt_act_src(tunnel, gnode, snode, act);
1532 				continue;
1533 out_sub:;
1534 			}
1535 		}
1536 		break;
1537 	case AMT_OPS_SUB_REV:
1538 		/* B-A */
1539 		for (i = 0; i < nsrcs; i++) {
1540 			if (!v6)
1541 				src.ip4 = igmp_grec->grec_src[i];
1542 #if IS_ENABLED(CONFIG_IPV6)
1543 			else
1544 				memcpy(&src.ip6, &mld_grec->grec_src[i],
1545 				       sizeof(struct in6_addr));
1546 #endif
1547 			snode = amt_lookup_src(tunnel, gnode, AMT_FILTER_ALL,
1548 					       &src);
1549 			if (!snode) {
1550 				snode = amt_lookup_src(tunnel, gnode,
1551 						       filter, &src);
1552 				if (snode)
1553 					amt_act_src(tunnel, gnode, snode, act);
1554 			}
1555 		}
1556 		break;
1557 	default:
1558 		netdev_dbg(amt->dev, "Invalid type\n");
1559 		return;
1560 	}
1561 }
1562 
1563 static void amt_mcast_is_in_handler(struct amt_dev *amt,
1564 				    struct amt_tunnel_list *tunnel,
1565 				    struct amt_group_node *gnode,
1566 				    void *grec, void *zero_grec, bool v6)
1567 {
1568 	if (gnode->filter_mode == MCAST_INCLUDE) {
1569 /* Router State   Report Rec'd New Router State        Actions
1570  * ------------   ------------ ----------------        -------
1571  * INCLUDE (A)    IS_IN (B)    INCLUDE (A+B)           (B)=GMI
1572  */
1573 		/* Update IS_IN (B) as FWD/NEW */
1574 		amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_UNI,
1575 				    AMT_FILTER_NONE_NEW,
1576 				    AMT_ACT_STATUS_FWD_NEW,
1577 				    v6);
1578 		/* Update INCLUDE (A) as NEW */
1579 		amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_UNI,
1580 				    AMT_FILTER_FWD,
1581 				    AMT_ACT_STATUS_FWD_NEW,
1582 				    v6);
1583 		/* (B)=GMI */
1584 		amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_INT,
1585 				    AMT_FILTER_FWD_NEW,
1586 				    AMT_ACT_GMI,
1587 				    v6);
1588 	} else {
1589 /* State        Actions
1590  * ------------   ------------ ----------------        -------
1591  * EXCLUDE (X,Y)  IS_IN (A)    EXCLUDE (X+A,Y-A)       (A)=GMI
1592  */
1593 		/* Update (A) in (X, Y) as NONE/NEW */
1594 		amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_INT,
1595 				    AMT_FILTER_BOTH,
1596 				    AMT_ACT_STATUS_NONE_NEW,
1597 				    v6);
1598 		/* Update FWD/OLD as FWD/NEW */
1599 		amt_lookup_act_srcs(tunnel, gnode, zero_grec, AMT_OPS_UNI,
1600 				    AMT_FILTER_FWD,
1601 				    AMT_ACT_STATUS_FWD_NEW,
1602 				    v6);
1603 		/* Update IS_IN (A) as FWD/NEW */
1604 		amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_INT,
1605 				    AMT_FILTER_NONE_NEW,
1606 				    AMT_ACT_STATUS_FWD_NEW,
1607 				    v6);
1608 		/* Update EXCLUDE (, Y-A) as D_FWD_NEW */
1609 		amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_SUB,
1610 				    AMT_FILTER_D_FWD,
1611 				    AMT_ACT_STATUS_D_FWD_NEW,
1612 				    v6);
1613 	}
1614 }
1615 
1616 static void amt_mcast_is_ex_handler(struct amt_dev *amt,
1617 				    struct amt_tunnel_list *tunnel,
1618 				    struct amt_group_node *gnode,
1619 				    void *grec, void *zero_grec, bool v6)
1620 {
1621 	if (gnode->filter_mode == MCAST_INCLUDE) {
1622 /* Router State   Report Rec'd  New Router State         Actions
1623  * ------------   ------------  ----------------         -------
1624  * INCLUDE (A)    IS_EX (B)     EXCLUDE (A*B,B-A)        (B-A)=0
1625  *                                                       Delete (A-B)
1626  *                                                       Group Timer=GMI
1627  */
1628 		/* EXCLUDE(A*B, ) */
1629 		amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_INT,
1630 				    AMT_FILTER_FWD,
1631 				    AMT_ACT_STATUS_FWD_NEW,
1632 				    v6);
1633 		/* EXCLUDE(, B-A) */
1634 		amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_SUB_REV,
1635 				    AMT_FILTER_FWD,
1636 				    AMT_ACT_STATUS_D_FWD_NEW,
1637 				    v6);
1638 		/* (B-A)=0 */
1639 		amt_lookup_act_srcs(tunnel, gnode, zero_grec, AMT_OPS_UNI,
1640 				    AMT_FILTER_D_FWD_NEW,
1641 				    AMT_ACT_GMI_ZERO,
1642 				    v6);
1643 		/* Group Timer=GMI */
1644 		if (!mod_delayed_work(amt_wq, &gnode->group_timer,
1645 				      msecs_to_jiffies(amt_gmi(amt))))
1646 			dev_hold(amt->dev);
1647 		gnode->filter_mode = MCAST_EXCLUDE;
1648 		/* Delete (A-B) will be worked by amt_cleanup_srcs(). */
1649 	} else {
1650 /* Router State   Report Rec'd  New Router State	Actions
1651  * ------------   ------------  ----------------	-------
1652  * EXCLUDE (X,Y)  IS_EX (A)     EXCLUDE (A-Y,Y*A)	(A-X-Y)=GMI
1653  *							Delete (X-A)
1654  *							Delete (Y-A)
1655  *							Group Timer=GMI
1656  */
1657 		/* EXCLUDE (A-Y, ) */
1658 		amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_SUB_REV,
1659 				    AMT_FILTER_D_FWD,
1660 				    AMT_ACT_STATUS_FWD_NEW,
1661 				    v6);
1662 		/* EXCLUDE (, Y*A ) */
1663 		amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_INT,
1664 				    AMT_FILTER_D_FWD,
1665 				    AMT_ACT_STATUS_D_FWD_NEW,
1666 				    v6);
1667 		/* (A-X-Y)=GMI */
1668 		amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_SUB_REV,
1669 				    AMT_FILTER_BOTH_NEW,
1670 				    AMT_ACT_GMI,
1671 				    v6);
1672 		/* Group Timer=GMI */
1673 		if (!mod_delayed_work(amt_wq, &gnode->group_timer,
1674 				      msecs_to_jiffies(amt_gmi(amt))))
1675 			dev_hold(amt->dev);
1676 		/* Delete (X-A), (Y-A) will be worked by amt_cleanup_srcs(). */
1677 	}
1678 }
1679 
1680 static void amt_mcast_to_in_handler(struct amt_dev *amt,
1681 				    struct amt_tunnel_list *tunnel,
1682 				    struct amt_group_node *gnode,
1683 				    void *grec, void *zero_grec, bool v6)
1684 {
1685 	if (gnode->filter_mode == MCAST_INCLUDE) {
1686 /* Router State   Report Rec'd New Router State        Actions
1687  * ------------   ------------ ----------------        -------
1688  * INCLUDE (A)    TO_IN (B)    INCLUDE (A+B)           (B)=GMI
1689  *						       Send Q(G,A-B)
1690  */
1691 		/* Update TO_IN (B) sources as FWD/NEW */
1692 		amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_UNI,
1693 				    AMT_FILTER_NONE_NEW,
1694 				    AMT_ACT_STATUS_FWD_NEW,
1695 				    v6);
1696 		/* Update INCLUDE (A) sources as NEW */
1697 		amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_UNI,
1698 				    AMT_FILTER_FWD,
1699 				    AMT_ACT_STATUS_FWD_NEW,
1700 				    v6);
1701 		/* (B)=GMI */
1702 		amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_INT,
1703 				    AMT_FILTER_FWD_NEW,
1704 				    AMT_ACT_GMI,
1705 				    v6);
1706 	} else {
1707 /* Router State   Report Rec'd New Router State        Actions
1708  * ------------   ------------ ----------------        -------
1709  * EXCLUDE (X,Y)  TO_IN (A)    EXCLUDE (X+A,Y-A)       (A)=GMI
1710  *						       Send Q(G,X-A)
1711  *						       Send Q(G)
1712  */
1713 		/* Update TO_IN (A) sources as FWD/NEW */
1714 		amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_UNI,
1715 				    AMT_FILTER_NONE_NEW,
1716 				    AMT_ACT_STATUS_FWD_NEW,
1717 				    v6);
1718 		/* Update EXCLUDE(X,) sources as FWD/NEW */
1719 		amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_UNI,
1720 				    AMT_FILTER_FWD,
1721 				    AMT_ACT_STATUS_FWD_NEW,
1722 				    v6);
1723 		/* EXCLUDE (, Y-A)
1724 		 * (A) are already switched to FWD_NEW.
1725 		 * So, D_FWD/OLD -> D_FWD/NEW is okay.
1726 		 */
1727 		amt_lookup_act_srcs(tunnel, gnode, zero_grec, AMT_OPS_UNI,
1728 				    AMT_FILTER_D_FWD,
1729 				    AMT_ACT_STATUS_D_FWD_NEW,
1730 				    v6);
1731 		/* (A)=GMI
1732 		 * Only FWD_NEW will have (A) sources.
1733 		 */
1734 		amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_INT,
1735 				    AMT_FILTER_FWD_NEW,
1736 				    AMT_ACT_GMI,
1737 				    v6);
1738 	}
1739 }
1740 
1741 static void amt_mcast_to_ex_handler(struct amt_dev *amt,
1742 				    struct amt_tunnel_list *tunnel,
1743 				    struct amt_group_node *gnode,
1744 				    void *grec, void *zero_grec, bool v6)
1745 {
1746 	if (gnode->filter_mode == MCAST_INCLUDE) {
1747 /* Router State   Report Rec'd New Router State        Actions
1748  * ------------   ------------ ----------------        -------
1749  * INCLUDE (A)    TO_EX (B)    EXCLUDE (A*B,B-A)       (B-A)=0
1750  *						       Delete (A-B)
1751  *						       Send Q(G,A*B)
1752  *						       Group Timer=GMI
1753  */
1754 		/* EXCLUDE (A*B, ) */
1755 		amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_INT,
1756 				    AMT_FILTER_FWD,
1757 				    AMT_ACT_STATUS_FWD_NEW,
1758 				    v6);
1759 		/* EXCLUDE (, B-A) */
1760 		amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_SUB_REV,
1761 				    AMT_FILTER_FWD,
1762 				    AMT_ACT_STATUS_D_FWD_NEW,
1763 				    v6);
1764 		/* (B-A)=0 */
1765 		amt_lookup_act_srcs(tunnel, gnode, zero_grec, AMT_OPS_UNI,
1766 				    AMT_FILTER_D_FWD_NEW,
1767 				    AMT_ACT_GMI_ZERO,
1768 				    v6);
1769 		/* Group Timer=GMI */
1770 		if (!mod_delayed_work(amt_wq, &gnode->group_timer,
1771 				      msecs_to_jiffies(amt_gmi(amt))))
1772 			dev_hold(amt->dev);
1773 		gnode->filter_mode = MCAST_EXCLUDE;
1774 		/* Delete (A-B) will be worked by amt_cleanup_srcs(). */
1775 	} else {
1776 /* Router State   Report Rec'd New Router State        Actions
1777  * ------------   ------------ ----------------        -------
1778  * EXCLUDE (X,Y)  TO_EX (A)    EXCLUDE (A-Y,Y*A)       (A-X-Y)=Group Timer
1779  *						       Delete (X-A)
1780  *						       Delete (Y-A)
1781  *						       Send Q(G,A-Y)
1782  *						       Group Timer=GMI
1783  */
1784 		/* Update (A-X-Y) as NONE/OLD */
1785 		amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_SUB_REV,
1786 				    AMT_FILTER_BOTH,
1787 				    AMT_ACT_GT,
1788 				    v6);
1789 		/* EXCLUDE (A-Y, ) */
1790 		amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_SUB_REV,
1791 				    AMT_FILTER_D_FWD,
1792 				    AMT_ACT_STATUS_FWD_NEW,
1793 				    v6);
1794 		/* EXCLUDE (, Y*A) */
1795 		amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_INT,
1796 				    AMT_FILTER_D_FWD,
1797 				    AMT_ACT_STATUS_D_FWD_NEW,
1798 				    v6);
1799 		/* Group Timer=GMI */
1800 		if (!mod_delayed_work(amt_wq, &gnode->group_timer,
1801 				      msecs_to_jiffies(amt_gmi(amt))))
1802 			dev_hold(amt->dev);
1803 		/* Delete (X-A), (Y-A) will be worked by amt_cleanup_srcs(). */
1804 	}
1805 }
1806 
1807 static void amt_mcast_allow_handler(struct amt_dev *amt,
1808 				    struct amt_tunnel_list *tunnel,
1809 				    struct amt_group_node *gnode,
1810 				    void *grec, void *zero_grec, bool v6)
1811 {
1812 	if (gnode->filter_mode == MCAST_INCLUDE) {
1813 /* Router State   Report Rec'd New Router State        Actions
1814  * ------------   ------------ ----------------        -------
1815  * INCLUDE (A)    ALLOW (B)    INCLUDE (A+B)	       (B)=GMI
1816  */
1817 		/* INCLUDE (A+B) */
1818 		amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_UNI,
1819 				    AMT_FILTER_FWD,
1820 				    AMT_ACT_STATUS_FWD_NEW,
1821 				    v6);
1822 		/* (B)=GMI */
1823 		amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_INT,
1824 				    AMT_FILTER_FWD_NEW,
1825 				    AMT_ACT_GMI,
1826 				    v6);
1827 	} else {
1828 /* Router State   Report Rec'd New Router State        Actions
1829  * ------------   ------------ ----------------        -------
1830  * EXCLUDE (X,Y)  ALLOW (A)    EXCLUDE (X+A,Y-A)       (A)=GMI
1831  */
1832 		/* EXCLUDE (X+A, ) */
1833 		amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_UNI,
1834 				    AMT_FILTER_FWD,
1835 				    AMT_ACT_STATUS_FWD_NEW,
1836 				    v6);
1837 		/* EXCLUDE (, Y-A) */
1838 		amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_SUB,
1839 				    AMT_FILTER_D_FWD,
1840 				    AMT_ACT_STATUS_D_FWD_NEW,
1841 				    v6);
1842 		/* (A)=GMI
1843 		 * All (A) source are now FWD/NEW status.
1844 		 */
1845 		amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_INT,
1846 				    AMT_FILTER_FWD_NEW,
1847 				    AMT_ACT_GMI,
1848 				    v6);
1849 	}
1850 }
1851 
1852 static void amt_mcast_block_handler(struct amt_dev *amt,
1853 				    struct amt_tunnel_list *tunnel,
1854 				    struct amt_group_node *gnode,
1855 				    void *grec, void *zero_grec, bool v6)
1856 {
1857 	if (gnode->filter_mode == MCAST_INCLUDE) {
1858 /* Router State   Report Rec'd New Router State        Actions
1859  * ------------   ------------ ----------------        -------
1860  * INCLUDE (A)    BLOCK (B)    INCLUDE (A)             Send Q(G,A*B)
1861  */
1862 		/* INCLUDE (A) */
1863 		amt_lookup_act_srcs(tunnel, gnode, zero_grec, AMT_OPS_UNI,
1864 				    AMT_FILTER_FWD,
1865 				    AMT_ACT_STATUS_FWD_NEW,
1866 				    v6);
1867 	} else {
1868 /* Router State   Report Rec'd New Router State        Actions
1869  * ------------   ------------ ----------------        -------
1870  * EXCLUDE (X,Y)  BLOCK (A)    EXCLUDE (X+(A-Y),Y)     (A-X-Y)=Group Timer
1871  *						       Send Q(G,A-Y)
1872  */
1873 		/* (A-X-Y)=Group Timer */
1874 		amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_SUB_REV,
1875 				    AMT_FILTER_BOTH,
1876 				    AMT_ACT_GT,
1877 				    v6);
1878 		/* EXCLUDE (X, ) */
1879 		amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_UNI,
1880 				    AMT_FILTER_FWD,
1881 				    AMT_ACT_STATUS_FWD_NEW,
1882 				    v6);
1883 		/* EXCLUDE (X+(A-Y) */
1884 		amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_SUB_REV,
1885 				    AMT_FILTER_D_FWD,
1886 				    AMT_ACT_STATUS_FWD_NEW,
1887 				    v6);
1888 		/* EXCLUDE (, Y) */
1889 		amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_UNI,
1890 				    AMT_FILTER_D_FWD,
1891 				    AMT_ACT_STATUS_D_FWD_NEW,
1892 				    v6);
1893 	}
1894 }
1895 
1896 /* RFC 3376
1897  * 7.3.2. In the Presence of Older Version Group Members
1898  *
1899  * When Group Compatibility Mode is IGMPv2, a router internally
1900  * translates the following IGMPv2 messages for that group to their
1901  * IGMPv3 equivalents:
1902  *
1903  * IGMPv2 Message                IGMPv3 Equivalent
1904  * --------------                -----------------
1905  * Report                        IS_EX( {} )
1906  * Leave                         TO_IN( {} )
1907  */
1908 static void amt_igmpv2_report_handler(struct amt_dev *amt, struct sk_buff *skb,
1909 				      struct amt_tunnel_list *tunnel)
1910 {
1911 	struct igmphdr *ih = igmp_hdr(skb);
1912 	struct iphdr *iph = ip_hdr(skb);
1913 	struct amt_group_node *gnode;
1914 	union amt_addr group, host;
1915 
1916 	memset(&group, 0, sizeof(union amt_addr));
1917 	group.ip4 = ih->group;
1918 	memset(&host, 0, sizeof(union amt_addr));
1919 	host.ip4 = iph->saddr;
1920 
1921 	gnode = amt_lookup_group(tunnel, &group, &host, false);
1922 	if (!gnode) {
1923 		gnode = amt_add_group(amt, tunnel, &group, &host, false);
1924 		if (!IS_ERR(gnode)) {
1925 			gnode->filter_mode = MCAST_EXCLUDE;
1926 			if (!mod_delayed_work(amt_wq, &gnode->group_timer,
1927 					      msecs_to_jiffies(amt_gmi(amt))))
1928 				dev_hold(amt->dev);
1929 		}
1930 	}
1931 }
1932 
1933 /* RFC 3376
1934  * 7.3.2. In the Presence of Older Version Group Members
1935  *
1936  * When Group Compatibility Mode is IGMPv2, a router internally
1937  * translates the following IGMPv2 messages for that group to their
1938  * IGMPv3 equivalents:
1939  *
1940  * IGMPv2 Message                IGMPv3 Equivalent
1941  * --------------                -----------------
1942  * Report                        IS_EX( {} )
1943  * Leave                         TO_IN( {} )
1944  */
1945 static void amt_igmpv2_leave_handler(struct amt_dev *amt, struct sk_buff *skb,
1946 				     struct amt_tunnel_list *tunnel)
1947 {
1948 	struct igmphdr *ih = igmp_hdr(skb);
1949 	struct iphdr *iph = ip_hdr(skb);
1950 	struct amt_group_node *gnode;
1951 	union amt_addr group, host;
1952 
1953 	memset(&group, 0, sizeof(union amt_addr));
1954 	group.ip4 = ih->group;
1955 	memset(&host, 0, sizeof(union amt_addr));
1956 	host.ip4 = iph->saddr;
1957 
1958 	gnode = amt_lookup_group(tunnel, &group, &host, false);
1959 	if (gnode)
1960 		amt_del_group(amt, gnode);
1961 }
1962 
1963 static void amt_igmpv3_report_handler(struct amt_dev *amt, struct sk_buff *skb,
1964 				      struct amt_tunnel_list *tunnel)
1965 {
1966 	struct igmpv3_report *ihrv3 = igmpv3_report_hdr(skb);
1967 	int len = skb_transport_offset(skb) + sizeof(*ihrv3);
1968 	void *zero_grec = (void *)&igmpv3_zero_grec;
1969 	struct iphdr *iph = ip_hdr(skb);
1970 	struct amt_group_node *gnode;
1971 	union amt_addr group, host;
1972 	struct igmpv3_grec *grec;
1973 	u16 nsrcs;
1974 	int i;
1975 
1976 	for (i = 0; i < ntohs(ihrv3->ngrec); i++) {
1977 		len += sizeof(*grec);
1978 		if (!ip_mc_may_pull(skb, len))
1979 			break;
1980 
1981 		grec = (void *)(skb->data + len - sizeof(*grec));
1982 		nsrcs = ntohs(grec->grec_nsrcs);
1983 
1984 		len += nsrcs * sizeof(__be32);
1985 		if (!ip_mc_may_pull(skb, len))
1986 			break;
1987 
1988 		memset(&group, 0, sizeof(union amt_addr));
1989 		group.ip4 = grec->grec_mca;
1990 		memset(&host, 0, sizeof(union amt_addr));
1991 		host.ip4 = iph->saddr;
1992 		gnode = amt_lookup_group(tunnel, &group, &host, false);
1993 		if (!gnode) {
1994 			gnode = amt_add_group(amt, tunnel, &group, &host,
1995 					      false);
1996 			if (IS_ERR(gnode))
1997 				continue;
1998 		}
1999 
2000 		amt_add_srcs(amt, tunnel, gnode, grec, false);
2001 		switch (grec->grec_type) {
2002 		case IGMPV3_MODE_IS_INCLUDE:
2003 			amt_mcast_is_in_handler(amt, tunnel, gnode, grec,
2004 						zero_grec, false);
2005 			break;
2006 		case IGMPV3_MODE_IS_EXCLUDE:
2007 			amt_mcast_is_ex_handler(amt, tunnel, gnode, grec,
2008 						zero_grec, false);
2009 			break;
2010 		case IGMPV3_CHANGE_TO_INCLUDE:
2011 			amt_mcast_to_in_handler(amt, tunnel, gnode, grec,
2012 						zero_grec, false);
2013 			break;
2014 		case IGMPV3_CHANGE_TO_EXCLUDE:
2015 			amt_mcast_to_ex_handler(amt, tunnel, gnode, grec,
2016 						zero_grec, false);
2017 			break;
2018 		case IGMPV3_ALLOW_NEW_SOURCES:
2019 			amt_mcast_allow_handler(amt, tunnel, gnode, grec,
2020 						zero_grec, false);
2021 			break;
2022 		case IGMPV3_BLOCK_OLD_SOURCES:
2023 			amt_mcast_block_handler(amt, tunnel, gnode, grec,
2024 						zero_grec, false);
2025 			break;
2026 		default:
2027 			break;
2028 		}
2029 		amt_cleanup_srcs(amt, tunnel, gnode);
2030 	}
2031 }
2032 
2033 /* caller held tunnel->lock */
2034 static void amt_igmp_report_handler(struct amt_dev *amt, struct sk_buff *skb,
2035 				    struct amt_tunnel_list *tunnel)
2036 {
2037 	struct igmphdr *ih = igmp_hdr(skb);
2038 
2039 	switch (ih->type) {
2040 	case IGMPV3_HOST_MEMBERSHIP_REPORT:
2041 		amt_igmpv3_report_handler(amt, skb, tunnel);
2042 		break;
2043 	case IGMPV2_HOST_MEMBERSHIP_REPORT:
2044 		amt_igmpv2_report_handler(amt, skb, tunnel);
2045 		break;
2046 	case IGMP_HOST_LEAVE_MESSAGE:
2047 		amt_igmpv2_leave_handler(amt, skb, tunnel);
2048 		break;
2049 	default:
2050 		break;
2051 	}
2052 }
2053 
2054 #if IS_ENABLED(CONFIG_IPV6)
2055 /* RFC 3810
2056  * 8.3.2. In the Presence of MLDv1 Multicast Address Listeners
2057  *
2058  * When Multicast Address Compatibility Mode is MLDv2, a router acts
2059  * using the MLDv2 protocol for that multicast address.  When Multicast
2060  * Address Compatibility Mode is MLDv1, a router internally translates
2061  * the following MLDv1 messages for that multicast address to their
2062  * MLDv2 equivalents:
2063  *
2064  * MLDv1 Message                 MLDv2 Equivalent
2065  * --------------                -----------------
2066  * Report                        IS_EX( {} )
2067  * Done                          TO_IN( {} )
2068  */
2069 static void amt_mldv1_report_handler(struct amt_dev *amt, struct sk_buff *skb,
2070 				     struct amt_tunnel_list *tunnel)
2071 {
2072 	struct mld_msg *mld = (struct mld_msg *)icmp6_hdr(skb);
2073 	struct ipv6hdr *ip6h = ipv6_hdr(skb);
2074 	struct amt_group_node *gnode;
2075 	union amt_addr group, host;
2076 
2077 	memcpy(&group.ip6, &mld->mld_mca, sizeof(struct in6_addr));
2078 	memcpy(&host.ip6, &ip6h->saddr, sizeof(struct in6_addr));
2079 
2080 	gnode = amt_lookup_group(tunnel, &group, &host, true);
2081 	if (!gnode) {
2082 		gnode = amt_add_group(amt, tunnel, &group, &host, true);
2083 		if (!IS_ERR(gnode)) {
2084 			gnode->filter_mode = MCAST_EXCLUDE;
2085 			if (!mod_delayed_work(amt_wq, &gnode->group_timer,
2086 					      msecs_to_jiffies(amt_gmi(amt))))
2087 				dev_hold(amt->dev);
2088 		}
2089 	}
2090 }
2091 
2092 /* RFC 3810
2093  * 8.3.2. In the Presence of MLDv1 Multicast Address Listeners
2094  *
2095  * When Multicast Address Compatibility Mode is MLDv2, a router acts
2096  * using the MLDv2 protocol for that multicast address.  When Multicast
2097  * Address Compatibility Mode is MLDv1, a router internally translates
2098  * the following MLDv1 messages for that multicast address to their
2099  * MLDv2 equivalents:
2100  *
2101  * MLDv1 Message                 MLDv2 Equivalent
2102  * --------------                -----------------
2103  * Report                        IS_EX( {} )
2104  * Done                          TO_IN( {} )
2105  */
2106 static void amt_mldv1_leave_handler(struct amt_dev *amt, struct sk_buff *skb,
2107 				    struct amt_tunnel_list *tunnel)
2108 {
2109 	struct mld_msg *mld = (struct mld_msg *)icmp6_hdr(skb);
2110 	struct iphdr *iph = ip_hdr(skb);
2111 	struct amt_group_node *gnode;
2112 	union amt_addr group, host;
2113 
2114 	memcpy(&group.ip6, &mld->mld_mca, sizeof(struct in6_addr));
2115 	memset(&host, 0, sizeof(union amt_addr));
2116 	host.ip4 = iph->saddr;
2117 
2118 	gnode = amt_lookup_group(tunnel, &group, &host, true);
2119 	if (gnode) {
2120 		amt_del_group(amt, gnode);
2121 		return;
2122 	}
2123 }
2124 
2125 static void amt_mldv2_report_handler(struct amt_dev *amt, struct sk_buff *skb,
2126 				     struct amt_tunnel_list *tunnel)
2127 {
2128 	struct mld2_report *mld2r = (struct mld2_report *)icmp6_hdr(skb);
2129 	int len = skb_transport_offset(skb) + sizeof(*mld2r);
2130 	void *zero_grec = (void *)&mldv2_zero_grec;
2131 	struct ipv6hdr *ip6h = ipv6_hdr(skb);
2132 	struct amt_group_node *gnode;
2133 	union amt_addr group, host;
2134 	struct mld2_grec *grec;
2135 	u16 nsrcs;
2136 	int i;
2137 
2138 	for (i = 0; i < ntohs(mld2r->mld2r_ngrec); i++) {
2139 		len += sizeof(*grec);
2140 		if (!ipv6_mc_may_pull(skb, len))
2141 			break;
2142 
2143 		grec = (void *)(skb->data + len - sizeof(*grec));
2144 		nsrcs = ntohs(grec->grec_nsrcs);
2145 
2146 		len += nsrcs * sizeof(struct in6_addr);
2147 		if (!ipv6_mc_may_pull(skb, len))
2148 			break;
2149 
2150 		memset(&group, 0, sizeof(union amt_addr));
2151 		group.ip6 = grec->grec_mca;
2152 		memset(&host, 0, sizeof(union amt_addr));
2153 		host.ip6 = ip6h->saddr;
2154 		gnode = amt_lookup_group(tunnel, &group, &host, true);
2155 		if (!gnode) {
2156 			gnode = amt_add_group(amt, tunnel, &group, &host,
2157 					      ETH_P_IPV6);
2158 			if (IS_ERR(gnode))
2159 				continue;
2160 		}
2161 
2162 		amt_add_srcs(amt, tunnel, gnode, grec, true);
2163 		switch (grec->grec_type) {
2164 		case MLD2_MODE_IS_INCLUDE:
2165 			amt_mcast_is_in_handler(amt, tunnel, gnode, grec,
2166 						zero_grec, true);
2167 			break;
2168 		case MLD2_MODE_IS_EXCLUDE:
2169 			amt_mcast_is_ex_handler(amt, tunnel, gnode, grec,
2170 						zero_grec, true);
2171 			break;
2172 		case MLD2_CHANGE_TO_INCLUDE:
2173 			amt_mcast_to_in_handler(amt, tunnel, gnode, grec,
2174 						zero_grec, true);
2175 			break;
2176 		case MLD2_CHANGE_TO_EXCLUDE:
2177 			amt_mcast_to_ex_handler(amt, tunnel, gnode, grec,
2178 						zero_grec, true);
2179 			break;
2180 		case MLD2_ALLOW_NEW_SOURCES:
2181 			amt_mcast_allow_handler(amt, tunnel, gnode, grec,
2182 						zero_grec, true);
2183 			break;
2184 		case MLD2_BLOCK_OLD_SOURCES:
2185 			amt_mcast_block_handler(amt, tunnel, gnode, grec,
2186 						zero_grec, true);
2187 			break;
2188 		default:
2189 			break;
2190 		}
2191 		amt_cleanup_srcs(amt, tunnel, gnode);
2192 	}
2193 }
2194 
2195 /* caller held tunnel->lock */
2196 static void amt_mld_report_handler(struct amt_dev *amt, struct sk_buff *skb,
2197 				   struct amt_tunnel_list *tunnel)
2198 {
2199 	struct mld_msg *mld = (struct mld_msg *)icmp6_hdr(skb);
2200 
2201 	switch (mld->mld_type) {
2202 	case ICMPV6_MGM_REPORT:
2203 		amt_mldv1_report_handler(amt, skb, tunnel);
2204 		break;
2205 	case ICMPV6_MLD2_REPORT:
2206 		amt_mldv2_report_handler(amt, skb, tunnel);
2207 		break;
2208 	case ICMPV6_MGM_REDUCTION:
2209 		amt_mldv1_leave_handler(amt, skb, tunnel);
2210 		break;
2211 	default:
2212 		break;
2213 	}
2214 }
2215 #endif
2216 
2217 static bool amt_advertisement_handler(struct amt_dev *amt, struct sk_buff *skb)
2218 {
2219 	struct amt_header_advertisement *amta;
2220 	int hdr_size;
2221 
2222 	hdr_size = sizeof(*amta) - sizeof(struct amt_header);
2223 
2224 	if (!pskb_may_pull(skb, hdr_size))
2225 		return true;
2226 
2227 	amta = (struct amt_header_advertisement *)(udp_hdr(skb) + 1);
2228 	if (!amta->ip4)
2229 		return true;
2230 
2231 	if (amta->reserved || amta->version)
2232 		return true;
2233 
2234 	if (ipv4_is_loopback(amta->ip4) || ipv4_is_multicast(amta->ip4) ||
2235 	    ipv4_is_zeronet(amta->ip4))
2236 		return true;
2237 
2238 	amt->remote_ip = amta->ip4;
2239 	netdev_dbg(amt->dev, "advertised remote ip = %pI4\n", &amt->remote_ip);
2240 	mod_delayed_work(amt_wq, &amt->req_wq, 0);
2241 
2242 	amt_update_gw_status(amt, AMT_STATUS_RECEIVED_ADVERTISEMENT, true);
2243 	return false;
2244 }
2245 
2246 static bool amt_multicast_data_handler(struct amt_dev *amt, struct sk_buff *skb)
2247 {
2248 	struct amt_header_mcast_data *amtmd;
2249 	int hdr_size, len, err;
2250 	struct ethhdr *eth;
2251 	struct iphdr *iph;
2252 
2253 	amtmd = (struct amt_header_mcast_data *)(udp_hdr(skb) + 1);
2254 	if (amtmd->reserved || amtmd->version)
2255 		return true;
2256 
2257 	hdr_size = sizeof(*amtmd) + sizeof(struct udphdr);
2258 	if (iptunnel_pull_header(skb, hdr_size, htons(ETH_P_IP), false))
2259 		return true;
2260 	skb_reset_network_header(skb);
2261 	skb_push(skb, sizeof(*eth));
2262 	skb_reset_mac_header(skb);
2263 	skb_pull(skb, sizeof(*eth));
2264 	eth = eth_hdr(skb);
2265 	iph = ip_hdr(skb);
2266 	if (iph->version == 4) {
2267 		if (!ipv4_is_multicast(iph->daddr))
2268 			return true;
2269 		skb->protocol = htons(ETH_P_IP);
2270 		eth->h_proto = htons(ETH_P_IP);
2271 		ip_eth_mc_map(iph->daddr, eth->h_dest);
2272 #if IS_ENABLED(CONFIG_IPV6)
2273 	} else if (iph->version == 6) {
2274 		struct ipv6hdr *ip6h;
2275 
2276 		ip6h = ipv6_hdr(skb);
2277 		if (!ipv6_addr_is_multicast(&ip6h->daddr))
2278 			return true;
2279 		skb->protocol = htons(ETH_P_IPV6);
2280 		eth->h_proto = htons(ETH_P_IPV6);
2281 		ipv6_eth_mc_map(&ip6h->daddr, eth->h_dest);
2282 #endif
2283 	} else {
2284 		return true;
2285 	}
2286 
2287 	skb->pkt_type = PACKET_MULTICAST;
2288 	skb->ip_summed = CHECKSUM_NONE;
2289 	len = skb->len;
2290 	err = gro_cells_receive(&amt->gro_cells, skb);
2291 	if (likely(err == NET_RX_SUCCESS))
2292 		dev_sw_netstats_rx_add(amt->dev, len);
2293 	else
2294 		amt->dev->stats.rx_dropped++;
2295 
2296 	return false;
2297 }
2298 
2299 static bool amt_membership_query_handler(struct amt_dev *amt,
2300 					 struct sk_buff *skb)
2301 {
2302 	struct amt_header_membership_query *amtmq;
2303 	struct igmpv3_query *ihv3;
2304 	struct ethhdr *eth, *oeth;
2305 	struct iphdr *iph;
2306 	int hdr_size, len;
2307 
2308 	hdr_size = sizeof(*amtmq) - sizeof(struct amt_header);
2309 
2310 	if (!pskb_may_pull(skb, hdr_size))
2311 		return true;
2312 
2313 	amtmq = (struct amt_header_membership_query *)(udp_hdr(skb) + 1);
2314 	if (amtmq->reserved || amtmq->version)
2315 		return true;
2316 
2317 	hdr_size = sizeof(*amtmq) + sizeof(struct udphdr) - sizeof(*eth);
2318 	if (iptunnel_pull_header(skb, hdr_size, htons(ETH_P_TEB), false))
2319 		return true;
2320 	oeth = eth_hdr(skb);
2321 	skb_reset_mac_header(skb);
2322 	skb_pull(skb, sizeof(*eth));
2323 	skb_reset_network_header(skb);
2324 	eth = eth_hdr(skb);
2325 	iph = ip_hdr(skb);
2326 	if (iph->version == 4) {
2327 		if (!ipv4_is_multicast(iph->daddr))
2328 			return true;
2329 		if (!pskb_may_pull(skb, sizeof(*iph) + AMT_IPHDR_OPTS +
2330 				   sizeof(*ihv3)))
2331 			return true;
2332 
2333 		ihv3 = skb_pull(skb, sizeof(*iph) + AMT_IPHDR_OPTS);
2334 		skb_reset_transport_header(skb);
2335 		skb_push(skb, sizeof(*iph) + AMT_IPHDR_OPTS);
2336 		spin_lock_bh(&amt->lock);
2337 		amt->ready4 = true;
2338 		amt->mac = amtmq->response_mac;
2339 		amt->req_cnt = 0;
2340 		amt->qi = ihv3->qqic;
2341 		spin_unlock_bh(&amt->lock);
2342 		skb->protocol = htons(ETH_P_IP);
2343 		eth->h_proto = htons(ETH_P_IP);
2344 		ip_eth_mc_map(iph->daddr, eth->h_dest);
2345 #if IS_ENABLED(CONFIG_IPV6)
2346 	} else if (iph->version == 6) {
2347 		struct ipv6hdr *ip6h = ipv6_hdr(skb);
2348 		struct mld2_query *mld2q;
2349 
2350 		if (!ipv6_addr_is_multicast(&ip6h->daddr))
2351 			return true;
2352 		if (!pskb_may_pull(skb, sizeof(*ip6h) + AMT_IP6HDR_OPTS +
2353 				   sizeof(*mld2q)))
2354 			return true;
2355 
2356 		mld2q = skb_pull(skb, sizeof(*ip6h) + AMT_IP6HDR_OPTS);
2357 		skb_reset_transport_header(skb);
2358 		skb_push(skb, sizeof(*ip6h) + AMT_IP6HDR_OPTS);
2359 		spin_lock_bh(&amt->lock);
2360 		amt->ready6 = true;
2361 		amt->mac = amtmq->response_mac;
2362 		amt->req_cnt = 0;
2363 		amt->qi = mld2q->mld2q_qqic;
2364 		spin_unlock_bh(&amt->lock);
2365 		skb->protocol = htons(ETH_P_IPV6);
2366 		eth->h_proto = htons(ETH_P_IPV6);
2367 		ipv6_eth_mc_map(&ip6h->daddr, eth->h_dest);
2368 #endif
2369 	} else {
2370 		return true;
2371 	}
2372 
2373 	ether_addr_copy(eth->h_source, oeth->h_source);
2374 	skb->pkt_type = PACKET_MULTICAST;
2375 	skb->ip_summed = CHECKSUM_NONE;
2376 	len = skb->len;
2377 	if (netif_rx(skb) == NET_RX_SUCCESS) {
2378 		amt_update_gw_status(amt, AMT_STATUS_RECEIVED_QUERY, true);
2379 		dev_sw_netstats_rx_add(amt->dev, len);
2380 	} else {
2381 		amt->dev->stats.rx_dropped++;
2382 	}
2383 
2384 	return false;
2385 }
2386 
2387 static bool amt_update_handler(struct amt_dev *amt, struct sk_buff *skb)
2388 {
2389 	struct amt_header_membership_update *amtmu;
2390 	struct amt_tunnel_list *tunnel;
2391 	struct udphdr *udph;
2392 	struct ethhdr *eth;
2393 	struct iphdr *iph;
2394 	int len;
2395 
2396 	iph = ip_hdr(skb);
2397 	udph = udp_hdr(skb);
2398 
2399 	if (__iptunnel_pull_header(skb, sizeof(*udph), skb->protocol,
2400 				   false, false))
2401 		return true;
2402 
2403 	amtmu = (struct amt_header_membership_update *)skb->data;
2404 	if (amtmu->reserved || amtmu->version)
2405 		return true;
2406 
2407 	skb_pull(skb, sizeof(*amtmu));
2408 	skb_reset_network_header(skb);
2409 
2410 	list_for_each_entry_rcu(tunnel, &amt->tunnel_list, list) {
2411 		if (tunnel->ip4 == iph->saddr) {
2412 			if ((amtmu->nonce == tunnel->nonce &&
2413 			     amtmu->response_mac == tunnel->mac)) {
2414 				mod_delayed_work(amt_wq, &tunnel->gc_wq,
2415 						 msecs_to_jiffies(amt_gmi(amt))
2416 								  * 3);
2417 				goto report;
2418 			} else {
2419 				netdev_dbg(amt->dev, "Invalid MAC\n");
2420 				return true;
2421 			}
2422 		}
2423 	}
2424 
2425 	return false;
2426 
2427 report:
2428 	iph = ip_hdr(skb);
2429 	if (iph->version == 4) {
2430 		if (ip_mc_check_igmp(skb)) {
2431 			netdev_dbg(amt->dev, "Invalid IGMP\n");
2432 			return true;
2433 		}
2434 
2435 		spin_lock_bh(&tunnel->lock);
2436 		amt_igmp_report_handler(amt, skb, tunnel);
2437 		spin_unlock_bh(&tunnel->lock);
2438 
2439 		skb_push(skb, sizeof(struct ethhdr));
2440 		skb_reset_mac_header(skb);
2441 		eth = eth_hdr(skb);
2442 		skb->protocol = htons(ETH_P_IP);
2443 		eth->h_proto = htons(ETH_P_IP);
2444 		ip_eth_mc_map(iph->daddr, eth->h_dest);
2445 #if IS_ENABLED(CONFIG_IPV6)
2446 	} else if (iph->version == 6) {
2447 		struct ipv6hdr *ip6h = ipv6_hdr(skb);
2448 
2449 		if (ipv6_mc_check_mld(skb)) {
2450 			netdev_dbg(amt->dev, "Invalid MLD\n");
2451 			return true;
2452 		}
2453 
2454 		spin_lock_bh(&tunnel->lock);
2455 		amt_mld_report_handler(amt, skb, tunnel);
2456 		spin_unlock_bh(&tunnel->lock);
2457 
2458 		skb_push(skb, sizeof(struct ethhdr));
2459 		skb_reset_mac_header(skb);
2460 		eth = eth_hdr(skb);
2461 		skb->protocol = htons(ETH_P_IPV6);
2462 		eth->h_proto = htons(ETH_P_IPV6);
2463 		ipv6_eth_mc_map(&ip6h->daddr, eth->h_dest);
2464 #endif
2465 	} else {
2466 		netdev_dbg(amt->dev, "Unsupported Protocol\n");
2467 		return true;
2468 	}
2469 
2470 	skb_pull(skb, sizeof(struct ethhdr));
2471 	skb->pkt_type = PACKET_MULTICAST;
2472 	skb->ip_summed = CHECKSUM_NONE;
2473 	len = skb->len;
2474 	if (netif_rx(skb) == NET_RX_SUCCESS) {
2475 		amt_update_relay_status(tunnel, AMT_STATUS_RECEIVED_UPDATE,
2476 					true);
2477 		dev_sw_netstats_rx_add(amt->dev, len);
2478 	} else {
2479 		amt->dev->stats.rx_dropped++;
2480 	}
2481 
2482 	return false;
2483 }
2484 
2485 static void amt_send_advertisement(struct amt_dev *amt, __be32 nonce,
2486 				   __be32 daddr, __be16 dport)
2487 {
2488 	struct amt_header_advertisement *amta;
2489 	int hlen, tlen, offset;
2490 	struct socket *sock;
2491 	struct udphdr *udph;
2492 	struct sk_buff *skb;
2493 	struct iphdr *iph;
2494 	struct rtable *rt;
2495 	struct flowi4 fl4;
2496 	u32 len;
2497 	int err;
2498 
2499 	rcu_read_lock();
2500 	sock = rcu_dereference(amt->sock);
2501 	if (!sock)
2502 		goto out;
2503 
2504 	if (!netif_running(amt->stream_dev) || !netif_running(amt->dev))
2505 		goto out;
2506 
2507 	rt = ip_route_output_ports(amt->net, &fl4, sock->sk,
2508 				   daddr, amt->local_ip,
2509 				   dport, amt->relay_port,
2510 				   IPPROTO_UDP, 0,
2511 				   amt->stream_dev->ifindex);
2512 	if (IS_ERR(rt)) {
2513 		amt->dev->stats.tx_errors++;
2514 		goto out;
2515 	}
2516 
2517 	hlen = LL_RESERVED_SPACE(amt->dev);
2518 	tlen = amt->dev->needed_tailroom;
2519 	len = hlen + tlen + sizeof(*iph) + sizeof(*udph) + sizeof(*amta);
2520 	skb = netdev_alloc_skb_ip_align(amt->dev, len);
2521 	if (!skb) {
2522 		ip_rt_put(rt);
2523 		amt->dev->stats.tx_errors++;
2524 		goto out;
2525 	}
2526 
2527 	skb->priority = TC_PRIO_CONTROL;
2528 	skb_dst_set(skb, &rt->dst);
2529 
2530 	len = sizeof(*iph) + sizeof(*udph) + sizeof(*amta);
2531 	skb_reset_network_header(skb);
2532 	skb_put(skb, len);
2533 	amta = skb_pull(skb, sizeof(*iph) + sizeof(*udph));
2534 	amta->version	= 0;
2535 	amta->type	= AMT_MSG_ADVERTISEMENT;
2536 	amta->reserved	= 0;
2537 	amta->nonce	= nonce;
2538 	amta->ip4	= amt->local_ip;
2539 	skb_push(skb, sizeof(*udph));
2540 	skb_reset_transport_header(skb);
2541 	udph		= udp_hdr(skb);
2542 	udph->source	= amt->relay_port;
2543 	udph->dest	= dport;
2544 	udph->len	= htons(sizeof(*amta) + sizeof(*udph));
2545 	udph->check	= 0;
2546 	offset = skb_transport_offset(skb);
2547 	skb->csum = skb_checksum(skb, offset, skb->len - offset, 0);
2548 	udph->check = csum_tcpudp_magic(amt->local_ip, daddr,
2549 					sizeof(*udph) + sizeof(*amta),
2550 					IPPROTO_UDP, skb->csum);
2551 
2552 	skb_push(skb, sizeof(*iph));
2553 	iph		= ip_hdr(skb);
2554 	iph->version	= 4;
2555 	iph->ihl	= (sizeof(struct iphdr)) >> 2;
2556 	iph->tos	= AMT_TOS;
2557 	iph->frag_off	= 0;
2558 	iph->ttl	= ip4_dst_hoplimit(&rt->dst);
2559 	iph->daddr	= daddr;
2560 	iph->saddr	= amt->local_ip;
2561 	iph->protocol	= IPPROTO_UDP;
2562 	iph->tot_len	= htons(len);
2563 
2564 	skb->ip_summed = CHECKSUM_NONE;
2565 	ip_select_ident(amt->net, skb, NULL);
2566 	ip_send_check(iph);
2567 	err = ip_local_out(amt->net, sock->sk, skb);
2568 	if (unlikely(net_xmit_eval(err)))
2569 		amt->dev->stats.tx_errors++;
2570 
2571 out:
2572 	rcu_read_unlock();
2573 }
2574 
2575 static bool amt_discovery_handler(struct amt_dev *amt, struct sk_buff *skb)
2576 {
2577 	struct amt_header_discovery *amtd;
2578 	struct udphdr *udph;
2579 	struct iphdr *iph;
2580 
2581 	if (!pskb_may_pull(skb, sizeof(*udph) + sizeof(*amtd)))
2582 		return true;
2583 
2584 	iph = ip_hdr(skb);
2585 	udph = udp_hdr(skb);
2586 	amtd = (struct amt_header_discovery *)(udp_hdr(skb) + 1);
2587 
2588 	if (amtd->reserved || amtd->version)
2589 		return true;
2590 
2591 	amt_send_advertisement(amt, amtd->nonce, iph->saddr, udph->source);
2592 
2593 	return false;
2594 }
2595 
2596 static bool amt_request_handler(struct amt_dev *amt, struct sk_buff *skb)
2597 {
2598 	struct amt_header_request *amtrh;
2599 	struct amt_tunnel_list *tunnel;
2600 	unsigned long long key;
2601 	struct udphdr *udph;
2602 	struct iphdr *iph;
2603 	u64 mac;
2604 	int i;
2605 
2606 	if (!pskb_may_pull(skb, sizeof(*udph) + sizeof(*amtrh)))
2607 		return true;
2608 
2609 	iph = ip_hdr(skb);
2610 	udph = udp_hdr(skb);
2611 	amtrh = (struct amt_header_request *)(udp_hdr(skb) + 1);
2612 
2613 	if (amtrh->reserved1 || amtrh->reserved2 || amtrh->version)
2614 		return true;
2615 
2616 	list_for_each_entry_rcu(tunnel, &amt->tunnel_list, list)
2617 		if (tunnel->ip4 == iph->saddr)
2618 			goto send;
2619 
2620 	if (amt->nr_tunnels >= amt->max_tunnels) {
2621 		icmp_ndo_send(skb, ICMP_DEST_UNREACH, ICMP_HOST_UNREACH, 0);
2622 		return true;
2623 	}
2624 
2625 	tunnel = kzalloc(sizeof(*tunnel) +
2626 			 (sizeof(struct hlist_head) * amt->hash_buckets),
2627 			 GFP_ATOMIC);
2628 	if (!tunnel)
2629 		return true;
2630 
2631 	tunnel->source_port = udph->source;
2632 	tunnel->ip4 = iph->saddr;
2633 
2634 	memcpy(&key, &tunnel->key, sizeof(unsigned long long));
2635 	tunnel->amt = amt;
2636 	spin_lock_init(&tunnel->lock);
2637 	for (i = 0; i < amt->hash_buckets; i++)
2638 		INIT_HLIST_HEAD(&tunnel->groups[i]);
2639 
2640 	INIT_DELAYED_WORK(&tunnel->gc_wq, amt_tunnel_expire);
2641 
2642 	spin_lock_bh(&amt->lock);
2643 	list_add_tail_rcu(&tunnel->list, &amt->tunnel_list);
2644 	tunnel->key = amt->key;
2645 	amt_update_relay_status(tunnel, AMT_STATUS_RECEIVED_REQUEST, true);
2646 	amt->nr_tunnels++;
2647 	mod_delayed_work(amt_wq, &tunnel->gc_wq,
2648 			 msecs_to_jiffies(amt_gmi(amt)));
2649 	spin_unlock_bh(&amt->lock);
2650 
2651 send:
2652 	tunnel->nonce = amtrh->nonce;
2653 	mac = siphash_3u32((__force u32)tunnel->ip4,
2654 			   (__force u32)tunnel->source_port,
2655 			   (__force u32)tunnel->nonce,
2656 			   &tunnel->key);
2657 	tunnel->mac = mac >> 16;
2658 
2659 	if (!netif_running(amt->dev) || !netif_running(amt->stream_dev))
2660 		return true;
2661 
2662 	if (!amtrh->p)
2663 		amt_send_igmp_gq(amt, tunnel);
2664 	else
2665 		amt_send_mld_gq(amt, tunnel);
2666 
2667 	return false;
2668 }
2669 
2670 static int amt_rcv(struct sock *sk, struct sk_buff *skb)
2671 {
2672 	struct amt_dev *amt;
2673 	struct iphdr *iph;
2674 	int type;
2675 	bool err;
2676 
2677 	rcu_read_lock_bh();
2678 	amt = rcu_dereference_sk_user_data(sk);
2679 	if (!amt) {
2680 		err = true;
2681 		goto out;
2682 	}
2683 
2684 	skb->dev = amt->dev;
2685 	iph = ip_hdr(skb);
2686 	type = amt_parse_type(skb);
2687 	if (type == -1) {
2688 		err = true;
2689 		goto drop;
2690 	}
2691 
2692 	if (amt->mode == AMT_MODE_GATEWAY) {
2693 		switch (type) {
2694 		case AMT_MSG_ADVERTISEMENT:
2695 			if (iph->saddr != amt->discovery_ip) {
2696 				netdev_dbg(amt->dev, "Invalid Relay IP\n");
2697 				err = true;
2698 				goto drop;
2699 			}
2700 			if (amt_advertisement_handler(amt, skb))
2701 				amt->dev->stats.rx_dropped++;
2702 			goto out;
2703 		case AMT_MSG_MULTICAST_DATA:
2704 			if (iph->saddr != amt->remote_ip) {
2705 				netdev_dbg(amt->dev, "Invalid Relay IP\n");
2706 				err = true;
2707 				goto drop;
2708 			}
2709 			err = amt_multicast_data_handler(amt, skb);
2710 			if (err)
2711 				goto drop;
2712 			else
2713 				goto out;
2714 		case AMT_MSG_MEMBERSHIP_QUERY:
2715 			if (iph->saddr != amt->remote_ip) {
2716 				netdev_dbg(amt->dev, "Invalid Relay IP\n");
2717 				err = true;
2718 				goto drop;
2719 			}
2720 			err = amt_membership_query_handler(amt, skb);
2721 			if (err)
2722 				goto drop;
2723 			else
2724 				goto out;
2725 		default:
2726 			err = true;
2727 			netdev_dbg(amt->dev, "Invalid type of Gateway\n");
2728 			break;
2729 		}
2730 	} else {
2731 		switch (type) {
2732 		case AMT_MSG_DISCOVERY:
2733 			err = amt_discovery_handler(amt, skb);
2734 			break;
2735 		case AMT_MSG_REQUEST:
2736 			err = amt_request_handler(amt, skb);
2737 			break;
2738 		case AMT_MSG_MEMBERSHIP_UPDATE:
2739 			err = amt_update_handler(amt, skb);
2740 			if (err)
2741 				goto drop;
2742 			else
2743 				goto out;
2744 		default:
2745 			err = true;
2746 			netdev_dbg(amt->dev, "Invalid type of relay\n");
2747 			break;
2748 		}
2749 	}
2750 drop:
2751 	if (err) {
2752 		amt->dev->stats.rx_dropped++;
2753 		kfree_skb(skb);
2754 	} else {
2755 		consume_skb(skb);
2756 	}
2757 out:
2758 	rcu_read_unlock_bh();
2759 	return 0;
2760 }
2761 
2762 static int amt_err_lookup(struct sock *sk, struct sk_buff *skb)
2763 {
2764 	struct amt_dev *amt;
2765 	int type;
2766 
2767 	rcu_read_lock_bh();
2768 	amt = rcu_dereference_sk_user_data(sk);
2769 	if (!amt)
2770 		goto drop;
2771 
2772 	if (amt->mode != AMT_MODE_GATEWAY)
2773 		goto drop;
2774 
2775 	type = amt_parse_type(skb);
2776 	if (type == -1)
2777 		goto drop;
2778 
2779 	netdev_dbg(amt->dev, "Received IGMP Unreachable of %s\n",
2780 		   type_str[type]);
2781 	switch (type) {
2782 	case AMT_MSG_DISCOVERY:
2783 		break;
2784 	case AMT_MSG_REQUEST:
2785 	case AMT_MSG_MEMBERSHIP_UPDATE:
2786 		if (amt->status >= AMT_STATUS_RECEIVED_ADVERTISEMENT)
2787 			mod_delayed_work(amt_wq, &amt->req_wq, 0);
2788 		break;
2789 	default:
2790 		goto drop;
2791 	}
2792 	rcu_read_unlock_bh();
2793 	return 0;
2794 drop:
2795 	rcu_read_unlock_bh();
2796 	amt->dev->stats.rx_dropped++;
2797 	return 0;
2798 }
2799 
2800 static struct socket *amt_create_sock(struct net *net, __be16 port)
2801 {
2802 	struct udp_port_cfg udp_conf;
2803 	struct socket *sock;
2804 	int err;
2805 
2806 	memset(&udp_conf, 0, sizeof(udp_conf));
2807 	udp_conf.family = AF_INET;
2808 	udp_conf.local_ip.s_addr = htonl(INADDR_ANY);
2809 
2810 	udp_conf.local_udp_port = port;
2811 
2812 	err = udp_sock_create(net, &udp_conf, &sock);
2813 	if (err < 0)
2814 		return ERR_PTR(err);
2815 
2816 	return sock;
2817 }
2818 
2819 static int amt_socket_create(struct amt_dev *amt)
2820 {
2821 	struct udp_tunnel_sock_cfg tunnel_cfg;
2822 	struct socket *sock;
2823 
2824 	sock = amt_create_sock(amt->net, amt->relay_port);
2825 	if (IS_ERR(sock))
2826 		return PTR_ERR(sock);
2827 
2828 	/* Mark socket as an encapsulation socket */
2829 	memset(&tunnel_cfg, 0, sizeof(tunnel_cfg));
2830 	tunnel_cfg.sk_user_data = amt;
2831 	tunnel_cfg.encap_type = 1;
2832 	tunnel_cfg.encap_rcv = amt_rcv;
2833 	tunnel_cfg.encap_err_lookup = amt_err_lookup;
2834 	tunnel_cfg.encap_destroy = NULL;
2835 	setup_udp_tunnel_sock(amt->net, sock, &tunnel_cfg);
2836 
2837 	rcu_assign_pointer(amt->sock, sock);
2838 	return 0;
2839 }
2840 
2841 static int amt_dev_open(struct net_device *dev)
2842 {
2843 	struct amt_dev *amt = netdev_priv(dev);
2844 	int err;
2845 
2846 	amt->ready4 = false;
2847 	amt->ready6 = false;
2848 
2849 	err = amt_socket_create(amt);
2850 	if (err)
2851 		return err;
2852 
2853 	amt->req_cnt = 0;
2854 	amt->remote_ip = 0;
2855 	get_random_bytes(&amt->key, sizeof(siphash_key_t));
2856 
2857 	amt->status = AMT_STATUS_INIT;
2858 	if (amt->mode == AMT_MODE_GATEWAY) {
2859 		mod_delayed_work(amt_wq, &amt->discovery_wq, 0);
2860 		mod_delayed_work(amt_wq, &amt->req_wq, 0);
2861 	} else if (amt->mode == AMT_MODE_RELAY) {
2862 		mod_delayed_work(amt_wq, &amt->secret_wq,
2863 				 msecs_to_jiffies(AMT_SECRET_TIMEOUT));
2864 	}
2865 	return err;
2866 }
2867 
2868 static int amt_dev_stop(struct net_device *dev)
2869 {
2870 	struct amt_dev *amt = netdev_priv(dev);
2871 	struct amt_tunnel_list *tunnel, *tmp;
2872 	struct socket *sock;
2873 
2874 	cancel_delayed_work_sync(&amt->req_wq);
2875 	cancel_delayed_work_sync(&amt->discovery_wq);
2876 	cancel_delayed_work_sync(&amt->secret_wq);
2877 
2878 	/* shutdown */
2879 	sock = rtnl_dereference(amt->sock);
2880 	RCU_INIT_POINTER(amt->sock, NULL);
2881 	synchronize_net();
2882 	if (sock)
2883 		udp_tunnel_sock_release(sock);
2884 
2885 	amt->ready4 = false;
2886 	amt->ready6 = false;
2887 	amt->req_cnt = 0;
2888 	amt->remote_ip = 0;
2889 
2890 	list_for_each_entry_safe(tunnel, tmp, &amt->tunnel_list, list) {
2891 		list_del_rcu(&tunnel->list);
2892 		amt->nr_tunnels--;
2893 		cancel_delayed_work_sync(&tunnel->gc_wq);
2894 		amt_clear_groups(tunnel);
2895 		kfree_rcu(tunnel, rcu);
2896 	}
2897 
2898 	return 0;
2899 }
2900 
2901 static const struct device_type amt_type = {
2902 	.name = "amt",
2903 };
2904 
2905 static int amt_dev_init(struct net_device *dev)
2906 {
2907 	struct amt_dev *amt = netdev_priv(dev);
2908 	int err;
2909 
2910 	amt->dev = dev;
2911 	dev->tstats = netdev_alloc_pcpu_stats(struct pcpu_sw_netstats);
2912 	if (!dev->tstats)
2913 		return -ENOMEM;
2914 
2915 	err = gro_cells_init(&amt->gro_cells, dev);
2916 	if (err) {
2917 		free_percpu(dev->tstats);
2918 		return err;
2919 	}
2920 
2921 	return 0;
2922 }
2923 
2924 static void amt_dev_uninit(struct net_device *dev)
2925 {
2926 	struct amt_dev *amt = netdev_priv(dev);
2927 
2928 	gro_cells_destroy(&amt->gro_cells);
2929 	free_percpu(dev->tstats);
2930 }
2931 
2932 static const struct net_device_ops amt_netdev_ops = {
2933 	.ndo_init               = amt_dev_init,
2934 	.ndo_uninit             = amt_dev_uninit,
2935 	.ndo_open		= amt_dev_open,
2936 	.ndo_stop		= amt_dev_stop,
2937 	.ndo_start_xmit         = amt_dev_xmit,
2938 	.ndo_get_stats64        = dev_get_tstats64,
2939 };
2940 
2941 static void amt_link_setup(struct net_device *dev)
2942 {
2943 	dev->netdev_ops         = &amt_netdev_ops;
2944 	dev->needs_free_netdev  = true;
2945 	SET_NETDEV_DEVTYPE(dev, &amt_type);
2946 	dev->min_mtu		= ETH_MIN_MTU;
2947 	dev->max_mtu		= ETH_MAX_MTU;
2948 	dev->type		= ARPHRD_NONE;
2949 	dev->flags		= IFF_POINTOPOINT | IFF_NOARP | IFF_MULTICAST;
2950 	dev->hard_header_len	= 0;
2951 	dev->addr_len		= 0;
2952 	dev->priv_flags		|= IFF_NO_QUEUE;
2953 	dev->features		|= NETIF_F_LLTX;
2954 	dev->features		|= NETIF_F_GSO_SOFTWARE;
2955 	dev->features		|= NETIF_F_NETNS_LOCAL;
2956 	dev->hw_features	|= NETIF_F_SG | NETIF_F_HW_CSUM;
2957 	dev->hw_features	|= NETIF_F_FRAGLIST | NETIF_F_RXCSUM;
2958 	dev->hw_features	|= NETIF_F_GSO_SOFTWARE;
2959 	eth_hw_addr_random(dev);
2960 	eth_zero_addr(dev->broadcast);
2961 	ether_setup(dev);
2962 }
2963 
2964 static const struct nla_policy amt_policy[IFLA_AMT_MAX + 1] = {
2965 	[IFLA_AMT_MODE]		= { .type = NLA_U32 },
2966 	[IFLA_AMT_RELAY_PORT]	= { .type = NLA_U16 },
2967 	[IFLA_AMT_GATEWAY_PORT]	= { .type = NLA_U16 },
2968 	[IFLA_AMT_LINK]		= { .type = NLA_U32 },
2969 	[IFLA_AMT_LOCAL_IP]	= { .len = sizeof_field(struct iphdr, daddr) },
2970 	[IFLA_AMT_REMOTE_IP]	= { .len = sizeof_field(struct iphdr, daddr) },
2971 	[IFLA_AMT_DISCOVERY_IP]	= { .len = sizeof_field(struct iphdr, daddr) },
2972 	[IFLA_AMT_MAX_TUNNELS]	= { .type = NLA_U32 },
2973 };
2974 
2975 static int amt_validate(struct nlattr *tb[], struct nlattr *data[],
2976 			struct netlink_ext_ack *extack)
2977 {
2978 	if (!data)
2979 		return -EINVAL;
2980 
2981 	if (!data[IFLA_AMT_LINK]) {
2982 		NL_SET_ERR_MSG_ATTR(extack, data[IFLA_AMT_LINK],
2983 				    "Link attribute is required");
2984 		return -EINVAL;
2985 	}
2986 
2987 	if (!data[IFLA_AMT_MODE]) {
2988 		NL_SET_ERR_MSG_ATTR(extack, data[IFLA_AMT_MODE],
2989 				    "Mode attribute is required");
2990 		return -EINVAL;
2991 	}
2992 
2993 	if (nla_get_u32(data[IFLA_AMT_MODE]) > AMT_MODE_MAX) {
2994 		NL_SET_ERR_MSG_ATTR(extack, data[IFLA_AMT_MODE],
2995 				    "Mode attribute is not valid");
2996 		return -EINVAL;
2997 	}
2998 
2999 	if (!data[IFLA_AMT_LOCAL_IP]) {
3000 		NL_SET_ERR_MSG_ATTR(extack, data[IFLA_AMT_DISCOVERY_IP],
3001 				    "Local attribute is required");
3002 		return -EINVAL;
3003 	}
3004 
3005 	if (!data[IFLA_AMT_DISCOVERY_IP] &&
3006 	    nla_get_u32(data[IFLA_AMT_MODE]) == AMT_MODE_GATEWAY) {
3007 		NL_SET_ERR_MSG_ATTR(extack, data[IFLA_AMT_LOCAL_IP],
3008 				    "Discovery attribute is required");
3009 		return -EINVAL;
3010 	}
3011 
3012 	return 0;
3013 }
3014 
3015 static int amt_newlink(struct net *net, struct net_device *dev,
3016 		       struct nlattr *tb[], struct nlattr *data[],
3017 		       struct netlink_ext_ack *extack)
3018 {
3019 	struct amt_dev *amt = netdev_priv(dev);
3020 	int err = -EINVAL;
3021 
3022 	amt->net = net;
3023 	amt->mode = nla_get_u32(data[IFLA_AMT_MODE]);
3024 
3025 	if (data[IFLA_AMT_MAX_TUNNELS] &&
3026 	    nla_get_u32(data[IFLA_AMT_MAX_TUNNELS]))
3027 		amt->max_tunnels = nla_get_u32(data[IFLA_AMT_MAX_TUNNELS]);
3028 	else
3029 		amt->max_tunnels = AMT_MAX_TUNNELS;
3030 
3031 	spin_lock_init(&amt->lock);
3032 	amt->max_groups = AMT_MAX_GROUP;
3033 	amt->max_sources = AMT_MAX_SOURCE;
3034 	amt->hash_buckets = AMT_HSIZE;
3035 	amt->nr_tunnels = 0;
3036 	get_random_bytes(&amt->hash_seed, sizeof(amt->hash_seed));
3037 	amt->stream_dev = dev_get_by_index(net,
3038 					   nla_get_u32(data[IFLA_AMT_LINK]));
3039 	if (!amt->stream_dev) {
3040 		NL_SET_ERR_MSG_ATTR(extack, tb[IFLA_AMT_LINK],
3041 				    "Can't find stream device");
3042 		return -ENODEV;
3043 	}
3044 
3045 	if (amt->stream_dev->type != ARPHRD_ETHER) {
3046 		NL_SET_ERR_MSG_ATTR(extack, tb[IFLA_AMT_LINK],
3047 				    "Invalid stream device type");
3048 		goto err;
3049 	}
3050 
3051 	amt->local_ip = nla_get_in_addr(data[IFLA_AMT_LOCAL_IP]);
3052 	if (ipv4_is_loopback(amt->local_ip) ||
3053 	    ipv4_is_zeronet(amt->local_ip) ||
3054 	    ipv4_is_multicast(amt->local_ip)) {
3055 		NL_SET_ERR_MSG_ATTR(extack, tb[IFLA_AMT_LOCAL_IP],
3056 				    "Invalid Local address");
3057 		goto err;
3058 	}
3059 
3060 	if (data[IFLA_AMT_RELAY_PORT])
3061 		amt->relay_port = nla_get_be16(data[IFLA_AMT_RELAY_PORT]);
3062 	else
3063 		amt->relay_port = htons(IANA_AMT_UDP_PORT);
3064 
3065 	if (data[IFLA_AMT_GATEWAY_PORT])
3066 		amt->gw_port = nla_get_be16(data[IFLA_AMT_GATEWAY_PORT]);
3067 	else
3068 		amt->gw_port = htons(IANA_AMT_UDP_PORT);
3069 
3070 	if (!amt->relay_port) {
3071 		NL_SET_ERR_MSG_ATTR(extack, tb[IFLA_AMT_DISCOVERY_IP],
3072 				    "relay port must not be 0");
3073 		goto err;
3074 	}
3075 	if (amt->mode == AMT_MODE_RELAY) {
3076 		amt->qrv = amt->net->ipv4.sysctl_igmp_qrv;
3077 		amt->qri = 10;
3078 		dev->needed_headroom = amt->stream_dev->needed_headroom +
3079 				       AMT_RELAY_HLEN;
3080 		dev->mtu = amt->stream_dev->mtu - AMT_RELAY_HLEN;
3081 		dev->max_mtu = dev->mtu;
3082 		dev->min_mtu = ETH_MIN_MTU + AMT_RELAY_HLEN;
3083 	} else {
3084 		if (!data[IFLA_AMT_DISCOVERY_IP]) {
3085 			NL_SET_ERR_MSG_ATTR(extack, tb[IFLA_AMT_DISCOVERY_IP],
3086 					    "discovery must be set in gateway mode");
3087 			goto err;
3088 		}
3089 		if (!amt->gw_port) {
3090 			NL_SET_ERR_MSG_ATTR(extack, tb[IFLA_AMT_DISCOVERY_IP],
3091 					    "gateway port must not be 0");
3092 			goto err;
3093 		}
3094 		amt->remote_ip = 0;
3095 		amt->discovery_ip = nla_get_in_addr(data[IFLA_AMT_DISCOVERY_IP]);
3096 		if (ipv4_is_loopback(amt->discovery_ip) ||
3097 		    ipv4_is_zeronet(amt->discovery_ip) ||
3098 		    ipv4_is_multicast(amt->discovery_ip)) {
3099 			NL_SET_ERR_MSG_ATTR(extack, tb[IFLA_AMT_DISCOVERY_IP],
3100 					    "discovery must be unicast");
3101 			goto err;
3102 		}
3103 
3104 		dev->needed_headroom = amt->stream_dev->needed_headroom +
3105 				       AMT_GW_HLEN;
3106 		dev->mtu = amt->stream_dev->mtu - AMT_GW_HLEN;
3107 		dev->max_mtu = dev->mtu;
3108 		dev->min_mtu = ETH_MIN_MTU + AMT_GW_HLEN;
3109 	}
3110 	amt->qi = AMT_INIT_QUERY_INTERVAL;
3111 
3112 	err = register_netdevice(dev);
3113 	if (err < 0) {
3114 		netdev_dbg(dev, "failed to register new netdev %d\n", err);
3115 		goto err;
3116 	}
3117 
3118 	err = netdev_upper_dev_link(amt->stream_dev, dev, extack);
3119 	if (err < 0) {
3120 		unregister_netdevice(dev);
3121 		goto err;
3122 	}
3123 
3124 	INIT_DELAYED_WORK(&amt->discovery_wq, amt_discovery_work);
3125 	INIT_DELAYED_WORK(&amt->req_wq, amt_req_work);
3126 	INIT_DELAYED_WORK(&amt->secret_wq, amt_secret_work);
3127 	INIT_LIST_HEAD(&amt->tunnel_list);
3128 
3129 	return 0;
3130 err:
3131 	dev_put(amt->stream_dev);
3132 	return err;
3133 }
3134 
3135 static void amt_dellink(struct net_device *dev, struct list_head *head)
3136 {
3137 	struct amt_dev *amt = netdev_priv(dev);
3138 
3139 	unregister_netdevice_queue(dev, head);
3140 	netdev_upper_dev_unlink(amt->stream_dev, dev);
3141 	dev_put(amt->stream_dev);
3142 }
3143 
3144 static size_t amt_get_size(const struct net_device *dev)
3145 {
3146 	return nla_total_size(sizeof(__u32)) + /* IFLA_AMT_MODE */
3147 	       nla_total_size(sizeof(__u16)) + /* IFLA_AMT_RELAY_PORT */
3148 	       nla_total_size(sizeof(__u16)) + /* IFLA_AMT_GATEWAY_PORT */
3149 	       nla_total_size(sizeof(__u32)) + /* IFLA_AMT_LINK */
3150 	       nla_total_size(sizeof(__u32)) + /* IFLA_MAX_TUNNELS */
3151 	       nla_total_size(sizeof(struct iphdr)) + /* IFLA_AMT_DISCOVERY_IP */
3152 	       nla_total_size(sizeof(struct iphdr)) + /* IFLA_AMT_REMOTE_IP */
3153 	       nla_total_size(sizeof(struct iphdr)); /* IFLA_AMT_LOCAL_IP */
3154 }
3155 
3156 static int amt_fill_info(struct sk_buff *skb, const struct net_device *dev)
3157 {
3158 	struct amt_dev *amt = netdev_priv(dev);
3159 
3160 	if (nla_put_u32(skb, IFLA_AMT_MODE, amt->mode))
3161 		goto nla_put_failure;
3162 	if (nla_put_be16(skb, IFLA_AMT_RELAY_PORT, amt->relay_port))
3163 		goto nla_put_failure;
3164 	if (nla_put_be16(skb, IFLA_AMT_GATEWAY_PORT, amt->gw_port))
3165 		goto nla_put_failure;
3166 	if (nla_put_u32(skb, IFLA_AMT_LINK, amt->stream_dev->ifindex))
3167 		goto nla_put_failure;
3168 	if (nla_put_in_addr(skb, IFLA_AMT_LOCAL_IP, amt->local_ip))
3169 		goto nla_put_failure;
3170 	if (nla_put_in_addr(skb, IFLA_AMT_DISCOVERY_IP, amt->discovery_ip))
3171 		goto nla_put_failure;
3172 	if (amt->remote_ip)
3173 		if (nla_put_in_addr(skb, IFLA_AMT_REMOTE_IP, amt->remote_ip))
3174 			goto nla_put_failure;
3175 	if (nla_put_u32(skb, IFLA_AMT_MAX_TUNNELS, amt->max_tunnels))
3176 		goto nla_put_failure;
3177 
3178 	return 0;
3179 
3180 nla_put_failure:
3181 	return -EMSGSIZE;
3182 }
3183 
3184 static struct rtnl_link_ops amt_link_ops __read_mostly = {
3185 	.kind		= "amt",
3186 	.maxtype	= IFLA_AMT_MAX,
3187 	.policy		= amt_policy,
3188 	.priv_size	= sizeof(struct amt_dev),
3189 	.setup		= amt_link_setup,
3190 	.validate	= amt_validate,
3191 	.newlink	= amt_newlink,
3192 	.dellink	= amt_dellink,
3193 	.get_size       = amt_get_size,
3194 	.fill_info      = amt_fill_info,
3195 };
3196 
3197 static struct net_device *amt_lookup_upper_dev(struct net_device *dev)
3198 {
3199 	struct net_device *upper_dev;
3200 	struct amt_dev *amt;
3201 
3202 	for_each_netdev(dev_net(dev), upper_dev) {
3203 		if (netif_is_amt(upper_dev)) {
3204 			amt = netdev_priv(upper_dev);
3205 			if (amt->stream_dev == dev)
3206 				return upper_dev;
3207 		}
3208 	}
3209 
3210 	return NULL;
3211 }
3212 
3213 static int amt_device_event(struct notifier_block *unused,
3214 			    unsigned long event, void *ptr)
3215 {
3216 	struct net_device *dev = netdev_notifier_info_to_dev(ptr);
3217 	struct net_device *upper_dev;
3218 	struct amt_dev *amt;
3219 	LIST_HEAD(list);
3220 	int new_mtu;
3221 
3222 	upper_dev = amt_lookup_upper_dev(dev);
3223 	if (!upper_dev)
3224 		return NOTIFY_DONE;
3225 	amt = netdev_priv(upper_dev);
3226 
3227 	switch (event) {
3228 	case NETDEV_UNREGISTER:
3229 		amt_dellink(amt->dev, &list);
3230 		unregister_netdevice_many(&list);
3231 		break;
3232 	case NETDEV_CHANGEMTU:
3233 		if (amt->mode == AMT_MODE_RELAY)
3234 			new_mtu = dev->mtu - AMT_RELAY_HLEN;
3235 		else
3236 			new_mtu = dev->mtu - AMT_GW_HLEN;
3237 
3238 		dev_set_mtu(amt->dev, new_mtu);
3239 		break;
3240 	}
3241 
3242 	return NOTIFY_DONE;
3243 }
3244 
3245 static struct notifier_block amt_notifier_block __read_mostly = {
3246 	.notifier_call = amt_device_event,
3247 };
3248 
3249 static int __init amt_init(void)
3250 {
3251 	int err;
3252 
3253 	err = register_netdevice_notifier(&amt_notifier_block);
3254 	if (err < 0)
3255 		goto err;
3256 
3257 	err = rtnl_link_register(&amt_link_ops);
3258 	if (err < 0)
3259 		goto unregister_notifier;
3260 
3261 	amt_wq = alloc_workqueue("amt", WQ_UNBOUND, 1);
3262 	if (!amt_wq)
3263 		goto rtnl_unregister;
3264 
3265 	spin_lock_init(&source_gc_lock);
3266 	spin_lock_bh(&source_gc_lock);
3267 	INIT_DELAYED_WORK(&source_gc_wq, amt_source_gc_work);
3268 	mod_delayed_work(amt_wq, &source_gc_wq,
3269 			 msecs_to_jiffies(AMT_GC_INTERVAL));
3270 	spin_unlock_bh(&source_gc_lock);
3271 
3272 	return 0;
3273 
3274 rtnl_unregister:
3275 	rtnl_link_unregister(&amt_link_ops);
3276 unregister_notifier:
3277 	unregister_netdevice_notifier(&amt_notifier_block);
3278 err:
3279 	pr_err("error loading AMT module loaded\n");
3280 	return err;
3281 }
3282 late_initcall(amt_init);
3283 
3284 static void __exit amt_fini(void)
3285 {
3286 	rtnl_link_unregister(&amt_link_ops);
3287 	unregister_netdevice_notifier(&amt_notifier_block);
3288 	flush_delayed_work(&source_gc_wq);
3289 	__amt_source_gc_work();
3290 	destroy_workqueue(amt_wq);
3291 }
3292 module_exit(amt_fini);
3293 
3294 MODULE_LICENSE("GPL");
3295 MODULE_AUTHOR("Taehee Yoo <ap420073@gmail.com>");
3296 MODULE_ALIAS_RTNL_LINK("amt");
3297