iwlwifi: cleanup namespace
[linux-2.6.git] / 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 <