rtlwifi/rtl8723be/phy.c

0001 // SPDX-License-Identifier: GPL-2.0
0002 /* Copyright(c) 2009-2014  Realtek Corporation.*/
0003
0004 #include "../wifi.h"
0005 #include "../pci.h"
0006 #include "../ps.h"
0007 #include "reg.h"
0008 #include "def.h"
0009 #include "phy.h"
0010 #include "../rtl8723com/phy_common.h"
0011 #include "rf.h"
0012 #include "dm.h"
0013 #include "../rtl8723com/dm_common.h"
0014 #include "table.h"
0015 #include "trx.h"
0016 #include <linux/kernel.h>
0017
0018 static bool _rtl8723be_phy_bb8723b_config_parafile(struct ieee80211_hw *hw);
0019 static bool _rtl8723be_phy_config_mac_with_headerfile(struct ieee80211_hw *hw);
0020 static bool _rtl8723be_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
0021                              u8 configtype);
0022 static bool _rtl8723be_phy_config_bb_with_pgheaderfile(struct ieee80211_hw *hw,
0023                                u8 configtype);
0024 static bool _rtl8723be_phy_sw_chnl_step_by_step(struct ieee80211_hw *hw,
0025                         u8 channel, u8 *stage,
0026                         u8 *step, u32 *delay);
0027
0028 static void rtl8723be_phy_set_rf_on(struct ieee80211_hw *hw);
0029 static void rtl8723be_phy_set_io(struct ieee80211_hw *hw);
0030
0031 u32 rtl8723be_phy_query_rf_reg(struct ieee80211_hw *hw, enum radio_path rfpath,
0032                    u32 regaddr, u32 bitmask)
0033 {
0034     struct rtl_priv *rtlpriv = rtl_priv(hw);
0035     u32 original_value, readback_value, bitshift;
0036
0037     rtl_dbg(rtlpriv, COMP_RF, DBG_TRACE,
0038         "regaddr(%#x), rfpath(%#x), bitmask(%#x)\n",
0039         regaddr, rfpath, bitmask);
0040
0041     spin_lock(&rtlpriv->locks.rf_lock);
0042
0043     original_value = rtl8723_phy_rf_serial_read(hw, rfpath, regaddr);
0044     bitshift = rtl8723_phy_calculate_bit_shift(bitmask);
0045     readback_value = (original_value & bitmask) >> bitshift;
0046
0047     spin_unlock(&rtlpriv->locks.rf_lock);
0048
0049     rtl_dbg(rtlpriv, COMP_RF, DBG_TRACE,
0050         "regaddr(%#x), rfpath(%#x), bitmask(%#x), original_value(%#x)\n",
0051         regaddr, rfpath, bitmask, original_value);
0052
0053     return readback_value;
0054 }
0055
0056 void rtl8723be_phy_set_rf_reg(struct ieee80211_hw *hw, enum radio_path path,
0057                   u32 regaddr, u32 bitmask, u32 data)
0058 {
0059     struct rtl_priv *rtlpriv = rtl_priv(hw);
0060     u32 original_value, bitshift;
0061
0062     rtl_dbg(rtlpriv, COMP_RF, DBG_TRACE,
0063         "regaddr(%#x), bitmask(%#x), data(%#x), rfpath(%#x)\n",
0064         regaddr, bitmask, data, path);
0065
0066     spin_lock(&rtlpriv->locks.rf_lock);
0067
0068     if (bitmask != RFREG_OFFSET_MASK) {
0069             original_value = rtl8723_phy_rf_serial_read(hw, path,
0070                                     regaddr);
0071             bitshift = rtl8723_phy_calculate_bit_shift(bitmask);
0072             data = ((original_value & (~bitmask)) |
0073                 (data << bitshift));
0074         }
0075
0076     rtl8723_phy_rf_serial_write(hw, path, regaddr, data);
0077
0078     spin_unlock(&rtlpriv->locks.rf_lock);
0079
0080     rtl_dbg(rtlpriv, COMP_RF, DBG_TRACE,
0081         "regaddr(%#x), bitmask(%#x), data(%#x), rfpath(%#x)\n",
0082         regaddr, bitmask, data, path);
0083
0084 }
0085
0086 bool rtl8723be_phy_mac_config(struct ieee80211_hw *hw)
0087 {
0088     struct rtl_priv *rtlpriv = rtl_priv(hw);
0089     bool rtstatus = _rtl8723be_phy_config_mac_with_headerfile(hw);
0090
0091     rtl_write_byte(rtlpriv, 0x04CA, 0x0B);
0092     return rtstatus;
0093 }
0094
0095 bool rtl8723be_phy_bb_config(struct ieee80211_hw *hw)
0096 {
0097     bool rtstatus = true;
0098     struct rtl_priv *rtlpriv = rtl_priv(hw);
0099     u16 regval;
0100     u8 b_reg_hwparafile = 1;
0101     u32 tmp;
0102     u8 crystalcap = rtlpriv->efuse.crystalcap;
0103     rtl8723_phy_init_bb_rf_reg_def(hw);
0104     regval = rtl_read_word(rtlpriv, REG_SYS_FUNC_EN);
0105     rtl_write_word(rtlpriv, REG_SYS_FUNC_EN,
0106                regval | BIT(13) | BIT(0) | BIT(1));
0107
0108     rtl_write_byte(rtlpriv, REG_RF_CTRL, RF_EN | RF_RSTB | RF_SDMRSTB);
0109     rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN,
0110                FEN_PPLL | FEN_PCIEA | FEN_DIO_PCIE |
0111                FEN_BB_GLB_RSTN | FEN_BBRSTB);
0112     tmp = rtl_read_dword(rtlpriv, 0x4c);
0113     rtl_write_dword(rtlpriv, 0x4c, tmp | BIT(23));
0114
0115     rtl_write_byte(rtlpriv, REG_AFE_XTAL_CTRL + 1, 0x80);
0116
0117     if (b_reg_hwparafile == 1)
0118         rtstatus = _rtl8723be_phy_bb8723b_config_parafile(hw);
0119
0120     crystalcap = crystalcap & 0x3F;
0121     rtl_set_bbreg(hw, REG_MAC_PHY_CTRL, 0xFFF000,
0122               (crystalcap | crystalcap << 6));
0123
0124     return rtstatus;
0125 }
0126
0127 bool rtl8723be_phy_rf_config(struct ieee80211_hw *hw)
0128 {
0129     return rtl8723be_phy_rf6052_config(hw);
0130 }
0131
0132 static bool _rtl8723be_check_positive(struct ieee80211_hw *hw,
0133                       const u32 condition1,
0134                       const u32 condition2)
0135 {
0136     struct rtl_priv *rtlpriv = rtl_priv(hw);
0137     struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
0138     u32 cut_ver = ((rtlhal->version & CHIP_VER_RTL_MASK)
0139                     >> CHIP_VER_RTL_SHIFT);
0140     u32 intf = (rtlhal->interface == INTF_USB ? BIT(1) : BIT(0));
0141
0142     u8  board_type = ((rtlhal->board_type & BIT(4)) >> 4) << 0 | /* _GLNA */
0143              ((rtlhal->board_type & BIT(3)) >> 3) << 1 | /* _GPA  */
0144              ((rtlhal->board_type & BIT(7)) >> 7) << 2 | /* _ALNA */
0145              ((rtlhal->board_type & BIT(6)) >> 6) << 3 | /* _APA  */
0146              ((rtlhal->board_type & BIT(2)) >> 2) << 4;  /* _BT   */
0147
0148     u32 cond1 = condition1, cond2 = condition2;
0149     u32 driver1 = cut_ver << 24 |   /* CUT ver */
0150               0 << 20 |         /* interface 2/2 */
0151               0x04 << 16 |      /* platform */
0152               rtlhal->package_type << 12 |
0153               intf << 8 |           /* interface 1/2 */
0154               board_type;
0155
0156     u32 driver2 = rtlhal->type_glna <<  0 |
0157               rtlhal->type_gpa  <<  8 |
0158               rtlhal->type_alna << 16 |
0159               rtlhal->type_apa  << 24;
0160
0161     rtl_dbg(rtlpriv, COMP_INIT, DBG_TRACE,
0162         "===> [8812A] CheckPositive (cond1, cond2) = (0x%X 0x%X)\n",
0163         cond1, cond2);
0164     rtl_dbg(rtlpriv, COMP_INIT, DBG_TRACE,
0165         "===> [8812A] CheckPositive (driver1, driver2) = (0x%X 0x%X)\n",
0166         driver1, driver2);
0167
0168     rtl_dbg(rtlpriv, COMP_INIT, DBG_TRACE,
0169         "(Platform, Interface) = (0x%X, 0x%X)\n", 0x04, intf);
0170     rtl_dbg(rtlpriv, COMP_INIT, DBG_TRACE,
0171         "(Board, Package) = (0x%X, 0x%X)\n",
0172         rtlhal->board_type, rtlhal->package_type);
0173
0174     /*============== Value Defined Check ===============*/
0175     /*QFN Type [15:12] and Cut Version [27:24] need to do value check*/
0176
0177     if (((cond1 & 0x0000F000) != 0) && ((cond1 & 0x0000F000) !=
0178         (driver1 & 0x0000F000)))
0179         return false;
0180     if (((cond1 & 0x0F000000) != 0) && ((cond1 & 0x0F000000) !=
0181         (driver1 & 0x0F000000)))
0182         return false;
0183
0184     /*=============== Bit Defined Check ================*/
0185     /* We don't care [31:28] */
0186
0187     cond1   &= 0x00FF0FFF;
0188     driver1 &= 0x00FF0FFF;
0189
0190     if ((cond1 & driver1) == cond1) {
0191         u32 mask = 0;
0192
0193         if ((cond1 & 0x0F) == 0) /* BoardType is DONTCARE*/
0194             return true;
0195
0196         if ((cond1 & BIT(0)) != 0) /*GLNA*/
0197             mask |= 0x000000FF;
0198         if ((cond1 & BIT(1)) != 0) /*GPA*/
0199             mask |= 0x0000FF00;
0200         if ((cond1 & BIT(2)) != 0) /*ALNA*/
0201             mask |= 0x00FF0000;
0202         if ((cond1 & BIT(3)) != 0) /*APA*/
0203             mask |= 0xFF000000;
0204
0205         /* BoardType of each RF path is matched*/
0206         if ((cond2 & mask) == (driver2 & mask))
0207             return true;
0208         else
0209             return false;
0210     }
0211     return false;
0212 }
0213
0214 static void _rtl8723be_config_rf_reg(struct ieee80211_hw *hw, u32 addr,
0215                      u32 data, enum radio_path rfpath,
0216                      u32 regaddr)
0217 {
0218     if (addr == 0xfe || addr == 0xffe) {
0219         /* In order not to disturb BT music
0220          *  when wifi init.(1ant NIC only)
0221          */
0222         mdelay(50);
0223     } else {
0224         rtl_set_rfreg(hw, rfpath, regaddr, RFREG_OFFSET_MASK, data);
0225         udelay(1);
0226     }
0227 }
0228 static void _rtl8723be_config_rf_radio_a(struct ieee80211_hw *hw,
0229                      u32 addr, u32 data)
0230 {
0231     u32 content = 0x1000; /*RF Content: radio_a_txt*/
0232     u32 maskforphyset = (u32)(content & 0xE000);
0233
0234     _rtl8723be_config_rf_reg(hw, addr, data, RF90_PATH_A,
0235                  addr | maskforphyset);
0236
0237 }
0238
0239 static void _rtl8723be_phy_init_tx_power_by_rate(struct ieee80211_hw *hw)
0240 {
0241     struct rtl_priv *rtlpriv = rtl_priv(hw);
0242     struct rtl_phy *rtlphy = &rtlpriv->phy;
0243
0244     u8 band, path, txnum, section;
0245
0246     for (band = BAND_ON_2_4G; band <= BAND_ON_5G; ++band)
0247         for (path = 0; path < TX_PWR_BY_RATE_NUM_RF; ++path)
0248             for (txnum = 0; txnum < TX_PWR_BY_RATE_NUM_RF; ++txnum)
0249                 for (section = 0;
0250                      section < TX_PWR_BY_RATE_NUM_SECTION;
0251                      ++section)
0252                     rtlphy->tx_power_by_rate_offset
0253                       [band][path][txnum][section] = 0;
0254 }
0255
0256 static void _rtl8723be_config_bb_reg(struct ieee80211_hw *hw,
0257                      u32 addr, u32 data)
0258 {
0259     if (addr == 0xfe) {
0260         mdelay(50);
0261     } else if (addr == 0xfd) {
0262         mdelay(5);
0263     } else if (addr == 0xfc) {
0264         mdelay(1);
0265     } else if (addr == 0xfb) {
0266         udelay(50);
0267     } else if (addr == 0xfa) {
0268         udelay(5);
0269     } else if (addr == 0xf9) {
0270         udelay(1);
0271     } else {
0272         rtl_set_bbreg(hw, addr, MASKDWORD, data);
0273         udelay(1);
0274     }
0275 }
0276
0277 static void _rtl8723be_phy_set_txpower_by_rate_base(struct ieee80211_hw *hw,
0278                             u8 band,
0279                             u8 path, u8 rate_section,
0280                             u8 txnum, u8 value)
0281 {
0282     struct rtl_priv *rtlpriv = rtl_priv(hw);
0283     struct rtl_phy *rtlphy = &rtlpriv->phy;
0284
0285     if (path > RF90_PATH_D) {
0286         rtl_dbg(rtlpriv, COMP_INIT, DBG_LOUD,
0287             "Invalid Rf Path %d in phy_SetTxPowerByRatBase()\n",
0288             path);
0289         return;
0290     }
0291
0292     if (band == BAND_ON_2_4G) {
0293         switch (rate_section) {
0294         case CCK:
0295             rtlphy->txpwr_by_rate_base_24g[path][txnum][0] = value;
0296             break;
0297         case OFDM:
0298             rtlphy->txpwr_by_rate_base_24g[path][txnum][1] = value;
0299             break;
0300         case HT_MCS0_MCS7:
0301             rtlphy->txpwr_by_rate_base_24g[path][txnum][2] = value;
0302             break;
0303         case HT_MCS8_MCS15:
0304             rtlphy->txpwr_by_rate_base_24g[path][txnum][3] = value;
0305             break;
0306         default:
0307             rtl_dbg(rtlpriv, COMP_INIT, DBG_LOUD,
0308                 "Invalid RateSection %d in Band 2.4G, Rf Path %d, %dTx in PHY_SetTxPowerByRateBase()\n",
0309                 rate_section, path, txnum);
0310             break;
0311         }
0312     } else {
0313         rtl_dbg(rtlpriv, COMP_INIT, DBG_LOUD,
0314             "Invalid Band %d in PHY_SetTxPowerByRateBase()\n",
0315             band);
0316     }
0317
0318 }
0319
0320 static u8 _rtl8723be_phy_get_txpower_by_rate_base(struct ieee80211_hw *hw,
0321                           u8 band, u8 path, u8 txnum,
0322                           u8 rate_section)
0323 {
0324     struct rtl_priv *rtlpriv = rtl_priv(hw);
0325     struct rtl_phy *rtlphy = &rtlpriv->phy;
0326     u8 value = 0;
0327     if (path > RF90_PATH_D) {
0328         rtl_dbg(rtlpriv, COMP_INIT, DBG_LOUD,
0329             "Invalid Rf Path %d in PHY_GetTxPowerByRateBase()\n",
0330             path);
0331         return 0;
0332     }
0333
0334     if (band == BAND_ON_2_4G) {
0335         switch (rate_section) {
0336         case CCK:
0337             value = rtlphy->txpwr_by_rate_base_24g[path][txnum][0];
0338             break;
0339         case OFDM:
0340             value = rtlphy->txpwr_by_rate_base_24g[path][txnum][1];
0341             break;
0342         case HT_MCS0_MCS7:
0343             value = rtlphy->txpwr_by_rate_base_24g[path][txnum][2];
0344             break;
0345         case HT_MCS8_MCS15:
0346             value = rtlphy->txpwr_by_rate_base_24g[path][txnum][3];
0347             break;
0348         default:
0349             rtl_dbg(rtlpriv, COMP_INIT, DBG_LOUD,
0350                 "Invalid RateSection %d in Band 2.4G, Rf Path %d, %dTx in PHY_GetTxPowerByRateBase()\n",
0351                 rate_section, path, txnum);
0352             break;
0353         }
0354     } else {
0355         rtl_dbg(rtlpriv, COMP_INIT, DBG_LOUD,
0356             "Invalid Band %d in PHY_GetTxPowerByRateBase()\n",
0357             band);
0358     }
0359
0360     return value;
0361 }
0362
0363 static void _rtl8723be_phy_store_txpower_by_rate_base(struct ieee80211_hw *hw)
0364 {
0365     struct rtl_priv *rtlpriv = rtl_priv(hw);
0366     struct rtl_phy *rtlphy = &rtlpriv->phy;
0367     u16 rawvalue = 0;
0368     u8 base = 0, path = 0;
0369
0370     for (path = RF90_PATH_A; path <= RF90_PATH_B; ++path) {
0371         if (path == RF90_PATH_A) {
0372             rawvalue = (u16)(rtlphy->tx_power_by_rate_offset
0373                 [BAND_ON_2_4G][path][RF_1TX][3] >> 24) & 0xFF;
0374             base = (rawvalue >> 4) * 10 + (rawvalue & 0xF);
0375             _rtl8723be_phy_set_txpower_by_rate_base(hw,
0376                 BAND_ON_2_4G, path, CCK, RF_1TX, base);
0377         } else if (path == RF90_PATH_B) {
0378             rawvalue = (u16)(rtlphy->tx_power_by_rate_offset
0379                 [BAND_ON_2_4G][path][RF_1TX][3] >> 0) & 0xFF;
0380             base = (rawvalue >> 4) * 10 + (rawvalue & 0xF);
0381             _rtl8723be_phy_set_txpower_by_rate_base(hw,
0382                                 BAND_ON_2_4G,
0383                                 path, CCK,
0384                                 RF_1TX, base);
0385         }
0386         rawvalue = (u16)(rtlphy->tx_power_by_rate_offset
0387                 [BAND_ON_2_4G][path][RF_1TX][1] >> 24) & 0xFF;
0388         base = (rawvalue >> 4) * 10 + (rawvalue & 0xF);
0389         _rtl8723be_phy_set_txpower_by_rate_base(hw, BAND_ON_2_4G,
0390                             path, OFDM, RF_1TX,
0391                             base);
0392
0393         rawvalue = (u16)(rtlphy->tx_power_by_rate_offset
0394                 [BAND_ON_2_4G][path][RF_1TX][5] >> 24) & 0xFF;
0395         base = (rawvalue >> 4) * 10 + (rawvalue & 0xF);
0396         _rtl8723be_phy_set_txpower_by_rate_base(hw, BAND_ON_2_4G,
0397                             path, HT_MCS0_MCS7,
0398                             RF_1TX, base);
0399
0400         rawvalue = (u16)(rtlphy->tx_power_by_rate_offset
0401                 [BAND_ON_2_4G][path][RF_2TX][7] >> 24) & 0xFF;
0402         base = (rawvalue >> 4) * 10 + (rawvalue & 0xF);
0403         _rtl8723be_phy_set_txpower_by_rate_base(hw, BAND_ON_2_4G,
0404                             path, HT_MCS8_MCS15,
0405                             RF_2TX, base);
0406     }
0407 }
0408
0409 static void _phy_convert_txpower_dbm_to_relative_value(u32 *data, u8 start,
0410                         u8 end, u8 base_val)
0411 {
0412     s8 i = 0;
0413     u8 temp_value = 0;
0414     u32 temp_data = 0;
0415
0416     for (i = 3; i >= 0; --i) {
0417         if (i >= start && i <= end) {
0418             /* Get the exact value */
0419             temp_value = (u8)(*data >> (i * 8)) & 0xF;
0420             temp_value += ((u8)((*data >> (i*8 + 4)) & 0xF)) * 10;
0421
0422             /* Change the value to a relative value */
0423             temp_value = (temp_value > base_val) ?
0424                      temp_value - base_val :
0425                      base_val - temp_value;
0426         } else {
0427             temp_value = (u8)(*data >> (i * 8)) & 0xFF;
0428         }
0429         temp_data <<= 8;
0430         temp_data |= temp_value;
0431     }
0432     *data = temp_data;
0433 }
0434
0435 static void _rtl8723be_phy_convert_txpower_dbm_to_relative_value(
0436                             struct ieee80211_hw *hw)
0437 {
0438     struct rtl_priv *rtlpriv = rtl_priv(hw);
0439     struct rtl_phy *rtlphy = &rtlpriv->phy;
0440     u8 base = 0, rfpath = RF90_PATH_A;
0441
0442     base = _rtl8723be_phy_get_txpower_by_rate_base(hw,
0443             BAND_ON_2_4G, rfpath, RF_1TX, CCK);
0444     _phy_convert_txpower_dbm_to_relative_value(
0445         &rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfpath][RF_1TX][2],
0446         1, 1, base);
0447     _phy_convert_txpower_dbm_to_relative_value(
0448         &rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfpath][RF_1TX][3],
0449         1, 3, base);
0450
0451     base = _rtl8723be_phy_get_txpower_by_rate_base(hw, BAND_ON_2_4G, rfpath,
0452                                RF_1TX, OFDM);
0453     _phy_convert_txpower_dbm_to_relative_value(
0454         &rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfpath][RF_1TX][0],
0455         0, 3, base);
0456     _phy_convert_txpower_dbm_to_relative_value(
0457         &rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfpath][RF_1TX][1],
0458         0, 3, base);
0459
0460     base = _rtl8723be_phy_get_txpower_by_rate_base(hw, BAND_ON_2_4G,
0461                         rfpath, RF_1TX, HT_MCS0_MCS7);
0462     _phy_convert_txpower_dbm_to_relative_value(
0463         &rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfpath][RF_1TX][4],
0464         0, 3, base);
0465     _phy_convert_txpower_dbm_to_relative_value(
0466         &rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfpath][RF_1TX][5],
0467         0, 3, base);
0468
0469     base = _rtl8723be_phy_get_txpower_by_rate_base(hw, BAND_ON_2_4G,
0470                                rfpath, RF_2TX,
0471                                HT_MCS8_MCS15);
0472     _phy_convert_txpower_dbm_to_relative_value(
0473         &rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfpath][RF_2TX][6],
0474         0, 3, base);
0475
0476     _phy_convert_txpower_dbm_to_relative_value(
0477         &rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfpath][RF_2TX][7],
0478         0, 3, base);
0479
0480     rtl_dbg(rtlpriv, COMP_POWER, DBG_TRACE,
0481         "<===%s\n", __func__);
0482 }
0483
0484 static void phy_txpower_by_rate_config(struct ieee80211_hw *hw)
0485 {
0486     _rtl8723be_phy_store_txpower_by_rate_base(hw);
0487     _rtl8723be_phy_convert_txpower_dbm_to_relative_value(hw);
0488 }
0489
0490 static bool _rtl8723be_phy_bb8723b_config_parafile(struct ieee80211_hw *hw)
0491 {
0492     struct rtl_priv *rtlpriv = rtl_priv(hw);
0493     struct rtl_phy *rtlphy = &rtlpriv->phy;
0494     struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
0495     bool rtstatus;
0496
0497     /* switch ant to BT */
0498     if (rtlpriv->rtlhal.interface == INTF_USB) {
0499         rtl_write_dword(rtlpriv, 0x948, 0x0);
0500     } else {
0501         if (rtlpriv->btcoexist.btc_info.single_ant_path == 0)
0502             rtl_write_dword(rtlpriv, 0x948, 0x280);
0503         else
0504             rtl_write_dword(rtlpriv, 0x948, 0x0);
0505     }
0506
0507     rtstatus = _rtl8723be_phy_config_bb_with_headerfile(hw,
0508                         BASEBAND_CONFIG_PHY_REG);
0509     if (!rtstatus) {
0510         pr_err("Write BB Reg Fail!!\n");
0511         return false;
0512     }
0513     _rtl8723be_phy_init_tx_power_by_rate(hw);
0514     if (!rtlefuse->autoload_failflag) {
0515         rtlphy->pwrgroup_cnt = 0;
0516         rtstatus = _rtl8723be_phy_config_bb_with_pgheaderfile(hw,
0517                         BASEBAND_CONFIG_PHY_REG);
0518     }
0519     phy_txpower_by_rate_config(hw);
0520     if (!rtstatus) {
0521         pr_err("BB_PG Reg Fail!!\n");
0522         return false;
0523     }
0524     rtstatus = _rtl8723be_phy_config_bb_with_headerfile(hw,
0525                         BASEBAND_CONFIG_AGC_TAB);
0526     if (!rtstatus) {
0527         pr_err("AGC Table Fail\n");
0528         return false;
0529     }
0530     rtlphy->cck_high_power = (bool)(rtl_get_bbreg(hw,
0531                               RFPGA0_XA_HSSIPARAMETER2,
0532                               0x200));
0533     return true;
0534 }
0535
0536 static bool rtl8723be_phy_config_with_headerfile(struct ieee80211_hw *hw,
0537                          u32 *array_table,
0538                          u16 arraylen,
0539         void (*set_reg)(struct ieee80211_hw *hw, u32 regaddr, u32 data))
0540 {
0541     #define COND_ELSE  2
0542     #define COND_ENDIF 3
0543
0544     int i = 0;
0545     u8 cond;
0546     bool matched = true, skipped = false;
0547
0548     while ((i + 1) < arraylen) {
0549         u32 v1 = array_table[i];
0550         u32 v2 = array_table[i + 1];
0551
0552         if (v1 & (BIT(31) | BIT(30))) {/*positive & negative condition*/
0553             if (v1 & BIT(31)) {/* positive condition*/
0554                 cond  = (u8)((v1 & (BIT(29) | BIT(28))) >> 28);
0555                 if (cond == COND_ENDIF) { /*end*/
0556                     matched = true;
0557                     skipped = false;
0558                 } else if (cond == COND_ELSE) { /*else*/
0559                     matched = skipped ? false : true;
0560                 } else {/*if , else if*/
0561                     if (skipped) {
0562                         matched = false;
0563                     } else {
0564                         if (_rtl8723be_check_positive(
0565                                 hw, v1, v2)) {
0566                             matched = true;
0567                             skipped = true;
0568                         } else {
0569                             matched = false;
0570                             skipped = false;
0571                         }
0572                     }
0573                 }
0574             } else if (v1 & BIT(30)) { /*negative condition*/
0575             /*do nothing*/
0576             }
0577         } else {
0578             if (matched)
0579                 set_reg(hw, v1, v2);
0580         }
0581         i = i + 2;
0582     }
0583
0584     return true;
0585 }
0586
0587 static bool _rtl8723be_phy_config_mac_with_headerfile(struct ieee80211_hw *hw)
0588 {
0589     struct rtl_priv *rtlpriv = rtl_priv(hw);
0590
0591     rtl_dbg(rtlpriv, COMP_INIT, DBG_TRACE, "Read rtl8723beMACPHY_Array\n");
0592
0593     return rtl8723be_phy_config_with_headerfile(hw,
0594             RTL8723BEMAC_1T_ARRAY, RTL8723BEMAC_1T_ARRAYLEN,
0595             rtl_write_byte_with_val32);
0596 }
0597
0598 static bool _rtl8723be_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
0599                              u8 configtype)
0600 {
0601
0602     if (configtype == BASEBAND_CONFIG_PHY_REG)
0603         return rtl8723be_phy_config_with_headerfile(hw,
0604                 RTL8723BEPHY_REG_1TARRAY,
0605                 RTL8723BEPHY_REG_1TARRAYLEN,
0606                 _rtl8723be_config_bb_reg);
0607     else if (configtype == BASEBAND_CONFIG_AGC_TAB)
0608         return rtl8723be_phy_config_with_headerfile(hw,
0609                 RTL8723BEAGCTAB_1TARRAY,
0610                 RTL8723BEAGCTAB_1TARRAYLEN,
0611                 rtl_set_bbreg_with_dwmask);
0612
0613     return false;
0614 }
0615
0616 static u8 _rtl8723be_get_rate_section_index(u32 regaddr)
0617 {
0618     u8 index = 0;
0619
0620     switch (regaddr) {
0621     case RTXAGC_A_RATE18_06:
0622         index = 0;
0623     break;
0624     case RTXAGC_A_RATE54_24:
0625         index = 1;
0626     break;
0627     case RTXAGC_A_CCK1_MCS32:
0628         index = 2;
0629     break;
0630     case RTXAGC_B_CCK11_A_CCK2_11:
0631         index = 3;
0632     break;
0633     case RTXAGC_A_MCS03_MCS00:
0634         index = 4;
0635     break;
0636     case RTXAGC_A_MCS07_MCS04:
0637         index = 5;
0638     break;
0639     case RTXAGC_A_MCS11_MCS08:
0640         index = 6;
0641     break;
0642     case RTXAGC_A_MCS15_MCS12:
0643         index = 7;
0644     break;
0645     case RTXAGC_B_RATE18_06:
0646         index = 0;
0647     break;
0648     case RTXAGC_B_RATE54_24:
0649         index = 1;
0650     break;
0651     case RTXAGC_B_CCK1_55_MCS32:
0652         index = 2;
0653     break;
0654     case RTXAGC_B_MCS03_MCS00:
0655         index = 4;
0656     break;
0657     case RTXAGC_B_MCS07_MCS04:
0658         index = 5;
0659     break;
0660     case RTXAGC_B_MCS11_MCS08:
0661         index = 6;
0662     break;
0663     case RTXAGC_B_MCS15_MCS12:
0664         index = 7;
0665     break;
0666     default:
0667         regaddr &= 0xFFF;
0668         if (regaddr >= 0xC20 && regaddr <= 0xC4C)
0669             index = (u8)((regaddr - 0xC20) / 4);
0670         else if (regaddr >= 0xE20 && regaddr <= 0xE4C)
0671             index = (u8)((regaddr - 0xE20) / 4);
0672         break;
0673     }
0674     return index;
0675 }
0676
0677 static void _rtl8723be_store_tx_power_by_rate(struct ieee80211_hw *hw,
0678                           u32 band, u32 rfpath,
0679                           u32 txnum, u32 regaddr,
0680                           u32 bitmask, u32 data)
0681 {
0682     struct rtl_priv *rtlpriv = rtl_priv(hw);
0683     struct rtl_phy *rtlphy = &rtlpriv->phy;
0684     u8 rate_section = _rtl8723be_get_rate_section_index(regaddr);
0685
0686     if (band != BAND_ON_2_4G && band != BAND_ON_5G) {
0687         rtl_dbg(rtlpriv, FPHY, PHY_TXPWR, "Invalid Band %d\n", band);
0688         return;
0689     }
0690     if (rfpath > MAX_RF_PATH - 1) {
0691         rtl_dbg(rtlpriv, FPHY, PHY_TXPWR,
0692             "Invalid RfPath %d\n", rfpath);
0693         return;
0694     }
0695     if (txnum > MAX_RF_PATH - 1) {
0696         rtl_dbg(rtlpriv, FPHY, PHY_TXPWR, "Invalid TxNum %d\n", txnum);
0697         return;
0698     }
0699
0700     rtlphy->tx_power_by_rate_offset[band][rfpath][txnum][rate_section] =
0701                                     data;
0702
0703 }
0704
0705 static bool _rtl8723be_phy_config_bb_with_pgheaderfile(struct ieee80211_hw *hw,
0706                                u8 configtype)
0707 {
0708     struct rtl_priv *rtlpriv = rtl_priv(hw);
0709     int i;
0710     u32 *phy_regarray_table_pg;
0711     u16 phy_regarray_pg_len;
0712     u32 v1 = 0, v2 = 0, v3 = 0, v4 = 0, v5 = 0, v6 = 0;
0713
0714     phy_regarray_pg_len = RTL8723BEPHY_REG_ARRAY_PGLEN;
0715     phy_regarray_table_pg = RTL8723BEPHY_REG_ARRAY_PG;
0716
0717     if (configtype == BASEBAND_CONFIG_PHY_REG) {
0718         for (i = 0; i < phy_regarray_pg_len; i = i + 6) {
0719             v1 = phy_regarray_table_pg[i];
0720             v2 = phy_regarray_table_pg[i+1];
0721             v3 = phy_regarray_table_pg[i+2];
0722             v4 = phy_regarray_table_pg[i+3];
0723             v5 = phy_regarray_table_pg[i+4];
0724             v6 = phy_regarray_table_pg[i+5];
0725
0726             if (v1 < 0xcdcdcdcd) {
0727                 if (phy_regarray_table_pg[i] == 0xfe ||
0728                     phy_regarray_table_pg[i] == 0xffe)
0729                     mdelay(50);
0730                 else
0731                     _rtl8723be_store_tx_power_by_rate(hw,
0732                             v1, v2, v3, v4, v5, v6);
0733                 continue;
0734             }
0735         }
0736     } else {
0737         rtl_dbg(rtlpriv, COMP_SEND, DBG_TRACE,
0738             "configtype != BaseBand_Config_PHY_REG\n");
0739     }
0740     return true;
0741 }
0742
0743 bool rtl8723be_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
0744                          enum radio_path rfpath)
0745 {
0746     struct rtl_priv *rtlpriv = rtl_priv(hw);
0747     struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
0748     bool ret = true;
0749
0750     rtl_dbg(rtlpriv, COMP_INIT, DBG_LOUD, "Radio No %x\n", rfpath);
0751     switch (rfpath) {
0752     case RF90_PATH_A:
0753         ret =  rtl8723be_phy_config_with_headerfile(hw,
0754                 RTL8723BE_RADIOA_1TARRAY,
0755                 RTL8723BE_RADIOA_1TARRAYLEN,
0756                 _rtl8723be_config_rf_radio_a);
0757
0758         if (rtlhal->oem_id == RT_CID_819X_HP)
0759             _rtl8723be_config_rf_radio_a(hw, 0x52, 0x7E4BD);
0760         break;
0761     case RF90_PATH_B:
0762     case RF90_PATH_C:
0763         break;
0764     case RF90_PATH_D:
0765         rtl_dbg(rtlpriv, COMP_ERR, DBG_LOUD,
0766             "switch case %#x not processed\n", rfpath);
0767         break;
0768     }
0769     return ret;
0770 }
0771
0772 void rtl8723be_phy_get_hw_reg_originalvalue(struct ieee80211_hw *hw)
0773 {
0774     struct rtl_priv *rtlpriv = rtl_priv(hw);
0775     struct rtl_phy *rtlphy = &rtlpriv->phy;
0776
0777     rtlphy->default_initialgain[0] =
0778         (u8)rtl_get_bbreg(hw, ROFDM0_XAAGCCORE1, MASKBYTE0);
0779     rtlphy->default_initialgain[1] =
0780         (u8)rtl_get_bbreg(hw, ROFDM0_XBAGCCORE1, MASKBYTE0);
0781     rtlphy->default_initialgain[2] =
0782         (u8)rtl_get_bbreg(hw, ROFDM0_XCAGCCORE1, MASKBYTE0);
0783     rtlphy->default_initialgain[3] =
0784         (u8)rtl_get_bbreg(hw, ROFDM0_XDAGCCORE1, MASKBYTE0);
0785
0786     rtl_dbg(rtlpriv, COMP_INIT, DBG_TRACE,
0787         "Default initial gain (c50=0x%x, c58=0x%x, c60=0x%x, c68=0x%x\n",
0788         rtlphy->default_initialgain[0],
0789         rtlphy->default_initialgain[1],
0790         rtlphy->default_initialgain[2],
0791         rtlphy->default_initialgain[3]);
0792
0793     rtlphy->framesync = (u8)rtl_get_bbreg(hw, ROFDM0_RXDETECTOR3,
0794                            MASKBYTE0);
0795     rtlphy->framesync_c34 = rtl_get_bbreg(hw, ROFDM0_RXDETECTOR2,
0796                           MASKDWORD);
0797
0798     rtl_dbg(rtlpriv, COMP_INIT, DBG_TRACE,
0799         "Default framesync (0x%x) = 0x%x\n",
0800         ROFDM0_RXDETECTOR3, rtlphy->framesync);
0801 }
0802
0803 static u8 _rtl8723be_phy_get_ratesection_intxpower_byrate(enum radio_path path,
0804                               u8 rate)
0805 {
0806     u8 rate_section = 0;
0807
0808     switch (rate) {
0809     case DESC92C_RATE1M:
0810         rate_section = 2;
0811         break;
0812
0813     case DESC92C_RATE2M:
0814     case DESC92C_RATE5_5M:
0815         if (path == RF90_PATH_A)
0816             rate_section = 3;
0817         else if (path == RF90_PATH_B)
0818             rate_section = 2;
0819         break;
0820
0821     case DESC92C_RATE11M:
0822         rate_section = 3;
0823         break;
0824
0825     case DESC92C_RATE6M:
0826     case DESC92C_RATE9M:
0827     case DESC92C_RATE12M:
0828     case DESC92C_RATE18M:
0829         rate_section = 0;
0830         break;
0831
0832     case DESC92C_RATE24M:
0833     case DESC92C_RATE36M:
0834     case DESC92C_RATE48M:
0835     case DESC92C_RATE54M:
0836         rate_section = 1;
0837         break;
0838
0839     case DESC92C_RATEMCS0:
0840     case DESC92C_RATEMCS1:
0841     case DESC92C_RATEMCS2:
0842     case DESC92C_RATEMCS3:
0843         rate_section = 4;
0844         break;
0845
0846     case DESC92C_RATEMCS4:
0847     case DESC92C_RATEMCS5:
0848     case DESC92C_RATEMCS6:
0849     case DESC92C_RATEMCS7:
0850         rate_section = 5;
0851         break;
0852
0853     case DESC92C_RATEMCS8:
0854     case DESC92C_RATEMCS9:
0855     case DESC92C_RATEMCS10:
0856     case DESC92C_RATEMCS11:
0857         rate_section = 6;
0858         break;
0859
0860     case DESC92C_RATEMCS12:
0861     case DESC92C_RATEMCS13:
0862     case DESC92C_RATEMCS14:
0863     case DESC92C_RATEMCS15:
0864         rate_section = 7;
0865         break;
0866
0867     default:
0868         WARN_ONCE(true, "rtl8723be: Rate_Section is Illegal\n");
0869         break;
0870     }
0871
0872     return rate_section;
0873 }
0874
0875 static u8 _rtl8723be_get_txpower_by_rate(struct ieee80211_hw *hw,
0876                      enum band_type band,
0877                      enum radio_path rfpath, u8 rate)
0878 {
0879     struct rtl_priv *rtlpriv = rtl_priv(hw);
0880     struct rtl_phy *rtlphy = &rtlpriv->phy;
0881     u8 shift = 0, rate_section, tx_num;
0882     s8 tx_pwr_diff = 0;
0883
0884     rate_section = _rtl8723be_phy_get_ratesection_intxpower_byrate(rfpath,
0885                                        rate);
0886     tx_num = RF_TX_NUM_NONIMPLEMENT;
0887
0888     if (tx_num == RF_TX_NUM_NONIMPLEMENT) {
0889         if (rate >= DESC92C_RATEMCS8 && rate <= DESC92C_RATEMCS15)
0890             tx_num = RF_2TX;
0891         else
0892             tx_num = RF_1TX;
0893     }
0894
0895     switch (rate) {
0896     case DESC92C_RATE6M:
0897     case DESC92C_RATE24M:
0898     case DESC92C_RATEMCS0:
0899     case DESC92C_RATEMCS4:
0900     case DESC92C_RATEMCS8:
0901     case DESC92C_RATEMCS12:
0902         shift = 0;
0903         break;
0904     case DESC92C_RATE1M:
0905     case DESC92C_RATE2M:
0906     case DESC92C_RATE9M:
0907     case DESC92C_RATE36M:
0908     case DESC92C_RATEMCS1:
0909     case DESC92C_RATEMCS5:
0910     case DESC92C_RATEMCS9:
0911     case DESC92C_RATEMCS13:
0912         shift = 8;
0913         break;
0914     case DESC92C_RATE5_5M:
0915     case DESC92C_RATE12M:
0916     case DESC92C_RATE48M:
0917     case DESC92C_RATEMCS2:
0918     case DESC92C_RATEMCS6:
0919     case DESC92C_RATEMCS10:
0920     case DESC92C_RATEMCS14:
0921         shift = 16;
0922         break;
0923     case DESC92C_RATE11M:
0924     case DESC92C_RATE18M:
0925     case DESC92C_RATE54M:
0926     case DESC92C_RATEMCS3:
0927     case DESC92C_RATEMCS7:
0928     case DESC92C_RATEMCS11:
0929     case DESC92C_RATEMCS15:
0930         shift = 24;
0931         break;
0932     default:
0933         WARN_ONCE(true, "rtl8723be: Rate_Section is Illegal\n");
0934         break;
0935     }
0936     tx_pwr_diff = (u8)(rtlphy->tx_power_by_rate_offset[band][rfpath][tx_num]
0937                       [rate_section] >> shift) & 0xff;
0938
0939     return  tx_pwr_diff;
0940 }
0941
0942 static u8 _rtl8723be_get_txpower_index(struct ieee80211_hw *hw, u8 path,
0943                        u8 rate, u8 bandwidth, u8 channel)
0944 {
0945     struct rtl_priv *rtlpriv = rtl_priv(hw);
0946     struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
0947     u8 index = (channel - 1);
0948     u8 txpower = 0;
0949     u8 power_diff_byrate = 0;
0950
0951     if (channel > 14 || channel < 1) {
0952         index = 0;
0953         rtl_dbg(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
0954             "Illegal channel!\n");
0955     }
0956     if (RX_HAL_IS_CCK_RATE(rate))
0957         txpower = rtlefuse->txpwrlevel_cck[path][index];
0958     else if (DESC92C_RATE6M <= rate)
0959         txpower = rtlefuse->txpwrlevel_ht40_1s[path][index];
0960     else
0961         rtl_dbg(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
0962             "invalid rate\n");
0963
0964     if (DESC92C_RATE6M <= rate && rate <= DESC92C_RATE54M &&
0965         !RX_HAL_IS_CCK_RATE(rate))
0966         txpower += rtlefuse->txpwr_legacyhtdiff[0][TX_1S];
0967
0968     if (bandwidth == HT_CHANNEL_WIDTH_20) {
0969         if (DESC92C_RATEMCS0 <= rate && rate <= DESC92C_RATEMCS15)
0970             txpower += rtlefuse->txpwr_ht20diff[0][TX_1S];
0971         if (DESC92C_RATEMCS8 <= rate && rate <= DESC92C_RATEMCS15)
0972             txpower += rtlefuse->txpwr_ht20diff[0][TX_2S];
0973     } else if (bandwidth == HT_CHANNEL_WIDTH_20_40) {
0974         if (DESC92C_RATEMCS0 <= rate && rate <= DESC92C_RATEMCS15)
0975             txpower += rtlefuse->txpwr_ht40diff[0][TX_1S];
0976         if (DESC92C_RATEMCS8 <= rate && rate <= DESC92C_RATEMCS15)
0977             txpower += rtlefuse->txpwr_ht40diff[0][TX_2S];
0978     }
0979
0980     if (rtlefuse->eeprom_regulatory != 2)
0981         power_diff_byrate = _rtl8723be_get_txpower_by_rate(hw,
0982                                    BAND_ON_2_4G,
0983                                    path, rate);
0984
0985     txpower += power_diff_byrate;
0986
0987     if (txpower > MAX_POWER_INDEX)
0988         txpower = MAX_POWER_INDEX;
0989
0990     return txpower;
0991 }
0992
0993 static void _rtl8723be_phy_set_txpower_index(struct ieee80211_hw *hw,
0994                          u8 power_index, u8 path, u8 rate)
0995 {
0996     struct rtl_priv *rtlpriv = rtl_priv(hw);
0997     if (path == RF90_PATH_A) {
0998         switch (rate) {
0999         case DESC92C_RATE1M:
1000             rtl8723_phy_set_bb_reg(hw, RTXAGC_A_CCK1_MCS32,
1001                            MASKBYTE1, power_index);
1002             break;
1003         case DESC92C_RATE2M:
1004             rtl8723_phy_set_bb_reg(hw, RTXAGC_B_CCK11_A_CCK2_11,
1005                            MASKBYTE1, power_index);
1006             break;
1007         case DESC92C_RATE5_5M:
1008             rtl8723_phy_set_bb_reg(hw, RTXAGC_B_CCK11_A_CCK2_11,
1009                            MASKBYTE2, power_index);
1010             break;
1011         case DESC92C_RATE11M:
1012             rtl8723_phy_set_bb_reg(hw, RTXAGC_B_CCK11_A_CCK2_11,
1013                            MASKBYTE3, power_index);
1014             break;
1015
1016         case DESC92C_RATE6M:
1017             rtl8723_phy_set_bb_reg(hw, RTXAGC_A_RATE18_06,
1018                            MASKBYTE0, power_index);
1019             break;
1020         case DESC92C_RATE9M:
1021             rtl8723_phy_set_bb_reg(hw, RTXAGC_A_RATE18_06,
1022                            MASKBYTE1, power_index);
1023             break;
1024         case DESC92C_RATE12M:
1025             rtl8723_phy_set_bb_reg(hw, RTXAGC_A_RATE18_06,
1026                            MASKBYTE2, power_index);
1027             break;
1028         case DESC92C_RATE18M:
1029             rtl8723_phy_set_bb_reg(hw, RTXAGC_A_RATE18_06,
1030                            MASKBYTE3, power_index);
1031             break;
1032
1033         case DESC92C_RATE24M:
1034             rtl8723_phy_set_bb_reg(hw, RTXAGC_A_RATE54_24,
1035                            MASKBYTE0, power_index);
1036             break;
1037         case DESC92C_RATE36M:
1038             rtl8723_phy_set_bb_reg(hw, RTXAGC_A_RATE54_24,
1039                            MASKBYTE1, power_index);
1040             break;
1041         case DESC92C_RATE48M:
1042             rtl8723_phy_set_bb_reg(hw, RTXAGC_A_RATE54_24,
1043                            MASKBYTE2, power_index);
1044             break;
1045         case DESC92C_RATE54M:
1046             rtl8723_phy_set_bb_reg(hw, RTXAGC_A_RATE54_24,
1047                            MASKBYTE3, power_index);
1048             break;
1049
1050         case DESC92C_RATEMCS0:
1051             rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS03_MCS00,
1052                            MASKBYTE0, power_index);
1053             break;
1054         case DESC92C_RATEMCS1:
1055             rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS03_MCS00,
1056                            MASKBYTE1, power_index);
1057             break;
1058         case DESC92C_RATEMCS2:
1059             rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS03_MCS00,
1060                            MASKBYTE2, power_index);
1061             break;
1062         case DESC92C_RATEMCS3:
1063             rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS03_MCS00,
1064                            MASKBYTE3, power_index);
1065             break;
1066
1067         case DESC92C_RATEMCS4:
1068             rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS07_MCS04,
1069                            MASKBYTE0, power_index);
1070             break;
1071         case DESC92C_RATEMCS5:
1072             rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS07_MCS04,
1073                            MASKBYTE1, power_index);
1074             break;
1075         case DESC92C_RATEMCS6:
1076             rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS07_MCS04,
1077                            MASKBYTE2, power_index);
1078             break;
1079         case DESC92C_RATEMCS7:
1080             rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS07_MCS04,
1081                            MASKBYTE3, power_index);
1082             break;
1083
1084         case DESC92C_RATEMCS8:
1085             rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS11_MCS08,
1086                            MASKBYTE0, power_index);
1087             break;
1088         case DESC92C_RATEMCS9:
1089             rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS11_MCS08,
1090                            MASKBYTE1, power_index);
1091             break;
1092         case DESC92C_RATEMCS10:
1093             rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS11_MCS08,
1094                            MASKBYTE2, power_index);
1095             break;
1096         case DESC92C_RATEMCS11:
1097             rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS11_MCS08,
1098                            MASKBYTE3, power_index);
1099             break;
1100
1101         default:
1102             rtl_dbg(rtlpriv, COMP_POWER, DBG_LOUD, "Invalid Rate!!\n");
1103             break;
1104         }
1105     } else {
1106         rtl_dbg(rtlpriv, COMP_POWER, DBG_LOUD, "Invalid RFPath!!\n");
1107     }
1108 }
1109
1110 void rtl8723be_phy_set_txpower_level(struct ieee80211_hw *hw, u8 channel)
1111 {
1112     struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
1113     u8 cck_rates[]  = {DESC92C_RATE1M, DESC92C_RATE2M,
1114                DESC92C_RATE5_5M, DESC92C_RATE11M};
1115     u8 ofdm_rates[]  = {DESC92C_RATE6M, DESC92C_RATE9M,
1116                 DESC92C_RATE12M, DESC92C_RATE18M,
1117                 DESC92C_RATE24M, DESC92C_RATE36M,
1118                 DESC92C_RATE48M, DESC92C_RATE54M};
1119     u8 ht_rates_1t[]  = {DESC92C_RATEMCS0, DESC92C_RATEMCS1,
1120                  DESC92C_RATEMCS2, DESC92C_RATEMCS3,
1121                  DESC92C_RATEMCS4, DESC92C_RATEMCS5,
1122                  DESC92C_RATEMCS6, DESC92C_RATEMCS7};
1123     u8 i;
1124     u8 power_index;
1125
1126     if (!rtlefuse->txpwr_fromeprom)
1127         return;
1128
1129     for (i = 0; i < ARRAY_SIZE(cck_rates); i++) {
1130         power_index = _rtl8723be_get_txpower_index(hw, RF90_PATH_A,
1131                     cck_rates[i],
1132                     rtl_priv(hw)->phy.current_chan_bw,
1133                     channel);
1134         _rtl8723be_phy_set_txpower_index(hw, power_index, RF90_PATH_A,
1135                          cck_rates[i]);
1136     }
1137     for (i = 0; i < ARRAY_SIZE(ofdm_rates); i++) {
1138         power_index = _rtl8723be_get_txpower_index(hw, RF90_PATH_A,
1139                     ofdm_rates[i],
1140                     rtl_priv(hw)->phy.current_chan_bw,
1141                     channel);
1142         _rtl8723be_phy_set_txpower_index(hw, power_index, RF90_PATH_A,
1143                          ofdm_rates[i]);
1144     }
1145     for (i = 0; i < ARRAY_SIZE(ht_rates_1t); i++) {
1146         power_index = _rtl8723be_get_txpower_index(hw, RF90_PATH_A,
1147                     ht_rates_1t[i],
1148                     rtl_priv(hw)->phy.current_chan_bw,
1149                     channel);
1150         _rtl8723be_phy_set_txpower_index(hw, power_index, RF90_PATH_A,
1151                          ht_rates_1t[i]);
1152     }
1153 }
1154
1155 void rtl8723be_phy_scan_operation_backup(struct ieee80211_hw *hw, u8 operation)
1156 {
1157     struct rtl_priv *rtlpriv = rtl_priv(hw);
1158     struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
1159     enum io_type iotype;
1160
1161     if (!is_hal_stop(rtlhal)) {
1162         switch (operation) {
1163         case SCAN_OPT_BACKUP_BAND0:
1164             iotype = IO_CMD_PAUSE_BAND0_DM_BY_SCAN;
1165             rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_IO_CMD,
1166                               (u8 *)&iotype);
1167
1168             break;
1169         case SCAN_OPT_RESTORE:
1170             iotype = IO_CMD_RESUME_DM_BY_SCAN;
1171             rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_IO_CMD,
1172                               (u8 *)&iotype);
1173             break;
1174         default:
1175             pr_err("Unknown Scan Backup operation.\n");
1176             break;
1177         }
1178     }
1179 }
1180
1181 void rtl8723be_phy_set_bw_mode_callback(struct ieee80211_hw *hw)
1182 {
1183     struct rtl_priv *rtlpriv = rtl_priv(hw);
1184     struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
1185     struct rtl_phy *rtlphy = &rtlpriv->phy;
1186     struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
1187     u8 reg_bw_opmode;
1188     u8 reg_prsr_rsc;
1189
1190     rtl_dbg(rtlpriv, COMP_SCAN, DBG_TRACE,
1191         "Switch to %s bandwidth\n",
1192         rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20 ?
1193         "20MHz" : "40MHz");
1194
1195     if (is_hal_stop(rtlhal)) {
1196         rtlphy->set_bwmode_inprogress = false;
1197         return;
1198     }
1199
1200     reg_bw_opmode = rtl_read_byte(rtlpriv, REG_BWOPMODE);
1201     reg_prsr_rsc = rtl_read_byte(rtlpriv, REG_RRSR + 2);
1202
1203     switch (rtlphy->current_chan_bw) {
1204     case HT_CHANNEL_WIDTH_20:
1205         reg_bw_opmode |= BW_OPMODE_20MHZ;
1206         rtl_write_byte(rtlpriv, REG_BWOPMODE, reg_bw_opmode);
1207         break;
1208     case HT_CHANNEL_WIDTH_20_40:
1209         reg_bw_opmode &= ~BW_OPMODE_20MHZ;
1210         rtl_write_byte(rtlpriv, REG_BWOPMODE, reg_bw_opmode);
1211         reg_prsr_rsc = (reg_prsr_rsc & 0x90) |
1212                    (mac->cur_40_prime_sc << 5);
1213         rtl_write_byte(rtlpriv, REG_RRSR + 2, reg_prsr_rsc);
1214         break;
1215     default:
1216         pr_err("unknown bandwidth: %#X\n",
1217                rtlphy->current_chan_bw);
1218         break;
1219     }
1220
1221     switch (rtlphy->current_chan_bw) {
1222     case HT_CHANNEL_WIDTH_20:
1223         rtl_set_bbreg(hw, RFPGA0_RFMOD, BRFMOD, 0x0);
1224         rtl_set_bbreg(hw, RFPGA1_RFMOD, BRFMOD, 0x0);
1225     /*  rtl_set_bbreg(hw, RFPGA0_ANALOGPARAMETER2, BIT(10), 1);*/
1226         break;
1227     case HT_CHANNEL_WIDTH_20_40:
1228         rtl_set_bbreg(hw, RFPGA0_RFMOD, BRFMOD, 0x1);
1229         rtl_set_bbreg(hw, RFPGA1_RFMOD, BRFMOD, 0x1);
1230
1231         rtl_set_bbreg(hw, RCCK0_SYSTEM, BCCK_SIDEBAND,
1232                   (mac->cur_40_prime_sc >> 1));
1233         rtl_set_bbreg(hw, ROFDM1_LSTF, 0xC00, mac->cur_40_prime_sc);
1234         /*rtl_set_bbreg(hw, RFPGA0_ANALOGPARAMETER2, BIT(10), 0);*/
1235
1236         rtl_set_bbreg(hw, 0x818, (BIT(26) | BIT(27)),
1237                   (mac->cur_40_prime_sc ==
1238                    HAL_PRIME_CHNL_OFFSET_LOWER) ? 2 : 1);
1239         break;
1240     default:
1241         pr_err("unknown bandwidth: %#X\n",
1242                rtlphy->current_chan_bw);
1243         break;
1244     }
1245     rtl8723be_phy_rf6052_set_bandwidth(hw, rtlphy->current_chan_bw);
1246     rtlphy->set_bwmode_inprogress = false;
1247     rtl_dbg(rtlpriv, COMP_SCAN, DBG_LOUD, "\n");
1248 }
1249
1250 void rtl8723be_phy_set_bw_mode(struct ieee80211_hw *hw,
1251                 enum nl80211_channel_type ch_type)
1252 {
1253     struct rtl_priv *rtlpriv = rtl_priv(hw);
1254     struct rtl_phy *rtlphy = &rtlpriv->phy;
1255     struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
1256     u8 tmp_bw = rtlphy->current_chan_bw;
1257
1258     if (rtlphy->set_bwmode_inprogress)
1259         return;
1260     rtlphy->set_bwmode_inprogress = true;
1261     if ((!is_hal_stop(rtlhal)) && !(RT_CANNOT_IO(hw))) {
1262         rtl8723be_phy_set_bw_mode_callback(hw);
1263     } else {
1264         rtl_dbg(rtlpriv, COMP_ERR, DBG_WARNING,
1265             "false driver sleep or unload\n");
1266         rtlphy->set_bwmode_inprogress = false;
1267         rtlphy->current_chan_bw = tmp_bw;
1268     }
1269 }
1270
1271 void rtl8723be_phy_sw_chnl_callback(struct ieee80211_hw *hw)
1272 {
1273     struct rtl_priv *rtlpriv = rtl_priv(hw);
1274     struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
1275     struct rtl_phy *rtlphy = &rtlpriv->phy;
1276     u32 delay = 0;
1277
1278     rtl_dbg(rtlpriv, COMP_SCAN, DBG_TRACE,
1279         "switch to channel%d\n", rtlphy->current_channel);
1280     if (is_hal_stop(rtlhal))
1281         return;
1282     do {
1283         if (!rtlphy->sw_chnl_inprogress)
1284             break;
1285         if (!_rtl8723be_phy_sw_chnl_step_by_step(hw,
1286                              rtlphy->current_channel,
1287                              &rtlphy->sw_chnl_stage,
1288                              &rtlphy->sw_chnl_step,
1289                              &delay)) {
1290             if (delay > 0)
1291                 mdelay(delay);
1292             else
1293                 continue;
1294         } else {
1295             rtlphy->sw_chnl_inprogress = false;
1296         }
1297         break;
1298     } while (true);
1299     rtl_dbg(rtlpriv, COMP_SCAN, DBG_TRACE, "\n");
1300 }
1301
1302 u8 rtl8723be_phy_sw_chnl(struct ieee80211_hw *hw)
1303 {
1304     struct rtl_priv *rtlpriv = rtl_priv(hw);
1305     struct rtl_phy *rtlphy = &rtlpriv->phy;
1306     struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
1307
1308     if (rtlphy->sw_chnl_inprogress)
1309         return 0;
1310     if (rtlphy->set_bwmode_inprogress)
1311         return 0;
1312     WARN_ONCE((rtlphy->current_channel > 14),
1313           "rtl8723be: WIRELESS_MODE_G but channel>14");
1314     rtlphy->sw_chnl_inprogress = true;
1315     rtlphy->sw_chnl_stage = 0;
1316     rtlphy->sw_chnl_step = 0;
1317     if (!(is_hal_stop(rtlhal)) && !(RT_CANNOT_IO(hw))) {
1318         rtl8723be_phy_sw_chnl_callback(hw);
1319         rtl_dbg(rtlpriv, COMP_CHAN, DBG_LOUD,
1320             "sw_chnl_inprogress false schedule workitem current channel %d\n",
1321             rtlphy->current_channel);
1322         rtlphy->sw_chnl_inprogress = false;
1323     } else {
1324         rtl_dbg(rtlpriv, COMP_CHAN, DBG_LOUD,
1325             "sw_chnl_inprogress false driver sleep or unload\n");
1326         rtlphy->sw_chnl_inprogress = false;
1327     }
1328     return 1;
1329 }
1330
1331 static bool _rtl8723be_phy_sw_chnl_step_by_step(struct ieee80211_hw *hw,
1332                         u8 channel, u8 *stage,
1333                         u8 *step, u32 *delay)
1334 {
1335     struct rtl_priv *rtlpriv = rtl_priv(hw);
1336     struct rtl_phy *rtlphy = &rtlpriv->phy;
1337     struct swchnlcmd precommoncmd[MAX_PRECMD_CNT];
1338     u32 precommoncmdcnt;
1339     struct swchnlcmd postcommoncmd[MAX_POSTCMD_CNT];
1340     u32 postcommoncmdcnt;
1341     struct swchnlcmd rfdependcmd[MAX_RFDEPENDCMD_CNT];
1342     u32 rfdependcmdcnt;
1343     struct swchnlcmd *currentcmd = NULL;
1344     u8 rfpath;
1345     u8 num_total_rfpath = rtlphy->num_total_rfpath;
1346
1347     precommoncmdcnt = 0;
1348     rtl8723_phy_set_sw_chnl_cmdarray(precommoncmd, precommoncmdcnt++,
1349                      MAX_PRECMD_CNT,
1350                      CMDID_SET_TXPOWEROWER_LEVEL,
1351                      0, 0, 0);
1352     rtl8723_phy_set_sw_chnl_cmdarray(precommoncmd, precommoncmdcnt++,
1353                      MAX_PRECMD_CNT, CMDID_END, 0, 0, 0);
1354
1355     postcommoncmdcnt = 0;
1356
1357     rtl8723_phy_set_sw_chnl_cmdarray(postcommoncmd, postcommoncmdcnt++,
1358                      MAX_POSTCMD_CNT, CMDID_END,
1359                         0, 0, 0);
1360
1361     rfdependcmdcnt = 0;
1362
1363     WARN_ONCE((channel < 1 || channel > 14),
1364           "rtl8723be: illegal channel for Zebra: %d\n", channel);
1365
1366     rtl8723_phy_set_sw_chnl_cmdarray(rfdependcmd, rfdependcmdcnt++,
1367                      MAX_RFDEPENDCMD_CNT,
1368                      CMDID_RF_WRITEREG,
1369                      RF_CHNLBW, channel, 10);
1370
1371     rtl8723_phy_set_sw_chnl_cmdarray(rfdependcmd, rfdependcmdcnt++,
1372                      MAX_RFDEPENDCMD_CNT,
1373                         CMDID_END, 0, 0, 0);
1374
1375     do {
1376         switch (*stage) {
1377         case 0:
1378             currentcmd = &precommoncmd[*step];
1379             break;
1380         case 1:
1381             currentcmd = &rfdependcmd[*step];
1382             break;
1383         case 2:
1384             currentcmd = &postcommoncmd[*step];
1385             break;
1386         default:
1387             pr_err("Invalid 'stage' = %d, Check it!\n",
1388                    *stage);
1389             return true;
1390         }
1391
1392         if (currentcmd->cmdid == CMDID_END) {
1393             if ((*stage) == 2) {
1394                 return true;
1395             } else {
1396                 (*stage)++;
1397                 (*step) = 0;
1398                 continue;
1399             }
1400         }
1401
1402         switch (currentcmd->cmdid) {
1403         case CMDID_SET_TXPOWEROWER_LEVEL:
1404             rtl8723be_phy_set_txpower_level(hw, channel);
1405             break;
1406         case CMDID_WRITEPORT_ULONG:
1407             rtl_write_dword(rtlpriv, currentcmd->para1,
1408                     currentcmd->para2);
1409             break;
1410         case CMDID_WRITEPORT_USHORT:
1411             rtl_write_word(rtlpriv, currentcmd->para1,
1412                        (u16)currentcmd->para2);
1413             break;
1414         case CMDID_WRITEPORT_UCHAR:
1415             rtl_write_byte(rtlpriv, currentcmd->para1,
1416                        (u8)currentcmd->para2);
1417             break;
1418         case CMDID_RF_WRITEREG:
1419             for (rfpath = 0; rfpath < num_total_rfpath; rfpath++) {
1420                 rtlphy->rfreg_chnlval[rfpath] =
1421                     ((rtlphy->rfreg_chnlval[rfpath] &
1422                       0xfffffc00) | currentcmd->para2);
1423
1424                 rtl_set_rfreg(hw, (enum radio_path)rfpath,
1425                           currentcmd->para1,
1426                           RFREG_OFFSET_MASK,
1427                           rtlphy->rfreg_chnlval[rfpath]);
1428             }
1429             break;
1430         default:
1431             rtl_dbg(rtlpriv, COMP_ERR, DBG_LOUD,
1432                 "switch case %#x not processed\n",
1433                 currentcmd->cmdid);
1434             break;
1435         }
1436
1437         break;
1438     } while (true);
1439
1440     (*delay) = currentcmd->msdelay;
1441     (*step)++;
1442     return false;
1443 }
1444
1445 static u8 _rtl8723be_phy_path_a_iqk(struct ieee80211_hw *hw)
1446 {
1447     u32 reg_eac, reg_e94, reg_e9c, tmp;
1448     u8 result = 0x00;
1449
1450     /* leave IQK mode */
1451     rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
1452     /* switch to path A */
1453     rtl_set_bbreg(hw, 0x948, MASKDWORD, 0x00000000);
1454     /* enable path A PA in TXIQK mode */
1455     rtl_set_rfreg(hw, RF90_PATH_A, RF_WE_LUT, RFREG_OFFSET_MASK, 0x800a0);
1456     rtl_set_rfreg(hw, RF90_PATH_A, RF_RCK_OS, RFREG_OFFSET_MASK, 0x20000);
1457     rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G1, RFREG_OFFSET_MASK, 0x0003f);
1458     rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G2, RFREG_OFFSET_MASK, 0xc7f87);
1459
1460     /* 1. TX IQK */
1461     /* path-A IQK setting */
1462     /* IQK setting */
1463     rtl_set_bbreg(hw, RTX_IQK, MASKDWORD, 0x01007c00);
1464     rtl_set_bbreg(hw, RRX_IQK, MASKDWORD, 0x01004800);
1465     /* path-A IQK setting */
1466     rtl_set_bbreg(hw, RTX_IQK_TONE_A, MASKDWORD, 0x18008c1c);
1467     rtl_set_bbreg(hw, RRX_IQK_TONE_A, MASKDWORD, 0x38008c1c);
1468     rtl_set_bbreg(hw, RTX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
1469     rtl_set_bbreg(hw, RRX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
1470
1471     rtl_set_bbreg(hw, RTX_IQK_PI_A, MASKDWORD, 0x821403ea);
1472     rtl_set_bbreg(hw, RRX_IQK_PI_A, MASKDWORD, 0x28160000);
1473     rtl_set_bbreg(hw, RTX_IQK_PI_B, MASKDWORD, 0x82110000);
1474     rtl_set_bbreg(hw, RRX_IQK_PI_B, MASKDWORD, 0x28110000);
1475     /* LO calibration setting */
1476     rtl_set_bbreg(hw, RIQK_AGC_RSP, MASKDWORD, 0x00462911);
1477     /* enter IQK mode */
1478     rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
1479
1480     /* One shot, path A LOK & IQK */
1481     rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf9000000);
1482     rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf8000000);
1483
1484     mdelay(IQK_DELAY_TIME);
1485
1486     /* leave IQK mode */
1487     rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
1488
1489     /* Check failed */
1490     reg_eac = rtl_get_bbreg(hw, 0xeac, MASKDWORD);
1491     reg_e94 = rtl_get_bbreg(hw, 0xe94, MASKDWORD);
1492     reg_e9c = rtl_get_bbreg(hw, 0xe9c, MASKDWORD);
1493
1494     if (!(reg_eac & BIT(28)) &&
1495         (((reg_e94 & 0x03FF0000) >> 16) != 0x142) &&
1496         (((reg_e9c & 0x03FF0000) >> 16) != 0x42))
1497         result |= 0x01;
1498     else /* if Tx not OK, ignore Rx */
1499         return result;
1500
1501     /* Allen 20131125 */
1502     tmp = (reg_e9c & 0x03FF0000) >> 16;
1503     if ((tmp & 0x200) > 0)
1504         tmp = 0x400 - tmp;
1505
1506     if (!(reg_eac & BIT(28)) &&
1507         (((reg_e94 & 0x03FF0000) >> 16) < 0x110) &&
1508         (((reg_e94 & 0x03FF0000) >> 16) > 0xf0) &&
1509         (tmp < 0xf))
1510         result |= 0x01;
1511     else /* if Tx not OK, ignore Rx */
1512         return result;
1513
1514     return result;
1515 }
1516
1517 /* bit0 = 1 => Tx OK, bit1 = 1 => Rx OK */
1518 static u8 _rtl8723be_phy_path_a_rx_iqk(struct ieee80211_hw *hw)
1519 {
1520     u32 reg_eac, reg_e94, reg_e9c, reg_ea4, u32tmp, tmp;
1521     u8 result = 0x00;
1522
1523     /* leave IQK mode */
1524     rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
1525
1526     /* switch to path A */
1527     rtl_set_bbreg(hw, 0x948, MASKDWORD, 0x00000000);
1528
1529     /* 1 Get TXIMR setting */
1530     /* modify RXIQK mode table */
1531     rtl_set_rfreg(hw, RF90_PATH_A, RF_WE_LUT, 0x80000, 0x1);
1532     rtl_set_rfreg(hw, RF90_PATH_A, RF_RCK_OS, RFREG_OFFSET_MASK, 0x30000);
1533     rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G1, RFREG_OFFSET_MASK, 0x0001f);
1534     /* LNA2 off, PA on for Dcut */
1535     rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G2, RFREG_OFFSET_MASK, 0xf7fb7);
1536     rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
1537
1538     /* IQK setting */
1539     rtl_set_bbreg(hw, RTX_IQK, MASKDWORD, 0x01007c00);
1540     rtl_set_bbreg(hw, RRX_IQK, MASKDWORD, 0x01004800);
1541
1542     /* path-A IQK setting */
1543     rtl_set_bbreg(hw, RTX_IQK_TONE_A, MASKDWORD, 0x18008c1c);
1544     rtl_set_bbreg(hw, RRX_IQK_TONE_A, MASKDWORD, 0x38008c1c);
1545     rtl_set_bbreg(hw, RTX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
1546     rtl_set_bbreg(hw, RRX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
1547
1548     rtl_set_bbreg(hw, RTX_IQK_PI_A, MASKDWORD, 0x82160ff0);
1549     rtl_set_bbreg(hw, RRX_IQK_PI_A, MASKDWORD, 0x28110000);
1550     rtl_set_bbreg(hw, RTX_IQK_PI_B, MASKDWORD, 0x82110000);
1551     rtl_set_bbreg(hw, RRX_IQK_PI_B, MASKDWORD, 0x28110000);
1552
1553     /* LO calibration setting */
1554     rtl_set_bbreg(hw, RIQK_AGC_RSP, MASKDWORD, 0x0046a911);
1555
1556     /* enter IQK mode */
1557     rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
1558
1559     /* One shot, path A LOK & IQK */
1560     rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf9000000);
1561     rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf8000000);
1562
1563     mdelay(IQK_DELAY_TIME);
1564
1565     /* leave IQK mode */
1566     rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
1567
1568     /* Check failed */
1569     reg_eac = rtl_get_bbreg(hw, RRX_POWER_AFTER_IQK_A_2, MASKDWORD);
1570     reg_e94 = rtl_get_bbreg(hw, RTX_POWER_BEFORE_IQK_A, MASKDWORD);
1571     reg_e9c = rtl_get_bbreg(hw, RTX_POWER_AFTER_IQK_A, MASKDWORD);
1572
1573     if (!(reg_eac & BIT(28)) &&
1574         (((reg_e94 & 0x03FF0000) >> 16) != 0x142) &&
1575         (((reg_e9c & 0x03FF0000) >> 16) != 0x42))
1576         result |= 0x01;
1577     else /* if Tx not OK, ignore Rx */
1578         return result;
1579
1580     /* Allen 20131125 */
1581     tmp = (reg_e9c & 0x03FF0000) >> 16;
1582     if ((tmp & 0x200) > 0)
1583         tmp = 0x400 - tmp;
1584
1585     if (!(reg_eac & BIT(28)) &&
1586         (((reg_e94 & 0x03FF0000) >> 16) < 0x110) &&
1587         (((reg_e94 & 0x03FF0000) >> 16) > 0xf0) &&
1588         (tmp < 0xf))
1589         result |= 0x01;
1590     else /* if Tx not OK, ignore Rx */
1591         return result;
1592
1593     u32tmp = 0x80007C00 | (reg_e94 & 0x3FF0000) |
1594          ((reg_e9c & 0x3FF0000) >> 16);
1595     rtl_set_bbreg(hw, RTX_IQK, MASKDWORD, u32tmp);
1596
1597     /* 1 RX IQK */
1598     /* modify RXIQK mode table */
1599     rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
1600     rtl_set_rfreg(hw, RF90_PATH_A, RF_WE_LUT, 0x80000, 0x1);
1601     rtl_set_rfreg(hw, RF90_PATH_A, RF_RCK_OS, RFREG_OFFSET_MASK, 0x30000);
1602     rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G1, RFREG_OFFSET_MASK, 0x0001f);
1603     /* LAN2 on, PA off for Dcut */
1604     rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G2, RFREG_OFFSET_MASK, 0xf7d77);
1605
1606     /* PA, PAD setting */
1607     rtl_set_rfreg(hw, RF90_PATH_A, 0xdf, RFREG_OFFSET_MASK, 0xf80);
1608     rtl_set_rfreg(hw, RF90_PATH_A, 0x55, RFREG_OFFSET_MASK, 0x4021f);
1609
1610     /* IQK setting */
1611     rtl_set_bbreg(hw, RRX_IQK, MASKDWORD, 0x01004800);
1612
1613     /* path-A IQK setting */
1614     rtl_set_bbreg(hw, RTX_IQK_TONE_A, MASKDWORD, 0x38008c1c);
1615     rtl_set_bbreg(hw, RRX_IQK_TONE_A, MASKDWORD, 0x18008c1c);
1616     rtl_set_bbreg(hw, RTX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
1617     rtl_set_bbreg(hw, RRX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
1618
1619     rtl_set_bbreg(hw, RTX_IQK_PI_A, MASKDWORD, 0x82110000);
1620     rtl_set_bbreg(hw, RRX_IQK_PI_A, MASKDWORD, 0x2816001f);
1621     rtl_set_bbreg(hw, RTX_IQK_PI_B, MASKDWORD, 0x82110000);
1622     rtl_set_bbreg(hw, RRX_IQK_PI_B, MASKDWORD, 0x28110000);
1623
1624     /* LO calibration setting */
1625     rtl_set_bbreg(hw, RIQK_AGC_RSP, MASKDWORD, 0x0046a8d1);
1626
1627     /* enter IQK mode */
1628     rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
1629
1630     /* One shot, path A LOK & IQK */
1631     rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf9000000);
1632     rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf8000000);
1633
1634     mdelay(IQK_DELAY_TIME);
1635
1636     /* leave IQK mode */
1637     rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
1638
1639     /* Check failed */
1640     reg_eac = rtl_get_bbreg(hw, RRX_POWER_AFTER_IQK_A_2, MASKDWORD);
1641     reg_ea4 = rtl_get_bbreg(hw, RRX_POWER_BEFORE_IQK_A_2, MASKDWORD);
1642
1643     /* leave IQK mode */
1644     rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
1645     rtl_set_rfreg(hw, RF90_PATH_A, 0xdf, RFREG_OFFSET_MASK, 0x780);
1646
1647     /* Allen 20131125 */
1648     tmp = (reg_eac & 0x03FF0000) >> 16;
1649     if ((tmp & 0x200) > 0)
1650         tmp = 0x400 - tmp;
1651     /* if Tx is OK, check whether Rx is OK */
1652     if (!(reg_eac & BIT(27)) &&
1653         (((reg_ea4 & 0x03FF0000) >> 16) != 0x132) &&
1654         (((reg_eac & 0x03FF0000) >> 16) != 0x36))
1655         result |= 0x02;
1656     else if (!(reg_eac & BIT(27)) &&
1657          (((reg_ea4 & 0x03FF0000) >> 16) < 0x110) &&
1658          (((reg_ea4 & 0x03FF0000) >> 16) > 0xf0) &&
1659          (tmp < 0xf))
1660         result |= 0x02;
1661
1662     return result;
1663 }
1664
1665 static u8 _rtl8723be_phy_path_b_iqk(struct ieee80211_hw *hw)
1666 {
1667     u32 reg_eac, reg_e94, reg_e9c, tmp;
1668     u8 result = 0x00;
1669
1670     /* leave IQK mode */
1671     rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
1672     /* switch to path B */
1673     rtl_set_bbreg(hw, 0x948, MASKDWORD, 0x00000280);
1674
1675     /* enable path B PA in TXIQK mode */
1676     rtl_set_rfreg(hw, RF90_PATH_A, 0xed, RFREG_OFFSET_MASK, 0x00020);
1677     rtl_set_rfreg(hw, RF90_PATH_A, 0x43, RFREG_OFFSET_MASK, 0x40fc1);
1678
1679     /* 1 Tx IQK */
1680     /* IQK setting */
1681     rtl_set_bbreg(hw, RTX_IQK, MASKDWORD, 0x01007c00);
1682     rtl_set_bbreg(hw, RRX_IQK, MASKDWORD, 0x01004800);
1683     /* path-A IQK setting */
1684     rtl_set_bbreg(hw, RTX_IQK_TONE_A, MASKDWORD, 0x18008c1c);
1685     rtl_set_bbreg(hw, RRX_IQK_TONE_A, MASKDWORD, 0x38008c1c);
1686     rtl_set_bbreg(hw, RTX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
1687     rtl_set_bbreg(hw, RRX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
1688
1689     rtl_set_bbreg(hw, RTX_IQK_PI_A, MASKDWORD, 0x821403ea);
1690     rtl_set_bbreg(hw, RRX_IQK_PI_A, MASKDWORD, 0x28110000);
1691     rtl_set_bbreg(hw, RTX_IQK_PI_B, MASKDWORD, 0x82110000);
1692     rtl_set_bbreg(hw, RRX_IQK_PI_B, MASKDWORD, 0x28110000);
1693
1694     /* LO calibration setting */
1695     rtl_set_bbreg(hw, RIQK_AGC_RSP, MASKDWORD, 0x00462911);
1696
1697     /* enter IQK mode */
1698     rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
1699
1700     /* One shot, path B LOK & IQK */
1701     rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf9000000);
1702     rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf8000000);
1703
1704     mdelay(IQK_DELAY_TIME);
1705
1706     /* leave IQK mode */
1707     rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
1708
1709     /* Check failed */
1710     reg_eac = rtl_get_bbreg(hw, RRX_POWER_AFTER_IQK_A_2, MASKDWORD);
1711     reg_e94 = rtl_get_bbreg(hw, RTX_POWER_BEFORE_IQK_A, MASKDWORD);
1712     reg_e9c = rtl_get_bbreg(hw, RTX_POWER_AFTER_IQK_A, MASKDWORD);
1713
1714     if (!(reg_eac & BIT(28)) &&
1715         (((reg_e94 & 0x03FF0000) >> 16) != 0x142) &&
1716         (((reg_e9c & 0x03FF0000) >> 16) != 0x42))
1717         result |= 0x01;
1718     else
1719         return result;
1720
1721     /* Allen 20131125 */
1722     tmp = (reg_e9c & 0x03FF0000) >> 16;
1723     if ((tmp & 0x200) > 0)
1724         tmp = 0x400 - tmp;
1725
1726     if (!(reg_eac & BIT(28)) &&
1727         (((reg_e94 & 0x03FF0000) >> 16) < 0x110) &&
1728         (((reg_e94 & 0x03FF0000) >> 16) > 0xf0) &&
1729         (tmp < 0xf))
1730         result |= 0x01;
1731     else
1732         return result;
1733
1734     return result;
1735 }
1736
1737 /* bit0 = 1 => Tx OK, bit1 = 1 => Rx OK */
1738 static u8 _rtl8723be_phy_path_b_rx_iqk(struct ieee80211_hw *hw)
1739 {
1740     u32 reg_e94, reg_e9c, reg_ea4, reg_eac, u32tmp, tmp;
1741     u8 result = 0x00;
1742
1743     /* leave IQK mode */
1744     rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
1745     /* switch to path B */
1746     rtl_set_bbreg(hw, 0x948, MASKDWORD, 0x00000280);
1747
1748     /* 1 Get TXIMR setting */
1749     /* modify RXIQK mode table */
1750     rtl_set_rfreg(hw, RF90_PATH_A, RF_WE_LUT, RFREG_OFFSET_MASK, 0x800a0);
1751     rtl_set_rfreg(hw, RF90_PATH_A, RF_RCK_OS, RFREG_OFFSET_MASK, 0x30000);
1752     rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G1, RFREG_OFFSET_MASK, 0x0001f);
1753     rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G2, RFREG_OFFSET_MASK, 0xf7ff7);
1754
1755     /* open PA S1 & SMIXER */
1756     rtl_set_rfreg(hw, RF90_PATH_A, 0xed, RFREG_OFFSET_MASK, 0x00020);
1757     rtl_set_rfreg(hw, RF90_PATH_A, 0x43, RFREG_OFFSET_MASK, 0x60fed);
1758
1759     /* IQK setting */
1760     rtl_set_bbreg(hw, RTX_IQK, MASKDWORD, 0x01007c00);
1761     rtl_set_bbreg(hw, RRX_IQK, MASKDWORD, 0x01004800);
1762
1763     /* path-B IQK setting */
1764     rtl_set_bbreg(hw, RTX_IQK_TONE_A, MASKDWORD, 0x18008c1c);
1765     rtl_set_bbreg(hw, RRX_IQK_TONE_A, MASKDWORD, 0x38008c1c);
1766     rtl_set_bbreg(hw, RTX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
1767     rtl_set_bbreg(hw, RRX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
1768
1769     rtl_set_bbreg(hw, RTX_IQK_PI_A, MASKDWORD, 0x82160ff0);
1770     rtl_set_bbreg(hw, RRX_IQK_PI_A, MASKDWORD, 0x28110000);
1771     rtl_set_bbreg(hw, RTX_IQK_PI_B, MASKDWORD, 0x82110000);
1772     rtl_set_bbreg(hw, RRX_IQK_PI_B, MASKDWORD, 0x28110000);
1773
1774     /* LO calibration setting */
1775     rtl_set_bbreg(hw, RIQK_AGC_RSP, MASKDWORD, 0x0046a911);
1776     /* enter IQK mode */
1777     rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
1778
1779     /* One shot, path B TXIQK @ RXIQK */
1780     rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf9000000);
1781     rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf8000000);
1782
1783     mdelay(IQK_DELAY_TIME);
1784
1785     /* leave IQK mode */
1786     rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
1787     /* Check failed */
1788     reg_eac = rtl_get_bbreg(hw, RRX_POWER_AFTER_IQK_A_2, MASKDWORD);
1789     reg_e94 = rtl_get_bbreg(hw, RTX_POWER_BEFORE_IQK_A, MASKDWORD);
1790     reg_e9c = rtl_get_bbreg(hw, RTX_POWER_AFTER_IQK_A, MASKDWORD);
1791
1792     if (!(reg_eac & BIT(28)) &&
1793         (((reg_e94 & 0x03FF0000) >> 16) != 0x142) &&
1794         (((reg_e9c & 0x03FF0000) >> 16) != 0x42))
1795         result |= 0x01;
1796     else    /* if Tx not OK, ignore Rx */
1797         return result;
1798
1799     /* Allen 20131125 */
1800     tmp = (reg_e9c & 0x03FF0000) >> 16;
1801     if ((tmp & 0x200) > 0)
1802         tmp = 0x400 - tmp;
1803
1804     if (!(reg_eac & BIT(28)) &&
1805         (((reg_e94 & 0x03FF0000) >> 16) < 0x110) &&
1806         (((reg_e94 & 0x03FF0000) >> 16) > 0xf0) &&
1807         (tmp < 0xf))
1808         result |= 0x01;
1809     else
1810         return result;
1811
1812     u32tmp = 0x80007C00 | (reg_e94 & 0x3FF0000)  |
1813          ((reg_e9c & 0x3FF0000) >> 16);
1814     rtl_set_bbreg(hw, RTX_IQK, MASKDWORD, u32tmp);
1815
1816     /* 1 RX IQK */
1817
1818     /* <20121009, Kordan> RF Mode = 3 */
1819     rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
1820     rtl_set_rfreg(hw, RF90_PATH_A, RF_WE_LUT, 0x80000, 0x1);
1821     rtl_set_rfreg(hw, RF90_PATH_A, RF_RCK_OS, RFREG_OFFSET_MASK, 0x30000);
1822     rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G1, RFREG_OFFSET_MASK, 0x0001f);
1823     rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G2, RFREG_OFFSET_MASK, 0xf7d77);
1824     rtl_set_rfreg(hw, RF90_PATH_A, RF_WE_LUT, 0x80000, 0x0);
1825
1826     /* open PA S1 & close SMIXER */
1827     rtl_set_rfreg(hw, RF90_PATH_A, 0xed, RFREG_OFFSET_MASK, 0x00020);
1828     rtl_set_rfreg(hw, RF90_PATH_A, 0x43, RFREG_OFFSET_MASK, 0x60fbd);
1829
1830     /* IQK setting */
1831     rtl_set_bbreg(hw, RRX_IQK, MASKDWORD, 0x01004800);
1832
1833     /* path-B IQK setting */
1834     rtl_set_bbreg(hw, RTX_IQK_TONE_A, MASKDWORD, 0x38008c1c);
1835     rtl_set_bbreg(hw, RRX_IQK_TONE_A, MASKDWORD, 0x18008c1c);
1836     rtl_set_bbreg(hw, RTX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
1837     rtl_set_bbreg(hw, RRX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
1838
1839     rtl_set_bbreg(hw, RTX_IQK_PI_A, MASKDWORD, 0x82110000);
1840     rtl_set_bbreg(hw, RRX_IQK_PI_A, MASKDWORD, 0x2816001f);
1841     rtl_set_bbreg(hw, RTX_IQK_PI_B, MASKDWORD, 0x82110000);
1842     rtl_set_bbreg(hw, RRX_IQK_PI_B, MASKDWORD, 0x28110000);
1843
1844     /* LO calibration setting */
1845     rtl_set_bbreg(hw, RIQK_AGC_RSP, MASKDWORD, 0x0046a8d1);
1846     /* enter IQK mode */
1847     rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
1848
1849     /* One shot, path B LOK & IQK */
1850     rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf9000000);
1851     rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf8000000);
1852
1853     mdelay(IQK_DELAY_TIME);
1854
1855     /* leave IQK mode */
1856     rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
1857     /* Check failed */
1858     reg_eac = rtl_get_bbreg(hw, RRX_POWER_AFTER_IQK_A_2, MASKDWORD);
1859     reg_ea4 = rtl_get_bbreg(hw, RRX_POWER_BEFORE_IQK_A_2, MASKDWORD);
1860
1861     /* Allen 20131125 */
1862     tmp = (reg_eac & 0x03FF0000) >> 16;
1863     if ((tmp & 0x200) > 0)
1864         tmp = 0x400 - tmp;
1865
1866     /* if Tx is OK, check whether Rx is OK */
1867     if (!(reg_eac & BIT(27)) &&
1868         (((reg_ea4 & 0x03FF0000) >> 16) != 0x132) &&
1869         (((reg_eac & 0x03FF0000) >> 16) != 0x36))
1870         result |= 0x02;
1871     else if (!(reg_eac & BIT(27)) &&
1872          (((reg_ea4 & 0x03FF0000) >> 16) < 0x110) &&
1873          (((reg_ea4 & 0x03FF0000) >> 16) > 0xf0) &&
1874          (tmp < 0xf))
1875         result |= 0x02;
1876     else
1877         return result;
1878
1879     return result;
1880 }
1881
1882 static void _rtl8723be_phy_path_b_fill_iqk_matrix(struct ieee80211_hw *hw,
1883                           bool b_iqk_ok,
1884                           long result[][8],
1885                           u8 final_candidate,
1886                           bool btxonly)
1887 {
1888     u32 oldval_1, x, tx1_a, reg;
1889     long y, tx1_c;
1890
1891     if (final_candidate == 0xFF) {
1892         return;
1893     } else if (b_iqk_ok) {
1894         oldval_1 = (rtl_get_bbreg(hw, ROFDM0_XBTXIQIMBALANCE,
1895                       MASKDWORD) >> 22) & 0x3FF;
1896         x = result[final_candidate][4];
1897         if ((x & 0x00000200) != 0)
1898             x = x | 0xFFFFFC00;
1899         tx1_a = (x * oldval_1) >> 8;
1900         rtl_set_bbreg(hw, ROFDM0_XBTXIQIMBALANCE, 0x3FF, tx1_a);
1901         rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD, BIT(27),
1902                   ((x * oldval_1 >> 7) & 0x1));
1903         y = result[final_candidate][5];
1904         if ((y & 0x00000200) != 0)
1905             y = y | 0xFFFFFC00;
1906         tx1_c = (y * oldval_1) >> 8;
1907         rtl_set_bbreg(hw, ROFDM0_XDTXAFE, 0xF0000000,
1908                   ((tx1_c & 0x3C0) >> 6));
1909         rtl_set_bbreg(hw, ROFDM0_XBTXIQIMBALANCE, 0x003F0000,
1910                   (tx1_c & 0x3F));
1911         rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD, BIT(25),
1912                   ((y * oldval_1 >> 7) & 0x1));
1913         if (btxonly)
1914             return;
1915         reg = result[final_candidate][6];
1916         rtl_set_bbreg(hw, ROFDM0_XBRXIQIMBALANCE, 0x3FF, reg);
1917         reg = result[final_candidate][7] & 0x3F;
1918         rtl_set_bbreg(hw, ROFDM0_XBRXIQIMBALANCE, 0xFC00, reg);
1919         reg = (result[final_candidate][7] >> 6) & 0xF;
1920         /* rtl_set_bbreg(hw, 0xca0, 0xF0000000, reg); */
1921     }
1922 }
1923
1924 static bool _rtl8723be_phy_simularity_compare(struct ieee80211_hw *hw,
1925                           long result[][8], u8 c1, u8 c2)
1926 {
1927     u32 i, j, diff, simularity_bitmap, bound = 0;
1928
1929     u8 final_candidate[2] = {0xFF, 0xFF}; /* for path A and path B */
1930     bool bresult = true; /* is2t = true*/
1931     s32 tmp1 = 0, tmp2 = 0;
1932
1933     bound = 8;
1934
1935     simularity_bitmap = 0;
1936
1937     for (i = 0; i < bound; i++) {
1938         if ((i == 1) || (i == 3) || (i == 5) || (i == 7)) {
1939             if ((result[c1][i] & 0x00000200) != 0)
1940                 tmp1 = result[c1][i] | 0xFFFFFC00;
1941             else
1942                 tmp1 = result[c1][i];
1943
1944             if ((result[c2][i] & 0x00000200) != 0)
1945                 tmp2 = result[c2][i] | 0xFFFFFC00;
1946             else
1947                 tmp2 = result[c2][i];
1948         } else {
1949             tmp1 = result[c1][i];
1950             tmp2 = result[c2][i];
1951         }
1952
1953         diff = (tmp1 > tmp2) ? (tmp1 - tmp2) : (tmp2 - tmp1);
1954
1955         if (diff > MAX_TOLERANCE) {
1956             if ((i == 2 || i == 6) && !simularity_bitmap) {
1957                 if (result[c1][i] + result[c1][i + 1] == 0)
1958                     final_candidate[(i / 4)] = c2;
1959                 else if (result[c2][i] + result[c2][i + 1] == 0)
1960                     final_candidate[(i / 4)] = c1;
1961                 else
1962                     simularity_bitmap |= (1 << i);
1963             } else
1964                 simularity_bitmap |= (1 << i);
1965         }
1966     }
1967
1968     if (simularity_bitmap == 0) {
1969         for (i = 0; i < (bound / 4); i++) {
1970             if (final_candidate[i] != 0xFF) {
1971                 for (j = i * 4; j < (i + 1) * 4 - 2; j++)
1972                     result[3][j] =
1973                         result[final_candidate[i]][j];
1974                 bresult = false;
1975             }
1976         }
1977         return bresult;
1978     } else {
1979         if (!(simularity_bitmap & 0x03)) { /* path A TX OK */
1980             for (i = 0; i < 2; i++)
1981                 result[3][i] = result[c1][i];
1982         }
1983         if (!(simularity_bitmap & 0x0c)) { /* path A RX OK */
1984             for (i = 2; i < 4; i++)
1985                 result[3][i] = result[c1][i];
1986         }
1987         if (!(simularity_bitmap & 0x30)) { /* path B TX OK */
1988             for (i = 4; i < 6; i++)
1989                 result[3][i] = result[c1][i];
1990         }
1991         if (!(simularity_bitmap & 0xc0)) { /* path B RX OK */
1992             for (i = 6; i < 8; i++)
1993                 result[3][i] = result[c1][i];
1994         }
1995         return false;
1996     }
1997 }
1998
1999 static void _rtl8723be_phy_iq_calibrate(struct ieee80211_hw *hw,
2000                     long result[][8], u8 t, bool is2t)
2001 {
2002     struct rtl_priv *rtlpriv = rtl_priv(hw);
2003     struct rtl_phy *rtlphy = &rtlpriv->phy;
2004     u32 i;
2005     u8 patha_ok, pathb_ok;
2006     u32 adda_reg[IQK_ADDA_REG_NUM] = {
2007         0x85c, 0xe6c, 0xe70, 0xe74,
2008         0xe78, 0xe7c, 0xe80, 0xe84,
2009         0xe88, 0xe8c, 0xed0, 0xed4,
2010         0xed8, 0xedc, 0xee0, 0xeec
2011     };
2012
2013     u32 iqk_mac_reg[IQK_MAC_REG_NUM] = {
2014         0x522, 0x550, 0x551, 0x040
2015     };
2016     u32 iqk_bb_reg[IQK_BB_REG_NUM] = {
2017         ROFDM0_TRXPATHENABLE, ROFDM0_TRMUXPAR,
2018         RFPGA0_XCD_RFINTERFACESW, 0xb68, 0xb6c,
2019         0x870, 0x860,
2020         0x864, 0xa04
2021     };
2022     const u32 retrycount = 2;
2023
2024     u32 path_sel_bb;/* path_sel_rf */
2025
2026     u8 tmp_reg_c50, tmp_reg_c58;
2027
2028     tmp_reg_c50 = rtl_get_bbreg(hw, 0xc50, MASKBYTE0);
2029     tmp_reg_c58 = rtl_get_bbreg(hw, 0xc58, MASKBYTE0);
2030
2031     if (t == 0) {
2032         rtl8723_save_adda_registers(hw, adda_reg,
2033                         rtlphy->adda_backup, 16);
2034         rtl8723_phy_save_mac_registers(hw, iqk_mac_reg,
2035                            rtlphy->iqk_mac_backup);
2036         rtl8723_save_adda_registers(hw, iqk_bb_reg,
2037                         rtlphy->iqk_bb_backup,
2038                         IQK_BB_REG_NUM);
2039     }
2040     rtl8723_phy_path_adda_on(hw, adda_reg, true, is2t);
2041     if (t == 0) {
2042         rtlphy->rfpi_enable = (u8)rtl_get_bbreg(hw,
2043                         RFPGA0_XA_HSSIPARAMETER1,
2044                         BIT(8));
2045     }
2046
2047     path_sel_bb = rtl_get_bbreg(hw, 0x948, MASKDWORD);
2048
2049     rtl8723_phy_mac_setting_calibration(hw, iqk_mac_reg,
2050                         rtlphy->iqk_mac_backup);
2051     /*BB Setting*/
2052     rtl_set_bbreg(hw, 0xa04, 0x0f000000, 0xf);
2053     rtl_set_bbreg(hw, 0xc04, MASKDWORD, 0x03a05600);
2054     rtl_set_bbreg(hw, 0xc08, MASKDWORD, 0x000800e4);
2055     rtl_set_bbreg(hw, 0x874, MASKDWORD, 0x22204000);
2056
2057     /* path A TX IQK */
2058     for (i = 0; i < retrycount; i++) {
2059         patha_ok = _rtl8723be_phy_path_a_iqk(hw);
2060         if (patha_ok == 0x01) {
2061             rtl_dbg(rtlpriv, COMP_INIT, DBG_LOUD,
2062                 "Path A Tx IQK Success!!\n");
2063             result[t][0] = (rtl_get_bbreg(hw, 0xe94, MASKDWORD) &
2064                     0x3FF0000) >> 16;
2065             result[t][1] = (rtl_get_bbreg(hw, 0xe9c, MASKDWORD) &
2066                     0x3FF0000) >> 16;
2067             break;
2068         } else {
2069             rtl_dbg(rtlpriv, COMP_INIT, DBG_LOUD,
2070                 "Path A Tx IQK Fail!!\n");
2071         }
2072     }
2073     /* path A RX IQK */
2074     for (i = 0; i < retrycount; i++) {
2075         patha_ok = _rtl8723be_phy_path_a_rx_iqk(hw);
2076         if (patha_ok == 0x03) {
2077             rtl_dbg(rtlpriv, COMP_INIT, DBG_LOUD,
2078                 "Path A Rx IQK Success!!\n");
2079             result[t][2] = (rtl_get_bbreg(hw, 0xea4, MASKDWORD) &
2080                     0x3FF0000) >> 16;
2081             result[t][3] = (rtl_get_bbreg(hw, 0xeac, MASKDWORD) &
2082                     0x3FF0000) >> 16;
2083             break;
2084         }
2085         rtl_dbg(rtlpriv, COMP_INIT, DBG_LOUD,
2086             "Path A Rx IQK Fail!!\n");
2087     }
2088
2089     if (0x00 == patha_ok)
2090         rtl_dbg(rtlpriv, COMP_INIT, DBG_LOUD, "Path A IQK Fail!!\n");
2091
2092     if (is2t) {
2093         /* path B TX IQK */
2094         for (i = 0; i < retrycount; i++) {
2095             pathb_ok = _rtl8723be_phy_path_b_iqk(hw);
2096             if (pathb_ok == 0x01) {
2097                 rtl_dbg(rtlpriv, COMP_INIT, DBG_LOUD,
2098                     "Path B Tx IQK Success!!\n");
2099                 result[t][4] = (rtl_get_bbreg(hw, 0xe94,
2100                                   MASKDWORD) &
2101                                   0x3FF0000) >> 16;
2102                 result[t][5] = (rtl_get_bbreg(hw, 0xe9c,
2103                                   MASKDWORD) &
2104                                   0x3FF0000) >> 16;
2105                 break;
2106             }
2107             rtl_dbg(rtlpriv, COMP_INIT, DBG_LOUD,
2108                 "Path B Tx IQK Fail!!\n");
2109         }
2110         /* path B RX IQK */
2111         for (i = 0; i < retrycount; i++) {
2112             pathb_ok = _rtl8723be_phy_path_b_rx_iqk(hw);
2113             if (pathb_ok == 0x03) {
2114                 rtl_dbg(rtlpriv, COMP_INIT, DBG_LOUD,
2115                     "Path B Rx IQK Success!!\n");
2116                 result[t][6] = (rtl_get_bbreg(hw, 0xea4,
2117                                   MASKDWORD) &
2118                                   0x3FF0000) >> 16;
2119                 result[t][7] = (rtl_get_bbreg(hw, 0xeac,
2120                                   MASKDWORD) &
2121                                   0x3FF0000) >> 16;
2122                 break;
2123             }
2124             rtl_dbg(rtlpriv, COMP_INIT, DBG_LOUD,
2125                 "Path B Rx IQK Fail!!\n");
2126         }
2127     }
2128
2129     /* Back to BB mode, load original value */
2130     rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0);
2131
2132     if (t != 0) {
2133         rtl8723_phy_reload_adda_registers(hw, adda_reg,
2134                           rtlphy->adda_backup, 16);
2135         rtl8723_phy_reload_mac_registers(hw, iqk_mac_reg,
2136                          rtlphy->iqk_mac_backup);
2137         rtl8723_phy_reload_adda_registers(hw, iqk_bb_reg,
2138                           rtlphy->iqk_bb_backup,
2139                           IQK_BB_REG_NUM);
2140
2141         rtl_set_bbreg(hw, 0x948, MASKDWORD, path_sel_bb);
2142         /*rtl_set_rfreg(hw, RF90_PATH_B, 0xb0, 0xfffff, path_sel_rf);*/
2143
2144         rtl_set_bbreg(hw, 0xc50, MASKBYTE0, 0x50);
2145         rtl_set_bbreg(hw, 0xc50, MASKBYTE0, tmp_reg_c50);
2146         if (is2t) {
2147             rtl_set_bbreg(hw, 0xc58, MASKBYTE0, 0x50);
2148             rtl_set_bbreg(hw, 0xc58, MASKBYTE0, tmp_reg_c58);
2149         }
2150         rtl_set_bbreg(hw, 0xe30, MASKDWORD, 0x01008c00);
2151         rtl_set_bbreg(hw, 0xe34, MASKDWORD, 0x01008c00);
2152     }
2153     rtl_dbg(rtlpriv, COMP_INIT, DBG_LOUD, "8723be IQK Finish!!\n");
2154 }
2155
2156 static u8 _get_right_chnl_place_for_iqk(u8 chnl)
2157 {
2158     u8 channel_all[TARGET_CHNL_NUM_2G_5G] = {
2159             1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12,
2160             13, 14, 36, 38, 40, 42, 44, 46,
2161             48, 50, 52, 54, 56, 58, 60, 62, 64,
2162             100, 102, 104, 106, 108, 110,
2163             112, 114, 116, 118, 120, 122,
2164             124, 126, 128, 130, 132, 134, 136,
2165             138, 140, 149, 151, 153, 155, 157,
2166             159, 161, 163, 165};
2167     u8 place = chnl;
2168
2169     if (chnl > 14) {
2170         for (place = 14; place < sizeof(channel_all); place++) {
2171             if (channel_all[place] == chnl)
2172                 return place - 13;
2173         }
2174     }
2175     return 0;
2176 }
2177
2178 static void _rtl8723be_phy_lc_calibrate(struct ieee80211_hw *hw, bool is2t)
2179 {
2180     u8 tmpreg;
2181     u32 rf_a_mode = 0, rf_b_mode = 0;
2182     struct rtl_priv *rtlpriv = rtl_priv(hw);
2183
2184     tmpreg = rtl_read_byte(rtlpriv, 0xd03);
2185
2186     if ((tmpreg & 0x70) != 0)
2187         rtl_write_byte(rtlpriv, 0xd03, tmpreg & 0x8F);
2188     else
2189         rtl_write_byte(rtlpriv, REG_TXPAUSE, 0xFF);
2190
2191     if ((tmpreg & 0x70) != 0) {
2192         rf_a_mode = rtl_get_rfreg(hw, RF90_PATH_A, 0x00, MASK12BITS);
2193
2194         if (is2t)
2195             rf_b_mode = rtl_get_rfreg(hw, RF90_PATH_B, 0x00,
2196                           MASK12BITS);
2197
2198         rtl_set_rfreg(hw, RF90_PATH_A, 0x00, MASK12BITS,
2199                   (rf_a_mode & 0x8FFFF) | 0x10000);
2200
2201         if (is2t)
2202             rtl_set_rfreg(hw, RF90_PATH_B, 0x00, MASK12BITS,
2203                       (rf_b_mode & 0x8FFFF) | 0x10000);
2204     }
2205     rtl_get_rfreg(hw, RF90_PATH_A, 0x18, MASK12BITS);
2206
2207     rtl_set_rfreg(hw, RF90_PATH_A, 0xb0, RFREG_OFFSET_MASK, 0xdfbe0);
2208     rtl_set_rfreg(hw, RF90_PATH_A, 0x18, MASK12BITS, 0x8c0a);
2209
2210     /* In order not to disturb BT music when wifi init.(1ant NIC only) */
2211     /*mdelay(100);*/
2212     /* In order not to disturb BT music when wifi init.(1ant NIC only) */
2213     mdelay(50);
2214
2215     rtl_set_rfreg(hw, RF90_PATH_A, 0xb0, RFREG_OFFSET_MASK, 0xdffe0);
2216
2217     if ((tmpreg & 0x70) != 0) {
2218         rtl_write_byte(rtlpriv, 0xd03, tmpreg);
2219         rtl_set_rfreg(hw, RF90_PATH_A, 0x00, MASK12BITS, rf_a_mode);
2220
2221         if (is2t)
2222             rtl_set_rfreg(hw, RF90_PATH_B, 0x00,
2223                       MASK12BITS, rf_b_mode);
2224     } else {
2225         rtl_write_byte(rtlpriv, REG_TXPAUSE, 0x00);
2226     }
2227     rtl_dbg(rtlpriv, COMP_INIT, DBG_LOUD, "\n");
2228 }
2229
2230 static void _rtl8723be_phy_set_rfpath_switch(struct ieee80211_hw *hw,
2231                          bool bmain, bool is2t)
2232 {
2233     struct rtl_priv *rtlpriv = rtl_priv(hw);
2234     rtl_dbg(rtlpriv, COMP_INIT, DBG_LOUD, "\n");
2235
2236     if (bmain) /* left antenna */
2237         rtl_set_bbreg(hw, 0x92C, MASKDWORD, 0x1);
2238     else
2239         rtl_set_bbreg(hw, 0x92C, MASKDWORD, 0x2);
2240 }
2241
2242 #undef IQK_ADDA_REG_NUM
2243 #undef IQK_DELAY_TIME
2244 /* IQK is merge from Merge Temp */
2245 void rtl8723be_phy_iq_calibrate(struct ieee80211_hw *hw, bool b_recovery)
2246 {
2247     struct rtl_priv *rtlpriv = rtl_priv(hw);
2248     struct rtl_phy *rtlphy = &rtlpriv->phy;
2249     long result[4][8];
2250     u8 i, final_candidate, idx;
2251     bool b_patha_ok, b_pathb_ok;
2252     long reg_e94, reg_e9c, reg_ea4, reg_eb4, reg_ebc, reg_ec4;
2253     long reg_tmp = 0;
2254     bool is12simular, is13simular, is23simular;
2255     u32 iqk_bb_reg[9] = {
2256         ROFDM0_XARXIQIMBALANCE,
2257         ROFDM0_XBRXIQIMBALANCE,
2258         ROFDM0_ECCATHRESHOLD,
2259         ROFDM0_AGCRSSITABLE,
2260         ROFDM0_XATXIQIMBALANCE,
2261         ROFDM0_XBTXIQIMBALANCE,
2262         ROFDM0_XCTXAFE,
2263         ROFDM0_XDTXAFE,
2264         ROFDM0_RXIQEXTANTA
2265     };
2266     u32 path_sel_bb = 0; /* path_sel_rf = 0 */
2267
2268     if (rtlphy->lck_inprogress)
2269         return;
2270
2271     spin_lock(&rtlpriv->locks.iqk_lock);
2272     rtlphy->lck_inprogress = true;
2273     spin_unlock(&rtlpriv->locks.iqk_lock);
2274
2275     if (b_recovery) {
2276         rtl8723_phy_reload_adda_registers(hw, iqk_bb_reg,
2277                           rtlphy->iqk_bb_backup, 9);
2278         goto label_done;
2279     }
2280     /* Save RF Path */
2281     path_sel_bb = rtl_get_bbreg(hw, 0x948, MASKDWORD);
2282     /* path_sel_rf = rtl_get_rfreg(hw, RF90_PATH_A, 0xb0, 0xfffff); */
2283
2284     for (i = 0; i < 8; i++) {
2285         result[0][i] = 0;
2286         result[1][i] = 0;
2287         result[2][i] = 0;
2288         result[3][i] = 0;
2289     }
2290     final_candidate = 0xff;
2291     b_patha_ok = false;
2292     b_pathb_ok = false;
2293     is12simular = false;
2294     is23simular = false;
2295     is13simular = false;
2296     for (i = 0; i < 3; i++) {
2297         _rtl8723be_phy_iq_calibrate(hw, result, i, true);
2298         if (i == 1) {
2299             is12simular = _rtl8723be_phy_simularity_compare(hw,
2300                                     result,
2301                                     0, 1);
2302             if (is12simular) {
2303                 final_candidate = 0;
2304                 break;
2305             }
2306         }
2307         if (i == 2) {
2308             is13simular = _rtl8723be_phy_simularity_compare(hw,
2309                                     result,
2310                                     0, 2);
2311             if (is13simular) {
2312                 final_candidate = 0;
2313                 break;
2314             }
2315             is23simular = _rtl8723be_phy_simularity_compare(hw,
2316                                     result,
2317                                     1, 2);
2318             if (is23simular) {
2319                 final_candidate = 1;
2320             } else {
2321                 for (i = 0; i < 8; i++)
2322                     reg_tmp += result[3][i];
2323
2324                 if (reg_tmp != 0)
2325                     final_candidate = 3;
2326                 else
2327                     final_candidate = 0xFF;
2328             }
2329         }
2330     }
2331     for (i = 0; i < 4; i++) {
2332         reg_e94 = result[i][0];
2333         reg_e9c = result[i][1];
2334         reg_ea4 = result[i][2];
2335         reg_eb4 = result[i][4];
2336         reg_ebc = result[i][5];
2337         reg_ec4 = result[i][6];
2338     }
2339     if (final_candidate != 0xff) {
2340         reg_e94 = result[final_candidate][0];
2341         rtlphy->reg_e94 = reg_e94;
2342         reg_e9c = result[final_candidate][1];
2343         rtlphy->reg_e9c = reg_e9c;
2344         reg_ea4 = result[final_candidate][2];
2345         reg_eb4 = result[final_candidate][4];
2346         rtlphy->reg_eb4 = reg_eb4;
2347         reg_ebc = result[final_candidate][5];
2348         rtlphy->reg_ebc = reg_ebc;
2349         reg_ec4 = result[final_candidate][6];
2350         b_patha_ok = true;
2351         b_pathb_ok = true;
2352     } else {
2353         rtlphy->reg_e94 = 0x100;
2354         rtlphy->reg_eb4 = 0x100;
2355         rtlphy->reg_e9c = 0x0;
2356         rtlphy->reg_ebc = 0x0;
2357     }
2358     if (reg_e94 != 0)
2359         rtl8723_phy_path_a_fill_iqk_matrix(hw, b_patha_ok, result,
2360                            final_candidate,
2361                            (reg_ea4 == 0));
2362     if (reg_eb4 != 0)
2363         _rtl8723be_phy_path_b_fill_iqk_matrix(hw, b_pathb_ok, result,
2364                               final_candidate,
2365                               (reg_ec4 == 0));
2366
2367     idx = _get_right_chnl_place_for_iqk(rtlphy->current_channel);
2368
2369     if (final_candidate < 4) {
2370         for (i = 0; i < IQK_MATRIX_REG_NUM; i++)
2371             rtlphy->iqk_matrix[idx].value[0][i] =
2372                         result[final_candidate][i];
2373         rtlphy->iqk_matrix[idx].iqk_done = true;
2374
2375     }
2376     rtl8723_save_adda_registers(hw, iqk_bb_reg,
2377                     rtlphy->iqk_bb_backup, 9);
2378
2379     rtl_set_bbreg(hw, 0x948, MASKDWORD, path_sel_bb);
2380     /* rtl_set_rfreg(hw, RF90_PATH_A, 0xb0, 0xfffff, path_sel_rf); */
2381
2382 label_done:
2383     spin_lock(&rtlpriv->locks.iqk_lock);
2384     rtlphy->lck_inprogress = false;
2385     spin_unlock(&rtlpriv->locks.iqk_lock);
2386 }
2387
2388 void rtl8723be_phy_lc_calibrate(struct ieee80211_hw *hw)
2389 {
2390     struct rtl_priv *rtlpriv = rtl_priv(hw);
2391     struct rtl_phy *rtlphy = &rtlpriv->phy;
2392     struct rtl_hal *rtlhal = &rtlpriv->rtlhal;
2393     u32 timeout = 2000, timecount = 0;
2394
2395     while (rtlpriv->mac80211.act_scanning && timecount < timeout) {
2396         udelay(50);
2397         timecount += 50;
2398     }
2399
2400     rtlphy->lck_inprogress = true;
2401     RTPRINT(rtlpriv, FINIT, INIT_IQK,
2402         "LCK:Start!!! currentband %x delay %d ms\n",
2403          rtlhal->current_bandtype, timecount);
2404
2405     _rtl8723be_phy_lc_calibrate(hw, false);
2406
2407     rtlphy->lck_inprogress = false;
2408 }
2409
2410 void rtl8723be_phy_set_rfpath_switch(struct ieee80211_hw *hw, bool bmain)
2411 {
2412     _rtl8723be_phy_set_rfpath_switch(hw, bmain, true);
2413 }
2414
2415 bool rtl8723be_phy_set_io_cmd(struct ieee80211_hw *hw, enum io_type iotype)
2416 {
2417     struct rtl_priv *rtlpriv = rtl_priv(hw);
2418     struct rtl_phy *rtlphy = &rtlpriv->phy;
2419     bool b_postprocessing = false;
2420
2421     rtl_dbg(rtlpriv, COMP_CMD, DBG_TRACE,
2422         "-->IO Cmd(%#x), set_io_inprogress(%d)\n",
2423         iotype, rtlphy->set_io_inprogress);
2424     do {
2425         switch (iotype) {
2426         case IO_CMD_RESUME_DM_BY_SCAN:
2427             rtl_dbg(rtlpriv, COMP_CMD, DBG_TRACE,
2428                 "[IO CMD] Resume DM after scan.\n");
2429             b_postprocessing = true;
2430             break;
2431         case IO_CMD_PAUSE_BAND0_DM_BY_SCAN:
2432             rtl_dbg(rtlpriv, COMP_CMD, DBG_TRACE,
2433                 "[IO CMD] Pause DM before scan.\n");
2434             b_postprocessing = true;
2435             break;
2436         default:
2437             rtl_dbg(rtlpriv, COMP_ERR, DBG_LOUD,
2438                 "switch case %#x not processed\n", iotype);
2439             break;
2440         }
2441     } while (false);
2442     if (b_postprocessing && !rtlphy->set_io_inprogress) {
2443         rtlphy->set_io_inprogress = true;
2444         rtlphy->current_io_type = iotype;
2445     } else {
2446         return false;
2447     }
2448     rtl8723be_phy_set_io(hw);
2449     rtl_dbg(rtlpriv, COMP_CMD, DBG_TRACE, "IO Type(%#x)\n", iotype);
2450     return true;
2451 }
2452
2453 static void rtl8723be_phy_set_io(struct ieee80211_hw *hw)
2454 {
2455     struct rtl_priv *rtlpriv = rtl_priv(hw);
2456     struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
2457     struct rtl_phy *rtlphy = &rtlpriv->phy;
2458
2459     rtl_dbg(rtlpriv, COMP_CMD, DBG_TRACE,
2460         "--->Cmd(%#x), set_io_inprogress(%d)\n",
2461         rtlphy->current_io_type, rtlphy->set_io_inprogress);
2462     switch (rtlphy->current_io_type) {
2463     case IO_CMD_RESUME_DM_BY_SCAN:
2464         dm_digtable->cur_igvalue = rtlphy->initgain_backup.xaagccore1;
2465         /*rtl92c_dm_write_dig(hw);*/
2466         rtl8723be_phy_set_txpower_level(hw, rtlphy->current_channel);
2467         rtl_set_bbreg(hw, RCCK0_CCA, 0xff0000, 0x83);
2468         break;
2469     case IO_CMD_PAUSE_BAND0_DM_BY_SCAN:
2470         rtlphy->initgain_backup.xaagccore1 = dm_digtable->cur_igvalue;
2471         dm_digtable->cur_igvalue = 0x17;
2472         rtl_set_bbreg(hw, RCCK0_CCA, 0xff0000, 0x40);
2473         break;
2474     default:
2475         rtl_dbg(rtlpriv, COMP_ERR, DBG_LOUD,
2476             "switch case %#x not processed\n",
2477             rtlphy->current_io_type);
2478         break;
2479     }
2480     rtlphy->set_io_inprogress = false;
2481     rtl_dbg(rtlpriv, COMP_CMD, DBG_TRACE,
2482         "(%#x)\n", rtlphy->current_io_type);
2483 }
2484
2485 static void rtl8723be_phy_set_rf_on(struct ieee80211_hw *hw)
2486 {
2487     struct rtl_priv *rtlpriv = rtl_priv(hw);
2488
2489     rtl_write_byte(rtlpriv, REG_SPS0_CTRL, 0x2b);
2490     rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE3);
2491     rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE2);
2492     rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE3);
2493     rtl_write_byte(rtlpriv, REG_TXPAUSE, 0x00);
2494 }
2495
2496 static void _rtl8723be_phy_set_rf_sleep(struct ieee80211_hw *hw)
2497 {
2498     struct rtl_priv *rtlpriv = rtl_priv(hw);
2499
2500     rtl_write_byte(rtlpriv, REG_TXPAUSE, 0xFF);
2501     rtl_set_rfreg(hw, RF90_PATH_A, 0x00, RFREG_OFFSET_MASK, 0x00);
2502     rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE2);
2503     rtl_write_byte(rtlpriv, REG_SPS0_CTRL, 0x22);
2504 }
2505
2506 static bool _rtl8723be_phy_set_rf_power_state(struct ieee80211_hw *hw,
2507                           enum rf_pwrstate rfpwr_state)
2508 {
2509     struct rtl_priv *rtlpriv = rtl_priv(hw);
2510     struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
2511     struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
2512     struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
2513     bool bresult = true;
2514     u8 i, queue_id;
2515     struct rtl8192_tx_ring *ring = NULL;
2516
2517     switch (rfpwr_state) {
2518     case ERFON:
2519         if ((ppsc->rfpwr_state == ERFOFF) &&
2520              RT_IN_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC)) {
2521             bool rtstatus;
2522             u32 initializecount = 0;
2523             do {
2524                 initializecount++;
2525                 rtl_dbg(rtlpriv, COMP_RF, DBG_DMESG,
2526                     "IPS Set eRf nic enable\n");
2527                 rtstatus = rtl_ps_enable_nic(hw);
2528             } while (!rtstatus && (initializecount < 10));
2529             RT_CLEAR_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
2530         } else {
2531             rtl_dbg(rtlpriv, COMP_RF, DBG_DMESG,
2532                 "Set ERFON slept:%d ms\n",
2533                 jiffies_to_msecs(jiffies -
2534                          ppsc->last_sleep_jiffies));
2535             ppsc->last_awake_jiffies = jiffies;
2536             rtl8723be_phy_set_rf_on(hw);
2537         }
2538         if (mac->link_state == MAC80211_LINKED)
2539             rtlpriv->cfg->ops->led_control(hw, LED_CTL_LINK);
2540         else
2541             rtlpriv->cfg->ops->led_control(hw, LED_CTL_NO_LINK);
2542
2543         break;
2544
2545     case ERFOFF:
2546         for (queue_id = 0, i = 0;
2547              queue_id < RTL_PCI_MAX_TX_QUEUE_COUNT;) {
2548             ring = &pcipriv->dev.tx_ring[queue_id];
2549             /* Don't check BEACON Q.
2550              * BEACON Q is always not empty,
2551              * because '_rtl8723be_cmd_send_packet'
2552              */
2553             if (queue_id == BEACON_QUEUE ||
2554                 skb_queue_len(&ring->queue) == 0) {
2555                 queue_id++;
2556                 continue;
2557             } else {
2558                 rtl_dbg(rtlpriv, COMP_ERR, DBG_WARNING,
2559                     "eRf Off/Sleep: %d times TcbBusyQueue[%d] =%d before doze!\n",
2560                     (i + 1), queue_id,
2561                     skb_queue_len(&ring->queue));
2562
2563                 udelay(10);
2564                 i++;
2565             }
2566             if (i >= MAX_DOZE_WAITING_TIMES_9x) {
2567                 rtl_dbg(rtlpriv, COMP_ERR, DBG_WARNING,
2568                     "ERFSLEEP: %d times TcbBusyQueue[%d] = %d !\n",
2569                     MAX_DOZE_WAITING_TIMES_9x,
2570                     queue_id,
2571                     skb_queue_len(&ring->queue));
2572                 break;
2573             }
2574         }
2575
2576         if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_HALT_NIC) {
2577             rtl_dbg(rtlpriv, COMP_RF, DBG_DMESG,
2578                 "IPS Set eRf nic disable\n");
2579             rtl_ps_disable_nic(hw);
2580             RT_SET_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
2581         } else {
2582             if (ppsc->rfoff_reason == RF_CHANGE_BY_IPS) {
2583                 rtlpriv->cfg->ops->led_control(hw,
2584                                    LED_CTL_NO_LINK);
2585             } else {
2586                 rtlpriv->cfg->ops->led_control(hw,
2587                                  LED_CTL_POWER_OFF);
2588             }
2589         }
2590         break;
2591
2592     case ERFSLEEP:
2593         if (ppsc->rfpwr_state == ERFOFF)
2594             break;
2595         for (queue_id = 0, i = 0;
2596              queue_id < RTL_PCI_MAX_TX_QUEUE_COUNT;) {
2597             ring = &pcipriv->dev.tx_ring[queue_id];
2598             if (skb_queue_len(&ring->queue) == 0) {
2599                 queue_id++;
2600                 continue;
2601             } else {
2602                 rtl_dbg(rtlpriv, COMP_ERR, DBG_WARNING,
2603                     "eRf Off/Sleep: %d times TcbBusyQueue[%d] =%d before doze!\n",
2604                     (i + 1), queue_id,
2605                     skb_queue_len(&ring->queue));
2606
2607                 udelay(10);
2608                 i++;
2609             }
2610             if (i >= MAX_DOZE_WAITING_TIMES_9x) {
2611                 rtl_dbg(rtlpriv, COMP_ERR, DBG_WARNING,
2612                     "ERFSLEEP: %d times TcbBusyQueue[%d] = %d !\n",
2613                     MAX_DOZE_WAITING_TIMES_9x,
2614                     queue_id,
2615                     skb_queue_len(&ring->queue));
2616                 break;
2617             }
2618         }
2619         rtl_dbg(rtlpriv, COMP_RF, DBG_DMESG,
2620             "Set ERFSLEEP awaked:%d ms\n",
2621             jiffies_to_msecs(jiffies -
2622                      ppsc->last_awake_jiffies));
2623         ppsc->last_sleep_jiffies = jiffies;
2624         _rtl8723be_phy_set_rf_sleep(hw);
2625         break;
2626
2627     default:
2628         rtl_dbg(rtlpriv, COMP_ERR, DBG_LOUD,
2629             "switch case %#x not processed\n", rfpwr_state);
2630         bresult = false;
2631         break;
2632     }
2633     if (bresult)
2634         ppsc->rfpwr_state = rfpwr_state;
2635     return bresult;
2636 }
2637
2638 bool rtl8723be_phy_set_rf_power_state(struct ieee80211_hw *hw,
2639                       enum rf_pwrstate rfpwr_state)
2640 {
2641     struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
2642
2643     bool bresult = false;
2644
2645     if (rfpwr_state == ppsc->rfpwr_state)
2646         return bresult;
2647     bresult = _rtl8723be_phy_set_rf_power_state(hw, rfpwr_state);
2648     return bresult;
2649 }