/******************************************************************************
 *
 * Copyright(c) 2003 - 2008 Intel Corporation. All rights reserved.
 *
 * Portions of this file are derived from the ipw3945 project, as well
 * as portions of the ieee80211 subsystem header files.
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms of version 2 of the GNU General Public License as
 * published by the Free Software Foundation.
 *
 * This program is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
 * more details.
 *
 * You should have received a copy of the GNU General Public License along with
 * this program; if not, write to the Free Software Foundation, Inc.,
 * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
 *
 * The full GNU General Public License is included in this distribution in the
 * file called LICENSE.
 *
 * Contact Information:
 * James P. Ketrenos <ipw2100-admin@linux.intel.com>
 * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
 *
 *****************************************************************************/

#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/init.h>
#include <linux/pci.h>
#include <linux/dma-mapping.h>
#include <linux/delay.h>
#include <linux/skbuff.h>
#include <linux/netdevice.h>
#include <linux/wireless.h>
#include <linux/firmware.h>
#include <linux/etherdevice.h>
#include <linux/if_arp.h>

#include <net/ieee80211_radiotap.h>
#include <net/mac80211.h>

#include <asm/div64.h>

#include "iwl-3945-core.h"
#include "iwl-3945.h"
#include "iwl-helpers.h"

#ifdef CONFIG_IWL3945_DEBUG
u32 iwl3945_debug_level;
#endif

static int iwl3945_tx_queue_update_write_ptr(struct iwl3945_priv *priv,
                                  struct iwl3945_tx_queue *txq);

/******************************************************************************
 *
 * module boiler plate
 *
 ******************************************************************************/

/* module parameters */
static int iwl3945_param_disable_hw_scan; /* def: 0 = use 3945's h/w scan */
static int iwl3945_param_debug;    /* def: 0 = minimal debug log messages */
static int iwl3945_param_disable;  /* def: 0 = enable radio */
static int iwl3945_param_antenna;  /* def: 0 = both antennas (use diversity) */
int iwl3945_param_hwcrypto;        /* def: 0 = use software encryption */
static int iwl3945_param_qos_enable = 1; /* def: 1 = use quality of service */
int iwl3945_param_queues_num = IWL39_MAX_NUM_QUEUES; /* def: 8 Tx queues */

/*
 * module name, copyright, version, etc.
 * NOTE: DRV_NAME is defined in iwlwifi.h for use by iwl-debug.h and printk
 */

#define DRV_DESCRIPTION \
"Intel(R) PRO/Wireless 3945ABG/BG Network Connection driver for Linux"

#ifdef CONFIG_IWL3945_DEBUG
#define VD "d"
#else
#define VD
#endif

#ifdef CONFIG_IWL3945_SPECTRUM_MEASUREMENT
#define VS "s"
#else
#define VS
#endif

#define IWLWIFI_VERSION "1.2.26k" VD VS
#define DRV_COPYRIGHT   "Copyright(c) 2003-2008 Intel Corporation"
#define DRV_VERSION     IWLWIFI_VERSION


MODULE_DESCRIPTION(DRV_DESCRIPTION);
MODULE_VERSION(DRV_VERSION);
MODULE_AUTHOR(DRV_COPYRIGHT);
MODULE_LICENSE("GPL");

static const struct ieee80211_supported_band *iwl3945_get_band(
                struct iwl3945_priv *priv, enum ieee80211_band band)
{
        return priv->hw->wiphy->bands[band];
}

static int iwl3945_is_empty_essid(const char *essid, int essid_len)
{
        /* Single white space is for Linksys APs */
        if (essid_len == 1 && essid[0] == ' ')
                return 1;

        /* Otherwise, if the entire essid is 0, we assume it is hidden */
        while (essid_len) {
                essid_len--;
                if (essid[essid_len] != '\0')
                        return 0;
        }

        return 1;
}

static const char *iwl3945_escape_essid(const char *essid, u8 essid_len)
{
        static char escaped[IW_ESSID_MAX_SIZE * 2 + 1];
        const char *s = essid;
        char *d = escaped;

        if (iwl3945_is_empty_essid(essid, essid_len)) {
                memcpy(escaped, "<hidden>", sizeof("<hidden>"));
                return escaped;
        }

        essid_len = min(essid_len, (u8) IW_ESSID_MAX_SIZE);
        while (essid_len--) {
                if (*s == '\0') {
                        *d++ = '\\';
                        *d++ = '0';
                        s++;
                } else
                        *d++ = *s++;
        }
        *d = '\0';
        return escaped;
}

/*************** DMA-QUEUE-GENERAL-FUNCTIONS  *****
 * DMA services
 *
 * Theory of operation
 *
 * A Tx or Rx queue resides in host DRAM, and is comprised of a circular buffer
 * of buffer descriptors, each of which points to one or more data buffers for
 * the device to read from or fill.  Driver and device exchange status of each
 * queue via "read" and "write" pointers.  Driver keeps minimum of 2 empty
 * entries in each circular buffer, to protect against confusing empty and full
 * queue states.
 *
 * The device reads or writes the data in the queues via the device's several
 * DMA/FIFO channels.  Each queue is mapped to a single DMA channel.
 *
 * For Tx queue, there are low mark and high mark limits. If, after queuing
 * the packet for Tx, free space become < low mark, Tx queue stopped. When
 * reclaiming packets (on 'tx done IRQ), if free space become > high mark,
 * Tx queue resumed.
 *
 * The 3945 operates with six queues:  One receive queue, one transmit queue
 * (#4) for sending commands to the device firmware, and four transmit queues
 * (#0-3) for data tx via EDCA.  An additional 2 HCCA queues are unused.
 ***************************************************/

int iwl3945_queue_space(const struct iwl3945_queue *q)
{
        int s = q->read_ptr - q->write_ptr;

        if (q->read_ptr > q->write_ptr)
                s -= q->n_bd;

        if (s <= 0)
                s += q->n_window;
        /* keep some reserve to not confuse empty and full situations */
        s -= 2;
        if (s < 0)
                s = 0;
        return s;
}

int iwl3945_x2_queue_used(const struct iwl3945_queue *q, int i)
{
        return q->write_ptr > q->read_ptr ?
                (i >= q->read_ptr && i < q->write_ptr) :
                !(i < q->read_ptr && i >= q->write_ptr);
}


static inline u8 get_cmd_index(struct iwl3945_queue *q, u32 index, int is_huge)
{
        /* This is for scan command, the big buffer at end of command array */
        if (is_huge)
                return q->n_window;     /* must be power of 2 */

        /* Otherwise, use normal size buffers */
        return index & (q->n_window - 1);
}

/**
 * iwl3945_queue_init - Initialize queue's high/low-water and read/write indexes
 */
static int iwl3945_queue_init(struct iwl3945_priv *priv, struct iwl3945_queue *q,
                          int count, int slots_num, u32 id)
{
        q->n_bd = count;
        q->n_window = slots_num;
        q->id = id;

        /* count must be power-of-two size, otherwise iwl_queue_inc_wrap
         * and iwl_queue_dec_wrap are broken. */
        BUG_ON(!is_power_of_2(count));

        /* slots_num must be power-of-two size, otherwise
         * get_cmd_index is broken. */
        BUG_ON(!is_power_of_2(slots_num));

        q->low_mark = q->n_window / 4;
        if (q->low_mark < 4)
                q->low_mark = 4;

        q->high_mark = q->n_window / 8;
        if (q->high_mark < 2)
                q->high_mark = 2;

        q->write_ptr = q->read_ptr = 0;

        return 0;
}

/**
 * iwl3945_tx_queue_alloc - Alloc driver data and TFD CB for one Tx/cmd queue
 */
static int iwl3945_tx_queue_alloc(struct iwl3945_priv *priv,
                              struct iwl3945_tx_queue *txq, u32 id)
{
        struct pci_dev *dev = priv->pci_dev;

        /* Driver private data, only for Tx (not command) queues,
         * not shared with device. */
        if (id != IWL_CMD_QUEUE_NUM) {
                txq->txb = kmalloc(sizeof(txq->txb[0]) *
                                   TFD_QUEUE_SIZE_MAX, GFP_KERNEL);
                if (!txq->txb) {
                        IWL_ERROR("kmalloc for auxiliary BD "
                                  "structures failed\n");
                        goto error;
                }
        } else
                txq->txb = NULL;

        /* Circular buffer of transmit frame descriptors (TFDs),
         * shared with device */
        txq->bd = pci_alloc_consistent(dev,
                        sizeof(txq->bd[0]) * TFD_QUEUE_SIZE_MAX,
                        &txq->q.dma_addr);

        if (!txq->bd) {
                IWL_ERROR("pci_alloc_consistent(%zd) failed\n",
                          sizeof(txq->bd[0]) * TFD_QUEUE_SIZE_MAX);
                goto error;
        }
        txq->q.id = id;

        return 0;

 error:
        kfree(txq->txb);
        txq->txb = NULL;

        return -ENOMEM;
}

/**
 * iwl3945_tx_queue_init - Allocate and initialize one tx/cmd queue
 */
int iwl3945_tx_queue_init(struct iwl3945_priv *priv,
                      struct iwl3945_tx_queue *txq, int slots_num, u32 txq_id)
{
        struct pci_dev *dev = priv->pci_dev;
        int len;
        int rc = 0;

        /*
         * Alloc buffer array for commands (Tx or other types of commands).
         * For the command queue (#4), allocate command space + one big
         * command for scan, since scan command is very huge; the system will
         * not have two scans at the same time, so only one is needed.
         * For data Tx queues (all other queues), no super-size command
         * space is needed.
         */
        len = sizeof(struct iwl3945_cmd) * slots_num;
        if (txq_id == IWL_CMD_QUEUE_NUM)
                len +=  IWL_MAX_SCAN_SIZE;
        txq->cmd = pci_alloc_consistent(dev, len, &txq->dma_addr_cmd);
        if (!txq->cmd)
                return -ENOMEM;

        /* Alloc driver data array and TFD circular buffer */
        rc = iwl3945_tx_queue_alloc(priv, txq, txq_id);
        if (rc) {
                pci_free_consistent(dev, len, txq->cmd, txq->dma_addr_cmd);

                return -ENOMEM;
        }
        txq->need_update = 0;

        /* TFD_QUEUE_SIZE_MAX must be power-of-two size, otherwise
         * iwl_queue_inc_wrap and iwl_queue_dec_wrap are broken. */
        BUILD_BUG_ON(TFD_QUEUE_SIZE_MAX & (TFD_QUEUE_SIZE_MAX - 1));

        /* Initialize queue high/low-water, head/tail indexes */
        iwl3945_queue_init(priv, &txq->q, TFD_QUEUE_SIZE_MAX, slots_num, txq_id);

        /* Tell device where to find queue, enable DMA channel. */
        iwl3945_hw_tx_queue_init(priv, txq);

        return 0;
}

/**
 * iwl3945_tx_queue_free - Deallocate DMA queue.
 * @txq: Transmit queue to deallocate.
 *
 * Empty queue by removing and destroying all BD's.
 * Free all buffers.
 * 0-fill, but do not free "txq" descriptor structure.
 */
void iwl3945_tx_queue_free(struct iwl3945_priv *priv, struct iwl3945_tx_queue *txq)
{
        struct iwl3945_queue *q = &txq->q;
        struct pci_dev *dev = priv->pci_dev;
        int len;

        if (q->n_bd == 0)
                return;

        /* first, empty all BD's */
        for (; q->write_ptr != q->read_ptr;
             q->read_ptr = iwl_queue_inc_wrap(q->read_ptr, q->n_bd))
                iwl3945_hw_txq_free_tfd(priv, txq);

        len = sizeof(struct iwl3945_cmd) * q->n_window;
        if (q->id == IWL_CMD_QUEUE_NUM)
                len += IWL_MAX_SCAN_SIZE;

        /* De-alloc array of command/tx buffers */
        pci_free_consistent(dev, len, txq->cmd, txq->dma_addr_cmd);

        /* De-alloc circular buffer of TFDs */
        if (txq->q.n_bd)
                pci_free_consistent(dev, sizeof(struct iwl3945_tfd_frame) *
                                    txq->q.n_bd, txq->bd, txq->q.dma_addr);

        /* De-alloc array of per-TFD driver data */
        kfree(txq->txb);
        txq->txb = NULL;

        /* 0-fill queue descriptor structure */
        memset(txq, 0, sizeof(*txq));
}

const u8 iwl3945_broadcast_addr[ETH_ALEN] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };

/*************** STATION TABLE MANAGEMENT ****
 * mac80211 should be examined to determine if sta_info is duplicating
 * the functionality provided here
 */

/**************************************************************/
#if 0 /* temporary disable till we add real remove station */
/**
 * iwl3945_remove_station - Remove driver's knowledge of station.
 *
 * NOTE:  This does not remove station from device's station table.
 */
static u8 iwl3945_remove_station(struct iwl3945_priv *priv, const u8 *addr, int is_ap)
{
        int index = IWL_INVALID_STATION;
        int i;
        unsigned long flags;

        spin_lock_irqsave(&priv->sta_lock, flags);

        if (is_ap)
                index = IWL_AP_ID;
        else if (is_broadcast_ether_addr(addr))
                index = priv->hw_setting.bcast_sta_id;
        else
                for (i = IWL_STA_ID; i < priv->hw_setting.max_stations; i++)
                        if (priv->stations[i].used &&
                            !compare_ether_addr(priv->stations[i].sta.sta.addr,
                                                addr)) {
                                index = i;
                                break;
                        }

        if (unlikely(index == IWL_INVALID_STATION))
                goto out;

        if (priv->stations[index].used) {
                priv->stations[index].used = 0;
                priv->num_stations--;
        }

        BUG_ON(priv->num_stations < 0);

out:
        spin_unlock_irqrestore(&priv->sta_lock, flags);
        return 0;
}
#endif

/**
 * iwl3945_clear_stations_table - Clear the driver's station table
 *
 * NOTE:  This does not clear or otherwise alter the device's station table.
 */
static void iwl3945_clear_stations_table(struct iwl3945_priv *priv)
{
        unsigned long flags;

        spin_lock_irqsave(&priv->sta_lock, flags);

        priv->num_stations = 0;
        memset(priv->stations, 0, sizeof(priv->stations));

        spin_unlock_irqrestore(&priv->sta_lock, flags);
}

/**
 * iwl3945_add_station - Add station to station tables in driver and device
 */
