// SPDX-License-Identifier: GPL-2.0-only
/*
 * HWSIM IEEE 802.15.4 interface
 *
 * (C) 2018 Mojatau, Alexander Aring <aring@mojatau.com>
 * Copyright 2007-2012 Siemens AG
 *
 * Based on fakelb, original Written by:
 * Sergey Lapin <slapin@ossfans.org>
 * Dmitry Eremin-Solenikov <dbaryshkov@gmail.com>
 * Alexander Smirnov <alex.bluesman.smirnov@gmail.com>
 */

#include <linux/module.h>
#include <linux/timer.h>
#include <linux/platform_device.h>
#include <linux/rtnetlink.h>
#include <linux/netdevice.h>
#include <linux/device.h>
#include <linux/spinlock.h>
#include <net/ieee802154_netdev.h>
#include <net/mac802154.h>
#include <net/cfg802154.h>
#include <net/genetlink.h>
#include "mac802154_hwsim.h"

MODULE_DESCRIPTION("Software simulator of IEEE 802.15.4 radio(s) for mac802154");
MODULE_LICENSE("GPL");

static LIST_HEAD(hwsim_phys);
static DEFINE_MUTEX(hwsim_phys_lock);

static struct platform_device *mac802154hwsim_dev;

/* MAC802154_HWSIM netlink family */
static struct genl_family hwsim_genl_family;

static int hwsim_radio_idx;

enum hwsim_multicast_groups {
        HWSIM_MCGRP_CONFIG,
};

static const struct genl_multicast_group hwsim_mcgrps[] = {
        [HWSIM_MCGRP_CONFIG] = { .name = "config", },
};

struct hwsim_pib {
        u8 page;
        u8 channel;
        struct ieee802154_hw_addr_filt filt;
        enum ieee802154_filtering_level filt_level;

        struct rcu_head rcu;
};

struct hwsim_edge_info {
        u8 lqi;

        struct rcu_head rcu;
};

struct hwsim_edge {
        struct hwsim_phy *endpoint;
        struct hwsim_edge_info __rcu *info;

        struct list_head list;
        struct rcu_head rcu;
};

struct hwsim_phy {
        struct ieee802154_hw *hw;
        u32 idx;

        struct hwsim_pib __rcu *pib;

        bool suspended;
        struct list_head edges;

        struct list_head list;
};

static int hwsim_add_one(struct genl_info *info, struct device *dev,
                         bool init);
static void hwsim_del(struct hwsim_phy *phy);

static int hwsim_hw_ed(struct ieee802154_hw *hw, u8 *level)
{
        *level = 0xbe;

        return 0;
}

static int hwsim_update_pib(struct ieee802154_hw *hw, u8 page, u8 channel,
                            struct ieee802154_hw_addr_filt *filt,
                            enum ieee802154_filtering_level filt_level)
{
        struct hwsim_phy *phy = hw->priv;
        struct hwsim_pib *pib, *pib_old;

        pib = kzalloc_obj(*pib, GFP_ATOMIC);
        if (!pib)
                return -ENOMEM;

        pib_old = rtnl_dereference(phy->pib);

        pib->page = page;
        pib->channel = channel;
        pib->filt.short_addr = filt->short_addr;
        pib->filt.pan_id = filt->pan_id;
        pib->filt.ieee_addr = filt->ieee_addr;
        pib->filt.pan_coord = filt->pan_coord;
        pib->filt_level = filt_level;

        rcu_assign_pointer(phy->pib, pib);
        kfree_rcu(pib_old, rcu);
        return 0;
}

static int hwsim_hw_channel(struct ieee802154_hw *hw, u8 page, u8 channel)
{
        struct hwsim_phy *phy = hw->priv;
        struct hwsim_pib *pib;
        int ret;

        rcu_read_lock();
        pib = rcu_dereference(phy->pib);
        ret = hwsim_update_pib(hw, page, channel, &pib->filt, pib->filt_level);
        rcu_read_unlock();

        return ret;
}

static int hwsim_hw_addr_filt(struct ieee802154_hw *hw,
                              struct ieee802154_hw_addr_filt *filt,
                              unsigned long changed)
{
        struct hwsim_phy *phy = hw->priv;
        struct hwsim_pib *pib;
        int ret;

        rcu_read_lock();
        pib = rcu_dereference(phy->pib);
        ret = hwsim_update_pib(hw, pib->page, pib->channel, filt, pib->filt_level);
        rcu_read_unlock();

        return ret;
}

