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 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 mutex_lock(&pnn->routes.lock); 277 for (i = 0; i < 64; i++) 278 if (rcu_access_pointer(pnn->routes.table[i]) == dev) { 279 RCU_INIT_POINTER(pnn->routes.table[i], NULL); 280 set_bit(i, deleted); 281 } 282 mutex_unlock(&pnn->routes.lock); 283 284 if (bitmap_empty(deleted, 64)) 285 return; /* short-circuit RCU */ 286 synchronize_rcu(); 287 for_each_set_bit(i, deleted, 64) { 288 rtm_phonet_notify(net, RTM_DELROUTE, ifindex, i); 289 dev_put(dev); 290 } 291 } 292 293 /* notify Phonet of device events */ 294 static int phonet_device_notify(struct notifier_block *me, unsigned long what, 295 void *ptr) 296 { 297 struct net_device *dev = netdev_notifier_info_to_dev(ptr); 298 299 switch (what) { 300 case NETDEV_REGISTER: 301 if (dev->type == ARPHRD_PHONET) 302 phonet_device_autoconf(dev); 303 break; 304 case NETDEV_UNREGISTER: 305 phonet_device_destroy(dev); 306 phonet_route_autodel(dev); 307 break; 308 } 309 return 0; 310 311 } 312 313 static struct notifier_block phonet_device_notifier = { 314 .notifier_call = phonet_device_notify, 315 .priority = 0, 316 }; 317 318 /* Per-namespace Phonet devices handling */ 319 static int __net_init phonet_init_net(struct net *net) 320 { 321 struct phonet_net *pnn = phonet_pernet(net); 322 323 if (!proc_create_net("phonet", 0, net->proc_net, &pn_sock_seq_ops, 324 sizeof(struct seq_net_private))) 325 return -ENOMEM; 326 327 INIT_LIST_HEAD(&pnn->pndevs.list); 328 spin_lock_init(&pnn->pndevs.lock); 329 mutex_init(&pnn->routes.lock); 330 return 0; 331 } 332 333 static void __net_exit phonet_exit_net(struct net *net) 334 { 335 struct phonet_net *pnn = phonet_pernet(net); 336 337 remove_proc_entry("phonet", net->proc_net); 338 WARN_ON_ONCE(!list_empty(&pnn->pndevs.list)); 339 } 340 341 static struct pernet_operations phonet_net_ops = { 342 .init = phonet_init_net, 343 .exit = phonet_exit_net, 344 .id = &phonet_net_id, 345 .size = sizeof(struct phonet_net), 346 }; 347 348 /* Initialize Phonet devices list */ 349 int __init phonet_device_init(void) 350 { 351 int err = register_pernet_subsys(&phonet_net_ops); 352 if (err) 353 return err; 354 355 proc_create_net("pnresource", 0, init_net.proc_net, &pn_res_seq_ops, 356 sizeof(struct seq_net_private)); 357 register_netdevice_notifier(&phonet_device_notifier); 358 err = phonet_netlink_register(); 359 if (err) 360 phonet_device_exit(); 361 return err; 362 } 363 364 void phonet_device_exit(void) 365 { 366 rtnl_unregister_all(PF_PHONET); 367 unregister_netdevice_notifier(&phonet_device_notifier); 368 unregister_pernet_subsys(&phonet_net_ops); 369 remove_proc_entry("pnresource", init_net.proc_net); 370 } 371 372 int phonet_route_add(struct net_device *dev, u8 daddr) 373 { 374 struct phonet_net *pnn = phonet_pernet(dev_net(dev)); 375 struct phonet_routes *routes = &pnn->routes; 376 int err = -EEXIST; 377 378 daddr = daddr >> 2; 379 mutex_lock(&routes->lock); 380 if (routes->table[daddr] == NULL) { 381 rcu_assign_pointer(routes->table[daddr], dev); 382 dev_hold(dev); 383 err = 0; 384 } 385 mutex_unlock(&routes->lock); 386 return err; 387 } 388 389 int phonet_route_del(struct net_device *dev, u8 daddr) 390 { 391 struct phonet_net *pnn = phonet_pernet(dev_net(dev)); 392 struct phonet_routes *routes = &pnn->routes; 393 394 daddr = daddr >> 2; 395 mutex_lock(&routes->lock); 396 if (rcu_access_pointer(routes->table[daddr]) == dev) 397 RCU_INIT_POINTER(routes->table[daddr], NULL); 398 else 399 dev = NULL; 400 mutex_unlock(&routes->lock); 401 402 if (!dev) 403 return -ENOENT; 404 synchronize_rcu(); 405 dev_put(dev); 406 return 0; 407 } 408 409 struct net_device *phonet_route_get_rcu(struct net *net, u8 daddr) 410 { 411 struct phonet_net *pnn = phonet_pernet(net); 412 struct phonet_routes *routes = &pnn->routes; 413 struct net_device *dev; 414 415 daddr >>= 2; 416 dev = rcu_dereference(routes->table[daddr]); 417 return dev; 418 } 419 420 struct net_device *phonet_route_output(struct net *net, u8 daddr) 421 { 422 struct phonet_net *pnn = phonet_pernet(net); 423 struct phonet_routes *routes = &pnn->routes; 424 struct net_device *dev; 425 426 daddr >>= 2; 427 rcu_read_lock(); 428 dev = rcu_dereference(routes->table[daddr]); 429 dev_hold(dev); 430 rcu_read_unlock(); 431 432 if (!dev) 433 dev = phonet_device_get(net); /* Default route */ 434 return dev; 435 } 436