u8 iwl3945_add_station(struct iwl3945_priv *priv, const u8 *addr, int is_ap, u8 flags)
{
        int i;
        int index = IWL_INVALID_STATION;
        struct iwl3945_station_entry *station;
        unsigned long flags_spin;
        DECLARE_MAC_BUF(mac);
        u8 rate;

        spin_lock_irqsave(&priv->sta_lock, flags_spin);
        if (is_ap)
                index = IWL_AP_ID;
        else if (is_broadcast_ether_addr(addr))
                index = priv->hw_setting.bcast_sta_id;
        else
                for (i = IWL_STA_ID; i < priv->hw_setting.max_stations; i++) {
                        if (!compare_ether_addr(priv->stations[i].sta.sta.addr,
                                                addr)) {
                                index = i;
                                break;
                        }

                        if (!priv->stations[i].used &&
                            index == IWL_INVALID_STATION)
                                index = i;
                }

        /* These two conditions has the same outcome but keep them separate
          since they have different meaning */
        if (unlikely(index == IWL_INVALID_STATION)) {
                spin_unlock_irqrestore(&priv->sta_lock, flags_spin);
                return index;
        }

        if (priv->stations[index].used &&
           !compare_ether_addr(priv->stations[index].sta.sta.addr, addr)) {
                spin_unlock_irqrestore(&priv->sta_lock, flags_spin);
                return index;
        }

        IWL_DEBUG_ASSOC("Add STA ID %d: %s\n", index, print_mac(mac, addr));
        station = &priv->stations[index];
        station->used = 1;
        priv->num_stations++;

        /* Set up the REPLY_ADD_STA command to send to device */
        memset(&station->sta, 0, sizeof(struct iwl3945_addsta_cmd));
        memcpy(station->sta.sta.addr, addr, ETH_ALEN);
        station->sta.mode = 0;
        station->sta.sta.sta_id = index;
        station->sta.station_flags = 0;

        if (priv->band == IEEE80211_BAND_5GHZ)
                rate = IWL_RATE_6M_PLCP;
        else
                rate =  IWL_RATE_1M_PLCP;

        /* Turn on both antennas for the station... */
        station->sta.rate_n_flags =
                        iwl3945_hw_set_rate_n_flags(rate, RATE_MCS_ANT_AB_MSK);
        station->current_rate.rate_n_flags =
                        le16_to_cpu(station->sta.rate_n_flags);

        spin_unlock_irqrestore(&priv->sta_lock, flags_spin);

        /* Add station to device's station table */
        iwl3945_send_add_station(priv, &station->sta, flags);
        return index;

}

/*************** DRIVER STATUS FUNCTIONS   *****/

static inline int iwl3945_is_ready(struct iwl3945_priv *priv)
{
        /* The adapter is 'ready' if READY and GEO_CONFIGURED bits are
         * set but EXIT_PENDING is not */
        return test_bit(STATUS_READY, &priv->status) &&
               test_bit(STATUS_GEO_CONFIGURED, &priv->status) &&
               !test_bit(STATUS_EXIT_PENDING, &priv->status);
}

static inline int iwl3945_is_alive(struct iwl3945_priv *priv)
{
        return test_bit(STATUS_ALIVE, &priv->status);
}

static inline int iwl3945_is_init(struct iwl3945_priv *priv)
{
        return test_bit(STATUS_INIT, &priv->status);
}

static inline int iwl3945_is_rfkill_sw(struct iwl3945_priv *priv)
{
        return test_bit(STATUS_RF_KILL_SW, &priv->status);
}

static inline int iwl3945_is_rfkill_hw(struct iwl3945_priv *priv)
{
        return test_bit(STATUS_RF_KILL_HW, &priv->status);
}

static inline int iwl3945_is_rfkill(struct iwl3945_priv *priv)
{
        return iwl3945_is_rfkill_hw(priv) ||
                iwl3945_is_rfkill_sw(priv);
}

static inline int iwl3945_is_ready_rf(struct iwl3945_priv *priv)
{

        if (iwl3945_is_rfkill(priv))
                return 0;

        return iwl3945_is_ready(priv);
}

/*************** HOST COMMAND QUEUE FUNCTIONS   *****/

#define IWL_CMD(x) case x : return #x

static const char *get_cmd_string(u8 cmd)
{
        switch (cmd) {
                IWL_CMD(REPLY_ALIVE);
                IWL_CMD(REPLY_ERROR);
                IWL_CMD(REPLY_RXON);
                IWL_CMD(REPLY_RXON_ASSOC);
                IWL_CMD(REPLY_QOS_PARAM);
                IWL_CMD(REPLY_RXON_TIMING);
                IWL_CMD(REPLY_ADD_STA);
                IWL_CMD(REPLY_REMOVE_STA);
                IWL_CMD(REPLY_REMOVE_ALL_STA);
                IWL_CMD(REPLY_3945_RX);
                IWL_CMD(REPLY_TX);
                IWL_CMD(REPLY_RATE_SCALE);
                IWL_CMD(REPLY_LEDS_CMD);
                IWL_CMD(REPLY_TX_LINK_QUALITY_CMD);
                IWL_CMD(RADAR_NOTIFICATION);
                IWL_CMD(REPLY_QUIET_CMD);
                IWL_CMD(REPLY_CHANNEL_SWITCH);
                IWL_CMD(CHANNEL_SWITCH_NOTIFICATION);
                IWL_CMD(REPLY_SPECTRUM_MEASUREMENT_CMD);
                IWL_CMD(SPECTRUM_MEASURE_NOTIFICATION);
                IWL_CMD(POWER_TABLE_CMD);
                IWL_CMD(PM_SLEEP_NOTIFICATION);
                IWL_CMD(PM_DEBUG_STATISTIC_NOTIFIC);
                IWL_CMD(REPLY_SCAN_CMD);
                IWL_CMD(REPLY_SCAN_ABORT_CMD);
                IWL_CMD(SCAN_START_NOTIFICATION);
                IWL_CMD(SCAN_RESULTS_NOTIFICATION);
                IWL_CMD(SCAN_COMPLETE_NOTIFICATION);
                IWL_CMD(BEACON_NOTIFICATION);
                IWL_CMD(REPLY_TX_BEACON);
                IWL_CMD(WHO_IS_AWAKE_NOTIFICATION);
                IWL_CMD(QUIET_NOTIFICATION);
                IWL_CMD(REPLY_TX_PWR_TABLE_CMD);
                IWL_CMD(MEASURE_ABORT_NOTIFICATION);
                IWL_CMD(REPLY_BT_CONFIG);
                IWL_CMD(REPLY_STATISTICS_CMD);
                IWL_CMD(STATISTICS_NOTIFICATION);
                IWL_CMD(REPLY_CARD_STATE_CMD);
                IWL_CMD(CARD_STATE_NOTIFICATION);
                IWL_CMD(MISSED_BEACONS_NOTIFICATION);
        default:
                return "UNKNOWN";

        }
}

#define HOST_COMPLETE_TIMEOUT (HZ / 2)

/**
 * iwl3945_enqueue_hcmd - enqueue a uCode command
 * @priv: device private data point
 * @cmd: a point to the ucode command structure
 *
 * The function returns < 0 values to indicate the operation is
 * failed. On success, it turns the index (> 0) of command in the
 * command queue.
 */
static int iwl3945_enqueue_hcmd(struct iwl3945_priv *priv, struct iwl3945_host_cmd *cmd)
{
        struct iwl3945_tx_queue *txq = &priv->txq[IWL_CMD_QUEUE_NUM];
        struct iwl3945_queue *q = &txq->q;
        struct iwl3945_tfd_frame *tfd;
        u32 *control_flags;
        struct iwl3945_cmd *out_cmd;
        u32 idx;
        u16 fix_size = (u16)(cmd->len + sizeof(out_cmd->hdr));
        dma_addr_t phys_addr;
        int pad;
        u16 count;
        int ret;
        unsigned long flags;

        /* If any of the command structures end up being larger than
         * the TFD_MAX_PAYLOAD_SIZE, and it sent as a 'small' command then
         * we will need to increase the size of the TFD entries */
        BUG_ON((fix_size > TFD_MAX_PAYLOAD_SIZE) &&
               !(cmd->meta.flags & CMD_SIZE_HUGE));


        if (iwl3945_is_rfkill(priv)) {
                IWL_DEBUG_INFO("Not sending command - RF KILL");
                return -EIO;
        }

        if (iwl3945_queue_space(q) < ((cmd->meta.flags & CMD_ASYNC) ? 2 : 1)) {
                IWL_ERROR("No space for Tx\n");
                return -ENOSPC;
        }

        spin_lock_irqsave(&priv->hcmd_lock, flags);

        tfd = &txq->bd[q->write_ptr];
        memset(tfd, 0, sizeof(*tfd));

        control_flags = (u32 *) tfd;

        idx = get_cmd_index(q, q->write_ptr, cmd->meta.flags & CMD_SIZE_HUGE);
        out_cmd = &txq->cmd[idx];

        out_cmd->hdr.cmd = cmd->id;
        memcpy(&out_cmd->meta, &cmd->meta, sizeof(cmd->meta));
        memcpy(&out_cmd->cmd.payload, cmd->data, cmd->len);

        /* At this point, the out_cmd now has all of the incoming cmd
         * information */

        out_cmd->hdr.flags = 0;
        out_cmd->hdr.sequence = cpu_to_le16(QUEUE_TO_SEQ(IWL_CMD_QUEUE_NUM) |
                        INDEX_TO_SEQ(q->write_ptr));
        if (out_cmd->meta.flags & CMD_SIZE_HUGE)
                out_cmd->hdr.sequence |= cpu_to_le16(SEQ_HUGE_FRAME);

        phys_addr = txq->dma_addr_cmd + sizeof(txq->cmd[0]) * idx +
                        offsetof(struct iwl3945_cmd, hdr);
        iwl3945_hw_txq_attach_buf_to_tfd(priv, tfd, phys_addr, fix_size);

        pad = U32_PAD(cmd->len);
        count = TFD_CTL_COUNT_GET(*control_flags);
        *control_flags = TFD_CTL_COUNT_SET(count) | TFD_CTL_PAD_SET(pad);

        IWL_DEBUG_HC("Sending command %s (#%x), seq: 0x%04X, "
                     "%d bytes at %d[%d]:%d\n",
                     get_cmd_string(out_cmd->hdr.cmd),
                     out_cmd->hdr.cmd, le16_to_cpu(out_cmd->hdr.sequence),
                     fix_size, q->write_ptr, idx, IWL_CMD_QUEUE_NUM);

        txq->need_update = 1;

        /* Increment and update queue's write index */
        q->write_ptr = iwl_queue_inc_wrap(q->write_ptr, q->n_bd);
        ret = iwl3945_tx_queue_update_write_ptr(priv, txq);

        spin_unlock_irqrestore(&priv->hcmd_lock, flags);
        return ret ? ret : idx;
}

static int iwl3945_send_cmd_async(struct iwl3945_priv *priv, struct iwl3945_host_cmd *cmd)
{
        int ret;

        BUG_ON(!(cmd->meta.flags & CMD_ASYNC));

        /* An asynchronous command can not expect an SKB to be set. */
        BUG_ON(cmd->meta.flags & CMD_WANT_SKB);

        /* An asynchronous command MUST have a callback. */
        BUG_ON(!cmd->meta.u.callback);

        if (test_bit(STATUS_EXIT_PENDING, &priv->status))
                return -EBUSY;

        ret = iwl3945_enqueue_hcmd(priv, cmd);
        if (ret < 0) {
                IWL_ERROR("Error sending %s: iwl3945_enqueue_hcmd failed: %d\n",
                          get_cmd_string(cmd->id), ret);
                return ret;
        }
        return 0;
}

static int iwl3945_send_cmd_sync(struct iwl3945_priv *priv, struct iwl3945_host_cmd *cmd)
{
        int cmd_idx;
        int ret;

        BUG_ON(cmd->meta.flags & CMD_ASYNC);

         /* A synchronous command can not have a callback set. */
        BUG_ON(cmd->meta.u.callback != NULL);

        if (test_and_set_bit(STATUS_HCMD_SYNC_ACTIVE, &priv->status)) {
                IWL_ERROR("Error sending %s: Already sending a host command\n",
                          get_cmd_string(cmd->id));
                ret = -EBUSY;
                goto out;
        }

        set_bit(STATUS_HCMD_ACTIVE, &priv->status);

        if (cmd->meta.flags & CMD_WANT_SKB)
                cmd->meta.source = &cmd->meta;

        cmd_idx = iwl3945_enqueue_hcmd(priv, cmd);
        if (cmd_idx < 0) {
                ret = cmd_idx;
                IWL_ERROR("Error sending %s: iwl3945_enqueue_hcmd failed: %d\n",
                          get_cmd_string(cmd->id), ret);
                goto out;
        }

        ret = wait_event_interruptible_timeout(priv->wait_command_queue,
                        !test_bit(STATUS_HCMD_ACTIVE, &priv->status),
                        HOST_COMPLETE_TIMEOUT);
        if (!ret) {
                if (test_bit(STATUS_HCMD_ACTIVE, &priv->status)) {
                        IWL_ERROR("Error sending %s: time out after %dms.\n",
                                  get_cmd_string(cmd->id),
                                  jiffies_to_msecs(HOST_COMPLETE_TIMEOUT));

                        clear_bit(STATUS_HCMD_ACTIVE, &priv->status);
                        ret = -ETIMEDOUT;
                        goto cancel;
                }
        }

        if (test_bit(STATUS_RF_KILL_HW, &priv->status)) {
                IWL_DEBUG_INFO("Command %s aborted: RF KILL Switch\n",
                               get_cmd_string(cmd->id));
                ret = -ECANCELED;
                goto fail;
        }
        if (test_bit(STATUS_FW_ERROR, &priv->status)) {
                IWL_DEBUG_INFO("Command %s failed: FW Error\n",
                               get_cmd_string(cmd->id));
                ret = -EIO;
                goto fail;
        }
        if ((cmd->meta.flags & CMD_WANT_SKB) && !cmd->meta.u.skb) {
                IWL_ERROR("Error: Response NULL in '%s'\n",
                          get_cmd_string(cmd->id));
                ret = -EIO;
                goto out;
        }

        ret = 0;
        goto out;

cancel:
        if (cmd->meta.flags & CMD_WANT_SKB) {
                struct iwl3945_cmd *qcmd;

                /* Cancel the CMD_WANT_SKB flag for the cmd in the
                 * TX cmd queue. Otherwise in case the cmd comes
                 * in later, it will possibly set an invalid
                 * address (cmd->meta.source). */
                qcmd = &priv->txq[IWL_CMD_QUEUE_NUM].cmd[cmd_idx];
                qcmd->meta.flags &= ~CMD_WANT_SKB;
        }
fail:
        if (cmd->meta.u.skb) {
                dev_kfree_skb_any(cmd->meta.u.skb);
                cmd->meta.u.skb = NULL;
        }
out:
        clear_bit(STATUS_HCMD_SYNC_ACTIVE, &priv->status);
        return ret;
}