static void hwsim_hw_receive(struct ieee802154_hw *hw, struct sk_buff *skb,
                             u8 lqi)
{
        struct ieee802154_hdr hdr;
        struct hwsim_phy *phy = hw->priv;
        struct hwsim_pib *pib;

        rcu_read_lock();
        pib = rcu_dereference(phy->pib);

        if (!pskb_may_pull(skb, 3)) {
                dev_dbg(hw->parent, "invalid frame\n");
                goto drop;
        }

        memcpy(&hdr, skb->data, 3);

        /* Level 4 filtering: Frame fields validity */
        if (pib->filt_level == IEEE802154_FILTERING_4_FRAME_FIELDS) {
                /* a) Drop reserved frame types */
                switch (mac_cb(skb)->type) {
                case IEEE802154_FC_TYPE_BEACON:
                case IEEE802154_FC_TYPE_DATA:
                case IEEE802154_FC_TYPE_ACK:
                case IEEE802154_FC_TYPE_MAC_CMD:
                        break;
                default:
                        dev_dbg(hw->parent, "unrecognized frame type 0x%x\n",
                                mac_cb(skb)->type);
                        goto drop;
                }

                /* b) Drop reserved frame versions */
                switch (hdr.fc.version) {
                case IEEE802154_2003_STD:
                case IEEE802154_2006_STD:
                case IEEE802154_STD:
                        break;
                default:
                        dev_dbg(hw->parent,
                                "unrecognized frame version 0x%x\n",
                                hdr.fc.version);
                        goto drop;
                }

                /* c) PAN ID constraints */
                if ((mac_cb(skb)->dest.mode == IEEE802154_ADDR_LONG ||
                     mac_cb(skb)->dest.mode == IEEE802154_ADDR_SHORT) &&
                    mac_cb(skb)->dest.pan_id != pib->filt.pan_id &&
                    mac_cb(skb)->dest.pan_id != cpu_to_le16(IEEE802154_PANID_BROADCAST)) {
                        dev_dbg(hw->parent,
                                "unrecognized PAN ID %04x\n",
                                le16_to_cpu(mac_cb(skb)->dest.pan_id));
                        goto drop;
                }

                /* d1) Short address constraints */
                if (mac_cb(skb)->dest.mode == IEEE802154_ADDR_SHORT &&
                    mac_cb(skb)->dest.short_addr != pib->filt.short_addr &&
                    mac_cb(skb)->dest.short_addr != cpu_to_le16(IEEE802154_ADDR_BROADCAST)) {
                        dev_dbg(hw->parent,
                                "unrecognized short address %04x\n",
                                le16_to_cpu(mac_cb(skb)->dest.short_addr));
                        goto drop;
                }

                /* d2) Extended address constraints */
                if (mac_cb(skb)->dest.mode == IEEE802154_ADDR_LONG &&
                    mac_cb(skb)->dest.extended_addr != pib->filt.ieee_addr) {
                        dev_dbg(hw->parent,
                                "unrecognized long address 0x%016llx\n",
                                mac_cb(skb)->dest.extended_addr);
                        goto drop;
                }

                /* d4) Specific PAN coordinator case (no parent) */
                if ((mac_cb(skb)->type == IEEE802154_FC_TYPE_DATA ||
                     mac_cb(skb)->type == IEEE802154_FC_TYPE_MAC_CMD) &&
                    mac_cb(skb)->dest.mode == IEEE802154_ADDR_NONE) {
                        dev_dbg(hw->parent,
                                "relaying is not supported\n");
                        goto drop;
                }

                /* e) Beacon frames follow specific PAN ID rules */
                if (mac_cb(skb)->type == IEEE802154_FC_TYPE_BEACON &&
                    pib->filt.pan_id != cpu_to_le16(IEEE802154_PANID_BROADCAST) &&
                    mac_cb(skb)->dest.pan_id != pib->filt.pan_id) {
                        dev_dbg(hw->parent,
                                "invalid beacon PAN ID %04x\n",
                                le16_to_cpu(mac_cb(skb)->dest.pan_id));
                        goto drop;
                }
        }

        rcu_read_unlock();

        ieee802154_rx_irqsafe(hw, skb, lqi);

        return;

drop:
        rcu_read_unlock();
        kfree_skb(skb);
}

static int hwsim_hw_xmit(struct ieee802154_hw *hw, struct sk_buff *skb)
{
        struct hwsim_phy *current_phy = hw->priv;
        struct hwsim_pib *current_pib, *endpoint_pib;
        struct hwsim_edge_info *einfo;
        struct hwsim_edge *e;

        WARN_ON(current_phy->suspended);

        rcu_read_lock();
        current_pib = rcu_dereference(current_phy->pib);
        list_for_each_entry_rcu(e, &current_phy->edges, list) {
                /* Can be changed later in rx_irqsafe, but this is only a
                 * performance tweak. Received radio should drop the frame
                 * in mac802154 stack anyway... so we don't need to be
                 * 100% of locking here to check on suspended
                 */
                if (e->endpoint->suspended)
                        continue;

                endpoint_pib = rcu_dereference(e->endpoint->pib);
                if (current_pib->page == endpoint_pib->page &&
                    current_pib->channel == endpoint_pib->channel) {
                        struct sk_buff *newskb = pskb_copy(skb, GFP_ATOMIC);

                        einfo = rcu_dereference(e->info);
                        if (newskb)
                                hwsim_hw_receive(e->endpoint->hw, newskb, einfo->lqi);
                }
        }
        rcu_read_unlock();

        ieee802154_xmit_complete(hw, skb, false);
        return 0;
}

