1// SPDX-License-Identifier: GPL-2.0-only 2/* 3 * File: pn_dev.c 4 * 5 * Phonet network device 6 * 7 * Copyright (C) 2008 Nokia Corporation. 8 * 9 * Authors: Sakari Ailus <sakari.ailus@nokia.com> 10 * Rémi Denis-Courmont 11 */ 12 13#include <linux/kernel.h> 14#include <linux/net.h> 15#include <linux/slab.h> 16#include <linux/netdevice.h> 17#include <linux/phonet.h> 18#include <linux/proc_fs.h> 19#include <linux/if_arp.h> 20#include <net/sock.h> 21#include <net/netns/generic.h> 22#include <net/phonet/pn_dev.h> 23 24struct phonet_routes { 25 struct mutex lock; 26 struct net_device __rcu *table[64]; 27}; 28 29struct phonet_net { 30 struct phonet_device_list pndevs; 31 struct phonet_routes routes; 32}; 33 34static unsigned int phonet_net_id __read_mostly; 35 36static struct phonet_net *phonet_pernet(struct net *net) 37{ 38 return net_generic(net, phonet_net_id); 39} 40 41struct phonet_device_list *phonet_device_list(struct net *net) 42{ 43 struct phonet_net *pnn = phonet_pernet(net); 44 return &pnn->pndevs; 45} 46 47/* Allocate new Phonet device. */ 48static struct phonet_device *__phonet_device_alloc(struct net_device *dev) 49{ 50 struct phonet_device_list *pndevs = phonet_device_list(dev_net(dev)); 51 struct phonet_device *pnd = kmalloc(sizeof(*pnd), GFP_ATOMIC); 52 if (pnd == NULL) 53 return NULL; 54 pnd->netdev = dev; 55 bitmap_zero(pnd->addrs, 64); 56 57 BUG_ON(!mutex_is_locked(&pndevs->lock)); 58 list_add_rcu(&pnd->list, &pndevs->list); 59 return pnd; 60} 61 62static struct phonet_device *__phonet_get(struct net_device *dev) 63{ 64 struct phonet_device_list *pndevs = phonet_device_list(dev_net(dev)); 65 struct phonet_device *pnd; 66 67 BUG_ON(!mutex_is_locked(&pndevs->lock)); 68 list_for_each_entry(pnd, &pndevs->list, list) { 69 if (pnd->netdev == dev) 70 return pnd; 71 } 72 return NULL; 73} 74 75static struct phonet_device *__phonet_get_rcu(struct net_device *dev) 76{ 77 struct phonet_device_list *pndevs = phonet_device_list(dev_net(dev)); 78 struct phonet_device *pnd; 79 80 list_for_each_entry_rcu(pnd, &pndevs->list, list) { 81 if (pnd->netdev == dev) 82 return pnd; 83 } 84 return NULL; 85} 86 87static void phonet_device_destroy(struct net_device *dev) 88{ 89 struct phonet_device_list *pndevs = phonet_device_list(dev_net(dev)); 90 struct phonet_device *pnd; 91 92 ASSERT_RTNL(); 93 94 mutex_lock(&pndevs->lock); 95 pnd = __phonet_get(dev); 96 if (pnd) 97 list_del_rcu(&pnd->list); 98 mutex_unlock(&pndevs->lock); 99 100 if (pnd) { 101 u8 addr; 102 103 for_each_set_bit(addr, pnd->addrs, 64) 104 phonet_address_notify(RTM_DELADDR, dev, addr); 105 kfree(pnd); 106 } 107} 108 109struct net_device *phonet_device_get(struct net *net) 110{ 111 struct phonet_device_list *pndevs = phonet_device_list(net); 112 struct phonet_device *pnd; 113 struct net_device *dev = NULL; 114 115 rcu_read_lock(); 116 list_for_each_entry_rcu(pnd, &pndevs->list, list) { 117 dev = pnd->netdev; 118 BUG_ON(!dev); 119 120 if ((dev->reg_state == NETREG_REGISTERED) && 121 ((pnd->netdev->flags & IFF_UP)) == IFF_UP) 122 break; 123 dev = NULL; 124 } 125 if (dev) 126 dev_hold(dev); 127 rcu_read_unlock(); 128 return dev; 129} 130 131int phonet_address_add(struct net_device *dev, u8 addr) 132{ 133 struct phonet_device_list *pndevs = phonet_device_list(dev_net(dev)); 134 struct phonet_device *pnd; 135 int err = 0; 136 137 mutex_lock(&pndevs->lock); 138 /* Find or create Phonet-specific device data */ 139 pnd = __phonet_get(dev); 140 if (pnd == NULL) 141 pnd = __phonet_device_alloc(dev); 142 if (unlikely(pnd == NULL)) 143 err = -ENOMEM; 144 else if (test_and_set_bit(addr >> 2, pnd->addrs)) 145 err = -EEXIST; 146 mutex_unlock(&pndevs->lock); 147 return err; 148} 149 150int phonet_address_del(struct net_device *dev, u8 addr) 151{ 152 struct phonet_device_list *pndevs = phonet_device_list(dev_net(dev)); 153 struct phonet_device *pnd; 154 int err = 0; 155 156 mutex_lock(&pndevs->lock); 157 pnd = __phonet_get(dev); 158 if (!pnd || !test_and_clear_bit(addr >> 2, pnd->addrs)) { 159 err = -EADDRNOTAVAIL; 160 pnd = NULL; 161 } else if (bitmap_empty(pnd->addrs, 64)) 162 list_del_rcu(&pnd->list); 163 else 164 pnd = NULL; 165 mutex_unlock(&pndevs->lock); 166 167 if (pnd) 168 kfree_rcu(pnd, rcu); 169 170 return err; 171} 172 173/* Gets a source address toward a destination, through a interface. */ 174u8 phonet_address_get(struct net_device *dev, u8 daddr) 175{ 176 struct phonet_device *pnd; 177 u8 saddr; 178 179 rcu_read_lock(); 180 pnd = __phonet_get_rcu(dev); 181 if (pnd) { 182 BUG_ON(bitmap_empty(pnd->addrs, 64)); 183 184 /* Use same source address as destination, if possible */ 185 if (test_bit(daddr >> 2, pnd->addrs)) 186 saddr = daddr; 187 else 188 saddr = find_first_bit(pnd->addrs, 64) << 2; 189 } else 190 saddr = PN_NO_ADDR; 191 rcu_read_unlock(); 192 193 if (saddr == PN_NO_ADDR) { 194 /* Fallback to another device */ 195 struct net_device *def_dev; 196 197 def_dev = phonet_device_get(dev_net(dev)); 198 if (def_dev) { 199 if (def_dev != dev) 200 saddr = phonet_address_get(def_dev, daddr); 201 dev_put(def_dev); 202 } 203 } 204 return saddr; 205} 206 207int phonet_address_lookup(struct net *net, u8 addr) 208{ 209 struct phonet_device_list *pndevs = phonet_device_list(net); 210 struct phonet_device *pnd; 211 int err = -EADDRNOTAVAIL; 212 213 rcu_read_lock(); 214 list_for_each_entry_rcu(pnd, &pndevs->list, list) { 215 /* Don't allow unregistering devices! */ 216 if ((pnd->netdev->reg_state != NETREG_REGISTERED) || 217 ((pnd->netdev->flags & IFF_UP)) != IFF_UP) 218 continue; 219 220 if (test_bit(addr >> 2, pnd->addrs)) { 221 err = 0; 222 goto found; 223 } 224 } 225found: 226 rcu_read_unlock(); 227 return err; 228} 229 230/* automatically configure a Phonet device, if supported */ 231static int phonet_device_autoconf(struct net_device *dev) 232{ 233 struct if_phonet_req req; 234 int ret; 235 236 if (!dev->netdev_ops->ndo_do_ioctl) 237 return -EOPNOTSUPP; 238 239 ret = dev->netdev_ops->ndo_do_ioctl(dev, (struct ifreq *)&req, 240 SIOCPNGAUTOCONF); 241 if (ret < 0) 242 return ret; 243 244 ASSERT_RTNL(); 245 ret = phonet_address_add(dev, req.ifr_phonet_autoconf.device); 246 if (ret) 247 return ret; 248 phonet_address_notify(RTM_NEWADDR, dev, 249 req.ifr_phonet_autoconf.device); 250 return 0; 251} 252 253static void phonet_route_autodel(struct net_device *dev) 254{ 255 struct phonet_net *pnn = phonet_pernet(dev_net(dev)); 256 unsigned int i; 257 DECLARE_BITMAP(deleted, 64); 258 259 /* Remove left-over Phonet routes */ 260 bitmap_zero(deleted, 64); 261 mutex_lock(&pnn->routes.lock); 262 for (i = 0; i < 64; i++) 263 if (rcu_access_pointer(pnn->routes.table[i]) == dev) { 264 RCU_INIT_POINTER(pnn->routes.table[i], NULL); 265 set_bit(i, deleted); 266 } 267 mutex_unlock(&pnn->routes.lock); 268 269 if (bitmap_empty(deleted, 64)) 270 return; /* short-circuit RCU */ 271 synchronize_rcu(); 272 for_each_set_bit(i, deleted, 64) { 273 rtm_phonet_notify(RTM_DELROUTE, dev, i); 274 dev_put(dev); 275 } 276} 277 278/* notify Phonet of device events */ 279static int phonet_device_notify(struct notifier_block *me, unsigned long what, 280 void *ptr) 281{ 282 struct net_device *dev = netdev_notifier_info_to_dev(ptr); 283 284 switch (what) { 285 case NETDEV_REGISTER: 286 if (dev->type == ARPHRD_PHONET) 287 phonet_device_autoconf(dev); 288 break; 289 case NETDEV_UNREGISTER: 290 phonet_device_destroy(dev); 291 phonet_route_autodel(dev); 292 break; 293 } 294 return 0; 295 296} 297 298static struct notifier_block phonet_device_notifier = { 299 .notifier_call = phonet_device_notify, 300 .priority = 0, 301}; 302 303/* Per-namespace Phonet devices handling */ 304static int __net_init phonet_init_net(struct net *net) 305{ 306 struct phonet_net *pnn = phonet_pernet(net); 307 308 if (!proc_create_net("phonet", 0, net->proc_net, &pn_sock_seq_ops, 309 sizeof(struct seq_net_private))) 310 return -ENOMEM; 311 312 INIT_LIST_HEAD(&pnn->pndevs.list); 313 mutex_init(&pnn->pndevs.lock); 314 mutex_init(&pnn->routes.lock); 315 return 0; 316} 317 318static void __net_exit phonet_exit_net(struct net *net) 319{ 320 struct phonet_net *pnn = phonet_pernet(net); 321 322 remove_proc_entry("phonet", net->proc_net); 323 WARN_ON_ONCE(!list_empty(&pnn->pndevs.list)); 324} 325 326static struct pernet_operations phonet_net_ops = { 327 .init = phonet_init_net, 328 .exit = phonet_exit_net, 329 .id = &phonet_net_id, 330 .size = sizeof(struct phonet_net), 331}; 332 333/* Initialize Phonet devices list */ 334int __init phonet_device_init(void) 335{ 336 int err = register_pernet_subsys(&phonet_net_ops); 337 if (err) 338 return err; 339 340 proc_create_net("pnresource", 0, init_net.proc_net, &pn_res_seq_ops, 341 sizeof(struct seq_net_private)); 342 register_netdevice_notifier(&phonet_device_notifier); 343 err = phonet_netlink_register(); 344 if (err) 345 phonet_device_exit(); 346 return err; 347} 348 349void phonet_device_exit(void) 350{ 351 rtnl_unregister_all(PF_PHONET); 352 unregister_netdevice_notifier(&phonet_device_notifier); 353 unregister_pernet_subsys(&phonet_net_ops); 354 remove_proc_entry("pnresource", init_net.proc_net); 355} 356 357int phonet_route_add(struct net_device *dev, u8 daddr) 358{ 359 struct phonet_net *pnn = phonet_pernet(dev_net(dev)); 360 struct phonet_routes *routes = &pnn->routes; 361 int err = -EEXIST; 362 363 daddr = daddr >> 2; 364 mutex_lock(&routes->lock); 365 if (routes->table[daddr] == NULL) { 366 rcu_assign_pointer(routes->table[daddr], dev); 367 dev_hold(dev); 368 err = 0; 369 } 370 mutex_unlock(&routes->lock); 371 return err; 372} 373 374int phonet_route_del(struct net_device *dev, u8 daddr) 375{ 376 struct phonet_net *pnn = phonet_pernet(dev_net(dev)); 377 struct phonet_routes *routes = &pnn->routes; 378 379 daddr = daddr >> 2; 380 mutex_lock(&routes->lock); 381 if (rcu_access_pointer(routes->table[daddr]) == dev) 382 RCU_INIT_POINTER(routes->table[daddr], NULL); 383 else 384 dev = NULL; 385 mutex_unlock(&routes->lock); 386 387 if (!dev) 388 return -ENOENT; 389 synchronize_rcu(); 390 dev_put(dev); 391 return 0; 392} 393 394struct net_device *phonet_route_get_rcu(struct net *net, u8 daddr) 395{ 396 struct phonet_net *pnn = phonet_pernet(net); 397 struct phonet_routes *routes = &pnn->routes; 398 struct net_device *dev; 399 400 daddr >>= 2; 401 dev = rcu_dereference(routes->table[daddr]); 402 return dev; 403} 404 405struct net_device *phonet_route_output(struct net *net, u8 daddr) 406{ 407 struct phonet_net *pnn = phonet_pernet(net); 408 struct phonet_routes *routes = &pnn->routes; 409 struct net_device *dev; 410 411 daddr >>= 2; 412 rcu_read_lock(); 413 dev = rcu_dereference(routes->table[daddr]); 414 if (dev) 415 dev_hold(dev); 416 rcu_read_unlock(); 417 418 if (!dev) 419 dev = phonet_device_get(net); /* Default route */ 420 return dev; 421} 422