int iwl3945_send_cmd(struct iwl3945_priv *priv, struct iwl3945_host_cmd *cmd)
{
        if (cmd->meta.flags & CMD_ASYNC)
                return iwl3945_send_cmd_async(priv, cmd);

        return iwl3945_send_cmd_sync(priv, cmd);
}

int iwl3945_send_cmd_pdu(struct iwl3945_priv *priv, u8 id, u16 len, const void *data)
{
        struct iwl3945_host_cmd cmd = {
                .id = id,
                .len = len,
                .data = data,
        };

        return iwl3945_send_cmd_sync(priv, &cmd);
}

static int __must_check iwl3945_send_cmd_u32(struct iwl3945_priv *priv, u8 id, u32 val)
{
        struct iwl3945_host_cmd cmd = {
                .id = id,
                .len = sizeof(val),
                .data = &val,
        };

        return iwl3945_send_cmd_sync(priv, &cmd);
}

int iwl3945_send_statistics_request(struct iwl3945_priv *priv)
{
        return iwl3945_send_cmd_u32(priv, REPLY_STATISTICS_CMD, 0);
}

/**
 * iwl3945_set_rxon_channel - Set the phymode and channel values in staging RXON
 * @band: 2.4 or 5 GHz band
 * @channel: Any channel valid for the requested band

 * In addition to setting the staging RXON, priv->band is also set.
 *
 * NOTE:  Does not commit to the hardware; it sets appropriate bit fields
 * in the staging RXON flag structure based on the band
 */
static int iwl3945_set_rxon_channel(struct iwl3945_priv *priv,
                                    enum ieee80211_band band,
                                    u16 channel)
{
        if (!iwl3945_get_channel_info(priv, band, channel)) {
                IWL_DEBUG_INFO("Could not set channel to %d [%d]\n",
                               channel, band);
                return -EINVAL;
        }

        if ((le16_to_cpu(priv->staging_rxon.channel) == channel) &&
            (priv->band == band))
                return 0;

        priv->staging_rxon.channel = cpu_to_le16(channel);
        if (band == IEEE80211_BAND_5GHZ)
                priv->staging_rxon.flags &= ~RXON_FLG_BAND_24G_MSK;
        else
                priv->staging_rxon.flags |= RXON_FLG_BAND_24G_MSK;

        priv->band = band;

        IWL_DEBUG_INFO("Staging channel set to %d [%d]\n", channel, band);

        return 0;
}

/**
 * iwl3945_check_rxon_cmd - validate RXON structure is valid
 *
 * NOTE:  This is really only useful during development and can eventually
 * be #ifdef'd out once the driver is stable and folks aren't actively
 * making changes
 */
static int iwl3945_check_rxon_cmd(struct iwl3945_rxon_cmd *rxon)
{
        int error = 0;
        int counter = 1;

        if (rxon->flags & RXON_FLG_BAND_24G_MSK) {
                error |= le32_to_cpu(rxon->flags &
                                (RXON_FLG_TGJ_NARROW_BAND_MSK |
                                 RXON_FLG_RADAR_DETECT_MSK));
                if (error)
                        IWL_WARNING("check 24G fields %d | %d\n",
                                    counter++, error);
        } else {
                error |= (rxon->flags & RXON_FLG_SHORT_SLOT_MSK) ?
                                0 : le32_to_cpu(RXON_FLG_SHORT_SLOT_MSK);
                if (error)
                        IWL_WARNING("check 52 fields %d | %d\n",
                                    counter++, error);
                error |= le32_to_cpu(rxon->flags & RXON_FLG_CCK_MSK);
                if (error)
                        IWL_WARNING("check 52 CCK %d | %d\n",
                                    counter++, error);
        }
        error |= (rxon->node_addr[0] | rxon->bssid_addr[0]) & 0x1;
        if (error)
                IWL_WARNING("check mac addr %d | %d\n", counter++, error);

        /* make sure basic rates 6Mbps and 1Mbps are supported */
        error |= (((rxon->ofdm_basic_rates & IWL_RATE_6M_MASK) == 0) &&
                  ((rxon->cck_basic_rates & IWL_RATE_1M_MASK) == 0));
        if (error)
                IWL_WARNING("check basic rate %d | %d\n", counter++, error);

        error |= (le16_to_cpu(rxon->assoc_id) > 2007);
        if (error)
                IWL_WARNING("check assoc id %d | %d\n", counter++, error);

        error |= ((rxon->flags & (RXON_FLG_CCK_MSK | RXON_FLG_SHORT_SLOT_MSK))
                        == (RXON_FLG_CCK_MSK | RXON_FLG_SHORT_SLOT_MSK));
        if (error)
                IWL_WARNING("check CCK and short slot %d | %d\n",
                            counter++, error);

        error |= ((rxon->flags & (RXON_FLG_CCK_MSK | RXON_FLG_AUTO_DETECT_MSK))
                        == (RXON_FLG_CCK_MSK | RXON_FLG_AUTO_DETECT_MSK));
        if (error)
                IWL_WARNING("check CCK & auto detect %d | %d\n",
                            counter++, error);

        error |= ((rxon->flags & (RXON_FLG_AUTO_DETECT_MSK |
                        RXON_FLG_TGG_PROTECT_MSK)) == RXON_FLG_TGG_PROTECT_MSK);
        if (error)
                IWL_WARNING("check TGG and auto detect %d | %d\n",
                            counter++, error);

        if ((rxon->flags & RXON_FLG_DIS_DIV_MSK))
                error |= ((rxon->flags & (RXON_FLG_ANT_B_MSK |
                                RXON_FLG_ANT_A_MSK)) == 0);
        if (error)
                IWL_WARNING("check antenna %d %d\n", counter++, error);

        if (error)
                IWL_WARNING("Tuning to channel %d\n",
                            le16_to_cpu(rxon->channel));

        if (error) {
                IWL_ERROR("Not a valid iwl3945_rxon_assoc_cmd field values\n");
                return -1;
        }
        return 0;
}

/**
 * iwl3945_full_rxon_required - check if full RXON (vs RXON_ASSOC) cmd is needed
 * @priv: staging_rxon is compared to active_rxon
 *
 * If the RXON structure is changing enough to require a new tune,
 * or is clearing the RXON_FILTER_ASSOC_MSK, then return 1 to indicate that
 * a new tune (full RXON command, rather than RXON_ASSOC cmd) is required.
 */
static int iwl3945_full_rxon_required(struct iwl3945_priv *priv)
{

        /* These items are only settable from the full RXON command */
        if (!(iwl3945_is_associated(priv)) ||
            compare_ether_addr(priv->staging_rxon.bssid_addr,
                               priv->active_rxon.bssid_addr) ||
            compare_ether_addr(priv->staging_rxon.node_addr,
                               priv->active_rxon.node_addr) ||
            compare_ether_addr(priv->staging_rxon.wlap_bssid_addr,
                               priv->active_rxon.wlap_bssid_addr) ||
            (priv->staging_rxon.dev_type != priv->active_rxon.dev_type) ||
            (priv->staging_rxon.channel != priv->active_rxon.channel) ||
            (priv->staging_rxon.air_propagation !=
             priv->active_rxon.air_propagation) ||
            (priv->staging_rxon.assoc_id != priv->active_rxon.assoc_id))
                return 1;

        /* flags, filter_flags, ofdm_basic_rates, and cck_basic_rates can
         * be updated with the RXON_ASSOC command -- however only some
         * flag transitions are allowed using RXON_ASSOC */

        /* Check if we are not switching bands */
        if ((priv->staging_rxon.flags & RXON_FLG_BAND_24G_MSK) !=
            (priv->active_rxon.flags & RXON_FLG_BAND_24G_MSK))
                return 1;

        /* Check if we are switching association toggle */
        if ((priv->staging_rxon.filter_flags & RXON_FILTER_ASSOC_MSK) !=
                (priv->active_rxon.filter_flags & RXON_FILTER_ASSOC_MSK))
                return 1;

        return 0;
}

static int iwl3945_send_rxon_assoc(struct iwl3945_priv *priv)
{
        int rc = 0;
        struct iwl3945_rx_packet *res = NULL;
        struct iwl3945_rxon_assoc_cmd rxon_assoc;
        struct iwl3945_host_cmd cmd = {
                .id = REPLY_RXON_ASSOC,
                .len = sizeof(rxon_assoc),
                .meta.flags = CMD_WANT_SKB,
                .data = &rxon_assoc,
        };
        const struct iwl3945_rxon_cmd *rxon1 = &priv->staging_rxon;
        const struct iwl3945_rxon_cmd *rxon2 = &priv->active_rxon;

        if ((rxon1->flags == rxon2->flags) &&
            (rxon1->filter_flags == rxon2->filter_flags) &&
            (rxon1->cck_basic_rates == rxon2->cck_basic_rates) &&
            (rxon1->ofdm_basic_rates == rxon2->ofdm_basic_rates)) {
                IWL_DEBUG_INFO("Using current RXON_ASSOC.  Not resending.\n");
                return 0;
        }

        rxon_assoc.flags = priv->staging_rxon.flags;
        rxon_assoc.filter_flags = priv->staging_rxon.filter_flags;
        rxon_assoc.ofdm_basic_rates = priv->staging_rxon.ofdm_basic_rates;
        rxon_assoc.cck_basic_rates = priv->staging_rxon.cck_basic_rates;
        rxon_assoc.reserved = 0;

        rc = iwl3945_send_cmd_sync(priv, &cmd);
        if (rc)
                return rc;

        res = (struct iwl3945_rx_packet *)cmd.meta.u.skb->data;
        if (res->hdr.flags & IWL_CMD_FAILED_MSK) {
                IWL_ERROR("Bad return from REPLY_RXON_ASSOC command\n");
                rc = -EIO;
        }

        priv->alloc_rxb_skb--;
        dev_kfree_skb_any(cmd.meta.u.skb);

        return rc;
}

/**
 * iwl3945_commit_rxon - commit staging_rxon to hardware
 *
 * The RXON command in staging_rxon is committed to the hardware and
 * the active_rxon structure is updated with the new data.  This
 * function correctly transitions out of the RXON_ASSOC_MSK state if
 * a HW tune is required based on the RXON structure changes.
 */
static int iwl3945_commit_rxon(struct iwl3945_priv *priv)
{
        /* cast away the const for active_rxon in this function */
        struct iwl3945_rxon_cmd *active_rxon = (void *)&priv->active_rxon;
        int rc = 0;
        DECLARE_MAC_BUF(mac);

        if (!iwl3945_is_alive(priv))
                return -1;

        /* always get timestamp with Rx frame */
        priv->staging_rxon.flags |= RXON_FLG_TSF2HOST_MSK;

        /* select antenna */
        priv->staging_rxon.flags &=
            ~(RXON_FLG_DIS_DIV_MSK | RXON_FLG_ANT_SEL_MSK);
        priv->staging_rxon.flags |= iwl3945_get_antenna_flags(priv);

        rc = iwl3945_check_rxon_cmd(&priv->staging_rxon);
        if (rc) {
                IWL_ERROR("Invalid RXON configuration.  Not committing.\n");
                return -EINVAL;
        }

        /* If we don't need to send a full RXON, we can use
         * iwl3945_rxon_assoc_cmd which is used to reconfigure filter
         * and other flags for the current radio configuration. */
        if (!iwl3945_full_rxon_required(priv)) {
                rc = iwl3945_send_rxon_assoc(priv);
                if (rc) {
                        IWL_ERROR("Error setting RXON_ASSOC "
                                  "configuration (%d).\n", rc);
                        return rc;
                }

                memcpy(active_rxon, &priv->staging_rxon, sizeof(*active_rxon));

                return 0;
        }

        /* If we are currently associated and the new config requires
         * an RXON_ASSOC and the new config wants the associated mask enabled,
         * we must clear the associated from the active configuration
         * before we apply the new config */
        if (iwl3945_is_associated(priv) &&
            (priv->staging_rxon.filter_flags & RXON_FILTER_ASSOC_MSK)) {
                IWL_DEBUG_INFO("Toggling associated bit on current RXON\n");
                active_rxon->filter_flags &= ~RXON_FILTER_ASSOC_MSK;

                rc = iwl3945_send_cmd_pdu(priv, REPLY_RXON,
                                      sizeof(struct iwl3945_rxon_cmd),
                                      &priv->active_rxon);

                /* If the mask clearing failed then we set
                 * active_rxon back to what it was previously */
                if (rc) {
                        active_rxon->filter_flags |= RXON_FILTER_ASSOC_MSK;
                        IWL_ERROR("Error clearing ASSOC_MSK on current "
                                  "configuration (%d).\n", rc);
                        return rc;
                }
        }

        IWL_DEBUG_INFO("Sending RXON\n"
                       "* with%s RXON_FILTER_ASSOC_MSK\n"
                       "* channel = %d\n"
                       "* bssid = %s\n",
                       ((priv->staging_rxon.filter_flags &
                         RXON_FILTER_ASSOC_MSK) ? "" : "out"),
                       le16_to_cpu(priv->staging_rxon.channel),
                       print_mac(mac, priv->staging_rxon.bssid_addr));

        /* Apply the new configuration */
        rc = iwl3945_send_cmd_pdu(priv, REPLY_RXON,
                              sizeof(struct iwl3945_rxon_cmd), &priv->staging_rxon);
        if (rc) {
                IWL_ERROR("Error setting new configuration (%d).\n", rc);
                return rc;
        }

        memcpy(active_rxon, &priv->staging_rxon, sizeof(*active_rxon));

        iwl3945_clear_stations_table(priv);

        /* If we issue a new RXON command which required a tune then we must
         * send a new TXPOWER command or we won't be able to Tx any frames */
        rc = iwl3945_hw_reg_send_txpower(priv);
        if (rc) {
                IWL_ERROR("Error setting Tx power (%d).\n", rc);
                return rc;
        }

        /* Add the broadcast address so we can send broadcast frames */
        if (iwl3945_add_station(priv, iwl3945_broadcast_addr, 0, 0) ==
            IWL_INVALID_STATION) {
                IWL_ERROR("Error adding BROADCAST address for transmit.\n");
                return -EIO;
        }

        /* If we have set the ASSOC_MSK and we are in BSS mode then
         * add the IWL_AP_ID to the station rate table */
        if (iwl3945_is_associated(priv) &&
            (priv->iw_mode == IEEE80211_IF_TYPE_STA))
                if (iwl3945_add_station(priv, priv->active_rxon.bssid_addr, 1, 0)
                    == IWL_INVALID_STATION) {
                        IWL_ERROR("Error adding AP address for transmit.\n");
                        return -EIO;
                }

        /* Init the hardware's rate fallback order based on the band */
        rc = iwl3945_init_hw_rate_table(priv);
        if (rc) {
                IWL_ERROR("Error setting HW rate table: %02X\n", rc);
                return -EIO;
        }

        return 0;
}

