3 files changed, 69 insertions, 12 deletions

diff --git a/drivers/net/wireless/rtl818x/rtl8187.h b/drivers/net/wireless/rtl818x/rtl8187.h
index 33725d0978b3..f09872eec3d5 100644
--- a/drivers/net/wireless/rtl818x/rtl8187.h
+++ b/drivers/net/wireless/rtl818x/rtl8187.h
@@ -111,6 +111,8 @@ struct rtl8187_priv {
         u8 signal;
         u8 quality;
         u8 noise;
+        u8 slot_time;
+        u8 aifsn[4];
 };
 
 void rtl8187_write_phy(struct ieee80211_hw *dev, u8 addr, u32 data);
diff --git a/drivers/net/wireless/rtl818x/rtl8187_dev.c b/drivers/net/wireless/rtl818x/rtl8187_dev.c
index 5fe7473124e8..cb5bcefd1c3e 100644
--- a/drivers/net/wireless/rtl818x/rtl8187_dev.c
+++ b/drivers/net/wireless/rtl818x/rtl8187_dev.c
@@ -712,6 +712,13 @@ static int rtl8187b_init_hw(struct ieee80211_hw *dev)
 
         rtl818x_iowrite16_idx(priv, (__le16 *)0xFFEC, 0x0800, 1);
 
+        priv->slot_time = 0x9;
+        priv->aifsn[0] = 2; /* AIFSN[AC_VO] */
+        priv->aifsn[1] = 2; /* AIFSN[AC_VI] */
+        priv->aifsn[2] = 7; /* AIFSN[AC_BK] */
+        priv->aifsn[3] = 3; /* AIFSN[AC_BE] */
+        rtl818x_iowrite8(priv, &priv->map->ACM_CONTROL, 0);
+
         return 0;
 }
 
@@ -919,24 +926,38 @@ static int rtl8187_config_interface(struct ieee80211_hw *dev,
         return 0;
 }
 
+/*
+ * With 8187B, AC_*_PARAM clashes with FEMR definition in struct rtl818x_csr for
+ * example. Thus we have to use raw values for AC_*_PARAM register addresses.
+ */
+static __le32 *rtl8187b_ac_addr[4] = {
+        (__le32 *) 0xFFF0, /* AC_VO */
+        (__le32 *) 0xFFF4, /* AC_VI */
+        (__le32 *) 0xFFFC, /* AC_BK */
+        (__le32 *) 0xFFF8, /* AC_BE */
+};
+
+#define SIFS_TIME 0xa
+
 static void rtl8187_conf_erp(struct rtl8187_priv *priv, bool use_short_slot,
                              bool use_short_preamble)
 {
         if (priv->is_rtl8187b) {
-                u8 difs, eifs, slot_time;
+                u8 difs, eifs;
                 u16 ack_timeout;
+                int queue;
 
                 if (use_short_slot) {
-                        slot_time = 0x9;
+                        priv->slot_time = 0x9;
                         difs = 0x1c;
                         eifs = 0x53;
                 } else {
-                        slot_time = 0x14;
+                        priv->slot_time = 0x14;
                         difs = 0x32;
                         eifs = 0x5b;
                 }
                 rtl818x_iowrite8(priv, &priv->map->SIFS, 0x22);
-                rtl818x_iowrite8(priv, &priv->map->SLOT, slot_time);
+                rtl818x_iowrite8(priv, &priv->map->SLOT, priv->slot_time);
                 rtl818x_iowrite8(priv, &priv->map->DIFS, difs);
 
                 /*
@@ -957,18 +978,21 @@ static void rtl8187_conf_erp(struct rtl8187_priv *priv, bool use_short_slot,
                         ack_timeout += 144;
                 rtl818x_iowrite8(priv, &priv->map->CARRIER_SENSE_COUNTER,
                                  DIV_ROUND_UP(ack_timeout, 4));
+
+                for (queue = 0; queue < 4; queue++)
+                        rtl818x_iowrite8(priv, (u8 *) rtl8187b_ac_addr[queue],
+                                         priv->aifsn[queue] * priv->slot_time +
+                                         SIFS_TIME);
         } else {
                 rtl818x_iowrite8(priv, &priv->map->SIFS, 0x22);
                 if (use_short_slot) {
                         rtl818x_iowrite8(priv, &priv->map->SLOT, 0x9);
                         rtl818x_iowrite8(priv, &priv->map->DIFS, 0x14);
                         rtl818x_iowrite8(priv, &priv->map->EIFS, 91 - 0x14);
-                        rtl818x_iowrite8(priv, &priv->map->CW_VAL, 0x73);
                 } else {
                         rtl818x_iowrite8(priv, &priv->map->SLOT, 0x14);
                         rtl818x_iowrite8(priv, &priv->map->DIFS, 0x24);
                         rtl818x_iowrite8(priv, &priv->map->EIFS, 91 - 0x24);
-                        rtl818x_iowrite8(priv, &priv->map->CW_VAL, 0xa5);
                 }
         }
 }
@@ -1017,6 +1041,42 @@ static void rtl8187_configure_filter(struct ieee80211_hw *dev,
         rtl818x_iowrite32_async(priv, &priv->map->RX_CONF, priv->rx_conf);
 }
 
+static int rtl8187_conf_tx(struct ieee80211_hw *dev, u16 queue,
+                           const struct ieee80211_tx_queue_params *params)
+{
+        struct rtl8187_priv *priv = dev->priv;
+        u8 cw_min, cw_max;
+
+        if (queue > 3)
+                return -EINVAL;
+
+        cw_min = fls(params->cw_min);
+        cw_max = fls(params->cw_max);
+
+        if (priv->is_rtl8187b) {
+                priv->aifsn[queue] = params->aifs;
+
+                /*
+                 * This is the structure of AC_*_PARAM registers in 8187B:
+                 * - TXOP limit field, bit offset = 16
+                 * - ECWmax, bit offset = 12
+                 * - ECWmin, bit offset = 8
+                 * - AIFS, bit offset = 0
+                 */
+                rtl818x_iowrite32(priv, rtl8187b_ac_addr[queue],
+                                  (params->txop << 16) | (cw_max << 12) |
+                                  (cw_min << 8) | (params->aifs *
+                                  priv->slot_time + SIFS_TIME));
+        } else {
+                if (queue != 0)
+                        return -EINVAL;
+
+                rtl818x_iowrite8(priv, &priv->map->CW_VAL,
+                                 cw_min | (cw_max << 4));
+        }
+        return 0;
+}
+
 static const struct ieee80211_ops rtl8187_ops = {
         .tx                     = rtl8187_tx,
         .start                  = rtl8187_start,
@@ -1027,6 +1087,7 @@ static const struct ieee80211_ops rtl8187_ops = {
         .config_interface       = rtl8187_config_interface,
         .bss_info_changed       = rtl8187_bss_info_changed,
         .configure_filter       = rtl8187_configure_filter,
+        .conf_tx                = rtl8187_conf_tx
 };
 
 static void rtl8187_eeprom_register_read(struct eeprom_93cx6 *eeprom)
diff --git a/drivers/net/wireless/rtl818x/rtl8187_rtl8225.c b/drivers/net/wireless/rtl818x/rtl8187_rtl8225.c
index 69030be62b36..4e75e8e7fa90 100644
--- a/drivers/net/wireless/rtl818x/rtl8187_rtl8225.c
+++ b/drivers/net/wireless/rtl818x/rtl8187_rtl8225.c
@@ -878,12 +878,6 @@ static void rtl8225z2_b_rf_init(struct ieee80211_hw *dev)
         for (i = 0; i < ARRAY_SIZE(rtl8225z2_ofdm); i++)
                 rtl8225_write_phy_ofdm(dev, i, rtl8225z2_ofdm[i]);
 
-        rtl818x_iowrite32(priv, (__le32 *)0xFFF0, (7 << 12) | (3 << 8) | 28);
-        rtl818x_iowrite32(priv, (__le32 *)0xFFF4, (7 << 12) | (3 << 8) | 28);
-        rtl818x_iowrite32(priv, (__le32 *)0xFFF8, (7 << 12) | (3 << 8) | 28);
-        rtl818x_iowrite32(priv, (__le32 *)0xFFFC, (7 << 12) | (3 << 8) | 28);
-        rtl818x_iowrite8(priv, &priv->map->ACM_CONTROL, 0);
-
         rtl8225_write_phy_ofdm(dev, 0x97, 0x46);
         rtl8225_write_phy_ofdm(dev, 0xa4, 0xb6);
         rtl8225_write_phy_ofdm(dev, 0x85, 0xfc);