--- /dev/null
+/*
+ * HWSIM IEEE 802.15.4 interface
+ *
+ * (C) 2018 Mojatau, Alexander Aring <aring@mojatau.com>
+ * Copyright 2007-2012 Siemens AG
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * 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/netdevice.h>
+#include <linux/device.h>
+#include <linux/spinlock.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 __rcu LIST_HEAD(hwsim_ifup_phys);
+
+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 rcu_head rcu;
+};
+
+struct hwsim_edge_info {
+ u8 lqi;
+
+ struct rcu_head rcu;
+};
+
+struct hwsim_edge {
+ struct hwsim_phy *endpoint;
+ struct hwsim_edge_info *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 __rcu edges;
+
+ struct list_head list;
+ struct list_head list_ifup;
+};
+
+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_hw_channel(struct ieee802154_hw *hw, u8 page, u8 channel)
+{
+ struct hwsim_phy *phy = hw->priv;
+ struct hwsim_pib *pib, *pib_old;
+
+ pib = kzalloc(sizeof(*pib), GFP_KERNEL);
+ if (!pib)
+ return -ENOMEM;
+
+ pib->page = page;
+ pib->channel = channel;
+
+ pib_old = phy->pib;
+ rcu_assign_pointer(phy->pib, pib);
+ kfree_rcu(pib_old, rcu);
+ return 0;
+}
+
+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, ¤t_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)
+ ieee802154_rx_irqsafe(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;
+ list_add_rcu(&phy->list_ifup, &hwsim_ifup_phys);
+ synchronize_rcu();
+
+ return 0;
+}
+
+static void hwsim_hw_stop(struct ieee802154_hw *hw)
+{
+ struct hwsim_phy *phy = hw->priv;
+
+ phy->suspended = true;
+ list_del_rcu(&phy->list_ifup);
+ synchronize_rcu();
+}
+
+static int
+hwsim_set_promiscuous_mode(struct ieee802154_hw *hw, const bool on)
+{
+ return 0;
+}
+
+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,
+};
+
+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(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(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 = -EMSGSIZE;
+
+ 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;
+ }
+
+ 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(sizeof(*e), GFP_KERNEL);
+ if (!e)
+ return NULL;
+
+ einfo = kzalloc(sizeof(*einfo), GFP_KERNEL);
+ if (!einfo) {
+ kfree(e);
+ return NULL;
+ }
+
+ einfo->lqi = 0xff;
+ e->info = einfo;
+ e->endpoint = endpoint;
+
+ return e;
+}
+
+static void hwsim_free_edge(struct hwsim_edge *e)
+{
+ kfree_rcu(e->info, 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(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(edge_attrs, MAC802154_HWSIM_EDGE_ATTR_MAX + 1,
+ 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;
+ 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(edge_attrs, MAC802154_HWSIM_EDGE_ATTR_MAX + 1,
+ 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(sizeof(*einfo), GFP_KERNEL);
+ if (!info) {
+ 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;
+ rcu_assign_pointer(e->info, einfo);
+ rcu_read_unlock();
+ 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_ops hwsim_nl_ops[] = {
+ {
+ .cmd = MAC802154_HWSIM_CMD_NEW_RADIO,
+ .policy = hwsim_genl_policy,
+ .doit = hwsim_new_radio_nl,
+ .flags = GENL_UNS_ADMIN_PERM,
+ },
+ {
+ .cmd = MAC802154_HWSIM_CMD_DEL_RADIO,
+ .policy = hwsim_genl_policy,
+ .doit = hwsim_del_radio_nl,
+ .flags = GENL_UNS_ADMIN_PERM,
+ },
+ {
+ .cmd = MAC802154_HWSIM_CMD_GET_RADIO,
+ .policy = hwsim_genl_policy,
+ .doit = hwsim_get_radio_nl,
+ .dumpit = hwsim_dump_radio_nl,
+ },
+ {
+ .cmd = MAC802154_HWSIM_CMD_NEW_EDGE,
+ .policy = hwsim_genl_policy,
+ .doit = hwsim_new_edge_nl,
+ .flags = GENL_UNS_ADMIN_PERM,
+ },
+ {
+ .cmd = MAC802154_HWSIM_CMD_DEL_EDGE,
+ .policy = hwsim_genl_policy,
+ .doit = hwsim_del_edge_nl,
+ .flags = GENL_UNS_ADMIN_PERM,
+ },
+ {
+ .cmd = MAC802154_HWSIM_CMD_SET_EDGE,
+ .policy = hwsim_genl_policy,
+ .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,
+ .module = THIS_MODULE,
+ .ops = hwsim_nl_ops,
+ .n_ops = ARRAY_SIZE(hwsim_nl_ops),
+ .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;
+
+me_fail:
+ list_for_each_entry(phy, &hwsim_phys, list) {
+ list_del_rcu(&e->list);
+ hwsim_free_edge(e);
+ }
+sub_fail:
+ hwsim_edge_unsubscribe_me(phy);
+ 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(sizeof(*pib), GFP_KERNEL);
+ if (!pib) {
+ err = -ENOMEM;
+ goto err_pib;
+ }
+
+ 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)
+ goto err_reg;
+ }
+ list_add_tail(&phy->list, &hwsim_phys);
+ mutex_unlock(&hwsim_phys_lock);
+
+ hwsim_mcast_new_radio(info, phy);
+
+ return idx;
+
+err_reg:
+ kfree(pib);
+err_pib:
+ ieee802154_free_hw(phy->hw);
+ return err;
+}
+
+static void hwsim_del(struct hwsim_phy *phy)
+{
+ hwsim_edge_unsubscribe_me(phy);
+
+ list_del(&phy->list);
+ kfree_rcu(phy->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 int 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);
+
+ return 0;
+}
+
+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:
+ genl_unregister_family(&hwsim_genl_family);
+platform_dev:
+ platform_device_unregister(mac802154hwsim_dev);
+ 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);
projects / platform / kernel / linux-rpi3.git / commitdiff