static int iwl3945_send_bt_config(struct iwl3945_priv *priv)
{
        struct iwl3945_bt_cmd bt_cmd = {
                .flags = 3,
                .lead_time = 0xAA,
                .max_kill = 1,
                .kill_ack_mask = 0,
                .kill_cts_mask = 0,
        };

        return iwl3945_send_cmd_pdu(priv, REPLY_BT_CONFIG,
                                sizeof(struct iwl3945_bt_cmd), &bt_cmd);
}

static int iwl3945_send_scan_abort(struct iwl3945_priv *priv)
{
        int rc = 0;
        struct iwl3945_rx_packet *res;
        struct iwl3945_host_cmd cmd = {
                .id = REPLY_SCAN_ABORT_CMD,
                .meta.flags = CMD_WANT_SKB,
        };

        /* If there isn't a scan actively going on in the hardware
         * then we are in between scan bands and not actually
         * actively scanning, so don't send the abort command */
        if (!test_bit(STATUS_SCAN_HW, &priv->status)) {
                clear_bit(STATUS_SCAN_ABORTING, &priv->status);
                return 0;
        }

        rc = iwl3945_send_cmd_sync(priv, &cmd);
        if (rc) {
                clear_bit(STATUS_SCAN_ABORTING, &priv->status);
                return rc;
        }

        res = (struct iwl3945_rx_packet *)cmd.meta.u.skb->data;
        if (res->u.status != CAN_ABORT_STATUS) {
                /* The scan abort will return 1 for success or
                 * 2 for "failure".  A failure condition can be
                 * due to simply not being in an active scan which
                 * can occur if we send the scan abort before we
                 * the microcode has notified us that a scan is
                 * completed. */
                IWL_DEBUG_INFO("SCAN_ABORT returned %d.\n", res->u.status);
                clear_bit(STATUS_SCAN_ABORTING, &priv->status);
                clear_bit(STATUS_SCAN_HW, &priv->status);
        }

        dev_kfree_skb_any(cmd.meta.u.skb);

        return rc;
}

static int iwl3945_card_state_sync_callback(struct iwl3945_priv *priv,
                                        struct iwl3945_cmd *cmd,
                                        struct sk_buff *skb)
{
        return 1;
}

/*
 * CARD_STATE_CMD
 *
 * Use: Sets the device's internal card state to enable, disable, or halt
 *
 * When in the 'enable' state the card operates as normal.
 * When in the 'disable' state, the card enters into a low power mode.
 * When in the 'halt' state, the card is shut down and must be fully
 * restarted to come back on.
 */
static int iwl3945_send_card_state(struct iwl3945_priv *priv, u32 flags, u8 meta_flag)
{
        struct iwl3945_host_cmd cmd = {
                .id = REPLY_CARD_STATE_CMD,
                .len = sizeof(u32),
                .data = &flags,
                .meta.flags = meta_flag,
        };

        if (meta_flag & CMD_ASYNC)
                cmd.meta.u.callback = iwl3945_card_state_sync_callback;

        return iwl3945_send_cmd(priv, &cmd);
}

static int iwl3945_add_sta_sync_callback(struct iwl3945_priv *priv,
                                     struct iwl3945_cmd *cmd, struct sk_buff *skb)
{
        struct iwl3945_rx_packet *res = NULL;

        if (!skb) {
                IWL_ERROR("Error: Response NULL in REPLY_ADD_STA.\n");
                return 1;
        }

        res = (struct iwl3945_rx_packet *)skb->data;
        if (res->hdr.flags & IWL_CMD_FAILED_MSK) {
                IWL_ERROR("Bad return from REPLY_ADD_STA (0x%08X)\n",
                          res->hdr.flags);
                return 1;
        }

        switch (res->u.add_sta.status) {
        case ADD_STA_SUCCESS_MSK:
                break;
        default:
                break;
        }

        /* We didn't cache the SKB; let the caller free it */
        return 1;
}

int iwl3945_send_add_station(struct iwl3945_priv *priv,
                         struct iwl3945_addsta_cmd *sta, u8 flags)
{
        struct iwl3945_rx_packet *res = NULL;
        int rc = 0;
        struct iwl3945_host_cmd cmd = {
                .id = REPLY_ADD_STA,
                .len = sizeof(struct iwl3945_addsta_cmd),
                .meta.flags = flags,
                .data = sta,
        };

        if (flags & CMD_ASYNC)
                cmd.meta.u.callback = iwl3945_add_sta_sync_callback;
        else
                cmd.meta.flags |= CMD_WANT_SKB;

        rc = iwl3945_send_cmd(priv, &cmd);

        if (rc || (flags & CMD_ASYNC))
                return rc;

        res = (struct iwl3945_rx_packet *)cmd.meta.u.skb->data;
        if (res->hdr.flags & IWL_CMD_FAILED_MSK) {
                IWL_ERROR("Bad return from REPLY_ADD_STA (0x%08X)\n",
                          res->hdr.flags);
                rc = -EIO;
        }

        if (rc == 0) {
                switch (res->u.add_sta.status) {
                case ADD_STA_SUCCESS_MSK:
                        IWL_DEBUG_INFO("REPLY_ADD_STA PASSED\n");
                        break;
                default:
                        rc = -EIO;
                        IWL_WARNING("REPLY_ADD_STA failed\n");
                        break;
                }
        }

        priv->alloc_rxb_skb--;
        dev_kfree_skb_any(cmd.meta.u.skb);

        return rc;
}

static int iwl3945_update_sta_key_info(struct iwl3945_priv *priv,
                                   struct ieee80211_key_conf *keyconf,
                                   u8 sta_id)
{
        unsigned long flags;
        __le16 key_flags = 0;

        switch (keyconf->alg) {
        case ALG_CCMP:
                key_flags |= STA_KEY_FLG_CCMP;
                key_flags |= cpu_to_le16(
                                keyconf->keyidx << STA_KEY_FLG_KEYID_POS);
                key_flags &= ~STA_KEY_FLG_INVALID;
                break;
        case ALG_TKIP:
        case ALG_WEP:
        default:
                return -EINVAL;
        }
        spin_lock_irqsave(&priv->sta_lock, flags);
        priv->stations[sta_id].keyinfo.alg = keyconf->alg;
        priv->stations[sta_id].keyinfo.keylen = keyconf->keylen;
        memcpy(priv->stations[sta_id].keyinfo.key, keyconf->key,
               keyconf->keylen);

        memcpy(priv->stations[sta_id].sta.key.key, keyconf->key,
               keyconf->keylen);
        priv->stations[sta_id].sta.key.key_flags = key_flags;
        priv->stations[sta_id].sta.sta.modify_mask = STA_MODIFY_KEY_MASK;
        priv->stations[sta_id].sta.mode = STA_CONTROL_MODIFY_MSK;

        spin_unlock_irqrestore(&priv->sta_lock, flags);

        IWL_DEBUG_INFO("hwcrypto: modify ucode station key info\n");
        iwl3945_send_add_station(priv, &priv->stations[sta_id].sta, 0);
        return 0;
}

static int iwl3945_clear_sta_key_info(struct iwl3945_priv *priv, u8 sta_id)
{
        unsigned long flags;

        spin_lock_irqsave(&priv->sta_lock, flags);
        memset(&priv->stations[sta_id].keyinfo, 0, sizeof(struct iwl3945_hw_key));
        memset(&priv->stations[sta_id].sta.key, 0, sizeof(struct iwl3945_keyinfo));
        priv->stations[sta_id].sta.key.key_flags = STA_KEY_FLG_NO_ENC;
        priv->stations[sta_id].sta.sta.modify_mask = STA_MODIFY_KEY_MASK;
        priv->stations[sta_id].sta.mode = STA_CONTROL_MODIFY_MSK;
        spin_unlock_irqrestore(&priv->sta_lock, flags);

        IWL_DEBUG_INFO("hwcrypto: clear ucode station key info\n");
        iwl3945_send_add_station(priv, &priv->stations[sta_id].sta, 0);
        return 0;
}

static void iwl3945_clear_free_frames(struct iwl3945_priv *priv)
{
        struct list_head *element;

        IWL_DEBUG_INFO("%d frames on pre-allocated heap on clear.\n",
                       priv->frames_count);

        while (!list_empty(&priv->free_frames)) {
                element = priv->free_frames.next;
                list_del(element);
                kfree(list_entry(element, struct iwl3945_frame, list));
                priv->frames_count--;
        }

        if (priv->frames_count) {
                IWL_WARNING("%d frames still in use.  Did we lose one?\n",
                            priv->frames_count);
                priv->frames_count = 0;
        }
}

static struct iwl3945_frame *iwl3945_get_free_frame(struct iwl3945_priv *priv)
{
        struct iwl3945_frame *frame;
        struct list_head *element;
        if (list_empty(&priv->free_frames)) {
                frame = kzalloc(sizeof(*frame), GFP_KERNEL);
                if (!frame) {
                        IWL_ERROR("Could not allocate frame!\n");
                        return NULL;
                }

                priv->frames_count++;
                return frame;
        }

        element = priv->free_frames.next;
        list_del(element);
        return list_entry(element, struct iwl3945_frame, list);
}

static void iwl3945_free_frame(struct iwl3945_priv *priv, struct iwl3945_frame *frame)
{
        memset(frame, 0, sizeof(*frame));
        list_add(&frame->list, &priv->free_frames);
}

unsigned int iwl3945_fill_beacon_frame(struct iwl3945_priv *priv,
                                struct ieee80211_hdr *hdr,
                                const u8 *dest, int left)
{

        if (!iwl3945_is_associated(priv) || !priv->ibss_beacon ||
            ((priv->iw_mode != IEEE80211_IF_TYPE_IBSS) &&
             (priv->iw_mode != IEEE80211_IF_TYPE_AP)))
                return 0;

        if (priv->ibss_beacon->len > left)
                return 0;

        memcpy(hdr, priv->ibss_beacon->data, priv->ibss_beacon->len);

        return priv->ibss_beacon->len;
}

static u8 iwl3945_rate_get_lowest_plcp(int rate_mask)
{
        u8 i;

        for (i = IWL_RATE_1M_INDEX; i != IWL_RATE_INVALID;
             i = iwl3945_rates[i].next_ieee) {
                if (rate_mask & (1 << i))
                        return iwl3945_rates[i].plcp;
        }

        return IWL_RATE_INVALID;
}

static int iwl3945_send_beacon_cmd(struct iwl3945_priv *priv)
{
        struct iwl3945_frame *frame;
        unsigned int frame_size;
        int rc;
        u8 rate;

        frame = iwl3945_get_free_frame(priv);

        if (!frame) {
                IWL_ERROR("Could not obtain free frame buffer for beacon "
                          "command.\n");
                return -ENOMEM;
        }

        if (!(priv->staging_rxon.flags & RXON_FLG_BAND_24G_MSK)) {
                rate = iwl3945_rate_get_lowest_plcp(priv->active_rate_basic &
                                                0xFF0);
                if (rate == IWL_INVALID_RATE)
                        rate = IWL_RATE_6M_PLCP;
        } else {
                rate = iwl3945_rate_get_lowest_plcp(priv->active_rate_basic & 0xF);
                if (rate == IWL_INVALID_RATE)
                        rate = IWL_RATE_1M_PLCP;
        }

        frame_size = iwl3945_hw_get_beacon_cmd(priv, frame, rate);

        rc = iwl3945_send_cmd_pdu(priv, REPLY_TX_BEACON, frame_size,
                              &frame->u.cmd[0]);

        iwl3945_free_frame(priv, frame);

        return rc;
}

/******************************************************************************
 *
 * EEPROM related functions
 *
 ******************************************************************************/

static void get_eeprom_mac(struct iwl3945_priv *priv, u8 *mac)
{
        memcpy(mac, priv->eeprom.mac_address, 6);
}

/*
 * Clear the OWNER_MSK, to establish driver (instead of uCode running on
 * embedded controller) as EEPROM reader; each read is a series of pulses
 * to/from the EEPROM chip, not a single event, so even reads could conflict
 * if they weren't arbitrated by some ownership mechanism.  Here, the driver
 * simply claims ownership, which should be safe when this function is called
 * (i.e. before loading uCode!).
 */
static inline int iwl3945_eeprom_acquire_semaphore(struct iwl3945_priv *priv)
{
        _iwl3945_clear_bit(priv, CSR_EEPROM_GP, CSR_EEPROM_GP_IF_OWNER_MSK);
        return 0;
}

/**
 * iwl3945_eeprom_init - read EEPROM contents
 *
 * Load the EEPROM contents from adapter into priv->eeprom
 *
 * NOTE:  This routine uses the non-debug IO access functions.
 */
