net/phonet/pn_dev.c

   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 <[email protected]>
  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 }