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