static int hwsim_hw_start(struct ieee802154_hw *hw)
{
        struct hwsim_phy *phy = hw->priv;

        phy->suspended = false;

        return 0;
}

static void hwsim_hw_stop(struct ieee802154_hw *hw)
{
        struct hwsim_phy *phy = hw->priv;

        phy->suspended = true;
}

static int
hwsim_set_promiscuous_mode(struct ieee802154_hw *hw, const bool on)
{
        enum ieee802154_filtering_level filt_level;
        struct hwsim_phy *phy = hw->priv;
        struct hwsim_pib *pib;
        int ret;

        if (on)
                filt_level = IEEE802154_FILTERING_NONE;
        else
                filt_level = IEEE802154_FILTERING_4_FRAME_FIELDS;

        rcu_read_lock();
        pib = rcu_dereference(phy->pib);
        ret = hwsim_update_pib(hw, pib->page, pib->channel, &pib->filt, filt_level);
        rcu_read_unlock();

        return ret;
}

static const struct ieee802154_ops hwsim_ops = {
        .owner = THIS_MODULE,
        .xmit_async = hwsim_hw_xmit,
        .ed = hwsim_hw_ed,
        .set_channel = hwsim_hw_channel,
        .start = hwsim_hw_start,
        .stop = hwsim_hw_stop,
        .set_promiscuous_mode = hwsim_set_promiscuous_mode,
        .set_hw_addr_filt = hwsim_hw_addr_filt,
};

static int hwsim_new_radio_nl(struct sk_buff *msg, struct genl_info *info)
{
        return hwsim_add_one(info, &mac802154hwsim_dev->dev, false);
}

static int hwsim_del_radio_nl(struct sk_buff *msg, struct genl_info *info)
{
        struct hwsim_phy *phy, *tmp;
        s64 idx = -1;

        if (!info->attrs[MAC802154_HWSIM_ATTR_RADIO_ID])
                return -EINVAL;

        idx = nla_get_u32(info->attrs[MAC802154_HWSIM_ATTR_RADIO_ID]);

        mutex_lock(&hwsim_phys_lock);
        list_for_each_entry_safe(phy, tmp, &hwsim_phys, list) {
                if (idx == phy->idx) {
                        hwsim_del(phy);
                        mutex_unlock(&hwsim_phys_lock);
                        return 0;
                }
        }
        mutex_unlock(&hwsim_phys_lock);

        return -ENODEV;
}

static int append_radio_msg(struct sk_buff *skb, struct hwsim_phy *phy)
{
        struct nlattr *nl_edges, *nl_edge;
        struct hwsim_edge_info *einfo;
        struct hwsim_edge *e;
        int ret;

        ret = nla_put_u32(skb, MAC802154_HWSIM_ATTR_RADIO_ID, phy->idx);
        if (ret < 0)
                return ret;

        rcu_read_lock();
        if (list_empty(&phy->edges)) {
                rcu_read_unlock();
                return 0;
        }

        nl_edges = nla_nest_start_noflag(skb,
                                         MAC802154_HWSIM_ATTR_RADIO_EDGES);
        if (!nl_edges) {
                rcu_read_unlock();
                return -ENOBUFS;
        }

        list_for_each_entry_rcu(e, &phy->edges, list) {
                nl_edge = nla_nest_start_noflag(skb,
                                                MAC802154_HWSIM_ATTR_RADIO_EDGE);
                if (!nl_edge) {
                        rcu_read_unlock();
                        nla_nest_cancel(skb, nl_edges);
                        return -ENOBUFS;
                }

                ret = nla_put_u32(skb, MAC802154_HWSIM_EDGE_ATTR_ENDPOINT_ID,
                                  e->endpoint->idx);
                if (ret < 0) {
                        rcu_read_unlock();
                        nla_nest_cancel(skb, nl_edge);
                        nla_nest_cancel(skb, nl_edges);
                        return ret;
                }

                einfo = rcu_dereference(e->info);
                ret = nla_put_u8(skb, MAC802154_HWSIM_EDGE_ATTR_LQI,
                                 einfo->lqi);
                if (ret < 0) {
                        rcu_read_unlock();
                        nla_nest_cancel(skb, nl_edge);
                        nla_nest_cancel(skb, nl_edges);
                        return ret;
                }

                nla_nest_end(skb, nl_edge);
        }
        rcu_read_unlock();

        nla_nest_end(skb, nl_edges);

        return 0;
}

