6 files changed, 147 insertions, 8 deletions

diff --git a/net/mac80211/driver-ops.h b/net/mac80211/driver-ops.h
index ee8b63f92f71..4f2271316650 100644
--- a/net/mac80211/driver-ops.h
+++ b/net/mac80211/driver-ops.h
@@ -371,4 +371,15 @@ static inline void drv_flush(struct ieee80211_local *local, bool drop)
         if (local->ops->flush)
                 local->ops->flush(&local->hw, drop);
 }
+
+static inline void drv_channel_switch(struct ieee80211_local *local,
+                                     struct ieee80211_channel_switch *ch_switch)
+{
+        might_sleep();
+
+        local->ops->channel_switch(&local->hw, ch_switch);
+
+        trace_drv_channel_switch(local, ch_switch);
+}
+
 #endif /* __MAC80211_DRIVER_OPS */
diff --git a/net/mac80211/driver-trace.h b/net/mac80211/driver-trace.h
index ce734b58d07a..6a9b2342a9c2 100644
--- a/net/mac80211/driver-trace.h
+++ b/net/mac80211/driver-trace.h
@@ -774,6 +774,34 @@ TRACE_EVENT(drv_flush,
         )
 );
 
+TRACE_EVENT(drv_channel_switch,
+        TP_PROTO(struct ieee80211_local *local,
+                 struct ieee80211_channel_switch *ch_switch),
+
+        TP_ARGS(local, ch_switch),
+
+        TP_STRUCT__entry(
+                LOCAL_ENTRY
+                __field(u64, timestamp)
+                __field(bool, block_tx)
+                __field(u16, freq)
+                __field(u8, count)
+        ),
+
+        TP_fast_assign(
+                LOCAL_ASSIGN;
+                __entry->timestamp = ch_switch->timestamp;
+                __entry->block_tx = ch_switch->block_tx;
+                __entry->freq = ch_switch->channel->center_freq;
+                __entry->count = ch_switch->count;
+        ),
+
+        TP_printk(
+                LOCAL_PR_FMT " new freq:%u count:%d",
+                LOCAL_PR_ARG, __entry->freq, __entry->count
+        )
+);
+
 /*
  * Tracing for API calls that drivers call.
  */
@@ -992,6 +1020,27 @@ TRACE_EVENT(api_sta_block_awake,
         )
 );
 
+TRACE_EVENT(api_chswitch_done,
+        TP_PROTO(struct ieee80211_sub_if_data *sdata, bool success),
+
+        TP_ARGS(sdata, success),
+
+        TP_STRUCT__entry(
+                VIF_ENTRY
+                __field(bool, success)
+        ),
+
+        TP_fast_assign(
+                VIF_ASSIGN;
+                __entry->success = success;
+        ),
+
+        TP_printk(
+                VIF_PR_FMT " success=%d",
+                VIF_PR_ARG, __entry->success
+        )
+);
+
 /*
  * Tracing for internal functions
  * (which may also be called in response to driver calls)
diff --git a/net/mac80211/ieee80211_i.h b/net/mac80211/ieee80211_i.h
index 7ef7798d04cd..1a9e2da37a93 100644
--- a/net/mac80211/ieee80211_i.h
+++ b/net/mac80211/ieee80211_i.h
@@ -998,7 +998,8 @@ int ieee80211_max_network_latency(struct notifier_block *nb,
                                   unsigned long data, void *dummy);
 void ieee80211_sta_process_chanswitch(struct ieee80211_sub_if_data *sdata,
                                       struct ieee80211_channel_sw_ie *sw_elem,
-                                      struct ieee80211_bss *bss);
+                                      struct ieee80211_bss *bss,
+                                      u64 timestamp);
 void ieee80211_sta_quiesce(struct ieee80211_sub_if_data *sdata);
 void ieee80211_sta_restart(struct ieee80211_sub_if_data *sdata);
 
diff --git a/net/mac80211/mlme.c b/net/mac80211/mlme.c
index 3093e46273c3..c6c1f49cc456 100644
--- a/net/mac80211/mlme.c
+++ b/net/mac80211/mlme.c
@@ -342,7 +342,11 @@ static void ieee80211_chswitch_work(struct work_struct *work)
                 goto out;
 
         sdata->local->oper_channel = sdata->local->csa_channel;
-        ieee80211_hw_config(sdata->local, IEEE80211_CONF_CHANGE_CHANNEL);
+        if (!sdata->local->ops->channel_switch) {
+                /* call "hw_config" only if doing sw channel switch */
+                ieee80211_hw_config(sdata->local,
+                        IEEE80211_CONF_CHANGE_CHANNEL);
+        }
 
         /* XXX: shouldn't really modify cfg80211-owned data! */
         ifmgd->associated->channel = sdata->local->oper_channel;
