// SPDX-License-Identifier: GPL-2.0-only
/*
 * File: pn_dev.c
 *
 * Phonet network device
 *
 * Copyright (C) 2008 Nokia Corporation.
 *
 * Authors: Sakari Ailus <sakari.ailus@nokia.com>
 *          Rémi Denis-Courmont
 */

#include <linux/kernel.h>
#include <linux/net.h>
#include <linux/slab.h>
#include <linux/netdevice.h>
#include <linux/phonet.h>
#include <linux/proc_fs.h>
#include <linux/if_arp.h>
#include <net/sock.h>
#include <net/netns/generic.h>
#include <net/phonet/pn_dev.h>

struct phonet_routes {
        spinlock_t              lock;
        struct net_device __rcu *table[64];
};

struct phonet_net {
        struct phonet_device_list pndevs;
        struct phonet_routes routes;
};

static unsigned int phonet_net_id __read_mostly;

static struct phonet_net *phonet_pernet(struct net *net)
{
        return net_generic(net, phonet_net_id);
}

struct phonet_device_list *phonet_device_list(struct net *net)
{
        struct phonet_net *pnn = phonet_pernet(net);
        return &pnn->pndevs;
}

/* Allocate new Phonet device. */
static struct phonet_device *__phonet_device_alloc(struct net_device *dev)
{
        struct phonet_device_list *pndevs = phonet_device_list(dev_net(dev));
        struct phonet_device *pnd = kmalloc_obj(*pnd, GFP_ATOMIC);
        if (pnd == NULL)
                return NULL;
        pnd->netdev = dev;
        bitmap_zero(pnd->addrs, 64);

        lockdep_assert_held(&pndevs->lock);
        list_add_rcu(&pnd->list, &pndevs->list);
        return pnd;
}

static struct phonet_device *__phonet_get(struct net_device *dev)
{
        struct phonet_device_list *pndevs = phonet_device_list(dev_net(dev));
        struct phonet_device *pnd;

        lockdep_assert_held(&pndevs->lock);

        list_for_each_entry(pnd, &pndevs->list, list) {
                if (pnd->netdev == dev)
                        return pnd;
        }
        return NULL;
}

static struct phonet_device *__phonet_get_rcu(struct net_device *dev)
{
        struct phonet_device_list *pndevs = phonet_device_list(dev_net(dev));
        struct phonet_device *pnd;

        list_for_each_entry_rcu(pnd, &pndevs->list, list) {
                if (pnd->netdev == dev)
                        return pnd;
        }
        return NULL;
}

static void phonet_device_destroy(struct net_device *dev)
{
        struct phonet_device_list *pndevs = phonet_device_list(dev_net(dev));
        struct phonet_device *pnd;

        ASSERT_RTNL();

        spin_lock(&pndevs->lock);

        pnd = __phonet_get(dev);
        if (pnd)
                list_del_rcu(&pnd->list);

        spin_unlock(&pndevs->lock);

        if (pnd) {
                struct net *net = dev_net(dev);
                u32 ifindex = dev->ifindex;
                u8 addr;

                for_each_set_bit(addr, pnd->addrs, 64)
                        phonet_address_notify(net, RTM_DELADDR, ifindex, addr);

                kfree(pnd);
        }
}

struct net_device *phonet_device_get(struct net *net)
{
        struct phonet_device_list *pndevs = phonet_device_list(net);
        struct phonet_device *pnd;
        struct net_device *dev = NULL;

        rcu_read_lock();
        list_for_each_entry_rcu(pnd, &pndevs->list, list) {
                dev = pnd->netdev;
                BUG_ON(!dev);

                if ((dev->reg_state == NETREG_REGISTERED) &&
                        ((pnd->netdev->flags & IFF_UP)) == IFF_UP)
                        break;
                dev = NULL;
        }
        dev_hold(dev);
        rcu_read_unlock();
        return dev;
}

int phonet_address_add(struct net_device *dev, u8 addr)
{
        struct phonet_device_list *pndevs = phonet_device_list(dev_net(dev));
        struct phonet_device *pnd;
        int err = 0;

        spin_lock(&pndevs->lock);

        /* Find or create Phonet-specific device data */
        pnd = __phonet_get(dev);
        if (pnd == NULL)
                pnd = __phonet_device_alloc(dev);
        if (unlikely(pnd == NULL))
                err = -ENOMEM;
        else if (test_and_set_bit(addr >> 2, pnd->addrs))
                err = -EEXIST;

        spin_unlock(&pndevs->lock);

        return err;
}

