Back to home page

OSCL-LXR
 
 

     

0001 // SPDX-License-Identifier: GPL-2.0-only
0002 /*
0003  * HWSIM IEEE 802.15.4 interface
0004  *
0005  * (C) 2018 Mojatau, Alexander Aring <aring@mojatau.com>
0006  * Copyright 2007-2012 Siemens AG
0007  *
0008  * Based on fakelb, original Written by:
0009  * Sergey Lapin <slapin@ossfans.org>
0010  * Dmitry Eremin-Solenikov <dbaryshkov@gmail.com>
0011  * Alexander Smirnov <alex.bluesman.smirnov@gmail.com>
0012  */
0013 
0014 #include <linux/module.h>
0015 #include <linux/timer.h>
0016 #include <linux/platform_device.h>
0017 #include <linux/rtnetlink.h>
0018 #include <linux/netdevice.h>
0019 #include <linux/device.h>
0020 #include <linux/spinlock.h>
0021 #include <net/mac802154.h>
0022 #include <net/cfg802154.h>
0023 #include <net/genetlink.h>
0024 #include "mac802154_hwsim.h"
0025 
0026 MODULE_DESCRIPTION("Software simulator of IEEE 802.15.4 radio(s) for mac802154");
0027 MODULE_LICENSE("GPL");
0028 
0029 static LIST_HEAD(hwsim_phys);
0030 static DEFINE_MUTEX(hwsim_phys_lock);
0031 
0032 static struct platform_device *mac802154hwsim_dev;
0033 
0034 /* MAC802154_HWSIM netlink family */
0035 static struct genl_family hwsim_genl_family;
0036 
0037 static int hwsim_radio_idx;
0038 
0039 enum hwsim_multicast_groups {
0040     HWSIM_MCGRP_CONFIG,
0041 };
0042 
0043 static const struct genl_multicast_group hwsim_mcgrps[] = {
0044     [HWSIM_MCGRP_CONFIG] = { .name = "config", },
0045 };
0046 
0047 struct hwsim_pib {
0048     u8 page;
0049     u8 channel;
0050 
0051     struct rcu_head rcu;
0052 };
0053 
0054 struct hwsim_edge_info {
0055     u8 lqi;
0056 
0057     struct rcu_head rcu;
0058 };
0059 
0060 struct hwsim_edge {
0061     struct hwsim_phy *endpoint;
0062     struct hwsim_edge_info __rcu *info;
0063 
0064     struct list_head list;
0065     struct rcu_head rcu;
0066 };
0067 
0068 struct hwsim_phy {
0069     struct ieee802154_hw *hw;
0070     u32 idx;
0071 
0072     struct hwsim_pib __rcu *pib;
0073 
0074     bool suspended;
0075     struct list_head edges;
0076 
0077     struct list_head list;
0078 };
0079 
0080 static int hwsim_add_one(struct genl_info *info, struct device *dev,
0081              bool init);
0082 static void hwsim_del(struct hwsim_phy *phy);
0083 
0084 static int hwsim_hw_ed(struct ieee802154_hw *hw, u8 *level)
0085 {
0086     *level = 0xbe;
0087 
0088     return 0;
0089 }
0090 
0091 static int hwsim_hw_channel(struct ieee802154_hw *hw, u8 page, u8 channel)
0092 {
0093     struct hwsim_phy *phy = hw->priv;
0094     struct hwsim_pib *pib, *pib_old;
0095 
0096     pib = kzalloc(sizeof(*pib), GFP_KERNEL);
0097     if (!pib)
0098         return -ENOMEM;
0099 
0100     pib->page = page;
0101     pib->channel = channel;
0102 
0103     pib_old = rtnl_dereference(phy->pib);
0104     rcu_assign_pointer(phy->pib, pib);
0105     kfree_rcu(pib_old, rcu);
0106     return 0;
0107 }
0108 
0109 static int hwsim_hw_xmit(struct ieee802154_hw *hw, struct sk_buff *skb)
0110 {
0111     struct hwsim_phy *current_phy = hw->priv;
0112     struct hwsim_pib *current_pib, *endpoint_pib;
0113     struct hwsim_edge_info *einfo;
0114     struct hwsim_edge *e;
0115 
0116     WARN_ON(current_phy->suspended);
0117 
0118     rcu_read_lock();
0119     current_pib = rcu_dereference(current_phy->pib);
0120     list_for_each_entry_rcu(e, &current_phy->edges, list) {
0121         /* Can be changed later in rx_irqsafe, but this is only a
0122          * performance tweak. Received radio should drop the frame
0123          * in mac802154 stack anyway... so we don't need to be
0124          * 100% of locking here to check on suspended
0125          */
0126         if (e->endpoint->suspended)
0127             continue;
0128 
0129         endpoint_pib = rcu_dereference(e->endpoint->pib);
0130         if (current_pib->page == endpoint_pib->page &&
0131             current_pib->channel == endpoint_pib->channel) {
0132             struct sk_buff *newskb = pskb_copy(skb, GFP_ATOMIC);
0133 
0134             einfo = rcu_dereference(e->info);
0135             if (newskb)
0136                 ieee802154_rx_irqsafe(e->endpoint->hw, newskb,
0137                               einfo->lqi);
0138         }
0139     }
0140     rcu_read_unlock();
0141 
0142     ieee802154_xmit_complete(hw, skb, false);
0143     return 0;
0144 }
0145 
0146 static int hwsim_hw_start(struct ieee802154_hw *hw)
0147 {
0148     struct hwsim_phy *phy = hw->priv;
0149 
0150     phy->suspended = false;
0151     return 0;
0152 }
0153 
0154 static void hwsim_hw_stop(struct ieee802154_hw *hw)
0155 {
0156     struct hwsim_phy *phy = hw->priv;
0157 
0158     phy->suspended = true;
0159 }
0160 
0161 static int
0162 hwsim_set_promiscuous_mode(struct ieee802154_hw *hw, const bool on)
0163 {
0164     return 0;
0165 }
0166 
0167 static const struct ieee802154_ops hwsim_ops = {
0168     .owner = THIS_MODULE,
0169     .xmit_async = hwsim_hw_xmit,
0170     .ed = hwsim_hw_ed,
0171     .set_channel = hwsim_hw_channel,
0172     .start = hwsim_hw_start,
0173     .stop = hwsim_hw_stop,
0174     .set_promiscuous_mode = hwsim_set_promiscuous_mode,
0175 };
0176 
0177 static int hwsim_new_radio_nl(struct sk_buff *msg, struct genl_info *info)
0178 {
0179     return hwsim_add_one(info, &mac802154hwsim_dev->dev, false);
0180 }
0181 
0182 static int hwsim_del_radio_nl(struct sk_buff *msg, struct genl_info *info)
0183 {
0184     struct hwsim_phy *phy, *tmp;
0185     s64 idx = -1;
0186 
0187     if (!info->attrs[MAC802154_HWSIM_ATTR_RADIO_ID])
0188         return -EINVAL;
0189 
0190     idx = nla_get_u32(info->attrs[MAC802154_HWSIM_ATTR_RADIO_ID]);
0191 
0192     mutex_lock(&hwsim_phys_lock);
0193     list_for_each_entry_safe(phy, tmp, &hwsim_phys, list) {
0194         if (idx == phy->idx) {
0195             hwsim_del(phy);
0196             mutex_unlock(&hwsim_phys_lock);
0197             return 0;
0198         }
0199     }
0200     mutex_unlock(&hwsim_phys_lock);
0201 
0202     return -ENODEV;
0203 }
0204 
0205 static int append_radio_msg(struct sk_buff *skb, struct hwsim_phy *phy)
0206 {
0207     struct nlattr *nl_edges, *nl_edge;
0208     struct hwsim_edge_info *einfo;
0209     struct hwsim_edge *e;
0210     int ret;
0211 
0212     ret = nla_put_u32(skb, MAC802154_HWSIM_ATTR_RADIO_ID, phy->idx);
0213     if (ret < 0)
0214         return ret;
0215 
0216     rcu_read_lock();
0217     if (list_empty(&phy->edges)) {
0218         rcu_read_unlock();
0219         return 0;
0220     }
0221 
0222     nl_edges = nla_nest_start_noflag(skb,
0223                      MAC802154_HWSIM_ATTR_RADIO_EDGES);
0224     if (!nl_edges) {
0225         rcu_read_unlock();
0226         return -ENOBUFS;
0227     }
0228 
0229     list_for_each_entry_rcu(e, &phy->edges, list) {
0230         nl_edge = nla_nest_start_noflag(skb,
0231                         MAC802154_HWSIM_ATTR_RADIO_EDGE);
0232         if (!nl_edge) {
0233             rcu_read_unlock();
0234             nla_nest_cancel(skb, nl_edges);
0235             return -ENOBUFS;
0236         }
0237 
0238         ret = nla_put_u32(skb, MAC802154_HWSIM_EDGE_ATTR_ENDPOINT_ID,
0239                   e->endpoint->idx);
0240         if (ret < 0) {
0241             rcu_read_unlock();
0242             nla_nest_cancel(skb, nl_edge);
0243             nla_nest_cancel(skb, nl_edges);
0244             return ret;
0245         }
0246 
0247         einfo = rcu_dereference(e->info);
0248         ret = nla_put_u8(skb, MAC802154_HWSIM_EDGE_ATTR_LQI,
0249                  einfo->lqi);
0250         if (ret < 0) {
0251             rcu_read_unlock();
0252             nla_nest_cancel(skb, nl_edge);
0253             nla_nest_cancel(skb, nl_edges);
0254             return ret;
0255         }
0256 
0257         nla_nest_end(skb, nl_edge);
0258     }
0259     rcu_read_unlock();
0260 
0261     nla_nest_end(skb, nl_edges);
0262 
0263     return 0;
0264 }
0265 
0266 static int hwsim_get_radio(struct sk_buff *skb, struct hwsim_phy *phy,
0267                u32 portid, u32 seq,
0268                struct netlink_callback *cb, int flags)
0269 {
0270     void *hdr;
0271     int res;
0272 
0273     hdr = genlmsg_put(skb, portid, seq, &hwsim_genl_family, flags,
0274               MAC802154_HWSIM_CMD_GET_RADIO);
0275     if (!hdr)
0276         return -EMSGSIZE;
0277 
0278     if (cb)
0279         genl_dump_check_consistent(cb, hdr);
0280 
0281     res = append_radio_msg(skb, phy);
0282     if (res < 0)
0283         goto out_err;
0284 
0285     genlmsg_end(skb, hdr);
0286     return 0;
0287 
0288 out_err:
0289     genlmsg_cancel(skb, hdr);
0290     return res;
0291 }
0292 
0293 static int hwsim_get_radio_nl(struct sk_buff *msg, struct genl_info *info)
0294 {
0295     struct hwsim_phy *phy;
0296     struct sk_buff *skb;
0297     int idx, res = -ENODEV;
0298 
0299     if (!info->attrs[MAC802154_HWSIM_ATTR_RADIO_ID])
0300         return -EINVAL;
0301     idx = nla_get_u32(info->attrs[MAC802154_HWSIM_ATTR_RADIO_ID]);
0302 
0303     mutex_lock(&hwsim_phys_lock);
0304     list_for_each_entry(phy, &hwsim_phys, list) {
0305         if (phy->idx != idx)
0306             continue;
0307 
0308         skb = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC);
0309         if (!skb) {
0310             res = -ENOMEM;
0311             goto out_err;
0312         }
0313 
0314         res = hwsim_get_radio(skb, phy, info->snd_portid,
0315                       info->snd_seq, NULL, 0);
0316         if (res < 0) {
0317             nlmsg_free(skb);
0318             goto out_err;
0319         }
0320 
0321         res = genlmsg_reply(skb, info);
0322         break;
0323     }
0324 
0325 out_err:
0326     mutex_unlock(&hwsim_phys_lock);
0327 
0328     return res;
0329 }
0330 
0331 static int hwsim_dump_radio_nl(struct sk_buff *skb,
0332                    struct netlink_callback *cb)
0333 {
0334     int idx = cb->args[0];
0335     struct hwsim_phy *phy;
0336     int res;
0337 
0338     mutex_lock(&hwsim_phys_lock);
0339 
0340     if (idx == hwsim_radio_idx)
0341         goto done;
0342 
0343     list_for_each_entry(phy, &hwsim_phys, list) {
0344         if (phy->idx < idx)
0345             continue;
0346 
0347         res = hwsim_get_radio(skb, phy, NETLINK_CB(cb->skb).portid,
0348                       cb->nlh->nlmsg_seq, cb, NLM_F_MULTI);
0349         if (res < 0)
0350             break;
0351 
0352         idx = phy->idx + 1;
0353     }
0354 
0355     cb->args[0] = idx;
0356 
0357 done:
0358     mutex_unlock(&hwsim_phys_lock);
0359     return skb->len;
0360 }
0361 
0362 /* caller need to held hwsim_phys_lock */
0363 static struct hwsim_phy *hwsim_get_radio_by_id(uint32_t idx)
0364 {
0365     struct hwsim_phy *phy;
0366 
0367     list_for_each_entry(phy, &hwsim_phys, list) {
0368         if (phy->idx == idx)
0369             return phy;
0370     }
0371 
0372     return NULL;
0373 }
0374 
0375 static const struct nla_policy hwsim_edge_policy[MAC802154_HWSIM_EDGE_ATTR_MAX + 1] = {
0376     [MAC802154_HWSIM_EDGE_ATTR_ENDPOINT_ID] = { .type = NLA_U32 },
0377     [MAC802154_HWSIM_EDGE_ATTR_LQI] = { .type = NLA_U8 },
0378 };
0379 
0380 static struct hwsim_edge *hwsim_alloc_edge(struct hwsim_phy *endpoint, u8 lqi)
0381 {
0382     struct hwsim_edge_info *einfo;
0383     struct hwsim_edge *e;
0384 
0385     e = kzalloc(sizeof(*e), GFP_KERNEL);
0386     if (!e)
0387         return NULL;
0388 
0389     einfo = kzalloc(sizeof(*einfo), GFP_KERNEL);
0390     if (!einfo) {
0391         kfree(e);
0392         return NULL;
0393     }
0394 
0395     einfo->lqi = 0xff;
0396     rcu_assign_pointer(e->info, einfo);
0397     e->endpoint = endpoint;
0398 
0399     return e;
0400 }
0401 
0402 static void hwsim_free_edge(struct hwsim_edge *e)
0403 {
0404     struct hwsim_edge_info *einfo;
0405 
0406     rcu_read_lock();
0407     einfo = rcu_dereference(e->info);
0408     rcu_read_unlock();
0409 
0410     kfree_rcu(einfo, rcu);
0411     kfree_rcu(e, rcu);
0412 }
0413 
0414 static int hwsim_new_edge_nl(struct sk_buff *msg, struct genl_info *info)
0415 {
0416     struct nlattr *edge_attrs[MAC802154_HWSIM_EDGE_ATTR_MAX + 1];
0417     struct hwsim_phy *phy_v0, *phy_v1;
0418     struct hwsim_edge *e;
0419     u32 v0, v1;
0420 
0421     if (!info->attrs[MAC802154_HWSIM_ATTR_RADIO_ID] ||
0422         !info->attrs[MAC802154_HWSIM_ATTR_RADIO_EDGE])
0423         return -EINVAL;
0424 
0425     if (nla_parse_nested_deprecated(edge_attrs, MAC802154_HWSIM_EDGE_ATTR_MAX, info->attrs[MAC802154_HWSIM_ATTR_RADIO_EDGE], hwsim_edge_policy, NULL))
0426         return -EINVAL;
0427 
0428     if (!edge_attrs[MAC802154_HWSIM_EDGE_ATTR_ENDPOINT_ID])
0429         return -EINVAL;
0430 
0431     v0 = nla_get_u32(info->attrs[MAC802154_HWSIM_ATTR_RADIO_ID]);
0432     v1 = nla_get_u32(edge_attrs[MAC802154_HWSIM_EDGE_ATTR_ENDPOINT_ID]);
0433 
0434     if (v0 == v1)
0435         return -EINVAL;
0436 
0437     mutex_lock(&hwsim_phys_lock);
0438     phy_v0 = hwsim_get_radio_by_id(v0);
0439     if (!phy_v0) {
0440         mutex_unlock(&hwsim_phys_lock);
0441         return -ENOENT;
0442     }
0443 
0444     phy_v1 = hwsim_get_radio_by_id(v1);
0445     if (!phy_v1) {
0446         mutex_unlock(&hwsim_phys_lock);
0447         return -ENOENT;
0448     }
0449 
0450     rcu_read_lock();
0451     list_for_each_entry_rcu(e, &phy_v0->edges, list) {
0452         if (e->endpoint->idx == v1) {
0453             mutex_unlock(&hwsim_phys_lock);
0454             rcu_read_unlock();
0455             return -EEXIST;
0456         }
0457     }
0458     rcu_read_unlock();
0459 
0460     e = hwsim_alloc_edge(phy_v1, 0xff);
0461     if (!e) {
0462         mutex_unlock(&hwsim_phys_lock);
0463         return -ENOMEM;
0464     }
0465     list_add_rcu(&e->list, &phy_v0->edges);
0466     /* wait until changes are done under hwsim_phys_lock lock
0467      * should prevent of calling this function twice while
0468      * edges list has not the changes yet.
0469      */
0470     synchronize_rcu();
0471     mutex_unlock(&hwsim_phys_lock);
0472 
0473     return 0;
0474 }
0475 
0476 static int hwsim_del_edge_nl(struct sk_buff *msg, struct genl_info *info)
0477 {
0478     struct nlattr *edge_attrs[MAC802154_HWSIM_EDGE_ATTR_MAX + 1];
0479     struct hwsim_phy *phy_v0;
0480     struct hwsim_edge *e;
0481     u32 v0, v1;
0482 
0483     if (!info->attrs[MAC802154_HWSIM_ATTR_RADIO_ID] ||
0484         !info->attrs[MAC802154_HWSIM_ATTR_RADIO_EDGE])
0485         return -EINVAL;
0486 
0487     if (nla_parse_nested_deprecated(edge_attrs, MAC802154_HWSIM_EDGE_ATTR_MAX, info->attrs[MAC802154_HWSIM_ATTR_RADIO_EDGE], hwsim_edge_policy, NULL))
0488         return -EINVAL;
0489 
0490     if (!edge_attrs[MAC802154_HWSIM_EDGE_ATTR_ENDPOINT_ID])
0491         return -EINVAL;
0492 
0493     v0 = nla_get_u32(info->attrs[MAC802154_HWSIM_ATTR_RADIO_ID]);
0494     v1 = nla_get_u32(edge_attrs[MAC802154_HWSIM_EDGE_ATTR_ENDPOINT_ID]);
0495 
0496     mutex_lock(&hwsim_phys_lock);
0497     phy_v0 = hwsim_get_radio_by_id(v0);
0498     if (!phy_v0) {
0499         mutex_unlock(&hwsim_phys_lock);
0500         return -ENOENT;
0501     }
0502 
0503     rcu_read_lock();
0504     list_for_each_entry_rcu(e, &phy_v0->edges, list) {
0505         if (e->endpoint->idx == v1) {
0506             rcu_read_unlock();
0507             list_del_rcu(&e->list);
0508             hwsim_free_edge(e);
0509             /* same again - wait until list changes are done */
0510             synchronize_rcu();
0511             mutex_unlock(&hwsim_phys_lock);
0512             return 0;
0513         }
0514     }
0515     rcu_read_unlock();
0516 
0517     mutex_unlock(&hwsim_phys_lock);
0518 
0519     return -ENOENT;
0520 }
0521 
0522 static int hwsim_set_edge_lqi(struct sk_buff *msg, struct genl_info *info)
0523 {
0524     struct nlattr *edge_attrs[MAC802154_HWSIM_EDGE_ATTR_MAX + 1];
0525     struct hwsim_edge_info *einfo;
0526     struct hwsim_phy *phy_v0;
0527     struct hwsim_edge *e;
0528     u32 v0, v1;
0529     u8 lqi;
0530 
0531     if (!info->attrs[MAC802154_HWSIM_ATTR_RADIO_ID] ||
0532         !info->attrs[MAC802154_HWSIM_ATTR_RADIO_EDGE])
0533         return -EINVAL;
0534 
0535     if (nla_parse_nested_deprecated(edge_attrs, MAC802154_HWSIM_EDGE_ATTR_MAX, info->attrs[MAC802154_HWSIM_ATTR_RADIO_EDGE], hwsim_edge_policy, NULL))
0536         return -EINVAL;
0537 
0538     if (!edge_attrs[MAC802154_HWSIM_EDGE_ATTR_ENDPOINT_ID] ||
0539         !edge_attrs[MAC802154_HWSIM_EDGE_ATTR_LQI])
0540         return -EINVAL;
0541 
0542     v0 = nla_get_u32(info->attrs[MAC802154_HWSIM_ATTR_RADIO_ID]);
0543     v1 = nla_get_u32(edge_attrs[MAC802154_HWSIM_EDGE_ATTR_ENDPOINT_ID]);
0544     lqi = nla_get_u8(edge_attrs[MAC802154_HWSIM_EDGE_ATTR_LQI]);
0545 
0546     mutex_lock(&hwsim_phys_lock);
0547     phy_v0 = hwsim_get_radio_by_id(v0);
0548     if (!phy_v0) {
0549         mutex_unlock(&hwsim_phys_lock);
0550         return -ENOENT;
0551     }
0552 
0553     einfo = kzalloc(sizeof(*einfo), GFP_KERNEL);
0554     if (!einfo) {
0555         mutex_unlock(&hwsim_phys_lock);
0556         return -ENOMEM;
0557     }
0558 
0559     rcu_read_lock();
0560     list_for_each_entry_rcu(e, &phy_v0->edges, list) {
0561         if (e->endpoint->idx == v1) {
0562             einfo->lqi = lqi;
0563             rcu_assign_pointer(e->info, einfo);
0564             rcu_read_unlock();
0565             mutex_unlock(&hwsim_phys_lock);
0566             return 0;
0567         }
0568     }
0569     rcu_read_unlock();
0570 
0571     kfree(einfo);
0572     mutex_unlock(&hwsim_phys_lock);
0573 
0574     return -ENOENT;
0575 }
0576 
0577 /* MAC802154_HWSIM netlink policy */
0578 
0579 static const struct nla_policy hwsim_genl_policy[MAC802154_HWSIM_ATTR_MAX + 1] = {
0580     [MAC802154_HWSIM_ATTR_RADIO_ID] = { .type = NLA_U32 },
0581     [MAC802154_HWSIM_ATTR_RADIO_EDGE] = { .type = NLA_NESTED },
0582     [MAC802154_HWSIM_ATTR_RADIO_EDGES] = { .type = NLA_NESTED },
0583 };
0584 
0585 /* Generic Netlink operations array */
0586 static const struct genl_small_ops hwsim_nl_ops[] = {
0587     {
0588         .cmd = MAC802154_HWSIM_CMD_NEW_RADIO,
0589         .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
0590         .doit = hwsim_new_radio_nl,
0591         .flags = GENL_UNS_ADMIN_PERM,
0592     },
0593     {
0594         .cmd = MAC802154_HWSIM_CMD_DEL_RADIO,
0595         .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
0596         .doit = hwsim_del_radio_nl,
0597         .flags = GENL_UNS_ADMIN_PERM,
0598     },
0599     {
0600         .cmd = MAC802154_HWSIM_CMD_GET_RADIO,
0601         .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
0602         .doit = hwsim_get_radio_nl,
0603         .dumpit = hwsim_dump_radio_nl,
0604     },
0605     {
0606         .cmd = MAC802154_HWSIM_CMD_NEW_EDGE,
0607         .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
0608         .doit = hwsim_new_edge_nl,
0609         .flags = GENL_UNS_ADMIN_PERM,
0610     },
0611     {
0612         .cmd = MAC802154_HWSIM_CMD_DEL_EDGE,
0613         .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
0614         .doit = hwsim_del_edge_nl,
0615         .flags = GENL_UNS_ADMIN_PERM,
0616     },
0617     {
0618         .cmd = MAC802154_HWSIM_CMD_SET_EDGE,
0619         .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
0620         .doit = hwsim_set_edge_lqi,
0621         .flags = GENL_UNS_ADMIN_PERM,
0622     },
0623 };
0624 
0625 static struct genl_family hwsim_genl_family __ro_after_init = {
0626     .name = "MAC802154_HWSIM",
0627     .version = 1,
0628     .maxattr = MAC802154_HWSIM_ATTR_MAX,
0629     .policy = hwsim_genl_policy,
0630     .module = THIS_MODULE,
0631     .small_ops = hwsim_nl_ops,
0632     .n_small_ops = ARRAY_SIZE(hwsim_nl_ops),
0633     .mcgrps = hwsim_mcgrps,
0634     .n_mcgrps = ARRAY_SIZE(hwsim_mcgrps),
0635 };
0636 
0637 static void hwsim_mcast_config_msg(struct sk_buff *mcast_skb,
0638                    struct genl_info *info)
0639 {
0640     if (info)
0641         genl_notify(&hwsim_genl_family, mcast_skb, info,
0642                 HWSIM_MCGRP_CONFIG, GFP_KERNEL);
0643     else
0644         genlmsg_multicast(&hwsim_genl_family, mcast_skb, 0,
0645                   HWSIM_MCGRP_CONFIG, GFP_KERNEL);
0646 }
0647 
0648 static void hwsim_mcast_new_radio(struct genl_info *info, struct hwsim_phy *phy)
0649 {
0650     struct sk_buff *mcast_skb;
0651     void *data;
0652 
0653     mcast_skb = genlmsg_new(GENLMSG_DEFAULT_SIZE, GFP_KERNEL);
0654     if (!mcast_skb)
0655         return;
0656 
0657     data = genlmsg_put(mcast_skb, 0, 0, &hwsim_genl_family, 0,
0658                MAC802154_HWSIM_CMD_NEW_RADIO);
0659     if (!data)
0660         goto out_err;
0661 
0662     if (append_radio_msg(mcast_skb, phy) < 0)
0663         goto out_err;
0664 
0665     genlmsg_end(mcast_skb, data);
0666 
0667     hwsim_mcast_config_msg(mcast_skb, info);
0668     return;
0669 
0670 out_err:
0671     genlmsg_cancel(mcast_skb, data);
0672     nlmsg_free(mcast_skb);
0673 }
0674 
0675 static void hwsim_edge_unsubscribe_me(struct hwsim_phy *phy)
0676 {
0677     struct hwsim_phy *tmp;
0678     struct hwsim_edge *e;
0679 
0680     rcu_read_lock();
0681     /* going to all phy edges and remove phy from it */
0682     list_for_each_entry(tmp, &hwsim_phys, list) {
0683         list_for_each_entry_rcu(e, &tmp->edges, list) {
0684             if (e->endpoint->idx == phy->idx) {
0685                 list_del_rcu(&e->list);
0686                 hwsim_free_edge(e);
0687             }
0688         }
0689     }
0690     rcu_read_unlock();
0691 
0692     synchronize_rcu();
0693 }
0694 
0695 static int hwsim_subscribe_all_others(struct hwsim_phy *phy)
0696 {
0697     struct hwsim_phy *sub;
0698     struct hwsim_edge *e;
0699 
0700     list_for_each_entry(sub, &hwsim_phys, list) {
0701         e = hwsim_alloc_edge(sub, 0xff);
0702         if (!e)
0703             goto me_fail;
0704 
0705         list_add_rcu(&e->list, &phy->edges);
0706     }
0707 
0708     list_for_each_entry(sub, &hwsim_phys, list) {
0709         e = hwsim_alloc_edge(phy, 0xff);
0710         if (!e)
0711             goto sub_fail;
0712 
0713         list_add_rcu(&e->list, &sub->edges);
0714     }
0715 
0716     return 0;
0717 
0718 sub_fail:
0719     hwsim_edge_unsubscribe_me(phy);
0720 me_fail:
0721     rcu_read_lock();
0722     list_for_each_entry_rcu(e, &phy->edges, list) {
0723         list_del_rcu(&e->list);
0724         hwsim_free_edge(e);
0725     }
0726     rcu_read_unlock();
0727     return -ENOMEM;
0728 }
0729 
0730 static int hwsim_add_one(struct genl_info *info, struct device *dev,
0731              bool init)
0732 {
0733     struct ieee802154_hw *hw;
0734     struct hwsim_phy *phy;
0735     struct hwsim_pib *pib;
0736     int idx;
0737     int err;
0738 
0739     idx = hwsim_radio_idx++;
0740 
0741     hw = ieee802154_alloc_hw(sizeof(*phy), &hwsim_ops);
0742     if (!hw)
0743         return -ENOMEM;
0744 
0745     phy = hw->priv;
0746     phy->hw = hw;
0747 
0748     /* 868 MHz BPSK 802.15.4-2003 */
0749     hw->phy->supported.channels[0] |= 1;
0750     /* 915 MHz BPSK 802.15.4-2003 */
0751     hw->phy->supported.channels[0] |= 0x7fe;
0752     /* 2.4 GHz O-QPSK 802.15.4-2003 */
0753     hw->phy->supported.channels[0] |= 0x7FFF800;
0754     /* 868 MHz ASK 802.15.4-2006 */
0755     hw->phy->supported.channels[1] |= 1;
0756     /* 915 MHz ASK 802.15.4-2006 */
0757     hw->phy->supported.channels[1] |= 0x7fe;
0758     /* 868 MHz O-QPSK 802.15.4-2006 */
0759     hw->phy->supported.channels[2] |= 1;
0760     /* 915 MHz O-QPSK 802.15.4-2006 */
0761     hw->phy->supported.channels[2] |= 0x7fe;
0762     /* 2.4 GHz CSS 802.15.4a-2007 */
0763     hw->phy->supported.channels[3] |= 0x3fff;
0764     /* UWB Sub-gigahertz 802.15.4a-2007 */
0765     hw->phy->supported.channels[4] |= 1;
0766     /* UWB Low band 802.15.4a-2007 */
0767     hw->phy->supported.channels[4] |= 0x1e;
0768     /* UWB High band 802.15.4a-2007 */
0769     hw->phy->supported.channels[4] |= 0xffe0;
0770     /* 750 MHz O-QPSK 802.15.4c-2009 */
0771     hw->phy->supported.channels[5] |= 0xf;
0772     /* 750 MHz MPSK 802.15.4c-2009 */
0773     hw->phy->supported.channels[5] |= 0xf0;
0774     /* 950 MHz BPSK 802.15.4d-2009 */
0775     hw->phy->supported.channels[6] |= 0x3ff;
0776     /* 950 MHz GFSK 802.15.4d-2009 */
0777     hw->phy->supported.channels[6] |= 0x3ffc00;
0778 
0779     ieee802154_random_extended_addr(&hw->phy->perm_extended_addr);
0780 
0781     /* hwsim phy channel 13 as default */
0782     hw->phy->current_channel = 13;
0783     pib = kzalloc(sizeof(*pib), GFP_KERNEL);
0784     if (!pib) {
0785         err = -ENOMEM;
0786         goto err_pib;
0787     }
0788 
0789     pib->channel = 13;
0790     rcu_assign_pointer(phy->pib, pib);
0791     phy->idx = idx;
0792     INIT_LIST_HEAD(&phy->edges);
0793 
0794     hw->flags = IEEE802154_HW_PROMISCUOUS | IEEE802154_HW_RX_DROP_BAD_CKSUM;
0795     hw->parent = dev;
0796 
0797     err = ieee802154_register_hw(hw);
0798     if (err)
0799         goto err_reg;
0800 
0801     mutex_lock(&hwsim_phys_lock);
0802     if (init) {
0803         err = hwsim_subscribe_all_others(phy);
0804         if (err < 0) {
0805             mutex_unlock(&hwsim_phys_lock);
0806             goto err_subscribe;
0807         }
0808     }
0809     list_add_tail(&phy->list, &hwsim_phys);
0810     mutex_unlock(&hwsim_phys_lock);
0811 
0812     hwsim_mcast_new_radio(info, phy);
0813 
0814     return idx;
0815 
0816 err_subscribe:
0817     ieee802154_unregister_hw(phy->hw);
0818 err_reg:
0819     kfree(pib);
0820 err_pib:
0821     ieee802154_free_hw(phy->hw);
0822     return err;
0823 }
0824 
0825 static void hwsim_del(struct hwsim_phy *phy)
0826 {
0827     struct hwsim_pib *pib;
0828     struct hwsim_edge *e;
0829 
0830     hwsim_edge_unsubscribe_me(phy);
0831 
0832     list_del(&phy->list);
0833 
0834     rcu_read_lock();
0835     list_for_each_entry_rcu(e, &phy->edges, list) {
0836         list_del_rcu(&e->list);
0837         hwsim_free_edge(e);
0838     }
0839     pib = rcu_dereference(phy->pib);
0840     rcu_read_unlock();
0841 
0842     kfree_rcu(pib, rcu);
0843 
0844     ieee802154_unregister_hw(phy->hw);
0845     ieee802154_free_hw(phy->hw);
0846 }
0847 
0848 static int hwsim_probe(struct platform_device *pdev)
0849 {
0850     struct hwsim_phy *phy, *tmp;
0851     int err, i;
0852 
0853     for (i = 0; i < 2; i++) {
0854         err = hwsim_add_one(NULL, &pdev->dev, true);
0855         if (err < 0)
0856             goto err_slave;
0857     }
0858 
0859     dev_info(&pdev->dev, "Added 2 mac802154 hwsim hardware radios\n");
0860     return 0;
0861 
0862 err_slave:
0863     mutex_lock(&hwsim_phys_lock);
0864     list_for_each_entry_safe(phy, tmp, &hwsim_phys, list)
0865         hwsim_del(phy);
0866     mutex_unlock(&hwsim_phys_lock);
0867     return err;
0868 }
0869 
0870 static int hwsim_remove(struct platform_device *pdev)
0871 {
0872     struct hwsim_phy *phy, *tmp;
0873 
0874     mutex_lock(&hwsim_phys_lock);
0875     list_for_each_entry_safe(phy, tmp, &hwsim_phys, list)
0876         hwsim_del(phy);
0877     mutex_unlock(&hwsim_phys_lock);
0878 
0879     return 0;
0880 }
0881 
0882 static struct platform_driver mac802154hwsim_driver = {
0883     .probe = hwsim_probe,
0884     .remove = hwsim_remove,
0885     .driver = {
0886             .name = "mac802154_hwsim",
0887     },
0888 };
0889 
0890 static __init int hwsim_init_module(void)
0891 {
0892     int rc;
0893 
0894     rc = genl_register_family(&hwsim_genl_family);
0895     if (rc)
0896         return rc;
0897 
0898     mac802154hwsim_dev = platform_device_register_simple("mac802154_hwsim",
0899                                  -1, NULL, 0);
0900     if (IS_ERR(mac802154hwsim_dev)) {
0901         rc = PTR_ERR(mac802154hwsim_dev);
0902         goto platform_dev;
0903     }
0904 
0905     rc = platform_driver_register(&mac802154hwsim_driver);
0906     if (rc < 0)
0907         goto platform_drv;
0908 
0909     return 0;
0910 
0911 platform_drv:
0912     platform_device_unregister(mac802154hwsim_dev);
0913 platform_dev:
0914     genl_unregister_family(&hwsim_genl_family);
0915     return rc;
0916 }
0917 
0918 static __exit void hwsim_remove_module(void)
0919 {
0920     genl_unregister_family(&hwsim_genl_family);
0921     platform_driver_unregister(&mac802154hwsim_driver);
0922     platform_device_unregister(mac802154hwsim_dev);
0923 }
0924 
0925 module_init(hwsim_init_module);
0926 module_exit(hwsim_remove_module);
[ Source navigation ] [ Diff markup ] [ Identifier search ] [ general search ]

This page was automatically generated by the 2.1.0 LXR engine.

The LXR team
Valid CSS 2.1! Valid HTML 4.01!