linux - drivers/net/amt.c

3495 lines · 89.8 KiB

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