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 BUG_ON(!mutex_is_locked(&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 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 75 static 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 87 static 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 109 struct 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 131 int 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 150 int 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. */ 174 u8 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 207 int 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 } 225 found: 226 rcu_read_unlock(); 227 return err; 228 } 229 230 /* automatically configure a Phonet device, if supported */ 231 static 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 253 static 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 */ 279 static 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 298 static struct notifier_block phonet_device_notifier = { 299 .notifier_call = phonet_device_notify, 300 .priority = 0, 301 }; 302 303 /* Per-namespace Phonet devices handling */ 304 static 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 318 static 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 326 static 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 */ 334 int __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 349 void 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 357 int 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 374 int 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 394 struct 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 405 struct 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