@@ -354,6 +358,29 @@ static void ieee80211_chswitch_work(struct work_struct *work)
         mutex_unlock(&ifmgd->mtx);
 }
 
+void ieee80211_chswitch_done(struct ieee80211_vif *vif, bool success)
+{
+        struct ieee80211_sub_if_data *sdata;
+        struct ieee80211_if_managed *ifmgd;
+
+        sdata = vif_to_sdata(vif);
+        ifmgd = &sdata->u.mgd;
+
+        trace_api_chswitch_done(sdata, success);
+        if (!success) {
+                /*
+                 * If the channel switch was not successful, stay
+                 * around on the old channel. We currently lack
+                 * good handling of this situation, possibly we
+                 * should just drop the association.
+                 */
+                sdata->local->csa_channel = sdata->local->oper_channel;
+        }
+
+        ieee80211_queue_work(&sdata->local->hw, &ifmgd->chswitch_work);
+}
+EXPORT_SYMBOL(ieee80211_chswitch_done);
+
 static void ieee80211_chswitch_timer(unsigned long data)
 {
         struct ieee80211_sub_if_data *sdata =
@@ -370,7 +397,8 @@ static void ieee80211_chswitch_timer(unsigned long data)
 
 void ieee80211_sta_process_chanswitch(struct ieee80211_sub_if_data *sdata,
                                       struct ieee80211_channel_sw_ie *sw_elem,
-                                      struct ieee80211_bss *bss)
+                                      struct ieee80211_bss *bss,
+                                      u64 timestamp)
 {
         struct cfg80211_bss *cbss =
                 container_of((void *)bss, struct cfg80211_bss, priv);
@@ -398,10 +426,29 @@ void ieee80211_sta_process_chanswitch(struct ieee80211_sub_if_data *sdata,
 
         sdata->local->csa_channel = new_ch;
 
+        if (sdata->local->ops->channel_switch) {
+                /* use driver's channel switch callback */
+                struct ieee80211_channel_switch ch_switch;
+                memset(&ch_switch, 0, sizeof(ch_switch));
+                ch_switch.timestamp = timestamp;
+                if (sw_elem->mode) {
+                        ch_switch.block_tx = true;
+                        ieee80211_stop_queues_by_reason(&sdata->local->hw,
+                                        IEEE80211_QUEUE_STOP_REASON_CSA);
+                }
+                ch_switch.channel = new_ch;
+                ch_switch.count = sw_elem->count;
+                ifmgd->flags |= IEEE80211_STA_CSA_RECEIVED;
+                drv_channel_switch(sdata->local, &ch_switch);
+                return;
+        }
+
+        /* channel switch handled in software */
         if (sw_elem->count <= 1) {
                 ieee80211_queue_work(&sdata->local->hw, &ifmgd->chswitch_work);
         } else {
-                ieee80211_stop_queues_by_reason(&sdata->local->hw,
+                if (sw_elem->mode)
+                        ieee80211_stop_queues_by_reason(&sdata->local->hw,
                                         IEEE80211_QUEUE_STOP_REASON_CSA);
                 ifmgd->flags |= IEEE80211_STA_CSA_RECEIVED;
                 mod_timer(&ifmgd->chswitch_timer,
@@ -1317,7 +1364,8 @@ static void ieee80211_rx_bss_info(struct ieee80211_sub_if_data *sdata,
                                                         ETH_ALEN) == 0)) {
                 struct ieee80211_channel_sw_ie *sw_elem =
                         (struct ieee80211_channel_sw_ie *)elems->ch_switch_elem;
-                ieee80211_sta_process_chanswitch(sdata, sw_elem, bss);
+                ieee80211_sta_process_chanswitch(sdata, sw_elem,
+                                                 bss, rx_status->mactime);
         }
 }
 
@@ -1649,7 +1697,8 @@ static void ieee80211_sta_rx_queued_mgmt(struct ieee80211_sub_if_data *sdata,
 
                         ieee80211_sta_process_chanswitch(sdata,
                                         &mgmt->u.action.u.chan_switch.sw_elem,
-                                        (void *)ifmgd->associated->priv);
+                                        (void *)ifmgd->associated->priv,
+                                        rx_status->mactime);
                         break;
                 }
                 mutex_unlock(&ifmgd->mtx);
diff --git a/net/mac80211/rx.c b/net/mac80211/rx.c
index 9a08f2c446c6..6e2a7bcd8cb8 100644
--- a/net/mac80211/rx.c
+++ b/net/mac80211/rx.c
@@ -1253,6 +1253,12 @@ ieee80211_rx_h_defragment(struct ieee80211_rx_data *rx)
         if (skb_linearize(rx->skb))
                 return RX_DROP_UNUSABLE;
 
+        /*
+         *  skb_linearize() might change the skb->data and
+         *  previously cached variables (in this case, hdr) need to
+         *  be refreshed with the new data.
+         */
+        hdr = (struct ieee80211_hdr *)rx->skb->data;
         seq = (sc & IEEE80211_SCTL_SEQ) >> 4;
 
         if (frag == 0) {
diff --git a/net/mac80211/work.c b/net/mac80211/work.c
index 3dd07600199d..be3d4a698692 100644
--- a/net/mac80211/work.c
+++ b/net/mac80211/work.c
@@ -33,6 +33,7 @@
 #define IEEE80211_MAX_PROBE_TRIES 5
 
 enum work_action {
+        WORK_ACT_MISMATCH,
         WORK_ACT_NONE,
         WORK_ACT_TIMEOUT,
         WORK_ACT_DONE,
@@ -585,7 +586,7 @@ ieee80211_rx_mgmt_auth(struct ieee80211_work *wk,
         u16 auth_alg, auth_transaction, status_code;
 
         if (wk->type != IEEE80211_WORK_AUTH)
-                return WORK_ACT_NONE;
+                return WORK_ACT_MISMATCH;
 
         if (len < 24 + 6)
                 return WORK_ACT_NONE;
@@ -636,6 +637,9 @@ ieee80211_rx_mgmt_assoc_resp(struct ieee80211_work *wk,
         struct ieee802_11_elems elems;
         u8 *pos;
 
+        if (wk->type != IEEE80211_WORK_ASSOC)
+                return WORK_ACT_MISMATCH;
+
         /*
          * AssocResp and ReassocResp have identical structure, so process both
          * of them in this function.
@@ -691,6 +695,12 @@ ieee80211_rx_mgmt_probe_resp(struct ieee80211_work *wk,
 
         ASSERT_WORK_MTX(local);
 
+        if (wk->type != IEEE80211_WORK_DIRECT_PROBE)
+                return WORK_ACT_MISMATCH;
+
+        if (len < 24 + 12)
+                return WORK_ACT_NONE;
+
         baselen = (u8 *) mgmt->u.probe_resp.variable - (u8 *) mgmt;
         if (baselen > len)
                 return WORK_ACT_NONE;
@@ -705,7 +715,7 @@ static void ieee80211_work_rx_queued_mgmt(struct ieee80211_local *local,
         struct ieee80211_rx_status *rx_status;
         struct ieee80211_mgmt *mgmt;
         struct ieee80211_work *wk;
-        enum work_action rma = WORK_ACT_NONE;
+        enum work_action rma;
         u16 fc;
 
         rx_status = (struct ieee80211_rx_status *) skb->cb;
@@ -752,7 +762,17 @@ static void ieee80211_work_rx_queued_mgmt(struct ieee80211_local *local,
                         break;
                 default:
                         WARN_ON(1);
+                        rma = WORK_ACT_NONE;
                 }
+
+                /*
+                 * We've either received an unexpected frame, or we have
+                 * multiple work items and need to match the frame to the
+                 * right one.
+                 */
+                if (rma == WORK_ACT_MISMATCH)
+                        continue;
+
                 /*
                  * We've processed this frame for that work, so it can't
                  * belong to another work struct.
@@ -762,6 +782,9 @@ static void ieee80211_work_rx_queued_mgmt(struct ieee80211_local *local,
         }
 
         switch (rma) {
+        case WORK_ACT_MISMATCH:
+                /* ignore this unmatched frame */
+                break;
         case WORK_ACT_NONE:
                 break;
         case WORK_ACT_DONE: