1 files changed, 58 insertions, 11 deletions

diff --git a/drivers/net/wireless/rt2x00/rt61pci.c b/drivers/net/wireless/rt2x00/rt61pci.c
index 9d27ce0e54fe..52537101c908 100644
--- a/drivers/net/wireless/rt2x00/rt61pci.c
+++ b/drivers/net/wireless/rt2x00/rt61pci.c
@@ -1478,16 +1478,6 @@ static int rt61pci_init_registers(struct rt2x00_dev *rt2x00dev)
 
         rt2x00pci_register_write(rt2x00dev, M2H_CMD_DONE_CSR, 0xffffffff);
 
-        rt2x00pci_register_read(rt2x00dev, AC_TXOP_CSR0, &reg);
-        rt2x00_set_field32(&reg, AC_TXOP_CSR0_AC0_TX_OP, 0);
-        rt2x00_set_field32(&reg, AC_TXOP_CSR0_AC1_TX_OP, 0);
-        rt2x00pci_register_write(rt2x00dev, AC_TXOP_CSR0, reg);
-
-        rt2x00pci_register_read(rt2x00dev, AC_TXOP_CSR1, &reg);
-        rt2x00_set_field32(&reg, AC_TXOP_CSR1_AC2_TX_OP, 192);
-        rt2x00_set_field32(&reg, AC_TXOP_CSR1_AC3_TX_OP, 48);
-        rt2x00pci_register_write(rt2x00dev, AC_TXOP_CSR1, reg);
-
         /*
          * Clear all beacons
          * For the Beacon base registers we only need to clear
@@ -2652,6 +2642,63 @@ static int rt61pci_set_retry_limit(struct ieee80211_hw *hw,
         return 0;
 }
 
+static int rt61pci_conf_tx(struct ieee80211_hw *hw, u16 queue_idx,
+                           const struct ieee80211_tx_queue_params *params)
+{
+        struct rt2x00_dev *rt2x00dev = hw->priv;
+        struct data_queue *queue;
+        struct rt2x00_field32 field;
+        int retval;
+        u32 reg;
+
+        /*
+         * First pass the configuration through rt2x00lib, that will
+         * update the queue settings and validate the input. After that
+         * we are free to update the registers based on the value
+         * in the queue parameter.
+         */
+        retval = rt2x00mac_conf_tx(hw, queue_idx, params);
+        if (retval)
+                return retval;
+
+        queue = rt2x00queue_get_queue(rt2x00dev, queue_idx);
+
+        /* Update WMM TXOP register */
+        if (queue_idx < 2) {
+                field.bit_offset = queue_idx * 16;
+                field.bit_mask = 0xffff << field.bit_offset;
+
+                rt2x00pci_register_read(rt2x00dev, AC_TXOP_CSR0, &reg);
+                rt2x00_set_field32(&reg, field, queue->txop);
+                rt2x00pci_register_write(rt2x00dev, AC_TXOP_CSR0, reg);
+        } else if (queue_idx < 4) {
+                field.bit_offset = (queue_idx - 2) * 16;
+                field.bit_mask = 0xffff << field.bit_offset;
+
+                rt2x00pci_register_read(rt2x00dev, AC_TXOP_CSR1, &reg);
+                rt2x00_set_field32(&reg, field, queue->txop);
+                rt2x00pci_register_write(rt2x00dev, AC_TXOP_CSR1, reg);
+        }
+
+        /* Update WMM registers */
+        field.bit_offset = queue_idx * 4;
+        field.bit_mask = 0xf << field.bit_offset;
+
+        rt2x00pci_register_read(rt2x00dev, AIFSN_CSR, &reg);
+        rt2x00_set_field32(&reg, field, queue->aifs);
+        rt2x00pci_register_write(rt2x00dev, AIFSN_CSR, reg);
+
+        rt2x00pci_register_read(rt2x00dev, CWMIN_CSR, &reg);
+        rt2x00_set_field32(&reg, field, queue->cw_min);
+        rt2x00pci_register_write(rt2x00dev, CWMIN_CSR, reg);
+
+        rt2x00pci_register_read(rt2x00dev, CWMAX_CSR, &reg);
+        rt2x00_set_field32(&reg, field, queue->cw_max);
+        rt2x00pci_register_write(rt2x00dev, CWMAX_CSR, reg);
+
+        return 0;
+}
+
 static u64 rt61pci_get_tsf(struct ieee80211_hw *hw)
 {
         struct rt2x00_dev *rt2x00dev = hw->priv;
@@ -2679,7 +2726,7 @@ static const struct ieee80211_ops rt61pci_mac80211_ops = {
         .get_stats              = rt2x00mac_get_stats,
         .set_retry_limit        = rt61pci_set_retry_limit,
         .bss_info_changed       = rt2x00mac_bss_info_changed,
-        .conf_tx                = rt2x00mac_conf_tx,
+        .conf_tx                = rt61pci_conf_tx,
         .get_tx_stats           = rt2x00mac_get_tx_stats,
         .get_tsf                = rt61pci_get_tsf,
 };