1 files changed, 49 insertions, 9 deletions

diff --git a/drivers/net/wireless/iwlwifi/iwl-5000.c b/drivers/net/wireless/iwlwifi/iwl-5000.c
index a28af7eb67eb..32710a801cb0 100644
--- a/drivers/net/wireless/iwlwifi/iwl-5000.c
+++ b/drivers/net/wireless/iwlwifi/iwl-5000.c
@@ -208,6 +208,8 @@ static int iwl5000_hw_set_hw_params(struct iwl_priv *priv)
                 BIT(IWL_CALIB_TX_IQ_PERD)       |
                 BIT(IWL_CALIB_BASE_BAND);
 
+        priv->hw_params.beacon_time_tsf_bits = IWLAGN_EXT_BEACON_TIME_POS;
+
         return 0;
 }
 
@@ -252,6 +254,8 @@ static int iwl5150_hw_set_hw_params(struct iwl_priv *priv)
                 BIT(IWL_CALIB_TX_IQ)            |
                 BIT(IWL_CALIB_BASE_BAND);
 
+        priv->hw_params.beacon_time_tsf_bits = IWLAGN_EXT_BEACON_TIME_POS;
+
         return 0;
 }
 
@@ -267,33 +271,69 @@ static void iwl5150_temperature(struct iwl_priv *priv)
         iwl_tt_handler(priv);
 }
 
-static int iwl5000_hw_channel_switch(struct iwl_priv *priv, u16 channel)
+static int iwl5000_hw_channel_switch(struct iwl_priv *priv,
+                                     struct ieee80211_channel_switch *ch_switch)
 {
         struct iwl5000_channel_switch_cmd cmd;
         const struct iwl_channel_info *ch_info;
+        u32 switch_time_in_usec, ucode_switch_time;
+        u16 ch;
+        u32 tsf_low;
+        u8 switch_count;
+        u16 beacon_interval = le16_to_cpu(priv->rxon_timing.beacon_interval);
+        struct ieee80211_vif *vif = priv->vif;
         struct iwl_host_cmd hcmd = {
                 .id = REPLY_CHANNEL_SWITCH,
                 .len = sizeof(cmd),
-                .flags = CMD_SIZE_HUGE,
+                .flags = CMD_SYNC,
                 .data = &cmd,
         };
 
-        IWL_DEBUG_11H(priv, "channel switch from %d to %d\n",
-                priv->active_rxon.channel, channel);
         cmd.band = priv->band == IEEE80211_BAND_2GHZ;
-        cmd.channel = cpu_to_le16(channel);
+        ch = ieee80211_frequency_to_channel(ch_switch->channel->center_freq);
+        IWL_DEBUG_11H(priv, "channel switch from %d to %d\n",
+                priv->active_rxon.channel, ch);
+        cmd.channel = cpu_to_le16(ch);
         cmd.rxon_flags = priv->staging_rxon.flags;
         cmd.rxon_filter_flags = priv->staging_rxon.filter_flags;
-        cmd.switch_time = cpu_to_le32(priv->ucode_beacon_time);
-        ch_info = iwl_get_channel_info(priv, priv->band, channel);
+        switch_count = ch_switch->count;
+        tsf_low = ch_switch->timestamp & 0x0ffffffff;
+        /*
+         * calculate the ucode channel switch time
+         * adding TSF as one of the factor for when to switch
+         */
+        if ((priv->ucode_beacon_time > tsf_low) && beacon_interval) {
+                if (switch_count > ((priv->ucode_beacon_time - tsf_low) /
+                    beacon_interval)) {
+                        switch_count -= (priv->ucode_beacon_time -
+                                tsf_low) / beacon_interval;
+                } else
+                        switch_count = 0;
+        }
+        if (switch_count <= 1)
+                cmd.switch_time = cpu_to_le32(priv->ucode_beacon_time);
+        else {
+                switch_time_in_usec =
+                        vif->bss_conf.beacon_int * switch_count * TIME_UNIT;
+                ucode_switch_time = iwl_usecs_to_beacons(priv,
+                                                         switch_time_in_usec,
+                                                         beacon_interval);
+                cmd.switch_time = iwl_add_beacon_time(priv,
+                                                      priv->ucode_beacon_time,
+                                                      ucode_switch_time,
+                                                      beacon_interval);
+        }
+        IWL_DEBUG_11H(priv, "uCode time for the switch is 0x%x\n",
+                      cmd.switch_time);
+        ch_info = iwl_get_channel_info(priv, priv->band, ch);
         if (ch_info)
                 cmd.expect_beacon = is_channel_radar(ch_info);
         else {
                 IWL_ERR(priv, "invalid channel switch from %u to %u\n",
-                        priv->active_rxon.channel, channel);
+                        priv->active_rxon.channel, ch);
                 return -EFAULT;
         }
-        priv->switch_rxon.channel = cpu_to_le16(channel);
+        priv->switch_rxon.channel = cmd.channel;
         priv->switch_rxon.switch_in_progress = true;
 
         return iwl_send_cmd_sync(priv, &hcmd);