int phonet_address_del(struct net_device *dev, u8 addr)
{
        struct phonet_device_list *pndevs = phonet_device_list(dev_net(dev));
        struct phonet_device *pnd;
        int err = 0;

        spin_lock(&pndevs->lock);

        pnd = __phonet_get(dev);
        if (!pnd || !test_and_clear_bit(addr >> 2, pnd->addrs)) {
                err = -EADDRNOTAVAIL;
                pnd = NULL;
        } else if (bitmap_empty(pnd->addrs, 64))
                list_del_rcu(&pnd->list);
        else
                pnd = NULL;

        spin_unlock(&pndevs->lock);

        if (pnd)
                kfree_rcu(pnd, rcu);

        return err;
}

/* Gets a source address toward a destination, through a interface. */
u8 phonet_address_get(struct net_device *dev, u8 daddr)
{
        struct phonet_device *pnd;
        u8 saddr;

        rcu_read_lock();
        pnd = __phonet_get_rcu(dev);
        if (pnd) {
                BUG_ON(bitmap_empty(pnd->addrs, 64));

                /* Use same source address as destination, if possible */
                if (test_bit(daddr >> 2, pnd->addrs))
                        saddr = daddr;
                else
                        saddr = find_first_bit(pnd->addrs, 64) << 2;
        } else
                saddr = PN_NO_ADDR;
        rcu_read_unlock();

        if (saddr == PN_NO_ADDR) {
                /* Fallback to another device */
                struct net_device *def_dev;

                def_dev = phonet_device_get(dev_net(dev));
                if (def_dev) {
                        if (def_dev != dev)
                                saddr = phonet_address_get(def_dev, daddr);
                        dev_put(def_dev);
                }
        }
        return saddr;
}

int phonet_address_lookup(struct net *net, u8 addr)
{
        struct phonet_device_list *pndevs = phonet_device_list(net);
        struct phonet_device *pnd;
        int err = -EADDRNOTAVAIL;

        rcu_read_lock();
        list_for_each_entry_rcu(pnd, &pndevs->list, list) {
                /* Don't allow unregistering devices! */
                if ((pnd->netdev->reg_state != NETREG_REGISTERED) ||
                                ((pnd->netdev->flags & IFF_UP)) != IFF_UP)
                        continue;

                if (test_bit(addr >> 2, pnd->addrs)) {
                        err = 0;
                        goto found;
                }
        }
found:
        rcu_read_unlock();
        return err;
}

/* automatically configure a Phonet device, if supported */
static int phonet_device_autoconf(struct net_device *dev)
{
        struct if_phonet_req req;
        int ret;

        if (!dev->netdev_ops->ndo_siocdevprivate)
                return -EOPNOTSUPP;

        ret = dev->netdev_ops->ndo_siocdevprivate(dev, (struct ifreq *)&req,
                                                  NULL, SIOCPNGAUTOCONF);
        if (ret < 0)
                return ret;

        ASSERT_RTNL();
        ret = phonet_address_add(dev, req.ifr_phonet_autoconf.device);
        if (ret)
                return ret;

        phonet_address_notify(dev_net(dev), RTM_NEWADDR, dev->ifindex,
                              req.ifr_phonet_autoconf.device);
        return 0;
}

static void phonet_route_autodel(struct net_device *dev)
{
        struct net *net = dev_net(dev);
        DECLARE_BITMAP(deleted, 64);
        u32 ifindex = dev->ifindex;
        struct phonet_net *pnn;
        unsigned int i;

        pnn = phonet_pernet(net);

        /* Remove left-over Phonet routes */
        bitmap_zero(deleted, 64);

        spin_lock(&pnn->routes.lock);
        for (i = 0; i < 64; i++) {
                if (rcu_access_pointer(pnn->routes.table[i]) == dev) {
                        RCU_INIT_POINTER(pnn->routes.table[i], NULL);
                        set_bit(i, deleted);
                }
        }
        spin_unlock(&pnn->routes.lock);

        if (bitmap_empty(deleted, 64))
                return; /* short-circuit RCU */
        synchronize_rcu();
        for_each_set_bit(i, deleted, 64) {
                rtm_phonet_notify(net, RTM_DELROUTE, ifindex, i);
                dev_put(dev);
        }
}

