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

   1 /******************************************************************************
   2  *
   3  * Copyright(c) 2003 - 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
  27 #include <linux/kernel.h>
  28 #include <linux/module.h>
  29 #include <linux/version.h>
  30 #include <linux/init.h>
  31 #include <linux/pci.h>
  32 #include <linux/dma-mapping.h>
  33 #include <linux/delay.h>
  34 #include <linux/skbuff.h>
  35 #include <linux/netdevice.h>
  36 #include <linux/wireless.h>
  37 #include <net/mac80211.h>
  38 #include <linux/etherdevice.h>
  39
  40 #include "iwl-4965.h"
  41 #include "iwl-helpers.h"
  42
  43 static void iwl4965_hw_card_show_info(struct iwl4965_priv *priv);
  44
  45 #define IWL_DECLARE_RATE_INFO(r, s, ip, in, rp, rn, pp, np)    \
  46         [IWL_RATE_##r##M_INDEX] = { IWL_RATE_##r##M_PLCP,      \
  47                                     IWL_RATE_SISO_##s##M_PLCP, \
  48                                     IWL_RATE_MIMO_##s##M_PLCP, \
  49                                     IWL_RATE_##r##M_IEEE,      \
  50                                     IWL_RATE_##ip##M_INDEX,    \
  51                                     IWL_RATE_##in##M_INDEX,    \
  52                                     IWL_RATE_##rp##M_INDEX,    \
  53                                     IWL_RATE_##rn##M_INDEX,    \
  54                                     IWL_RATE_##pp##M_INDEX,    \
  55                                     IWL_RATE_##np##M_INDEX }
  56
  57 /*
  58  * Parameter order:
  59  *   rate, ht rate, prev rate, next rate, prev tgg rate, next tgg rate
  60  *
  61  * If there isn't a valid next or previous rate then INV is used which
  62  * maps to IWL_RATE_INVALID
  63  *
  64  */
  65 const struct iwl4965_rate_info iwl4965_rates[IWL_RATE_COUNT] = {
  66         IWL_DECLARE_RATE_INFO(1, INV, INV, 2, INV, 2, INV, 2),    /*  1mbps */
  67         IWL_DECLARE_RATE_INFO(2, INV, 1, 5, 1, 5, 1, 5),          /*  2mbps */
  68         IWL_DECLARE_RATE_INFO(5, INV, 2, 6, 2, 11, 2, 11),        /*5.5mbps */
  69         IWL_DECLARE_RATE_INFO(11, INV, 9, 12, 9, 12, 5, 18),      /* 11mbps */
  70         IWL_DECLARE_RATE_INFO(6, 6, 5, 9, 5, 11, 5, 11),        /*  6mbps */
  71         IWL_DECLARE_RATE_INFO(9, 6, 6, 11, 6, 11, 5, 11),       /*  9mbps */
  72         IWL_DECLARE_RATE_INFO(12, 12, 11, 18, 11, 18, 11, 18),   /* 12mbps */
  73         IWL_DECLARE_RATE_INFO(18, 18, 12, 24, 12, 24, 11, 24),   /* 18mbps */
  74         IWL_DECLARE_RATE_INFO(24, 24, 18, 36, 18, 36, 18, 36),   /* 24mbps */
  75         IWL_DECLARE_RATE_INFO(36, 36, 24, 48, 24, 48, 24, 48),   /* 36mbps */
  76         IWL_DECLARE_RATE_INFO(48, 48, 36, 54, 36, 54, 36, 54),   /* 48mbps */
  77         IWL_DECLARE_RATE_INFO(54, 54, 48, INV, 48, INV, 48, INV),/* 54mbps */
  78         IWL_DECLARE_RATE_INFO(60, 60, 48, INV, 48, INV, 48, INV),/* 60mbps */
  79 };
  80
  81 static int is_fat_channel(__le32 rxon_flags)
  82 {
  83         return (rxon_flags & RXON_FLG_CHANNEL_MODE_PURE_40_MSK) ||
  84                 (rxon_flags & RXON_FLG_CHANNEL_MODE_MIXED_MSK);
  85 }
  86
  87 static u8 is_single_stream(struct iwl4965_priv *priv)
  88 {
  89 #ifdef CONFIG_IWL4965_HT
  90         if (!priv->is_ht_enabled || !priv->current_assoc_ht.is_ht ||
  91             (priv->active_rate_ht[1] == 0) ||
  92             (priv->ps_mode == IWL_MIMO_PS_STATIC))
  93                 return 1;
  94 #else
  95         return 1;
  96 #endif  /*CONFIG_IWL4965_HT */
  97         return 0;
  98 }
  99
 100 /*
 101  * Determine how many receiver/antenna chains to use.
 102  * More provides better reception via diversity.  Fewer saves power.
 103  * MIMO (dual stream) requires at least 2, but works better with 3.
 104  * This does not determine *which* chains to use, just how many.
 105  */
 106 static int iwl4965_get_rx_chain_counter(struct iwl4965_priv *priv,
 107                                         u8 *idle_state, u8 *rx_state)
 108 {
 109         u8 is_single = is_single_stream(priv);
 110         u8 is_cam = test_bit(STATUS_POWER_PMI, &priv->status) ? 0 : 1;
 111
 112         /* # of Rx chains to use when expecting MIMO. */
 113         if (is_single || (!is_cam && (priv->ps_mode == IWL_MIMO_PS_STATIC)))
 114                 *rx_state = 2;
 115         else
 116                 *rx_state = 3;
 117
 118         /* # Rx chains when idling and maybe trying to save power */
 119         switch (priv->ps_mode) {
 120         case IWL_MIMO_PS_STATIC:
 121         case IWL_MIMO_PS_DYNAMIC:
 122                 *idle_state = (is_cam) ? 2 : 1;
 123                 break;
 124         case IWL_MIMO_PS_NONE:
 125                 *idle_state = (is_cam) ? *rx_state : 1;
 126                 break;
 127         default:
 128                 *idle_state = 1;
 129                 break;
 130         }
 131
 132         return 0;
 133 }
 134
 135 int iwl4965_hw_rxq_stop(struct iwl4965_priv *priv)
 136 {
 137         int rc;
 138         unsigned long flags;
 139
 140         spin_lock_irqsave(&priv->lock, flags);
 141         rc = iwl4965_grab_nic_access(priv);
 142         if (rc) {
 143                 spin_unlock_irqrestore(&priv->lock, flags);
 144                 return rc;
 145         }
 146
 147         /* stop HW */
 148         iwl4965_write_direct32(priv, FH_MEM_RCSR_CHNL0_CONFIG_REG, 0);
 149         rc = iwl4965_poll_direct_bit(priv, FH_MEM_RSSR_RX_STATUS_REG,
 150                                      (1 << 24), 1000);
 151         if (rc < 0)
 152                 IWL_ERROR("Can't stop Rx DMA.\n");
 153
 154         iwl4965_release_nic_access(priv);
 155         spin_unlock_irqrestore(&priv->lock, flags);
 156
 157         return 0;
 158 }
 159
 160 u8 iwl4965_hw_find_station(struct iwl4965_priv *priv, const u8 *addr)
 161 {
 162         int i;
 163         int start = 0;
 164         int ret = IWL_INVALID_STATION;
 165         unsigned long flags;
 166         DECLARE_MAC_BUF(mac);
 167
 168         if ((priv->iw_mode == IEEE80211_IF_TYPE_IBSS) ||
 169             (priv->iw_mode == IEEE80211_IF_TYPE_AP))
 170                 start = IWL_STA_ID;
 171
 172         if (is_broadcast_ether_addr(addr))
 173                 return IWL4965_BROADCAST_ID;
 174
 175         spin_lock_irqsave(&priv->sta_lock, flags);
 176         for (i = start; i < priv->hw_setting.max_stations; i++)
 177                 if ((priv->stations[i].used) &&
 178                     (!compare_ether_addr
 179                      (priv->stations[i].sta.sta.addr, addr))) {
 180                         ret = i;
 181                         goto out;
 182                 }
 183
 184         IWL_DEBUG_ASSOC_LIMIT("can not find STA %s total %d\n",
 185                         print_mac(mac, addr), priv->num_stations);
 186
 187  out:
 188         spin_unlock_irqrestore(&priv->sta_lock, flags);
 189         return ret;
 190 }
 191
 192 static int iwl4965_nic_set_pwr_src(struct iwl4965_priv *priv, int pwr_max)
 193 {
 194         int ret;
 195         unsigned long flags;
 196
 197         spin_lock_irqsave(&priv->lock, flags);
 198         ret = iwl4965_grab_nic_access(priv);
 199         if (ret) {
 200                 spin_unlock_irqrestore(&priv->lock, flags);
 201                 return ret;
 202         }
 203
 204         if (!pwr_max) {
 205                 u32 val;
 206
 207                 ret = pci_read_config_dword(priv->pci_dev, PCI_POWER_SOURCE,
 208                                            &val);
 209
 210                 if (val & PCI_CFG_PMC_PME_FROM_D3COLD_SUPPORT)
 211                         iwl4965_set_bits_mask_prph(priv, APMG_PS_CTRL_REG,
 212                                 APMG_PS_CTRL_VAL_PWR_SRC_VAUX,
 213                                 ~APMG_PS_CTRL_MSK_PWR_SRC);
 214         } else
 215                 iwl4965_set_bits_mask_prph(priv, APMG_PS_CTRL_REG,
 216                         APMG_PS_CTRL_VAL_PWR_SRC_VMAIN,
 217                         ~APMG_PS_CTRL_MSK_PWR_SRC);
 218
 219         iwl4965_release_nic_access(priv);
 220         spin_unlock_irqrestore(&priv->lock, flags);
 221
 222         return ret;
 223 }
 224
 225 static int iwl4965_rx_init(struct iwl4965_priv *priv, struct iwl4965_rx_queue *rxq)
 226 {
 227         int rc;
 228         unsigned long flags;
 229
 230         spin_lock_irqsave(&priv->lock, flags);
 231         rc = iwl4965_grab_nic_access(priv);
 232         if (rc) {
 233                 spin_unlock_irqrestore(&priv->lock, flags);
 234                 return rc;
 235         }
 236
 237         /* stop HW */
 238         iwl4965_write_direct32(priv, FH_MEM_RCSR_CHNL0_CONFIG_REG, 0);
 239
 240         iwl4965_write_direct32(priv, FH_RSCSR_CHNL0_RBDCB_WPTR_REG, 0);
 241         iwl4965_write_direct32(priv, FH_RSCSR_CHNL0_RBDCB_BASE_REG,
 242                              rxq->dma_addr >> 8);
 243
 244         iwl4965_write_direct32(priv, FH_RSCSR_CHNL0_STTS_WPTR_REG,
 245                              (priv->hw_setting.shared_phys +
 246                               offsetof(struct iwl4965_shared, val0)) >> 4);
 247
 248         iwl4965_write_direct32(priv, FH_MEM_RCSR_CHNL0_CONFIG_REG,
 249                              FH_RCSR_RX_CONFIG_CHNL_EN_ENABLE_VAL |
 250                              FH_RCSR_CHNL0_RX_CONFIG_IRQ_DEST_INT_HOST_VAL |
 251                              IWL_FH_RCSR_RX_CONFIG_REG_VAL_RB_SIZE_4K |
 252                              /*0x10 << 4 | */
 253                              (RX_QUEUE_SIZE_LOG <<
 254                               FH_RCSR_RX_CONFIG_RBDCB_SIZE_BITSHIFT));
 255
 256         /*
 257          * iwl4965_write32(priv,CSR_INT_COAL_REG,0);
 258          */
 259
 260         iwl4965_release_nic_access(priv);
 261         spin_unlock_irqrestore(&priv->lock, flags);
 262
 263         return 0;
 264 }
 265
 266 static int iwl4965_kw_init(struct iwl4965_priv *priv)
 267 {
 268         unsigned long flags;
 269         int rc;
 270
 271         spin_lock_irqsave(&priv->lock, flags);
 272         rc = iwl4965_grab_nic_access(priv);
 273         if (rc)
 274                 goto out;
 275
 276         iwl4965_write_direct32(priv, IWL_FH_KW_MEM_ADDR_REG,
 277                              priv->kw.dma_addr >> 4);
 278         iwl4965_release_nic_access(priv);
 279 out:
 280         spin_unlock_irqrestore(&priv->lock, flags);
 281         return rc;
 282 }
 283
 284 static int iwl4965_kw_alloc(struct iwl4965_priv *priv)
 285 {
 286         struct pci_dev *dev = priv->pci_dev;
 287         struct iwl4965_kw *kw = &priv->kw;
 288
 289         kw->size = IWL4965_KW_SIZE;     /* TBW need set somewhere else */
 290         kw->v_addr = pci_alloc_consistent(dev, kw->size, &kw->dma_addr);
 291         if (!kw->v_addr)
 292                 return -ENOMEM;
 293
 294         return 0;
 295 }
 296
 297 #define CHECK_AND_PRINT(x) ((eeprom_ch->flags & EEPROM_CHANNEL_##x) \
 298                             ? # x " " : "")
 299
 300 int iwl4965_set_fat_chan_info(struct iwl4965_priv *priv, int phymode, u16 channel,
 301                               const struct iwl4965_eeprom_channel *eeprom_ch,
 302                               u8 fat_extension_channel)
 303 {
 304         struct iwl4965_channel_info *ch_info;
 305
 306         ch_info = (struct iwl4965_channel_info *)
 307                         iwl4965_get_channel_info(priv, phymode, channel);
 308
 309         if (!is_channel_valid(ch_info))
 310                 return -1;
 311
 312         IWL_DEBUG_INFO("FAT Ch. %d [%sGHz] %s%s%s%s%s%s(0x%02x"
 313                         " %ddBm): Ad-Hoc %ssupported\n",
 314                         ch_info->channel,
 315                         is_channel_a_band(ch_info) ?
 316                         "5.2" : "2.4",
 317                         CHECK_AND_PRINT(IBSS),
 318                         CHECK_AND_PRINT(ACTIVE),
 319                         CHECK_AND_PRINT(RADAR),
 320                         CHECK_AND_PRINT(WIDE),
 321                         CHECK_AND_PRINT(NARROW),
 322                         CHECK_AND_PRINT(DFS),
 323                         eeprom_ch->flags,
 324                         eeprom_ch->max_power_avg,
 325                         ((eeprom_ch->flags & EEPROM_CHANNEL_IBSS)
 326                          && !(eeprom_ch->flags & EEPROM_CHANNEL_RADAR)) ?
 327                         "" : "not ");
 328
 329         ch_info->fat_eeprom = *eeprom_ch;
 330         ch_info->fat_max_power_avg = eeprom_ch->max_power_avg;
 331         ch_info->fat_curr_txpow = eeprom_ch->max_power_avg;
 332         ch_info->fat_min_power = 0;
 333         ch_info->fat_scan_power = eeprom_ch->max_power_avg;
 334         ch_info->fat_flags = eeprom_ch->flags;
 335         ch_info->fat_extension_channel = fat_extension_channel;
 336
 337         return 0;
 338 }
 339
 340 static void iwl4965_kw_free(struct iwl4965_priv *priv)
 341 {
 342         struct pci_dev *dev = priv->pci_dev;
 343         struct iwl4965_kw *kw = &priv->kw;
 344
 345         if (kw->v_addr) {
 346                 pci_free_consistent(dev, kw->size, kw->v_addr, kw->dma_addr);
 347                 memset(kw, 0, sizeof(*kw));
 348         }
 349 }
 350
 351 /**
 352  * iwl4965_txq_ctx_reset - Reset TX queue context
 353  * Destroys all DMA structures and initialise them again
 354  *
 355  * @param priv
 356  * @return error code
 357  */
 358 static int iwl4965_txq_ctx_reset(struct iwl4965_priv *priv)
 359 {
 360         int rc = 0;
 361         int txq_id, slots_num;
 362         unsigned long flags;
 363
 364         iwl4965_kw_free(priv);
 365
 366         iwl4965_hw_txq_ctx_free(priv);
 367
 368         /* Tx CMD queue */
 369         rc = iwl4965_kw_alloc(priv);
 370         if (rc) {
 371                 IWL_ERROR("Keep Warm allocation failed");
 372                 goto error_kw;
 373         }
 374
 375         spin_lock_irqsave(&priv->lock, flags);
 376
 377         rc = iwl4965_grab_nic_access(priv);
 378         if (unlikely(rc)) {
 379                 IWL_ERROR("TX reset failed");
 380                 spin_unlock_irqrestore(&priv->lock, flags);
 381                 goto error_reset;
 382         }
 383
 384         iwl4965_write_prph(priv, KDR_SCD_TXFACT, 0);
 385         iwl4965_release_nic_access(priv);
 386         spin_unlock_irqrestore(&priv->lock, flags);
 387
 388         rc = iwl4965_kw_init(priv);
 389         if (rc) {
 390                 IWL_ERROR("kw_init failed\n");
 391                 goto error_reset;
 392         }
 393
 394         /* Tx queue(s) */
 395         for (txq_id = 0; txq_id < priv->hw_setting.max_txq_num; txq_id++) {
 396                 slots_num = (txq_id == IWL_CMD_QUEUE_NUM) ?
 397                                         TFD_CMD_SLOTS : TFD_TX_CMD_SLOTS;
 398                 rc = iwl4965_tx_queue_init(priv, &priv->txq[txq_id], slots_num,
 399                                        txq_id);
 400                 if (rc) {
 401                         IWL_ERROR("Tx %d queue init failed\n", txq_id);
 402                         goto error;
 403                 }
 404         }
 405
 406         return rc;
 407
 408  error:
 409         iwl4965_hw_txq_ctx_free(priv);
 410  error_reset:
 411         iwl4965_kw_free(priv);
 412  error_kw:
 413         return rc;
 414 }
 415
 416 int iwl4965_hw_nic_init(struct iwl4965_priv *priv)
 417 {
 418         int rc;
 419         unsigned long flags;
 420         struct iwl4965_rx_queue *rxq = &priv->rxq;
 421         u8 rev_id;
 422         u32 val;
 423         u8 val_link;
 424
 425         iwl4965_power_init_handle(priv);
 426
 427         /* nic_init */
 428         spin_lock_irqsave(&priv->lock, flags);
 429
 430         iwl4965_set_bit(priv, CSR_GIO_CHICKEN_BITS,
 431                     CSR_GIO_CHICKEN_BITS_REG_BIT_DIS_L0S_EXIT_TIMER);
 432
 433         iwl4965_set_bit(priv, CSR_GP_CNTRL, CSR_GP_CNTRL_REG_FLAG_INIT_DONE);
 434         rc = iwl4965_poll_bit(priv, CSR_GP_CNTRL,
 435                           CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY,
 436                           CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY, 25000);
 437         if (rc < 0) {
 438                 spin_unlock_irqrestore(&priv->lock, flags);
 439                 IWL_DEBUG_INFO("Failed to init the card\n");
 440                 return rc;
 441         }
 442
 443         rc = iwl4965_grab_nic_access(priv);
 444         if (rc) {
 445                 spin_unlock_irqrestore(&priv->lock, flags);
 446                 return rc;
 447         }
 448
 449         iwl4965_read_prph(priv, APMG_CLK_CTRL_REG);
 450
 451         iwl4965_write_prph(priv, APMG_CLK_CTRL_REG,
 452                                  APMG_CLK_VAL_DMA_CLK_RQT |
 453                                  APMG_CLK_VAL_BSM_CLK_RQT);
 454         iwl4965_read_prph(priv, APMG_CLK_CTRL_REG);
 455
 456         udelay(20);
 457
 458         iwl4965_set_bits_prph(priv, APMG_PCIDEV_STT_REG,
 459                                     APMG_PCIDEV_STT_VAL_L1_ACT_DIS);
 460
 461         iwl4965_release_nic_access(priv);
 462         iwl4965_write32(priv, CSR_INT_COALESCING, 512 / 32);
 463         spin_unlock_irqrestore(&priv->lock, flags);
 464
 465         /* Determine HW type */
 466         rc = pci_read_config_byte(priv->pci_dev, PCI_REVISION_ID, &rev_id);
 467         if (rc)
 468                 return rc;
 469
 470         IWL_DEBUG_INFO("HW Revision ID = 0x%X\n", rev_id);
 471
 472         iwl4965_nic_set_pwr_src(priv, 1);
 473         spin_lock_irqsave(&priv->lock, flags);
 474
 475         if ((rev_id & 0x80) == 0x80 && (rev_id & 0x7f) < 8) {
 476                 pci_read_config_dword(priv->pci_dev, PCI_REG_WUM8, &val);
 477                 /* Enable No Snoop field */
 478                 pci_write_config_dword(priv->pci_dev, PCI_REG_WUM8,
 479                                        val & ~(1 << 11));
 480         }
 481
 482         spin_unlock_irqrestore(&priv->lock, flags);
 483
 484         /* Read the EEPROM */
 485         rc = iwl4965_eeprom_init(priv);
 486         if (rc)
 487                 return rc;
 488
 489         if (priv->eeprom.calib_version < EEPROM_TX_POWER_VERSION_NEW) {
 490                 IWL_ERROR("Older EEPROM detected!  Aborting.\n");
 491                 return -EINVAL;
 492         }
 493
 494         pci_read_config_byte(priv->pci_dev, PCI_LINK_CTRL, &val_link);
 495
 496         /* disable L1 entry -- workaround for pre-B1 */
 497         pci_write_config_byte(priv->pci_dev, PCI_LINK_CTRL, val_link & ~0x02);
 498
 499         spin_lock_irqsave(&priv->lock, flags);
 500
 501         /* set CSR_HW_CONFIG_REG for uCode use */
 502
 503         iwl4965_set_bit(priv, CSR_SW_VER, CSR_HW_IF_CONFIG_REG_BIT_KEDRON_R |
 504                     CSR_HW_IF_CONFIG_REG_BIT_RADIO_SI |
 505                     CSR_HW_IF_CONFIG_REG_BIT_MAC_SI);
 506
 507         rc = iwl4965_grab_nic_access(priv);
 508         if (rc < 0) {
 509                 spin_unlock_irqrestore(&priv->lock, flags);
 510                 IWL_DEBUG_INFO("Failed to init the card\n");
 511                 return rc;
 512         }
 513
 514         iwl4965_read_prph(priv, APMG_PS_CTRL_REG);
 515         iwl4965_set_bits_prph(priv, APMG_PS_CTRL_REG,
 516                                     APMG_PS_CTRL_VAL_RESET_REQ);
 517         udelay(5);
 518         iwl4965_clear_bits_prph(priv, APMG_PS_CTRL_REG,
 519                                       APMG_PS_CTRL_VAL_RESET_REQ);
 520
 521         iwl4965_release_nic_access(priv);
 522         spin_unlock_irqrestore(&priv->lock, flags);
 523
 524         iwl4965_hw_card_show_info(priv);
 525
 526         /* end nic_init */
 527
 528         /* Allocate the RX queue, or reset if it is already allocated */
 529         if (!rxq->bd) {
 530                 rc = iwl4965_rx_queue_alloc(priv);
 531                 if (rc) {
 532                         IWL_ERROR("Unable to initialize Rx queue\n");
 533                         return -ENOMEM;
 534                 }
 535         } else
 536                 iwl4965_rx_queue_reset(priv, rxq);
 537
 538         iwl4965_rx_replenish(priv);
 539
 540         iwl4965_rx_init(priv, rxq);
 541
 542         spin_lock_irqsave(&priv->lock, flags);
 543
 544         rxq->need_update = 1;
 545         iwl4965_rx_queue_update_write_ptr(priv, rxq);
 546
 547         spin_unlock_irqrestore(&priv->lock, flags);
 548         rc = iwl4965_txq_ctx_reset(priv);
 549         if (rc)
 550                 return rc;
 551
 552         if (priv->eeprom.sku_cap & EEPROM_SKU_CAP_SW_RF_KILL_ENABLE)
 553                 IWL_DEBUG_RF_KILL("SW RF KILL supported in EEPROM.\n");
 554
 555         if (priv->eeprom.sku_cap & EEPROM_SKU_CAP_HW_RF_KILL_ENABLE)
 556                 IWL_DEBUG_RF_KILL("HW RF KILL supported in EEPROM.\n");
 557
 558         set_bit(STATUS_INIT, &priv->status);
 559
 560         return 0;
 561 }
 562
 563 int iwl4965_hw_nic_stop_master(struct iwl4965_priv *priv)
 564 {
 565         int rc = 0;
 566         u32 reg_val;
 567         unsigned long flags;
 568
 569         spin_lock_irqsave(&priv->lock, flags);
 570
 571         /* set stop master bit */
 572         iwl4965_set_bit(priv, CSR_RESET, CSR_RESET_REG_FLAG_STOP_MASTER);
 573
 574         reg_val = iwl4965_read32(priv, CSR_GP_CNTRL);
 575
 576         if (CSR_GP_CNTRL_REG_FLAG_MAC_POWER_SAVE ==
 577             (reg_val & CSR_GP_CNTRL_REG_MSK_POWER_SAVE_TYPE))
 578                 IWL_DEBUG_INFO("Card in power save, master is already "
 579                                "stopped\n");
 580         else {
 581                 rc = iwl4965_poll_bit(priv, CSR_RESET,
 582                                   CSR_RESET_REG_FLAG_MASTER_DISABLED,
 583                                   CSR_RESET_REG_FLAG_MASTER_DISABLED, 100);
 584                 if (rc < 0) {
 585                         spin_unlock_irqrestore(&priv->lock, flags);
 586                         return rc;
 587                 }
 588         }
 589
 590         spin_unlock_irqrestore(&priv->lock, flags);
 591         IWL_DEBUG_INFO("stop master\n");
 592
 593         return rc;
 594 }
 595
 596 void iwl4965_hw_txq_ctx_stop(struct iwl4965_priv *priv)
 597 {
 598
 599         int txq_id;
 600         unsigned long flags;
 601
 602         /* reset TFD queues */
 603         for (txq_id = 0; txq_id < priv->hw_setting.max_txq_num; txq_id++) {
 604                 spin_lock_irqsave(&priv->lock, flags);
 605                 if (iwl4965_grab_nic_access(priv)) {
 606                         spin_unlock_irqrestore(&priv->lock, flags);
 607                         continue;
 608                 }
 609
 610                 iwl4965_write_direct32(priv,
 611                                      IWL_FH_TCSR_CHNL_TX_CONFIG_REG(txq_id),
 612                                      0x0);
 613                 iwl4965_poll_direct_bit(priv, IWL_FH_TSSR_TX_STATUS_REG,
 614                                         IWL_FH_TSSR_TX_STATUS_REG_MSK_CHNL_IDLE
 615                                         (txq_id), 200);
 616                 iwl4965_release_nic_access(priv);
 617                 spin_unlock_irqrestore(&priv->lock, flags);
 618         }
 619
 620         iwl4965_hw_txq_ctx_free(priv);
 621 }
 622
 623 int iwl4965_hw_nic_reset(struct iwl4965_priv *priv)
 624 {
 625         int rc = 0;
 626         unsigned long flags;
 627
 628         iwl4965_hw_nic_stop_master(priv);
 629
 630         spin_lock_irqsave(&priv->lock, flags);
 631
 632         iwl4965_set_bit(priv, CSR_RESET, CSR_RESET_REG_FLAG_SW_RESET);
 633
 634         udelay(10);
 635
 636         iwl4965_set_bit(priv, CSR_GP_CNTRL, CSR_GP_CNTRL_REG_FLAG_INIT_DONE);
 637         rc = iwl4965_poll_bit(priv, CSR_RESET,
 638                           CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY,
 639                           CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY, 25);
 640
 641         udelay(10);
 642
 643         rc = iwl4965_grab_nic_access(priv);
 644         if (!rc) {
 645                 iwl4965_write_prph(priv, APMG_CLK_EN_REG,
 646                                          APMG_CLK_VAL_DMA_CLK_RQT |
 647                                          APMG_CLK_VAL_BSM_CLK_RQT);
 648
 649                 udelay(10);
 650
 651                 iwl4965_set_bits_prph(priv, APMG_PCIDEV_STT_REG,
 652                                 APMG_PCIDEV_STT_VAL_L1_ACT_DIS);
 653
 654                 iwl4965_release_nic_access(priv);
 655         }
 656
 657         clear_bit(STATUS_HCMD_ACTIVE, &priv->status);
 658         wake_up_interruptible(&priv->wait_command_queue);
 659
 660         spin_unlock_irqrestore(&priv->lock, flags);
 661
 662         return rc;
 663
 664 }
 665
 666 #define REG_RECALIB_PERIOD (60)
 667
 668 /**
 669  * iwl4965_bg_statistics_periodic - Timer callback to queue statistics
 670  *
 671  * This callback is provided in order to queue the statistics_work
 672  * in work_queue context (v. softirq)
 673  *
 674  * This timer function is continually reset to execute within
 675  * REG_RECALIB_PERIOD seconds since the last STATISTICS_NOTIFICATION
 676  * was received.  We need to ensure we receive the statistics in order
 677  * to update the temperature used for calibrating the TXPOWER.  However,
 678  * we can't send the statistics command from softirq context (which
 679  * is the context which timers run at) so we have to queue off the
 680  * statistics_work to actually send the command to the hardware.
 681  */
 682 static void iwl4965_bg_statistics_periodic(unsigned long data)
 683 {
 684         struct iwl4965_priv *priv = (struct iwl4965_priv *)data;
 685
 686         queue_work(priv->workqueue, &priv->statistics_work);
 687 }
 688
 689 /**
 690  * iwl4965_bg_statistics_work - Send the statistics request to the hardware.
 691  *
 692  * This is queued by iwl4965_bg_statistics_periodic.
 693  */
 694 static void iwl4965_bg_statistics_work(struct work_struct *work)
 695 {
 696         struct iwl4965_priv *priv = container_of(work, struct iwl4965_priv,
 697                                              statistics_work);
 698
 699         if (test_bit(STATUS_EXIT_PENDING, &priv->status))
 700                 return;
 701
 702         mutex_lock(&priv->mutex);
 703         iwl4965_send_statistics_request(priv);
 704         mutex_unlock(&priv->mutex);
 705 }
 706
 707 #define CT_LIMIT_CONST          259
 708 #define TM_CT_KILL_THRESHOLD    110
 709
 710 void iwl4965_rf_kill_ct_config(struct iwl4965_priv *priv)
 711 {
 712         struct iwl4965_ct_kill_config cmd;
 713         u32 R1, R2, R3;
 714         u32 temp_th;
 715         u32 crit_temperature;
 716         unsigned long flags;
 717         int rc = 0;
 718
 719         spin_lock_irqsave(&priv->lock, flags);
 720         iwl4965_write32(priv, CSR_UCODE_DRV_GP1_CLR,
 721                     CSR_UCODE_DRV_GP1_REG_BIT_CT_KILL_EXIT);
 722         spin_unlock_irqrestore(&priv->lock, flags);
 723
 724         if (priv->statistics.flag & STATISTICS_REPLY_FLG_FAT_MODE_MSK) {
 725                 R1 = (s32)le32_to_cpu(priv->card_alive_init.therm_r1[1]);
 726                 R2 = (s32)le32_to_cpu(priv->card_alive_init.therm_r2[1]);
 727                 R3 = (s32)le32_to_cpu(priv->card_alive_init.therm_r3[1]);
 728         } else {
 729                 R1 = (s32)le32_to_cpu(priv->card_alive_init.therm_r1[0]);
 730                 R2 = (s32)le32_to_cpu(priv->card_alive_init.therm_r2[0]);
 731                 R3 = (s32)le32_to_cpu(priv->card_alive_init.therm_r3[0]);
 732         }
 733
 734         temp_th = CELSIUS_TO_KELVIN(TM_CT_KILL_THRESHOLD);
 735
 736         crit_temperature = ((temp_th * (R3-R1))/CT_LIMIT_CONST) + R2;
 737         cmd.critical_temperature_R =  cpu_to_le32(crit_temperature);
 738         rc = iwl4965_send_cmd_pdu(priv,
 739                               REPLY_CT_KILL_CONFIG_CMD, sizeof(cmd), &cmd);
 740         if (rc)
 741                 IWL_ERROR("REPLY_CT_KILL_CONFIG_CMD failed\n");
 742         else
 743                 IWL_DEBUG_INFO("REPLY_CT_KILL_CONFIG_CMD succeeded\n");
 744 }
 745
 746 #ifdef CONFIG_IWL4965_SENSITIVITY
 747
 748 /* "false alarms" are signals that our DSP tries to lock onto,
 749  *   but then determines that they are either noise, or transmissions
 750  *   from a distant wireless network (also "noise", really) that get
 751  *   "stepped on" by stronger transmissions within our own network.
 752  * This algorithm attempts to set a sensitivity level that is high
 753  *   enough to receive all of our own network traffic, but not so
 754  *   high that our DSP gets too busy trying to lock onto non-network
 755  *   activity/noise. */
 756 static int iwl4965_sens_energy_cck(struct iwl4965_priv *priv,
 757                                    u32 norm_fa,
 758                                    u32 rx_enable_time,
 759                                    struct statistics_general_data *rx_info)
 760 {
 761         u32 max_nrg_cck = 0;
 762         int i = 0;
 763         u8 max_silence_rssi = 0;
 764         u32 silence_ref = 0;
 765         u8 silence_rssi_a = 0;
 766         u8 silence_rssi_b = 0;
 767         u8 silence_rssi_c = 0;
 768         u32 val;
 769
 770         /* "false_alarms" values below are cross-multiplications to assess the
 771          *   numbers of false alarms within the measured period of actual Rx
 772          *   (Rx is off when we're txing), vs the min/max expected false alarms
 773          *   (some should be expected if rx is sensitive enough) in a
 774          *   hypothetical listening period of 200 time units (TU), 204.8 msec:
 775          *
 776          * MIN_FA/fixed-time < false_alarms/actual-rx-time < MAX_FA/beacon-time
 777          *
 778          * */
 779         u32 false_alarms = norm_fa * 200 * 1024;
 780         u32 max_false_alarms = MAX_FA_CCK * rx_enable_time;
 781         u32 min_false_alarms = MIN_FA_CCK * rx_enable_time;
 782         struct iwl4965_sensitivity_data *data = NULL;
 783
 784         data = &(priv->sensitivity_data);
 785
 786         data->nrg_auto_corr_silence_diff = 0;
 787
 788         /* Find max silence rssi among all 3 receivers.
 789          * This is background noise, which may include transmissions from other
 790          *    networks, measured during silence before our network's beacon */
 791         silence_rssi_a = (u8)((rx_info->beacon_silence_rssi_a &
 792                             ALL_BAND_FILTER)>>8);
 793         silence_rssi_b = (u8)((rx_info->beacon_silence_rssi_b &
 794                             ALL_BAND_FILTER)>>8);
 795         silence_rssi_c = (u8)((rx_info->beacon_silence_rssi_c &
 796                             ALL_BAND_FILTER)>>8);
 797
 798         val = max(silence_rssi_b, silence_rssi_c);
 799         max_silence_rssi = max(silence_rssi_a, (u8) val);
 800
 801         /* Store silence rssi in 20-beacon history table */
 802         data->nrg_silence_rssi[data->nrg_silence_idx] = max_silence_rssi;
 803         data->nrg_silence_idx++;
 804         if (data->nrg_silence_idx >= NRG_NUM_PREV_STAT_L)
 805                 data->nrg_silence_idx = 0;
 806
 807         /* Find max silence rssi across 20 beacon history */
 808         for (i = 0; i < NRG_NUM_PREV_STAT_L; i++) {
 809                 val = data->nrg_silence_rssi[i];
 810                 silence_ref = max(silence_ref, val);
 811         }
 812         IWL_DEBUG_CALIB("silence a %u, b %u, c %u, 20-bcn max %u\n",
 813                         silence_rssi_a, silence_rssi_b, silence_rssi_c,
 814                         silence_ref);
 815
 816         /* Find max rx energy (min value!) among all 3 receivers,
 817          *   measured during beacon frame.
 818          * Save it in 10-beacon history table. */
 819         i = data->nrg_energy_idx;
 820         val = min(rx_info->beacon_energy_b, rx_info->beacon_energy_c);
 821         data->nrg_value[i] = min(rx_info->beacon_energy_a, val);
 822
 823         data->nrg_energy_idx++;
 824         if (data->nrg_energy_idx >= 10)
 825                 data->nrg_energy_idx = 0;
 826
 827         /* Find min rx energy (max value) across 10 beacon history.
 828          * This is the minimum signal level that we want to receive well.
 829          * Add backoff (margin so we don't miss slightly lower energy frames).
 830          * This establishes an upper bound (min value) for energy threshold. */
 831         max_nrg_cck = data->nrg_value[0];
 832         for (i = 1; i < 10; i++)
 833                 max_nrg_cck = (u32) max(max_nrg_cck, (data->nrg_value[i]));
 834         max_nrg_cck += 6;
 835
 836         IWL_DEBUG_CALIB("rx energy a %u, b %u, c %u, 10-bcn max/min %u\n",
 837                         rx_info->beacon_energy_a, rx_info->beacon_energy_b,
 838                         rx_info->beacon_energy_c, max_nrg_cck - 6);
 839
 840         /* Count number of consecutive beacons with fewer-than-desired
 841          *   false alarms. */
 842         if (false_alarms < min_false_alarms)
 843                 data->num_in_cck_no_fa++;
 844         else
 845                 data->num_in_cck_no_fa = 0;
 846         IWL_DEBUG_CALIB("consecutive bcns with few false alarms = %u\n",
 847                         data->num_in_cck_no_fa);
 848
 849         /* If we got too many false alarms this time, reduce sensitivity */
 850         if (false_alarms > max_false_alarms) {
 851                 IWL_DEBUG_CALIB("norm FA %u > max FA %u\n",
 852                              false_alarms, max_false_alarms);
 853                 IWL_DEBUG_CALIB("... reducing sensitivity\n");
 854                 data->nrg_curr_state = IWL_FA_TOO_MANY;
 855
 856                 if (data->auto_corr_cck > AUTO_CORR_MAX_TH_CCK) {
 857                         /* Store for "fewer than desired" on later beacon */
 858                         data->nrg_silence_ref = silence_ref;
 859
 860                         /* increase energy threshold (reduce nrg value)
 861                          *   to decrease sensitivity */
 862                         if (data->nrg_th_cck > (NRG_MAX_CCK + NRG_STEP_CCK))
 863                                 data->nrg_th_cck = data->nrg_th_cck
 864                                                          - NRG_STEP_CCK;
 865                 }
 866
 867                 /* increase auto_corr values to decrease sensitivity */
 868                 if (data->auto_corr_cck < AUTO_CORR_MAX_TH_CCK)
 869                         data->auto_corr_cck = AUTO_CORR_MAX_TH_CCK + 1;
 870                 else {
 871                         val = data->auto_corr_cck + AUTO_CORR_STEP_CCK;
 872                         data->auto_corr_cck = min((u32)AUTO_CORR_MAX_CCK, val);
 873                 }
 874                 val = data->auto_corr_cck_mrc + AUTO_CORR_STEP_CCK;
 875                 data->auto_corr_cck_mrc = min((u32)AUTO_CORR_MAX_CCK_MRC, val);
 876
 877         /* Else if we got fewer than desired, increase sensitivity */
 878         } else if (false_alarms < min_false_alarms) {
 879                 data->nrg_curr_state = IWL_FA_TOO_FEW;
 880
 881                 /* Compare silence level with silence level for most recent
 882                  *   healthy number or too many false alarms */
 883                 data->nrg_auto_corr_silence_diff = (s32)data->nrg_silence_ref -
 884                                                    (s32)silence_ref;
 885
 886                 IWL_DEBUG_CALIB("norm FA %u < min FA %u, silence diff %d\n",
 887                          false_alarms, min_false_alarms,
 888                          data->nrg_auto_corr_silence_diff);
 889
 890                 /* Increase value to increase sensitivity, but only if:
 891                  * 1a) previous beacon did *not* have *too many* false alarms
 892                  * 1b) AND there's a significant difference in Rx levels
 893                  *      from a previous beacon with too many, or healthy # FAs
 894                  * OR 2) We've seen a lot of beacons (100) with too few
 895                  *       false alarms */
 896                 if ((data->nrg_prev_state != IWL_FA_TOO_MANY) &&
 897                         ((data->nrg_auto_corr_silence_diff > NRG_DIFF) ||
 898                         (data->num_in_cck_no_fa > MAX_NUMBER_CCK_NO_FA))) {
 899
 900                         IWL_DEBUG_CALIB("... increasing sensitivity\n");
 901                         /* Increase nrg value to increase sensitivity */
 902                         val = data->nrg_th_cck + NRG_STEP_CCK;
 903                         data->nrg_th_cck = min((u32)NRG_MIN_CCK, val);
 904
 905                         /* Decrease auto_corr values to increase sensitivity */
 906                         val = data->auto_corr_cck - AUTO_CORR_STEP_CCK;
 907                         data->auto_corr_cck = max((u32)AUTO_CORR_MIN_CCK, val);
 908
 909                         val = data->auto_corr_cck_mrc - AUTO_CORR_STEP_CCK;
 910                         data->auto_corr_cck_mrc =
 911                                          max((u32)AUTO_CORR_MIN_CCK_MRC, val);
 912
 913                 } else
 914                         IWL_DEBUG_CALIB("... but not changing sensitivity\n");
 915
 916         /* Else we got a healthy number of false alarms, keep status quo */
 917         } else {
 918                 IWL_DEBUG_CALIB(" FA in safe zone\n");
 919                 data->nrg_curr_state = IWL_FA_GOOD_RANGE;
 920
 921                 /* Store for use in "fewer than desired" with later beacon */
 922                 data->nrg_silence_ref = silence_ref;
 923
 924                 /* If previous beacon had too many false alarms,
 925                  *   give it some extra margin by reducing sensitivity again
 926                  *   (but don't go below measured energy of desired Rx) */
 927                 if (IWL_FA_TOO_MANY == data->nrg_prev_state) {
 928                         IWL_DEBUG_CALIB("... increasing margin\n");
 929                         data->nrg_th_cck -= NRG_MARGIN;
 930                 }
 931         }
 932
 933         /* Make sure the energy threshold does not go above the measured
 934          * energy of the desired Rx signals (reduced by backoff margin),
 935          * or else we might start missing Rx frames.
 936          * Lower value is higher energy, so we use max()!
 937          */
 938         data->nrg_th_cck = max(max_nrg_cck, data->nrg_th_cck);
 939         IWL_DEBUG_CALIB("new nrg_th_cck %u\n", data->nrg_th_cck);
 940
 941         data->nrg_prev_state = data->nrg_curr_state;
 942
 943         return 0;
 944 }
 945
 946
 947 static int iwl4965_sens_auto_corr_ofdm(struct iwl4965_priv *priv,
 948                                        u32 norm_fa,
 949                                        u32 rx_enable_time)
 950 {
 951         u32 val;
 952         u32 false_alarms = norm_fa * 200 * 1024;
 953         u32 max_false_alarms = MAX_FA_OFDM * rx_enable_time;
 954         u32 min_false_alarms = MIN_FA_OFDM * rx_enable_time;
 955         struct iwl4965_sensitivity_data *data = NULL;
 956
 957         data = &(priv->sensitivity_data);
 958
 959         /* If we got too many false alarms this time, reduce sensitivity */
 960         if (false_alarms > max_false_alarms) {
 961
 962                 IWL_DEBUG_CALIB("norm FA %u > max FA %u)\n",
 963                              false_alarms, max_false_alarms);
 964
 965                 val = data->auto_corr_ofdm + AUTO_CORR_STEP_OFDM;
 966                 data->auto_corr_ofdm =
 967                                 min((u32)AUTO_CORR_MAX_OFDM, val);
 968
 969                 val = data->auto_corr_ofdm_mrc + AUTO_CORR_STEP_OFDM;
 970                 data->auto_corr_ofdm_mrc =
 971                                 min((u32)AUTO_CORR_MAX_OFDM_MRC, val);
 972
 973                 val = data->auto_corr_ofdm_x1 + AUTO_CORR_STEP_OFDM;
 974                 data->auto_corr_ofdm_x1 =
 975                                 min((u32)AUTO_CORR_MAX_OFDM_X1, val);
 976
 977                 val = data->auto_corr_ofdm_mrc_x1 + AUTO_CORR_STEP_OFDM;
 978                 data->auto_corr_ofdm_mrc_x1 =
 979                                 min((u32)AUTO_CORR_MAX_OFDM_MRC_X1, val);
 980         }
 981
 982         /* Else if we got fewer than desired, increase sensitivity */
 983         else if (false_alarms < min_false_alarms) {
 984
 985                 IWL_DEBUG_CALIB("norm FA %u < min FA %u\n",
 986                              false_alarms, min_false_alarms);
 987
 988                 val = data->auto_corr_ofdm - AUTO_CORR_STEP_OFDM;
 989                 data->auto_corr_ofdm =
 990                                 max((u32)AUTO_CORR_MIN_OFDM, val);
 991
 992                 val = data->auto_corr_ofdm_mrc - AUTO_CORR_STEP_OFDM;
 993                 data->auto_corr_ofdm_mrc =
 994                                 max((u32)AUTO_CORR_MIN_OFDM_MRC, val);
 995
 996                 val = data->auto_corr_ofdm_x1 - AUTO_CORR_STEP_OFDM;
 997                 data->auto_corr_ofdm_x1 =
 998                                 max((u32)AUTO_CORR_MIN_OFDM_X1, val);
 999
1000                 val = data->auto_corr_ofdm_mrc_x1 - AUTO_CORR_STEP_OFDM;
1001                 data->auto_corr_ofdm_mrc_x1 =
1002                                 max((u32)AUTO_CORR_MIN_OFDM_MRC_X1, val);
1003         }
1004
1005         else
1006                 IWL_DEBUG_CALIB("min FA %u < norm FA %u < max FA %u OK\n",
1007                          min_false_alarms, false_alarms, max_false_alarms);
1008
1009         return 0;
1010 }
1011
1012 static int iwl4965_sensitivity_callback(struct iwl4965_priv *priv,
1013                                     struct iwl4965_cmd *cmd, struct sk_buff *skb)
1014 {
1015         /* We didn't cache the SKB; let the caller free it */
1016         return 1;
1017 }
1018
1019 /* Prepare a SENSITIVITY_CMD, send to uCode if values have changed */
1020 static int iwl4965_sensitivity_write(struct iwl4965_priv *priv, u8 flags)
1021 {
1022         int rc = 0;
1023         struct iwl4965_sensitivity_cmd cmd ;
1024         struct iwl4965_sensitivity_data *data = NULL;
1025         struct iwl4965_host_cmd cmd_out = {
1026                 .id = SENSITIVITY_CMD,
1027                 .len = sizeof(struct iwl4965_sensitivity_cmd),
1028                 .meta.flags = flags,
1029                 .data = &cmd,
1030         };
1031
1032         data = &(priv->sensitivity_data);
1033
1034         memset(&cmd, 0, sizeof(cmd));
1035
1036         cmd.table[HD_AUTO_CORR32_X4_TH_ADD_MIN_INDEX] =
1037                                 cpu_to_le16((u16)data->auto_corr_ofdm);
1038         cmd.table[HD_AUTO_CORR32_X4_TH_ADD_MIN_MRC_INDEX] =
1039                                 cpu_to_le16((u16)data->auto_corr_ofdm_mrc);
1040         cmd.table[HD_AUTO_CORR32_X1_TH_ADD_MIN_INDEX] =
1041                                 cpu_to_le16((u16)data->auto_corr_ofdm_x1);
1042         cmd.table[HD_AUTO_CORR32_X1_TH_ADD_MIN_MRC_INDEX] =
1043                                 cpu_to_le16((u16)data->auto_corr_ofdm_mrc_x1);
1044
1045         cmd.table[HD_AUTO_CORR40_X4_TH_ADD_MIN_INDEX] =
1046                                 cpu_to_le16((u16)data->auto_corr_cck);
1047         cmd.table[HD_AUTO_CORR40_X4_TH_ADD_MIN_MRC_INDEX] =
1048                                 cpu_to_le16((u16)data->auto_corr_cck_mrc);
1049
1050         cmd.table[HD_MIN_ENERGY_CCK_DET_INDEX] =
1051                                 cpu_to_le16((u16)data->nrg_th_cck);
1052         cmd.table[HD_MIN_ENERGY_OFDM_DET_INDEX] =
1053                                 cpu_to_le16((u16)data->nrg_th_ofdm);
1054
1055         cmd.table[HD_BARKER_CORR_TH_ADD_MIN_INDEX] =
1056                                 __constant_cpu_to_le16(190);
1057         cmd.table[HD_BARKER_CORR_TH_ADD_MIN_MRC_INDEX] =
1058                                 __constant_cpu_to_le16(390);
1059         cmd.table[HD_OFDM_ENERGY_TH_IN_INDEX] =
1060                                 __constant_cpu_to_le16(62);
1061
1062         IWL_DEBUG_CALIB("ofdm: ac %u mrc %u x1 %u mrc_x1 %u thresh %u\n",
1063                         data->auto_corr_ofdm, data->auto_corr_ofdm_mrc,
1064                         data->auto_corr_ofdm_x1, data->auto_corr_ofdm_mrc_x1,
1065                         data->nrg_th_ofdm);
1066
1067         IWL_DEBUG_CALIB("cck: ac %u mrc %u thresh %u\n",
1068                         data->auto_corr_cck, data->auto_corr_cck_mrc,
1069                         data->nrg_th_cck);
1070
1071         cmd.control = SENSITIVITY_CMD_CONTROL_WORK_TABLE;
1072
1073         if (flags & CMD_ASYNC)
1074                 cmd_out.meta.u.callback = iwl4965_sensitivity_callback;
1075
1076         /* Don't send command to uCode if nothing has changed */
1077         if (!memcmp(&cmd.table[0], &(priv->sensitivity_tbl[0]),
1078                     sizeof(u16)*HD_TABLE_SIZE)) {
1079                 IWL_DEBUG_CALIB("No change in SENSITIVITY_CMD\n");
1080                 return 0;
1081         }
1082
1083         /* Copy table for comparison next time */
1084         memcpy(&(priv->sensitivity_tbl[0]), &(cmd.table[0]),
1085                sizeof(u16)*HD_TABLE_SIZE);
1086
1087         rc = iwl4965_send_cmd(priv, &cmd_out);
1088         if (!rc) {
1089                 IWL_DEBUG_CALIB("SENSITIVITY_CMD succeeded\n");
1090                 return rc;
1091         }
1092
1093         return 0;
1094 }
1095
1096 void iwl4965_init_sensitivity(struct iwl4965_priv *priv, u8 flags, u8 force)
1097 {
1098         int rc = 0;
1099         int i;
1100         struct iwl4965_sensitivity_data *data = NULL;
1101
1102         IWL_DEBUG_CALIB("Start iwl4965_init_sensitivity\n");
1103
1104         if (force)
1105                 memset(&(priv->sensitivity_tbl[0]), 0,
1106                         sizeof(u16)*HD_TABLE_SIZE);
1107
1108         /* Clear driver's sensitivity algo data */
1109         data = &(priv->sensitivity_data);
1110         memset(data, 0, sizeof(struct iwl4965_sensitivity_data));
1111
1112         data->num_in_cck_no_fa = 0;
1113         data->nrg_curr_state = IWL_FA_TOO_MANY;
1114         data->nrg_prev_state = IWL_FA_TOO_MANY;
1115         data->nrg_silence_ref = 0;
1116         data->nrg_silence_idx = 0;
1117         data->nrg_energy_idx = 0;
1118
1119         for (i = 0; i < 10; i++)
1120                 data->nrg_value[i] = 0;
1121
1122         for (i = 0; i < NRG_NUM_PREV_STAT_L; i++)
1123                 data->nrg_silence_rssi[i] = 0;
1124
1125         data->auto_corr_ofdm = 90;
1126         data->auto_corr_ofdm_mrc = 170;
1127         data->auto_corr_ofdm_x1  = 105;
1128         data->auto_corr_ofdm_mrc_x1 = 220;
1129         data->auto_corr_cck = AUTO_CORR_CCK_MIN_VAL_DEF;
1130         data->auto_corr_cck_mrc = 200;
1131         data->nrg_th_cck = 100;
1132         data->nrg_th_ofdm = 100;
1133
1134         data->last_bad_plcp_cnt_ofdm = 0;
1135         data->last_fa_cnt_ofdm = 0;
1136         data->last_bad_plcp_cnt_cck = 0;
1137         data->last_fa_cnt_cck = 0;
1138
1139         /* Clear prior Sensitivity command data to force send to uCode */
1140         if (force)
1141                 memset(&(priv->sensitivity_tbl[0]), 0,
1142                     sizeof(u16)*HD_TABLE_SIZE);
1143
1144         rc |= iwl4965_sensitivity_write(priv, flags);
1145         IWL_DEBUG_CALIB("<<return 0x%X\n", rc);
1146
1147         return;
1148 }
1149
1150
1151 /* Reset differential Rx gains in NIC to prepare for chain noise calibration.
1152  * Called after every association, but this runs only once!
1153  *  ... once chain noise is calibrated the first time, it's good forever.  */
1154 void iwl4965_chain_noise_reset(struct iwl4965_priv *priv)
1155 {
1156         struct iwl4965_chain_noise_data *data = NULL;
1157         int rc = 0;
1158
1159         data = &(priv->chain_noise_data);
1160         if ((data->state == IWL_CHAIN_NOISE_ALIVE) && iwl4965_is_associated(priv)) {
1161                 struct iwl4965_calibration_cmd cmd;
1162
1163                 memset(&cmd, 0, sizeof(cmd));
1164                 cmd.opCode = PHY_CALIBRATE_DIFF_GAIN_CMD;
1165                 cmd.diff_gain_a = 0;
1166                 cmd.diff_gain_b = 0;
1167                 cmd.diff_gain_c = 0;
1168                 rc = iwl4965_send_cmd_pdu(priv, REPLY_PHY_CALIBRATION_CMD,
1169                                  sizeof(cmd), &cmd);
1170                 msleep(4);
1171                 data->state = IWL_CHAIN_NOISE_ACCUMULATE;
1172                 IWL_DEBUG_CALIB("Run chain_noise_calibrate\n");
1173         }
1174         return;
1175 }
1176
1177 /*
1178  * Accumulate 20 beacons of signal and noise statistics for each of
1179  *   3 receivers/antennas/rx-chains, then figure out:
1180  * 1)  Which antennas are connected.
1181  * 2)  Differential rx gain settings to balance the 3 receivers.
1182  */
1183 static void iwl4965_noise_calibration(struct iwl4965_priv *priv,
1184                                       struct iwl4965_notif_statistics *stat_resp)
1185 {
1186         struct iwl4965_chain_noise_data *data = NULL;
1187         int rc = 0;
1188
1189         u32 chain_noise_a;
1190         u32 chain_noise_b;
1191         u32 chain_noise_c;
1192         u32 chain_sig_a;
1193         u32 chain_sig_b;
1194         u32 chain_sig_c;
1195         u32 average_sig[NUM_RX_CHAINS] = {INITIALIZATION_VALUE};
1196         u32 average_noise[NUM_RX_CHAINS] = {INITIALIZATION_VALUE};
1197         u32 max_average_sig;
1198         u16 max_average_sig_antenna_i;
1199         u32 min_average_noise = MIN_AVERAGE_NOISE_MAX_VALUE;
1200         u16 min_average_noise_antenna_i = INITIALIZATION_VALUE;
1201         u16 i = 0;
1202         u16 chan_num = INITIALIZATION_VALUE;
1203         u32 band = INITIALIZATION_VALUE;
1204         u32 active_chains = 0;
1205         unsigned long flags;
1206         struct statistics_rx_non_phy *rx_info = &(stat_resp->rx.general);
1207
1208         data = &(priv->chain_noise_data);
1209
1210         /* Accumulate just the first 20 beacons after the first association,
1211          *   then we're done forever. */
1212         if (data->state != IWL_CHAIN_NOISE_ACCUMULATE) {
1213                 if (data->state == IWL_CHAIN_NOISE_ALIVE)
1214                         IWL_DEBUG_CALIB("Wait for noise calib reset\n");
1215                 return;
1216         }
1217
1218         spin_lock_irqsave(&priv->lock, flags);
1219         if (rx_info->interference_data_flag != INTERFERENCE_DATA_AVAILABLE) {
1220                 IWL_DEBUG_CALIB(" << Interference data unavailable\n");
1221                 spin_unlock_irqrestore(&priv->lock, flags);
1222                 return;
1223         }
1224
1225         band = (priv->staging_rxon.flags & RXON_FLG_BAND_24G_MSK) ? 0 : 1;
1226         chan_num = le16_to_cpu(priv->staging_rxon.channel);
1227
1228         /* Make sure we accumulate data for just the associated channel
1229          *   (even if scanning). */
1230         if ((chan_num != (le32_to_cpu(stat_resp->flag) >> 16)) ||
1231             ((STATISTICS_REPLY_FLG_BAND_24G_MSK ==
1232              (stat_resp->flag & STATISTICS_REPLY_FLG_BAND_24G_MSK)) && band)) {
1233                 IWL_DEBUG_CALIB("Stats not from chan=%d, band=%d\n",
1234                                 chan_num, band);
1235                 spin_unlock_irqrestore(&priv->lock, flags);
1236                 return;
1237         }
1238
1239         /* Accumulate beacon statistics values across 20 beacons */
1240         chain_noise_a = le32_to_cpu(rx_info->beacon_silence_rssi_a) &
1241                                 IN_BAND_FILTER;
1242         chain_noise_b = le32_to_cpu(rx_info->beacon_silence_rssi_b) &
1243                                 IN_BAND_FILTER;
1244         chain_noise_c = le32_to_cpu(rx_info->beacon_silence_rssi_c) &
1245                                 IN_BAND_FILTER;
1246
1247         chain_sig_a = le32_to_cpu(rx_info->beacon_rssi_a) & IN_BAND_FILTER;
1248         chain_sig_b = le32_to_cpu(rx_info->beacon_rssi_b) & IN_BAND_FILTER;
1249         chain_sig_c = le32_to_cpu(rx_info->beacon_rssi_c) & IN_BAND_FILTER;
1250
1251         spin_unlock_irqrestore(&priv->lock, flags);
1252
1253         data->beacon_count++;
1254
1255         data->chain_noise_a = (chain_noise_a + data->chain_noise_a);
1256         data->chain_noise_b = (chain_noise_b + data->chain_noise_b);
1257         data->chain_noise_c = (chain_noise_c + data->chain_noise_c);
1258
1259         data->chain_signal_a = (chain_sig_a + data->chain_signal_a);
1260         data->chain_signal_b = (chain_sig_b + data->chain_signal_b);
1261         data->chain_signal_c = (chain_sig_c + data->chain_signal_c);
1262
1263         IWL_DEBUG_CALIB("chan=%d, band=%d, beacon=%d\n", chan_num, band,
1264                         data->beacon_count);
1265         IWL_DEBUG_CALIB("chain_sig: a %d b %d c %d\n",
1266                         chain_sig_a, chain_sig_b, chain_sig_c);
1267         IWL_DEBUG_CALIB("chain_noise: a %d b %d c %d\n",
1268                         chain_noise_a, chain_noise_b, chain_noise_c);
1269
1270         /* If this is the 20th beacon, determine:
1271          * 1)  Disconnected antennas (using signal strengths)
1272          * 2)  Differential gain (using silence noise) to balance receivers */
1273         if (data->beacon_count == CAL_NUM_OF_BEACONS) {
1274
1275                 /* Analyze signal for disconnected antenna */
1276                 average_sig[0] = (data->chain_signal_a) / CAL_NUM_OF_BEACONS;
1277                 average_sig[1] = (data->chain_signal_b) / CAL_NUM_OF_BEACONS;
1278                 average_sig[2] = (data->chain_signal_c) / CAL_NUM_OF_BEACONS;
1279
1280                 if (average_sig[0] >= average_sig[1]) {
1281                         max_average_sig = average_sig[0];
1282                         max_average_sig_antenna_i = 0;
1283                         active_chains = (1 << max_average_sig_antenna_i);
1284                 } else {
1285                         max_average_sig = average_sig[1];
1286                         max_average_sig_antenna_i = 1;
1287                         active_chains = (1 << max_average_sig_antenna_i);
1288                 }
1289
1290                 if (average_sig[2] >= max_average_sig) {
1291                         max_average_sig = average_sig[2];
1292                         max_average_sig_antenna_i = 2;
1293                         active_chains = (1 << max_average_sig_antenna_i);
1294                 }
1295
1296                 IWL_DEBUG_CALIB("average_sig: a %d b %d c %d\n",
1297                              average_sig[0], average_sig[1], average_sig[2]);
1298                 IWL_DEBUG_CALIB("max_average_sig = %d, antenna %d\n",
1299                              max_average_sig, max_average_sig_antenna_i);
1300
1301                 /* Compare signal strengths for all 3 receivers. */
1302                 for (i = 0; i < NUM_RX_CHAINS; i++) {
1303                         if (i != max_average_sig_antenna_i) {
1304                                 s32 rssi_delta = (max_average_sig -
1305                                                   average_sig[i]);
1306
1307                                 /* If signal is very weak, compared with
1308                                  * strongest, mark it as disconnected. */
1309                                 if (rssi_delta > MAXIMUM_ALLOWED_PATHLOSS)
1310                                         data->disconn_array[i] = 1;
1311                                 else
1312                                         active_chains |= (1 << i);
1313                         IWL_DEBUG_CALIB("i = %d  rssiDelta = %d  "
1314                                      "disconn_array[i] = %d\n",
1315                                      i, rssi_delta, data->disconn_array[i]);
1316                         }
1317                 }
1318
1319                 /*If both chains A & B are disconnected -
1320                  * connect B and leave A as is */
1321                 if (data->disconn_array[CHAIN_A] &&
1322                     data->disconn_array[CHAIN_B]) {
1323                         data->disconn_array[CHAIN_B] = 0;
1324                         active_chains |= (1 << CHAIN_B);
1325                         IWL_DEBUG_CALIB("both A & B chains are disconnected! "
1326                                      "W/A - declare B as connected\n");
1327                 }
1328
1329                 IWL_DEBUG_CALIB("active_chains (bitwise) = 0x%x\n",
1330                                 active_chains);
1331
1332                 /* Save for use within RXON, TX, SCAN commands, etc. */
1333                 priv->valid_antenna = active_chains;
1334
1335                 /* Analyze noise for rx balance */
1336                 average_noise[0] = ((data->chain_noise_a)/CAL_NUM_OF_BEACONS);
1337                 average_noise[1] = ((data->chain_noise_b)/CAL_NUM_OF_BEACONS);
1338                 average_noise[2] = ((data->chain_noise_c)/CAL_NUM_OF_BEACONS);
1339
1340                 for (i = 0; i < NUM_RX_CHAINS; i++) {
1341                         if (!(data->disconn_array[i]) &&
1342                            (average_noise[i] <= min_average_noise)) {
1343                                 /* This means that chain i is active and has
1344                                  * lower noise values so far: */
1345                                 min_average_noise = average_noise[i];
1346                                 min_average_noise_antenna_i = i;
1347                         }
1348                 }
1349
1350                 data->delta_gain_code[min_average_noise_antenna_i] = 0;
1351
1352                 IWL_DEBUG_CALIB("average_noise: a %d b %d c %d\n",
1353                                 average_noise[0], average_noise[1],
1354                                 average_noise[2]);
1355
1356                 IWL_DEBUG_CALIB("min_average_noise = %d, antenna %d\n",
1357                                 min_average_noise, min_average_noise_antenna_i);
1358
1359                 for (i = 0; i < NUM_RX_CHAINS; i++) {
1360                         s32 delta_g = 0;
1361
1362                         if (!(data->disconn_array[i]) &&
1363                             (data->delta_gain_code[i] ==
1364                              CHAIN_NOISE_DELTA_GAIN_INIT_VAL)) {
1365                                 delta_g = average_noise[i] - min_average_noise;
1366                                 data->delta_gain_code[i] = (u8)((delta_g *
1367                                                                     10) / 15);
1368                                 if (CHAIN_NOISE_MAX_DELTA_GAIN_CODE <
1369                                    data->delta_gain_code[i])
1370                                         data->delta_gain_code[i] =
1371                                           CHAIN_NOISE_MAX_DELTA_GAIN_CODE;
1372
1373                                 data->delta_gain_code[i] =
1374                                         (data->delta_gain_code[i] | (1 << 2));
1375                         } else
1376                                 data->delta_gain_code[i] = 0;
1377                 }
1378                 IWL_DEBUG_CALIB("delta_gain_codes: a %d b %d c %d\n",
1379                              data->delta_gain_code[0],
1380                              data->delta_gain_code[1],
1381                              data->delta_gain_code[2]);
1382
1383                 /* Differential gain gets sent to uCode only once */
1384                 if (!data->radio_write) {
1385                         struct iwl4965_calibration_cmd cmd;
1386                         data->radio_write = 1;
1387
1388                         memset(&cmd, 0, sizeof(cmd));
1389                         cmd.opCode = PHY_CALIBRATE_DIFF_GAIN_CMD;
1390                         cmd.diff_gain_a = data->delta_gain_code[0];
1391                         cmd.diff_gain_b = data->delta_gain_code[1];
1392                         cmd.diff_gain_c = data->delta_gain_code[2];
1393                         rc = iwl4965_send_cmd_pdu(priv, REPLY_PHY_CALIBRATION_CMD,
1394                                               sizeof(cmd), &cmd);
1395                         if (rc)
1396                                 IWL_DEBUG_CALIB("fail sending cmd "
1397                                              "REPLY_PHY_CALIBRATION_CMD \n");
1398
1399                         /* TODO we might want recalculate
1400                          * rx_chain in rxon cmd */
1401
1402                         /* Mark so we run this algo only once! */
1403                         data->state = IWL_CHAIN_NOISE_CALIBRATED;
1404                 }
1405                 data->chain_noise_a = 0;
1406                 data->chain_noise_b = 0;
1407                 data->chain_noise_c = 0;
1408                 data->chain_signal_a = 0;
1409                 data->chain_signal_b = 0;
1410                 data->chain_signal_c = 0;
1411                 data->beacon_count = 0;
1412         }
1413         return;
1414 }
1415
1416 static void iwl4965_sensitivity_calibration(struct iwl4965_priv *priv,
1417                                             struct iwl4965_notif_statistics *resp)
1418 {
1419         int rc = 0;
1420         u32 rx_enable_time;
1421         u32 fa_cck;
1422         u32 fa_ofdm;
1423         u32 bad_plcp_cck;
1424         u32 bad_plcp_ofdm;
1425         u32 norm_fa_ofdm;
1426         u32 norm_fa_cck;
1427         struct iwl4965_sensitivity_data *data = NULL;
1428         struct statistics_rx_non_phy *rx_info = &(resp->rx.general);
1429         struct statistics_rx *statistics = &(resp->rx);
1430         unsigned long flags;
1431         struct statistics_general_data statis;
1432
1433         data = &(priv->sensitivity_data);
1434
1435         if (!iwl4965_is_associated(priv)) {
1436                 IWL_DEBUG_CALIB("<< - not associated\n");
1437                 return;
1438         }
1439
1440         spin_lock_irqsave(&priv->lock, flags);
1441         if (rx_info->interference_data_flag != INTERFERENCE_DATA_AVAILABLE) {
1442                 IWL_DEBUG_CALIB("<< invalid data.\n");
1443                 spin_unlock_irqrestore(&priv->lock, flags);
1444                 return;
1445         }
1446
1447         /* Extract Statistics: */
1448         rx_enable_time = le32_to_cpu(rx_info->channel_load);
1449         fa_cck = le32_to_cpu(statistics->cck.false_alarm_cnt);
1450         fa_ofdm = le32_to_cpu(statistics->ofdm.false_alarm_cnt);
1451         bad_plcp_cck = le32_to_cpu(statistics->cck.plcp_err);
1452         bad_plcp_ofdm = le32_to_cpu(statistics->ofdm.plcp_err);
1453
1454         statis.beacon_silence_rssi_a =
1455                         le32_to_cpu(statistics->general.beacon_silence_rssi_a);
1456         statis.beacon_silence_rssi_b =
1457                         le32_to_cpu(statistics->general.beacon_silence_rssi_b);
1458         statis.beacon_silence_rssi_c =
1459                         le32_to_cpu(statistics->general.beacon_silence_rssi_c);
1460         statis.beacon_energy_a =
1461                         le32_to_cpu(statistics->general.beacon_energy_a);
1462         statis.beacon_energy_b =
1463                         le32_to_cpu(statistics->general.beacon_energy_b);
1464         statis.beacon_energy_c =
1465                         le32_to_cpu(statistics->general.beacon_energy_c);
1466
1467         spin_unlock_irqrestore(&priv->lock, flags);
1468
1469         IWL_DEBUG_CALIB("rx_enable_time = %u usecs\n", rx_enable_time);
1470
1471         if (!rx_enable_time) {
1472                 IWL_DEBUG_CALIB("<< RX Enable Time == 0! \n");
1473                 return;
1474         }
1475
1476         /* These statistics increase monotonically, and do not reset
1477          *   at each beacon.  Calculate difference from last value, or just
1478          *   use the new statistics value if it has reset or wrapped around. */
1479         if (data->last_bad_plcp_cnt_cck > bad_plcp_cck)
1480                 data->last_bad_plcp_cnt_cck = bad_plcp_cck;
1481         else {
1482                 bad_plcp_cck -= data->last_bad_plcp_cnt_cck;
1483                 data->last_bad_plcp_cnt_cck += bad_plcp_cck;
1484         }
1485
1486         if (data->last_bad_plcp_cnt_ofdm > bad_plcp_ofdm)
1487                 data->last_bad_plcp_cnt_ofdm = bad_plcp_ofdm;
1488         else {
1489                 bad_plcp_ofdm -= data->last_bad_plcp_cnt_ofdm;
1490                 data->last_bad_plcp_cnt_ofdm += bad_plcp_ofdm;
1491         }
1492
1493         if (data->last_fa_cnt_ofdm > fa_ofdm)
1494                 data->last_fa_cnt_ofdm = fa_ofdm;
1495         else {
1496                 fa_ofdm -= data->last_fa_cnt_ofdm;
1497                 data->last_fa_cnt_ofdm += fa_ofdm;
1498         }
1499
1500         if (data->last_fa_cnt_cck > fa_cck)
1501                 data->last_fa_cnt_cck = fa_cck;
1502         else {
1503                 fa_cck -= data->last_fa_cnt_cck;
1504                 data->last_fa_cnt_cck += fa_cck;
1505         }
1506
1507         /* Total aborted signal locks */
1508         norm_fa_ofdm = fa_ofdm + bad_plcp_ofdm;
1509         norm_fa_cck = fa_cck + bad_plcp_cck;
1510
1511         IWL_DEBUG_CALIB("cck: fa %u badp %u  ofdm: fa %u badp %u\n", fa_cck,
1512                         bad_plcp_cck, fa_ofdm, bad_plcp_ofdm);
1513
1514         iwl4965_sens_auto_corr_ofdm(priv, norm_fa_ofdm, rx_enable_time);
1515         iwl4965_sens_energy_cck(priv, norm_fa_cck, rx_enable_time, &statis);
1516         rc |= iwl4965_sensitivity_write(priv, CMD_ASYNC);
1517
1518         return;
1519 }
1520
1521 static void iwl4965_bg_sensitivity_work(struct work_struct *work)
1522 {
1523         struct iwl4965_priv *priv = container_of(work, struct iwl4965_priv,
1524                         sensitivity_work);
1525
1526         mutex_lock(&priv->mutex);
1527
1528         if (test_bit(STATUS_EXIT_PENDING, &priv->status) ||
1529             test_bit(STATUS_SCANNING, &priv->status)) {
1530                 mutex_unlock(&priv->mutex);
1531                 return;
1532         }
1533
1534         if (priv->start_calib) {
1535                 iwl4965_noise_calibration(priv, &priv->statistics);
1536
1537                 if (priv->sensitivity_data.state ==
1538                                         IWL_SENS_CALIB_NEED_REINIT) {
1539                         iwl4965_init_sensitivity(priv, CMD_ASYNC, 0);
1540                         priv->sensitivity_data.state = IWL_SENS_CALIB_ALLOWED;
1541                 } else
1542                         iwl4965_sensitivity_calibration(priv,
1543                                         &priv->statistics);
1544         }
1545
1546         mutex_unlock(&priv->mutex);
1547         return;
1548 }
1549 #endif /*CONFIG_IWL4965_SENSITIVITY*/
1550
1551 static void iwl4965_bg_txpower_work(struct work_struct *work)
1552 {
1553         struct iwl4965_priv *priv = container_of(work, struct iwl4965_priv,
1554                         txpower_work);
1555
1556         /* If a scan happened to start before we got here
1557          * then just return; the statistics notification will
1558          * kick off another scheduled work to compensate for
1559          * any temperature delta we missed here. */
1560         if (test_bit(STATUS_EXIT_PENDING, &priv->status) ||
1561             test_bit(STATUS_SCANNING, &priv->status))
1562                 return;
1563
1564         mutex_lock(&priv->mutex);
1565
1566         /* Regardless of if we are assocaited, we must reconfigure the
1567          * TX power since frames can be sent on non-radar channels while
1568          * not associated */
1569         iwl4965_hw_reg_send_txpower(priv);
1570
1571         /* Update last_temperature to keep is_calib_needed from running
1572          * when it isn't needed... */
1573         priv->last_temperature = priv->temperature;
1574
1575         mutex_unlock(&priv->mutex);
1576 }
1577
1578 /*
1579  * Acquire priv->lock before calling this function !
1580  */
1581 static void iwl4965_set_wr_ptrs(struct iwl4965_priv *priv, int txq_id, u32 index)
1582 {
1583         iwl4965_write_direct32(priv, HBUS_TARG_WRPTR,
1584                              (index & 0xff) | (txq_id << 8));
1585         iwl4965_write_prph(priv, KDR_SCD_QUEUE_RDPTR(txq_id), index);
1586 }
1587
1588 /*
1589  * Acquire priv->lock before calling this function !
1590  */
1591 static void iwl4965_tx_queue_set_status(struct iwl4965_priv *priv,
1592                                         struct iwl4965_tx_queue *txq,
1593                                         int tx_fifo_id, int scd_retry)
1594 {
1595         int txq_id = txq->q.id;
1596         int active = test_bit(txq_id, &priv->txq_ctx_active_msk)?1:0;
1597
1598         iwl4965_write_prph(priv, KDR_SCD_QUEUE_STATUS_BITS(txq_id),
1599                                  (active << SCD_QUEUE_STTS_REG_POS_ACTIVE) |
1600                                  (tx_fifo_id << SCD_QUEUE_STTS_REG_POS_TXF) |
1601                                  (scd_retry << SCD_QUEUE_STTS_REG_POS_WSL) |
1602                                  (scd_retry << SCD_QUEUE_STTS_REG_POS_SCD_ACK) |
1603                                  SCD_QUEUE_STTS_REG_MSK);
1604
1605         txq->sched_retry = scd_retry;
1606
1607         IWL_DEBUG_INFO("%s %s Queue %d on AC %d\n",
1608                        active ? "Activete" : "Deactivate",
1609                        scd_retry ? "BA" : "AC", txq_id, tx_fifo_id);
1610 }
1611
1612 static const u16 default_queue_to_tx_fifo[] = {
1613         IWL_TX_FIFO_AC3,
1614         IWL_TX_FIFO_AC2,
1615         IWL_TX_FIFO_AC1,
1616         IWL_TX_FIFO_AC0,
1617         IWL_CMD_FIFO_NUM,
1618         IWL_TX_FIFO_HCCA_1,
1619         IWL_TX_FIFO_HCCA_2
1620 };
1621
1622 static inline void iwl4965_txq_ctx_activate(struct iwl4965_priv *priv, int txq_id)
1623 {
1624         set_bit(txq_id, &priv->txq_ctx_active_msk);
1625 }
1626
1627 static inline void iwl4965_txq_ctx_deactivate(struct iwl4965_priv *priv, int txq_id)
1628 {
1629         clear_bit(txq_id, &priv->txq_ctx_active_msk);
1630 }
1631
1632 int iwl4965_alive_notify(struct iwl4965_priv *priv)
1633 {
1634         u32 a;
1635         int i = 0;
1636         unsigned long flags;
1637         int rc;
1638
1639         spin_lock_irqsave(&priv->lock, flags);
1640
1641 #ifdef CONFIG_IWL4965_SENSITIVITY
1642         memset(&(priv->sensitivity_data), 0,
1643                sizeof(struct iwl4965_sensitivity_data));
1644         memset(&(priv->chain_noise_data), 0,
1645                sizeof(struct iwl4965_chain_noise_data));
1646         for (i = 0; i < NUM_RX_CHAINS; i++)
1647                 priv->chain_noise_data.delta_gain_code[i] =
1648                                 CHAIN_NOISE_DELTA_GAIN_INIT_VAL;
1649 #endif /* CONFIG_IWL4965_SENSITIVITY*/
1650         rc = iwl4965_grab_nic_access(priv);
1651         if (rc) {
1652                 spin_unlock_irqrestore(&priv->lock, flags);
1653                 return rc;
1654         }
1655
1656         priv->scd_base_addr = iwl4965_read_prph(priv, KDR_SCD_SRAM_BASE_ADDR);
1657         a = priv->scd_base_addr + SCD_CONTEXT_DATA_OFFSET;
1658         for (; a < priv->scd_base_addr + SCD_TX_STTS_BITMAP_OFFSET; a += 4)
1659                 iwl4965_write_targ_mem(priv, a, 0);
1660         for (; a < priv->scd_base_addr + SCD_TRANSLATE_TBL_OFFSET; a += 4)
1661                 iwl4965_write_targ_mem(priv, a, 0);
1662         for (; a < sizeof(u16) * priv->hw_setting.max_txq_num; a += 4)
1663                 iwl4965_write_targ_mem(priv, a, 0);
1664
1665         iwl4965_write_prph(priv, KDR_SCD_DRAM_BASE_ADDR,
1666                 (priv->hw_setting.shared_phys +
1667                  offsetof(struct iwl4965_shared, queues_byte_cnt_tbls)) >> 10);
1668         iwl4965_write_prph(priv, KDR_SCD_QUEUECHAIN_SEL, 0);
1669
1670         /* initiate the queues */
1671         for (i = 0; i < priv->hw_setting.max_txq_num; i++) {
1672                 iwl4965_write_prph(priv, KDR_SCD_QUEUE_RDPTR(i), 0);
1673                 iwl4965_write_direct32(priv, HBUS_TARG_WRPTR, 0 | (i << 8));
1674                 iwl4965_write_targ_mem(priv, priv->scd_base_addr +
1675                                         SCD_CONTEXT_QUEUE_OFFSET(i),
1676                                         (SCD_WIN_SIZE <<
1677                                         SCD_QUEUE_CTX_REG1_WIN_SIZE_POS) &
1678                                         SCD_QUEUE_CTX_REG1_WIN_SIZE_MSK);
1679                 iwl4965_write_targ_mem(priv, priv->scd_base_addr +
1680                                         SCD_CONTEXT_QUEUE_OFFSET(i) +
1681                                         sizeof(u32),
1682                                         (SCD_FRAME_LIMIT <<
1683                                         SCD_QUEUE_CTX_REG2_FRAME_LIMIT_POS) &
1684                                         SCD_QUEUE_CTX_REG2_FRAME_LIMIT_MSK);
1685
1686         }
1687         iwl4965_write_prph(priv, KDR_SCD_INTERRUPT_MASK,
1688                                  (1 << priv->hw_setting.max_txq_num) - 1);
1689
1690         iwl4965_write_prph(priv, KDR_SCD_TXFACT,
1691                                  SCD_TXFACT_REG_TXFIFO_MASK(0, 7));
1692
1693         iwl4965_set_wr_ptrs(priv, IWL_CMD_QUEUE_NUM, 0);
1694         /* map qos queues to fifos one-to-one */
1695         for (i = 0; i < ARRAY_SIZE(default_queue_to_tx_fifo); i++) {
1696                 int ac = default_queue_to_tx_fifo[i];
1697                 iwl4965_txq_ctx_activate(priv, i);
1698                 iwl4965_tx_queue_set_status(priv, &priv->txq[i], ac, 0);
1699         }
1700
1701         iwl4965_release_nic_access(priv);
1702         spin_unlock_irqrestore(&priv->lock, flags);
1703
1704         return 0;
1705 }
1706
1707 int iwl4965_hw_set_hw_setting(struct iwl4965_priv *priv)
1708 {
1709         priv->hw_setting.shared_virt =
1710             pci_alloc_consistent(priv->pci_dev,
1711                                  sizeof(struct iwl4965_shared),
1712                                  &priv->hw_setting.shared_phys);
1713
1714         if (!priv->hw_setting.shared_virt)
1715                 return -1;
1716
1717         memset(priv->hw_setting.shared_virt, 0, sizeof(struct iwl4965_shared));
1718
1719         priv->hw_setting.max_txq_num = iwl4965_param_queues_num;
1720         priv->hw_setting.ac_queue_count = AC_NUM;
1721         priv->hw_setting.tx_cmd_len = sizeof(struct iwl4965_tx_cmd);
1722         priv->hw_setting.max_rxq_size = RX_QUEUE_SIZE;
1723         priv->hw_setting.max_rxq_log = RX_QUEUE_SIZE_LOG;
1724
1725         priv->hw_setting.max_stations = IWL4965_STATION_COUNT;
1726         priv->hw_setting.bcast_sta_id = IWL4965_BROADCAST_ID;
1727         return 0;
1728 }
1729
1730 /**
1731  * iwl4965_hw_txq_ctx_free - Free TXQ Context
1732  *
1733  * Destroy all TX DMA queues and structures
1734  */
1735 void iwl4965_hw_txq_ctx_free(struct iwl4965_priv *priv)
1736 {
1737         int txq_id;
1738
1739         /* Tx queues */
1740         for (txq_id = 0; txq_id < priv->hw_setting.max_txq_num; txq_id++)
1741                 iwl4965_tx_queue_free(priv, &priv->txq[txq_id]);
1742
1743         iwl4965_kw_free(priv);
1744 }
1745
1746 /**
1747  * iwl4965_hw_txq_free_tfd -  Free one TFD, those at index [txq->q.read_ptr]
1748  *
1749  * Does NOT advance any indexes
1750  */
1751 int iwl4965_hw_txq_free_tfd(struct iwl4965_priv *priv, struct iwl4965_tx_queue *txq)
1752 {
1753         struct iwl4965_tfd_frame *bd_tmp = (struct iwl4965_tfd_frame *)&txq->bd[0];
1754         struct iwl4965_tfd_frame *bd = &bd_tmp[txq->q.read_ptr];
1755         struct pci_dev *dev = priv->pci_dev;
1756         int i;
1757         int counter = 0;
1758         int index, is_odd;
1759
1760         /* classify bd */
1761         if (txq->q.id == IWL_CMD_QUEUE_NUM)
1762                 /* nothing to cleanup after for host commands */
1763                 return 0;
1764
1765         /* sanity check */
1766         counter = IWL_GET_BITS(*bd, num_tbs);
1767         if (counter > MAX_NUM_OF_TBS) {
1768                 IWL_ERROR("Too many chunks: %i\n", counter);
1769                 /* @todo issue fatal error, it is quite serious situation */
1770                 return 0;
1771         }
1772
1773         /* unmap chunks if any */
1774
1775         for (i = 0; i < counter; i++) {
1776                 index = i / 2;
1777                 is_odd = i & 0x1;
1778
1779                 if (is_odd)
1780                         pci_unmap_single(
1781                                 dev,
1782                                 IWL_GET_BITS(bd->pa[index], tb2_addr_lo16) |
1783                                 (IWL_GET_BITS(bd->pa[index],
1784                                               tb2_addr_hi20) << 16),
1785                                 IWL_GET_BITS(bd->pa[index], tb2_len),
1786                                 PCI_DMA_TODEVICE);
1787
1788                 else if (i > 0)
1789                         pci_unmap_single(dev,
1790                                          le32_to_cpu(bd->pa[index].tb1_addr),
1791                                          IWL_GET_BITS(bd->pa[index], tb1_len),
1792                                          PCI_DMA_TODEVICE);
1793
1794                 if (txq->txb[txq->q.read_ptr].skb[i]) {
1795                         struct sk_buff *skb = txq->txb[txq->q.read_ptr].skb[i];
1796
1797                         dev_kfree_skb(skb);
1798                         txq->txb[txq->q.read_ptr].skb[i] = NULL;
1799                 }
1800         }
1801         return 0;
1802 }
1803
1804 int iwl4965_hw_reg_set_txpower(struct iwl4965_priv *priv, s8 power)
1805 {
1806         IWL_ERROR("TODO: Implement iwl4965_hw_reg_set_txpower!\n");
1807         return -EINVAL;
1808 }
1809
1810 static s32 iwl4965_math_div_round(s32 num, s32 denom, s32 *res)
1811 {
1812         s32 sign = 1;
1813
1814         if (num < 0) {
1815                 sign = -sign;
1816                 num = -num;
1817         }
1818         if (denom < 0) {
1819                 sign = -sign;
1820                 denom = -denom;
1821         }
1822         *res = 1;
1823         *res = ((num * 2 + denom) / (denom * 2)) * sign;
1824
1825         return 1;
1826 }
1827
1828 static s32 iwl4965_get_voltage_compensation(s32 eeprom_voltage,
1829                                             s32 current_voltage)
1830 {
1831         s32 comp = 0;
1832
1833         if ((TX_POWER_IWL_ILLEGAL_VOLTAGE == eeprom_voltage) ||
1834             (TX_POWER_IWL_ILLEGAL_VOLTAGE == current_voltage))
1835                 return 0;
1836
1837         iwl4965_math_div_round(current_voltage - eeprom_voltage,
1838                                TX_POWER_IWL_VOLTAGE_CODES_PER_03V, &comp);
1839
1840         if (current_voltage > eeprom_voltage)
1841                 comp *= 2;
1842         if ((comp < -2) || (comp > 2))
1843                 comp = 0;
1844
1845         return comp;
1846 }
1847
1848 static const struct iwl4965_channel_info *
1849 iwl4965_get_channel_txpower_info(struct iwl4965_priv *priv, u8 phymode, u16 channel)
1850 {
1851         const struct iwl4965_channel_info *ch_info;
1852
1853         ch_info = iwl4965_get_channel_info(priv, phymode, channel);
1854
1855         if (!is_channel_valid(ch_info))
1856                 return NULL;
1857
1858         return ch_info;
1859 }
1860
1861 static s32 iwl4965_get_tx_atten_grp(u16 channel)
1862 {
1863         if (channel >= CALIB_IWL_TX_ATTEN_GR5_FCH &&
1864             channel <= CALIB_IWL_TX_ATTEN_GR5_LCH)
1865                 return CALIB_CH_GROUP_5;
1866
1867         if (channel >= CALIB_IWL_TX_ATTEN_GR1_FCH &&
1868             channel <= CALIB_IWL_TX_ATTEN_GR1_LCH)
1869                 return CALIB_CH_GROUP_1;
1870
1871         if (channel >= CALIB_IWL_TX_ATTEN_GR2_FCH &&
1872             channel <= CALIB_IWL_TX_ATTEN_GR2_LCH)
1873                 return CALIB_CH_GROUP_2;
1874
1875         if (channel >= CALIB_IWL_TX_ATTEN_GR3_FCH &&
1876             channel <= CALIB_IWL_TX_ATTEN_GR3_LCH)
1877                 return CALIB_CH_GROUP_3;
1878
1879         if (channel >= CALIB_IWL_TX_ATTEN_GR4_FCH &&
1880             channel <= CALIB_IWL_TX_ATTEN_GR4_LCH)
1881                 return CALIB_CH_GROUP_4;
1882
1883         IWL_ERROR("Can't find txatten group for channel %d.\n", channel);
1884         return -1;
1885 }
1886
1887 static u32 iwl4965_get_sub_band(const struct iwl4965_priv *priv, u32 channel)
1888 {
1889         s32 b = -1;
1890
1891         for (b = 0; b < EEPROM_TX_POWER_BANDS; b++) {
1892                 if (priv->eeprom.calib_info.band_info[b].ch_from == 0)
1893                         continue;
1894
1895                 if ((channel >= priv->eeprom.calib_info.band_info[b].ch_from)
1896                     && (channel <= priv->eeprom.calib_info.band_info[b].ch_to))
1897                         break;
1898         }
1899
1900         return b;
1901 }
1902
1903 static s32 iwl4965_interpolate_value(s32 x, s32 x1, s32 y1, s32 x2, s32 y2)
1904 {
1905         s32 val;
1906
1907         if (x2 == x1)
1908                 return y1;
1909         else {
1910                 iwl4965_math_div_round((x2 - x) * (y1 - y2), (x2 - x1), &val);
1911                 return val + y2;
1912         }
1913 }
1914
1915 static int iwl4965_interpolate_chan(struct iwl4965_priv *priv, u32 channel,
1916                                     struct iwl4965_eeprom_calib_ch_info *chan_info)
1917 {
1918         s32 s = -1;
1919         u32 c;
1920         u32 m;
1921         const struct iwl4965_eeprom_calib_measure *m1;
1922         const struct iwl4965_eeprom_calib_measure *m2;
1923         struct iwl4965_eeprom_calib_measure *omeas;
1924         u32 ch_i1;
1925         u32 ch_i2;
1926
1927         s = iwl4965_get_sub_band(priv, channel);
1928         if (s >= EEPROM_TX_POWER_BANDS) {
1929                 IWL_ERROR("Tx Power can not find channel %d ", channel);
1930                 return -1;
1931         }
1932
1933         ch_i1 = priv->eeprom.calib_info.band_info[s].ch1.ch_num;
1934         ch_i2 = priv->eeprom.calib_info.band_info[s].ch2.ch_num;
1935         chan_info->ch_num = (u8) channel;
1936
1937         IWL_DEBUG_TXPOWER("channel %d subband %d factory cal ch %d & %d\n",
1938                           channel, s, ch_i1, ch_i2);
1939
1940         for (c = 0; c < EEPROM_TX_POWER_TX_CHAINS; c++) {
1941                 for (m = 0; m < EEPROM_TX_POWER_MEASUREMENTS; m++) {
1942                         m1 = &(priv->eeprom.calib_info.band_info[s].ch1.
1943                                measurements[c][m]);
1944                         m2 = &(priv->eeprom.calib_info.band_info[s].ch2.
1945                                measurements[c][m]);
1946                         omeas = &(chan_info->measurements[c][m]);
1947
1948                         omeas->actual_pow =
1949                             (u8) iwl4965_interpolate_value(channel, ch_i1,
1950                                                            m1->actual_pow,
1951                                                            ch_i2,
1952                                                            m2->actual_pow);
1953                         omeas->gain_idx =
1954                             (u8) iwl4965_interpolate_value(channel, ch_i1,
1955                                                            m1->gain_idx, ch_i2,
1956                                                            m2->gain_idx);
1957                         omeas->temperature =
1958                             (u8) iwl4965_interpolate_value(channel, ch_i1,
1959                                                            m1->temperature,
1960                                                            ch_i2,
1961                                                            m2->temperature);
1962                         omeas->pa_det =
1963                             (s8) iwl4965_interpolate_value(channel, ch_i1,
1964                                                            m1->pa_det, ch_i2,
1965                                                            m2->pa_det);
1966
1967                         IWL_DEBUG_TXPOWER
1968                             ("chain %d meas %d AP1=%d AP2=%d AP=%d\n", c, m,
1969                              m1->actual_pow, m2->actual_pow, omeas->actual_pow);
1970                         IWL_DEBUG_TXPOWER
1971                             ("chain %d meas %d NI1=%d NI2=%d NI=%d\n", c, m,
1972                              m1->gain_idx, m2->gain_idx, omeas->gain_idx);
1973                         IWL_DEBUG_TXPOWER
1974                             ("chain %d meas %d PA1=%d PA2=%d PA=%d\n", c, m,
1975                              m1->pa_det, m2->pa_det, omeas->pa_det);
1976                         IWL_DEBUG_TXPOWER
1977                             ("chain %d meas %d  T1=%d  T2=%d  T=%d\n", c, m,
1978                              m1->temperature, m2->temperature,
1979                              omeas->temperature);
1980                 }
1981         }
1982
1983         return 0;
1984 }
1985
1986 /* bit-rate-dependent table to prevent Tx distortion, in half-dB units,
1987  * for OFDM 6, 12, 18, 24, 36, 48, 54, 60 MBit, and CCK all rates. */
1988 static s32 back_off_table[] = {
1989         10, 10, 10, 10, 10, 15, 17, 20, /* OFDM SISO 20 MHz */
1990         10, 10, 10, 10, 10, 15, 17, 20, /* OFDM MIMO 20 MHz */
1991         10, 10, 10, 10, 10, 15, 17, 20, /* OFDM SISO 40 MHz */
1992         10, 10, 10, 10, 10, 15, 17, 20, /* OFDM MIMO 40 MHz */
1993         10                      /* CCK */
1994 };
1995
1996 /* Thermal compensation values for txpower for various frequency ranges ...
1997  *   ratios from 3:1 to 4.5:1 of degrees (Celsius) per half-dB gain adjust */
1998 static struct iwl4965_txpower_comp_entry {
1999         s32 degrees_per_05db_a;
2000         s32 degrees_per_05db_a_denom;
2001 } tx_power_cmp_tble[CALIB_CH_GROUP_MAX] = {
2002         {9, 2},                 /* group 0 5.2, ch  34-43 */
2003         {4, 1},                 /* group 1 5.2, ch  44-70 */
2004         {4, 1},                 /* group 2 5.2, ch  71-124 */
2005         {4, 1},                 /* group 3 5.2, ch 125-200 */
2006         {3, 1}                  /* group 4 2.4, ch   all */
2007 };
2008
2009 static s32 get_min_power_index(s32 rate_power_index, u32 band)
2010 {
2011         if (!band) {
2012                 if ((rate_power_index & 7) <= 4)
2013                         return MIN_TX_GAIN_INDEX_52GHZ_EXT;
2014         }
2015         return MIN_TX_GAIN_INDEX;
2016 }
2017
2018 struct gain_entry {
2019         u8 dsp;
2020         u8 radio;
2021 };
2022
2023 static const struct gain_entry gain_table[2][108] = {
2024         /* 5.2GHz power gain index table */
2025         {
2026          {123, 0x3F},           /* highest txpower */
2027          {117, 0x3F},
2028          {110, 0x3F},
2029          {104, 0x3F},
2030          {98, 0x3F},
2031          {110, 0x3E},
2032          {104, 0x3E},
2033          {98, 0x3E},
2034          {110, 0x3D},
2035          {104, 0x3D},
2036          {98, 0x3D},
2037          {110, 0x3C},
2038          {104, 0x3C},
2039          {98, 0x3C},
2040          {110, 0x3B},
2041          {104, 0x3B},
2042          {98, 0x3B},
2043          {110, 0x3A},
2044          {104, 0x3A},
2045          {98, 0x3A},
2046          {110, 0x39},
2047          {104, 0x39},
2048          {98, 0x39},
2049          {110, 0x38},
2050          {104, 0x38},
2051          {98, 0x38},
2052          {110, 0x37},
2053          {104, 0x37},
2054          {98, 0x37},
2055          {110, 0x36},
2056          {104, 0x36},
2057          {98, 0x36},
2058          {110, 0x35},
2059          {104, 0x35},
2060          {98, 0x35},
2061          {110, 0x34},
2062          {104, 0x34},
2063          {98, 0x34},
2064          {110, 0x33},
2065          {104, 0x33},
2066          {98, 0x33},
2067          {110, 0x32},
2068          {104, 0x32},
2069          {98, 0x32},
2070          {110, 0x31},
2071          {104, 0x31},
2072          {98, 0x31},
2073          {110, 0x30},
2074          {104, 0x30},
2075          {98, 0x30},
2076          {110, 0x25},
2077          {104, 0x25},
2078          {98, 0x25},
2079          {110, 0x24},
2080          {104, 0x24},
2081          {98, 0x24},
2082          {110, 0x23},
2083          {104, 0x23},
2084          {98, 0x23},
2085          {110, 0x22},
2086          {104, 0x18},
2087          {98, 0x18},
2088          {110, 0x17},
2089          {104, 0x17},
2090          {98, 0x17},
2091          {110, 0x16},
2092          {104, 0x16},
2093          {98, 0x16},
2094          {110, 0x15},
2095          {104, 0x15},
2096          {98, 0x15},
2097          {110, 0x14},
2098          {104, 0x14},
2099          {98, 0x14},
2100          {110, 0x13},
2101          {104, 0x13},
2102          {98, 0x13},
2103          {110, 0x12},
2104          {104, 0x08},
2105          {98, 0x08},
2106          {110, 0x07},
2107          {104, 0x07},
2108          {98, 0x07},
2109          {110, 0x06},
2110          {104, 0x06},
2111          {98, 0x06},
2112          {110, 0x05},
2113          {104, 0x05},
2114          {98, 0x05},
2115          {110, 0x04},
2116          {104, 0x04},
2117          {98, 0x04},
2118          {110, 0x03},
2119          {104, 0x03},
2120          {98, 0x03},
2121          {110, 0x02},
2122          {104, 0x02},
2123          {98, 0x02},
2124          {110, 0x01},
2125          {104, 0x01},
2126          {98, 0x01},
2127          {110, 0x00},
2128          {104, 0x00},
2129          {98, 0x00},
2130          {93, 0x00},
2131          {88, 0x00},
2132          {83, 0x00},
2133          {78, 0x00},
2134          },
2135         /* 2.4GHz power gain index table */
2136         {
2137          {110, 0x3f},           /* highest txpower */
2138          {104, 0x3f},
2139          {98, 0x3f},
2140          {110, 0x3e},
2141          {104, 0x3e},
2142          {98, 0x3e},
2143          {110, 0x3d},
2144          {104, 0x3d},
2145          {98, 0x3d},
2146          {110, 0x3c},
2147          {104, 0x3c},
2148          {98, 0x3c},
2149          {110, 0x3b},
2150          {104, 0x3b},
2151          {98, 0x3b},
2152          {110, 0x3a},
2153          {104, 0x3a},
2154          {98, 0x3a},
2155          {110, 0x39},
2156          {104, 0x39},
2157          {98, 0x39},
2158          {110, 0x38},
2159          {104, 0x38},
2160          {98, 0x38},
2161          {110, 0x37},
2162          {104, 0x37},
2163          {98, 0x37},
2164          {110, 0x36},
2165          {104, 0x36},
2166          {98, 0x36},
2167          {110, 0x35},
2168          {104, 0x35},
2169          {98, 0x35},
2170          {110, 0x34},
2171          {104, 0x34},
2172          {98, 0x34},
2173          {110, 0x33},
2174          {104, 0x33},
2175          {98, 0x33},
2176          {110, 0x32},
2177          {104, 0x32},
2178          {98, 0x32},
2179          {110, 0x31},
2180          {104, 0x31},
2181          {98, 0x31},
2182          {110, 0x30},
2183          {104, 0x30},
2184          {98, 0x30},
2185          {110, 0x6},
2186          {104, 0x6},
2187          {98, 0x6},
2188          {110, 0x5},
2189          {104, 0x5},
2190          {98, 0x5},
2191          {110, 0x4},
2192          {104, 0x4},
2193          {98, 0x4},
2194          {110, 0x3},
2195          {104, 0x3},
2196          {98, 0x3},
2197          {110, 0x2},
2198          {104, 0x2},
2199          {98, 0x2},
2200          {110, 0x1},
2201          {104, 0x1},
2202          {98, 0x1},
2203          {110, 0x0},
2204          {104, 0x0},
2205          {98, 0x0},
2206          {97, 0},
2207          {96, 0},
2208          {95, 0},
2209          {94, 0},
2210          {93, 0},
2211          {92, 0},
2212          {91, 0},
2213          {90, 0},
2214          {89, 0},
2215          {88, 0},
2216          {87, 0},
2217          {86, 0},
2218          {85, 0},
2219          {84, 0},
2220          {83, 0},
2221          {82, 0},
2222          {81, 0},
2223          {80, 0},
2224          {79, 0},
2225          {78, 0},
2226          {77, 0},
2227          {76, 0},
2228          {75, 0},
2229          {74, 0},
2230          {73, 0},
2231          {72, 0},
2232          {71, 0},
2233          {70, 0},
2234          {69, 0},
2235          {68, 0},
2236          {67, 0},
2237          {66, 0},
2238          {65, 0},
2239          {64, 0},
2240          {63, 0},
2241          {62, 0},
2242          {61, 0},
2243          {60, 0},
2244          {59, 0},
2245          }
2246 };
2247
2248 static int iwl4965_fill_txpower_tbl(struct iwl4965_priv *priv, u8 band, u16 channel,
2249                                     u8 is_fat, u8 ctrl_chan_high,
2250                                     struct iwl4965_tx_power_db *tx_power_tbl)
2251 {
2252         u8 saturation_power;
2253         s32 target_power;
2254         s32 user_target_power;
2255         s32 power_limit;
2256         s32 current_temp;
2257         s32 reg_limit;
2258         s32 current_regulatory;
2259         s32 txatten_grp = CALIB_CH_GROUP_MAX;
2260         int i;
2261         int c;
2262         const struct iwl4965_channel_info *ch_info = NULL;
2263         struct iwl4965_eeprom_calib_ch_info ch_eeprom_info;
2264         const struct iwl4965_eeprom_calib_measure *measurement;
2265         s16 voltage;
2266         s32 init_voltage;
2267         s32 voltage_compensation;
2268         s32 degrees_per_05db_num;
2269         s32 degrees_per_05db_denom;
2270         s32 factory_temp;
2271         s32 temperature_comp[2];
2272         s32 factory_gain_index[2];
2273         s32 factory_actual_pwr[2];
2274         s32 power_index;
2275
2276         /* Sanity check requested level (dBm) */
2277         if (priv->user_txpower_limit < IWL_TX_POWER_TARGET_POWER_MIN) {
2278                 IWL_WARNING("Requested user TXPOWER %d below limit.\n",
2279                             priv->user_txpower_limit);
2280                 return -EINVAL;
2281         }
2282         if (priv->user_txpower_limit > IWL_TX_POWER_TARGET_POWER_MAX) {
2283                 IWL_WARNING("Requested user TXPOWER %d above limit.\n",
2284                             priv->user_txpower_limit);
2285                 return -EINVAL;
2286         }
2287
2288         /* user_txpower_limit is in dBm, convert to half-dBm (half-dB units
2289          *   are used for indexing into txpower table) */
2290         user_target_power = 2 * priv->user_txpower_limit;
2291
2292         /* Get current (RXON) channel, band, width */
2293         ch_info =
2294                 iwl4965_get_channel_txpower_info(priv, priv->phymode, channel);
2295
2296         IWL_DEBUG_TXPOWER("chan %d band %d is_fat %d\n", channel, band,
2297                           is_fat);
2298
2299         if (!ch_info)
2300                 return -EINVAL;
2301
2302         /* get txatten group, used to select 1) thermal txpower adjustment
2303          *   and 2) mimo txpower balance between Tx chains. */
2304         txatten_grp = iwl4965_get_tx_atten_grp(channel);
2305         if (txatten_grp < 0)
2306                 return -EINVAL;
2307
2308         IWL_DEBUG_TXPOWER("channel %d belongs to txatten group %d\n",
2309                           channel, txatten_grp);
2310
2311         if (is_fat) {
2312                 if (ctrl_chan_high)
2313                         channel -= 2;
2314                 else
2315                         channel += 2;
2316         }
2317
2318         /* hardware txpower limits ...
2319          * saturation (clipping distortion) txpowers are in half-dBm */
2320         if (band)
2321                 saturation_power = priv->eeprom.calib_info.saturation_power24;
2322         else
2323                 saturation_power = priv->eeprom.calib_info.saturation_power52;
2324
2325         if (saturation_power < IWL_TX_POWER_SATURATION_MIN ||
2326             saturation_power > IWL_TX_POWER_SATURATION_MAX) {
2327                 if (band)
2328                         saturation_power = IWL_TX_POWER_DEFAULT_SATURATION_24;
2329                 else
2330                         saturation_power = IWL_TX_POWER_DEFAULT_SATURATION_52;
2331         }
2332
2333         /* regulatory txpower limits ... reg_limit values are in half-dBm,
2334          *   max_power_avg values are in dBm, convert * 2 */
2335         if (is_fat)
2336                 reg_limit = ch_info->fat_max_power_avg * 2;
2337         else
2338                 reg_limit = ch_info->max_power_avg * 2;
2339
2340         if ((reg_limit < IWL_TX_POWER_REGULATORY_MIN) ||
2341             (reg_limit > IWL_TX_POWER_REGULATORY_MAX)) {
2342                 if (band)
2343                         reg_limit = IWL_TX_POWER_DEFAULT_REGULATORY_24;
2344                 else
2345                         reg_limit = IWL_TX_POWER_DEFAULT_REGULATORY_52;
2346         }
2347
2348         /* Interpolate txpower calibration values for this channel,
2349          *   based on factory calibration tests on spaced channels. */
2350         iwl4965_interpolate_chan(priv, channel, &ch_eeprom_info);
2351
2352         /* calculate tx gain adjustment based on power supply voltage */
2353         voltage = priv->eeprom.calib_info.voltage;
2354         init_voltage = (s32)le32_to_cpu(priv->card_alive_init.voltage);
2355         voltage_compensation =
2356             iwl4965_get_voltage_compensation(voltage, init_voltage);
2357
2358         IWL_DEBUG_TXPOWER("curr volt %d eeprom volt %d volt comp %d\n",
2359                           init_voltage,
2360                           voltage, voltage_compensation);
2361
2362         /* get current temperature (Celsius) */
2363         current_temp = max(priv->temperature, IWL_TX_POWER_TEMPERATURE_MIN);
2364         current_temp = min(priv->temperature, IWL_TX_POWER_TEMPERATURE_MAX);
2365         current_temp = KELVIN_TO_CELSIUS(current_temp);
2366
2367         /* select thermal txpower adjustment params, based on channel group
2368          *   (same frequency group used for mimo txatten adjustment) */
2369         degrees_per_05db_num =
2370             tx_power_cmp_tble[txatten_grp].degrees_per_05db_a;
2371         degrees_per_05db_denom =
2372             tx_power_cmp_tble[txatten_grp].degrees_per_05db_a_denom;
2373
2374         /* get per-chain txpower values from factory measurements */
2375         for (c = 0; c < 2; c++) {
2376                 measurement = &ch_eeprom_info.measurements[c][1];
2377
2378                 /* txgain adjustment (in half-dB steps) based on difference
2379                  *   between factory and current temperature */
2380                 factory_temp = measurement->temperature;
2381                 iwl4965_math_div_round((current_temp - factory_temp) *
2382                                        degrees_per_05db_denom,
2383                                        degrees_per_05db_num,
2384                                        &temperature_comp[c]);
2385
2386                 factory_gain_index[c] = measurement->gain_idx;
2387                 factory_actual_pwr[c] = measurement->actual_pow;
2388
2389                 IWL_DEBUG_TXPOWER("chain = %d\n", c);
2390                 IWL_DEBUG_TXPOWER("fctry tmp %d, "
2391                                   "curr tmp %d, comp %d steps\n",
2392                                   factory_temp, current_temp,
2393                                   temperature_comp[c]);
2394
2395                 IWL_DEBUG_TXPOWER("fctry idx %d, fctry pwr %d\n",
2396                                   factory_gain_index[c],
2397                                   factory_actual_pwr[c]);
2398         }
2399
2400         /* for each of 33 bit-rates (including 1 for CCK) */
2401         for (i = 0; i < POWER_TABLE_NUM_ENTRIES; i++) {
2402                 u8 is_mimo_rate;
2403                 union iwl4965_tx_power_dual_stream tx_power;
2404
2405                 /* for mimo, reduce each chain's txpower by half
2406                  * (3dB, 6 steps), so total output power is regulatory
2407                  * compliant. */
2408                 if (i & 0x8) {
2409                         current_regulatory = reg_limit -
2410                             IWL_TX_POWER_MIMO_REGULATORY_COMPENSATION;
2411                         is_mimo_rate = 1;
2412                 } else {
2413                         current_regulatory = reg_limit;
2414                         is_mimo_rate = 0;
2415                 }
2416
2417                 /* find txpower limit, either hardware or regulatory */
2418                 power_limit = saturation_power - back_off_table[i];
2419                 if (power_limit > current_regulatory)
2420                         power_limit = current_regulatory;
2421
2422                 /* reduce user's txpower request if necessary
2423                  * for this rate on this channel */
2424                 target_power = user_target_power;
2425                 if (target_power > power_limit)
2426                         target_power = power_limit;
2427
2428                 IWL_DEBUG_TXPOWER("rate %d sat %d reg %d usr %d tgt %d\n",
2429                                   i, saturation_power - back_off_table[i],
2430                                   current_regulatory, user_target_power,
2431                                   target_power);
2432
2433                 /* for each of 2 Tx chains (radio transmitters) */
2434                 for (c = 0; c < 2; c++) {
2435                         s32 atten_value;
2436
2437                         if (is_mimo_rate)
2438                                 atten_value =
2439                                     (s32)le32_to_cpu(priv->card_alive_init.
2440                                     tx_atten[txatten_grp][c]);
2441                         else
2442                                 atten_value = 0;
2443
2444                         /* calculate index; higher index means lower txpower */
2445                         power_index = (u8) (factory_gain_index[c] -
2446                                             (target_power -
2447                                              factory_actual_pwr[c]) -
2448                                             temperature_comp[c] -
2449                                             voltage_compensation +
2450                                             atten_value);
2451
2452 /*                      IWL_DEBUG_TXPOWER("calculated txpower index %d\n",
2453                                                 power_index); */
2454
2455                         if (power_index < get_min_power_index(i, band))
2456                                 power_index = get_min_power_index(i, band);
2457
2458                         /* adjust 5 GHz index to support negative indexes */
2459                         if (!band)
2460                                 power_index += 9;
2461
2462                         /* CCK, rate 32, reduce txpower for CCK */
2463                         if (i == POWER_TABLE_CCK_ENTRY)
2464                                 power_index +=
2465                                     IWL_TX_POWER_CCK_COMPENSATION_C_STEP;
2466
2467                         /* stay within the table! */
2468                         if (power_index > 107) {
2469                                 IWL_WARNING("txpower index %d > 107\n",
2470                                             power_index);
2471                                 power_index = 107;
2472                         }
2473                         if (power_index < 0) {
2474                                 IWL_WARNING("txpower index %d < 0\n",
2475                                             power_index);
2476                                 power_index = 0;
2477                         }
2478
2479                         /* fill txpower command for this rate/chain */
2480                         tx_power.s.radio_tx_gain[c] =
2481                                 gain_table[band][power_index].radio;
2482                         tx_power.s.dsp_predis_atten[c] =
2483                                 gain_table[band][power_index].dsp;
2484
2485                         IWL_DEBUG_TXPOWER("chain %d mimo %d index %d "
2486                                           "gain 0x%02x dsp %d\n",
2487                                           c, atten_value, power_index,
2488                                         tx_power.s.radio_tx_gain[c],
2489                                         tx_power.s.dsp_predis_atten[c]);
2490                 }/* for each chain */
2491
2492                 tx_power_tbl->power_tbl[i].dw = cpu_to_le32(tx_power.dw);
2493
2494         }/* for each rate */
2495
2496         return 0;
2497 }
2498
2499 /**
2500  * iwl4965_hw_reg_send_txpower - Configure the TXPOWER level user limit
2501  *
2502  * Uses the active RXON for channel, band, and characteristics (fat, high)
2503  * The power limit is taken from priv->user_txpower_limit.
2504  */
2505 int iwl4965_hw_reg_send_txpower(struct iwl4965_priv *priv)
2506 {
2507         struct iwl4965_txpowertable_cmd cmd = { 0 };
2508         int rc = 0;
2509         u8 band = 0;
2510         u8 is_fat = 0;
2511         u8 ctrl_chan_high = 0;
2512
2513         if (test_bit(STATUS_SCANNING, &priv->status)) {
2514                 /* If this gets hit a lot, switch it to a BUG() and catch
2515                  * the stack trace to find out who is calling this during
2516                  * a scan. */
2517                 IWL_WARNING("TX Power requested while scanning!\n");
2518                 return -EAGAIN;
2519         }
2520
2521         band = ((priv->phymode == MODE_IEEE80211B) ||
2522                 (priv->phymode == MODE_IEEE80211G));
2523
2524         is_fat =  is_fat_channel(priv->active_rxon.flags);
2525
2526         if (is_fat &&
2527             (priv->active_rxon.flags & RXON_FLG_CTRL_CHANNEL_LOC_HI_MSK))
2528                 ctrl_chan_high = 1;
2529
2530         cmd.band = band;
2531         cmd.channel = priv->active_rxon.channel;
2532
2533         rc = iwl4965_fill_txpower_tbl(priv, band,
2534                                 le16_to_cpu(priv->active_rxon.channel),
2535                                 is_fat, ctrl_chan_high, &cmd.tx_power);
2536         if (rc)
2537                 return rc;
2538
2539         rc = iwl4965_send_cmd_pdu(priv, REPLY_TX_PWR_TABLE_CMD, sizeof(cmd), &cmd);
2540         return rc;
2541 }
2542
2543 int iwl4965_hw_channel_switch(struct iwl4965_priv *priv, u16 channel)
2544 {
2545         int rc;
2546         u8 band = 0;
2547         u8 is_fat = 0;
2548         u8 ctrl_chan_high = 0;
2549         struct iwl4965_channel_switch_cmd cmd = { 0 };
2550         const struct iwl4965_channel_info *ch_info;
2551
2552         band = ((priv->phymode == MODE_IEEE80211B) ||
2553                 (priv->phymode == MODE_IEEE80211G));
2554
2555         ch_info = iwl4965_get_channel_info(priv, priv->phymode, channel);
2556
2557         is_fat = is_fat_channel(priv->staging_rxon.flags);
2558
2559         if (is_fat &&
2560             (priv->active_rxon.flags & RXON_FLG_CTRL_CHANNEL_LOC_HI_MSK))
2561                 ctrl_chan_high = 1;
2562
2563         cmd.band = band;
2564         cmd.expect_beacon = 0;
2565         cmd.channel = cpu_to_le16(channel);
2566         cmd.rxon_flags = priv->active_rxon.flags;
2567         cmd.rxon_filter_flags = priv->active_rxon.filter_flags;
2568         cmd.switch_time = cpu_to_le32(priv->ucode_beacon_time);
2569         if (ch_info)
2570                 cmd.expect_beacon = is_channel_radar(ch_info);
2571         else
2572                 cmd.expect_beacon = 1;
2573
2574         rc = iwl4965_fill_txpower_tbl(priv, band, channel, is_fat,
2575                                       ctrl_chan_high, &cmd.tx_power);
2576         if (rc) {
2577                 IWL_DEBUG_11H("error:%d  fill txpower_tbl\n", rc);
2578                 return rc;
2579         }
2580
2581         rc = iwl4965_send_cmd_pdu(priv, REPLY_CHANNEL_SWITCH, sizeof(cmd), &cmd);
2582         return rc;
2583 }
2584
2585 #define RTS_HCCA_RETRY_LIMIT            3
2586 #define RTS_DFAULT_RETRY_LIMIT          60
2587
2588 void iwl4965_hw_build_tx_cmd_rate(struct iwl4965_priv *priv,
2589                               struct iwl4965_cmd *cmd,
2590                               struct ieee80211_tx_control *ctrl,
2591                               struct ieee80211_hdr *hdr, int sta_id,
2592                               int is_hcca)
2593 {
2594         u8 rate;
2595         u8 rts_retry_limit = 0;
2596         u8 data_retry_limit = 0;
2597         __le32 tx_flags;
2598         u16 fc = le16_to_cpu(hdr->frame_control);
2599
2600         tx_flags = cmd->cmd.tx.tx_flags;
2601
2602         rate = iwl4965_rates[ctrl->tx_rate].plcp;
2603
2604         rts_retry_limit = (is_hcca) ?
2605             RTS_HCCA_RETRY_LIMIT : RTS_DFAULT_RETRY_LIMIT;
2606
2607         if (ieee80211_is_probe_response(fc)) {
2608                 data_retry_limit = 3;
2609                 if (data_retry_limit < rts_retry_limit)
2610                         rts_retry_limit = data_retry_limit;
2611         } else
2612                 data_retry_limit = IWL_DEFAULT_TX_RETRY;
2613
2614         if (priv->data_retry_limit != -1)
2615                 data_retry_limit = priv->data_retry_limit;
2616
2617         if ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT) {
2618                 switch (fc & IEEE80211_FCTL_STYPE) {
2619                 case IEEE80211_STYPE_AUTH:
2620                 case IEEE80211_STYPE_DEAUTH:
2621                 case IEEE80211_STYPE_ASSOC_REQ:
2622                 case IEEE80211_STYPE_REASSOC_REQ:
2623                         if (tx_flags & TX_CMD_FLG_RTS_MSK) {
2624                                 tx_flags &= ~TX_CMD_FLG_RTS_MSK;
2625                                 tx_flags |= TX_CMD_FLG_CTS_MSK;
2626                         }
2627                         break;
2628                 default:
2629                         break;
2630                 }
2631         }
2632
2633         cmd->cmd.tx.rts_retry_limit = rts_retry_limit;
2634         cmd->cmd.tx.data_retry_limit = data_retry_limit;
2635         cmd->cmd.tx.rate_n_flags = iwl4965_hw_set_rate_n_flags(rate, 0);
2636         cmd->cmd.tx.tx_flags = tx_flags;
2637 }
2638
2639 int iwl4965_hw_get_rx_read(struct iwl4965_priv *priv)
2640 {
2641         struct iwl4965_shared *shared_data = priv->hw_setting.shared_virt;
2642
2643         return IWL_GET_BITS(*shared_data, rb_closed_stts_rb_num);
2644 }
2645
2646 int iwl4965_hw_get_temperature(struct iwl4965_priv *priv)
2647 {
2648         return priv->temperature;
2649 }
2650
2651 unsigned int iwl4965_hw_get_beacon_cmd(struct iwl4965_priv *priv,
2652                           struct iwl4965_frame *frame, u8 rate)
2653 {
2654         struct iwl4965_tx_beacon_cmd *tx_beacon_cmd;
2655         unsigned int frame_size;
2656
2657         tx_beacon_cmd = &frame->u.beacon;
2658         memset(tx_beacon_cmd, 0, sizeof(*tx_beacon_cmd));
2659
2660         tx_beacon_cmd->tx.sta_id = IWL4965_BROADCAST_ID;
2661         tx_beacon_cmd->tx.stop_time.life_time = TX_CMD_LIFE_TIME_INFINITE;
2662
2663         frame_size = iwl4965_fill_beacon_frame(priv,
2664                                 tx_beacon_cmd->frame,
2665                                 iwl4965_broadcast_addr,
2666                                 sizeof(frame->u) - sizeof(*tx_beacon_cmd));
2667
2668         BUG_ON(frame_size > MAX_MPDU_SIZE);
2669         tx_beacon_cmd->tx.len = cpu_to_le16((u16)frame_size);
2670
2671         if ((rate == IWL_RATE_1M_PLCP) || (rate >= IWL_RATE_2M_PLCP))
2672                 tx_beacon_cmd->tx.rate_n_flags =
2673                         iwl4965_hw_set_rate_n_flags(rate, RATE_MCS_CCK_MSK);
2674         else
2675                 tx_beacon_cmd->tx.rate_n_flags =
2676                         iwl4965_hw_set_rate_n_flags(rate, 0);
2677
2678         tx_beacon_cmd->tx.tx_flags = (TX_CMD_FLG_SEQ_CTL_MSK |
2679                                 TX_CMD_FLG_TSF_MSK | TX_CMD_FLG_STA_RATE_MSK);
2680         return (sizeof(*tx_beacon_cmd) + frame_size);
2681 }
2682
2683 int iwl4965_hw_tx_queue_init(struct iwl4965_priv *priv, struct iwl4965_tx_queue *txq)
2684 {
2685         int rc;
2686         unsigned long flags;
2687         int txq_id = txq->q.id;
2688
2689         spin_lock_irqsave(&priv->lock, flags);
2690         rc = iwl4965_grab_nic_access(priv);
2691         if (rc) {
2692                 spin_unlock_irqrestore(&priv->lock, flags);
2693                 return rc;
2694         }
2695
2696         iwl4965_write_direct32(priv, FH_MEM_CBBC_QUEUE(txq_id),
2697                              txq->q.dma_addr >> 8);
2698         iwl4965_write_direct32(
2699                 priv, IWL_FH_TCSR_CHNL_TX_CONFIG_REG(txq_id),
2700                 IWL_FH_TCSR_TX_CONFIG_REG_VAL_DMA_CHNL_ENABLE |
2701                 IWL_FH_TCSR_TX_CONFIG_REG_VAL_DMA_CREDIT_ENABLE_VAL);
2702         iwl4965_release_nic_access(priv);
2703         spin_unlock_irqrestore(&priv->lock, flags);
2704
2705         return 0;
2706 }
2707
2708 static inline u8 iwl4965_get_dma_hi_address(dma_addr_t addr)
2709 {
2710         return sizeof(addr) > sizeof(u32) ? (addr >> 16) >> 16 : 0;
2711 }
2712
2713 int iwl4965_hw_txq_attach_buf_to_tfd(struct iwl4965_priv *priv, void *ptr,
2714                                  dma_addr_t addr, u16 len)
2715 {
2716         int index, is_odd;
2717         struct iwl4965_tfd_frame *tfd = ptr;
2718         u32 num_tbs = IWL_GET_BITS(*tfd, num_tbs);
2719
2720         if ((num_tbs >= MAX_NUM_OF_TBS) || (num_tbs < 0)) {
2721                 IWL_ERROR("Error can not send more than %d chunks\n",
2722                           MAX_NUM_OF_TBS);
2723                 return -EINVAL;
2724         }
2725
2726         index = num_tbs / 2;
2727         is_odd = num_tbs & 0x1;
2728
2729         if (!is_odd) {
2730                 tfd->pa[index].tb1_addr = cpu_to_le32(addr);
2731                 IWL_SET_BITS(tfd->pa[index], tb1_addr_hi,
2732                              iwl4965_get_dma_hi_address(addr));
2733                 IWL_SET_BITS(tfd->pa[index], tb1_len, len);
2734         } else {
2735                 IWL_SET_BITS(tfd->pa[index], tb2_addr_lo16,
2736                              (u32) (addr & 0xffff));
2737                 IWL_SET_BITS(tfd->pa[index], tb2_addr_hi20, addr >> 16);
2738                 IWL_SET_BITS(tfd->pa[index], tb2_len, len);
2739         }
2740
2741         IWL_SET_BITS(*tfd, num_tbs, num_tbs + 1);
2742
2743         return 0;
2744 }
2745
2746 static void iwl4965_hw_card_show_info(struct iwl4965_priv *priv)
2747 {
2748         u16 hw_version = priv->eeprom.board_revision_4965;
2749
2750         IWL_DEBUG_INFO("4965ABGN HW Version %u.%u.%u\n",
2751                        ((hw_version >> 8) & 0x0F),
2752                        ((hw_version >> 8) >> 4), (hw_version & 0x00FF));
2753
2754         IWL_DEBUG_INFO("4965ABGN PBA Number %.16s\n",
2755                        priv->eeprom.board_pba_number_4965);
2756 }
2757
2758 #define IWL_TX_CRC_SIZE         4
2759 #define IWL_TX_DELIMITER_SIZE   4
2760
2761 int iwl4965_tx_queue_update_wr_ptr(struct iwl4965_priv *priv,
2762                                    struct iwl4965_tx_queue *txq, u16 byte_cnt)
2763 {
2764         int len;
2765         int txq_id = txq->q.id;
2766         struct iwl4965_shared *shared_data = priv->hw_setting.shared_virt;
2767
2768         if (txq->need_update == 0)
2769                 return 0;
2770
2771         len = byte_cnt + IWL_TX_CRC_SIZE + IWL_TX_DELIMITER_SIZE;
2772
2773         IWL_SET_BITS16(shared_data->queues_byte_cnt_tbls[txq_id].
2774                        tfd_offset[txq->q.write_ptr], byte_cnt, len);
2775
2776         if (txq->q.write_ptr < IWL4965_MAX_WIN_SIZE)
2777                 IWL_SET_BITS16(shared_data->queues_byte_cnt_tbls[txq_id].
2778                         tfd_offset[IWL4965_QUEUE_SIZE + txq->q.write_ptr],
2779                         byte_cnt, len);
2780
2781         return 0;
2782 }
2783
2784 /* Set up Rx receiver/antenna/chain usage in "staging" RXON image.
2785  * This should not be used for scan command ... it puts data in wrong place.  */
2786 void iwl4965_set_rxon_chain(struct iwl4965_priv *priv)
2787 {
2788         u8 is_single = is_single_stream(priv);
2789         u8 idle_state, rx_state;
2790
2791         priv->staging_rxon.rx_chain = 0;
2792         rx_state = idle_state = 3;
2793
2794         /* Tell uCode which antennas are actually connected.
2795          * Before first association, we assume all antennas are connected.
2796          * Just after first association, iwl4965_noise_calibration()
2797          *    checks which antennas actually *are* connected. */
2798         priv->staging_rxon.rx_chain |=
2799             cpu_to_le16(priv->valid_antenna << RXON_RX_CHAIN_VALID_POS);
2800
2801         /* How many receivers should we use? */
2802         iwl4965_get_rx_chain_counter(priv, &idle_state, &rx_state);
2803         priv->staging_rxon.rx_chain |=
2804                 cpu_to_le16(rx_state << RXON_RX_CHAIN_MIMO_CNT_POS);
2805         priv->staging_rxon.rx_chain |=
2806                 cpu_to_le16(idle_state << RXON_RX_CHAIN_CNT_POS);
2807
2808         if (!is_single && (rx_state >= 2) &&
2809             !test_bit(STATUS_POWER_PMI, &priv->status))
2810                 priv->staging_rxon.rx_chain |= RXON_RX_CHAIN_MIMO_FORCE_MSK;
2811         else
2812                 priv->staging_rxon.rx_chain &= ~RXON_RX_CHAIN_MIMO_FORCE_MSK;
2813
2814         IWL_DEBUG_ASSOC("rx chain %X\n", priv->staging_rxon.rx_chain);
2815 }
2816
2817 #ifdef CONFIG_IWL4965_HT
2818 #ifdef CONFIG_IWL4965_HT_AGG
2819 /*
2820         get the traffic load value for tid
2821 */
2822 static u32 iwl4965_tl_get_load(struct iwl4965_priv *priv, u8 tid)
2823 {
2824         u32 load = 0;
2825         u32 current_time = jiffies_to_msecs(jiffies);
2826         u32 time_diff;
2827         s32 index;
2828         unsigned long flags;
2829         struct iwl4965_traffic_load *tid_ptr = NULL;
2830
2831         if (tid >= TID_MAX_LOAD_COUNT)
2832                 return 0;
2833
2834         tid_ptr = &(priv->lq_mngr.agg_ctrl.traffic_load[tid]);
2835
2836         current_time -= current_time % TID_ROUND_VALUE;
2837
2838         spin_lock_irqsave(&priv->lq_mngr.lock, flags);
2839         if (!(tid_ptr->queue_count))
2840                 goto out;
2841
2842         time_diff = TIME_WRAP_AROUND(tid_ptr->time_stamp, current_time);
2843         index = time_diff / TID_QUEUE_CELL_SPACING;
2844
2845         if (index >= TID_QUEUE_MAX_SIZE) {
2846                 u32 oldest_time = current_time - TID_MAX_TIME_DIFF;
2847
2848                 while (tid_ptr->queue_count &&
2849                        (tid_ptr->time_stamp < oldest_time)) {
2850                         tid_ptr->total -= tid_ptr->packet_count[tid_ptr->head];
2851                         tid_ptr->packet_count[tid_ptr->head] = 0;
2852                         tid_ptr->time_stamp += TID_QUEUE_CELL_SPACING;
2853                         tid_ptr->queue_count--;
2854                         tid_ptr->head++;
2855                         if (tid_ptr->head >= TID_QUEUE_MAX_SIZE)
2856                                 tid_ptr->head = 0;
2857                 }
2858         }
2859         load = tid_ptr->total;
2860
2861  out:
2862         spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
2863         return load;
2864 }
2865
2866 /*
2867         increment traffic load value for tid and also remove
2868         any old values if passed the certian time period
2869 */
2870 static void iwl4965_tl_add_packet(struct iwl4965_priv *priv, u8 tid)
2871 {
2872         u32 current_time = jiffies_to_msecs(jiffies);
2873         u32 time_diff;
2874         s32 index;
2875         unsigned long flags;
2876         struct iwl4965_traffic_load *tid_ptr = NULL;
2877
2878         if (tid >= TID_MAX_LOAD_COUNT)
2879                 return;
2880
2881         tid_ptr = &(priv->lq_mngr.agg_ctrl.traffic_load[tid]);
2882
2883         current_time -= current_time % TID_ROUND_VALUE;
2884
2885         spin_lock_irqsave(&priv->lq_mngr.lock, flags);
2886         if (!(tid_ptr->queue_count)) {
2887                 tid_ptr->total = 1;
2888                 tid_ptr->time_stamp = current_time;
2889                 tid_ptr->queue_count = 1;
2890                 tid_ptr->head = 0;
2891                 tid_ptr->packet_count[0] = 1;
2892                 goto out;
2893         }
2894
2895         time_diff = TIME_WRAP_AROUND(tid_ptr->time_stamp, current_time);
2896         index = time_diff / TID_QUEUE_CELL_SPACING;
2897
2898         if (index >= TID_QUEUE_MAX_SIZE) {
2899                 u32 oldest_time = current_time - TID_MAX_TIME_DIFF;
2900
2901                 while (tid_ptr->queue_count &&
2902                        (tid_ptr->time_stamp < oldest_time)) {
2903                         tid_ptr->total -= tid_ptr->packet_count[tid_ptr->head];
2904                         tid_ptr->packet_count[tid_ptr->head] = 0;
2905                         tid_ptr->time_stamp += TID_QUEUE_CELL_SPACING;
2906                         tid_ptr->queue_count--;
2907                         tid_ptr->head++;
2908                         if (tid_ptr->head >= TID_QUEUE_MAX_SIZE)
2909                                 tid_ptr->head = 0;
2910                 }
2911         }
2912
2913         index = (tid_ptr->head + index) % TID_QUEUE_MAX_SIZE;
2914         tid_ptr->packet_count[index] = tid_ptr->packet_count[index] + 1;
2915         tid_ptr->total = tid_ptr->total + 1;
2916
2917         if ((index + 1) > tid_ptr->queue_count)
2918                 tid_ptr->queue_count = index + 1;
2919  out:
2920         spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
2921
2922 }
2923
2924 #define MMAC_SCHED_MAX_NUMBER_OF_HT_BACK_FLOWS   7
2925 enum HT_STATUS {
2926         BA_STATUS_FAILURE = 0,
2927         BA_STATUS_INITIATOR_DELBA,
2928         BA_STATUS_RECIPIENT_DELBA,
2929         BA_STATUS_RENEW_ADDBA_REQUEST,
2930         BA_STATUS_ACTIVE,
2931 };
2932
2933 static u8 iwl4964_tl_ba_avail(struct iwl4965_priv *priv)
2934 {
2935         int i;
2936         struct iwl4965_lq_mngr *lq;
2937         u8 count = 0;
2938         u16 msk;
2939
2940         lq = (struct iwl4965_lq_mngr *)&(priv->lq_mngr);
2941         for (i = 0; i < TID_MAX_LOAD_COUNT ; i++) {
2942                 msk = 1 << i;
2943                 if ((lq->agg_ctrl.granted_ba & msk) ||
2944                     (lq->agg_ctrl.wait_for_agg_status & msk))
2945                         count++;
2946         }
2947
2948         if (count < MMAC_SCHED_MAX_NUMBER_OF_HT_BACK_FLOWS)
2949                 return 1;
2950
2951         return 0;
2952 }
2953
2954 static void iwl4965_ba_status(struct iwl4965_priv *priv,
2955                               u8 tid, enum HT_STATUS status);
2956
2957 static int iwl4965_perform_addba(struct iwl4965_priv *priv, u8 tid, u32 length,
2958                                  u32 ba_timeout)
2959 {
2960         int rc;
2961
2962         rc = ieee80211_start_BA_session(priv->hw, priv->bssid, tid);
2963         if (rc)
2964                 iwl4965_ba_status(priv, tid, BA_STATUS_FAILURE);
2965
2966         return rc;
2967 }
2968
2969 static int iwl4965_perform_delba(struct iwl4965_priv *priv, u8 tid)
2970 {
2971         int rc;
2972
2973         rc = ieee80211_stop_BA_session(priv->hw, priv->bssid, tid);
2974         if (rc)
2975                 iwl4965_ba_status(priv, tid, BA_STATUS_FAILURE);
2976
2977         return rc;
2978 }
2979
2980 static void iwl4965_turn_on_agg_for_tid(struct iwl4965_priv *priv,
2981                                         struct iwl4965_lq_mngr *lq,
2982                                         u8 auto_agg, u8 tid)
2983 {
2984         u32 tid_msk = (1 << tid);
2985         unsigned long flags;
2986
2987         spin_lock_irqsave(&priv->lq_mngr.lock, flags);
2988 /*
2989         if ((auto_agg) && (!lq->enable_counter)){
2990                 lq->agg_ctrl.next_retry = 0;
2991                 lq->agg_ctrl.tid_retry = 0;
2992                 spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
2993                 return;
2994         }
2995 */
2996         if (!(lq->agg_ctrl.granted_ba & tid_msk) &&
2997             (lq->agg_ctrl.requested_ba & tid_msk)) {
2998                 u8 available_queues;
2999                 u32 load;
3000
3001                 spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
3002                 available_queues = iwl4964_tl_ba_avail(priv);
3003                 load = iwl4965_tl_get_load(priv, tid);
3004
3005                 spin_lock_irqsave(&priv->lq_mngr.lock, flags);
3006                 if (!available_queues) {
3007                         if (auto_agg)
3008                                 lq->agg_ctrl.tid_retry |= tid_msk;
3009                         else {
3010                                 lq->agg_ctrl.requested_ba &= ~tid_msk;
3011                                 lq->agg_ctrl.wait_for_agg_status &= ~tid_msk;
3012                         }
3013                 } else if ((auto_agg) &&
3014                            ((load <= lq->agg_ctrl.tid_traffic_load_threshold) ||
3015                             ((lq->agg_ctrl.wait_for_agg_status & tid_msk))))
3016                         lq->agg_ctrl.tid_retry |= tid_msk;
3017                 else {
3018                         lq->agg_ctrl.wait_for_agg_status |= tid_msk;
3019                         spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
3020                         iwl4965_perform_addba(priv, tid, 0x40,
3021                                               lq->agg_ctrl.ba_timeout);
3022                         spin_lock_irqsave(&priv->lq_mngr.lock, flags);
3023                 }
3024         }
3025         spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
3026 }
3027
3028 static void iwl4965_turn_on_agg(struct iwl4965_priv *priv, u8 tid)
3029 {
3030         struct iwl4965_lq_mngr *lq;
3031         unsigned long flags;
3032
3033         lq = (struct iwl4965_lq_mngr *)&(priv->lq_mngr);
3034
3035         if ((tid < TID_MAX_LOAD_COUNT))
3036                 iwl4965_turn_on_agg_for_tid(priv, lq, lq->agg_ctrl.auto_agg,
3037                                             tid);
3038         else if (tid == TID_ALL_SPECIFIED) {
3039                 if (lq->agg_ctrl.requested_ba) {
3040                         for (tid = 0; tid < TID_MAX_LOAD_COUNT; tid++)
3041                                 iwl4965_turn_on_agg_for_tid(priv, lq,
3042                                         lq->agg_ctrl.auto_agg, tid);
3043                 } else {
3044                         spin_lock_irqsave(&priv->lq_mngr.lock, flags);
3045                         lq->agg_ctrl.tid_retry = 0;
3046                         lq->agg_ctrl.next_retry = 0;
3047                         spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
3048                 }
3049         }
3050
3051 }
3052
3053 void iwl4965_turn_off_agg(struct iwl4965_priv *priv, u8 tid)
3054 {
3055         u32 tid_msk;
3056         struct iwl4965_lq_mngr *lq;
3057         unsigned long flags;
3058
3059         lq = (struct iwl4965_lq_mngr *)&(priv->lq_mngr);
3060
3061         if ((tid < TID_MAX_LOAD_COUNT)) {
3062                 tid_msk = 1 << tid;
3063                 spin_lock_irqsave(&priv->lq_mngr.lock, flags);
3064                 lq->agg_ctrl.wait_for_agg_status |= tid_msk;
3065                 lq->agg_ctrl.requested_ba &= ~tid_msk;
3066                 spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
3067                 iwl4965_perform_delba(priv, tid);
3068         } else if (tid == TID_ALL_SPECIFIED) {
3069                 spin_lock_irqsave(&priv->lq_mngr.lock, flags);
3070                 for (tid = 0; tid < TID_MAX_LOAD_COUNT; tid++) {
3071                         tid_msk = 1 << tid;
3072                         lq->agg_ctrl.wait_for_agg_status |= tid_msk;
3073                         spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
3074                         iwl4965_perform_delba(priv, tid);
3075                         spin_lock_irqsave(&priv->lq_mngr.lock, flags);
3076                 }
3077                 lq->agg_ctrl.requested_ba = 0;
3078                 spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
3079         }
3080 }
3081
3082 static void iwl4965_ba_status(struct iwl4965_priv *priv,
3083                                 u8 tid, enum HT_STATUS status)
3084 {
3085         struct iwl4965_lq_mngr *lq;
3086         u32 tid_msk = (1 << tid);
3087         unsigned long flags;
3088
3089         lq = (struct iwl4965_lq_mngr *)&(priv->lq_mngr);
3090
3091         if ((tid >= TID_MAX_LOAD_COUNT))
3092                 goto out;
3093
3094         spin_lock_irqsave(&priv->lq_mngr.lock, flags);
3095         switch (status) {
3096         case BA_STATUS_ACTIVE:
3097                 if (!(lq->agg_ctrl.granted_ba & tid_msk))
3098                         lq->agg_ctrl.granted_ba |= tid_msk;
3099                 break;
3100         default:
3101                 if ((lq->agg_ctrl.granted_ba & tid_msk))
3102                         lq->agg_ctrl.granted_ba &= ~tid_msk;
3103                 break;
3104         }
3105
3106         lq->agg_ctrl.wait_for_agg_status &= ~tid_msk;
3107         if (status != BA_STATUS_ACTIVE) {
3108                 if (lq->agg_ctrl.auto_agg) {
3109                         lq->agg_ctrl.tid_retry |= tid_msk;
3110                         lq->agg_ctrl.next_retry =
3111                             jiffies + msecs_to_jiffies(500);
3112                 } else
3113                         lq->agg_ctrl.requested_ba &= ~tid_msk;
3114         }
3115         spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
3116  out:
3117         return;
3118 }
3119
3120 static void iwl4965_bg_agg_work(struct work_struct *work)
3121 {
3122         struct iwl4965_priv *priv = container_of(work, struct iwl4965_priv,
3123                                         agg_work);
3124
3125         u32 tid;
3126         u32 retry_tid;
3127         u32 tid_msk;
3128         unsigned long flags;
3129         struct iwl4965_lq_mngr *lq = (struct iwl4965_lq_mngr *)&(priv->lq_mngr);
3130
3131         spin_lock_irqsave(&priv->lq_mngr.lock, flags);
3132         retry_tid = lq->agg_ctrl.tid_retry;
3133         lq->agg_ctrl.tid_retry = 0;
3134         spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
3135
3136         if (retry_tid == TID_ALL_SPECIFIED)
3137                 iwl4965_turn_on_agg(priv, TID_ALL_SPECIFIED);
3138         else {
3139                 for (tid = 0; tid < TID_MAX_LOAD_COUNT; tid++) {
3140                         tid_msk = (1 << tid);
3141                         if (retry_tid & tid_msk)
3142                                 iwl4965_turn_on_agg(priv, tid);
3143                 }
3144         }
3145
3146         spin_lock_irqsave(&priv->lq_mngr.lock, flags);
3147         if (lq->agg_ctrl.tid_retry)
3148                 lq->agg_ctrl.next_retry = jiffies + msecs_to_jiffies(500);
3149         spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
3150         return;
3151 }
3152 #endif /*CONFIG_IWL4965_HT_AGG */
3153 #endif /* CONFIG_IWL4965_HT */
3154
3155 int iwl4965_tx_cmd(struct iwl4965_priv *priv, struct iwl4965_cmd *out_cmd,
3156                    u8 sta_id, dma_addr_t txcmd_phys,
3157                    struct ieee80211_hdr *hdr, u8 hdr_len,
3158                    struct ieee80211_tx_control *ctrl, void *sta_in)
3159 {
3160         struct iwl4965_tx_cmd cmd;
3161         struct iwl4965_tx_cmd *tx = (struct iwl4965_tx_cmd *)&out_cmd->cmd.payload[0];
3162         dma_addr_t scratch_phys;
3163         u8 unicast = 0;
3164         u8 is_data = 1;
3165         u16 fc;
3166         u16 rate_flags;
3167         int rate_index = min(ctrl->tx_rate & 0xffff, IWL_RATE_COUNT - 1);
3168 #ifdef CONFIG_IWL4965_HT
3169 #ifdef CONFIG_IWL4965_HT_AGG
3170         __le16 *qc;
3171 #endif /*CONFIG_IWL4965_HT_AGG */
3172 #endif /* CONFIG_IWL4965_HT */
3173
3174         unicast = !is_multicast_ether_addr(hdr->addr1);
3175
3176         fc = le16_to_cpu(hdr->frame_control);
3177         if ((fc & IEEE80211_FCTL_FTYPE) != IEEE80211_FTYPE_DATA)
3178                 is_data = 0;
3179
3180         memcpy(&cmd, &(out_cmd->cmd.tx), sizeof(struct iwl4965_tx_cmd));
3181         memset(tx, 0, sizeof(struct iwl4965_tx_cmd));
3182         memcpy(tx->hdr, hdr, hdr_len);
3183
3184         tx->len = cmd.len;
3185         tx->driver_txop = cmd.driver_txop;
3186         tx->stop_time.life_time = cmd.stop_time.life_time;
3187         tx->tx_flags = cmd.tx_flags;
3188         tx->sta_id = cmd.sta_id;
3189         tx->tid_tspec = cmd.tid_tspec;
3190         tx->timeout.pm_frame_timeout = cmd.timeout.pm_frame_timeout;
3191         tx->next_frame_len = cmd.next_frame_len;
3192
3193         tx->sec_ctl = cmd.sec_ctl;
3194         memcpy(&(tx->key[0]), &(cmd.key[0]), 16);
3195         tx->tx_flags = cmd.tx_flags;
3196
3197         tx->rts_retry_limit = cmd.rts_retry_limit;
3198         tx->data_retry_limit = cmd.data_retry_limit;
3199
3200         scratch_phys = txcmd_phys + sizeof(struct iwl4965_cmd_header) +
3201             offsetof(struct iwl4965_tx_cmd, scratch);
3202         tx->dram_lsb_ptr = cpu_to_le32(scratch_phys);
3203         tx->dram_msb_ptr = iwl4965_get_dma_hi_address(scratch_phys);
3204
3205         /* Hard coded to start at the highest retry fallback position
3206          * until the 4965 specific rate control algorithm is tied in */
3207         tx->initial_rate_index = LINK_QUAL_MAX_RETRY_NUM - 1;
3208
3209         /* Alternate between antenna A and B for successive frames */
3210         if (priv->use_ant_b_for_management_frame) {
3211                 priv->use_ant_b_for_management_frame = 0;
3212                 rate_flags = RATE_MCS_ANT_B_MSK;
3213         } else {
3214                 priv->use_ant_b_for_management_frame = 1;
3215                 rate_flags = RATE_MCS_ANT_A_MSK;
3216         }
3217
3218         if (!unicast || !is_data) {
3219                 if ((rate_index >= IWL_FIRST_CCK_RATE) &&
3220                     (rate_index <= IWL_LAST_CCK_RATE))
3221                         rate_flags |= RATE_MCS_CCK_MSK;
3222         } else {
3223                 tx->initial_rate_index = 0;
3224                 tx->tx_flags |= TX_CMD_FLG_STA_RATE_MSK;
3225         }
3226
3227         tx->rate_n_flags = iwl4965_hw_set_rate_n_flags(iwl4965_rates[rate_index].plcp,
3228                                                 rate_flags);
3229
3230         if (ieee80211_is_back_request(fc))
3231                 tx->tx_flags |= TX_CMD_FLG_ACK_MSK |
3232                         TX_CMD_FLG_IMM_BA_RSP_MASK;
3233 #ifdef CONFIG_IWL4965_HT
3234 #ifdef CONFIG_IWL4965_HT_AGG
3235         qc = ieee80211_get_qos_ctrl(hdr);
3236         if (qc &&
3237             (priv->iw_mode != IEEE80211_IF_TYPE_IBSS)) {
3238                 u8 tid = 0;
3239                 tid = (u8) (le16_to_cpu(*qc) & 0xF);
3240                 if (tid < TID_MAX_LOAD_COUNT)
3241                         iwl4965_tl_add_packet(priv, tid);
3242         }
3243
3244         if (priv->lq_mngr.agg_ctrl.next_retry &&
3245             (time_after(priv->lq_mngr.agg_ctrl.next_retry, jiffies))) {
3246                 unsigned long flags;
3247
3248                 spin_lock_irqsave(&priv->lq_mngr.lock, flags);
3249                 priv->lq_mngr.agg_ctrl.next_retry = 0;
3250                 spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
3251                 schedule_work(&priv->agg_work);
3252         }
3253 #endif
3254 #endif
3255         return 0;
3256 }
3257
3258 /**
3259  * sign_extend - Sign extend a value using specified bit as sign-bit
3260  *
3261  * Example: sign_extend(9, 3) would return -7 as bit3 of 1001b is 1
3262  * and bit0..2 is 001b which when sign extended to 1111111111111001b is -7.
3263  *
3264  * @param oper value to sign extend
3265  * @param index 0 based bit index (0<=index<32) to sign bit
3266  */
3267 static s32 sign_extend(u32 oper, int index)
3268 {
3269         u8 shift = 31 - index;
3270
3271         return (s32)(oper << shift) >> shift;
3272 }
3273
3274 /**
3275  * iwl4965_get_temperature - return the calibrated temperature (in Kelvin)
3276  * @statistics: Provides the temperature reading from the uCode
3277  *
3278  * A return of <0 indicates bogus data in the statistics
3279  */
3280 int iwl4965_get_temperature(const struct iwl4965_priv *priv)
3281 {
3282         s32 temperature;
3283         s32 vt;
3284         s32 R1, R2, R3;
3285         u32 R4;
3286
3287         if (test_bit(STATUS_TEMPERATURE, &priv->status) &&
3288                 (priv->statistics.flag & STATISTICS_REPLY_FLG_FAT_MODE_MSK)) {
3289                 IWL_DEBUG_TEMP("Running FAT temperature calibration\n");
3290                 R1 = (s32)le32_to_cpu(priv->card_alive_init.therm_r1[1]);
3291                 R2 = (s32)le32_to_cpu(priv->card_alive_init.therm_r2[1]);
3292                 R3 = (s32)le32_to_cpu(priv->card_alive_init.therm_r3[1]);
3293                 R4 = le32_to_cpu(priv->card_alive_init.therm_r4[1]);
3294         } else {
3295                 IWL_DEBUG_TEMP("Running temperature calibration\n");
3296                 R1 = (s32)le32_to_cpu(priv->card_alive_init.therm_r1[0]);
3297                 R2 = (s32)le32_to_cpu(priv->card_alive_init.therm_r2[0]);
3298                 R3 = (s32)le32_to_cpu(priv->card_alive_init.therm_r3[0]);
3299                 R4 = le32_to_cpu(priv->card_alive_init.therm_r4[0]);
3300         }
3301
3302         /*
3303          * Temperature is only 23 bits so sign extend out to 32
3304          *
3305          * NOTE If we haven't received a statistics notification yet
3306          * with an updated temperature, use R4 provided to us in the
3307          * ALIVE response. */
3308         if (!test_bit(STATUS_TEMPERATURE, &priv->status))
3309                 vt = sign_extend(R4, 23);
3310         else
3311                 vt = sign_extend(
3312                         le32_to_cpu(priv->statistics.general.temperature), 23);
3313
3314         IWL_DEBUG_TEMP("Calib values R[1-3]: %d %d %d R4: %d\n",
3315                        R1, R2, R3, vt);
3316
3317         if (R3 == R1) {
3318                 IWL_ERROR("Calibration conflict R1 == R3\n");
3319                 return -1;
3320         }
3321
3322         /* Calculate temperature in degrees Kelvin, adjust by 97%.
3323          * Add offset to center the adjustment around 0 degrees Centigrade. */
3324         temperature = TEMPERATURE_CALIB_A_VAL * (vt - R2);
3325         temperature /= (R3 - R1);
3326         temperature = (temperature * 97) / 100 +
3327             TEMPERATURE_CALIB_KELVIN_OFFSET;
3328
3329         IWL_DEBUG_TEMP("Calibrated temperature: %dK, %dC\n", temperature,
3330             KELVIN_TO_CELSIUS(temperature));
3331
3332         return temperature;
3333 }
3334
3335 /* Adjust Txpower only if temperature variance is greater than threshold. */
3336 #define IWL_TEMPERATURE_THRESHOLD   3
3337
3338 /**
3339  * iwl4965_is_temp_calib_needed - determines if new calibration is needed
3340  *
3341  * If the temperature changed has changed sufficiently, then a recalibration
3342  * is needed.
3343  *
3344  * Assumes caller will replace priv->last_temperature once calibration
3345  * executed.
3346  */
3347 static int iwl4965_is_temp_calib_needed(struct iwl4965_priv *priv)
3348 {
3349         int temp_diff;
3350
3351         if (!test_bit(STATUS_STATISTICS, &priv->status)) {
3352                 IWL_DEBUG_TEMP("Temperature not updated -- no statistics.\n");
3353                 return 0;
3354         }
3355
3356         temp_diff = priv->temperature - priv->last_temperature;
3357
3358         /* get absolute value */
3359         if (temp_diff < 0) {
3360                 IWL_DEBUG_POWER("Getting cooler, delta %d, \n", temp_diff);
3361                 temp_diff = -temp_diff;
3362         } else if (temp_diff == 0)
3363                 IWL_DEBUG_POWER("Same temp, \n");
3364         else
3365                 IWL_DEBUG_POWER("Getting warmer, delta %d, \n", temp_diff);
3366
3367         if (temp_diff < IWL_TEMPERATURE_THRESHOLD) {
3368                 IWL_DEBUG_POWER("Thermal txpower calib not needed\n");
3369                 return 0;
3370         }
3371
3372         IWL_DEBUG_POWER("Thermal txpower calib needed\n");
3373
3374         return 1;
3375 }
3376
3377 /* Calculate noise level, based on measurements during network silence just
3378  *   before arriving beacon.  This measurement can be done only if we know
3379  *   exactly when to expect beacons, therefore only when we're associated. */
3380 static void iwl4965_rx_calc_noise(struct iwl4965_priv *priv)
3381 {
3382         struct statistics_rx_non_phy *rx_info
3383                                 = &(priv->statistics.rx.general);
3384         int num_active_rx = 0;
3385         int total_silence = 0;
3386         int bcn_silence_a =
3387                 le32_to_cpu(rx_info->beacon_silence_rssi_a) & IN_BAND_FILTER;
3388         int bcn_silence_b =
3389                 le32_to_cpu(rx_info->beacon_silence_rssi_b) & IN_BAND_FILTER;
3390         int bcn_silence_c =
3391                 le32_to_cpu(rx_info->beacon_silence_rssi_c) & IN_BAND_FILTER;
3392
3393         if (bcn_silence_a) {
3394                 total_silence += bcn_silence_a;
3395                 num_active_rx++;
3396         }
3397         if (bcn_silence_b) {
3398                 total_silence += bcn_silence_b;
3399                 num_active_rx++;
3400         }
3401         if (bcn_silence_c) {
3402                 total_silence += bcn_silence_c;
3403                 num_active_rx++;
3404         }
3405
3406         /* Average among active antennas */
3407         if (num_active_rx)
3408                 priv->last_rx_noise = (total_silence / num_active_rx) - 107;
3409         else
3410                 priv->last_rx_noise = IWL_NOISE_MEAS_NOT_AVAILABLE;
3411
3412         IWL_DEBUG_CALIB("inband silence a %u, b %u, c %u, dBm %d\n",
3413                         bcn_silence_a, bcn_silence_b, bcn_silence_c,
3414                         priv->last_rx_noise);
3415 }
3416
3417 void iwl4965_hw_rx_statistics(struct iwl4965_priv *priv, struct iwl4965_rx_mem_buffer *rxb)
3418 {
3419         struct iwl4965_rx_packet *pkt = (void *)rxb->skb->data;
3420         int change;
3421         s32 temp;
3422
3423         IWL_DEBUG_RX("Statistics notification received (%d vs %d).\n",
3424                      (int)sizeof(priv->statistics), pkt->len);
3425
3426         change = ((priv->statistics.general.temperature !=
3427                    pkt->u.stats.general.temperature) ||
3428                   ((priv->statistics.flag &
3429                     STATISTICS_REPLY_FLG_FAT_MODE_MSK) !=
3430                    (pkt->u.stats.flag & STATISTICS_REPLY_FLG_FAT_MODE_MSK)));
3431
3432         memcpy(&priv->statistics, &pkt->u.stats, sizeof(priv->statistics));
3433
3434         set_bit(STATUS_STATISTICS, &priv->status);
3435
3436         /* Reschedule the statistics timer to occur in
3437          * REG_RECALIB_PERIOD seconds to ensure we get a
3438          * thermal update even if the uCode doesn't give
3439          * us one */
3440         mod_timer(&priv->statistics_periodic, jiffies +
3441                   msecs_to_jiffies(REG_RECALIB_PERIOD * 1000));
3442
3443         if (unlikely(!test_bit(STATUS_SCANNING, &priv->status)) &&
3444             (pkt->hdr.cmd == STATISTICS_NOTIFICATION)) {
3445                 iwl4965_rx_calc_noise(priv);
3446 #ifdef CONFIG_IWL4965_SENSITIVITY
3447                 queue_work(priv->workqueue, &priv->sensitivity_work);
3448 #endif
3449         }
3450
3451         /* If the hardware hasn't reported a change in
3452          * temperature then don't bother computing a
3453          * calibrated temperature value */
3454         if (!change)
3455                 return;
3456
3457         temp = iwl4965_get_temperature(priv);
3458         if (temp < 0)
3459                 return;
3460
3461         if (priv->temperature != temp) {
3462                 if (priv->temperature)
3463                         IWL_DEBUG_TEMP("Temperature changed "
3464                                        "from %dC to %dC\n",
3465                                        KELVIN_TO_CELSIUS(priv->temperature),
3466                                        KELVIN_TO_CELSIUS(temp));
3467                 else
3468                         IWL_DEBUG_TEMP("Temperature "
3469                                        "initialized to %dC\n",
3470                                        KELVIN_TO_CELSIUS(temp));
3471         }
3472
3473         priv->temperature = temp;
3474         set_bit(STATUS_TEMPERATURE, &priv->status);
3475
3476         if (unlikely(!test_bit(STATUS_SCANNING, &priv->status)) &&
3477                      iwl4965_is_temp_calib_needed(priv))
3478                 queue_work(priv->workqueue, &priv->txpower_work);
3479 }
3480
3481 static void iwl4965_handle_data_packet(struct iwl4965_priv *priv, int is_data,
3482                                        int include_phy,
3483                                        struct iwl4965_rx_mem_buffer *rxb,
3484                                        struct ieee80211_rx_status *stats)
3485 {
3486         struct iwl4965_rx_packet *pkt = (struct iwl4965_rx_packet *)rxb->skb->data;
3487         struct iwl4965_rx_phy_res *rx_start = (include_phy) ?
3488             (struct iwl4965_rx_phy_res *)&(pkt->u.raw[0]) : NULL;
3489         struct ieee80211_hdr *hdr;
3490         u16 len;
3491         __le32 *rx_end;
3492         unsigned int skblen;
3493         u32 ampdu_status;
3494
3495         if (!include_phy && priv->last_phy_res[0])
3496                 rx_start = (struct iwl4965_rx_phy_res *)&priv->last_phy_res[1];
3497
3498         if (!rx_start) {
3499                 IWL_ERROR("MPDU frame without a PHY data\n");
3500                 return;
3501         }
3502         if (include_phy) {
3503                 hdr = (struct ieee80211_hdr *)((u8 *) & rx_start[1] +
3504                                                rx_start->cfg_phy_cnt);
3505
3506                 len = le16_to_cpu(rx_start->byte_count);
3507
3508                 rx_end = (__le32 *) ((u8 *) & pkt->u.raw[0] +
3509                                   sizeof(struct iwl4965_rx_phy_res) +
3510                                   rx_start->cfg_phy_cnt + len);
3511
3512         } else {
3513                 struct iwl4965_rx_mpdu_res_start *amsdu =
3514                     (struct iwl4965_rx_mpdu_res_start *)pkt->u.raw;
3515
3516                 hdr = (struct ieee80211_hdr *)(pkt->u.raw +
3517                                sizeof(struct iwl4965_rx_mpdu_res_start));
3518                 len =  le16_to_cpu(amsdu->byte_count);
3519                 rx_start->byte_count = amsdu->byte_count;
3520                 rx_end = (__le32 *) (((u8 *) hdr) + len);
3521         }
3522         if (len > IWL_RX_BUF_SIZE || len < 16) {
3523                 IWL_WARNING("byte count out of range [16,4K]"
3524                                " : %d\n", len);
3525                 return;
3526         }
3527
3528         ampdu_status = le32_to_cpu(*rx_end);
3529         skblen = ((u8 *) rx_end - (u8 *) & pkt->u.raw[0]) + sizeof(u32);
3530
3531         /* start from MAC */
3532         skb_reserve(rxb->skb, (void *)hdr - (void *)pkt);
3533         skb_put(rxb->skb, len); /* end where data ends */
3534
3535         /* We only process data packets if the interface is open */
3536         if (unlikely(!priv->is_open)) {
3537                 IWL_DEBUG_DROP_LIMIT
3538                     ("Dropping packet while interface is not open.\n");
3539                 return;
3540         }
3541
3542         if (priv->iw_mode == IEEE80211_IF_TYPE_MNTR) {
3543                 if (iwl4965_param_hwcrypto)
3544                         iwl4965_set_decrypted_flag(priv, rxb->skb,
3545                                                ampdu_status, stats);
3546                 iwl4965_handle_data_packet_monitor(priv, rxb, hdr, len, stats, 0);
3547                 return;
3548         }
3549
3550         stats->flag = 0;
3551         hdr = (struct ieee80211_hdr *)rxb->skb->data;
3552
3553         if (iwl4965_param_hwcrypto)
3554                 iwl4965_set_decrypted_flag(priv, rxb->skb, ampdu_status, stats);
3555
3556         ieee80211_rx_irqsafe(priv->hw, rxb->skb, stats);
3557         priv->alloc_rxb_skb--;
3558         rxb->skb = NULL;
3559 #ifdef LED
3560         priv->led_packets += len;
3561         iwl4965_setup_activity_timer(priv);
3562 #endif
3563 }
3564
3565 /* Calc max signal level (dBm) among 3 possible receivers */
3566 static int iwl4965_calc_rssi(struct iwl4965_rx_phy_res *rx_resp)
3567 {
3568         /* data from PHY/DSP regarding signal strength, etc.,
3569          *   contents are always there, not configurable by host.  */
3570         struct iwl4965_rx_non_cfg_phy *ncphy =
3571             (struct iwl4965_rx_non_cfg_phy *)rx_resp->non_cfg_phy;
3572         u32 agc = (le16_to_cpu(ncphy->agc_info) & IWL_AGC_DB_MASK)
3573                         >> IWL_AGC_DB_POS;
3574
3575         u32 valid_antennae =
3576             (le16_to_cpu(rx_resp->phy_flags) & RX_PHY_FLAGS_ANTENNAE_MASK)
3577                         >> RX_PHY_FLAGS_ANTENNAE_OFFSET;
3578         u8 max_rssi = 0;
3579         u32 i;
3580
3581         /* Find max rssi among 3 possible receivers.
3582          * These values are measured by the digital signal processor (DSP).
3583          * They should stay fairly constant even as the signal strength varies,
3584          *   if the radio's automatic gain control (AGC) is working right.
3585          * AGC value (see below) will provide the "interesting" info. */
3586         for (i = 0; i < 3; i++)
3587                 if (valid_antennae & (1 << i))
3588                         max_rssi = max(ncphy->rssi_info[i << 1], max_rssi);
3589
3590         IWL_DEBUG_STATS("Rssi In A %d B %d C %d Max %d AGC dB %d\n",
3591                 ncphy->rssi_info[0], ncphy->rssi_info[2], ncphy->rssi_info[4],
3592                 max_rssi, agc);
3593
3594         /* dBm = max_rssi dB - agc dB - constant.
3595          * Higher AGC (higher radio gain) means lower signal. */
3596         return (max_rssi - agc - IWL_RSSI_OFFSET);
3597 }
3598
3599 #ifdef CONFIG_IWL4965_HT
3600
3601 /* Parsed Information Elements */
3602 struct ieee802_11_elems {
3603         u8 *ds_params;
3604         u8 ds_params_len;
3605         u8 *tim;
3606         u8 tim_len;
3607         u8 *ibss_params;
3608         u8 ibss_params_len;
3609         u8 *erp_info;
3610         u8 erp_info_len;
3611         u8 *ht_cap_param;
3612         u8 ht_cap_param_len;
3613         u8 *ht_extra_param;
3614         u8 ht_extra_param_len;
3615 };
3616
3617 static int parse_elems(u8 *start, size_t len, struct ieee802_11_elems *elems)
3618 {
3619         size_t left = len;
3620         u8 *pos = start;
3621         int unknown = 0;
3622
3623         memset(elems, 0, sizeof(*elems));
3624
3625         while (left >= 2) {
3626                 u8 id, elen;
3627
3628                 id = *pos++;
3629                 elen = *pos++;
3630                 left -= 2;
3631
3632                 if (elen > left)
3633                         return -1;
3634
3635                 switch (id) {
3636                 case WLAN_EID_DS_PARAMS:
3637                         elems->ds_params = pos;
3638                         elems->ds_params_len = elen;
3639                         break;
3640                 case WLAN_EID_TIM:
3641                         elems->tim = pos;
3642                         elems->tim_len = elen;
3643                         break;
3644                 case WLAN_EID_IBSS_PARAMS:
3645                         elems->ibss_params = pos;
3646                         elems->ibss_params_len = elen;
3647                         break;
3648                 case WLAN_EID_ERP_INFO:
3649                         elems->erp_info = pos;
3650                         elems->erp_info_len = elen;
3651                         break;
3652                 case WLAN_EID_HT_CAPABILITY:
3653                         elems->ht_cap_param = pos;
3654                         elems->ht_cap_param_len = elen;
3655                         break;
3656                 case WLAN_EID_HT_EXTRA_INFO:
3657                         elems->ht_extra_param = pos;
3658                         elems->ht_extra_param_len = elen;
3659                         break;
3660                 default:
3661                         unknown++;
3662                         break;
3663                 }
3664
3665                 left -= elen;
3666                 pos += elen;
3667         }
3668
3669         return 0;
3670 }
3671 #endif /* CONFIG_IWL4965_HT */
3672
3673 static void iwl4965_sta_modify_ps_wake(struct iwl4965_priv *priv, int sta_id)
3674 {
3675         unsigned long flags;
3676
3677         spin_lock_irqsave(&priv->sta_lock, flags);
3678         priv->stations[sta_id].sta.station_flags &= ~STA_FLG_PWR_SAVE_MSK;
3679         priv->stations[sta_id].sta.station_flags_msk = STA_FLG_PWR_SAVE_MSK;
3680         priv->stations[sta_id].sta.sta.modify_mask = 0;
3681         priv->stations[sta_id].sta.mode = STA_CONTROL_MODIFY_MSK;
3682         spin_unlock_irqrestore(&priv->sta_lock, flags);
3683
3684         iwl4965_send_add_station(priv, &priv->stations[sta_id].sta, CMD_ASYNC);
3685 }
3686
3687 static void iwl4965_update_ps_mode(struct iwl4965_priv *priv, u16 ps_bit, u8 *addr)
3688 {
3689         /* FIXME: need locking over ps_status ??? */
3690         u8 sta_id = iwl4965_hw_find_station(priv, addr);
3691
3692         if (sta_id != IWL_INVALID_STATION) {
3693                 u8 sta_awake = priv->stations[sta_id].
3694                                 ps_status == STA_PS_STATUS_WAKE;
3695
3696                 if (sta_awake && ps_bit)
3697                         priv->stations[sta_id].ps_status = STA_PS_STATUS_SLEEP;
3698                 else if (!sta_awake && !ps_bit) {
3699                         iwl4965_sta_modify_ps_wake(priv, sta_id);
3700                         priv->stations[sta_id].ps_status = STA_PS_STATUS_WAKE;
3701                 }
3702         }
3703 }
3704
3705 /* Called for REPLY_4965_RX (legacy ABG frames), or
3706  * REPLY_RX_MPDU_CMD (HT high-throughput N frames). */
3707 static void iwl4965_rx_reply_rx(struct iwl4965_priv *priv,
3708                                 struct iwl4965_rx_mem_buffer *rxb)
3709 {
3710         struct iwl4965_rx_packet *pkt = (void *)rxb->skb->data;
3711         /* Use phy data (Rx signal strength, etc.) contained within
3712          *   this rx packet for legacy frames,
3713          *   or phy data cached from REPLY_RX_PHY_CMD for HT frames. */
3714         int include_phy = (pkt->hdr.cmd == REPLY_4965_RX);
3715         struct iwl4965_rx_phy_res *rx_start = (include_phy) ?
3716                 (struct iwl4965_rx_phy_res *)&(pkt->u.raw[0]) :
3717                 (struct iwl4965_rx_phy_res *)&priv->last_phy_res[1];
3718         __le32 *rx_end;
3719         unsigned int len = 0;
3720         struct ieee80211_hdr *header;
3721         u16 fc;
3722         struct ieee80211_rx_status stats = {
3723                 .mactime = le64_to_cpu(rx_start->timestamp),
3724                 .channel = le16_to_cpu(rx_start->channel),
3725                 .phymode =
3726                         (rx_start->phy_flags & RX_RES_PHY_FLAGS_BAND_24_MSK) ?
3727                         MODE_IEEE80211G : MODE_IEEE80211A,
3728                 .antenna = 0,
3729                 .rate = iwl4965_hw_get_rate(rx_start->rate_n_flags),
3730                 .flag = 0,
3731 #ifdef CONFIG_IWL4965_HT_AGG
3732                 .ordered = 0
3733 #endif /* CONFIG_IWL4965_HT_AGG */
3734         };
3735         u8 network_packet;
3736
3737         if ((unlikely(rx_start->cfg_phy_cnt > 20))) {
3738                 IWL_DEBUG_DROP
3739                         ("dsp size out of range [0,20]: "
3740                          "%d/n", rx_start->cfg_phy_cnt);
3741                 return;
3742         }
3743         if (!include_phy) {
3744                 if (priv->last_phy_res[0])
3745                         rx_start = (struct iwl4965_rx_phy_res *)
3746                                 &priv->last_phy_res[1];
3747                 else
3748                         rx_start = NULL;
3749         }
3750
3751         if (!rx_start) {
3752                 IWL_ERROR("MPDU frame without a PHY data\n");
3753                 return;
3754         }
3755
3756         if (include_phy) {
3757                 header = (struct ieee80211_hdr *)((u8 *) & rx_start[1]
3758                                                   + rx_start->cfg_phy_cnt);
3759
3760                 len = le16_to_cpu(rx_start->byte_count);
3761                 rx_end = (__le32 *) (pkt->u.raw + rx_start->cfg_phy_cnt +
3762                                   sizeof(struct iwl4965_rx_phy_res) + len);
3763         } else {
3764                 struct iwl4965_rx_mpdu_res_start *amsdu =
3765                         (struct iwl4965_rx_mpdu_res_start *)pkt->u.raw;
3766
3767                 header = (void *)(pkt->u.raw +
3768                         sizeof(struct iwl4965_rx_mpdu_res_start));
3769                 len = le16_to_cpu(amsdu->byte_count);
3770                 rx_end = (__le32 *) (pkt->u.raw +
3771                         sizeof(struct iwl4965_rx_mpdu_res_start) + len);
3772         }
3773
3774         if (!(*rx_end & RX_RES_STATUS_NO_CRC32_ERROR) ||
3775             !(*rx_end & RX_RES_STATUS_NO_RXE_OVERFLOW)) {
3776                 IWL_DEBUG_RX("Bad CRC or FIFO: 0x%08X.\n",
3777                                 le32_to_cpu(*rx_end));
3778                 return;
3779         }
3780
3781         priv->ucode_beacon_time = le32_to_cpu(rx_start->beacon_time_stamp);
3782
3783         stats.freq = ieee80211chan2mhz(stats.channel);
3784
3785         /* Find max signal strength (dBm) among 3 antenna/receiver chains */
3786         stats.ssi = iwl4965_calc_rssi(rx_start);
3787
3788         /* Meaningful noise values are available only from beacon statistics,
3789          *   which are gathered only when associated, and indicate noise
3790          *   only for the associated network channel ...
3791          * Ignore these noise values while scanning (other channels) */
3792         if (iwl4965_is_associated(priv) &&
3793             !test_bit(STATUS_SCANNING, &priv->status)) {
3794                 stats.noise = priv->last_rx_noise;
3795                 stats.signal = iwl4965_calc_sig_qual(stats.ssi, stats.noise);
3796         } else {
3797                 stats.noise = IWL_NOISE_MEAS_NOT_AVAILABLE;
3798                 stats.signal = iwl4965_calc_sig_qual(stats.ssi, 0);
3799         }
3800
3801         /* Reset beacon noise level if not associated. */
3802         if (!iwl4965_is_associated(priv))
3803                 priv->last_rx_noise = IWL_NOISE_MEAS_NOT_AVAILABLE;
3804
3805 #ifdef CONFIG_IWL4965_DEBUG
3806         /* TODO:  Parts of iwl4965_report_frame are broken for 4965 */
3807         if (iwl4965_debug_level & (IWL_DL_RX))
3808                 /* Set "1" to report good data frames in groups of 100 */
3809                 iwl4965_report_frame(priv, pkt, header, 1);
3810
3811         if (iwl4965_debug_level & (IWL_DL_RX | IWL_DL_STATS))
3812         IWL_DEBUG_RX("Rssi %d, noise %d, qual %d, TSF %lu\n",
3813                 stats.ssi, stats.noise, stats.signal,
3814                  (long unsigned int)le64_to_cpu(rx_start->timestamp));
3815 #endif
3816
3817         network_packet = iwl4965_is_network_packet(priv, header);
3818         if (network_packet) {
3819                 priv->last_rx_rssi = stats.ssi;
3820                 priv->last_beacon_time =  priv->ucode_beacon_time;
3821                 priv->last_tsf = le64_to_cpu(rx_start->timestamp);
3822         }
3823
3824         fc = le16_to_cpu(header->frame_control);
3825         switch (fc & IEEE80211_FCTL_FTYPE) {
3826         case IEEE80211_FTYPE_MGMT:
3827
3828                 if (priv->iw_mode == IEEE80211_IF_TYPE_AP)
3829                         iwl4965_update_ps_mode(priv, fc  & IEEE80211_FCTL_PM,
3830                                                 header->addr2);
3831                 switch (fc & IEEE80211_FCTL_STYPE) {
3832                 case IEEE80211_STYPE_PROBE_RESP:
3833                 case IEEE80211_STYPE_BEACON:
3834                         if ((priv->iw_mode == IEEE80211_IF_TYPE_STA &&
3835                              !compare_ether_addr(header->addr2, priv->bssid)) ||
3836                             (priv->iw_mode == IEEE80211_IF_TYPE_IBSS &&
3837                              !compare_ether_addr(header->addr3, priv->bssid))) {
3838                                 struct ieee80211_mgmt *mgmt =
3839                                         (struct ieee80211_mgmt *)header;
3840                                 u64 timestamp =
3841                                         le64_to_cpu(mgmt->u.beacon.timestamp);
3842
3843                                 priv->timestamp0 = timestamp & 0xFFFFFFFF;
3844                                 priv->timestamp1 =
3845                                         (timestamp >> 32) & 0xFFFFFFFF;
3846                                 priv->beacon_int = le16_to_cpu(
3847                                     mgmt->u.beacon.beacon_int);
3848                                 if (priv->call_post_assoc_from_beacon &&
3849                                     (priv->iw_mode == IEEE80211_IF_TYPE_STA)) {
3850                                         priv->call_post_assoc_from_beacon = 0;
3851                                         queue_work(priv->workqueue,
3852                                             &priv->post_associate.work);
3853                                 }
3854                         }
3855                         break;
3856
3857                 case IEEE80211_STYPE_ACTION:
3858                         break;
3859
3860                         /*
3861                          * TODO: There is no callback function from upper
3862                          * stack to inform us when associated status. this
3863                          * work around to sniff assoc_resp management frame
3864                          * and finish the association process.
3865                          */
3866                 case IEEE80211_STYPE_ASSOC_RESP:
3867                 case IEEE80211_STYPE_REASSOC_RESP:
3868                         if (network_packet) {
3869 #ifdef CONFIG_IWL4965_HT
3870                                 u8 *pos = NULL;
3871                                 struct ieee802_11_elems elems;
3872 #endif                          /*CONFIG_IWL4965_HT */
3873                                 struct ieee80211_mgmt *mgnt =
3874                                         (struct ieee80211_mgmt *)header;
3875
3876                                 priv->assoc_id = (~((1 << 15) | (1 << 14))
3877                                         & le16_to_cpu(mgnt->u.assoc_resp.aid));
3878                                 priv->assoc_capability =
3879                                         le16_to_cpu(
3880                                                 mgnt->u.assoc_resp.capab_info);
3881 #ifdef CONFIG_IWL4965_HT
3882                                 pos = mgnt->u.assoc_resp.variable;
3883                                 if (!parse_elems(pos,
3884                                                  len - (pos - (u8 *) mgnt),
3885                                                  &elems)) {
3886                                         if (elems.ht_extra_param &&
3887                                             elems.ht_cap_param)
3888                                                 break;
3889                                 }
3890 #endif                          /*CONFIG_IWL4965_HT */
3891                                 /* assoc_id is 0 no association */
3892                                 if (!priv->assoc_id)
3893                                         break;
3894                                 if (priv->beacon_int)
3895                                         queue_work(priv->workqueue,
3896                                             &priv->post_associate.work);
3897                                 else
3898                                         priv->call_post_assoc_from_beacon = 1;
3899                         }
3900
3901                         break;
3902
3903                 case IEEE80211_STYPE_PROBE_REQ:
3904                         if ((priv->iw_mode == IEEE80211_IF_TYPE_IBSS) &&
3905                             !iwl4965_is_associated(priv)) {
3906                                 DECLARE_MAC_BUF(mac1);
3907                                 DECLARE_MAC_BUF(mac2);
3908                                 DECLARE_MAC_BUF(mac3);
3909
3910                                 IWL_DEBUG_DROP("Dropping (non network): "
3911                                                "%s, %s, %s\n",
3912                                                print_mac(mac1, header->addr1),
3913                                                print_mac(mac2, header->addr2),
3914                                                print_mac(mac3, header->addr3));
3915                                 return;
3916                         }
3917                 }
3918                 iwl4965_handle_data_packet(priv, 0, include_phy, rxb, &stats);
3919                 break;
3920
3921         case IEEE80211_FTYPE_CTL:
3922 #ifdef CONFIG_IWL4965_HT_AGG
3923                 switch (fc & IEEE80211_FCTL_STYPE) {
3924                 case IEEE80211_STYPE_BACK_REQ:
3925                         IWL_DEBUG_HT("IEEE80211_STYPE_BACK_REQ arrived\n");
3926                         iwl4965_handle_data_packet(priv, 0, include_phy,
3927                                                 rxb, &stats);
3928                         break;
3929                 default:
3930                         break;
3931                 }
3932 #endif
3933
3934                 break;
3935
3936         case IEEE80211_FTYPE_DATA: {
3937                 DECLARE_MAC_BUF(mac1);
3938                 DECLARE_MAC_BUF(mac2);
3939                 DECLARE_MAC_BUF(mac3);
3940
3941                 if (priv->iw_mode == IEEE80211_IF_TYPE_AP)
3942                         iwl4965_update_ps_mode(priv, fc  & IEEE80211_FCTL_PM,
3943                                                 header->addr2);
3944
3945                 if (unlikely(!network_packet))
3946                         IWL_DEBUG_DROP("Dropping (non network): "
3947                                        "%s, %s, %s\n",
3948                                        print_mac(mac1, header->addr1),
3949                                        print_mac(mac2, header->addr2),
3950                                        print_mac(mac3, header->addr3));
3951                 else if (unlikely(iwl4965_is_duplicate_packet(priv, header)))
3952                         IWL_DEBUG_DROP("Dropping (dup): %s, %s, %s\n",
3953                                        print_mac(mac1, header->addr1),
3954                                        print_mac(mac2, header->addr2),
3955                                        print_mac(mac3, header->addr3));
3956                 else
3957                         iwl4965_handle_data_packet(priv, 1, include_phy, rxb,
3958                                                    &stats);
3959                 break;
3960         }
3961         default:
3962                 break;
3963
3964         }
3965 }
3966
3967 /* Cache phy data (Rx signal strength, etc) for HT frame (REPLY_RX_PHY_CMD).
3968  * This will be used later in iwl4965_rx_reply_rx() for REPLY_RX_MPDU_CMD. */
3969 static void iwl4965_rx_reply_rx_phy(struct iwl4965_priv *priv,
3970                                     struct iwl4965_rx_mem_buffer *rxb)
3971 {
3972         struct iwl4965_rx_packet *pkt = (void *)rxb->skb->data;
3973         priv->last_phy_res[0] = 1;
3974         memcpy(&priv->last_phy_res[1], &(pkt->u.raw[0]),
3975                sizeof(struct iwl4965_rx_phy_res));
3976 }
3977
3978 static void iwl4965_rx_missed_beacon_notif(struct iwl4965_priv *priv,
3979                                            struct iwl4965_rx_mem_buffer *rxb)
3980
3981 {
3982 #ifdef CONFIG_IWL4965_SENSITIVITY
3983         struct iwl4965_rx_packet *pkt = (void *)rxb->skb->data;
3984         struct iwl4965_missed_beacon_notif *missed_beacon;
3985
3986         missed_beacon = &pkt->u.missed_beacon;
3987         if (le32_to_cpu(missed_beacon->consequtive_missed_beacons) > 5) {
3988                 IWL_DEBUG_CALIB("missed bcn cnsq %d totl %d rcd %d expctd %d\n",
3989                     le32_to_cpu(missed_beacon->consequtive_missed_beacons),
3990                     le32_to_cpu(missed_beacon->total_missed_becons),
3991                     le32_to_cpu(missed_beacon->num_recvd_beacons),
3992                     le32_to_cpu(missed_beacon->num_expected_beacons));
3993                 priv->sensitivity_data.state = IWL_SENS_CALIB_NEED_REINIT;
3994                 if (unlikely(!test_bit(STATUS_SCANNING, &priv->status)))
3995                         queue_work(priv->workqueue, &priv->sensitivity_work);
3996         }
3997 #endif /*CONFIG_IWL4965_SENSITIVITY*/
3998 }
3999
4000 #ifdef CONFIG_IWL4965_HT
4001 #ifdef CONFIG_IWL4965_HT_AGG
4002
4003 static void iwl4965_set_tx_status(struct iwl4965_priv *priv, int txq_id, int idx,
4004                                   u32 status, u32 retry_count, u32 rate)
4005 {
4006         struct ieee80211_tx_status *tx_status =
4007                 &(priv->txq[txq_id].txb[idx].status);
4008
4009         tx_status->flags = status ? IEEE80211_TX_STATUS_ACK : 0;
4010         tx_status->retry_count += retry_count;
4011         tx_status->control.tx_rate = rate;
4012 }
4013
4014
4015 static void iwl4965_sta_modify_enable_tid_tx(struct iwl4965_priv *priv,
4016                                          int sta_id, int tid)
4017 {
4018         unsigned long flags;
4019
4020         spin_lock_irqsave(&priv->sta_lock, flags);
4021         priv->stations[sta_id].sta.sta.modify_mask = STA_MODIFY_TID_DISABLE_TX;
4022         priv->stations[sta_id].sta.tid_disable_tx &= cpu_to_le16(~(1 << tid));
4023         priv->stations[sta_id].sta.mode = STA_CONTROL_MODIFY_MSK;
4024         spin_unlock_irqrestore(&priv->sta_lock, flags);
4025
4026         iwl4965_send_add_station(priv, &priv->stations[sta_id].sta, CMD_ASYNC);
4027 }
4028
4029
4030 static int iwl4965_tx_status_reply_compressed_ba(struct iwl4965_priv *priv,
4031                                                  struct iwl4965_ht_agg *agg,
4032                                                  struct iwl4965_compressed_ba_resp*
4033                                                  ba_resp)
4034
4035 {
4036         int i, sh, ack;
4037         u16 ba_seq_ctl = le16_to_cpu(ba_resp->ba_seq_ctl);
4038         u32 bitmap0, bitmap1;
4039         u32 resp_bitmap0 = le32_to_cpu(ba_resp->ba_bitmap0);
4040         u32 resp_bitmap1 = le32_to_cpu(ba_resp->ba_bitmap1);
4041
4042         if (unlikely(!agg->wait_for_ba))  {
4043                 IWL_ERROR("Received BA when not expected\n");
4044                 return -EINVAL;
4045         }
4046         agg->wait_for_ba = 0;
4047         IWL_DEBUG_TX_REPLY("BA %d %d\n", agg->start_idx, ba_resp->ba_seq_ctl);
4048         sh = agg->start_idx - SEQ_TO_INDEX(ba_seq_ctl>>4);
4049         if (sh < 0) /* tbw something is wrong with indices */
4050                 sh += 0x100;
4051
4052         /* don't use 64 bits for now */
4053         bitmap0 = resp_bitmap0 >> sh;
4054         bitmap1 = resp_bitmap1 >> sh;
4055         bitmap0 |= (resp_bitmap1 & ((1<<sh)|((1<<sh)-1))) << (32 - sh);
4056
4057         if (agg->frame_count > (64 - sh)) {
4058                 IWL_DEBUG_TX_REPLY("more frames than bitmap size");
4059                 return -1;
4060         }
4061
4062         /* check for success or failure according to the
4063          * transmitted bitmap and back bitmap */
4064         bitmap0 &= agg->bitmap0;
4065         bitmap1 &= agg->bitmap1;
4066
4067         for (i = 0; i < agg->frame_count ; i++) {
4068                 int idx = (agg->start_idx + i) & 0xff;
4069                 ack = bitmap0 & (1 << i);
4070                 IWL_DEBUG_TX_REPLY("%s ON i=%d idx=%d raw=%d\n",
4071                         ack? "ACK":"NACK", i, idx, agg->start_idx + i);
4072                 iwl4965_set_tx_status(priv, agg->txq_id, idx, ack, 0,
4073                         agg->rate_n_flags);
4074
4075         }
4076
4077         IWL_DEBUG_TX_REPLY("Bitmap %x%x\n", bitmap0, bitmap1);
4078
4079         return 0;
4080 }
4081
4082 static inline int iwl4965_queue_dec_wrap(int index, int n_bd)
4083 {
4084         return (index == 0) ? n_bd - 1 : index - 1;
4085 }
4086
4087 static void iwl4965_rx_reply_compressed_ba(struct iwl4965_priv *priv,
4088                                            struct iwl4965_rx_mem_buffer *rxb)
4089 {
4090         struct iwl4965_rx_packet *pkt = (void *)rxb->skb->data;
4091         struct iwl4965_compressed_ba_resp *ba_resp = &pkt->u.compressed_ba;
4092         int index;
4093         struct iwl4965_tx_queue *txq = NULL;
4094         struct iwl4965_ht_agg *agg;
4095         u16 ba_resp_scd_flow = le16_to_cpu(ba_resp->scd_flow);
4096         u16 ba_resp_scd_ssn = le16_to_cpu(ba_resp->scd_ssn);
4097
4098         if (ba_resp_scd_flow >= ARRAY_SIZE(priv->txq)) {
4099                 IWL_ERROR("BUG_ON scd_flow is bigger than number of queues");
4100                 return;
4101         }
4102
4103         txq = &priv->txq[ba_resp_scd_flow];
4104         agg = &priv->stations[ba_resp->sta_id].tid[ba_resp->tid].agg;
4105         index = iwl4965_queue_dec_wrap(ba_resp_scd_ssn & 0xff, txq->q.n_bd);
4106
4107         /* TODO: Need to get this copy more safely - now good for debug */
4108 /*
4109         {
4110         DECLARE_MAC_BUF(mac);
4111         IWL_DEBUG_TX_REPLY("REPLY_COMPRESSED_BA [%d]Received from %s, "
4112                            "sta_id = %d\n",
4113                            agg->wait_for_ba,
4114                            print_mac(mac, (u8*) &ba_resp->sta_addr_lo32),
4115                            ba_resp->sta_id);
4116         IWL_DEBUG_TX_REPLY("TID = %d, SeqCtl = %d, bitmap = 0x%X%X, scd_flow = "
4117                            "%d, scd_ssn = %d\n",
4118                            ba_resp->tid,
4119                            ba_resp->ba_seq_ctl,
4120                            ba_resp->ba_bitmap1,
4121                            ba_resp->ba_bitmap0,
4122                            ba_resp->scd_flow,
4123                            ba_resp->scd_ssn);
4124         IWL_DEBUG_TX_REPLY("DAT start_idx = %d, bitmap = 0x%X%X \n",
4125                            agg->start_idx,
4126                            agg->bitmap1,
4127                            agg->bitmap0);
4128         }
4129 */
4130         iwl4965_tx_status_reply_compressed_ba(priv, agg, ba_resp);
4131         /* releases all the TFDs until the SSN */
4132         if (txq->q.read_ptr != (ba_resp_scd_ssn & 0xff))
4133                 iwl4965_tx_queue_reclaim(priv, ba_resp_scd_flow, index);
4134
4135 }
4136
4137
4138 static void iwl4965_tx_queue_stop_scheduler(struct iwl4965_priv *priv, u16 txq_id)
4139 {
4140         iwl4965_write_prph(priv,
4141                 KDR_SCD_QUEUE_STATUS_BITS(txq_id),
4142                 (0 << SCD_QUEUE_STTS_REG_POS_ACTIVE)|
4143                 (1 << SCD_QUEUE_STTS_REG_POS_SCD_ACT_EN));
4144 }
4145
4146 static int iwl4965_tx_queue_set_q2ratid(struct iwl4965_priv *priv, u16 ra_tid,
4147                                         u16 txq_id)
4148 {
4149         u32 tbl_dw_addr;
4150         u32 tbl_dw;
4151         u16 scd_q2ratid;
4152
4153         scd_q2ratid = ra_tid & SCD_QUEUE_RA_TID_MAP_RATID_MSK;
4154
4155         tbl_dw_addr = priv->scd_base_addr +
4156                         SCD_TRANSLATE_TBL_OFFSET_QUEUE(txq_id);
4157
4158         tbl_dw = iwl4965_read_targ_mem(priv, tbl_dw_addr);
4159
4160         if (txq_id & 0x1)
4161                 tbl_dw = (scd_q2ratid << 16) | (tbl_dw & 0x0000FFFF);
4162         else
4163                 tbl_dw = scd_q2ratid | (tbl_dw & 0xFFFF0000);
4164
4165         iwl4965_write_targ_mem(priv, tbl_dw_addr, tbl_dw);
4166
4167         return 0;
4168 }
4169
4170 /**
4171  * txq_id must be greater than IWL_BACK_QUEUE_FIRST_ID
4172  */
4173 static int iwl4965_tx_queue_agg_enable(struct iwl4965_priv *priv, int txq_id,
4174                                        int tx_fifo, int sta_id, int tid,
4175                                        u16 ssn_idx)
4176 {
4177         unsigned long flags;
4178         int rc;
4179         u16 ra_tid;
4180
4181         if (IWL_BACK_QUEUE_FIRST_ID > txq_id)
4182                 IWL_WARNING("queue number too small: %d, must be > %d\n",
4183                         txq_id, IWL_BACK_QUEUE_FIRST_ID);
4184
4185         ra_tid = BUILD_RAxTID(sta_id, tid);
4186
4187         iwl4965_sta_modify_enable_tid_tx(priv, sta_id, tid);
4188
4189         spin_lock_irqsave(&priv->lock, flags);
4190         rc = iwl4965_grab_nic_access(priv);
4191         if (rc) {
4192                 spin_unlock_irqrestore(&priv->lock, flags);
4193                 return rc;
4194         }
4195
4196         iwl4965_tx_queue_stop_scheduler(priv, txq_id);
4197
4198         iwl4965_tx_queue_set_q2ratid(priv, ra_tid, txq_id);
4199
4200
4201         iwl4965_set_bits_prph(priv, KDR_SCD_QUEUECHAIN_SEL, (1<<txq_id));
4202
4203         priv->txq[txq_id].q.read_ptr = (ssn_idx & 0xff);
4204         priv->txq[txq_id].q.write_ptr = (ssn_idx & 0xff);
4205
4206         /* supposes that ssn_idx is valid (!= 0xFFF) */
4207         iwl4965_set_wr_ptrs(priv, txq_id, ssn_idx);
4208
4209         iwl4965_write_targ_mem(priv,
4210                         priv->scd_base_addr + SCD_CONTEXT_QUEUE_OFFSET(txq_id),
4211                         (SCD_WIN_SIZE << SCD_QUEUE_CTX_REG1_WIN_SIZE_POS) &
4212                         SCD_QUEUE_CTX_REG1_WIN_SIZE_MSK);
4213
4214         iwl4965_write_targ_mem(priv, priv->scd_base_addr +
4215                         SCD_CONTEXT_QUEUE_OFFSET(txq_id) + sizeof(u32),
4216                         (SCD_FRAME_LIMIT << SCD_QUEUE_CTX_REG2_FRAME_LIMIT_POS)
4217                         & SCD_QUEUE_CTX_REG2_FRAME_LIMIT_MSK);
4218
4219         iwl4965_set_bits_prph(priv, KDR_SCD_INTERRUPT_MASK, (1 << txq_id));
4220
4221         iwl4965_tx_queue_set_status(priv, &priv->txq[txq_id], tx_fifo, 1);
4222
4223         iwl4965_release_nic_access(priv);
4224         spin_unlock_irqrestore(&priv->lock, flags);
4225
4226         return 0;
4227 }
4228
4229 /**
4230  * txq_id must be greater than IWL_BACK_QUEUE_FIRST_ID
4231  */
4232 static int iwl4965_tx_queue_agg_disable(struct iwl4965_priv *priv, u16 txq_id,
4233                                         u16 ssn_idx, u8 tx_fifo)
4234 {
4235         unsigned long flags;
4236         int rc;
4237
4238         if (IWL_BACK_QUEUE_FIRST_ID > txq_id) {
4239                 IWL_WARNING("queue number too small: %d, must be > %d\n",
4240                                 txq_id, IWL_BACK_QUEUE_FIRST_ID);
4241                 return -EINVAL;
4242         }
4243
4244         spin_lock_irqsave(&priv->lock, flags);
4245         rc = iwl4965_grab_nic_access(priv);
4246         if (rc) {
4247                 spin_unlock_irqrestore(&priv->lock, flags);
4248                 return rc;
4249         }
4250
4251         iwl4965_tx_queue_stop_scheduler(priv, txq_id);
4252
4253         iwl4965_clear_bits_prph(priv, KDR_SCD_QUEUECHAIN_SEL, (1 << txq_id));
4254
4255         priv->txq[txq_id].q.read_ptr = (ssn_idx & 0xff);
4256         priv->txq[txq_id].q.write_ptr = (ssn_idx & 0xff);
4257         /* supposes that ssn_idx is valid (!= 0xFFF) */
4258         iwl4965_set_wr_ptrs(priv, txq_id, ssn_idx);
4259
4260         iwl4965_clear_bits_prph(priv, KDR_SCD_INTERRUPT_MASK, (1 << txq_id));
4261         iwl4965_txq_ctx_deactivate(priv, txq_id);
4262         iwl4965_tx_queue_set_status(priv, &priv->txq[txq_id], tx_fifo, 0);
4263
4264         iwl4965_release_nic_access(priv);
4265         spin_unlock_irqrestore(&priv->lock, flags);
4266
4267         return 0;
4268 }
4269
4270 #endif/* CONFIG_IWL4965_HT_AGG */
4271 #endif /* CONFIG_IWL4965_HT */
4272 /*
4273  * RATE SCALE CODE
4274  */
4275 int iwl4965_init_hw_rates(struct iwl4965_priv *priv, struct ieee80211_rate *rates)
4276 {
4277         return 0;
4278 }
4279
4280
4281 /**
4282  * iwl4965_add_station - Initialize a station's hardware rate table
4283  *
4284  * The uCode contains a table of fallback rates and retries per rate
4285  * for automatic fallback during transmission.
4286  *
4287  * NOTE: This initializes the table for a single retry per data rate
4288  * which is not optimal.  Setting up an intelligent retry per rate
4289  * requires feedback from transmission, which isn't exposed through
4290  * rc80211_simple which is what this driver is currently using.
4291  *
4292  */
4293 void iwl4965_add_station(struct iwl4965_priv *priv, const u8 *addr, int is_ap)
4294 {
4295         int i, r;
4296         struct iwl4965_link_quality_cmd link_cmd = {
4297                 .reserved1 = 0,
4298         };
4299         u16 rate_flags;
4300
4301         /* Set up the rate scaling to start at 54M and fallback
4302          * all the way to 1M in IEEE order and then spin on IEEE */
4303         if (is_ap)
4304                 r = IWL_RATE_54M_INDEX;
4305         else if (priv->phymode == MODE_IEEE80211A)
4306                 r = IWL_RATE_6M_INDEX;
4307         else
4308                 r = IWL_RATE_1M_INDEX;
4309
4310         for (i = 0; i < LINK_QUAL_MAX_RETRY_NUM; i++) {
4311                 rate_flags = 0;
4312                 if (r >= IWL_FIRST_CCK_RATE && r <= IWL_LAST_CCK_RATE)
4313                         rate_flags |= RATE_MCS_CCK_MSK;
4314
4315                 rate_flags |= RATE_MCS_ANT_B_MSK;
4316                 rate_flags &= ~RATE_MCS_ANT_A_MSK;
4317                 link_cmd.rs_table[i].rate_n_flags =
4318                         iwl4965_hw_set_rate_n_flags(iwl4965_rates[r].plcp, rate_flags);
4319                 r = iwl4965_get_prev_ieee_rate(r);
4320         }
4321
4322         link_cmd.general_params.single_stream_ant_msk = 2;
4323         link_cmd.general_params.dual_stream_ant_msk = 3;
4324         link_cmd.agg_params.agg_dis_start_th = 3;
4325         link_cmd.agg_params.agg_time_limit = cpu_to_le16(4000);
4326
4327         /* Update the rate scaling for control frame Tx to AP */
4328         link_cmd.sta_id = is_ap ? IWL_AP_ID : IWL4965_BROADCAST_ID;
4329
4330         iwl4965_send_cmd_pdu(priv, REPLY_TX_LINK_QUALITY_CMD, sizeof(link_cmd),
4331                          &link_cmd);
4332 }
4333
4334 #ifdef CONFIG_IWL4965_HT
4335
4336 static u8 iwl4965_is_channel_extension(struct iwl4965_priv *priv, int phymode,
4337                                    u16 channel, u8 extension_chan_offset)
4338 {
4339         const struct iwl4965_channel_info *ch_info;
4340
4341         ch_info = iwl4965_get_channel_info(priv, phymode, channel);
4342         if (!is_channel_valid(ch_info))
4343                 return 0;
4344
4345         if (extension_chan_offset == IWL_EXT_CHANNEL_OFFSET_AUTO)
4346                 return 0;
4347
4348         if ((ch_info->fat_extension_channel == extension_chan_offset) ||
4349             (ch_info->fat_extension_channel == HT_IE_EXT_CHANNEL_MAX))
4350                 return 1;
4351
4352         return 0;
4353 }
4354
4355 static u8 iwl4965_is_fat_tx_allowed(struct iwl4965_priv *priv,
4356                                 const struct sta_ht_info *ht_info)
4357 {
4358
4359         if (priv->channel_width != IWL_CHANNEL_WIDTH_40MHZ)
4360                 return 0;
4361
4362         if (ht_info->supported_chan_width != IWL_CHANNEL_WIDTH_40MHZ)
4363                 return 0;
4364
4365         if (ht_info->extension_chan_offset == IWL_EXT_CHANNEL_OFFSET_AUTO)
4366                 return 0;
4367
4368         /* no fat tx allowed on 2.4GHZ */
4369         if (priv->phymode != MODE_IEEE80211A)
4370                 return 0;
4371         return (iwl4965_is_channel_extension(priv, priv->phymode,
4372                                          ht_info->control_channel,
4373                                          ht_info->extension_chan_offset));
4374 }
4375
4376 void iwl4965_set_rxon_ht(struct iwl4965_priv *priv, struct sta_ht_info *ht_info)
4377 {
4378         struct iwl4965_rxon_cmd *rxon = &priv->staging_rxon;
4379         u32 val;
4380
4381         if (!ht_info->is_ht)
4382                 return;
4383
4384         if (iwl4965_is_fat_tx_allowed(priv, ht_info))
4385                 rxon->flags |= RXON_FLG_CHANNEL_MODE_MIXED_MSK;
4386         else
4387                 rxon->flags &= ~(RXON_FLG_CHANNEL_MODE_MIXED_MSK |
4388                                  RXON_FLG_CHANNEL_MODE_PURE_40_MSK);
4389
4390         if (le16_to_cpu(rxon->channel) != ht_info->control_channel) {
4391                 IWL_DEBUG_ASSOC("control diff than current %d %d\n",
4392                                 le16_to_cpu(rxon->channel),
4393                                 ht_info->control_channel);
4394                 rxon->channel = cpu_to_le16(ht_info->control_channel);
4395                 return;
4396         }
4397
4398         /* Note: control channel is oposit to extension channel */
4399         switch (ht_info->extension_chan_offset) {
4400         case IWL_EXT_CHANNEL_OFFSET_ABOVE:
4401                 rxon->flags &= ~(RXON_FLG_CTRL_CHANNEL_LOC_HI_MSK);
4402                 break;
4403         case IWL_EXT_CHANNEL_OFFSET_BELOW:
4404                 rxon->flags |= RXON_FLG_CTRL_CHANNEL_LOC_HI_MSK;
4405                 break;
4406         case IWL_EXT_CHANNEL_OFFSET_AUTO:
4407                 rxon->flags &= ~RXON_FLG_CHANNEL_MODE_MIXED_MSK;
4408                 break;
4409         default:
4410                 rxon->flags &= ~RXON_FLG_CHANNEL_MODE_MIXED_MSK;
4411                 break;
4412         }
4413
4414         val = ht_info->operating_mode;
4415
4416         rxon->flags |= cpu_to_le32(val << RXON_FLG_HT_OPERATING_MODE_POS);
4417
4418         priv->active_rate_ht[0] = ht_info->supp_rates[0];
4419         priv->active_rate_ht[1] = ht_info->supp_rates[1];
4420         iwl4965_set_rxon_chain(priv);
4421
4422         IWL_DEBUG_ASSOC("supported HT rate 0x%X %X "
4423                         "rxon flags 0x%X operation mode :0x%X "
4424                         "extension channel offset 0x%x "
4425                         "control chan %d\n",
4426                         priv->active_rate_ht[0], priv->active_rate_ht[1],
4427                         le32_to_cpu(rxon->flags), ht_info->operating_mode,
4428                         ht_info->extension_chan_offset,
4429                         ht_info->control_channel);
4430         return;
4431 }
4432
4433 void iwl4965_set_ht_add_station(struct iwl4965_priv *priv, u8 index)
4434 {
4435         __le32 sta_flags;
4436         struct sta_ht_info *ht_info = &priv->current_assoc_ht;
4437
4438         priv->current_channel_width = IWL_CHANNEL_WIDTH_20MHZ;
4439         if (!ht_info->is_ht)
4440                 goto done;
4441
4442         sta_flags = priv->stations[index].sta.station_flags;
4443
4444         if (ht_info->tx_mimo_ps_mode == IWL_MIMO_PS_DYNAMIC)
4445                 sta_flags |= STA_FLG_RTS_MIMO_PROT_MSK;
4446         else
4447                 sta_flags &= ~STA_FLG_RTS_MIMO_PROT_MSK;
4448
4449         sta_flags |= cpu_to_le32(
4450                 (u32)ht_info->ampdu_factor << STA_FLG_MAX_AGG_SIZE_POS);
4451
4452         sta_flags |= cpu_to_le32(
4453                 (u32)ht_info->mpdu_density << STA_FLG_AGG_MPDU_DENSITY_POS);
4454
4455         sta_flags &= (~STA_FLG_FAT_EN_MSK);
4456         ht_info->tx_chan_width = IWL_CHANNEL_WIDTH_20MHZ;
4457         ht_info->chan_width_cap = IWL_CHANNEL_WIDTH_20MHZ;
4458
4459         if (iwl4965_is_fat_tx_allowed(priv, ht_info)) {
4460                 sta_flags |= STA_FLG_FAT_EN_MSK;
4461                 ht_info->chan_width_cap = IWL_CHANNEL_WIDTH_40MHZ;
4462                 if (ht_info->supported_chan_width == IWL_CHANNEL_WIDTH_40MHZ)
4463                         ht_info->tx_chan_width = IWL_CHANNEL_WIDTH_40MHZ;
4464         }
4465         priv->current_channel_width = ht_info->tx_chan_width;
4466         priv->stations[index].sta.station_flags = sta_flags;
4467  done:
4468         return;
4469 }
4470
4471 #ifdef CONFIG_IWL4965_HT_AGG
4472
4473 static void iwl4965_sta_modify_add_ba_tid(struct iwl4965_priv *priv,
4474                                           int sta_id, int tid, u16 ssn)
4475 {
4476         unsigned long flags;
4477
4478         spin_lock_irqsave(&priv->sta_lock, flags);
4479         priv->stations[sta_id].sta.station_flags_msk = 0;
4480         priv->stations[sta_id].sta.sta.modify_mask = STA_MODIFY_ADDBA_TID_MSK;
4481         priv->stations[sta_id].sta.add_immediate_ba_tid = (u8)tid;
4482         priv->stations[sta_id].sta.add_immediate_ba_ssn = cpu_to_le16(ssn);
4483         priv->stations[sta_id].sta.mode = STA_CONTROL_MODIFY_MSK;
4484         spin_unlock_irqrestore(&priv->sta_lock, flags);
4485
4486         iwl4965_send_add_station(priv, &priv->stations[sta_id].sta, CMD_ASYNC);
4487 }
4488
4489 static void iwl4965_sta_modify_del_ba_tid(struct iwl4965_priv *priv,
4490                                           int sta_id, int tid)
4491 {
4492         unsigned long flags;
4493
4494         spin_lock_irqsave(&priv->sta_lock, flags);
4495         priv->stations[sta_id].sta.station_flags_msk = 0;
4496         priv->stations[sta_id].sta.sta.modify_mask = STA_MODIFY_DELBA_TID_MSK;
4497         priv->stations[sta_id].sta.remove_immediate_ba_tid = (u8)tid;
4498         priv->stations[sta_id].sta.mode = STA_CONTROL_MODIFY_MSK;
4499         spin_unlock_irqrestore(&priv->sta_lock, flags);
4500
4501         iwl4965_send_add_station(priv, &priv->stations[sta_id].sta, CMD_ASYNC);
4502 }
4503
4504 static const u16 default_tid_to_tx_fifo[] = {
4505         IWL_TX_FIFO_AC1,
4506         IWL_TX_FIFO_AC0,
4507         IWL_TX_FIFO_AC0,
4508         IWL_TX_FIFO_AC1,
4509         IWL_TX_FIFO_AC2,
4510         IWL_TX_FIFO_AC2,
4511         IWL_TX_FIFO_AC3,
4512         IWL_TX_FIFO_AC3,
4513         IWL_TX_FIFO_NONE,
4514         IWL_TX_FIFO_NONE,
4515         IWL_TX_FIFO_NONE,
4516         IWL_TX_FIFO_NONE,
4517         IWL_TX_FIFO_NONE,
4518         IWL_TX_FIFO_NONE,
4519         IWL_TX_FIFO_NONE,
4520         IWL_TX_FIFO_NONE,
4521         IWL_TX_FIFO_AC3
4522 };
4523
4524 static int iwl4965_txq_ctx_activate_free(struct iwl4965_priv *priv)
4525 {
4526         int txq_id;
4527
4528         for (txq_id = 0; txq_id < priv->hw_setting.max_txq_num; txq_id++)
4529                 if (!test_and_set_bit(txq_id, &priv->txq_ctx_active_msk))
4530                         return txq_id;
4531         return -1;
4532 }
4533
4534 int iwl4965_mac_ht_tx_agg_start(struct ieee80211_hw *hw, u8 *da, u16 tid,
4535                             u16 *start_seq_num)
4536 {
4537
4538         struct iwl4965_priv *priv = hw->priv;
4539         int sta_id;
4540         int tx_fifo;
4541         int txq_id;
4542         int ssn = -1;
4543         unsigned long flags;
4544         struct iwl4965_tid_data *tid_data;
4545         DECLARE_MAC_BUF(mac);
4546
4547         if (likely(tid < ARRAY_SIZE(default_tid_to_tx_fifo)))
4548                 tx_fifo = default_tid_to_tx_fifo[tid];
4549         else
4550                 return -EINVAL;
4551
4552         IWL_WARNING("iwl-AGG iwl4965_mac_ht_tx_agg_start on da=%s"
4553                     " tid=%d\n", print_mac(mac, da), tid);
4554
4555         sta_id = iwl4965_hw_find_station(priv, da);
4556         if (sta_id == IWL_INVALID_STATION)
4557                 return -ENXIO;
4558
4559         txq_id = iwl4965_txq_ctx_activate_free(priv);
4560         if (txq_id == -1)
4561                 return -ENXIO;
4562
4563         spin_lock_irqsave(&priv->sta_lock, flags);
4564         tid_data = &priv->stations[sta_id].tid[tid];
4565         ssn = SEQ_TO_SN(tid_data->seq_number);
4566         tid_data->agg.txq_id = txq_id;
4567         spin_unlock_irqrestore(&priv->sta_lock, flags);
4568
4569         *start_seq_num = ssn;
4570         iwl4965_ba_status(priv, tid, BA_STATUS_ACTIVE);
4571         return iwl4965_tx_queue_agg_enable(priv, txq_id, tx_fifo,
4572                                            sta_id, tid, ssn);
4573 }
4574
4575
4576 int iwl4965_mac_ht_tx_agg_stop(struct ieee80211_hw *hw, u8 *da, u16 tid,
4577                            int generator)
4578 {
4579
4580         struct iwl4965_priv *priv = hw->priv;
4581         int tx_fifo_id, txq_id, sta_id, ssn = -1;
4582         struct iwl4965_tid_data *tid_data;
4583         int rc;
4584         DECLARE_MAC_BUF(mac);
4585
4586         if (!da) {
4587                 IWL_ERROR("%s: da = NULL\n", __func__);
4588                 return -EINVAL;
4589         }
4590
4591         if (likely(tid < ARRAY_SIZE(default_tid_to_tx_fifo)))
4592                 tx_fifo_id = default_tid_to_tx_fifo[tid];
4593         else
4594                 return -EINVAL;
4595
4596         sta_id = iwl4965_hw_find_station(priv, da);
4597
4598         if (sta_id == IWL_INVALID_STATION)
4599                 return -ENXIO;
4600
4601         tid_data = &priv->stations[sta_id].tid[tid];
4602         ssn = (tid_data->seq_number & IEEE80211_SCTL_SEQ) >> 4;
4603         txq_id = tid_data->agg.txq_id;
4604
4605         rc = iwl4965_tx_queue_agg_disable(priv, txq_id, ssn, tx_fifo_id);
4606         /* FIXME: need more safe way to handle error condition */
4607         if (rc)
4608                 return rc;
4609
4610         iwl4965_ba_status(priv, tid, BA_STATUS_INITIATOR_DELBA);
4611         IWL_DEBUG_INFO("iwl4965_mac_ht_tx_agg_stop on da=%s tid=%d\n",
4612                        print_mac(mac, da), tid);
4613
4614         return 0;
4615 }
4616
4617 int iwl4965_mac_ht_rx_agg_start(struct ieee80211_hw *hw, u8 *da,
4618                             u16 tid, u16 start_seq_num)
4619 {
4620         struct iwl4965_priv *priv = hw->priv;
4621         int sta_id;
4622         DECLARE_MAC_BUF(mac);
4623
4624         IWL_WARNING("iwl-AGG iwl4965_mac_ht_rx_agg_start on da=%s"
4625                     " tid=%d\n", print_mac(mac, da), tid);
4626         sta_id = iwl4965_hw_find_station(priv, da);
4627         iwl4965_sta_modify_add_ba_tid(priv, sta_id, tid, start_seq_num);
4628         return 0;
4629 }
4630
4631 int iwl4965_mac_ht_rx_agg_stop(struct ieee80211_hw *hw, u8 *da,
4632                            u16 tid, int generator)
4633 {
4634         struct iwl4965_priv *priv = hw->priv;
4635         int sta_id;
4636         DECLARE_MAC_BUF(mac);
4637
4638         IWL_WARNING("iwl-AGG iwl4965_mac_ht_rx_agg_stop on da=%s tid=%d\n",
4639                     print_mac(mac, da), tid);
4640         sta_id = iwl4965_hw_find_station(priv, da);
4641         iwl4965_sta_modify_del_ba_tid(priv, sta_id, tid);
4642         return 0;
4643 }
4644
4645 #endif /* CONFIG_IWL4965_HT_AGG */
4646 #endif /* CONFIG_IWL4965_HT */
4647
4648 /* Set up 4965-specific Rx frame reply handlers */
4649 void iwl4965_hw_rx_handler_setup(struct iwl4965_priv *priv)
4650 {
4651         /* Legacy Rx frames */
4652         priv->rx_handlers[REPLY_4965_RX] = iwl4965_rx_reply_rx;
4653
4654         /* High-throughput (HT) Rx frames */
4655         priv->rx_handlers[REPLY_RX_PHY_CMD] = iwl4965_rx_reply_rx_phy;
4656         priv->rx_handlers[REPLY_RX_MPDU_CMD] = iwl4965_rx_reply_rx;
4657
4658         priv->rx_handlers[MISSED_BEACONS_NOTIFICATION] =
4659             iwl4965_rx_missed_beacon_notif;
4660
4661 #ifdef CONFIG_IWL4965_HT
4662 #ifdef CONFIG_IWL4965_HT_AGG
4663         priv->rx_handlers[REPLY_COMPRESSED_BA] = iwl4965_rx_reply_compressed_ba;
4664 #endif /* CONFIG_IWL4965_HT_AGG */
4665 #endif /* CONFIG_IWL4965_HT */
4666 }
4667
4668 void iwl4965_hw_setup_deferred_work(struct iwl4965_priv *priv)
4669 {
4670         INIT_WORK(&priv->txpower_work, iwl4965_bg_txpower_work);
4671         INIT_WORK(&priv->statistics_work, iwl4965_bg_statistics_work);
4672 #ifdef CONFIG_IWL4965_SENSITIVITY
4673         INIT_WORK(&priv->sensitivity_work, iwl4965_bg_sensitivity_work);
4674 #endif
4675 #ifdef CONFIG_IWL4965_HT
4676 #ifdef CONFIG_IWL4965_HT_AGG
4677         INIT_WORK(&priv->agg_work, iwl4965_bg_agg_work);
4678 #endif /* CONFIG_IWL4965_AGG */
4679 #endif /* CONFIG_IWL4965_HT */
4680         init_timer(&priv->statistics_periodic);
4681         priv->statistics_periodic.data = (unsigned long)priv;
4682         priv->statistics_periodic.function = iwl4965_bg_statistics_periodic;
4683 }
4684
4685 void iwl4965_hw_cancel_deferred_work(struct iwl4965_priv *priv)
4686 {
4687         del_timer_sync(&priv->statistics_periodic);
4688
4689         cancel_delayed_work(&priv->init_alive_start);
4690 }
4691
4692 struct pci_device_id iwl4965_hw_card_ids[] = {
4693         {PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0x4229)},
4694         {PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0x4230)},
4695         {0}
4696 };
4697
4698 int iwl4965_eeprom_acquire_semaphore(struct iwl4965_priv *priv)
4699 {
4700         u16 count;
4701         int rc;
4702
4703         for (count = 0; count < EEPROM_SEM_RETRY_LIMIT; count++) {
4704                 iwl4965_set_bit(priv, CSR_HW_IF_CONFIG_REG,
4705                         CSR_HW_IF_CONFIG_REG_BIT_EEPROM_OWN_SEM);
4706                 rc = iwl4965_poll_bit(priv, CSR_HW_IF_CONFIG_REG,
4707                                         CSR_HW_IF_CONFIG_REG_BIT_EEPROM_OWN_SEM,
4708                                         CSR_HW_IF_CONFIG_REG_BIT_EEPROM_OWN_SEM,
4709                                         EEPROM_SEM_TIMEOUT);
4710                 if (rc >= 0) {
4711                         IWL_DEBUG_IO("Acquired semaphore after %d tries.\n",
4712                                 count+1);
4713                         return rc;
4714                 }
4715         }
4716
4717         return rc;
4718 }
4719
4720 inline void iwl4965_eeprom_release_semaphore(struct iwl4965_priv *priv)
4721 {
4722         iwl4965_clear_bit(priv, CSR_HW_IF_CONFIG_REG,
4723                 CSR_HW_IF_CONFIG_REG_BIT_EEPROM_OWN_SEM);
4724 }
4725
4726
4727 MODULE_DEVICE_TABLE(pci, iwl4965_hw_card_ids);