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