int iwl3945_eeprom_init(struct iwl3945_priv *priv)
{
        u16 *e = (u16 *)&priv->eeprom;
        u32 gp = iwl3945_read32(priv, CSR_EEPROM_GP);
        u32 r;
        int sz = sizeof(priv->eeprom);
        int rc;
        int i;
        u16 addr;

        /* The EEPROM structure has several padding buffers within it
         * and when adding new EEPROM maps is subject to programmer errors
         * which may be very difficult to identify without explicitly
         * checking the resulting size of the eeprom map. */
        BUILD_BUG_ON(sizeof(priv->eeprom) != IWL_EEPROM_IMAGE_SIZE);

        if ((gp & CSR_EEPROM_GP_VALID_MSK) == CSR_EEPROM_GP_BAD_SIGNATURE) {
                IWL_ERROR("EEPROM not found, EEPROM_GP=0x%08x\n", gp);
                return -ENOENT;
        }

        /* Make sure driver (instead of uCode) is allowed to read EEPROM */
        rc = iwl3945_eeprom_acquire_semaphore(priv);
        if (rc < 0) {
                IWL_ERROR("Failed to acquire EEPROM semaphore.\n");
                return -ENOENT;
        }

        /* eeprom is an array of 16bit values */
        for (addr = 0; addr < sz; addr += sizeof(u16)) {
                _iwl3945_write32(priv, CSR_EEPROM_REG, addr << 1);
                _iwl3945_clear_bit(priv, CSR_EEPROM_REG, CSR_EEPROM_REG_BIT_CMD);

                for (i = 0; i < IWL_EEPROM_ACCESS_TIMEOUT;
                                        i += IWL_EEPROM_ACCESS_DELAY) {
                        r = _iwl3945_read_direct32(priv, CSR_EEPROM_REG);
                        if (r & CSR_EEPROM_REG_READ_VALID_MSK)
                                break;
                        udelay(IWL_EEPROM_ACCESS_DELAY);
                }

                if (!(r & CSR_EEPROM_REG_READ_VALID_MSK)) {
                        IWL_ERROR("Time out reading EEPROM[%d]\n", addr);
                        return -ETIMEDOUT;
                }
                e[addr / 2] = le16_to_cpu((__force __le16)(r >> 16));
        }

        return 0;
}

static void iwl3945_unset_hw_setting(struct iwl3945_priv *priv)
{
        if (priv->hw_setting.shared_virt)
                pci_free_consistent(priv->pci_dev,
                                    sizeof(struct iwl3945_shared),
                                    priv->hw_setting.shared_virt,
                                    priv->hw_setting.shared_phys);
}

/**
 * iwl3945_supported_rate_to_ie - fill in the supported rate in IE field
 *
 * return : set the bit for each supported rate insert in ie
 */
static u16 iwl3945_supported_rate_to_ie(u8 *ie, u16 supported_rate,
                                    u16 basic_rate, int *left)
{
        u16 ret_rates = 0, bit;
        int i;
        u8 *cnt = ie;
        u8 *rates = ie + 1;

        for (bit = 1, i = 0; i < IWL_RATE_COUNT; i++, bit <<= 1) {
                if (bit & supported_rate) {
                        ret_rates |= bit;
                        rates[*cnt] = iwl3945_rates[i].ieee |
                                ((bit & basic_rate) ? 0x80 : 0x00);
                        (*cnt)++;
                        (*left)--;
                        if ((*left <= 0) ||
                            (*cnt >= IWL_SUPPORTED_RATES_IE_LEN))
                                break;
                }
        }

        return ret_rates;
}

/**
 * iwl3945_fill_probe_req - fill in all required fields and IE for probe request
 */
static u16 iwl3945_fill_probe_req(struct iwl3945_priv *priv,
                              struct ieee80211_mgmt *frame,
                              int left, int is_direct)
{
        int len = 0;
        u8 *pos = NULL;
        u16 active_rates, ret_rates, cck_rates;

        /* Make sure there is enough space for the probe request,
         * two mandatory IEs and the data */
        left -= 24;
        if (left < 0)
                return 0;
        len += 24;

        frame->frame_control = cpu_to_le16(IEEE80211_STYPE_PROBE_REQ);
        memcpy(frame->da, iwl3945_broadcast_addr, ETH_ALEN);
        memcpy(frame->sa, priv->mac_addr, ETH_ALEN);
        memcpy(frame->bssid, iwl3945_broadcast_addr, ETH_ALEN);
        frame->seq_ctrl = 0;

        /* fill in our indirect SSID IE */
        /* ...next IE... */

        left -= 2;
        if (left < 0)
                return 0;
        len += 2;
        pos = &(frame->u.probe_req.variable[0]);
        *pos++ = WLAN_EID_SSID;
        *pos++ = 0;

        /* fill in our direct SSID IE... */
        if (is_direct) {
                /* ...next IE... */
                left -= 2 + priv->essid_len;
                if (left < 0)
                        return 0;
                /* ... fill it in... */
                *pos++ = WLAN_EID_SSID;
                *pos++ = priv->essid_len;
                memcpy(pos, priv->essid, priv->essid_len);
                pos += priv->essid_len;
                len += 2 + priv->essid_len;
        }

        /* fill in supported rate */
        /* ...next IE... */
        left -= 2;
        if (left < 0)
                return 0;

        /* ... fill it in... */
        *pos++ = WLAN_EID_SUPP_RATES;
        *pos = 0;

        priv->active_rate = priv->rates_mask;
        active_rates = priv->active_rate;
        priv->active_rate_basic = priv->rates_mask & IWL_BASIC_RATES_MASK;

        cck_rates = IWL_CCK_RATES_MASK & active_rates;
        ret_rates = iwl3945_supported_rate_to_ie(pos, cck_rates,
                        priv->active_rate_basic, &left);
        active_rates &= ~ret_rates;

        ret_rates = iwl3945_supported_rate_to_ie(pos, active_rates,
                                 priv->active_rate_basic, &left);
        active_rates &= ~ret_rates;

        len += 2 + *pos;
        pos += (*pos) + 1;
        if (active_rates == 0)
                goto fill_end;

        /* fill in supported extended rate */
        /* ...next IE... */
        left -= 2;
        if (left < 0)
                return 0;
        /* ... fill it in... */
        *pos++ = WLAN_EID_EXT_SUPP_RATES;
        *pos = 0;
        iwl3945_supported_rate_to_ie(pos, active_rates,
                                 priv->active_rate_basic, &left);
        if (*pos > 0)
                len += 2 + *pos;

 fill_end:
        return (u16)len;
}

/*
 * QoS  support
*/
static int iwl3945_send_qos_params_command(struct iwl3945_priv *priv,
                                       struct iwl3945_qosparam_cmd *qos)
{

        return iwl3945_send_cmd_pdu(priv, REPLY_QOS_PARAM,
                                sizeof(struct iwl3945_qosparam_cmd), qos);
}

static void iwl3945_reset_qos(struct iwl3945_priv *priv)
{
        u16 cw_min = 15;
        u16 cw_max = 1023;
        u8 aifs = 2;
        u8 is_legacy = 0;
        unsigned long flags;
        int i;

        spin_lock_irqsave(&priv->lock, flags);
        priv->qos_data.qos_active = 0;

        if (priv->iw_mode == IEEE80211_IF_TYPE_IBSS) {
                if (priv->qos_data.qos_enable)
                        priv->qos_data.qos_active = 1;
                if (!(priv->active_rate & 0xfff0)) {
                        cw_min = 31;
                        is_legacy = 1;
                }
        } else if (priv->iw_mode == IEEE80211_IF_TYPE_AP) {
                if (priv->qos_data.qos_enable)
                        priv->qos_data.qos_active = 1;
        } else if (!(priv->staging_rxon.flags & RXON_FLG_SHORT_SLOT_MSK)) {
                cw_min = 31;
                is_legacy = 1;
        }

        if (priv->qos_data.qos_active)
                aifs = 3;

        priv->qos_data.def_qos_parm.ac[0].cw_min = cpu_to_le16(cw_min);
        priv->qos_data.def_qos_parm.ac[0].cw_max = cpu_to_le16(cw_max);
        priv->qos_data.def_qos_parm.ac[0].aifsn = aifs;
        priv->qos_data.def_qos_parm.ac[0].edca_txop = 0;
        priv->qos_data.def_qos_parm.ac[0].reserved1 = 0;

        if (priv->qos_data.qos_active) {
                i = 1;
                priv->qos_data.def_qos_parm.ac[i].cw_min = cpu_to_le16(cw_min);
                priv->qos_data.def_qos_parm.ac[i].cw_max = cpu_to_le16(cw_max);
                priv->qos_data.def_qos_parm.ac[i].aifsn = 7;
                priv->qos_data.def_qos_parm.ac[i].edca_txop = 0;
                priv->qos_data.def_qos_parm.ac[i].reserved1 = 0;

                i = 2;
                priv->qos_data.def_qos_parm.ac[i].cw_min =
                        cpu_to_le16((cw_min + 1) / 2 - 1);
                priv->qos_data.def_qos_parm.ac[i].cw_max =
                        cpu_to_le16(cw_max);
                priv->qos_data.def_qos_parm.ac[i].aifsn = 2;
                if (is_legacy)
                        priv->qos_data.def_qos_parm.ac[i].edca_txop =
                                cpu_to_le16(6016);
                else
                        priv->qos_data.def_qos_parm.ac[i].edca_txop =
                                cpu_to_le16(3008);
                priv->qos_data.def_qos_parm.ac[i].reserved1 = 0;

                i = 3;
                priv->qos_data.def_qos_parm.ac[i].cw_min =
                        cpu_to_le16((cw_min + 1) / 4 - 1);
                priv->qos_data.def_qos_parm.ac[i].cw_max =
                        cpu_to_le16((cw_max + 1) / 2 - 1);
                priv->qos_data.def_qos_parm.ac[i].aifsn = 2;
                priv->qos_data.def_qos_parm.ac[i].reserved1 = 0;
                if (is_legacy)
                        priv->qos_data.def_qos_parm.ac[i].edca_txop =
                                cpu_to_le16(3264);
                else
                        priv->qos_data.def_qos_parm.ac[i].edca_txop =
                                cpu_to_le16(1504);
        } else {
                for (i = 1; i < 4; i++) {
                        priv->qos_data.def_qos_parm.ac[i].cw_min =
                                cpu_to_le16(cw_min);
                        priv->qos_data.def_qos_parm.ac[i].cw_max =
                                cpu_to_le16(cw_max);
                        priv->qos_data.def_qos_parm.ac[i].aifsn = aifs;
                        priv->qos_data.def_qos_parm.ac[i].edca_txop = 0;
                        priv->qos_data.def_qos_parm.ac[i].reserved1 = 0;
                }
        }
        IWL_DEBUG_QOS("set QoS to default \n");

        spin_unlock_irqrestore(&priv->lock, flags);
}

static void iwl3945_activate_qos(struct iwl3945_priv *priv, u8 force)
{
        unsigned long flags;

        if (test_bit(STATUS_EXIT_PENDING, &priv->status))
                return;

        if (!priv->qos_data.qos_enable)
                return;

        spin_lock_irqsave(&priv->lock, flags);
        priv->qos_data.def_qos_parm.qos_flags = 0;

        if (priv->qos_data.qos_cap.q_AP.queue_request &&
            !priv->qos_data.qos_cap.q_AP.txop_request)
                priv->qos_data.def_qos_parm.qos_flags |=
                        QOS_PARAM_FLG_TXOP_TYPE_MSK;

        if (priv->qos_data.qos_active)
                priv->qos_data.def_qos_parm.qos_flags |=
                        QOS_PARAM_FLG_UPDATE_EDCA_MSK;

        spin_unlock_irqrestore(&priv->lock, flags);

        if (force || iwl3945_is_associated(priv)) {
                IWL_DEBUG_QOS("send QoS cmd with Qos active %d \n",
                              priv->qos_data.qos_active);

                iwl3945_send_qos_params_command(priv,
                                &(priv->qos_data.def_qos_parm));
        }
}

/*
 * Power management (not Tx power!) functions
 */
#define MSEC_TO_USEC 1024

#define NOSLP __constant_cpu_to_le32(0)
#define SLP IWL_POWER_DRIVER_ALLOW_SLEEP_MSK
#define SLP_TIMEOUT(T) __constant_cpu_to_le32((T) * MSEC_TO_USEC)
#define SLP_VEC(X0, X1, X2, X3, X4) {__constant_cpu_to_le32(X0), \
                                     __constant_cpu_to_le32(X1), \
                                     __constant_cpu_to_le32(X2), \
                                     __constant_cpu_to_le32(X3), \
                                     __constant_cpu_to_le32(X4)}


/* default power management (not Tx power) table values */
/* for tim  0-10 */
static struct iwl3945_power_vec_entry range_0[IWL_POWER_AC] = {
        {{NOSLP, SLP_TIMEOUT(0), SLP_TIMEOUT(0), SLP_VEC(0, 0, 0, 0, 0)}, 0},
        {{SLP, SLP_TIMEOUT(200), SLP_TIMEOUT(500), SLP_VEC(1, 2, 3, 4, 4)}, 0},
        {{SLP, SLP_TIMEOUT(200), SLP_TIMEOUT(300), SLP_VEC(2, 4, 6, 7, 7)}, 0},
        {{SLP, SLP_TIMEOUT(50), SLP_TIMEOUT(100), SLP_VEC(2, 6, 9, 9, 10)}, 0},
        {{SLP, SLP_TIMEOUT(50), SLP_TIMEOUT(25), SLP_VEC(2, 7, 9, 9, 10)}, 1},
        {{SLP, SLP_TIMEOUT(25), SLP_TIMEOUT(25), SLP_VEC(4, 7, 10, 10, 10)}, 1}
};

/* for tim > 10 */
static struct iwl3945_power_vec_entry range_1[IWL_POWER_AC] = {
        {{NOSLP, SLP_TIMEOUT(0), SLP_TIMEOUT(0), SLP_VEC(0, 0, 0, 0, 0)}, 0},
        {{SLP, SLP_TIMEOUT(200), SLP_TIMEOUT(500),
                 SLP_VEC(1, 2, 3, 4, 0xFF)}, 0},
        {{SLP, SLP_TIMEOUT(200), SLP_TIMEOUT(300),
                 SLP_VEC(2, 4, 6, 7, 0xFF)}, 0},
        {{SLP, SLP_TIMEOUT(50), SLP_TIMEOUT(100),
                 SLP_VEC(2, 6, 9, 9, 0xFF)}, 0},
        {{SLP, SLP_TIMEOUT(50), SLP_TIMEOUT(25), SLP_VEC(2, 7, 9, 9, 0xFF)}, 0},
        {{SLP, SLP_TIMEOUT(25), SLP_TIMEOUT(25),
                 SLP_VEC(4, 7, 10, 10, 0xFF)}, 0}
};

