+static const struct nla_policy sta_flags_policy[NL80211_STA_FLAG_MAX + 1] = {
+ [NL80211_STA_FLAG_AUTHORIZED] = { .type = NLA_FLAG },
+ [NL80211_STA_FLAG_SHORT_PREAMBLE] = { .type = NLA_FLAG },
+ [NL80211_STA_FLAG_WME] = { .type = NLA_FLAG },
+};
+
+static int parse_station_flags(struct nlattr *nla, u32 *staflags)
+{
+ struct nlattr *flags[NL80211_STA_FLAG_MAX + 1];
+ int flag;
+
+ *staflags = 0;
+
+ if (!nla)
+ return 0;
+
+ if (nla_parse_nested(flags, NL80211_STA_FLAG_MAX,
+ nla, sta_flags_policy))
+ return -EINVAL;
+
+ *staflags = STATION_FLAG_CHANGED;
+
+ for (flag = 1; flag <= NL80211_STA_FLAG_MAX; flag++)
+ if (flags[flag])
+ *staflags |= (1<<flag);
+
+ return 0;
+}
+
+static int nl80211_send_station(struct sk_buff *msg, u32 pid, u32 seq,
+ int flags, struct net_device *dev,
+ u8 *mac_addr, struct station_info *sinfo)
+{
+ void *hdr;
+ struct nlattr *sinfoattr;
+
+ hdr = nl80211hdr_put(msg, pid, seq, flags, NL80211_CMD_NEW_STATION);
+ if (!hdr)
+ return -1;
+
+ NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, dev->ifindex);
+ NLA_PUT(msg, NL80211_ATTR_MAC, ETH_ALEN, mac_addr);
+
+ sinfoattr = nla_nest_start(msg, NL80211_ATTR_STA_INFO);
+ if (!sinfoattr)
+ goto nla_put_failure;
+ if (sinfo->filled & STATION_INFO_INACTIVE_TIME)
+ NLA_PUT_U32(msg, NL80211_STA_INFO_INACTIVE_TIME,
+ sinfo->inactive_time);
+ if (sinfo->filled & STATION_INFO_RX_BYTES)
+ NLA_PUT_U32(msg, NL80211_STA_INFO_RX_BYTES,
+ sinfo->rx_bytes);
+ if (sinfo->filled & STATION_INFO_TX_BYTES)
+ NLA_PUT_U32(msg, NL80211_STA_INFO_TX_BYTES,
+ sinfo->tx_bytes);
+ if (sinfo->filled & STATION_INFO_LLID)
+ NLA_PUT_U16(msg, NL80211_STA_INFO_LLID,
+ sinfo->llid);
+ if (sinfo->filled & STATION_INFO_PLID)
+ NLA_PUT_U16(msg, NL80211_STA_INFO_PLID,
+ sinfo->plid);
+ if (sinfo->filled & STATION_INFO_PLINK_STATE)
+ NLA_PUT_U8(msg, NL80211_STA_INFO_PLINK_STATE,
+ sinfo->plink_state);
+
+ nla_nest_end(msg, sinfoattr);
+
+ return genlmsg_end(msg, hdr);
+
+ nla_put_failure:
+ return genlmsg_cancel(msg, hdr);
+}
+
+static int nl80211_dump_station(struct sk_buff *skb,
+ struct netlink_callback *cb)
+{
+ int wp_idx = 0;
+ int if_idx = 0;
+ int sta_idx = cb->args[2];
+ int wp_start = cb->args[0];
+ int if_start = cb->args[1];
+ struct station_info sinfo;
+ struct cfg80211_registered_device *dev;
+ struct wireless_dev *wdev;
+ u8 mac_addr[ETH_ALEN];
+ int err;
+ int exit = 0;
+
+ /* TODO: filter by device */
+ mutex_lock(&cfg80211_drv_mutex);
+ list_for_each_entry(dev, &cfg80211_drv_list, list) {
+ if (exit)
+ break;
+ if (++wp_idx < wp_start)
+ continue;
+ if_idx = 0;
+
+ mutex_lock(&dev->devlist_mtx);
+ list_for_each_entry(wdev, &dev->netdev_list, list) {
+ if (exit)
+ break;
+ if (++if_idx < if_start)
+ continue;
+ if (!dev->ops->dump_station)
+ continue;
+
+ for (;; ++sta_idx) {
+ rtnl_lock();
+ err = dev->ops->dump_station(&dev->wiphy,
+ wdev->netdev, sta_idx, mac_addr,
+ &sinfo);
+ rtnl_unlock();
+ if (err) {
+ sta_idx = 0;
+ break;
+ }
+ if (nl80211_send_station(skb,
+ NETLINK_CB(cb->skb).pid,
+ cb->nlh->nlmsg_seq, NLM_F_MULTI,
+ wdev->netdev, mac_addr,
+ &sinfo) < 0) {
+ exit = 1;
+ break;
+ }
+ }
+ }
+ mutex_unlock(&dev->devlist_mtx);
+ }
+ mutex_unlock(&cfg80211_drv_mutex);
+
+ cb->args[0] = wp_idx;
+ cb->args[1] = if_idx;
+ cb->args[2] = sta_idx;
+
+ return skb->len;
+}
+
+static int nl80211_get_station(struct sk_buff *skb, struct genl_info *info)
+{
+ struct cfg80211_registered_device *drv;
+ int err;
+ struct net_device *dev;
+ struct station_info sinfo;
+ struct sk_buff *msg;
+ u8 *mac_addr = NULL;
+
+ memset(&sinfo, 0, sizeof(sinfo));
+
+ if (!info->attrs[NL80211_ATTR_MAC])
+ return -EINVAL;
+
+ mac_addr = nla_data(info->attrs[NL80211_ATTR_MAC]);
+
+ err = get_drv_dev_by_info_ifindex(info, &drv, &dev);
+ if (err)
+ return err;
+
+ if (!drv->ops->get_station) {
+ err = -EOPNOTSUPP;
+ goto out;
+ }
+
+ rtnl_lock();
+ err = drv->ops->get_station(&drv->wiphy, dev, mac_addr, &sinfo);
+ rtnl_unlock();
+
+ if (err)
+ goto out;
+
+ msg = nlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
+ if (!msg)
+ goto out;
+
+ if (nl80211_send_station(msg, info->snd_pid, info->snd_seq, 0,
+ dev, mac_addr, &sinfo) < 0)
+ goto out_free;
+
+ err = genlmsg_unicast(msg, info->snd_pid);
+ goto out;
+
+ out_free:
+ nlmsg_free(msg);
+
+ out:
+ cfg80211_put_dev(drv);
+ dev_put(dev);
+ return err;
+}
+
+/*
+ * Get vlan interface making sure it is on the right wiphy.
+ */
+static int get_vlan(struct nlattr *vlanattr,
+ struct cfg80211_registered_device *rdev,
+ struct net_device **vlan)
+{
+ *vlan = NULL;
+
+ if (vlanattr) {
+ *vlan = dev_get_by_index(&init_net, nla_get_u32(vlanattr));
+ if (!*vlan)
+ return -ENODEV;
+ if (!(*vlan)->ieee80211_ptr)
+ return -EINVAL;
+ if ((*vlan)->ieee80211_ptr->wiphy != &rdev->wiphy)
+ return -EINVAL;
+ }
+ return 0;
+}
+
+static int nl80211_set_station(struct sk_buff *skb, struct genl_info *info)
+{
+ struct cfg80211_registered_device *drv;
+ int err;
+ struct net_device *dev;
+ struct station_parameters params;
+ u8 *mac_addr = NULL;
+
+ memset(¶ms, 0, sizeof(params));
+
+ params.listen_interval = -1;
+
+ if (info->attrs[NL80211_ATTR_STA_AID])
+ return -EINVAL;
+
+ if (!info->attrs[NL80211_ATTR_MAC])
+ return -EINVAL;
+
+ mac_addr = nla_data(info->attrs[NL80211_ATTR_MAC]);
+
+ if (info->attrs[NL80211_ATTR_STA_SUPPORTED_RATES]) {
+ params.supported_rates =
+ nla_data(info->attrs[NL80211_ATTR_STA_SUPPORTED_RATES]);
+ params.supported_rates_len =
+ nla_len(info->attrs[NL80211_ATTR_STA_SUPPORTED_RATES]);
+ }
+
+ if (info->attrs[NL80211_ATTR_STA_LISTEN_INTERVAL])
+ params.listen_interval =
+ nla_get_u16(info->attrs[NL80211_ATTR_STA_LISTEN_INTERVAL]);
+
+ if (parse_station_flags(info->attrs[NL80211_ATTR_STA_FLAGS],
+ ¶ms.station_flags))
+ return -EINVAL;
+
+ if (info->attrs[NL80211_ATTR_STA_PLINK_ACTION])
+ params.plink_action =
+ nla_get_u8(info->attrs[NL80211_ATTR_STA_PLINK_ACTION]);
+
+ err = get_drv_dev_by_info_ifindex(info, &drv, &dev);
+ if (err)
+ return err;
+
+ err = get_vlan(info->attrs[NL80211_ATTR_STA_VLAN], drv, ¶ms.vlan);
+ if (err)
+ goto out;
+
+ if (!drv->ops->change_station) {
+ err = -EOPNOTSUPP;
+ goto out;
+ }
+
+ rtnl_lock();
+ err = drv->ops->change_station(&drv->wiphy, dev, mac_addr, ¶ms);
+ rtnl_unlock();
+
+ out:
+ if (params.vlan)
+ dev_put(params.vlan);
+ cfg80211_put_dev(drv);
+ dev_put(dev);
+ return err;
+}
+
+static int nl80211_new_station(struct sk_buff *skb, struct genl_info *info)
+{
+ struct cfg80211_registered_device *drv;
+ int err;
+ struct net_device *dev;
+ struct station_parameters params;
+ u8 *mac_addr = NULL;
+
+ memset(¶ms, 0, sizeof(params));
+
+ if (!info->attrs[NL80211_ATTR_MAC])
+ return -EINVAL;
+
+ if (!info->attrs[NL80211_ATTR_STA_AID])
+ return -EINVAL;
+
+ if (!info->attrs[NL80211_ATTR_STA_LISTEN_INTERVAL])
+ return -EINVAL;
+
+ if (!info->attrs[NL80211_ATTR_STA_SUPPORTED_RATES])
+ return -EINVAL;
+
+ mac_addr = nla_data(info->attrs[NL80211_ATTR_MAC]);
+ params.supported_rates =
+ nla_data(info->attrs[NL80211_ATTR_STA_SUPPORTED_RATES]);
+ params.supported_rates_len =
+ nla_len(info->attrs[NL80211_ATTR_STA_SUPPORTED_RATES]);
+ params.listen_interval =
+ nla_get_u16(info->attrs[NL80211_ATTR_STA_LISTEN_INTERVAL]);
+ params.listen_interval = nla_get_u16(info->attrs[NL80211_ATTR_STA_AID]);
+
+ if (parse_station_flags(info->attrs[NL80211_ATTR_STA_FLAGS],
+ ¶ms.station_flags))
+ return -EINVAL;
+
+ err = get_drv_dev_by_info_ifindex(info, &drv, &dev);
+ if (err)
+ return err;
+
+ err = get_vlan(info->attrs[NL80211_ATTR_STA_VLAN], drv, ¶ms.vlan);
+ if (err)
+ goto out;
+
+ if (!drv->ops->add_station) {
+ err = -EOPNOTSUPP;
+ goto out;
+ }
+
+ rtnl_lock();
+ err = drv->ops->add_station(&drv->wiphy, dev, mac_addr, ¶ms);
+ rtnl_unlock();
+
+ out:
+ if (params.vlan)
+ dev_put(params.vlan);
+ cfg80211_put_dev(drv);
+ dev_put(dev);
+ return err;
+}
+
+static int nl80211_del_station(struct sk_buff *skb, struct genl_info *info)
+{
+ struct cfg80211_registered_device *drv;
+ int err;
+ struct net_device *dev;
+ u8 *mac_addr = NULL;
+
+ if (info->attrs[NL80211_ATTR_MAC])
+ mac_addr = nla_data(info->attrs[NL80211_ATTR_MAC]);
+
+ err = get_drv_dev_by_info_ifindex(info, &drv, &dev);
+ if (err)
+ return err;
+
+ if (!drv->ops->del_station) {
+ err = -EOPNOTSUPP;
+ goto out;
+ }
+
+ rtnl_lock();
+ err = drv->ops->del_station(&drv->wiphy, dev, mac_addr);
+ rtnl_unlock();
+
+ out:
+ cfg80211_put_dev(drv);
+ dev_put(dev);
+ return err;
+}
+
+static int nl80211_send_mpath(struct sk_buff *msg, u32 pid, u32 seq,
+ int flags, struct net_device *dev,
+ u8 *dst, u8 *next_hop,
+ struct mpath_info *pinfo)
+{
+ void *hdr;
+ struct nlattr *pinfoattr;
+
+ hdr = nl80211hdr_put(msg, pid, seq, flags, NL80211_CMD_NEW_STATION);
+ if (!hdr)
+ return -1;
+
+ NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, dev->ifindex);
+ NLA_PUT(msg, NL80211_ATTR_MAC, ETH_ALEN, dst);
+ NLA_PUT(msg, NL80211_ATTR_MPATH_NEXT_HOP, ETH_ALEN, next_hop);
+
+ pinfoattr = nla_nest_start(msg, NL80211_ATTR_MPATH_INFO);
+ if (!pinfoattr)
+ goto nla_put_failure;
+ if (pinfo->filled & MPATH_INFO_FRAME_QLEN)
+ NLA_PUT_U32(msg, NL80211_MPATH_INFO_FRAME_QLEN,
+ pinfo->frame_qlen);
+ if (pinfo->filled & MPATH_INFO_DSN)
+ NLA_PUT_U32(msg, NL80211_MPATH_INFO_DSN,
+ pinfo->dsn);
+ if (pinfo->filled & MPATH_INFO_METRIC)
+ NLA_PUT_U32(msg, NL80211_MPATH_INFO_METRIC,
+ pinfo->metric);
+ if (pinfo->filled & MPATH_INFO_EXPTIME)
+ NLA_PUT_U32(msg, NL80211_MPATH_INFO_EXPTIME,
+ pinfo->exptime);
+ if (pinfo->filled & MPATH_INFO_FLAGS)
+ NLA_PUT_U8(msg, NL80211_MPATH_INFO_FLAGS,
+ pinfo->flags);
+ if (pinfo->filled & MPATH_INFO_DISCOVERY_TIMEOUT)
+ NLA_PUT_U32(msg, NL80211_MPATH_INFO_DISCOVERY_TIMEOUT,
+ pinfo->discovery_timeout);
+ if (pinfo->filled & MPATH_INFO_DISCOVERY_RETRIES)
+ NLA_PUT_U8(msg, NL80211_MPATH_INFO_DISCOVERY_RETRIES,
+ pinfo->discovery_retries);
+
+ nla_nest_end(msg, pinfoattr);
+
+ return genlmsg_end(msg, hdr);
+
+ nla_put_failure:
+ return genlmsg_cancel(msg, hdr);
+}
+
+static int nl80211_dump_mpath(struct sk_buff *skb,
+ struct netlink_callback *cb)
+{
+ int wp_idx = 0;
+ int if_idx = 0;
+ int sta_idx = cb->args[2];
+ int wp_start = cb->args[0];
+ int if_start = cb->args[1];
+ struct mpath_info pinfo;
+ struct cfg80211_registered_device *dev;
+ struct wireless_dev *wdev;
+ u8 dst[ETH_ALEN];
+ u8 next_hop[ETH_ALEN];
+ int err;
+ int exit = 0;
+
+ /* TODO: filter by device */
+ mutex_lock(&cfg80211_drv_mutex);
+ list_for_each_entry(dev, &cfg80211_drv_list, list) {
+ if (exit)
+ break;
+ if (++wp_idx < wp_start)
+ continue;
+ if_idx = 0;
+
+ mutex_lock(&dev->devlist_mtx);
+ list_for_each_entry(wdev, &dev->netdev_list, list) {
+ if (exit)
+ break;
+ if (++if_idx < if_start)
+ continue;
+ if (!dev->ops->dump_mpath)
+ continue;
+
+ for (;; ++sta_idx) {
+ rtnl_lock();
+ err = dev->ops->dump_mpath(&dev->wiphy,
+ wdev->netdev, sta_idx, dst,
+ next_hop, &pinfo);
+ rtnl_unlock();
+ if (err) {
+ sta_idx = 0;
+ break;
+ }
+ if (nl80211_send_mpath(skb,
+ NETLINK_CB(cb->skb).pid,
+ cb->nlh->nlmsg_seq, NLM_F_MULTI,
+ wdev->netdev, dst, next_hop,
+ &pinfo) < 0) {
+ exit = 1;
+ break;
+ }
+ }
+ }
+ mutex_unlock(&dev->devlist_mtx);
+ }
+ mutex_unlock(&cfg80211_drv_mutex);
+
+ cb->args[0] = wp_idx;
+ cb->args[1] = if_idx;
+ cb->args[2] = sta_idx;
+
+ return skb->len;
+}
+
+static int nl80211_get_mpath(struct sk_buff *skb, struct genl_info *info)
+{
+ struct cfg80211_registered_device *drv;
+ int err;
+ struct net_device *dev;
+ struct mpath_info pinfo;
+ struct sk_buff *msg;
+ u8 *dst = NULL;
+ u8 next_hop[ETH_ALEN];
+
+ memset(&pinfo, 0, sizeof(pinfo));
+
+ if (!info->attrs[NL80211_ATTR_MAC])
+ return -EINVAL;
+
+ dst = nla_data(info->attrs[NL80211_ATTR_MAC]);
+
+ err = get_drv_dev_by_info_ifindex(info, &drv, &dev);
+ if (err)
+ return err;
+
+ if (!drv->ops->get_mpath) {
+ err = -EOPNOTSUPP;
+ goto out;
+ }
+
+ rtnl_lock();
+ err = drv->ops->get_mpath(&drv->wiphy, dev, dst, next_hop, &pinfo);
+ rtnl_unlock();
+
+ if (err)
+ goto out;
+
+ msg = nlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
+ if (!msg)
+ goto out;
+
+ if (nl80211_send_mpath(msg, info->snd_pid, info->snd_seq, 0,
+ dev, dst, next_hop, &pinfo) < 0)
+ goto out_free;
+
+ err = genlmsg_unicast(msg, info->snd_pid);
+ goto out;
+
+ out_free:
+ nlmsg_free(msg);
+
+ out:
+ cfg80211_put_dev(drv);
+ dev_put(dev);
+ return err;
+}
+
+static int nl80211_set_mpath(struct sk_buff *skb, struct genl_info *info)
+{
+ struct cfg80211_registered_device *drv;
+ int err;
+ struct net_device *dev;
+ u8 *dst = NULL;
+ u8 *next_hop = NULL;
+
+ if (!info->attrs[NL80211_ATTR_MAC])
+ return -EINVAL;
+
+ if (!info->attrs[NL80211_ATTR_MPATH_NEXT_HOP])
+ return -EINVAL;
+
+ dst = nla_data(info->attrs[NL80211_ATTR_MAC]);
+ next_hop = nla_data(info->attrs[NL80211_ATTR_MPATH_NEXT_HOP]);
+
+ err = get_drv_dev_by_info_ifindex(info, &drv, &dev);
+ if (err)
+ return err;
+
+ if (!drv->ops->change_mpath) {
+ err = -EOPNOTSUPP;
+ goto out;
+ }
+
+ rtnl_lock();
+ err = drv->ops->change_mpath(&drv->wiphy, dev, dst, next_hop);
+ rtnl_unlock();
+
+ out:
+ cfg80211_put_dev(drv);
+ dev_put(dev);
+ return err;
+}
+static int nl80211_new_mpath(struct sk_buff *skb, struct genl_info *info)
+{
+ struct cfg80211_registered_device *drv;
+ int err;
+ struct net_device *dev;
+ u8 *dst = NULL;
+ u8 *next_hop = NULL;
+
+ if (!info->attrs[NL80211_ATTR_MAC])
+ return -EINVAL;
+
+ if (!info->attrs[NL80211_ATTR_MPATH_NEXT_HOP])
+ return -EINVAL;
+
+ dst = nla_data(info->attrs[NL80211_ATTR_MAC]);
+ next_hop = nla_data(info->attrs[NL80211_ATTR_MPATH_NEXT_HOP]);
+
+ err = get_drv_dev_by_info_ifindex(info, &drv, &dev);
+ if (err)
+ return err;
+
+ if (!drv->ops->add_mpath) {
+ err = -EOPNOTSUPP;
+ goto out;
+ }
+
+ rtnl_lock();
+ err = drv->ops->add_mpath(&drv->wiphy, dev, dst, next_hop);
+ rtnl_unlock();
+
+ out:
+ cfg80211_put_dev(drv);
+ dev_put(dev);
+ return err;
+}
+
+static int nl80211_del_mpath(struct sk_buff *skb, struct genl_info *info)
+{
+ struct cfg80211_registered_device *drv;
+ int err;
+ struct net_device *dev;
+ u8 *dst = NULL;
+
+ if (info->attrs[NL80211_ATTR_MAC])
+ dst = nla_data(info->attrs[NL80211_ATTR_MAC]);
+
+ err = get_drv_dev_by_info_ifindex(info, &drv, &dev);
+ if (err)
+ return err;
+
+ if (!drv->ops->del_mpath) {
+ err = -EOPNOTSUPP;
+ goto out;
+ }
+
+ rtnl_lock();
+ err = drv->ops->del_mpath(&drv->wiphy, dev, dst);
+ rtnl_unlock();
+
+ out:
+ cfg80211_put_dev(drv);
+ dev_put(dev);
+ return err;
+}
+