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 <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         dev_hold(dev);
 126         rcu_read_unlock();
 127         return dev;
 128 }
 129
 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
 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. */
 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
 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 */
 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
 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 */
 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 */
 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
 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 */
 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
 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
 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
 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
 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
 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 }