int iwl3945_power_init_handle(struct iwl3945_priv *priv)
{
        int rc = 0, i;
        struct iwl3945_power_mgr *pow_data;
        int size = sizeof(struct iwl3945_power_vec_entry) * IWL_POWER_AC;
        u16 pci_pm;

        IWL_DEBUG_POWER("Initialize power \n");

        pow_data = &(priv->power_data);

        memset(pow_data, 0, sizeof(*pow_data));

        pow_data->active_index = IWL_POWER_RANGE_0;
        pow_data->dtim_val = 0xffff;

        memcpy(&pow_data->pwr_range_0[0], &range_0[0], size);
        memcpy(&pow_data->pwr_range_1[0], &range_1[0], size);

        rc = pci_read_config_word(priv->pci_dev, PCI_LINK_CTRL, &pci_pm);
        if (rc != 0)
                return 0;
        else {
                struct iwl3945_powertable_cmd *cmd;

                IWL_DEBUG_POWER("adjust power command flags\n");

                for (i = 0; i < IWL_POWER_AC; i++) {
                        cmd = &pow_data->pwr_range_0[i].cmd;

                        if (pci_pm & 0x1)
                                cmd->flags &= ~IWL_POWER_PCI_PM_MSK;
                        else
                                cmd->flags |= IWL_POWER_PCI_PM_MSK;
                }
        }
        return rc;
}

static int iwl3945_update_power_cmd(struct iwl3945_priv *priv,
                                struct iwl3945_powertable_cmd *cmd, u32 mode)
{
        int rc = 0, i;
        u8 skip;
        u32 max_sleep = 0;
        struct iwl3945_power_vec_entry *range;
        u8 period = 0;
        struct iwl3945_power_mgr *pow_data;

        if (mode > IWL_POWER_INDEX_5) {
                IWL_DEBUG_POWER("Error invalid power mode \n");
                return -1;
        }
        pow_data = &(priv->power_data);

        if (pow_data->active_index == IWL_POWER_RANGE_0)
                range = &pow_data->pwr_range_0[0];
        else
                range = &pow_data->pwr_range_1[1];

        memcpy(cmd, &range[mode].cmd, sizeof(struct iwl3945_powertable_cmd));

#ifdef IWL_MAC80211_DISABLE
        if (priv->assoc_network != NULL) {
                unsigned long flags;

                period = priv->assoc_network->tim.tim_period;
        }
#endif  /*IWL_MAC80211_DISABLE */
        skip = range[mode].no_dtim;

        if (period == 0) {
                period = 1;
                skip = 0;
        }

        if (skip == 0) {
                max_sleep = period;
                cmd->flags &= ~IWL_POWER_SLEEP_OVER_DTIM_MSK;
        } else {
                __le32 slp_itrvl = cmd->sleep_interval[IWL_POWER_VEC_SIZE - 1];
                max_sleep = (le32_to_cpu(slp_itrvl) / period) * period;
                cmd->flags |= IWL_POWER_SLEEP_OVER_DTIM_MSK;
        }

        for (i = 0; i < IWL_POWER_VEC_SIZE; i++) {
                if (le32_to_cpu(cmd->sleep_interval[i]) > max_sleep)
                        cmd->sleep_interval[i] = cpu_to_le32(max_sleep);
        }

        IWL_DEBUG_POWER("Flags value = 0x%08X\n", cmd->flags);
        IWL_DEBUG_POWER("Tx timeout = %u\n", le32_to_cpu(cmd->tx_data_timeout));
        IWL_DEBUG_POWER("Rx timeout = %u\n", le32_to_cpu(cmd->rx_data_timeout));
        IWL_DEBUG_POWER("Sleep interval vector = { %d , %d , %d , %d , %d }\n",
                        le32_to_cpu(cmd->sleep_interval[0]),
                        le32_to_cpu(cmd->sleep_interval[1]),
                        le32_to_cpu(cmd->sleep_interval[2]),
                        le32_to_cpu(cmd->sleep_interval[3]),
                        le32_to_cpu(cmd->sleep_interval[4]));

        return rc;
}

static int iwl3945_send_power_mode(struct iwl3945_priv *priv, u32 mode)

{
        u32 uninitialized_var(final_mode);
        int rc;
        struct iwl3945_powertable_cmd cmd;

        /* If on battery, set to 3,
         * if plugged into AC power, set to CAM ("continuously aware mode"),
         * else user level */
        switch (mode) {
        case IWL_POWER_BATTERY:
                final_mode = IWL_POWER_INDEX_3;
                break;
        case IWL_POWER_AC:
                final_mode = IWL_POWER_MODE_CAM;
                break;
        default:
                final_mode = mode;
                break;
        }

        iwl3945_update_power_cmd(priv, &cmd, final_mode);

        rc = iwl3945_send_cmd_pdu(priv, POWER_TABLE_CMD, sizeof(cmd), &cmd);

        if (final_mode == IWL_POWER_MODE_CAM)
                clear_bit(STATUS_POWER_PMI, &priv->status);
        else
                set_bit(STATUS_POWER_PMI, &priv->status);

        return rc;
}

/**
 * iwl3945_scan_cancel - Cancel any currently executing HW scan
 *
 * NOTE: priv->mutex is not required before calling this function
 */
static int iwl3945_scan_cancel(struct iwl3945_priv *priv)
{
        if (!test_bit(STATUS_SCAN_HW, &priv->status)) {
                clear_bit(STATUS_SCANNING, &priv->status);
                return 0;
        }

        if (test_bit(STATUS_SCANNING, &priv->status)) {
                if (!test_bit(STATUS_SCAN_ABORTING, &priv->status)) {
                        IWL_DEBUG_SCAN("Queuing scan abort.\n");
                        set_bit(STATUS_SCAN_ABORTING, &priv->status);
                        queue_work(priv->workqueue, &priv->abort_scan);

                } else
                        IWL_DEBUG_SCAN("Scan abort already in progress.\n");

                return test_bit(STATUS_SCANNING, &priv->status);
        }

        return 0;
}

/**
 * iwl3945_scan_cancel_timeout - Cancel any currently executing HW scan
 * @ms: amount of time to wait (in milliseconds) for scan to abort
 *
 * NOTE: priv->mutex must be held before calling this function
 */
static int iwl3945_scan_cancel_timeout(struct iwl3945_priv *priv, unsigned long ms)
{
        unsigned long now = jiffies;
        int ret;

        ret = iwl3945_scan_cancel(priv);
        if (ret && ms) {
                mutex_unlock(&priv->mutex);
                while (!time_after(jiffies, now + msecs_to_jiffies(ms)) &&
                                test_bit(STATUS_SCANNING, &priv->status))
                        msleep(1);
                mutex_lock(&priv->mutex);

                return test_bit(STATUS_SCANNING, &priv->status);
        }

        return ret;
}

#define MAX_UCODE_BEACON_INTERVAL       1024
#define INTEL_CONN_LISTEN_INTERVAL      __constant_cpu_to_le16(0xA)

static __le16 iwl3945_adjust_beacon_interval(u16 beacon_val)
{
        u16 new_val = 0;
        u16 beacon_factor = 0;

        beacon_factor =
            (beacon_val + MAX_UCODE_BEACON_INTERVAL)
                / MAX_UCODE_BEACON_INTERVAL;
        new_val = beacon_val / beacon_factor;

        return cpu_to_le16(new_val);
}

static void iwl3945_setup_rxon_timing(struct iwl3945_priv *priv)
{
        u64 interval_tm_unit;
        u64 tsf, result;
        unsigned long flags;
        struct ieee80211_conf *conf = NULL;
        u16 beacon_int = 0;

        conf = ieee80211_get_hw_conf(priv->hw);

        spin_lock_irqsave(&priv->lock, flags);
        priv->rxon_timing.timestamp.dw[1] = cpu_to_le32(priv->timestamp1);
        priv->rxon_timing.timestamp.dw[0] = cpu_to_le32(priv->timestamp0);

        priv->rxon_timing.listen_interval = INTEL_CONN_LISTEN_INTERVAL;

        tsf = priv->timestamp1;
        tsf = ((tsf << 32) | priv->timestamp0);

        beacon_int = priv->beacon_int;
        spin_unlock_irqrestore(&priv->lock, flags);

        if (priv->iw_mode == IEEE80211_IF_TYPE_STA) {
                if (beacon_int == 0) {
                        priv->rxon_timing.beacon_interval = cpu_to_le16(100);
                        priv->rxon_timing.beacon_init_val = cpu_to_le32(102400);
                } else {
                        priv->rxon_timing.beacon_interval =
                                cpu_to_le16(beacon_int);
                        priv->rxon_timing.beacon_interval =
                            iwl3945_adjust_beacon_interval(
                                le16_to_cpu(priv->rxon_timing.beacon_interval));
                }

                priv->rxon_timing.atim_window = 0;
        } else {
                priv->rxon_timing.beacon_interval =
                        iwl3945_adjust_beacon_interval(conf->beacon_int);
                /* TODO: we need to get atim_window from upper stack
                 * for now we set to 0 */
                priv->rxon_timing.atim_window = 0;
        }

        interval_tm_unit =
                (le16_to_cpu(priv->rxon_timing.beacon_interval) * 1024);
        result = do_div(tsf, interval_tm_unit);
        priv->rxon_timing.beacon_init_val =
            cpu_to_le32((u32) ((u64) interval_tm_unit - result));

        IWL_DEBUG_ASSOC
            ("beacon interval %d beacon timer %d beacon tim %d\n",
                le16_to_cpu(priv->rxon_timing.beacon_interval),
                le32_to_cpu(priv->rxon_timing.beacon_init_val),
                le16_to_cpu(priv->rxon_timing.atim_window));
}

static int iwl3945_scan_initiate(struct iwl3945_priv *priv)
{
        if (priv->iw_mode == IEEE80211_IF_TYPE_AP) {
                IWL_ERROR("APs don't scan.\n");
                return 0;
        }

        if (!iwl3945_is_ready_rf(priv)) {
                IWL_DEBUG_SCAN("Aborting scan due to not ready.\n");
                return -EIO;
        }

        if (test_bit(STATUS_SCANNING, &priv->status)) {
                IWL_DEBUG_SCAN("Scan already in progress.\n");
                return -EAGAIN;
        }

        if (test_bit(STATUS_SCAN_ABORTING, &priv->status)) {
                IWL_DEBUG_SCAN("Scan request while abort pending.  "
                               "Queuing.\n");
                return -EAGAIN;
        }

        IWL_DEBUG_INFO("Starting scan...\n");
        if (priv->cfg->sku & IWL_SKU_G)
                priv->scan_bands |= BIT(IEEE80211_BAND_2GHZ);
        if (priv->cfg->sku & IWL_SKU_A)
                priv->scan_bands |= BIT(IEEE80211_BAND_5GHZ);
        set_bit(STATUS_SCANNING, &priv->status);
        priv->scan_start = jiffies;
        priv->scan_pass_start = priv->scan_start;

        queue_work(priv->workqueue, &priv->request_scan);

        return 0;
}

static int iwl3945_set_rxon_hwcrypto(struct iwl3945_priv *priv, int hw_decrypt)
{
        struct iwl3945_rxon_cmd *rxon = &priv->staging_rxon;

        if (hw_decrypt)
                rxon->filter_flags &= ~RXON_FILTER_DIS_DECRYPT_MSK;
        else
                rxon->filter_flags |= RXON_FILTER_DIS_DECRYPT_MSK;

        return 0;
}

static void iwl3945_set_flags_for_phymode(struct iwl3945_priv *priv,
                                          enum ieee80211_band band)
{
        if (band == IEEE80211_BAND_5GHZ) {
                priv->staging_rxon.flags &=
                    ~(RXON_FLG_BAND_24G_MSK | RXON_FLG_AUTO_DETECT_MSK
                      | RXON_FLG_CCK_MSK);
                priv->staging_rxon.flags |= RXON_FLG_SHORT_SLOT_MSK;
        } else {
                /* Copied from iwl3945_bg_post_associate() */
                if (priv->assoc_capability & WLAN_CAPABILITY_SHORT_SLOT_TIME)
                        priv->staging_rxon.flags |= RXON_FLG_SHORT_SLOT_MSK;
                else
                        priv->staging_rxon.flags &= ~RXON_FLG_SHORT_SLOT_MSK;

                if (priv->iw_mode == IEEE80211_IF_TYPE_IBSS)
                        priv->staging_rxon.flags &= ~RXON_FLG_SHORT_SLOT_MSK;

                priv->staging_rxon.flags |= RXON_FLG_BAND_24G_MSK;
                priv->staging_rxon.flags |= RXON_FLG_AUTO_DETECT_MSK;
                priv->staging_rxon.flags &= ~RXON_FLG_CCK_MSK;
        }
}

/*
 * initialize rxon structure with default values from eeprom
 */
static void iwl3945_connection_init_rx_config(struct iwl3945_priv *priv)
{
        const struct iwl3945_channel_info *ch_info;

        memset(&priv->staging_rxon, 0, sizeof(priv->staging_rxon));

        switch (priv->iw_mode) {
        case IEEE80211_IF_TYPE_AP:
                priv->staging_rxon.dev_type = RXON_DEV_TYPE_AP;
                break;

        case IEEE80211_IF_TYPE_STA:
                priv->staging_rxon.dev_type = RXON_DEV_TYPE_ESS;
                priv->staging_rxon.filter_flags = RXON_FILTER_ACCEPT_GRP_MSK;
                break;

        case IEEE80211_IF_TYPE_IBSS:
                priv->staging_rxon.dev_type = RXON_DEV_TYPE_IBSS;
                priv->staging_rxon.flags = RXON_FLG_SHORT_PREAMBLE_MSK;
                priv->staging_rxon.filter_flags = RXON_FILTER_BCON_AWARE_MSK |
                                                  RXON_FILTER_ACCEPT_GRP_MSK;
                break;

        case IEEE80211_IF_TYPE_MNTR:
                priv->staging_rxon.dev_type = RXON_DEV_TYPE_SNIFFER;
                priv->staging_rxon.filter_flags = RXON_FILTER_PROMISC_MSK |
                    RXON_FILTER_CTL2HOST_MSK | RXON_FILTER_ACCEPT_GRP_MSK;
                break;
        default:
                IWL_ERROR("Unsupported interface type %d\n", priv->iw_mode);
                break;
        }

#if 0
        /* TODO:  Figure out when short_preamble would be set and cache from
         * that */
        if (!hw_to_local(priv->hw)->short_preamble)
                priv->staging_rxon.flags &= ~RXON_FLG_SHORT_PREAMBLE_MSK;
        else
                priv->staging_rxon.flags |= RXON_FLG_SHORT_PREAMBLE_MSK;
#endif

        ch_info = iwl3945_get_channel_info(priv, priv->band,
                                       le16_to_cpu(priv->active_rxon.channel));

        if (!ch_info)
                ch_info = &priv->channel_info[0];

        /*
         * in some case A channels are all non IBSS
         * in this case force B/G channel
         */
        if ((priv->iw_mode == IEEE80211_IF_TYPE_IBSS) &&
            !(is_channel_ibss(ch_info)))
                ch_info = &priv->channel_info[0];

        priv->staging_rxon.channel = cpu_to_le16(ch_info->channel);
        if (is_channel_a_band(ch_info))
                priv->band = IEEE80211_BAND_5GHZ;
        else
                priv->band = IEEE80211_BAND_2GHZ;

        iwl3945_set_flags_for_phymode(priv, priv->band);

        priv->staging_rxon.ofdm_basic_rates =
            (IWL_OFDM_RATES_MASK >> IWL_FIRST_OFDM_RATE) & 0xFF;
        priv->staging_rxon.cck_basic_rates =
            (IWL_CCK_RATES_MASK >> IWL_FIRST_CCK_RATE) & 0xF;
}

