drivers/net/wireless/iwlwifi/iwl-4965-rs.c

   1 /******************************************************************************
   2  *
   3  * Copyright(c) 2005 - 2007 Intel Corporation. All rights reserved.
   4  *
   5  * This program is free software; you can redistribute it and/or modify it
   6  * under the terms of version 2 of the GNU General Public License as
   7  * published by the Free Software Foundation.
   8  *
   9  * This program is distributed in the hope that it will be useful, but WITHOUT
  10  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  11  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  12  * more details.
  13  *
  14  * You should have received a copy of the GNU General Public License along with
  15  * this program; if not, write to the Free Software Foundation, Inc.,
  16  * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
  17  *
  18  * The full GNU General Public License is included in this distribution in the
  19  * file called LICENSE.
  20  *
  21  * Contact Information:
  22  * James P. Ketrenos <ipw2100-admin@linux.intel.com>
  23  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  24  *
  25  *****************************************************************************/
  26 #include <linux/kernel.h>
  27 #include <linux/init.h>
  28 #include <linux/skbuff.h>
  29 #include <linux/wireless.h>
  30 #include <net/mac80211.h>
  31 #include <net/ieee80211.h>
  32
  33 #include <linux/netdevice.h>
  34 #include <linux/etherdevice.h>
  35 #include <linux/delay.h>
  36
  37 #include <linux/workqueue.h>
  38
  39 #include <net/mac80211.h>
  40 #include <linux/wireless.h>
  41
  42 #define IWL 4965
  43
  44 #include "../net/mac80211/ieee80211_rate.h"
  45
  46 #include "iwlwifi.h"
  47 #include "iwl-helpers.h"
  48
  49 #define RS_NAME "iwl-4965-rs"
  50
  51 #define NUM_TRY_BEFORE_ANTENNA_TOGGLE 1
  52 #define IWL_NUMBER_TRY      1
  53 #define IWL_HT_NUMBER_TRY   3
  54
  55 #define IWL_RATE_MAX_WINDOW             62
  56 #define IWL_RATE_HIGH_TH                10880
  57 #define IWL_RATE_MIN_FAILURE_TH         6
  58 #define IWL_RATE_MIN_SUCCESS_TH         8
  59 #define IWL_RATE_DECREASE_TH            1920
  60 #define IWL_RATE_INCREASE_TH            8960
  61 #define IWL_RATE_SCALE_FLUSH_INTVL   (2*HZ)        /*2 seconds */
  62
  63 static u8 rs_ht_to_legacy[] = {
  64         IWL_RATE_6M_INDEX, IWL_RATE_6M_INDEX,
  65         IWL_RATE_6M_INDEX, IWL_RATE_6M_INDEX,
  66         IWL_RATE_6M_INDEX,
  67         IWL_RATE_6M_INDEX, IWL_RATE_9M_INDEX,
  68         IWL_RATE_12M_INDEX, IWL_RATE_18M_INDEX,
  69         IWL_RATE_24M_INDEX, IWL_RATE_36M_INDEX,
  70         IWL_RATE_48M_INDEX, IWL_RATE_54M_INDEX
  71 };
  72
  73 struct iwl_rate {
  74         u32 rate_n_flags;
  75 } __attribute__ ((packed));
  76
  77 struct iwl_rate_scale_data {
  78         u64 data;
  79         s32 success_counter;
  80         s32 success_ratio;
  81         s32 counter;
  82         s32 average_tpt;
  83         unsigned long stamp;
  84 };
  85
  86 struct iwl_scale_tbl_info {
  87         enum iwl_table_type lq_type;
  88         enum iwl_antenna_type antenna_type;
  89         u8 is_SGI;
  90         u8 is_fat;
  91         u8 is_dup;
  92         u8 action;
  93         s32 *expected_tpt;
  94         struct iwl_rate current_rate;
  95         struct iwl_rate_scale_data win[IWL_RATE_COUNT];
  96 };
  97
  98 struct iwl_rate_scale_priv {
  99         u8 active_tbl;
 100         u8 enable_counter;
 101         u8 stay_in_tbl;
 102         u8 search_better_tbl;
 103         s32 last_tpt;
 104         u32 table_count_limit;
 105         u32 max_failure_limit;
 106         u32 max_success_limit;
 107         u32 table_count;
 108         u32 total_failed;
 109         u32 total_success;
 110         u32 flush_timer;
 111         u8 action_counter;
 112         u8 antenna;
 113         u8 valid_antenna;
 114         u8 is_green;
 115         u8 is_dup;
 116         u8 phymode;
 117         u8 ibss_sta_added;
 118         u32 supp_rates;
 119         u16 active_rate;
 120         u16 active_siso_rate;
 121         u16 active_mimo_rate;
 122         u16 active_rate_basic;
 123         struct iwl_link_quality_cmd lq;
 124         struct iwl_scale_tbl_info lq_info[LQ_SIZE];
 125 #ifdef CONFIG_MAC80211_DEBUGFS
 126         struct dentry *rs_sta_dbgfs_scale_table_file;
 127         struct iwl_rate dbg_fixed;
 128         struct iwl_priv *drv;
 129 #endif
 130 };
 131
 132 static void rs_rate_scale_perform(struct iwl_priv *priv,
 133                                    struct net_device *dev,
 134                                    struct ieee80211_hdr *hdr,
 135                                    struct sta_info *sta);
 136 static void rs_fill_link_cmd(struct iwl_rate_scale_priv *lq_data,
 137                              struct iwl_rate *tx_mcs,
 138                              struct iwl_link_quality_cmd *tbl);
 139
 140
 141 #ifdef CONFIG_MAC80211_DEBUGFS
 142 static void rs_dbgfs_set_mcs(struct iwl_rate_scale_priv *rs_priv,
 143                                 struct iwl_rate *mcs, int index);
 144 #else
 145 static void rs_dbgfs_set_mcs(struct iwl_rate_scale_priv *rs_priv,
 146                                 struct iwl_rate *mcs, int index)
 147 {}
 148 #endif
 149 static s32 expected_tpt_A[IWL_RATE_COUNT] = {
 150         0, 0, 0, 0, 40, 57, 72, 98, 121, 154, 177, 186, 186
 151 };
 152
 153 static s32 expected_tpt_G[IWL_RATE_COUNT] = {
 154         7, 13, 35, 58, 40, 57, 72, 98, 121, 154, 177, 186, 186
 155 };
 156
 157 static s32 expected_tpt_siso20MHz[IWL_RATE_COUNT] = {
 158         0, 0, 0, 0, 42, 42, 76, 102, 124, 159, 183, 193, 202
 159 };
 160
 161 static s32 expected_tpt_siso20MHzSGI[IWL_RATE_COUNT] = {
 162         0, 0, 0, 0, 46, 46, 82, 110, 132, 168, 192, 202, 211
 163 };
 164
 165 static s32 expected_tpt_mimo20MHz[IWL_RATE_COUNT] = {
 166         0, 0, 0, 0, 74, 74, 123, 155, 179, 214, 236, 244, 251
 167 };
 168
 169 static s32 expected_tpt_mimo20MHzSGI[IWL_RATE_COUNT] = {
 170         0, 0, 0, 0, 81, 81, 131, 164, 188, 222, 243, 251, 257
 171 };
 172
 173 static s32 expected_tpt_siso40MHz[IWL_RATE_COUNT] = {
 174         0, 0, 0, 0, 77, 77, 127, 160, 184, 220, 242, 250, 257
 175 };
 176
 177 static s32 expected_tpt_siso40MHzSGI[IWL_RATE_COUNT] = {
 178         0, 0, 0, 0, 83, 83, 135, 169, 193, 229, 250, 257, 264
 179 };
 180
 181 static s32 expected_tpt_mimo40MHz[IWL_RATE_COUNT] = {
 182         0, 0, 0, 0, 123, 123, 182, 214, 235, 264, 279, 285, 289
 183 };
 184
 185 static s32 expected_tpt_mimo40MHzSGI[IWL_RATE_COUNT] = {
 186         0, 0, 0, 0, 131, 131, 191, 222, 242, 270, 284, 289, 293
 187 };
 188
 189 static int iwl_lq_sync_callback(struct iwl_priv *priv,
 190                                 struct iwl_cmd *cmd, struct sk_buff *skb)
 191 {
 192         /*We didn't cache the SKB; let the caller free it */
 193         return 1;
 194 }
 195
 196 static inline u8 iwl_rate_get_rate(u32 rate_n_flags)
 197 {
 198         return (u8)(rate_n_flags & 0xFF);
 199 }
 200
 201 static int rs_send_lq_cmd(struct iwl_priv *priv,
 202                           struct iwl_link_quality_cmd *lq, u8 flags)
 203 {
 204 #ifdef CONFIG_IWLWIFI_DEBUG
 205         int i;
 206 #endif
 207         int rc = -1;
 208
 209         struct iwl_host_cmd cmd = {
 210                 .id = REPLY_TX_LINK_QUALITY_CMD,
 211                 .len = sizeof(struct iwl_link_quality_cmd),
 212                 .meta.flags = flags,
 213                 .data = lq,
 214         };
 215
 216         if ((lq->sta_id == 0xFF) &&
 217             (priv->iw_mode == IEEE80211_IF_TYPE_IBSS))
 218                 return rc;
 219
 220         if (lq->sta_id == 0xFF)
 221                 lq->sta_id = IWL_AP_ID;
 222
 223         IWL_DEBUG_RATE("lq station id 0x%x\n", lq->sta_id);
 224         IWL_DEBUG_RATE("lq dta 0x%X 0x%X\n",
 225                        lq->general_params.single_stream_ant_msk,
 226                        lq->general_params.dual_stream_ant_msk);
 227 #ifdef CONFIG_IWLWIFI_DEBUG
 228         for (i = 0; i < LINK_QUAL_MAX_RETRY_NUM; i++)
 229                 IWL_DEBUG_RATE("lq index %d 0x%X\n",
 230                                 i, lq->rs_table[i].rate_n_flags);
 231 #endif
 232
 233         if (flags & CMD_ASYNC)
 234                 cmd.meta.u.callback = iwl_lq_sync_callback;
 235
 236         if (iwl_is_associated(priv) && priv->assoc_station_added &&
 237             priv->lq_mngr.lq_ready)
 238                 rc = iwl_send_cmd(priv, &cmd);
 239
 240         return rc;
 241 }
 242
 243 static int rs_rate_scale_clear_window(struct iwl_rate_scale_data *window)
 244 {
 245         window->data = 0;
 246         window->success_counter = 0;
 247         window->success_ratio = IWL_INVALID_VALUE;
 248         window->counter = 0;
 249         window->average_tpt = IWL_INVALID_VALUE;
 250         window->stamp = 0;
 251
 252         return 0;
 253 }
 254
 255 static int rs_collect_tx_data(struct iwl_rate_scale_data *windows,
 256                               int scale_index, s32 tpt, u32 status)
 257 {
 258         int rc = 0;
 259         struct iwl_rate_scale_data *window = NULL;
 260         u64 mask;
 261         u8 win_size = IWL_RATE_MAX_WINDOW;
 262         s32 fail_count;
 263
 264         if (scale_index < 0)
 265                 return -1;
 266
 267         if (scale_index >= IWL_RATE_COUNT)
 268                 return -1;
 269
 270         window = &(windows[scale_index]);
 271
 272         if (window->counter >= win_size) {
 273
 274                 window->counter = win_size - 1;
 275                 mask = 1;
 276                 mask = (mask << (win_size - 1));
 277                 if ((window->data & mask)) {
 278                         window->data &= ~mask;
 279                         window->success_counter = window->success_counter - 1;
 280                 }
 281         }
 282
 283         window->counter = window->counter + 1;
 284         mask = window->data;
 285         window->data = (mask << 1);
 286         if (status != 0) {
 287                 window->success_counter = window->success_counter + 1;
 288                 window->data |= 0x1;
 289         }
 290
 291         if (window->counter > 0)
 292                 window->success_ratio = 128 * (100 * window->success_counter)
 293                                         / window->counter;
 294         else
 295                 window->success_ratio = IWL_INVALID_VALUE;
 296
 297         fail_count = window->counter - window->success_counter;
 298
 299         if ((fail_count >= IWL_RATE_MIN_FAILURE_TH) ||
 300             (window->success_counter >= IWL_RATE_MIN_SUCCESS_TH))
 301                 window->average_tpt = (window->success_ratio * tpt + 64) / 128;
 302         else
 303                 window->average_tpt = IWL_INVALID_VALUE;
 304
 305         window->stamp = jiffies;
 306
 307         return rc;
 308 }
 309
 310 int static rs_mcs_from_tbl(struct iwl_rate *mcs_rate,
 311                            struct iwl_scale_tbl_info *tbl,
 312                            int index, u8 use_green)
 313 {
 314         int rc = 0;
 315
 316         if (is_legacy(tbl->lq_type)) {
 317                 mcs_rate->rate_n_flags = iwl_rates[index].plcp;
 318                 if (index >= IWL_FIRST_CCK_RATE && index <= IWL_LAST_CCK_RATE)
 319                         mcs_rate->rate_n_flags |= RATE_MCS_CCK_MSK;
 320
 321         } else if (is_siso(tbl->lq_type)) {
 322                 if (index > IWL_LAST_OFDM_RATE)
 323                         index = IWL_LAST_OFDM_RATE;
 324                  mcs_rate->rate_n_flags = iwl_rates[index].plcp_siso |
 325                                           RATE_MCS_HT_MSK;
 326         } else {
 327                 if (index > IWL_LAST_OFDM_RATE)
 328                         index = IWL_LAST_OFDM_RATE;
 329                 mcs_rate->rate_n_flags = iwl_rates[index].plcp_mimo |
 330                                          RATE_MCS_HT_MSK;
 331         }
 332
 333         switch (tbl->antenna_type) {
 334         case ANT_BOTH:
 335                 mcs_rate->rate_n_flags |= RATE_MCS_ANT_AB_MSK;
 336                 break;
 337         case ANT_MAIN:
 338                 mcs_rate->rate_n_flags |= RATE_MCS_ANT_A_MSK;
 339                 break;
 340         case ANT_AUX:
 341                 mcs_rate->rate_n_flags |= RATE_MCS_ANT_B_MSK;
 342                 break;
 343         case ANT_NONE:
 344                 break;
 345         }
 346
 347         if (is_legacy(tbl->lq_type))
 348                 return rc;
 349
 350         if (tbl->is_fat) {
 351                 if (tbl->is_dup)
 352                         mcs_rate->rate_n_flags |= RATE_MCS_DUP_MSK;
 353                 else
 354                         mcs_rate->rate_n_flags |= RATE_MCS_FAT_MSK;
 355         }
 356         if (tbl->is_SGI)
 357                 mcs_rate->rate_n_flags |= RATE_MCS_SGI_MSK;
 358
 359         if (use_green) {
 360                 mcs_rate->rate_n_flags |= RATE_MCS_GF_MSK;
 361                 if (is_siso(tbl->lq_type))
 362                         mcs_rate->rate_n_flags &= ~RATE_MCS_SGI_MSK;
 363         }
 364         return rc;
 365 }
 366
 367 static int rs_get_tbl_info_from_mcs(const struct iwl_rate *mcs_rate,
 368                                     int phymode, struct iwl_scale_tbl_info *tbl,
 369                                     int *rate_idx)
 370 {
 371         int index;
 372         u32 ant_msk;
 373
 374         index = iwl_rate_index_from_plcp(mcs_rate->rate_n_flags);
 375
 376         if (index  == IWL_RATE_INVALID) {
 377                 *rate_idx = -1;
 378                 return -1;
 379         }
 380         tbl->is_SGI = 0;
 381         tbl->is_fat = 0;
 382         tbl->is_dup = 0;
 383         tbl->antenna_type = ANT_BOTH;
 384
 385         if (!(mcs_rate->rate_n_flags & RATE_MCS_HT_MSK)) {
 386                 ant_msk = (mcs_rate->rate_n_flags & RATE_MCS_ANT_AB_MSK);
 387
 388                 if (ant_msk == RATE_MCS_ANT_AB_MSK)
 389                         tbl->lq_type = LQ_NONE;
 390                 else {
 391
 392                         if (phymode == MODE_IEEE80211A)
 393                                 tbl->lq_type = LQ_A;
 394                         else
 395                                 tbl->lq_type = LQ_G;
 396
 397                         if (mcs_rate->rate_n_flags & RATE_MCS_ANT_A_MSK)
 398                                 tbl->antenna_type = ANT_MAIN;
 399                         else
 400                                 tbl->antenna_type = ANT_AUX;
 401                 }
 402                 *rate_idx = index;
 403
 404         } else if (iwl_rate_get_rate(mcs_rate->rate_n_flags)
 405                                         <= IWL_RATE_SISO_60M_PLCP) {
 406                 tbl->lq_type = LQ_SISO;
 407
 408                 ant_msk = (mcs_rate->rate_n_flags & RATE_MCS_ANT_AB_MSK);
 409                 if (ant_msk == RATE_MCS_ANT_AB_MSK)
 410                         tbl->lq_type = LQ_NONE;
 411                 else {
 412                         if (mcs_rate->rate_n_flags & RATE_MCS_ANT_A_MSK)
 413                                 tbl->antenna_type = ANT_MAIN;
 414                         else
 415                                 tbl->antenna_type = ANT_AUX;
 416                 }
 417                 if (mcs_rate->rate_n_flags & RATE_MCS_SGI_MSK)
 418                         tbl->is_SGI = 1;
 419
 420                 if ((mcs_rate->rate_n_flags & RATE_MCS_FAT_MSK) ||
 421                     (mcs_rate->rate_n_flags & RATE_MCS_DUP_MSK))
 422                         tbl->is_fat = 1;
 423
 424                 if (mcs_rate->rate_n_flags & RATE_MCS_DUP_MSK)
 425                         tbl->is_dup = 1;
 426
 427                 *rate_idx = index;
 428         } else {
 429                 tbl->lq_type = LQ_MIMO;
 430                 if (mcs_rate->rate_n_flags & RATE_MCS_SGI_MSK)
 431                         tbl->is_SGI = 1;
 432
 433                 if ((mcs_rate->rate_n_flags & RATE_MCS_FAT_MSK) ||
 434                     (mcs_rate->rate_n_flags & RATE_MCS_DUP_MSK))
 435                         tbl->is_fat = 1;
 436
 437                 if (mcs_rate->rate_n_flags & RATE_MCS_DUP_MSK)
 438                         tbl->is_dup = 1;
 439                 *rate_idx = index;
 440         }
 441         return 0;
 442 }
 443
 444 static inline void rs_toggle_antenna(struct iwl_rate *new_rate,
 445                                      struct iwl_scale_tbl_info *tbl)
 446 {
 447         if (tbl->antenna_type == ANT_AUX) {
 448                 tbl->antenna_type = ANT_MAIN;
 449                 new_rate->rate_n_flags &= ~RATE_MCS_ANT_B_MSK;
 450                 new_rate->rate_n_flags |= RATE_MCS_ANT_A_MSK;
 451         } else {
 452                 tbl->antenna_type = ANT_AUX;
 453                 new_rate->rate_n_flags &= ~RATE_MCS_ANT_A_MSK;
 454                 new_rate->rate_n_flags |= RATE_MCS_ANT_B_MSK;
 455         }
 456 }
 457
 458 static inline s8 rs_use_green(struct iwl_priv *priv)
 459 {
 460         s8 rc = 0;
 461 #ifdef CONFIG_IWLWIFI_HT
 462         if (!priv->is_ht_enabled || !priv->current_assoc_ht.is_ht)
 463                 return 0;
 464
 465         if ((priv->current_assoc_ht.is_green_field) &&
 466             !(priv->current_assoc_ht.operating_mode & 0x4))
 467                 rc = 1;
 468 #endif  /*CONFIG_IWLWIFI_HT */
 469         return rc;
 470 }
 471
 472 /**
 473  * rs_get_supported_rates - get the available rates
 474  *
 475  * if management frame or broadcast frame only return
 476  * basic available rates.
 477  *
 478  */
 479 static void rs_get_supported_rates(struct iwl_rate_scale_priv *lq_data,
 480                                    struct ieee80211_hdr *hdr,
 481                                    enum iwl_table_type rate_type,
 482                                    u16 *data_rate)
 483 {
 484         if (is_legacy(rate_type))
 485                 *data_rate = lq_data->active_rate;
 486         else {
 487                 if (is_siso(rate_type))
 488                         *data_rate = lq_data->active_siso_rate;
 489                 else
 490                         *data_rate = lq_data->active_mimo_rate;
 491         }
 492
 493         if (hdr && is_multicast_ether_addr(hdr->addr1) &&
 494             lq_data->active_rate_basic)
 495                 *data_rate = lq_data->active_rate_basic;
 496 }
 497
 498 static u16 rs_get_adjacent_rate(u8 index, u16 rate_mask, int rate_type)
 499 {
 500         u8 high = IWL_RATE_INVALID;
 501         u8 low = IWL_RATE_INVALID;
 502
 503         /* 802.11A or ht walks to the next literal adjascent rate in
 504          * the rate table */
 505         if (is_a_band(rate_type) || !is_legacy(rate_type)) {
 506                 int i;
 507                 u32 mask;
 508
 509                 /* Find the previous rate that is in the rate mask */
 510                 i = index - 1;
 511                 for (mask = (1 << i); i >= 0; i--, mask >>= 1) {
 512                         if (rate_mask & mask) {
 513                                 low = i;
 514                                 break;
 515                         }
 516                 }
 517
 518                 /* Find the next rate that is in the rate mask */
 519                 i = index + 1;
 520                 for (mask = (1 << i); i < IWL_RATE_COUNT; i++, mask <<= 1) {
 521                         if (rate_mask & mask) {
 522                                 high = i;
 523                                 break;
 524                         }
 525                 }
 526
 527                 return (high << 8) | low;
 528         }
 529
 530         low = index;
 531         while (low != IWL_RATE_INVALID) {
 532                 low = iwl_rates[low].prev_rs;
 533                 if (low == IWL_RATE_INVALID)
 534                         break;
 535                 if (rate_mask & (1 << low))
 536                         break;
 537                 IWL_DEBUG_RATE("Skipping masked lower rate: %d\n", low);
 538         }
 539
 540         high = index;
 541         while (high != IWL_RATE_INVALID) {
 542                 high = iwl_rates[high].next_rs;
 543                 if (high == IWL_RATE_INVALID)
 544                         break;
 545                 if (rate_mask & (1 << high))
 546                         break;
 547                 IWL_DEBUG_RATE("Skipping masked higher rate: %d\n", high);
 548         }
 549
 550         return (high << 8) | low;
 551 }
 552
 553 static int rs_get_lower_rate(struct iwl_rate_scale_priv *lq_data,
 554                              struct iwl_scale_tbl_info *tbl, u8 scale_index,
 555                              u8 ht_possible, struct iwl_rate *mcs_rate)
 556 {
 557         s32 low;
 558         u16 rate_mask;
 559         u16 high_low;
 560         u8 switch_to_legacy = 0;
 561         u8 is_green = lq_data->is_green;
 562
 563         /* check if we need to switch from HT to legacy rates.
 564          * assumption is that mandatory rates (1Mbps or 6Mbps)
 565          * are always supported (spec demand) */
 566         if (!is_legacy(tbl->lq_type) && (!ht_possible || !scale_index)) {
 567                 switch_to_legacy = 1;
 568                 scale_index = rs_ht_to_legacy[scale_index];
 569                 if (lq_data->phymode == MODE_IEEE80211A)
 570                         tbl->lq_type = LQ_A;
 571                 else
 572                         tbl->lq_type = LQ_G;
 573
 574                 if ((tbl->antenna_type == ANT_BOTH) ||
 575                     (tbl->antenna_type == ANT_NONE))
 576                         tbl->antenna_type = ANT_MAIN;
 577
 578                 tbl->is_fat = 0;
 579                 tbl->is_SGI = 0;
 580         }
 581
 582         rs_get_supported_rates(lq_data, NULL, tbl->lq_type, &rate_mask);
 583
 584         /* mask with station rate restriction */
 585         if (is_legacy(tbl->lq_type)) {
 586                 if (lq_data->phymode == (u8) MODE_IEEE80211A)
 587                         rate_mask  = (u16)(rate_mask &
 588                            (lq_data->supp_rates << IWL_FIRST_OFDM_RATE));
 589                 else
 590                         rate_mask = (u16)(rate_mask & lq_data->supp_rates);
 591         }
 592
 593         /* if we did switched from HT to legacy check current rate */
 594         if ((switch_to_legacy) &&
 595             (rate_mask & (1 << scale_index))) {
 596                 rs_mcs_from_tbl(mcs_rate, tbl, scale_index, is_green);
 597                 return 0;
 598         }
 599
 600         high_low = rs_get_adjacent_rate(scale_index, rate_mask, tbl->lq_type);
 601         low = high_low & 0xff;
 602
 603         if (low != IWL_RATE_INVALID)
 604                 rs_mcs_from_tbl(mcs_rate, tbl, low, is_green);
 605         else
 606                 rs_mcs_from_tbl(mcs_rate, tbl, scale_index, is_green);
 607
 608         return 0;
 609 }
 610
 611 static void rs_tx_status(void *priv_rate,
 612                          struct net_device *dev,
 613                          struct sk_buff *skb,
 614                          struct ieee80211_tx_status *tx_resp)
 615 {
 616         int status;
 617         u8 retries;
 618         int rs_index, index = 0;
 619         struct iwl_rate_scale_priv *lq;
 620         struct iwl_link_quality_cmd *table;
 621         struct sta_info *sta;
 622         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
 623         struct iwl_priv *priv = (struct iwl_priv *)priv_rate;
 624         struct ieee80211_local *local = wdev_priv(dev->ieee80211_ptr);
 625         struct iwl_rate_scale_data *window = NULL;
 626         struct iwl_rate_scale_data *search_win = NULL;
 627         struct iwl_rate tx_mcs;
 628         struct iwl_scale_tbl_info tbl_type;
 629         struct iwl_scale_tbl_info *curr_tbl, *search_tbl;
 630         u8 active_index = 0;
 631         u16 fc = le16_to_cpu(hdr->frame_control);
 632         s32 tpt = 0;
 633
 634         IWL_DEBUG_RATE_LIMIT("get frame ack response, update rate scale window\n");
 635
 636         if (!ieee80211_is_data(fc) || is_multicast_ether_addr(hdr->addr1))
 637                 return;
 638
 639         retries = tx_resp->retry_count;
 640
 641         if (retries > 15)
 642                 retries = 15;
 643
 644
 645         sta = sta_info_get(local, hdr->addr1);
 646
 647         if (!sta || !sta->rate_ctrl_priv) {
 648                 if (sta)
 649                         sta_info_put(sta);
 650                 return;
 651         }
 652
 653         lq = (struct iwl_rate_scale_priv *)sta->rate_ctrl_priv;
 654
 655         if (!priv->lq_mngr.lq_ready)
 656                 return;
 657
 658         if ((priv->iw_mode == IEEE80211_IF_TYPE_IBSS) && !lq->ibss_sta_added)
 659                 return;
 660
 661         table = &lq->lq;
 662         active_index = lq->active_tbl;
 663
 664         lq->antenna = (lq->valid_antenna & local->hw.conf.antenna_sel_tx);
 665         if (!lq->antenna)
 666                 lq->antenna = lq->valid_antenna;
 667
 668         lq->antenna = lq->valid_antenna;
 669         curr_tbl = &(lq->lq_info[active_index]);
 670         search_tbl = &(lq->lq_info[(1 - active_index)]);
 671         window = (struct iwl_rate_scale_data *)
 672             &(curr_tbl->win[0]);
 673         search_win = (struct iwl_rate_scale_data *)
 674             &(search_tbl->win[0]);
 675
 676         tx_mcs.rate_n_flags = tx_resp->control.tx_rate;
 677
 678         rs_get_tbl_info_from_mcs(&tx_mcs, priv->phymode,
 679                                   &tbl_type, &rs_index);
 680         if ((rs_index < 0) || (rs_index >= IWL_RATE_COUNT)) {
 681                 IWL_DEBUG_RATE("bad rate index at: %d rate 0x%X\n",
 682                              rs_index, tx_mcs.rate_n_flags);
 683                 sta_info_put(sta);
 684                 return;
 685         }
 686
 687         if (retries &&
 688             (tx_mcs.rate_n_flags !=
 689                                 le32_to_cpu(table->rs_table[0].rate_n_flags))) {
 690                 IWL_DEBUG_RATE("initial rate does not match 0x%x 0x%x\n",
 691                                 tx_mcs.rate_n_flags,
 692                                 le32_to_cpu(table->rs_table[0].rate_n_flags));
 693                 sta_info_put(sta);
 694                 return;
 695         }
 696
 697         while (retries) {
 698                 tx_mcs.rate_n_flags =
 699                     le32_to_cpu(table->rs_table[index].rate_n_flags);
 700                 rs_get_tbl_info_from_mcs(&tx_mcs, priv->phymode,
 701                                           &tbl_type, &rs_index);
 702
 703                 if ((tbl_type.lq_type == search_tbl->lq_type) &&
 704                     (tbl_type.antenna_type == search_tbl->antenna_type) &&
 705                     (tbl_type.is_SGI == search_tbl->is_SGI)) {
 706                         if (search_tbl->expected_tpt)
 707                                 tpt = search_tbl->expected_tpt[rs_index];
 708                         else
 709                                 tpt = 0;
 710                         rs_collect_tx_data(search_win,
 711                                             rs_index, tpt, 0);
 712                 } else if ((tbl_type.lq_type == curr_tbl->lq_type) &&
 713                            (tbl_type.antenna_type == curr_tbl->antenna_type) &&
 714                            (tbl_type.is_SGI == curr_tbl->is_SGI)) {
 715                         if (curr_tbl->expected_tpt)
 716                                 tpt = curr_tbl->expected_tpt[rs_index];
 717                         else
 718                                 tpt = 0;
 719                         rs_collect_tx_data(window, rs_index, tpt, 0);
 720                 }
 721                 if (lq->stay_in_tbl)
 722                         lq->total_failed++;
 723                 --retries;
 724                 index++;
 725
 726         }
 727
 728         if (!tx_resp->retry_count)
 729                 tx_mcs.rate_n_flags = tx_resp->control.tx_rate;
 730         else
 731                 tx_mcs.rate_n_flags =
 732                         le32_to_cpu(table->rs_table[index].rate_n_flags);
 733
 734         rs_get_tbl_info_from_mcs(&tx_mcs, priv->phymode,
 735                                   &tbl_type, &rs_index);
 736
 737         if (tx_resp->flags & IEEE80211_TX_STATUS_ACK)
 738                 status = 1;
 739         else
 740                 status = 0;
 741
 742         if ((tbl_type.lq_type == search_tbl->lq_type) &&
 743             (tbl_type.antenna_type == search_tbl->antenna_type) &&
 744             (tbl_type.is_SGI == search_tbl->is_SGI)) {
 745                 if (search_tbl->expected_tpt)
 746                         tpt = search_tbl->expected_tpt[rs_index];
 747                 else
 748                         tpt = 0;
 749                 rs_collect_tx_data(search_win,
 750                                     rs_index, tpt, status);
 751         } else if ((tbl_type.lq_type == curr_tbl->lq_type) &&
 752                    (tbl_type.antenna_type == curr_tbl->antenna_type) &&
 753                    (tbl_type.is_SGI == curr_tbl->is_SGI)) {
 754                 if (curr_tbl->expected_tpt)
 755                         tpt = curr_tbl->expected_tpt[rs_index];
 756                 else
 757                         tpt = 0;
 758                 rs_collect_tx_data(window, rs_index, tpt, status);
 759         }
 760
 761         if (lq->stay_in_tbl) {
 762                 if (status)
 763                         lq->total_success++;
 764                 else
 765                         lq->total_failed++;
 766         }
 767
 768         rs_rate_scale_perform(priv, dev, hdr, sta);
 769         sta_info_put(sta);
 770         return;
 771 }
 772
 773 static u8 rs_is_ant_connected(u8 valid_antenna,
 774                               enum iwl_antenna_type antenna_type)
 775 {
 776         if (antenna_type == ANT_AUX)
 777                 return ((valid_antenna & 0x2) ? 1:0);
 778         else if (antenna_type == ANT_MAIN)
 779                 return ((valid_antenna & 0x1) ? 1:0);
 780         else if (antenna_type == ANT_BOTH) {
 781                 if ((valid_antenna & 0x3) == 0x3)
 782                         return 1;
 783                 else
 784                         return 0;
 785         }
 786
 787         return 1;
 788 }
 789
 790 static u8 rs_is_other_ant_connected(u8 valid_antenna,
 791                                     enum iwl_antenna_type antenna_type)
 792 {
 793         if (antenna_type == ANT_AUX)
 794                 return (rs_is_ant_connected(valid_antenna, ANT_MAIN));
 795         else
 796                 return (rs_is_ant_connected(valid_antenna, ANT_AUX));
 797
 798         return 0;
 799 }
 800
 801 static void rs_set_stay_in_table(u8 is_legacy,
 802                                  struct iwl_rate_scale_priv *lq_data)
 803 {
 804         IWL_DEBUG_HT("we are staying in the same table\n");
 805         lq_data->stay_in_tbl = 1;
 806         if (is_legacy) {
 807                 lq_data->table_count_limit = IWL_LEGACY_TABLE_COUNT;
 808                 lq_data->max_failure_limit = IWL_LEGACY_FAILURE_LIMIT;
 809                 lq_data->max_success_limit = IWL_LEGACY_TABLE_COUNT;
 810         } else {
 811                 lq_data->table_count_limit = IWL_NONE_LEGACY_TABLE_COUNT;
 812                 lq_data->max_failure_limit = IWL_NONE_LEGACY_FAILURE_LIMIT;
 813                 lq_data->max_success_limit = IWL_NONE_LEGACY_SUCCESS_LIMIT;
 814         }
 815         lq_data->table_count = 0;
 816         lq_data->total_failed = 0;
 817         lq_data->total_success = 0;
 818 }
 819
 820 static void rs_get_expected_tpt_table(struct iwl_rate_scale_priv *lq_data,
 821                                       struct iwl_scale_tbl_info *tbl)
 822 {
 823         if (is_legacy(tbl->lq_type)) {
 824                 if (!is_a_band(tbl->lq_type))
 825                         tbl->expected_tpt = expected_tpt_G;
 826                 else
 827                         tbl->expected_tpt = expected_tpt_A;
 828         } else if (is_siso(tbl->lq_type)) {
 829                 if (tbl->is_fat && !lq_data->is_dup)
 830                         if (tbl->is_SGI)
 831                                 tbl->expected_tpt = expected_tpt_siso40MHzSGI;
 832                         else
 833                                 tbl->expected_tpt = expected_tpt_siso40MHz;
 834                 else if (tbl->is_SGI)
 835                         tbl->expected_tpt = expected_tpt_siso20MHzSGI;
 836                 else
 837                         tbl->expected_tpt = expected_tpt_siso20MHz;
 838
 839         } else if (is_mimo(tbl->lq_type)) {
 840                 if (tbl->is_fat && !lq_data->is_dup)
 841                         if (tbl->is_SGI)
 842                                 tbl->expected_tpt = expected_tpt_mimo40MHzSGI;
 843                         else
 844                                 tbl->expected_tpt = expected_tpt_mimo40MHz;
 845                 else if (tbl->is_SGI)
 846                         tbl->expected_tpt = expected_tpt_mimo20MHzSGI;
 847                 else
 848                         tbl->expected_tpt = expected_tpt_mimo20MHz;
 849         } else
 850                 tbl->expected_tpt = expected_tpt_G;
 851 }
 852
 853 #ifdef CONFIG_IWLWIFI_HT
 854 static s32 rs_get_best_rate(struct iwl_priv *priv,
 855                             struct iwl_rate_scale_priv *lq_data,
 856                             struct iwl_scale_tbl_info *tbl,
 857                             u16 rate_mask, s8 index, s8 rate)
 858 {
 859         struct iwl_scale_tbl_info *active_tbl =
 860             &(lq_data->lq_info[lq_data->active_tbl]);
 861         s32 new_rate, high, low, start_hi;
 862         s32 active_sr = active_tbl->win[index].success_ratio;
 863         s32 *tpt_tbl = tbl->expected_tpt;
 864         s32 active_tpt = active_tbl->expected_tpt[index];
 865         u16 high_low;
 866
 867         new_rate = high = low = start_hi = IWL_RATE_INVALID;
 868
 869         for (; ;) {
 870                 high_low = rs_get_adjacent_rate(rate, rate_mask, tbl->lq_type);
 871
 872                 low = high_low & 0xff;
 873                 high = (high_low >> 8) & 0xff;
 874
 875                 if ((((100 * tpt_tbl[rate]) > lq_data->last_tpt) &&
 876                      ((active_sr > IWL_RATE_DECREASE_TH) &&
 877                       (active_sr <= IWL_RATE_HIGH_TH) &&
 878                       (tpt_tbl[rate] <= active_tpt))) ||
 879                     ((active_sr >= IWL_RATE_SCALE_SWITCH) &&
 880                      (tpt_tbl[rate] > active_tpt))) {
 881
 882                         if (start_hi != IWL_RATE_INVALID) {
 883                                 new_rate = start_hi;
 884                                 break;
 885                         }
 886                         new_rate = rate;
 887                         if (low != IWL_RATE_INVALID)
 888                                 rate = low;
 889                         else
 890                                 break;
 891                 } else {
 892                         if (new_rate != IWL_RATE_INVALID)
 893                                 break;
 894                         else if (high != IWL_RATE_INVALID) {
 895                                 start_hi = high;
 896                                 rate = high;
 897                         } else {
 898                                 new_rate = rate;
 899                                 break;
 900                         }
 901                 }
 902         }
 903
 904         return new_rate;
 905 }
 906 #endif                          /* CONFIG_IWLWIFI_HT */
 907
 908 static inline u8 rs_is_both_ant_supp(u8 valid_antenna)
 909 {
 910         return (rs_is_ant_connected(valid_antenna, ANT_BOTH));
 911 }
 912
 913 static int rs_switch_to_mimo(struct iwl_priv *priv,
 914                              struct iwl_rate_scale_priv *lq_data,
 915                              struct iwl_scale_tbl_info *tbl, int index)
 916 {
 917         int rc = -1;
 918 #ifdef CONFIG_IWLWIFI_HT
 919         u16 rate_mask;
 920         s32 rate;
 921         s8 is_green = lq_data->is_green;
 922
 923         if (!priv->is_ht_enabled || !priv->current_assoc_ht.is_ht)
 924                 return -1;
 925
 926         IWL_DEBUG_HT("LQ: try to switch to MIMO\n");
 927         tbl->lq_type = LQ_MIMO;
 928         rs_get_supported_rates(lq_data, NULL, tbl->lq_type,
 929                                 &rate_mask);
 930
 931         if (priv->current_assoc_ht.tx_mimo_ps_mode == IWL_MIMO_PS_STATIC)
 932                 return -1;
 933
 934         if (!rs_is_both_ant_supp(lq_data->antenna))
 935                 return -1;
 936
 937         rc = 0;
 938         tbl->is_dup = lq_data->is_dup;
 939         tbl->action = 0;
 940         if (priv->current_channel_width == IWL_CHANNEL_WIDTH_40MHZ)
 941                 tbl->is_fat = 1;
 942         else
 943                 tbl->is_fat = 0;
 944
 945         if (tbl->is_fat) {
 946                 if (priv->current_assoc_ht.sgf & HT_SHORT_GI_40MHZ_ONLY)
 947                         tbl->is_SGI = 1;
 948                 else
 949                         tbl->is_SGI = 0;
 950         } else if (priv->current_assoc_ht.sgf & HT_SHORT_GI_20MHZ_ONLY)
 951                 tbl->is_SGI = 1;
 952         else
 953                 tbl->is_SGI = 0;
 954
 955         rs_get_expected_tpt_table(lq_data, tbl);
 956
 957         rate = rs_get_best_rate(priv, lq_data, tbl, rate_mask, index, index);
 958
 959         IWL_DEBUG_HT("LQ: MIMO best rate %d mask %X\n", rate, rate_mask);
 960         if ((rate == IWL_RATE_INVALID) || !((1 << rate) & rate_mask))
 961                 return -1;
 962         rs_mcs_from_tbl(&tbl->current_rate, tbl, rate, is_green);
 963
 964         IWL_DEBUG_HT("LQ: Switch to new mcs %X index is green %X\n",
 965                      tbl->current_rate.rate_n_flags, is_green);
 966
 967 #endif                          /*CONFIG_IWLWIFI_HT */
 968         return rc;
 969 }
 970
 971 static int rs_switch_to_siso(struct iwl_priv *priv,
 972                              struct iwl_rate_scale_priv *lq_data,
 973                              struct iwl_scale_tbl_info *tbl, int index)
 974 {
 975         int rc = -1;
 976 #ifdef CONFIG_IWLWIFI_HT
 977         u16 rate_mask;
 978         u8 is_green = lq_data->is_green;
 979         s32 rate;
 980
 981         IWL_DEBUG_HT("LQ: try to switch to SISO\n");
 982         if (!priv->is_ht_enabled || !priv->current_assoc_ht.is_ht)
 983                 return -1;
 984
 985         rc = 0;
 986         tbl->is_dup = lq_data->is_dup;
 987         tbl->lq_type = LQ_SISO;
 988         tbl->action = 0;
 989         rs_get_supported_rates(lq_data, NULL, tbl->lq_type,
 990                                 &rate_mask);
 991
 992         if (priv->current_channel_width == IWL_CHANNEL_WIDTH_40MHZ)
 993                 tbl->is_fat = 1;
 994         else
 995                 tbl->is_fat = 0;
 996
 997         if (tbl->is_fat) {
 998                 if (priv->current_assoc_ht.sgf & HT_SHORT_GI_40MHZ_ONLY)
 999                         tbl->is_SGI = 1;
1000                 else
1001                         tbl->is_SGI = 0;
1002         } else if (priv->current_assoc_ht.sgf & HT_SHORT_GI_20MHZ_ONLY)
1003                 tbl->is_SGI = 1;
1004         else
1005                 tbl->is_SGI = 0;
1006
1007         if (is_green)
1008                 tbl->is_SGI = 0;
1009
1010         rs_get_expected_tpt_table(lq_data, tbl);
1011         rate = rs_get_best_rate(priv, lq_data, tbl, rate_mask, index, index);
1012
1013         IWL_DEBUG_HT("LQ: get best rate %d mask %X\n", rate, rate_mask);
1014         if ((rate == IWL_RATE_INVALID) || !((1 << rate) & rate_mask)) {
1015                 IWL_DEBUG_HT("can not switch with index %d rate mask %x\n",
1016                              rate, rate_mask);
1017                 return -1;
1018         }
1019         rs_mcs_from_tbl(&tbl->current_rate, tbl, rate, is_green);
1020         IWL_DEBUG_HT("LQ: Switch to new mcs %X index is green %X\n",
1021                      tbl->current_rate.rate_n_flags, is_green);
1022
1023 #endif                          /*CONFIG_IWLWIFI_HT */
1024         return rc;
1025 }
1026
1027 static int rs_move_legacy_other(struct iwl_priv *priv,
1028                                 struct iwl_rate_scale_priv *lq_data,
1029                                 int index)
1030 {
1031         int rc = 0;
1032         struct iwl_scale_tbl_info *tbl =
1033             &(lq_data->lq_info[lq_data->active_tbl]);
1034         struct iwl_scale_tbl_info *search_tbl =
1035             &(lq_data->lq_info[(1 - lq_data->active_tbl)]);
1036         struct iwl_rate_scale_data *window = &(tbl->win[index]);
1037         u32 sz = (sizeof(struct iwl_scale_tbl_info) -
1038                   (sizeof(struct iwl_rate_scale_data) * IWL_RATE_COUNT));
1039         u8 start_action = tbl->action;
1040
1041         for (; ;) {
1042                 switch (tbl->action) {
1043                 case IWL_LEGACY_SWITCH_ANTENNA:
1044                         IWL_DEBUG_HT("LQ Legacy switch Antenna\n");
1045
1046                         search_tbl->lq_type = LQ_NONE;
1047                         lq_data->action_counter++;
1048                         if (window->success_ratio >= IWL_RS_GOOD_RATIO)
1049                                 break;
1050                         if (!rs_is_other_ant_connected(lq_data->antenna,
1051                                                         tbl->antenna_type))
1052                                 break;
1053
1054                         memcpy(search_tbl, tbl, sz);
1055
1056                         rs_toggle_antenna(&(search_tbl->current_rate),
1057                                            search_tbl);
1058                         rs_get_expected_tpt_table(lq_data, search_tbl);
1059                         lq_data->search_better_tbl = 1;
1060                         goto out;
1061
1062                 case IWL_LEGACY_SWITCH_SISO:
1063                         IWL_DEBUG_HT("LQ: Legacy switch to SISO\n");
1064                         memcpy(search_tbl, tbl, sz);
1065                         search_tbl->lq_type = LQ_SISO;
1066                         search_tbl->is_SGI = 0;
1067                         search_tbl->is_fat = 0;
1068                         rc = rs_switch_to_siso(priv, lq_data, search_tbl,
1069                                                index);
1070                         if (!rc) {
1071                                 lq_data->search_better_tbl = 1;
1072                                 lq_data->action_counter = 0;
1073                         }
1074                         if (!rc)
1075                                 goto out;
1076
1077                         break;
1078                 case IWL_LEGACY_SWITCH_MIMO:
1079                         IWL_DEBUG_HT("LQ: Legacy switch MIMO\n");
1080                         memcpy(search_tbl, tbl, sz);
1081                         search_tbl->lq_type = LQ_MIMO;
1082                         search_tbl->is_SGI = 0;
1083                         search_tbl->is_fat = 0;
1084                         search_tbl->antenna_type = ANT_BOTH;
1085                         rc = rs_switch_to_mimo(priv, lq_data, search_tbl,
1086                                                index);
1087                         if (!rc) {
1088                                 lq_data->search_better_tbl = 1;
1089                                 lq_data->action_counter = 0;
1090                         }
1091                         if (!rc)
1092                                 goto out;
1093                         break;
1094                 }
1095                 tbl->action++;
1096                 if (tbl->action > IWL_LEGACY_SWITCH_MIMO)
1097                         tbl->action = IWL_LEGACY_SWITCH_ANTENNA;
1098
1099                 if (tbl->action == start_action)
1100                         break;
1101
1102         }
1103         return 0;
1104
1105  out:
1106         tbl->action++;
1107         if (tbl->action > IWL_LEGACY_SWITCH_MIMO)
1108                 tbl->action = IWL_LEGACY_SWITCH_ANTENNA;
1109         return 0;
1110
1111 }
1112
1113 static int rs_move_siso_to_other(struct iwl_priv *priv,
1114                                  struct iwl_rate_scale_priv *lq_data,
1115                                  int index)
1116 {
1117         int rc = -1;
1118         u8 is_green = lq_data->is_green;
1119         struct iwl_scale_tbl_info *tbl =
1120             &(lq_data->lq_info[lq_data->active_tbl]);
1121         struct iwl_scale_tbl_info *search_tbl =
1122             &(lq_data->lq_info[(1 - lq_data->active_tbl)]);
1123         struct iwl_rate_scale_data *window = &(tbl->win[index]);
1124         u32 sz = (sizeof(struct iwl_scale_tbl_info) -
1125                   (sizeof(struct iwl_rate_scale_data) * IWL_RATE_COUNT));
1126         u8 start_action = tbl->action;
1127
1128         for (;;) {
1129                 lq_data->action_counter++;
1130                 switch (tbl->action) {
1131                 case IWL_SISO_SWITCH_ANTENNA:
1132                         IWL_DEBUG_HT("LQ: SISO SWITCH ANTENNA SISO\n");
1133                         search_tbl->lq_type = LQ_NONE;
1134                         if (window->success_ratio >= IWL_RS_GOOD_RATIO)
1135                                 break;
1136                         if (!rs_is_other_ant_connected(lq_data->antenna,
1137                                                        tbl->antenna_type))
1138                                 break;
1139
1140                         memcpy(search_tbl, tbl, sz);
1141                         search_tbl->action = IWL_SISO_SWITCH_MIMO;
1142                         rs_toggle_antenna(&(search_tbl->current_rate),
1143                                            search_tbl);
1144                         lq_data->search_better_tbl = 1;
1145
1146                         goto out;
1147
1148                 case IWL_SISO_SWITCH_MIMO:
1149                         IWL_DEBUG_HT("LQ: SISO SWITCH TO MIMO FROM SISO\n");
1150                         memcpy(search_tbl, tbl, sz);
1151                         search_tbl->lq_type = LQ_MIMO;
1152                         search_tbl->is_SGI = 0;
1153                         search_tbl->is_fat = 0;
1154                         search_tbl->antenna_type = ANT_BOTH;
1155                         rc = rs_switch_to_mimo(priv, lq_data, search_tbl,
1156                                                index);
1157                         if (!rc)
1158                                 lq_data->search_better_tbl = 1;
1159
1160                         if (!rc)
1161                                 goto out;
1162                         break;
1163                 case IWL_SISO_SWITCH_GI:
1164                         IWL_DEBUG_HT("LQ: SISO SWITCH TO GI\n");
1165                         memcpy(search_tbl, tbl, sz);
1166                         search_tbl->action = 0;
1167                         if (search_tbl->is_SGI)
1168                                 search_tbl->is_SGI = 0;
1169                         else if (!is_green)
1170                                 search_tbl->is_SGI = 1;
1171                         else
1172                                 break;
1173                         lq_data->search_better_tbl = 1;
1174                         if ((tbl->lq_type == LQ_SISO) &&
1175                             (tbl->is_SGI)) {
1176                                 s32 tpt = lq_data->last_tpt / 100;
1177                                 if (((!tbl->is_fat) &&
1178                                      (tpt >= expected_tpt_siso20MHz[index])) ||
1179                                     ((tbl->is_fat) &&
1180                                      (tpt >= expected_tpt_siso40MHz[index])))
1181                                         lq_data->search_better_tbl = 0;
1182                         }
1183                         rs_get_expected_tpt_table(lq_data, search_tbl);
1184                         rs_mcs_from_tbl(&search_tbl->current_rate,
1185                                              search_tbl, index, is_green);
1186                         goto out;
1187                 }
1188                 tbl->action++;
1189                 if (tbl->action > IWL_SISO_SWITCH_GI)
1190                         tbl->action = IWL_SISO_SWITCH_ANTENNA;
1191
1192                 if (tbl->action == start_action)
1193                         break;
1194         }
1195         return 0;
1196
1197  out:
1198         tbl->action++;
1199         if (tbl->action > IWL_SISO_SWITCH_GI)
1200                 tbl->action = IWL_SISO_SWITCH_ANTENNA;
1201         return 0;
1202 }
1203
1204 static int rs_move_mimo_to_other(struct iwl_priv *priv,
1205                                  struct iwl_rate_scale_priv *lq_data,
1206                                  int index)
1207 {
1208         int rc = -1;
1209         s8 is_green = lq_data->is_green;
1210         struct iwl_scale_tbl_info *tbl =
1211             &(lq_data->lq_info[lq_data->active_tbl]);
1212         struct iwl_scale_tbl_info *search_tbl =
1213             &(lq_data->lq_info[(1 - lq_data->active_tbl)]);
1214         u32 sz = (sizeof(struct iwl_scale_tbl_info) -
1215                   (sizeof(struct iwl_rate_scale_data) * IWL_RATE_COUNT));
1216         u8 start_action = tbl->action;
1217
1218         for (;;) {
1219                 lq_data->action_counter++;
1220                 switch (tbl->action) {
1221                 case IWL_MIMO_SWITCH_ANTENNA_A:
1222                 case IWL_MIMO_SWITCH_ANTENNA_B:
1223                         IWL_DEBUG_HT("LQ: MIMO SWITCH TO SISO\n");
1224                         memcpy(search_tbl, tbl, sz);
1225                         search_tbl->lq_type = LQ_SISO;
1226                         search_tbl->is_SGI = 0;
1227                         search_tbl->is_fat = 0;
1228                         if (tbl->action == IWL_MIMO_SWITCH_ANTENNA_A)
1229                                 search_tbl->antenna_type = ANT_MAIN;
1230                         else
1231                                 search_tbl->antenna_type = ANT_AUX;
1232
1233                         rc = rs_switch_to_siso(priv, lq_data, search_tbl,
1234                                                index);
1235                         if (!rc) {
1236                                 lq_data->search_better_tbl = 1;
1237                                 goto out;
1238                         }
1239                         break;
1240
1241                 case IWL_MIMO_SWITCH_GI:
1242                         IWL_DEBUG_HT("LQ: MIMO SWITCH TO GI\n");
1243                         memcpy(search_tbl, tbl, sz);
1244                         search_tbl->lq_type = LQ_MIMO;
1245                         search_tbl->antenna_type = ANT_BOTH;
1246                         search_tbl->action = 0;
1247                         if (search_tbl->is_SGI)
1248                                 search_tbl->is_SGI = 0;
1249                         else
1250                                 search_tbl->is_SGI = 1;
1251                         lq_data->search_better_tbl = 1;
1252                         if ((tbl->lq_type == LQ_MIMO) &&
1253                             (tbl->is_SGI)) {
1254                                 s32 tpt = lq_data->last_tpt / 100;
1255                                 if (((!tbl->is_fat) &&
1256                                      (tpt >= expected_tpt_mimo20MHz[index])) ||
1257                                     ((tbl->is_fat) &&
1258                                      (tpt >= expected_tpt_mimo40MHz[index])))
1259                                         lq_data->search_better_tbl = 0;
1260                         }
1261                         rs_get_expected_tpt_table(lq_data, search_tbl);
1262                         rs_mcs_from_tbl(&search_tbl->current_rate,
1263                                              search_tbl, index, is_green);
1264                         goto out;
1265
1266                 }
1267                 tbl->action++;
1268                 if (tbl->action > IWL_MIMO_SWITCH_GI)
1269                         tbl->action = IWL_MIMO_SWITCH_ANTENNA_A;
1270
1271                 if (tbl->action == start_action)
1272                         break;
1273         }
1274
1275         return 0;
1276  out:
1277         tbl->action++;
1278         if (tbl->action > IWL_MIMO_SWITCH_GI)
1279                 tbl->action = IWL_MIMO_SWITCH_ANTENNA_A;
1280         return 0;
1281
1282 }
1283
1284 static void rs_stay_in_table(struct iwl_rate_scale_priv *lq_data)
1285 {
1286         struct iwl_scale_tbl_info *tbl;
1287         int i;
1288         int active_tbl;
1289         int flush_interval_passed = 0;
1290
1291         active_tbl = lq_data->active_tbl;
1292
1293         tbl = &(lq_data->lq_info[active_tbl]);
1294
1295         if (lq_data->stay_in_tbl) {
1296
1297                 if (lq_data->flush_timer)
1298                         flush_interval_passed =
1299                             time_after(jiffies,
1300                                        (unsigned long)(lq_data->flush_timer +
1301                                         IWL_RATE_SCALE_FLUSH_INTVL));
1302
1303                 flush_interval_passed = 0;
1304                 if ((lq_data->total_failed > lq_data->max_failure_limit) ||
1305                     (lq_data->total_success > lq_data->max_success_limit) ||
1306                     ((!lq_data->search_better_tbl) && (lq_data->flush_timer)
1307                      && (flush_interval_passed))) {
1308                         IWL_DEBUG_HT("LQ: stay is expired %d %d %d\n:",
1309                                      lq_data->total_failed,
1310                                      lq_data->total_success,
1311                                      flush_interval_passed);
1312                         lq_data->stay_in_tbl = 0;
1313                         lq_data->total_failed = 0;
1314                         lq_data->total_success = 0;
1315                         lq_data->flush_timer = 0;
1316                 } else if (lq_data->table_count > 0) {
1317                         lq_data->table_count++;
1318                         if (lq_data->table_count >=
1319                             lq_data->table_count_limit) {
1320                                 lq_data->table_count = 0;
1321
1322                                 IWL_DEBUG_HT("LQ: stay in table clear win\n");
1323                                 for (i = 0; i < IWL_RATE_COUNT; i++)
1324                                         rs_rate_scale_clear_window(
1325                                                 &(tbl->win[i]));
1326                         }
1327                 }
1328
1329                 if (!lq_data->stay_in_tbl) {
1330                         for (i = 0; i < IWL_RATE_COUNT; i++)
1331                                 rs_rate_scale_clear_window(&(tbl->win[i]));
1332                 }
1333         }
1334 }
1335
1336 static void rs_rate_scale_perform(struct iwl_priv *priv,
1337                                   struct net_device *dev,
1338                                   struct ieee80211_hdr *hdr,
1339                                   struct sta_info *sta)
1340 {
1341         int low = IWL_RATE_INVALID;
1342         int high = IWL_RATE_INVALID;
1343         int index;
1344         int i;
1345         struct iwl_rate_scale_data *window = NULL;
1346         int current_tpt = IWL_INVALID_VALUE;
1347         int low_tpt = IWL_INVALID_VALUE;
1348         int high_tpt = IWL_INVALID_VALUE;
1349         u32 fail_count;
1350         s8 scale_action = 0;
1351         u16 fc, rate_mask;
1352         u8 update_lq = 0;
1353         struct iwl_rate_scale_priv *lq_data;
1354         struct iwl_scale_tbl_info *tbl, *tbl1;
1355         u16 rate_scale_index_msk = 0;
1356         struct iwl_rate mcs_rate;
1357         u8 is_green = 0;
1358         u8 active_tbl = 0;
1359         u8 done_search = 0;
1360         u16 high_low;
1361
1362         IWL_DEBUG_RATE("rate scale calculate new rate for skb\n");
1363
1364         fc = le16_to_cpu(hdr->frame_control);
1365         if (!ieee80211_is_data(fc) || is_multicast_ether_addr(hdr->addr1)) {
1366                 /* Send management frames and broadcast/multicast data using
1367                  * lowest rate. */
1368                 /* TODO: this could probably be improved.. */
1369                 return;
1370         }
1371
1372         if (!sta || !sta->rate_ctrl_priv)
1373                 return;
1374
1375         if (!priv->lq_mngr.lq_ready) {
1376                 IWL_DEBUG_RATE("still rate scaling not ready\n");
1377                 return;
1378         }
1379         lq_data = (struct iwl_rate_scale_priv *)sta->rate_ctrl_priv;
1380
1381         if (!lq_data->search_better_tbl)
1382                 active_tbl = lq_data->active_tbl;
1383         else
1384                 active_tbl = 1 - lq_data->active_tbl;
1385
1386         tbl = &(lq_data->lq_info[active_tbl]);
1387         is_green = lq_data->is_green;
1388
1389         index = sta->last_txrate;
1390
1391         IWL_DEBUG_RATE("Rate scale index %d for type %d\n", index,
1392                        tbl->lq_type);
1393
1394         rs_get_supported_rates(lq_data, hdr, tbl->lq_type,
1395                                 &rate_mask);
1396
1397         IWL_DEBUG_RATE("mask 0x%04X \n", rate_mask);
1398
1399         /* mask with station rate restriction */
1400         if (is_legacy(tbl->lq_type)) {
1401                 if (lq_data->phymode == (u8) MODE_IEEE80211A)
1402                         rate_scale_index_msk = (u16) (rate_mask &
1403                                 (lq_data->supp_rates << IWL_FIRST_OFDM_RATE));
1404                 else
1405                         rate_scale_index_msk = (u16) (rate_mask &
1406                                                       lq_data->supp_rates);
1407
1408         } else
1409                 rate_scale_index_msk = rate_mask;
1410
1411         if (!rate_scale_index_msk)
1412                 rate_scale_index_msk = rate_mask;
1413
1414         if (index < 0 || !((1 << index) & rate_scale_index_msk)) {
1415                 index = IWL_INVALID_VALUE;
1416                 update_lq = 1;
1417
1418                 /* get the lowest availabe rate */
1419                 for (i = 0; i <= IWL_RATE_COUNT; i++) {
1420                         if ((1 << i) & rate_scale_index_msk)
1421                                 index = i;
1422                 }
1423
1424                 if (index == IWL_INVALID_VALUE) {
1425                         IWL_WARNING("Can not find a suitable rate\n");
1426                         return;
1427                 }
1428         }
1429
1430         if (!tbl->expected_tpt)
1431                 rs_get_expected_tpt_table(lq_data, tbl);
1432
1433         window = &(tbl->win[index]);
1434
1435         fail_count = window->counter - window->success_counter;
1436         if (((fail_count < IWL_RATE_MIN_FAILURE_TH) &&
1437              (window->success_counter < IWL_RATE_MIN_SUCCESS_TH))
1438             || (tbl->expected_tpt == NULL)) {
1439                 IWL_DEBUG_RATE("LQ: still below TH succ %d total %d "
1440                                "for index %d\n",
1441                                window->success_counter, window->counter, index);
1442                 window->average_tpt = IWL_INVALID_VALUE;
1443                 rs_stay_in_table(lq_data);
1444                 if (update_lq) {
1445                         rs_mcs_from_tbl(&mcs_rate, tbl, index, is_green);
1446                         rs_fill_link_cmd(lq_data, &mcs_rate, &lq_data->lq);
1447                         rs_send_lq_cmd(priv, &lq_data->lq, CMD_ASYNC);
1448                 }
1449                 goto out;
1450
1451         } else
1452                 window->average_tpt = ((window->success_ratio *
1453                                         tbl->expected_tpt[index] + 64) / 128);
1454
1455         if (lq_data->search_better_tbl) {
1456                 int success_limit = IWL_RATE_SCALE_SWITCH;
1457
1458                 if ((window->success_ratio > success_limit) ||
1459                     (window->average_tpt > lq_data->last_tpt)) {
1460                         if (!is_legacy(tbl->lq_type)) {
1461                                 IWL_DEBUG_HT("LQ: we are switching to HT"
1462                                              " rate suc %d current tpt %d"
1463                                              " old tpt %d\n",
1464                                              window->success_ratio,
1465                                              window->average_tpt,
1466                                              lq_data->last_tpt);
1467                                 lq_data->enable_counter = 1;
1468                         }
1469                         lq_data->active_tbl = active_tbl;
1470                         current_tpt = window->average_tpt;
1471                 } else {
1472                         tbl->lq_type = LQ_NONE;
1473                         active_tbl = lq_data->active_tbl;
1474                         tbl = &(lq_data->lq_info[active_tbl]);
1475
1476                         index = iwl_rate_index_from_plcp(
1477                                 tbl->current_rate.rate_n_flags);
1478
1479                         update_lq = 1;
1480                         current_tpt = lq_data->last_tpt;
1481                         IWL_DEBUG_HT("XXY GO BACK TO OLD TABLE\n");
1482                 }
1483                 lq_data->search_better_tbl = 0;
1484                 done_search = 1;
1485                 goto lq_update;
1486         }
1487
1488         high_low = rs_get_adjacent_rate(index, rate_scale_index_msk,
1489                                         tbl->lq_type);
1490         low = high_low & 0xff;
1491         high = (high_low >> 8) & 0xff;
1492
1493         current_tpt = window->average_tpt;
1494
1495         if (low != IWL_RATE_INVALID)
1496                 low_tpt = tbl->win[low].average_tpt;
1497
1498         if (high != IWL_RATE_INVALID)
1499                 high_tpt = tbl->win[high].average_tpt;
1500
1501
1502         scale_action = 1;
1503
1504         if ((window->success_ratio <= IWL_RATE_DECREASE_TH) ||
1505             (current_tpt == 0)) {
1506                 IWL_DEBUG_RATE("decrease rate because of low success_ratio\n");
1507                 scale_action = -1;
1508         } else if ((low_tpt == IWL_INVALID_VALUE) &&
1509                    (high_tpt == IWL_INVALID_VALUE))
1510                 scale_action = 1;
1511         else if ((low_tpt != IWL_INVALID_VALUE) &&
1512                  (high_tpt != IWL_INVALID_VALUE) &&
1513                  (low_tpt < current_tpt) &&
1514                  (high_tpt < current_tpt))
1515                 scale_action = 0;
1516         else {
1517                 if (high_tpt != IWL_INVALID_VALUE) {
1518                         if (high_tpt > current_tpt)
1519                                 scale_action = 1;
1520                         else {
1521                                 IWL_DEBUG_RATE
1522                                     ("decrease rate because of high tpt\n");
1523                                 scale_action = -1;
1524                         }
1525                 } else if (low_tpt != IWL_INVALID_VALUE) {
1526                         if (low_tpt > current_tpt) {
1527                                 IWL_DEBUG_RATE
1528                                     ("decrease rate because of low tpt\n");
1529                                 scale_action = -1;
1530                         } else
1531                                 scale_action = 1;
1532                 }
1533         }
1534
1535         if (scale_action == -1) {
1536                 if ((low != IWL_RATE_INVALID) &&
1537                     ((window->success_ratio > IWL_RATE_HIGH_TH) ||
1538                      (current_tpt > (100 * tbl->expected_tpt[low]))))
1539                         scale_action = 0;
1540         } else if ((scale_action == 1) &&
1541                    (window->success_ratio < IWL_RATE_INCREASE_TH))
1542                 scale_action = 0;
1543
1544         switch (scale_action) {
1545         case -1:
1546                 if (low != IWL_RATE_INVALID) {
1547                         update_lq = 1;
1548                         index = low;
1549                 }
1550                 break;
1551         case 1:
1552                 if (high != IWL_RATE_INVALID) {
1553                         update_lq = 1;
1554                         index = high;
1555                 }
1556
1557                 break;
1558         case 0:
1559         default:
1560                 break;
1561         }
1562
1563         IWL_DEBUG_HT("choose rate scale index %d action %d low %d "
1564                     "high %d type %d\n",
1565                      index, scale_action, low, high, tbl->lq_type);
1566
1567  lq_update:
1568         if (update_lq) {
1569                 rs_mcs_from_tbl(&mcs_rate, tbl, index, is_green);
1570                 rs_fill_link_cmd(lq_data, &mcs_rate, &lq_data->lq);
1571                 rs_send_lq_cmd(priv, &lq_data->lq, CMD_ASYNC);
1572         }
1573         rs_stay_in_table(lq_data);
1574
1575         if (!update_lq && !done_search && !lq_data->stay_in_tbl) {
1576                 lq_data->last_tpt = current_tpt;
1577
1578                 if (is_legacy(tbl->lq_type))
1579                         rs_move_legacy_other(priv, lq_data, index);
1580                 else if (is_siso(tbl->lq_type))
1581                         rs_move_siso_to_other(priv, lq_data, index);
1582                 else
1583                         rs_move_mimo_to_other(priv, lq_data, index);
1584
1585                 if (lq_data->search_better_tbl) {
1586                         tbl = &(lq_data->lq_info[(1 - lq_data->active_tbl)]);
1587                         for (i = 0; i < IWL_RATE_COUNT; i++)
1588                                 rs_rate_scale_clear_window(&(tbl->win[i]));
1589
1590                         index = iwl_rate_index_from_plcp(
1591                                         tbl->current_rate.rate_n_flags);
1592
1593                         IWL_DEBUG_HT("Switch current  mcs: %X index: %d\n",
1594                                      tbl->current_rate.rate_n_flags, index);
1595                         rs_fill_link_cmd(lq_data, &tbl->current_rate,
1596                                          &lq_data->lq);
1597                         rs_send_lq_cmd(priv, &lq_data->lq, CMD_ASYNC);
1598                 }
1599                 tbl1 = &(lq_data->lq_info[lq_data->active_tbl]);
1600
1601                 if (is_legacy(tbl1->lq_type) &&
1602 #ifdef CONFIG_IWLWIFI_HT
1603                     !priv->current_assoc_ht.is_ht &&
1604 #endif
1605                     (lq_data->action_counter >= 1)) {
1606                         lq_data->action_counter = 0;
1607                         IWL_DEBUG_HT("LQ: STAY in legacy table\n");
1608                         rs_set_stay_in_table(1, lq_data);
1609                 }
1610
1611                 if (lq_data->enable_counter &&
1612                     (lq_data->action_counter >= IWL_ACTION_LIMIT)) {
1613 #ifdef CONFIG_IWLWIFI_HT_AGG
1614                         if ((lq_data->last_tpt > TID_AGG_TPT_THREHOLD) &&
1615                             (priv->lq_mngr.agg_ctrl.auto_agg)) {
1616                                 priv->lq_mngr.agg_ctrl.tid_retry =
1617                                     TID_ALL_SPECIFIED;
1618                                 schedule_work(&priv->agg_work);
1619                         }
1620 #endif /*CONFIG_IWLWIFI_HT_AGG */
1621                         lq_data->action_counter = 0;
1622                         rs_set_stay_in_table(0, lq_data);
1623                 }
1624         } else {
1625                 if ((!update_lq) && (!done_search) && (!lq_data->flush_timer))
1626                         lq_data->flush_timer = jiffies;
1627         }
1628
1629 out:
1630         rs_mcs_from_tbl(&tbl->current_rate, tbl, index, is_green);
1631         i = index;
1632         sta->last_txrate = i;
1633
1634         /* sta->txrate is an index to A mode rates which start
1635          * at IWL_FIRST_OFDM_RATE
1636          */
1637         if (lq_data->phymode == (u8) MODE_IEEE80211A)
1638                 sta->txrate = i - IWL_FIRST_OFDM_RATE;
1639         else
1640                 sta->txrate = i;
1641
1642         return;
1643 }
1644
1645
1646 static void rs_initialize_lq(struct iwl_priv *priv,
1647                              struct sta_info *sta)
1648 {
1649         int i;
1650         struct iwl_rate_scale_priv *lq;
1651         struct iwl_scale_tbl_info *tbl;
1652         u8 active_tbl = 0;
1653         int rate_idx;
1654         u8 use_green = rs_use_green(priv);
1655         struct iwl_rate mcs_rate;
1656
1657         if (!sta || !sta->rate_ctrl_priv)
1658                 goto out;
1659
1660         lq = (struct iwl_rate_scale_priv *)sta->rate_ctrl_priv;
1661         i = sta->last_txrate;
1662
1663         if ((lq->lq.sta_id == 0xff) &&
1664             (priv->iw_mode == IEEE80211_IF_TYPE_IBSS))
1665                 goto out;
1666
1667         if (!lq->search_better_tbl)
1668                 active_tbl = lq->active_tbl;
1669         else
1670                 active_tbl = 1 - lq->active_tbl;
1671
1672         tbl = &(lq->lq_info[active_tbl]);
1673
1674         if ((i < 0) || (i >= IWL_RATE_COUNT))
1675                 i = 0;
1676
1677         mcs_rate.rate_n_flags = iwl_rates[i].plcp ;
1678         mcs_rate.rate_n_flags |= RATE_MCS_ANT_B_MSK;
1679         mcs_rate.rate_n_flags &= ~RATE_MCS_ANT_A_MSK;
1680
1681         if (i >= IWL_FIRST_CCK_RATE && i <= IWL_LAST_CCK_RATE)
1682                 mcs_rate.rate_n_flags |= RATE_MCS_CCK_MSK;
1683
1684         tbl->antenna_type = ANT_AUX;
1685         rs_get_tbl_info_from_mcs(&mcs_rate, priv->phymode, tbl, &rate_idx);
1686         if (!rs_is_ant_connected(priv->valid_antenna, tbl->antenna_type))
1687             rs_toggle_antenna(&mcs_rate, tbl);
1688
1689         rs_mcs_from_tbl(&mcs_rate, tbl, rate_idx, use_green);
1690         tbl->current_rate.rate_n_flags = mcs_rate.rate_n_flags;
1691         rs_get_expected_tpt_table(lq, tbl);
1692         rs_fill_link_cmd(lq, &mcs_rate, &lq->lq);
1693         rs_send_lq_cmd(priv, &lq->lq, CMD_ASYNC);
1694  out:
1695         return;
1696 }
1697
1698 static struct ieee80211_rate *rs_get_lowest_rate(struct ieee80211_local
1699                                                  *local)
1700 {
1701         struct ieee80211_hw_mode *mode = local->oper_hw_mode;
1702         int i;
1703
1704         for (i = 0; i < mode->num_rates; i++) {
1705                 struct ieee80211_rate *rate = &mode->rates[i];
1706
1707                 if (rate->flags & IEEE80211_RATE_SUPPORTED)
1708                         return rate;
1709         }
1710
1711         return &mode->rates[0];
1712 }
1713
1714 static struct ieee80211_rate *rs_get_rate(void *priv_rate,
1715                                                struct net_device *dev,
1716                                                struct sk_buff *skb,
1717                                                struct rate_control_extra
1718                                                *extra)
1719 {
1720
1721         int i;
1722         struct ieee80211_local *local = wdev_priv(dev->ieee80211_ptr);
1723         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
1724         struct sta_info *sta;
1725         u16 fc;
1726         struct iwl_priv *priv = (struct iwl_priv *)priv_rate;
1727         struct iwl_rate_scale_priv *lq;
1728
1729         IWL_DEBUG_RATE_LIMIT("rate scale calculate new rate for skb\n");
1730
1731         memset(extra, 0, sizeof(*extra));
1732
1733         fc = le16_to_cpu(hdr->frame_control);
1734         if (!ieee80211_is_data(fc) || is_multicast_ether_addr(hdr->addr1)) {
1735                 /* Send management frames and broadcast/multicast data using
1736                  * lowest rate. */
1737                 /* TODO: this could probably be improved.. */
1738                 return rs_get_lowest_rate(local);
1739         }
1740
1741         sta = sta_info_get(local, hdr->addr1);
1742
1743         if (!sta || !sta->rate_ctrl_priv) {
1744                 if (sta)
1745                         sta_info_put(sta);
1746                 return rs_get_lowest_rate(local);
1747         }
1748
1749         lq = (struct iwl_rate_scale_priv *)sta->rate_ctrl_priv;
1750         i = sta->last_txrate;
1751
1752         if ((priv->iw_mode == IEEE80211_IF_TYPE_IBSS) && !lq->ibss_sta_added) {
1753                 u8 sta_id = iwl_hw_find_station(priv, hdr->addr1);
1754                 DECLARE_MAC_BUF(mac);
1755
1756                 if (sta_id == IWL_INVALID_STATION) {
1757                         IWL_DEBUG_RATE("LQ: ADD station %s\n",
1758                                        print_mac(mac, hdr->addr1));
1759                         sta_id = iwl_add_station(priv,
1760                                                  hdr->addr1, 0, CMD_ASYNC);
1761                 }
1762                 if ((sta_id != IWL_INVALID_STATION)) {
1763                         lq->lq.sta_id = sta_id;
1764                         lq->lq.rs_table[0].rate_n_flags = 0;
1765                         lq->ibss_sta_added = 1;
1766                         rs_initialize_lq(priv, sta);
1767                 }
1768                 if (!lq->ibss_sta_added)
1769                         goto done;
1770         }
1771
1772  done:
1773         sta_info_put(sta);
1774         if ((i < 0) || (i > IWL_RATE_COUNT))
1775                 return rs_get_lowest_rate(local);
1776
1777         return &priv->ieee_rates[i];
1778 }
1779
1780 static void *rs_alloc_sta(void *priv, gfp_t gfp)
1781 {
1782         struct iwl_rate_scale_priv *crl;
1783         int i, j;
1784
1785         IWL_DEBUG_RATE("create station rate scale window\n");
1786
1787         crl = kzalloc(sizeof(struct iwl_rate_scale_priv), gfp);
1788
1789         if (crl == NULL)
1790                 return NULL;
1791         crl->lq.sta_id = 0xff;
1792
1793
1794         for (j = 0; j < LQ_SIZE; j++)
1795                 for (i = 0; i < IWL_RATE_COUNT; i++)
1796                         rs_rate_scale_clear_window(&(crl->lq_info[j].win[i]));
1797
1798         return crl;
1799 }
1800
1801 static void rs_rate_init(void *priv_rate, void *priv_sta,
1802                          struct ieee80211_local *local,
1803                          struct sta_info *sta)
1804 {
1805         int i, j;
1806         struct ieee80211_hw_mode *mode = local->oper_hw_mode;
1807         struct iwl_priv *priv = (struct iwl_priv *)priv_rate;
1808         struct iwl_rate_scale_priv *crl = priv_sta;
1809
1810         crl->flush_timer = 0;
1811         crl->supp_rates = sta->supp_rates;
1812         sta->txrate = 3;
1813         for (j = 0; j < LQ_SIZE; j++)
1814                 for (i = 0; i < IWL_RATE_COUNT; i++)
1815                         rs_rate_scale_clear_window(&(crl->lq_info[j].win[i]));
1816
1817         IWL_DEBUG_RATE("rate scale global init\n");
1818         /* TODO: what is a good starting rate for STA? About middle? Maybe not
1819          * the lowest or the highest rate.. Could consider using RSSI from
1820          * previous packets? Need to have IEEE 802.1X auth succeed immediately
1821          * after assoc.. */
1822
1823         crl->ibss_sta_added = 0;
1824         if (priv->iw_mode == IEEE80211_IF_TYPE_AP) {
1825                 u8 sta_id = iwl_hw_find_station(priv, sta->addr);
1826                 DECLARE_MAC_BUF(mac);
1827
1828                 /* for IBSS the call are from tasklet */
1829                 IWL_DEBUG_HT("LQ: ADD station %s\n",
1830                              print_mac(mac, sta->addr));
1831
1832                 if (sta_id == IWL_INVALID_STATION) {
1833                         IWL_DEBUG_RATE("LQ: ADD station %s\n",
1834                                        print_mac(mac, sta->addr));
1835                         sta_id = iwl_add_station(priv,
1836                                                  sta->addr, 0, CMD_ASYNC);
1837                 }
1838                 if ((sta_id != IWL_INVALID_STATION)) {
1839                         crl->lq.sta_id = sta_id;
1840                         crl->lq.rs_table[0].rate_n_flags = 0;
1841                 }
1842                 /* FIXME: this is w/a remove it later */
1843                 priv->assoc_station_added = 1;
1844         }
1845
1846         for (i = 0; i < mode->num_rates; i++) {
1847                 if ((sta->supp_rates & BIT(i)) &&
1848                     (mode->rates[i].flags & IEEE80211_RATE_SUPPORTED))
1849                         sta->txrate = i;
1850         }
1851         sta->last_txrate = sta->txrate;
1852         /* For MODE_IEEE80211A mode cck rate are at end
1853          * rate table
1854          */
1855         if (local->hw.conf.phymode == MODE_IEEE80211A)
1856                 sta->last_txrate += IWL_FIRST_OFDM_RATE;
1857
1858         crl->is_dup = priv->is_dup;
1859         crl->valid_antenna = priv->valid_antenna;
1860         crl->antenna = priv->antenna;
1861         crl->is_green = rs_use_green(priv);
1862         crl->active_rate = priv->active_rate;
1863         crl->active_rate &= ~(0x1000);
1864         crl->active_rate_basic = priv->active_rate_basic;
1865         crl->phymode = priv->phymode;
1866 #ifdef CONFIG_IWLWIFI_HT
1867         crl->active_siso_rate = (priv->current_assoc_ht.supp_rates[0] << 1);
1868         crl->active_siso_rate |= (priv->current_assoc_ht.supp_rates[0] & 0x1);
1869         crl->active_siso_rate &= ~((u16)0x2);
1870         crl->active_siso_rate = crl->active_siso_rate << IWL_FIRST_OFDM_RATE;
1871
1872         crl->active_mimo_rate = (priv->current_assoc_ht.supp_rates[1] << 1);
1873         crl->active_mimo_rate |= (priv->current_assoc_ht.supp_rates[1] & 0x1);
1874         crl->active_mimo_rate &= ~((u16)0x2);
1875         crl->active_mimo_rate = crl->active_mimo_rate << IWL_FIRST_OFDM_RATE;
1876         IWL_DEBUG_HT("MIMO RATE 0x%X SISO MASK 0x%X\n", crl->active_siso_rate,
1877                      crl->active_mimo_rate);
1878 #endif /*CONFIG_IWLWIFI_HT*/
1879 #ifdef CONFIG_MAC80211_DEBUGFS
1880         crl->drv = priv;
1881 #endif
1882
1883         if (priv->assoc_station_added)
1884                 priv->lq_mngr.lq_ready = 1;
1885
1886         rs_initialize_lq(priv, sta);
1887 }
1888
1889 static void rs_fill_link_cmd(struct iwl_rate_scale_priv *lq_data,
1890                             struct iwl_rate *tx_mcs,
1891                             struct iwl_link_quality_cmd *lq_cmd)
1892 {
1893         int index = 0;
1894         int rate_idx;
1895         int repeat_rate = 0;
1896         u8 ant_toggle_count = 0;
1897         u8 use_ht_possible = 1;
1898         struct iwl_rate new_rate;
1899         struct iwl_scale_tbl_info tbl_type = { 0 };
1900
1901         rs_dbgfs_set_mcs(lq_data, tx_mcs, index);
1902
1903         rs_get_tbl_info_from_mcs(tx_mcs, lq_data->phymode,
1904                                   &tbl_type, &rate_idx);
1905
1906         if (is_legacy(tbl_type.lq_type)) {
1907                 ant_toggle_count = 1;
1908                 repeat_rate = IWL_NUMBER_TRY;
1909         } else
1910                 repeat_rate = IWL_HT_NUMBER_TRY;
1911
1912         lq_cmd->general_params.mimo_delimiter =
1913                         is_mimo(tbl_type.lq_type) ? 1 : 0;
1914         lq_cmd->rs_table[index].rate_n_flags =
1915                         cpu_to_le32(tx_mcs->rate_n_flags);
1916         new_rate.rate_n_flags = tx_mcs->rate_n_flags;
1917
1918         if (is_mimo(tbl_type.lq_type) || (tbl_type.antenna_type == ANT_MAIN))
1919                 lq_cmd->general_params.single_stream_ant_msk = 1;
1920         else
1921                 lq_cmd->general_params.single_stream_ant_msk = 2;
1922
1923         index++;
1924         repeat_rate--;
1925
1926         while (index < LINK_QUAL_MAX_RETRY_NUM) {
1927                 while (repeat_rate > 0 && (index < LINK_QUAL_MAX_RETRY_NUM)) {
1928                         if (is_legacy(tbl_type.lq_type)) {
1929                                 if (ant_toggle_count <
1930                                     NUM_TRY_BEFORE_ANTENNA_TOGGLE)
1931                                         ant_toggle_count++;
1932                                 else {
1933                                         rs_toggle_antenna(&new_rate, &tbl_type);
1934                                         ant_toggle_count = 1;
1935                                 }
1936                         }
1937
1938                         rs_dbgfs_set_mcs(lq_data, &new_rate, index);
1939                         lq_cmd->rs_table[index].rate_n_flags =
1940                                         cpu_to_le32(new_rate.rate_n_flags);
1941                         repeat_rate--;
1942                         index++;
1943                 }
1944
1945                 rs_get_tbl_info_from_mcs(&new_rate, lq_data->phymode, &tbl_type,
1946                                                 &rate_idx);
1947
1948                 if (is_mimo(tbl_type.lq_type))
1949                         lq_cmd->general_params.mimo_delimiter = index;
1950
1951                 rs_get_lower_rate(lq_data, &tbl_type, rate_idx,
1952                                   use_ht_possible, &new_rate);
1953
1954                 if (is_legacy(tbl_type.lq_type)) {
1955                         if (ant_toggle_count < NUM_TRY_BEFORE_ANTENNA_TOGGLE)
1956                                 ant_toggle_count++;
1957                         else {
1958                                 rs_toggle_antenna(&new_rate, &tbl_type);
1959                                 ant_toggle_count = 1;
1960                         }
1961                         repeat_rate = IWL_NUMBER_TRY;
1962                 } else
1963                         repeat_rate = IWL_HT_NUMBER_TRY;
1964
1965                 use_ht_possible = 0;
1966
1967                 rs_dbgfs_set_mcs(lq_data, &new_rate, index);
1968                 lq_cmd->rs_table[index].rate_n_flags =
1969                                 cpu_to_le32(new_rate.rate_n_flags);
1970
1971                 index++;
1972                 repeat_rate--;
1973         }
1974
1975         lq_cmd->general_params.dual_stream_ant_msk = 3;
1976         lq_cmd->agg_params.agg_dis_start_th = 3;
1977         lq_cmd->agg_params.agg_time_limit = cpu_to_le16(4000);
1978 }
1979
1980 static void *rs_alloc(struct ieee80211_local *local)
1981 {
1982         return local->hw.priv;
1983 }
1984 /* rate scale requires free function to be implemented */
1985 static void rs_free(void *priv_rate)
1986 {
1987         return;
1988 }
1989
1990 static void rs_clear(void *priv_rate)
1991 {
1992         struct iwl_priv *priv = (struct iwl_priv *) priv_rate;
1993
1994         IWL_DEBUG_RATE("enter\n");
1995
1996         priv->lq_mngr.lq_ready = 0;
1997 #ifdef CONFIG_IWLWIFI_HT
1998 #ifdef CONFIG_IWLWIFI_HT_AGG
1999         if (priv->lq_mngr.agg_ctrl.granted_ba)
2000                 iwl4965_turn_off_agg(priv, TID_ALL_SPECIFIED);
2001 #endif /*CONFIG_IWLWIFI_HT_AGG */
2002 #endif /* CONFIG_IWLWIFI_HT */
2003
2004         IWL_DEBUG_RATE("leave\n");
2005 }
2006
2007 static void rs_free_sta(void *priv, void *priv_sta)
2008 {
2009         struct iwl_rate_scale_priv *rs_priv = priv_sta;
2010
2011         IWL_DEBUG_RATE("enter\n");
2012         kfree(rs_priv);
2013         IWL_DEBUG_RATE("leave\n");
2014 }
2015
2016
2017 #ifdef CONFIG_MAC80211_DEBUGFS
2018 static int open_file_generic(struct inode *inode, struct file *file)
2019 {
2020         file->private_data = inode->i_private;
2021         return 0;
2022 }
2023 static void rs_dbgfs_set_mcs(struct iwl_rate_scale_priv *rs_priv,
2024                                 struct iwl_rate *mcs, int index)
2025 {
2026         const u32 cck_rate = 0x820A;
2027         if (rs_priv->dbg_fixed.rate_n_flags) {
2028                 if (index < 12)
2029                         mcs->rate_n_flags = rs_priv->dbg_fixed.rate_n_flags;
2030                 else
2031                         mcs->rate_n_flags = cck_rate;
2032                 IWL_DEBUG_RATE("Fixed rate ON\n");
2033                 return;
2034         }
2035
2036         IWL_DEBUG_RATE("Fixed rate OFF\n");
2037 }
2038
2039 static ssize_t rs_sta_dbgfs_scale_table_write(struct file *file,
2040                         const char __user *user_buf, size_t count, loff_t *ppos)
2041 {
2042         struct iwl_rate_scale_priv *rs_priv = file->private_data;
2043         char buf[64];
2044         int buf_size;
2045         u32 parsed_rate;
2046
2047         memset(buf, 0, sizeof(buf));
2048         buf_size = min(count, sizeof(buf) -  1);
2049         if (copy_from_user(buf, user_buf, buf_size))
2050                 return -EFAULT;
2051
2052         if (sscanf(buf, "%x", &parsed_rate) == 1)
2053                 rs_priv->dbg_fixed.rate_n_flags = parsed_rate;
2054         else
2055                 rs_priv->dbg_fixed.rate_n_flags = 0;
2056
2057         rs_priv->active_rate = 0x0FFF;
2058         rs_priv->active_siso_rate = 0x1FD0;
2059         rs_priv->active_mimo_rate = 0x1FD0;
2060
2061         IWL_DEBUG_RATE("sta_id %d rate 0x%X\n",
2062                 rs_priv->lq.sta_id, rs_priv->dbg_fixed.rate_n_flags);
2063
2064         if (rs_priv->dbg_fixed.rate_n_flags) {
2065                 rs_fill_link_cmd(rs_priv, &rs_priv->dbg_fixed, &rs_priv->lq);
2066                 rs_send_lq_cmd(rs_priv->drv, &rs_priv->lq, CMD_ASYNC);
2067         }
2068
2069         return count;
2070 }
2071 static ssize_t rs_sta_dbgfs_scale_table_read(struct file *file,
2072                         char __user *user_buf, size_t count, loff_t *ppos)
2073 {
2074         char buff[1024];
2075         int desc = 0;
2076         int i = 0;
2077
2078         struct iwl_rate_scale_priv *rs_priv = file->private_data;
2079
2080         desc += sprintf(buff+desc, "sta_id %d\n", rs_priv->lq.sta_id);
2081         desc += sprintf(buff+desc, "failed=%d success=%d rate=0%X\n",
2082                         rs_priv->total_failed, rs_priv->total_success,
2083                         rs_priv->active_rate);
2084         desc += sprintf(buff+desc, "fixed rate 0x%X\n",
2085                         rs_priv->dbg_fixed.rate_n_flags);
2086         desc += sprintf(buff+desc, "general:"
2087                 "flags=0x%X mimo-d=%d s-ant0x%x d-ant=0x%x\n",
2088                 rs_priv->lq.general_params.flags,
2089                 rs_priv->lq.general_params.mimo_delimiter,
2090                 rs_priv->lq.general_params.single_stream_ant_msk,
2091                 rs_priv->lq.general_params.dual_stream_ant_msk);
2092
2093         desc += sprintf(buff+desc, "agg:"
2094                         "time_limit=%d dist_start_th=%d frame_cnt_limit=%d\n",
2095                         le16_to_cpu(rs_priv->lq.agg_params.agg_time_limit),
2096                         rs_priv->lq.agg_params.agg_dis_start_th,
2097                         rs_priv->lq.agg_params.agg_frame_cnt_limit);
2098
2099         desc += sprintf(buff+desc,
2100                         "Start idx [0]=0x%x [1]=0x%x [2]=0x%x [3]=0x%x\n",
2101                         rs_priv->lq.general_params.start_rate_index[0],
2102                         rs_priv->lq.general_params.start_rate_index[1],
2103                         rs_priv->lq.general_params.start_rate_index[2],
2104                         rs_priv->lq.general_params.start_rate_index[3]);
2105
2106
2107         for (i = 0; i < LINK_QUAL_MAX_RETRY_NUM; i++)
2108                 desc += sprintf(buff+desc, " rate[%d] 0x%X\n",
2109                         i, le32_to_cpu(rs_priv->lq.rs_table[i].rate_n_flags));
2110
2111         return simple_read_from_buffer(user_buf, count, ppos, buff, desc);
2112 }
2113
2114 static const struct file_operations rs_sta_dbgfs_scale_table_ops = {
2115         .write = rs_sta_dbgfs_scale_table_write,
2116         .read = rs_sta_dbgfs_scale_table_read,
2117         .open = open_file_generic,
2118 };
2119
2120 static void rs_add_debugfs(void *priv, void *priv_sta,
2121                                         struct dentry *dir)
2122 {
2123         struct iwl_rate_scale_priv *rs_priv = priv_sta;
2124         rs_priv->rs_sta_dbgfs_scale_table_file =
2125                 debugfs_create_file("rate_scale_table", 0444, dir,
2126                                 rs_priv, &rs_sta_dbgfs_scale_table_ops);
2127 }
2128
2129 static void rs_remove_debugfs(void *priv, void *priv_sta)
2130 {
2131         struct iwl_rate_scale_priv *rs_priv = priv_sta;
2132         debugfs_remove(rs_priv->rs_sta_dbgfs_scale_table_file);
2133 }
2134 #endif
2135
2136 static struct rate_control_ops rs_ops = {
2137         .module = NULL,
2138         .name = RS_NAME,
2139         .tx_status = rs_tx_status,
2140         .get_rate = rs_get_rate,
2141         .rate_init = rs_rate_init,
2142         .clear = rs_clear,
2143         .alloc = rs_alloc,
2144         .free = rs_free,
2145         .alloc_sta = rs_alloc_sta,
2146         .free_sta = rs_free_sta,
2147 #ifdef CONFIG_MAC80211_DEBUGFS
2148         .add_sta_debugfs = rs_add_debugfs,
2149         .remove_sta_debugfs = rs_remove_debugfs,
2150 #endif
2151 };
2152
2153 int iwl_fill_rs_info(struct ieee80211_hw *hw, char *buf, u8 sta_id)
2154 {
2155         struct ieee80211_local *local = hw_to_local(hw);
2156         struct iwl_priv *priv = hw->priv;
2157         struct iwl_rate_scale_priv *rs_priv;
2158         struct sta_info *sta;
2159         int count = 0, i;
2160         u32 samples = 0, success = 0, good = 0;
2161         unsigned long now = jiffies;
2162         u32 max_time = 0;
2163         u8 lq_type, antenna;
2164
2165         sta = sta_info_get(local, priv->stations[sta_id].sta.sta.addr);
2166         if (!sta || !sta->rate_ctrl_priv) {
2167                 if (sta) {
2168                         sta_info_put(sta);
2169                         IWL_DEBUG_RATE("leave - no private rate data!\n");
2170                 } else
2171                         IWL_DEBUG_RATE("leave - no station!\n");
2172                 return sprintf(buf, "station %d not found\n", sta_id);
2173         }
2174
2175         rs_priv = (void *)sta->rate_ctrl_priv;
2176
2177         lq_type = rs_priv->lq_info[rs_priv->active_tbl].lq_type;
2178         antenna = rs_priv->lq_info[rs_priv->active_tbl].antenna_type;
2179
2180         if (is_legacy(lq_type))
2181                 i = IWL_RATE_54M_INDEX;
2182         else
2183                 i = IWL_RATE_60M_INDEX;
2184         while (1) {
2185                 u64 mask;
2186                 int j;
2187                 int active = rs_priv->active_tbl;
2188
2189                 count +=
2190                     sprintf(&buf[count], " %2dMbs: ", iwl_rates[i].ieee / 2);
2191
2192                 mask = (1ULL << (IWL_RATE_MAX_WINDOW - 1));
2193                 for (j = 0; j < IWL_RATE_MAX_WINDOW; j++, mask >>= 1)
2194                         buf[count++] =
2195                                 (rs_priv->lq_info[active].win[i].data & mask)
2196                                 ? '1' : '0';
2197
2198                 samples += rs_priv->lq_info[active].win[i].counter;
2199                 good += rs_priv->lq_info[active].win[i].success_counter;
2200                 success += rs_priv->lq_info[active].win[i].success_counter *
2201                            iwl_rates[i].ieee;
2202
2203                 if (rs_priv->lq_info[active].win[i].stamp) {
2204                         int delta =
2205                                    jiffies_to_msecs(now -
2206                                    rs_priv->lq_info[active].win[i].stamp);
2207
2208                         if (delta > max_time)
2209                                 max_time = delta;
2210
2211                         count += sprintf(&buf[count], "%5dms\n", delta);
2212                 } else
2213                         buf[count++] = '\n';
2214
2215                 j = iwl_get_prev_ieee_rate(i);
2216                 if (j == i)
2217                         break;
2218                 i = j;
2219         }
2220
2221         /* Display the average rate of all samples taken.
2222          *
2223          * NOTE:  We multiple # of samples by 2 since the IEEE measurement
2224          * added from iwl_rates is actually 2X the rate */
2225         if (samples)
2226                 count += sprintf(&buf[count],
2227                          "\nAverage rate is %3d.%02dMbs over last %4dms\n"
2228                          "%3d%% success (%d good packets over %d tries)\n",
2229                          success / (2 * samples), (success * 5 / samples) % 10,
2230                          max_time, good * 100 / samples, good, samples);
2231         else
2232                 count += sprintf(&buf[count], "\nAverage rate: 0Mbs\n");
2233         count += sprintf(&buf[count], "\nrate scale type %d anntena %d "
2234                          "active_search %d rate index %d\n", lq_type, antenna,
2235                          rs_priv->search_better_tbl, sta->last_txrate);
2236
2237         sta_info_put(sta);
2238         return count;
2239 }
2240
2241 void iwl_rate_scale_init(struct ieee80211_hw *hw, s32 sta_id)
2242 {
2243         struct iwl_priv *priv = hw->priv;
2244
2245         priv->lq_mngr.lq_ready = 1;
2246 }
2247
2248 void iwl_rate_control_register(struct ieee80211_hw *hw)
2249 {
2250         ieee80211_rate_control_register(&rs_ops);
2251 }
2252
2253 void iwl_rate_control_unregister(struct ieee80211_hw *hw)
2254 {
2255         ieee80211_rate_control_unregister(&rs_ops);
2256 }
2257