Merge branch 'master' of git://git.kernel.org/pub/scm/linux/kernel/git/davem/net-2.6
David S. Miller [Thu, 15 May 2008 07:34:44 +0000 (00:34 -0700)]
Conflicts:

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

18 files changed:
1  2 
drivers/net/dm9000.c
drivers/net/gianfar.c
drivers/net/niu.h
drivers/net/pcnet32.c
drivers/net/ucc_geth_ethtool.c
drivers/net/wireless/iwlwifi/Kconfig
drivers/net/wireless/iwlwifi/iwl-3945.c
drivers/net/wireless/iwlwifi/iwl-4965-rs.c
drivers/net/wireless/iwlwifi/iwl-4965.c
drivers/net/wireless/rt2x00/rt2x00dev.c
drivers/net/wireless/rt2x00/rt2x00pci.c
drivers/net/wireless/rt2x00/rt61pci.c
net/mac80211/iface.c
net/mac80211/main.c
net/mac80211/mlme.c
net/mac80211/rx.c
net/mac80211/wme.c
net/tipc/core.h

diff --combined drivers/net/dm9000.c
@@@ -117,6 -117,9 +117,9 @@@ typedef struct board_info 
  
        struct mutex     addr_lock;     /* phy and eeprom access lock */
  
+       struct delayed_work phy_poll;
+       struct net_device  *ndev;
        spinlock_t lock;
  
        struct mii_if_info mii;
@@@ -297,6 -300,10 +300,10 @@@ static void dm9000_set_io(struct board_
        }
  }
  
+ static void dm9000_schedule_poll(board_info_t *db)
+ {
+       schedule_delayed_work(&db->phy_poll, HZ * 2);
+ }
  
  /* Our watchdog timed out. Called by the networking layer */
  static void dm9000_timeout(struct net_device *dev)
@@@ -465,6 -472,17 +472,17 @@@ static const struct ethtool_ops dm9000_
        .set_eeprom             = dm9000_set_eeprom,
  };
  
+ static void
+ dm9000_poll_work(struct work_struct *w)
+ {
+       struct delayed_work *dw = container_of(w, struct delayed_work, work);
+       board_info_t *db = container_of(dw, board_info_t, phy_poll);
+       mii_check_media(&db->mii, netif_msg_link(db), 0);
+       
+       if (netif_running(db->ndev))
+               dm9000_schedule_poll(db);
+ }
  
  /* dm9000_release_board
   *
@@@ -503,7 -521,7 +521,7 @@@ dm9000_release_board(struct platform_de
  /*
   * Search DM9000 board, allocate space and register it
   */