static int iwl3945_set_mode(struct iwl3945_priv *priv, int mode)
{
        if (mode == IEEE80211_IF_TYPE_IBSS) {
                const struct iwl3945_channel_info *ch_info;

                ch_info = iwl3945_get_channel_info(priv,
                        priv->band,
                        le16_to_cpu(priv->staging_rxon.channel));

                if (!ch_info || !is_channel_ibss(ch_info)) {
                        IWL_ERROR("channel %d not IBSS channel\n",
                                  le16_to_cpu(priv->staging_rxon.channel));
                        return -EINVAL;
                }
        }

        priv->iw_mode = mode;

        iwl3945_connection_init_rx_config(priv);
        memcpy(priv->staging_rxon.node_addr, priv->mac_addr, ETH_ALEN);

        iwl3945_clear_stations_table(priv);

        /* dont commit rxon if rf-kill is on*/
        if (!iwl3945_is_ready_rf(priv))
                return -EAGAIN;

        cancel_delayed_work(&priv->scan_check);
        if (iwl3945_scan_cancel_timeout(priv, 100)) {
                IWL_WARNING("Aborted scan still in progress after 100ms\n");
                IWL_DEBUG_MAC80211("leaving - scan abort failed.\n");
                return -EAGAIN;
        }

        iwl3945_commit_rxon(priv);

        return 0;
}

static void iwl3945_build_tx_cmd_hwcrypto(struct iwl3945_priv *priv,
                                      struct ieee80211_tx_info *info,
                                      struct iwl3945_cmd *cmd,
                                      struct sk_buff *skb_frag,
                                      int last_frag)
{
        struct iwl3945_hw_key *keyinfo =
            &priv->stations[info->control.hw_key->hw_key_idx].keyinfo;

        switch (keyinfo->alg) {
        case ALG_CCMP:
                cmd->cmd.tx.sec_ctl = TX_CMD_SEC_CCM;
                memcpy(cmd->cmd.tx.key, keyinfo->key, keyinfo->keylen);
                IWL_DEBUG_TX("tx_cmd with aes hwcrypto\n");
                break;

        case ALG_TKIP:
#if 0
                cmd->cmd.tx.sec_ctl = TX_CMD_SEC_TKIP;

                if (last_frag)
                        memcpy(cmd->cmd.tx.tkip_mic.byte, skb_frag->tail - 8,
                               8);
                else
                        memset(cmd->cmd.tx.tkip_mic.byte, 0, 8);
#endif
                break;

        case ALG_WEP:
                cmd->cmd.tx.sec_ctl = TX_CMD_SEC_WEP |
                    (info->control.hw_key->hw_key_idx & TX_CMD_SEC_MSK) << TX_CMD_SEC_SHIFT;

                if (keyinfo->keylen == 13)
                        cmd->cmd.tx.sec_ctl |= TX_CMD_SEC_KEY128;

                memcpy(&cmd->cmd.tx.key[3], keyinfo->key, keyinfo->keylen);

                IWL_DEBUG_TX("Configuring packet for WEP encryption "
                             "with key %d\n", info->control.hw_key->hw_key_idx);
                break;

        default:
                printk(KERN_ERR "Unknown encode alg %d\n", keyinfo->alg);
                break;
        }
}

/*
 * handle build REPLY_TX command notification.
 */
static void iwl3945_build_tx_cmd_basic(struct iwl3945_priv *priv,
                                  struct iwl3945_cmd *cmd,
                                  struct ieee80211_tx_info *info,
                                  struct ieee80211_hdr *hdr,
                                  int is_unicast, u8 std_id)
{
        __le16 fc = hdr->frame_control;
        __le32 tx_flags = cmd->cmd.tx.tx_flags;

        cmd->cmd.tx.stop_time.life_time = TX_CMD_LIFE_TIME_INFINITE;
        if (!(info->flags & IEEE80211_TX_CTL_NO_ACK)) {
                tx_flags |= TX_CMD_FLG_ACK_MSK;
                if (ieee80211_is_mgmt(fc))
                        tx_flags |= TX_CMD_FLG_SEQ_CTL_MSK;
                if (ieee80211_is_probe_resp(fc) &&
                    !(le16_to_cpu(hdr->seq_ctrl) & 0xf))
                        tx_flags |= TX_CMD_FLG_TSF_MSK;
        } else {
                tx_flags &= (~TX_CMD_FLG_ACK_MSK);
                tx_flags |= TX_CMD_FLG_SEQ_CTL_MSK;
        }

        cmd->cmd.tx.sta_id = std_id;
        if (ieee80211_has_morefrags(fc))
                tx_flags |= TX_CMD_FLG_MORE_FRAG_MSK;

        if (ieee80211_is_data_qos(fc)) {
                u8 *qc = ieee80211_get_qos_ctl(hdr);
                cmd->cmd.tx.tid_tspec = qc[0] & 0xf;
                tx_flags &= ~TX_CMD_FLG_SEQ_CTL_MSK;
        } else {
                tx_flags |= TX_CMD_FLG_SEQ_CTL_MSK;
        }

        if (info->flags & IEEE80211_TX_CTL_USE_RTS_CTS) {
                tx_flags |= TX_CMD_FLG_RTS_MSK;
                tx_flags &= ~TX_CMD_FLG_CTS_MSK;
        } else if (info->flags & IEEE80211_TX_CTL_USE_CTS_PROTECT) {
                tx_flags &= ~TX_CMD_FLG_RTS_MSK;
                tx_flags |= TX_CMD_FLG_CTS_MSK;
        }

        if ((tx_flags & TX_CMD_FLG_RTS_MSK) || (tx_flags & TX_CMD_FLG_CTS_MSK))
                tx_flags |= TX_CMD_FLG_FULL_TXOP_PROT_MSK;

        tx_flags &= ~(TX_CMD_FLG_ANT_SEL_MSK);
        if (ieee80211_is_mgmt(fc)) {
                if (ieee80211_is_assoc_req(fc) || ieee80211_is_reassoc_req(fc))
                        cmd->cmd.tx.timeout.pm_frame_timeout = cpu_to_le16(3);
                else
                        cmd->cmd.tx.timeout.pm_frame_timeout = cpu_to_le16(2);
        } else {
                cmd->cmd.tx.timeout.pm_frame_timeout = 0;
#ifdef CONFIG_IWL3945_LEDS
                priv->rxtxpackets += le16_to_cpu(cmd->cmd.tx.len);
#endif
        }

        cmd->cmd.tx.driver_txop = 0;
        cmd->cmd.tx.tx_flags = tx_flags;
        cmd->cmd.tx.next_frame_len = 0;
}

/**
 * iwl3945_get_sta_id - Find station's index within station table
 */
static int iwl3945_get_sta_id(struct iwl3945_priv *priv, struct ieee80211_hdr *hdr)
{
        int sta_id;
        u16 fc = le16_to_cpu(hdr->frame_control);

        /* If this frame is broadcast or management, use broadcast station id */
        if (((fc & IEEE80211_FCTL_FTYPE) != IEEE80211_FTYPE_DATA) ||
            is_multicast_ether_addr(hdr->addr1))
                return priv->hw_setting.bcast_sta_id;

        switch (priv->iw_mode) {

        /* If we are a client station in a BSS network, use the special
         * AP station entry (that's the only station we communicate with) */
        case IEEE80211_IF_TYPE_STA:
                return IWL_AP_ID;

        /* If we are an AP, then find the station, or use BCAST */
        case IEEE80211_IF_TYPE_AP:
                sta_id = iwl3945_hw_find_station(priv, hdr->addr1);
                if (sta_id != IWL_INVALID_STATION)
                        return sta_id;
                return priv->hw_setting.bcast_sta_id;

        /* If this frame is going out to an IBSS network, find the station,
         * or create a new station table entry */
        case IEEE80211_IF_TYPE_IBSS: {
                DECLARE_MAC_BUF(mac);

                /* Create new station table entry */
                sta_id = iwl3945_hw_find_station(priv, hdr->addr1);
                if (sta_id != IWL_INVALID_STATION)
                        return sta_id;

                sta_id = iwl3945_add_station(priv, hdr->addr1, 0, CMD_ASYNC);

                if (sta_id != IWL_INVALID_STATION)
                        return sta_id;

                IWL_DEBUG_DROP("Station %s not in station map. "
                               "Defaulting to broadcast...\n",
                               print_mac(mac, hdr->addr1));
                iwl3945_print_hex_dump(IWL_DL_DROP, (u8 *) hdr, sizeof(*hdr));
                return priv->hw_setting.bcast_sta_id;
        }
        /* If we are in monitor mode, use BCAST. This is required for
         * packet injection. */
        case IEEE80211_IF_TYPE_MNTR:
                return priv->hw_setting.bcast_sta_id;

        default:
                IWL_WARNING("Unknown mode of operation: %d\n", priv->iw_mode);
                return priv->hw_setting.bcast_sta_id;
        }
}

/*
 * start REPLY_TX command process
 */
static int iwl3945_tx_skb(struct iwl3945_priv *priv, struct sk_buff *skb)
{
        struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
        struct iwl3945_tfd_frame *tfd;
        u32 *control_flags;
        int txq_id = skb_get_queue_mapping(skb);
        struct iwl3945_tx_queue *txq = NULL;
        struct iwl3945_queue *q = NULL;
        dma_addr_t phys_addr;
        dma_addr_t txcmd_phys;
        struct iwl3945_cmd *out_cmd = NULL;
        u16 len, idx, len_org, hdr_len;
        u8 id;
        u8 unicast;
        u8 sta_id;
        u8 tid = 0;
        u16 seq_number = 0;
        __le16 fc;
        u8 wait_write_ptr = 0;
        u8 *qc = NULL;
        unsigned long flags;
        int rc;

        spin_lock_irqsave(&priv->lock, flags);
        if (iwl3945_is_rfkill(priv)) {
                IWL_DEBUG_DROP("Dropping - RF KILL\n");
                goto drop_unlock;
        }

        if ((ieee80211_get_tx_rate(priv->hw, info)->hw_value & 0xFF) == IWL_INVALID_RATE) {
                IWL_ERROR("ERROR: No TX rate available.\n");
                goto drop_unlock;
        }

        unicast = !is_multicast_ether_addr(hdr->addr1);
        id = 0;

        fc = hdr->frame_control;

#ifdef CONFIG_IWL3945_DEBUG
        if (ieee80211_is_auth(fc))
                IWL_DEBUG_TX("Sending AUTH frame\n");
        else if (ieee80211_is_assoc_req(fc))
                IWL_DEBUG_TX("Sending ASSOC frame\n");
        else if (ieee80211_is_reassoc_req(fc))
                IWL_DEBUG_TX("Sending REASSOC frame\n");
#endif

        /* drop all data frame if we are not associated */
        if (ieee80211_is_data(fc) &&
            (priv->iw_mode != IEEE80211_IF_TYPE_MNTR) && /* packet injection */
            (!iwl3945_is_associated(priv) ||
             ((priv->iw_mode == IEEE80211_IF_TYPE_STA) && !priv->assoc_id))) {
                IWL_DEBUG_DROP("Dropping - !iwl3945_is_associated\n");
                goto drop_unlock;
        }

        spin_unlock_irqrestore(&priv->lock, flags);

        hdr_len = ieee80211_get_hdrlen(le16_to_cpu(fc));

        /* Find (or create) index into station table for destination station */
        sta_id = iwl3945_get_sta_id(priv, hdr);
        if (sta_id == IWL_INVALID_STATION) {
                DECLARE_MAC_BUF(mac);

                IWL_DEBUG_DROP("Dropping - INVALID STATION: %s\n",
                               print_mac(mac, hdr->addr1));
                goto drop;
        }

        IWL_DEBUG_RATE("station Id %d\n", sta_id);

        if (ieee80211_is_data_qos(fc)) {
                qc = ieee80211_get_qos_ctl(hdr);
                tid = qc[0] & 0xf;
                seq_number = priv->stations[sta_id].tid[tid].seq_number &
                                IEEE80211_SCTL_SEQ;
                hdr->seq_ctrl = cpu_to_le16(seq_number) |
                        (hdr->seq_ctrl &
                                __constant_cpu_to_le16(IEEE80211_SCTL_FRAG));
                seq_number += 0x10;
        }

        /* Descriptor for chosen Tx queue */
        txq = &priv->txq[txq_id];
        q = &txq->q;

        spin_lock_irqsave(&priv->lock, flags);

        /* Set up first empty TFD within this queue's circular TFD buffer */
        tfd = &txq->bd[q->write_ptr];
        memset(tfd, 0, sizeof(*tfd));
        control_flags = (u32 *) tfd;
        idx = get_cmd_index(q, q->write_ptr, 0);

        /* Set up driver data for this TFD */
        memset(&(txq->txb[q->write_ptr]), 0, sizeof(struct iwl3945_tx_info));
        txq->txb[q->write_ptr].skb[0] = skb;

        /* Init first empty entry in queue's array of Tx/cmd buffers */
        out_cmd = &txq->cmd[idx];
        memset(&out_cmd->hdr, 0, sizeof(out_cmd->hdr));
        memset(&out_cmd->cmd.tx, 0, sizeof(out_cmd->cmd.tx));

        /*
         * Set up the Tx-command (not MAC!) header.
         * Store the chosen Tx queue and TFD index within the sequence field;
         * after Tx, uCode's Tx response will return this value so driver can
         * locate the frame within the tx queue and do post-tx processing.
         */
        out_cmd->hdr.cmd = REPLY_TX;
        out_cmd->hdr.sequence = cpu_to_le16((u16)(QUEUE_TO_SEQ(txq_id) |
                                INDEX_TO_SEQ(q->write_ptr)));

        /* Copy MAC header from skb into command buffer */
        memcpy(out_cmd->cmd.tx.hdr, hdr, hdr_len);

        /*
         * Use the first empty entry in this queue's command buffer array
         * to contain the Tx command and MAC header concatenated together
         * (payload data will be in another buffer).
         * Size of this varies, due to varying MAC header length.
         * If end is not dword aligned, we'll have 2 extra bytes at the end
         * of the MAC header (device reads on dword boundaries).
         * We'll tell device about this padding later.
         */
        len = priv->hw_setting.tx_cmd_len +
                sizeof(struct iwl3945_cmd_header) + hdr_len;

        len_org = len;
        len = (len + 3) & ~3;

        if (len_org != len)
                len_org = 1;
        else
                len_org = 0;

        /* Physical address of this Tx command's header (not MAC header!),
         * within command buffer array. */
        txcmd_phys = txq->dma_addr_cmd + sizeof(struct iwl3945_cmd) * idx +
                     offsetof(struct iwl3945_cmd, hdr);

        /* Add buffer containing Tx command and MAC(!) header to TFD's
         * first entry */
        iwl3945_hw_txq_attach_buf_to_tfd(priv, tfd, txcmd_phys, len);

        if (info->control.hw_key)
                iwl3945_build_tx_cmd_hwcrypto(priv, info, out_cmd, skb, 0);

        /* Set up TFD's 2nd entry to point directly to remainder of skb,
         * if any (802.11 null frames have no payload). */
        len = skb->len - hdr_len;
        if (len) {
                phys_addr = pci_map_single(priv->pci_dev, skb->data + hdr_len,
                                           len, PCI_DMA_TODEVICE);
                iwl3945_hw_txq_attach_buf_to_tfd(priv, tfd, phys_addr, len);
        }

        if (!len)
                /* If there is no payload, then we use only one Tx buffer */
                *control_flags = TFD_CTL_COUNT_SET(1);
        else
                /* Else use 2 buffers.
                 * Tell 3945 about any padding after MAC header */
                *control_flags = TFD_CTL_COUNT_SET(2) |
                        TFD_CTL_PAD_SET(U32_PAD(len));

        /* Total # bytes to be transmitted */
        len = (u16)skb->len;
        out_cmd->cmd.tx.len = cpu_to_le16(len);

        /* TODO need this for burst mode later on */
        iwl3945_build_tx_cmd_basic(priv, out_cmd, info, hdr, unicast, sta_id);

        /* set is_hcca to 0; it probably will never be implemented */
        iwl3945_hw_build_tx_cmd_rate(priv, out_cmd, info, hdr, sta_id, 0);

        out_cmd->cmd.tx.tx_flags &= ~TX_CMD_FLG_ANT_A_MSK;
        out_cmd->cmd.tx.tx_flags &= ~TX_CMD_FLG_ANT_B_MSK;

        if (!ieee80211_has_morefrags(hdr->frame_control)) {
                txq->need_update = 1;
                if (qc)
                        priv->stations[sta_id].tid[tid].seq_number = seq_number;
        } else {
                wait_write_ptr = 1;
                txq->need_update = 0;
        }

        iwl3945_print_hex_dump(IWL_DL_TX, out_cmd->cmd.payload,
                           sizeof(out_cmd->cmd.tx));

        iwl3945_print_hex_dump(IWL_DL_TX, (u8 *)out_cmd->cmd.tx.hdr,
                           ieee80211_get_hdrlen(le16_to_cpu(fc)));

        /* Tell device the write index *just past* this latest filled TFD */
        q->write_ptr = iwl_queue_inc_wrap(q->write_ptr, q->n_bd);
        rc = iwl3945_tx_queue_update_write_ptr(priv, txq);
        spin_unlock_irqrestore(&priv->lock, flags);

        if (rc)
                return rc;

        if ((iwl3945_queue_space(q) < q->high_mark)
            && priv->mac80211_registered) {
                if (wait_write_ptr) {
                        spin_lock_irqsave(&priv->lock, flags);
                        txq->need_update = 1;
                        iwl3945_tx_queue_update_write_ptr(priv, txq);
                        spin_unlock_irqrestore(&priv->lock, flags);
                }

                ieee80211_stop_queue(priv->hw, skb_get_queue_mapping(skb));
        }

        return 0;

drop_unlock:
        spin_unlock_irqrestore(&priv->lock, flags);
drop:
        return -1;
}

