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