- static int
+ static int __devinit
  dm9000_probe(struct platform_device *pdev)
  {
        struct dm9000_plat_data *pdata = pdev->dev.platform_data;
  
        SET_NETDEV_DEV(ndev, &pdev->dev);
  
-       dev_dbg(&pdev->dev, "dm9000_probe()");
+       dev_dbg(&pdev->dev, "dm9000_probe()\n");
  
        /* setup board info structure */
        db = (struct board_info *) ndev->priv;
        memset(db, 0, sizeof (*db));
  
        db->dev = &pdev->dev;
+       db->ndev = ndev;
  
        spin_lock_init(&db->lock);
        mutex_init(&db->addr_lock);
  
+       INIT_DELAYED_WORK(&db->phy_poll, dm9000_poll_work);
        if (pdev->num_resources < 2) {
                ret = -ENODEV;
                goto out;
  
        if (!is_valid_ether_addr(ndev->dev_addr)) {
                /* try reading from mac */
 -              
 +
                mac_src = "chip";
                for (i = 0; i < 6; i++)
                        ndev->dev_addr[i] = ior(db, i+DM9000_PAR);
@@@ -746,7 -768,7 +768,7 @@@ dm9000_open(struct net_device *dev
                dev_warn(db->dev, "WARNING: no IRQ resource flags set.\n");
                irqflags = DEFAULT_TRIGGER;
        }
 -      
 +
        irqflags |= IRQF_SHARED;
  
        if (request_irq(dev->irq, &dm9000_interrupt, irqflags, dev->name, dev))
  
        mii_check_media(&db->mii, netif_msg_link(db), 1);
        netif_start_queue(dev);
+       
+       dm9000_schedule_poll(db);
  
        return 0;
  }
@@@ -879,6 -903,8 +903,8 @@@ dm9000_stop(struct net_device *ndev
        if (netif_msg_ifdown(db))
                dev_dbg(db->dev, "shutting down %s\n", ndev->name);
  
+       cancel_delayed_work(&db->phy_poll);
        netif_stop_queue(ndev);
        netif_carrier_off(ndev);
  
@@@ -1089,7 -1115,7 +1115,7 @@@ static int dm9000_wait_eeprom(board_inf
        /* The DM9000 data sheets say we should be able to
         * poll the ERRE bit in EPCR to wait for the EEPROM
         * operation. From testing several chips, this bit
 -       * does not seem to work. 
 +       * does not seem to work.
         *
         * We attempt to use the bit, but fall back to the
         * timeout (which is why we do not return an error
@@@ -1288,6 -1314,8 +1314,8 @@@ dm9000_phy_read(struct net_device *dev
        spin_unlock_irqrestore(&db->lock,flags);
  
        mutex_unlock(&db->addr_lock);
+       dm9000_dbg(db, 5, "phy_read[%02x] -> %04x\n", reg, ret);
        return ret;
  }
  
@@@ -1301,6 -1329,7 +1329,7 @@@ dm9000_phy_write(struct net_device *dev
        unsigned long flags;
        unsigned long reg_save;
  
+       dm9000_dbg(db, 5, "phy_write[%02x] = %04x\n", reg, value);
        mutex_lock(&db->addr_lock);
  
        spin_lock_irqsave(&db->lock,flags);
@@@ -1372,7 -1401,7 +1401,7 @@@ dm9000_drv_resume(struct platform_devic
        return 0;
  }
  
- static int
+ static int __devexit
  dm9000_drv_remove(struct platform_device *pdev)
  {
        struct net_device *ndev = platform_get_drvdata(pdev);
@@@ -1393,7 -1422,7 +1422,7 @@@ static struct platform_driver dm9000_dr
                .owner   = THIS_MODULE,
        },
        .probe   = dm9000_probe,
-       .remove  = dm9000_drv_remove,
+       .remove  = __devexit_p(dm9000_drv_remove),
        .suspend = dm9000_drv_suspend,
        .resume  = dm9000_drv_resume,
  };
diff --combined drivers/net/gianfar.c
@@@ -138,6 -138,7 +138,7 @@@ static int gfar_poll(struct napi_struc
  static void gfar_netpoll(struct net_device *dev);
  #endif
  int gfar_clean_rx_ring(struct net_device *dev, int rx_work_limit);
+ static int gfar_clean_tx_ring(struct net_device *dev);
  static int gfar_process_frame(struct net_device *dev, struct sk_buff *skb, int length);
  static void gfar_vlan_rx_register(struct net_device *netdev,
                                struct vlan_group *grp);
@@@ -634,6 -635,8 +635,8 @@@ static void free_skb_resources(struct g
                        dev_kfree_skb_any(priv->tx_skbuff[i]);
                        priv->tx_skbuff[i] = NULL;
                }
+               txbdp++;
        }
  
        kfree(priv->tx_skbuff);
@@@ -925,7 -928,7 +928,7 @@@ rx_irq_fail
  tx_irq_fail:
        free_irq(priv->interruptError, dev);
  err_irq_fail:
 -err_rxalloc_fail:     
 +err_rxalloc_fail:
  rx_skb_fail:
        free_skb_resources(priv);
  tx_skb_fail:
@@@ -1141,7 -1144,7 +1144,7 @@@ static int gfar_close(struct net_devic
  }
  
  /* Changes the mac address if the controller is not running. */
- int gfar_set_mac_address(struct net_device *dev)
+ static int gfar_set_mac_address(struct net_device *dev)
  {
        gfar_set_mac_for_addr(dev, 0, dev->dev_addr);
  
@@@ -1260,7 -1263,7 +1263,7 @@@ static void gfar_timeout(struct net_dev
  }
  
  /* Interrupt Handler for Transmit complete */
- int gfar_clean_tx_ring(struct net_device *dev)
+ static int gfar_clean_tx_ring(struct net_device *dev)
  {
        struct txbd8 *bdp;
        struct gfar_private *priv = netdev_priv(dev);
diff --combined drivers/net/niu.h
  #define XMAC_ADDR1                    0x000a8UL
  #define  XMAC_ADDR1_ADDR1             0x000000000000ffffULL
  
 -#define XMAC_ADDR2                    0x000b0UL 
 +#define XMAC_ADDR2                    0x000b0UL
  #define  XMAC_ADDR2_ADDR2             0x000000000000ffffULL
  
  #define XMAC_ADDR_CMPEN                       0x00208UL
@@@ -2946,6 -2946,15 +2946,15 @@@ struct rx_ring_info 
  #define       NIU_ALONSO_BM_STR       "373-0202"
  #define       NIU_FOXXY_BM_STR        "501-7961"
  #define       NIU_2XGF_MRVL_BM_STR    "SK-6E82"
+ #define       NIU_QGC_LP_MDL_STR      "SUNW,pcie-qgc"
+ #define       NIU_2XGF_LP_MDL_STR     "SUNW,pcie-2xgf"
+ #define       NIU_QGC_PEM_MDL_STR     "SUNW,pcie-qgc-pem"
+ #define       NIU_2XGF_PEM_MDL_STR    "SUNW,pcie-2xgf-pem"
+ #define       NIU_ALONSO_MDL_STR      "SUNW,CP3220"
+ #define       NIU_KIMI_MDL_STR        "SUNW,CP3260"
+ #define       NIU_MARAMBA_MDL_STR     "SUNW,pcie-neptune"
+ #define       NIU_FOXXY_MDL_STR       "SUNW,pcie-rfem"
+ #define       NIU_2XGF_MRVL_MDL_STR   "SysKonnect,pcie-2xgf"
  
  #define NIU_VPD_MIN_MAJOR     3
  #define NIU_VPD_MIN_MINOR     4
diff --combined drivers/net/pcnet32.c
   *************************************************************************/
  
  #define DRV_NAME      "pcnet32"
- #ifdef CONFIG_PCNET32_NAPI
- #define DRV_VERSION   "1.34-NAPI"
- #else
- #define DRV_VERSION   "1.34"
- #endif
- #define DRV_RELDATE   "14.Aug.2007"
+ #define DRV_VERSION   "1.35"
+ #define DRV_RELDATE   "21.Apr.2008"
  #define PFX           DRV_NAME ": "
  
  static const char *const version =
@@@ -445,30 -441,24 +441,24 @@@ static struct pcnet32_access pcnet32_dw
  
  static void pcnet32_netif_stop(struct net_device *dev)
  {
- #ifdef CONFIG_PCNET32_NAPI
        struct pcnet32_private *lp = netdev_priv(dev);
- #endif
        dev->trans_start = jiffies;
- #ifdef CONFIG_PCNET32_NAPI
        napi_disable(&lp->napi);
- #endif
        netif_tx_disable(dev);
  }
  
  static void pcnet32_netif_start(struct net_device *dev)
  {
        struct pcnet32_private *lp = netdev_priv(dev);
        ulong ioaddr = dev->base_addr;
        u16 val;
- #endif
        netif_wake_queue(dev);
- #ifdef CONFIG_PCNET32_NAPI
        val = lp->a.read_csr(ioaddr, CSR3);
        val &= 0x00ff;
        lp->a.write_csr(ioaddr, CSR3, val);
        napi_enable(&lp->napi);
- #endif
  }
  
  /*
@@@ -911,11 -901,7 +901,7 @@@ static int pcnet32_loopback_test(struc
        rc = 1;                 /* default to fail */
  
        if (netif_running(dev))
- #ifdef CONFIG_PCNET32_NAPI
                pcnet32_netif_stop(dev);
- #else
-               pcnet32_close(dev);
- #endif
  
        spin_lock_irqsave(&lp->lock, flags);
        lp->a.write_csr(ioaddr, CSR0, CSR0_STOP);       /* stop the chip */
        x = a->read_bcr(ioaddr, 32);    /* reset internal loopback */
        a->write_bcr(ioaddr, 32, (x & ~0x0002));
  
- #ifdef CONFIG_PCNET32_NAPI
        if (netif_running(dev)) {
                pcnet32_netif_start(dev);
                pcnet32_restart(dev, CSR0_NORMAL);
                lp->a.write_bcr(ioaddr, 20, 4); /* return to 16bit mode */
        }
        spin_unlock_irqrestore(&lp->lock, flags);
- #else
-       if (netif_running(dev)) {
-               spin_unlock_irqrestore(&lp->lock, flags);
-               pcnet32_open(dev);
-       } else {
-               pcnet32_purge_rx_ring(dev);
-               lp->a.write_bcr(ioaddr, 20, 4); /* return to 16bit mode */
-               spin_unlock_irqrestore(&lp->lock, flags);
-       }
- #endif
  
        return (rc);
  }                             /* end pcnet32_loopback_test  */
@@@ -1270,11 -1245,7 +1245,7 @@@ static void pcnet32_rx_entry(struct net
        }
        dev->stats.rx_bytes += skb->len;
        skb->protocol = eth_type_trans(skb, dev);
- #ifdef CONFIG_PCNET32_NAPI
        netif_receive_skb(skb);
- #else
-       netif_rx(skb);
- #endif
        dev->last_rx = jiffies;
        dev->stats.rx_packets++;
        return;
@@@ -1403,7 -1374,6 +1374,6 @@@ static int pcnet32_tx(struct net_devic
        return must_restart;
  }
  
- #ifdef CONFIG_PCNET32_NAPI
  static int pcnet32_poll(struct napi_struct *napi, int budget)
  {
        struct pcnet32_private *lp = container_of(napi, struct pcnet32_private, napi);
        }
        return work_done;
  }
- #endif
  
  #define PCNET32_REGS_PER_PHY  32
  #define PCNET32_MAX_PHYS      32
@@@ -1864,9 -1833,7 +1833,7 @@@ pcnet32_probe1(unsigned long ioaddr, in
        /* napi.weight is used in both the napi and non-napi cases */
        lp->napi.weight = lp->rx_ring_size / 2;
  
- #ifdef CONFIG_PCNET32_NAPI
        netif_napi_add(dev, &lp->napi, pcnet32_poll, lp->rx_ring_size / 2);
- #endif
  
        if (fdx && !(lp->options & PCNET32_PORT_ASEL) &&
            ((cards_found >= MAX_UNITS) || full_duplex[cards_found]))
        err_free_ring:
        pcnet32_free_ring(dev);
        err_free_consistent:
 -      pci_free_consistent(lp->pci_dev, sizeof(*lp->init_block), 
 +      pci_free_consistent(lp->pci_dev, sizeof(*lp->init_block),
                            lp->init_block, lp->init_dma_addr);
        err_free_netdev:
        free_netdev(dev);
@@@ -2297,9 -2264,7 +2264,7 @@@ static int pcnet32_open(struct net_devi
                goto err_free_ring;
        }
  
- #ifdef CONFIG_PCNET32_NAPI
        napi_enable(&lp->napi);
- #endif
  
        /* Re-initialize the PCNET32, and start it when done. */
        lp->a.write_csr(ioaddr, 1, (lp->init_dma_addr & 0xffff));
@@@ -2623,7 -2588,6 +2588,6 @@@ pcnet32_interrupt(int irq, void *dev_id
                                       dev->name, csr0);
                        /* unlike for the lance, there is no restart needed */
                }
- #ifdef CONFIG_PCNET32_NAPI
                if (netif_rx_schedule_prep(dev, &lp->napi)) {
                        u16 val;
                        /* set interrupt masks */
                        __netif_rx_schedule(dev, &lp->napi);
                        break;
                }
                csr0 = lp->a.read_csr(ioaddr, CSR0);
        }
  
- #ifndef CONFIG_PCNET32_NAPI
-       /* Set interrupt enable. */
-       lp->a.write_csr(ioaddr, CSR0, CSR0_INTEN);
- #endif
        if (netif_msg_intr(lp))
                printk(KERN_DEBUG "%s: exiting interrupt, csr0=%#4.4x.\n",
                       dev->name, lp->a.read_csr(ioaddr, CSR0));
@@@ -2670,9 -2619,7 +2619,7 @@@ static int pcnet32_close(struct net_dev
        del_timer_sync(&lp->watchdog_timer);
  
        netif_stop_queue(dev);
- #ifdef CONFIG_PCNET32_NAPI
        napi_disable(&lp->napi);
- #endif
  
        spin_lock_irqsave(&lp->lock, flags);
  
@@@ -3006,7 -2953,7 +2953,7 @@@ static void __devexit pcnet32_remove_on
                unregister_netdev(dev);
                pcnet32_free_ring(dev);
                release_region(dev->base_addr, PCNET32_TOTAL_SIZE);
 -              pci_free_consistent(lp->pci_dev, sizeof(*lp->init_block), 
 +              pci_free_consistent(lp->pci_dev, sizeof(*lp->init_block),
                                    lp->init_block, lp->init_dma_addr);
                free_netdev(dev);
                pci_disable_device(pdev);
@@@ -3089,7 -3036,7 +3036,7 @@@ static void __exit pcnet32_cleanup_modu
                unregister_netdev(pcnet32_dev);
                pcnet32_free_ring(pcnet32_dev);
                release_region(pcnet32_dev->base_addr, PCNET32_TOTAL_SIZE);
 -              pci_free_consistent(lp->pci_dev, sizeof(*lp->init_block), 
 +              pci_free_consistent(lp->pci_dev, sizeof(*lp->init_block),
                                    lp->init_block, lp->init_dma_addr);
                free_netdev(pcnet32_dev);
                pcnet32_dev = next_dev;
@@@ -5,7 -5,7 +5,7 @@@
   *
   * Author: Li Yang <leoli@freescale.com>
   *
 - * Limitation: 
 + * Limitation:
   * Can only get/set setttings of the first queue.
   * Need to re-open the interface manually after changing some paramters.
   *
@@@ -108,12 -108,6 +108,6 @@@ static char rx_fw_stat_gstrings[][ETH_G
  #define UEC_TX_FW_STATS_LEN ARRAY_SIZE(tx_fw_stat_gstrings)
  #define UEC_RX_FW_STATS_LEN ARRAY_SIZE(rx_fw_stat_gstrings)
  
- extern int init_flow_control_params(u32 automatic_flow_control_mode,
-               int rx_flow_control_enable,
-               int tx_flow_control_enable, u16 pause_period,
-               u16 extension_field, volatile u32 *upsmr_register,
-               volatile u32 *uempr_register, volatile u32 *maccfg1_register);
  static int
  uec_get_settings(struct net_device *netdev, struct ethtool_cmd *ecmd)
  {
@@@ -165,7 -159,7 +159,7 @@@ uec_set_pauseparam(struct net_device *n
  
        ugeth->ug_info->receiveFlowControl = pause->rx_pause;
        ugeth->ug_info->transmitFlowControl = pause->tx_pause;
 -      
 +
        if (ugeth->phydev->autoneg) {
                if (netif_running(netdev)) {
                        /* FIXME: automatically restart */
@@@ -1,6 -1,5 +1,5 @@@
  config IWLWIFI
-       bool
-       default n
+       tristate
  
  config IWLCORE
        tristate "Intel Wireless Wifi Core"
@@@ -15,15 -14,6 +14,15 @@@ config IWLWIFI_LED
        bool
        default n
  
 +config IWLWIFI_RUN_TIME_CALIB
 +      bool
 +      depends on IWLCORE
 +      default n
 +      ---help---
 +        This option will enable run time calibration for the iwlwifi driver.
 +        These calibrations are Sensitivity and Chain Noise.
 +
 +
  config IWLWIFI_RFKILL
        boolean "IWLWIFI RF kill support"
        depends on IWLCORE
@@@ -77,14 -67,12 +76,14 @@@ config IWL4965_SPECTRUM_MEASUREMEN
        ---help---
          This option will enable spectrum measurement for the iwl4965 driver.
  
 -config IWL4965_SENSITIVITY
 -      bool "Enable Sensitivity Calibration in iwl4965 driver"
 +config IWL4965_RUN_TIME_CALIB
 +      bool "Enable run time Calibration for 4965 NIC"
 +      select IWLWIFI_RUN_TIME_CALIB
        depends on IWL4965
 +      default y
        ---help---
 -        This option will enable sensitivity calibration for the iwl4965
 -        driver.
 +        This option will enable run time calibration for the iwl4965 driver.
 +        These calibrations are Sensitivity and Chain Noise. If unsure, say yes
  
  config IWLWIFI_DEBUG
        bool "Enable full debugging output in iwl4965 driver"
          as the debug information can assist others in helping you resolve
          any problems you may encounter.
  
 +config IWL5000
 +      bool "Intel Wireless WiFi 5000AGN"
 +      depends on IWL4965
 +      ---help---
 +        This option enables support for Intel Wireless WiFi Link 5000AGN Family
 +        Dependency on 4965 is temporary
 +
 +config IWL5000_RUN_TIME_CALIB
 +      bool "Enable run time Calibration for 5000 NIC"
 +      select IWLWIFI_RUN_TIME_CALIB
 +      depends on IWL5000
 +      default y
 +      ---help---
 +        This option will enable run time calibration for the iwl5000 driver.
 +        These calibrations are Sensitivity and Chain Noise. If unsure, say yes
 +
 +
  config IWLWIFI_DEBUGFS
          bool "Iwlwifi debugfs support"
          depends on IWLCORE && IWLWIFI_DEBUG && MAC80211_DEBUGFS
@@@ -666,7 -666,7 +666,7 @@@ static void iwl3945_rx_reply_rx(struct 
        rx_status.flag = 0;
        rx_status.mactime = le64_to_cpu(rx_end->timestamp);
        rx_status.freq =
-               ieee80211_frequency_to_channel(le16_to_cpu(rx_hdr->channel));
+               ieee80211_channel_to_frequency(le16_to_cpu(rx_hdr->channel));
        rx_status.band = (rx_hdr->phy_flags & RX_RES_PHY_FLAGS_BAND_24_MSK) ?
                                IEEE80211_BAND_2GHZ : IEEE80211_BAND_5GHZ;
  
@@@ -1229,7 -1229,7 +1229,7 @@@ int iwl3945_hw_nic_init(struct iwl3945_
        iwl3945_power_init_handle(priv);
  
        spin_lock_irqsave(&priv->lock, flags);
 -      iwl3945_set_bit(priv, CSR_ANA_PLL_CFG, (1 << 24));
 +      iwl3945_set_bit(priv, CSR_ANA_PLL_CFG, CSR39_ANA_PLL_CFG_VAL);
        iwl3945_set_bit(priv, CSR_GIO_CHICKEN_BITS,
                    CSR_GIO_CHICKEN_BITS_REG_BIT_L1A_NO_L0S_RX);
  
@@@ -28,6 -28,7 +28,6 @@@
  #include <linux/skbuff.h>
  #include <linux/wireless.h>
  #include <net/mac80211.h>
 -#include <net/ieee80211.h>
  
  #include <linux/netdevice.h>
  #include <linux/etherdevice.h>
  
  #include "../net/mac80211/rate.h"
  
 -#include "iwl-4965.h"
 +#include "iwl-dev.h"
  #include "iwl-core.h"
  #include "iwl-helpers.h"
  
  #define RS_NAME "iwl-4965-rs"
  
 -#define NUM_TRY_BEFORE_ANTENNA_TOGGLE 1
 +#define NUM_TRY_BEFORE_ANT_TOGGLE 1
  #define IWL_NUMBER_TRY      1
  #define IWL_HT_NUMBER_TRY   3
  
@@@ -64,16 -65,9 +64,16 @@@ static u8 rs_ht_to_legacy[] = 
        IWL_RATE_48M_INDEX, IWL_RATE_54M_INDEX
  };
  
 -struct iwl4965_rate {
 -      u32 rate_n_flags;
 -} __attribute__ ((packed));
 +static const u8 ant_toggle_lookup[] = {
 +      /*ANT_NONE -> */ ANT_NONE,
 +      /*ANT_A    -> */ ANT_B,
 +      /*ANT_B    -> */ ANT_C,
 +      /*ANT_AB   -> */ ANT_BC,
 +      /*ANT_C    -> */ ANT_A,
 +      /*ANT_AC   -> */ ANT_AB,
 +      /*ANT_BC   -> */ ANT_AC,
 +      /*ANT_ABC  -> */ ANT_ABC,
 +};
  
  /**
   * struct iwl4965_rate_scale_data -- tx success history for one rate
@@@ -94,14 -88,14 +94,14 @@@ struct iwl4965_rate_scale_data 
   * one for "active", and one for "search".
   */
  struct iwl4965_scale_tbl_info {
 -      enum iwl4965_table_type lq_type;
 -      enum iwl4965_antenna_type antenna_type;
 +      enum iwl_table_type lq_type;
 +      u8 ant_type;
        u8 is_SGI;      /* 1 = short guard interval */
        u8 is_fat;      /* 1 = 40 MHz channel width */
        u8 is_dup;      /* 1 = duplicated data streams */
        u8 action;      /* change modulation; IWL_[LEGACY/SISO/MIMO]_SWITCH_* */
        s32 *expected_tpt;      /* throughput metrics; expected_tpt_G, etc. */
 -      struct iwl4965_rate current_rate;  /* rate_n_flags, uCode API format */
 +      u32 current_rate;  /* rate_n_flags, uCode API format */
        struct iwl4965_rate_scale_data win[IWL_RATE_COUNT]; /* rate histories */
  };
  
@@@ -142,6 -136,8 +142,6 @@@ struct iwl4965_lq_sta 
        u32 flush_timer;        /* time staying in mode before new search */
  
        u8 action_counter;      /* # mode-switch actions tried */
 -      u8 antenna;
 -      u8 valid_antenna;
        u8 is_green;
        u8 is_dup;
        enum ieee80211_band band;
  
        /* The following are bitmaps of rates; IWL_RATE_6M_MASK, etc. */
        u32 supp_rates;
 -      u16 active_rate;
 +      u16 active_legacy_rate;
        u16 active_siso_rate;
 -      u16 active_mimo_rate;
 +      u16 active_mimo2_rate;
 +      u16 active_mimo3_rate;
        u16 active_rate_basic;
  
        struct iwl_link_quality_cmd lq;
  #ifdef CONFIG_IWL4965_HT
        struct dentry *rs_sta_dbgfs_tx_agg_tid_en_file;
  #endif
 -      struct iwl4965_rate dbg_fixed;
 +      u32 dbg_fixed_rate;
-       struct iwl_priv *drv;
  #endif
+       struct iwl_priv *drv;
  };
  
  static void rs_rate_scale_perform(struct iwl_priv *priv,
                                   struct net_device *dev,
                                   struct ieee80211_hdr *hdr,
                                   struct sta_info *sta);
 -static void rs_fill_link_cmd(struct iwl4965_lq_sta *lq_sta,
 -                           struct iwl4965_rate *tx_mcs,
 -                           struct iwl_link_quality_cmd *tbl);
 +static void rs_fill_link_cmd(const struct iwl_priv *priv,
 +                           struct iwl4965_lq_sta *lq_sta,
 +                           u32 rate_n_flags);
  
  
  #ifdef CONFIG_MAC80211_DEBUGFS
  static void rs_dbgfs_set_mcs(struct iwl4965_lq_sta *lq_sta,
 -                              struct iwl4965_rate *mcs, int index);
 +                                      u32 *rate_n_flags, int index);
  #else
  static void rs_dbgfs_set_mcs(struct iwl4965_lq_sta *lq_sta,
 -                              struct iwl4965_rate *mcs, int index)
 +                                      u32 *rate_n_flags, int index)
  {}
  #endif
  
   * 1, 2, 5.5, 11, 6, 9, 12, 18, 24, 36, 48, 54, 60 MBits
   * "G" is the only table that supports CCK (the first 4 rates).
   */
 +/*FIXME:RS:need to spearate tables for MIMO2/MIMO3*/
  static s32 expected_tpt_A[IWL_RATE_COUNT] = {
        0, 0, 0, 0, 40, 57, 72, 98, 121, 154, 177, 186, 186
  };
@@@ -236,7 -230,7 +236,7 @@@ static s32 expected_tpt_mimo40MHzSGI[IW
        0, 0, 0, 0, 131, 131, 191, 222, 242, 270, 284, 289, 293
  };
  
 -static inline u8 iwl4965_rate_get_rate(u32 rate_n_flags)
 +static inline u8 rs_extract_rate(u32 rate_n_flags)
  {
        return (u8)(rate_n_flags & 0xFF);
  }
@@@ -251,11 -245,6 +251,11 @@@ static void rs_rate_scale_clear_window(
        window->stamp = 0;
  }
  
 +static inline u8 rs_is_valid_ant(u8 valid_antenna, u8 ant_type)
 +{
 +      return ((ant_type & valid_antenna) == ant_type);
 +}
 +
  #ifdef CONFIG_IWL4965_HT
  /*
   *    removes the old data from the statistics. All data that is older than
@@@ -385,13 -374,6 +385,13 @@@ static void rs_tl_turn_on_agg(struct iw
  
  #endif /* CONFIG_IWLWIFI_HT */
  
 +static inline int get_num_of_ant_from_rate(u32 rate_n_flags)
 +{
 +      return (!!(rate_n_flags & RATE_MCS_ANT_A_MSK) +
 +              !!(rate_n_flags & RATE_MCS_ANT_B_MSK) +
 +              !!(rate_n_flags & RATE_MCS_ANT_C_MSK));
 +}
 +
  /**
   * rs_collect_tx_data - Update the success/failure sliding window
   *
@@@ -404,7 -386,8 +404,7 @@@ static int rs_collect_tx_data(struct iw
                              int successes)
  {
        struct iwl4965_rate_scale_data *window = NULL;
 -      u64 mask;
 -      u8 win_size = IWL_RATE_MAX_WINDOW;
 +      static const u64 mask = (((u64)1) << (IWL_RATE_MAX_WINDOW - 1));
        s32 fail_count;
  
        if (scale_index < 0 || scale_index >= IWL_RATE_COUNT)
         * we keep these bitmaps!).
         */
        while (retries > 0) {
 -              if (window->counter >= win_size) {
 -                      window->counter = win_size - 1;
 -                      mask = 1;
 -                      mask = (mask << (win_size - 1));
 +              if (window->counter >= IWL_RATE_MAX_WINDOW) {
 +
 +                      /* remove earliest */
 +                      window->counter = IWL_RATE_MAX_WINDOW - 1;
 +
                        if (window->data & mask) {
                                window->data &= ~mask;
 -                              window->success_counter =
 -                                      window->success_counter - 1;
 +                              window->success_counter--;
                        }
                }
  
                /* Shift bitmap by one frame (throw away oldest history),
                 * OR in "1", and increment "success" if this
                 * frame was successful. */
 -              mask = window->data;
 -              window->data = (mask << 1);
 +              window->data <<= 1;;
                if (successes > 0) {
 -                      window->success_counter = window->success_counter + 1;
 +                      window->success_counter++;
                        window->data |= 0x1;
                        successes--;
                }
  /*
   * Fill uCode API rate_n_flags field, based on "search" or "active" table.
   */
 -static void rs_mcs_from_tbl(struct iwl4965_rate *mcs_rate,
 -                         struct iwl4965_scale_tbl_info *tbl,
 -                         int index, u8 use_green)
 +/* FIXME:RS:remove this function and put the flags statically in the table */
 +static u32 rate_n_flags_from_tbl(struct iwl4965_scale_tbl_info *tbl,
 +                                     int index, u8 use_green)
  {
 +      u32 rate_n_flags = 0;
 +
        if (is_legacy(tbl->lq_type)) {
 -              mcs_rate->rate_n_flags = iwl4965_rates[index].plcp;
 +              rate_n_flags = iwl4965_rates[index].plcp;
                if (index >= IWL_FIRST_CCK_RATE && index <= IWL_LAST_CCK_RATE)
 -                      mcs_rate->rate_n_flags |= RATE_MCS_CCK_MSK;
 +                      rate_n_flags |= RATE_MCS_CCK_MSK;
  
 -      } else if (is_siso(tbl->lq_type)) {
 -              if (index > IWL_LAST_OFDM_RATE)
 -                      index = IWL_LAST_OFDM_RATE;
 -               mcs_rate->rate_n_flags = iwl4965_rates[index].plcp_siso |
 -                                        RATE_MCS_HT_MSK;
 -      } else {
 -              if (index > IWL_LAST_OFDM_RATE)
 +      } else if (is_Ht(tbl->lq_type)) {
 +              if (index > IWL_LAST_OFDM_RATE) {
 +                      IWL_ERROR("invalid HT rate index %d\n", index);
                        index = IWL_LAST_OFDM_RATE;
 -              mcs_rate->rate_n_flags = iwl4965_rates[index].plcp_mimo |
 -                                       RATE_MCS_HT_MSK;
 -      }
 -
 -      switch (tbl->antenna_type) {
 -      case ANT_BOTH:
 -              mcs_rate->rate_n_flags |= RATE_MCS_ANT_AB_MSK;
 -              break;
 -      case ANT_MAIN:
 -              mcs_rate->rate_n_flags |= RATE_MCS_ANT_A_MSK;
 -              break;
 -      case ANT_AUX:
 -              mcs_rate->rate_n_flags |= RATE_MCS_ANT_B_MSK;
 -              break;
 -      case ANT_NONE:
 -              break;
 -      }
 -
 -      if (is_legacy(tbl->lq_type))
 -              return;
 +              }
 +              rate_n_flags = RATE_MCS_HT_MSK;
  
 -      if (tbl->is_fat) {
 -              if (tbl->is_dup)
 -                      mcs_rate->rate_n_flags |= RATE_MCS_DUP_MSK;
 +              if (is_siso(tbl->lq_type))
 +                      rate_n_flags |= iwl4965_rates[index].plcp_siso;
 +              else if (is_mimo2(tbl->lq_type))
 +                      rate_n_flags |= iwl4965_rates[index].plcp_mimo2;
                else
 -                      mcs_rate->rate_n_flags |= RATE_MCS_FAT_MSK;
 +                      rate_n_flags |= iwl4965_rates[index].plcp_mimo3;
 +      } else {
 +              IWL_ERROR("Invalid tbl->lq_type %d\n", tbl->lq_type);
        }
 -      if (tbl->is_SGI)
 -              mcs_rate->rate_n_flags |= RATE_MCS_SGI_MSK;
  
 -      if (use_green) {
 -              mcs_rate->rate_n_flags |= RATE_MCS_GF_MSK;
 -              if (is_siso(tbl->lq_type))
 -                      mcs_rate->rate_n_flags &= ~RATE_MCS_SGI_MSK;
 +      rate_n_flags |= ((tbl->ant_type << RATE_MCS_ANT_POS) &
 +                                                   RATE_MCS_ANT_ABC_MSK);
 +
 +      if (is_Ht(tbl->lq_type)) {
 +              if (tbl->is_fat) {
 +                      if (tbl->is_dup)
 +                              rate_n_flags |= RATE_MCS_DUP_MSK;
 +                      else
 +                              rate_n_flags |= RATE_MCS_FAT_MSK;
 +              }
 +              if (tbl->is_SGI)
 +                      rate_n_flags |= RATE_MCS_SGI_MSK;
 +
 +              if (use_green) {
 +                      rate_n_flags |= RATE_MCS_GF_MSK;
 +                      if (is_siso(tbl->lq_type) && tbl->is_SGI) {
 +                              rate_n_flags &= ~RATE_MCS_SGI_MSK;
 +                              IWL_ERROR("GF was set with SGI:SISO\n");
 +                      }
 +              }
        }
 +      return rate_n_flags;
  }
  
  /*
   * Interpret uCode API's rate_n_flags format,
   * fill "search" or "active" tx mode table.
   */
 -static int rs_get_tbl_info_from_mcs(const struct iwl4965_rate *mcs_rate,
 +static int rs_get_tbl_info_from_mcs(const u32 rate_n_flags,
                                    enum ieee80211_band band,
                                    struct iwl4965_scale_tbl_info *tbl,
                                    int *rate_idx)
  {
 -      int index;
 -      u32 ant_msk;
 +      u32 ant_msk = (rate_n_flags & RATE_MCS_ANT_ABC_MSK);
 +      u8 num_of_ant = get_num_of_ant_from_rate(rate_n_flags);
 +      u8 mcs;
  
 -      index = iwl4965_hwrate_to_plcp_idx(mcs_rate->rate_n_flags);
 +      *rate_idx = iwl4965_hwrate_to_plcp_idx(rate_n_flags);
  
 -      if (index  == IWL_RATE_INVALID) {
 +      if (*rate_idx  == IWL_RATE_INVALID) {
                *rate_idx = -1;
                return -EINVAL;
        }
        tbl->is_SGI = 0;        /* default legacy setup */
        tbl->is_fat = 0;
        tbl->is_dup = 0;
 -      tbl->antenna_type = ANT_BOTH;   /* default MIMO setup */
 +      tbl->ant_type = (ant_msk >> RATE_MCS_ANT_POS);
 +      tbl->lq_type = LQ_NONE;
  
        /* legacy rate format */
 -      if (!(mcs_rate->rate_n_flags & RATE_MCS_HT_MSK)) {
 -              ant_msk = (mcs_rate->rate_n_flags & RATE_MCS_ANT_AB_MSK);
 -
 -              if (ant_msk == RATE_MCS_ANT_AB_MSK)
 -                      tbl->lq_type = LQ_NONE;
 -              else {
 -
 +      if (!(rate_n_flags & RATE_MCS_HT_MSK)) {
 +              if (num_of_ant == 1) {
                        if (band == IEEE80211_BAND_5GHZ)
                                tbl->lq_type = LQ_A;
                        else
                                tbl->lq_type = LQ_G;
 -
 -                      if (mcs_rate->rate_n_flags & RATE_MCS_ANT_A_MSK)
 -                              tbl->antenna_type = ANT_MAIN;
 -                      else
 -                              tbl->antenna_type = ANT_AUX;
                }
 -              *rate_idx = index;
 -
 -      /* HT rate format, SISO (might be 20 MHz legacy or 40 MHz fat width) */
 -      } else if (iwl4965_rate_get_rate(mcs_rate->rate_n_flags)
 -                                      <= IWL_RATE_SISO_60M_PLCP) {
 -              tbl->lq_type = LQ_SISO;
 -
 -              ant_msk = (mcs_rate->rate_n_flags & RATE_MCS_ANT_AB_MSK);
 -              if (ant_msk == RATE_MCS_ANT_AB_MSK)
 -                      tbl->lq_type = LQ_NONE;
 -              else {
 -                      if (mcs_rate->rate_n_flags & RATE_MCS_ANT_A_MSK)
 -                              tbl->antenna_type = ANT_MAIN;
 -                      else
 -                              tbl->antenna_type = ANT_AUX;
 -              }
 -              if (mcs_rate->rate_n_flags & RATE_MCS_SGI_MSK)
 -                      tbl->is_SGI = 1;
 -
 -              if ((mcs_rate->rate_n_flags & RATE_MCS_FAT_MSK) ||
 -                  (mcs_rate->rate_n_flags & RATE_MCS_DUP_MSK))
 -                      tbl->is_fat = 1;
 -
 -              if (mcs_rate->rate_n_flags & RATE_MCS_DUP_MSK)
 -                      tbl->is_dup = 1;
 -
 -              *rate_idx = index;
 -
 -      /* HT rate format, MIMO (might be 20 MHz legacy or 40 MHz fat width) */
 +      /* HT rate format */
        } else {
 -              tbl->lq_type = LQ_MIMO;
 -              if (mcs_rate->rate_n_flags & RATE_MCS_SGI_MSK)
 +              if (rate_n_flags & RATE_MCS_SGI_MSK)
                        tbl->is_SGI = 1;
  
 -              if ((mcs_rate->rate_n_flags & RATE_MCS_FAT_MSK) ||
 -                  (mcs_rate->rate_n_flags & RATE_MCS_DUP_MSK))
 +              if ((rate_n_flags & RATE_MCS_FAT_MSK) ||
 +                  (rate_n_flags & RATE_MCS_DUP_MSK))
                        tbl->is_fat = 1;
  
 -              if (mcs_rate->rate_n_flags & RATE_MCS_DUP_MSK)
 +              if (rate_n_flags & RATE_MCS_DUP_MSK)
                        tbl->is_dup = 1;
 -              *rate_idx = index;
 +
 +              mcs = rs_extract_rate(rate_n_flags);
 +
 +              /* SISO */
 +              if (mcs <= IWL_RATE_SISO_60M_PLCP) {
 +                      if (num_of_ant == 1)
 +                              tbl->lq_type = LQ_SISO; /*else NONE*/
 +              /* MIMO2 */
 +              } else if (mcs <= IWL_RATE_MIMO2_60M_PLCP) {
 +                      if (num_of_ant == 2)
 +                              tbl->lq_type = LQ_MIMO2;
 +              /* MIMO3 */
 +              } else {
 +                      if (num_of_ant == 3)
 +                              tbl->lq_type = LQ_MIMO3;
 +              }
        }
        return 0;
  }
  
 -static inline void rs_toggle_antenna(struct iwl4965_rate *new_rate,
 -                                   struct iwl4965_scale_tbl_info *tbl)
 +/* switch to another antenna/antennas and return 1 */
 +/* if no other valid antenna found, return 0 */
 +static int rs_toggle_antenna(u32 valid_ant, u32 *rate_n_flags,
 +                            struct iwl4965_scale_tbl_info *tbl)
  {
 -      if (tbl->antenna_type == ANT_AUX) {
 -              tbl->antenna_type = ANT_MAIN;
 -              new_rate->rate_n_flags &= ~RATE_MCS_ANT_B_MSK;
 -              new_rate->rate_n_flags |= RATE_MCS_ANT_A_MSK;
 -      } else {
 -              tbl->antenna_type = ANT_AUX;
 -              new_rate->rate_n_flags &= ~RATE_MCS_ANT_A_MSK;
 -              new_rate->rate_n_flags |= RATE_MCS_ANT_B_MSK;
 -      }
 +      u8 new_ant_type;
 +
 +      if (!tbl->ant_type || tbl->ant_type > ANT_ABC)
 +              return 0;
 +
 +      if (!rs_is_valid_ant(valid_ant, tbl->ant_type))
 +              return 0;
 +
 +      new_ant_type = ant_toggle_lookup[tbl->ant_type];
 +
 +      while ((new_ant_type != tbl->ant_type) &&
 +             !rs_is_valid_ant(valid_ant, new_ant_type))
 +              new_ant_type = ant_toggle_lookup[new_ant_type];
 +
 +      if (new_ant_type == tbl->ant_type)
 +              return 0;
 +
 +      tbl->ant_type = new_ant_type;
 +      *rate_n_flags &= ~RATE_MCS_ANT_ABC_MSK;
 +      *rate_n_flags |= new_ant_type << RATE_MCS_ANT_POS;
 +      return 1;
  }
  
 -static inline u8 rs_use_green(struct iwl_priv *priv,
 -                            struct ieee80211_conf *conf)
 +/* FIXME:RS: in 4965 we don't use greenfield at all */
 +/* FIXME:RS: don't use greenfield for now in TX */
 +/* #ifdef CONFIG_IWL4965_HT */
 +#if 0
 +static inline u8 rs_use_green(struct iwl_priv *priv, struct ieee80211_conf *conf)
  {
        return ((conf->flags & IEEE80211_CONF_SUPPORT_HT_MODE) &&
                priv->current_ht_config.is_green_field &&
                !priv->current_ht_config.non_GF_STA_present);
 -#endif        /* CONFIG_IWL4965_HT */
 +}
 +#else
 +static inline u8 rs_use_green(struct iwl_priv *priv, struct ieee80211_conf *conf)
 +{
        return 0;
  }
 +#endif        /* CONFIG_IWL4965_HT */
  
  /**
   * rs_get_supported_rates - get the available rates
   * basic available rates.
   *
   */
 -static void rs_get_supported_rates(struct iwl4965_lq_sta *lq_sta,
 +static u16 rs_get_supported_rates(struct iwl4965_lq_sta *lq_sta,
                                   struct ieee80211_hdr *hdr,
 -                                 enum iwl4965_table_type rate_type,
 -                                 u16 *data_rate)
 +                                 enum iwl_table_type rate_type)
  {
 -      if (is_legacy(rate_type))
 -              *data_rate = lq_sta->active_rate;
 -      else {
 +      if (hdr && is_multicast_ether_addr(hdr->addr1) &&
 +          lq_sta->active_rate_basic)
 +              return lq_sta->active_rate_basic;
 +
 +      if (is_legacy(rate_type)) {
 +              return lq_sta->active_legacy_rate;
 +      } else {
                if (is_siso(rate_type))
 -                      *data_rate = lq_sta->active_siso_rate;
 +                      return lq_sta->active_siso_rate;
 +              else if (is_mimo2(rate_type))
 +                      return lq_sta->active_mimo2_rate;
                else
 -                      *data_rate = lq_sta->active_mimo_rate;
 -      }
 -
 -      if (hdr && is_multicast_ether_addr(hdr->addr1) &&
 -          lq_sta->active_rate_basic) {
 -              *data_rate = lq_sta->active_rate_basic;
 +                      return lq_sta->active_mimo3_rate;
        }
  }
  
@@@ -717,9 -705,9 +717,9 @@@ static u16 rs_get_adjacent_rate(u8 inde
        return (high << 8) | low;
  }
  
 -static void rs_get_lower_rate(struct iwl4965_lq_sta *lq_sta,
 +static u32 rs_get_lower_rate(struct iwl4965_lq_sta *lq_sta,
                             struct iwl4965_scale_tbl_info *tbl, u8 scale_index,
 -                           u8 ht_possible, struct iwl4965_rate *mcs_rate)
 +                           u8 ht_possible)
  {
        s32 low;
        u16 rate_mask;
                else
                        tbl->lq_type = LQ_G;
  
 -              if ((tbl->antenna_type == ANT_BOTH) ||
 -                  (tbl->antenna_type == ANT_NONE))
 -                      tbl->antenna_type = ANT_MAIN;
 +              if (num_of_ant(tbl->ant_type) > 1)
 +                      tbl->ant_type = ANT_A;/*FIXME:RS*/
  
                tbl->is_fat = 0;
                tbl->is_SGI = 0;
        }
  
 -      rs_get_supported_rates(lq_sta, NULL, tbl->lq_type, &rate_mask);
 +      rate_mask = rs_get_supported_rates(lq_sta, NULL, tbl->lq_type);
  
        /* Mask with station rate restriction */
        if (is_legacy(tbl->lq_type)) {
  
        /* If we switched from HT to legacy, check current rate */
        if (switch_to_legacy && (rate_mask & (1 << scale_index))) {
 -              rs_mcs_from_tbl(mcs_rate, tbl, scale_index, is_green);
 -              return;
 +              low = scale_index;
 +              goto out;
        }
  
        high_low = rs_get_adjacent_rate(scale_index, rate_mask, tbl->lq_type);
        low = high_low & 0xff;
  
 -      if (low != IWL_RATE_INVALID)
 -              rs_mcs_from_tbl(mcs_rate, tbl, low, is_green);
 -      else
 -              rs_mcs_from_tbl(mcs_rate, tbl, scale_index, is_green);
 +      if (low == IWL_RATE_INVALID)
 +              low = scale_index;
 +
 +out:
 +      return rate_n_flags_from_tbl(tbl, low, is_green);
  }
  
  /*
@@@ -792,7 -780,7 +792,7 @@@ static void rs_tx_status(void *priv_rat
        struct ieee80211_hw *hw = local_to_hw(local);
        struct iwl4965_rate_scale_data *window = NULL;
        struct iwl4965_rate_scale_data *search_win = NULL;
 -      struct iwl4965_rate tx_mcs;
 +      u32 tx_rate;
        struct iwl4965_scale_tbl_info tbl_type;
        struct iwl4965_scale_tbl_info *curr_tbl, *search_tbl;
        u8 active_index = 0;
        table = &lq_sta->lq;
        active_index = lq_sta->active_tbl;
  
 -      /* Get mac80211 antenna info */
 -      lq_sta->antenna =
 -              (lq_sta->valid_antenna & local->hw.conf.antenna_sel_tx);
 -      if (!lq_sta->antenna)
 -              lq_sta->antenna = lq_sta->valid_antenna;
 -
 -      /* Ignore mac80211 antenna info for now */
 -      lq_sta->antenna = lq_sta->valid_antenna;
 -
        curr_tbl = &(lq_sta->lq_info[active_index]);
        search_tbl = &(lq_sta->lq_info[(1 - active_index)]);
        window = (struct iwl4965_rate_scale_data *)
         * to check "search" mode, or a prior "search" mode after we've moved
         * to a new "search" mode (which might become the new "active" mode).
         */
 -      tx_mcs.rate_n_flags = le32_to_cpu(table->rs_table[0].rate_n_flags);
 -      rs_get_tbl_info_from_mcs(&tx_mcs, priv->band, &tbl_type, &rs_index);
 +      tx_rate = le32_to_cpu(table->rs_table[0].rate_n_flags);
 +      rs_get_tbl_info_from_mcs(tx_rate, priv->band, &tbl_type, &rs_index);
        if (priv->band == IEEE80211_BAND_5GHZ)
                rs_index -= IWL_FIRST_OFDM_RATE;
  
                !!(tx_resp->control.flags & IEEE80211_TXCTL_40_MHZ_WIDTH)) ||
            (tbl_type.is_dup ^
                !!(tx_resp->control.flags & IEEE80211_TXCTL_DUP_DATA)) ||
 -          (tbl_type.antenna_type ^
 -              tx_resp->control.antenna_sel_tx) ||
 -          (!!(tx_mcs.rate_n_flags & RATE_MCS_HT_MSK) ^
 +          (tbl_type.ant_type ^ tx_resp->control.antenna_sel_tx) ||
 +          (!!(tx_rate & RATE_MCS_HT_MSK) ^
                !!(tx_resp->control.flags & IEEE80211_TXCTL_OFDM_HT)) ||
 -          (!!(tx_mcs.rate_n_flags & RATE_MCS_GF_MSK) ^
 +          (!!(tx_rate & RATE_MCS_GF_MSK) ^
                !!(tx_resp->control.flags & IEEE80211_TXCTL_GREEN_FIELD)) ||
            (hw->wiphy->bands[priv->band]->bitrates[rs_index].bitrate !=
                tx_resp->control.tx_rate->bitrate)) {
 -              IWL_DEBUG_RATE("initial rate does not match 0x%x\n",
 -                              tx_mcs.rate_n_flags);
 +              IWL_DEBUG_RATE("initial rate does not match 0x%x\n", tx_rate);
                goto out;
        }
  
        while (retries) {
                /* Look up the rate and other info used for each tx attempt.
                 * Each tx attempt steps one entry deeper in the rate table. */
 -              tx_mcs.rate_n_flags =
 -                  le32_to_cpu(table->rs_table[index].rate_n_flags);
 -              rs_get_tbl_info_from_mcs(&tx_mcs, priv->band,
 +              tx_rate = le32_to_cpu(table->rs_table[index].rate_n_flags);
 +              rs_get_tbl_info_from_mcs(tx_rate, priv->band,
                                          &tbl_type, &rs_index);
  
                /* If type matches "search" table,
                 * add failure to "search" history */
                if ((tbl_type.lq_type == search_tbl->lq_type) &&
 -                  (tbl_type.antenna_type == search_tbl->antenna_type) &&
 +                  (tbl_type.ant_type == search_tbl->ant_type) &&
                    (tbl_type.is_SGI == search_tbl->is_SGI)) {
                        if (search_tbl->expected_tpt)
                                tpt = search_tbl->expected_tpt[rs_index];
                /* Else if type matches "current/active" table,
                 * add failure to "current/active" history */
                } else if ((tbl_type.lq_type == curr_tbl->lq_type) &&
 -                         (tbl_type.antenna_type == curr_tbl->antenna_type) &&
 +                         (tbl_type.ant_type == curr_tbl->ant_type) &&
                           (tbl_type.is_SGI == curr_tbl->is_SGI)) {
                        if (curr_tbl->expected_tpt)
                                tpt = curr_tbl->expected_tpt[rs_index];
         * if Tx was successful first try, use original rate,
         * else look up the rate that was, finally, successful.
         */
 -      tx_mcs.rate_n_flags = le32_to_cpu(table->rs_table[index].rate_n_flags);
 -      rs_get_tbl_info_from_mcs(&tx_mcs, priv->band, &tbl_type, &rs_index);
 +      tx_rate = le32_to_cpu(table->rs_table[index].rate_n_flags);
 +      rs_get_tbl_info_from_mcs(tx_rate, priv->band, &tbl_type, &rs_index);
  
        /* Update frame history window with "success" if Tx got ACKed ... */
        if (tx_resp->flags & IEEE80211_TX_STATUS_ACK)
        /* If type matches "search" table,
         * add final tx status to "search" history */
        if ((tbl_type.lq_type == search_tbl->lq_type) &&
 -          (tbl_type.antenna_type == search_tbl->antenna_type) &&
 +          (tbl_type.ant_type == search_tbl->ant_type) &&
            (tbl_type.is_SGI == search_tbl->is_SGI)) {
                if (search_tbl->expected_tpt)
                        tpt = search_tbl->expected_tpt[rs_index];
        /* Else if type matches "current/active" table,
         * add final tx status to "current/active" history */
        } else if ((tbl_type.lq_type == curr_tbl->lq_type) &&
 -                 (tbl_type.antenna_type == curr_tbl->antenna_type) &&
 +                 (tbl_type.ant_type == curr_tbl->ant_type) &&
                   (tbl_type.is_SGI == curr_tbl->is_SGI)) {
                if (curr_tbl->expected_tpt)
                        tpt = curr_tbl->expected_tpt[rs_index];
@@@ -982,6 -982,30 +982,6 @@@ out
        return;
  }
  
 -static u8 rs_is_ant_connected(u8 valid_antenna,
 -                            enum iwl4965_antenna_type antenna_type)
 -{
 -      if (antenna_type == ANT_AUX)
 -              return ((valid_antenna & 0x2) ? 1:0);
 -      else if (antenna_type == ANT_MAIN)
 -              return ((valid_antenna & 0x1) ? 1:0);
 -      else if (antenna_type == ANT_BOTH)
 -              return ((valid_antenna & 0x3) == 0x3);
 -
 -      return 1;
 -}
 -
 -static u8 rs_is_other_ant_connected(u8 valid_antenna,
 -                                  enum iwl4965_antenna_type antenna_type)
 -{
 -      if (antenna_type == ANT_AUX)
 -              return rs_is_ant_connected(valid_antenna, ANT_MAIN);
 -      else
 -              return rs_is_ant_connected(valid_antenna, ANT_AUX);
 -
 -      return 0;
 -}
 -
  /*
   * Begin a period of staying with a selected modulation mode.
   * Set "stay_in_tbl" flag to prevent any mode switches.
  static void rs_set_stay_in_table(u8 is_legacy,
                                 struct iwl4965_lq_sta *lq_sta)
  {
 -      IWL_DEBUG_HT("we are staying in the same table\n");
 +      IWL_DEBUG_RATE("we are staying in the same table\n");
        lq_sta->stay_in_tbl = 1;        /* only place this gets set */
        if (is_legacy) {
                lq_sta->table_count_limit = IWL_LEGACY_TABLE_COUNT;
  /*
   * Find correct throughput table for given mode of modulation
   */
 -static void rs_get_expected_tpt_table(struct iwl4965_lq_sta *lq_sta,
 +static void rs_set_expected_tpt_table(struct iwl4965_lq_sta *lq_sta,
                                      struct iwl4965_scale_tbl_info *tbl)
  {
        if (is_legacy(tbl->lq_type)) {
                else
                        tbl->expected_tpt = expected_tpt_siso20MHz;
  
 -      } else if (is_mimo(tbl->lq_type)) {
 +      } else if (is_mimo(tbl->lq_type)) { /* FIXME:need to separate mimo2/3 */
                if (tbl->is_fat && !lq_sta->is_dup)
                        if (tbl->is_SGI)
                                tbl->expected_tpt = expected_tpt_mimo40MHzSGI;
  static s32 rs_get_best_rate(struct iwl_priv *priv,
                            struct iwl4965_lq_sta *lq_sta,
                            struct iwl4965_scale_tbl_info *tbl, /* "search" */
 -                          u16 rate_mask, s8 index, s8 rate)
 +                          u16 rate_mask, s8 index)
  {
        /* "active" values */
        struct iwl4965_scale_tbl_info *active_tbl =
  
        s32 new_rate, high, low, start_hi;
        u16 high_low;
 +      s8 rate = index;
  
        new_rate = high = low = start_hi = IWL_RATE_INVALID;
  
  }
  #endif                                /* CONFIG_IWL4965_HT */
  
  /*
   * Set up search table for MIMO
   */
 -static int rs_switch_to_mimo(struct iwl_priv *priv,
 +#ifdef CONFIG_IWL4965_HT
 +static int rs_switch_to_mimo2(struct iwl_priv *priv,
                             struct iwl4965_lq_sta *lq_sta,
                             struct ieee80211_conf *conf,
                             struct sta_info *sta,
                             struct iwl4965_scale_tbl_info *tbl, int index)
  {
 -#ifdef CONFIG_IWL4965_HT
        u16 rate_mask;
        s32 rate;
        s8 is_green = lq_sta->is_green;
            !sta->ht_info.ht_supported)
                return -1;
  
 -      IWL_DEBUG_HT("LQ: try to switch to MIMO\n");
 -      tbl->lq_type = LQ_MIMO;
 -      rs_get_supported_rates(lq_sta, NULL, tbl->lq_type,
 -                              &rate_mask);
 -
        if (priv->current_ht_config.tx_mimo_ps_mode == IWL_MIMO_PS_STATIC)
                return -1;
  
        /* Need both Tx chains/antennas to support MIMO */
 -      if (!rs_is_both_ant_supp(lq_sta->antenna))
 +      if (priv->hw_params.tx_chains_num < 2)
                return -1;
  
 +      IWL_DEBUG_RATE("LQ: try to switch to MIMO2\n");
 +
 +      tbl->lq_type = LQ_MIMO2;
        tbl->is_dup = lq_sta->is_dup;
        tbl->action = 0;
 +      rate_mask = lq_sta->active_mimo2_rate;
 +
        if (priv->current_ht_config.supported_chan_width
 -          == IWL_CHANNEL_WIDTH_40MHZ)
 +                                      == IWL_CHANNEL_WIDTH_40MHZ)
                tbl->is_fat = 1;
        else
                tbl->is_fat = 0;
  
 +      /* FIXME: - don't toggle SGI here
        if (tbl->is_fat) {
                if (priv->current_ht_config.sgf & HT_SHORT_GI_40MHZ_ONLY)
                        tbl->is_SGI = 1;
                tbl->is_SGI = 1;
        else
                tbl->is_SGI = 0;
 +      */
 +
 +      rs_set_expected_tpt_table(lq_sta, tbl);
  
 -      rs_get_expected_tpt_table(lq_sta, tbl);
 +      rate = rs_get_best_rate(priv, lq_sta, tbl, rate_mask, index);
  
 -      rate = rs_get_best_rate(priv, lq_sta, tbl, rate_mask, index, index);
 +      IWL_DEBUG_RATE("LQ: MIMO2 best rate %d mask %X\n", rate, rate_mask);
  
 -      IWL_DEBUG_HT("LQ: MIMO best rate %d mask %X\n", rate, rate_mask);
 -      if ((rate == IWL_RATE_INVALID) || !((1 << rate) & rate_mask))
 +      if ((rate == IWL_RATE_INVALID) || !((1 << rate) & rate_mask)) {
 +              IWL_DEBUG_RATE("Can't switch with index %d rate mask %x\n",
 +                                              rate, rate_mask);
                return -1;
 -      rs_mcs_from_tbl(&tbl->current_rate, tbl, rate, is_green);
 +      }
 +      tbl->current_rate = rate_n_flags_from_tbl(tbl, rate, is_green);
  
 -      IWL_DEBUG_HT("LQ: Switch to new mcs %X index is green %X\n",
 -                   tbl->current_rate.rate_n_flags, is_green);
 +      IWL_DEBUG_RATE("LQ: Switch to new mcs %X index is green %X\n",
 +                   tbl->current_rate, is_green);
        return 0;
 +}
  #else
 +static int rs_switch_to_mimo2(struct iwl_priv *priv,
 +                           struct iwl4965_lq_sta *lq_sta,
 +                           struct ieee80211_conf *conf,
 +                           struct sta_info *sta,
 +                           struct iwl4965_scale_tbl_info *tbl, int index)
 +{
        return -1;
 -#endif        /*CONFIG_IWL4965_HT */
  }
 +#endif        /*CONFIG_IWL4965_HT */
  
  /*
   * Set up search table for SISO
@@@ -1241,16 -1256,16 +1241,16 @@@ static int rs_switch_to_siso(struct iwl
        u8 is_green = lq_sta->is_green;
        s32 rate;
  
 -      IWL_DEBUG_HT("LQ: try to switch to SISO\n");
        if (!(conf->flags & IEEE80211_CONF_SUPPORT_HT_MODE) ||
            !sta->ht_info.ht_supported)
                return -1;
  
 +      IWL_DEBUG_RATE("LQ: try to switch to SISO\n");
 +
        tbl->is_dup = lq_sta->is_dup;
        tbl->lq_type = LQ_SISO;
        tbl->action = 0;
 -      rs_get_supported_rates(lq_sta, NULL, tbl->lq_type,
 -                              &rate_mask);
 +      rate_mask = lq_sta->active_siso_rate;
  
        if (priv->current_ht_config.supported_chan_width
            == IWL_CHANNEL_WIDTH_40MHZ)
        else
                tbl->is_fat = 0;
  
 +      /* FIXME: - don't toggle SGI here
        if (tbl->is_fat) {
                if (priv->current_ht_config.sgf & HT_SHORT_GI_40MHZ_ONLY)
                        tbl->is_SGI = 1;
                tbl->is_SGI = 1;
        else
                tbl->is_SGI = 0;
 +      */
  
        if (is_green)
 -              tbl->is_SGI = 0;
 +              tbl->is_SGI = 0; /*11n spec: no SGI in SISO+Greenfield*/
  
 -      rs_get_expected_tpt_table(lq_sta, tbl);
 -      rate = rs_get_best_rate(priv, lq_sta, tbl, rate_mask, index, index);
 +      rs_set_expected_tpt_table(lq_sta, tbl);
 +      rate = rs_get_best_rate(priv, lq_sta, tbl, rate_mask, index);
  
 -      IWL_DEBUG_HT("LQ: get best rate %d mask %X\n", rate, rate_mask);
 +      IWL_DEBUG_RATE("LQ: get best rate %d mask %X\n", rate, rate_mask);
        if ((rate == IWL_RATE_INVALID) || !((1 << rate) & rate_mask)) {
 -              IWL_DEBUG_HT("can not switch with index %d rate mask %x\n",
 +              IWL_DEBUG_RATE("can not switch with index %d rate mask %x\n",
                             rate, rate_mask);
                return -1;
        }
 -      rs_mcs_from_tbl(&tbl->current_rate, tbl, rate, is_green);
 -      IWL_DEBUG_HT("LQ: Switch to new mcs %X index is green %X\n",
 -                   tbl->current_rate.rate_n_flags, is_green);
 +      tbl->current_rate = rate_n_flags_from_tbl(tbl, rate, is_green);
 +      IWL_DEBUG_RATE("LQ: Switch to new mcs %X index is green %X\n",
 +                   tbl->current_rate, is_green);
        return 0;
  #else
        return -1;
 -
  #endif        /*CONFIG_IWL4965_HT */
  }
  
@@@ -1300,6 -1314,7 +1300,6 @@@ static int rs_move_legacy_other(struct 
                                struct sta_info *sta,
                                int index)
  {
 -      int ret = 0;
        struct iwl4965_scale_tbl_info *tbl =
            &(lq_sta->lq_info[lq_sta->active_tbl]);
        struct iwl4965_scale_tbl_info *search_tbl =
        u32 sz = (sizeof(struct iwl4965_scale_tbl_info) -
                  (sizeof(struct iwl4965_rate_scale_data) * IWL_RATE_COUNT));
        u8 start_action = tbl->action;
 +      u8 valid_tx_ant = priv->hw_params.valid_tx_ant;
 +      int ret = 0;
  
        for (; ;) {
                switch (tbl->action) {
                case IWL_LEGACY_SWITCH_ANTENNA:
 -                      IWL_DEBUG_HT("LQ Legacy switch Antenna\n");
 +                      IWL_DEBUG_RATE("LQ: Legacy toggle Antenna\n");
  
 -                      search_tbl->lq_type = LQ_NONE;
                        lq_sta->action_counter++;
  
                        /* Don't change antenna if success has been great */
                        if (window->success_ratio >= IWL_RS_GOOD_RATIO)
                                break;
  
                        /* Set up search table to try other antenna */
                        memcpy(search_tbl, tbl, sz);
  
 -                      rs_toggle_antenna(&(search_tbl->current_rate),
 -                                         search_tbl);
 -                      rs_get_expected_tpt_table(lq_sta, search_tbl);
 -                      lq_sta->search_better_tbl = 1;
 -                      goto out;
 +                      if (rs_toggle_antenna(valid_tx_ant,
 +                              &search_tbl->current_rate, search_tbl)) {
 +                              lq_sta->search_better_tbl = 1;
 +                              goto out;
 +                      }
  
                case IWL_LEGACY_SWITCH_SISO:
 -                      IWL_DEBUG_HT("LQ: Legacy switch to SISO\n");
 +                      IWL_DEBUG_RATE("LQ: Legacy switch to SISO\n");
  
                        /* Set up search table to try SISO */
                        memcpy(search_tbl, tbl, sz);
 -                      search_tbl->lq_type = LQ_SISO;
                        search_tbl->is_SGI = 0;
 -                      search_tbl->is_fat = 0;
                        ret = rs_switch_to_siso(priv, lq_sta, conf, sta,
                                                 search_tbl, index);
                        if (!ret) {
                        }
  
                        break;
 -              case IWL_LEGACY_SWITCH_MIMO:
 -                      IWL_DEBUG_HT("LQ: Legacy switch MIMO\n");
 +              case IWL_LEGACY_SWITCH_MIMO2:
 +                      IWL_DEBUG_RATE("LQ: Legacy switch to MIMO2\n");
  
                        /* Set up search table to try MIMO */
                        memcpy(search_tbl, tbl, sz);
 -                      search_tbl->lq_type = LQ_MIMO;
                        search_tbl->is_SGI = 0;
 -                      search_tbl->is_fat = 0;
 -                      search_tbl->antenna_type = ANT_BOTH;
 -                      ret = rs_switch_to_mimo(priv, lq_sta, conf, sta,
 +                      search_tbl->ant_type = ANT_AB;/*FIXME:RS*/
 +                              /*FIXME:RS:need to check ant validity*/
 +                      ret = rs_switch_to_mimo2(priv, lq_sta, conf, sta,
                                                 search_tbl, index);
                        if (!ret) {
                                lq_sta->search_better_tbl = 1;
                        break;
                }
                tbl->action++;
 -              if (tbl->action > IWL_LEGACY_SWITCH_MIMO)
 +              if (tbl->action > IWL_LEGACY_SWITCH_MIMO2)
                        tbl->action = IWL_LEGACY_SWITCH_ANTENNA;
  
                if (tbl->action == start_action)
  
   out:
        tbl->action++;
 -      if (tbl->action > IWL_LEGACY_SWITCH_MIMO)
 +      if (tbl->action > IWL_LEGACY_SWITCH_MIMO2)
                tbl->action = IWL_LEGACY_SWITCH_ANTENNA;
        return 0;
  
@@@ -1390,6 -1412,7 +1390,6 @@@ static int rs_move_siso_to_other(struc
                                 struct sta_info *sta,
                                 int index)
  {
 -      int ret;
        u8 is_green = lq_sta->is_green;
        struct iwl4965_scale_tbl_info *tbl =
            &(lq_sta->lq_info[lq_sta->active_tbl]);
        u32 sz = (sizeof(struct iwl4965_scale_tbl_info) -
                  (sizeof(struct iwl4965_rate_scale_data) * IWL_RATE_COUNT));
        u8 start_action = tbl->action;
 +      u8 valid_tx_ant = priv->hw_params.valid_tx_ant;
 +      int ret;
  
        for (;;) {
                lq_sta->action_counter++;
                switch (tbl->action) {
                case IWL_SISO_SWITCH_ANTENNA:
 -                      IWL_DEBUG_HT("LQ: SISO SWITCH ANTENNA SISO\n");
 -                      search_tbl->lq_type = LQ_NONE;
 +                      IWL_DEBUG_RATE("LQ: SISO toggle Antenna\n");
                        if (window->success_ratio >= IWL_RS_GOOD_RATIO)
                                break;
 -                      if (!rs_is_other_ant_connected(lq_sta->antenna,
 -                                                     tbl->antenna_type))
 -                              break;
  
                        memcpy(search_tbl, tbl, sz);
 -                      search_tbl->action = IWL_SISO_SWITCH_MIMO;
 -                      rs_toggle_antenna(&(search_tbl->current_rate),
 -                                         search_tbl);
 -                      lq_sta->search_better_tbl = 1;
 -
 -                      goto out;
 +                      if (rs_toggle_antenna(valid_tx_ant,
 +                                     &search_tbl->current_rate, search_tbl)) {
 +                              lq_sta->search_better_tbl = 1;
 +                              goto out;
 +                      }
  
 -              case IWL_SISO_SWITCH_MIMO:
 -                      IWL_DEBUG_HT("LQ: SISO SWITCH TO MIMO FROM SISO\n");
 +              case IWL_SISO_SWITCH_MIMO2:
 +                      IWL_DEBUG_RATE("LQ: SISO switch to MIMO\n");
                        memcpy(search_tbl, tbl, sz);
 -                      search_tbl->lq_type = LQ_MIMO;
                        search_tbl->is_SGI = 0;
 -                      search_tbl->is_fat = 0;
 -                      search_tbl->antenna_type = ANT_BOTH;
 -                      ret = rs_switch_to_mimo(priv, lq_sta, conf, sta,
 +                      search_tbl->ant_type = ANT_AB; /*FIXME:RS*/
 +                      ret = rs_switch_to_mimo2(priv, lq_sta, conf, sta,
                                                 search_tbl, index);
                        if (!ret) {
                                lq_sta->search_better_tbl = 1;
                        }
                        break;
                case IWL_SISO_SWITCH_GI:
 -                      IWL_DEBUG_HT("LQ: SISO SWITCH TO GI\n");
 +                      IWL_DEBUG_RATE("LQ: SISO toggle SGI/NGI\n");
  
                        memcpy(search_tbl, tbl, sz);
 -                      search_tbl->action = 0;
 -                      if (search_tbl->is_SGI)
 -                              search_tbl->is_SGI = 0;
 -                      else if (!is_green)
 -                              search_tbl->is_SGI = 1;
 -                      else
 -                              break;
 -                      lq_sta->search_better_tbl = 1;
 -                      if ((tbl->lq_type == LQ_SISO) &&
 -                          (tbl->is_SGI)) {
 +                      if (is_green) {
 +                              if (!tbl->is_SGI)
 +                                      break;
 +                              else
 +                                      IWL_ERROR("SGI was set in GF+SISO\n");
 +                      }
 +                      search_tbl->is_SGI = !tbl->is_SGI;
 +                      rs_set_expected_tpt_table(lq_sta, search_tbl);
 +                      if (tbl->is_SGI) {
                                s32 tpt = lq_sta->last_tpt / 100;
 -                              if (((!tbl->is_fat) &&
 -                                   (tpt >= expected_tpt_siso20MHz[index])) ||
 -                                  ((tbl->is_fat) &&
 -                                   (tpt >= expected_tpt_siso40MHz[index])))
 -                                      lq_sta->search_better_tbl = 0;
 +                              if (tpt >= search_tbl->expected_tpt[index])
 +                                      break;
                        }
 -                      rs_get_expected_tpt_table(lq_sta, search_tbl);
 -                      rs_mcs_from_tbl(&search_tbl->current_rate,
 -                                           search_tbl, index, is_green);
 +                      search_tbl->current_rate = rate_n_flags_from_tbl(
 +                                              search_tbl, index, is_green);
 +                      lq_sta->search_better_tbl = 1;
                        goto out;
                }
                tbl->action++;
@@@ -1476,6 -1508,7 +1476,6 @@@ static int rs_move_mimo_to_other(struc
                                 struct sta_info *sta,
                                 int index)
  {
 -      int ret;
        s8 is_green = lq_sta->is_green;
        struct iwl4965_scale_tbl_info *tbl =
            &(lq_sta->lq_info[lq_sta->active_tbl]);
        u32 sz = (sizeof(struct iwl4965_scale_tbl_info) -
                  (sizeof(struct iwl4965_rate_scale_data) * IWL_RATE_COUNT));
        u8 start_action = tbl->action;
 +      /*u8 valid_tx_ant = priv->hw_params.valid_tx_ant;*/
 +      int ret;
  
        for (;;) {
                lq_sta->action_counter++;
                switch (tbl->action) {
                case IWL_MIMO_SWITCH_ANTENNA_A:
                case IWL_MIMO_SWITCH_ANTENNA_B:
 -                      IWL_DEBUG_HT("LQ: MIMO SWITCH TO SISO\n");
 -
 +                      IWL_DEBUG_RATE("LQ: MIMO2 switch to SISO\n");
  
                        /* Set up new search table for SISO */
                        memcpy(search_tbl, tbl, sz);
 -                      search_tbl->lq_type = LQ_SISO;
 -                      search_tbl->is_SGI = 0;
 -                      search_tbl->is_fat = 0;
 +
 +                      /*FIXME:RS:need to check ant validity + C*/
                        if (tbl->action == IWL_MIMO_SWITCH_ANTENNA_A)
 -                              search_tbl->antenna_type = ANT_MAIN;
 +                              search_tbl->ant_type = ANT_A;
                        else
 -                              search_tbl->antenna_type = ANT_AUX;
 +                              search_tbl->ant_type = ANT_B;
  
                        ret = rs_switch_to_siso(priv, lq_sta, conf, sta,
                                                 search_tbl, index);
                        break;
  
                case IWL_MIMO_SWITCH_GI:
 -                      IWL_DEBUG_HT("LQ: MIMO SWITCH TO GI\n");
 +                      IWL_DEBUG_RATE("LQ: MIMO toggle SGI/NGI\n");
  
                        /* Set up new search table for MIMO */
                        memcpy(search_tbl, tbl, sz);
 -                      search_tbl->lq_type = LQ_MIMO;
 -                      search_tbl->antenna_type = ANT_BOTH;
 -                      search_tbl->action = 0;
 -                      if (search_tbl->is_SGI)
 -                              search_tbl->is_SGI = 0;
 -                      else
 -                              search_tbl->is_SGI = 1;
 -                      lq_sta->search_better_tbl = 1;
 -
 +                      search_tbl->is_SGI = !tbl->is_SGI;
 +                      rs_set_expected_tpt_table(lq_sta, search_tbl);
                        /*
                         * If active table already uses the fastest possible
                         * modulation (dual stream with short guard interval),
                         * and it's working well, there's no need to look
                         * for a better type of modulation!
                         */
 -                      if ((tbl->lq_type == LQ_MIMO) &&
 -                          (tbl->is_SGI)) {
 +                      if (tbl->is_SGI) {
                                s32 tpt = lq_sta->last_tpt / 100;
 -                              if (((!tbl->is_fat) &&
 -                                   (tpt >= expected_tpt_mimo20MHz[index])) ||
 -                                  ((tbl->is_fat) &&
 -                                   (tpt >= expected_tpt_mimo40MHz[index])))
 -                                      lq_sta->search_better_tbl = 0;
 +                              if (tpt >= search_tbl->expected_tpt[index])
 +                                      break;
                        }
 -                      rs_get_expected_tpt_table(lq_sta, search_tbl);
 -                      rs_mcs_from_tbl(&search_tbl->current_rate,
 -                                           search_tbl, index, is_green);
 +                      search_tbl->current_rate = rate_n_flags_from_tbl(
 +                                              search_tbl, index, is_green);
 +                      lq_sta->search_better_tbl = 1;
                        goto out;
  
                }
@@@ -1580,6 -1624,9 +1580,6 @@@ static void rs_stay_in_table(struct iwl
                                       (unsigned long)(lq_sta->flush_timer +
                                        IWL_RATE_SCALE_FLUSH_INTVL));
  
 -              /* For now, disable the elapsed time criterion */
 -              flush_interval_passed = 0;
 -
                /*
                 * Check if we should allow search for new modulation mode.
                 * If many frames have failed or succeeded, or we've used
                    (lq_sta->total_success > lq_sta->max_success_limit) ||
                    ((!lq_sta->search_better_tbl) && (lq_sta->flush_timer)
                     && (flush_interval_passed))) {
 -                      IWL_DEBUG_HT("LQ: stay is expired %d %d %d\n:",
 +                      IWL_DEBUG_RATE("LQ: stay is expired %d %d %d\n:",
                                     lq_sta->total_failed,
                                     lq_sta->total_success,
                                     flush_interval_passed);
                            lq_sta->table_count_limit) {
                                lq_sta->table_count = 0;
  
 -                              IWL_DEBUG_HT("LQ: stay in table clear win\n");
 +                              IWL_DEBUG_RATE("LQ: stay in table clear win\n");
                                for (i = 0; i < IWL_RATE_COUNT; i++)
                                        rs_rate_scale_clear_window(
                                                &(tbl->win[i]));
@@@ -1658,7 -1705,7 +1658,7 @@@ static void rs_rate_scale_perform(struc
        struct iwl4965_lq_sta *lq_sta;
        struct iwl4965_scale_tbl_info *tbl, *tbl1;
        u16 rate_scale_index_msk = 0;
 -      struct iwl4965_rate mcs_rate;
 +      u32 rate;
        u8 is_green = 0;
        u8 active_tbl = 0;
        u8 done_search = 0;
                       tbl->lq_type);
  
        /* rates available for this association, and for modulation mode */
 -      rs_get_supported_rates(lq_sta, hdr, tbl->lq_type,
 -                              &rate_mask);
 +      rate_mask = rs_get_supported_rates(lq_sta, hdr, tbl->lq_type);
  
        IWL_DEBUG_RATE("mask 0x%04X \n", rate_mask);
  
        if (!rate_scale_index_msk)
                rate_scale_index_msk = rate_mask;
  
 -      /* If current rate is no longer supported on current association,
 -       * or user changed preferences for rates, find a new supported rate. */
 -      if (index < 0 || !((1 << index) & rate_scale_index_msk)) {
 -              index = IWL_INVALID_VALUE;
 -              update_lq = 1;
 -
 -              /* get the highest available rate */
 -              for (i = 0; i <= IWL_RATE_COUNT; i++) {
 -                      if ((1 << i) & rate_scale_index_msk)
 -                              index = i;
 -              }
 -
 -              if (index == IWL_INVALID_VALUE) {
 -                      IWL_WARNING("Can not find a suitable rate\n");
 -                      return;
 -              }
 +      if (!((1 << index) & rate_scale_index_msk)) {
 +              IWL_ERROR("Current Rate is not valid\n");
 +              return;
        }
  
        /* Get expected throughput table and history window for current rate */
 -      if (!tbl->expected_tpt)
 -              rs_get_expected_tpt_table(lq_sta, tbl);
 +      if (!tbl->expected_tpt) {
 +              IWL_ERROR("tbl->expected_tpt is NULL\n");
 +              return;
 +      }
  
        window = &(tbl->win[index]);
  
         * in current association (use new rate found above).
         */
        fail_count = window->counter - window->success_counter;
 -      if (((fail_count < IWL_RATE_MIN_FAILURE_TH) &&
 -           (window->success_counter < IWL_RATE_MIN_SUCCESS_TH))
 -          || (tbl->expected_tpt == NULL)) {
 -              IWL_DEBUG_RATE("LQ: still below TH succ %d total %d "
 +      if ((fail_count < IWL_RATE_MIN_FAILURE_TH) &&
 +                      (window->success_counter < IWL_RATE_MIN_SUCCESS_TH)) {
 +              IWL_DEBUG_RATE("LQ: still below TH. succ=%d total=%d "
                               "for index %d\n",
                               window->success_counter, window->counter, index);
  
                 * or search for a new one? */
                rs_stay_in_table(lq_sta);
  
 -              /* Set up new rate table in uCode, if needed */
 -              if (update_lq) {
 -                      rs_mcs_from_tbl(&mcs_rate, tbl, index, is_green);
 -                      rs_fill_link_cmd(lq_sta, &mcs_rate, &lq_sta->lq);
 -                      iwl_send_lq_cmd(priv, &lq_sta->lq, CMD_ASYNC);
 -              }
                goto out;
  
        /* Else we have enough samples; calculate estimate of
         * actual average throughput */
 -      } else
 -              window->average_tpt = ((window->success_ratio *
 +      } else {
 +              /*FIXME:RS remove this else if we don't get this error*/
 +              if (window->average_tpt != ((window->success_ratio *
 +                              tbl->expected_tpt[index] + 64) / 128)) {
 +                      IWL_ERROR("expected_tpt should have been calculated"
 +                                                              " by now\n");
 +                      window->average_tpt = ((window->success_ratio *
                                        tbl->expected_tpt[index] + 64) / 128);
 +              }
 +      }
  
        /* If we are searching for better modulation mode, check success. */
        if (lq_sta->search_better_tbl) {
 -              int success_limit = IWL_RATE_SCALE_SWITCH;
  
                /* If good success, continue using the "search" mode;
                 * no need to send new link quality command, since we're
                 * continuing to use the setup that we've been trying. */
 -              if ((window->success_ratio > success_limit) ||
 -                  (window->average_tpt > lq_sta->last_tpt)) {
 -                      if (!is_legacy(tbl->lq_type)) {
 -                              IWL_DEBUG_HT("LQ: we are switching to HT"
 -                                           " rate suc %d current tpt %d"
 -                                           " old tpt %d\n",
 -                                           window->success_ratio,
 -                                           window->average_tpt,
 -                                           lq_sta->last_tpt);
 +              if (window->average_tpt > lq_sta->last_tpt) {
 +
 +                      IWL_DEBUG_RATE("LQ: SWITCHING TO CURRENT TABLE "
 +                                      "suc=%d cur-tpt=%d old-tpt=%d\n",
 +                                      window->success_ratio,
 +                                      window->average_tpt,
 +                                      lq_sta->last_tpt);
 +
 +                      if (!is_legacy(tbl->lq_type))
                                lq_sta->enable_counter = 1;
 -                      }
 +
                        /* Swap tables; "search" becomes "active" */
                        lq_sta->active_tbl = active_tbl;
                        current_tpt = window->average_tpt;
  
                /* Else poor success; go back to mode in "active" table */
                } else {
 +
 +                      IWL_DEBUG_RATE("LQ: GOING BACK TO THE OLD TABLE "
 +                                      "suc=%d cur-tpt=%d old-tpt=%d\n",
 +                                      window->success_ratio,
 +                                      window->average_tpt,
 +                                      lq_sta->last_tpt);
 +
                        /* Nullify "search" table */
                        tbl->lq_type = LQ_NONE;
  
  
                        /* Revert to "active" rate and throughput info */
                        index = iwl4965_hwrate_to_plcp_idx(
 -                              tbl->current_rate.rate_n_flags);
 +                                                      tbl->current_rate);
                        current_tpt = lq_sta->last_tpt;
  
                        /* Need to set up a new rate table in uCode */
                        update_lq = 1;
 -                      IWL_DEBUG_HT("XXY GO BACK TO OLD TABLE\n");
                }
  
                /* Either way, we've made a decision; modulation mode
                break;
        }
  
 -      IWL_DEBUG_HT("choose rate scale index %d action %d low %d "
 +      IWL_DEBUG_RATE("choose rate scale index %d action %d low %d "
                    "high %d type %d\n",
                     index, scale_action, low, high, tbl->lq_type);
  
   lq_update:
        /* Replace uCode's rate table for the destination station. */
        if (update_lq) {
 -              rs_mcs_from_tbl(&mcs_rate, tbl, index, is_green);
 -              rs_fill_link_cmd(lq_sta, &mcs_rate, &lq_sta->lq);
 +              rate = rate_n_flags_from_tbl(tbl, index, is_green);
 +              rs_fill_link_cmd(priv, lq_sta, rate);
                iwl_send_lq_cmd(priv, &lq_sta->lq, CMD_ASYNC);
        }
  
  
                        /* Use new "search" start rate */
                        index = iwl4965_hwrate_to_plcp_idx(
 -                                      tbl->current_rate.rate_n_flags);
 +                                                      tbl->current_rate);
  
 -                      IWL_DEBUG_HT("Switch current  mcs: %X index: %d\n",
 -                                   tbl->current_rate.rate_n_flags, index);
 -                      rs_fill_link_cmd(lq_sta, &tbl->current_rate,
 -                                       &lq_sta->lq);
 +                      IWL_DEBUG_RATE("Switch current  mcs: %X index: %d\n",
 +                                   tbl->current_rate, index);
 +                      rs_fill_link_cmd(priv, lq_sta, tbl->current_rate);
                        iwl_send_lq_cmd(priv, &lq_sta->lq, CMD_ASYNC);
                }
  
  #endif
                    (lq_sta->action_counter >= 1)) {
                        lq_sta->action_counter = 0;
 -                      IWL_DEBUG_HT("LQ: STAY in legacy table\n");
 +                      IWL_DEBUG_RATE("LQ: STAY in legacy table\n");
                        rs_set_stay_in_table(1, lq_sta);
                }
  
                        if ((lq_sta->last_tpt > IWL_AGG_TPT_THREHOLD) &&
                            (lq_sta->tx_agg_tid_en & (1 << tid)) &&
                            (tid != MAX_TID_COUNT)) {
 -                              IWL_DEBUG_HT("try to aggregate tid %d\n", tid);
 +                              IWL_DEBUG_RATE("try to aggregate tid %d\n", tid);
                                rs_tl_turn_on_agg(priv, tid, lq_sta, sta);
                        }
  #endif /*CONFIG_IWL4965_HT */
        }
  
  out:
 -      rs_mcs_from_tbl(&tbl->current_rate, tbl, index, is_green);
 +      tbl->current_rate = rate_n_flags_from_tbl(tbl, index, is_green);
        i = index;
        sta->last_txrate_idx = i;
  
@@@ -2051,14 -2106,13 +2051,14 @@@ static void rs_initialize_lq(struct iwl
                             struct ieee80211_conf *conf,
                             struct sta_info *sta)
  {
 -      int i;
        struct iwl4965_lq_sta *lq_sta;
        struct iwl4965_scale_tbl_info *tbl;
 -      u8 active_tbl = 0;
        int rate_idx;
 +      int i;
 +      u32 rate;
        u8 use_green = rs_use_green(priv, conf);
 -      struct iwl4965_rate mcs_rate;
 +      u8 active_tbl = 0;
 +      u8 valid_tx_ant;
  
        if (!sta || !sta->rate_ctrl_priv)
                goto out;
            (priv->iw_mode == IEEE80211_IF_TYPE_IBSS))
                goto out;
  
 +      valid_tx_ant = priv->hw_params.valid_tx_ant;
 +
        if (!lq_sta->search_better_tbl)
                active_tbl = lq_sta->active_tbl;
        else
        if ((i < 0) || (i >= IWL_RATE_COUNT))
                i = 0;
  
 -      mcs_rate.rate_n_flags = iwl4965_rates[i].plcp ;
 -      mcs_rate.rate_n_flags |= RATE_MCS_ANT_B_MSK;
 -      mcs_rate.rate_n_flags &= ~RATE_MCS_ANT_A_MSK;
 +      /* FIXME:RS: This is also wrong in 4965 */
 +      rate = iwl4965_rates[i].plcp;
 +      rate |= RATE_MCS_ANT_B_MSK;
 +      rate &= ~RATE_MCS_ANT_A_MSK;
  
        if (i >= IWL_FIRST_CCK_RATE && i <= IWL_LAST_CCK_RATE)
 -              mcs_rate.rate_n_flags |= RATE_MCS_CCK_MSK;
 +              rate |= RATE_MCS_CCK_MSK;
  
 -      tbl->antenna_type = ANT_AUX;
 -      rs_get_tbl_info_from_mcs(&mcs_rate, priv->band, tbl, &rate_idx);
 -      if (!rs_is_ant_connected(priv->valid_antenna, tbl->antenna_type))
 -          rs_toggle_antenna(&mcs_rate, tbl);
 +      tbl->ant_type = ANT_B;
 +      rs_get_tbl_info_from_mcs(rate, priv->band, tbl, &rate_idx);
 +      if (!rs_is_valid_ant(valid_tx_ant, tbl->ant_type))
 +          rs_toggle_antenna(valid_tx_ant, &rate, tbl);
  
 -      rs_mcs_from_tbl(&mcs_rate, tbl, rate_idx, use_green);
 -      tbl->current_rate.rate_n_flags = mcs_rate.rate_n_flags;
 -      rs_get_expected_tpt_table(lq_sta, tbl);
 -      rs_fill_link_cmd(lq_sta, &mcs_rate, &lq_sta->lq);
 +      rate = rate_n_flags_from_tbl(tbl, rate_idx, use_green);
 +      tbl->current_rate = rate;
 +      rs_set_expected_tpt_table(lq_sta, tbl);
 +      rs_fill_link_cmd(NULL, lq_sta, rate);
        iwl_send_lq_cmd(priv, &lq_sta->lq, CMD_ASYNC);
   out:
        return;
@@@ -2139,7 -2190,7 +2139,7 @@@ static void rs_get_rate(void *priv_rate
  
        if ((priv->iw_mode == IEEE80211_IF_TYPE_IBSS) &&
            !lq_sta->ibss_sta_added) {
 -              u8 sta_id = iwl4965_hw_find_station(priv, hdr->addr1);
 +              u8 sta_id = iwl_find_station(priv, hdr->addr1);
                DECLARE_MAC_BUF(mac);
  
                if (sta_id == IWL_INVALID_STATION) {
@@@ -2209,7 -2260,7 +2209,7 @@@ static void rs_rate_init(void *priv_rat
                for (i = 0; i < IWL_RATE_COUNT; i++)
                        rs_rate_scale_clear_window(&(lq_sta->lq_info[j].win[i]));
  
 -      IWL_DEBUG_RATE("rate scale global init\n");
 +      IWL_DEBUG_RATE("LQ: *** rate scale global init ***\n");
        /* TODO: what is a good starting rate for STA? About middle? Maybe not
         * the lowest or the highest rate.. Could consider using RSSI from
         * previous packets? Need to have IEEE 802.1X auth succeed immediately
  
        lq_sta->ibss_sta_added = 0;
        if (priv->iw_mode == IEEE80211_IF_TYPE_AP) {
 -              u8 sta_id = iwl4965_hw_find_station(priv, sta->addr);
 +              u8 sta_id = iwl_find_station(priv, sta->addr);
                DECLARE_MAC_BUF(mac);
  
                /* for IBSS the call are from tasklet */
 -              IWL_DEBUG_HT("LQ: ADD station %s\n",
 +              IWL_DEBUG_RATE("LQ: ADD station %s\n",
                             print_mac(mac, sta->addr));
  
                if (sta_id == IWL_INVALID_STATION) {
                sta->last_txrate_idx += IWL_FIRST_OFDM_RATE;
  
        lq_sta->is_dup = 0;
 -      lq_sta->valid_antenna = priv->valid_antenna;
 -      lq_sta->antenna = priv->antenna;
        lq_sta->is_green = rs_use_green(priv, conf);
 -      lq_sta->active_rate = priv->active_rate;
 -      lq_sta->active_rate &= ~(0x1000);
 +      lq_sta->active_legacy_rate = priv->active_rate & ~(0x1000);
        lq_sta->active_rate_basic = priv->active_rate_basic;
        lq_sta->band = priv->band;
  #ifdef CONFIG_IWL4965_HT
         * active_siso_rate mask includes 9 MBits (bit 5), and CCK (bits 0-3),
         * supp_rates[] does not; shift to convert format, force 9 MBits off.
         */
 -      lq_sta->active_siso_rate = (priv->current_ht_config.supp_mcs_set[0] << 1);
 +      lq_sta->active_siso_rate =
 +              priv->current_ht_config.supp_mcs_set[0] << 1;
        lq_sta->active_siso_rate |=
 -                      (priv->current_ht_config.supp_mcs_set[0] & 0x1);
 +              priv->current_ht_config.supp_mcs_set[0] & 0x1;
        lq_sta->active_siso_rate &= ~((u16)0x2);
 -      lq_sta->active_siso_rate =
 -                      lq_sta->active_siso_rate << IWL_FIRST_OFDM_RATE;
 +      lq_sta->active_siso_rate <<= IWL_FIRST_OFDM_RATE;
  
        /* Same here */
 -      lq_sta->active_mimo_rate = (priv->current_ht_config.supp_mcs_set[1] << 1);
 -      lq_sta->active_mimo_rate |=
 -                      (priv->current_ht_config.supp_mcs_set[1] & 0x1);
 -      lq_sta->active_mimo_rate &= ~((u16)0x2);
 -      lq_sta->active_mimo_rate =
 -                      lq_sta->active_mimo_rate << IWL_FIRST_OFDM_RATE;
 -      IWL_DEBUG_HT("SISO RATE 0x%X MIMO RATE 0x%X\n",
 +      lq_sta->active_mimo2_rate =
 +              priv->current_ht_config.supp_mcs_set[1] << 1;
 +      lq_sta->active_mimo2_rate |=
 +              priv->current_ht_config.supp_mcs_set[1] & 0x1;
 +      lq_sta->active_mimo2_rate &= ~((u16)0x2);
 +      lq_sta->active_mimo2_rate <<= IWL_FIRST_OFDM_RATE;
 +
 +      lq_sta->active_mimo3_rate =
 +              priv->current_ht_config.supp_mcs_set[2] << 1;
 +      lq_sta->active_mimo3_rate |=
 +              priv->current_ht_config.supp_mcs_set[2] & 0x1;
 +      lq_sta->active_mimo3_rate &= ~((u16)0x2);
 +      lq_sta->active_mimo3_rate <<= IWL_FIRST_OFDM_RATE;
 +
 +      IWL_DEBUG_RATE("SISO-RATE=%X MIMO2-RATE=%X MIMO3-RATE=%X\n",
                     lq_sta->active_siso_rate,
 -                   lq_sta->active_mimo_rate);
 +                   lq_sta->active_mimo2_rate,
 +                   lq_sta->active_mimo3_rate);
 +
 +      /* These values will be overriden later */
 +      lq_sta->lq.general_params.single_stream_ant_msk = ANT_A;
 +      lq_sta->lq.general_params.dual_stream_ant_msk = ANT_AB;
 +
        /* as default allow aggregation for all tids */
        lq_sta->tx_agg_tid_en = IWL_AGG_ALL_TID;
  #endif /*CONFIG_IWL4965_HT*/
        rs_initialize_lq(priv, conf, sta);
  }
  
 -static void rs_fill_link_cmd(struct iwl4965_lq_sta *lq_sta,
 -                          struct iwl4965_rate *tx_mcs,
 -                          struct iwl_link_quality_cmd *lq_cmd)
 +static void rs_fill_link_cmd(const struct iwl_priv *priv,
 +                           struct iwl4965_lq_sta *lq_sta,
 +                           u32 new_rate)
  {
 +      struct iwl4965_scale_tbl_info tbl_type;
        int index = 0;
        int rate_idx;
        int repeat_rate = 0;
 -      u8 ant_toggle_count = 0;
 +      u8 ant_toggle_cnt = 0;
        u8 use_ht_possible = 1;
 -      struct iwl4965_rate new_rate;
 -      struct iwl4965_scale_tbl_info tbl_type = { 0 };
 +      u8 valid_tx_ant = 0;
 +      struct iwl_link_quality_cmd *lq_cmd = &lq_sta->lq;
  
        /* Override starting rate (index 0) if needed for debug purposes */
 -      rs_dbgfs_set_mcs(lq_sta, tx_mcs, index);
 +      rs_dbgfs_set_mcs(lq_sta, &new_rate, index);
  
 -      /* Interpret rate_n_flags */
 -      rs_get_tbl_info_from_mcs(tx_mcs, lq_sta->band,
 +      /* Interpret new_rate (rate_n_flags) */
 +      memset(&tbl_type, 0, sizeof(tbl_type));
 +      rs_get_tbl_info_from_mcs(new_rate, lq_sta->band,
                                  &tbl_type, &rate_idx);
  
        /* How many times should we repeat the initial rate? */
        if (is_legacy(tbl_type.lq_type)) {
 -              ant_toggle_count = 1;
 +              ant_toggle_cnt = 1;
                repeat_rate = IWL_NUMBER_TRY;
 -      } else
 +      } else {
                repeat_rate = IWL_HT_NUMBER_TRY;
 +      }
  
        lq_cmd->general_params.mimo_delimiter =
                        is_mimo(tbl_type.lq_type) ? 1 : 0;
  
        /* Fill 1st table entry (index 0) */
 -      lq_cmd->rs_table[index].rate_n_flags =
 -                      cpu_to_le32(tx_mcs->rate_n_flags);
 -      new_rate.rate_n_flags = tx_mcs->rate_n_flags;
 +      lq_cmd->rs_table[index].rate_n_flags = cpu_to_le32(new_rate);
  
 -      if (is_mimo(tbl_type.lq_type) || (tbl_type.antenna_type == ANT_MAIN))
 -              lq_cmd->general_params.single_stream_ant_msk
 -                      = LINK_QUAL_ANT_A_MSK;
 -      else
 -              lq_cmd->general_params.single_stream_ant_msk
 -                      = LINK_QUAL_ANT_B_MSK;
 +      if (num_of_ant(tbl_type.ant_type) == 1) {
 +              lq_cmd->general_params.single_stream_ant_msk =
 +                                              tbl_type.ant_type;
 +      } else if (num_of_ant(tbl_type.ant_type) == 2) {
 +              lq_cmd->general_params.dual_stream_ant_msk =
 +                                              tbl_type.ant_type;
 +      } /* otherwise we don't modify the existing value */
  
        index++;
        repeat_rate--;
  
 +      if (priv)
 +              valid_tx_ant = priv->hw_params.valid_tx_ant;
 +
        /* Fill rest of rate table */
        while (index < LINK_QUAL_MAX_RETRY_NUM) {
                /* Repeat initial/next rate.
                 * For HT IWL_HT_NUMBER_TRY == 3, this executes twice. */
                while (repeat_rate > 0 && (index < LINK_QUAL_MAX_RETRY_NUM)) {
                        if (is_legacy(tbl_type.lq_type)) {
 -                              if (ant_toggle_count <
 -                                  NUM_TRY_BEFORE_ANTENNA_TOGGLE)
 -                                      ant_toggle_count++;
 -                              else {
 -                                      rs_toggle_antenna(&new_rate, &tbl_type);
 -                                      ant_toggle_count = 1;
 -                              }
 -                      }
 +                              if (ant_toggle_cnt < NUM_TRY_BEFORE_ANT_TOGGLE)
 +                                      ant_toggle_cnt++;
 +                              else if (priv &&
 +                                       rs_toggle_antenna(valid_tx_ant,
 +                                                      &new_rate, &tbl_type))
 +                                      ant_toggle_cnt = 1;
 +}
  
                        /* Override next rate if needed for debug purposes */
                        rs_dbgfs_set_mcs(lq_sta, &new_rate, index);
  
                        /* Fill next table entry */
                        lq_cmd->rs_table[index].rate_n_flags =
 -                                      cpu_to_le32(new_rate.rate_n_flags);
 +                                      cpu_to_le32(new_rate);
                        repeat_rate--;
                        index++;
                }
  
 -              rs_get_tbl_info_from_mcs(&new_rate, lq_sta->band, &tbl_type,
 +              rs_get_tbl_info_from_mcs(new_rate, lq_sta->band, &tbl_type,
                                                &rate_idx);
  
                /* Indicate to uCode which entries might be MIMO.
                        lq_cmd->general_params.mimo_delimiter = index;
  
                /* Get next rate */
 -              rs_get_lower_rate(lq_sta, &tbl_type, rate_idx,
 -                                use_ht_possible, &new_rate);
 +              new_rate = rs_get_lower_rate(lq_sta, &tbl_type, rate_idx,
 +                                           use_ht_possible);
  
                /* How many times should we repeat the next rate? */
                if (is_legacy(tbl_type.lq_type)) {
 -                      if (ant_toggle_count < NUM_TRY_BEFORE_ANTENNA_TOGGLE)
 -                              ant_toggle_count++;
 -                      else {
 -                              rs_toggle_antenna(&new_rate, &tbl_type);
 -                              ant_toggle_count = 1;
 -                      }
 +                      if (ant_toggle_cnt < NUM_TRY_BEFORE_ANT_TOGGLE)
 +                              ant_toggle_cnt++;
 +                      else if (priv &&
 +                               rs_toggle_antenna(valid_tx_ant,
 +                                                 &new_rate, &tbl_type))
 +                              ant_toggle_cnt = 1;
 +
                        repeat_rate = IWL_NUMBER_TRY;
 -              } else
 +              } else {
                        repeat_rate = IWL_HT_NUMBER_TRY;
 +              }
  
                /* Don't allow HT rates after next pass.
                 * rs_get_lower_rate() will change type to LQ_A or LQ_G. */
                rs_dbgfs_set_mcs(lq_sta, &new_rate, index);
  
                /* Fill next table entry */
 -              lq_cmd->rs_table[index].rate_n_flags =
 -                              cpu_to_le32(new_rate.rate_n_flags);
 +              lq_cmd->rs_table[index].rate_n_flags = cpu_to_le32(new_rate);
  
                index++;
                repeat_rate--;
        }
  
 -      lq_cmd->general_params.dual_stream_ant_msk = 3;
        lq_cmd->agg_params.agg_dis_start_th = 3;
        lq_cmd->agg_params.agg_time_limit = cpu_to_le16(4000);
  }
@@@ -2460,21 -2496,25 +2460,21 @@@ static int open_file_generic(struct ino
        return 0;
  }
  static void rs_dbgfs_set_mcs(struct iwl4965_lq_sta *lq_sta,
 -                              struct iwl4965_rate *mcs, int index)
 +                              u32 *rate_n_flags, int index)
  {
 -      u32 base_rate;
 -
 -      if (lq_sta->band == IEEE80211_BAND_5GHZ)
 -              base_rate = 0x800D;
 -      else
 -              base_rate = 0x820A;
 -
 -      if (lq_sta->dbg_fixed.rate_n_flags) {
 -              if (index < 12)
 -                      mcs->rate_n_flags = lq_sta->dbg_fixed.rate_n_flags;
 -              else
 -                      mcs->rate_n_flags = base_rate;
 +      if (lq_sta->dbg_fixed_rate) {
 +              if (index < 12) {
 +                      *rate_n_flags = lq_sta->dbg_fixed_rate;
 +              } else {
 +                      if (lq_sta->band == IEEE80211_BAND_5GHZ)
 +                              *rate_n_flags = 0x800D;
 +                      else
 +                              *rate_n_flags = 0x820A;
 +              }
                IWL_DEBUG_RATE("Fixed rate ON\n");
 -              return;
 +      } else {
 +              IWL_DEBUG_RATE("Fixed rate OFF\n");
        }
 -
 -      IWL_DEBUG_RATE("Fixed rate OFF\n");
  }
  
  static ssize_t rs_sta_dbgfs_scale_table_write(struct file *file,
                return -EFAULT;
  
        if (sscanf(buf, "%x", &parsed_rate) == 1)
 -              lq_sta->dbg_fixed.rate_n_flags = parsed_rate;
 +              lq_sta->dbg_fixed_rate = parsed_rate;
        else
 -              lq_sta->dbg_fixed.rate_n_flags = 0;
 +              lq_sta->dbg_fixed_rate = 0;
  
 -      lq_sta->active_rate = 0x0FFF;   /* 1 - 54 MBits, includes CCK */
 -      lq_sta->active_siso_rate = 0x1FD0;      /* 6 - 60 MBits, no 9, no CCK */
 -      lq_sta->active_mimo_rate = 0x1FD0;      /* 6 - 60 MBits, no 9, no CCK */
 +      lq_sta->active_legacy_rate = 0x0FFF;    /* 1 - 54 MBits, includes CCK */
 +      lq_sta->active_siso_rate   = 0x1FD0;    /* 6 - 60 MBits, no 9, no CCK */
 +      lq_sta->active_mimo2_rate  = 0x1FD0;    /* 6 - 60 MBits, no 9, no CCK */
 +      lq_sta->active_mimo3_rate  = 0x1FD0;    /* 6 - 60 MBits, no 9, no CCK */
  
        IWL_DEBUG_RATE("sta_id %d rate 0x%X\n",
 -              lq_sta->lq.sta_id, lq_sta->dbg_fixed.rate_n_flags);
 +              lq_sta->lq.sta_id, lq_sta->dbg_fixed_rate);
  
 -      if (lq_sta->dbg_fixed.rate_n_flags) {
 -              rs_fill_link_cmd(lq_sta, &lq_sta->dbg_fixed, &lq_sta->lq);
 +      if (lq_sta->dbg_fixed_rate) {
 +              rs_fill_link_cmd(NULL, lq_sta, lq_sta->dbg_fixed_rate);
                iwl_send_lq_cmd(lq_sta->drv, &lq_sta->lq, CMD_ASYNC);
        }
  
@@@ -2523,9 -2562,9 +2523,9 @@@ static ssize_t rs_sta_dbgfs_scale_table
        desc += sprintf(buff+desc, "sta_id %d\n", lq_sta->lq.sta_id);
        desc += sprintf(buff+desc, "failed=%d success=%d rate=0%X\n",
                        lq_sta->total_failed, lq_sta->total_success,
 -                      lq_sta->active_rate);
 +                      lq_sta->active_legacy_rate);
        desc += sprintf(buff+desc, "fixed rate 0x%X\n",
 -                      lq_sta->dbg_fixed.rate_n_flags);
 +                      lq_sta->dbg_fixed_rate);
        desc += sprintf(buff+desc, "general:"
                "flags=0x%X mimo-d=%d s-ant0x%x d-ant=0x%x\n",
                lq_sta->lq.general_params.flags,
@@@ -2575,7 -2614,7 +2575,7 @@@ static ssize_t rs_sta_dbgfs_stats_table
                                lq_sta->lq_info[i].is_SGI,
                                lq_sta->lq_info[i].is_fat,
                                lq_sta->lq_info[i].is_dup,
 -                              lq_sta->lq_info[i].current_rate.rate_n_flags);
 +                              lq_sta->lq_info[i].current_rate);
                for (j = 0; j < IWL_RATE_COUNT; j++) {
                        desc += sprintf(buff+desc,
                                "counter=%d success=%d %%=%d\n",
@@@ -2665,7 -2704,7 +2665,7 @@@ int iwl4965_fill_rs_info(struct ieee802
        lq_sta = (void *)sta->rate_ctrl_priv;
  
        lq_type = lq_sta->lq_info[lq_sta->active_tbl].lq_type;
 -      antenna = lq_sta->lq_info[lq_sta->active_tbl].antenna_type;
 +      antenna = lq_sta->lq_info[lq_sta->active_tbl].ant_type;
  
        if (is_legacy(lq_type))
                i = IWL_RATE_54M_INDEX;
  #include <asm/unaligned.h>
  
  #include "iwl-eeprom.h"
 -#include "iwl-4965.h"
 +#include "iwl-dev.h"
  #include "iwl-core.h"
  #include "iwl-io.h"
  #include "iwl-helpers.h"
 +#include "iwl-calib.h"
  
  /* module parameters */
  static struct iwl_mod_params iwl4965_mod_params = {
 -      .num_of_queues = IWL4965_MAX_NUM_QUEUES,
 +      .num_of_queues = IWL49_NUM_QUEUES,
        .enable_qos = 1,
        .amsdu_size_8K = 1,
        /* the rest are 0 by default */
  
  static void iwl4965_hw_card_show_info(struct iwl_priv *priv);
  
 -#define IWL_DECLARE_RATE_INFO(r, s, ip, in, rp, rn, pp, np)    \
 -      [IWL_RATE_##r##M_INDEX] = { IWL_RATE_##r##M_PLCP,      \
 -                                  IWL_RATE_SISO_##s##M_PLCP, \
 -                                  IWL_RATE_MIMO_##s##M_PLCP, \
 -                                  IWL_RATE_##r##M_IEEE,      \
 -                                  IWL_RATE_##ip##M_INDEX,    \
 -                                  IWL_RATE_##in##M_INDEX,    \
 -                                  IWL_RATE_##rp##M_INDEX,    \
 -                                  IWL_RATE_##rn##M_INDEX,    \
 -                                  IWL_RATE_##pp##M_INDEX,    \
 -                                  IWL_RATE_##np##M_INDEX }
 -
 -/*
 - * Parameter order:
 - *   rate, ht rate, prev rate, next rate, prev tgg rate, next tgg rate
 - *
 - * If there isn't a valid next or previous rate then INV is used which
 - * maps to IWL_RATE_INVALID
 - *
 - */
 -const struct iwl4965_rate_info iwl4965_rates[IWL_RATE_COUNT] = {
 -      IWL_DECLARE_RATE_INFO(1, INV, INV, 2, INV, 2, INV, 2),    /*  1mbps */
 -      IWL_DECLARE_RATE_INFO(2, INV, 1, 5, 1, 5, 1, 5),          /*  2mbps */
 -      IWL_DECLARE_RATE_INFO(5, INV, 2, 6, 2, 11, 2, 11),        /*5.5mbps */
 -      IWL_DECLARE_RATE_INFO(11, INV, 9, 12, 9, 12, 5, 18),      /* 11mbps */
 -      IWL_DECLARE_RATE_INFO(6, 6, 5, 9, 5, 11, 5, 11),        /*  6mbps */
 -      IWL_DECLARE_RATE_INFO(9, 6, 6, 11, 6, 11, 5, 11),       /*  9mbps */
 -      IWL_DECLARE_RATE_INFO(12, 12, 11, 18, 11, 18, 11, 18),   /* 12mbps */
 -      IWL_DECLARE_RATE_INFO(18, 18, 12, 24, 12, 24, 11, 24),   /* 18mbps */
 -      IWL_DECLARE_RATE_INFO(24, 24, 18, 36, 18, 36, 18, 36),   /* 24mbps */
 -      IWL_DECLARE_RATE_INFO(36, 36, 24, 48, 24, 48, 24, 48),   /* 36mbps */
 -      IWL_DECLARE_RATE_INFO(48, 48, 36, 54, 36, 54, 36, 54),   /* 48mbps */
 -      IWL_DECLARE_RATE_INFO(54, 54, 48, INV, 48, INV, 48, INV),/* 54mbps */
 -      IWL_DECLARE_RATE_INFO(60, 60, 48, INV, 48, INV, 48, INV),/* 60mbps */
 -};
 -
  #ifdef CONFIG_IWL4965_HT
  
  static const u16 default_tid_to_tx_fifo[] = {
@@@ -224,12 -259,120 +224,12 @@@ static int iwl4965_load_bsm(struct iwl_
        return 0;
  }
  
 -static int iwl4965_init_drv(struct iwl_priv *priv)
 -{
 -      int ret;
 -      int i;
 -
 -      priv->antenna = (enum iwl4965_antenna)priv->cfg->mod_params->antenna;
 -      priv->retry_rate = 1;
 -      priv->ibss_beacon = NULL;
 -
 -      spin_lock_init(&priv->lock);
 -      spin_lock_init(&priv->power_data.lock);
 -      spin_lock_init(&priv->sta_lock);
 -      spin_lock_init(&priv->hcmd_lock);
 -      spin_lock_init(&priv->lq_mngr.lock);
 -
 -      priv->shared_virt = pci_alloc_consistent(priv->pci_dev,
 -                                      sizeof(struct iwl4965_shared),
 -                                      &priv->shared_phys);
 -
 -      if (!priv->shared_virt) {
 -              ret = -ENOMEM;
 -              goto err;
 -      }
 -
 -      memset(priv->shared_virt, 0, sizeof(struct iwl4965_shared));
 -
 -
 -      for (i = 0; i < IWL_IBSS_MAC_HASH_SIZE; i++)
 -              INIT_LIST_HEAD(&priv->ibss_mac_hash[i]);
 -
 -      INIT_LIST_HEAD(&priv->free_frames);
 -
 -      mutex_init(&priv->mutex);
 -
 -      /* Clear the driver's (not device's) station table */
 -      iwlcore_clear_stations_table(priv);
 -
 -      priv->data_retry_limit = -1;
 -      priv->ieee_channels = NULL;
 -      priv->ieee_rates = NULL;
 -      priv->band = IEEE80211_BAND_2GHZ;
 -
 -      priv->iw_mode = IEEE80211_IF_TYPE_STA;
 -
 -      priv->use_ant_b_for_management_frame = 1; /* start with ant B */
 -      priv->valid_antenna = 0x7;      /* assume all 3 connected */
 -      priv->ps_mode = IWL_MIMO_PS_NONE;
 -
 -      /* Choose which receivers/antennas to use */
 -      iwl4965_set_rxon_chain(priv);
 -
 -      iwlcore_reset_qos(priv);
 -
 -      priv->qos_data.qos_active = 0;
 -      priv->qos_data.qos_cap.val = 0;
 -
 -      iwlcore_set_rxon_channel(priv, IEEE80211_BAND_2GHZ, 6);
 -
 -      priv->rates_mask = IWL_RATES_MASK;
 -      /* If power management is turned on, default to AC mode */
 -      priv->power_mode = IWL_POWER_AC;
 -      priv->user_txpower_limit = IWL_DEFAULT_TX_POWER;
 -
 -      ret = iwl_init_channel_map(priv);
 -      if (ret) {
 -              IWL_ERROR("initializing regulatory failed: %d\n", ret);
 -              goto err;
 -      }
 -
 -      ret = iwl4965_init_geos(priv);
 -      if (ret) {
 -              IWL_ERROR("initializing geos failed: %d\n", ret);
 -              goto err_free_channel_map;
 -      }
 -
 -      ret = ieee80211_register_hw(priv->hw);
 -      if (ret) {
 -              IWL_ERROR("Failed to register network device (error %d)\n",
 -                              ret);
 -              goto err_free_geos;
 -      }
 -
 -      priv->hw->conf.beacon_int = 100;
 -      priv->mac80211_registered = 1;
 -
 -      return 0;
 -
 -err_free_geos:
 -      iwl4965_free_geos(priv);
 -err_free_channel_map:
 -      iwl_free_channel_map(priv);
 -err:
 -      return ret;
 -}
 -
  static int is_fat_channel(__le32 rxon_flags)
  {
        return (rxon_flags & RXON_FLG_CHANNEL_MODE_PURE_40_MSK) ||
                (rxon_flags & RXON_FLG_CHANNEL_MODE_MIXED_MSK);
  }
  
 -static u8 is_single_stream(struct iwl_priv *priv)
 -{
 -#ifdef CONFIG_IWL4965_HT
 -      if (!priv->current_ht_config.is_ht ||
 -          (priv->current_ht_config.supp_mcs_set[1] == 0) ||
 -          (priv->ps_mode == IWL_MIMO_PS_STATIC))
 -              return 1;
 -#else
 -      return 1;
 -#endif        /*CONFIG_IWL4965_HT */
 -      return 0;
 -}
 -
  int iwl4965_hwrate_to_plcp_idx(u32 rate_n_flags)
  {
        int idx = 0;
        if (rate_n_flags & RATE_MCS_HT_MSK) {
                idx = (rate_n_flags & 0xff);
  
 -              if (idx >= IWL_RATE_MIMO_6M_PLCP)
 -                      idx = idx - IWL_RATE_MIMO_6M_PLCP;
 +              if (idx >= IWL_RATE_MIMO2_6M_PLCP)
 +                      idx = idx - IWL_RATE_MIMO2_6M_PLCP;
  
                idx += IWL_FIRST_OFDM_RATE;
                /* skip 9M not supported in ht*/
@@@ -267,7 -410,7 +267,7 @@@ void iwl4965_hwrate_to_tx_control(struc
        int rate_index;
  
        control->antenna_sel_tx =
 -              ((rate_n_flags & RATE_MCS_ANT_AB_MSK) >> RATE_MCS_ANT_POS);
 +              ((rate_n_flags & RATE_MCS_ANT_ABC_MSK) >> RATE_MCS_ANT_POS);
        if (rate_n_flags & RATE_MCS_HT_MSK)
                control->flags |= IEEE80211_TXCTL_OFDM_HT;
        if (rate_n_flags & RATE_MCS_GF_MSK)
                        &priv->bands[IEEE80211_BAND_2GHZ].bitrates[rate_index];
  }
  
 -/*
 - * Determine how many receiver/antenna chains to use.
 - * More provides better reception via diversity.  Fewer saves power.
 - * MIMO (dual stream) requires at least 2, but works better with 3.
 - * This does not determine *which* chains to use, just how many.
 - */
 -static int iwl4965_get_rx_chain_counter(struct iwl_priv *priv,
 -                                      u8 *idle_state, u8 *rx_state)
 -{
 -      u8 is_single = is_single_stream(priv);
 -      u8 is_cam = test_bit(STATUS_POWER_PMI, &priv->status) ? 0 : 1;
 -
 -      /* # of Rx chains to use when expecting MIMO. */
 -      if (is_single || (!is_cam && (priv->ps_mode == IWL_MIMO_PS_STATIC)))
 -              *rx_state = 2;
 -      else
 -              *rx_state = 3;
 -
 -      /* # Rx chains when idling and maybe trying to save power */
 -      switch (priv->ps_mode) {
 -      case IWL_MIMO_PS_STATIC:
 -      case IWL_MIMO_PS_DYNAMIC:
 -              *idle_state = (is_cam) ? 2 : 1;
 -              break;
 -      case IWL_MIMO_PS_NONE:
 -              *idle_state = (is_cam) ? *rx_state : 1;
 -              break;
 -      default:
 -              *idle_state = 1;
 -              break;
 -      }
 -
 -      return 0;
 -}
 -
  int iwl4965_hw_rxq_stop(struct iwl_priv *priv)
  {
        int rc;
        return 0;
  }
  
 -u8 iwl4965_hw_find_station(struct iwl_priv *priv, const u8 *addr)
 +/*
 + * EEPROM handlers
 + */
 +
 +static int iwl4965_eeprom_check_version(struct iwl_priv *priv)
  {
 -      int i;
 -      int start = 0;
 -      int ret = IWL_INVALID_STATION;
 -      unsigned long flags;
 -      DECLARE_MAC_BUF(mac);
 +      u16 eeprom_ver;
 +      u16 calib_ver;
  
 -      if ((priv->iw_mode == IEEE80211_IF_TYPE_IBSS) ||
 -          (priv->iw_mode == IEEE80211_IF_TYPE_AP))
 -              start = IWL_STA_ID;
 +      eeprom_ver = iwl_eeprom_query16(priv, EEPROM_VERSION);
  
 -      if (is_broadcast_ether_addr(addr))
 -              return priv->hw_params.bcast_sta_id;
 +      calib_ver = iwl_eeprom_query16(priv, EEPROM_4965_CALIB_VERSION_OFFSET);
  
 -      spin_lock_irqsave(&priv->sta_lock, flags);
 -      for (i = start; i < priv->hw_params.max_stations; i++)
 -              if ((priv->stations[i].used) &&
 -                  (!compare_ether_addr
 -                   (priv->stations[i].sta.sta.addr, addr))) {
 -                      ret = i;
 -                      goto out;
 -              }
 +      if (eeprom_ver < EEPROM_4965_EEPROM_VERSION ||
 +          calib_ver < EEPROM_4965_TX_POWER_VERSION)
 +              goto err;
  
 -      IWL_DEBUG_ASSOC_LIMIT("can not find STA %s total %d\n",
 -                      print_mac(mac, addr), priv->num_stations);
 +      return 0;
 +err:
 +      IWL_ERROR("Unsuported EEPROM VER=0x%x < 0x%x CALIB=0x%x < 0x%x\n",
 +                eeprom_ver, EEPROM_4965_EEPROM_VERSION,
 +                calib_ver, EEPROM_4965_TX_POWER_VERSION);
 +      return -EINVAL;
  
 - out:
 -      spin_unlock_irqrestore(&priv->sta_lock, flags);
 -      return ret;
  }
 -
 -static int iwl4965_nic_set_pwr_src(struct iwl_priv *priv, int pwr_max)
 +int iwl4965_set_pwr_src(struct iwl_priv *priv, enum iwl_pwr_src src)
  {
        int ret;
        unsigned long flags;
                return ret;
        }
  
 -      if (!pwr_max) {
 +      if (src == IWL_PWR_SRC_VAUX) {
                u32 val;
 -
                ret = pci_read_config_dword(priv->pci_dev, PCI_POWER_SOURCE,
 -                                         &val);
 +                                          &val);
  
 -              if (val & PCI_CFG_PMC_PME_FROM_D3COLD_SUPPORT)
 +              if (val & PCI_CFG_PMC_PME_FROM_D3COLD_SUPPORT) {
                        iwl_set_bits_mask_prph(priv, APMG_PS_CTRL_REG,
 -                              APMG_PS_CTRL_VAL_PWR_SRC_VAUX,
 -                              ~APMG_PS_CTRL_MSK_PWR_SRC);
 -      } else
 +                                             APMG_PS_CTRL_VAL_PWR_SRC_VAUX,
 +                                             ~APMG_PS_CTRL_MSK_PWR_SRC);
 +              }
 +      } else {
                iwl_set_bits_mask_prph(priv, APMG_PS_CTRL_REG,
 -                      APMG_PS_CTRL_VAL_PWR_SRC_VMAIN,
 -                      ~APMG_PS_CTRL_MSK_PWR_SRC);
 +                                     APMG_PS_CTRL_VAL_PWR_SRC_VMAIN,
 +                                     ~APMG_PS_CTRL_MSK_PWR_SRC);
 +      }
  
        iwl_release_nic_access(priv);
        spin_unlock_irqrestore(&priv->lock, flags);
@@@ -435,7 -619,7 +435,7 @@@ static int iwl4965_kw_init(struct iwl_p
        if (rc)
                goto out;
  
 -      iwl_write_direct32(priv, IWL_FH_KW_MEM_ADDR_REG,
 +      iwl_write_direct32(priv, FH_KW_MEM_ADDR_REG,
                             priv->kw.dma_addr >> 4);
        iwl_release_nic_access(priv);
  out:
@@@ -538,115 -722,116 +538,115 @@@ static int iwl4965_txq_ctx_reset(struc
   error_kw:
        return rc;
  }
 -
 -int iwl4965_hw_nic_init(struct iwl_priv *priv)
 +static int iwl4965_apm_init(struct iwl_priv *priv)
  {
 -      int rc;
        unsigned long flags;
 -      struct iwl4965_rx_queue *rxq = &priv->rxq;
 -      u8 rev_id;
 -      u32 val;
 -      u8 val_link;
 -
 -      iwl4965_power_init_handle(priv);
 +      int ret = 0;
  
 -      /* nic_init */
        spin_lock_irqsave(&priv->lock, flags);
 -
        iwl_set_bit(priv, CSR_GIO_CHICKEN_BITS,
 -                  CSR_GIO_CHICKEN_BITS_REG_BIT_DIS_L0S_EXIT_TIMER);
 +                        CSR_GIO_CHICKEN_BITS_REG_BIT_DIS_L0S_EXIT_TIMER);
  
 +      /* set "initialization complete" bit to move adapter
 +       * D0U* --> D0A* state */
        iwl_set_bit(priv, CSR_GP_CNTRL, CSR_GP_CNTRL_REG_FLAG_INIT_DONE);
 -      rc = iwl_poll_bit(priv, CSR_GP_CNTRL,
 -                        CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY,
 -                        CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY, 25000);
 -      if (rc < 0) {
 -              spin_unlock_irqrestore(&priv->lock, flags);
 -              IWL_DEBUG_INFO("Failed to init the card\n");
 -              return rc;
 -      }
  
 -      rc = iwl_grab_nic_access(priv);
 -      if (rc) {
 -              spin_unlock_irqrestore(&priv->lock, flags);
 -              return rc;
 +      /* wait for clock stabilization */
 +      ret = iwl_poll_bit(priv, CSR_GP_CNTRL,
 +                         CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY,
 +                         CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY, 25000);
 +      if (ret < 0) {
 +              IWL_DEBUG_INFO("Failed to init the card\n");
 +              goto out;
        }
  
 -      iwl_read_prph(priv, APMG_CLK_CTRL_REG);
 +      ret = iwl_grab_nic_access(priv);
 +      if (ret)
 +              goto out;
  
 +      /* enable DMA */
        iwl_write_prph(priv, APMG_CLK_CTRL_REG,
                        APMG_CLK_VAL_DMA_CLK_RQT | APMG_CLK_VAL_BSM_CLK_RQT);
 -      iwl_read_prph(priv, APMG_CLK_CTRL_REG);
  
        udelay(20);
  
        iwl_set_bits_prph(priv, APMG_PCIDEV_STT_REG,
 -                              APMG_PCIDEV_STT_VAL_L1_ACT_DIS);
 +                        APMG_PCIDEV_STT_VAL_L1_ACT_DIS);
  
        iwl_release_nic_access(priv);
 -      iwl_write32(priv, CSR_INT_COALESCING, 512 / 32);
 +out:
        spin_unlock_irqrestore(&priv->lock, flags);
 +      return ret;
 +}
  
 -      /* Determine HW type */
 -      rc = pci_read_config_byte(priv->pci_dev, PCI_REVISION_ID, &rev_id);
 -      if (rc)
 -              return rc;
  
 -      IWL_DEBUG_INFO("HW Revision ID = 0x%X\n", rev_id);
 +static void iwl4965_nic_config(struct iwl_priv *priv)
 +{
 +      unsigned long flags;
 +      u32 val;
 +      u16 radio_cfg;
 +      u8 val_link;
  
 -      iwl4965_nic_set_pwr_src(priv, 1);
        spin_lock_irqsave(&priv->lock, flags);
  
 -      if ((rev_id & 0x80) == 0x80 && (rev_id & 0x7f) < 8) {
 +      if ((priv->rev_id & 0x80) == 0x80 && (priv->rev_id & 0x7f) < 8) {
                pci_read_config_dword(priv->pci_dev, PCI_REG_WUM8, &val);
                /* Enable No Snoop field */
                pci_write_config_dword(priv->pci_dev, PCI_REG_WUM8,
                                       val & ~(1 << 11));
        }
  
 -      spin_unlock_irqrestore(&priv->lock, flags);
 -
 -      if (priv->eeprom.calib_version < EEPROM_TX_POWER_VERSION_NEW) {
 -              IWL_ERROR("Older EEPROM detected!  Aborting.\n");
 -              return -EINVAL;
 -      }
 -
        pci_read_config_byte(priv->pci_dev, PCI_LINK_CTRL, &val_link);
  
        /* disable L1 entry -- workaround for pre-B1 */
        pci_write_config_byte(priv->pci_dev, PCI_LINK_CTRL, val_link & ~0x02);
  
 -      spin_lock_irqsave(&priv->lock, flags);
 +      radio_cfg = iwl_eeprom_query16(priv, EEPROM_RADIO_CONFIG);
  
 -      /* set CSR_HW_CONFIG_REG for uCode use */
 +      /* write radio config values to register */
 +      if (EEPROM_RF_CFG_TYPE_MSK(radio_cfg) == EEPROM_4965_RF_CFG_TYPE_MAX)
 +              iwl_set_bit(priv, CSR_HW_IF_CONFIG_REG,
 +                          EEPROM_RF_CFG_TYPE_MSK(radio_cfg) |
 +                          EEPROM_RF_CFG_STEP_MSK(radio_cfg) |
 +                          EEPROM_RF_CFG_DASH_MSK(radio_cfg));
  
 +      /* set CSR_HW_CONFIG_REG for uCode use */
        iwl_set_bit(priv, CSR_HW_IF_CONFIG_REG,
 -                  CSR49_HW_IF_CONFIG_REG_BIT_4965_R |
 -                  CSR49_HW_IF_CONFIG_REG_BIT_RADIO_SI |
 -                  CSR49_HW_IF_CONFIG_REG_BIT_MAC_SI);
 +                  CSR_HW_IF_CONFIG_REG_BIT_RADIO_SI |
 +                  CSR_HW_IF_CONFIG_REG_BIT_MAC_SI);
  
 -      rc = iwl_grab_nic_access(priv);
 -      if (rc < 0) {
 -              spin_unlock_irqrestore(&priv->lock, flags);
 -              IWL_DEBUG_INFO("Failed to init the card\n");
 -              return rc;
 -      }
 +      priv->calib_info = (struct iwl_eeprom_calib_info *)
 +              iwl_eeprom_query_addr(priv, EEPROM_4965_CALIB_TXPOWER_OFFSET);
  
 -      iwl_read_prph(priv, APMG_PS_CTRL_REG);
 -      iwl_set_bits_prph(priv, APMG_PS_CTRL_REG, APMG_PS_CTRL_VAL_RESET_REQ);
 -      udelay(5);
 -      iwl_clear_bits_prph(priv, APMG_PS_CTRL_REG, APMG_PS_CTRL_VAL_RESET_REQ);
 +      spin_unlock_irqrestore(&priv->lock, flags);
 +}
  
 -      iwl_release_nic_access(priv);
 +
 +int iwl4965_hw_nic_init(struct iwl_priv *priv)
 +{
 +      unsigned long flags;
 +      struct iwl4965_rx_queue *rxq = &priv->rxq;
 +      int ret;
 +
 +      /* nic_init */
 +      priv->cfg->ops->lib->apm_ops.init(priv);
 +
 +      spin_lock_irqsave(&priv->lock, flags);
 +      iwl_write32(priv, CSR_INT_COALESCING, 512 / 32);
        spin_unlock_irqrestore(&priv->lock, flags);
  
 +      ret = priv->cfg->ops->lib->apm_ops.set_pwr_src(priv, IWL_PWR_SRC_VMAIN);
 +
 +      priv->cfg->ops->lib->apm_ops.config(priv);
 +
        iwl4965_hw_card_show_info(priv);
  
        /* end nic_init */
  
        /* Allocate the RX queue, or reset if it is already allocated */
        if (!rxq->bd) {
 -              rc = iwl4965_rx_queue_alloc(priv);
 -              if (rc) {
 +              ret = iwl4965_rx_queue_alloc(priv);
 +              if (ret) {
                        IWL_ERROR("Unable to initialize Rx queue\n");
                        return -ENOMEM;
                }
        spin_unlock_irqrestore(&priv->lock, flags);
  
        /* Allocate and init all Tx and Command queues */
 -      rc = iwl4965_txq_ctx_reset(priv);
 -      if (rc)
 -              return rc;
 -
 -      if (priv->eeprom.sku_cap & EEPROM_SKU_CAP_SW_RF_KILL_ENABLE)
 -              IWL_DEBUG_RF_KILL("SW RF KILL supported in EEPROM.\n");
 -
 -      if (priv->eeprom.sku_cap & EEPROM_SKU_CAP_HW_RF_KILL_ENABLE)
 -              IWL_DEBUG_RF_KILL("HW RF KILL supported in EEPROM.\n");
 +      ret = iwl4965_txq_ctx_reset(priv);
 +      if (ret)
 +              return ret;
  
        set_bit(STATUS_INIT, &priv->status);
  
@@@ -725,9 -916,9 +725,9 @@@ void iwl4965_hw_txq_ctx_stop(struct iwl
                }
  
                iwl_write_direct32(priv,
 -                                 IWL_FH_TCSR_CHNL_TX_CONFIG_REG(txq_id), 0x0);
 -              iwl_poll_direct_bit(priv, IWL_FH_TSSR_TX_STATUS_REG,
 -                                  IWL_FH_TSSR_TX_STATUS_REG_MSK_CHNL_IDLE
 +                                 FH_TCSR_CHNL_TX_CONFIG_REG(txq_id), 0x0);
 +              iwl_poll_direct_bit(priv, FH_TSSR_TX_STATUS_REG,
 +                                  FH_TSSR_TX_STATUS_REG_MSK_CHNL_IDLE
                                    (txq_id), 200);
                iwl_release_nic_access(priv);
                spin_unlock_irqrestore(&priv->lock, flags);
@@@ -802,9 -993,15 +802,9 @@@ static void iwl4965_bg_statistics_perio
        iwl_send_statistics_request(priv, CMD_ASYNC);
  }
  
  void iwl4965_rf_kill_ct_config(struct iwl_priv *priv)
  {
        struct iwl4965_ct_kill_config cmd;
 -      u32 R1, R2, R3;
 -      u32 temp_th;
 -      u32 crit_temperature;
        unsigned long flags;
        int ret = 0;
  
                    CSR_UCODE_DRV_GP1_REG_BIT_CT_KILL_EXIT);
        spin_unlock_irqrestore(&priv->lock, flags);
  
 -      if (priv->statistics.flag & STATISTICS_REPLY_FLG_FAT_MODE_MSK) {
 -              R1 = (s32)le32_to_cpu(priv->card_alive_init.therm_r1[1]);
 -              R2 = (s32)le32_to_cpu(priv->card_alive_init.therm_r2[1]);
 -              R3 = (s32)le32_to_cpu(priv->card_alive_init.therm_r3[1]);
 -      } else {
 -              R1 = (s32)le32_to_cpu(priv->card_alive_init.therm_r1[0]);
 -              R2 = (s32)le32_to_cpu(priv->card_alive_init.therm_r2[0]);
 -              R3 = (s32)le32_to_cpu(priv->card_alive_init.therm_r3[0]);
 -      }
 -
 -      temp_th = CELSIUS_TO_KELVIN(TM_CT_KILL_THRESHOLD);
 +      cmd.critical_temperature_R =
 +              cpu_to_le32(priv->hw_params.ct_kill_threshold);
  
 -      crit_temperature = ((temp_th * (R3-R1))/CT_LIMIT_CONST) + R2;
 -      cmd.critical_temperature_R =  cpu_to_le32(crit_temperature);
        ret = iwl_send_cmd_pdu(priv, REPLY_CT_KILL_CONFIG_CMD,
                               sizeof(cmd), &cmd);
        if (ret)
                IWL_ERROR("REPLY_CT_KILL_CONFIG_CMD failed\n");
        else
 -              IWL_DEBUG_INFO("REPLY_CT_KILL_CONFIG_CMD succeeded\n");
 -}
 -
 -#ifdef CONFIG_IWL4965_SENSITIVITY
 -
 -/* "false alarms" are signals that our DSP tries to lock onto,
 - *   but then determines that they are either noise, or transmissions
 - *   from a distant wireless network (also "noise", really) that get
 - *   "stepped on" by stronger transmissions within our own network.
 - * This algorithm attempts to set a sensitivity level that is high
 - *   enough to receive all of our own network traffic, but not so
 - *   high that our DSP gets too busy trying to lock onto non-network
 - *   activity/noise. */
 -static int iwl4965_sens_energy_cck(struct iwl_priv *priv,
 -                                 u32 norm_fa,
 -                                 u32 rx_enable_time,
 -                                 struct statistics_general_data *rx_info)
 -{
 -      u32 max_nrg_cck = 0;
 -      int i = 0;
 -      u8 max_silence_rssi = 0;
 -      u32 silence_ref = 0;
 -      u8 silence_rssi_a = 0;
 -      u8 silence_rssi_b = 0;
 -      u8 silence_rssi_c = 0;
 -      u32 val;
 -
 -      /* "false_alarms" values below are cross-multiplications to assess the
 -       *   numbers of false alarms within the measured period of actual Rx
 -       *   (Rx is off when we're txing), vs the min/max expected false alarms
 -       *   (some should be expected if rx is sensitive enough) in a
 -       *   hypothetical listening period of 200 time units (TU), 204.8 msec:
 -       *
 -       * MIN_FA/fixed-time < false_alarms/actual-rx-time < MAX_FA/beacon-time
 -       *
 -       * */
 -      u32 false_alarms = norm_fa * 200 * 1024;
 -      u32 max_false_alarms = MAX_FA_CCK * rx_enable_time;
 -      u32 min_false_alarms = MIN_FA_CCK * rx_enable_time;
 -      struct iwl4965_sensitivity_data *data = NULL;
 -
 -      data = &(priv->sensitivity_data);
 -
 -      data->nrg_auto_corr_silence_diff = 0;
 -
 -      /* Find max silence rssi among all 3 receivers.
 -       * This is background noise, which may include transmissions from other
 -       *    networks, measured during silence before our network's beacon */
 -      silence_rssi_a = (u8)((rx_info->beacon_silence_rssi_a &
 -                          ALL_BAND_FILTER) >> 8);
 -      silence_rssi_b = (u8)((rx_info->beacon_silence_rssi_b &
 -                          ALL_BAND_FILTER) >> 8);
 -      silence_rssi_c = (u8)((rx_info->beacon_silence_rssi_c &
 -                          ALL_BAND_FILTER) >> 8);
 -
 -      val = max(silence_rssi_b, silence_rssi_c);
 -      max_silence_rssi = max(silence_rssi_a, (u8) val);
 -
 -      /* Store silence rssi in 20-beacon history table */
 -      data->nrg_silence_rssi[data->nrg_silence_idx] = max_silence_rssi;
 -      data->nrg_silence_idx++;
 -      if (data->nrg_silence_idx >= NRG_NUM_PREV_STAT_L)
 -              data->nrg_silence_idx = 0;
 -
 -      /* Find max silence rssi across 20 beacon history */
 -      for (i = 0; i < NRG_NUM_PREV_STAT_L; i++) {
 -              val = data->nrg_silence_rssi[i];
 -              silence_ref = max(silence_ref, val);
 -      }
 -      IWL_DEBUG_CALIB("silence a %u, b %u, c %u, 20-bcn max %u\n",
 -                      silence_rssi_a, silence_rssi_b, silence_rssi_c,
 -                      silence_ref);
 -
 -      /* Find max rx energy (min value!) among all 3 receivers,
 -       *   measured during beacon frame.
 -       * Save it in 10-beacon history table. */
 -      i = data->nrg_energy_idx;
 -      val = min(rx_info->beacon_energy_b, rx_info->beacon_energy_c);
 -      data->nrg_value[i] = min(rx_info->beacon_energy_a, val);
 -
 -      data->nrg_energy_idx++;
 -      if (data->nrg_energy_idx >= 10)
 -              data->nrg_energy_idx = 0;
 -
 -      /* Find min rx energy (max value) across 10 beacon history.
 -       * This is the minimum signal level that we want to receive well.
 -       * Add backoff (margin so we don't miss slightly lower energy frames).
 -       * This establishes an upper bound (min value) for energy threshold. */
 -      max_nrg_cck = data->nrg_value[0];
 -      for (i = 1; i < 10; i++)
 -              max_nrg_cck = (u32) max(max_nrg_cck, (data->nrg_value[i]));
 -      max_nrg_cck += 6;
 -
 -      IWL_DEBUG_CALIB("rx energy a %u, b %u, c %u, 10-bcn max/min %u\n",
 -                      rx_info->beacon_energy_a, rx_info->beacon_energy_b,
 -                      rx_info->beacon_energy_c, max_nrg_cck - 6);
 -
 -      /* Count number of consecutive beacons with fewer-than-desired
 -       *   false alarms. */
 -      if (false_alarms < min_false_alarms)
 -              data->num_in_cck_no_fa++;
 -      else
 -              data->num_in_cck_no_fa = 0;
 -      IWL_DEBUG_CALIB("consecutive bcns with few false alarms = %u\n",
 -                      data->num_in_cck_no_fa);
 -
 -      /* If we got too many false alarms this time, reduce sensitivity */
 -      if (false_alarms > max_false_alarms) {
 -              IWL_DEBUG_CALIB("norm FA %u > max FA %u\n",
 -                           false_alarms, max_false_alarms);
 -              IWL_DEBUG_CALIB("... reducing sensitivity\n");
 -              data->nrg_curr_state = IWL_FA_TOO_MANY;
 -
 -              if (data->auto_corr_cck > AUTO_CORR_MAX_TH_CCK) {
 -                      /* Store for "fewer than desired" on later beacon */
 -                      data->nrg_silence_ref = silence_ref;
 -
 -                      /* increase energy threshold (reduce nrg value)
 -                       *   to decrease sensitivity */
 -                      if (data->nrg_th_cck > (NRG_MAX_CCK + NRG_STEP_CCK))
 -                              data->nrg_th_cck = data->nrg_th_cck
 -                                                       - NRG_STEP_CCK;
 -              }
 -
 -              /* increase auto_corr values to decrease sensitivity */
 -              if (data->auto_corr_cck < AUTO_CORR_MAX_TH_CCK)
 -                      data->auto_corr_cck = AUTO_CORR_MAX_TH_CCK + 1;
 -              else {
 -                      val = data->auto_corr_cck + AUTO_CORR_STEP_CCK;
 -                      data->auto_corr_cck = min((u32)AUTO_CORR_MAX_CCK, val);
 -              }
 -              val = data->auto_corr_cck_mrc + AUTO_CORR_STEP_CCK;
 -              data->auto_corr_cck_mrc = min((u32)AUTO_CORR_MAX_CCK_MRC, val);
 -
 -      /* Else if we got fewer than desired, increase sensitivity */
 -      } else if (false_alarms < min_false_alarms) {
 -              data->nrg_curr_state = IWL_FA_TOO_FEW;
 -
 -              /* Compare silence level with silence level for most recent
 -               *   healthy number or too many false alarms */
 -              data->nrg_auto_corr_silence_diff = (s32)data->nrg_silence_ref -
 -                                                 (s32)silence_ref;
 -
 -              IWL_DEBUG_CALIB("norm FA %u < min FA %u, silence diff %d\n",
 -                       false_alarms, min_false_alarms,
 -                       data->nrg_auto_corr_silence_diff);
 -
 -              /* Increase value to increase sensitivity, but only if:
 -               * 1a) previous beacon did *not* have *too many* false alarms
 -               * 1b) AND there's a significant difference in Rx levels
 -               *      from a previous beacon with too many, or healthy # FAs
 -               * OR 2) We've seen a lot of beacons (100) with too few
 -               *       false alarms */
 -              if ((data->nrg_prev_state != IWL_FA_TOO_MANY) &&
 -                      ((data->nrg_auto_corr_silence_diff > NRG_DIFF) ||
 -                      (data->num_in_cck_no_fa > MAX_NUMBER_CCK_NO_FA))) {
 -
 -                      IWL_DEBUG_CALIB("... increasing sensitivity\n");
 -                      /* Increase nrg value to increase sensitivity */
 -                      val = data->nrg_th_cck + NRG_STEP_CCK;
 -                      data->nrg_th_cck = min((u32)NRG_MIN_CCK, val);
 -
 -                      /* Decrease auto_corr values to increase sensitivity */
 -                      val = data->auto_corr_cck - AUTO_CORR_STEP_CCK;
 -                      data->auto_corr_cck = max((u32)AUTO_CORR_MIN_CCK, val);
 -
 -                      val = data->auto_corr_cck_mrc - AUTO_CORR_STEP_CCK;
 -                      data->auto_corr_cck_mrc =
 -                                       max((u32)AUTO_CORR_MIN_CCK_MRC, val);
 -
 -              } else
 -                      IWL_DEBUG_CALIB("... but not changing sensitivity\n");
 -
 -      /* Else we got a healthy number of false alarms, keep status quo */
 -      } else {
 -              IWL_DEBUG_CALIB(" FA in safe zone\n");
 -              data->nrg_curr_state = IWL_FA_GOOD_RANGE;
 -
 -              /* Store for use in "fewer than desired" with later beacon */
 -              data->nrg_silence_ref = silence_ref;
 -
 -              /* If previous beacon had too many false alarms,
 -               *   give it some extra margin by reducing sensitivity again
 -               *   (but don't go below measured energy of desired Rx) */
 -              if (IWL_FA_TOO_MANY == data->nrg_prev_state) {
 -                      IWL_DEBUG_CALIB("... increasing margin\n");
 -                      data->nrg_th_cck -= NRG_MARGIN;
 -              }
 -      }
 -
 -      /* Make sure the energy threshold does not go above the measured
 -       * energy of the desired Rx signals (reduced by backoff margin),
 -       * or else we might start missing Rx frames.
 -       * Lower value is higher energy, so we use max()!
 -       */
 -      data->nrg_th_cck = max(max_nrg_cck, data->nrg_th_cck);
 -      IWL_DEBUG_CALIB("new nrg_th_cck %u\n", data->nrg_th_cck);
 -
 -      data->nrg_prev_state = data->nrg_curr_state;
 -
 -      return 0;
 -}
 -
 -
 -static int iwl4965_sens_auto_corr_ofdm(struct iwl_priv *priv,
 -                                     u32 norm_fa,
 -                                     u32 rx_enable_time)
 -{
 -      u32 val;
 -      u32 false_alarms = norm_fa * 200 * 1024;
 -      u32 max_false_alarms = MAX_FA_OFDM * rx_enable_time;
 -      u32 min_false_alarms = MIN_FA_OFDM * rx_enable_time;
 -      struct iwl4965_sensitivity_data *data = NULL;
 -
 -      data = &(priv->sensitivity_data);
 -
 -      /* If we got too many false alarms this time, reduce sensitivity */
 -      if (false_alarms > max_false_alarms) {
 -
 -              IWL_DEBUG_CALIB("norm FA %u > max FA %u)\n",
 -                           false_alarms, max_false_alarms);
 -
 -              val = data->auto_corr_ofdm + AUTO_CORR_STEP_OFDM;
 -              data->auto_corr_ofdm =
 -                              min((u32)AUTO_CORR_MAX_OFDM, val);
 -
 -              val = data->auto_corr_ofdm_mrc + AUTO_CORR_STEP_OFDM;
 -              data->auto_corr_ofdm_mrc =
 -                              min((u32)AUTO_CORR_MAX_OFDM_MRC, val);
 -
 -              val = data->auto_corr_ofdm_x1 + AUTO_CORR_STEP_OFDM;
 -              data->auto_corr_ofdm_x1 =
 -                              min((u32)AUTO_CORR_MAX_OFDM_X1, val);
 -
 -              val = data->auto_corr_ofdm_mrc_x1 + AUTO_CORR_STEP_OFDM;
 -              data->auto_corr_ofdm_mrc_x1 =
 -                              min((u32)AUTO_CORR_MAX_OFDM_MRC_X1, val);
 -      }
 -
 -      /* Else if we got fewer than desired, increase sensitivity */
 -      else if (false_alarms < min_false_alarms) {
 -
 -              IWL_DEBUG_CALIB("norm FA %u < min FA %u\n",
 -                           false_alarms, min_false_alarms);
 -
 -              val = data->auto_corr_ofdm - AUTO_CORR_STEP_OFDM;
 -              data->auto_corr_ofdm =
 -                              max((u32)AUTO_CORR_MIN_OFDM, val);
 -
 -              val = data->auto_corr_ofdm_mrc - AUTO_CORR_STEP_OFDM;
 -              data->auto_corr_ofdm_mrc =
 -                              max((u32)AUTO_CORR_MIN_OFDM_MRC, val);
 -
 -              val = data->auto_corr_ofdm_x1 - AUTO_CORR_STEP_OFDM;
 -              data->auto_corr_ofdm_x1 =
 -                              max((u32)AUTO_CORR_MIN_OFDM_X1, val);
 -
 -              val = data->auto_corr_ofdm_mrc_x1 - AUTO_CORR_STEP_OFDM;
 -              data->auto_corr_ofdm_mrc_x1 =
 -                              max((u32)AUTO_CORR_MIN_OFDM_MRC_X1, val);
 -      }
 -
 -      else
 -              IWL_DEBUG_CALIB("min FA %u < norm FA %u < max FA %u OK\n",
 -                       min_false_alarms, false_alarms, max_false_alarms);
 -
 -      return 0;
 -}
 -
 -static int iwl4965_sensitivity_callback(struct iwl_priv *priv,
 -                                  struct iwl_cmd *cmd, struct sk_buff *skb)
 -{
 -      /* We didn't cache the SKB; let the caller free it */
 -      return 1;
 -}
 -
 -/* Prepare a SENSITIVITY_CMD, send to uCode if values have changed */
 -static int iwl4965_sensitivity_write(struct iwl_priv *priv, u8 flags)
 -{
 -      struct iwl4965_sensitivity_cmd cmd ;
 -      struct iwl4965_sensitivity_data *data = NULL;
 -      struct iwl_host_cmd cmd_out = {
 -              .id = SENSITIVITY_CMD,
 -              .len = sizeof(struct iwl4965_sensitivity_cmd),
 -              .meta.flags = flags,
 -              .data = &cmd,
 -      };
 -      int ret;
 -
 -      data = &(priv->sensitivity_data);
 -
 -      memset(&cmd, 0, sizeof(cmd));
 -
 -      cmd.table[HD_AUTO_CORR32_X4_TH_ADD_MIN_INDEX] =
 -                              cpu_to_le16((u16)data->auto_corr_ofdm);
 -      cmd.table[HD_AUTO_CORR32_X4_TH_ADD_MIN_MRC_INDEX] =
 -                              cpu_to_le16((u16)data->auto_corr_ofdm_mrc);
 -      cmd.table[HD_AUTO_CORR32_X1_TH_ADD_MIN_INDEX] =
 -                              cpu_to_le16((u16)data->auto_corr_ofdm_x1);
 -      cmd.table[HD_AUTO_CORR32_X1_TH_ADD_MIN_MRC_INDEX] =
 -                              cpu_to_le16((u16)data->auto_corr_ofdm_mrc_x1);
 -
 -      cmd.table[HD_AUTO_CORR40_X4_TH_ADD_MIN_INDEX] =
 -                              cpu_to_le16((u16)data->auto_corr_cck);
 -      cmd.table[HD_AUTO_CORR40_X4_TH_ADD_MIN_MRC_INDEX] =
 -                              cpu_to_le16((u16)data->auto_corr_cck_mrc);
 -
 -      cmd.table[HD_MIN_ENERGY_CCK_DET_INDEX] =
 -                              cpu_to_le16((u16)data->nrg_th_cck);
 -      cmd.table[HD_MIN_ENERGY_OFDM_DET_INDEX] =
 -                              cpu_to_le16((u16)data->nrg_th_ofdm);
 -
 -      cmd.table[HD_BARKER_CORR_TH_ADD_MIN_INDEX] =
 -                              __constant_cpu_to_le16(190);
 -      cmd.table[HD_BARKER_CORR_TH_ADD_MIN_MRC_INDEX] =
 -                              __constant_cpu_to_le16(390);
 -      cmd.table[HD_OFDM_ENERGY_TH_IN_INDEX] =
 -                              __constant_cpu_to_le16(62);
 -
 -      IWL_DEBUG_CALIB("ofdm: ac %u mrc %u x1 %u mrc_x1 %u thresh %u\n",
 -                      data->auto_corr_ofdm, data->auto_corr_ofdm_mrc,
 -                      data->auto_corr_ofdm_x1, data->auto_corr_ofdm_mrc_x1,
 -                      data->nrg_th_ofdm);
 -
 -      IWL_DEBUG_CALIB("cck: ac %u mrc %u thresh %u\n",
 -                      data->auto_corr_cck, data->auto_corr_cck_mrc,
 -                      data->nrg_th_cck);
 -
 -      /* Update uCode's "work" table, and copy it to DSP */
 -      cmd.control = SENSITIVITY_CMD_CONTROL_WORK_TABLE;
 -
 -      if (flags & CMD_ASYNC)
 -              cmd_out.meta.u.callback = iwl4965_sensitivity_callback;
 -
 -      /* Don't send command to uCode if nothing has changed */
 -      if (!memcmp(&cmd.table[0], &(priv->sensitivity_tbl[0]),
 -                  sizeof(u16)*HD_TABLE_SIZE)) {
 -              IWL_DEBUG_CALIB("No change in SENSITIVITY_CMD\n");
 -              return 0;
 -      }
 -
 -      /* Copy table for comparison next time */
 -      memcpy(&(priv->sensitivity_tbl[0]), &(cmd.table[0]),
 -             sizeof(u16)*HD_TABLE_SIZE);
 -
 -      ret = iwl_send_cmd(priv, &cmd_out);
 -      if (ret)
 -              IWL_ERROR("SENSITIVITY_CMD failed\n");
 -
 -      return ret;
 -}
 -
 -void iwl4965_init_sensitivity(struct iwl_priv *priv, u8 flags, u8 force)
 -{
 -      struct iwl4965_sensitivity_data *data = NULL;
 -      int i;
 -      int ret  = 0;
 -
 -      IWL_DEBUG_CALIB("Start iwl4965_init_sensitivity\n");
 -
 -      if (force)
 -              memset(&(priv->sensitivity_tbl[0]), 0,
 -                      sizeof(u16)*HD_TABLE_SIZE);
 -
 -      /* Clear driver's sensitivity algo data */
 -      data = &(priv->sensitivity_data);
 -      memset(data, 0, sizeof(struct iwl4965_sensitivity_data));
 -
 -      data->num_in_cck_no_fa = 0;
 -      data->nrg_curr_state = IWL_FA_TOO_MANY;
 -      data->nrg_prev_state = IWL_FA_TOO_MANY;
 -      data->nrg_silence_ref = 0;
 -      data->nrg_silence_idx = 0;
 -      data->nrg_energy_idx = 0;
 -
 -      for (i = 0; i < 10; i++)
 -              data->nrg_value[i] = 0;
 -
 -      for (i = 0; i < NRG_NUM_PREV_STAT_L; i++)
 -              data->nrg_silence_rssi[i] = 0;
 -
 -      data->auto_corr_ofdm = 90;
 -      data->auto_corr_ofdm_mrc = 170;
 -      data->auto_corr_ofdm_x1  = 105;
 -      data->auto_corr_ofdm_mrc_x1 = 220;
 -      data->auto_corr_cck = AUTO_CORR_CCK_MIN_VAL_DEF;
 -      data->auto_corr_cck_mrc = 200;
 -      data->nrg_th_cck = 100;
 -      data->nrg_th_ofdm = 100;
 -
 -      data->last_bad_plcp_cnt_ofdm = 0;
 -      data->last_fa_cnt_ofdm = 0;
 -      data->last_bad_plcp_cnt_cck = 0;
 -      data->last_fa_cnt_cck = 0;
 -
 -      /* Clear prior Sensitivity command data to force send to uCode */
 -      if (force)
 -              memset(&(priv->sensitivity_tbl[0]), 0,
 -                  sizeof(u16)*HD_TABLE_SIZE);
 -
 -      ret |= iwl4965_sensitivity_write(priv, flags);
 -      IWL_DEBUG_CALIB("<<return 0x%X\n", ret);
 -
 -      return;
 +              IWL_DEBUG_INFO("REPLY_CT_KILL_CONFIG_CMD succeeded, "
 +                      "critical temperature is %d\n",
 +                      cmd.critical_temperature_R);
  }
  
 +#ifdef CONFIG_IWL4965_RUN_TIME_CALIB
  
  /* Reset differential Rx gains in NIC to prepare for chain noise calibration.
   * Called after every association, but this runs only once!
   *  ... once chain noise is calibrated the first time, it's good forever.  */
 -void iwl4965_chain_noise_reset(struct iwl_priv *priv)
 +static void iwl4965_chain_noise_reset(struct iwl_priv *priv)
  {
 -      struct iwl4965_chain_noise_data *data = NULL;
 +      struct iwl_chain_noise_data *data = &(priv->chain_noise_data);
  
 -      data = &(priv->chain_noise_data);
        if ((data->state == IWL_CHAIN_NOISE_ALIVE) && iwl_is_associated(priv)) {
                struct iwl4965_calibration_cmd cmd;
  
                cmd.diff_gain_a = 0;
                cmd.diff_gain_b = 0;
                cmd.diff_gain_c = 0;
 -              iwl_send_cmd_pdu_async(priv, REPLY_PHY_CALIBRATION_CMD,
 -                               sizeof(cmd), &cmd, NULL);
 -              msleep(4);
 +              if (iwl_send_cmd_pdu(priv, REPLY_PHY_CALIBRATION_CMD,
 +                               sizeof(cmd), &cmd))
 +                      IWL_ERROR("Could not send REPLY_PHY_CALIBRATION_CMD\n");
                data->state = IWL_CHAIN_NOISE_ACCUMULATE;
                IWL_DEBUG_CALIB("Run chain_noise_calibrate\n");
        }
 -      return;
  }
  
 -/*
 - * Accumulate 20 beacons of signal and noise statistics for each of
 - *   3 receivers/antennas/rx-chains, then figure out:
 - * 1)  Which antennas are connected.
 - * 2)  Differential rx gain settings to balance the 3 receivers.
 - */
 -static void iwl4965_noise_calibration(struct iwl_priv *priv,
 -                                    struct iwl4965_notif_statistics *stat_resp)
 +static void iwl4965_gain_computation(struct iwl_priv *priv,
 +              u32 *average_noise,
 +              u16 min_average_noise_antenna_i,
 +              u32 min_average_noise)
  {
 -      struct iwl4965_chain_noise_data *data = NULL;
 -      int ret = 0;
 -
 -      u32 chain_noise_a;
 -      u32 chain_noise_b;
 -      u32 chain_noise_c;
 -      u32 chain_sig_a;
 -      u32 chain_sig_b;
 -      u32 chain_sig_c;
 -      u32 average_sig[NUM_RX_CHAINS] = {INITIALIZATION_VALUE};
 -      u32 average_noise[NUM_RX_CHAINS] = {INITIALIZATION_VALUE};
 -      u32 max_average_sig;
 -      u16 max_average_sig_antenna_i;
 -      u32 min_average_noise = MIN_AVERAGE_NOISE_MAX_VALUE;
 -      u16 min_average_noise_antenna_i = INITIALIZATION_VALUE;
 -      u16 i = 0;
 -      u16 chan_num = INITIALIZATION_VALUE;
 -      u32 band = INITIALIZATION_VALUE;
 -      u32 active_chains = 0;
 -      unsigned long flags;
 -      struct statistics_rx_non_phy *rx_info = &(stat_resp->rx.general);
 -
 -      data = &(priv->chain_noise_data);
 -
 -      /* Accumulate just the first 20 beacons after the first association,
 -       *   then we're done forever. */
 -      if (data->state != IWL_CHAIN_NOISE_ACCUMULATE) {
 -              if (data->state == IWL_CHAIN_NOISE_ALIVE)
 -                      IWL_DEBUG_CALIB("Wait for noise calib reset\n");
 -              return;
 -      }
 -
 -      spin_lock_irqsave(&priv->lock, flags);
 -      if (rx_info->interference_data_flag != INTERFERENCE_DATA_AVAILABLE) {
 -              IWL_DEBUG_CALIB(" << Interference data unavailable\n");
 -              spin_unlock_irqrestore(&priv->lock, flags);
 -              return;
 -      }
 -
 -      band = (priv->staging_rxon.flags & RXON_FLG_BAND_24G_MSK) ? 0 : 1;
 -      chan_num = le16_to_cpu(priv->staging_rxon.channel);
 -
 -      /* Make sure we accumulate data for just the associated channel
 -       *   (even if scanning). */
 -      if ((chan_num != (le32_to_cpu(stat_resp->flag) >> 16)) ||
 -          ((STATISTICS_REPLY_FLG_BAND_24G_MSK ==
 -           (stat_resp->flag & STATISTICS_REPLY_FLG_BAND_24G_MSK)) && band)) {
 -              IWL_DEBUG_CALIB("Stats not from chan=%d, band=%d\n",
 -                              chan_num, band);
 -              spin_unlock_irqrestore(&priv->lock, flags);
 -              return;
 -      }
 -
 -      /* Accumulate beacon statistics values across 20 beacons */
 -      chain_noise_a = le32_to_cpu(rx_info->beacon_silence_rssi_a) &
 -                              IN_BAND_FILTER;
 -      chain_noise_b = le32_to_cpu(rx_info->beacon_silence_rssi_b) &
 -                              IN_BAND_FILTER;
 -      chain_noise_c = le32_to_cpu(rx_info->beacon_silence_rssi_c) &
 -                              IN_BAND_FILTER;
 -
 -      chain_sig_a = le32_to_cpu(rx_info->beacon_rssi_a) & IN_BAND_FILTER;
 -      chain_sig_b = le32_to_cpu(rx_info->beacon_rssi_b) & IN_BAND_FILTER;
 -      chain_sig_c = le32_to_cpu(rx_info->beacon_rssi_c) & IN_BAND_FILTER;
 -
 -      spin_unlock_irqrestore(&priv->lock, flags);
 -
 -      data->beacon_count++;
 -
 -      data->chain_noise_a = (chain_noise_a + data->chain_noise_a);
 -      data->chain_noise_b = (chain_noise_b + data->chain_noise_b);
 -      data->chain_noise_c = (chain_noise_c + data->chain_noise_c);
 -
 -      data->chain_signal_a = (chain_sig_a + data->chain_signal_a);
 -      data->chain_signal_b = (chain_sig_b + data->chain_signal_b);
 -      data->chain_signal_c = (chain_sig_c + data->chain_signal_c);
 -
 -      IWL_DEBUG_CALIB("chan=%d, band=%d, beacon=%d\n", chan_num, band,
 -                      data->beacon_count);
 -      IWL_DEBUG_CALIB("chain_sig: a %d b %d c %d\n",
 -                      chain_sig_a, chain_sig_b, chain_sig_c);
 -      IWL_DEBUG_CALIB("chain_noise: a %d b %d c %d\n",
 -                      chain_noise_a, chain_noise_b, chain_noise_c);
 -
 -      /* If this is the 20th beacon, determine:
 -       * 1)  Disconnected antennas (using signal strengths)
 -       * 2)  Differential gain (using silence noise) to balance receivers */
 -      if (data->beacon_count == CAL_NUM_OF_BEACONS) {
 -
 -              /* Analyze signal for disconnected antenna */
 -              average_sig[0] = (data->chain_signal_a) / CAL_NUM_OF_BEACONS;
 -              average_sig[1] = (data->chain_signal_b) / CAL_NUM_OF_BEACONS;
 -              average_sig[2] = (data->chain_signal_c) / CAL_NUM_OF_BEACONS;
 -
 -              if (average_sig[0] >= average_sig[1]) {
 -                      max_average_sig = average_sig[0];
 -                      max_average_sig_antenna_i = 0;
 -                      active_chains = (1 << max_average_sig_antenna_i);
 -              } else {
 -                      max_average_sig = average_sig[1];
 -                      max_average_sig_antenna_i = 1;
 -                      active_chains = (1 << max_average_sig_antenna_i);
 -              }
 -
 -              if (average_sig[2] >= max_average_sig) {
 -                      max_average_sig = average_sig[2];
 -                      max_average_sig_antenna_i = 2;
 -                      active_chains = (1 << max_average_sig_antenna_i);
 -              }
 -
 -              IWL_DEBUG_CALIB("average_sig: a %d b %d c %d\n",
 -                           average_sig[0], average_sig[1], average_sig[2]);
 -              IWL_DEBUG_CALIB("max_average_sig = %d, antenna %d\n",
 -                           max_average_sig, max_average_sig_antenna_i);
 -
 -              /* Compare signal strengths for all 3 receivers. */
 -              for (i = 0; i < NUM_RX_CHAINS; i++) {
 -                      if (i != max_average_sig_antenna_i) {
 -                              s32 rssi_delta = (max_average_sig -
 -                                                average_sig[i]);
 -
 -                              /* If signal is very weak, compared with
 -                               * strongest, mark it as disconnected. */
 -                              if (rssi_delta > MAXIMUM_ALLOWED_PATHLOSS)
 -                                      data->disconn_array[i] = 1;
 -                              else
 -                                      active_chains |= (1 << i);
 -                      IWL_DEBUG_CALIB("i = %d  rssiDelta = %d  "
 -                                   "disconn_array[i] = %d\n",
 -                                   i, rssi_delta, data->disconn_array[i]);
 -                      }
 -              }
 -
 -              /*If both chains A & B are disconnected -
 -               * connect B and leave A as is */
 -              if (data->disconn_array[CHAIN_A] &&
 -                  data->disconn_array[CHAIN_B]) {
 -                      data->disconn_array[CHAIN_B] = 0;
 -                      active_chains |= (1 << CHAIN_B);
 -                      IWL_DEBUG_CALIB("both A & B chains are disconnected! "
 -                                   "W/A - declare B as connected\n");
 -              }
 -
 -              IWL_DEBUG_CALIB("active_chains (bitwise) = 0x%x\n",
 -                              active_chains);
 -
 -              /* Save for use within RXON, TX, SCAN commands, etc. */
 -              priv->valid_antenna = active_chains;
 -
 -              /* Analyze noise for rx balance */
 -              average_noise[0] = ((data->chain_noise_a)/CAL_NUM_OF_BEACONS);
 -              average_noise[1] = ((data->chain_noise_b)/CAL_NUM_OF_BEACONS);
 -              average_noise[2] = ((data->chain_noise_c)/CAL_NUM_OF_BEACONS);
 -
 -              for (i = 0; i < NUM_RX_CHAINS; i++) {
 -                      if (!(data->disconn_array[i]) &&
 -                         (average_noise[i] <= min_average_noise)) {
 -                              /* This means that chain i is active and has
 -                               * lower noise values so far: */
 -                              min_average_noise = average_noise[i];
 -                              min_average_noise_antenna_i = i;
 -                      }
 -              }
 -
 -              data->delta_gain_code[min_average_noise_antenna_i] = 0;
 +      int i, ret;
 +      struct iwl_chain_noise_data *data = &priv->chain_noise_data;
  
 -              IWL_DEBUG_CALIB("average_noise: a %d b %d c %d\n",
 -                              average_noise[0], average_noise[1],
 -                              average_noise[2]);
 +      data->delta_gain_code[min_average_noise_antenna_i] = 0;
  
 -              IWL_DEBUG_CALIB("min_average_noise = %d, antenna %d\n",
 -                              min_average_noise, min_average_noise_antenna_i);
 +      for (i = 0; i < NUM_RX_CHAINS; i++) {
 +              s32 delta_g = 0;
  
 -              for (i = 0; i < NUM_RX_CHAINS; i++) {
 -                      s32 delta_g = 0;
 -
 -                      if (!(data->disconn_array[i]) &&
 -                          (data->delta_gain_code[i] ==
 +              if (!(data->disconn_array[i]) &&
 +                  (data->delta_gain_code[i] ==
                             CHAIN_NOISE_DELTA_GAIN_INIT_VAL)) {
 -                              delta_g = average_noise[i] - min_average_noise;
 -                              data->delta_gain_code[i] = (u8)((delta_g *
 -                                                                  10) / 15);
 -                              if (CHAIN_NOISE_MAX_DELTA_GAIN_CODE <
 -                                 data->delta_gain_code[i])
 -                                      data->delta_gain_code[i] =
 -                                        CHAIN_NOISE_MAX_DELTA_GAIN_CODE;
 -
 -                              data->delta_gain_code[i] =
 -                                      (data->delta_gain_code[i] | (1 << 2));
 -                      } else
 -                              data->delta_gain_code[i] = 0;
 -              }
 -              IWL_DEBUG_CALIB("delta_gain_codes: a %d b %d c %d\n",
 -                           data->delta_gain_code[0],
 -                           data->delta_gain_code[1],
 -                           data->delta_gain_code[2]);
 -
 -              /* Differential gain gets sent to uCode only once */
 -              if (!data->radio_write) {
 -                      struct iwl4965_calibration_cmd cmd;
 -                      data->radio_write = 1;
 -
 -                      memset(&cmd, 0, sizeof(cmd));
 -                      cmd.opCode = PHY_CALIBRATE_DIFF_GAIN_CMD;
 -                      cmd.diff_gain_a = data->delta_gain_code[0];
 -                      cmd.diff_gain_b = data->delta_gain_code[1];
 -                      cmd.diff_gain_c = data->delta_gain_code[2];
 -                      ret = iwl_send_cmd_pdu(priv, REPLY_PHY_CALIBRATION_CMD,
 -                                            sizeof(cmd), &cmd);
 -                      if (ret)
 -                              IWL_DEBUG_CALIB("fail sending cmd "
 -                                           "REPLY_PHY_CALIBRATION_CMD \n");
 -
 -                      /* TODO we might want recalculate
 -                       * rx_chain in rxon cmd */
 -
 -                      /* Mark so we run this algo only once! */
 -                      data->state = IWL_CHAIN_NOISE_CALIBRATED;
 +                      delta_g = average_noise[i] - min_average_noise;
 +                      data->delta_gain_code[i] = (u8)((delta_g * 10) / 15);
 +                      data->delta_gain_code[i] =
 +                              min(data->delta_gain_code[i],
 +                              (u8) CHAIN_NOISE_MAX_DELTA_GAIN_CODE);
 +
 +                      data->delta_gain_code[i] =
 +                              (data->delta_gain_code[i] | (1 << 2));
 +              } else {
 +                      data->delta_gain_code[i] = 0;
                }
 -              data->chain_noise_a = 0;
 -              data->chain_noise_b = 0;
 -              data->chain_noise_c = 0;
 -              data->chain_signal_a = 0;
 -              data->chain_signal_b = 0;
 -              data->chain_signal_c = 0;
 -              data->beacon_count = 0;
 -      }
 -      return;
 -}
 -
 -static void iwl4965_sensitivity_calibration(struct iwl_priv *priv,
 -                                          struct iwl4965_notif_statistics *resp)
 -{
 -      u32 rx_enable_time;
 -      u32 fa_cck;
 -      u32 fa_ofdm;
 -      u32 bad_plcp_cck;
 -      u32 bad_plcp_ofdm;
 -      u32 norm_fa_ofdm;
 -      u32 norm_fa_cck;
 -      struct iwl4965_sensitivity_data *data = NULL;
 -      struct statistics_rx_non_phy *rx_info = &(resp->rx.general);
 -      struct statistics_rx *statistics = &(resp->rx);
 -      unsigned long flags;
 -      struct statistics_general_data statis;
 -      int ret;
 -
 -      data = &(priv->sensitivity_data);
 -
 -      if (!iwl_is_associated(priv)) {
 -              IWL_DEBUG_CALIB("<< - not associated\n");
 -              return;
        }
 +      IWL_DEBUG_CALIB("delta_gain_codes: a %d b %d c %d\n",
 +                   data->delta_gain_code[0],
 +                   data->delta_gain_code[1],
 +                   data->delta_gain_code[2]);
  
 -      spin_lock_irqsave(&priv->lock, flags);
 -      if (rx_info->interference_data_flag != INTERFERENCE_DATA_AVAILABLE) {
 -              IWL_DEBUG_CALIB("<< invalid data.\n");
 -              spin_unlock_irqrestore(&priv->lock, flags);
 -              return;
 -      }
 -
 -      /* Extract Statistics: */
 -      rx_enable_time = le32_to_cpu(rx_info->channel_load);
 -      fa_cck = le32_to_cpu(statistics->cck.false_alarm_cnt);
 -      fa_ofdm = le32_to_cpu(statistics->ofdm.false_alarm_cnt);
 -      bad_plcp_cck = le32_to_cpu(statistics->cck.plcp_err);
 -      bad_plcp_ofdm = le32_to_cpu(statistics->ofdm.plcp_err);
 -
 -      statis.beacon_silence_rssi_a =
 -                      le32_to_cpu(statistics->general.beacon_silence_rssi_a);
 -      statis.beacon_silence_rssi_b =
 -                      le32_to_cpu(statistics->general.beacon_silence_rssi_b);
 -      statis.beacon_silence_rssi_c =
 -                      le32_to_cpu(statistics->general.beacon_silence_rssi_c);
 -      statis.beacon_energy_a =
 -                      le32_to_cpu(statistics->general.beacon_energy_a);
 -      statis.beacon_energy_b =
 -                      le32_to_cpu(statistics->general.beacon_energy_b);
 -      statis.beacon_energy_c =
 -                      le32_to_cpu(statistics->general.beacon_energy_c);
 -
 -      spin_unlock_irqrestore(&priv->lock, flags);
 -
 -      IWL_DEBUG_CALIB("rx_enable_time = %u usecs\n", rx_enable_time);
 -
 -      if (!rx_enable_time) {
 -              IWL_DEBUG_CALIB("<< RX Enable Time == 0! \n");
 -              return;
 -      }
 -
 -      /* These statistics increase monotonically, and do not reset
 -       *   at each beacon.  Calculate difference from last value, or just
 -       *   use the new statistics value if it has reset or wrapped around. */
 -      if (data->last_bad_plcp_cnt_cck > bad_plcp_cck)
 -              data->last_bad_plcp_cnt_cck = bad_plcp_cck;
 -      else {
 -              bad_plcp_cck -= data->last_bad_plcp_cnt_cck;
 -              data->last_bad_plcp_cnt_cck += bad_plcp_cck;
 -      }
 -
 -      if (data->last_bad_plcp_cnt_ofdm > bad_plcp_ofdm)
 -              data->last_bad_plcp_cnt_ofdm = bad_plcp_ofdm;
 -      else {
 -              bad_plcp_ofdm -= data->last_bad_plcp_cnt_ofdm;
 -              data->last_bad_plcp_cnt_ofdm += bad_plcp_ofdm;
 -      }
 -
 -      if (data->last_fa_cnt_ofdm > fa_ofdm)
 -              data->last_fa_cnt_ofdm = fa_ofdm;
 -      else {
 -              fa_ofdm -= data->last_fa_cnt_ofdm;
 -              data->last_fa_cnt_ofdm += fa_ofdm;
 -      }
 +      /* Differential gain gets sent to uCode only once */
 +      if (!data->radio_write) {
 +              struct iwl4965_calibration_cmd cmd;
 +              data->radio_write = 1;
  
 -      if (data->last_fa_cnt_cck > fa_cck)
 -              data->last_fa_cnt_cck = fa_cck;
 -      else {
 -              fa_cck -= data->last_fa_cnt_cck;
 -              data->last_fa_cnt_cck += fa_cck;
 +              memset(&cmd, 0, sizeof(cmd));
 +              cmd.opCode = PHY_CALIBRATE_DIFF_GAIN_CMD;
 +              cmd.diff_gain_a = data->delta_gain_code[0];
 +              cmd.diff_gain_b = data->delta_gain_code[1];
 +              cmd.diff_gain_c = data->delta_gain_code[2];
 +              ret = iwl_send_cmd_pdu(priv, REPLY_PHY_CALIBRATION_CMD,
 +                                    sizeof(cmd), &cmd);
 +              if (ret)
 +                      IWL_DEBUG_CALIB("fail sending cmd "
 +                                   "REPLY_PHY_CALIBRATION_CMD \n");
 +
 +              /* TODO we might want recalculate
 +               * rx_chain in rxon cmd */
 +
 +              /* Mark so we run this algo only once! */
 +              data->state = IWL_CHAIN_NOISE_CALIBRATED;
        }
 -
 -      /* Total aborted signal locks */
 -      norm_fa_ofdm = fa_ofdm + bad_plcp_ofdm;
 -      norm_fa_cck = fa_cck + bad_plcp_cck;
 -
 -      IWL_DEBUG_CALIB("cck: fa %u badp %u  ofdm: fa %u badp %u\n", fa_cck,
 -                      bad_plcp_cck, fa_ofdm, bad_plcp_ofdm);
 -
 -      iwl4965_sens_auto_corr_ofdm(priv, norm_fa_ofdm, rx_enable_time);
 -      iwl4965_sens_energy_cck(priv, norm_fa_cck, rx_enable_time, &statis);
 -      ret = iwl4965_sensitivity_write(priv, CMD_ASYNC);
 -
 -      return;
 +      data->chain_noise_a = 0;
 +      data->chain_noise_b = 0;
 +      data->chain_noise_c = 0;
 +      data->chain_signal_a = 0;
 +      data->chain_signal_b = 0;
 +      data->chain_signal_c = 0;
 +      data->beacon_count = 0;
  }
  
  static void iwl4965_bg_sensitivity_work(struct work_struct *work)
        }
  
        if (priv->start_calib) {
 -              iwl4965_noise_calibration(priv, &priv->statistics);
 -
 -              if (priv->sensitivity_data.state ==
 -                                      IWL_SENS_CALIB_NEED_REINIT) {
 -                      iwl4965_init_sensitivity(priv, CMD_ASYNC, 0);
 -                      priv->sensitivity_data.state = IWL_SENS_CALIB_ALLOWED;
 -              } else
 -                      iwl4965_sensitivity_calibration(priv,
 -                                      &priv->statistics);
 +              iwl_chain_noise_calibration(priv, &priv->statistics);
 +
 +              iwl_sensitivity_calibration(priv, &priv->statistics);
        }
  
        mutex_unlock(&priv->mutex);
        return;
  }
 -#endif /*CONFIG_IWL4965_SENSITIVITY*/
 +#endif /*CONFIG_IWL4965_RUN_TIME_CALIB*/
  
  static void iwl4965_bg_txpower_work(struct work_struct *work)
  {
@@@ -994,11 -1890,11 +994,11 @@@ static void iwl4965_tx_queue_set_status
  
        /* Set up and activate */
        iwl_write_prph(priv, IWL49_SCD_QUEUE_STATUS_BITS(txq_id),
 -                               (active << SCD_QUEUE_STTS_REG_POS_ACTIVE) |
 -                               (tx_fifo_id << SCD_QUEUE_STTS_REG_POS_TXF) |
 -                               (scd_retry << SCD_QUEUE_STTS_REG_POS_WSL) |
 -                               (scd_retry << SCD_QUEUE_STTS_REG_POS_SCD_ACK) |
 -                               SCD_QUEUE_STTS_REG_MSK);
 +                       (active << IWL49_SCD_QUEUE_STTS_REG_POS_ACTIVE) |
 +                       (tx_fifo_id << IWL49_SCD_QUEUE_STTS_REG_POS_TXF) |
 +                       (scd_retry << IWL49_SCD_QUEUE_STTS_REG_POS_WSL) |
 +                       (scd_retry << IWL49_SCD_QUEUE_STTS_REG_POS_SCD_ACK) |
 +                       IWL49_SCD_QUEUE_STTS_REG_MSK);
  
        txq->sched_retry = scd_retry;
  
@@@ -1012,7 -1908,7 +1012,7 @@@ static const u16 default_queue_to_tx_fi
        IWL_TX_FIFO_AC2,
        IWL_TX_FIFO_AC1,
        IWL_TX_FIFO_AC0,
 -      IWL_CMD_FIFO_NUM,
 +      IWL49_CMD_FIFO_NUM,
        IWL_TX_FIFO_HCCA_1,
        IWL_TX_FIFO_HCCA_2
  };
@@@ -1036,15 -1932,15 +1036,15 @@@ int iwl4965_alive_notify(struct iwl_pri
  
        spin_lock_irqsave(&priv->lock, flags);
  
 -#ifdef CONFIG_IWL4965_SENSITIVITY
 +#ifdef CONFIG_IWL4965_RUN_TIME_CALIB
        memset(&(priv->sensitivity_data), 0,
 -             sizeof(struct iwl4965_sensitivity_data));
 +             sizeof(struct iwl_sensitivity_data));
        memset(&(priv->chain_noise_data), 0,
 -             sizeof(struct iwl4965_chain_noise_data));
 +             sizeof(struct iwl_chain_noise_data));
        for (i = 0; i < NUM_RX_CHAINS; i++)
                priv->chain_noise_data.delta_gain_code[i] =
                                CHAIN_NOISE_DELTA_GAIN_INIT_VAL;
 -#endif /* CONFIG_IWL4965_SENSITIVITY*/
 +#endif /* CONFIG_IWL4965_RUN_TIME_CALIB*/
        ret = iwl_grab_nic_access(priv);
        if (ret) {
                spin_unlock_irqrestore(&priv->lock, flags);
  
        /* Clear 4965's internal Tx Scheduler data base */
        priv->scd_base_addr = iwl_read_prph(priv, IWL49_SCD_SRAM_BASE_ADDR);
 -      a = priv->scd_base_addr + SCD_CONTEXT_DATA_OFFSET;
 -      for (; a < priv->scd_base_addr + SCD_TX_STTS_BITMAP_OFFSET; a += 4)
 +      a = priv->scd_base_addr + IWL49_SCD_CONTEXT_DATA_OFFSET;
 +      for (; a < priv->scd_base_addr + IWL49_SCD_TX_STTS_BITMAP_OFFSET; a += 4)
                iwl_write_targ_mem(priv, a, 0);
 -      for (; a < priv->scd_base_addr + SCD_TRANSLATE_TBL_OFFSET; a += 4)
 +      for (; a < priv->scd_base_addr + IWL49_SCD_TRANSLATE_TBL_OFFSET; a += 4)
                iwl_write_targ_mem(priv, a, 0);
        for (; a < sizeof(u16) * priv->hw_params.max_txq_num; a += 4)
                iwl_write_targ_mem(priv, a, 0);
  
                /* Max Tx Window size for Scheduler-ACK mode */
                iwl_write_targ_mem(priv, priv->scd_base_addr +
 -                                      SCD_CONTEXT_QUEUE_OFFSET(i),
 -                                      (SCD_WIN_SIZE <<
 -                                      SCD_QUEUE_CTX_REG1_WIN_SIZE_POS) &
 -                                      SCD_QUEUE_CTX_REG1_WIN_SIZE_MSK);
 +                              IWL49_SCD_CONTEXT_QUEUE_OFFSET(i),
 +                              (SCD_WIN_SIZE <<
 +                              IWL49_SCD_QUEUE_CTX_REG1_WIN_SIZE_POS) &
 +                              IWL49_SCD_QUEUE_CTX_REG1_WIN_SIZE_MSK);
  
                /* Frame limit */
                iwl_write_targ_mem(priv, priv->scd_base_addr +
 -                                      SCD_CONTEXT_QUEUE_OFFSET(i) +
 -                                      sizeof(u32),
 -                                      (SCD_FRAME_LIMIT <<
 -                                      SCD_QUEUE_CTX_REG2_FRAME_LIMIT_POS) &
 -                                      SCD_QUEUE_CTX_REG2_FRAME_LIMIT_MSK);
 +                              IWL49_SCD_CONTEXT_QUEUE_OFFSET(i) +
 +                              sizeof(u32),
 +                              (SCD_FRAME_LIMIT <<
 +                              IWL49_SCD_QUEUE_CTX_REG2_FRAME_LIMIT_POS) &
 +                              IWL49_SCD_QUEUE_CTX_REG2_FRAME_LIMIT_MSK);
  
        }
        iwl_write_prph(priv, IWL49_SCD_INTERRUPT_MASK,
        return ret;
  }
  
 +#ifdef CONFIG_IWL4965_RUN_TIME_CALIB
 +static struct iwl_sensitivity_ranges iwl4965_sensitivity = {
 +      .min_nrg_cck = 97,
 +      .max_nrg_cck = 0,
 +
 +      .auto_corr_min_ofdm = 85,
 +      .auto_corr_min_ofdm_mrc = 170,
 +      .auto_corr_min_ofdm_x1 = 105,
 +      .auto_corr_min_ofdm_mrc_x1 = 220,
 +
 +      .auto_corr_max_ofdm = 120,
 +      .auto_corr_max_ofdm_mrc = 210,
 +      .auto_corr_max_ofdm_x1 = 140,
 +      .auto_corr_max_ofdm_mrc_x1 = 270,
 +
 +      .auto_corr_min_cck = 125,
 +      .auto_corr_max_cck = 200,
 +      .auto_corr_min_cck_mrc = 200,
 +      .auto_corr_max_cck_mrc = 400,
 +
 +      .nrg_th_cck = 100,
 +      .nrg_th_ofdm = 100,
 +};
 +#endif
 +
  /**
   * iwl4965_hw_set_hw_params
   *
  int iwl4965_hw_set_hw_params(struct iwl_priv *priv)
  {
  
 -      if ((priv->cfg->mod_params->num_of_queues > IWL4965_MAX_NUM_QUEUES) ||
 +      if ((priv->cfg->mod_params->num_of_queues > IWL49_NUM_QUEUES) ||
            (priv->cfg->mod_params->num_of_queues < IWL_MIN_NUM_QUEUES)) {
                IWL_ERROR("invalid queues_num, should be between %d and %d\n",
 -                        IWL_MIN_NUM_QUEUES, IWL4965_MAX_NUM_QUEUES);
 +                        IWL_MIN_NUM_QUEUES, IWL49_NUM_QUEUES);
                return -EINVAL;
        }
  
        priv->hw_params.max_txq_num = priv->cfg->mod_params->num_of_queues;
 +      priv->hw_params.sw_crypto = priv->cfg->mod_params->sw_crypto;
        priv->hw_params.tx_cmd_len = sizeof(struct iwl4965_tx_cmd);
        priv->hw_params.max_rxq_size = RX_QUEUE_SIZE;
        priv->hw_params.max_rxq_log = RX_QUEUE_SIZE_LOG;
        priv->hw_params.max_stations = IWL4965_STATION_COUNT;
        priv->hw_params.bcast_sta_id = IWL4965_BROADCAST_ID;
  
 +      priv->hw_params.max_data_size = IWL49_RTC_DATA_SIZE;
 +      priv->hw_params.max_inst_size = IWL49_RTC_INST_SIZE;
 +      priv->hw_params.max_bsm_size = BSM_SRAM_SIZE;
 +      priv->hw_params.fat_channel = BIT(IEEE80211_BAND_5GHZ);
 +
        priv->hw_params.tx_chains_num = 2;
        priv->hw_params.rx_chains_num = 2;
 -      priv->hw_params.valid_tx_ant = (IWL_ANTENNA_MAIN | IWL_ANTENNA_AUX);
 -      priv->hw_params.valid_rx_ant = (IWL_ANTENNA_MAIN | IWL_ANTENNA_AUX);
 +      priv->hw_params.valid_tx_ant = ANT_A | ANT_B;
 +      priv->hw_params.valid_rx_ant = ANT_A | ANT_B;
 +      priv->hw_params.ct_kill_threshold = CELSIUS_TO_KELVIN(CT_KILL_THRESHOLD);
 +
 +#ifdef CONFIG_IWL4965_RUN_TIME_CALIB
 +      priv->hw_params.sens = &iwl4965_sensitivity;
 +#endif
  
        return 0;
  }
@@@ -1264,17 -2124,6 +1264,17 @@@ int iwl4965_hw_txq_free_tfd(struct iwl_
        return 0;
  }
  
 +/* set card power command */
 +static int iwl4965_set_power(struct iwl_priv *priv,
 +                    void *cmd)
 +{
 +      int ret = 0;
 +
 +      ret = iwl_send_cmd_pdu_async(priv, POWER_TABLE_CMD,
 +                                  sizeof(struct iwl4965_powertable_cmd),
 +                                  cmd, NULL);
 +      return ret;
 +}
  int iwl4965_hw_reg_set_txpower(struct iwl_priv *priv, s8 power)
  {
        IWL_ERROR("TODO: Implement iwl4965_hw_reg_set_txpower!\n");
@@@ -1375,11 -2224,11 +1375,11 @@@ static u32 iwl4965_get_sub_band(const s
        s32 b = -1;
  
        for (b = 0; b < EEPROM_TX_POWER_BANDS; b++) {
 -              if (priv->eeprom.calib_info.band_info[b].ch_from == 0)
 +              if (priv->calib_info->band_info[b].ch_from == 0)
                        continue;
  
 -              if ((channel >= priv->eeprom.calib_info.band_info[b].ch_from)
 -                  && (channel <= priv->eeprom.calib_info.band_info[b].ch_to))
 +              if ((channel >= priv->calib_info->band_info[b].ch_from)
 +                  && (channel <= priv->calib_info->band_info[b].ch_to))
                        break;
        }
  
@@@ -1407,14 -2256,14 +1407,14 @@@ static s32 iwl4965_interpolate_value(s3
   * in channel number.
   */
  static int iwl4965_interpolate_chan(struct iwl_priv *priv, u32 channel,
 -                                  struct iwl4965_eeprom_calib_ch_info *chan_info)
 +                                  struct iwl_eeprom_calib_ch_info *chan_info)
  {
        s32 s = -1;
        u32 c;
        u32 m;
 -      const struct iwl4965_eeprom_calib_measure *m1;
 -      const struct iwl4965_eeprom_calib_measure *m2;
 -      struct iwl4965_eeprom_calib_measure *omeas;
 +      const struct iwl_eeprom_calib_measure *m1;
 +      const struct iwl_eeprom_calib_measure *m2;
 +      struct iwl_eeprom_calib_measure *omeas;
        u32 ch_i1;
        u32 ch_i2;
  
                return -1;
        }
  
 -      ch_i1 = priv->eeprom.calib_info.band_info[s].ch1.ch_num;
 -      ch_i2 = priv->eeprom.calib_info.band_info[s].ch2.ch_num;
 +      ch_i1 = priv->calib_info->band_info[s].ch1.ch_num;
 +      ch_i2 = priv->calib_info->band_info[s].ch2.ch_num;
        chan_info->ch_num = (u8) channel;
  
        IWL_DEBUG_TXPOWER("channel %d subband %d factory cal ch %d & %d\n",
  
        for (c = 0; c < EEPROM_TX_POWER_TX_CHAINS; c++) {
                for (m = 0; m < EEPROM_TX_POWER_MEASUREMENTS; m++) {
 -                      m1 = &(priv->eeprom.calib_info.band_info[s].ch1.
 +                      m1 = &(priv->calib_info->band_info[s].ch1.
                               measurements[c][m]);
 -                      m2 = &(priv->eeprom.calib_info.band_info[s].ch2.
 +                      m2 = &(priv->calib_info->band_info[s].ch2.
                               measurements[c][m]);
                        omeas = &(chan_info->measurements[c][m]);
  
@@@ -1754,8 -2603,8 +1754,8 @@@ static int iwl4965_fill_txpower_tbl(str
        int i;
        int c;
        const struct iwl_channel_info *ch_info = NULL;
 -      struct iwl4965_eeprom_calib_ch_info ch_eeprom_info;
 -      const struct iwl4965_eeprom_calib_measure *measurement;
 +      struct iwl_eeprom_calib_ch_info ch_eeprom_info;
 +      const struct iwl_eeprom_calib_measure *measurement;
        s16 voltage;
        s32 init_voltage;
        s32 voltage_compensation;
        /* hardware txpower limits ...
         * saturation (clipping distortion) txpowers are in half-dBm */
        if (band)
 -              saturation_power = priv->eeprom.calib_info.saturation_power24;
 +              saturation_power = priv->calib_info->saturation_power24;
        else
 -              saturation_power = priv->eeprom.calib_info.saturation_power52;
 +              saturation_power = priv->calib_info->saturation_power52;
  
        if (saturation_power < IWL_TX_POWER_SATURATION_MIN ||
            saturation_power > IWL_TX_POWER_SATURATION_MAX) {
        iwl4965_interpolate_chan(priv, channel, &ch_eeprom_info);
  
        /* calculate tx gain adjustment based on power supply voltage */
 -      voltage = priv->eeprom.calib_info.voltage;
 +      voltage = priv->calib_info->voltage;
        init_voltage = (s32)le32_to_cpu(priv->card_alive_init.voltage);
        voltage_compensation =
            iwl4965_get_voltage_compensation(voltage, init_voltage);
@@@ -2255,9 -3104,9 +2255,9 @@@ int iwl4965_hw_tx_queue_init(struct iwl
  
        /* Enable DMA channel, using same id as for TFD queue */
        iwl_write_direct32(
 -              priv, IWL_FH_TCSR_CHNL_TX_CONFIG_REG(txq_id),
 -              IWL_FH_TCSR_TX_CONFIG_REG_VAL_DMA_CHNL_ENABLE |
 -              IWL_FH_TCSR_TX_CONFIG_REG_VAL_DMA_CREDIT_ENABLE_VAL);
 +              priv, FH_TCSR_CHNL_TX_CONFIG_REG(txq_id),
 +              FH_TCSR_TX_CONFIG_REG_VAL_DMA_CHNL_ENABLE |
 +              FH_TCSR_TX_CONFIG_REG_VAL_DMA_CREDIT_ENABLE_VAL);
        iwl_release_nic_access(priv);
        spin_unlock_irqrestore(&priv->lock, flags);
  
@@@ -2300,37 -3149,18 +2300,37 @@@ int iwl4965_hw_txq_attach_buf_to_tfd(st
  
  static void iwl4965_hw_card_show_info(struct iwl_priv *priv)
  {
 -      u16 hw_version = priv->eeprom.board_revision_4965;
 +      u16 hw_version = iwl_eeprom_query16(priv, EEPROM_4965_BOARD_REVISION);
  
        IWL_DEBUG_INFO("4965ABGN HW Version %u.%u.%u\n",
                       ((hw_version >> 8) & 0x0F),
                       ((hw_version >> 8) >> 4), (hw_version & 0x00FF));
  
        IWL_DEBUG_INFO("4965ABGN PBA Number %.16s\n",
 -                     priv->eeprom.board_pba_number_4965);
 +                     &priv->eeprom[EEPROM_4965_BOARD_PBA]);
  }
  
 -#define IWL_TX_CRC_SIZE               4
 -#define IWL_TX_DELIMITER_SIZE 4
 +static int iwl4965_alloc_shared_mem(struct iwl_priv *priv)
 +{
 +      priv->shared_virt = pci_alloc_consistent(priv->pci_dev,
 +                                      sizeof(struct iwl4965_shared),
 +                                      &priv->shared_phys);
 +      if (!priv->shared_virt)
 +              return -ENOMEM;
 +
 +      memset(priv->shared_virt, 0, sizeof(struct iwl4965_shared));
 +
 +      return 0;
 +}
 +
 +static void iwl4965_free_shared_mem(struct iwl_priv *priv)
 +{
 +      if (priv->shared_virt)
 +              pci_free_consistent(priv->pci_dev,
 +                                  sizeof(struct iwl4965_shared),
 +                                  priv->shared_virt,
 +                                  priv->shared_phys);
 +}
  
  /**
   * iwl4965_txq_update_byte_cnt_tbl - Set up entry in Tx byte-count array
@@@ -2350,13 -3180,50 +2350,13 @@@ static void iwl4965_txq_update_byte_cnt
                       tfd_offset[txq->q.write_ptr], byte_cnt, len);
  
        /* If within first 64 entries, duplicate at end */
 -      if (txq->q.write_ptr < IWL4965_MAX_WIN_SIZE)
 +      if (txq->q.write_ptr < IWL49_MAX_WIN_SIZE)
                IWL_SET_BITS16(shared_data->queues_byte_cnt_tbls[txq_id].
 -                      tfd_offset[IWL4965_QUEUE_SIZE + txq->q.write_ptr],
 +                      tfd_offset[IWL49_QUEUE_SIZE + txq->q.write_ptr],
                        byte_cnt, len);
  }
  
  /**
 - * iwl4965_set_rxon_chain - Set up Rx chain usage in "staging" RXON image
 - *
 - * Selects how many and which Rx receivers/antennas/chains to use.
 - * This should not be used for scan command ... it puts data in wrong place.
 - */
 -void iwl4965_set_rxon_chain(struct iwl_priv *priv)
 -{
 -      u8 is_single = is_single_stream(priv);
 -      u8 idle_state, rx_state;
 -
 -      priv->staging_rxon.rx_chain = 0;
 -      rx_state = idle_state = 3;
 -
 -      /* Tell uCode which antennas are actually connected.
 -       * Before first association, we assume all antennas are connected.
 -       * Just after first association, iwl4965_noise_calibration()
 -       *    checks which antennas actually *are* connected. */
 -      priv->staging_rxon.rx_chain |=
 -          cpu_to_le16(priv->valid_antenna << RXON_RX_CHAIN_VALID_POS);
 -
 -      /* How many receivers should we use? */
 -      iwl4965_get_rx_chain_counter(priv, &idle_state, &rx_state);
 -      priv->staging_rxon.rx_chain |=
 -              cpu_to_le16(rx_state << RXON_RX_CHAIN_MIMO_CNT_POS);
 -      priv->staging_rxon.rx_chain |=
 -              cpu_to_le16(idle_state << RXON_RX_CHAIN_CNT_POS);
 -
 -      if (!is_single && (rx_state >= 2) &&
 -          !test_bit(STATUS_POWER_PMI, &priv->status))
 -              priv->staging_rxon.rx_chain |= RXON_RX_CHAIN_MIMO_FORCE_MSK;
 -      else
 -              priv->staging_rxon.rx_chain &= ~RXON_RX_CHAIN_MIMO_FORCE_MSK;
 -
 -      IWL_DEBUG_ASSOC("rx chain %X\n", priv->staging_rxon.rx_chain);
 -}
 -
 -/**
   * sign_extend - Sign extend a value using specified bit as sign-bit
   *
   * Example: sign_extend(9, 3) would return -7 as bit3 of 1001b is 1
@@@ -2545,7 -3412,7 +2545,7 @@@ void iwl4965_hw_rx_statistics(struct iw
        if (unlikely(!test_bit(STATUS_SCANNING, &priv->status)) &&
            (pkt->hdr.cmd == STATISTICS_NOTIFICATION)) {
                iwl4965_rx_calc_noise(priv);
 -#ifdef CONFIG_IWL4965_SENSITIVITY
 +#ifdef CONFIG_IWL4965_RUN_TIME_CALIB
                queue_work(priv->workqueue, &priv->sensitivity_work);
  #endif
        }
@@@ -2695,53 -3562,6 +2695,53 @@@ static void iwl_update_rx_stats(struct 
        priv->rx_stats[idx].bytes += len;
  }
  
 +/*
 + * returns non-zero if packet should be dropped
 + */
 +static int iwl4965_set_decrypted_flag(struct iwl_priv *priv,
 +                                    struct ieee80211_hdr *hdr,
 +                                    u32 decrypt_res,
 +                                    struct ieee80211_rx_status *stats)
 +{
 +      u16 fc = le16_to_cpu(hdr->frame_control);
 +
 +      if (priv->active_rxon.filter_flags & RXON_FILTER_DIS_DECRYPT_MSK)
 +              return 0;
 +
 +      if (!(fc & IEEE80211_FCTL_PROTECTED))
 +              return 0;
 +
 +      IWL_DEBUG_RX("decrypt_res:0x%x\n", decrypt_res);
 +      switch (decrypt_res & RX_RES_STATUS_SEC_TYPE_MSK) {
 +      case RX_RES_STATUS_SEC_TYPE_TKIP:
 +              /* The uCode has got a bad phase 1 Key, pushes the packet.
 +               * Decryption will be done in SW. */
 +              if ((decrypt_res & RX_RES_STATUS_DECRYPT_TYPE_MSK) ==
 +                  RX_RES_STATUS_BAD_KEY_TTAK)
 +                      break;
 +
 +              if ((decrypt_res & RX_RES_STATUS_DECRYPT_TYPE_MSK) ==
 +                  RX_RES_STATUS_BAD_ICV_MIC) {
 +                      /* bad ICV, the packet is destroyed since the
 +                       * decryption is inplace, drop it */
 +                      IWL_DEBUG_RX("Packet destroyed\n");
 +                      return -1;
 +              }
 +      case RX_RES_STATUS_SEC_TYPE_WEP:
 +      case RX_RES_STATUS_SEC_TYPE_CCMP:
 +              if ((decrypt_res & RX_RES_STATUS_DECRYPT_TYPE_MSK) ==
 +                  RX_RES_STATUS_DECRYPT_OK) {
 +                      IWL_DEBUG_RX("hw decrypt successfully!!!\n");
 +                      stats->flag |= RX_FLAG_DECRYPTED;
 +              }
 +              break;
 +
 +      default:
 +              break;
 +      }
 +      return 0;
 +}
 +
  static u32 iwl4965_translate_rx_status(u32 decrypt_in)
  {
        u32 decrypt_out = 0;
@@@ -2871,10 -3691,8 +2871,10 @@@ static void iwl4965_handle_data_packet(
        stats->flag = 0;
        hdr = (struct ieee80211_hdr *)rxb->skb->data;
  
 -      if (!priv->cfg->mod_params->sw_crypto)
 -              iwl4965_set_decrypted_flag(priv, rxb->skb, ampdu_status, stats);
 +      /*  in case of HW accelerated crypto and bad decryption, drop */
 +      if (!priv->hw_params.sw_crypto &&
 +          iwl4965_set_decrypted_flag(priv, hdr, ampdu_status, stats))
 +              return;
  
        if (priv->add_radiotap)
                iwl4965_add_radiotap(priv, rxb->skb, rx_start, stats, ampdu_status);
@@@ -2919,6 -3737,38 +2919,6 @@@ static int iwl4965_calc_rssi(struct iwl
        return (max_rssi - agc - IWL_RSSI_OFFSET);
  }
  
 -#ifdef CONFIG_IWL4965_HT
 -
 -void iwl4965_init_ht_hw_capab(struct iwl_priv *priv,
 -                            struct ieee80211_ht_info *ht_info,
 -                            enum ieee80211_band band)
 -{
 -      ht_info->cap = 0;
 -      memset(ht_info->supp_mcs_set, 0, 16);
 -
 -      ht_info->ht_supported = 1;
 -
 -      if (band == IEEE80211_BAND_5GHZ) {
 -              ht_info->cap |= (u16)IEEE80211_HT_CAP_SUP_WIDTH;
 -              ht_info->cap |= (u16)IEEE80211_HT_CAP_SGI_40;
 -              ht_info->supp_mcs_set[4] = 0x01;
 -      }
 -      ht_info->cap |= (u16)IEEE80211_HT_CAP_GRN_FLD;
 -      ht_info->cap |= (u16)IEEE80211_HT_CAP_SGI_20;
 -      ht_info->cap |= (u16)(IEEE80211_HT_CAP_MIMO_PS &
 -                           (IWL_MIMO_PS_NONE << 2));
 -
 -      if (priv->cfg->mod_params->amsdu_size_8K)
 -              ht_info->cap |= (u16)IEEE80211_HT_CAP_MAX_AMSDU;
 -
 -      ht_info->ampdu_factor = CFG_HT_RX_AMPDU_FACTOR_DEF;
 -      ht_info->ampdu_density = CFG_HT_MPDU_DENSITY_DEF;
 -
 -      ht_info->supp_mcs_set[0] = 0xFF;
 -      ht_info->supp_mcs_set[1] = 0xFF;
 -}
 -#endif /* CONFIG_IWL4965_HT */
 -
  static void iwl4965_sta_modify_ps_wake(struct iwl_priv *priv, int sta_id)
  {
        unsigned long flags;
  static void iwl4965_update_ps_mode(struct iwl_priv *priv, u16 ps_bit, u8 *addr)
  {
        /* FIXME: need locking over ps_status ??? */
 -      u8 sta_id = iwl4965_hw_find_station(priv, addr);
 +      u8 sta_id = iwl_find_station(priv, addr);
  
        if (sta_id != IWL_INVALID_STATION) {
                u8 sta_awake = priv->stations[sta_id].
@@@ -3128,7 -3978,7 +3128,7 @@@ static void iwl4965_rx_reply_rx(struct 
  
        rx_status.mactime = le64_to_cpu(rx_start->timestamp);
        rx_status.freq =
-               ieee80211_frequency_to_channel(le16_to_cpu(rx_start->channel));
+               ieee80211_channel_to_frequency(le16_to_cpu(rx_start->channel));
        rx_status.band = (rx_start->phy_flags & RX_RES_PHY_FLAGS_BAND_24_MSK) ?
                                IEEE80211_BAND_2GHZ : IEEE80211_BAND_5GHZ;
        rx_status.rate_idx =
@@@ -3289,7 -4139,7 +3289,7 @@@ static void iwl4965_rx_missed_beacon_no
                                           struct iwl4965_rx_mem_buffer *rxb)
  
  {
 -#ifdef CONFIG_IWL4965_SENSITIVITY
 +#ifdef CONFIG_IWL4965_RUN_TIME_CALIB
        struct iwl4965_rx_packet *pkt = (void *)rxb->skb->data;
        struct iwl4965_missed_beacon_notif *missed_beacon;
  
                    le32_to_cpu(missed_beacon->total_missed_becons),
                    le32_to_cpu(missed_beacon->num_recvd_beacons),
                    le32_to_cpu(missed_beacon->num_expected_beacons));
 -              priv->sensitivity_data.state = IWL_SENS_CALIB_NEED_REINIT;
 -              if (unlikely(!test_bit(STATUS_SCANNING, &priv->status)))
 -                      queue_work(priv->workqueue, &priv->sensitivity_work);
 +              if (!test_bit(STATUS_SCANNING, &priv->status))
 +                      iwl_init_sensitivity(priv);
        }
 -#endif /*CONFIG_IWL4965_SENSITIVITY*/
 +#endif /*CONFIG_IWL4965_RUN_TIME_CALIB*/
  }
  #ifdef CONFIG_IWL4965_HT
  
@@@ -3403,8 -4254,8 +3403,8 @@@ static void iwl4965_tx_queue_stop_sched
         * the SCD_ACT_EN bit is the write-enable mask for the ACTIVE bit. */
        iwl_write_prph(priv,
                IWL49_SCD_QUEUE_STATUS_BITS(txq_id),
 -              (0 << SCD_QUEUE_STTS_REG_POS_ACTIVE)|
 -              (1 << SCD_QUEUE_STTS_REG_POS_SCD_ACT_EN));
 +              (0 << IWL49_SCD_QUEUE_STTS_REG_POS_ACTIVE)|
 +              (1 << IWL49_SCD_QUEUE_STTS_REG_POS_SCD_ACT_EN));
  }
  
  /**
@@@ -3569,10 -4420,10 +3569,10 @@@ static int iwl4965_tx_queue_set_q2ratid
        u32 tbl_dw;
        u16 scd_q2ratid;
  
 -      scd_q2ratid = ra_tid & SCD_QUEUE_RA_TID_MAP_RATID_MSK;
 +      scd_q2ratid = ra_tid & IWL49_SCD_QUEUE_RA_TID_MAP_RATID_MSK;
  
        tbl_dw_addr = priv->scd_base_addr +
 -                      SCD_TRANSLATE_TBL_OFFSET_QUEUE(txq_id);
 +                      IWL49_SCD_TRANSLATE_TBL_OFFSET_QUEUE(txq_id);
  
        tbl_dw = iwl_read_targ_mem(priv, tbl_dw_addr);
  
@@@ -3634,14 -4485,14 +3634,14 @@@ static int iwl4965_tx_queue_agg_enable(
  
        /* Set up Tx window size and frame limit for this queue */
        iwl_write_targ_mem(priv,
 -                      priv->scd_base_addr + SCD_CONTEXT_QUEUE_OFFSET(txq_id),
 -                      (SCD_WIN_SIZE << SCD_QUEUE_CTX_REG1_WIN_SIZE_POS) &
 -                      SCD_QUEUE_CTX_REG1_WIN_SIZE_MSK);
 +              priv->scd_base_addr + IWL49_SCD_CONTEXT_QUEUE_OFFSET(txq_id),
 +              (SCD_WIN_SIZE << IWL49_SCD_QUEUE_CTX_REG1_WIN_SIZE_POS) &
 +              IWL49_SCD_QUEUE_CTX_REG1_WIN_SIZE_MSK);
  
        iwl_write_targ_mem(priv, priv->scd_base_addr +
 -                      SCD_CONTEXT_QUEUE_OFFSET(txq_id) + sizeof(u32),
 -                      (SCD_FRAME_LIMIT << SCD_QUEUE_CTX_REG2_FRAME_LIMIT_POS)
 -                      & SCD_QUEUE_CTX_REG2_FRAME_LIMIT_MSK);
 +              IWL49_SCD_CONTEXT_QUEUE_OFFSET(txq_id) + sizeof(u32),
 +              (SCD_FRAME_LIMIT << IWL49_SCD_QUEUE_CTX_REG2_FRAME_LIMIT_POS)
 +              & IWL49_SCD_QUEUE_CTX_REG2_FRAME_LIMIT_MSK);
  
        iwl_set_bits_prph(priv, IWL49_SCD_INTERRUPT_MASK, (1 << txq_id));
  
@@@ -3693,7 -4544,8 +3693,7 @@@ void iwl4965_add_station(struct iwl_pri
                        rate_flags |= RATE_MCS_CCK_MSK;
  
                /* Use Tx antenna B only */
 -              rate_flags |= RATE_MCS_ANT_B_MSK;
 -              rate_flags &= ~RATE_MCS_ANT_A_MSK;
 +              rate_flags |= RATE_MCS_ANT_B_MSK; /*FIXME:RS*/
  
                link_cmd.rs_table[i].rate_n_flags =
                        iwl4965_hw_set_rate_n_flags(iwl4965_rates[r].plcp, rate_flags);
@@@ -3796,15 -4648,13 +3796,15 @@@ void iwl4965_set_rxon_ht(struct iwl_pri
  
        rxon->flags |= cpu_to_le32(val << RXON_FLG_HT_OPERATING_MODE_POS);
  
 -      iwl4965_set_rxon_chain(priv);
 +      iwl_set_rxon_chain(priv);
  
 -      IWL_DEBUG_ASSOC("supported HT rate 0x%X %X "
 +      IWL_DEBUG_ASSOC("supported HT rate 0x%X 0x%X 0x%X "
                        "rxon flags 0x%X operation mode :0x%X "
                        "extension channel offset 0x%x "
                        "control chan %d\n",
 -                      ht_info->supp_mcs_set[0], ht_info->supp_mcs_set[1],
 +                      ht_info->supp_mcs_set[0],
 +                      ht_info->supp_mcs_set[1],
 +                      ht_info->supp_mcs_set[2],
                        le32_to_cpu(rxon->flags), ht_info->ht_protection,
                        ht_info->extension_chan_offset,
                        ht_info->control_channel);
@@@ -3856,15 -4706,10 +3856,15 @@@ void iwl4965_set_ht_add_station(struct 
        return;
  }
  
 -static void iwl4965_sta_modify_add_ba_tid(struct iwl_priv *priv,
 -                                        int sta_id, int tid, u16 ssn)
 +static int iwl4965_rx_agg_start(struct iwl_priv *priv,
 +                              const u8 *addr, int tid, u16 ssn)
  {
        unsigned long flags;
 +      int sta_id;
 +
 +      sta_id = iwl_find_station(priv, addr);
 +      if (sta_id == IWL_INVALID_STATION)
 +              return -ENXIO;
  
        spin_lock_irqsave(&priv->sta_lock, flags);
        priv->stations[sta_id].sta.station_flags_msk = 0;
        priv->stations[sta_id].sta.mode = STA_CONTROL_MODIFY_MSK;
        spin_unlock_irqrestore(&priv->sta_lock, flags);
  
 -      iwl4965_send_add_station(priv, &priv->stations[sta_id].sta, CMD_ASYNC);
 +      return iwl4965_send_add_station(priv, &priv->stations[sta_id].sta,
 +                                      CMD_ASYNC);
  }
  
 -static void iwl4965_sta_modify_del_ba_tid(struct iwl_priv *priv,
 -                                        int sta_id, int tid)
 +static int iwl4965_rx_agg_stop(struct iwl_priv *priv,
 +                             const u8 *addr, int tid)
  {
        unsigned long flags;
 +      int sta_id;
 +
 +      sta_id = iwl_find_station(priv, addr);
 +      if (sta_id == IWL_INVALID_STATION)
 +              return -ENXIO;
  
        spin_lock_irqsave(&priv->sta_lock, flags);
        priv->stations[sta_id].sta.station_flags_msk = 0;
        priv->stations[sta_id].sta.mode = STA_CONTROL_MODIFY_MSK;
        spin_unlock_irqrestore(&priv->sta_lock, flags);
  
 -      iwl4965_send_add_station(priv, &priv->stations[sta_id].sta, CMD_ASYNC);
 +      return iwl4965_send_add_station(priv, &priv->stations[sta_id].sta,
 +                                      CMD_ASYNC);
  }
  
  /*
@@@ -3915,8 -4753,8 +3915,8 @@@ static int iwl4965_txq_ctx_activate_fre
        return -1;
  }
  
 -static int iwl4965_mac_ht_tx_agg_start(struct ieee80211_hw *hw, const u8 *da,
 -                                     u16 tid, u16 *start_seq_num)
 +static int iwl4965_tx_agg_start(struct ieee80211_hw *hw, const u8 *ra,
 +                              u16 tid, u16 *start_seq_num)
  {
        struct iwl_priv *priv = hw->priv;
        int sta_id;
        else
                return -EINVAL;
  
 -      IWL_WARNING("%s on da = %s tid = %d\n",
 -                      __func__, print_mac(mac, da), tid);
 +      IWL_WARNING("%s on ra = %s tid = %d\n",
 +                      __func__, print_mac(mac, ra), tid);
  
 -      sta_id = iwl4965_hw_find_station(priv, da);
 +      sta_id = iwl_find_station(priv, ra);
        if (sta_id == IWL_INVALID_STATION)
                return -ENXIO;
  
        if (tid_data->tfds_in_queue == 0) {
                printk(KERN_ERR "HW queue is empty\n");
                tid_data->agg.state = IWL_AGG_ON;
 -              ieee80211_start_tx_ba_cb_irqsafe(hw, da, tid);
 +              ieee80211_start_tx_ba_cb_irqsafe(hw, ra, tid);
        } else {
                IWL_DEBUG_HT("HW queue is NOT empty: %d packets in HW queue\n",
                                tid_data->tfds_in_queue);
        return ret;
  }
  
 -static int iwl4965_mac_ht_tx_agg_stop(struct ieee80211_hw *hw, const u8 *da,
 -                                    u16 tid)
 +static int iwl4965_tx_agg_stop(struct ieee80211_hw *hw, const u8 *ra, u16 tid)
  {
 -
        struct iwl_priv *priv = hw->priv;
        int tx_fifo_id, txq_id, sta_id, ssn = -1;
        struct iwl4965_tid_data *tid_data;
        unsigned long flags;
        DECLARE_MAC_BUF(mac);
  
 -      if (!da) {
 -              IWL_ERROR("da = NULL\n");
 +      if (!ra) {
 +              IWL_ERROR("ra = NULL\n");
                return -EINVAL;
        }
  
        else
                return -EINVAL;
  
 -      sta_id = iwl4965_hw_find_station(priv, da);
 +      sta_id = iwl_find_station(priv, ra);
  
        if (sta_id == IWL_INVALID_STATION)
                return -ENXIO;
                return 0;
        }
  
 -      IWL_DEBUG_HT("HW queue empty\n");;
 +      IWL_DEBUG_HT("HW queue is empty\n");
        priv->stations[sta_id].tid[tid].agg.state = IWL_AGG_OFF;
  
        spin_lock_irqsave(&priv->lock, flags);
        if (ret)
                return ret;
  
 -      ieee80211_stop_tx_ba_cb_irqsafe(priv->hw, da, tid);
 -
 -      IWL_DEBUG_INFO("iwl4965_mac_ht_tx_agg_stop on da=%s tid=%d\n",
 -                      print_mac(mac, da), tid);
 +      ieee80211_stop_tx_ba_cb_irqsafe(priv->hw, ra, tid);
  
        return 0;
  }
@@@ -4035,24 -4878,27 +4035,24 @@@ int iwl4965_mac_ampdu_action(struct iee
                             const u8 *addr, u16 tid, u16 *ssn)
  {
        struct iwl_priv *priv = hw->priv;
 -      int sta_id;
        DECLARE_MAC_BUF(mac);
  
 -      IWL_DEBUG_HT("A-MPDU action on da=%s tid=%d ",
 -                      print_mac(mac, addr), tid);
 -      sta_id = iwl4965_hw_find_station(priv, addr);
 +      IWL_DEBUG_HT("A-MPDU action on addr %s tid %d\n",
 +                   print_mac(mac, addr), tid);
 +
        switch (action) {
        case IEEE80211_AMPDU_RX_START:
                IWL_DEBUG_HT("start Rx\n");
 -              iwl4965_sta_modify_add_ba_tid(priv, sta_id, tid, *ssn);
 -              break;
 +              return iwl4965_rx_agg_start(priv, addr, tid, *ssn);
        case IEEE80211_AMPDU_RX_STOP:
                IWL_DEBUG_HT("stop Rx\n");
 -              iwl4965_sta_modify_del_ba_tid(priv, sta_id, tid);
 -              break;
 +              return iwl4965_rx_agg_stop(priv, addr, tid);
        case IEEE80211_AMPDU_TX_START:
                IWL_DEBUG_HT("start Tx\n");
 -              return iwl4965_mac_ht_tx_agg_start(hw, addr, tid, ssn);
 +              return iwl4965_tx_agg_start(hw, addr, tid, ssn);
        case IEEE80211_AMPDU_TX_STOP:
                IWL_DEBUG_HT("stop Tx\n");
 -              return iwl4965_mac_ht_tx_agg_stop(hw, addr, tid);
 +              return iwl4965_tx_agg_stop(hw, addr, tid);
        default:
                IWL_DEBUG_HT("unknown\n");
                return -EINVAL;
  #endif /* CONFIG_IWL4965_HT */
  
  /* Set up 4965-specific Rx frame reply handlers */
 -void iwl4965_hw_rx_handler_setup(struct iwl_priv *priv)
 +static void iwl4965_rx_handler_setup(struct iwl_priv *priv)
  {
        /* Legacy Rx frames */
        priv->rx_handlers[REPLY_RX] = iwl4965_rx_reply_rx;
  void iwl4965_hw_setup_deferred_work(struct iwl_priv *priv)
  {
        INIT_WORK(&priv->txpower_work, iwl4965_bg_txpower_work);
 -#ifdef CONFIG_IWL4965_SENSITIVITY
 +#ifdef CONFIG_IWL4965_RUN_TIME_CALIB
        INIT_WORK(&priv->sensitivity_work, iwl4965_bg_sensitivity_work);
  #endif
        init_timer(&priv->statistics_periodic);
@@@ -4106,46 -4952,22 +4106,46 @@@ static struct iwl_hcmd_ops iwl4965_hcm
  
  static struct iwl_hcmd_utils_ops iwl4965_hcmd_utils = {
        .enqueue_hcmd = iwl4965_enqueue_hcmd,
 +#ifdef CONFIG_IWL4965_RUN_TIME_CALIB
 +      .chain_noise_reset = iwl4965_chain_noise_reset,
 +      .gain_computation = iwl4965_gain_computation,
 +#endif
  };
  
  static struct iwl_lib_ops iwl4965_lib = {
 -      .init_drv = iwl4965_init_drv,
        .set_hw_params = iwl4965_hw_set_hw_params,
 +      .alloc_shared_mem = iwl4965_alloc_shared_mem,
 +      .free_shared_mem = iwl4965_free_shared_mem,
        .txq_update_byte_cnt_tbl = iwl4965_txq_update_byte_cnt_tbl,
        .hw_nic_init = iwl4965_hw_nic_init,
 +      .rx_handler_setup = iwl4965_rx_handler_setup,
        .is_valid_rtc_data_addr = iwl4965_hw_valid_rtc_data_addr,
        .alive_notify = iwl4965_alive_notify,
        .load_ucode = iwl4965_load_bsm,
 +      .apm_ops = {
 +              .init = iwl4965_apm_init,
 +              .config = iwl4965_nic_config,
 +              .set_pwr_src = iwl4965_set_pwr_src,
 +      },
        .eeprom_ops = {
 +              .regulatory_bands = {
 +                      EEPROM_REGULATORY_BAND_1_CHANNELS,
 +                      EEPROM_REGULATORY_BAND_2_CHANNELS,
 +                      EEPROM_REGULATORY_BAND_3_CHANNELS,
 +                      EEPROM_REGULATORY_BAND_4_CHANNELS,
 +                      EEPROM_REGULATORY_BAND_5_CHANNELS,
 +                      EEPROM_4965_REGULATORY_BAND_24_FAT_CHANNELS,
 +                      EEPROM_4965_REGULATORY_BAND_52_FAT_CHANNELS
 +              },
                .verify_signature  = iwlcore_eeprom_verify_signature,
                .acquire_semaphore = iwlcore_eeprom_acquire_semaphore,
                .release_semaphore = iwlcore_eeprom_release_semaphore,
 +              .check_version = iwl4965_eeprom_check_version,
 +              .query_addr = iwlcore_eeprom_query_addr,
        },
        .radio_kill_sw = iwl4965_radio_kill_sw,
 +      .set_power = iwl4965_set_power,
 +      .update_chain_flags = iwl4965_update_chain_flags,
  };
  
  static struct iwl_ops iwl4965_ops = {
@@@ -4158,7 -4980,6 +4158,7 @@@ struct iwl_cfg iwl4965_agn_cfg = 
        .name = "4965AGN",
        .fw_name = "iwlwifi-4965" IWL4965_UCODE_API ".ucode",
        .sku = IWL_SKU_A|IWL_SKU_G|IWL_SKU_N,
 +      .eeprom_size = IWL4965_EEPROM_IMG_SIZE,
        .ops = &iwl4965_ops,
        .mod_params = &iwl4965_mod_params,
  };
@@@ -525,6 -525,9 +525,6 @@@ void rt2x00lib_txdone(struct queue_entr
                        rt2x00dev->low_level_stats.dot11ACKFailureCount++;
        }
  
 -      tx_status.queue_length = entry->queue->limit;
 -      tx_status.queue_number = tx_status.control.queue;
 -
        if (tx_status.control.flags & IEEE80211_TXCTL_USE_RTS_CTS) {
                if (success)
                        rt2x00dev->low_level_stats.dot11RTSSuccessCount++;
@@@ -684,7 -687,8 +684,7 @@@ void rt2x00lib_write_tx_desc(struct rt2
         * Beacons and probe responses require the tsf timestamp
         * to be inserted into the frame.
         */
 -      if (control->queue == RT2X00_BCN_QUEUE_BEACON ||
 -          is_probe_resp(frame_control))
 +      if (txdesc.queue == QID_BEACON || is_probe_resp(frame_control))
                __set_bit(ENTRY_TXD_REQ_TIMESTAMP, &txdesc.flags);
  
        /*
@@@ -1028,8 -1032,10 +1028,10 @@@ static int rt2x00lib_initialize(struct 
         * Initialize the device.
         */
        status = rt2x00dev->ops->lib->initialize(rt2x00dev);
-       if (status)
-               goto exit;
+       if (status) {
+               rt2x00queue_uninitialize(rt2x00dev);
+               return status;
+       }
  
        __set_bit(DEVICE_INITIALIZED, &rt2x00dev->flags);
  
        rt2x00rfkill_register(rt2x00dev);
  
        return 0;
- exit:
-       rt2x00lib_uninitialize(rt2x00dev);
-       return status;
  }
  
  int rt2x00lib_start(struct rt2x00_dev *rt2x00dev)
@@@ -53,7 -53,7 +53,7 @@@ int rt2x00pci_write_tx_data(struct rt2x
                ERROR(rt2x00dev,
                      "Arrived at non-free entry in the non-full queue %d.\n"
                      "Please file bug report to %s.\n",
 -                    control->queue, DRV_PROJECT);
 +                    entry->queue->qid, DRV_PROJECT);
                return -EINVAL;
        }
  
@@@ -314,13 -314,14 +314,14 @@@ int rt2x00pci_initialize(struct rt2x00_
        if (status) {
                ERROR(rt2x00dev, "IRQ %d allocation failed (error %d).\n",
                      pci_dev->irq, status);
-               return status;
+               goto exit;
        }
  
        return 0;
  
  exit:
-       rt2x00pci_uninitialize(rt2x00dev);
+       queue_for_each(rt2x00dev, queue)
+               rt2x00pci_free_queue_dma(rt2x00dev, queue);
  
        return status;
  }
@@@ -1591,11 -1591,11 +1591,11 @@@ static void rt61pci_write_tx_desc(struc
   * TX data initialization
   */
  static void rt61pci_kick_tx_queue(struct rt2x00_dev *rt2x00dev,
 -                                const unsigned int queue)
 +                                const enum data_queue_qid queue)
  {
        u32 reg;
  
 -      if (queue == RT2X00_BCN_QUEUE_BEACON) {
 +      if (queue == QID_BEACON) {
                /*
                 * For Wi-Fi faily generated beacons between participating
                 * stations. Set TBTT phase adaptive adjustment step to 8us.
        }
  
        rt2x00pci_register_read(rt2x00dev, TX_CNTL_CSR, &reg);
 -      rt2x00_set_field32(&reg, TX_CNTL_CSR_KICK_TX_AC0,
 -                         (queue == IEEE80211_TX_QUEUE_DATA0));
 -      rt2x00_set_field32(&reg, TX_CNTL_CSR_KICK_TX_AC1,
 -                         (queue == IEEE80211_TX_QUEUE_DATA1));
 -      rt2x00_set_field32(&reg, TX_CNTL_CSR_KICK_TX_AC2,
 -                         (queue == IEEE80211_TX_QUEUE_DATA2));
 -      rt2x00_set_field32(&reg, TX_CNTL_CSR_KICK_TX_AC3,
 -                         (queue == IEEE80211_TX_QUEUE_DATA3));
 +      rt2x00_set_field32(&reg, TX_CNTL_CSR_KICK_TX_AC0, (queue == QID_AC_BE));
 +      rt2x00_set_field32(&reg, TX_CNTL_CSR_KICK_TX_AC1, (queue == QID_AC_BK));
 +      rt2x00_set_field32(&reg, TX_CNTL_CSR_KICK_TX_AC2, (queue == QID_AC_VI));
 +      rt2x00_set_field32(&reg, TX_CNTL_CSR_KICK_TX_AC3, (queue == QID_AC_VO));
        rt2x00pci_register_write(rt2x00dev, TX_CNTL_CSR, reg);
  }
  
@@@ -2362,6 -2366,7 +2362,7 @@@ static int rt61pci_beacon_update(struc
  {
        struct rt2x00_dev *rt2x00dev = hw->priv;
        struct rt2x00_intf *intf = vif_to_intf(control->vif);
+       struct queue_entry_priv_pci_tx *priv_tx;
        struct skb_frame_desc *skbdesc;
        unsigned int beacon_base;
        u32 reg;
        if (unlikely(!intf->beacon))
                return -ENOBUFS;
  
-       /*
-        * We need to append the descriptor in front of the
-        * beacon frame.
-        */
-       if (skb_headroom(skb) < intf->beacon->queue->desc_size) {
-               if (pskb_expand_head(skb, intf->beacon->queue->desc_size,
-                                    0, GFP_ATOMIC))
-                       return -ENOMEM;
-       }
-       /*
-        * Add the descriptor in front of the skb.
-        */
-       skb_push(skb, intf->beacon->queue->desc_size);
-       memset(skb->data, 0, intf->beacon->queue->desc_size);
+       priv_tx = intf->beacon->priv_data;
+       memset(priv_tx->desc, 0, intf->beacon->queue->desc_size);
  
        /*
         * Fill in skb descriptor
        skbdesc = get_skb_frame_desc(skb);
        memset(skbdesc, 0, sizeof(*skbdesc));
        skbdesc->flags |= FRAME_DESC_DRIVER_GENERATED;
-       skbdesc->data = skb->data + intf->beacon->queue->desc_size;
-       skbdesc->data_len = skb->len - intf->beacon->queue->desc_size;
-       skbdesc->desc = skb->data;
+       skbdesc->data = skb->data;
+       skbdesc->data_len = skb->len;
+       skbdesc->desc = priv_tx->desc;
        skbdesc->desc_len = intf->beacon->queue->desc_size;
        skbdesc->entry = intf->beacon;
  
        rt2x00pci_register_write(rt2x00dev, TXRX_CSR9, reg);
  
        /*
         * Write entire beacon with descriptor to register,
         * and kick the beacon generator.
         */
 +      rt2x00lib_write_tx_desc(rt2x00dev, skb, control);
        beacon_base = HW_BEACON_OFFSET(intf->beacon->entry_idx);
        rt2x00pci_register_multiwrite(rt2x00dev, beacon_base,
-                                     skb->data, skb->len);
+                                     skbdesc->desc, skbdesc->desc_len);
+       rt2x00pci_register_multiwrite(rt2x00dev,
+                                     beacon_base + skbdesc->desc_len,
+                                     skbdesc->data, skbdesc->data_len);
 -      rt61pci_kick_tx_queue(rt2x00dev, control->queue);
 +      rt61pci_kick_tx_queue(rt2x00dev, QID_BEACON);
  
        return 0;
  }
@@@ -2479,7 -2481,7 +2470,7 @@@ static const struct data_queue_desc rt6
  
  static const struct data_queue_desc rt61pci_queue_bcn = {
        .entry_num              = 4 * BEACON_ENTRIES,
-       .data_size              = MGMT_FRAME_SIZE,
+       .data_size              = 0, /* No DMA required for beacons */
        .desc_size              = TXINFO_SIZE,
        .priv_size              = sizeof(struct queue_entry_priv_pci_tx),
  };
diff --combined net/mac80211/iface.c
@@@ -33,8 -33,9 +33,8 @@@ static void ieee80211_if_sdata_deinit(s
  {
        int i;
  
 -      for (i = 0; i < IEEE80211_FRAGMENT_MAX; i++) {
 +      for (i = 0; i < IEEE80211_FRAGMENT_MAX; i++)
                __skb_queue_purge(&sdata->fragments[i].skb_list);
 -      }
  }
  
  /* Must be called with rtnl lock held. */
@@@ -53,6 -54,15 +53,15 @@@ int ieee80211_if_add(struct net_device 
        if (!ndev)
                return -ENOMEM;
  
+       ndev->needed_headroom = local->tx_headroom +
+                               4*6 /* four MAC addresses */
+                               + 2 + 2 + 2 + 2 /* ctl, dur, seq, qos */
+                               + 6 /* mesh */
+                               + 8 /* rfc1042/bridge tunnel */
+                               - ETH_HLEN /* ethernet hard_header_len */
+                               + IEEE80211_ENCRYPT_HEADROOM;
+       ndev->needed_tailroom = IEEE80211_ENCRYPT_TAILROOM;
        ret = dev_alloc_name(ndev, ndev->name);
        if (ret < 0)
                goto fail;
diff --combined net/mac80211/main.c
@@@ -385,8 -385,8 +385,8 @@@ static int ieee80211_open(struct net_de
         * yet be effective. Trigger execution of ieee80211_sta_work
         * to fix this.
         */
 -      if(sdata->vif.type == IEEE80211_IF_TYPE_STA ||
 -         sdata->vif.type == IEEE80211_IF_TYPE_IBSS) {
 +      if (sdata->vif.type == IEEE80211_IF_TYPE_STA ||
 +          sdata->vif.type == IEEE80211_IF_TYPE_IBSS) {
                struct ieee80211_if_sta *ifsta = &sdata->u.sta;
                queue_work(local->hw.workqueue, &ifsta->work);
        }
@@@ -1482,7 -1482,7 +1482,7 @@@ void ieee80211_tx_status(struct ieee802
                return;
        }
  
 -      rthdr = (struct ieee80211_tx_status_rtap_hdr*)
 +      rthdr = (struct ieee80211_tx_status_rtap_hdr *)
                                skb_push(skb, sizeof(*rthdr));
  
        memset(rthdr, 0, sizeof(*rthdr));
@@@ -1745,11 -1745,6 +1745,11 @@@ int ieee80211_register_hw(struct ieee80
                goto fail_wep;
        }
  
 +      if (hw->queues > IEEE80211_MAX_QUEUES)
 +              hw->queues = IEEE80211_MAX_QUEUES;
 +      if (hw->ampdu_queues > IEEE80211_MAX_AMPDU_QUEUES)
 +              hw->ampdu_queues = IEEE80211_MAX_AMPDU_QUEUES;
 +
        ieee80211_install_qdisc(local->mdev);
  
        /* add one default STA interface */
@@@ -1771,6 -1766,7 +1771,7 @@@ fail_wep
  fail_rate:
        ieee80211_debugfs_remove_netdev(IEEE80211_DEV_TO_SUB_IF(local->mdev));
        unregister_netdevice(local->mdev);
+       local->mdev = NULL;
  fail_dev:
        rtnl_unlock();
        sta_info_stop(local);
@@@ -1778,8 -1774,10 +1779,10 @@@ fail_sta_info
        debugfs_hw_del(local);
        destroy_workqueue(local->hw.workqueue);
  fail_workqueue:
-       ieee80211_if_free(local->mdev);
-       local->mdev = NULL;
+       if (local->mdev != NULL) {
+               ieee80211_if_free(local->mdev);
+               local->mdev = NULL;
+       }
  fail_mdev_alloc:
        wiphy_unregister(local->hw.wiphy);
        return result;
diff --combined net/mac80211/mlme.c
@@@ -87,7 -87,6 +87,7 @@@ static int ieee80211_sta_start_scan(str
                                    u8 *ssid, size_t ssid_len);
  static int ieee80211_sta_config_auth(struct net_device *dev,
                                     struct ieee80211_if_sta *ifsta);
 +static void sta_rx_agg_session_timer_expired(unsigned long data);
  
  
  void ieee802_11_parse_elems(u8 *start, size_t len,
@@@ -257,8 -256,19 +257,8 @@@ static void ieee80211_sta_def_wmm_param
                qparam.cw_max = 1023;
                qparam.txop = 0;
  
 -              for (i = IEEE80211_TX_QUEUE_DATA0; i < NUM_TX_DATA_QUEUES; i++)
 -                      local->ops->conf_tx(local_to_hw(local),
 -                                         i + IEEE80211_TX_QUEUE_DATA0,
 -                                         &qparam);
 -
 -              if (ibss) {
 -                      /* IBSS uses different parameters for Beacon sending */
 -                      qparam.cw_min++;
 -                      qparam.cw_min *= 2;
 -                      qparam.cw_min--;
 -                      local->ops->conf_tx(local_to_hw(local),
 -                                         IEEE80211_TX_QUEUE_BEACON, &qparam);
 -              }
 +              for (i = 0; i < local_to_hw(local)->queues; i++)
 +                      local->ops->conf_tx(local_to_hw(local), i, &qparam);
        }
  }
  
@@@ -295,25 -305,29 +295,25 @@@ static void ieee80211_sta_wmm_params(st
  
                switch (aci) {
                case 1:
 -                      queue = IEEE80211_TX_QUEUE_DATA3;
 -                      if (acm) {
 +                      queue = 3;
 +                      if (acm)
                                local->wmm_acm |= BIT(0) | BIT(3);
 -                      }
                        break;
                case 2:
 -                      queue = IEEE80211_TX_QUEUE_DATA1;
 -                      if (acm) {
 +                      queue = 1;
 +                      if (acm)
                                local->wmm_acm |= BIT(4) | BIT(5);
 -                      }
                        break;
                case 3:
 -                      queue = IEEE80211_TX_QUEUE_DATA0;
 -                      if (acm) {
 +                      queue = 0;
 +                      if (acm)
                                local->wmm_acm |= BIT(6) | BIT(7);
                        break;
                case 0:
                default:
 -                      queue = IEEE80211_TX_QUEUE_DATA2;
 -                      if (acm) {
 +                      queue = 2;
 +                      if (acm)
                                local->wmm_acm |= BIT(1) | BIT(2);
 -                      }
                        break;
                }
  
@@@ -651,6 -665,26 +651,26 @@@ static void ieee80211_authenticate(stru
        mod_timer(&ifsta->timer, jiffies + IEEE80211_AUTH_TIMEOUT);
  }
  
+ static int ieee80211_compatible_rates(struct ieee80211_sta_bss *bss,
+                                     struct ieee80211_supported_band *sband,
+                                     u64 *rates)
+ {
+       int i, j, count;
+       *rates = 0;
+       count = 0;
+       for (i = 0; i < bss->supp_rates_len; i++) {
+               int rate = (bss->supp_rates[i] & 0x7F) * 5;
+               for (j = 0; j < sband->n_bitrates; j++)
+                       if (sband->bitrates[j].bitrate == rate) {
+                               *rates |= BIT(j);
+                               count++;
+                               break;
+                       }
+       }
+       return count;
+ }
  
  static void ieee80211_send_assoc(struct net_device *dev,
                                 struct ieee80211_if_sta *ifsta)
        struct sk_buff *skb;
        struct ieee80211_mgmt *mgmt;
        u8 *pos, *ies;
-       int i, len;
+       int i, len, count, rates_len, supp_rates_len;
        u16 capab;
        struct ieee80211_sta_bss *bss;
        int wmm = 0;
        struct ieee80211_supported_band *sband;
+       u64 rates = 0;
  
        skb = dev_alloc_skb(local->hw.extra_tx_headroom +
                            sizeof(*mgmt) + 200 + ifsta->extra_ie_len +
        if (bss) {
                if (bss->capability & WLAN_CAPABILITY_PRIVACY)
                        capab |= WLAN_CAPABILITY_PRIVACY;
 -              if (bss->wmm_ie) {
 +              if (bss->wmm_ie)
                        wmm = 1;
 -              }
                ieee80211_rx_bss_put(dev, bss);
        }
  
        *pos++ = ifsta->ssid_len;
        memcpy(pos, ifsta->ssid, ifsta->ssid_len);
  
+       /* all supported rates should be added here but some APs
+        * (e.g. D-Link DAP 1353 in b-only mode) don't like that
+        * Therefore only add rates the AP supports */
+       rates_len = ieee80211_compatible_rates(bss, sband, &rates);
+       supp_rates_len = rates_len;
+       if (supp_rates_len > 8)
+               supp_rates_len = 8;
        len = sband->n_bitrates;
-       if (len > 8)
-               len = 8;
-       pos = skb_put(skb, len + 2);
+       pos = skb_put(skb, supp_rates_len + 2);
        *pos++ = WLAN_EID_SUPP_RATES;
-       *pos++ = len;
-       for (i = 0; i < len; i++) {
-               int rate = sband->bitrates[i].bitrate;
-               *pos++ = (u8) (rate / 5);
-       }
+       *pos++ = supp_rates_len;
  
-       if (sband->n_bitrates > len) {
-               pos = skb_put(skb, sband->n_bitrates - len + 2);
-               *pos++ = WLAN_EID_EXT_SUPP_RATES;
-               *pos++ = sband->n_bitrates - len;
-               for (i = len; i < sband->n_bitrates; i++) {
+       count = 0;
+       for (i = 0; i < sband->n_bitrates; i++) {
+               if (BIT(i) & rates) {
                        int rate = sband->bitrates[i].bitrate;
                        *pos++ = (u8) (rate / 5);
+                       if (++count == 8)
+                               break;
+               }
+       }
+       if (count == 8) {
+               pos = skb_put(skb, rates_len - count + 2);
+               *pos++ = WLAN_EID_EXT_SUPP_RATES;
+               *pos++ = rates_len - count;
+               for (i++; i < sband->n_bitrates; i++) {
+                       if (BIT(i) & rates) {
+                               int rate = sband->bitrates[i].bitrate;
+                               *pos++ = (u8) (rate / 5);
+                       }
                }
        }
  
@@@ -1083,8 -1134,8 +1119,8 @@@ static void ieee80211_send_addba_resp(s
        struct ieee80211_mgmt *mgmt;
        u16 capab;
  
 -      skb = dev_alloc_skb(sizeof(*mgmt) + local->hw.extra_tx_headroom + 1 +
 -                                      sizeof(mgmt->u.action.u.addba_resp));
 +      skb = dev_alloc_skb(sizeof(*mgmt) + local->hw.extra_tx_headroom);
 +
        if (!skb) {
                printk(KERN_DEBUG "%s: failed to allocate buffer "
                       "for addba resp frame\n", dev->name);
@@@ -1132,7 -1183,9 +1168,7 @@@ void ieee80211_send_addba_request(struc
        struct ieee80211_mgmt *mgmt;
        u16 capab;
  
 -      skb = dev_alloc_skb(sizeof(*mgmt) + local->hw.extra_tx_headroom + 1 +
 -                              sizeof(mgmt->u.action.u.addba_req));
 -
 +      skb = dev_alloc_skb(sizeof(*mgmt) + local->hw.extra_tx_headroom);
  
        if (!skb) {
                printk(KERN_ERR "%s: failed to allocate buffer "
@@@ -1394,7 -1447,8 +1430,7 @@@ void ieee80211_send_delba(struct net_de
        struct ieee80211_mgmt *mgmt;
        u16 params;
  
 -      skb = dev_alloc_skb(sizeof(*mgmt) + local->hw.extra_tx_headroom + 1 +
 -                                      sizeof(mgmt->u.action.u.delba));
 +      skb = dev_alloc_skb(sizeof(*mgmt) + local->hw.extra_tx_headroom);
  
        if (!skb) {
                printk(KERN_ERR "%s: failed to allocate buffer "
@@@ -1598,7 -1652,7 +1634,7 @@@ timer_expired_exit
   * resetting it after each frame that arrives from the originator.
   * if this timer expires ieee80211_sta_stop_rx_ba_session will be executed.
   */
 -void sta_rx_agg_session_timer_expired(unsigned long data)
 +static void sta_rx_agg_session_timer_expired(unsigned long data)
  {
        /* not an elegant detour, but there is no choice as the timer passes
         * only one argument, and verious sta_info are needed here, so init
@@@ -1787,8 -1841,9 +1823,8 @@@ static void ieee80211_rx_mgmt_deauth(st
               " (reason=%d)\n",
               dev->name, print_mac(mac, mgmt->sa), reason_code);
  
 -      if (ifsta->flags & IEEE80211_STA_AUTHENTICATED) {
 +      if (ifsta->flags & IEEE80211_STA_AUTHENTICATED)
                printk(KERN_DEBUG "%s: deauthenticated\n", dev->name);
 -      }
  
        if (ifsta->state == IEEE80211_AUTHENTICATE ||
            ifsta->state == IEEE80211_ASSOCIATE ||
@@@ -3498,12 -3553,10 +3534,12 @@@ static int ieee80211_sta_create_ibss(st
        bss->beacon_int = local->hw.conf.beacon_int;
        bss->last_update = jiffies;
        bss->capability = WLAN_CAPABILITY_IBSS;
 -      if (sdata->default_key) {
 +
 +      if (sdata->default_key)
                bss->capability |= WLAN_CAPABILITY_PRIVACY;
 -      } else
 +      else
                sdata->drop_unencrypted = 0;
 +
        bss->supp_rates_len = sband->n_bitrates;
        pos = bss->supp_rates;
        for (i = 0; i < sband->n_bitrates; i++) {
@@@ -4186,7 -4239,6 +4222,7 @@@ int ieee80211_sta_set_extra_ie(struct n
  {
        struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
        struct ieee80211_if_sta *ifsta = &sdata->u.sta;
 +
        kfree(ifsta->extra_ie);
        if (len == 0) {
                ifsta->extra_ie = NULL;
  }
  
  
 -struct sta_info * ieee80211_ibss_add_sta(struct net_device *dev,
 -                                       struct sk_buff *skb, u8 *bssid,
 -                                       u8 *addr)
 +struct sta_info *ieee80211_ibss_add_sta(struct net_device *dev,
 +                                      struct sk_buff *skb, u8 *bssid,
 +                                      u8 *addr)
  {
        struct ieee80211_local *local = wdev_priv(dev->ieee80211_ptr);
        struct sta_info *sta;
diff --combined net/mac80211/rx.c
@@@ -275,6 -275,11 +275,6 @@@ static void ieee80211_parse_qos(struct 
                }
        }
  
 -      I802_DEBUG_INC(rx->local->wme_rx_queue[tid]);
 -      /* only a debug counter, sta might not be assigned properly yet */
 -      if (rx->sta)
 -              I802_DEBUG_INC(rx->sta->wme_rx_queue[tid]);
 -
        rx->queue = tid;
        /* Set skb->priority to 1d tag if highest order bit of TID is not set.
         * For now, set skb->priority to 0 for other cases. */
@@@ -1300,11 -1305,11 +1300,11 @@@ ieee80211_deliver_skb(struct ieee80211_
                if (is_multicast_ether_addr(skb->data)) {
                        if (*mesh_ttl > 0) {
                                xmit_skb = skb_copy(skb, GFP_ATOMIC);
-                               if (!xmit_skb && net_ratelimit())
+                               if (xmit_skb)
+                                       xmit_skb->pkt_type = PACKET_OTHERHOST;
+                               else if (net_ratelimit())
                                        printk(KERN_DEBUG "%s: failed to clone "
                                               "multicast frame\n", dev->name);
-                               else
-                                       xmit_skb->pkt_type = PACKET_OTHERHOST;
                        } else
                                IEEE80211_IFSTA_MESH_CTR_INC(&sdata->u.sta,
                                                             dropped_frames_ttl);
@@@ -1390,7 -1395,7 +1390,7 @@@ ieee80211_rx_h_amsdu(struct ieee80211_r
                padding = ((4 - subframe_len) & 0x3);
                /* the last MSDU has no padding */
                if (subframe_len > remaining) {
-                       printk(KERN_DEBUG "%s: wrong buffer size", dev->name);
+                       printk(KERN_DEBUG "%s: wrong buffer size\n", dev->name);
                        return RX_DROP_UNUSABLE;
                }
  
                        eth = (struct ethhdr *) skb_pull(skb, ntohs(len) +
                                                        padding);
                        if (!eth) {
-                               printk(KERN_DEBUG "%s: wrong buffer size ",
+                               printk(KERN_DEBUG "%s: wrong buffer size\n",
                                       dev->name);
                                dev_kfree_skb(frame);
                                return RX_DROP_UNUSABLE;
@@@ -1947,7 -1952,7 +1947,7 @@@ static void __ieee80211_rx_handle_packe
                if (!skb_new) {
                        if (net_ratelimit())
                                printk(KERN_DEBUG "%s: failed to copy "
-                                      "multicast frame for %s",
+                                      "multicast frame for %s\n",
                                       wiphy_name(local->hw.wiphy),
                                       prev->dev->name);
                        continue;
diff --combined net/mac80211/wme.c
  #include "wme.h"
  
  /* maximum number of hardware queues we support. */
 -#define TC_80211_MAX_QUEUES 16
 +#define QD_MAX_QUEUES (IEEE80211_MAX_AMPDU_QUEUES + IEEE80211_MAX_QUEUES)
 +/* current number of hardware queues we support. */
 +#define QD_NUM(hw) ((hw)->queues + (hw)->ampdu_queues)
  
 +/*
 + * Default mapping in classifier to work with default
 + * queue setup.
 + */
  const int ieee802_1d_to_ac[8] = { 2, 3, 3, 2, 1, 1, 0, 0 };
  
  struct ieee80211_sched_data
  {
 -      unsigned long qdisc_pool[BITS_TO_LONGS(TC_80211_MAX_QUEUES)];
 +      unsigned long qdisc_pool[BITS_TO_LONGS(QD_MAX_QUEUES)];
        struct tcf_proto *filter_list;
 -      struct Qdisc *queues[TC_80211_MAX_QUEUES];
 -      struct sk_buff_head requeued[TC_80211_MAX_QUEUES];
 +      struct Qdisc *queues[QD_MAX_QUEUES];
 +      struct sk_buff_head requeued[QD_MAX_QUEUES];
  };
  
  static const char llc_ip_hdr[8] = {0xAA, 0xAA, 0x3, 0, 0, 0, 0x08, 0};
@@@ -101,7 -95,7 +101,7 @@@ static inline int wme_downgrade_ac(stru
  
  /* positive return value indicates which queue to use
   * negative return value indicates to drop the frame */
 -static inline int classify80211(struct sk_buff *skb, struct Qdisc *qd)
 +static int classify80211(struct sk_buff *skb, struct Qdisc *qd)
  {
        struct ieee80211_local *local = wdev_priv(qd->dev->ieee80211_ptr);
        struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
        if (unlikely((fc & IEEE80211_FCTL_FTYPE) != IEEE80211_FTYPE_DATA)) {
                /* management frames go on AC_VO queue, but are sent
                * without QoS control fields */
 -              return IEEE80211_TX_QUEUE_DATA0;
 +              return 0;
        }
  
        if (0 /* injected */) {
  static int wme_qdiscop_enqueue(struct sk_buff *skb, struct Qdisc* qd)
  {
        struct ieee80211_local *local = wdev_priv(qd->dev->ieee80211_ptr);
 +      struct ieee80211_hw *hw = &local->hw;
        struct ieee80211_sched_data *q = qdisc_priv(qd);
        struct ieee80211_tx_packet_data *pkt_data =
                (struct ieee80211_tx_packet_data *) skb->cb;
        struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
        unsigned short fc = le16_to_cpu(hdr->frame_control);
        struct Qdisc *qdisc;
 -      int err, queue;
        struct sta_info *sta;
 +      int err, queue;
        u8 tid;
  
        if (pkt_data->flags & IEEE80211_TXPD_REQUEUE) {
                tid = skb->priority & QOS_CONTROL_TAG1D_MASK;
                if (sta) {
                        int ampdu_queue = sta->tid_to_tx_q[tid];
 -                      if ((ampdu_queue < local->hw.queues) &&
 +                      if ((ampdu_queue < QD_NUM(hw)) &&
                            test_bit(ampdu_queue, q->qdisc_pool)) {
                                queue = ampdu_queue;
                                pkt_data->flags |= IEEE80211_TXPD_AMPDU;
  
        queue = classify80211(skb, qd);
  
 +      if (unlikely(queue >= local->hw.queues))
 +              queue = local->hw.queues - 1;
 +
        /* now we know the 1d priority, fill in the QoS header if there is one
         */
        if (WLAN_FC_IS_QOS_DATA(fc)) {
                sta = sta_info_get(local, hdr->addr1);
                if (sta) {
                        int ampdu_queue = sta->tid_to_tx_q[tid];
 -                      if ((ampdu_queue < local->hw.queues) &&
 -                              test_bit(ampdu_queue, q->qdisc_pool)) {
 +                      if ((ampdu_queue < QD_NUM(hw)) &&
 +                          test_bit(ampdu_queue, q->qdisc_pool)) {
                                queue = ampdu_queue;
                                pkt_data->flags |= IEEE80211_TXPD_AMPDU;
                        } else {
                rcu_read_unlock();
        }
  
 -      if (unlikely(queue >= local->hw.queues)) {
 -#if 0
 -              if (net_ratelimit()) {
 -                      printk(KERN_DEBUG "%s - queue=%d (hw does not "
 -                             "support) -> %d\n",
 -                             __func__, queue, local->hw.queues - 1);
 -              }
 -#endif
 -              queue = local->hw.queues - 1;
 -      }
 -
        if (unlikely(queue < 0)) {
                        kfree_skb(skb);
                        err = NET_XMIT_DROP;
@@@ -269,7 -270,7 +269,7 @@@ static struct sk_buff *wme_qdiscop_dequ
        int queue;
  
        /* check all the h/w queues in numeric/priority order */
 -      for (queue = 0; queue < hw->queues; queue++) {
 +      for (queue = 0; queue < QD_NUM(hw); queue++) {
                /* see if there is room in this hardware queue */
                if ((test_bit(IEEE80211_LINK_STATE_XOFF,
                                &local->state[queue])) ||
@@@ -307,7 -308,7 +307,7 @@@ static void wme_qdiscop_reset(struct Qd
  
        /* QUESTION: should we have some hardware flush functionality here? */
  
 -      for (queue = 0; queue < hw->queues; queue++) {
 +      for (queue = 0; queue < QD_NUM(hw); queue++) {
                skb_queue_purge(&q->requeued[queue]);
                qdisc_reset(q->queues[queue]);
        }
@@@ -325,7 -326,7 +325,7 @@@ static void wme_qdiscop_destroy(struct 
        tcf_destroy_chain(q->filter_list);
        q->filter_list = NULL;
  
 -      for (queue=0; queue < hw->queues; queue++) {
 +      for (queue = 0; queue < QD_NUM(hw); queue++) {
                skb_queue_purge(&q->requeued[queue]);
                qdisc_destroy(q->queues[queue]);
                q->queues[queue] = &noop_qdisc;
  /* called whenever parameters are updated on existing qdisc */
  static int wme_qdiscop_tune(struct Qdisc *qd, struct nlattr *opt)
  {
 -/*    struct ieee80211_sched_data *q = qdisc_priv(qd);
 -*/
 -      /* check our options block is the right size */
 -      /* copy any options to our local structure */
 -/*    Ignore options block for now - always use static mapping
 -      struct tc_ieee80211_qopt *qopt = nla_data(opt);
 -
 -      if (opt->nla_len < nla_attr_size(sizeof(*qopt)))
 -              return -EINVAL;
 -      memcpy(q->tag2queue, qopt->tag2queue, sizeof(qopt->tag2queue));
 -*/
        return 0;
  }
  
@@@ -346,7 -358,7 +346,7 @@@ static int wme_qdiscop_init(struct Qdis
        struct ieee80211_sched_data *q = qdisc_priv(qd);
        struct net_device *dev = qd->dev;
        struct ieee80211_local *local;
 -      int queues;
 +      struct ieee80211_hw *hw;
        int err = 0, i;
  
        /* check that device is a mac80211 device */
            dev->ieee80211_ptr->wiphy->privid != mac80211_wiphy_privid)
                return -EINVAL;
  
 -      /* check this device is an ieee80211 master type device */
 -      if (dev->type != ARPHRD_IEEE80211)
 +      local = wdev_priv(dev->ieee80211_ptr);
 +      hw = &local->hw;
 +
 +      /* only allow on master dev */
 +      if (dev != local->mdev)
                return -EINVAL;
  
 -      /* check that there is no qdisc currently attached to device
 -       * this ensures that we will be the root qdisc. (I can't find a better
 -       * way to test this explicitly) */
 -      if (dev->qdisc_sleeping != &noop_qdisc)
 +      /* ensure that we are root qdisc */
 +      if (qd->parent != TC_H_ROOT)
                return -EINVAL;
  
        if (qd->flags & TCQ_F_INGRESS)
                return -EINVAL;
  
 -      local = wdev_priv(dev->ieee80211_ptr);
 -      queues = local->hw.queues;
 -
        /* if options were passed in, set them */
 -      if (opt) {
 +      if (opt)
                err = wme_qdiscop_tune(qd, opt);
 -      }
  
        /* create child queues */
 -      for (i = 0; i < queues; i++) {
 +      for (i = 0; i < QD_NUM(hw); i++) {
                skb_queue_head_init(&q->requeued[i]);
                q->queues[i] = qdisc_create_dflt(qd->dev, &pfifo_qdisc_ops,
                                                 qd->handle);
                if (!q->queues[i]) {
                        q->queues[i] = &noop_qdisc;
-                       printk(KERN_ERR "%s child qdisc %i creation failed", dev->name, i);
+                       printk(KERN_ERR "%s child qdisc %i creation failed\n",
+                              dev->name, i);
                }
        }
  
 -      /* reserve all legacy QoS queues */
 -      for (i = 0; i < min(IEEE80211_TX_QUEUE_DATA4, queues); i++)
 +      /* non-aggregation queues: reserve/mark as used */
 +      for (i = 0; i < local->hw.queues; i++)
                set_bit(i, q->qdisc_pool);
  
        return err;
  
  static int wme_qdiscop_dump(struct Qdisc *qd, struct sk_buff *skb)
  {
 -/*    struct ieee80211_sched_data *q = qdisc_priv(qd);
 -      unsigned char *p = skb->tail;
 -      struct tc_ieee80211_qopt opt;
 -
 -      memcpy(&opt.tag2queue, q->tag2queue, TC_80211_MAX_TAG + 1);
 -      NLA_PUT(skb, TCA_OPTIONS, sizeof(opt), &opt);
 -*/    return skb->len;
 -/*
 -nla_put_failure:
 -      skb_trim(skb, p - skb->data);*/
        return -1;
  }
  
@@@ -404,7 -430,7 +405,7 @@@ static int wme_classop_graft(struct Qdi
        struct ieee80211_hw *hw = &local->hw;
        unsigned long queue = arg - 1;
  
 -      if (queue >= hw->queues)
 +      if (queue >= QD_NUM(hw))
                return -EINVAL;
  
        if (!new)
@@@ -428,7 -454,7 +429,7 @@@ wme_classop_leaf(struct Qdisc *qd, unsi
        struct ieee80211_hw *hw = &local->hw;
        unsigned long queue = arg - 1;
  
 -      if (queue >= hw->queues)
 +      if (queue >= QD_NUM(hw))
                return NULL;
  
        return q->queues[queue];
@@@ -441,7 -467,7 +442,7 @@@ static unsigned long wme_classop_get(st
        struct ieee80211_hw *hw = &local->hw;
        unsigned long queue = TC_H_MIN(classid);
  
 -      if (queue - 1 >= hw->queues)
 +      if (queue - 1 >= QD_NUM(hw))
                return 0;
  
        return queue;
@@@ -467,7 -493,7 +468,7 @@@ static int wme_classop_change(struct Qd
        struct ieee80211_local *local = wdev_priv(qd->dev->ieee80211_ptr);
        struct ieee80211_hw *hw = &local->hw;
  
 -      if (cl - 1 > hw->queues)
 +      if (cl - 1 > QD_NUM(hw))
                return -ENOENT;
  
        /* TODO: put code to program hardware queue parameters here,
@@@ -484,7 -510,7 +485,7 @@@ static int wme_classop_delete(struct Qd
        struct ieee80211_local *local = wdev_priv(qd->dev->ieee80211_ptr);
        struct ieee80211_hw *hw = &local->hw;
  
 -      if (cl - 1 > hw->queues)
 +      if (cl - 1 > QD_NUM(hw))
                return -ENOENT;
        return 0;
  }
@@@ -497,7 -523,7 +498,7 @@@ static int wme_classop_dump_class(struc
        struct ieee80211_local *local = wdev_priv(qd->dev->ieee80211_ptr);
        struct ieee80211_hw *hw = &local->hw;
  
 -      if (cl - 1 > hw->queues)
 +      if (cl - 1 > QD_NUM(hw))
                return -ENOENT;
        tcm->tcm_handle = TC_H_MIN(cl);
        tcm->tcm_parent = qd->handle;
@@@ -515,7 -541,7 +516,7 @@@ static void wme_classop_walk(struct Qdi
        if (arg->stop)
                return;
  
 -      for (queue = 0; queue < hw->queues; queue++) {
 +      for (queue = 0; queue < QD_NUM(hw); queue++) {
                if (arg->count < arg->skip) {
                        arg->count++;
                        continue;
@@@ -632,13 -658,10 +633,13 @@@ int ieee80211_ht_agg_queue_add(struct i
        DECLARE_MAC_BUF(mac);
  
        /* prepare the filter and save it for the SW queue
 -       * matching the recieved HW queue */
 +       * matching the received HW queue */
 +
 +      if (!local->hw.ampdu_queues)
 +              return -EPERM;
  
        /* try to get a Qdisc from the pool */
 -      for (i = IEEE80211_TX_QUEUE_BEACON; i < local->hw.queues; i++)
 +      for (i = local->hw.queues; i < QD_NUM(&local->hw); i++)
                if (!test_and_set_bit(i, q->qdisc_pool)) {
                        ieee80211_stop_queue(local_to_hw(local), i);
                        sta->tid_to_tx_q[tid] = i;
@@@ -667,14 -690,13 +668,14 @@@ void ieee80211_ht_agg_queue_remove(stru
                                   struct sta_info *sta, u16 tid,
                                   u8 requeue)
  {
 +      struct ieee80211_hw *hw = &local->hw;
        struct ieee80211_sched_data *q =
                qdisc_priv(local->mdev->qdisc_sleeping);
        int agg_queue = sta->tid_to_tx_q[tid];
  
        /* return the qdisc to the pool */
        clear_bit(agg_queue, q->qdisc_pool);
 -      sta->tid_to_tx_q[tid] = local->hw.queues;
 +      sta->tid_to_tx_q[tid] = QD_NUM(hw);
  
        if (requeue)
                ieee80211_requeue(local, agg_queue);
diff --combined net/tipc/core.h
@@@ -2,7 -2,7 +2,7 @@@
   * net/tipc/core.h: Include file for TIPC global declarations
   *
   * Copyright (c) 2005-2006, Ericsson AB
 - * Copyright (c) 2005-2006, Wind River Systems
 + * Copyright (c) 2005-2007, Wind River Systems
   * All rights reserved.
   *
   * Redistribution and use in source and binary forms, with or without
  #include <linux/vmalloc.h>
  
  /*
 - * TIPC debugging code
 + * TIPC sanity test macros
   */
  
  #define assert(i)  BUG_ON(!(i))
  
 -struct tipc_msg;
 -extern struct print_buf *TIPC_NULL, *TIPC_CONS, *TIPC_LOG;
 -extern struct print_buf *TIPC_TEE(struct print_buf *, struct print_buf *);
 -void tipc_msg_print(struct print_buf*,struct tipc_msg *,const char*);
 -void tipc_printf(struct print_buf *, const char *fmt, ...);
 -void tipc_dump(struct print_buf*,const char *fmt, ...);
 -
 -#ifdef CONFIG_TIPC_DEBUG
 -
  /*
 - * TIPC debug support included:
 - * - system messages are printed to TIPC_OUTPUT print buffer
 - * - debug messages are printed to DBG_OUTPUT print buffer
 + * TIPC system monitoring code
   */
  
 -#define err(fmt, arg...)  tipc_printf(TIPC_OUTPUT, KERN_ERR "TIPC: " fmt, ## arg)
 -#define warn(fmt, arg...) tipc_printf(TIPC_OUTPUT, KERN_WARNING "TIPC: " fmt, ## arg)
 -#define info(fmt, arg...) tipc_printf(TIPC_OUTPUT, KERN_NOTICE "TIPC: " fmt, ## arg)
 +/*
 + * TIPC's print buffer subsystem supports the following print buffers:
 + *
 + * TIPC_NULL : null buffer (i.e. print nowhere)
 + * TIPC_CONS : system console
 + * TIPC_LOG  : TIPC log buffer
 + * &buf            : user-defined buffer (struct print_buf *)
 + *
 + * Note: TIPC_LOG is configured to echo its output to the system console;
 + *       user-defined buffers can be configured to do the same thing.
 + */
  
 -#define dbg(fmt, arg...)  do {if (DBG_OUTPUT != TIPC_NULL) tipc_printf(DBG_OUTPUT, fmt, ## arg);} while(0)
 -#define msg_dbg(msg, txt) do {if (DBG_OUTPUT != TIPC_NULL) tipc_msg_print(DBG_OUTPUT, msg, txt);} while(0)
 -#define dump(fmt, arg...) do {if (DBG_OUTPUT != TIPC_NULL) tipc_dump(DBG_OUTPUT, fmt, ##arg);} while(0)
 +extern struct print_buf *const TIPC_NULL;
 +extern struct print_buf *const TIPC_CONS;
 +extern struct print_buf *const TIPC_LOG;
  
 +void tipc_printf(struct print_buf *, const char *fmt, ...);
  
  /*
 - * By default, TIPC_OUTPUT is defined to be system console and TIPC log buffer,
 - * while DBG_OUTPUT is the null print buffer.  These defaults can be changed
 - * here, or on a per .c file basis, by redefining these symbols.  The following
 - * print buffer options are available:
 - *
 - * TIPC_NULL             : null buffer (i.e. print nowhere)
 - * TIPC_CONS             : system console
 - * TIPC_LOG              : TIPC log buffer
 - * &buf                          : user-defined buffer (struct print_buf *)
 - * TIPC_TEE(&buf_a,&buf_b) : list of buffers (eg. TIPC_TEE(TIPC_CONS,TIPC_LOG))
 + * TIPC_OUTPUT is the destination print buffer for system messages.
   */
  
  #ifndef TIPC_OUTPUT
 -#define TIPC_OUTPUT TIPC_TEE(TIPC_CONS,TIPC_LOG)
 -#endif
 -
 -#ifndef DBG_OUTPUT
 -#define DBG_OUTPUT TIPC_NULL
 +#define TIPC_OUTPUT TIPC_LOG
  #endif
  
 -#else
 -
  /*
 - * TIPC debug support not included:
 - * - system messages are printed to system console
 - * - debug messages are not printed
 + * TIPC can be configured to send system messages to TIPC_OUTPUT
 + * or to the system console only.
   */
  
 +#ifdef CONFIG_TIPC_DEBUG
 +
 +#define err(fmt, arg...)  tipc_printf(TIPC_OUTPUT, \
 +                                      KERN_ERR "TIPC: " fmt, ## arg)
 +#define warn(fmt, arg...) tipc_printf(TIPC_OUTPUT, \
 +                                      KERN_WARNING "TIPC: " fmt, ## arg)
 +#define info(fmt, arg...) tipc_printf(TIPC_OUTPUT, \
 +                                      KERN_NOTICE "TIPC: " fmt, ## arg)
 +
 +#else
 +
  #define err(fmt, arg...)  printk(KERN_ERR "TIPC: " fmt , ## arg)
  #define info(fmt, arg...) printk(KERN_INFO "TIPC: " fmt , ## arg)
  #define warn(fmt, arg...) printk(KERN_WARNING "TIPC: " fmt , ## arg)
  
 -#define dbg(fmt, arg...) do {} while (0)
 -#define msg_dbg(msg,txt) do {} while (0)
 -#define dump(fmt,arg...) do {} while (0)
 +#endif
  
 +/*
 + * DBG_OUTPUT is the destination print buffer for debug messages.
 + * It defaults to the the null print buffer, but can be redefined
 + * (typically in the individual .c files being debugged) to allow
 + * selected debug messages to be generated where needed.
 + */
 +
 +#ifndef DBG_OUTPUT
 +#define DBG_OUTPUT TIPC_NULL
 +#endif
  
  /*
 - * TIPC_OUTPUT is defined to be the system console, while DBG_OUTPUT is
 - * the null print buffer.  Thes ensures that any system or debug messages
 - * that are generated without using the above macros are handled correctly.
 + * TIPC can be configured to send debug messages to the specified print buffer
 + * (typically DBG_OUTPUT) or to suppress them entirely.
   */
  
 -#undef  TIPC_OUTPUT
 -#define TIPC_OUTPUT TIPC_CONS
 +#ifdef CONFIG_TIPC_DEBUG
  
 -#undef  DBG_OUTPUT
 -#define DBG_OUTPUT TIPC_NULL
 +#define dbg(fmt, arg...)  \
 +      do { \
 +              if (DBG_OUTPUT != TIPC_NULL) \
 +                      tipc_printf(DBG_OUTPUT, fmt, ## arg); \
 +      } while (0)
 +#define msg_dbg(msg, txt) \
 +      do { \
 +              if (DBG_OUTPUT != TIPC_NULL) \
 +                      tipc_msg_dbg(DBG_OUTPUT, msg, txt); \
 +      } while (0)
 +#define dump(fmt, arg...) \
 +      do { \
 +              if (DBG_OUTPUT != TIPC_NULL) \
 +                      tipc_dump_dbg(DBG_OUTPUT, fmt, ##arg); \
 +      } while (0)
 +
 +void tipc_msg_dbg(struct print_buf *, struct tipc_msg *, const char *);
 +void tipc_dump_dbg(struct print_buf *, const char *fmt, ...);
 +
 +#else
 +
 +#define dbg(fmt, arg...)      do {} while (0)
 +#define msg_dbg(msg, txt)     do {} while (0)
 +#define dump(fmt, arg...)     do {} while (0)
 +
 +#define tipc_msg_dbg(...)     do {} while (0)
 +#define tipc_dump_dbg(...)    do {} while (0)
  
  #endif
  
@@@ -303,15 -279,14 +303,14 @@@ static inline void k_term_timer(struct 
  /*
   * TIPC message buffer code
   *
-  * TIPC message buffer headroom reserves space for a link-level header
-  * (in case the message is sent off-node),
-  * while ensuring TIPC header is word aligned for quicker access
+  * TIPC message buffer headroom reserves space for the worst-case
+  * link-level device header (in case the message is sent off-node).
   *
-  * The largest header currently supported is 18 bytes, which is used when
-  * the standard 14 byte Ethernet header has 4 added bytes for VLAN info
+  * Note: Headroom should be a multiple of 4 to ensure the TIPC header fields
+  *       are word aligned for quicker access
   */
  
- #define BUF_HEADROOM 20u
+ #define BUF_HEADROOM LL_MAX_HEADER
  
  struct tipc_skb_cb {
        void *handle;