net/mpls/af_mpls.c

   1 #include <linux/types.h>
   2 #include <linux/skbuff.h>
   3 #include <linux/socket.h>
   4 #include <linux/sysctl.h>
   5 #include <linux/net.h>
   6 #include <linux/module.h>
   7 #include <linux/if_arp.h>
   8 #include <linux/ipv6.h>
   9 #include <linux/mpls.h>
  10 #include <linux/vmalloc.h>
  11 #include <net/ip.h>
  12 #include <net/dst.h>
  13 #include <net/sock.h>
  14 #include <net/arp.h>
  15 #include <net/ip_fib.h>
  16 #include <net/netevent.h>
  17 #include <net/netns/generic.h>
  18 #if IS_ENABLED(CONFIG_IPV6)
  19 #include <net/ipv6.h>
  20 #include <net/addrconf.h>
  21 #endif
  22 #include <net/nexthop.h>
  23 #include "internal.h"
  24
  25 /* Maximum number of labels to look ahead at when selecting a path of
  26  * a multipath route
  27  */
  28 #define MAX_MP_SELECT_LABELS 4
  29
  30 static int zero = 0;
  31 static int label_limit = (1 << 20) - 1;
  32
  33 static void rtmsg_lfib(int event, u32 label, struct mpls_route *rt,
  34                        struct nlmsghdr *nlh, struct net *net, u32 portid,
  35                        unsigned int nlm_flags);
  36
  37 static struct mpls_route *mpls_route_input_rcu(struct net *net, unsigned index)
  38 {
  39         struct mpls_route *rt = NULL;
  40
  41         if (index < net->mpls.platform_labels) {
  42                 struct mpls_route __rcu **platform_label =
  43                         rcu_dereference(net->mpls.platform_label);
  44                 rt = rcu_dereference(platform_label[index]);
  45         }
  46         return rt;
  47 }
  48
  49 static inline struct mpls_dev *mpls_dev_get(const struct net_device *dev)
  50 {
  51         return rcu_dereference_rtnl(dev->mpls_ptr);
  52 }
  53
  54 bool mpls_output_possible(const struct net_device *dev)
  55 {
  56         return dev && (dev->flags & IFF_UP) && netif_carrier_ok(dev);
  57 }
  58 EXPORT_SYMBOL_GPL(mpls_output_possible);
  59
  60 static u8 *__mpls_nh_via(struct mpls_route *rt, struct mpls_nh *nh)
  61 {
  62         u8 *nh0_via = PTR_ALIGN((u8 *)&rt->rt_nh[rt->rt_nhn], VIA_ALEN_ALIGN);
  63         int nh_index = nh - rt->rt_nh;
  64
  65         return nh0_via + rt->rt_max_alen * nh_index;
  66 }
  67
  68 static const u8 *mpls_nh_via(const struct mpls_route *rt,
  69                              const struct mpls_nh *nh)
  70 {
  71         return __mpls_nh_via((struct mpls_route *)rt, (struct mpls_nh *)nh);
  72 }
  73
  74 static unsigned int mpls_nh_header_size(const struct mpls_nh *nh)
  75 {
  76         /* The size of the layer 2.5 labels to be added for this route */
  77         return nh->nh_labels * sizeof(struct mpls_shim_hdr);
  78 }
  79
  80 unsigned int mpls_dev_mtu(const struct net_device *dev)
  81 {
  82         /* The amount of data the layer 2 frame can hold */
  83         return dev->mtu;
  84 }
  85 EXPORT_SYMBOL_GPL(mpls_dev_mtu);
  86
  87 bool mpls_pkt_too_big(const struct sk_buff *skb, unsigned int mtu)
  88 {
  89         if (skb->len <= mtu)
  90                 return false;
  91
  92         if (skb_is_gso(skb) && skb_gso_network_seglen(skb) <= mtu)
  93                 return false;
  94
  95         return true;
  96 }
  97 EXPORT_SYMBOL_GPL(mpls_pkt_too_big);
  98
  99 static u32 mpls_multipath_hash(struct mpls_route *rt,
 100                                struct sk_buff *skb, bool bos)
 101 {
 102         struct mpls_entry_decoded dec;
 103         struct mpls_shim_hdr *hdr;
 104         bool eli_seen = false;
 105         int label_index;
 106         u32 hash = 0;
 107
 108         for (label_index = 0; label_index < MAX_MP_SELECT_LABELS && !bos;
 109              label_index++) {
 110                 if (!pskb_may_pull(skb, sizeof(*hdr) * label_index))
 111                         break;
 112
 113                 /* Read and decode the current label */
 114                 hdr = mpls_hdr(skb) + label_index;
 115                 dec = mpls_entry_decode(hdr);
 116
 117                 /* RFC6790 - reserved labels MUST NOT be used as keys
 118                  * for the load-balancing function
 119                  */
 120                 if (likely(dec.label >= MPLS_LABEL_FIRST_UNRESERVED)) {
 121                         hash = jhash_1word(dec.label, hash);
 122
 123                         /* The entropy label follows the entropy label
 124                          * indicator, so this means that the entropy
 125                          * label was just added to the hash - no need to
 126                          * go any deeper either in the label stack or in the
 127                          * payload
 128                          */
 129                         if (eli_seen)
 130                                 break;
 131                 } else if (dec.label == MPLS_LABEL_ENTROPY) {
 132                         eli_seen = true;
 133                 }
 134
 135                 bos = dec.bos;
 136                 if (bos && pskb_may_pull(skb, sizeof(*hdr) * label_index +
 137                                          sizeof(struct iphdr))) {
 138                         const struct iphdr *v4hdr;
 139
 140                         v4hdr = (const struct iphdr *)(mpls_hdr(skb) +
 141                                                        label_index);
 142                         if (v4hdr->version == 4) {
 143                                 hash = jhash_3words(ntohl(v4hdr->saddr),
 144                                                     ntohl(v4hdr->daddr),
 145                                                     v4hdr->protocol, hash);
 146                         } else if (v4hdr->version == 6 &&
 147                                 pskb_may_pull(skb, sizeof(*hdr) * label_index +
 148                                               sizeof(struct ipv6hdr))) {
 149                                 const struct ipv6hdr *v6hdr;
 150
 151                                 v6hdr = (const struct ipv6hdr *)(mpls_hdr(skb) +
 152                                                                 label_index);
 153
 154                                 hash = __ipv6_addr_jhash(&v6hdr->saddr, hash);
 155                                 hash = __ipv6_addr_jhash(&v6hdr->daddr, hash);
 156                                 hash = jhash_1word(v6hdr->nexthdr, hash);
 157                         }
 158                 }
 159         }
 160
 161         return hash;
 162 }
 163
 164 static struct mpls_nh *mpls_select_multipath(struct mpls_route *rt,
 165                                              struct sk_buff *skb, bool bos)
 166 {
 167         int alive = ACCESS_ONCE(rt->rt_nhn_alive);
 168         u32 hash = 0;
 169         int nh_index = 0;
 170         int n = 0;
 171
 172         /* No need to look further into packet if there's only
 173          * one path
 174          */
 175         if (rt->rt_nhn == 1)
 176                 goto out;
 177
 178         if (alive <= 0)
 179                 return NULL;
 180
 181         hash = mpls_multipath_hash(rt, skb, bos);
 182         nh_index = hash % alive;
 183         if (alive == rt->rt_nhn)
 184                 goto out;
 185         for_nexthops(rt) {
 186                 if (nh->nh_flags & (RTNH_F_DEAD | RTNH_F_LINKDOWN))
 187                         continue;
 188                 if (n == nh_index)
 189                         return nh;
 190                 n++;
 191         } endfor_nexthops(rt);
 192
 193 out:
 194         return &rt->rt_nh[nh_index];
 195 }
 196
 197 static bool mpls_egress(struct mpls_route *rt, struct sk_buff *skb,
 198                         struct mpls_entry_decoded dec)
 199 {
 200         enum mpls_payload_type payload_type;
 201         bool success = false;
 202
 203         /* The IPv4 code below accesses through the IPv4 header
 204          * checksum, which is 12 bytes into the packet.
 205          * The IPv6 code below accesses through the IPv6 hop limit
 206          * which is 8 bytes into the packet.
 207          *
 208          * For all supported cases there should always be at least 12
 209          * bytes of packet data present.  The IPv4 header is 20 bytes
 210          * without options and the IPv6 header is always 40 bytes
 211          * long.
 212          */
 213         if (!pskb_may_pull(skb, 12))
 214                 return false;
 215
 216         payload_type = rt->rt_payload_type;
 217         if (payload_type == MPT_UNSPEC)
 218                 payload_type = ip_hdr(skb)->version;
 219
 220         switch (payload_type) {
 221         case MPT_IPV4: {
 222                 struct iphdr *hdr4 = ip_hdr(skb);
 223                 skb->protocol = htons(ETH_P_IP);
 224                 csum_replace2(&hdr4->check,
 225                               htons(hdr4->ttl << 8),
 226                               htons(dec.ttl << 8));
 227                 hdr4->ttl = dec.ttl;
 228                 success = true;
 229                 break;
 230         }
 231         case MPT_IPV6: {
 232                 struct ipv6hdr *hdr6 = ipv6_hdr(skb);
 233                 skb->protocol = htons(ETH_P_IPV6);
 234                 hdr6->hop_limit = dec.ttl;
 235                 success = true;
 236                 break;
 237         }
 238         case MPT_UNSPEC:
 239                 break;
 240         }
 241
 242         return success;
 243 }
 244
 245 static int mpls_forward(struct sk_buff *skb, struct net_device *dev,
 246                         struct packet_type *pt, struct net_device *orig_dev)
 247 {
 248         struct net *net = dev_net(dev);
 249         struct mpls_shim_hdr *hdr;
 250         struct mpls_route *rt;
 251         struct mpls_nh *nh;
 252         struct mpls_entry_decoded dec;
 253         struct net_device *out_dev;
 254         struct mpls_dev *mdev;
 255         unsigned int hh_len;
 256         unsigned int new_header_size;
 257         unsigned int mtu;
 258         int err;
 259
 260         /* Careful this entire function runs inside of an rcu critical section */
 261
 262         mdev = mpls_dev_get(dev);
 263         if (!mdev || !mdev->input_enabled)
 264                 goto drop;
 265
 266         if (skb->pkt_type != PACKET_HOST)
 267                 goto drop;
 268
 269         if ((skb = skb_share_check(skb, GFP_ATOMIC)) == NULL)
 270                 goto drop;
 271
 272         if (!pskb_may_pull(skb, sizeof(*hdr)))
 273                 goto drop;
 274
 275         /* Read and decode the label */
 276         hdr = mpls_hdr(skb);
 277         dec = mpls_entry_decode(hdr);
 278
 279         /* Pop the label */
 280         skb_pull(skb, sizeof(*hdr));
 281         skb_reset_network_header(skb);
 282
 283         skb_orphan(skb);
 284
 285         rt = mpls_route_input_rcu(net, dec.label);
 286         if (!rt)
 287                 goto drop;
 288
 289         nh = mpls_select_multipath(rt, skb, dec.bos);
 290         if (!nh)
 291                 goto drop;
 292
 293         /* Find the output device */
 294         out_dev = rcu_dereference(nh->nh_dev);
 295         if (!mpls_output_possible(out_dev))
 296                 goto drop;
 297
 298         if (skb_warn_if_lro(skb))
 299                 goto drop;
 300
 301         skb_forward_csum(skb);
 302
 303         /* Verify ttl is valid */
 304         if (dec.ttl <= 1)
 305                 goto drop;
 306         dec.ttl -= 1;
 307
 308         /* Verify the destination can hold the packet */
 309         new_header_size = mpls_nh_header_size(nh);
 310         mtu = mpls_dev_mtu(out_dev);
 311         if (mpls_pkt_too_big(skb, mtu - new_header_size))
 312                 goto drop;
 313
 314         hh_len = LL_RESERVED_SPACE(out_dev);
 315         if (!out_dev->header_ops)
 316                 hh_len = 0;
 317
 318         /* Ensure there is enough space for the headers in the skb */
 319         if (skb_cow(skb, hh_len + new_header_size))
 320                 goto drop;
 321
 322         skb->dev = out_dev;
 323         skb->protocol = htons(ETH_P_MPLS_UC);
 324
 325         if (unlikely(!new_header_size && dec.bos)) {
 326                 /* Penultimate hop popping */
 327                 if (!mpls_egress(rt, skb, dec))
 328                         goto drop;
 329         } else {
 330                 bool bos;
 331                 int i;
 332                 skb_push(skb, new_header_size);
 333                 skb_reset_network_header(skb);
 334                 /* Push the new labels */
 335                 hdr = mpls_hdr(skb);
 336                 bos = dec.bos;
 337                 for (i = nh->nh_labels - 1; i >= 0; i--) {
 338                         hdr[i] = mpls_entry_encode(nh->nh_label[i],
 339                                                    dec.ttl, 0, bos);
 340                         bos = false;
 341                 }
 342         }
 343
 344         err = neigh_xmit(nh->nh_via_table, out_dev, mpls_nh_via(rt, nh), skb);
 345         if (err)
 346                 net_dbg_ratelimited("%s: packet transmission failed: %d\n",
 347                                     __func__, err);
 348         return 0;
 349
 350 drop:
 351         kfree_skb(skb);
 352         return NET_RX_DROP;
 353 }
 354
 355 static struct packet_type mpls_packet_type __read_mostly = {
 356         .type = cpu_to_be16(ETH_P_MPLS_UC),
 357         .func = mpls_forward,
 358 };
 359
 360 static const struct nla_policy rtm_mpls_policy[RTA_MAX+1] = {
 361         [RTA_DST]               = { .type = NLA_U32 },
 362         [RTA_OIF]               = { .type = NLA_U32 },
 363 };
 364
 365 struct mpls_route_config {
 366         u32                     rc_protocol;
 367         u32                     rc_ifindex;
 368         u8                      rc_via_table;
 369         u8                      rc_via_alen;
 370         u8                      rc_via[MAX_VIA_ALEN];
 371         u32                     rc_label;
 372         u8                      rc_output_labels;
 373         u32                     rc_output_label[MAX_NEW_LABELS];
 374         u32                     rc_nlflags;
 375         enum mpls_payload_type  rc_payload_type;
 376         struct nl_info          rc_nlinfo;
 377         struct rtnexthop        *rc_mp;
 378         int                     rc_mp_len;
 379 };
 380
 381 static struct mpls_route *mpls_rt_alloc(int num_nh, u8 max_alen)
 382 {
 383         u8 max_alen_aligned = ALIGN(max_alen, VIA_ALEN_ALIGN);
 384         struct mpls_route *rt;
 385
 386         rt = kzalloc(ALIGN(sizeof(*rt) + num_nh * sizeof(*rt->rt_nh),
 387                            VIA_ALEN_ALIGN) +
 388                      num_nh * max_alen_aligned,
 389                      GFP_KERNEL);
 390         if (rt) {
 391                 rt->rt_nhn = num_nh;
 392                 rt->rt_nhn_alive = num_nh;
 393                 rt->rt_max_alen = max_alen_aligned;
 394         }
 395
 396         return rt;
 397 }
 398
 399 static void mpls_rt_free(struct mpls_route *rt)
 400 {
 401         if (rt)
 402                 kfree_rcu(rt, rt_rcu);
 403 }
 404
 405 static void mpls_notify_route(struct net *net, unsigned index,
 406                               struct mpls_route *old, struct mpls_route *new,
 407                               const struct nl_info *info)
 408 {
 409         struct nlmsghdr *nlh = info ? info->nlh : NULL;
 410         unsigned portid = info ? info->portid : 0;
 411         int event = new ? RTM_NEWROUTE : RTM_DELROUTE;
 412         struct mpls_route *rt = new ? new : old;
 413         unsigned nlm_flags = (old && new) ? NLM_F_REPLACE : 0;
 414         /* Ignore reserved labels for now */
 415         if (rt && (index >= MPLS_LABEL_FIRST_UNRESERVED))
 416                 rtmsg_lfib(event, index, rt, nlh, net, portid, nlm_flags);
 417 }
 418
 419 static void mpls_route_update(struct net *net, unsigned index,
 420                               struct mpls_route *new,
 421                               const struct nl_info *info)
 422 {
 423         struct mpls_route __rcu **platform_label;
 424         struct mpls_route *rt;
 425
 426         ASSERT_RTNL();
 427
 428         platform_label = rtnl_dereference(net->mpls.platform_label);
 429         rt = rtnl_dereference(platform_label[index]);
 430         rcu_assign_pointer(platform_label[index], new);
 431
 432         mpls_notify_route(net, index, rt, new, info);
 433
 434         /* If we removed a route free it now */
 435         mpls_rt_free(rt);
 436 }
 437
 438 static unsigned find_free_label(struct net *net)
 439 {
 440         struct mpls_route __rcu **platform_label;
 441         size_t platform_labels;
 442         unsigned index;
 443
 444         platform_label = rtnl_dereference(net->mpls.platform_label);
 445         platform_labels = net->mpls.platform_labels;
 446         for (index = MPLS_LABEL_FIRST_UNRESERVED; index < platform_labels;
 447              index++) {
 448                 if (!rtnl_dereference(platform_label[index]))
 449                         return index;
 450         }
 451         return LABEL_NOT_SPECIFIED;
 452 }
 453
 454 #if IS_ENABLED(CONFIG_INET)
 455 static struct net_device *inet_fib_lookup_dev(struct net *net,
 456                                               const void *addr)
 457 {
 458         struct net_device *dev;
 459         struct rtable *rt;
 460         struct in_addr daddr;
 461
 462         memcpy(&daddr, addr, sizeof(struct in_addr));
 463         rt = ip_route_output(net, daddr.s_addr, 0, 0, 0);
 464         if (IS_ERR(rt))
 465                 return ERR_CAST(rt);
 466
 467         dev = rt->dst.dev;
 468         dev_hold(dev);
 469
 470         ip_rt_put(rt);
 471
 472         return dev;
 473 }
 474 #else
 475 static struct net_device *inet_fib_lookup_dev(struct net *net,
 476                                               const void *addr)
 477 {
 478         return ERR_PTR(-EAFNOSUPPORT);
 479 }
 480 #endif
 481
 482 #if IS_ENABLED(CONFIG_IPV6)
 483 static struct net_device *inet6_fib_lookup_dev(struct net *net,
 484                                                const void *addr)
 485 {
 486         struct net_device *dev;
 487         struct dst_entry *dst;
 488         struct flowi6 fl6;
 489         int err;
 490
 491         if (!ipv6_stub)
 492                 return ERR_PTR(-EAFNOSUPPORT);
 493
 494         memset(&fl6, 0, sizeof(fl6));
 495         memcpy(&fl6.daddr, addr, sizeof(struct in6_addr));
 496         err = ipv6_stub->ipv6_dst_lookup(net, NULL, &dst, &fl6);
 497         if (err)
 498                 return ERR_PTR(err);
 499
 500         dev = dst->dev;
 501         dev_hold(dev);
 502         dst_release(dst);
 503
 504         return dev;
 505 }
 506 #else
 507 static struct net_device *inet6_fib_lookup_dev(struct net *net,
 508                                                const void *addr)
 509 {
 510         return ERR_PTR(-EAFNOSUPPORT);
 511 }
 512 #endif
 513
 514 static struct net_device *find_outdev(struct net *net,
 515                                       struct mpls_route *rt,
 516                                       struct mpls_nh *nh, int oif)
 517 {
 518         struct net_device *dev = NULL;
 519
 520         if (!oif) {
 521                 switch (nh->nh_via_table) {
 522                 case NEIGH_ARP_TABLE:
 523                         dev = inet_fib_lookup_dev(net, mpls_nh_via(rt, nh));
 524                         break;
 525                 case NEIGH_ND_TABLE:
 526                         dev = inet6_fib_lookup_dev(net, mpls_nh_via(rt, nh));
 527                         break;
 528                 case NEIGH_LINK_TABLE:
 529                         break;
 530                 }
 531         } else {
 532                 dev = dev_get_by_index(net, oif);
 533         }
 534
 535         if (!dev)
 536                 return ERR_PTR(-ENODEV);
 537
 538         /* The caller is holding rtnl anyways, so release the dev reference */
 539         dev_put(dev);
 540
 541         return dev;
 542 }
 543
 544 static int mpls_nh_assign_dev(struct net *net, struct mpls_route *rt,
 545                               struct mpls_nh *nh, int oif)
 546 {
 547         struct net_device *dev = NULL;
 548         int err = -ENODEV;
 549
 550         dev = find_outdev(net, rt, nh, oif);
 551         if (IS_ERR(dev)) {
 552                 err = PTR_ERR(dev);
 553                 dev = NULL;
 554                 goto errout;
 555         }
 556
 557         /* Ensure this is a supported device */
 558         err = -EINVAL;
 559         if (!mpls_dev_get(dev))
 560                 goto errout;
 561
 562         RCU_INIT_POINTER(nh->nh_dev, dev);
 563
 564         if (!(dev->flags & IFF_UP)) {
 565                 nh->nh_flags |= RTNH_F_DEAD;
 566         } else {
 567                 unsigned int flags;
 568
 569                 flags = dev_get_flags(dev);
 570                 if (!(flags & (IFF_RUNNING | IFF_LOWER_UP)))
 571                         nh->nh_flags |= RTNH_F_LINKDOWN;
 572         }
 573
 574         return 0;
 575
 576 errout:
 577         return err;
 578 }
 579
 580 static int mpls_nh_build_from_cfg(struct mpls_route_config *cfg,
 581                                   struct mpls_route *rt)
 582 {
 583         struct net *net = cfg->rc_nlinfo.nl_net;
 584         struct mpls_nh *nh = rt->rt_nh;
 585         int err;
 586         int i;
 587
 588         if (!nh)
 589                 return -ENOMEM;
 590
 591         err = -EINVAL;
 592         /* Ensure only a supported number of labels are present */
 593         if (cfg->rc_output_labels > MAX_NEW_LABELS)
 594                 goto errout;
 595
 596         nh->nh_labels = cfg->rc_output_labels;
 597         for (i = 0; i < nh->nh_labels; i++)
 598                 nh->nh_label[i] = cfg->rc_output_label[i];
 599
 600         nh->nh_via_table = cfg->rc_via_table;
 601         memcpy(__mpls_nh_via(rt, nh), cfg->rc_via, cfg->rc_via_alen);
 602         nh->nh_via_alen = cfg->rc_via_alen;
 603
 604         err = mpls_nh_assign_dev(net, rt, nh, cfg->rc_ifindex);
 605         if (err)
 606                 goto errout;
 607
 608         if (nh->nh_flags & (RTNH_F_DEAD | RTNH_F_LINKDOWN))
 609                 rt->rt_nhn_alive--;
 610
 611         return 0;
 612
 613 errout:
 614         return err;
 615 }
 616
 617 static int mpls_nh_build(struct net *net, struct mpls_route *rt,
 618                          struct mpls_nh *nh, int oif, struct nlattr *via,
 619                          struct nlattr *newdst)
 620 {
 621         int err = -ENOMEM;
 622
 623         if (!nh)
 624                 goto errout;
 625
 626         if (newdst) {
 627                 err = nla_get_labels(newdst, MAX_NEW_LABELS,
 628                                      &nh->nh_labels, nh->nh_label);
 629                 if (err)
 630                         goto errout;
 631         }
 632
 633         err = nla_get_via(via, &nh->nh_via_alen, &nh->nh_via_table,
 634                           __mpls_nh_via(rt, nh));
 635         if (err)
 636                 goto errout;
 637
 638         err = mpls_nh_assign_dev(net, rt, nh, oif);
 639         if (err)
 640                 goto errout;
 641
 642         return 0;
 643
 644 errout:
 645         return err;
 646 }
 647
 648 static int mpls_count_nexthops(struct rtnexthop *rtnh, int len,
 649                                u8 cfg_via_alen, u8 *max_via_alen)
 650 {
 651         int nhs = 0;
 652         int remaining = len;
 653
 654         if (!rtnh) {
 655                 *max_via_alen = cfg_via_alen;
 656                 return 1;
 657         }
 658
 659         *max_via_alen = 0;
 660
 661         while (rtnh_ok(rtnh, remaining)) {
 662                 struct nlattr *nla, *attrs = rtnh_attrs(rtnh);
 663                 int attrlen;
 664
 665                 attrlen = rtnh_attrlen(rtnh);
 666                 nla = nla_find(attrs, attrlen, RTA_VIA);
 667                 if (nla && nla_len(nla) >=
 668                     offsetof(struct rtvia, rtvia_addr)) {
 669                         int via_alen = nla_len(nla) -
 670                                 offsetof(struct rtvia, rtvia_addr);
 671
 672                         if (via_alen <= MAX_VIA_ALEN)
 673                                 *max_via_alen = max_t(u16, *max_via_alen,
 674                                                       via_alen);
 675                 }
 676
 677                 nhs++;
 678                 rtnh = rtnh_next(rtnh, &remaining);
 679         }
 680
 681         /* leftover implies invalid nexthop configuration, discard it */
 682         return remaining > 0 ? 0 : nhs;
 683 }
 684
 685 static int mpls_nh_build_multi(struct mpls_route_config *cfg,
 686                                struct mpls_route *rt)
 687 {
 688         struct rtnexthop *rtnh = cfg->rc_mp;
 689         struct nlattr *nla_via, *nla_newdst;
 690         int remaining = cfg->rc_mp_len;
 691         int nhs = 0;
 692         int err = 0;
 693
 694         change_nexthops(rt) {
 695                 int attrlen;
 696
 697                 nla_via = NULL;
 698                 nla_newdst = NULL;
 699
 700                 err = -EINVAL;
 701                 if (!rtnh_ok(rtnh, remaining))
 702                         goto errout;
 703
 704                 /* neither weighted multipath nor any flags
 705                  * are supported
 706                  */
 707                 if (rtnh->rtnh_hops || rtnh->rtnh_flags)
 708                         goto errout;
 709
 710                 attrlen = rtnh_attrlen(rtnh);
 711                 if (attrlen > 0) {
 712                         struct nlattr *attrs = rtnh_attrs(rtnh);
 713
 714                         nla_via = nla_find(attrs, attrlen, RTA_VIA);
 715                         nla_newdst = nla_find(attrs, attrlen, RTA_NEWDST);
 716                 }
 717
 718                 if (!nla_via)
 719                         goto errout;
 720
 721                 err = mpls_nh_build(cfg->rc_nlinfo.nl_net, rt, nh,
 722                                     rtnh->rtnh_ifindex, nla_via, nla_newdst);
 723                 if (err)
 724                         goto errout;
 725
 726                 if (nh->nh_flags & (RTNH_F_DEAD | RTNH_F_LINKDOWN))
 727                         rt->rt_nhn_alive--;
 728
 729                 rtnh = rtnh_next(rtnh, &remaining);
 730                 nhs++;
 731         } endfor_nexthops(rt);
 732
 733         rt->rt_nhn = nhs;
 734
 735         return 0;
 736
 737 errout:
 738         return err;
 739 }
 740
 741 static int mpls_route_add(struct mpls_route_config *cfg)
 742 {
 743         struct mpls_route __rcu **platform_label;
 744         struct net *net = cfg->rc_nlinfo.nl_net;
 745         struct mpls_route *rt, *old;
 746         int err = -EINVAL;
 747         u8 max_via_alen;
 748         unsigned index;
 749         int nhs;
 750
 751         index = cfg->rc_label;
 752
 753         /* If a label was not specified during insert pick one */
 754         if ((index == LABEL_NOT_SPECIFIED) &&
 755             (cfg->rc_nlflags & NLM_F_CREATE)) {
 756                 index = find_free_label(net);
 757         }
 758
 759         /* Reserved labels may not be set */
 760         if (index < MPLS_LABEL_FIRST_UNRESERVED)
 761                 goto errout;
 762
 763         /* The full 20 bit range may not be supported. */
 764         if (index >= net->mpls.platform_labels)
 765                 goto errout;
 766
 767         /* Append makes no sense with mpls */
 768         err = -EOPNOTSUPP;
 769         if (cfg->rc_nlflags & NLM_F_APPEND)
 770                 goto errout;
 771
 772         err = -EEXIST;
 773         platform_label = rtnl_dereference(net->mpls.platform_label);
 774         old = rtnl_dereference(platform_label[index]);
 775         if ((cfg->rc_nlflags & NLM_F_EXCL) && old)
 776                 goto errout;
 777
 778         err = -EEXIST;
 779         if (!(cfg->rc_nlflags & NLM_F_REPLACE) && old)
 780                 goto errout;
 781
 782         err = -ENOENT;
 783         if (!(cfg->rc_nlflags & NLM_F_CREATE) && !old)
 784                 goto errout;
 785
 786         err = -EINVAL;
 787         nhs = mpls_count_nexthops(cfg->rc_mp, cfg->rc_mp_len,
 788                                   cfg->rc_via_alen, &max_via_alen);
 789         if (nhs == 0)
 790                 goto errout;
 791
 792         err = -ENOMEM;
 793         rt = mpls_rt_alloc(nhs, max_via_alen);
 794         if (!rt)
 795                 goto errout;
 796
 797         rt->rt_protocol = cfg->rc_protocol;
 798         rt->rt_payload_type = cfg->rc_payload_type;
 799
 800         if (cfg->rc_mp)
 801                 err = mpls_nh_build_multi(cfg, rt);
 802         else
 803                 err = mpls_nh_build_from_cfg(cfg, rt);
 804         if (err)
 805                 goto freert;
 806
 807         mpls_route_update(net, index, rt, &cfg->rc_nlinfo);
 808
 809         return 0;
 810
 811 freert:
 812         mpls_rt_free(rt);
 813 errout:
 814         return err;
 815 }
 816
 817 static int mpls_route_del(struct mpls_route_config *cfg)
 818 {
 819         struct net *net = cfg->rc_nlinfo.nl_net;
 820         unsigned index;
 821         int err = -EINVAL;
 822
 823         index = cfg->rc_label;
 824
 825         /* Reserved labels may not be removed */
 826         if (index < MPLS_LABEL_FIRST_UNRESERVED)
 827                 goto errout;
 828
 829         /* The full 20 bit range may not be supported */
 830         if (index >= net->mpls.platform_labels)
 831                 goto errout;
 832
 833         mpls_route_update(net, index, NULL, &cfg->rc_nlinfo);
 834
 835         err = 0;
 836 errout:
 837         return err;
 838 }
 839
 840 #define MPLS_PERDEV_SYSCTL_OFFSET(field)        \
 841         (&((struct mpls_dev *)0)->field)
 842
 843 static const struct ctl_table mpls_dev_table[] = {
 844         {
 845                 .procname       = "input",
 846                 .maxlen         = sizeof(int),
 847                 .mode           = 0644,
 848                 .proc_handler   = proc_dointvec,
 849                 .data           = MPLS_PERDEV_SYSCTL_OFFSET(input_enabled),
 850         },
 851         { }
 852 };
 853
 854 static int mpls_dev_sysctl_register(struct net_device *dev,
 855                                     struct mpls_dev *mdev)
 856 {
 857         char path[sizeof("net/mpls/conf/") + IFNAMSIZ];
 858         struct ctl_table *table;
 859         int i;
 860
 861         table = kmemdup(&mpls_dev_table, sizeof(mpls_dev_table), GFP_KERNEL);
 862         if (!table)
 863                 goto out;
 864
 865         /* Table data contains only offsets relative to the base of
 866          * the mdev at this point, so make them absolute.
 867          */
 868         for (i = 0; i < ARRAY_SIZE(mpls_dev_table); i++)
 869                 table[i].data = (char *)mdev + (uintptr_t)table[i].data;
 870
 871         snprintf(path, sizeof(path), "net/mpls/conf/%s", dev->name);
 872
 873         mdev->sysctl = register_net_sysctl(dev_net(dev), path, table);
 874         if (!mdev->sysctl)
 875                 goto free;
 876
 877         return 0;
 878
 879 free:
 880         kfree(table);
 881 out:
 882         return -ENOBUFS;
 883 }
 884
 885 static void mpls_dev_sysctl_unregister(struct mpls_dev *mdev)
 886 {
 887         struct ctl_table *table;
 888
 889         table = mdev->sysctl->ctl_table_arg;
 890         unregister_net_sysctl_table(mdev->sysctl);
 891         kfree(table);
 892 }
 893
 894 static struct mpls_dev *mpls_add_dev(struct net_device *dev)
 895 {
 896         struct mpls_dev *mdev;
 897         int err = -ENOMEM;
 898
 899         ASSERT_RTNL();
 900
 901         mdev = kzalloc(sizeof(*mdev), GFP_KERNEL);
 902         if (!mdev)
 903                 return ERR_PTR(err);
 904
 905         err = mpls_dev_sysctl_register(dev, mdev);
 906         if (err)
 907                 goto free;
 908
 909         rcu_assign_pointer(dev->mpls_ptr, mdev);
 910
 911         return mdev;
 912
 913 free:
 914         kfree(mdev);
 915         return ERR_PTR(err);
 916 }
 917
 918 static void mpls_ifdown(struct net_device *dev, int event)
 919 {
 920         struct mpls_route __rcu **platform_label;
 921         struct net *net = dev_net(dev);
 922         unsigned index;
 923
 924         platform_label = rtnl_dereference(net->mpls.platform_label);
 925         for (index = 0; index < net->mpls.platform_labels; index++) {
 926                 struct mpls_route *rt = rtnl_dereference(platform_label[index]);
 927
 928                 if (!rt)
 929                         continue;
 930
 931                 change_nexthops(rt) {
 932                         if (rtnl_dereference(nh->nh_dev) != dev)
 933                                 continue;
 934                         switch (event) {
 935                         case NETDEV_DOWN:
 936                         case NETDEV_UNREGISTER:
 937                                 nh->nh_flags |= RTNH_F_DEAD;
 938                                 /* fall through */
 939                         case NETDEV_CHANGE:
 940                                 nh->nh_flags |= RTNH_F_LINKDOWN;
 941                                 ACCESS_ONCE(rt->rt_nhn_alive) = rt->rt_nhn_alive - 1;
 942                                 break;
 943                         }
 944                         if (event == NETDEV_UNREGISTER)
 945                                 RCU_INIT_POINTER(nh->nh_dev, NULL);
 946                 } endfor_nexthops(rt);
 947         }
 948
 949
 950         return;
 951 }
 952
 953 static void mpls_ifup(struct net_device *dev, unsigned int nh_flags)
 954 {
 955         struct mpls_route __rcu **platform_label;
 956         struct net *net = dev_net(dev);
 957         unsigned index;
 958         int alive;
 959
 960         platform_label = rtnl_dereference(net->mpls.platform_label);
 961         for (index = 0; index < net->mpls.platform_labels; index++) {
 962                 struct mpls_route *rt = rtnl_dereference(platform_label[index]);
 963
 964                 if (!rt)
 965                         continue;
 966
 967                 alive = 0;
 968                 change_nexthops(rt) {
 969                         struct net_device *nh_dev =
 970                                 rtnl_dereference(nh->nh_dev);
 971
 972                         if (!(nh->nh_flags & nh_flags)) {
 973                                 alive++;
 974                                 continue;
 975                         }
 976                         if (nh_dev != dev)
 977                                 continue;
 978                         alive++;
 979                         nh->nh_flags &= ~nh_flags;
 980                 } endfor_nexthops(rt);
 981
 982                 ACCESS_ONCE(rt->rt_nhn_alive) = alive;
 983         }
 984
 985         return;
 986 }
 987
 988 static int mpls_dev_notify(struct notifier_block *this, unsigned long event,
 989                            void *ptr)
 990 {
 991         struct net_device *dev = netdev_notifier_info_to_dev(ptr);
 992         struct mpls_dev *mdev;
 993         unsigned int flags;
 994
 995         if (event == NETDEV_REGISTER) {
 996                 /* For now just support ethernet devices */
 997                 if ((dev->type == ARPHRD_ETHER) ||
 998                     (dev->type == ARPHRD_LOOPBACK)) {
 999                         mdev = mpls_add_dev(dev);
1000                         if (IS_ERR(mdev))
1001                                 return notifier_from_errno(PTR_ERR(mdev));
1002                 }
1003                 return NOTIFY_OK;
1004         }
1005
1006         mdev = mpls_dev_get(dev);
1007         if (!mdev)
1008                 return NOTIFY_OK;
1009
1010         switch (event) {
1011         case NETDEV_DOWN:
1012                 mpls_ifdown(dev, event);
1013                 break;
1014         case NETDEV_UP:
1015                 flags = dev_get_flags(dev);
1016                 if (flags & (IFF_RUNNING | IFF_LOWER_UP))
1017                         mpls_ifup(dev, RTNH_F_DEAD | RTNH_F_LINKDOWN);
1018                 else
1019                         mpls_ifup(dev, RTNH_F_DEAD);
1020                 break;
1021         case NETDEV_CHANGE:
1022                 flags = dev_get_flags(dev);
1023                 if (flags & (IFF_RUNNING | IFF_LOWER_UP))
1024                         mpls_ifup(dev, RTNH_F_DEAD | RTNH_F_LINKDOWN);
1025                 else
1026                         mpls_ifdown(dev, event);
1027                 break;
1028         case NETDEV_UNREGISTER:
1029                 mpls_ifdown(dev, event);
1030                 mdev = mpls_dev_get(dev);
1031                 if (mdev) {
1032                         mpls_dev_sysctl_unregister(mdev);
1033                         RCU_INIT_POINTER(dev->mpls_ptr, NULL);
1034                         kfree_rcu(mdev, rcu);
1035                 }
1036                 break;
1037         case NETDEV_CHANGENAME:
1038                 mdev = mpls_dev_get(dev);
1039                 if (mdev) {
1040                         int err;
1041
1042                         mpls_dev_sysctl_unregister(mdev);
1043                         err = mpls_dev_sysctl_register(dev, mdev);
1044                         if (err)
1045                                 return notifier_from_errno(err);
1046                 }
1047                 break;
1048         }
1049         return NOTIFY_OK;
1050 }
1051
1052 static struct notifier_block mpls_dev_notifier = {
1053         .notifier_call = mpls_dev_notify,
1054 };
1055
1056 static int nla_put_via(struct sk_buff *skb,
1057                        u8 table, const void *addr, int alen)
1058 {
1059         static const int table_to_family[NEIGH_NR_TABLES + 1] = {
1060                 AF_INET, AF_INET6, AF_DECnet, AF_PACKET,
1061         };
1062         struct nlattr *nla;
1063         struct rtvia *via;
1064         int family = AF_UNSPEC;
1065
1066         nla = nla_reserve(skb, RTA_VIA, alen + 2);
1067         if (!nla)
1068                 return -EMSGSIZE;
1069
1070         if (table <= NEIGH_NR_TABLES)
1071                 family = table_to_family[table];
1072
1073         via = nla_data(nla);
1074         via->rtvia_family = family;
1075         memcpy(via->rtvia_addr, addr, alen);
1076         return 0;
1077 }
1078
1079 int nla_put_labels(struct sk_buff *skb, int attrtype,
1080                    u8 labels, const u32 label[])
1081 {
1082         struct nlattr *nla;
1083         struct mpls_shim_hdr *nla_label;
1084         bool bos;
1085         int i;
1086         nla = nla_reserve(skb, attrtype, labels*4);
1087         if (!nla)
1088                 return -EMSGSIZE;
1089
1090         nla_label = nla_data(nla);
1091         bos = true;
1092         for (i = labels - 1; i >= 0; i--) {
1093                 nla_label[i] = mpls_entry_encode(label[i], 0, 0, bos);
1094                 bos = false;
1095         }
1096
1097         return 0;
1098 }
1099 EXPORT_SYMBOL_GPL(nla_put_labels);
1100
1101 int nla_get_labels(const struct nlattr *nla,
1102                    u32 max_labels, u8 *labels, u32 label[])
1103 {
1104         unsigned len = nla_len(nla);
1105         unsigned nla_labels;
1106         struct mpls_shim_hdr *nla_label;
1107         bool bos;
1108         int i;
1109
1110         /* len needs to be an even multiple of 4 (the label size) */
1111         if (len & 3)
1112                 return -EINVAL;
1113
1114         /* Limit the number of new labels allowed */
1115         nla_labels = len/4;
1116         if (nla_labels > max_labels)
1117                 return -EINVAL;
1118
1119         nla_label = nla_data(nla);
1120         bos = true;
1121         for (i = nla_labels - 1; i >= 0; i--, bos = false) {
1122                 struct mpls_entry_decoded dec;
1123                 dec = mpls_entry_decode(nla_label + i);
1124
1125                 /* Ensure the bottom of stack flag is properly set
1126                  * and ttl and tc are both clear.
1127                  */
1128                 if ((dec.bos != bos) || dec.ttl || dec.tc)
1129                         return -EINVAL;
1130
1131                 switch (dec.label) {
1132                 case MPLS_LABEL_IMPLNULL:
1133                         /* RFC3032: This is a label that an LSR may
1134                          * assign and distribute, but which never
1135                          * actually appears in the encapsulation.
1136                          */
1137                         return -EINVAL;
1138                 }
1139
1140                 label[i] = dec.label;
1141         }
1142         *labels = nla_labels;
1143         return 0;
1144 }
1145 EXPORT_SYMBOL_GPL(nla_get_labels);
1146
1147 int nla_get_via(const struct nlattr *nla, u8 *via_alen,
1148                 u8 *via_table, u8 via_addr[])
1149 {
1150         struct rtvia *via = nla_data(nla);
1151         int err = -EINVAL;
1152         int alen;
1153
1154         if (nla_len(nla) < offsetof(struct rtvia, rtvia_addr))
1155                 goto errout;
1156         alen = nla_len(nla) -
1157                         offsetof(struct rtvia, rtvia_addr);
1158         if (alen > MAX_VIA_ALEN)
1159                 goto errout;
1160
1161         /* Validate the address family */
1162         switch (via->rtvia_family) {
1163         case AF_PACKET:
1164                 *via_table = NEIGH_LINK_TABLE;
1165                 break;
1166         case AF_INET:
1167                 *via_table = NEIGH_ARP_TABLE;
1168                 if (alen != 4)
1169                         goto errout;
1170                 break;
1171         case AF_INET6:
1172                 *via_table = NEIGH_ND_TABLE;
1173                 if (alen != 16)
1174                         goto errout;
1175                 break;
1176         default:
1177                 /* Unsupported address family */
1178                 goto errout;
1179         }
1180
1181         memcpy(via_addr, via->rtvia_addr, alen);
1182         *via_alen = alen;
1183         err = 0;
1184
1185 errout:
1186         return err;
1187 }
1188
1189 static int rtm_to_route_config(struct sk_buff *skb,  struct nlmsghdr *nlh,
1190                                struct mpls_route_config *cfg)
1191 {
1192         struct rtmsg *rtm;
1193         struct nlattr *tb[RTA_MAX+1];
1194         int index;
1195         int err;
1196
1197         err = nlmsg_parse(nlh, sizeof(*rtm), tb, RTA_MAX, rtm_mpls_policy);
1198         if (err < 0)
1199                 goto errout;
1200
1201         err = -EINVAL;
1202         rtm = nlmsg_data(nlh);
1203         memset(cfg, 0, sizeof(*cfg));
1204
1205         if (rtm->rtm_family != AF_MPLS)
1206                 goto errout;
1207         if (rtm->rtm_dst_len != 20)
1208                 goto errout;
1209         if (rtm->rtm_src_len != 0)
1210                 goto errout;
1211         if (rtm->rtm_tos != 0)
1212                 goto errout;
1213         if (rtm->rtm_table != RT_TABLE_MAIN)
1214                 goto errout;
1215         /* Any value is acceptable for rtm_protocol */
1216
1217         /* As mpls uses destination specific addresses
1218          * (or source specific address in the case of multicast)
1219          * all addresses have universal scope.
1220          */
1221         if (rtm->rtm_scope != RT_SCOPE_UNIVERSE)
1222                 goto errout;
1223         if (rtm->rtm_type != RTN_UNICAST)
1224                 goto errout;
1225         if (rtm->rtm_flags != 0)
1226                 goto errout;
1227
1228         cfg->rc_label           = LABEL_NOT_SPECIFIED;
1229         cfg->rc_protocol        = rtm->rtm_protocol;
1230         cfg->rc_nlflags         = nlh->nlmsg_flags;
1231         cfg->rc_nlinfo.portid   = NETLINK_CB(skb).portid;
1232         cfg->rc_nlinfo.nlh      = nlh;
1233         cfg->rc_nlinfo.nl_net   = sock_net(skb->sk);
1234
1235         for (index = 0; index <= RTA_MAX; index++) {
1236                 struct nlattr *nla = tb[index];
1237                 if (!nla)
1238                         continue;
1239
1240                 switch(index) {
1241                 case RTA_OIF:
1242                         cfg->rc_ifindex = nla_get_u32(nla);
1243                         break;
1244                 case RTA_NEWDST:
1245                         if (nla_get_labels(nla, MAX_NEW_LABELS,
1246                                            &cfg->rc_output_labels,
1247                                            cfg->rc_output_label))
1248                                 goto errout;
1249                         break;
1250                 case RTA_DST:
1251                 {
1252                         u8 label_count;
1253                         if (nla_get_labels(nla, 1, &label_count,
1254                                            &cfg->rc_label))
1255                                 goto errout;
1256
1257                         /* Reserved labels may not be set */
1258                         if (cfg->rc_label < MPLS_LABEL_FIRST_UNRESERVED)
1259                                 goto errout;
1260
1261                         break;
1262                 }
1263                 case RTA_VIA:
1264                 {
1265                         if (nla_get_via(nla, &cfg->rc_via_alen,
1266                                         &cfg->rc_via_table, cfg->rc_via))
1267                                 goto errout;
1268                         break;
1269                 }
1270                 case RTA_MULTIPATH:
1271                 {
1272                         cfg->rc_mp = nla_data(nla);
1273                         cfg->rc_mp_len = nla_len(nla);
1274                         break;
1275                 }
1276                 default:
1277                         /* Unsupported attribute */
1278                         goto errout;
1279                 }
1280         }
1281
1282         err = 0;
1283 errout:
1284         return err;
1285 }
1286
1287 static int mpls_rtm_delroute(struct sk_buff *skb, struct nlmsghdr *nlh)
1288 {
1289         struct mpls_route_config cfg;
1290         int err;
1291
1292         err = rtm_to_route_config(skb, nlh, &cfg);
1293         if (err < 0)
1294                 return err;
1295
1296         return mpls_route_del(&cfg);
1297 }
1298
1299
1300 static int mpls_rtm_newroute(struct sk_buff *skb, struct nlmsghdr *nlh)
1301 {
1302         struct mpls_route_config cfg;
1303         int err;
1304
1305         err = rtm_to_route_config(skb, nlh, &cfg);
1306         if (err < 0)
1307                 return err;
1308
1309         return mpls_route_add(&cfg);
1310 }
1311
1312 static int mpls_dump_route(struct sk_buff *skb, u32 portid, u32 seq, int event,
1313                            u32 label, struct mpls_route *rt, int flags)
1314 {
1315         struct net_device *dev;
1316         struct nlmsghdr *nlh;
1317         struct rtmsg *rtm;
1318
1319         nlh = nlmsg_put(skb, portid, seq, event, sizeof(*rtm), flags);
1320         if (nlh == NULL)
1321                 return -EMSGSIZE;
1322
1323         rtm = nlmsg_data(nlh);
1324         rtm->rtm_family = AF_MPLS;
1325         rtm->rtm_dst_len = 20;
1326         rtm->rtm_src_len = 0;
1327         rtm->rtm_tos = 0;
1328         rtm->rtm_table = RT_TABLE_MAIN;
1329         rtm->rtm_protocol = rt->rt_protocol;
1330         rtm->rtm_scope = RT_SCOPE_UNIVERSE;
1331         rtm->rtm_type = RTN_UNICAST;
1332         rtm->rtm_flags = 0;
1333
1334         if (nla_put_labels(skb, RTA_DST, 1, &label))
1335                 goto nla_put_failure;
1336         if (rt->rt_nhn == 1) {
1337                 const struct mpls_nh *nh = rt->rt_nh;
1338
1339                 if (nh->nh_labels &&
1340                     nla_put_labels(skb, RTA_NEWDST, nh->nh_labels,
1341                                    nh->nh_label))
1342                         goto nla_put_failure;
1343                 if (nla_put_via(skb, nh->nh_via_table, mpls_nh_via(rt, nh),
1344                                 nh->nh_via_alen))
1345                         goto nla_put_failure;
1346                 dev = rtnl_dereference(nh->nh_dev);
1347                 if (dev && nla_put_u32(skb, RTA_OIF, dev->ifindex))
1348                         goto nla_put_failure;
1349                 if (nh->nh_flags & RTNH_F_LINKDOWN)
1350                         rtm->rtm_flags |= RTNH_F_LINKDOWN;
1351                 if (nh->nh_flags & RTNH_F_DEAD)
1352                         rtm->rtm_flags |= RTNH_F_DEAD;
1353         } else {
1354                 struct rtnexthop *rtnh;
1355                 struct nlattr *mp;
1356                 int dead = 0;
1357                 int linkdown = 0;
1358
1359                 mp = nla_nest_start(skb, RTA_MULTIPATH);
1360                 if (!mp)
1361                         goto nla_put_failure;
1362
1363                 for_nexthops(rt) {
1364                         rtnh = nla_reserve_nohdr(skb, sizeof(*rtnh));
1365                         if (!rtnh)
1366                                 goto nla_put_failure;
1367
1368                         dev = rtnl_dereference(nh->nh_dev);
1369                         if (dev)
1370                                 rtnh->rtnh_ifindex = dev->ifindex;
1371                         if (nh->nh_flags & RTNH_F_LINKDOWN) {
1372                                 rtnh->rtnh_flags |= RTNH_F_LINKDOWN;
1373                                 linkdown++;
1374                         }
1375                         if (nh->nh_flags & RTNH_F_DEAD) {
1376                                 rtnh->rtnh_flags |= RTNH_F_DEAD;
1377                                 dead++;
1378                         }
1379
1380                         if (nh->nh_labels && nla_put_labels(skb, RTA_NEWDST,
1381                                                             nh->nh_labels,
1382                                                             nh->nh_label))
1383                                 goto nla_put_failure;
1384                         if (nla_put_via(skb, nh->nh_via_table,
1385                                         mpls_nh_via(rt, nh),
1386                                         nh->nh_via_alen))
1387                                 goto nla_put_failure;
1388
1389                         /* length of rtnetlink header + attributes */
1390                         rtnh->rtnh_len = nlmsg_get_pos(skb) - (void *)rtnh;
1391                 } endfor_nexthops(rt);
1392
1393                 if (linkdown == rt->rt_nhn)
1394                         rtm->rtm_flags |= RTNH_F_LINKDOWN;
1395                 if (dead == rt->rt_nhn)
1396                         rtm->rtm_flags |= RTNH_F_DEAD;
1397
1398                 nla_nest_end(skb, mp);
1399         }
1400
1401         nlmsg_end(skb, nlh);
1402         return 0;
1403
1404 nla_put_failure:
1405         nlmsg_cancel(skb, nlh);
1406         return -EMSGSIZE;
1407 }
1408
1409 static int mpls_dump_routes(struct sk_buff *skb, struct netlink_callback *cb)
1410 {
1411         struct net *net = sock_net(skb->sk);
1412         struct mpls_route __rcu **platform_label;
1413         size_t platform_labels;
1414         unsigned int index;
1415
1416         ASSERT_RTNL();
1417
1418         index = cb->args[0];
1419         if (index < MPLS_LABEL_FIRST_UNRESERVED)
1420                 index = MPLS_LABEL_FIRST_UNRESERVED;
1421
1422         platform_label = rtnl_dereference(net->mpls.platform_label);
1423         platform_labels = net->mpls.platform_labels;
1424         for (; index < platform_labels; index++) {
1425                 struct mpls_route *rt;
1426                 rt = rtnl_dereference(platform_label[index]);
1427                 if (!rt)
1428                         continue;
1429
1430                 if (mpls_dump_route(skb, NETLINK_CB(cb->skb).portid,
1431                                     cb->nlh->nlmsg_seq, RTM_NEWROUTE,
1432                                     index, rt, NLM_F_MULTI) < 0)
1433                         break;
1434         }
1435         cb->args[0] = index;
1436
1437         return skb->len;
1438 }
1439
1440 static inline size_t lfib_nlmsg_size(struct mpls_route *rt)
1441 {
1442         size_t payload =
1443                 NLMSG_ALIGN(sizeof(struct rtmsg))
1444                 + nla_total_size(4);                    /* RTA_DST */
1445
1446         if (rt->rt_nhn == 1) {
1447                 struct mpls_nh *nh = rt->rt_nh;
1448
1449                 if (nh->nh_dev)
1450                         payload += nla_total_size(4); /* RTA_OIF */
1451                 payload += nla_total_size(2 + nh->nh_via_alen); /* RTA_VIA */
1452                 if (nh->nh_labels) /* RTA_NEWDST */
1453                         payload += nla_total_size(nh->nh_labels * 4);
1454         } else {
1455                 /* each nexthop is packed in an attribute */
1456                 size_t nhsize = 0;
1457
1458                 for_nexthops(rt) {
1459                         nhsize += nla_total_size(sizeof(struct rtnexthop));
1460                         nhsize += nla_total_size(2 + nh->nh_via_alen);
1461                         if (nh->nh_labels)
1462                                 nhsize += nla_total_size(nh->nh_labels * 4);
1463                 } endfor_nexthops(rt);
1464                 /* nested attribute */
1465                 payload += nla_total_size(nhsize);
1466         }
1467
1468         return payload;
1469 }
1470
1471 static void rtmsg_lfib(int event, u32 label, struct mpls_route *rt,
1472                        struct nlmsghdr *nlh, struct net *net, u32 portid,
1473                        unsigned int nlm_flags)
1474 {
1475         struct sk_buff *skb;
1476         u32 seq = nlh ? nlh->nlmsg_seq : 0;
1477         int err = -ENOBUFS;
1478
1479         skb = nlmsg_new(lfib_nlmsg_size(rt), GFP_KERNEL);
1480         if (skb == NULL)
1481                 goto errout;
1482
1483         err = mpls_dump_route(skb, portid, seq, event, label, rt, nlm_flags);
1484         if (err < 0) {
1485                 /* -EMSGSIZE implies BUG in lfib_nlmsg_size */
1486                 WARN_ON(err == -EMSGSIZE);
1487                 kfree_skb(skb);
1488                 goto errout;
1489         }
1490         rtnl_notify(skb, net, portid, RTNLGRP_MPLS_ROUTE, nlh, GFP_KERNEL);
1491
1492         return;
1493 errout:
1494         if (err < 0)
1495                 rtnl_set_sk_err(net, RTNLGRP_MPLS_ROUTE, err);
1496 }
1497
1498 static int resize_platform_label_table(struct net *net, size_t limit)
1499 {
1500         size_t size = sizeof(struct mpls_route *) * limit;
1501         size_t old_limit;
1502         size_t cp_size;
1503         struct mpls_route __rcu **labels = NULL, **old;
1504         struct mpls_route *rt0 = NULL, *rt2 = NULL;
1505         unsigned index;
1506
1507         if (size) {
1508                 labels = kzalloc(size, GFP_KERNEL | __GFP_NOWARN | __GFP_NORETRY);
1509                 if (!labels)
1510                         labels = vzalloc(size);
1511
1512                 if (!labels)
1513                         goto nolabels;
1514         }
1515
1516         /* In case the predefined labels need to be populated */
1517         if (limit > MPLS_LABEL_IPV4NULL) {
1518                 struct net_device *lo = net->loopback_dev;
1519                 rt0 = mpls_rt_alloc(1, lo->addr_len);
1520                 if (!rt0)
1521                         goto nort0;
1522                 RCU_INIT_POINTER(rt0->rt_nh->nh_dev, lo);
1523                 rt0->rt_protocol = RTPROT_KERNEL;
1524                 rt0->rt_payload_type = MPT_IPV4;
1525                 rt0->rt_nh->nh_via_table = NEIGH_LINK_TABLE;
1526                 rt0->rt_nh->nh_via_alen = lo->addr_len;
1527                 memcpy(__mpls_nh_via(rt0, rt0->rt_nh), lo->dev_addr,
1528                        lo->addr_len);
1529         }
1530         if (limit > MPLS_LABEL_IPV6NULL) {
1531                 struct net_device *lo = net->loopback_dev;
1532                 rt2 = mpls_rt_alloc(1, lo->addr_len);
1533                 if (!rt2)
1534                         goto nort2;
1535                 RCU_INIT_POINTER(rt2->rt_nh->nh_dev, lo);
1536                 rt2->rt_protocol = RTPROT_KERNEL;
1537                 rt2->rt_payload_type = MPT_IPV6;
1538                 rt2->rt_nh->nh_via_table = NEIGH_LINK_TABLE;
1539                 rt2->rt_nh->nh_via_alen = lo->addr_len;
1540                 memcpy(__mpls_nh_via(rt2, rt2->rt_nh), lo->dev_addr,
1541                        lo->addr_len);
1542         }
1543
1544         rtnl_lock();
1545         /* Remember the original table */
1546         old = rtnl_dereference(net->mpls.platform_label);
1547         old_limit = net->mpls.platform_labels;
1548
1549         /* Free any labels beyond the new table */
1550         for (index = limit; index < old_limit; index++)
1551                 mpls_route_update(net, index, NULL, NULL);
1552
1553         /* Copy over the old labels */
1554         cp_size = size;
1555         if (old_limit < limit)
1556                 cp_size = old_limit * sizeof(struct mpls_route *);
1557
1558         memcpy(labels, old, cp_size);
1559
1560         /* If needed set the predefined labels */
1561         if ((old_limit <= MPLS_LABEL_IPV6NULL) &&
1562             (limit > MPLS_LABEL_IPV6NULL)) {
1563                 RCU_INIT_POINTER(labels[MPLS_LABEL_IPV6NULL], rt2);
1564                 rt2 = NULL;
1565         }
1566
1567         if ((old_limit <= MPLS_LABEL_IPV4NULL) &&
1568             (limit > MPLS_LABEL_IPV4NULL)) {
1569                 RCU_INIT_POINTER(labels[MPLS_LABEL_IPV4NULL], rt0);
1570                 rt0 = NULL;
1571         }
1572
1573         /* Update the global pointers */
1574         net->mpls.platform_labels = limit;
1575         rcu_assign_pointer(net->mpls.platform_label, labels);
1576
1577         rtnl_unlock();
1578
1579         mpls_rt_free(rt2);
1580         mpls_rt_free(rt0);
1581
1582         if (old) {
1583                 synchronize_rcu();
1584                 kvfree(old);
1585         }
1586         return 0;
1587
1588 nort2:
1589         mpls_rt_free(rt0);
1590 nort0:
1591         kvfree(labels);
1592 nolabels:
1593         return -ENOMEM;
1594 }
1595
1596 static int mpls_platform_labels(struct ctl_table *table, int write,
1597                                 void __user *buffer, size_t *lenp, loff_t *ppos)
1598 {
1599         struct net *net = table->data;
1600         int platform_labels = net->mpls.platform_labels;
1601         int ret;
1602         struct ctl_table tmp = {
1603                 .procname       = table->procname,
1604                 .data           = &platform_labels,
1605                 .maxlen         = sizeof(int),
1606                 .mode           = table->mode,
1607                 .extra1         = &zero,
1608                 .extra2         = &label_limit,
1609         };
1610
1611         ret = proc_dointvec_minmax(&tmp, write, buffer, lenp, ppos);
1612
1613         if (write && ret == 0)
1614                 ret = resize_platform_label_table(net, platform_labels);
1615
1616         return ret;
1617 }
1618
1619 static const struct ctl_table mpls_table[] = {
1620         {
1621                 .procname       = "platform_labels",
1622                 .data           = NULL,
1623                 .maxlen         = sizeof(int),
1624                 .mode           = 0644,
1625                 .proc_handler   = mpls_platform_labels,
1626         },
1627         { }
1628 };
1629
1630 static int mpls_net_init(struct net *net)
1631 {
1632         struct ctl_table *table;
1633
1634         net->mpls.platform_labels = 0;
1635         net->mpls.platform_label = NULL;
1636
1637         table = kmemdup(mpls_table, sizeof(mpls_table), GFP_KERNEL);
1638         if (table == NULL)
1639                 return -ENOMEM;
1640
1641         table[0].data = net;
1642         net->mpls.ctl = register_net_sysctl(net, "net/mpls", table);
1643         if (net->mpls.ctl == NULL) {
1644                 kfree(table);
1645                 return -ENOMEM;
1646         }
1647
1648         return 0;
1649 }
1650
1651 static void mpls_net_exit(struct net *net)
1652 {
1653         struct mpls_route __rcu **platform_label;
1654         size_t platform_labels;
1655         struct ctl_table *table;
1656         unsigned int index;
1657
1658         table = net->mpls.ctl->ctl_table_arg;
1659         unregister_net_sysctl_table(net->mpls.ctl);
1660         kfree(table);
1661
1662         /* An rcu grace period has passed since there was a device in
1663          * the network namespace (and thus the last in flight packet)
1664          * left this network namespace.  This is because
1665          * unregister_netdevice_many and netdev_run_todo has completed
1666          * for each network device that was in this network namespace.
1667          *
1668          * As such no additional rcu synchronization is necessary when
1669          * freeing the platform_label table.
1670          */
1671         rtnl_lock();
1672         platform_label = rtnl_dereference(net->mpls.platform_label);
1673         platform_labels = net->mpls.platform_labels;
1674         for (index = 0; index < platform_labels; index++) {
1675                 struct mpls_route *rt = rtnl_dereference(platform_label[index]);
1676                 RCU_INIT_POINTER(platform_label[index], NULL);
1677                 mpls_rt_free(rt);
1678         }
1679         rtnl_unlock();
1680
1681         kvfree(platform_label);
1682 }
1683
1684 static struct pernet_operations mpls_net_ops = {
1685         .init = mpls_net_init,
1686         .exit = mpls_net_exit,
1687 };
1688
1689 static int __init mpls_init(void)
1690 {
1691         int err;
1692
1693         BUILD_BUG_ON(sizeof(struct mpls_shim_hdr) != 4);
1694
1695         err = register_pernet_subsys(&mpls_net_ops);
1696         if (err)
1697                 goto out;
1698
1699         err = register_netdevice_notifier(&mpls_dev_notifier);
1700         if (err)
1701                 goto out_unregister_pernet;
1702
1703         dev_add_pack(&mpls_packet_type);
1704
1705         rtnl_register(PF_MPLS, RTM_NEWROUTE, mpls_rtm_newroute, NULL, NULL);
1706         rtnl_register(PF_MPLS, RTM_DELROUTE, mpls_rtm_delroute, NULL, NULL);
1707         rtnl_register(PF_MPLS, RTM_GETROUTE, NULL, mpls_dump_routes, NULL);
1708         err = 0;
1709 out:
1710         return err;
1711
1712 out_unregister_pernet:
1713         unregister_pernet_subsys(&mpls_net_ops);
1714         goto out;
1715 }
1716 module_init(mpls_init);
1717
1718 static void __exit mpls_exit(void)
1719 {
1720         rtnl_unregister_all(PF_MPLS);
1721         dev_remove_pack(&mpls_packet_type);
1722         unregister_netdevice_notifier(&mpls_dev_notifier);
1723         unregister_pernet_subsys(&mpls_net_ops);
1724 }
1725 module_exit(mpls_exit);
1726
1727 MODULE_DESCRIPTION("MultiProtocol Label Switching");
1728 MODULE_LICENSE("GPL v2");
1729 MODULE_ALIAS_NETPROTO(PF_MPLS);