/* notify Phonet of device events */
static int phonet_device_notify(struct notifier_block *me, unsigned long what,
                                void *ptr)
{
        struct net_device *dev = netdev_notifier_info_to_dev(ptr);

        switch (what) {
        case NETDEV_REGISTER:
                if (dev->type == ARPHRD_PHONET)
                        phonet_device_autoconf(dev);
                break;
        case NETDEV_UNREGISTER:
                phonet_device_destroy(dev);
                phonet_route_autodel(dev);
                break;
        }
        return 0;

}

static struct notifier_block phonet_device_notifier = {
        .notifier_call = phonet_device_notify,
        .priority = 0,
};

/* Per-namespace Phonet devices handling */
static int __net_init phonet_init_net(struct net *net)
{
        struct phonet_net *pnn = phonet_pernet(net);

        if (!proc_create_net("phonet", 0, net->proc_net, &pn_sock_seq_ops,
                        sizeof(struct seq_net_private)))
                return -ENOMEM;

        INIT_LIST_HEAD(&pnn->pndevs.list);
        spin_lock_init(&pnn->pndevs.lock);
        spin_lock_init(&pnn->routes.lock);
        return 0;
}

static void __net_exit phonet_exit_net(struct net *net)
{
        struct phonet_net *pnn = phonet_pernet(net);

        remove_proc_entry("phonet", net->proc_net);
        WARN_ON_ONCE(!list_empty(&pnn->pndevs.list));
}

static struct pernet_operations phonet_net_ops = {
        .init = phonet_init_net,
        .exit = phonet_exit_net,
        .id   = &phonet_net_id,
        .size = sizeof(struct phonet_net),
};

/* Initialize Phonet devices list */
int __init phonet_device_init(void)
{
        int err = register_pernet_subsys(&phonet_net_ops);
        if (err)
                return err;

        proc_create_net("pnresource", 0, init_net.proc_net, &pn_res_seq_ops,
                        sizeof(struct seq_net_private));
        register_netdevice_notifier(&phonet_device_notifier);
        err = phonet_netlink_register();
        if (err)
                phonet_device_exit();
        return err;
}

void phonet_device_exit(void)
{
        rtnl_unregister_all(PF_PHONET);
        unregister_netdevice_notifier(&phonet_device_notifier);
        unregister_pernet_subsys(&phonet_net_ops);
        remove_proc_entry("pnresource", init_net.proc_net);
}

int phonet_route_add(struct net_device *dev, u8 daddr)
{
        struct phonet_net *pnn = phonet_pernet(dev_net(dev));
        struct phonet_routes *routes = &pnn->routes;
        int err = -EEXIST;

        daddr = daddr >> 2;

        spin_lock(&routes->lock);
        if (routes->table[daddr] == NULL) {
                rcu_assign_pointer(routes->table[daddr], dev);
                dev_hold(dev);
                err = 0;
        }
        spin_unlock(&routes->lock);

        return err;
}

int phonet_route_del(struct net_device *dev, u8 daddr)
{
        struct phonet_net *pnn = phonet_pernet(dev_net(dev));
        struct phonet_routes *routes = &pnn->routes;

        daddr = daddr >> 2;

        spin_lock(&routes->lock);
        if (rcu_access_pointer(routes->table[daddr]) == dev)
                RCU_INIT_POINTER(routes->table[daddr], NULL);
        else
                dev = NULL;
        spin_unlock(&routes->lock);

        if (!dev)
                return -ENOENT;

        /* Note : our caller must call synchronize_rcu() and dev_put(dev) */

        return 0;
}

struct net_device *phonet_route_get_rcu(struct net *net, u8 daddr)
{
        struct phonet_net *pnn = phonet_pernet(net);
        struct phonet_routes *routes = &pnn->routes;
        struct net_device *dev;

        daddr >>= 2;
        dev = rcu_dereference(routes->table[daddr]);
        return dev;
}

struct net_device *phonet_route_output(struct net *net, u8 daddr)
{
        struct phonet_net *pnn = phonet_pernet(net);
        struct phonet_routes *routes = &pnn->routes;
        struct net_device *dev;

        daddr >>= 2;
        rcu_read_lock();
        dev = rcu_dereference(routes->table[daddr]);
        dev_hold(dev);
        rcu_read_unlock();

        if (!dev)
                dev = phonet_device_get(net); /* Default route */
        return dev;
}