net/phonet/pn_netlink.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * File: pn_netlink.c
   4  *
   5  * Phonet netlink interface
   6  *
   7  * Copyright (C) 2008 Nokia Corporation.
   8  *
   9  * Authors: Sakari Ailus <sakari.ailus@nokia.com>
  10  *          Remi Denis-Courmont
  11  */
  12
  13 #include <linux/kernel.h>
  14 #include <linux/netlink.h>
  15 #include <linux/phonet.h>
  16 #include <linux/slab.h>
  17 #include <net/sock.h>
  18 #include <net/phonet/pn_dev.h>
  19
  20 /* Device address handling */
  21
  22 static int fill_addr(struct sk_buff *skb, struct net_device *dev, u8 addr,
  23                      u32 portid, u32 seq, int event);
  24
  25 void phonet_address_notify(int event, struct net_device *dev, u8 addr)
  26 {
  27         struct sk_buff *skb;
  28         int err = -ENOBUFS;
  29
  30         skb = nlmsg_new(NLMSG_ALIGN(sizeof(struct ifaddrmsg)) +
  31                         nla_total_size(1), GFP_KERNEL);
  32         if (skb == NULL)
  33                 goto errout;
  34         err = fill_addr(skb, dev, addr, 0, 0, event);
  35         if (err < 0) {
  36                 WARN_ON(err == -EMSGSIZE);
  37                 kfree_skb(skb);
  38                 goto errout;
  39         }
  40         rtnl_notify(skb, dev_net(dev), 0,
  41                     RTNLGRP_PHONET_IFADDR, NULL, GFP_KERNEL);
  42         return;
  43 errout:
  44         rtnl_set_sk_err(dev_net(dev), RTNLGRP_PHONET_IFADDR, err);
  45 }
  46
  47 static const struct nla_policy ifa_phonet_policy[IFA_MAX+1] = {
  48         [IFA_LOCAL] = { .type = NLA_U8 },
  49 };
  50
  51 static int addr_doit(struct sk_buff *skb, struct nlmsghdr *nlh,
  52                      struct netlink_ext_ack *extack)
  53 {
  54         struct net *net = sock_net(skb->sk);
  55         struct nlattr *tb[IFA_MAX+1];
  56         struct net_device *dev;
  57         struct ifaddrmsg *ifm;
  58         int err;
  59         u8 pnaddr;
  60
  61         if (!netlink_capable(skb, CAP_NET_ADMIN))
  62                 return -EPERM;
  63
  64         if (!netlink_capable(skb, CAP_SYS_ADMIN))
  65                 return -EPERM;
  66
  67         ASSERT_RTNL();
  68
  69         err = nlmsg_parse_deprecated(nlh, sizeof(*ifm), tb, IFA_MAX,
  70                                      ifa_phonet_policy, extack);
  71         if (err < 0)
  72                 return err;
  73
  74         ifm = nlmsg_data(nlh);
  75         if (tb[IFA_LOCAL] == NULL)
  76                 return -EINVAL;
  77         pnaddr = nla_get_u8(tb[IFA_LOCAL]);
  78         if (pnaddr & 3)
  79                 /* Phonet addresses only have 6 high-order bits */
  80                 return -EINVAL;
  81
  82         dev = __dev_get_by_index(net, ifm->ifa_index);
  83         if (dev == NULL)
  84                 return -ENODEV;
  85
  86         if (nlh->nlmsg_type == RTM_NEWADDR)
  87                 err = phonet_address_add(dev, pnaddr);
  88         else
  89                 err = phonet_address_del(dev, pnaddr);
  90         if (!err)
  91                 phonet_address_notify(nlh->nlmsg_type, dev, pnaddr);
  92         return err;
  93 }
  94
  95 static int fill_addr(struct sk_buff *skb, struct net_device *dev, u8 addr,
  96                         u32 portid, u32 seq, int event)
  97 {
  98         struct ifaddrmsg *ifm;
  99         struct nlmsghdr *nlh;
 100
 101         nlh = nlmsg_put(skb, portid, seq, event, sizeof(*ifm), 0);
 102         if (nlh == NULL)
 103                 return -EMSGSIZE;
 104
 105         ifm = nlmsg_data(nlh);
 106         ifm->ifa_family = AF_PHONET;
 107         ifm->ifa_prefixlen = 0;
 108         ifm->ifa_flags = IFA_F_PERMANENT;
 109         ifm->ifa_scope = RT_SCOPE_LINK;
 110         ifm->ifa_index = dev->ifindex;
 111         if (nla_put_u8(skb, IFA_LOCAL, addr))
 112                 goto nla_put_failure;
 113         nlmsg_end(skb, nlh);
 114         return 0;
 115
 116 nla_put_failure:
 117         nlmsg_cancel(skb, nlh);
 118         return -EMSGSIZE;
 119 }
 120
 121 static int getaddr_dumpit(struct sk_buff *skb, struct netlink_callback *cb)
 122 {
 123         struct phonet_device_list *pndevs;
 124         struct phonet_device *pnd;
 125         int dev_idx = 0, dev_start_idx = cb->args[0];
 126         int addr_idx = 0, addr_start_idx = cb->args[1];
 127
 128         pndevs = phonet_device_list(sock_net(skb->sk));
 129         rcu_read_lock();
 130         list_for_each_entry_rcu(pnd, &pndevs->list, list) {
 131                 u8 addr;
 132
 133                 if (dev_idx > dev_start_idx)
 134                         addr_start_idx = 0;
 135                 if (dev_idx++ < dev_start_idx)
 136                         continue;
 137
 138                 addr_idx = 0;
 139                 for_each_set_bit(addr, pnd->addrs, 64) {
 140                         if (addr_idx++ < addr_start_idx)
 141                                 continue;
 142
 143                         if (fill_addr(skb, pnd->netdev, addr << 2,
 144                                          NETLINK_CB(cb->skb).portid,
 145                                         cb->nlh->nlmsg_seq, RTM_NEWADDR) < 0)
 146                                 goto out;
 147                 }
 148         }
 149
 150 out:
 151         rcu_read_unlock();
 152         cb->args[0] = dev_idx;
 153         cb->args[1] = addr_idx;
 154
 155         return skb->len;
 156 }
 157
 158 /* Routes handling */
 159
 160 static int fill_route(struct sk_buff *skb, struct net_device *dev, u8 dst,
 161                         u32 portid, u32 seq, int event)
 162 {
 163         struct rtmsg *rtm;
 164         struct nlmsghdr *nlh;
 165
 166         nlh = nlmsg_put(skb, portid, seq, event, sizeof(*rtm), 0);
 167         if (nlh == NULL)
 168                 return -EMSGSIZE;
 169
 170         rtm = nlmsg_data(nlh);
 171         rtm->rtm_family = AF_PHONET;
 172         rtm->rtm_dst_len = 6;
 173         rtm->rtm_src_len = 0;
 174         rtm->rtm_tos = 0;
 175         rtm->rtm_table = RT_TABLE_MAIN;
 176         rtm->rtm_protocol = RTPROT_STATIC;
 177         rtm->rtm_scope = RT_SCOPE_UNIVERSE;
 178         rtm->rtm_type = RTN_UNICAST;
 179         rtm->rtm_flags = 0;
 180         if (nla_put_u8(skb, RTA_DST, dst) ||
 181             nla_put_u32(skb, RTA_OIF, dev->ifindex))
 182                 goto nla_put_failure;
 183         nlmsg_end(skb, nlh);
 184         return 0;
 185
 186 nla_put_failure:
 187         nlmsg_cancel(skb, nlh);
 188         return -EMSGSIZE;
 189 }
 190
 191 void rtm_phonet_notify(int event, struct net_device *dev, u8 dst)
 192 {
 193         struct sk_buff *skb;
 194         int err = -ENOBUFS;
 195
 196         skb = nlmsg_new(NLMSG_ALIGN(sizeof(struct ifaddrmsg)) +
 197                         nla_total_size(1) + nla_total_size(4), GFP_KERNEL);
 198         if (skb == NULL)
 199                 goto errout;
 200         err = fill_route(skb, dev, dst, 0, 0, event);
 201         if (err < 0) {
 202                 WARN_ON(err == -EMSGSIZE);
 203                 kfree_skb(skb);
 204                 goto errout;
 205         }
 206         rtnl_notify(skb, dev_net(dev), 0,
 207                           RTNLGRP_PHONET_ROUTE, NULL, GFP_KERNEL);
 208         return;
 209 errout:
 210         rtnl_set_sk_err(dev_net(dev), RTNLGRP_PHONET_ROUTE, err);
 211 }
 212
 213 static const struct nla_policy rtm_phonet_policy[RTA_MAX+1] = {
 214         [RTA_DST] = { .type = NLA_U8 },
 215         [RTA_OIF] = { .type = NLA_U32 },
 216 };
 217
 218 static int route_doit(struct sk_buff *skb, struct nlmsghdr *nlh,
 219                       struct netlink_ext_ack *extack)
 220 {
 221         struct net *net = sock_net(skb->sk);
 222         struct nlattr *tb[RTA_MAX+1];
 223         struct net_device *dev;
 224         struct rtmsg *rtm;
 225         int err;
 226         u8 dst;
 227
 228         if (!netlink_capable(skb, CAP_NET_ADMIN))
 229                 return -EPERM;
 230
 231         if (!netlink_capable(skb, CAP_SYS_ADMIN))
 232                 return -EPERM;
 233
 234         ASSERT_RTNL();
 235
 236         err = nlmsg_parse_deprecated(nlh, sizeof(*rtm), tb, RTA_MAX,
 237                                      rtm_phonet_policy, extack);
 238         if (err < 0)
 239                 return err;
 240
 241         rtm = nlmsg_data(nlh);
 242         if (rtm->rtm_table != RT_TABLE_MAIN || rtm->rtm_type != RTN_UNICAST)
 243                 return -EINVAL;
 244         if (tb[RTA_DST] == NULL || tb[RTA_OIF] == NULL)
 245                 return -EINVAL;
 246         dst = nla_get_u8(tb[RTA_DST]);
 247         if (dst & 3) /* Phonet addresses only have 6 high-order bits */
 248                 return -EINVAL;
 249
 250         dev = __dev_get_by_index(net, nla_get_u32(tb[RTA_OIF]));
 251         if (dev == NULL)
 252                 return -ENODEV;
 253
 254         if (nlh->nlmsg_type == RTM_NEWROUTE)
 255                 err = phonet_route_add(dev, dst);
 256         else
 257                 err = phonet_route_del(dev, dst);
 258         if (!err)
 259                 rtm_phonet_notify(nlh->nlmsg_type, dev, dst);
 260         return err;
 261 }
 262
 263 static int route_dumpit(struct sk_buff *skb, struct netlink_callback *cb)
 264 {
 265         struct net *net = sock_net(skb->sk);
 266         u8 addr;
 267
 268         rcu_read_lock();
 269         for (addr = cb->args[0]; addr < 64; addr++) {
 270                 struct net_device *dev = phonet_route_get_rcu(net, addr << 2);
 271
 272                 if (!dev)
 273                         continue;
 274
 275                 if (fill_route(skb, dev, addr << 2, NETLINK_CB(cb->skb).portid,
 276                                cb->nlh->nlmsg_seq, RTM_NEWROUTE) < 0)
 277                         goto out;
 278         }
 279
 280 out:
 281         rcu_read_unlock();
 282         cb->args[0] = addr;
 283
 284         return skb->len;
 285 }
 286
 287 int __init phonet_netlink_register(void)
 288 {
 289         int err = rtnl_register_module(THIS_MODULE, PF_PHONET, RTM_NEWADDR,
 290                                        addr_doit, NULL, 0);
 291         if (err)
 292                 return err;
 293
 294         /* Further rtnl_register_module() cannot fail */
 295         rtnl_register_module(THIS_MODULE, PF_PHONET, RTM_DELADDR,
 296                              addr_doit, NULL, 0);
 297         rtnl_register_module(THIS_MODULE, PF_PHONET, RTM_GETADDR,
 298                              NULL, getaddr_dumpit, 0);
 299         rtnl_register_module(THIS_MODULE, PF_PHONET, RTM_NEWROUTE,
 300                              route_doit, NULL, 0);
 301         rtnl_register_module(THIS_MODULE, PF_PHONET, RTM_DELROUTE,
 302                              route_doit, NULL, 0);
 303         rtnl_register_module(THIS_MODULE, PF_PHONET, RTM_GETROUTE,
 304                              NULL, route_dumpit, 0);
 305         return 0;
 306 }