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 } 307