11 files changed, 686 insertions, 30 deletions

diff --git a/drivers/net/wireless/rt2x00/Kconfig b/drivers/net/wireless/rt2x00/Kconfig
index d485a86bba75..2f60e175f039 100644
--- a/drivers/net/wireless/rt2x00/Kconfig
+++ b/drivers/net/wireless/rt2x00/Kconfig
@@ -33,6 +33,10 @@ config RT2X00_LIB_FIRMWARE
         depends on RT2X00_LIB
         select FW_LOADER
 
+config RT2X00_LIB_CRYPTO
+        boolean
+        depends on RT2X00_LIB
+
 config RT2X00_LIB_RFKILL
         boolean
         depends on RT2X00_LIB
diff --git a/drivers/net/wireless/rt2x00/Makefile b/drivers/net/wireless/rt2x00/Makefile
index 1087dbcf1a04..917cb4f3b038 100644
--- a/drivers/net/wireless/rt2x00/Makefile
+++ b/drivers/net/wireless/rt2x00/Makefile
@@ -3,6 +3,7 @@ rt2x00lib-y				+= rt2x00mac.o
 rt2x00lib-y                             += rt2x00config.o
 rt2x00lib-y                             += rt2x00queue.o
 rt2x00lib-$(CONFIG_RT2X00_LIB_DEBUGFS)  += rt2x00debug.o
+rt2x00lib-$(CONFIG_RT2X00_LIB_CRYPTO)   += rt2x00crypto.o
 rt2x00lib-$(CONFIG_RT2X00_LIB_RFKILL)   += rt2x00rfkill.o
 rt2x00lib-$(CONFIG_RT2X00_LIB_FIRMWARE) += rt2x00firmware.o
 rt2x00lib-$(CONFIG_RT2X00_LIB_LEDS)     += rt2x00leds.o
diff --git a/drivers/net/wireless/rt2x00/rt2x00.h b/drivers/net/wireless/rt2x00/rt2x00.h
index 82c28dcc471d..0ffd972bb856 100644
--- a/drivers/net/wireless/rt2x00/rt2x00.h
+++ b/drivers/net/wireless/rt2x00/rt2x00.h
@@ -452,6 +452,23 @@ struct rt2x00lib_erp {
 };
 
 /*
+ * Configuration structure for hardware encryption.
+ */
+struct rt2x00lib_crypto {
+        enum cipher cipher;
+
+        enum set_key_cmd cmd;
+        const u8 *address;
+
+        u32 bssidx;
+        u32 aid;
+
+        u8 key[16];
+        u8 tx_mic[8];
+        u8 rx_mic[8];
+};
+
+/*
  * Configuration structure wrapper around the
  * rt2x00 interface configuration handler.
  */
