net/mctp/device.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Management Component Transport Protocol (MCTP) - device implementation.
   4  *
   5  * Copyright (c) 2021 Code Construct
   6  * Copyright (c) 2021 Google
   7  */
   8
   9 #include <linux/if_link.h>
  10 #include <linux/mctp.h>
  11 #include <linux/netdevice.h>
  12 #include <linux/rcupdate.h>
  13 #include <linux/rtnetlink.h>
  14
  15 #include <net/addrconf.h>
  16 #include <net/netlink.h>
  17 #include <net/mctp.h>
  18 #include <net/mctpdevice.h>
  19 #include <net/sock.h>
  20
  21 struct mctp_dump_cb {
  22         int h;
  23         int idx;
  24         size_t a_idx;
  25 };
  26
  27 /* unlocked: caller must hold rcu_read_lock */
  28 struct mctp_dev *__mctp_dev_get(const struct net_device *dev)
  29 {
  30         return rcu_dereference(dev->mctp_ptr);
  31 }
  32
  33 struct mctp_dev *mctp_dev_get_rtnl(const struct net_device *dev)
  34 {
  35         return rtnl_dereference(dev->mctp_ptr);
  36 }
  37
  38 static void mctp_dev_destroy(struct mctp_dev *mdev)
  39 {
  40         struct net_device *dev = mdev->dev;
  41
  42         dev_put(dev);
  43         kfree_rcu(mdev, rcu);
  44 }
  45
  46 static int mctp_fill_addrinfo(struct sk_buff *skb, struct netlink_callback *cb,
  47                               struct mctp_dev *mdev, mctp_eid_t eid)
  48 {
  49         struct ifaddrmsg *hdr;
  50         struct nlmsghdr *nlh;
  51
  52         nlh = nlmsg_put(skb, NETLINK_CB(cb->skb).portid, cb->nlh->nlmsg_seq,
  53                         RTM_NEWADDR, sizeof(*hdr), NLM_F_MULTI);
  54         if (!nlh)
  55                 return -EMSGSIZE;
  56
  57         hdr = nlmsg_data(nlh);
  58         hdr->ifa_family = AF_MCTP;
  59         hdr->ifa_prefixlen = 0;
  60         hdr->ifa_flags = 0;
  61         hdr->ifa_scope = 0;
  62         hdr->ifa_index = mdev->dev->ifindex;
  63
  64         if (nla_put_u8(skb, IFA_LOCAL, eid))
  65                 goto cancel;
  66
  67         if (nla_put_u8(skb, IFA_ADDRESS, eid))
  68                 goto cancel;
  69
  70         nlmsg_end(skb, nlh);
  71
  72         return 0;
  73
  74 cancel:
  75         nlmsg_cancel(skb, nlh);
  76         return -EMSGSIZE;
  77 }
  78
  79 static int mctp_dump_dev_addrinfo(struct mctp_dev *mdev, struct sk_buff *skb,
  80                                   struct netlink_callback *cb)
  81 {
  82         struct mctp_dump_cb *mcb = (void *)cb->ctx;
  83         int rc = 0;
  84
  85         for (; mcb->a_idx < mdev->num_addrs; mcb->a_idx++) {
  86                 rc = mctp_fill_addrinfo(skb, cb, mdev, mdev->addrs[mcb->a_idx]);
  87                 if (rc < 0)
  88                         break;
  89         }
  90
  91         return rc;
  92 }
  93
  94 static int mctp_dump_addrinfo(struct sk_buff *skb, struct netlink_callback *cb)
  95 {
  96         struct mctp_dump_cb *mcb = (void *)cb->ctx;
  97         struct net *net = sock_net(skb->sk);
  98         struct hlist_head *head;
  99         struct net_device *dev;
 100         struct ifaddrmsg *hdr;
 101         struct mctp_dev *mdev;
 102         int ifindex;
 103         int idx, rc;
 104
 105         hdr = nlmsg_data(cb->nlh);
 106         // filter by ifindex if requested
 107         ifindex = hdr->ifa_index;
 108
 109         rcu_read_lock();
 110         for (; mcb->h < NETDEV_HASHENTRIES; mcb->h++, mcb->idx = 0) {
 111                 idx = 0;
 112                 head = &net->dev_index_head[mcb->h];
 113                 hlist_for_each_entry_rcu(dev, head, index_hlist) {
 114                         if (idx >= mcb->idx &&
 115                             (ifindex == 0 || ifindex == dev->ifindex)) {
 116                                 mdev = __mctp_dev_get(dev);
 117                                 if (mdev) {
 118                                         rc = mctp_dump_dev_addrinfo(mdev,
 119                                                                     skb, cb);
 120                                         // Error indicates full buffer, this
 121                                         // callback will get retried.
 122                                         if (rc < 0)
 123                                                 goto out;
 124                                 }
 125                         }
 126                         idx++;
 127                         // reset for next iteration
 128                         mcb->a_idx = 0;
 129                 }
 130         }
 131 out:
 132         rcu_read_unlock();
 133         mcb->idx = idx;
 134
 135         return skb->len;
 136 }
 137
 138 static const struct nla_policy ifa_mctp_policy[IFA_MAX + 1] = {
 139         [IFA_ADDRESS]           = { .type = NLA_U8 },
 140         [IFA_LOCAL]             = { .type = NLA_U8 },
 141 };
 142
 143 static int mctp_rtm_newaddr(struct sk_buff *skb, struct nlmsghdr *nlh,
 144                             struct netlink_ext_ack *extack)
 145 {
 146         struct net *net = sock_net(skb->sk);
 147         struct nlattr *tb[IFA_MAX + 1];
 148         struct net_device *dev;
 149         struct mctp_addr *addr;
 150         struct mctp_dev *mdev;
 151         struct ifaddrmsg *ifm;
 152         unsigned long flags;
 153         u8 *tmp_addrs;
 154         int rc;
 155
 156         rc = nlmsg_parse(nlh, sizeof(*ifm), tb, IFA_MAX, ifa_mctp_policy,
 157                          extack);
 158         if (rc < 0)
 159                 return rc;
 160
 161         ifm = nlmsg_data(nlh);
 162
 163         if (tb[IFA_LOCAL])
 164                 addr = nla_data(tb[IFA_LOCAL]);
 165         else if (tb[IFA_ADDRESS])
 166                 addr = nla_data(tb[IFA_ADDRESS]);
 167         else
 168                 return -EINVAL;
 169
 170         /* find device */
 171         dev = __dev_get_by_index(net, ifm->ifa_index);
 172         if (!dev)
 173                 return -ENODEV;
 174
 175         mdev = mctp_dev_get_rtnl(dev);
 176         if (!mdev)
 177                 return -ENODEV;
 178
 179         if (!mctp_address_ok(addr->s_addr))
 180                 return -EINVAL;
 181
 182         /* Prevent duplicates. Under RTNL so don't need to lock for reading */
 183         if (memchr(mdev->addrs, addr->s_addr, mdev->num_addrs))
 184                 return -EEXIST;
 185
 186         tmp_addrs = kmalloc(mdev->num_addrs + 1, GFP_KERNEL);
 187         if (!tmp_addrs)
 188                 return -ENOMEM;
 189         memcpy(tmp_addrs, mdev->addrs, mdev->num_addrs);
 190         tmp_addrs[mdev->num_addrs] = addr->s_addr;
 191
 192         /* Lock to write */
 193         spin_lock_irqsave(&mdev->addrs_lock, flags);
 194         mdev->num_addrs++;
 195         swap(mdev->addrs, tmp_addrs);
 196         spin_unlock_irqrestore(&mdev->addrs_lock, flags);
 197
 198         kfree(tmp_addrs);
 199
 200         mctp_route_add_local(mdev, addr->s_addr);
 201
 202         return 0;
 203 }
 204
 205 static int mctp_rtm_deladdr(struct sk_buff *skb, struct nlmsghdr *nlh,
 206                             struct netlink_ext_ack *extack)
 207 {
 208         struct net *net = sock_net(skb->sk);
 209         struct nlattr *tb[IFA_MAX + 1];
 210         struct net_device *dev;
 211         struct mctp_addr *addr;
 212         struct mctp_dev *mdev;
 213         struct ifaddrmsg *ifm;
 214         unsigned long flags;
 215         u8 *pos;
 216         int rc;
 217
 218         rc = nlmsg_parse(nlh, sizeof(*ifm), tb, IFA_MAX, ifa_mctp_policy,
 219                          extack);
 220         if (rc < 0)
 221                 return rc;
 222
 223         ifm = nlmsg_data(nlh);
 224
 225         if (tb[IFA_LOCAL])
 226                 addr = nla_data(tb[IFA_LOCAL]);
 227         else if (tb[IFA_ADDRESS])
 228                 addr = nla_data(tb[IFA_ADDRESS]);
 229         else
 230                 return -EINVAL;
 231
 232         /* find device */
 233         dev = __dev_get_by_index(net, ifm->ifa_index);
 234         if (!dev)
 235                 return -ENODEV;
 236
 237         mdev = mctp_dev_get_rtnl(dev);
 238         if (!mdev)
 239                 return -ENODEV;
 240
 241         pos = memchr(mdev->addrs, addr->s_addr, mdev->num_addrs);
 242         if (!pos)
 243                 return -ENOENT;
 244
 245         rc = mctp_route_remove_local(mdev, addr->s_addr);
 246         // we can ignore -ENOENT in the case a route was already removed
 247         if (rc < 0 && rc != -ENOENT)
 248                 return rc;
 249
 250         spin_lock_irqsave(&mdev->addrs_lock, flags);
 251         memmove(pos, pos + 1, mdev->num_addrs - 1 - (pos - mdev->addrs));
 252         mdev->num_addrs--;
 253         spin_unlock_irqrestore(&mdev->addrs_lock, flags);
 254
 255         return 0;
 256 }
 257
 258 static struct mctp_dev *mctp_add_dev(struct net_device *dev)
 259 {
 260         struct mctp_dev *mdev;
 261
 262         ASSERT_RTNL();
 263
 264         mdev = kzalloc(sizeof(*mdev), GFP_KERNEL);
 265         if (!mdev)
 266                 return ERR_PTR(-ENOMEM);
 267
 268         spin_lock_init(&mdev->addrs_lock);
 269
 270         mdev->net = mctp_default_net(dev_net(dev));
 271
 272         /* associate to net_device */
 273         rcu_assign_pointer(dev->mctp_ptr, mdev);
 274         dev_hold(dev);
 275         mdev->dev = dev;
 276
 277         return mdev;
 278 }
 279
 280 static int mctp_fill_link_af(struct sk_buff *skb,
 281                              const struct net_device *dev, u32 ext_filter_mask)
 282 {
 283         struct mctp_dev *mdev;
 284
 285         mdev = mctp_dev_get_rtnl(dev);
 286         if (!mdev)
 287                 return -ENODATA;
 288         if (nla_put_u32(skb, IFLA_MCTP_NET, mdev->net))
 289                 return -EMSGSIZE;
 290         return 0;
 291 }
 292
 293 static size_t mctp_get_link_af_size(const struct net_device *dev,
 294                                     u32 ext_filter_mask)
 295 {
 296         struct mctp_dev *mdev;
 297         unsigned int ret;
 298
 299         /* caller holds RCU */
 300         mdev = __mctp_dev_get(dev);
 301         if (!mdev)
 302                 return 0;
 303         ret = nla_total_size(4); /* IFLA_MCTP_NET */
 304         return ret;
 305 }
 306
 307 static const struct nla_policy ifla_af_mctp_policy[IFLA_MCTP_MAX + 1] = {
 308         [IFLA_MCTP_NET]         = { .type = NLA_U32 },
 309 };
 310
 311 static int mctp_set_link_af(struct net_device *dev, const struct nlattr *attr,
 312                             struct netlink_ext_ack *extack)
 313 {
 314         struct nlattr *tb[IFLA_MCTP_MAX + 1];
 315         struct mctp_dev *mdev;
 316         int rc;
 317
 318         rc = nla_parse_nested(tb, IFLA_MCTP_MAX, attr, ifla_af_mctp_policy,
 319                               NULL);
 320         if (rc)
 321                 return rc;
 322
 323         mdev = mctp_dev_get_rtnl(dev);
 324         if (!mdev)
 325                 return 0;
 326
 327         if (tb[IFLA_MCTP_NET])
 328                 WRITE_ONCE(mdev->net, nla_get_u32(tb[IFLA_MCTP_NET]));
 329
 330         return 0;
 331 }
 332
 333 static void mctp_unregister(struct net_device *dev)
 334 {
 335         struct mctp_dev *mdev;
 336
 337         mdev = mctp_dev_get_rtnl(dev);
 338
 339         if (!mdev)
 340                 return;
 341
 342         RCU_INIT_POINTER(mdev->dev->mctp_ptr, NULL);
 343
 344         mctp_route_remove_dev(mdev);
 345         mctp_neigh_remove_dev(mdev);
 346         kfree(mdev->addrs);
 347
 348         mctp_dev_destroy(mdev);
 349 }
 350
 351 static int mctp_register(struct net_device *dev)
 352 {
 353         struct mctp_dev *mdev;
 354
 355         /* Already registered? */
 356         if (rtnl_dereference(dev->mctp_ptr))
 357                 return 0;
 358
 359         /* only register specific types; MCTP-specific and loopback for now */
 360         if (dev->type != ARPHRD_MCTP && dev->type != ARPHRD_LOOPBACK)
 361                 return 0;
 362
 363         mdev = mctp_add_dev(dev);
 364         if (IS_ERR(mdev))
 365                 return PTR_ERR(mdev);
 366
 367         return 0;
 368 }
 369
 370 static int mctp_dev_notify(struct notifier_block *this, unsigned long event,
 371                            void *ptr)
 372 {
 373         struct net_device *dev = netdev_notifier_info_to_dev(ptr);
 374         int rc;
 375
 376         switch (event) {
 377         case NETDEV_REGISTER:
 378                 rc = mctp_register(dev);
 379                 if (rc)
 380                         return notifier_from_errno(rc);
 381                 break;
 382         case NETDEV_UNREGISTER:
 383                 mctp_unregister(dev);
 384                 break;
 385         }
 386
 387         return NOTIFY_OK;
 388 }
 389
 390 static struct rtnl_af_ops mctp_af_ops = {
 391         .family = AF_MCTP,
 392         .fill_link_af = mctp_fill_link_af,
 393         .get_link_af_size = mctp_get_link_af_size,
 394         .set_link_af = mctp_set_link_af,
 395 };
 396
 397 static struct notifier_block mctp_dev_nb = {
 398         .notifier_call = mctp_dev_notify,
 399         .priority = ADDRCONF_NOTIFY_PRIORITY,
 400 };
 401
 402 void __init mctp_device_init(void)
 403 {
 404         register_netdevice_notifier(&mctp_dev_nb);
 405
 406         rtnl_register_module(THIS_MODULE, PF_MCTP, RTM_GETADDR,
 407                              NULL, mctp_dump_addrinfo, 0);
 408         rtnl_register_module(THIS_MODULE, PF_MCTP, RTM_NEWADDR,
 409                              mctp_rtm_newaddr, NULL, 0);
 410         rtnl_register_module(THIS_MODULE, PF_MCTP, RTM_DELADDR,
 411                              mctp_rtm_deladdr, NULL, 0);
 412         rtnl_af_register(&mctp_af_ops);
 413 }
 414
 415 void __exit mctp_device_exit(void)
 416 {
 417         rtnl_af_unregister(&mctp_af_ops);
 418         rtnl_unregister(PF_MCTP, RTM_DELADDR);
 419         rtnl_unregister(PF_MCTP, RTM_NEWADDR);
 420         rtnl_unregister(PF_MCTP, RTM_GETADDR);
 421
 422         unregister_netdevice_notifier(&mctp_dev_nb);
 423 }