package/kernel/mac80211/patches/brcm/119-v5.6-0002-brcmfmac-add-initial-support-for-monitor-mode.patch

   1 From 20f2c5fa3af060401c72e444999470a4cab641cf Mon Sep 17 00:00:00 2001
   2 From: =?UTF-8?q?Rafa=C5=82=20Mi=C5=82ecki?= <rafal@milecki.pl>
   3 Date: Thu, 26 Dec 2019 14:30:50 +0100
   4 Subject: [PATCH] brcmfmac: add initial support for monitor mode
   5 MIME-Version: 1.0
   6 Content-Type: text/plain; charset=UTF-8
   7 Content-Transfer-Encoding: 8bit
   8
   9 Report monitor interface availability using cfg80211 and support it in
  10 the add_virtual_intf() and del_virtual_intf() callbacks. This new
  11 feature is conditional and depends on firmware flagging monitor packets.
  12 Receiving monitor frames is already handled by the brcmf_netif_mon_rx().
  13
  14 Signed-off-by: Rafał Miłecki <rafal@milecki.pl>
  15 Signed-off-by: Kalle Valo <kvalo@codeaurora.org>
  16 ---
  17  .../broadcom/brcm80211/brcmfmac/cfg80211.c    | 112 ++++++++++++++++--
  18  .../broadcom/brcm80211/brcmfmac/core.c        |  68 ++++++++++-
  19  .../broadcom/brcm80211/brcmfmac/core.h        |   2 +
  20  .../broadcom/brcm80211/brcmfmac/feature.c     |   1 +
  21  .../broadcom/brcm80211/brcmfmac/feature.h     |   2 +
  22  .../broadcom/brcm80211/brcmfmac/fwil.h        |   2 +
  23  6 files changed, 174 insertions(+), 13 deletions(-)
  24
  25 --- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/cfg80211.c
  26 +++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/cfg80211.c
  27 @@ -11,6 +11,7 @@
  28  #include <linux/vmalloc.h>
  29  #include <net/cfg80211.h>
  30  #include <net/netlink.h>
  31 +#include <uapi/linux/if_arp.h>
  32
  33  #include <brcmu_utils.h>
  34  #include <defs.h>
  35 @@ -619,6 +620,82 @@ static bool brcmf_is_ibssmode(struct brc
  36         return vif->wdev.iftype == NL80211_IFTYPE_ADHOC;
  37  }
  38
  39 +/**
  40 + * brcmf_mon_add_vif() - create monitor mode virtual interface
  41 + *
  42 + * @wiphy: wiphy device of new interface.
  43 + * @name: name of the new interface.
  44 + */
  45 +static struct wireless_dev *brcmf_mon_add_vif(struct wiphy *wiphy,
  46 +                                             const char *name)
  47 +{
  48 +       struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
  49 +       struct brcmf_cfg80211_vif *vif;
  50 +       struct net_device *ndev;
  51 +       struct brcmf_if *ifp;
  52 +       int err;
  53 +
  54 +       if (cfg->pub->mon_if) {
  55 +               err = -EEXIST;
  56 +               goto err_out;
  57 +       }
  58 +
  59 +       vif = brcmf_alloc_vif(cfg, NL80211_IFTYPE_MONITOR);
  60 +       if (IS_ERR(vif)) {
  61 +               err = PTR_ERR(vif);
  62 +               goto err_out;
  63 +       }
  64 +
  65 +       ndev = alloc_netdev(sizeof(*ifp), name, NET_NAME_UNKNOWN, ether_setup);
  66 +       if (!ndev) {
  67 +               err = -ENOMEM;
  68 +               goto err_free_vif;
  69 +       }
  70 +       ndev->type = ARPHRD_IEEE80211_RADIOTAP;
  71 +       ndev->ieee80211_ptr = &vif->wdev;
  72 +       ndev->needs_free_netdev = true;
  73 +       ndev->priv_destructor = brcmf_cfg80211_free_netdev;
  74 +       SET_NETDEV_DEV(ndev, wiphy_dev(cfg->wiphy));
  75 +
  76 +       ifp = netdev_priv(ndev);
  77 +       ifp->vif = vif;
  78 +       ifp->ndev = ndev;
  79 +       ifp->drvr = cfg->pub;
  80 +
  81 +       vif->ifp = ifp;
  82 +       vif->wdev.netdev = ndev;
  83 +
  84 +       err = brcmf_net_mon_attach(ifp);
  85 +       if (err) {
  86 +               brcmf_err("Failed to attach %s device\n", ndev->name);
  87 +               free_netdev(ndev);
  88 +               goto err_free_vif;
  89 +       }
  90 +
  91 +       cfg->pub->mon_if = ifp;
  92 +
  93 +       return &vif->wdev;
  94 +
  95 +err_free_vif:
  96 +       brcmf_free_vif(vif);
  97 +err_out:
  98 +       return ERR_PTR(err);
  99 +}
 100 +
 101 +static int brcmf_mon_del_vif(struct wiphy *wiphy, struct wireless_dev *wdev)
 102 +{
 103 +       struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
 104 +       struct net_device *ndev = wdev->netdev;
 105 +
 106 +       ndev->netdev_ops->ndo_stop(ndev);
 107 +
 108 +       brcmf_net_detach(ndev, true);
 109 +
 110 +       cfg->pub->mon_if = NULL;
 111 +
 112 +       return 0;
 113 +}
 114 +
 115  static struct wireless_dev *brcmf_cfg80211_add_iface(struct wiphy *wiphy,
 116                                                      const char *name,
 117                                                      unsigned char name_assign_type,
 118 @@ -641,9 +718,10 @@ static struct wireless_dev *brcmf_cfg802
 119         case NL80211_IFTYPE_STATION:
 120         case NL80211_IFTYPE_AP_VLAN:
 121         case NL80211_IFTYPE_WDS:
 122 -       case NL80211_IFTYPE_MONITOR:
 123         case NL80211_IFTYPE_MESH_POINT:
 124                 return ERR_PTR(-EOPNOTSUPP);
 125 +       case NL80211_IFTYPE_MONITOR:
 126 +               return brcmf_mon_add_vif(wiphy, name);
 127         case NL80211_IFTYPE_AP:
 128                 wdev = brcmf_ap_add_vif(wiphy, name, params);
 129                 break;
 130 @@ -826,9 +904,10 @@ int brcmf_cfg80211_del_iface(struct wiph
 131         case NL80211_IFTYPE_STATION:
 132         case NL80211_IFTYPE_AP_VLAN:
 133         case NL80211_IFTYPE_WDS:
 134 -       case NL80211_IFTYPE_MONITOR:
 135         case NL80211_IFTYPE_MESH_POINT:
 136                 return -EOPNOTSUPP;
 137 +       case NL80211_IFTYPE_MONITOR:
 138 +               return brcmf_mon_del_vif(wiphy, wdev);
 139         case NL80211_IFTYPE_AP:
 140                 return brcmf_cfg80211_del_ap_iface(wiphy, wdev);
 141         case NL80211_IFTYPE_P2P_CLIENT:
 142 @@ -6547,9 +6626,10 @@ static int brcmf_setup_ifmodes(struct wi
 143         struct ieee80211_iface_limit *c0_limits = NULL;
 144         struct ieee80211_iface_limit *p2p_limits = NULL;
 145         struct ieee80211_iface_limit *mbss_limits = NULL;
 146 -       bool mbss, p2p, rsdb, mchan;
 147 -       int i, c, n_combos;
 148 +       bool mon_flag, mbss, p2p, rsdb, mchan;
 149 +       int i, c, n_combos, n_limits;
 150
 151 +       mon_flag = brcmf_feat_is_enabled(ifp, BRCMF_FEAT_MONITOR_FLAG);
 152         mbss = brcmf_feat_is_enabled(ifp, BRCMF_FEAT_MBSS);
 153         p2p = brcmf_feat_is_enabled(ifp, BRCMF_FEAT_P2P);
 154         rsdb = brcmf_feat_is_enabled(ifp, BRCMF_FEAT_RSDB);
 155 @@ -6563,6 +6643,8 @@ static int brcmf_setup_ifmodes(struct wi
 156         wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION) |
 157                                  BIT(NL80211_IFTYPE_ADHOC) |
 158                                  BIT(NL80211_IFTYPE_AP);
 159 +       if (mon_flag)
 160 +               wiphy->interface_modes |= BIT(NL80211_IFTYPE_MONITOR);
 161         if (p2p)
 162                 wiphy->interface_modes |= BIT(NL80211_IFTYPE_P2P_CLIENT) |
 163                                           BIT(NL80211_IFTYPE_P2P_GO) |
 164 @@ -6570,18 +6652,18 @@ static int brcmf_setup_ifmodes(struct wi
 165
 166         c = 0;
 167         i = 0;
 168 -       if (p2p && rsdb)
 169 -               c0_limits = kcalloc(4, sizeof(*c0_limits), GFP_KERNEL);
 170 -       else if (p2p)
 171 -               c0_limits = kcalloc(3, sizeof(*c0_limits), GFP_KERNEL);
 172 -       else
 173 -               c0_limits = kcalloc(2, sizeof(*c0_limits), GFP_KERNEL);
 174 +       n_limits = 1 + mon_flag + (p2p ? 2 : 0) + (rsdb || !p2p);
 175 +       c0_limits = kcalloc(n_limits, sizeof(*c0_limits), GFP_KERNEL);
 176         if (!c0_limits)
 177                 goto err;
 178
 179         combo[c].num_different_channels = 1 + (rsdb || (p2p && mchan));
 180         c0_limits[i].max = 1 + rsdb;
 181         c0_limits[i++].types = BIT(NL80211_IFTYPE_STATION);
 182 +       if (mon_flag) {
 183 +               c0_limits[i].max = 1;
 184 +               c0_limits[i++].types = BIT(NL80211_IFTYPE_MONITOR);
 185 +       }
 186         if (p2p) {
 187                 c0_limits[i].max = 1;
 188                 c0_limits[i++].types = BIT(NL80211_IFTYPE_P2P_DEVICE);
 189 @@ -6630,14 +6712,20 @@ static int brcmf_setup_ifmodes(struct wi
 190         if (mbss) {
 191                 c++;
 192                 i = 0;
 193 -               mbss_limits = kcalloc(1, sizeof(*mbss_limits), GFP_KERNEL);
 194 +               n_limits = 1 + mon_flag;
 195 +               mbss_limits = kcalloc(n_limits, sizeof(*mbss_limits),
 196 +                                     GFP_KERNEL);
 197                 if (!mbss_limits)
 198                         goto err;
 199                 mbss_limits[i].max = 4;
 200                 mbss_limits[i++].types = BIT(NL80211_IFTYPE_AP);
 201 +               if (mon_flag) {
 202 +                       mbss_limits[i].max = 1;
 203 +                       mbss_limits[i++].types = BIT(NL80211_IFTYPE_MONITOR);
 204 +               }
 205                 combo[c].beacon_int_infra_match = true;
 206                 combo[c].num_different_channels = 1;
 207 -               combo[c].max_interfaces = 4;
 208 +               combo[c].max_interfaces = 4 + mon_flag;
 209                 combo[c].n_limits = i;
 210                 combo[c].limits = mbss_limits;
 211         }
 212 --- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/core.c
 213 +++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/core.c
 214 @@ -690,7 +690,7 @@ fail:
 215         return -EBADE;
 216  }
 217
 218 -static void brcmf_net_detach(struct net_device *ndev, bool rtnl_locked)
 219 +void brcmf_net_detach(struct net_device *ndev, bool rtnl_locked)
 220  {
 221         if (ndev->reg_state == NETREG_REGISTERED) {
 222                 if (rtnl_locked)
 223 @@ -703,6 +703,72 @@ static void brcmf_net_detach(struct net_
 224         }
 225  }
 226
 227 +static int brcmf_net_mon_open(struct net_device *ndev)
 228 +{
 229 +       struct brcmf_if *ifp = netdev_priv(ndev);
 230 +       struct brcmf_pub *drvr = ifp->drvr;
 231 +       u32 monitor;
 232 +       int err;
 233 +
 234 +       brcmf_dbg(TRACE, "Enter\n");
 235 +
 236 +       err = brcmf_fil_cmd_int_get(ifp, BRCMF_C_GET_MONITOR, &monitor);
 237 +       if (err) {
 238 +               bphy_err(drvr, "BRCMF_C_GET_MONITOR error (%d)\n", err);
 239 +               return err;
 240 +       } else if (monitor) {
 241 +               bphy_err(drvr, "Monitor mode is already enabled\n");
 242 +               return -EEXIST;
 243 +       }
 244 +
 245 +       monitor = 3;
 246 +       err = brcmf_fil_cmd_int_set(ifp, BRCMF_C_SET_MONITOR, monitor);
 247 +       if (err)
 248 +               bphy_err(drvr, "BRCMF_C_SET_MONITOR error (%d)\n", err);
 249 +
 250 +       return err;
 251 +}
 252 +
 253 +static int brcmf_net_mon_stop(struct net_device *ndev)
 254 +{
 255 +       struct brcmf_if *ifp = netdev_priv(ndev);
 256 +       struct brcmf_pub *drvr = ifp->drvr;
 257 +       u32 monitor;
 258 +       int err;
 259 +
 260 +       brcmf_dbg(TRACE, "Enter\n");
 261 +
 262 +       monitor = 0;
 263 +       err = brcmf_fil_cmd_int_set(ifp, BRCMF_C_SET_MONITOR, monitor);
 264 +       if (err)
 265 +               bphy_err(drvr, "BRCMF_C_SET_MONITOR error (%d)\n", err);
 266 +
 267 +       return err;
 268 +}
 269 +
 270 +static const struct net_device_ops brcmf_netdev_ops_mon = {
 271 +       .ndo_open = brcmf_net_mon_open,
 272 +       .ndo_stop = brcmf_net_mon_stop,
 273 +};
 274 +
 275 +int brcmf_net_mon_attach(struct brcmf_if *ifp)
 276 +{
 277 +       struct brcmf_pub *drvr = ifp->drvr;
 278 +       struct net_device *ndev;
 279 +       int err;
 280 +
 281 +       brcmf_dbg(TRACE, "Enter\n");
 282 +
 283 +       ndev = ifp->ndev;
 284 +       ndev->netdev_ops = &brcmf_netdev_ops_mon;
 285 +
 286 +       err = register_netdevice(ndev);
 287 +       if (err)
 288 +               bphy_err(drvr, "Failed to register %s device\n", ndev->name);
 289 +
 290 +       return err;
 291 +}
 292 +
 293  void brcmf_net_setcarrier(struct brcmf_if *ifp, bool on)
 294  {
 295         struct net_device *ndev;
 296 --- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/core.h
 297 +++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/core.h
 298 @@ -210,6 +210,8 @@ void brcmf_txflowblock_if(struct brcmf_i
 299  void brcmf_txfinalize(struct brcmf_if *ifp, struct sk_buff *txp, bool success);
 300  void brcmf_netif_rx(struct brcmf_if *ifp, struct sk_buff *skb);
 301  void brcmf_netif_mon_rx(struct brcmf_if *ifp, struct sk_buff *skb);
 302 +void brcmf_net_detach(struct net_device *ndev, bool rtnl_locked);
 303 +int brcmf_net_mon_attach(struct brcmf_if *ifp);
 304  void brcmf_net_setcarrier(struct brcmf_if *ifp, bool on);
 305  int __init brcmf_core_init(void);
 306  void __exit brcmf_core_exit(void);
 307 --- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/feature.c
 308 +++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/feature.c
 309 @@ -38,6 +38,7 @@ static const struct brcmf_feat_fwcap brc
 310         { BRCMF_FEAT_MCHAN, "mchan" },
 311         { BRCMF_FEAT_P2P, "p2p" },
 312         { BRCMF_FEAT_MONITOR, "monitor" },
 313 +       { BRCMF_FEAT_MONITOR_FLAG, "rtap" },
 314         { BRCMF_FEAT_MONITOR_FMT_RADIOTAP, "rtap" },
 315         { BRCMF_FEAT_DOT11H, "802.11h" },
 316         { BRCMF_FEAT_SAE, "sae" },
 317 --- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/feature.h
 318 +++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/feature.h
 319 @@ -23,6 +23,7 @@
 320   * GSCAN: enhanced scan offload feature.
 321   * FWSUP: Firmware supplicant.
 322   * MONITOR: firmware can pass monitor packets to host.
 323 + * MONITOR_FLAG: firmware flags monitor packets.
 324   * MONITOR_FMT_RADIOTAP: firmware provides monitor packets with radiotap header
 325   * MONITOR_FMT_HW_RX_HDR: firmware provides monitor packets with hw/ucode header
 326   * DOT11H: firmware supports 802.11h
 327 @@ -44,6 +45,7 @@
 328         BRCMF_FEAT_DEF(GSCAN) \
 329         BRCMF_FEAT_DEF(FWSUP) \
 330         BRCMF_FEAT_DEF(MONITOR) \
 331 +       BRCMF_FEAT_DEF(MONITOR_FLAG) \
 332         BRCMF_FEAT_DEF(MONITOR_FMT_RADIOTAP) \
 333         BRCMF_FEAT_DEF(MONITOR_FMT_HW_RX_HDR) \
 334         BRCMF_FEAT_DEF(DOT11H) \
 335 --- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fwil.h
 336 +++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fwil.h
 337 @@ -49,6 +49,8 @@
 338  #define BRCMF_C_GET_PM                         85
 339  #define BRCMF_C_SET_PM                         86
 340  #define BRCMF_C_GET_REVINFO                    98
 341 +#define BRCMF_C_GET_MONITOR                    107
 342 +#define BRCMF_C_SET_MONITOR                    108
 343  #define BRCMF_C_GET_CURR_RATESET               114
 344  #define BRCMF_C_GET_AP                         117
 345  #define BRCMF_C_SET_AP                         118