static int hwsim_get_radio(struct sk_buff *skb, struct hwsim_phy *phy,
                           u32 portid, u32 seq,
                           struct netlink_callback *cb, int flags)
{
        void *hdr;
        int res;

        hdr = genlmsg_put(skb, portid, seq, &hwsim_genl_family, flags,
                          MAC802154_HWSIM_CMD_GET_RADIO);
        if (!hdr)
                return -EMSGSIZE;

        if (cb)
                genl_dump_check_consistent(cb, hdr);

        res = append_radio_msg(skb, phy);
        if (res < 0)
                goto out_err;

        genlmsg_end(skb, hdr);
        return 0;

out_err:
        genlmsg_cancel(skb, hdr);
        return res;
}

static int hwsim_get_radio_nl(struct sk_buff *msg, struct genl_info *info)
{
        struct hwsim_phy *phy;
        struct sk_buff *skb;
        int idx, res = -ENODEV;

        if (!info->attrs[MAC802154_HWSIM_ATTR_RADIO_ID])
                return -EINVAL;
        idx = nla_get_u32(info->attrs[MAC802154_HWSIM_ATTR_RADIO_ID]);

        mutex_lock(&hwsim_phys_lock);
        list_for_each_entry(phy, &hwsim_phys, list) {
                if (phy->idx != idx)
                        continue;

                skb = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC);
                if (!skb) {
                        res = -ENOMEM;
                        goto out_err;
                }

                res = hwsim_get_radio(skb, phy, info->snd_portid,
                                      info->snd_seq, NULL, 0);
                if (res < 0) {
                        nlmsg_free(skb);
                        goto out_err;
                }

                res = genlmsg_reply(skb, info);
                break;
        }

out_err:
        mutex_unlock(&hwsim_phys_lock);

        return res;
}

static int hwsim_dump_radio_nl(struct sk_buff *skb,
                               struct netlink_callback *cb)
{
        int idx = cb->args[0];
        struct hwsim_phy *phy;
        int res;

        mutex_lock(&hwsim_phys_lock);

        if (idx == hwsim_radio_idx)
                goto done;

        list_for_each_entry(phy, &hwsim_phys, list) {
                if (phy->idx < idx)
                        continue;

                res = hwsim_get_radio(skb, phy, NETLINK_CB(cb->skb).portid,
                                      cb->nlh->nlmsg_seq, cb, NLM_F_MULTI);
                if (res < 0)
                        break;

                idx = phy->idx + 1;
        }

        cb->args[0] = idx;

done:
        mutex_unlock(&hwsim_phys_lock);
        return skb->len;
}

/* caller need to held hwsim_phys_lock */
static struct hwsim_phy *hwsim_get_radio_by_id(uint32_t idx)
{
        struct hwsim_phy *phy;

        list_for_each_entry(phy, &hwsim_phys, list) {
                if (phy->idx == idx)
                        return phy;
        }

        return NULL;
}

static const struct nla_policy hwsim_edge_policy[MAC802154_HWSIM_EDGE_ATTR_MAX + 1] = {
        [MAC802154_HWSIM_EDGE_ATTR_ENDPOINT_ID] = { .type = NLA_U32 },
        [MAC802154_HWSIM_EDGE_ATTR_LQI] = { .type = NLA_U8 },
};

static struct hwsim_edge *hwsim_alloc_edge(struct hwsim_phy *endpoint, u8 lqi)
{
        struct hwsim_edge_info *einfo;
        struct hwsim_edge *e;

        e = kzalloc_obj(*e);
        if (!e)
                return NULL;

        einfo = kzalloc_obj(*einfo);
        if (!einfo) {
                kfree(e);
                return NULL;
        }

        einfo->lqi = 0xff;
        rcu_assign_pointer(e->info, einfo);
        e->endpoint = endpoint;

        return e;
}

static void hwsim_free_edge(struct hwsim_edge *e)
{
        struct hwsim_edge_info *einfo;

        rcu_read_lock();
        einfo = rcu_dereference(e->info);
        rcu_read_unlock();

        kfree_rcu(einfo, rcu);
        kfree_rcu(e, rcu);
}

