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