@@ -547,6 +564,12 @@ struct rt2x00lib_ops {
         /*
          * Configuration handlers.
          */
+        int (*config_shared_key) (struct rt2x00_dev *rt2x00dev,
+                                  struct rt2x00lib_crypto *crypto,
+                                  struct ieee80211_key_conf *key);
+        int (*config_pairwise_key) (struct rt2x00_dev *rt2x00dev,
+                                    struct rt2x00lib_crypto *crypto,
+                                    struct ieee80211_key_conf *key);
         void (*config_filter) (struct rt2x00_dev *rt2x00dev,
                                const unsigned int filter_flags);
         void (*config_intf) (struct rt2x00_dev *rt2x00dev,
@@ -609,7 +632,7 @@ enum rt2x00_flags {
         DEVICE_DIRTY_CONFIG,
 
         /*
-         * Driver features
+         * Driver requirements
          */
         DRIVER_REQUIRE_FIRMWARE,
         DRIVER_REQUIRE_BEACON_GUARD,
@@ -618,9 +641,14 @@ enum rt2x00_flags {
         DRIVER_REQUIRE_DMA,
 
         /*
-         * Driver configuration
+         * Driver features
          */
         CONFIG_SUPPORT_HW_BUTTON,
+        CONFIG_SUPPORT_HW_CRYPTO,
+
+        /*
+         * Driver configuration
+         */
         CONFIG_FRAME_TYPE,
         CONFIG_RF_SEQUENCE,
         CONFIG_EXTERNAL_LNA_A,
@@ -966,6 +994,13 @@ void rt2x00mac_configure_filter(struct ieee80211_hw *hw,
                                 unsigned int changed_flags,
                                 unsigned int *total_flags,
                                 int mc_count, struct dev_addr_list *mc_list);
+#ifdef CONFIG_RT2X00_LIB_CRYPTO
+int rt2x00mac_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
+                      const u8 *local_address, const u8 *address,
+                      struct ieee80211_key_conf *key);
+#else
+#define rt2x00mac_set_key       NULL
+#endif /* CONFIG_RT2X00_LIB_CRYPTO */
 int rt2x00mac_get_stats(struct ieee80211_hw *hw,
                         struct ieee80211_low_level_stats *stats);
 int rt2x00mac_get_tx_stats(struct ieee80211_hw *hw,
diff --git a/drivers/net/wireless/rt2x00/rt2x00crypto.c b/drivers/net/wireless/rt2x00/rt2x00crypto.c
new file mode 100644
index 000000000000..e1448cfa9444
--- /dev/null
+++ b/drivers/net/wireless/rt2x00/rt2x00crypto.c
@@ -0,0 +1,215 @@
+/*
+        Copyright (C) 2004 - 2008 rt2x00 SourceForge Project
+        <http://rt2x00.serialmonkey.com>
+
+        This program is free software; you can redistribute it and/or modify
+        it under the terms of the GNU General Public License as published by
+        the Free Software Foundation; either version 2 of the License, or
+        (at your option) any later version.
+
+        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.,
+        59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+/*
+        Module: rt2x00lib
+        Abstract: rt2x00 crypto specific routines.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+
+#include "rt2x00.h"
+#include "rt2x00lib.h"
+
+enum cipher rt2x00crypto_key_to_cipher(struct ieee80211_key_conf *key)
+{
+        switch (key->alg) {
+        case ALG_WEP:
+                if (key->keylen == LEN_WEP40)
+                        return CIPHER_WEP64;
+                else
+                        return CIPHER_WEP128;
+        case ALG_TKIP:
+                return CIPHER_TKIP;
+        case ALG_CCMP:
+                return CIPHER_AES;
+        default:
+                return CIPHER_NONE;
+        }
+}
+
+unsigned int rt2x00crypto_tx_overhead(struct ieee80211_tx_info *tx_info)
+{
+        struct ieee80211_key_conf *key = tx_info->control.hw_key;
+        unsigned int overhead = 0;
+
+        /*
+         * Extend frame length to include IV/EIV/ICV/MMIC,
+         * note that these lengths should only be added when
+         * mac80211 does not generate it.
+         */
+        overhead += tx_info->control.icv_len;
+
+        if (!(key->flags & IEEE80211_KEY_FLAG_GENERATE_IV))
+                overhead += tx_info->control.iv_len;
+
+        if (!(key->flags & IEEE80211_KEY_FLAG_GENERATE_MMIC)) {
+                if (key->alg == ALG_TKIP)
+                        overhead += 8;
+        }
+
+        return overhead;
+}
+
+void rt2x00crypto_tx_remove_iv(struct sk_buff *skb, unsigned int iv_len)
+{
+        struct skb_frame_desc *skbdesc = get_skb_frame_desc(skb);
+        unsigned int header_length = ieee80211_get_hdrlen_from_skb(skb);
+
+        if (unlikely(!iv_len))
+                return;
+
+        /* Copy IV/EIV data */
+        if (iv_len >= 4)
+                memcpy(&skbdesc->iv, skb->data + header_length, 4);
+        if (iv_len >= 8)
+                memcpy(&skbdesc->eiv, skb->data + header_length + 4, 4);
+
+        /* Move ieee80211 header */
+        memmove(skb->data + iv_len, skb->data, header_length);
+
+        /* Pull buffer to correct size */
+        skb_pull(skb, iv_len);
+
+        /* IV/EIV data has officially be stripped */
+        skbdesc->flags |= FRAME_DESC_IV_STRIPPED;
+}
+
+void rt2x00crypto_tx_insert_iv(struct sk_buff *skb)
+{
+        struct skb_frame_desc *skbdesc = get_skb_frame_desc(skb);
+        unsigned int header_length = ieee80211_get_hdrlen_from_skb(skb);
+        const unsigned int iv_len =
+            ((!!(skbdesc->iv)) * 4) + ((!!(skbdesc->eiv)) * 4);
+
+        if (!(skbdesc->flags & FRAME_DESC_IV_STRIPPED))
+                return;
+
+        skb_push(skb, iv_len);
+
+        /* Move ieee80211 header */
+        memmove(skb->data, skb->data + iv_len, header_length);
+
+        /* Copy IV/EIV data */
+        if (iv_len >= 4)
+                memcpy(skb->data + header_length, &skbdesc->iv, 4);
+        if (iv_len >= 8)
+                memcpy(skb->data + header_length + 4, &skbdesc->eiv, 4);
+
+        /* IV/EIV data has returned into the frame */
+        skbdesc->flags &= ~FRAME_DESC_IV_STRIPPED;
+}
+
+void rt2x00crypto_rx_insert_iv(struct sk_buff *skb, unsigned int align,
+                               unsigned int header_length,
+                               struct rxdone_entry_desc *rxdesc)
+{
+        unsigned int payload_len = rxdesc->size - header_length;
+        unsigned int iv_len;
+        unsigned int icv_len;
+        unsigned int transfer = 0;
+
+        /*
+         * WEP64/WEP128: Provides IV & ICV
+         * TKIP: Provides IV/EIV & ICV
+         * AES: Provies IV/EIV & ICV
+         */
+        switch (rxdesc->cipher) {
+        case CIPHER_WEP64:
+        case CIPHER_WEP128:
+                iv_len = 4;
+                icv_len = 4;
+                break;
+        case CIPHER_TKIP:
+                iv_len = 8;
+                icv_len = 4;
+                break;
+        case CIPHER_AES:
+                iv_len = 8;
+                icv_len = 8;
+                break;
+        default:
+                /* Unsupport type */
+                return;
+        }
+
+        /*
+         * Make room for new data, note that we increase both
+         * headsize and tailsize when required. The tailsize is
+         * only needed when ICV data needs to be inserted and
+         * the padding is smaller then the ICV data.
+         * When alignment requirements is greater then the
+         * ICV data we must trim the skb to the correct size
+         * because we need to remove the extra bytes.
+         */
+        skb_push(skb, iv_len + align);
+        if (align < icv_len)
+                skb_put(skb, icv_len - align);
+        else if (align > icv_len)
+                skb_trim(skb, rxdesc->size + iv_len + icv_len);
+
+        /* Move ieee80211 header */
+        memmove(skb->data + transfer,
+                skb->data + transfer + iv_len + align,
+                header_length);
+        transfer += header_length;
+
+        /* Copy IV data */
+        if (iv_len >= 4) {
+                memcpy(skb->data + transfer, &rxdesc->iv, 4);
+                transfer += 4;
+        }
+
+        /* Copy EIV data */
+        if (iv_len >= 8) {
+                memcpy(skb->data + transfer, &rxdesc->eiv, 4);
+                transfer += 4;
+        }
+
+        /* Move payload */
+        if (align) {
+                memmove(skb->data + transfer,
+                        skb->data + transfer + align,
+                        payload_len);
+        }
+
+        /*
+         * NOTE: Always count the payload as transfered,
+         * even when alignment was set to zero. This is required
+         * for determining the correct offset for the ICV data.
+         */
+        transfer += payload_len;
+
+        /* Copy ICV data */
+        if (icv_len >= 4) {
+                memcpy(skb->data + transfer, &rxdesc->icv, 4);
+                /*
+                 * AES appends 8 bytes, we can't fill the upper
+                 * 4 bytes, but mac80211 doesn't care about what
+                 * we provide here anyway and strips it immediately.
+                 */
+                transfer += icv_len;
+        }
+
+        /* IV/EIV/ICV has been inserted into frame */
+        rxdesc->size = transfer;
+        rxdesc->flags &= ~RX_FLAG_IV_STRIPPED;
+}
diff --git a/drivers/net/wireless/rt2x00/rt2x00debug.c b/drivers/net/wireless/rt2x00/rt2x00debug.c
index 6bee1d611bbf..5cf4c859e39d 100644
--- a/drivers/net/wireless/rt2x00/rt2x00debug.c
+++ b/drivers/net/wireless/rt2x00/rt2x00debug.c
@@ -35,6 +35,13 @@
 
 #define MAX_LINE_LENGTH 64
 
+struct rt2x00debug_crypto {
+        unsigned long success;
+        unsigned long icv_error;
+        unsigned long mic_error;
+        unsigned long key_error;
+};
+
 struct rt2x00debug_intf {
         /*
          * Pointer to driver structure where
@@ -63,6 +70,7 @@ struct rt2x00debug_intf {
          *   - queue folder
          *     - frame dump file
          *     - queue stats file
+         *     - crypto stats file
          */
         struct dentry *driver_folder;
         struct dentry *driver_entry;
@@ -80,6 +88,7 @@ struct rt2x00debug_intf {
         struct dentry *queue_folder;
         struct dentry *queue_frame_dump_entry;
         struct dentry *queue_stats_entry;
+        struct dentry *crypto_stats_entry;
 
         /*
          * The frame dump file only allows a single reader,
@@ -98,6 +107,12 @@ struct rt2x00debug_intf {
         wait_queue_head_t frame_dump_waitqueue;
 
         /*
+         * HW crypto statistics.
+         * All statistics are stored seperately per cipher type.
+         */
+        struct rt2x00debug_crypto crypto_stats[CIPHER_MAX];
+
+        /*
          * Driver and chipset files will use a data buffer
          * that has been created in advance. This will simplify
          * the code since we can use the debugfs functions.
@@ -114,6 +129,25 @@ struct rt2x00debug_intf {
         unsigned int offset_rf;
 };
 
+void rt2x00debug_update_crypto(struct rt2x00_dev *rt2x00dev,
+                               enum cipher cipher, enum rx_crypto status)
+{
+        struct rt2x00debug_intf *intf = rt2x00dev->debugfs_intf;
+
+        if (cipher == CIPHER_TKIP_NO_MIC)
+                cipher = CIPHER_TKIP;
+        if (cipher == CIPHER_NONE || cipher > CIPHER_MAX)
+                return;
+
+        /* Remove CIPHER_NONE index */
+        cipher--;
+
+        intf->crypto_stats[cipher].success += (status == RX_CRYPTO_SUCCESS);
+        intf->crypto_stats[cipher].icv_error += (status == RX_CRYPTO_FAIL_ICV);
+        intf->crypto_stats[cipher].mic_error += (status == RX_CRYPTO_FAIL_MIC);
+        intf->crypto_stats[cipher].key_error += (status == RX_CRYPTO_FAIL_KEY);
+}
+
 void rt2x00debug_dump_frame(struct rt2x00_dev *rt2x00dev,
                             enum rt2x00_dump_type type, struct sk_buff *skb)
 {
@@ -327,6 +361,59 @@ static const struct file_operations rt2x00debug_fop_queue_stats = {
         .release        = rt2x00debug_file_release,
 };
 
+#ifdef CONFIG_RT2X00_LIB_CRYPTO
+static ssize_t rt2x00debug_read_crypto_stats(struct file *file,
+                                             char __user *buf,
+                                             size_t length,
+                                             loff_t *offset)
+{
+        struct rt2x00debug_intf *intf = file->private_data;
+        char *name[] = { "WEP64", "WEP128", "TKIP", "AES" };
+        char *data;
+        char *temp;
+        size_t size;
+        unsigned int i;
+
+        if (*offset)
+                return 0;
+
+        data = kzalloc((1 + CIPHER_MAX)* MAX_LINE_LENGTH, GFP_KERNEL);
+        if (!data)
+                return -ENOMEM;
+
+        temp = data;
+        temp += sprintf(data, "cipher\tsuccess\ticv err\tmic err\tkey err\n");
+
+        for (i = 0; i < CIPHER_MAX; i++) {
+                temp += sprintf(temp, "%s\t%lu\t%lu\t%lu\t%lu\n", name[i],
+                                intf->crypto_stats[i].success,
+                                intf->crypto_stats[i].icv_error,
+                                intf->crypto_stats[i].mic_error,
+                                intf->crypto_stats[i].key_error);
+        }
+
+        size = strlen(data);
+        size = min(size, length);
+
+        if (copy_to_user(buf, data, size)) {
+                kfree(data);
+                return -EFAULT;
+        }
+
+        kfree(data);
+
+        *offset += size;
+        return size;
+}
+
+static const struct file_operations rt2x00debug_fop_crypto_stats = {
+        .owner          = THIS_MODULE,
+        .read           = rt2x00debug_read_crypto_stats,
+        .open           = rt2x00debug_file_open,
+        .release        = rt2x00debug_file_release,
+};
+#endif
+
 #define RT2X00DEBUGFS_OPS_READ(__name, __format, __type)        \
 static ssize_t rt2x00debug_read_##__name(struct file *file,     \
                                          char __user *buf,      \
@@ -569,6 +656,13 @@ void rt2x00debug_register(struct rt2x00_dev *rt2x00dev)
             debugfs_create_file("queue", S_IRUSR, intf->queue_folder,
                                 intf, &rt2x00debug_fop_queue_stats);
 
+#ifdef CONFIG_RT2X00_LIB_CRYPTO
+        if (test_bit(CONFIG_SUPPORT_HW_CRYPTO, &rt2x00dev->flags))
+                intf->crypto_stats_entry =
+                    debugfs_create_file("crypto", S_IRUGO, intf->queue_folder,
+                                        intf, &rt2x00debug_fop_crypto_stats);
+#endif
+
         return;
 
 exit:
@@ -587,6 +681,9 @@ void rt2x00debug_deregister(struct rt2x00_dev *rt2x00dev)
 
         skb_queue_purge(&intf->frame_dump_skbqueue);
 
+#ifdef CONFIG_RT2X00_LIB_CRYPTO
+        debugfs_remove(intf->crypto_stats_entry);
+#endif
         debugfs_remove(intf->queue_stats_entry);
         debugfs_remove(intf->queue_frame_dump_entry);
         debugfs_remove(intf->queue_folder);
diff --git a/drivers/net/wireless/rt2x00/rt2x00dev.c b/drivers/net/wireless/rt2x00/rt2x00dev.c
index f42283ad7b02..182952249a19 100644
--- a/drivers/net/wireless/rt2x00/rt2x00dev.c
+++ b/drivers/net/wireless/rt2x00/rt2x00dev.c
@@ -508,6 +508,15 @@ void rt2x00lib_txdone(struct queue_entry *entry,
         rt2x00queue_unmap_skb(rt2x00dev, entry->skb);
 
         /*
+         * If the IV/EIV data was stripped from the frame before it was
+         * passed to the hardware, we should now reinsert it again because
+         * mac80211 will expect the the same data to be present it the
+         * frame as it was passed to us.
+         */
+        if (test_bit(CONFIG_SUPPORT_HW_CRYPTO, &rt2x00dev->flags))
+                rt2x00crypto_tx_insert_iv(entry->skb);
+
+        /*
          * Send frame to debugfs immediately, after this call is completed
          * we are going to overwrite the skb->cb array.
          */
@@ -585,7 +594,7 @@ void rt2x00lib_rxdone(struct rt2x00_dev *rt2x00dev,
         struct ieee80211_supported_band *sband;
         struct ieee80211_hdr *hdr;
         const struct rt2x00_rate *rate;
-        unsigned int header_size;
+        unsigned int header_length;
         unsigned int align;
         unsigned int i;
         int idx = -1;
@@ -613,10 +622,19 @@ void rt2x00lib_rxdone(struct rt2x00_dev *rt2x00dev,
          * The data behind the ieee80211 header must be
          * aligned on a 4 byte boundary.
          */
-        header_size = ieee80211_get_hdrlen_from_skb(entry->skb);
-        align = ((unsigned long)(entry->skb->data + header_size)) & 3;
+        header_length = ieee80211_get_hdrlen_from_skb(entry->skb);
+        align = ((unsigned long)(entry->skb->data + header_length)) & 3;
 
-        if (align) {
+        /*
+         * Hardware might have stripped the IV/EIV/ICV data,
+         * in that case it is possible that the data was
+         * provided seperately (through hardware descriptor)
+         * in which case we should reinsert the data into the frame.
+         */
+        if ((rxdesc.flags & RX_FLAG_IV_STRIPPED)) {
+                rt2x00crypto_rx_insert_iv(entry->skb, align,
+                                          header_length, &rxdesc);
+        } else if (align) {
                 skb_push(entry->skb, align);
                 /* Move entire frame in 1 command */
                 memmove(entry->skb->data, entry->skb->data + align,
@@ -657,6 +675,10 @@ void rt2x00lib_rxdone(struct rt2x00_dev *rt2x00dev,
             (rxdesc.dev_flags & RXDONE_MY_BSS))
                 rt2x00lib_update_link_stats(&rt2x00dev->link, rxdesc.rssi);
 
+        rt2x00debug_update_crypto(rt2x00dev,
+                                  rxdesc.cipher,
+                                  rxdesc.cipher_status);
+
         rt2x00dev->link.qual.rx_success++;
 
         rx_status->mactime = rxdesc.timestamp;
diff --git a/drivers/net/wireless/rt2x00/rt2x00lib.h b/drivers/net/wireless/rt2x00/rt2x00lib.h
index c5fb3a72cf37..7bbc16b1b6c6 100644
--- a/drivers/net/wireless/rt2x00/rt2x00lib.h
+++ b/drivers/net/wireless/rt2x00/rt2x00lib.h
@@ -181,6 +181,8 @@ void rt2x00debug_register(struct rt2x00_dev *rt2x00dev);
 void rt2x00debug_deregister(struct rt2x00_dev *rt2x00dev);
 void rt2x00debug_dump_frame(struct rt2x00_dev *rt2x00dev,
                             enum rt2x00_dump_type type, struct sk_buff *skb);
+void rt2x00debug_update_crypto(struct rt2x00_dev *rt2x00dev,
+                               enum cipher cipher, enum rx_crypto status);
 #else
 static inline void rt2x00debug_register(struct rt2x00_dev *rt2x00dev)
 {
@@ -195,9 +197,54 @@ static inline void rt2x00debug_dump_frame(struct rt2x00_dev *rt2x00dev,
                                           struct sk_buff *skb)
 {
 }
+
+static inline void rt2x00debug_update_crypto(struct rt2x00_dev *rt2x00dev,
+                                             enum cipher cipher,
+                                             enum rx_crypto status)
+{
+}
 #endif /* CONFIG_RT2X00_LIB_DEBUGFS */
 
 /*
+ * Crypto handlers.
+ */
+#ifdef CONFIG_RT2X00_LIB_CRYPTO
+enum cipher rt2x00crypto_key_to_cipher(struct ieee80211_key_conf *key);
+unsigned int rt2x00crypto_tx_overhead(struct ieee80211_tx_info *tx_info);
+void rt2x00crypto_tx_remove_iv(struct sk_buff *skb, unsigned int iv_len);
+void rt2x00crypto_tx_insert_iv(struct sk_buff *skb);
+void rt2x00crypto_rx_insert_iv(struct sk_buff *skb, unsigned int align,
+                               unsigned int header_length,
+                               struct rxdone_entry_desc *rxdesc);
+#else
+static inline enum cipher rt2x00crypto_key_to_cipher(struct ieee80211_key_conf *key)
+{
+        return CIPHER_NONE;
+}
+
+static inline unsigned int rt2x00crypto_tx_overhead(struct ieee80211_tx_info *tx_info)
+{
+        return 0;
+}
+
+static inline void rt2x00crypto_tx_remove_iv(struct sk_buff *skb,
+                                             unsigned int iv_len)
+{
+}
+
+static inline void rt2x00crypto_tx_insert_iv(struct sk_buff *skb)
+{
+}
+
+static inline void rt2x00crypto_rx_insert_iv(struct sk_buff *skb,
+                                             unsigned int align,
+                                             unsigned int header_length,
+                                             struct rxdone_entry_desc *rxdesc)
+{
+}
+#endif
+
+/*
  * RFkill handlers.
  */
 #ifdef CONFIG_RT2X00_LIB_RFKILL
diff --git a/drivers/net/wireless/rt2x00/rt2x00mac.c b/drivers/net/wireless/rt2x00/rt2x00mac.c
index 4b9fde903cd6..3af427339417 100644
--- a/drivers/net/wireless/rt2x00/rt2x00mac.c
+++ b/drivers/net/wireless/rt2x00/rt2x00mac.c
@@ -36,22 +36,22 @@ static int rt2x00mac_tx_rts_cts(struct rt2x00_dev *rt2x00dev,
         struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(frag_skb);
         struct ieee80211_tx_info *rts_info;
         struct sk_buff *skb;
-        int size;
+        unsigned int data_length;
         int retval = 0;
 
         if (tx_info->flags & IEEE80211_TX_CTL_USE_CTS_PROTECT)
-                size = sizeof(struct ieee80211_cts);
+                data_length = sizeof(struct ieee80211_cts);
         else
-                size = sizeof(struct ieee80211_rts);
+                data_length = sizeof(struct ieee80211_rts);
 
-        skb = dev_alloc_skb(size + rt2x00dev->hw->extra_tx_headroom);
+        skb = dev_alloc_skb(data_length + rt2x00dev->hw->extra_tx_headroom);
         if (unlikely(!skb)) {
                 WARNING(rt2x00dev, "Failed to create RTS/CTS frame.\n");
                 return -ENOMEM;
         }
 
         skb_reserve(skb, rt2x00dev->hw->extra_tx_headroom);
-        skb_put(skb, size);
+        skb_put(skb, data_length);
 
         /*
          * Copy TX information over from original frame to
@@ -64,7 +64,6 @@ static int rt2x00mac_tx_rts_cts(struct rt2x00_dev *rt2x00dev,
          */
         memcpy(skb->cb, frag_skb->cb, sizeof(skb->cb));
         rts_info = IEEE80211_SKB_CB(skb);
-        rts_info->control.hw_key = NULL;
         rts_info->flags &= ~IEEE80211_TX_CTL_USE_RTS_CTS;
         rts_info->flags &= ~IEEE80211_TX_CTL_USE_CTS_PROTECT;
         rts_info->flags &= ~IEEE80211_TX_CTL_REQ_TX_STATUS;
@@ -74,13 +73,24 @@ static int rt2x00mac_tx_rts_cts(struct rt2x00_dev *rt2x00dev,
         else
                 rts_info->flags &= ~IEEE80211_TX_CTL_NO_ACK;
 
+        skb->do_not_encrypt = 1;
+
+        /*
+         * RTS/CTS frame should use the length of the frame plus any
+         * encryption overhead that will be added by the hardware.
+         */
+#ifdef CONFIG_RT2X00_LIB_CRYPTO
+        if (!frag_skb->do_not_encrypt)
+                data_length += rt2x00crypto_tx_overhead(tx_info);
+#endif /* CONFIG_RT2X00_LIB_CRYPTO */
+
         if (tx_info->flags & IEEE80211_TX_CTL_USE_CTS_PROTECT)
                 ieee80211_ctstoself_get(rt2x00dev->hw, tx_info->control.vif,
-                                        frag_skb->data, size, tx_info,
+                                        frag_skb->data, data_length, tx_info,
                                         (struct ieee80211_cts *)(skb->data));
         else
                 ieee80211_rts_get(rt2x00dev->hw, tx_info->control.vif,
-                                  frag_skb->data, size, tx_info,
+                                  frag_skb->data, data_length, tx_info,
                                   (struct ieee80211_rts *)(skb->data));
 
         retval = rt2x00queue_write_tx_frame(queue, skb);
@@ -464,6 +474,90 @@ void rt2x00mac_configure_filter(struct ieee80211_hw *hw,
 }
 EXPORT_SYMBOL_GPL(rt2x00mac_configure_filter);
 
+#ifdef CONFIG_RT2X00_LIB_CRYPTO
+int rt2x00mac_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
+                      const u8 *local_address, const u8 *address,
+                      struct ieee80211_key_conf *key)
+{
+        struct rt2x00_dev *rt2x00dev = hw->priv;
+        int (*set_key) (struct rt2x00_dev *rt2x00dev,
+                        struct rt2x00lib_crypto *crypto,
+                        struct ieee80211_key_conf *key);
+        struct rt2x00lib_crypto crypto;
+
+        if (!test_bit(CONFIG_SUPPORT_HW_CRYPTO, &rt2x00dev->flags))
+                return -EOPNOTSUPP;
+        else if (key->keylen > 32)
+                return -ENOSPC;
+
+        memset(&crypto, 0, sizeof(crypto));
+
+        /*
+         * When in STA mode, bssidx is always 0 otherwise local_address[5]
+         * contains the bss number, see BSS_ID_MASK comments for details.
+         */
+        if (rt2x00dev->intf_sta_count)
+                crypto.bssidx = 0;
+        else
+                crypto.bssidx =
+                    local_address[5] & (rt2x00dev->ops->max_ap_intf - 1);
+
+        crypto.cipher = rt2x00crypto_key_to_cipher(key);
+        if (crypto.cipher == CIPHER_NONE)
+                return -EOPNOTSUPP;
+
+        crypto.cmd = cmd;
+        crypto.address = address;
+
+        if (crypto.cipher == CIPHER_TKIP) {
+                if (key->keylen > NL80211_TKIP_DATA_OFFSET_ENCR_KEY)
+                        memcpy(&crypto.key,
+                               &key->key[NL80211_TKIP_DATA_OFFSET_ENCR_KEY],
+                               sizeof(crypto.key));
+
+                if (key->keylen > NL80211_TKIP_DATA_OFFSET_TX_MIC_KEY)
+                        memcpy(&crypto.tx_mic,
+                               &key->key[NL80211_TKIP_DATA_OFFSET_TX_MIC_KEY],
+                               sizeof(crypto.tx_mic));
+
+                if (key->keylen > NL80211_TKIP_DATA_OFFSET_RX_MIC_KEY)
+                        memcpy(&crypto.rx_mic,
+                               &key->key[NL80211_TKIP_DATA_OFFSET_RX_MIC_KEY],
+                               sizeof(crypto.rx_mic));
+        } else
+                memcpy(&crypto.key, &key->key[0], key->keylen);
+
+        /*
+         * Each BSS has a maximum of 4 shared keys.
+         * Shared key index values:
+         *      0) BSS0 key0
+         *      1) BSS0 key1
+         *      ...
+         *      4) BSS1 key0
+         *      ...
+         *      8) BSS2 key0
+         *      ...
+         * Both pairwise as shared key indeces are determined by
+         * driver. This is required because the hardware requires
+         * keys to be assigned in correct order (When key 1 is
+         * provided but key 0 is not, then the key is not found
+         * by the hardware during RX).
+         */
+        key->hw_key_idx = 0;
+
+        if (key->flags & IEEE80211_KEY_FLAG_PAIRWISE)
+                set_key = rt2x00dev->ops->lib->config_pairwise_key;
+        else
+                set_key = rt2x00dev->ops->lib->config_shared_key;
+
+        if (!set_key)
+                return -EOPNOTSUPP;
+
+        return set_key(rt2x00dev, &crypto, key);
+}
+EXPORT_SYMBOL_GPL(rt2x00mac_set_key);
+#endif /* CONFIG_RT2X00_LIB_CRYPTO */
+
 int rt2x00mac_get_stats(struct ieee80211_hw *hw,
                         struct ieee80211_low_level_stats *stats)
 {
diff --git a/drivers/net/wireless/rt2x00/rt2x00queue.c b/drivers/net/wireless/rt2x00/rt2x00queue.c
index 898cdd7f57d9..c0f97c53e5ce 100644
--- a/drivers/net/wireless/rt2x00/rt2x00queue.c
+++ b/drivers/net/wireless/rt2x00/rt2x00queue.c
@@ -33,10 +33,11 @@
 struct sk_buff *rt2x00queue_alloc_rxskb(struct rt2x00_dev *rt2x00dev,
                                         struct queue_entry *entry)
 {
-        unsigned int frame_size;
-        unsigned int reserved_size;
         struct sk_buff *skb;
         struct skb_frame_desc *skbdesc;
+        unsigned int frame_size;
+        unsigned int head_size = 0;
+        unsigned int tail_size = 0;
 
         /*
          * The frame size includes descriptor size, because the
@@ -49,16 +50,32 @@ struct sk_buff *rt2x00queue_alloc_rxskb(struct rt2x00_dev *rt2x00dev,
          * this means we need at least 3 bytes for moving the frame
          * into the correct offset.
          */
-        reserved_size = 4;
+        head_size = 4;
+
+        /*
+         * For IV/EIV/ICV assembly we must make sure there is
+         * at least 8 bytes bytes available in headroom for IV/EIV
+         * and 4 bytes for ICV data as tailroon.
+         */
+#ifdef CONFIG_RT2X00_LIB_CRYPTO
+        if (test_bit(CONFIG_SUPPORT_HW_CRYPTO, &rt2x00dev->flags)) {
+                head_size += 8;
+                tail_size += 4;
+        }
+#endif /* CONFIG_RT2X00_LIB_CRYPTO */
 
         /*
          * Allocate skbuffer.
          */
-        skb = dev_alloc_skb(frame_size + reserved_size);
+        skb = dev_alloc_skb(frame_size + head_size + tail_size);
         if (!skb)
                 return NULL;
 
-        skb_reserve(skb, reserved_size);
+        /*
+         * Make sure we not have a frame with the requested bytes
+         * available in the head and tail.
+         */
+        skb_reserve(skb, head_size);
         skb_put(skb, frame_size);
 
         /*
@@ -140,7 +157,7 @@ static void rt2x00queue_create_tx_descriptor(struct queue_entry *entry,
         txdesc->cw_max = entry->queue->cw_max;
         txdesc->aifs = entry->queue->aifs;
 
-        /* Data length should be extended with 4 bytes for CRC */
+        /* Data length + CRC + IV/EIV/ICV/MMIC (when using encryption) */
         data_length = entry->skb->len + 4;
 
         /*
@@ -149,6 +166,35 @@ static void rt2x00queue_create_tx_descriptor(struct queue_entry *entry,
         if (!(tx_info->flags & IEEE80211_TX_CTL_NO_ACK))
                 __set_bit(ENTRY_TXD_ACK, &txdesc->flags);
 
+#ifdef CONFIG_RT2X00_LIB_CRYPTO
+        if (test_bit(CONFIG_SUPPORT_HW_CRYPTO, &rt2x00dev->flags) &&
+            !entry->skb->do_not_encrypt) {
+                struct ieee80211_key_conf *hw_key = tx_info->control.hw_key;
+
+                __set_bit(ENTRY_TXD_ENCRYPT, &txdesc->flags);
+
+                txdesc->cipher = rt2x00crypto_key_to_cipher(hw_key);
+
+                if (hw_key->flags & IEEE80211_KEY_FLAG_PAIRWISE)
+                        __set_bit(ENTRY_TXD_ENCRYPT_PAIRWISE, &txdesc->flags);
+
+                txdesc->key_idx = hw_key->hw_key_idx;
+                txdesc->iv_offset = ieee80211_get_hdrlen_from_skb(entry->skb);
+
+                /*
+                 * Extend frame length to include all encryption overhead
+                 * that will be added by the hardware.
+                 */
+                data_length += rt2x00crypto_tx_overhead(tx_info);
+
+                if (!(hw_key->flags & IEEE80211_KEY_FLAG_GENERATE_IV))
+                        __set_bit(ENTRY_TXD_ENCRYPT_IV, &txdesc->flags);
+
+                if (!(hw_key->flags & IEEE80211_KEY_FLAG_GENERATE_MMIC))
+                        __set_bit(ENTRY_TXD_ENCRYPT_MMIC, &txdesc->flags);
+        }
+#endif /* CONFIG_RT2X00_LIB_CRYPTO */
+
         /*
          * Check if this is a RTS/CTS frame
          */
@@ -305,6 +351,7 @@ int rt2x00queue_write_tx_frame(struct data_queue *queue, struct sk_buff *skb)
         struct queue_entry *entry = rt2x00queue_get_entry(queue, Q_INDEX);
         struct txentry_desc txdesc;
         struct skb_frame_desc *skbdesc;
+        unsigned int iv_len = IEEE80211_SKB_CB(skb)->control.iv_len;
 
         if (unlikely(rt2x00queue_full(queue)))
                 return -EINVAL;
@@ -326,15 +373,33 @@ int rt2x00queue_write_tx_frame(struct data_queue *queue, struct sk_buff *skb)
         rt2x00queue_create_tx_descriptor(entry, &txdesc);
 
         /*
-         * skb->cb array is now ours and we are free to use it.
+         * All information is retreived from the skb->cb array,
+         * now we should claim ownership of the driver part of that
+         * array.
          */
         skbdesc = get_skb_frame_desc(entry->skb);
         memset(skbdesc, 0, sizeof(*skbdesc));
         skbdesc->entry = entry;
 
+        /*
+         * When hardware encryption is supported, and this frame
+         * is to be encrypted, we should strip the IV/EIV data from
+         * the frame so we can provide it to the driver seperately.
+         */
+        if (test_bit(ENTRY_TXD_ENCRYPT, &txdesc.flags) &&
+            !test_bit(ENTRY_TXD_ENCRYPT_IV, &txdesc.flags))
+                rt2x00crypto_tx_remove_iv(skb, iv_len);
+
+        /*
+         * It could be possible that the queue was corrupted and this
+         * call failed. Just drop the frame, we cannot rollback and pass
+         * the frame to mac80211 because the skb->cb has now been tainted.
+         */
         if (unlikely(queue->rt2x00dev->ops->lib->write_tx_data(entry))) {
                 __clear_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags);
-                return -EIO;
+                dev_kfree_skb_any(entry->skb);
+                entry->skb = NULL;
+                return 0;
         }
 
         if (test_bit(DRIVER_REQUIRE_DMA, &queue->rt2x00dev->flags))
diff --git a/drivers/net/wireless/rt2x00/rt2x00queue.h b/drivers/net/wireless/rt2x00/rt2x00queue.h
index ff78e52ce43c..37f3f98d58a2 100644
--- a/drivers/net/wireless/rt2x00/rt2x00queue.h
+++ b/drivers/net/wireless/rt2x00/rt2x00queue.h
@@ -87,10 +87,13 @@ enum data_queue_qid {
  *
  * @SKBDESC_DMA_MAPPED_RX: &skb_dma field has been mapped for RX
  * @SKBDESC_DMA_MAPPED_TX: &skb_dma field has been mapped for TX
+ * @FRAME_DESC_IV_STRIPPED: Frame contained a IV/EIV provided by
+ *      mac80211 but was stripped for processing by the driver.
  */
 enum skb_frame_desc_flags {
-        SKBDESC_DMA_MAPPED_RX = (1 << 0),
-        SKBDESC_DMA_MAPPED_TX = (1 << 1),
+        SKBDESC_DMA_MAPPED_RX = 1 << 0,
+        SKBDESC_DMA_MAPPED_TX = 1 << 1,
+        FRAME_DESC_IV_STRIPPED = 1 << 2,
 };
 
 /**
@@ -104,6 +107,8 @@ enum skb_frame_desc_flags {
  * @desc: Pointer to descriptor part of the frame.
  *      Note that this pointer could point to something outside
  *      of the scope of the skb->data pointer.
+ * @iv: IV data used during encryption/decryption.
+ * @eiv: EIV data used during encryption/decryption.
  * @skb_dma: (PCI-only) the DMA address associated with the sk buffer.
  * @entry: The entry to which this sk buffer belongs.
  */
@@ -113,6 +118,9 @@ struct skb_frame_desc {
         unsigned int desc_len;
         void *desc;
 
+        __le32 iv;
+        __le32 eiv;
+
         dma_addr_t skb_dma;
 
         struct queue_entry *entry;
@@ -152,7 +160,11 @@ enum rxdone_entry_desc_flags {
  * @size: Data size of the received frame.
  * @flags: MAC80211 receive flags (See &enum mac80211_rx_flags).
  * @dev_flags: Ralink receive flags (See &enum rxdone_entry_desc_flags).
-
+ * @cipher: Cipher type used during decryption.
+ * @cipher_status: Decryption status.
+ * @iv: IV data used during decryption.
+ * @eiv: EIV data used during decryption.
+ * @icv: ICV data used during decryption.
  */
 struct rxdone_entry_desc {
         u64 timestamp;
@@ -161,6 +173,12 @@ struct rxdone_entry_desc {
         int size;
         int flags;
         int dev_flags;
+        u8 cipher;
+        u8 cipher_status;
+
+        __le32 iv;
+        __le32 eiv;
+        __le32 icv;
 };
 
 /**
@@ -206,6 +224,10 @@ struct txdone_entry_desc {
  * @ENTRY_TXD_BURST: This frame belongs to the same burst event.
  * @ENTRY_TXD_ACK: An ACK is required for this frame.
  * @ENTRY_TXD_RETRY_MODE: When set, the long retry count is used.
+ * @ENTRY_TXD_ENCRYPT: This frame should be encrypted.
+ * @ENTRY_TXD_ENCRYPT_PAIRWISE: Use pairwise key table (instead of shared).
+ * @ENTRY_TXD_ENCRYPT_IV: Generate IV/EIV in hardware.
+ * @ENTRY_TXD_ENCRYPT_MMIC: Generate MIC in hardware.
  */
 enum txentry_desc_flags {
         ENTRY_TXD_RTS_FRAME,
@@ -218,6 +240,10 @@ enum txentry_desc_flags {
         ENTRY_TXD_BURST,
         ENTRY_TXD_ACK,
         ENTRY_TXD_RETRY_MODE,
+        ENTRY_TXD_ENCRYPT,
+        ENTRY_TXD_ENCRYPT_PAIRWISE,
+        ENTRY_TXD_ENCRYPT_IV,
+        ENTRY_TXD_ENCRYPT_MMIC,
 };
 
 /**
@@ -236,6 +262,9 @@ enum txentry_desc_flags {
  * @ifs: IFS value.
  * @cw_min: cwmin value.
  * @cw_max: cwmax value.
+ * @cipher: Cipher type used for encryption.
+ * @key_idx: Key index used for encryption.
+ * @iv_offset: Position where IV should be inserted by hardware.
  */
 struct txentry_desc {
         unsigned long flags;
@@ -252,6 +281,10 @@ struct txentry_desc {
         short ifs;
         short cw_min;
         short cw_max;
+
+        enum cipher cipher;
+        u16 key_idx;
+        u16 iv_offset;
 };
 
 /**
@@ -484,25 +517,51 @@ static inline int rt2x00queue_threshold(struct data_queue *queue)
 }
 
 /**
- * rt2x00_desc_read - Read a word from the hardware descriptor.
+ * _rt2x00_desc_read - Read a word from the hardware descriptor.
+ * @desc: Base descriptor address
+ * @word: Word index from where the descriptor should be read.
+ * @value: Address where the descriptor value should be written into.
+ */
+static inline void _rt2x00_desc_read(__le32 *desc, const u8 word, __le32 *value)
+{
+        *value = desc[word];
+}
+
+/**
+ * rt2x00_desc_read - Read a word from the hardware descriptor, this
+ * function will take care of the byte ordering.
  * @desc: Base descriptor address
  * @word: Word index from where the descriptor should be read.
  * @value: Address where the descriptor value should be written into.
  */
 static inline void rt2x00_desc_read(__le32 *desc, const u8 word, u32 *value)
 {
-        *value = le32_to_cpu(desc[word]);
+        __le32 tmp;
+        _rt2x00_desc_read(desc, word, &tmp);
+        *value = le32_to_cpu(tmp);
+}
+
+/**
+ * rt2x00_desc_write - write a word to the hardware descriptor, this
+ * function will take care of the byte ordering.
+ * @desc: Base descriptor address
+ * @word: Word index from where the descriptor should be written.
+ * @value: Value that should be written into the descriptor.
+ */
+static inline void _rt2x00_desc_write(__le32 *desc, const u8 word, __le32 value)
+{
+        desc[word] = value;
 }
 
 /**
- * rt2x00_desc_write - wrote a word to the hardware descriptor.
+ * rt2x00_desc_write - write a word to the hardware descriptor.
  * @desc: Base descriptor address
  * @word: Word index from where the descriptor should be written.
  * @value: Value that should be written into the descriptor.
  */
 static inline void rt2x00_desc_write(__le32 *desc, const u8 word, u32 value)
 {
-        desc[word] = cpu_to_le32(value);
+        _rt2x00_desc_write(desc, word, cpu_to_le32(value));
 }
 
 #endif /* RT2X00QUEUE_H */
diff --git a/drivers/net/wireless/rt2x00/rt2x00reg.h b/drivers/net/wireless/rt2x00/rt2x00reg.h
index 7e88ce5651b9..6d5acf99a1c5 100644
--- a/drivers/net/wireless/rt2x00/rt2x00reg.h
+++ b/drivers/net/wireless/rt2x00/rt2x00reg.h
@@ -27,6 +27,16 @@
 #define RT2X00REG_H
 
 /*
+ * RX crypto status
+ */
+enum rx_crypto {
+        RX_CRYPTO_SUCCESS = 0,
+        RX_CRYPTO_FAIL_ICV = 1,
+        RX_CRYPTO_FAIL_MIC = 2,
+        RX_CRYPTO_FAIL_KEY = 3,
+};
+
+/*
  * Antenna values
  */
 enum antenna {
@@ -104,7 +114,14 @@ enum cipher {
  */
         CIPHER_CKIP64 = 5,
         CIPHER_CKIP128 = 6,
-        CIPHER_TKIP_NO_MIC = 7,
+        CIPHER_TKIP_NO_MIC = 7, /* Don't send to device */
+
+/*
+ * Max cipher type.
+ * Note that CIPHER_NONE isn't counted, and CKIP64 and CKIP128
+ * are excluded due to limitations in mac80211.
+ */
+        CIPHER_MAX = 4,
 };
 
 /*