0001
0002
0003
0004 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
0005
0006 #include <linux/module.h>
0007 #include <linux/skbuff.h>
0008 #include <linux/udp.h>
0009 #include <linux/jhash.h>
0010 #include <linux/if_tunnel.h>
0011 #include <linux/net.h>
0012 #include <linux/igmp.h>
0013 #include <linux/workqueue.h>
0014 #include <net/sch_generic.h>
0015 #include <net/net_namespace.h>
0016 #include <net/ip.h>
0017 #include <net/udp.h>
0018 #include <net/udp_tunnel.h>
0019 #include <net/icmp.h>
0020 #include <net/mld.h>
0021 #include <net/amt.h>
0022 #include <uapi/linux/amt.h>
0023 #include <linux/security.h>
0024 #include <net/gro_cells.h>
0025 #include <net/ipv6.h>
0026 #include <net/if_inet6.h>
0027 #include <net/ndisc.h>
0028 #include <net/addrconf.h>
0029 #include <net/ip6_route.h>
0030 #include <net/inet_common.h>
0031 #include <net/ip6_checksum.h>
0032
0033 static struct workqueue_struct *amt_wq;
0034
0035 static HLIST_HEAD(source_gc_list);
0036
0037 static spinlock_t source_gc_lock;
0038 static struct delayed_work source_gc_wq;
0039 static char *status_str[] = {
0040 "AMT_STATUS_INIT",
0041 "AMT_STATUS_SENT_DISCOVERY",
0042 "AMT_STATUS_RECEIVED_DISCOVERY",
0043 "AMT_STATUS_SENT_ADVERTISEMENT",
0044 "AMT_STATUS_RECEIVED_ADVERTISEMENT",
0045 "AMT_STATUS_SENT_REQUEST",
0046 "AMT_STATUS_RECEIVED_REQUEST",
0047 "AMT_STATUS_SENT_QUERY",
0048 "AMT_STATUS_RECEIVED_QUERY",
0049 "AMT_STATUS_SENT_UPDATE",
0050 "AMT_STATUS_RECEIVED_UPDATE",
0051 };
0052
0053 static char *type_str[] = {
0054 "",
0055 "AMT_MSG_DISCOVERY",
0056 "AMT_MSG_ADVERTISEMENT",
0057 "AMT_MSG_REQUEST",
0058 "AMT_MSG_MEMBERSHIP_QUERY",
0059 "AMT_MSG_MEMBERSHIP_UPDATE",
0060 "AMT_MSG_MULTICAST_DATA",
0061 "AMT_MSG_TEARDOWN",
0062 };
0063
0064 static char *action_str[] = {
0065 "AMT_ACT_GMI",
0066 "AMT_ACT_GMI_ZERO",
0067 "AMT_ACT_GT",
0068 "AMT_ACT_STATUS_FWD_NEW",
0069 "AMT_ACT_STATUS_D_FWD_NEW",
0070 "AMT_ACT_STATUS_NONE_NEW",
0071 };
0072
0073 static struct igmpv3_grec igmpv3_zero_grec;
0074
0075 #if IS_ENABLED(CONFIG_IPV6)
0076 #define MLD2_ALL_NODE_INIT { { { 0xff, 0x02, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0x01 } } }
0077 static struct in6_addr mld2_all_node = MLD2_ALL_NODE_INIT;
0078 static struct mld2_grec mldv2_zero_grec;
0079 #endif
0080
0081 static struct amt_skb_cb *amt_skb_cb(struct sk_buff *skb)
0082 {
0083 BUILD_BUG_ON(sizeof(struct amt_skb_cb) + sizeof(struct qdisc_skb_cb) >
0084 sizeof_field(struct sk_buff, cb));
0085
0086 return (struct amt_skb_cb *)((void *)skb->cb +
0087 sizeof(struct qdisc_skb_cb));
0088 }
0089
0090 static void __amt_source_gc_work(void)
0091 {
0092 struct amt_source_node *snode;
0093 struct hlist_head gc_list;
0094 struct hlist_node *t;
0095
0096 spin_lock_bh(&source_gc_lock);
0097 hlist_move_list(&source_gc_list, &gc_list);
0098 spin_unlock_bh(&source_gc_lock);
0099
0100 hlist_for_each_entry_safe(snode, t, &gc_list, node) {
0101 hlist_del_rcu(&snode->node);
0102 kfree_rcu(snode, rcu);
0103 }
0104 }
0105
0106 static void amt_source_gc_work(struct work_struct *work)
0107 {
0108 __amt_source_gc_work();
0109
0110 spin_lock_bh(&source_gc_lock);
0111 mod_delayed_work(amt_wq, &source_gc_wq,
0112 msecs_to_jiffies(AMT_GC_INTERVAL));
0113 spin_unlock_bh(&source_gc_lock);
0114 }
0115
0116 static bool amt_addr_equal(union amt_addr *a, union amt_addr *b)
0117 {
0118 return !memcmp(a, b, sizeof(union amt_addr));
0119 }
0120
0121 static u32 amt_source_hash(struct amt_tunnel_list *tunnel, union amt_addr *src)
0122 {
0123 u32 hash = jhash(src, sizeof(*src), tunnel->amt->hash_seed);
0124
0125 return reciprocal_scale(hash, tunnel->amt->hash_buckets);
0126 }
0127
0128 static bool amt_status_filter(struct amt_source_node *snode,
0129 enum amt_filter filter)
0130 {
0131 bool rc = false;
0132
0133 switch (filter) {
0134 case AMT_FILTER_FWD:
0135 if (snode->status == AMT_SOURCE_STATUS_FWD &&
0136 snode->flags == AMT_SOURCE_OLD)
0137 rc = true;
0138 break;
0139 case AMT_FILTER_D_FWD:
0140 if (snode->status == AMT_SOURCE_STATUS_D_FWD &&
0141 snode->flags == AMT_SOURCE_OLD)
0142 rc = true;
0143 break;
0144 case AMT_FILTER_FWD_NEW:
0145 if (snode->status == AMT_SOURCE_STATUS_FWD &&
0146 snode->flags == AMT_SOURCE_NEW)
0147 rc = true;
0148 break;
0149 case AMT_FILTER_D_FWD_NEW:
0150 if (snode->status == AMT_SOURCE_STATUS_D_FWD &&
0151 snode->flags == AMT_SOURCE_NEW)
0152 rc = true;
0153 break;
0154 case AMT_FILTER_ALL:
0155 rc = true;
0156 break;
0157 case AMT_FILTER_NONE_NEW:
0158 if (snode->status == AMT_SOURCE_STATUS_NONE &&
0159 snode->flags == AMT_SOURCE_NEW)
0160 rc = true;
0161 break;
0162 case AMT_FILTER_BOTH:
0163 if ((snode->status == AMT_SOURCE_STATUS_D_FWD ||
0164 snode->status == AMT_SOURCE_STATUS_FWD) &&
0165 snode->flags == AMT_SOURCE_OLD)
0166 rc = true;
0167 break;
0168 case AMT_FILTER_BOTH_NEW:
0169 if ((snode->status == AMT_SOURCE_STATUS_D_FWD ||
0170 snode->status == AMT_SOURCE_STATUS_FWD) &&
0171 snode->flags == AMT_SOURCE_NEW)
0172 rc = true;
0173 break;
0174 default:
0175 WARN_ON_ONCE(1);
0176 break;
0177 }
0178
0179 return rc;
0180 }
0181
0182 static struct amt_source_node *amt_lookup_src(struct amt_tunnel_list *tunnel,
0183 struct amt_group_node *gnode,
0184 enum amt_filter filter,
0185 union amt_addr *src)
0186 {
0187 u32 hash = amt_source_hash(tunnel, src);
0188 struct amt_source_node *snode;
0189
0190 hlist_for_each_entry_rcu(snode, &gnode->sources[hash], node)
0191 if (amt_status_filter(snode, filter) &&
0192 amt_addr_equal(&snode->source_addr, src))
0193 return snode;
0194
0195 return NULL;
0196 }
0197
0198 static u32 amt_group_hash(struct amt_tunnel_list *tunnel, union amt_addr *group)
0199 {
0200 u32 hash = jhash(group, sizeof(*group), tunnel->amt->hash_seed);
0201
0202 return reciprocal_scale(hash, tunnel->amt->hash_buckets);
0203 }
0204
0205 static struct amt_group_node *amt_lookup_group(struct amt_tunnel_list *tunnel,
0206 union amt_addr *group,
0207 union amt_addr *host,
0208 bool v6)
0209 {
0210 u32 hash = amt_group_hash(tunnel, group);
0211 struct amt_group_node *gnode;
0212
0213 hlist_for_each_entry_rcu(gnode, &tunnel->groups[hash], node) {
0214 if (amt_addr_equal(&gnode->group_addr, group) &&
0215 amt_addr_equal(&gnode->host_addr, host) &&
0216 gnode->v6 == v6)
0217 return gnode;
0218 }
0219
0220 return NULL;
0221 }
0222
0223 static void amt_destroy_source(struct amt_source_node *snode)
0224 {
0225 struct amt_group_node *gnode = snode->gnode;
0226 struct amt_tunnel_list *tunnel;
0227
0228 tunnel = gnode->tunnel_list;
0229
0230 if (!gnode->v6) {
0231 netdev_dbg(snode->gnode->amt->dev,
0232 "Delete source %pI4 from %pI4\n",
0233 &snode->source_addr.ip4,
0234 &gnode->group_addr.ip4);
0235 #if IS_ENABLED(CONFIG_IPV6)
0236 } else {
0237 netdev_dbg(snode->gnode->amt->dev,
0238 "Delete source %pI6 from %pI6\n",
0239 &snode->source_addr.ip6,
0240 &gnode->group_addr.ip6);
0241 #endif
0242 }
0243
0244 cancel_delayed_work(&snode->source_timer);
0245 hlist_del_init_rcu(&snode->node);
0246 tunnel->nr_sources--;
0247 gnode->nr_sources--;
0248 spin_lock_bh(&source_gc_lock);
0249 hlist_add_head_rcu(&snode->node, &source_gc_list);
0250 spin_unlock_bh(&source_gc_lock);
0251 }
0252
0253 static void amt_del_group(struct amt_dev *amt, struct amt_group_node *gnode)
0254 {
0255 struct amt_source_node *snode;
0256 struct hlist_node *t;
0257 int i;
0258
0259 if (cancel_delayed_work(&gnode->group_timer))
0260 dev_put(amt->dev);
0261 hlist_del_rcu(&gnode->node);
0262 gnode->tunnel_list->nr_groups--;
0263
0264 if (!gnode->v6)
0265 netdev_dbg(amt->dev, "Leave group %pI4\n",
0266 &gnode->group_addr.ip4);
0267 #if IS_ENABLED(CONFIG_IPV6)
0268 else
0269 netdev_dbg(amt->dev, "Leave group %pI6\n",
0270 &gnode->group_addr.ip6);
0271 #endif
0272 for (i = 0; i < amt->hash_buckets; i++)
0273 hlist_for_each_entry_safe(snode, t, &gnode->sources[i], node)
0274 amt_destroy_source(snode);
0275
0276
0277
0278
0279 kfree_rcu(gnode, rcu);
0280 }
0281
0282
0283
0284
0285
0286
0287 static void amt_source_work(struct work_struct *work)
0288 {
0289 struct amt_source_node *snode = container_of(to_delayed_work(work),
0290 struct amt_source_node,
0291 source_timer);
0292 struct amt_group_node *gnode = snode->gnode;
0293 struct amt_dev *amt = gnode->amt;
0294 struct amt_tunnel_list *tunnel;
0295
0296 tunnel = gnode->tunnel_list;
0297 spin_lock_bh(&tunnel->lock);
0298 rcu_read_lock();
0299 if (gnode->filter_mode == MCAST_INCLUDE) {
0300 amt_destroy_source(snode);
0301 if (!gnode->nr_sources)
0302 amt_del_group(amt, gnode);
0303 } else {
0304
0305
0306
0307 snode->status = AMT_SOURCE_STATUS_D_FWD;
0308 }
0309 rcu_read_unlock();
0310 spin_unlock_bh(&tunnel->lock);
0311 }
0312
0313 static void amt_act_src(struct amt_tunnel_list *tunnel,
0314 struct amt_group_node *gnode,
0315 struct amt_source_node *snode,
0316 enum amt_act act)
0317 {
0318 struct amt_dev *amt = tunnel->amt;
0319
0320 switch (act) {
0321 case AMT_ACT_GMI:
0322 mod_delayed_work(amt_wq, &snode->source_timer,
0323 msecs_to_jiffies(amt_gmi(amt)));
0324 break;
0325 case AMT_ACT_GMI_ZERO:
0326 cancel_delayed_work(&snode->source_timer);
0327 break;
0328 case AMT_ACT_GT:
0329 mod_delayed_work(amt_wq, &snode->source_timer,
0330 gnode->group_timer.timer.expires);
0331 break;
0332 case AMT_ACT_STATUS_FWD_NEW:
0333 snode->status = AMT_SOURCE_STATUS_FWD;
0334 snode->flags = AMT_SOURCE_NEW;
0335 break;
0336 case AMT_ACT_STATUS_D_FWD_NEW:
0337 snode->status = AMT_SOURCE_STATUS_D_FWD;
0338 snode->flags = AMT_SOURCE_NEW;
0339 break;
0340 case AMT_ACT_STATUS_NONE_NEW:
0341 cancel_delayed_work(&snode->source_timer);
0342 snode->status = AMT_SOURCE_STATUS_NONE;
0343 snode->flags = AMT_SOURCE_NEW;
0344 break;
0345 default:
0346 WARN_ON_ONCE(1);
0347 return;
0348 }
0349
0350 if (!gnode->v6)
0351 netdev_dbg(amt->dev, "Source %pI4 from %pI4 Acted %s\n",
0352 &snode->source_addr.ip4,
0353 &gnode->group_addr.ip4,
0354 action_str[act]);
0355 #if IS_ENABLED(CONFIG_IPV6)
0356 else
0357 netdev_dbg(amt->dev, "Source %pI6 from %pI6 Acted %s\n",
0358 &snode->source_addr.ip6,
0359 &gnode->group_addr.ip6,
0360 action_str[act]);
0361 #endif
0362 }
0363
0364 static struct amt_source_node *amt_alloc_snode(struct amt_group_node *gnode,
0365 union amt_addr *src)
0366 {
0367 struct amt_source_node *snode;
0368
0369 snode = kzalloc(sizeof(*snode), GFP_ATOMIC);
0370 if (!snode)
0371 return NULL;
0372
0373 memcpy(&snode->source_addr, src, sizeof(union amt_addr));
0374 snode->gnode = gnode;
0375 snode->status = AMT_SOURCE_STATUS_NONE;
0376 snode->flags = AMT_SOURCE_NEW;
0377 INIT_HLIST_NODE(&snode->node);
0378 INIT_DELAYED_WORK(&snode->source_timer, amt_source_work);
0379
0380 return snode;
0381 }
0382
0383
0384
0385
0386
0387
0388
0389
0390
0391
0392
0393
0394
0395
0396
0397
0398
0399
0400
0401
0402
0403
0404
0405
0406
0407
0408 static void amt_group_work(struct work_struct *work)
0409 {
0410 struct amt_group_node *gnode = container_of(to_delayed_work(work),
0411 struct amt_group_node,
0412 group_timer);
0413 struct amt_tunnel_list *tunnel = gnode->tunnel_list;
0414 struct amt_dev *amt = gnode->amt;
0415 struct amt_source_node *snode;
0416 bool delete_group = true;
0417 struct hlist_node *t;
0418 int i, buckets;
0419
0420 buckets = amt->hash_buckets;
0421
0422 spin_lock_bh(&tunnel->lock);
0423 if (gnode->filter_mode == MCAST_INCLUDE) {
0424
0425 spin_unlock_bh(&tunnel->lock);
0426 goto out;
0427 }
0428
0429 rcu_read_lock();
0430 for (i = 0; i < buckets; i++) {
0431 hlist_for_each_entry_safe(snode, t,
0432 &gnode->sources[i], node) {
0433 if (!delayed_work_pending(&snode->source_timer) ||
0434 snode->status == AMT_SOURCE_STATUS_D_FWD) {
0435 amt_destroy_source(snode);
0436 } else {
0437 delete_group = false;
0438 snode->status = AMT_SOURCE_STATUS_FWD;
0439 }
0440 }
0441 }
0442 if (delete_group)
0443 amt_del_group(amt, gnode);
0444 else
0445 gnode->filter_mode = MCAST_INCLUDE;
0446 rcu_read_unlock();
0447 spin_unlock_bh(&tunnel->lock);
0448 out:
0449 dev_put(amt->dev);
0450 }
0451
0452
0453
0454
0455
0456
0457
0458
0459
0460
0461
0462
0463 static struct amt_group_node *amt_add_group(struct amt_dev *amt,
0464 struct amt_tunnel_list *tunnel,
0465 union amt_addr *group,
0466 union amt_addr *host,
0467 bool v6)
0468 {
0469 struct amt_group_node *gnode;
0470 u32 hash;
0471 int i;
0472
0473 if (tunnel->nr_groups >= amt->max_groups)
0474 return ERR_PTR(-ENOSPC);
0475
0476 gnode = kzalloc(sizeof(*gnode) +
0477 (sizeof(struct hlist_head) * amt->hash_buckets),
0478 GFP_ATOMIC);
0479 if (unlikely(!gnode))
0480 return ERR_PTR(-ENOMEM);
0481
0482 gnode->amt = amt;
0483 gnode->group_addr = *group;
0484 gnode->host_addr = *host;
0485 gnode->v6 = v6;
0486 gnode->tunnel_list = tunnel;
0487 gnode->filter_mode = MCAST_INCLUDE;
0488 INIT_HLIST_NODE(&gnode->node);
0489 INIT_DELAYED_WORK(&gnode->group_timer, amt_group_work);
0490 for (i = 0; i < amt->hash_buckets; i++)
0491 INIT_HLIST_HEAD(&gnode->sources[i]);
0492
0493 hash = amt_group_hash(tunnel, group);
0494 hlist_add_head_rcu(&gnode->node, &tunnel->groups[hash]);
0495 tunnel->nr_groups++;
0496
0497 if (!gnode->v6)
0498 netdev_dbg(amt->dev, "Join group %pI4\n",
0499 &gnode->group_addr.ip4);
0500 #if IS_ENABLED(CONFIG_IPV6)
0501 else
0502 netdev_dbg(amt->dev, "Join group %pI6\n",
0503 &gnode->group_addr.ip6);
0504 #endif
0505
0506 return gnode;
0507 }
0508
0509 static struct sk_buff *amt_build_igmp_gq(struct amt_dev *amt)
0510 {
0511 u8 ra[AMT_IPHDR_OPTS] = { IPOPT_RA, 4, 0, 0 };
0512 int hlen = LL_RESERVED_SPACE(amt->dev);
0513 int tlen = amt->dev->needed_tailroom;
0514 struct igmpv3_query *ihv3;
0515 void *csum_start = NULL;
0516 __sum16 *csum = NULL;
0517 struct sk_buff *skb;
0518 struct ethhdr *eth;
0519 struct iphdr *iph;
0520 unsigned int len;
0521 int offset;
0522
0523 len = hlen + tlen + sizeof(*iph) + AMT_IPHDR_OPTS + sizeof(*ihv3);
0524 skb = netdev_alloc_skb_ip_align(amt->dev, len);
0525 if (!skb)
0526 return NULL;
0527
0528 skb_reserve(skb, hlen);
0529 skb_push(skb, sizeof(*eth));
0530 skb->protocol = htons(ETH_P_IP);
0531 skb_reset_mac_header(skb);
0532 skb->priority = TC_PRIO_CONTROL;
0533 skb_put(skb, sizeof(*iph));
0534 skb_put_data(skb, ra, sizeof(ra));
0535 skb_put(skb, sizeof(*ihv3));
0536 skb_pull(skb, sizeof(*eth));
0537 skb_reset_network_header(skb);
0538
0539 iph = ip_hdr(skb);
0540 iph->version = 4;
0541 iph->ihl = (sizeof(struct iphdr) + AMT_IPHDR_OPTS) >> 2;
0542 iph->tos = AMT_TOS;
0543 iph->tot_len = htons(sizeof(*iph) + AMT_IPHDR_OPTS + sizeof(*ihv3));
0544 iph->frag_off = htons(IP_DF);
0545 iph->ttl = 1;
0546 iph->id = 0;
0547 iph->protocol = IPPROTO_IGMP;
0548 iph->daddr = htonl(INADDR_ALLHOSTS_GROUP);
0549 iph->saddr = htonl(INADDR_ANY);
0550 ip_send_check(iph);
0551
0552 eth = eth_hdr(skb);
0553 ether_addr_copy(eth->h_source, amt->dev->dev_addr);
0554 ip_eth_mc_map(htonl(INADDR_ALLHOSTS_GROUP), eth->h_dest);
0555 eth->h_proto = htons(ETH_P_IP);
0556
0557 ihv3 = skb_pull(skb, sizeof(*iph) + AMT_IPHDR_OPTS);
0558 skb_reset_transport_header(skb);
0559 ihv3->type = IGMP_HOST_MEMBERSHIP_QUERY;
0560 ihv3->code = 1;
0561 ihv3->group = 0;
0562 ihv3->qqic = amt->qi;
0563 ihv3->nsrcs = 0;
0564 ihv3->resv = 0;
0565 ihv3->suppress = false;
0566 ihv3->qrv = READ_ONCE(amt->net->ipv4.sysctl_igmp_qrv);
0567 ihv3->csum = 0;
0568 csum = &ihv3->csum;
0569 csum_start = (void *)ihv3;
0570 *csum = ip_compute_csum(csum_start, sizeof(*ihv3));
0571 offset = skb_transport_offset(skb);
0572 skb->csum = skb_checksum(skb, offset, skb->len - offset, 0);
0573 skb->ip_summed = CHECKSUM_NONE;
0574
0575 skb_push(skb, sizeof(*eth) + sizeof(*iph) + AMT_IPHDR_OPTS);
0576
0577 return skb;
0578 }
0579
0580 static void amt_update_gw_status(struct amt_dev *amt, enum amt_status status,
0581 bool validate)
0582 {
0583 if (validate && amt->status >= status)
0584 return;
0585 netdev_dbg(amt->dev, "Update GW status %s -> %s",
0586 status_str[amt->status], status_str[status]);
0587 WRITE_ONCE(amt->status, status);
0588 }
0589
0590 static void __amt_update_relay_status(struct amt_tunnel_list *tunnel,
0591 enum amt_status status,
0592 bool validate)
0593 {
0594 if (validate && tunnel->status >= status)
0595 return;
0596 netdev_dbg(tunnel->amt->dev,
0597 "Update Tunnel(IP = %pI4, PORT = %u) status %s -> %s",
0598 &tunnel->ip4, ntohs(tunnel->source_port),
0599 status_str[tunnel->status], status_str[status]);
0600 tunnel->status = status;
0601 }
0602
0603 static void amt_update_relay_status(struct amt_tunnel_list *tunnel,
0604 enum amt_status status, bool validate)
0605 {
0606 spin_lock_bh(&tunnel->lock);
0607 __amt_update_relay_status(tunnel, status, validate);
0608 spin_unlock_bh(&tunnel->lock);
0609 }
0610
0611 static void amt_send_discovery(struct amt_dev *amt)
0612 {
0613 struct amt_header_discovery *amtd;
0614 int hlen, tlen, offset;
0615 struct socket *sock;
0616 struct udphdr *udph;
0617 struct sk_buff *skb;
0618 struct iphdr *iph;
0619 struct rtable *rt;
0620 struct flowi4 fl4;
0621 u32 len;
0622 int err;
0623
0624 rcu_read_lock();
0625 sock = rcu_dereference(amt->sock);
0626 if (!sock)
0627 goto out;
0628
0629 if (!netif_running(amt->stream_dev) || !netif_running(amt->dev))
0630 goto out;
0631
0632 rt = ip_route_output_ports(amt->net, &fl4, sock->sk,
0633 amt->discovery_ip, amt->local_ip,
0634 amt->gw_port, amt->relay_port,
0635 IPPROTO_UDP, 0,
0636 amt->stream_dev->ifindex);
0637 if (IS_ERR(rt)) {
0638 amt->dev->stats.tx_errors++;
0639 goto out;
0640 }
0641
0642 hlen = LL_RESERVED_SPACE(amt->dev);
0643 tlen = amt->dev->needed_tailroom;
0644 len = hlen + tlen + sizeof(*iph) + sizeof(*udph) + sizeof(*amtd);
0645 skb = netdev_alloc_skb_ip_align(amt->dev, len);
0646 if (!skb) {
0647 ip_rt_put(rt);
0648 amt->dev->stats.tx_errors++;
0649 goto out;
0650 }
0651
0652 skb->priority = TC_PRIO_CONTROL;
0653 skb_dst_set(skb, &rt->dst);
0654
0655 len = sizeof(*iph) + sizeof(*udph) + sizeof(*amtd);
0656 skb_reset_network_header(skb);
0657 skb_put(skb, len);
0658 amtd = skb_pull(skb, sizeof(*iph) + sizeof(*udph));
0659 amtd->version = 0;
0660 amtd->type = AMT_MSG_DISCOVERY;
0661 amtd->reserved = 0;
0662 amtd->nonce = amt->nonce;
0663 skb_push(skb, sizeof(*udph));
0664 skb_reset_transport_header(skb);
0665 udph = udp_hdr(skb);
0666 udph->source = amt->gw_port;
0667 udph->dest = amt->relay_port;
0668 udph->len = htons(sizeof(*udph) + sizeof(*amtd));
0669 udph->check = 0;
0670 offset = skb_transport_offset(skb);
0671 skb->csum = skb_checksum(skb, offset, skb->len - offset, 0);
0672 udph->check = csum_tcpudp_magic(amt->local_ip, amt->discovery_ip,
0673 sizeof(*udph) + sizeof(*amtd),
0674 IPPROTO_UDP, skb->csum);
0675
0676 skb_push(skb, sizeof(*iph));
0677 iph = ip_hdr(skb);
0678 iph->version = 4;
0679 iph->ihl = (sizeof(struct iphdr)) >> 2;
0680 iph->tos = AMT_TOS;
0681 iph->frag_off = 0;
0682 iph->ttl = ip4_dst_hoplimit(&rt->dst);
0683 iph->daddr = amt->discovery_ip;
0684 iph->saddr = amt->local_ip;
0685 iph->protocol = IPPROTO_UDP;
0686 iph->tot_len = htons(len);
0687
0688 skb->ip_summed = CHECKSUM_NONE;
0689 ip_select_ident(amt->net, skb, NULL);
0690 ip_send_check(iph);
0691 err = ip_local_out(amt->net, sock->sk, skb);
0692 if (unlikely(net_xmit_eval(err)))
0693 amt->dev->stats.tx_errors++;
0694
0695 amt_update_gw_status(amt, AMT_STATUS_SENT_DISCOVERY, true);
0696 out:
0697 rcu_read_unlock();
0698 }
0699
0700 static void amt_send_request(struct amt_dev *amt, bool v6)
0701 {
0702 struct amt_header_request *amtrh;
0703 int hlen, tlen, offset;
0704 struct socket *sock;
0705 struct udphdr *udph;
0706 struct sk_buff *skb;
0707 struct iphdr *iph;
0708 struct rtable *rt;
0709 struct flowi4 fl4;
0710 u32 len;
0711 int err;
0712
0713 rcu_read_lock();
0714 sock = rcu_dereference(amt->sock);
0715 if (!sock)
0716 goto out;
0717
0718 if (!netif_running(amt->stream_dev) || !netif_running(amt->dev))
0719 goto out;
0720
0721 rt = ip_route_output_ports(amt->net, &fl4, sock->sk,
0722 amt->remote_ip, amt->local_ip,
0723 amt->gw_port, amt->relay_port,
0724 IPPROTO_UDP, 0,
0725 amt->stream_dev->ifindex);
0726 if (IS_ERR(rt)) {
0727 amt->dev->stats.tx_errors++;
0728 goto out;
0729 }
0730
0731 hlen = LL_RESERVED_SPACE(amt->dev);
0732 tlen = amt->dev->needed_tailroom;
0733 len = hlen + tlen + sizeof(*iph) + sizeof(*udph) + sizeof(*amtrh);
0734 skb = netdev_alloc_skb_ip_align(amt->dev, len);
0735 if (!skb) {
0736 ip_rt_put(rt);
0737 amt->dev->stats.tx_errors++;
0738 goto out;
0739 }
0740
0741 skb->priority = TC_PRIO_CONTROL;
0742 skb_dst_set(skb, &rt->dst);
0743
0744 len = sizeof(*iph) + sizeof(*udph) + sizeof(*amtrh);
0745 skb_reset_network_header(skb);
0746 skb_put(skb, len);
0747 amtrh = skb_pull(skb, sizeof(*iph) + sizeof(*udph));
0748 amtrh->version = 0;
0749 amtrh->type = AMT_MSG_REQUEST;
0750 amtrh->reserved1 = 0;
0751 amtrh->p = v6;
0752 amtrh->reserved2 = 0;
0753 amtrh->nonce = amt->nonce;
0754 skb_push(skb, sizeof(*udph));
0755 skb_reset_transport_header(skb);
0756 udph = udp_hdr(skb);
0757 udph->source = amt->gw_port;
0758 udph->dest = amt->relay_port;
0759 udph->len = htons(sizeof(*amtrh) + sizeof(*udph));
0760 udph->check = 0;
0761 offset = skb_transport_offset(skb);
0762 skb->csum = skb_checksum(skb, offset, skb->len - offset, 0);
0763 udph->check = csum_tcpudp_magic(amt->local_ip, amt->remote_ip,
0764 sizeof(*udph) + sizeof(*amtrh),
0765 IPPROTO_UDP, skb->csum);
0766
0767 skb_push(skb, sizeof(*iph));
0768 iph = ip_hdr(skb);
0769 iph->version = 4;
0770 iph->ihl = (sizeof(struct iphdr)) >> 2;
0771 iph->tos = AMT_TOS;
0772 iph->frag_off = 0;
0773 iph->ttl = ip4_dst_hoplimit(&rt->dst);
0774 iph->daddr = amt->remote_ip;
0775 iph->saddr = amt->local_ip;
0776 iph->protocol = IPPROTO_UDP;
0777 iph->tot_len = htons(len);
0778
0779 skb->ip_summed = CHECKSUM_NONE;
0780 ip_select_ident(amt->net, skb, NULL);
0781 ip_send_check(iph);
0782 err = ip_local_out(amt->net, sock->sk, skb);
0783 if (unlikely(net_xmit_eval(err)))
0784 amt->dev->stats.tx_errors++;
0785
0786 out:
0787 rcu_read_unlock();
0788 }
0789
0790 static void amt_send_igmp_gq(struct amt_dev *amt,
0791 struct amt_tunnel_list *tunnel)
0792 {
0793 struct sk_buff *skb;
0794
0795 skb = amt_build_igmp_gq(amt);
0796 if (!skb)
0797 return;
0798
0799 amt_skb_cb(skb)->tunnel = tunnel;
0800 dev_queue_xmit(skb);
0801 }
0802
0803 #if IS_ENABLED(CONFIG_IPV6)
0804 static struct sk_buff *amt_build_mld_gq(struct amt_dev *amt)
0805 {
0806 u8 ra[AMT_IP6HDR_OPTS] = { IPPROTO_ICMPV6, 0, IPV6_TLV_ROUTERALERT,
0807 2, 0, 0, IPV6_TLV_PAD1, IPV6_TLV_PAD1 };
0808 int hlen = LL_RESERVED_SPACE(amt->dev);
0809 int tlen = amt->dev->needed_tailroom;
0810 struct mld2_query *mld2q;
0811 void *csum_start = NULL;
0812 struct ipv6hdr *ip6h;
0813 struct sk_buff *skb;
0814 struct ethhdr *eth;
0815 u32 len;
0816
0817 len = hlen + tlen + sizeof(*ip6h) + sizeof(ra) + sizeof(*mld2q);
0818 skb = netdev_alloc_skb_ip_align(amt->dev, len);
0819 if (!skb)
0820 return NULL;
0821
0822 skb_reserve(skb, hlen);
0823 skb_push(skb, sizeof(*eth));
0824 skb_reset_mac_header(skb);
0825 eth = eth_hdr(skb);
0826 skb->priority = TC_PRIO_CONTROL;
0827 skb->protocol = htons(ETH_P_IPV6);
0828 skb_put_zero(skb, sizeof(*ip6h));
0829 skb_put_data(skb, ra, sizeof(ra));
0830 skb_put_zero(skb, sizeof(*mld2q));
0831 skb_pull(skb, sizeof(*eth));
0832 skb_reset_network_header(skb);
0833 ip6h = ipv6_hdr(skb);
0834 ip6h->payload_len = htons(sizeof(ra) + sizeof(*mld2q));
0835 ip6h->nexthdr = NEXTHDR_HOP;
0836 ip6h->hop_limit = 1;
0837 ip6h->daddr = mld2_all_node;
0838 ip6_flow_hdr(ip6h, 0, 0);
0839
0840 if (ipv6_dev_get_saddr(amt->net, amt->dev, &ip6h->daddr, 0,
0841 &ip6h->saddr)) {
0842 amt->dev->stats.tx_errors++;
0843 kfree_skb(skb);
0844 return NULL;
0845 }
0846
0847 eth->h_proto = htons(ETH_P_IPV6);
0848 ether_addr_copy(eth->h_source, amt->dev->dev_addr);
0849 ipv6_eth_mc_map(&mld2_all_node, eth->h_dest);
0850
0851 skb_pull(skb, sizeof(*ip6h) + sizeof(ra));
0852 skb_reset_transport_header(skb);
0853 mld2q = (struct mld2_query *)icmp6_hdr(skb);
0854 mld2q->mld2q_mrc = htons(1);
0855 mld2q->mld2q_type = ICMPV6_MGM_QUERY;
0856 mld2q->mld2q_code = 0;
0857 mld2q->mld2q_cksum = 0;
0858 mld2q->mld2q_resv1 = 0;
0859 mld2q->mld2q_resv2 = 0;
0860 mld2q->mld2q_suppress = 0;
0861 mld2q->mld2q_qrv = amt->qrv;
0862 mld2q->mld2q_nsrcs = 0;
0863 mld2q->mld2q_qqic = amt->qi;
0864 csum_start = (void *)mld2q;
0865 mld2q->mld2q_cksum = csum_ipv6_magic(&ip6h->saddr, &ip6h->daddr,
0866 sizeof(*mld2q),
0867 IPPROTO_ICMPV6,
0868 csum_partial(csum_start,
0869 sizeof(*mld2q), 0));
0870
0871 skb->ip_summed = CHECKSUM_NONE;
0872 skb_push(skb, sizeof(*eth) + sizeof(*ip6h) + sizeof(ra));
0873 return skb;
0874 }
0875
0876 static void amt_send_mld_gq(struct amt_dev *amt, struct amt_tunnel_list *tunnel)
0877 {
0878 struct sk_buff *skb;
0879
0880 skb = amt_build_mld_gq(amt);
0881 if (!skb)
0882 return;
0883
0884 amt_skb_cb(skb)->tunnel = tunnel;
0885 dev_queue_xmit(skb);
0886 }
0887 #else
0888 static void amt_send_mld_gq(struct amt_dev *amt, struct amt_tunnel_list *tunnel)
0889 {
0890 }
0891 #endif
0892
0893 static bool amt_queue_event(struct amt_dev *amt, enum amt_event event,
0894 struct sk_buff *skb)
0895 {
0896 int index;
0897
0898 spin_lock_bh(&amt->lock);
0899 if (amt->nr_events >= AMT_MAX_EVENTS) {
0900 spin_unlock_bh(&amt->lock);
0901 return 1;
0902 }
0903
0904 index = (amt->event_idx + amt->nr_events) % AMT_MAX_EVENTS;
0905 amt->events[index].event = event;
0906 amt->events[index].skb = skb;
0907 amt->nr_events++;
0908 amt->event_idx %= AMT_MAX_EVENTS;
0909 queue_work(amt_wq, &amt->event_wq);
0910 spin_unlock_bh(&amt->lock);
0911
0912 return 0;
0913 }
0914
0915 static void amt_secret_work(struct work_struct *work)
0916 {
0917 struct amt_dev *amt = container_of(to_delayed_work(work),
0918 struct amt_dev,
0919 secret_wq);
0920
0921 spin_lock_bh(&amt->lock);
0922 get_random_bytes(&amt->key, sizeof(siphash_key_t));
0923 spin_unlock_bh(&amt->lock);
0924 mod_delayed_work(amt_wq, &amt->secret_wq,
0925 msecs_to_jiffies(AMT_SECRET_TIMEOUT));
0926 }
0927
0928 static void amt_event_send_discovery(struct amt_dev *amt)
0929 {
0930 if (amt->status > AMT_STATUS_SENT_DISCOVERY)
0931 goto out;
0932 get_random_bytes(&amt->nonce, sizeof(__be32));
0933
0934 amt_send_discovery(amt);
0935 out:
0936 mod_delayed_work(amt_wq, &amt->discovery_wq,
0937 msecs_to_jiffies(AMT_DISCOVERY_TIMEOUT));
0938 }
0939
0940 static void amt_discovery_work(struct work_struct *work)
0941 {
0942 struct amt_dev *amt = container_of(to_delayed_work(work),
0943 struct amt_dev,
0944 discovery_wq);
0945
0946 if (amt_queue_event(amt, AMT_EVENT_SEND_DISCOVERY, NULL))
0947 mod_delayed_work(amt_wq, &amt->discovery_wq,
0948 msecs_to_jiffies(AMT_DISCOVERY_TIMEOUT));
0949 }
0950
0951 static void amt_event_send_request(struct amt_dev *amt)
0952 {
0953 u32 exp;
0954
0955 if (amt->status < AMT_STATUS_RECEIVED_ADVERTISEMENT)
0956 goto out;
0957
0958 if (amt->req_cnt > AMT_MAX_REQ_COUNT) {
0959 netdev_dbg(amt->dev, "Gateway is not ready");
0960 amt->qi = AMT_INIT_REQ_TIMEOUT;
0961 WRITE_ONCE(amt->ready4, false);
0962 WRITE_ONCE(amt->ready6, false);
0963 amt->remote_ip = 0;
0964 amt_update_gw_status(amt, AMT_STATUS_INIT, false);
0965 amt->req_cnt = 0;
0966 amt->nonce = 0;
0967 goto out;
0968 }
0969
0970 if (!amt->req_cnt) {
0971 WRITE_ONCE(amt->ready4, false);
0972 WRITE_ONCE(amt->ready6, false);
0973 get_random_bytes(&amt->nonce, sizeof(__be32));
0974 }
0975
0976 amt_send_request(amt, false);
0977 amt_send_request(amt, true);
0978 amt_update_gw_status(amt, AMT_STATUS_SENT_REQUEST, true);
0979 amt->req_cnt++;
0980 out:
0981 exp = min_t(u32, (1 * (1 << amt->req_cnt)), AMT_MAX_REQ_TIMEOUT);
0982 mod_delayed_work(amt_wq, &amt->req_wq, msecs_to_jiffies(exp * 1000));
0983 }
0984
0985 static void amt_req_work(struct work_struct *work)
0986 {
0987 struct amt_dev *amt = container_of(to_delayed_work(work),
0988 struct amt_dev,
0989 req_wq);
0990
0991 if (amt_queue_event(amt, AMT_EVENT_SEND_REQUEST, NULL))
0992 mod_delayed_work(amt_wq, &amt->req_wq,
0993 msecs_to_jiffies(100));
0994 }
0995
0996 static bool amt_send_membership_update(struct amt_dev *amt,
0997 struct sk_buff *skb,
0998 bool v6)
0999 {
1000 struct amt_header_membership_update *amtmu;
1001 struct socket *sock;
1002 struct iphdr *iph;
1003 struct flowi4 fl4;
1004 struct rtable *rt;
1005 int err;
1006
1007 sock = rcu_dereference_bh(amt->sock);
1008 if (!sock)
1009 return true;
1010
1011 err = skb_cow_head(skb, LL_RESERVED_SPACE(amt->dev) + sizeof(*amtmu) +
1012 sizeof(*iph) + sizeof(struct udphdr));
1013 if (err)
1014 return true;
1015
1016 skb_reset_inner_headers(skb);
1017 memset(&fl4, 0, sizeof(struct flowi4));
1018 fl4.flowi4_oif = amt->stream_dev->ifindex;
1019 fl4.daddr = amt->remote_ip;
1020 fl4.saddr = amt->local_ip;
1021 fl4.flowi4_tos = AMT_TOS;
1022 fl4.flowi4_proto = IPPROTO_UDP;
1023 rt = ip_route_output_key(amt->net, &fl4);
1024 if (IS_ERR(rt)) {
1025 netdev_dbg(amt->dev, "no route to %pI4\n", &amt->remote_ip);
1026 return true;
1027 }
1028
1029 amtmu = skb_push(skb, sizeof(*amtmu));
1030 amtmu->version = 0;
1031 amtmu->type = AMT_MSG_MEMBERSHIP_UPDATE;
1032 amtmu->reserved = 0;
1033 amtmu->nonce = amt->nonce;
1034 amtmu->response_mac = amt->mac;
1035
1036 if (!v6)
1037 skb_set_inner_protocol(skb, htons(ETH_P_IP));
1038 else
1039 skb_set_inner_protocol(skb, htons(ETH_P_IPV6));
1040 udp_tunnel_xmit_skb(rt, sock->sk, skb,
1041 fl4.saddr,
1042 fl4.daddr,
1043 AMT_TOS,
1044 ip4_dst_hoplimit(&rt->dst),
1045 0,
1046 amt->gw_port,
1047 amt->relay_port,
1048 false,
1049 false);
1050 amt_update_gw_status(amt, AMT_STATUS_SENT_UPDATE, true);
1051 return false;
1052 }
1053
1054 static void amt_send_multicast_data(struct amt_dev *amt,
1055 const struct sk_buff *oskb,
1056 struct amt_tunnel_list *tunnel,
1057 bool v6)
1058 {
1059 struct amt_header_mcast_data *amtmd;
1060 struct socket *sock;
1061 struct sk_buff *skb;
1062 struct iphdr *iph;
1063 struct flowi4 fl4;
1064 struct rtable *rt;
1065
1066 sock = rcu_dereference_bh(amt->sock);
1067 if (!sock)
1068 return;
1069
1070 skb = skb_copy_expand(oskb, sizeof(*amtmd) + sizeof(*iph) +
1071 sizeof(struct udphdr), 0, GFP_ATOMIC);
1072 if (!skb)
1073 return;
1074
1075 skb_reset_inner_headers(skb);
1076 memset(&fl4, 0, sizeof(struct flowi4));
1077 fl4.flowi4_oif = amt->stream_dev->ifindex;
1078 fl4.daddr = tunnel->ip4;
1079 fl4.saddr = amt->local_ip;
1080 fl4.flowi4_proto = IPPROTO_UDP;
1081 rt = ip_route_output_key(amt->net, &fl4);
1082 if (IS_ERR(rt)) {
1083 netdev_dbg(amt->dev, "no route to %pI4\n", &tunnel->ip4);
1084 kfree_skb(skb);
1085 return;
1086 }
1087
1088 amtmd = skb_push(skb, sizeof(*amtmd));
1089 amtmd->version = 0;
1090 amtmd->reserved = 0;
1091 amtmd->type = AMT_MSG_MULTICAST_DATA;
1092
1093 if (!v6)
1094 skb_set_inner_protocol(skb, htons(ETH_P_IP));
1095 else
1096 skb_set_inner_protocol(skb, htons(ETH_P_IPV6));
1097 udp_tunnel_xmit_skb(rt, sock->sk, skb,
1098 fl4.saddr,
1099 fl4.daddr,
1100 AMT_TOS,
1101 ip4_dst_hoplimit(&rt->dst),
1102 0,
1103 amt->relay_port,
1104 tunnel->source_port,
1105 false,
1106 false);
1107 }
1108
1109 static bool amt_send_membership_query(struct amt_dev *amt,
1110 struct sk_buff *skb,
1111 struct amt_tunnel_list *tunnel,
1112 bool v6)
1113 {
1114 struct amt_header_membership_query *amtmq;
1115 struct socket *sock;
1116 struct rtable *rt;
1117 struct flowi4 fl4;
1118 int err;
1119
1120 sock = rcu_dereference_bh(amt->sock);
1121 if (!sock)
1122 return true;
1123
1124 err = skb_cow_head(skb, LL_RESERVED_SPACE(amt->dev) + sizeof(*amtmq) +
1125 sizeof(struct iphdr) + sizeof(struct udphdr));
1126 if (err)
1127 return true;
1128
1129 skb_reset_inner_headers(skb);
1130 memset(&fl4, 0, sizeof(struct flowi4));
1131 fl4.flowi4_oif = amt->stream_dev->ifindex;
1132 fl4.daddr = tunnel->ip4;
1133 fl4.saddr = amt->local_ip;
1134 fl4.flowi4_tos = AMT_TOS;
1135 fl4.flowi4_proto = IPPROTO_UDP;
1136 rt = ip_route_output_key(amt->net, &fl4);
1137 if (IS_ERR(rt)) {
1138 netdev_dbg(amt->dev, "no route to %pI4\n", &tunnel->ip4);
1139 return true;
1140 }
1141
1142 amtmq = skb_push(skb, sizeof(*amtmq));
1143 amtmq->version = 0;
1144 amtmq->type = AMT_MSG_MEMBERSHIP_QUERY;
1145 amtmq->reserved = 0;
1146 amtmq->l = 0;
1147 amtmq->g = 0;
1148 amtmq->nonce = tunnel->nonce;
1149 amtmq->response_mac = tunnel->mac;
1150
1151 if (!v6)
1152 skb_set_inner_protocol(skb, htons(ETH_P_IP));
1153 else
1154 skb_set_inner_protocol(skb, htons(ETH_P_IPV6));
1155 udp_tunnel_xmit_skb(rt, sock->sk, skb,
1156 fl4.saddr,
1157 fl4.daddr,
1158 AMT_TOS,
1159 ip4_dst_hoplimit(&rt->dst),
1160 0,
1161 amt->relay_port,
1162 tunnel->source_port,
1163 false,
1164 false);
1165 amt_update_relay_status(tunnel, AMT_STATUS_SENT_QUERY, true);
1166 return false;
1167 }
1168
1169 static netdev_tx_t amt_dev_xmit(struct sk_buff *skb, struct net_device *dev)
1170 {
1171 struct amt_dev *amt = netdev_priv(dev);
1172 struct amt_tunnel_list *tunnel;
1173 struct amt_group_node *gnode;
1174 union amt_addr group = {0,};
1175 #if IS_ENABLED(CONFIG_IPV6)
1176 struct ipv6hdr *ip6h;
1177 struct mld_msg *mld;
1178 #endif
1179 bool report = false;
1180 struct igmphdr *ih;
1181 bool query = false;
1182 struct iphdr *iph;
1183 bool data = false;
1184 bool v6 = false;
1185 u32 hash;
1186
1187 iph = ip_hdr(skb);
1188 if (iph->version == 4) {
1189 if (!ipv4_is_multicast(iph->daddr))
1190 goto free;
1191
1192 if (!ip_mc_check_igmp(skb)) {
1193 ih = igmp_hdr(skb);
1194 switch (ih->type) {
1195 case IGMPV3_HOST_MEMBERSHIP_REPORT:
1196 case IGMP_HOST_MEMBERSHIP_REPORT:
1197 report = true;
1198 break;
1199 case IGMP_HOST_MEMBERSHIP_QUERY:
1200 query = true;
1201 break;
1202 default:
1203 goto free;
1204 }
1205 } else {
1206 data = true;
1207 }
1208 v6 = false;
1209 group.ip4 = iph->daddr;
1210 #if IS_ENABLED(CONFIG_IPV6)
1211 } else if (iph->version == 6) {
1212 ip6h = ipv6_hdr(skb);
1213 if (!ipv6_addr_is_multicast(&ip6h->daddr))
1214 goto free;
1215
1216 if (!ipv6_mc_check_mld(skb)) {
1217 mld = (struct mld_msg *)skb_transport_header(skb);
1218 switch (mld->mld_type) {
1219 case ICMPV6_MGM_REPORT:
1220 case ICMPV6_MLD2_REPORT:
1221 report = true;
1222 break;
1223 case ICMPV6_MGM_QUERY:
1224 query = true;
1225 break;
1226 default:
1227 goto free;
1228 }
1229 } else {
1230 data = true;
1231 }
1232 v6 = true;
1233 group.ip6 = ip6h->daddr;
1234 #endif
1235 } else {
1236 dev->stats.tx_errors++;
1237 goto free;
1238 }
1239
1240 if (!pskb_may_pull(skb, sizeof(struct ethhdr)))
1241 goto free;
1242
1243 skb_pull(skb, sizeof(struct ethhdr));
1244
1245 if (amt->mode == AMT_MODE_GATEWAY) {
1246
1247 if (!report)
1248 goto free;
1249 if ((!v6 && !READ_ONCE(amt->ready4)) ||
1250 (v6 && !READ_ONCE(amt->ready6)))
1251 goto free;
1252 if (amt_send_membership_update(amt, skb, v6))
1253 goto free;
1254 goto unlock;
1255 } else if (amt->mode == AMT_MODE_RELAY) {
1256 if (query) {
1257 tunnel = amt_skb_cb(skb)->tunnel;
1258 if (!tunnel) {
1259 WARN_ON(1);
1260 goto free;
1261 }
1262
1263
1264 if (amt_send_membership_query(amt, skb, tunnel, v6))
1265 goto free;
1266 goto unlock;
1267 }
1268
1269 if (!data)
1270 goto free;
1271 list_for_each_entry_rcu(tunnel, &amt->tunnel_list, list) {
1272 hash = amt_group_hash(tunnel, &group);
1273 hlist_for_each_entry_rcu(gnode, &tunnel->groups[hash],
1274 node) {
1275 if (!v6) {
1276 if (gnode->group_addr.ip4 == iph->daddr)
1277 goto found;
1278 #if IS_ENABLED(CONFIG_IPV6)
1279 } else {
1280 if (ipv6_addr_equal(&gnode->group_addr.ip6,
1281 &ip6h->daddr))
1282 goto found;
1283 #endif
1284 }
1285 }
1286 continue;
1287 found:
1288 amt_send_multicast_data(amt, skb, tunnel, v6);
1289 }
1290 }
1291
1292 dev_kfree_skb(skb);
1293 return NETDEV_TX_OK;
1294 free:
1295 dev_kfree_skb(skb);
1296 unlock:
1297 dev->stats.tx_dropped++;
1298 return NETDEV_TX_OK;
1299 }
1300
1301 static int amt_parse_type(struct sk_buff *skb)
1302 {
1303 struct amt_header *amth;
1304
1305 if (!pskb_may_pull(skb, sizeof(struct udphdr) +
1306 sizeof(struct amt_header)))
1307 return -1;
1308
1309 amth = (struct amt_header *)(udp_hdr(skb) + 1);
1310
1311 if (amth->version != 0)
1312 return -1;
1313
1314 if (amth->type >= __AMT_MSG_MAX || !amth->type)
1315 return -1;
1316 return amth->type;
1317 }
1318
1319 static void amt_clear_groups(struct amt_tunnel_list *tunnel)
1320 {
1321 struct amt_dev *amt = tunnel->amt;
1322 struct amt_group_node *gnode;
1323 struct hlist_node *t;
1324 int i;
1325
1326 spin_lock_bh(&tunnel->lock);
1327 rcu_read_lock();
1328 for (i = 0; i < amt->hash_buckets; i++)
1329 hlist_for_each_entry_safe(gnode, t, &tunnel->groups[i], node)
1330 amt_del_group(amt, gnode);
1331 rcu_read_unlock();
1332 spin_unlock_bh(&tunnel->lock);
1333 }
1334
1335 static void amt_tunnel_expire(struct work_struct *work)
1336 {
1337 struct amt_tunnel_list *tunnel = container_of(to_delayed_work(work),
1338 struct amt_tunnel_list,
1339 gc_wq);
1340 struct amt_dev *amt = tunnel->amt;
1341
1342 spin_lock_bh(&amt->lock);
1343 rcu_read_lock();
1344 list_del_rcu(&tunnel->list);
1345 amt->nr_tunnels--;
1346 amt_clear_groups(tunnel);
1347 rcu_read_unlock();
1348 spin_unlock_bh(&amt->lock);
1349 kfree_rcu(tunnel, rcu);
1350 }
1351
1352 static void amt_cleanup_srcs(struct amt_dev *amt,
1353 struct amt_tunnel_list *tunnel,
1354 struct amt_group_node *gnode)
1355 {
1356 struct amt_source_node *snode;
1357 struct hlist_node *t;
1358 int i;
1359
1360
1361 for (i = 0; i < amt->hash_buckets; i++) {
1362 hlist_for_each_entry_safe(snode, t, &gnode->sources[i], node) {
1363 if (snode->flags == AMT_SOURCE_OLD)
1364 amt_destroy_source(snode);
1365 }
1366 }
1367
1368
1369 for (i = 0; i < amt->hash_buckets; i++) {
1370 hlist_for_each_entry_rcu(snode, &gnode->sources[i], node) {
1371 snode->flags = AMT_SOURCE_OLD;
1372 if (!gnode->v6)
1373 netdev_dbg(snode->gnode->amt->dev,
1374 "Add source as OLD %pI4 from %pI4\n",
1375 &snode->source_addr.ip4,
1376 &gnode->group_addr.ip4);
1377 #if IS_ENABLED(CONFIG_IPV6)
1378 else
1379 netdev_dbg(snode->gnode->amt->dev,
1380 "Add source as OLD %pI6 from %pI6\n",
1381 &snode->source_addr.ip6,
1382 &gnode->group_addr.ip6);
1383 #endif
1384 }
1385 }
1386 }
1387
1388 static void amt_add_srcs(struct amt_dev *amt, struct amt_tunnel_list *tunnel,
1389 struct amt_group_node *gnode, void *grec,
1390 bool v6)
1391 {
1392 struct igmpv3_grec *igmp_grec;
1393 struct amt_source_node *snode;
1394 #if IS_ENABLED(CONFIG_IPV6)
1395 struct mld2_grec *mld_grec;
1396 #endif
1397 union amt_addr src = {0,};
1398 u16 nsrcs;
1399 u32 hash;
1400 int i;
1401
1402 if (!v6) {
1403 igmp_grec = grec;
1404 nsrcs = ntohs(igmp_grec->grec_nsrcs);
1405 } else {
1406 #if IS_ENABLED(CONFIG_IPV6)
1407 mld_grec = grec;
1408 nsrcs = ntohs(mld_grec->grec_nsrcs);
1409 #else
1410 return;
1411 #endif
1412 }
1413 for (i = 0; i < nsrcs; i++) {
1414 if (tunnel->nr_sources >= amt->max_sources)
1415 return;
1416 if (!v6)
1417 src.ip4 = igmp_grec->grec_src[i];
1418 #if IS_ENABLED(CONFIG_IPV6)
1419 else
1420 memcpy(&src.ip6, &mld_grec->grec_src[i],
1421 sizeof(struct in6_addr));
1422 #endif
1423 if (amt_lookup_src(tunnel, gnode, AMT_FILTER_ALL, &src))
1424 continue;
1425
1426 snode = amt_alloc_snode(gnode, &src);
1427 if (snode) {
1428 hash = amt_source_hash(tunnel, &snode->source_addr);
1429 hlist_add_head_rcu(&snode->node, &gnode->sources[hash]);
1430 tunnel->nr_sources++;
1431 gnode->nr_sources++;
1432
1433 if (!gnode->v6)
1434 netdev_dbg(snode->gnode->amt->dev,
1435 "Add source as NEW %pI4 from %pI4\n",
1436 &snode->source_addr.ip4,
1437 &gnode->group_addr.ip4);
1438 #if IS_ENABLED(CONFIG_IPV6)
1439 else
1440 netdev_dbg(snode->gnode->amt->dev,
1441 "Add source as NEW %pI6 from %pI6\n",
1442 &snode->source_addr.ip6,
1443 &gnode->group_addr.ip6);
1444 #endif
1445 }
1446 }
1447 }
1448
1449
1450
1451
1452
1453
1454
1455
1456
1457
1458
1459
1460
1461
1462
1463
1464
1465
1466
1467
1468 static void amt_lookup_act_srcs(struct amt_tunnel_list *tunnel,
1469 struct amt_group_node *gnode,
1470 void *grec,
1471 enum amt_ops ops,
1472 enum amt_filter filter,
1473 enum amt_act act,
1474 bool v6)
1475 {
1476 struct amt_dev *amt = tunnel->amt;
1477 struct amt_source_node *snode;
1478 struct igmpv3_grec *igmp_grec;
1479 #if IS_ENABLED(CONFIG_IPV6)
1480 struct mld2_grec *mld_grec;
1481 #endif
1482 union amt_addr src = {0,};
1483 struct hlist_node *t;
1484 u16 nsrcs;
1485 int i, j;
1486
1487 if (!v6) {
1488 igmp_grec = grec;
1489 nsrcs = ntohs(igmp_grec->grec_nsrcs);
1490 } else {
1491 #if IS_ENABLED(CONFIG_IPV6)
1492 mld_grec = grec;
1493 nsrcs = ntohs(mld_grec->grec_nsrcs);
1494 #else
1495 return;
1496 #endif
1497 }
1498
1499 memset(&src, 0, sizeof(union amt_addr));
1500 switch (ops) {
1501 case AMT_OPS_INT:
1502
1503 for (i = 0; i < nsrcs; i++) {
1504 if (!v6)
1505 src.ip4 = igmp_grec->grec_src[i];
1506 #if IS_ENABLED(CONFIG_IPV6)
1507 else
1508 memcpy(&src.ip6, &mld_grec->grec_src[i],
1509 sizeof(struct in6_addr));
1510 #endif
1511 snode = amt_lookup_src(tunnel, gnode, filter, &src);
1512 if (!snode)
1513 continue;
1514 amt_act_src(tunnel, gnode, snode, act);
1515 }
1516 break;
1517 case AMT_OPS_UNI:
1518
1519 for (i = 0; i < amt->hash_buckets; i++) {
1520 hlist_for_each_entry_safe(snode, t, &gnode->sources[i],
1521 node) {
1522 if (amt_status_filter(snode, filter))
1523 amt_act_src(tunnel, gnode, snode, act);
1524 }
1525 }
1526 for (i = 0; i < nsrcs; i++) {
1527 if (!v6)
1528 src.ip4 = igmp_grec->grec_src[i];
1529 #if IS_ENABLED(CONFIG_IPV6)
1530 else
1531 memcpy(&src.ip6, &mld_grec->grec_src[i],
1532 sizeof(struct in6_addr));
1533 #endif
1534 snode = amt_lookup_src(tunnel, gnode, filter, &src);
1535 if (!snode)
1536 continue;
1537 amt_act_src(tunnel, gnode, snode, act);
1538 }
1539 break;
1540 case AMT_OPS_SUB:
1541
1542 for (i = 0; i < amt->hash_buckets; i++) {
1543 hlist_for_each_entry_safe(snode, t, &gnode->sources[i],
1544 node) {
1545 if (!amt_status_filter(snode, filter))
1546 continue;
1547 for (j = 0; j < nsrcs; j++) {
1548 if (!v6)
1549 src.ip4 = igmp_grec->grec_src[j];
1550 #if IS_ENABLED(CONFIG_IPV6)
1551 else
1552 memcpy(&src.ip6,
1553 &mld_grec->grec_src[j],
1554 sizeof(struct in6_addr));
1555 #endif
1556 if (amt_addr_equal(&snode->source_addr,
1557 &src))
1558 goto out_sub;
1559 }
1560 amt_act_src(tunnel, gnode, snode, act);
1561 continue;
1562 out_sub:;
1563 }
1564 }
1565 break;
1566 case AMT_OPS_SUB_REV:
1567
1568 for (i = 0; i < nsrcs; i++) {
1569 if (!v6)
1570 src.ip4 = igmp_grec->grec_src[i];
1571 #if IS_ENABLED(CONFIG_IPV6)
1572 else
1573 memcpy(&src.ip6, &mld_grec->grec_src[i],
1574 sizeof(struct in6_addr));
1575 #endif
1576 snode = amt_lookup_src(tunnel, gnode, AMT_FILTER_ALL,
1577 &src);
1578 if (!snode) {
1579 snode = amt_lookup_src(tunnel, gnode,
1580 filter, &src);
1581 if (snode)
1582 amt_act_src(tunnel, gnode, snode, act);
1583 }
1584 }
1585 break;
1586 default:
1587 netdev_dbg(amt->dev, "Invalid type\n");
1588 return;
1589 }
1590 }
1591
1592 static void amt_mcast_is_in_handler(struct amt_dev *amt,
1593 struct amt_tunnel_list *tunnel,
1594 struct amt_group_node *gnode,
1595 void *grec, void *zero_grec, bool v6)
1596 {
1597 if (gnode->filter_mode == MCAST_INCLUDE) {
1598
1599
1600
1601
1602
1603 amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_UNI,
1604 AMT_FILTER_NONE_NEW,
1605 AMT_ACT_STATUS_FWD_NEW,
1606 v6);
1607
1608 amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_UNI,
1609 AMT_FILTER_FWD,
1610 AMT_ACT_STATUS_FWD_NEW,
1611 v6);
1612
1613 amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_INT,
1614 AMT_FILTER_FWD_NEW,
1615 AMT_ACT_GMI,
1616 v6);
1617 } else {
1618
1619
1620
1621
1622
1623 amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_INT,
1624 AMT_FILTER_BOTH,
1625 AMT_ACT_STATUS_NONE_NEW,
1626 v6);
1627
1628 amt_lookup_act_srcs(tunnel, gnode, zero_grec, AMT_OPS_UNI,
1629 AMT_FILTER_FWD,
1630 AMT_ACT_STATUS_FWD_NEW,
1631 v6);
1632
1633 amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_INT,
1634 AMT_FILTER_NONE_NEW,
1635 AMT_ACT_STATUS_FWD_NEW,
1636 v6);
1637
1638 amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_SUB,
1639 AMT_FILTER_D_FWD,
1640 AMT_ACT_STATUS_D_FWD_NEW,
1641 v6);
1642 }
1643 }
1644
1645 static void amt_mcast_is_ex_handler(struct amt_dev *amt,
1646 struct amt_tunnel_list *tunnel,
1647 struct amt_group_node *gnode,
1648 void *grec, void *zero_grec, bool v6)
1649 {
1650 if (gnode->filter_mode == MCAST_INCLUDE) {
1651
1652
1653
1654
1655
1656
1657
1658 amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_INT,
1659 AMT_FILTER_FWD,
1660 AMT_ACT_STATUS_FWD_NEW,
1661 v6);
1662
1663 amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_SUB_REV,
1664 AMT_FILTER_FWD,
1665 AMT_ACT_STATUS_D_FWD_NEW,
1666 v6);
1667
1668 amt_lookup_act_srcs(tunnel, gnode, zero_grec, AMT_OPS_UNI,
1669 AMT_FILTER_D_FWD_NEW,
1670 AMT_ACT_GMI_ZERO,
1671 v6);
1672
1673 if (!mod_delayed_work(amt_wq, &gnode->group_timer,
1674 msecs_to_jiffies(amt_gmi(amt))))
1675 dev_hold(amt->dev);
1676 gnode->filter_mode = MCAST_EXCLUDE;
1677
1678 } else {
1679
1680
1681
1682
1683
1684
1685
1686
1687 amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_SUB_REV,
1688 AMT_FILTER_D_FWD,
1689 AMT_ACT_STATUS_FWD_NEW,
1690 v6);
1691
1692 amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_INT,
1693 AMT_FILTER_D_FWD,
1694 AMT_ACT_STATUS_D_FWD_NEW,
1695 v6);
1696
1697 amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_SUB_REV,
1698 AMT_FILTER_BOTH_NEW,
1699 AMT_ACT_GMI,
1700 v6);
1701
1702 if (!mod_delayed_work(amt_wq, &gnode->group_timer,
1703 msecs_to_jiffies(amt_gmi(amt))))
1704 dev_hold(amt->dev);
1705
1706 }
1707 }
1708
1709 static void amt_mcast_to_in_handler(struct amt_dev *amt,
1710 struct amt_tunnel_list *tunnel,
1711 struct amt_group_node *gnode,
1712 void *grec, void *zero_grec, bool v6)
1713 {
1714 if (gnode->filter_mode == MCAST_INCLUDE) {
1715
1716
1717
1718
1719
1720
1721 amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_UNI,
1722 AMT_FILTER_NONE_NEW,
1723 AMT_ACT_STATUS_FWD_NEW,
1724 v6);
1725
1726 amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_UNI,
1727 AMT_FILTER_FWD,
1728 AMT_ACT_STATUS_FWD_NEW,
1729 v6);
1730
1731 amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_INT,
1732 AMT_FILTER_FWD_NEW,
1733 AMT_ACT_GMI,
1734 v6);
1735 } else {
1736
1737
1738
1739
1740
1741
1742
1743 amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_UNI,
1744 AMT_FILTER_NONE_NEW,
1745 AMT_ACT_STATUS_FWD_NEW,
1746 v6);
1747
1748 amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_UNI,
1749 AMT_FILTER_FWD,
1750 AMT_ACT_STATUS_FWD_NEW,
1751 v6);
1752
1753
1754
1755
1756 amt_lookup_act_srcs(tunnel, gnode, zero_grec, AMT_OPS_UNI,
1757 AMT_FILTER_D_FWD,
1758 AMT_ACT_STATUS_D_FWD_NEW,
1759 v6);
1760
1761
1762
1763 amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_INT,
1764 AMT_FILTER_FWD_NEW,
1765 AMT_ACT_GMI,
1766 v6);
1767 }
1768 }
1769
1770 static void amt_mcast_to_ex_handler(struct amt_dev *amt,
1771 struct amt_tunnel_list *tunnel,
1772 struct amt_group_node *gnode,
1773 void *grec, void *zero_grec, bool v6)
1774 {
1775 if (gnode->filter_mode == MCAST_INCLUDE) {
1776
1777
1778
1779
1780
1781
1782
1783
1784 amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_INT,
1785 AMT_FILTER_FWD,
1786 AMT_ACT_STATUS_FWD_NEW,
1787 v6);
1788
1789 amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_SUB_REV,
1790 AMT_FILTER_FWD,
1791 AMT_ACT_STATUS_D_FWD_NEW,
1792 v6);
1793
1794 amt_lookup_act_srcs(tunnel, gnode, zero_grec, AMT_OPS_UNI,
1795 AMT_FILTER_D_FWD_NEW,
1796 AMT_ACT_GMI_ZERO,
1797 v6);
1798
1799 if (!mod_delayed_work(amt_wq, &gnode->group_timer,
1800 msecs_to_jiffies(amt_gmi(amt))))
1801 dev_hold(amt->dev);
1802 gnode->filter_mode = MCAST_EXCLUDE;
1803
1804 } else {
1805
1806
1807
1808
1809
1810
1811
1812
1813
1814 amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_SUB_REV,
1815 AMT_FILTER_BOTH,
1816 AMT_ACT_GT,
1817 v6);
1818
1819 amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_SUB_REV,
1820 AMT_FILTER_D_FWD,
1821 AMT_ACT_STATUS_FWD_NEW,
1822 v6);
1823
1824 amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_INT,
1825 AMT_FILTER_D_FWD,
1826 AMT_ACT_STATUS_D_FWD_NEW,
1827 v6);
1828
1829 if (!mod_delayed_work(amt_wq, &gnode->group_timer,
1830 msecs_to_jiffies(amt_gmi(amt))))
1831 dev_hold(amt->dev);
1832
1833 }
1834 }
1835
1836 static void amt_mcast_allow_handler(struct amt_dev *amt,
1837 struct amt_tunnel_list *tunnel,
1838 struct amt_group_node *gnode,
1839 void *grec, void *zero_grec, bool v6)
1840 {
1841 if (gnode->filter_mode == MCAST_INCLUDE) {
1842
1843
1844
1845
1846
1847 amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_UNI,
1848 AMT_FILTER_FWD,
1849 AMT_ACT_STATUS_FWD_NEW,
1850 v6);
1851
1852 amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_INT,
1853 AMT_FILTER_FWD_NEW,
1854 AMT_ACT_GMI,
1855 v6);
1856 } else {
1857
1858
1859
1860
1861
1862 amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_UNI,
1863 AMT_FILTER_FWD,
1864 AMT_ACT_STATUS_FWD_NEW,
1865 v6);
1866
1867 amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_SUB,
1868 AMT_FILTER_D_FWD,
1869 AMT_ACT_STATUS_D_FWD_NEW,
1870 v6);
1871
1872
1873
1874 amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_INT,
1875 AMT_FILTER_FWD_NEW,
1876 AMT_ACT_GMI,
1877 v6);
1878 }
1879 }
1880
1881 static void amt_mcast_block_handler(struct amt_dev *amt,
1882 struct amt_tunnel_list *tunnel,
1883 struct amt_group_node *gnode,
1884 void *grec, void *zero_grec, bool v6)
1885 {
1886 if (gnode->filter_mode == MCAST_INCLUDE) {
1887
1888
1889
1890
1891
1892 amt_lookup_act_srcs(tunnel, gnode, zero_grec, AMT_OPS_UNI,
1893 AMT_FILTER_FWD,
1894 AMT_ACT_STATUS_FWD_NEW,
1895 v6);
1896 } else {
1897
1898
1899
1900
1901
1902
1903 amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_SUB_REV,
1904 AMT_FILTER_BOTH,
1905 AMT_ACT_GT,
1906 v6);
1907
1908 amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_UNI,
1909 AMT_FILTER_FWD,
1910 AMT_ACT_STATUS_FWD_NEW,
1911 v6);
1912
1913 amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_SUB_REV,
1914 AMT_FILTER_D_FWD,
1915 AMT_ACT_STATUS_FWD_NEW,
1916 v6);
1917
1918 amt_lookup_act_srcs(tunnel, gnode, grec, AMT_OPS_UNI,
1919 AMT_FILTER_D_FWD,
1920 AMT_ACT_STATUS_D_FWD_NEW,
1921 v6);
1922 }
1923 }
1924
1925
1926
1927
1928
1929
1930
1931
1932
1933
1934
1935
1936
1937 static void amt_igmpv2_report_handler(struct amt_dev *amt, struct sk_buff *skb,
1938 struct amt_tunnel_list *tunnel)
1939 {
1940 struct igmphdr *ih = igmp_hdr(skb);
1941 struct iphdr *iph = ip_hdr(skb);
1942 struct amt_group_node *gnode;
1943 union amt_addr group, host;
1944
1945 memset(&group, 0, sizeof(union amt_addr));
1946 group.ip4 = ih->group;
1947 memset(&host, 0, sizeof(union amt_addr));
1948 host.ip4 = iph->saddr;
1949
1950 gnode = amt_lookup_group(tunnel, &group, &host, false);
1951 if (!gnode) {
1952 gnode = amt_add_group(amt, tunnel, &group, &host, false);
1953 if (!IS_ERR(gnode)) {
1954 gnode->filter_mode = MCAST_EXCLUDE;
1955 if (!mod_delayed_work(amt_wq, &gnode->group_timer,
1956 msecs_to_jiffies(amt_gmi(amt))))
1957 dev_hold(amt->dev);
1958 }
1959 }
1960 }
1961
1962
1963
1964
1965
1966
1967
1968
1969
1970
1971
1972
1973
1974 static void amt_igmpv2_leave_handler(struct amt_dev *amt, struct sk_buff *skb,
1975 struct amt_tunnel_list *tunnel)
1976 {
1977 struct igmphdr *ih = igmp_hdr(skb);
1978 struct iphdr *iph = ip_hdr(skb);
1979 struct amt_group_node *gnode;
1980 union amt_addr group, host;
1981
1982 memset(&group, 0, sizeof(union amt_addr));
1983 group.ip4 = ih->group;
1984 memset(&host, 0, sizeof(union amt_addr));
1985 host.ip4 = iph->saddr;
1986
1987 gnode = amt_lookup_group(tunnel, &group, &host, false);
1988 if (gnode)
1989 amt_del_group(amt, gnode);
1990 }
1991
1992 static void amt_igmpv3_report_handler(struct amt_dev *amt, struct sk_buff *skb,
1993 struct amt_tunnel_list *tunnel)
1994 {
1995 struct igmpv3_report *ihrv3 = igmpv3_report_hdr(skb);
1996 int len = skb_transport_offset(skb) + sizeof(*ihrv3);
1997 void *zero_grec = (void *)&igmpv3_zero_grec;
1998 struct iphdr *iph = ip_hdr(skb);
1999 struct amt_group_node *gnode;
2000 union amt_addr group, host;
2001 struct igmpv3_grec *grec;
2002 u16 nsrcs;
2003 int i;
2004
2005 for (i = 0; i < ntohs(ihrv3->ngrec); i++) {
2006 len += sizeof(*grec);
2007 if (!ip_mc_may_pull(skb, len))
2008 break;
2009
2010 grec = (void *)(skb->data + len - sizeof(*grec));
2011 nsrcs = ntohs(grec->grec_nsrcs);
2012
2013 len += nsrcs * sizeof(__be32);
2014 if (!ip_mc_may_pull(skb, len))
2015 break;
2016
2017 memset(&group, 0, sizeof(union amt_addr));
2018 group.ip4 = grec->grec_mca;
2019 memset(&host, 0, sizeof(union amt_addr));
2020 host.ip4 = iph->saddr;
2021 gnode = amt_lookup_group(tunnel, &group, &host, false);
2022 if (!gnode) {
2023 gnode = amt_add_group(amt, tunnel, &group, &host,
2024 false);
2025 if (IS_ERR(gnode))
2026 continue;
2027 }
2028
2029 amt_add_srcs(amt, tunnel, gnode, grec, false);
2030 switch (grec->grec_type) {
2031 case IGMPV3_MODE_IS_INCLUDE:
2032 amt_mcast_is_in_handler(amt, tunnel, gnode, grec,
2033 zero_grec, false);
2034 break;
2035 case IGMPV3_MODE_IS_EXCLUDE:
2036 amt_mcast_is_ex_handler(amt, tunnel, gnode, grec,
2037 zero_grec, false);
2038 break;
2039 case IGMPV3_CHANGE_TO_INCLUDE:
2040 amt_mcast_to_in_handler(amt, tunnel, gnode, grec,
2041 zero_grec, false);
2042 break;
2043 case IGMPV3_CHANGE_TO_EXCLUDE:
2044 amt_mcast_to_ex_handler(amt, tunnel, gnode, grec,
2045 zero_grec, false);
2046 break;
2047 case IGMPV3_ALLOW_NEW_SOURCES:
2048 amt_mcast_allow_handler(amt, tunnel, gnode, grec,
2049 zero_grec, false);
2050 break;
2051 case IGMPV3_BLOCK_OLD_SOURCES:
2052 amt_mcast_block_handler(amt, tunnel, gnode, grec,
2053 zero_grec, false);
2054 break;
2055 default:
2056 break;
2057 }
2058 amt_cleanup_srcs(amt, tunnel, gnode);
2059 }
2060 }
2061
2062
2063 static void amt_igmp_report_handler(struct amt_dev *amt, struct sk_buff *skb,
2064 struct amt_tunnel_list *tunnel)
2065 {
2066 struct igmphdr *ih = igmp_hdr(skb);
2067
2068 switch (ih->type) {
2069 case IGMPV3_HOST_MEMBERSHIP_REPORT:
2070 amt_igmpv3_report_handler(amt, skb, tunnel);
2071 break;
2072 case IGMPV2_HOST_MEMBERSHIP_REPORT:
2073 amt_igmpv2_report_handler(amt, skb, tunnel);
2074 break;
2075 case IGMP_HOST_LEAVE_MESSAGE:
2076 amt_igmpv2_leave_handler(amt, skb, tunnel);
2077 break;
2078 default:
2079 break;
2080 }
2081 }
2082
2083 #if IS_ENABLED(CONFIG_IPV6)
2084
2085
2086
2087
2088
2089
2090
2091
2092
2093
2094
2095
2096
2097
2098 static void amt_mldv1_report_handler(struct amt_dev *amt, struct sk_buff *skb,
2099 struct amt_tunnel_list *tunnel)
2100 {
2101 struct mld_msg *mld = (struct mld_msg *)icmp6_hdr(skb);
2102 struct ipv6hdr *ip6h = ipv6_hdr(skb);
2103 struct amt_group_node *gnode;
2104 union amt_addr group, host;
2105
2106 memcpy(&group.ip6, &mld->mld_mca, sizeof(struct in6_addr));
2107 memcpy(&host.ip6, &ip6h->saddr, sizeof(struct in6_addr));
2108
2109 gnode = amt_lookup_group(tunnel, &group, &host, true);
2110 if (!gnode) {
2111 gnode = amt_add_group(amt, tunnel, &group, &host, true);
2112 if (!IS_ERR(gnode)) {
2113 gnode->filter_mode = MCAST_EXCLUDE;
2114 if (!mod_delayed_work(amt_wq, &gnode->group_timer,
2115 msecs_to_jiffies(amt_gmi(amt))))
2116 dev_hold(amt->dev);
2117 }
2118 }
2119 }
2120
2121
2122
2123
2124
2125
2126
2127
2128
2129
2130
2131
2132
2133
2134
2135 static void amt_mldv1_leave_handler(struct amt_dev *amt, struct sk_buff *skb,
2136 struct amt_tunnel_list *tunnel)
2137 {
2138 struct mld_msg *mld = (struct mld_msg *)icmp6_hdr(skb);
2139 struct iphdr *iph = ip_hdr(skb);
2140 struct amt_group_node *gnode;
2141 union amt_addr group, host;
2142
2143 memcpy(&group.ip6, &mld->mld_mca, sizeof(struct in6_addr));
2144 memset(&host, 0, sizeof(union amt_addr));
2145 host.ip4 = iph->saddr;
2146
2147 gnode = amt_lookup_group(tunnel, &group, &host, true);
2148 if (gnode) {
2149 amt_del_group(amt, gnode);
2150 return;
2151 }
2152 }
2153
2154 static void amt_mldv2_report_handler(struct amt_dev *amt, struct sk_buff *skb,
2155 struct amt_tunnel_list *tunnel)
2156 {
2157 struct mld2_report *mld2r = (struct mld2_report *)icmp6_hdr(skb);
2158 int len = skb_transport_offset(skb) + sizeof(*mld2r);
2159 void *zero_grec = (void *)&mldv2_zero_grec;
2160 struct ipv6hdr *ip6h = ipv6_hdr(skb);
2161 struct amt_group_node *gnode;
2162 union amt_addr group, host;
2163 struct mld2_grec *grec;
2164 u16 nsrcs;
2165 int i;
2166
2167 for (i = 0; i < ntohs(mld2r->mld2r_ngrec); i++) {
2168 len += sizeof(*grec);
2169 if (!ipv6_mc_may_pull(skb, len))
2170 break;
2171
2172 grec = (void *)(skb->data + len - sizeof(*grec));
2173 nsrcs = ntohs(grec->grec_nsrcs);
2174
2175 len += nsrcs * sizeof(struct in6_addr);
2176 if (!ipv6_mc_may_pull(skb, len))
2177 break;
2178
2179 memset(&group, 0, sizeof(union amt_addr));
2180 group.ip6 = grec->grec_mca;
2181 memset(&host, 0, sizeof(union amt_addr));
2182 host.ip6 = ip6h->saddr;
2183 gnode = amt_lookup_group(tunnel, &group, &host, true);
2184 if (!gnode) {
2185 gnode = amt_add_group(amt, tunnel, &group, &host,
2186 ETH_P_IPV6);
2187 if (IS_ERR(gnode))
2188 continue;
2189 }
2190
2191 amt_add_srcs(amt, tunnel, gnode, grec, true);
2192 switch (grec->grec_type) {
2193 case MLD2_MODE_IS_INCLUDE:
2194 amt_mcast_is_in_handler(amt, tunnel, gnode, grec,
2195 zero_grec, true);
2196 break;
2197 case MLD2_MODE_IS_EXCLUDE:
2198 amt_mcast_is_ex_handler(amt, tunnel, gnode, grec,
2199 zero_grec, true);
2200 break;
2201 case MLD2_CHANGE_TO_INCLUDE:
2202 amt_mcast_to_in_handler(amt, tunnel, gnode, grec,
2203 zero_grec, true);
2204 break;
2205 case MLD2_CHANGE_TO_EXCLUDE:
2206 amt_mcast_to_ex_handler(amt, tunnel, gnode, grec,
2207 zero_grec, true);
2208 break;
2209 case MLD2_ALLOW_NEW_SOURCES:
2210 amt_mcast_allow_handler(amt, tunnel, gnode, grec,
2211 zero_grec, true);
2212 break;
2213 case MLD2_BLOCK_OLD_SOURCES:
2214 amt_mcast_block_handler(amt, tunnel, gnode, grec,
2215 zero_grec, true);
2216 break;
2217 default:
2218 break;
2219 }
2220 amt_cleanup_srcs(amt, tunnel, gnode);
2221 }
2222 }
2223
2224
2225 static void amt_mld_report_handler(struct amt_dev *amt, struct sk_buff *skb,
2226 struct amt_tunnel_list *tunnel)
2227 {
2228 struct mld_msg *mld = (struct mld_msg *)icmp6_hdr(skb);
2229
2230 switch (mld->mld_type) {
2231 case ICMPV6_MGM_REPORT:
2232 amt_mldv1_report_handler(amt, skb, tunnel);
2233 break;
2234 case ICMPV6_MLD2_REPORT:
2235 amt_mldv2_report_handler(amt, skb, tunnel);
2236 break;
2237 case ICMPV6_MGM_REDUCTION:
2238 amt_mldv1_leave_handler(amt, skb, tunnel);
2239 break;
2240 default:
2241 break;
2242 }
2243 }
2244 #endif
2245
2246 static bool amt_advertisement_handler(struct amt_dev *amt, struct sk_buff *skb)
2247 {
2248 struct amt_header_advertisement *amta;
2249 int hdr_size;
2250
2251 hdr_size = sizeof(*amta) + sizeof(struct udphdr);
2252 if (!pskb_may_pull(skb, hdr_size))
2253 return true;
2254
2255 amta = (struct amt_header_advertisement *)(udp_hdr(skb) + 1);
2256 if (!amta->ip4)
2257 return true;
2258
2259 if (amta->reserved || amta->version)
2260 return true;
2261
2262 if (ipv4_is_loopback(amta->ip4) || ipv4_is_multicast(amta->ip4) ||
2263 ipv4_is_zeronet(amta->ip4))
2264 return true;
2265
2266 if (amt->status != AMT_STATUS_SENT_DISCOVERY ||
2267 amt->nonce != amta->nonce)
2268 return true;
2269
2270 amt->remote_ip = amta->ip4;
2271 netdev_dbg(amt->dev, "advertised remote ip = %pI4\n", &amt->remote_ip);
2272 mod_delayed_work(amt_wq, &amt->req_wq, 0);
2273
2274 amt_update_gw_status(amt, AMT_STATUS_RECEIVED_ADVERTISEMENT, true);
2275 return false;
2276 }
2277
2278 static bool amt_multicast_data_handler(struct amt_dev *amt, struct sk_buff *skb)
2279 {
2280 struct amt_header_mcast_data *amtmd;
2281 int hdr_size, len, err;
2282 struct ethhdr *eth;
2283 struct iphdr *iph;
2284
2285 if (READ_ONCE(amt->status) != AMT_STATUS_SENT_UPDATE)
2286 return true;
2287
2288 hdr_size = sizeof(*amtmd) + sizeof(struct udphdr);
2289 if (!pskb_may_pull(skb, hdr_size))
2290 return true;
2291
2292 amtmd = (struct amt_header_mcast_data *)(udp_hdr(skb) + 1);
2293 if (amtmd->reserved || amtmd->version)
2294 return true;
2295
2296 if (iptunnel_pull_header(skb, hdr_size, htons(ETH_P_IP), false))
2297 return true;
2298
2299 skb_reset_network_header(skb);
2300 skb_push(skb, sizeof(*eth));
2301 skb_reset_mac_header(skb);
2302 skb_pull(skb, sizeof(*eth));
2303 eth = eth_hdr(skb);
2304
2305 if (!pskb_may_pull(skb, sizeof(*iph)))
2306 return true;
2307 iph = ip_hdr(skb);
2308
2309 if (iph->version == 4) {
2310 if (!ipv4_is_multicast(iph->daddr))
2311 return true;
2312 skb->protocol = htons(ETH_P_IP);
2313 eth->h_proto = htons(ETH_P_IP);
2314 ip_eth_mc_map(iph->daddr, eth->h_dest);
2315 #if IS_ENABLED(CONFIG_IPV6)
2316 } else if (iph->version == 6) {
2317 struct ipv6hdr *ip6h;
2318
2319 if (!pskb_may_pull(skb, sizeof(*ip6h)))
2320 return true;
2321
2322 ip6h = ipv6_hdr(skb);
2323 if (!ipv6_addr_is_multicast(&ip6h->daddr))
2324 return true;
2325 skb->protocol = htons(ETH_P_IPV6);
2326 eth->h_proto = htons(ETH_P_IPV6);
2327 ipv6_eth_mc_map(&ip6h->daddr, eth->h_dest);
2328 #endif
2329 } else {
2330 return true;
2331 }
2332
2333 skb->pkt_type = PACKET_MULTICAST;
2334 skb->ip_summed = CHECKSUM_NONE;
2335 len = skb->len;
2336 err = gro_cells_receive(&amt->gro_cells, skb);
2337 if (likely(err == NET_RX_SUCCESS))
2338 dev_sw_netstats_rx_add(amt->dev, len);
2339 else
2340 amt->dev->stats.rx_dropped++;
2341
2342 return false;
2343 }
2344
2345 static bool amt_membership_query_handler(struct amt_dev *amt,
2346 struct sk_buff *skb)
2347 {
2348 struct amt_header_membership_query *amtmq;
2349 struct igmpv3_query *ihv3;
2350 struct ethhdr *eth, *oeth;
2351 struct iphdr *iph;
2352 int hdr_size, len;
2353
2354 hdr_size = sizeof(*amtmq) + sizeof(struct udphdr);
2355 if (!pskb_may_pull(skb, hdr_size))
2356 return true;
2357
2358 amtmq = (struct amt_header_membership_query *)(udp_hdr(skb) + 1);
2359 if (amtmq->reserved || amtmq->version)
2360 return true;
2361
2362 if (amtmq->nonce != amt->nonce)
2363 return true;
2364
2365 hdr_size -= sizeof(*eth);
2366 if (iptunnel_pull_header(skb, hdr_size, htons(ETH_P_TEB), false))
2367 return true;
2368
2369 oeth = eth_hdr(skb);
2370 skb_reset_mac_header(skb);
2371 skb_pull(skb, sizeof(*eth));
2372 skb_reset_network_header(skb);
2373 eth = eth_hdr(skb);
2374 if (!pskb_may_pull(skb, sizeof(*iph)))
2375 return true;
2376
2377 iph = ip_hdr(skb);
2378 if (iph->version == 4) {
2379 if (READ_ONCE(amt->ready4))
2380 return true;
2381
2382 if (!pskb_may_pull(skb, sizeof(*iph) + AMT_IPHDR_OPTS +
2383 sizeof(*ihv3)))
2384 return true;
2385
2386 if (!ipv4_is_multicast(iph->daddr))
2387 return true;
2388
2389 ihv3 = skb_pull(skb, sizeof(*iph) + AMT_IPHDR_OPTS);
2390 skb_reset_transport_header(skb);
2391 skb_push(skb, sizeof(*iph) + AMT_IPHDR_OPTS);
2392 WRITE_ONCE(amt->ready4, true);
2393 amt->mac = amtmq->response_mac;
2394 amt->req_cnt = 0;
2395 amt->qi = ihv3->qqic;
2396 skb->protocol = htons(ETH_P_IP);
2397 eth->h_proto = htons(ETH_P_IP);
2398 ip_eth_mc_map(iph->daddr, eth->h_dest);
2399 #if IS_ENABLED(CONFIG_IPV6)
2400 } else if (iph->version == 6) {
2401 struct mld2_query *mld2q;
2402 struct ipv6hdr *ip6h;
2403
2404 if (READ_ONCE(amt->ready6))
2405 return true;
2406
2407 if (!pskb_may_pull(skb, sizeof(*ip6h) + AMT_IP6HDR_OPTS +
2408 sizeof(*mld2q)))
2409 return true;
2410
2411 ip6h = ipv6_hdr(skb);
2412 if (!ipv6_addr_is_multicast(&ip6h->daddr))
2413 return true;
2414
2415 mld2q = skb_pull(skb, sizeof(*ip6h) + AMT_IP6HDR_OPTS);
2416 skb_reset_transport_header(skb);
2417 skb_push(skb, sizeof(*ip6h) + AMT_IP6HDR_OPTS);
2418 WRITE_ONCE(amt->ready6, true);
2419 amt->mac = amtmq->response_mac;
2420 amt->req_cnt = 0;
2421 amt->qi = mld2q->mld2q_qqic;
2422 skb->protocol = htons(ETH_P_IPV6);
2423 eth->h_proto = htons(ETH_P_IPV6);
2424 ipv6_eth_mc_map(&ip6h->daddr, eth->h_dest);
2425 #endif
2426 } else {
2427 return true;
2428 }
2429
2430 ether_addr_copy(eth->h_source, oeth->h_source);
2431 skb->pkt_type = PACKET_MULTICAST;
2432 skb->ip_summed = CHECKSUM_NONE;
2433 len = skb->len;
2434 local_bh_disable();
2435 if (__netif_rx(skb) == NET_RX_SUCCESS) {
2436 amt_update_gw_status(amt, AMT_STATUS_RECEIVED_QUERY, true);
2437 dev_sw_netstats_rx_add(amt->dev, len);
2438 } else {
2439 amt->dev->stats.rx_dropped++;
2440 }
2441 local_bh_enable();
2442
2443 return false;
2444 }
2445
2446 static bool amt_update_handler(struct amt_dev *amt, struct sk_buff *skb)
2447 {
2448 struct amt_header_membership_update *amtmu;
2449 struct amt_tunnel_list *tunnel;
2450 struct ethhdr *eth;
2451 struct iphdr *iph;
2452 int len, hdr_size;
2453
2454 iph = ip_hdr(skb);
2455
2456 hdr_size = sizeof(*amtmu) + sizeof(struct udphdr);
2457 if (!pskb_may_pull(skb, hdr_size))
2458 return true;
2459
2460 amtmu = (struct amt_header_membership_update *)(udp_hdr(skb) + 1);
2461 if (amtmu->reserved || amtmu->version)
2462 return true;
2463
2464 if (iptunnel_pull_header(skb, hdr_size, skb->protocol, false))
2465 return true;
2466
2467 skb_reset_network_header(skb);
2468
2469 list_for_each_entry_rcu(tunnel, &amt->tunnel_list, list) {
2470 if (tunnel->ip4 == iph->saddr) {
2471 if ((amtmu->nonce == tunnel->nonce &&
2472 amtmu->response_mac == tunnel->mac)) {
2473 mod_delayed_work(amt_wq, &tunnel->gc_wq,
2474 msecs_to_jiffies(amt_gmi(amt))
2475 * 3);
2476 goto report;
2477 } else {
2478 netdev_dbg(amt->dev, "Invalid MAC\n");
2479 return true;
2480 }
2481 }
2482 }
2483
2484 return true;
2485
2486 report:
2487 if (!pskb_may_pull(skb, sizeof(*iph)))
2488 return true;
2489
2490 iph = ip_hdr(skb);
2491 if (iph->version == 4) {
2492 if (ip_mc_check_igmp(skb)) {
2493 netdev_dbg(amt->dev, "Invalid IGMP\n");
2494 return true;
2495 }
2496
2497 spin_lock_bh(&tunnel->lock);
2498 amt_igmp_report_handler(amt, skb, tunnel);
2499 spin_unlock_bh(&tunnel->lock);
2500
2501 skb_push(skb, sizeof(struct ethhdr));
2502 skb_reset_mac_header(skb);
2503 eth = eth_hdr(skb);
2504 skb->protocol = htons(ETH_P_IP);
2505 eth->h_proto = htons(ETH_P_IP);
2506 ip_eth_mc_map(iph->daddr, eth->h_dest);
2507 #if IS_ENABLED(CONFIG_IPV6)
2508 } else if (iph->version == 6) {
2509 struct ipv6hdr *ip6h = ipv6_hdr(skb);
2510
2511 if (ipv6_mc_check_mld(skb)) {
2512 netdev_dbg(amt->dev, "Invalid MLD\n");
2513 return true;
2514 }
2515
2516 spin_lock_bh(&tunnel->lock);
2517 amt_mld_report_handler(amt, skb, tunnel);
2518 spin_unlock_bh(&tunnel->lock);
2519
2520 skb_push(skb, sizeof(struct ethhdr));
2521 skb_reset_mac_header(skb);
2522 eth = eth_hdr(skb);
2523 skb->protocol = htons(ETH_P_IPV6);
2524 eth->h_proto = htons(ETH_P_IPV6);
2525 ipv6_eth_mc_map(&ip6h->daddr, eth->h_dest);
2526 #endif
2527 } else {
2528 netdev_dbg(amt->dev, "Unsupported Protocol\n");
2529 return true;
2530 }
2531
2532 skb_pull(skb, sizeof(struct ethhdr));
2533 skb->pkt_type = PACKET_MULTICAST;
2534 skb->ip_summed = CHECKSUM_NONE;
2535 len = skb->len;
2536 if (__netif_rx(skb) == NET_RX_SUCCESS) {
2537 amt_update_relay_status(tunnel, AMT_STATUS_RECEIVED_UPDATE,
2538 true);
2539 dev_sw_netstats_rx_add(amt->dev, len);
2540 } else {
2541 amt->dev->stats.rx_dropped++;
2542 }
2543
2544 return false;
2545 }
2546
2547 static void amt_send_advertisement(struct amt_dev *amt, __be32 nonce,
2548 __be32 daddr, __be16 dport)
2549 {
2550 struct amt_header_advertisement *amta;
2551 int hlen, tlen, offset;
2552 struct socket *sock;
2553 struct udphdr *udph;
2554 struct sk_buff *skb;
2555 struct iphdr *iph;
2556 struct rtable *rt;
2557 struct flowi4 fl4;
2558 u32 len;
2559 int err;
2560
2561 rcu_read_lock();
2562 sock = rcu_dereference(amt->sock);
2563 if (!sock)
2564 goto out;
2565
2566 if (!netif_running(amt->stream_dev) || !netif_running(amt->dev))
2567 goto out;
2568
2569 rt = ip_route_output_ports(amt->net, &fl4, sock->sk,
2570 daddr, amt->local_ip,
2571 dport, amt->relay_port,
2572 IPPROTO_UDP, 0,
2573 amt->stream_dev->ifindex);
2574 if (IS_ERR(rt)) {
2575 amt->dev->stats.tx_errors++;
2576 goto out;
2577 }
2578
2579 hlen = LL_RESERVED_SPACE(amt->dev);
2580 tlen = amt->dev->needed_tailroom;
2581 len = hlen + tlen + sizeof(*iph) + sizeof(*udph) + sizeof(*amta);
2582 skb = netdev_alloc_skb_ip_align(amt->dev, len);
2583 if (!skb) {
2584 ip_rt_put(rt);
2585 amt->dev->stats.tx_errors++;
2586 goto out;
2587 }
2588
2589 skb->priority = TC_PRIO_CONTROL;
2590 skb_dst_set(skb, &rt->dst);
2591
2592 len = sizeof(*iph) + sizeof(*udph) + sizeof(*amta);
2593 skb_reset_network_header(skb);
2594 skb_put(skb, len);
2595 amta = skb_pull(skb, sizeof(*iph) + sizeof(*udph));
2596 amta->version = 0;
2597 amta->type = AMT_MSG_ADVERTISEMENT;
2598 amta->reserved = 0;
2599 amta->nonce = nonce;
2600 amta->ip4 = amt->local_ip;
2601 skb_push(skb, sizeof(*udph));
2602 skb_reset_transport_header(skb);
2603 udph = udp_hdr(skb);
2604 udph->source = amt->relay_port;
2605 udph->dest = dport;
2606 udph->len = htons(sizeof(*amta) + sizeof(*udph));
2607 udph->check = 0;
2608 offset = skb_transport_offset(skb);
2609 skb->csum = skb_checksum(skb, offset, skb->len - offset, 0);
2610 udph->check = csum_tcpudp_magic(amt->local_ip, daddr,
2611 sizeof(*udph) + sizeof(*amta),
2612 IPPROTO_UDP, skb->csum);
2613
2614 skb_push(skb, sizeof(*iph));
2615 iph = ip_hdr(skb);
2616 iph->version = 4;
2617 iph->ihl = (sizeof(struct iphdr)) >> 2;
2618 iph->tos = AMT_TOS;
2619 iph->frag_off = 0;
2620 iph->ttl = ip4_dst_hoplimit(&rt->dst);
2621 iph->daddr = daddr;
2622 iph->saddr = amt->local_ip;
2623 iph->protocol = IPPROTO_UDP;
2624 iph->tot_len = htons(len);
2625
2626 skb->ip_summed = CHECKSUM_NONE;
2627 ip_select_ident(amt->net, skb, NULL);
2628 ip_send_check(iph);
2629 err = ip_local_out(amt->net, sock->sk, skb);
2630 if (unlikely(net_xmit_eval(err)))
2631 amt->dev->stats.tx_errors++;
2632
2633 out:
2634 rcu_read_unlock();
2635 }
2636
2637 static bool amt_discovery_handler(struct amt_dev *amt, struct sk_buff *skb)
2638 {
2639 struct amt_header_discovery *amtd;
2640 struct udphdr *udph;
2641 struct iphdr *iph;
2642
2643 if (!pskb_may_pull(skb, sizeof(*udph) + sizeof(*amtd)))
2644 return true;
2645
2646 iph = ip_hdr(skb);
2647 udph = udp_hdr(skb);
2648 amtd = (struct amt_header_discovery *)(udp_hdr(skb) + 1);
2649
2650 if (amtd->reserved || amtd->version)
2651 return true;
2652
2653 amt_send_advertisement(amt, amtd->nonce, iph->saddr, udph->source);
2654
2655 return false;
2656 }
2657
2658 static bool amt_request_handler(struct amt_dev *amt, struct sk_buff *skb)
2659 {
2660 struct amt_header_request *amtrh;
2661 struct amt_tunnel_list *tunnel;
2662 unsigned long long key;
2663 struct udphdr *udph;
2664 struct iphdr *iph;
2665 u64 mac;
2666 int i;
2667
2668 if (!pskb_may_pull(skb, sizeof(*udph) + sizeof(*amtrh)))
2669 return true;
2670
2671 iph = ip_hdr(skb);
2672 udph = udp_hdr(skb);
2673 amtrh = (struct amt_header_request *)(udp_hdr(skb) + 1);
2674
2675 if (amtrh->reserved1 || amtrh->reserved2 || amtrh->version)
2676 return true;
2677
2678 list_for_each_entry_rcu(tunnel, &amt->tunnel_list, list)
2679 if (tunnel->ip4 == iph->saddr)
2680 goto send;
2681
2682 spin_lock_bh(&amt->lock);
2683 if (amt->nr_tunnels >= amt->max_tunnels) {
2684 spin_unlock_bh(&amt->lock);
2685 icmp_ndo_send(skb, ICMP_DEST_UNREACH, ICMP_HOST_UNREACH, 0);
2686 return true;
2687 }
2688
2689 tunnel = kzalloc(sizeof(*tunnel) +
2690 (sizeof(struct hlist_head) * amt->hash_buckets),
2691 GFP_ATOMIC);
2692 if (!tunnel) {
2693 spin_unlock_bh(&amt->lock);
2694 return true;
2695 }
2696
2697 tunnel->source_port = udph->source;
2698 tunnel->ip4 = iph->saddr;
2699
2700 memcpy(&key, &tunnel->key, sizeof(unsigned long long));
2701 tunnel->amt = amt;
2702 spin_lock_init(&tunnel->lock);
2703 for (i = 0; i < amt->hash_buckets; i++)
2704 INIT_HLIST_HEAD(&tunnel->groups[i]);
2705
2706 INIT_DELAYED_WORK(&tunnel->gc_wq, amt_tunnel_expire);
2707
2708 list_add_tail_rcu(&tunnel->list, &amt->tunnel_list);
2709 tunnel->key = amt->key;
2710 __amt_update_relay_status(tunnel, AMT_STATUS_RECEIVED_REQUEST, true);
2711 amt->nr_tunnels++;
2712 mod_delayed_work(amt_wq, &tunnel->gc_wq,
2713 msecs_to_jiffies(amt_gmi(amt)));
2714 spin_unlock_bh(&amt->lock);
2715
2716 send:
2717 tunnel->nonce = amtrh->nonce;
2718 mac = siphash_3u32((__force u32)tunnel->ip4,
2719 (__force u32)tunnel->source_port,
2720 (__force u32)tunnel->nonce,
2721 &tunnel->key);
2722 tunnel->mac = mac >> 16;
2723
2724 if (!netif_running(amt->dev) || !netif_running(amt->stream_dev))
2725 return true;
2726
2727 if (!amtrh->p)
2728 amt_send_igmp_gq(amt, tunnel);
2729 else
2730 amt_send_mld_gq(amt, tunnel);
2731
2732 return false;
2733 }
2734
2735 static void amt_gw_rcv(struct amt_dev *amt, struct sk_buff *skb)
2736 {
2737 int type = amt_parse_type(skb);
2738 int err = 1;
2739
2740 if (type == -1)
2741 goto drop;
2742
2743 if (amt->mode == AMT_MODE_GATEWAY) {
2744 switch (type) {
2745 case AMT_MSG_ADVERTISEMENT:
2746 err = amt_advertisement_handler(amt, skb);
2747 break;
2748 case AMT_MSG_MEMBERSHIP_QUERY:
2749 err = amt_membership_query_handler(amt, skb);
2750 if (!err)
2751 return;
2752 break;
2753 default:
2754 netdev_dbg(amt->dev, "Invalid type of Gateway\n");
2755 break;
2756 }
2757 }
2758 drop:
2759 if (err) {
2760 amt->dev->stats.rx_dropped++;
2761 kfree_skb(skb);
2762 } else {
2763 consume_skb(skb);
2764 }
2765 }
2766
2767 static int amt_rcv(struct sock *sk, struct sk_buff *skb)
2768 {
2769 struct amt_dev *amt;
2770 struct iphdr *iph;
2771 int type;
2772 bool err;
2773
2774 rcu_read_lock_bh();
2775 amt = rcu_dereference_sk_user_data(sk);
2776 if (!amt) {
2777 err = true;
2778 kfree_skb(skb);
2779 goto out;
2780 }
2781
2782 skb->dev = amt->dev;
2783 iph = ip_hdr(skb);
2784 type = amt_parse_type(skb);
2785 if (type == -1) {
2786 err = true;
2787 goto drop;
2788 }
2789
2790 if (amt->mode == AMT_MODE_GATEWAY) {
2791 switch (type) {
2792 case AMT_MSG_ADVERTISEMENT:
2793 if (iph->saddr != amt->discovery_ip) {
2794 netdev_dbg(amt->dev, "Invalid Relay IP\n");
2795 err = true;
2796 goto drop;
2797 }
2798 if (amt_queue_event(amt, AMT_EVENT_RECEIVE, skb)) {
2799 netdev_dbg(amt->dev, "AMT Event queue full\n");
2800 err = true;
2801 goto drop;
2802 }
2803 goto out;
2804 case AMT_MSG_MULTICAST_DATA:
2805 if (iph->saddr != amt->remote_ip) {
2806 netdev_dbg(amt->dev, "Invalid Relay IP\n");
2807 err = true;
2808 goto drop;
2809 }
2810 err = amt_multicast_data_handler(amt, skb);
2811 if (err)
2812 goto drop;
2813 else
2814 goto out;
2815 case AMT_MSG_MEMBERSHIP_QUERY:
2816 if (iph->saddr != amt->remote_ip) {
2817 netdev_dbg(amt->dev, "Invalid Relay IP\n");
2818 err = true;
2819 goto drop;
2820 }
2821 if (amt_queue_event(amt, AMT_EVENT_RECEIVE, skb)) {
2822 netdev_dbg(amt->dev, "AMT Event queue full\n");
2823 err = true;
2824 goto drop;
2825 }
2826 goto out;
2827 default:
2828 err = true;
2829 netdev_dbg(amt->dev, "Invalid type of Gateway\n");
2830 break;
2831 }
2832 } else {
2833 switch (type) {
2834 case AMT_MSG_DISCOVERY:
2835 err = amt_discovery_handler(amt, skb);
2836 break;
2837 case AMT_MSG_REQUEST:
2838 err = amt_request_handler(amt, skb);
2839 break;
2840 case AMT_MSG_MEMBERSHIP_UPDATE:
2841 err = amt_update_handler(amt, skb);
2842 if (err)
2843 goto drop;
2844 else
2845 goto out;
2846 default:
2847 err = true;
2848 netdev_dbg(amt->dev, "Invalid type of relay\n");
2849 break;
2850 }
2851 }
2852 drop:
2853 if (err) {
2854 amt->dev->stats.rx_dropped++;
2855 kfree_skb(skb);
2856 } else {
2857 consume_skb(skb);
2858 }
2859 out:
2860 rcu_read_unlock_bh();
2861 return 0;
2862 }
2863
2864 static void amt_event_work(struct work_struct *work)
2865 {
2866 struct amt_dev *amt = container_of(work, struct amt_dev, event_wq);
2867 struct sk_buff *skb;
2868 u8 event;
2869 int i;
2870
2871 for (i = 0; i < AMT_MAX_EVENTS; i++) {
2872 spin_lock_bh(&amt->lock);
2873 if (amt->nr_events == 0) {
2874 spin_unlock_bh(&amt->lock);
2875 return;
2876 }
2877 event = amt->events[amt->event_idx].event;
2878 skb = amt->events[amt->event_idx].skb;
2879 amt->events[amt->event_idx].event = AMT_EVENT_NONE;
2880 amt->events[amt->event_idx].skb = NULL;
2881 amt->nr_events--;
2882 amt->event_idx++;
2883 amt->event_idx %= AMT_MAX_EVENTS;
2884 spin_unlock_bh(&amt->lock);
2885
2886 switch (event) {
2887 case AMT_EVENT_RECEIVE:
2888 amt_gw_rcv(amt, skb);
2889 break;
2890 case AMT_EVENT_SEND_DISCOVERY:
2891 amt_event_send_discovery(amt);
2892 break;
2893 case AMT_EVENT_SEND_REQUEST:
2894 amt_event_send_request(amt);
2895 break;
2896 default:
2897 if (skb)
2898 kfree_skb(skb);
2899 break;
2900 }
2901 }
2902 }
2903
2904 static int amt_err_lookup(struct sock *sk, struct sk_buff *skb)
2905 {
2906 struct amt_dev *amt;
2907 int type;
2908
2909 rcu_read_lock_bh();
2910 amt = rcu_dereference_sk_user_data(sk);
2911 if (!amt)
2912 goto out;
2913
2914 if (amt->mode != AMT_MODE_GATEWAY)
2915 goto drop;
2916
2917 type = amt_parse_type(skb);
2918 if (type == -1)
2919 goto drop;
2920
2921 netdev_dbg(amt->dev, "Received IGMP Unreachable of %s\n",
2922 type_str[type]);
2923 switch (type) {
2924 case AMT_MSG_DISCOVERY:
2925 break;
2926 case AMT_MSG_REQUEST:
2927 case AMT_MSG_MEMBERSHIP_UPDATE:
2928 if (READ_ONCE(amt->status) >= AMT_STATUS_RECEIVED_ADVERTISEMENT)
2929 mod_delayed_work(amt_wq, &amt->req_wq, 0);
2930 break;
2931 default:
2932 goto drop;
2933 }
2934 out:
2935 rcu_read_unlock_bh();
2936 return 0;
2937 drop:
2938 rcu_read_unlock_bh();
2939 amt->dev->stats.rx_dropped++;
2940 return 0;
2941 }
2942
2943 static struct socket *amt_create_sock(struct net *net, __be16 port)
2944 {
2945 struct udp_port_cfg udp_conf;
2946 struct socket *sock;
2947 int err;
2948
2949 memset(&udp_conf, 0, sizeof(udp_conf));
2950 udp_conf.family = AF_INET;
2951 udp_conf.local_ip.s_addr = htonl(INADDR_ANY);
2952
2953 udp_conf.local_udp_port = port;
2954
2955 err = udp_sock_create(net, &udp_conf, &sock);
2956 if (err < 0)
2957 return ERR_PTR(err);
2958
2959 return sock;
2960 }
2961
2962 static int amt_socket_create(struct amt_dev *amt)
2963 {
2964 struct udp_tunnel_sock_cfg tunnel_cfg;
2965 struct socket *sock;
2966
2967 sock = amt_create_sock(amt->net, amt->relay_port);
2968 if (IS_ERR(sock))
2969 return PTR_ERR(sock);
2970
2971
2972 memset(&tunnel_cfg, 0, sizeof(tunnel_cfg));
2973 tunnel_cfg.sk_user_data = amt;
2974 tunnel_cfg.encap_type = 1;
2975 tunnel_cfg.encap_rcv = amt_rcv;
2976 tunnel_cfg.encap_err_lookup = amt_err_lookup;
2977 tunnel_cfg.encap_destroy = NULL;
2978 setup_udp_tunnel_sock(amt->net, sock, &tunnel_cfg);
2979
2980 rcu_assign_pointer(amt->sock, sock);
2981 return 0;
2982 }
2983
2984 static int amt_dev_open(struct net_device *dev)
2985 {
2986 struct amt_dev *amt = netdev_priv(dev);
2987 int err;
2988
2989 amt->ready4 = false;
2990 amt->ready6 = false;
2991 amt->event_idx = 0;
2992 amt->nr_events = 0;
2993
2994 err = amt_socket_create(amt);
2995 if (err)
2996 return err;
2997
2998 amt->req_cnt = 0;
2999 amt->remote_ip = 0;
3000 amt->nonce = 0;
3001 get_random_bytes(&amt->key, sizeof(siphash_key_t));
3002
3003 amt->status = AMT_STATUS_INIT;
3004 if (amt->mode == AMT_MODE_GATEWAY) {
3005 mod_delayed_work(amt_wq, &amt->discovery_wq, 0);
3006 mod_delayed_work(amt_wq, &amt->req_wq, 0);
3007 } else if (amt->mode == AMT_MODE_RELAY) {
3008 mod_delayed_work(amt_wq, &amt->secret_wq,
3009 msecs_to_jiffies(AMT_SECRET_TIMEOUT));
3010 }
3011 return err;
3012 }
3013
3014 static int amt_dev_stop(struct net_device *dev)
3015 {
3016 struct amt_dev *amt = netdev_priv(dev);
3017 struct amt_tunnel_list *tunnel, *tmp;
3018 struct socket *sock;
3019 struct sk_buff *skb;
3020 int i;
3021
3022 cancel_delayed_work_sync(&amt->req_wq);
3023 cancel_delayed_work_sync(&amt->discovery_wq);
3024 cancel_delayed_work_sync(&amt->secret_wq);
3025
3026
3027 sock = rtnl_dereference(amt->sock);
3028 RCU_INIT_POINTER(amt->sock, NULL);
3029 synchronize_net();
3030 if (sock)
3031 udp_tunnel_sock_release(sock);
3032
3033 cancel_work_sync(&amt->event_wq);
3034 for (i = 0; i < AMT_MAX_EVENTS; i++) {
3035 skb = amt->events[i].skb;
3036 if (skb)
3037 kfree_skb(skb);
3038 amt->events[i].event = AMT_EVENT_NONE;
3039 amt->events[i].skb = NULL;
3040 }
3041
3042 amt->ready4 = false;
3043 amt->ready6 = false;
3044 amt->req_cnt = 0;
3045 amt->remote_ip = 0;
3046
3047 list_for_each_entry_safe(tunnel, tmp, &amt->tunnel_list, list) {
3048 list_del_rcu(&tunnel->list);
3049 amt->nr_tunnels--;
3050 cancel_delayed_work_sync(&tunnel->gc_wq);
3051 amt_clear_groups(tunnel);
3052 kfree_rcu(tunnel, rcu);
3053 }
3054
3055 return 0;
3056 }
3057
3058 static const struct device_type amt_type = {
3059 .name = "amt",
3060 };
3061
3062 static int amt_dev_init(struct net_device *dev)
3063 {
3064 struct amt_dev *amt = netdev_priv(dev);
3065 int err;
3066
3067 amt->dev = dev;
3068 dev->tstats = netdev_alloc_pcpu_stats(struct pcpu_sw_netstats);
3069 if (!dev->tstats)
3070 return -ENOMEM;
3071
3072 err = gro_cells_init(&amt->gro_cells, dev);
3073 if (err) {
3074 free_percpu(dev->tstats);
3075 return err;
3076 }
3077
3078 return 0;
3079 }
3080
3081 static void amt_dev_uninit(struct net_device *dev)
3082 {
3083 struct amt_dev *amt = netdev_priv(dev);
3084
3085 gro_cells_destroy(&amt->gro_cells);
3086 free_percpu(dev->tstats);
3087 }
3088
3089 static const struct net_device_ops amt_netdev_ops = {
3090 .ndo_init = amt_dev_init,
3091 .ndo_uninit = amt_dev_uninit,
3092 .ndo_open = amt_dev_open,
3093 .ndo_stop = amt_dev_stop,
3094 .ndo_start_xmit = amt_dev_xmit,
3095 .ndo_get_stats64 = dev_get_tstats64,
3096 };
3097
3098 static void amt_link_setup(struct net_device *dev)
3099 {
3100 dev->netdev_ops = &amt_netdev_ops;
3101 dev->needs_free_netdev = true;
3102 SET_NETDEV_DEVTYPE(dev, &amt_type);
3103 dev->min_mtu = ETH_MIN_MTU;
3104 dev->max_mtu = ETH_MAX_MTU;
3105 dev->type = ARPHRD_NONE;
3106 dev->flags = IFF_POINTOPOINT | IFF_NOARP | IFF_MULTICAST;
3107 dev->hard_header_len = 0;
3108 dev->addr_len = 0;
3109 dev->priv_flags |= IFF_NO_QUEUE;
3110 dev->features |= NETIF_F_LLTX;
3111 dev->features |= NETIF_F_GSO_SOFTWARE;
3112 dev->features |= NETIF_F_NETNS_LOCAL;
3113 dev->hw_features |= NETIF_F_SG | NETIF_F_HW_CSUM;
3114 dev->hw_features |= NETIF_F_FRAGLIST | NETIF_F_RXCSUM;
3115 dev->hw_features |= NETIF_F_GSO_SOFTWARE;
3116 eth_hw_addr_random(dev);
3117 eth_zero_addr(dev->broadcast);
3118 ether_setup(dev);
3119 }
3120
3121 static const struct nla_policy amt_policy[IFLA_AMT_MAX + 1] = {
3122 [IFLA_AMT_MODE] = { .type = NLA_U32 },
3123 [IFLA_AMT_RELAY_PORT] = { .type = NLA_U16 },
3124 [IFLA_AMT_GATEWAY_PORT] = { .type = NLA_U16 },
3125 [IFLA_AMT_LINK] = { .type = NLA_U32 },
3126 [IFLA_AMT_LOCAL_IP] = { .len = sizeof_field(struct iphdr, daddr) },
3127 [IFLA_AMT_REMOTE_IP] = { .len = sizeof_field(struct iphdr, daddr) },
3128 [IFLA_AMT_DISCOVERY_IP] = { .len = sizeof_field(struct iphdr, daddr) },
3129 [IFLA_AMT_MAX_TUNNELS] = { .type = NLA_U32 },
3130 };
3131
3132 static int amt_validate(struct nlattr *tb[], struct nlattr *data[],
3133 struct netlink_ext_ack *extack)
3134 {
3135 if (!data)
3136 return -EINVAL;
3137
3138 if (!data[IFLA_AMT_LINK]) {
3139 NL_SET_ERR_MSG_ATTR(extack, data[IFLA_AMT_LINK],
3140 "Link attribute is required");
3141 return -EINVAL;
3142 }
3143
3144 if (!data[IFLA_AMT_MODE]) {
3145 NL_SET_ERR_MSG_ATTR(extack, data[IFLA_AMT_MODE],
3146 "Mode attribute is required");
3147 return -EINVAL;
3148 }
3149
3150 if (nla_get_u32(data[IFLA_AMT_MODE]) > AMT_MODE_MAX) {
3151 NL_SET_ERR_MSG_ATTR(extack, data[IFLA_AMT_MODE],
3152 "Mode attribute is not valid");
3153 return -EINVAL;
3154 }
3155
3156 if (!data[IFLA_AMT_LOCAL_IP]) {
3157 NL_SET_ERR_MSG_ATTR(extack, data[IFLA_AMT_DISCOVERY_IP],
3158 "Local attribute is required");
3159 return -EINVAL;
3160 }
3161
3162 if (!data[IFLA_AMT_DISCOVERY_IP] &&
3163 nla_get_u32(data[IFLA_AMT_MODE]) == AMT_MODE_GATEWAY) {
3164 NL_SET_ERR_MSG_ATTR(extack, data[IFLA_AMT_LOCAL_IP],
3165 "Discovery attribute is required");
3166 return -EINVAL;
3167 }
3168
3169 return 0;
3170 }
3171
3172 static int amt_newlink(struct net *net, struct net_device *dev,
3173 struct nlattr *tb[], struct nlattr *data[],
3174 struct netlink_ext_ack *extack)
3175 {
3176 struct amt_dev *amt = netdev_priv(dev);
3177 int err = -EINVAL;
3178
3179 amt->net = net;
3180 amt->mode = nla_get_u32(data[IFLA_AMT_MODE]);
3181
3182 if (data[IFLA_AMT_MAX_TUNNELS] &&
3183 nla_get_u32(data[IFLA_AMT_MAX_TUNNELS]))
3184 amt->max_tunnels = nla_get_u32(data[IFLA_AMT_MAX_TUNNELS]);
3185 else
3186 amt->max_tunnels = AMT_MAX_TUNNELS;
3187
3188 spin_lock_init(&amt->lock);
3189 amt->max_groups = AMT_MAX_GROUP;
3190 amt->max_sources = AMT_MAX_SOURCE;
3191 amt->hash_buckets = AMT_HSIZE;
3192 amt->nr_tunnels = 0;
3193 get_random_bytes(&amt->hash_seed, sizeof(amt->hash_seed));
3194 amt->stream_dev = dev_get_by_index(net,
3195 nla_get_u32(data[IFLA_AMT_LINK]));
3196 if (!amt->stream_dev) {
3197 NL_SET_ERR_MSG_ATTR(extack, tb[IFLA_AMT_LINK],
3198 "Can't find stream device");
3199 return -ENODEV;
3200 }
3201
3202 if (amt->stream_dev->type != ARPHRD_ETHER) {
3203 NL_SET_ERR_MSG_ATTR(extack, tb[IFLA_AMT_LINK],
3204 "Invalid stream device type");
3205 goto err;
3206 }
3207
3208 amt->local_ip = nla_get_in_addr(data[IFLA_AMT_LOCAL_IP]);
3209 if (ipv4_is_loopback(amt->local_ip) ||
3210 ipv4_is_zeronet(amt->local_ip) ||
3211 ipv4_is_multicast(amt->local_ip)) {
3212 NL_SET_ERR_MSG_ATTR(extack, tb[IFLA_AMT_LOCAL_IP],
3213 "Invalid Local address");
3214 goto err;
3215 }
3216
3217 if (data[IFLA_AMT_RELAY_PORT])
3218 amt->relay_port = nla_get_be16(data[IFLA_AMT_RELAY_PORT]);
3219 else
3220 amt->relay_port = htons(IANA_AMT_UDP_PORT);
3221
3222 if (data[IFLA_AMT_GATEWAY_PORT])
3223 amt->gw_port = nla_get_be16(data[IFLA_AMT_GATEWAY_PORT]);
3224 else
3225 amt->gw_port = htons(IANA_AMT_UDP_PORT);
3226
3227 if (!amt->relay_port) {
3228 NL_SET_ERR_MSG_ATTR(extack, tb[IFLA_AMT_DISCOVERY_IP],
3229 "relay port must not be 0");
3230 goto err;
3231 }
3232 if (amt->mode == AMT_MODE_RELAY) {
3233 amt->qrv = READ_ONCE(amt->net->ipv4.sysctl_igmp_qrv);
3234 amt->qri = 10;
3235 dev->needed_headroom = amt->stream_dev->needed_headroom +
3236 AMT_RELAY_HLEN;
3237 dev->mtu = amt->stream_dev->mtu - AMT_RELAY_HLEN;
3238 dev->max_mtu = dev->mtu;
3239 dev->min_mtu = ETH_MIN_MTU + AMT_RELAY_HLEN;
3240 } else {
3241 if (!data[IFLA_AMT_DISCOVERY_IP]) {
3242 NL_SET_ERR_MSG_ATTR(extack, tb[IFLA_AMT_DISCOVERY_IP],
3243 "discovery must be set in gateway mode");
3244 goto err;
3245 }
3246 if (!amt->gw_port) {
3247 NL_SET_ERR_MSG_ATTR(extack, tb[IFLA_AMT_DISCOVERY_IP],
3248 "gateway port must not be 0");
3249 goto err;
3250 }
3251 amt->remote_ip = 0;
3252 amt->discovery_ip = nla_get_in_addr(data[IFLA_AMT_DISCOVERY_IP]);
3253 if (ipv4_is_loopback(amt->discovery_ip) ||
3254 ipv4_is_zeronet(amt->discovery_ip) ||
3255 ipv4_is_multicast(amt->discovery_ip)) {
3256 NL_SET_ERR_MSG_ATTR(extack, tb[IFLA_AMT_DISCOVERY_IP],
3257 "discovery must be unicast");
3258 goto err;
3259 }
3260
3261 dev->needed_headroom = amt->stream_dev->needed_headroom +
3262 AMT_GW_HLEN;
3263 dev->mtu = amt->stream_dev->mtu - AMT_GW_HLEN;
3264 dev->max_mtu = dev->mtu;
3265 dev->min_mtu = ETH_MIN_MTU + AMT_GW_HLEN;
3266 }
3267 amt->qi = AMT_INIT_QUERY_INTERVAL;
3268
3269 err = register_netdevice(dev);
3270 if (err < 0) {
3271 netdev_dbg(dev, "failed to register new netdev %d\n", err);
3272 goto err;
3273 }
3274
3275 err = netdev_upper_dev_link(amt->stream_dev, dev, extack);
3276 if (err < 0) {
3277 unregister_netdevice(dev);
3278 goto err;
3279 }
3280
3281 INIT_DELAYED_WORK(&amt->discovery_wq, amt_discovery_work);
3282 INIT_DELAYED_WORK(&amt->req_wq, amt_req_work);
3283 INIT_DELAYED_WORK(&amt->secret_wq, amt_secret_work);
3284 INIT_WORK(&amt->event_wq, amt_event_work);
3285 INIT_LIST_HEAD(&amt->tunnel_list);
3286 return 0;
3287 err:
3288 dev_put(amt->stream_dev);
3289 return err;
3290 }
3291
3292 static void amt_dellink(struct net_device *dev, struct list_head *head)
3293 {
3294 struct amt_dev *amt = netdev_priv(dev);
3295
3296 unregister_netdevice_queue(dev, head);
3297 netdev_upper_dev_unlink(amt->stream_dev, dev);
3298 dev_put(amt->stream_dev);
3299 }
3300
3301 static size_t amt_get_size(const struct net_device *dev)
3302 {
3303 return nla_total_size(sizeof(__u32)) +
3304 nla_total_size(sizeof(__u16)) +
3305 nla_total_size(sizeof(__u16)) +
3306 nla_total_size(sizeof(__u32)) +
3307 nla_total_size(sizeof(__u32)) +
3308 nla_total_size(sizeof(struct iphdr)) +
3309 nla_total_size(sizeof(struct iphdr)) +
3310 nla_total_size(sizeof(struct iphdr));
3311 }
3312
3313 static int amt_fill_info(struct sk_buff *skb, const struct net_device *dev)
3314 {
3315 struct amt_dev *amt = netdev_priv(dev);
3316
3317 if (nla_put_u32(skb, IFLA_AMT_MODE, amt->mode))
3318 goto nla_put_failure;
3319 if (nla_put_be16(skb, IFLA_AMT_RELAY_PORT, amt->relay_port))
3320 goto nla_put_failure;
3321 if (nla_put_be16(skb, IFLA_AMT_GATEWAY_PORT, amt->gw_port))
3322 goto nla_put_failure;
3323 if (nla_put_u32(skb, IFLA_AMT_LINK, amt->stream_dev->ifindex))
3324 goto nla_put_failure;
3325 if (nla_put_in_addr(skb, IFLA_AMT_LOCAL_IP, amt->local_ip))
3326 goto nla_put_failure;
3327 if (nla_put_in_addr(skb, IFLA_AMT_DISCOVERY_IP, amt->discovery_ip))
3328 goto nla_put_failure;
3329 if (amt->remote_ip)
3330 if (nla_put_in_addr(skb, IFLA_AMT_REMOTE_IP, amt->remote_ip))
3331 goto nla_put_failure;
3332 if (nla_put_u32(skb, IFLA_AMT_MAX_TUNNELS, amt->max_tunnels))
3333 goto nla_put_failure;
3334
3335 return 0;
3336
3337 nla_put_failure:
3338 return -EMSGSIZE;
3339 }
3340
3341 static struct rtnl_link_ops amt_link_ops __read_mostly = {
3342 .kind = "amt",
3343 .maxtype = IFLA_AMT_MAX,
3344 .policy = amt_policy,
3345 .priv_size = sizeof(struct amt_dev),
3346 .setup = amt_link_setup,
3347 .validate = amt_validate,
3348 .newlink = amt_newlink,
3349 .dellink = amt_dellink,
3350 .get_size = amt_get_size,
3351 .fill_info = amt_fill_info,
3352 };
3353
3354 static struct net_device *amt_lookup_upper_dev(struct net_device *dev)
3355 {
3356 struct net_device *upper_dev;
3357 struct amt_dev *amt;
3358
3359 for_each_netdev(dev_net(dev), upper_dev) {
3360 if (netif_is_amt(upper_dev)) {
3361 amt = netdev_priv(upper_dev);
3362 if (amt->stream_dev == dev)
3363 return upper_dev;
3364 }
3365 }
3366
3367 return NULL;
3368 }
3369
3370 static int amt_device_event(struct notifier_block *unused,
3371 unsigned long event, void *ptr)
3372 {
3373 struct net_device *dev = netdev_notifier_info_to_dev(ptr);
3374 struct net_device *upper_dev;
3375 struct amt_dev *amt;
3376 LIST_HEAD(list);
3377 int new_mtu;
3378
3379 upper_dev = amt_lookup_upper_dev(dev);
3380 if (!upper_dev)
3381 return NOTIFY_DONE;
3382 amt = netdev_priv(upper_dev);
3383
3384 switch (event) {
3385 case NETDEV_UNREGISTER:
3386 amt_dellink(amt->dev, &list);
3387 unregister_netdevice_many(&list);
3388 break;
3389 case NETDEV_CHANGEMTU:
3390 if (amt->mode == AMT_MODE_RELAY)
3391 new_mtu = dev->mtu - AMT_RELAY_HLEN;
3392 else
3393 new_mtu = dev->mtu - AMT_GW_HLEN;
3394
3395 dev_set_mtu(amt->dev, new_mtu);
3396 break;
3397 }
3398
3399 return NOTIFY_DONE;
3400 }
3401
3402 static struct notifier_block amt_notifier_block __read_mostly = {
3403 .notifier_call = amt_device_event,
3404 };
3405
3406 static int __init amt_init(void)
3407 {
3408 int err;
3409
3410 err = register_netdevice_notifier(&amt_notifier_block);
3411 if (err < 0)
3412 goto err;
3413
3414 err = rtnl_link_register(&amt_link_ops);
3415 if (err < 0)
3416 goto unregister_notifier;
3417
3418 amt_wq = alloc_workqueue("amt", WQ_UNBOUND, 0);
3419 if (!amt_wq) {
3420 err = -ENOMEM;
3421 goto rtnl_unregister;
3422 }
3423
3424 spin_lock_init(&source_gc_lock);
3425 spin_lock_bh(&source_gc_lock);
3426 INIT_DELAYED_WORK(&source_gc_wq, amt_source_gc_work);
3427 mod_delayed_work(amt_wq, &source_gc_wq,
3428 msecs_to_jiffies(AMT_GC_INTERVAL));
3429 spin_unlock_bh(&source_gc_lock);
3430
3431 return 0;
3432
3433 rtnl_unregister:
3434 rtnl_link_unregister(&amt_link_ops);
3435 unregister_notifier:
3436 unregister_netdevice_notifier(&amt_notifier_block);
3437 err:
3438 pr_err("error loading AMT module loaded\n");
3439 return err;
3440 }
3441 late_initcall(amt_init);
3442
3443 static void __exit amt_fini(void)
3444 {
3445 rtnl_link_unregister(&amt_link_ops);
3446 unregister_netdevice_notifier(&amt_notifier_block);
3447 cancel_delayed_work_sync(&source_gc_wq);
3448 __amt_source_gc_work();
3449 destroy_workqueue(amt_wq);
3450 }
3451 module_exit(amt_fini);
3452
3453 MODULE_LICENSE("GPL");
3454 MODULE_AUTHOR("Taehee Yoo <ap420073@gmail.com>");
3455 MODULE_ALIAS_RTNL_LINK("amt");