static int hwsim_new_edge_nl(struct sk_buff *msg, struct genl_info *info)
{
        struct nlattr *edge_attrs[MAC802154_HWSIM_EDGE_ATTR_MAX + 1];
        struct hwsim_phy *phy_v0, *phy_v1;
        struct hwsim_edge *e;
        u32 v0, v1;

        if (!info->attrs[MAC802154_HWSIM_ATTR_RADIO_ID] ||
            !info->attrs[MAC802154_HWSIM_ATTR_RADIO_EDGE])
                return -EINVAL;

        if (nla_parse_nested_deprecated(edge_attrs, MAC802154_HWSIM_EDGE_ATTR_MAX, info->attrs[MAC802154_HWSIM_ATTR_RADIO_EDGE], hwsim_edge_policy, NULL))
                return -EINVAL;

        if (!edge_attrs[MAC802154_HWSIM_EDGE_ATTR_ENDPOINT_ID])
                return -EINVAL;

        v0 = nla_get_u32(info->attrs[MAC802154_HWSIM_ATTR_RADIO_ID]);
        v1 = nla_get_u32(edge_attrs[MAC802154_HWSIM_EDGE_ATTR_ENDPOINT_ID]);

        if (v0 == v1)
                return -EINVAL;

        mutex_lock(&hwsim_phys_lock);
        phy_v0 = hwsim_get_radio_by_id(v0);
        if (!phy_v0) {
                mutex_unlock(&hwsim_phys_lock);
                return -ENOENT;
        }

        phy_v1 = hwsim_get_radio_by_id(v1);
        if (!phy_v1) {
                mutex_unlock(&hwsim_phys_lock);
                return -ENOENT;
        }

        rcu_read_lock();
        list_for_each_entry_rcu(e, &phy_v0->edges, list) {
                if (e->endpoint->idx == v1) {
                        mutex_unlock(&hwsim_phys_lock);
                        rcu_read_unlock();
                        return -EEXIST;
                }
        }
        rcu_read_unlock();

        e = hwsim_alloc_edge(phy_v1, 0xff);
        if (!e) {
                mutex_unlock(&hwsim_phys_lock);
                return -ENOMEM;
        }
        list_add_rcu(&e->list, &phy_v0->edges);
        /* wait until changes are done under hwsim_phys_lock lock
         * should prevent of calling this function twice while
         * edges list has not the changes yet.
         */
        synchronize_rcu();
        mutex_unlock(&hwsim_phys_lock);

        return 0;
}

static int hwsim_del_edge_nl(struct sk_buff *msg, struct genl_info *info)
{
        struct nlattr *edge_attrs[MAC802154_HWSIM_EDGE_ATTR_MAX + 1];
        struct hwsim_phy *phy_v0;
        struct hwsim_edge *e;
        u32 v0, v1;

        if (!info->attrs[MAC802154_HWSIM_ATTR_RADIO_ID] ||
            !info->attrs[MAC802154_HWSIM_ATTR_RADIO_EDGE])
                return -EINVAL;

        if (nla_parse_nested_deprecated(edge_attrs, MAC802154_HWSIM_EDGE_ATTR_MAX, info->attrs[MAC802154_HWSIM_ATTR_RADIO_EDGE], hwsim_edge_policy, NULL))
                return -EINVAL;

        if (!edge_attrs[MAC802154_HWSIM_EDGE_ATTR_ENDPOINT_ID])
                return -EINVAL;

        v0 = nla_get_u32(info->attrs[MAC802154_HWSIM_ATTR_RADIO_ID]);
        v1 = nla_get_u32(edge_attrs[MAC802154_HWSIM_EDGE_ATTR_ENDPOINT_ID]);

        mutex_lock(&hwsim_phys_lock);
        phy_v0 = hwsim_get_radio_by_id(v0);
        if (!phy_v0) {
                mutex_unlock(&hwsim_phys_lock);
                return -ENOENT;
        }

        rcu_read_lock();
        list_for_each_entry_rcu(e, &phy_v0->edges, list) {
                if (e->endpoint->idx == v1) {
                        rcu_read_unlock();
                        list_del_rcu(&e->list);
                        hwsim_free_edge(e);
                        /* same again - wait until list changes are done */
                        synchronize_rcu();
                        mutex_unlock(&hwsim_phys_lock);
                        return 0;
                }
        }
        rcu_read_unlock();

        mutex_unlock(&hwsim_phys_lock);

        return -ENOENT;
}

