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