static void iwl3945_set_rate(struct iwl3945_priv *priv)
{
        const struct ieee80211_supported_band *sband = NULL;
        struct ieee80211_rate *rate;
        int i;

        sband = iwl3945_get_band(priv, priv->band);
        if (!sband) {
                IWL_ERROR("Failed to set rate: unable to get hw mode\n");
                return;
        }

        priv->active_rate = 0;
        priv->active_rate_basic = 0;

        IWL_DEBUG_RATE("Setting rates for %s GHz\n",
                       sband->band == IEEE80211_BAND_2GHZ ? "2.4" : "5");

        for (i = 0; i < sband->n_bitrates; i++) {
                rate = &sband->bitrates[i];
                if ((rate->hw_value < IWL_RATE_COUNT) &&
                    !(rate->flags & IEEE80211_CHAN_DISABLED)) {
                        IWL_DEBUG_RATE("Adding rate index %d (plcp %d)\n",
                                       rate->hw_value, iwl3945_rates[rate->hw_value].plcp);
                        priv->active_rate |= (1 << rate->hw_value);
                }
        }

        IWL_DEBUG_RATE("Set active_rate = %0x, active_rate_basic = %0x\n",
                       priv->active_rate, priv->active_rate_basic);

        /*
         * If a basic rate is configured, then use it (adding IWL_RATE_1M_MASK)
         * otherwise set it to the default of all CCK rates and 6, 12, 24 for
         * OFDM
         */
        if (priv->active_rate_basic & IWL_CCK_BASIC_RATES_MASK)
                priv->staging_rxon.cck_basic_rates =
                    ((priv->active_rate_basic &
                      IWL_CCK_RATES_MASK) >> IWL_FIRST_CCK_RATE) & 0xF;
        else
                priv->staging_rxon.cck_basic_rates =
                    (IWL_CCK_BASIC_RATES_MASK >> IWL_FIRST_CCK_RATE) & 0xF;

        if (priv->active_rate_basic & IWL_OFDM_BASIC_RATES_MASK)
                priv->staging_rxon.ofdm_basic_rates =
                    ((priv->active_rate_basic &
                      (IWL_OFDM_BASIC_RATES_MASK | IWL_RATE_6M_MASK)) >>
                      IWL_FIRST_OFDM_RATE) & 0xFF;
        else
                priv->staging_rxon.ofdm_basic_rates =
                   (IWL_OFDM_BASIC_RATES_MASK >> IWL_FIRST_OFDM_RATE) & 0xFF;
}

static void iwl3945_radio_kill_sw(struct iwl3945_priv *priv, int disable_radio)
{
        unsigned long flags;

        if (!!disable_radio == test_bit(STATUS_RF_KILL_SW, &priv->status))
                return;

        IWL_DEBUG_RF_KILL("Manual SW RF KILL set to: RADIO %s\n",
                          disable_radio ? "OFF" : "ON");

        if (disable_radio) {
                iwl3945_scan_cancel(priv);
                /* FIXME: This is a workaround for AP */
                if (priv->iw_mode != IEEE80211_IF_TYPE_AP) {
                        spin_lock_irqsave(&priv->lock, flags);
                        iwl3945_write32(priv, CSR_UCODE_DRV_GP1_SET,
                                    CSR_UCODE_SW_BIT_RFKILL);
                        spin_unlock_irqrestore(&priv->lock, flags);
                        iwl3945_send_card_state(priv, CARD_STATE_CMD_DISABLE, 0);
                        set_bit(STATUS_RF_KILL_SW, &priv->status);
                }
                return;
        }

        spin_lock_irqsave(&priv->lock, flags);
        iwl3945_write32(priv, CSR_UCODE_DRV_GP1_CLR, CSR_UCODE_SW_BIT_RFKILL);

        clear_bit(STATUS_RF_KILL_SW, &priv->status);
        spin_unlock_irqrestore(&priv->lock, flags);

        /* wake up ucode */
        msleep(10);

        spin_lock_irqsave(&priv->lock, flags);
        iwl3945_read32(priv, CSR_UCODE_DRV_GP1);
        if (!iwl3945_grab_nic_access(priv))
                iwl3945_release_nic_access(priv);
        spin_unlock_irqrestore(&priv->lock, flags);

        if (test_bit(STATUS_RF_KILL_HW, &priv->status)) {
                IWL_DEBUG_RF_KILL("Can not turn radio back on - "
                                  "disabled by HW switch\n");
                return;
        }

        if (priv->is_open)
                queue_work(priv->workqueue, &priv->restart);
        return;
}

void iwl3945_set_decrypted_flag(struct iwl3945_priv *priv, struct sk_buff *skb,
                            u32 decrypt_res, struct ieee80211_rx_status *stats)
{
        u16 fc =
            le16_to_cpu(((struct ieee80211_hdr *)skb->data)->frame_control);

        if (priv->active_rxon.filter_flags & RXON_FILTER_DIS_DECRYPT_MSK)
                return;

        if (!(fc & IEEE80211_FCTL_PROTECTED))
                return;

        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:
                if ((decrypt_res & RX_RES_STATUS_DECRYPT_TYPE_MSK) ==
                    RX_RES_STATUS_BAD_ICV_MIC)
                        stats->flag |= RX_FLAG_MMIC_ERROR;
        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;
        }
}

#ifdef CONFIG_IWL3945_SPECTRUM_MEASUREMENT

#include "iwl-spectrum.h"

#define BEACON_TIME_MASK_LOW    0x00FFFFFF
#define BEACON_TIME_MASK_HIGH   0xFF000000
#define TIME_UNIT               1024

/*
 * extended beacon time format
 * time in usec will be changed into a 32-bit value in 8:24 format
 * the high 1 byte is the beacon counts
 * the lower 3 bytes is the time in usec within one beacon interval
 */

static u32 iwl3945_usecs_to_beacons(u32 usec, u32 beacon_interval)
{
        u32 quot;
        u32 rem;
        u32 interval = beacon_interval * 1024;

        if (!interval || !usec)
                return 0;

        quot = (usec / interval) & (BEACON_TIME_MASK_HIGH >> 24);
        rem = (usec % interval) & BEACON_TIME_MASK_LOW;

        return (quot << 24) + rem;
}

/* base is usually what we get from ucode with each received frame,
 * the same as HW timer counter counting down
 */

static __le32 iwl3945_add_beacon_time(u32 base, u32 addon, u32 beacon_interval)
{
        u32 base_low = base & BEACON_TIME_MASK_LOW;
        u32 addon_low = addon & BEACON_TIME_MASK_LOW;
        u32 interval = beacon_interval * TIME_UNIT;
        u32 res = (base & BEACON_TIME_MASK_HIGH) +
            (addon & BEACON_TIME_MASK_HIGH);

        if (base_low > addon_low)
                res += base_low - addon_low;
        else if (base_low < addon_low) {
                res += interval + base_low - addon_low;
                res += (1 << 24);
        } else
                res += (1 << 24);

        return cpu_to_le32(res);
}

static int iwl3945_get_measurement(struct iwl3945_priv *priv,
                               struct ieee80211_measurement_params *params,
                               u8 type)
{
        struct iwl3945_spectrum_cmd spectrum;
        struct iwl3945_rx_packet *res;
        struct iwl3945_host_cmd cmd = {
                .id = REPLY_SPECTRUM_MEASUREMENT_CMD,
                .data = (void *)&spectrum,
                .meta.flags = CMD_WANT_SKB,
        };
        u32 add_time = le64_to_cpu(params->start_time);
        int rc;
        int spectrum_resp_status;
        int duration = le16_to_cpu(params->duration);

        if (iwl3945_is_associated(priv))
                add_time =
                    iwl3945_usecs_to_beacons(
                        le64_to_cpu(params->start_time) - priv->last_tsf,
                        le16_to_cpu(priv->rxon_timing.beacon_interval));

        memset(&spectrum, 0, sizeof(spectrum));

        spectrum.channel_count = cpu_to_le16(1);
        spectrum.flags =
            RXON_FLG_TSF2HOST_MSK | RXON_FLG_ANT_A_MSK | RXON_FLG_DIS_DIV_MSK;
        spectrum.filter_flags = MEASUREMENT_FILTER_FLAG;
        cmd.len = sizeof(spectrum);
        spectrum.len = cpu_to_le16(cmd.len - sizeof(spectrum.len));

        if (iwl3945_is_associated(priv))
                spectrum.start_time =
                    iwl3945_add_beacon_time(priv->last_beacon_time,
                                add_time,
                                le16_to_cpu(priv->rxon_timing.beacon_interval));
        else
                spectrum.start_time = 0;

        spectrum.channels[0].duration = cpu_to_le32(duration * TIME_UNIT);
        spectrum.channels[0].channel = params->channel;
        spectrum.channels[0].type = type;
        if (priv->active_rxon.flags & RXON_FLG_BAND_24G_MSK)
                spectrum.flags |= RXON_FLG_BAND_24G_MSK |
                    RXON_FLG_AUTO_DETECT_MSK | RXON_FLG_TGG_PROTECT_MSK;

        rc = iwl3945_send_cmd_sync(priv, &cmd);
        if (rc)
                return rc;

        res = (struct iwl3945_rx_packet *)cmd.meta.u.skb->data;
        if (res->hdr.flags & IWL_CMD_FAILED_MSK) {
                IWL_ERROR("Bad return from REPLY_RX_ON_ASSOC command\n");
                rc = -EIO;
        }

        spectrum_resp_status = le16_to_cpu(res->u.spectrum.status);
        switch (spectrum_resp_status) {
        case 0:         /* Command will be handled */
                if (res->u.spectrum.id != 0xff) {
                        IWL_DEBUG_INFO("Replaced existing measurement: %d\n",
                                                res->u.spectrum.id);
                        priv->measurement_status &= ~MEASUREMENT_READY;
                }
                priv->measurement_status |= MEASUREMENT_ACTIVE;
                rc = 0;
                break;

        case 1:         /* Command will not be handled */
                rc = -EAGAIN;