static int hwsim_set_edge_lqi(struct sk_buff *msg, struct genl_info *info)
{
        struct nlattr *edge_attrs[MAC802154_HWSIM_EDGE_ATTR_MAX + 1];
        struct hwsim_edge_info *einfo, *einfo_old;
        struct hwsim_phy *phy_v0;
        struct hwsim_edge *e;
        u32 v0, v1;
        u8 lqi;

        if (!info->attrs[MAC802154_HWSIM_ATTR_RADIO_ID] ||
            !info->attrs[MAC802154_HWSIM_ATTR_RADIO_EDGE])
                return -EINVAL;

        if (nla_parse_nested_deprecated(edge_attrs, MAC802154_HWSIM_EDGE_ATTR_MAX, info->attrs[MAC802154_HWSIM_ATTR_RADIO_EDGE], hwsim_edge_policy, NULL))
                return -EINVAL;

        if (!edge_attrs[MAC802154_HWSIM_EDGE_ATTR_ENDPOINT_ID] ||
            !edge_attrs[MAC802154_HWSIM_EDGE_ATTR_LQI])
                return -EINVAL;

        v0 = nla_get_u32(info->attrs[MAC802154_HWSIM_ATTR_RADIO_ID]);
        v1 = nla_get_u32(edge_attrs[MAC802154_HWSIM_EDGE_ATTR_ENDPOINT_ID]);
        lqi = nla_get_u8(edge_attrs[MAC802154_HWSIM_EDGE_ATTR_LQI]);

        mutex_lock(&hwsim_phys_lock);
        phy_v0 = hwsim_get_radio_by_id(v0);
        if (!phy_v0) {
                mutex_unlock(&hwsim_phys_lock);
                return -ENOENT;
        }

        einfo = kzalloc_obj(*einfo);
        if (!einfo) {
                mutex_unlock(&hwsim_phys_lock);
                return -ENOMEM;
        }

        rcu_read_lock();
        list_for_each_entry_rcu(e, &phy_v0->edges, list) {
                if (e->endpoint->idx == v1) {
                        einfo->lqi = lqi;
                        einfo_old = rcu_replace_pointer(e->info, einfo,
                                                        lockdep_is_held(&hwsim_phys_lock));
                        rcu_read_unlock();
                        kfree_rcu(einfo_old, rcu);
                        mutex_unlock(&hwsim_phys_lock);
                        return 0;
                }
        }
        rcu_read_unlock();

        kfree(einfo);
        mutex_unlock(&hwsim_phys_lock);

        return -ENOENT;
}

/* MAC802154_HWSIM netlink policy */

static const struct nla_policy hwsim_genl_policy[MAC802154_HWSIM_ATTR_MAX + 1] = {
        [MAC802154_HWSIM_ATTR_RADIO_ID] = { .type = NLA_U32 },
        [MAC802154_HWSIM_ATTR_RADIO_EDGE] = { .type = NLA_NESTED },
        [MAC802154_HWSIM_ATTR_RADIO_EDGES] = { .type = NLA_NESTED },
};

/* Generic Netlink operations array */
static const struct genl_small_ops hwsim_nl_ops[] = {
        {
                .cmd = MAC802154_HWSIM_CMD_NEW_RADIO,
                .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
                .doit = hwsim_new_radio_nl,
                .flags = GENL_UNS_ADMIN_PERM,
        },
        {
                .cmd = MAC802154_HWSIM_CMD_DEL_RADIO,
                .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
                .doit = hwsim_del_radio_nl,
                .flags = GENL_UNS_ADMIN_PERM,
        },
        {
                .cmd = MAC802154_HWSIM_CMD_GET_RADIO,
                .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
                .doit = hwsim_get_radio_nl,
                .dumpit = hwsim_dump_radio_nl,
        },
        {
                .cmd = MAC802154_HWSIM_CMD_NEW_EDGE,
                .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
                .doit = hwsim_new_edge_nl,
                .flags = GENL_UNS_ADMIN_PERM,
        },
        {
                .cmd = MAC802154_HWSIM_CMD_DEL_EDGE,
                .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
                .doit = hwsim_del_edge_nl,
                .flags = GENL_UNS_ADMIN_PERM,
        },
        {
                .cmd = MAC802154_HWSIM_CMD_SET_EDGE,
                .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
                .doit = hwsim_set_edge_lqi,
                .flags = GENL_UNS_ADMIN_PERM,
        },
};

static struct genl_family hwsim_genl_family __ro_after_init = {
        .name = "MAC802154_HWSIM",
        .version = 1,
        .maxattr = MAC802154_HWSIM_ATTR_MAX,
        .policy = hwsim_genl_policy,
        .module = THIS_MODULE,
        .small_ops = hwsim_nl_ops,
        .n_small_ops = ARRAY_SIZE(hwsim_nl_ops),
        .resv_start_op = MAC802154_HWSIM_CMD_NEW_EDGE + 1,
        .mcgrps = hwsim_mcgrps,
        .n_mcgrps = ARRAY_SIZE(hwsim_mcgrps),
};

