net/phonet/pn_netlink.c

0001 // SPDX-License-Identifier: GPL-2.0-only
0002 /*
0003  * File: pn_netlink.c
0004  *
0005  * Phonet netlink interface
0006  *
0007  * Copyright (C) 2008 Nokia Corporation.
0008  *
0009  * Authors: Sakari Ailus <sakari.ailus@nokia.com>
0010  *          Remi Denis-Courmont
0011  */
0012
0013 #include <linux/kernel.h>
0014 #include <linux/netlink.h>
0015 #include <linux/phonet.h>
0016 #include <linux/slab.h>
0017 #include <net/sock.h>
0018 #include <net/phonet/pn_dev.h>
0019
0020 /* Device address handling */
0021
0022 static int fill_addr(struct sk_buff *skb, struct net_device *dev, u8 addr,
0023              u32 portid, u32 seq, int event);
0024
0025 void phonet_address_notify(int event, struct net_device *dev, u8 addr)
0026 {
0027     struct sk_buff *skb;
0028     int err = -ENOBUFS;
0029
0030     skb = nlmsg_new(NLMSG_ALIGN(sizeof(struct ifaddrmsg)) +
0031             nla_total_size(1), GFP_KERNEL);
0032     if (skb == NULL)
0033         goto errout;
0034     err = fill_addr(skb, dev, addr, 0, 0, event);
0035     if (err < 0) {
0036         WARN_ON(err == -EMSGSIZE);
0037         kfree_skb(skb);
0038         goto errout;
0039     }
0040     rtnl_notify(skb, dev_net(dev), 0,
0041             RTNLGRP_PHONET_IFADDR, NULL, GFP_KERNEL);
0042     return;
0043 errout:
0044     rtnl_set_sk_err(dev_net(dev), RTNLGRP_PHONET_IFADDR, err);
0045 }
0046
0047 static const struct nla_policy ifa_phonet_policy[IFA_MAX+1] = {
0048     [IFA_LOCAL] = { .type = NLA_U8 },
0049 };
0050
0051 static int addr_doit(struct sk_buff *skb, struct nlmsghdr *nlh,
0052              struct netlink_ext_ack *extack)
0053 {
0054     struct net *net = sock_net(skb->sk);
0055     struct nlattr *tb[IFA_MAX+1];
0056     struct net_device *dev;
0057     struct ifaddrmsg *ifm;
0058     int err;
0059     u8 pnaddr;
0060
0061     if (!netlink_capable(skb, CAP_NET_ADMIN))
0062         return -EPERM;
0063
0064     if (!netlink_capable(skb, CAP_SYS_ADMIN))
0065         return -EPERM;
0066
0067     ASSERT_RTNL();
0068
0069     err = nlmsg_parse_deprecated(nlh, sizeof(*ifm), tb, IFA_MAX,
0070                      ifa_phonet_policy, extack);
0071     if (err < 0)
0072         return err;
0073
0074     ifm = nlmsg_data(nlh);
0075     if (tb[IFA_LOCAL] == NULL)
0076         return -EINVAL;
0077     pnaddr = nla_get_u8(tb[IFA_LOCAL]);
0078     if (pnaddr & 3)
0079         /* Phonet addresses only have 6 high-order bits */
0080         return -EINVAL;
0081
0082     dev = __dev_get_by_index(net, ifm->ifa_index);
0083     if (dev == NULL)
0084         return -ENODEV;
0085
0086     if (nlh->nlmsg_type == RTM_NEWADDR)
0087         err = phonet_address_add(dev, pnaddr);
0088     else
0089         err = phonet_address_del(dev, pnaddr);
0090     if (!err)
0091         phonet_address_notify(nlh->nlmsg_type, dev, pnaddr);
0092     return err;
0093 }
0094
0095 static int fill_addr(struct sk_buff *skb, struct net_device *dev, u8 addr,
0096             u32 portid, u32 seq, int event)
0097 {
0098     struct ifaddrmsg *ifm;
0099     struct nlmsghdr *nlh;
0100
0101     nlh = nlmsg_put(skb, portid, seq, event, sizeof(*ifm), 0);
0102     if (nlh == NULL)
0103         return -EMSGSIZE;
0104
0105     ifm = nlmsg_data(nlh);
0106     ifm->ifa_family = AF_PHONET;
0107     ifm->ifa_prefixlen = 0;
0108     ifm->ifa_flags = IFA_F_PERMANENT;
0109     ifm->ifa_scope = RT_SCOPE_LINK;
0110     ifm->ifa_index = dev->ifindex;
0111     if (nla_put_u8(skb, IFA_LOCAL, addr))
0112         goto nla_put_failure;
0113     nlmsg_end(skb, nlh);
0114     return 0;
0115
0116 nla_put_failure:
0117     nlmsg_cancel(skb, nlh);
0118     return -EMSGSIZE;
0119 }
0120
0121 static int getaddr_dumpit(struct sk_buff *skb, struct netlink_callback *cb)
0122 {
0123     struct phonet_device_list *pndevs;
0124     struct phonet_device *pnd;
0125     int dev_idx = 0, dev_start_idx = cb->args[0];
0126     int addr_idx = 0, addr_start_idx = cb->args[1];
0127
0128     pndevs = phonet_device_list(sock_net(skb->sk));
0129     rcu_read_lock();
0130     list_for_each_entry_rcu(pnd, &pndevs->list, list) {
0131         u8 addr;
0132
0133         if (dev_idx > dev_start_idx)
0134             addr_start_idx = 0;
0135         if (dev_idx++ < dev_start_idx)
0136             continue;
0137
0138         addr_idx = 0;
0139         for_each_set_bit(addr, pnd->addrs, 64) {
0140             if (addr_idx++ < addr_start_idx)
0141                 continue;
0142
0143             if (fill_addr(skb, pnd->netdev, addr << 2,
0144                      NETLINK_CB(cb->skb).portid,
0145                     cb->nlh->nlmsg_seq, RTM_NEWADDR) < 0)
0146                 goto out;
0147         }
0148     }
0149
0150 out:
0151     rcu_read_unlock();
0152     cb->args[0] = dev_idx;
0153     cb->args[1] = addr_idx;
0154
0155     return skb->len;
0156 }
0157
0158 /* Routes handling */
0159
0160 static int fill_route(struct sk_buff *skb, struct net_device *dev, u8 dst,
0161             u32 portid, u32 seq, int event)
0162 {
0163     struct rtmsg *rtm;
0164     struct nlmsghdr *nlh;
0165
0166     nlh = nlmsg_put(skb, portid, seq, event, sizeof(*rtm), 0);
0167     if (nlh == NULL)
0168         return -EMSGSIZE;
0169
0170     rtm = nlmsg_data(nlh);
0171     rtm->rtm_family = AF_PHONET;
0172     rtm->rtm_dst_len = 6;
0173     rtm->rtm_src_len = 0;
0174     rtm->rtm_tos = 0;
0175     rtm->rtm_table = RT_TABLE_MAIN;
0176     rtm->rtm_protocol = RTPROT_STATIC;
0177     rtm->rtm_scope = RT_SCOPE_UNIVERSE;
0178     rtm->rtm_type = RTN_UNICAST;
0179     rtm->rtm_flags = 0;
0180     if (nla_put_u8(skb, RTA_DST, dst) ||
0181         nla_put_u32(skb, RTA_OIF, dev->ifindex))
0182         goto nla_put_failure;
0183     nlmsg_end(skb, nlh);
0184     return 0;
0185
0186 nla_put_failure:
0187     nlmsg_cancel(skb, nlh);
0188     return -EMSGSIZE;
0189 }
0190
0191 void rtm_phonet_notify(int event, struct net_device *dev, u8 dst)
0192 {
0193     struct sk_buff *skb;
0194     int err = -ENOBUFS;
0195
0196     skb = nlmsg_new(NLMSG_ALIGN(sizeof(struct ifaddrmsg)) +
0197             nla_total_size(1) + nla_total_size(4), GFP_KERNEL);
0198     if (skb == NULL)
0199         goto errout;
0200     err = fill_route(skb, dev, dst, 0, 0, event);
0201     if (err < 0) {
0202         WARN_ON(err == -EMSGSIZE);
0203         kfree_skb(skb);
0204         goto errout;
0205     }
0206     rtnl_notify(skb, dev_net(dev), 0,
0207               RTNLGRP_PHONET_ROUTE, NULL, GFP_KERNEL);
0208     return;
0209 errout:
0210     rtnl_set_sk_err(dev_net(dev), RTNLGRP_PHONET_ROUTE, err);
0211 }
0212
0213 static const struct nla_policy rtm_phonet_policy[RTA_MAX+1] = {
0214     [RTA_DST] = { .type = NLA_U8 },
0215     [RTA_OIF] = { .type = NLA_U32 },
0216 };
0217
0218 static int route_doit(struct sk_buff *skb, struct nlmsghdr *nlh,
0219               struct netlink_ext_ack *extack)
0220 {
0221     struct net *net = sock_net(skb->sk);
0222     struct nlattr *tb[RTA_MAX+1];
0223     struct net_device *dev;
0224     struct rtmsg *rtm;
0225     int err;
0226     u8 dst;
0227
0228     if (!netlink_capable(skb, CAP_NET_ADMIN))
0229         return -EPERM;
0230
0231     if (!netlink_capable(skb, CAP_SYS_ADMIN))
0232         return -EPERM;
0233
0234     ASSERT_RTNL();
0235
0236     err = nlmsg_parse_deprecated(nlh, sizeof(*rtm), tb, RTA_MAX,
0237                      rtm_phonet_policy, extack);
0238     if (err < 0)
0239         return err;
0240
0241     rtm = nlmsg_data(nlh);
0242     if (rtm->rtm_table != RT_TABLE_MAIN || rtm->rtm_type != RTN_UNICAST)
0243         return -EINVAL;
0244     if (tb[RTA_DST] == NULL || tb[RTA_OIF] == NULL)
0245         return -EINVAL;
0246     dst = nla_get_u8(tb[RTA_DST]);
0247     if (dst & 3) /* Phonet addresses only have 6 high-order bits */
0248         return -EINVAL;
0249
0250     dev = __dev_get_by_index(net, nla_get_u32(tb[RTA_OIF]));
0251     if (dev == NULL)
0252         return -ENODEV;
0253
0254     if (nlh->nlmsg_type == RTM_NEWROUTE)
0255         err = phonet_route_add(dev, dst);
0256     else
0257         err = phonet_route_del(dev, dst);
0258     if (!err)
0259         rtm_phonet_notify(nlh->nlmsg_type, dev, dst);
0260     return err;
0261 }
0262
0263 static int route_dumpit(struct sk_buff *skb, struct netlink_callback *cb)
0264 {
0265     struct net *net = sock_net(skb->sk);
0266     u8 addr;
0267
0268     rcu_read_lock();
0269     for (addr = cb->args[0]; addr < 64; addr++) {
0270         struct net_device *dev = phonet_route_get_rcu(net, addr << 2);
0271
0272         if (!dev)
0273             continue;
0274
0275         if (fill_route(skb, dev, addr << 2, NETLINK_CB(cb->skb).portid,
0276                    cb->nlh->nlmsg_seq, RTM_NEWROUTE) < 0)
0277             goto out;
0278     }
0279
0280 out:
0281     rcu_read_unlock();
0282     cb->args[0] = addr;
0283
0284     return skb->len;
0285 }
0286
0287 int __init phonet_netlink_register(void)
0288 {
0289     int err = rtnl_register_module(THIS_MODULE, PF_PHONET, RTM_NEWADDR,
0290                        addr_doit, NULL, 0);
0291     if (err)
0292         return err;
0293
0294     /* Further rtnl_register_module() cannot fail */
0295     rtnl_register_module(THIS_MODULE, PF_PHONET, RTM_DELADDR,
0296                  addr_doit, NULL, 0);
0297     rtnl_register_module(THIS_MODULE, PF_PHONET, RTM_GETADDR,
0298                  NULL, getaddr_dumpit, 0);
0299     rtnl_register_module(THIS_MODULE, PF_PHONET, RTM_NEWROUTE,
0300                  route_doit, NULL, 0);
0301     rtnl_register_module(THIS_MODULE, PF_PHONET, RTM_DELROUTE,
0302                  route_doit, NULL, 0);
0303     rtnl_register_module(THIS_MODULE, PF_PHONET, RTM_GETROUTE,
0304                  NULL, route_dumpit, 0);
0305     return 0;
0306 }