git.librecmc.org Git - librecmc/librecmc.git/blob

   1 From: Felix Fietkau <nbd@nbd.name>
   2 Date: Tue, 6 Dec 2022 11:15:02 +0100
   3 Subject: [PATCH] wifi: mac80211: fix receiving A-MSDU frames on mesh
   4  interfaces
   5
   6 The current mac80211 mesh A-MSDU receive path fails to parse A-MSDU packets
   7 on mesh interfaces, because it assumes that the Mesh Control field is always
   8 directly after the 802.11 header.
   9 802.11-2020 9.3.2.2.2 Figure 9-70 shows that the Mesh Control field is
  10 actually part of the A-MSDU subframe header.
  11 This makes more sense, since it allows packets for multiple different
  12 destinations to be included in the same A-MSDU, as long as RA and TID are
  13 still the same.
  14 Another issue is the fact that the A-MSDU subframe length field was apparently
  15 accidentally defined as little-endian in the standard.
  16
  17 In order to fix this, the mesh forwarding path needs happen at a different
  18 point in the receive path.
  19
  20 ieee80211_data_to_8023_exthdr is changed to ignore the mesh control field
  21 and leave it in after the ethernet header. This also affects the source/dest
  22 MAC address fields, which now in the case of mesh point to the mesh SA/DA.
  23
  24 ieee80211_amsdu_to_8023s is changed to deal with the endian difference and
  25 to add the Mesh Control length to the subframe length, since it's not covered
  26 by the MSDU length field.
  27
  28 With these changes, the mac80211 will get the same packet structure for
  29 converted regular data packets and unpacked A-MSDU subframes.
  30
  31 The mesh forwarding checks are now only performed after the A-MSDU decap.
  32 For locally received packets, the Mesh Control header is stripped away.
  33 For forwarded packets, a new 802.11 header gets added.
  34
  35 Signed-off-by: Felix Fietkau <nbd@nbd.name>
  36 ---
  37
  38 --- a/drivers/net/wireless/marvell/mwifiex/11n_rxreorder.c
  39 +++ b/drivers/net/wireless/marvell/mwifiex/11n_rxreorder.c
  40 @@ -33,7 +33,7 @@ static int mwifiex_11n_dispatch_amsdu_pk
  41                 skb_trim(skb, le16_to_cpu(local_rx_pd->rx_pkt_length));
  42
  43                 ieee80211_amsdu_to_8023s(skb, &list, priv->curr_addr,
  44 -                                        priv->wdev.iftype, 0, NULL, NULL);
  45 +                                        priv->wdev.iftype, 0, NULL, NULL, false);
  46
  47                 while (!skb_queue_empty(&list)) {
  48                         struct rx_packet_hdr *rx_hdr;
  49 --- a/include/net/cfg80211.h
  50 +++ b/include/net/cfg80211.h
  51 @@ -6208,11 +6208,36 @@ static inline int ieee80211_data_to_8023
  52   * @extra_headroom: The hardware extra headroom for SKBs in the @list.
  53   * @check_da: DA to check in the inner ethernet header, or NULL
  54   * @check_sa: SA to check in the inner ethernet header, or NULL
  55 + * @mesh_control: A-MSDU subframe header includes the mesh control field
  56   */
  57  void ieee80211_amsdu_to_8023s(struct sk_buff *skb, struct sk_buff_head *list,
  58                               const u8 *addr, enum nl80211_iftype iftype,
  59                               const unsigned int extra_headroom,
  60 -                             const u8 *check_da, const u8 *check_sa);
  61 +                             const u8 *check_da, const u8 *check_sa,
  62 +                             bool mesh_control);
  63 +
  64 +/**
  65 + * ieee80211_get_8023_tunnel_proto - get RFC1042 or bridge tunnel encap protocol
  66 + *
  67 + * Check for RFC1042 or bridge tunnel header and fetch the encapsulated
  68 + * protocol.
  69 + *
  70 + * @hdr: pointer to the MSDU payload
  71 + * @proto: destination pointer to store the protocol
  72 + * Return: true if encapsulation was found
  73 + */
  74 +bool ieee80211_get_8023_tunnel_proto(const void *hdr, __be16 *proto);
  75 +
  76 +/**
  77 + * ieee80211_strip_8023_mesh_hdr - strip mesh header from converted 802.3 frames
  78 + *
  79 + * Strip the mesh header, which was left in by ieee80211_data_to_8023 as part
  80 + * of the MSDU data. Also move any source/destination addresses from the mesh
  81 + * header to the ethernet header (if present).
  82 + *
  83 + * @skb: The 802.3 frame with embedded mesh header
  84 + */
  85 +int ieee80211_strip_8023_mesh_hdr(struct sk_buff *skb);
  86
  87  /**
  88   * cfg80211_classify8021d - determine the 802.1p/1d tag for a data frame
  89 --- a/net/mac80211/rx.c
  90 +++ b/net/mac80211/rx.c
  91 @@ -2720,6 +2720,174 @@ ieee80211_deliver_skb(struct ieee80211_r
  92         }
  93  }
  94
  95 +static ieee80211_rx_result
  96 +ieee80211_rx_mesh_data(struct ieee80211_sub_if_data *sdata, struct sta_info *sta,
  97 +                      struct sk_buff *skb)
  98 +{
  99 +#ifdef CPTCFG_MAC80211_MESH
 100 +       struct ieee80211_if_mesh *ifmsh = &sdata->u.mesh;
 101 +       struct ieee80211_local *local = sdata->local;
 102 +       uint16_t fc = IEEE80211_FTYPE_DATA | IEEE80211_STYPE_QOS_DATA;
 103 +       struct ieee80211_hdr hdr = {
 104 +               .frame_control = cpu_to_le16(fc)
 105 +       };
 106 +       struct ieee80211_hdr *fwd_hdr;
 107 +       struct ieee80211s_hdr *mesh_hdr;
 108 +       struct ieee80211_tx_info *info;
 109 +       struct sk_buff *fwd_skb;
 110 +       struct ethhdr *eth;
 111 +       bool multicast;
 112 +       int tailroom = 0;
 113 +       int hdrlen, mesh_hdrlen;
 114 +       u8 *qos;
 115 +
 116 +       if (!ieee80211_vif_is_mesh(&sdata->vif))
 117 +               return RX_CONTINUE;
 118 +
 119 +       if (!pskb_may_pull(skb, sizeof(*eth) + 6))
 120 +               return RX_DROP_MONITOR;
 121 +
 122 +       mesh_hdr = (struct ieee80211s_hdr *)(skb->data + sizeof(*eth));
 123 +       mesh_hdrlen = ieee80211_get_mesh_hdrlen(mesh_hdr);
 124 +
 125 +       if (!pskb_may_pull(skb, sizeof(*eth) + mesh_hdrlen))
 126 +               return RX_DROP_MONITOR;
 127 +
 128 +       eth = (struct ethhdr *)skb->data;
 129 +       multicast = is_multicast_ether_addr(eth->h_dest);
 130 +
 131 +       mesh_hdr = (struct ieee80211s_hdr *)(eth + 1);
 132 +       if (!mesh_hdr->ttl)
 133 +               return RX_DROP_MONITOR;
 134 +
 135 +       /* frame is in RMC, don't forward */
 136 +       if (is_multicast_ether_addr(eth->h_dest) &&
 137 +           mesh_rmc_check(sdata, eth->h_source, mesh_hdr))
 138 +               return RX_DROP_MONITOR;
 139 +
 140 +       /* Frame has reached destination.  Don't forward */
 141 +       if (ether_addr_equal(sdata->vif.addr, eth->h_dest))
 142 +               goto rx_accept;
 143 +
 144 +       if (!ifmsh->mshcfg.dot11MeshForwarding) {
 145 +               if (is_multicast_ether_addr(eth->h_dest))
 146 +                       goto rx_accept;
 147 +
 148 +               return RX_DROP_MONITOR;
 149 +       }
 150 +
 151 +       /* forward packet */
 152 +       if (sdata->crypto_tx_tailroom_needed_cnt)
 153 +               tailroom = IEEE80211_ENCRYPT_TAILROOM;
 154 +
 155 +       if (!--mesh_hdr->ttl) {
 156 +               if (multicast)
 157 +                       goto rx_accept;
 158 +
 159 +               IEEE80211_IFSTA_MESH_CTR_INC(ifmsh, dropped_frames_ttl);
 160 +               return RX_DROP_MONITOR;
 161 +       }
 162 +
 163 +       if (mesh_hdr->flags & MESH_FLAGS_AE) {
 164 +               struct mesh_path *mppath;
 165 +               char *proxied_addr;
 166 +
 167 +               if (multicast)
 168 +                       proxied_addr = mesh_hdr->eaddr1;
 169 +               else if ((mesh_hdr->flags & MESH_FLAGS_AE) == MESH_FLAGS_AE_A5_A6)
 170 +                       /* has_a4 already checked in ieee80211_rx_mesh_check */
 171 +                       proxied_addr = mesh_hdr->eaddr2;
 172 +               else
 173 +                       return RX_DROP_MONITOR;
 174 +
 175 +               rcu_read_lock();
 176 +               mppath = mpp_path_lookup(sdata, proxied_addr);
 177 +               if (!mppath) {
 178 +                       mpp_path_add(sdata, proxied_addr, eth->h_source);
 179 +               } else {
 180 +                       spin_lock_bh(&mppath->state_lock);
 181 +                       if (!ether_addr_equal(mppath->mpp, eth->h_source))
 182 +                               memcpy(mppath->mpp, eth->h_source, ETH_ALEN);
 183 +                       mppath->exp_time = jiffies;
 184 +                       spin_unlock_bh(&mppath->state_lock);
 185 +               }
 186 +               rcu_read_unlock();
 187 +       }
 188 +
 189 +       skb_set_queue_mapping(skb, ieee802_1d_to_ac[skb->priority]);
 190 +
 191 +       ieee80211_fill_mesh_addresses(&hdr, &hdr.frame_control,
 192 +                                     eth->h_dest, eth->h_source);
 193 +       hdrlen = ieee80211_hdrlen(hdr.frame_control);
 194 +       if (multicast) {
 195 +               int extra_head = sizeof(struct ieee80211_hdr) - sizeof(*eth);
 196 +
 197 +               fwd_skb = skb_copy_expand(skb, local->tx_headroom + extra_head +
 198 +                                              IEEE80211_ENCRYPT_HEADROOM,
 199 +                                         tailroom, GFP_ATOMIC);
 200 +               if (!fwd_skb)
 201 +                       goto rx_accept;
 202 +       } else {
 203 +               fwd_skb = skb;
 204 +               skb = NULL;
 205 +
 206 +               if (skb_cow_head(fwd_skb, hdrlen - sizeof(struct ethhdr)))
 207 +                       return RX_DROP_UNUSABLE;
 208 +       }
 209 +
 210 +       fwd_hdr = skb_push(fwd_skb, hdrlen - sizeof(struct ethhdr));
 211 +       memcpy(fwd_hdr, &hdr, hdrlen - 2);
 212 +       qos = ieee80211_get_qos_ctl(fwd_hdr);
 213 +       qos[0] = qos[1] = 0;
 214 +
 215 +       skb_reset_mac_header(fwd_skb);
 216 +       hdrlen += mesh_hdrlen;
 217 +       if (ieee80211_get_8023_tunnel_proto(fwd_skb->data + hdrlen,
 218 +                                           &fwd_skb->protocol))
 219 +               hdrlen += ETH_ALEN;
 220 +       else
 221 +               fwd_skb->protocol = htons(fwd_skb->len - hdrlen);
 222 +       skb_set_network_header(fwd_skb, hdrlen);
 223 +
 224 +       info = IEEE80211_SKB_CB(fwd_skb);
 225 +       memset(info, 0, sizeof(*info));
 226 +       info->control.flags |= IEEE80211_TX_INTCFL_NEED_TXPROCESSING;
 227 +       info->control.vif = &sdata->vif;
 228 +       info->control.jiffies = jiffies;
 229 +       if (multicast) {
 230 +               IEEE80211_IFSTA_MESH_CTR_INC(ifmsh, fwded_mcast);
 231 +               memcpy(fwd_hdr->addr2, sdata->vif.addr, ETH_ALEN);
 232 +               /* update power mode indication when forwarding */
 233 +               ieee80211_mps_set_frame_flags(sdata, NULL, fwd_hdr);
 234 +       } else if (!mesh_nexthop_lookup(sdata, fwd_skb)) {
 235 +               /* mesh power mode flags updated in mesh_nexthop_lookup */
 236 +               IEEE80211_IFSTA_MESH_CTR_INC(ifmsh, fwded_unicast);
 237 +       } else {
 238 +               /* unable to resolve next hop */
 239 +               if (sta)
 240 +                       mesh_path_error_tx(sdata, ifmsh->mshcfg.element_ttl,
 241 +                                          hdr.addr3, 0,
 242 +                                          WLAN_REASON_MESH_PATH_NOFORWARD,
 243 +                                          sta->sta.addr);
 244 +               IEEE80211_IFSTA_MESH_CTR_INC(ifmsh, dropped_frames_no_route);
 245 +               kfree_skb(fwd_skb);
 246 +               goto rx_accept;
 247 +       }
 248 +
 249 +       IEEE80211_IFSTA_MESH_CTR_INC(ifmsh, fwded_frames);
 250 +       fwd_skb->dev = sdata->dev;
 251 +       ieee80211_add_pending_skb(local, fwd_skb);
 252 +
 253 +rx_accept:
 254 +       if (!skb)
 255 +               return RX_QUEUED;
 256 +
 257 +       ieee80211_strip_8023_mesh_hdr(skb);
 258 +#endif
 259 +
 260 +       return RX_CONTINUE;
 261 +}
 262 +
 263  static ieee80211_rx_result debug_noinline
 264  __ieee80211_rx_h_amsdu(struct ieee80211_rx_data *rx, u8 data_offset)
 265  {
 266 @@ -2728,8 +2896,10 @@ __ieee80211_rx_h_amsdu(struct ieee80211_
 267         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
 268         __le16 fc = hdr->frame_control;
 269         struct sk_buff_head frame_list;
 270 +       static ieee80211_rx_result res;
 271         struct ethhdr ethhdr;
 272         const u8 *check_da = ethhdr.h_dest, *check_sa = ethhdr.h_source;
 273 +       bool mesh = false;
 274
 275         if (unlikely(ieee80211_has_a4(hdr->frame_control))) {
 276                 check_da = NULL;
 277 @@ -2746,6 +2916,8 @@ __ieee80211_rx_h_amsdu(struct ieee80211_
 278                         break;
 279                 case NL80211_IFTYPE_MESH_POINT:
 280                         check_sa = NULL;
 281 +                       check_da = NULL;
 282 +                       mesh = true;
 283                         break;
 284                 default:
 285                         break;
 286 @@ -2763,17 +2935,29 @@ __ieee80211_rx_h_amsdu(struct ieee80211_
 287         ieee80211_amsdu_to_8023s(skb, &frame_list, dev->dev_addr,
 288                                  rx->sdata->vif.type,
 289                                  rx->local->hw.extra_tx_headroom,
 290 -                                check_da, check_sa);
 291 +                                check_da, check_sa, mesh);
 292
 293         while (!skb_queue_empty(&frame_list)) {
 294                 rx->skb = __skb_dequeue(&frame_list);
 295
 296 -               if (!ieee80211_frame_allowed(rx, fc)) {
 297 -                       dev_kfree_skb(rx->skb);
 298 +               res = ieee80211_rx_mesh_data(rx->sdata, rx->sta, rx->skb);
 299 +               switch (res) {
 300 +               case RX_QUEUED:
 301                         continue;
 302 +               case RX_CONTINUE:
 303 +                       break;
 304 +               default:
 305 +                       goto free;
 306                 }
 307
 308 +               if (!ieee80211_frame_allowed(rx, fc))
 309 +                       goto free;
 310 +
 311                 ieee80211_deliver_skb(rx);
 312 +               continue;
 313 +
 314 +free:
 315 +               dev_kfree_skb(rx->skb);
 316         }
 317
 318         return RX_QUEUED;
 319 @@ -2806,6 +2990,8 @@ ieee80211_rx_h_amsdu(struct ieee80211_rx
 320                         if (!rx->sdata->u.mgd.use_4addr)
 321                                 return RX_DROP_UNUSABLE;
 322                         break;
 323 +               case NL80211_IFTYPE_MESH_POINT:
 324 +                       break;
 325                 default:
 326                         return RX_DROP_UNUSABLE;
 327                 }
 328 @@ -2834,155 +3020,6 @@ ieee80211_rx_h_amsdu(struct ieee80211_rx
 329         return __ieee80211_rx_h_amsdu(rx, 0);
 330  }
 331
 332 -#ifdef CPTCFG_MAC80211_MESH
 333 -static ieee80211_rx_result
 334 -ieee80211_rx_h_mesh_fwding(struct ieee80211_rx_data *rx)
 335 -{
 336 -       struct ieee80211_hdr *fwd_hdr, *hdr;
 337 -       struct ieee80211_tx_info *info;
 338 -       struct ieee80211s_hdr *mesh_hdr;
 339 -       struct sk_buff *skb = rx->skb, *fwd_skb;
 340 -       struct ieee80211_local *local = rx->local;
 341 -       struct ieee80211_sub_if_data *sdata = rx->sdata;
 342 -       struct ieee80211_if_mesh *ifmsh = &sdata->u.mesh;
 343 -       u16 ac, q, hdrlen;
 344 -       int tailroom = 0;
 345 -
 346 -       hdr = (struct ieee80211_hdr *) skb->data;
 347 -       hdrlen = ieee80211_hdrlen(hdr->frame_control);
 348 -
 349 -       /* make sure fixed part of mesh header is there, also checks skb len */
 350 -       if (!pskb_may_pull(rx->skb, hdrlen + 6))
 351 -               return RX_DROP_MONITOR;
 352 -
 353 -       mesh_hdr = (struct ieee80211s_hdr *) (skb->data + hdrlen);
 354 -
 355 -       /* make sure full mesh header is there, also checks skb len */
 356 -       if (!pskb_may_pull(rx->skb,
 357 -                          hdrlen + ieee80211_get_mesh_hdrlen(mesh_hdr)))
 358 -               return RX_DROP_MONITOR;
 359 -
 360 -       /* reload pointers */
 361 -       hdr = (struct ieee80211_hdr *) skb->data;
 362 -       mesh_hdr = (struct ieee80211s_hdr *) (skb->data + hdrlen);
 363 -
 364 -       if (ieee80211_drop_unencrypted(rx, hdr->frame_control)) {
 365 -               int offset = hdrlen + ieee80211_get_mesh_hdrlen(mesh_hdr) +
 366 -                            sizeof(rfc1042_header);
 367 -               __be16 ethertype;
 368 -
 369 -               if (!ether_addr_equal(hdr->addr1, rx->sdata->vif.addr) ||
 370 -                   skb_copy_bits(rx->skb, offset, &ethertype, 2) != 0 ||
 371 -                   ethertype != rx->sdata->control_port_protocol)
 372 -                       return RX_DROP_MONITOR;
 373 -       }
 374 -
 375 -       /* frame is in RMC, don't forward */
 376 -       if (ieee80211_is_data(hdr->frame_control) &&
 377 -           is_multicast_ether_addr(hdr->addr1) &&
 378 -           mesh_rmc_check(rx->sdata, hdr->addr3, mesh_hdr))
 379 -               return RX_DROP_MONITOR;
 380 -
 381 -       if (!ieee80211_is_data(hdr->frame_control))
 382 -               return RX_CONTINUE;
 383 -
 384 -       if (!mesh_hdr->ttl)
 385 -               return RX_DROP_MONITOR;
 386 -
 387 -       if (mesh_hdr->flags & MESH_FLAGS_AE) {
 388 -               struct mesh_path *mppath;
 389 -               char *proxied_addr;
 390 -               char *mpp_addr;
 391 -
 392 -               if (is_multicast_ether_addr(hdr->addr1)) {
 393 -                       mpp_addr = hdr->addr3;
 394 -                       proxied_addr = mesh_hdr->eaddr1;
 395 -               } else if ((mesh_hdr->flags & MESH_FLAGS_AE) ==
 396 -                           MESH_FLAGS_AE_A5_A6) {
 397 -                       /* has_a4 already checked in ieee80211_rx_mesh_check */
 398 -                       mpp_addr = hdr->addr4;
 399 -                       proxied_addr = mesh_hdr->eaddr2;
 400 -               } else {
 401 -                       return RX_DROP_MONITOR;
 402 -               }
 403 -
 404 -               rcu_read_lock();
 405 -               mppath = mpp_path_lookup(sdata, proxied_addr);
 406 -               if (!mppath) {
 407 -                       mpp_path_add(sdata, proxied_addr, mpp_addr);
 408 -               } else {
 409 -                       spin_lock_bh(&mppath->state_lock);
 410 -                       if (!ether_addr_equal(mppath->mpp, mpp_addr))
 411 -                               memcpy(mppath->mpp, mpp_addr, ETH_ALEN);
 412 -                       mppath->exp_time = jiffies;
 413 -                       spin_unlock_bh(&mppath->state_lock);
 414 -               }
 415 -               rcu_read_unlock();
 416 -       }
 417 -
 418 -       /* Frame has reached destination.  Don't forward */
 419 -       if (!is_multicast_ether_addr(hdr->addr1) &&
 420 -           ether_addr_equal(sdata->vif.addr, hdr->addr3))
 421 -               return RX_CONTINUE;
 422 -
 423 -       ac = ieee802_1d_to_ac[skb->priority];
 424 -       skb_set_queue_mapping(skb, ac);
 425 -
 426 -       if (!--mesh_hdr->ttl) {
 427 -               if (!is_multicast_ether_addr(hdr->addr1))
 428 -                       IEEE80211_IFSTA_MESH_CTR_INC(ifmsh,
 429 -                                                    dropped_frames_ttl);
 430 -               goto out;
 431 -       }
 432 -
 433 -       if (!ifmsh->mshcfg.dot11MeshForwarding)
 434 -               goto out;
 435 -
 436 -       if (sdata->crypto_tx_tailroom_needed_cnt)
 437 -               tailroom = IEEE80211_ENCRYPT_TAILROOM;
 438 -
 439 -       fwd_skb = skb_copy_expand(skb, local->tx_headroom +
 440 -                                      IEEE80211_ENCRYPT_HEADROOM,
 441 -                                 tailroom, GFP_ATOMIC);
 442 -       if (!fwd_skb)
 443 -               goto out;
 444 -
 445 -       fwd_skb->dev = sdata->dev;
 446 -       fwd_hdr =  (struct ieee80211_hdr *) fwd_skb->data;
 447 -       fwd_hdr->frame_control &= ~cpu_to_le16(IEEE80211_FCTL_RETRY);
 448 -       info = IEEE80211_SKB_CB(fwd_skb);
 449 -       memset(info, 0, sizeof(*info));
 450 -       info->control.flags |= IEEE80211_TX_INTCFL_NEED_TXPROCESSING;
 451 -       info->control.vif = &rx->sdata->vif;
 452 -       info->control.jiffies = jiffies;
 453 -       if (is_multicast_ether_addr(fwd_hdr->addr1)) {
 454 -               IEEE80211_IFSTA_MESH_CTR_INC(ifmsh, fwded_mcast);
 455 -               memcpy(fwd_hdr->addr2, sdata->vif.addr, ETH_ALEN);
 456 -               /* update power mode indication when forwarding */
 457 -               ieee80211_mps_set_frame_flags(sdata, NULL, fwd_hdr);
 458 -       } else if (!mesh_nexthop_lookup(sdata, fwd_skb)) {
 459 -               /* mesh power mode flags updated in mesh_nexthop_lookup */
 460 -               IEEE80211_IFSTA_MESH_CTR_INC(ifmsh, fwded_unicast);
 461 -       } else {
 462 -               /* unable to resolve next hop */
 463 -               mesh_path_error_tx(sdata, ifmsh->mshcfg.element_ttl,
 464 -                                  fwd_hdr->addr3, 0,
 465 -                                  WLAN_REASON_MESH_PATH_NOFORWARD,
 466 -                                  fwd_hdr->addr2);
 467 -               IEEE80211_IFSTA_MESH_CTR_INC(ifmsh, dropped_frames_no_route);
 468 -               kfree_skb(fwd_skb);
 469 -               return RX_DROP_MONITOR;
 470 -       }
 471 -
 472 -       IEEE80211_IFSTA_MESH_CTR_INC(ifmsh, fwded_frames);
 473 -       ieee80211_add_pending_skb(local, fwd_skb);
 474 - out:
 475 -       if (is_multicast_ether_addr(hdr->addr1))
 476 -               return RX_CONTINUE;
 477 -       return RX_DROP_MONITOR;
 478 -}
 479 -#endif
 480 -
 481  static ieee80211_rx_result debug_noinline
 482  ieee80211_rx_h_data(struct ieee80211_rx_data *rx)
 483  {
 484 @@ -2991,6 +3028,7 @@ ieee80211_rx_h_data(struct ieee80211_rx_
 485         struct net_device *dev = sdata->dev;
 486         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)rx->skb->data;
 487         __le16 fc = hdr->frame_control;
 488 +       static ieee80211_rx_result res;
 489         bool port_control;
 490         int err;
 491
 492 @@ -3017,6 +3055,10 @@ ieee80211_rx_h_data(struct ieee80211_rx_
 493         if (unlikely(err))
 494                 return RX_DROP_UNUSABLE;
 495
 496 +       res = ieee80211_rx_mesh_data(rx->sdata, rx->sta, rx->skb);
 497 +       if (res != RX_CONTINUE)
 498 +               return res;
 499 +
 500         if (!ieee80211_frame_allowed(rx, fc))
 501                 return RX_DROP_MONITOR;
 502
 503 @@ -3987,10 +4029,6 @@ static void ieee80211_rx_handlers(struct
 504                 CALL_RXH(ieee80211_rx_h_defragment);
 505                 CALL_RXH(ieee80211_rx_h_michael_mic_verify);
 506                 /* must be after MMIC verify so header is counted in MPDU mic */
 507 -#ifdef CPTCFG_MAC80211_MESH
 508 -               if (ieee80211_vif_is_mesh(&rx->sdata->vif))
 509 -                       CALL_RXH(ieee80211_rx_h_mesh_fwding);
 510 -#endif
 511                 CALL_RXH(ieee80211_rx_h_amsdu);
 512                 CALL_RXH(ieee80211_rx_h_data);
 513
 514 --- a/net/wireless/util.c
 515 +++ b/net/wireless/util.c
 516 @@ -542,7 +542,7 @@ unsigned int ieee80211_get_mesh_hdrlen(s
 517  }
 518  EXPORT_SYMBOL(ieee80211_get_mesh_hdrlen);
 519
 520 -static bool ieee80211_get_8023_tunnel_proto(const void *hdr, __be16 *proto)
 521 +bool ieee80211_get_8023_tunnel_proto(const void *hdr, __be16 *proto)
 522  {
 523         const __be16 *hdr_proto = hdr + ETH_ALEN;
 524
 525 @@ -556,6 +556,49 @@ static bool ieee80211_get_8023_tunnel_pr
 526
 527         return true;
 528  }
 529 +EXPORT_SYMBOL(ieee80211_get_8023_tunnel_proto);
 530 +
 531 +int ieee80211_strip_8023_mesh_hdr(struct sk_buff *skb)
 532 +{
 533 +       const void *mesh_addr;
 534 +       struct {
 535 +               struct ethhdr eth;
 536 +               u8 flags;
 537 +       } payload;
 538 +       int hdrlen;
 539 +       int ret;
 540 +
 541 +       ret = skb_copy_bits(skb, 0, &payload, sizeof(payload));
 542 +       if (ret)
 543 +               return ret;
 544 +
 545 +       hdrlen = sizeof(payload.eth) + __ieee80211_get_mesh_hdrlen(payload.flags);
 546 +
 547 +       if (likely(pskb_may_pull(skb, hdrlen + 8) &&
 548 +                  ieee80211_get_8023_tunnel_proto(skb->data + hdrlen,
 549 +                                                  &payload.eth.h_proto)))
 550 +               hdrlen += ETH_ALEN + 2;
 551 +       else if (!pskb_may_pull(skb, hdrlen))
 552 +               return -EINVAL;
 553 +
 554 +       mesh_addr = skb->data + sizeof(payload.eth) + ETH_ALEN;
 555 +       switch (payload.flags & MESH_FLAGS_AE) {
 556 +       case MESH_FLAGS_AE_A4:
 557 +               memcpy(&payload.eth.h_source, mesh_addr, ETH_ALEN);
 558 +               break;
 559 +       case MESH_FLAGS_AE_A5_A6:
 560 +               memcpy(&payload.eth.h_dest, mesh_addr, 2 * ETH_ALEN);
 561 +               break;
 562 +       default:
 563 +               break;
 564 +       }
 565 +
 566 +       pskb_pull(skb, hdrlen - sizeof(payload.eth));
 567 +       memcpy(skb->data, &payload.eth, sizeof(payload.eth));
 568 +
 569 +       return 0;
 570 +}
 571 +EXPORT_SYMBOL(ieee80211_strip_8023_mesh_hdr);
 572
 573  int ieee80211_data_to_8023_exthdr(struct sk_buff *skb, struct ethhdr *ehdr,
 574                                   const u8 *addr, enum nl80211_iftype iftype,
 575 @@ -568,7 +611,6 @@ int ieee80211_data_to_8023_exthdr(struct
 576         } payload;
 577         struct ethhdr tmp;
 578         u16 hdrlen;
 579 -       u8 mesh_flags = 0;
 580
 581         if (unlikely(!ieee80211_is_data_present(hdr->frame_control)))
 582                 return -1;
 583 @@ -589,12 +631,6 @@ int ieee80211_data_to_8023_exthdr(struct
 584         memcpy(tmp.h_dest, ieee80211_get_DA(hdr), ETH_ALEN);
 585         memcpy(tmp.h_source, ieee80211_get_SA(hdr), ETH_ALEN);
 586
 587 -       if (iftype == NL80211_IFTYPE_MESH_POINT &&
 588 -           skb_copy_bits(skb, hdrlen, &mesh_flags, 1) < 0)
 589 -               return -1;
 590 -
 591 -       mesh_flags &= MESH_FLAGS_AE;
 592 -
 593         switch (hdr->frame_control &
 594                 cpu_to_le16(IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS)) {
 595         case cpu_to_le16(IEEE80211_FCTL_TODS):
 596 @@ -608,17 +644,6 @@ int ieee80211_data_to_8023_exthdr(struct
 597                              iftype != NL80211_IFTYPE_AP_VLAN &&
 598                              iftype != NL80211_IFTYPE_STATION))
 599                         return -1;
 600 -               if (iftype == NL80211_IFTYPE_MESH_POINT) {
 601 -                       if (mesh_flags == MESH_FLAGS_AE_A4)
 602 -                               return -1;
 603 -                       if (mesh_flags == MESH_FLAGS_AE_A5_A6 &&
 604 -                           skb_copy_bits(skb, hdrlen +
 605 -                                         offsetof(struct ieee80211s_hdr, eaddr1),
 606 -                                         tmp.h_dest, 2 * ETH_ALEN) < 0)
 607 -                               return -1;
 608 -
 609 -                       hdrlen += __ieee80211_get_mesh_hdrlen(mesh_flags);
 610 -               }
 611                 break;
 612         case cpu_to_le16(IEEE80211_FCTL_FROMDS):
 613                 if ((iftype != NL80211_IFTYPE_STATION &&
 614 @@ -627,16 +652,6 @@ int ieee80211_data_to_8023_exthdr(struct
 615                     (is_multicast_ether_addr(tmp.h_dest) &&
 616                      ether_addr_equal(tmp.h_source, addr)))
 617                         return -1;
 618 -               if (iftype == NL80211_IFTYPE_MESH_POINT) {
 619 -                       if (mesh_flags == MESH_FLAGS_AE_A5_A6)
 620 -                               return -1;
 621 -                       if (mesh_flags == MESH_FLAGS_AE_A4 &&
 622 -                           skb_copy_bits(skb, hdrlen +
 623 -                                         offsetof(struct ieee80211s_hdr, eaddr1),
 624 -                                         tmp.h_source, ETH_ALEN) < 0)
 625 -                               return -1;
 626 -                       hdrlen += __ieee80211_get_mesh_hdrlen(mesh_flags);
 627 -               }
 628                 break;
 629         case cpu_to_le16(0):
 630                 if (iftype != NL80211_IFTYPE_ADHOC &&
 631 @@ -646,7 +661,7 @@ int ieee80211_data_to_8023_exthdr(struct
 632                 break;
 633         }
 634
 635 -       if (likely(!is_amsdu &&
 636 +       if (likely(!is_amsdu && iftype != NL80211_IFTYPE_MESH_POINT &&
 637                    skb_copy_bits(skb, hdrlen, &payload, sizeof(payload)) == 0 &&
 638                    ieee80211_get_8023_tunnel_proto(&payload, &tmp.h_proto))) {
 639                 /* remove RFC1042 or Bridge-Tunnel encapsulation */
 640 @@ -722,7 +737,8 @@ __ieee80211_amsdu_copy_frag(struct sk_bu
 641
 642  static struct sk_buff *
 643  __ieee80211_amsdu_copy(struct sk_buff *skb, unsigned int hlen,
 644 -                      int offset, int len, bool reuse_frag)
 645 +                      int offset, int len, bool reuse_frag,
 646 +                      int min_len)
 647  {
 648         struct sk_buff *frame;
 649         int cur_len = len;
 650 @@ -736,7 +752,7 @@ __ieee80211_amsdu_copy(struct sk_buff *s
 651          * in the stack later.
 652          */
 653         if (reuse_frag)
 654 -               cur_len = min_t(int, len, 32);
 655 +               cur_len = min_t(int, len, min_len);
 656
 657         /*
 658          * Allocate and reserve two bytes more for payload
 659 @@ -746,6 +762,7 @@ __ieee80211_amsdu_copy(struct sk_buff *s
 660         if (!frame)
 661                 return NULL;
 662
 663 +       frame->priority = skb->priority;
 664         skb_reserve(frame, hlen + sizeof(struct ethhdr) + 2);
 665         skb_copy_bits(skb, offset, skb_put(frame, cur_len), cur_len);
 666
 667 @@ -762,23 +779,37 @@ __ieee80211_amsdu_copy(struct sk_buff *s
 668  void ieee80211_amsdu_to_8023s(struct sk_buff *skb, struct sk_buff_head *list,
 669                               const u8 *addr, enum nl80211_iftype iftype,
 670                               const unsigned int extra_headroom,
 671 -                             const u8 *check_da, const u8 *check_sa)
 672 +                             const u8 *check_da, const u8 *check_sa,
 673 +                             bool mesh_control)
 674  {
 675         unsigned int hlen = ALIGN(extra_headroom, 4);
 676         struct sk_buff *frame = NULL;
 677         int offset = 0, remaining;
 678 -       struct ethhdr eth;
 679 +       struct {
 680 +               struct ethhdr eth;
 681 +               uint8_t flags;
 682 +       } hdr;
 683         bool reuse_frag = skb->head_frag && !skb_has_frag_list(skb);
 684         bool reuse_skb = false;
 685         bool last = false;
 686 +       int copy_len = sizeof(hdr.eth);
 687 +
 688 +       if (iftype == NL80211_IFTYPE_MESH_POINT)
 689 +               copy_len = sizeof(hdr);
 690
 691         while (!last) {
 692                 unsigned int subframe_len;
 693 -               int len;
 694 +               int len, mesh_len = 0;
 695                 u8 padding;
 696
 697 -               skb_copy_bits(skb, offset, &eth, sizeof(eth));
 698 -               len = ntohs(eth.h_proto);
 699 +               skb_copy_bits(skb, offset, &hdr, copy_len);
 700 +               if (iftype == NL80211_IFTYPE_MESH_POINT)
 701 +                       mesh_len = __ieee80211_get_mesh_hdrlen(hdr.flags);
 702 +               if (mesh_control)
 703 +                       len = le16_to_cpu(*(__le16 *)&hdr.eth.h_proto) + mesh_len;
 704 +               else
 705 +                       len = ntohs(hdr.eth.h_proto);
 706 +
 707                 subframe_len = sizeof(struct ethhdr) + len;
 708                 padding = (4 - subframe_len) & 0x3;
 709
 710 @@ -787,16 +818,16 @@ void ieee80211_amsdu_to_8023s(struct sk_
 711                 if (subframe_len > remaining)
 712                         goto purge;
 713                 /* mitigate A-MSDU aggregation injection attacks */
 714 -               if (ether_addr_equal(eth.h_dest, rfc1042_header))
 715 +               if (ether_addr_equal(hdr.eth.h_dest, rfc1042_header))
 716                         goto purge;
 717
 718                 offset += sizeof(struct ethhdr);
 719                 last = remaining <= subframe_len + padding;
 720
 721                 /* FIXME: should we really accept multicast DA? */
 722 -               if ((check_da && !is_multicast_ether_addr(eth.h_dest) &&
 723 -                    !ether_addr_equal(check_da, eth.h_dest)) ||
 724 -                   (check_sa && !ether_addr_equal(check_sa, eth.h_source))) {
 725 +               if ((check_da && !is_multicast_ether_addr(hdr.eth.h_dest) &&
 726 +                    !ether_addr_equal(check_da, hdr.eth.h_dest)) ||
 727 +                   (check_sa && !ether_addr_equal(check_sa, hdr.eth.h_source))) {
 728                         offset += len + padding;
 729                         continue;
 730                 }
 731 @@ -808,7 +839,7 @@ void ieee80211_amsdu_to_8023s(struct sk_
 732                         reuse_skb = true;
 733                 } else {
 734                         frame = __ieee80211_amsdu_copy(skb, hlen, offset, len,
 735 -                                                      reuse_frag);
 736 +                                                      reuse_frag, 32 + mesh_len);
 737                         if (!frame)
 738                                 goto purge;
 739
 740 @@ -819,10 +850,11 @@ void ieee80211_amsdu_to_8023s(struct sk_
 741                 frame->dev = skb->dev;
 742                 frame->priority = skb->priority;
 743
 744 -               if (likely(ieee80211_get_8023_tunnel_proto(frame->data, &eth.h_proto)))
 745 +               if (likely(iftype != NL80211_IFTYPE_MESH_POINT &&
 746 +                          ieee80211_get_8023_tunnel_proto(frame->data, &hdr.eth.h_proto)))
 747                         skb_pull(frame, ETH_ALEN + 2);
 748
 749 -               memcpy(skb_push(frame, sizeof(eth)), &eth, sizeof(eth));
 750 +               memcpy(skb_push(frame, sizeof(hdr.eth)), &hdr.eth, sizeof(hdr.eth));
 751                 __skb_queue_tail(list, frame);
 752         }
 753