static void hwsim_mcast_config_msg(struct sk_buff *mcast_skb,
                                   struct genl_info *info)
{
        if (info)
                genl_notify(&hwsim_genl_family, mcast_skb, info,
                            HWSIM_MCGRP_CONFIG, GFP_KERNEL);
        else
                genlmsg_multicast(&hwsim_genl_family, mcast_skb, 0,
                                  HWSIM_MCGRP_CONFIG, GFP_KERNEL);
}

static void hwsim_mcast_new_radio(struct genl_info *info, struct hwsim_phy *phy)
{
        struct sk_buff *mcast_skb;
        void *data;

        mcast_skb = genlmsg_new(GENLMSG_DEFAULT_SIZE, GFP_KERNEL);
        if (!mcast_skb)
                return;

        data = genlmsg_put(mcast_skb, 0, 0, &hwsim_genl_family, 0,
                           MAC802154_HWSIM_CMD_NEW_RADIO);
        if (!data)
                goto out_err;

        if (append_radio_msg(mcast_skb, phy) < 0)
                goto out_err;

        genlmsg_end(mcast_skb, data);

        hwsim_mcast_config_msg(mcast_skb, info);
        return;

out_err:
        genlmsg_cancel(mcast_skb, data);
        nlmsg_free(mcast_skb);
}

static void hwsim_edge_unsubscribe_me(struct hwsim_phy *phy)
{
        struct hwsim_phy *tmp;
        struct hwsim_edge *e;

        rcu_read_lock();
        /* going to all phy edges and remove phy from it */
        list_for_each_entry(tmp, &hwsim_phys, list) {
                list_for_each_entry_rcu(e, &tmp->edges, list) {
                        if (e->endpoint->idx == phy->idx) {
                                list_del_rcu(&e->list);
                                hwsim_free_edge(e);
                        }
                }
        }
        rcu_read_unlock();

        synchronize_rcu();
}

static int hwsim_subscribe_all_others(struct hwsim_phy *phy)
{
        struct hwsim_phy *sub;
        struct hwsim_edge *e;

        list_for_each_entry(sub, &hwsim_phys, list) {
                e = hwsim_alloc_edge(sub, 0xff);
                if (!e)
                        goto me_fail;

                list_add_rcu(&e->list, &phy->edges);
        }

        list_for_each_entry(sub, &hwsim_phys, list) {
                e = hwsim_alloc_edge(phy, 0xff);
                if (!e)
                        goto sub_fail;

                list_add_rcu(&e->list, &sub->edges);
        }

        return 0;

sub_fail:
        hwsim_edge_unsubscribe_me(phy);
me_fail:
        rcu_read_lock();
        list_for_each_entry_rcu(e, &phy->edges, list) {
                list_del_rcu(&e->list);
                hwsim_free_edge(e);
        }
        rcu_read_unlock();
        return -ENOMEM;
}

static int hwsim_add_one(struct genl_info *info, struct device *dev,
                         bool init)
{
        struct ieee802154_hw *hw;
        struct hwsim_phy *phy;
        struct hwsim_pib *pib;
        int idx;
        int err;

        idx = hwsim_radio_idx++;

        hw = ieee802154_alloc_hw(sizeof(*phy), &hwsim_ops);
        if (!hw)
                return -ENOMEM;

        phy = hw->priv;
        phy->hw = hw;

        /* 868 MHz BPSK 802.15.4-2003 */
        hw->phy->supported.channels[0] |= 1;
        /* 915 MHz BPSK 802.15.4-2003 */
        hw->phy->supported.channels[0] |= 0x7fe;
        /* 2.4 GHz O-QPSK 802.15.4-2003 */
        hw->phy->supported.channels[0] |= 0x7FFF800;
        /* 868 MHz ASK 802.15.4-2006 */
        hw->phy->supported.channels[1] |= 1;
        /* 915 MHz ASK 802.15.4-2006 */
        hw->phy->supported.channels[1] |= 0x7fe;
        /* 868 MHz O-QPSK 802.15.4-2006 */
        hw->phy->supported.channels[2] |= 1;
        /* 915 MHz O-QPSK 802.15.4-2006 */
        hw->phy->supported.channels[2] |= 0x7fe;
        /* 2.4 GHz CSS 802.15.4a-2007 */
        hw->phy->supported.channels[3] |= 0x3fff;
        /* UWB Sub-gigahertz 802.15.4a-2007 */
        hw->phy->supported.channels[4] |= 1;
        /* UWB Low band 802.15.4a-2007 */
        hw->phy->supported.channels[4] |= 0x1e;
        /* UWB High band 802.15.4a-2007 */
        hw->phy->supported.channels[4] |= 0xffe0;
        /* 750 MHz O-QPSK 802.15.4c-2009 */
        hw->phy->supported.channels[5] |= 0xf;
        /* 750 MHz MPSK 802.15.4c-2009 */
        hw->phy->supported.channels[5] |= 0xf0;
        /* 950 MHz BPSK 802.15.4d-2009 */
        hw->phy->supported.channels[6] |= 0x3ff;
        /* 950 MHz GFSK 802.15.4d-2009 */
        hw->phy->supported.channels[6] |= 0x3ffc00;

        ieee802154_random_extended_addr(&hw->phy->perm_extended_addr);

        /* hwsim phy channel 13 as default */
        hw->phy->current_channel = 13;
        pib = kzalloc_obj(*pib);
        if (!pib) {
                err = -ENOMEM;
                goto err_pib;
        }

        pib->channel = 13;
        pib->filt.short_addr = cpu_to_le16(IEEE802154_ADDR_BROADCAST);
        pib->filt.pan_id = cpu_to_le16(IEEE802154_PANID_BROADCAST);
        rcu_assign_pointer(phy->pib, pib);
        phy->idx = idx;
        INIT_LIST_HEAD(&phy->edges);

        hw->flags = IEEE802154_HW_PROMISCUOUS;
        hw->parent = dev;

        err = ieee802154_register_hw(hw);
        if (err)
                goto err_reg;

        mutex_lock(&hwsim_phys_lock);
        if (init) {
                err = hwsim_subscribe_all_others(phy);
                if (err < 0) {
                        mutex_unlock(&hwsim_phys_lock);
                        goto err_subscribe;
                }
        }
        list_add_tail(&phy->list, &hwsim_phys);
        mutex_unlock(&hwsim_phys_lock);

        hwsim_mcast_new_radio(info, phy);

        return idx;

err_subscribe:
        ieee802154_unregister_hw(phy->hw);
err_reg:
        kfree(pib);
err_pib:
        ieee802154_free_hw(phy->hw);
        return err;
}

