net/mctp/route.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Management Component Transport Protocol (MCTP) - routing
   4  * implementation.
   5  *
   6  * This is currently based on a simple routing table, with no dst cache. The
   7  * number of routes should stay fairly small, so the lookup cost is small.
   8  *
   9  * Copyright (c) 2021 Code Construct
  10  * Copyright (c) 2021 Google
  11  */
  12
  13 #include <linux/idr.h>
  14 #include <linux/kconfig.h>
  15 #include <linux/mctp.h>
  16 #include <linux/netdevice.h>
  17 #include <linux/rtnetlink.h>
  18 #include <linux/skbuff.h>
  19
  20 #include <uapi/linux/if_arp.h>
  21
  22 #include <net/mctp.h>
  23 #include <net/mctpdevice.h>
  24 #include <net/netlink.h>
  25 #include <net/sock.h>
  26
  27 #include <trace/events/mctp.h>
  28
  29 static const unsigned int mctp_message_maxlen = 64 * 1024;
  30 static const unsigned long mctp_key_lifetime = 6 * CONFIG_HZ;
  31
  32 static void mctp_flow_prepare_output(struct sk_buff *skb, struct mctp_dev *dev);
  33
  34 /* route output callbacks */
  35 static int mctp_route_discard(struct mctp_route *route, struct sk_buff *skb)
  36 {
  37         kfree_skb(skb);
  38         return 0;
  39 }
  40
  41 static struct mctp_sock *mctp_lookup_bind(struct net *net, struct sk_buff *skb)
  42 {
  43         struct mctp_skb_cb *cb = mctp_cb(skb);
  44         struct mctp_hdr *mh;
  45         struct sock *sk;
  46         u8 type;
  47
  48         WARN_ON(!rcu_read_lock_held());
  49
  50         /* TODO: look up in skb->cb? */
  51         mh = mctp_hdr(skb);
  52
  53         if (!skb_headlen(skb))
  54                 return NULL;
  55
  56         type = (*(u8 *)skb->data) & 0x7f;
  57
  58         sk_for_each_rcu(sk, &net->mctp.binds) {
  59                 struct mctp_sock *msk = container_of(sk, struct mctp_sock, sk);
  60
  61                 if (msk->bind_net != MCTP_NET_ANY && msk->bind_net != cb->net)
  62                         continue;
  63
  64                 if (msk->bind_type != type)
  65                         continue;
  66
  67                 if (msk->bind_addr != MCTP_ADDR_ANY &&
  68                     msk->bind_addr != mh->dest)
  69                         continue;
  70
  71                 return msk;
  72         }
  73
  74         return NULL;
  75 }
  76
  77 static bool mctp_key_match(struct mctp_sk_key *key, mctp_eid_t local,
  78                            mctp_eid_t peer, u8 tag)
  79 {
  80         if (key->local_addr != local)
  81                 return false;
  82
  83         if (key->peer_addr != peer)
  84                 return false;
  85
  86         if (key->tag != tag)
  87                 return false;
  88
  89         return true;
  90 }
  91
  92 /* returns a key (with key->lock held, and refcounted), or NULL if no such
  93  * key exists.
  94  */
  95 static struct mctp_sk_key *mctp_lookup_key(struct net *net, struct sk_buff *skb,
  96                                            mctp_eid_t peer,
  97                                            unsigned long *irqflags)
  98         __acquires(&key->lock)
  99 {
 100         struct mctp_sk_key *key, *ret;
 101         unsigned long flags;
 102         struct mctp_hdr *mh;
 103         u8 tag;
 104
 105         mh = mctp_hdr(skb);
 106         tag = mh->flags_seq_tag & (MCTP_HDR_TAG_MASK | MCTP_HDR_FLAG_TO);
 107
 108         ret = NULL;
 109         spin_lock_irqsave(&net->mctp.keys_lock, flags);
 110
 111         hlist_for_each_entry(key, &net->mctp.keys, hlist) {
 112                 if (!mctp_key_match(key, mh->dest, peer, tag))
 113                         continue;
 114
 115                 spin_lock(&key->lock);
 116                 if (key->valid) {
 117                         refcount_inc(&key->refs);
 118                         ret = key;
 119                         break;
 120                 }
 121                 spin_unlock(&key->lock);
 122         }
 123
 124         if (ret) {
 125                 spin_unlock(&net->mctp.keys_lock);
 126                 *irqflags = flags;
 127         } else {
 128                 spin_unlock_irqrestore(&net->mctp.keys_lock, flags);
 129         }
 130
 131         return ret;
 132 }
 133
 134 static struct mctp_sk_key *mctp_key_alloc(struct mctp_sock *msk,
 135                                           mctp_eid_t local, mctp_eid_t peer,
 136                                           u8 tag, gfp_t gfp)
 137 {
 138         struct mctp_sk_key *key;
 139
 140         key = kzalloc(sizeof(*key), gfp);
 141         if (!key)
 142                 return NULL;
 143
 144         key->peer_addr = peer;
 145         key->local_addr = local;
 146         key->tag = tag;
 147         key->sk = &msk->sk;
 148         key->valid = true;
 149         spin_lock_init(&key->lock);
 150         refcount_set(&key->refs, 1);
 151
 152         return key;
 153 }
 154
 155 void mctp_key_unref(struct mctp_sk_key *key)
 156 {
 157         unsigned long flags;
 158
 159         if (!refcount_dec_and_test(&key->refs))
 160                 return;
 161
 162         /* even though no refs exist here, the lock allows us to stay
 163          * consistent with the locking requirement of mctp_dev_release_key
 164          */
 165         spin_lock_irqsave(&key->lock, flags);
 166         mctp_dev_release_key(key->dev, key);
 167         spin_unlock_irqrestore(&key->lock, flags);
 168
 169         kfree(key);
 170 }
 171
 172 static int mctp_key_add(struct mctp_sk_key *key, struct mctp_sock *msk)
 173 {
 174         struct net *net = sock_net(&msk->sk);
 175         struct mctp_sk_key *tmp;
 176         unsigned long flags;
 177         int rc = 0;
 178
 179         spin_lock_irqsave(&net->mctp.keys_lock, flags);
 180
 181         hlist_for_each_entry(tmp, &net->mctp.keys, hlist) {
 182                 if (mctp_key_match(tmp, key->local_addr, key->peer_addr,
 183                                    key->tag)) {
 184                         spin_lock(&tmp->lock);
 185                         if (tmp->valid)
 186                                 rc = -EEXIST;
 187                         spin_unlock(&tmp->lock);
 188                         if (rc)
 189                                 break;
 190                 }
 191         }
 192
 193         if (!rc) {
 194                 refcount_inc(&key->refs);
 195                 key->expiry = jiffies + mctp_key_lifetime;
 196                 timer_reduce(&msk->key_expiry, key->expiry);
 197
 198                 hlist_add_head(&key->hlist, &net->mctp.keys);
 199                 hlist_add_head(&key->sklist, &msk->keys);
 200         }
 201
 202         spin_unlock_irqrestore(&net->mctp.keys_lock, flags);
 203
 204         return rc;
 205 }
 206
 207 /* We're done with the key; unset valid and remove from lists. There may still
 208  * be outstanding refs on the key though...
 209  */
 210 static void __mctp_key_unlock_drop(struct mctp_sk_key *key, struct net *net,
 211                                    unsigned long flags)
 212         __releases(&key->lock)
 213 {
 214         struct sk_buff *skb;
 215
 216         skb = key->reasm_head;
 217         key->reasm_head = NULL;
 218         key->reasm_dead = true;
 219         key->valid = false;
 220         mctp_dev_release_key(key->dev, key);
 221         spin_unlock_irqrestore(&key->lock, flags);
 222
 223         spin_lock_irqsave(&net->mctp.keys_lock, flags);
 224         hlist_del(&key->hlist);
 225         hlist_del(&key->sklist);
 226         spin_unlock_irqrestore(&net->mctp.keys_lock, flags);
 227
 228         /* one unref for the lists */
 229         mctp_key_unref(key);
 230
 231         /* and one for the local reference */
 232         mctp_key_unref(key);
 233
 234         if (skb)
 235                 kfree_skb(skb);
 236
 237 }
 238
 239 #ifdef CONFIG_MCTP_FLOWS
 240 static void mctp_skb_set_flow(struct sk_buff *skb, struct mctp_sk_key *key)
 241 {
 242         struct mctp_flow *flow;
 243
 244         flow = skb_ext_add(skb, SKB_EXT_MCTP);
 245         if (!flow)
 246                 return;
 247
 248         refcount_inc(&key->refs);
 249         flow->key = key;
 250 }
 251
 252 static void mctp_flow_prepare_output(struct sk_buff *skb, struct mctp_dev *dev)
 253 {
 254         struct mctp_sk_key *key;
 255         struct mctp_flow *flow;
 256
 257         flow = skb_ext_find(skb, SKB_EXT_MCTP);
 258         if (!flow)
 259                 return;
 260
 261         key = flow->key;
 262
 263         if (WARN_ON(key->dev && key->dev != dev))
 264                 return;
 265
 266         mctp_dev_set_key(dev, key);
 267 }
 268 #else
 269 static void mctp_skb_set_flow(struct sk_buff *skb, struct mctp_sk_key *key) {}
 270 static void mctp_flow_prepare_output(struct sk_buff *skb, struct mctp_dev *dev) {}
 271 #endif
 272
 273 static int mctp_frag_queue(struct mctp_sk_key *key, struct sk_buff *skb)
 274 {
 275         struct mctp_hdr *hdr = mctp_hdr(skb);
 276         u8 exp_seq, this_seq;
 277
 278         this_seq = (hdr->flags_seq_tag >> MCTP_HDR_SEQ_SHIFT)
 279                 & MCTP_HDR_SEQ_MASK;
 280
 281         if (!key->reasm_head) {
 282                 key->reasm_head = skb;
 283                 key->reasm_tailp = &(skb_shinfo(skb)->frag_list);
 284                 key->last_seq = this_seq;
 285                 return 0;
 286         }
 287
 288         exp_seq = (key->last_seq + 1) & MCTP_HDR_SEQ_MASK;
 289
 290         if (this_seq != exp_seq)
 291                 return -EINVAL;
 292
 293         if (key->reasm_head->len + skb->len > mctp_message_maxlen)
 294                 return -EINVAL;
 295
 296         skb->next = NULL;
 297         skb->sk = NULL;
 298         *key->reasm_tailp = skb;
 299         key->reasm_tailp = &skb->next;
 300
 301         key->last_seq = this_seq;
 302
 303         key->reasm_head->data_len += skb->len;
 304         key->reasm_head->len += skb->len;
 305         key->reasm_head->truesize += skb->truesize;
 306
 307         return 0;
 308 }
 309
 310 static int mctp_route_input(struct mctp_route *route, struct sk_buff *skb)
 311 {
 312         struct net *net = dev_net(skb->dev);
 313         struct mctp_sk_key *key;
 314         struct mctp_sock *msk;
 315         struct mctp_hdr *mh;
 316         unsigned long f;
 317         u8 tag, flags;
 318         int rc;
 319
 320         msk = NULL;
 321         rc = -EINVAL;
 322
 323         /* we may be receiving a locally-routed packet; drop source sk
 324          * accounting
 325          */
 326         skb_orphan(skb);
 327
 328         /* ensure we have enough data for a header and a type */
 329         if (skb->len < sizeof(struct mctp_hdr) + 1)
 330                 goto out;
 331
 332         /* grab header, advance data ptr */
 333         mh = mctp_hdr(skb);
 334         skb_pull(skb, sizeof(struct mctp_hdr));
 335
 336         if (mh->ver != 1)
 337                 goto out;
 338
 339         flags = mh->flags_seq_tag & (MCTP_HDR_FLAG_SOM | MCTP_HDR_FLAG_EOM);
 340         tag = mh->flags_seq_tag & (MCTP_HDR_TAG_MASK | MCTP_HDR_FLAG_TO);
 341
 342         rcu_read_lock();
 343
 344         /* lookup socket / reasm context, exactly matching (src,dest,tag).
 345          * we hold a ref on the key, and key->lock held.
 346          */
 347         key = mctp_lookup_key(net, skb, mh->src, &f);
 348
 349         if (flags & MCTP_HDR_FLAG_SOM) {
 350                 if (key) {
 351                         msk = container_of(key->sk, struct mctp_sock, sk);
 352                 } else {
 353                         /* first response to a broadcast? do a more general
 354                          * key lookup to find the socket, but don't use this
 355                          * key for reassembly - we'll create a more specific
 356                          * one for future packets if required (ie, !EOM).
 357                          */
 358                         key = mctp_lookup_key(net, skb, MCTP_ADDR_ANY, &f);
 359                         if (key) {
 360                                 msk = container_of(key->sk,
 361                                                    struct mctp_sock, sk);
 362                                 spin_unlock_irqrestore(&key->lock, f);
 363                                 mctp_key_unref(key);
 364                                 key = NULL;
 365                         }
 366                 }
 367
 368                 if (!key && !msk && (tag & MCTP_HDR_FLAG_TO))
 369                         msk = mctp_lookup_bind(net, skb);
 370
 371                 if (!msk) {
 372                         rc = -ENOENT;
 373                         goto out_unlock;
 374                 }
 375
 376                 /* single-packet message? deliver to socket, clean up any
 377                  * pending key.
 378                  */
 379                 if (flags & MCTP_HDR_FLAG_EOM) {
 380                         sock_queue_rcv_skb(&msk->sk, skb);
 381                         if (key) {
 382                                 /* we've hit a pending reassembly; not much we
 383                                  * can do but drop it
 384                                  */
 385                                 trace_mctp_key_release(key,
 386                                                        MCTP_TRACE_KEY_REPLIED);
 387                                 __mctp_key_unlock_drop(key, net, f);
 388                                 key = NULL;
 389                         }
 390                         rc = 0;
 391                         goto out_unlock;
 392                 }
 393
 394                 /* broadcast response or a bind() - create a key for further
 395                  * packets for this message
 396                  */
 397                 if (!key) {
 398                         key = mctp_key_alloc(msk, mh->dest, mh->src,
 399                                              tag, GFP_ATOMIC);
 400                         if (!key) {
 401                                 rc = -ENOMEM;
 402                                 goto out_unlock;
 403                         }
 404
 405                         /* we can queue without the key lock here, as the
 406                          * key isn't observable yet
 407                          */
 408                         mctp_frag_queue(key, skb);
 409
 410                         /* if the key_add fails, we've raced with another
 411                          * SOM packet with the same src, dest and tag. There's
 412                          * no way to distinguish future packets, so all we
 413                          * can do is drop; we'll free the skb on exit from
 414                          * this function.
 415                          */
 416                         rc = mctp_key_add(key, msk);
 417                         if (rc)
 418                                 kfree(key);
 419
 420                         trace_mctp_key_acquire(key);
 421
 422                         /* we don't need to release key->lock on exit */
 423                         mctp_key_unref(key);
 424                         key = NULL;
 425
 426                 } else {
 427                         if (key->reasm_head || key->reasm_dead) {
 428                                 /* duplicate start? drop everything */
 429                                 trace_mctp_key_release(key,
 430                                                        MCTP_TRACE_KEY_INVALIDATED);
 431                                 __mctp_key_unlock_drop(key, net, f);
 432                                 rc = -EEXIST;
 433                                 key = NULL;
 434                         } else {
 435                                 rc = mctp_frag_queue(key, skb);
 436                         }
 437                 }
 438
 439         } else if (key) {
 440                 /* this packet continues a previous message; reassemble
 441                  * using the message-specific key
 442                  */
 443
 444                 /* we need to be continuing an existing reassembly... */
 445                 if (!key->reasm_head)
 446                         rc = -EINVAL;
 447                 else
 448                         rc = mctp_frag_queue(key, skb);
 449
 450                 /* end of message? deliver to socket, and we're done with
 451                  * the reassembly/response key
 452                  */
 453                 if (!rc && flags & MCTP_HDR_FLAG_EOM) {
 454                         sock_queue_rcv_skb(key->sk, key->reasm_head);
 455                         key->reasm_head = NULL;
 456                         trace_mctp_key_release(key, MCTP_TRACE_KEY_REPLIED);
 457                         __mctp_key_unlock_drop(key, net, f);
 458                         key = NULL;
 459                 }
 460
 461         } else {
 462                 /* not a start, no matching key */
 463                 rc = -ENOENT;
 464         }
 465
 466 out_unlock:
 467         rcu_read_unlock();
 468         if (key) {
 469                 spin_unlock_irqrestore(&key->lock, f);
 470                 mctp_key_unref(key);
 471         }
 472 out:
 473         if (rc)
 474                 kfree_skb(skb);
 475         return rc;
 476 }
 477
 478 static unsigned int mctp_route_mtu(struct mctp_route *rt)
 479 {
 480         return rt->mtu ?: READ_ONCE(rt->dev->dev->mtu);
 481 }
 482
 483 static int mctp_route_output(struct mctp_route *route, struct sk_buff *skb)
 484 {
 485         struct mctp_skb_cb *cb = mctp_cb(skb);
 486         struct mctp_hdr *hdr = mctp_hdr(skb);
 487         char daddr_buf[MAX_ADDR_LEN];
 488         char *daddr = NULL;
 489         unsigned int mtu;
 490         int rc;
 491
 492         skb->protocol = htons(ETH_P_MCTP);
 493
 494         mtu = READ_ONCE(skb->dev->mtu);
 495         if (skb->len > mtu) {
 496                 kfree_skb(skb);
 497                 return -EMSGSIZE;
 498         }
 499
 500         if (cb->ifindex) {
 501                 /* direct route; use the hwaddr we stashed in sendmsg */
 502                 daddr = cb->haddr;
 503         } else {
 504                 /* If lookup fails let the device handle daddr==NULL */
 505                 if (mctp_neigh_lookup(route->dev, hdr->dest, daddr_buf) == 0)
 506                         daddr = daddr_buf;
 507         }
 508
 509         rc = dev_hard_header(skb, skb->dev, ntohs(skb->protocol),
 510                              daddr, skb->dev->dev_addr, skb->len);
 511         if (rc) {
 512                 kfree_skb(skb);
 513                 return -EHOSTUNREACH;
 514         }
 515
 516         mctp_flow_prepare_output(skb, route->dev);
 517
 518         rc = dev_queue_xmit(skb);
 519         if (rc)
 520                 rc = net_xmit_errno(rc);
 521
 522         return rc;
 523 }
 524
 525 /* route alloc/release */
 526 static void mctp_route_release(struct mctp_route *rt)
 527 {
 528         if (refcount_dec_and_test(&rt->refs)) {
 529                 mctp_dev_put(rt->dev);
 530                 kfree_rcu(rt, rcu);
 531         }
 532 }
 533
 534 /* returns a route with the refcount at 1 */
 535 static struct mctp_route *mctp_route_alloc(void)
 536 {
 537         struct mctp_route *rt;
 538
 539         rt = kzalloc(sizeof(*rt), GFP_KERNEL);
 540         if (!rt)
 541                 return NULL;
 542
 543         INIT_LIST_HEAD(&rt->list);
 544         refcount_set(&rt->refs, 1);
 545         rt->output = mctp_route_discard;
 546
 547         return rt;
 548 }
 549
 550 unsigned int mctp_default_net(struct net *net)
 551 {
 552         return READ_ONCE(net->mctp.default_net);
 553 }
 554
 555 int mctp_default_net_set(struct net *net, unsigned int index)
 556 {
 557         if (index == 0)
 558                 return -EINVAL;
 559         WRITE_ONCE(net->mctp.default_net, index);
 560         return 0;
 561 }
 562
 563 /* tag management */
 564 static void mctp_reserve_tag(struct net *net, struct mctp_sk_key *key,
 565                              struct mctp_sock *msk)
 566 {
 567         struct netns_mctp *mns = &net->mctp;
 568
 569         lockdep_assert_held(&mns->keys_lock);
 570
 571         key->expiry = jiffies + mctp_key_lifetime;
 572         timer_reduce(&msk->key_expiry, key->expiry);
 573
 574         /* we hold the net->key_lock here, allowing updates to both
 575          * then net and sk
 576          */
 577         hlist_add_head_rcu(&key->hlist, &mns->keys);
 578         hlist_add_head_rcu(&key->sklist, &msk->keys);
 579         refcount_inc(&key->refs);
 580 }
 581
 582 /* Allocate a locally-owned tag value for (saddr, daddr), and reserve
 583  * it for the socket msk
 584  */
 585 static struct mctp_sk_key *mctp_alloc_local_tag(struct mctp_sock *msk,
 586                                                 mctp_eid_t saddr,
 587                                                 mctp_eid_t daddr, u8 *tagp)
 588 {
 589         struct net *net = sock_net(&msk->sk);
 590         struct netns_mctp *mns = &net->mctp;
 591         struct mctp_sk_key *key, *tmp;
 592         unsigned long flags;
 593         u8 tagbits;
 594
 595         /* for NULL destination EIDs, we may get a response from any peer */
 596         if (daddr == MCTP_ADDR_NULL)
 597                 daddr = MCTP_ADDR_ANY;
 598
 599         /* be optimistic, alloc now */
 600         key = mctp_key_alloc(msk, saddr, daddr, 0, GFP_KERNEL);
 601         if (!key)
 602                 return ERR_PTR(-ENOMEM);
 603
 604         /* 8 possible tag values */
 605         tagbits = 0xff;
 606
 607         spin_lock_irqsave(&mns->keys_lock, flags);
 608
 609         /* Walk through the existing keys, looking for potential conflicting
 610          * tags. If we find a conflict, clear that bit from tagbits
 611          */
 612         hlist_for_each_entry(tmp, &mns->keys, hlist) {
 613                 /* We can check the lookup fields (*_addr, tag) without the
 614                  * lock held, they don't change over the lifetime of the key.
 615                  */
 616
 617                 /* if we don't own the tag, it can't conflict */
 618                 if (tmp->tag & MCTP_HDR_FLAG_TO)
 619                         continue;
 620
 621                 if (!((tmp->peer_addr == daddr ||
 622                        tmp->peer_addr == MCTP_ADDR_ANY) &&
 623                        tmp->local_addr == saddr))
 624                         continue;
 625
 626                 spin_lock(&tmp->lock);
 627                 /* key must still be valid. If we find a match, clear the
 628                  * potential tag value
 629                  */
 630                 if (tmp->valid)
 631                         tagbits &= ~(1 << tmp->tag);
 632                 spin_unlock(&tmp->lock);
 633
 634                 if (!tagbits)
 635                         break;
 636         }
 637
 638         if (tagbits) {
 639                 key->tag = __ffs(tagbits);
 640                 mctp_reserve_tag(net, key, msk);
 641                 trace_mctp_key_acquire(key);
 642
 643                 *tagp = key->tag;
 644         }
 645
 646         spin_unlock_irqrestore(&mns->keys_lock, flags);
 647
 648         if (!tagbits) {
 649                 kfree(key);
 650                 return ERR_PTR(-EBUSY);
 651         }
 652
 653         return key;
 654 }
 655
 656 /* routing lookups */
 657 static bool mctp_rt_match_eid(struct mctp_route *rt,
 658                               unsigned int net, mctp_eid_t eid)
 659 {
 660         return READ_ONCE(rt->dev->net) == net &&
 661                 rt->min <= eid && rt->max >= eid;
 662 }
 663
 664 /* compares match, used for duplicate prevention */
 665 static bool mctp_rt_compare_exact(struct mctp_route *rt1,
 666                                   struct mctp_route *rt2)
 667 {
 668         ASSERT_RTNL();
 669         return rt1->dev->net == rt2->dev->net &&
 670                 rt1->min == rt2->min &&
 671                 rt1->max == rt2->max;
 672 }
 673
 674 struct mctp_route *mctp_route_lookup(struct net *net, unsigned int dnet,
 675                                      mctp_eid_t daddr)
 676 {
 677         struct mctp_route *tmp, *rt = NULL;
 678
 679         list_for_each_entry_rcu(tmp, &net->mctp.routes, list) {
 680                 /* TODO: add metrics */
 681                 if (mctp_rt_match_eid(tmp, dnet, daddr)) {
 682                         if (refcount_inc_not_zero(&tmp->refs)) {
 683                                 rt = tmp;
 684                                 break;
 685                         }
 686                 }
 687         }
 688
 689         return rt;
 690 }
 691
 692 static struct mctp_route *mctp_route_lookup_null(struct net *net,
 693                                                  struct net_device *dev)
 694 {
 695         struct mctp_route *rt;
 696
 697         list_for_each_entry_rcu(rt, &net->mctp.routes, list) {
 698                 if (rt->dev->dev == dev && rt->type == RTN_LOCAL &&
 699                     refcount_inc_not_zero(&rt->refs))
 700                         return rt;
 701         }
 702
 703         return NULL;
 704 }
 705
 706 static int mctp_do_fragment_route(struct mctp_route *rt, struct sk_buff *skb,
 707                                   unsigned int mtu, u8 tag)
 708 {
 709         const unsigned int hlen = sizeof(struct mctp_hdr);
 710         struct mctp_hdr *hdr, *hdr2;
 711         unsigned int pos, size;
 712         struct sk_buff *skb2;
 713         int rc;
 714         u8 seq;
 715
 716         hdr = mctp_hdr(skb);
 717         seq = 0;
 718         rc = 0;
 719
 720         if (mtu < hlen + 1) {
 721                 kfree_skb(skb);
 722                 return -EMSGSIZE;
 723         }
 724
 725         /* we've got the header */
 726         skb_pull(skb, hlen);
 727
 728         for (pos = 0; pos < skb->len;) {
 729                 /* size of message payload */
 730                 size = min(mtu - hlen, skb->len - pos);
 731
 732                 skb2 = alloc_skb(MCTP_HEADER_MAXLEN + hlen + size, GFP_KERNEL);
 733                 if (!skb2) {
 734                         rc = -ENOMEM;
 735                         break;
 736                 }
 737
 738                 /* generic skb copy */
 739                 skb2->protocol = skb->protocol;
 740                 skb2->priority = skb->priority;
 741                 skb2->dev = skb->dev;
 742                 memcpy(skb2->cb, skb->cb, sizeof(skb2->cb));
 743
 744                 if (skb->sk)
 745                         skb_set_owner_w(skb2, skb->sk);
 746
 747                 /* establish packet */
 748                 skb_reserve(skb2, MCTP_HEADER_MAXLEN);
 749                 skb_reset_network_header(skb2);
 750                 skb_put(skb2, hlen + size);
 751                 skb2->transport_header = skb2->network_header + hlen;
 752
 753                 /* copy header fields, calculate SOM/EOM flags & seq */
 754                 hdr2 = mctp_hdr(skb2);
 755                 hdr2->ver = hdr->ver;
 756                 hdr2->dest = hdr->dest;
 757                 hdr2->src = hdr->src;
 758                 hdr2->flags_seq_tag = tag &
 759                         (MCTP_HDR_TAG_MASK | MCTP_HDR_FLAG_TO);
 760
 761                 if (pos == 0)
 762                         hdr2->flags_seq_tag |= MCTP_HDR_FLAG_SOM;
 763
 764                 if (pos + size == skb->len)
 765                         hdr2->flags_seq_tag |= MCTP_HDR_FLAG_EOM;
 766
 767                 hdr2->flags_seq_tag |= seq << MCTP_HDR_SEQ_SHIFT;
 768
 769                 /* copy message payload */
 770                 skb_copy_bits(skb, pos, skb_transport_header(skb2), size);
 771
 772                 /* do route */
 773                 rc = rt->output(rt, skb2);
 774                 if (rc)
 775                         break;
 776
 777                 seq = (seq + 1) & MCTP_HDR_SEQ_MASK;
 778                 pos += size;
 779         }
 780
 781         consume_skb(skb);
 782         return rc;
 783 }
 784
 785 int mctp_local_output(struct sock *sk, struct mctp_route *rt,
 786                       struct sk_buff *skb, mctp_eid_t daddr, u8 req_tag)
 787 {
 788         struct mctp_sock *msk = container_of(sk, struct mctp_sock, sk);
 789         struct mctp_skb_cb *cb = mctp_cb(skb);
 790         struct mctp_route tmp_rt;
 791         struct mctp_sk_key *key;
 792         struct net_device *dev;
 793         struct mctp_hdr *hdr;
 794         unsigned long flags;
 795         unsigned int mtu;
 796         mctp_eid_t saddr;
 797         bool ext_rt;
 798         int rc;
 799         u8 tag;
 800
 801         rc = -ENODEV;
 802
 803         if (rt) {
 804                 ext_rt = false;
 805                 dev = NULL;
 806
 807                 if (WARN_ON(!rt->dev))
 808                         goto out_release;
 809
 810         } else if (cb->ifindex) {
 811                 ext_rt = true;
 812                 rt = &tmp_rt;
 813
 814                 rcu_read_lock();
 815                 dev = dev_get_by_index_rcu(sock_net(sk), cb->ifindex);
 816                 if (!dev) {
 817                         rcu_read_unlock();
 818                         return rc;
 819                 }
 820
 821                 rt->dev = __mctp_dev_get(dev);
 822                 rcu_read_unlock();
 823
 824                 if (!rt->dev)
 825                         goto out_release;
 826
 827                 /* establish temporary route - we set up enough to keep
 828                  * mctp_route_output happy
 829                  */
 830                 rt->output = mctp_route_output;
 831                 rt->mtu = 0;
 832
 833         } else {
 834                 return -EINVAL;
 835         }
 836
 837         spin_lock_irqsave(&rt->dev->addrs_lock, flags);
 838         if (rt->dev->num_addrs == 0) {
 839                 rc = -EHOSTUNREACH;
 840         } else {
 841                 /* use the outbound interface's first address as our source */
 842                 saddr = rt->dev->addrs[0];
 843                 rc = 0;
 844         }
 845         spin_unlock_irqrestore(&rt->dev->addrs_lock, flags);
 846
 847         if (rc)
 848                 goto out_release;
 849
 850         if (req_tag & MCTP_HDR_FLAG_TO) {
 851                 key = mctp_alloc_local_tag(msk, saddr, daddr, &tag);
 852                 if (IS_ERR(key)) {
 853                         rc = PTR_ERR(key);
 854                         goto out_release;
 855                 }
 856                 mctp_skb_set_flow(skb, key);
 857                 /* done with the key in this scope */
 858                 mctp_key_unref(key);
 859                 tag |= MCTP_HDR_FLAG_TO;
 860         } else {
 861                 key = NULL;
 862                 tag = req_tag;
 863         }
 864
 865         skb->protocol = htons(ETH_P_MCTP);
 866         skb->priority = 0;
 867         skb_reset_transport_header(skb);
 868         skb_push(skb, sizeof(struct mctp_hdr));
 869         skb_reset_network_header(skb);
 870         skb->dev = rt->dev->dev;
 871
 872         /* cb->net will have been set on initial ingress */
 873         cb->src = saddr;
 874
 875         /* set up common header fields */
 876         hdr = mctp_hdr(skb);
 877         hdr->ver = 1;
 878         hdr->dest = daddr;
 879         hdr->src = saddr;
 880
 881         mtu = mctp_route_mtu(rt);
 882
 883         if (skb->len + sizeof(struct mctp_hdr) <= mtu) {
 884                 hdr->flags_seq_tag = MCTP_HDR_FLAG_SOM |
 885                         MCTP_HDR_FLAG_EOM | tag;
 886                 rc = rt->output(rt, skb);
 887         } else {
 888                 rc = mctp_do_fragment_route(rt, skb, mtu, tag);
 889         }
 890
 891 out_release:
 892         if (!ext_rt)
 893                 mctp_route_release(rt);
 894
 895         if (dev)
 896                 dev_put(dev);
 897
 898         return rc;
 899
 900 }
 901
 902 /* route management */
 903 static int mctp_route_add(struct mctp_dev *mdev, mctp_eid_t daddr_start,
 904                           unsigned int daddr_extent, unsigned int mtu,
 905                           unsigned char type)
 906 {
 907         int (*rtfn)(struct mctp_route *rt, struct sk_buff *skb);
 908         struct net *net = dev_net(mdev->dev);
 909         struct mctp_route *rt, *ert;
 910
 911         if (!mctp_address_ok(daddr_start))
 912                 return -EINVAL;
 913
 914         if (daddr_extent > 0xff || daddr_start + daddr_extent >= 255)
 915                 return -EINVAL;
 916
 917         switch (type) {
 918         case RTN_LOCAL:
 919                 rtfn = mctp_route_input;
 920                 break;
 921         case RTN_UNICAST:
 922                 rtfn = mctp_route_output;
 923                 break;
 924         default:
 925                 return -EINVAL;
 926         }
 927
 928         rt = mctp_route_alloc();
 929         if (!rt)
 930                 return -ENOMEM;
 931
 932         rt->min = daddr_start;
 933         rt->max = daddr_start + daddr_extent;
 934         rt->mtu = mtu;
 935         rt->dev = mdev;
 936         mctp_dev_hold(rt->dev);
 937         rt->type = type;
 938         rt->output = rtfn;
 939
 940         ASSERT_RTNL();
 941         /* Prevent duplicate identical routes. */
 942         list_for_each_entry(ert, &net->mctp.routes, list) {
 943                 if (mctp_rt_compare_exact(rt, ert)) {
 944                         mctp_route_release(rt);
 945                         return -EEXIST;
 946                 }
 947         }
 948
 949         list_add_rcu(&rt->list, &net->mctp.routes);
 950
 951         return 0;
 952 }
 953
 954 static int mctp_route_remove(struct mctp_dev *mdev, mctp_eid_t daddr_start,
 955                              unsigned int daddr_extent, unsigned char type)
 956 {
 957         struct net *net = dev_net(mdev->dev);
 958         struct mctp_route *rt, *tmp;
 959         mctp_eid_t daddr_end;
 960         bool dropped;
 961
 962         if (daddr_extent > 0xff || daddr_start + daddr_extent >= 255)
 963                 return -EINVAL;
 964
 965         daddr_end = daddr_start + daddr_extent;
 966         dropped = false;
 967
 968         ASSERT_RTNL();
 969
 970         list_for_each_entry_safe(rt, tmp, &net->mctp.routes, list) {
 971                 if (rt->dev == mdev &&
 972                     rt->min == daddr_start && rt->max == daddr_end &&
 973                     rt->type == type) {
 974                         list_del_rcu(&rt->list);
 975                         /* TODO: immediate RTM_DELROUTE */
 976                         mctp_route_release(rt);
 977                         dropped = true;
 978                 }
 979         }
 980
 981         return dropped ? 0 : -ENOENT;
 982 }
 983
 984 int mctp_route_add_local(struct mctp_dev *mdev, mctp_eid_t addr)
 985 {
 986         return mctp_route_add(mdev, addr, 0, 0, RTN_LOCAL);
 987 }
 988
 989 int mctp_route_remove_local(struct mctp_dev *mdev, mctp_eid_t addr)
 990 {
 991         return mctp_route_remove(mdev, addr, 0, RTN_LOCAL);
 992 }
 993
 994 /* removes all entries for a given device */
 995 void mctp_route_remove_dev(struct mctp_dev *mdev)
 996 {
 997         struct net *net = dev_net(mdev->dev);
 998         struct mctp_route *rt, *tmp;
 999
1000         ASSERT_RTNL();
1001         list_for_each_entry_safe(rt, tmp, &net->mctp.routes, list) {
1002                 if (rt->dev == mdev) {
1003                         list_del_rcu(&rt->list);
1004                         /* TODO: immediate RTM_DELROUTE */
1005                         mctp_route_release(rt);
1006                 }
1007         }
1008 }
1009
1010 /* Incoming packet-handling */
1011
1012 static int mctp_pkttype_receive(struct sk_buff *skb, struct net_device *dev,
1013                                 struct packet_type *pt,
1014                                 struct net_device *orig_dev)
1015 {
1016         struct net *net = dev_net(dev);
1017         struct mctp_dev *mdev;
1018         struct mctp_skb_cb *cb;
1019         struct mctp_route *rt;
1020         struct mctp_hdr *mh;
1021
1022         rcu_read_lock();
1023         mdev = __mctp_dev_get(dev);
1024         rcu_read_unlock();
1025         if (!mdev) {
1026                 /* basic non-data sanity checks */
1027                 goto err_drop;
1028         }
1029
1030         if (!pskb_may_pull(skb, sizeof(struct mctp_hdr)))
1031                 goto err_drop;
1032
1033         skb_reset_transport_header(skb);
1034         skb_reset_network_header(skb);
1035
1036         /* We have enough for a header; decode and route */
1037         mh = mctp_hdr(skb);
1038         if (mh->ver < MCTP_VER_MIN || mh->ver > MCTP_VER_MAX)
1039                 goto err_drop;
1040
1041         /* MCTP drivers must populate halen/haddr */
1042         if (dev->type == ARPHRD_MCTP) {
1043                 cb = mctp_cb(skb);
1044         } else {
1045                 cb = __mctp_cb(skb);
1046                 cb->halen = 0;
1047         }
1048         cb->net = READ_ONCE(mdev->net);
1049         cb->ifindex = dev->ifindex;
1050
1051         rt = mctp_route_lookup(net, cb->net, mh->dest);
1052
1053         /* NULL EID, but addressed to our physical address */
1054         if (!rt && mh->dest == MCTP_ADDR_NULL && skb->pkt_type == PACKET_HOST)
1055                 rt = mctp_route_lookup_null(net, dev);
1056
1057         if (!rt)
1058                 goto err_drop;
1059
1060         rt->output(rt, skb);
1061         mctp_route_release(rt);
1062
1063         return NET_RX_SUCCESS;
1064
1065 err_drop:
1066         kfree_skb(skb);
1067         return NET_RX_DROP;
1068 }
1069
1070 static struct packet_type mctp_packet_type = {
1071         .type = cpu_to_be16(ETH_P_MCTP),
1072         .func = mctp_pkttype_receive,
1073 };
1074
1075 /* netlink interface */
1076
1077 static const struct nla_policy rta_mctp_policy[RTA_MAX + 1] = {
1078         [RTA_DST]               = { .type = NLA_U8 },
1079         [RTA_METRICS]           = { .type = NLA_NESTED },
1080         [RTA_OIF]               = { .type = NLA_U32 },
1081 };
1082
1083 /* Common part for RTM_NEWROUTE and RTM_DELROUTE parsing.
1084  * tb must hold RTA_MAX+1 elements.
1085  */
1086 static int mctp_route_nlparse(struct sk_buff *skb, struct nlmsghdr *nlh,
1087                               struct netlink_ext_ack *extack,
1088                               struct nlattr **tb, struct rtmsg **rtm,
1089                               struct mctp_dev **mdev, mctp_eid_t *daddr_start)
1090 {
1091         struct net *net = sock_net(skb->sk);
1092         struct net_device *dev;
1093         unsigned int ifindex;
1094         int rc;
1095
1096         rc = nlmsg_parse(nlh, sizeof(struct rtmsg), tb, RTA_MAX,
1097                          rta_mctp_policy, extack);
1098         if (rc < 0) {
1099                 NL_SET_ERR_MSG(extack, "incorrect format");
1100                 return rc;
1101         }
1102
1103         if (!tb[RTA_DST]) {
1104                 NL_SET_ERR_MSG(extack, "dst EID missing");
1105                 return -EINVAL;
1106         }
1107         *daddr_start = nla_get_u8(tb[RTA_DST]);
1108
1109         if (!tb[RTA_OIF]) {
1110                 NL_SET_ERR_MSG(extack, "ifindex missing");
1111                 return -EINVAL;
1112         }
1113         ifindex = nla_get_u32(tb[RTA_OIF]);
1114
1115         *rtm = nlmsg_data(nlh);
1116         if ((*rtm)->rtm_family != AF_MCTP) {
1117                 NL_SET_ERR_MSG(extack, "route family must be AF_MCTP");
1118                 return -EINVAL;
1119         }
1120
1121         dev = __dev_get_by_index(net, ifindex);
1122         if (!dev) {
1123                 NL_SET_ERR_MSG(extack, "bad ifindex");
1124                 return -ENODEV;
1125         }
1126         *mdev = mctp_dev_get_rtnl(dev);
1127         if (!*mdev)
1128                 return -ENODEV;
1129
1130         if (dev->flags & IFF_LOOPBACK) {
1131                 NL_SET_ERR_MSG(extack, "no routes to loopback");
1132                 return -EINVAL;
1133         }
1134
1135         return 0;
1136 }
1137
1138 static const struct nla_policy rta_metrics_policy[RTAX_MAX + 1] = {
1139         [RTAX_MTU]              = { .type = NLA_U32 },
1140 };
1141
1142 static int mctp_newroute(struct sk_buff *skb, struct nlmsghdr *nlh,
1143                          struct netlink_ext_ack *extack)
1144 {
1145         struct nlattr *tb[RTA_MAX + 1];
1146         struct nlattr *tbx[RTAX_MAX + 1];
1147         mctp_eid_t daddr_start;
1148         struct mctp_dev *mdev;
1149         struct rtmsg *rtm;
1150         unsigned int mtu;
1151         int rc;
1152
1153         rc = mctp_route_nlparse(skb, nlh, extack, tb,
1154                                 &rtm, &mdev, &daddr_start);
1155         if (rc < 0)
1156                 return rc;
1157
1158         if (rtm->rtm_type != RTN_UNICAST) {
1159                 NL_SET_ERR_MSG(extack, "rtm_type must be RTN_UNICAST");
1160                 return -EINVAL;
1161         }
1162
1163         mtu = 0;
1164         if (tb[RTA_METRICS]) {
1165                 rc = nla_parse_nested(tbx, RTAX_MAX, tb[RTA_METRICS],
1166                                       rta_metrics_policy, NULL);
1167                 if (rc < 0)
1168                         return rc;
1169                 if (tbx[RTAX_MTU])
1170                         mtu = nla_get_u32(tbx[RTAX_MTU]);
1171         }
1172
1173         if (rtm->rtm_type != RTN_UNICAST)
1174                 return -EINVAL;
1175
1176         rc = mctp_route_add(mdev, daddr_start, rtm->rtm_dst_len, mtu,
1177                             rtm->rtm_type);
1178         return rc;
1179 }
1180
1181 static int mctp_delroute(struct sk_buff *skb, struct nlmsghdr *nlh,
1182                          struct netlink_ext_ack *extack)
1183 {
1184         struct nlattr *tb[RTA_MAX + 1];
1185         mctp_eid_t daddr_start;
1186         struct mctp_dev *mdev;
1187         struct rtmsg *rtm;
1188         int rc;
1189
1190         rc = mctp_route_nlparse(skb, nlh, extack, tb,
1191                                 &rtm, &mdev, &daddr_start);
1192         if (rc < 0)
1193                 return rc;
1194
1195         /* we only have unicast routes */
1196         if (rtm->rtm_type != RTN_UNICAST)
1197                 return -EINVAL;
1198
1199         rc = mctp_route_remove(mdev, daddr_start, rtm->rtm_dst_len, RTN_UNICAST);
1200         return rc;
1201 }
1202
1203 static int mctp_fill_rtinfo(struct sk_buff *skb, struct mctp_route *rt,
1204                             u32 portid, u32 seq, int event, unsigned int flags)
1205 {
1206         struct nlmsghdr *nlh;
1207         struct rtmsg *hdr;
1208         void *metrics;
1209
1210         nlh = nlmsg_put(skb, portid, seq, event, sizeof(*hdr), flags);
1211         if (!nlh)
1212                 return -EMSGSIZE;
1213
1214         hdr = nlmsg_data(nlh);
1215         hdr->rtm_family = AF_MCTP;
1216
1217         /* we use the _len fields as a number of EIDs, rather than
1218          * a number of bits in the address
1219          */
1220         hdr->rtm_dst_len = rt->max - rt->min;
1221         hdr->rtm_src_len = 0;
1222         hdr->rtm_tos = 0;
1223         hdr->rtm_table = RT_TABLE_DEFAULT;
1224         hdr->rtm_protocol = RTPROT_STATIC; /* everything is user-defined */
1225         hdr->rtm_scope = RT_SCOPE_LINK; /* TODO: scope in mctp_route? */
1226         hdr->rtm_type = rt->type;
1227
1228         if (nla_put_u8(skb, RTA_DST, rt->min))
1229                 goto cancel;
1230
1231         metrics = nla_nest_start_noflag(skb, RTA_METRICS);
1232         if (!metrics)
1233                 goto cancel;
1234
1235         if (rt->mtu) {
1236                 if (nla_put_u32(skb, RTAX_MTU, rt->mtu))
1237                         goto cancel;
1238         }
1239
1240         nla_nest_end(skb, metrics);
1241
1242         if (rt->dev) {
1243                 if (nla_put_u32(skb, RTA_OIF, rt->dev->dev->ifindex))
1244                         goto cancel;
1245         }
1246
1247         /* TODO: conditional neighbour physaddr? */
1248
1249         nlmsg_end(skb, nlh);
1250
1251         return 0;
1252
1253 cancel:
1254         nlmsg_cancel(skb, nlh);
1255         return -EMSGSIZE;
1256 }
1257
1258 static int mctp_dump_rtinfo(struct sk_buff *skb, struct netlink_callback *cb)
1259 {
1260         struct net *net = sock_net(skb->sk);
1261         struct mctp_route *rt;
1262         int s_idx, idx;
1263
1264         /* TODO: allow filtering on route data, possibly under
1265          * cb->strict_check
1266          */
1267
1268         /* TODO: change to struct overlay */
1269         s_idx = cb->args[0];
1270         idx = 0;
1271
1272         rcu_read_lock();
1273         list_for_each_entry_rcu(rt, &net->mctp.routes, list) {
1274                 if (idx++ < s_idx)
1275                         continue;
1276                 if (mctp_fill_rtinfo(skb, rt,
1277                                      NETLINK_CB(cb->skb).portid,
1278                                      cb->nlh->nlmsg_seq,
1279                                      RTM_NEWROUTE, NLM_F_MULTI) < 0)
1280                         break;
1281         }
1282
1283         rcu_read_unlock();
1284         cb->args[0] = idx;
1285
1286         return skb->len;
1287 }
1288
1289 /* net namespace implementation */
1290 static int __net_init mctp_routes_net_init(struct net *net)
1291 {
1292         struct netns_mctp *ns = &net->mctp;
1293
1294         INIT_LIST_HEAD(&ns->routes);
1295         INIT_HLIST_HEAD(&ns->binds);
1296         mutex_init(&ns->bind_lock);
1297         INIT_HLIST_HEAD(&ns->keys);
1298         spin_lock_init(&ns->keys_lock);
1299         WARN_ON(mctp_default_net_set(net, MCTP_INITIAL_DEFAULT_NET));
1300         return 0;
1301 }
1302
1303 static void __net_exit mctp_routes_net_exit(struct net *net)
1304 {
1305         struct mctp_route *rt;
1306
1307         rcu_read_lock();
1308         list_for_each_entry_rcu(rt, &net->mctp.routes, list)
1309                 mctp_route_release(rt);
1310         rcu_read_unlock();
1311 }
1312
1313 static struct pernet_operations mctp_net_ops = {
1314         .init = mctp_routes_net_init,
1315         .exit = mctp_routes_net_exit,
1316 };
1317
1318 int __init mctp_routes_init(void)
1319 {
1320         dev_add_pack(&mctp_packet_type);
1321
1322         rtnl_register_module(THIS_MODULE, PF_MCTP, RTM_GETROUTE,
1323                              NULL, mctp_dump_rtinfo, 0);
1324         rtnl_register_module(THIS_MODULE, PF_MCTP, RTM_NEWROUTE,
1325                              mctp_newroute, NULL, 0);
1326         rtnl_register_module(THIS_MODULE, PF_MCTP, RTM_DELROUTE,
1327                              mctp_delroute, NULL, 0);
1328
1329         return register_pernet_subsys(&mctp_net_ops);
1330 }
1331
1332 void __exit mctp_routes_exit(void)
1333 {
1334         unregister_pernet_subsys(&mctp_net_ops);
1335         rtnl_unregister(PF_MCTP, RTM_DELROUTE);
1336         rtnl_unregister(PF_MCTP, RTM_NEWROUTE);
1337         rtnl_unregister(PF_MCTP, RTM_GETROUTE);
1338         dev_remove_pack(&mctp_packet_type);
1339 }
1340
1341 #if IS_ENABLED(CONFIG_MCTP_TEST)
1342 #include "test/route-test.c"
1343 #endif