static void hwsim_del(struct hwsim_phy *phy)
{
        struct hwsim_pib *pib;
        struct hwsim_edge *e;

        hwsim_edge_unsubscribe_me(phy);

        list_del(&phy->list);

        rcu_read_lock();
        list_for_each_entry_rcu(e, &phy->edges, list) {
                list_del_rcu(&e->list);
                hwsim_free_edge(e);
        }
        pib = rcu_dereference(phy->pib);
        rcu_read_unlock();

        kfree_rcu(pib, rcu);

        ieee802154_unregister_hw(phy->hw);
        ieee802154_free_hw(phy->hw);
}

static int hwsim_probe(struct platform_device *pdev)
{
        struct hwsim_phy *phy, *tmp;
        int err, i;

        for (i = 0; i < 2; i++) {
                err = hwsim_add_one(NULL, &pdev->dev, true);
                if (err < 0)
                        goto err_slave;
        }

        dev_info(&pdev->dev, "Added 2 mac802154 hwsim hardware radios\n");
        return 0;

err_slave:
        mutex_lock(&hwsim_phys_lock);
        list_for_each_entry_safe(phy, tmp, &hwsim_phys, list)
                hwsim_del(phy);
        mutex_unlock(&hwsim_phys_lock);
        return err;
}

static void hwsim_remove(struct platform_device *pdev)
{
        struct hwsim_phy *phy, *tmp;

        mutex_lock(&hwsim_phys_lock);
        list_for_each_entry_safe(phy, tmp, &hwsim_phys, list)
                hwsim_del(phy);
        mutex_unlock(&hwsim_phys_lock);
}

static struct platform_driver mac802154hwsim_driver = {
        .probe = hwsim_probe,
        .remove = hwsim_remove,
        .driver = {
                        .name = "mac802154_hwsim",
        },
};

static __init int hwsim_init_module(void)
{
        int rc;

        rc = genl_register_family(&hwsim_genl_family);
        if (rc)
                return rc;

        mac802154hwsim_dev = platform_device_register_simple("mac802154_hwsim",
                                                             -1, NULL, 0);
        if (IS_ERR(mac802154hwsim_dev)) {
                rc = PTR_ERR(mac802154hwsim_dev);
                goto platform_dev;
        }

        rc = platform_driver_register(&mac802154hwsim_driver);
        if (rc < 0)
                goto platform_drv;

        return 0;

platform_drv:
        platform_device_unregister(mac802154hwsim_dev);
platform_dev:
        genl_unregister_family(&hwsim_genl_family);
        return rc;
}

static __exit void hwsim_remove_module(void)
{
        genl_unregister_family(&hwsim_genl_family);
        platform_driver_unregister(&mac802154hwsim_driver);
        platform_device_unregister(mac802154hwsim_dev);
}

module_init(hwsim_init_module);
module_exit(hwsim_remove_module);