drivers/net/wireless/realtek/rtlwifi/rtl8723be/phy.c

   1 /******************************************************************************
   2  *
   3  * Copyright(c) 2009-2014  Realtek Corporation.
   4  *
   5  * This program is free software; you can redistribute it and/or modify it
   6  * under the terms of version 2 of the GNU General Public License as
   7  * published by the Free Software Foundation.
   8  *
   9  * This program is distributed in the hope that it will be useful, but WITHOUT
  10  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  11  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  12  * more details.
  13  *
  14  * The full GNU General Public License is included in this distribution in the
  15  * file called LICENSE.
  16  *
  17  * Contact Information:
  18  * wlanfae <wlanfae@realtek.com>
  19  * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
  20  * Hsinchu 300, Taiwan.
  21  *
  22  * Larry Finger <Larry.Finger@lwfinger.net>
  23  *
  24  *****************************************************************************/
  25
  26 #include "../wifi.h"
  27 #include "../pci.h"
  28 #include "../ps.h"
  29 #include "reg.h"
  30 #include "def.h"
  31 #include "phy.h"
  32 #include "../rtl8723com/phy_common.h"
  33 #include "rf.h"
  34 #include "dm.h"
  35 #include "../rtl8723com/dm_common.h"
  36 #include "table.h"
  37 #include "trx.h"
  38
  39 static bool _rtl8723be_phy_bb8723b_config_parafile(struct ieee80211_hw *hw);
  40 static bool _rtl8723be_phy_config_mac_with_headerfile(struct ieee80211_hw *hw);
  41 static bool _rtl8723be_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
  42                                                      u8 configtype);
  43 static bool _rtl8723be_phy_config_bb_with_pgheaderfile(struct ieee80211_hw *hw,
  44                                                        u8 configtype);
  45 static bool _rtl8723be_phy_sw_chnl_step_by_step(struct ieee80211_hw *hw,
  46                                                 u8 channel, u8 *stage,
  47                                                 u8 *step, u32 *delay);
  48
  49 static void rtl8723be_phy_set_rf_on(struct ieee80211_hw *hw);
  50 static void rtl8723be_phy_set_io(struct ieee80211_hw *hw);
  51
  52 u32 rtl8723be_phy_query_rf_reg(struct ieee80211_hw *hw, enum radio_path rfpath,
  53                                u32 regaddr, u32 bitmask)
  54 {
  55         struct rtl_priv *rtlpriv = rtl_priv(hw);
  56         u32 original_value, readback_value, bitshift;
  57         unsigned long flags;
  58
  59         RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
  60                  "regaddr(%#x), rfpath(%#x), bitmask(%#x)\n",
  61                   regaddr, rfpath, bitmask);
  62
  63         spin_lock_irqsave(&rtlpriv->locks.rf_lock, flags);
  64
  65         original_value = rtl8723_phy_rf_serial_read(hw, rfpath, regaddr);
  66         bitshift = rtl8723_phy_calculate_bit_shift(bitmask);
  67         readback_value = (original_value & bitmask) >> bitshift;
  68
  69         spin_unlock_irqrestore(&rtlpriv->locks.rf_lock, flags);
  70
  71         RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
  72                  "regaddr(%#x), rfpath(%#x), bitmask(%#x), original_value(%#x)\n",
  73                  regaddr, rfpath, bitmask, original_value);
  74
  75         return readback_value;
  76 }
  77
  78 void rtl8723be_phy_set_rf_reg(struct ieee80211_hw *hw, enum radio_path path,
  79                               u32 regaddr, u32 bitmask, u32 data)
  80 {
  81         struct rtl_priv *rtlpriv = rtl_priv(hw);
  82         u32 original_value, bitshift;
  83         unsigned long flags;
  84
  85         RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
  86                  "regaddr(%#x), bitmask(%#x), data(%#x), rfpath(%#x)\n",
  87                   regaddr, bitmask, data, path);
  88
  89         spin_lock_irqsave(&rtlpriv->locks.rf_lock, flags);
  90
  91         if (bitmask != RFREG_OFFSET_MASK) {
  92                         original_value = rtl8723_phy_rf_serial_read(hw, path,
  93                                                                     regaddr);
  94                         bitshift = rtl8723_phy_calculate_bit_shift(bitmask);
  95                         data = ((original_value & (~bitmask)) |
  96                                 (data << bitshift));
  97                 }
  98
  99         rtl8723_phy_rf_serial_write(hw, path, regaddr, data);
 100
 101         spin_unlock_irqrestore(&rtlpriv->locks.rf_lock, flags);
 102
 103         RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
 104                  "regaddr(%#x), bitmask(%#x), data(%#x), rfpath(%#x)\n",
 105                   regaddr, bitmask, data, path);
 106
 107 }
 108
 109 bool rtl8723be_phy_mac_config(struct ieee80211_hw *hw)
 110 {
 111         struct rtl_priv *rtlpriv = rtl_priv(hw);
 112         bool rtstatus = _rtl8723be_phy_config_mac_with_headerfile(hw);
 113
 114         rtl_write_byte(rtlpriv, 0x04CA, 0x0B);
 115         return rtstatus;
 116 }
 117
 118 bool rtl8723be_phy_bb_config(struct ieee80211_hw *hw)
 119 {
 120         bool rtstatus = true;
 121         struct rtl_priv *rtlpriv = rtl_priv(hw);
 122         u16 regval;
 123         u8 b_reg_hwparafile = 1;
 124         u32 tmp;
 125         u8 crystalcap = rtlpriv->efuse.crystalcap;
 126         rtl8723_phy_init_bb_rf_reg_def(hw);
 127         regval = rtl_read_word(rtlpriv, REG_SYS_FUNC_EN);
 128         rtl_write_word(rtlpriv, REG_SYS_FUNC_EN,
 129                        regval | BIT(13) | BIT(0) | BIT(1));
 130
 131         rtl_write_byte(rtlpriv, REG_RF_CTRL, RF_EN | RF_RSTB | RF_SDMRSTB);
 132         rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN,
 133                        FEN_PPLL | FEN_PCIEA | FEN_DIO_PCIE |
 134                        FEN_BB_GLB_RSTN | FEN_BBRSTB);
 135         tmp = rtl_read_dword(rtlpriv, 0x4c);
 136         rtl_write_dword(rtlpriv, 0x4c, tmp | BIT(23));
 137
 138         rtl_write_byte(rtlpriv, REG_AFE_XTAL_CTRL + 1, 0x80);
 139
 140         if (b_reg_hwparafile == 1)
 141                 rtstatus = _rtl8723be_phy_bb8723b_config_parafile(hw);
 142
 143         crystalcap = crystalcap & 0x3F;
 144         rtl_set_bbreg(hw, REG_MAC_PHY_CTRL, 0xFFF000,
 145                       (crystalcap | crystalcap << 6));
 146
 147         return rtstatus;
 148 }
 149
 150 bool rtl8723be_phy_rf_config(struct ieee80211_hw *hw)
 151 {
 152         return rtl8723be_phy_rf6052_config(hw);
 153 }
 154
 155 static bool _rtl8723be_check_positive(struct ieee80211_hw *hw,
 156                                       const u32 condition1,
 157                                       const u32 condition2)
 158 {
 159         struct rtl_priv *rtlpriv = rtl_priv(hw);
 160         struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
 161         u32 cut_ver = ((rtlhal->version & CHIP_VER_RTL_MASK)
 162                                         >> CHIP_VER_RTL_SHIFT);
 163         u32 intf = (rtlhal->interface == INTF_USB ? BIT(1) : BIT(0));
 164
 165         u8  board_type = ((rtlhal->board_type & BIT(4)) >> 4) << 0 | /* _GLNA */
 166                          ((rtlhal->board_type & BIT(3)) >> 3) << 1 | /* _GPA  */
 167                          ((rtlhal->board_type & BIT(7)) >> 7) << 2 | /* _ALNA */
 168                          ((rtlhal->board_type & BIT(6)) >> 6) << 3 | /* _APA  */
 169                          ((rtlhal->board_type & BIT(2)) >> 2) << 4;  /* _BT   */
 170
 171         u32 cond1 = condition1, cond2 = condition2;
 172         u32 driver1 = cut_ver << 24 |   /* CUT ver */
 173                       0 << 20 |                 /* interface 2/2 */
 174                       0x04 << 16 |              /* platform */
 175                       rtlhal->package_type << 12 |
 176                       intf << 8 |                       /* interface 1/2 */
 177                       board_type;
 178
 179         u32 driver2 = rtlhal->type_glna <<  0 |
 180                       rtlhal->type_gpa  <<  8 |
 181                       rtlhal->type_alna << 16 |
 182                       rtlhal->type_apa  << 24;
 183
 184         RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 185                  "===> [8812A] CheckPositive (cond1, cond2) = (0x%X 0x%X)\n",
 186                  cond1, cond2);
 187         RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 188                  "===> [8812A] CheckPositive (driver1, driver2) = (0x%X 0x%X)\n",
 189                  driver1, driver2);
 190
 191         RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 192                  "      (Platform, Interface) = (0x%X, 0x%X)\n", 0x04, intf);
 193         RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 194                  "      (Board, Package) = (0x%X, 0x%X)\n",
 195                  rtlhal->board_type, rtlhal->package_type);
 196
 197         /*============== Value Defined Check ===============*/
 198         /*QFN Type [15:12] and Cut Version [27:24] need to do value check*/
 199
 200         if (((cond1 & 0x0000F000) != 0) && ((cond1 & 0x0000F000) !=
 201                 (driver1 & 0x0000F000)))
 202                 return false;
 203         if (((cond1 & 0x0F000000) != 0) && ((cond1 & 0x0F000000) !=
 204                 (driver1 & 0x0F000000)))
 205                 return false;
 206
 207         /*=============== Bit Defined Check ================*/
 208         /* We don't care [31:28] */
 209
 210         cond1   &= 0x00FF0FFF;
 211         driver1 &= 0x00FF0FFF;
 212
 213         if ((cond1 & driver1) == cond1) {
 214                 u32 mask = 0;
 215
 216                 if ((cond1 & 0x0F) == 0) /* BoardType is DONTCARE*/
 217                         return true;
 218
 219                 if ((cond1 & BIT(0)) != 0) /*GLNA*/
 220                         mask |= 0x000000FF;
 221                 if ((cond1 & BIT(1)) != 0) /*GPA*/
 222                         mask |= 0x0000FF00;
 223                 if ((cond1 & BIT(2)) != 0) /*ALNA*/
 224                         mask |= 0x00FF0000;
 225                 if ((cond1 & BIT(3)) != 0) /*APA*/
 226                         mask |= 0xFF000000;
 227
 228                 /* BoardType of each RF path is matched*/
 229                 if ((cond2 & mask) == (driver2 & mask))
 230                         return true;
 231                 else
 232                         return false;
 233         }
 234         return false;
 235 }
 236
 237 static void _rtl8723be_config_rf_reg(struct ieee80211_hw *hw, u32 addr,
 238                                      u32 data, enum radio_path rfpath,
 239                                      u32 regaddr)
 240 {
 241         if (addr == 0xfe || addr == 0xffe) {
 242                 /* In order not to disturb BT music
 243                  *      when wifi init.(1ant NIC only)
 244                  */
 245                 mdelay(50);
 246         } else {
 247                 rtl_set_rfreg(hw, rfpath, regaddr, RFREG_OFFSET_MASK, data);
 248                 udelay(1);
 249         }
 250 }
 251 static void _rtl8723be_config_rf_radio_a(struct ieee80211_hw *hw,
 252                                          u32 addr, u32 data)
 253 {
 254         u32 content = 0x1000; /*RF Content: radio_a_txt*/
 255         u32 maskforphyset = (u32)(content & 0xE000);
 256
 257         _rtl8723be_config_rf_reg(hw, addr, data, RF90_PATH_A,
 258                                  addr | maskforphyset);
 259
 260 }
 261
 262 static void _rtl8723be_phy_init_tx_power_by_rate(struct ieee80211_hw *hw)
 263 {
 264         struct rtl_priv *rtlpriv = rtl_priv(hw);
 265         struct rtl_phy *rtlphy = &rtlpriv->phy;
 266
 267         u8 band, path, txnum, section;
 268
 269         for (band = BAND_ON_2_4G; band <= BAND_ON_5G; ++band)
 270                 for (path = 0; path < TX_PWR_BY_RATE_NUM_RF; ++path)
 271                         for (txnum = 0; txnum < TX_PWR_BY_RATE_NUM_RF; ++txnum)
 272                                 for (section = 0;
 273                                      section < TX_PWR_BY_RATE_NUM_SECTION;
 274                                      ++section)
 275                                         rtlphy->tx_power_by_rate_offset
 276                                           [band][path][txnum][section] = 0;
 277 }
 278
 279 static void _rtl8723be_config_bb_reg(struct ieee80211_hw *hw,
 280                                      u32 addr, u32 data)
 281 {
 282         if (addr == 0xfe) {
 283                 mdelay(50);
 284         } else if (addr == 0xfd) {
 285                 mdelay(5);
 286         } else if (addr == 0xfc) {
 287                 mdelay(1);
 288         } else if (addr == 0xfb) {
 289                 udelay(50);
 290         } else if (addr == 0xfa) {
 291                 udelay(5);
 292         } else if (addr == 0xf9) {
 293                 udelay(1);
 294         } else {
 295                 rtl_set_bbreg(hw, addr, MASKDWORD, data);
 296                 udelay(1);
 297         }
 298 }
 299
 300 static void _rtl8723be_phy_set_txpower_by_rate_base(struct ieee80211_hw *hw,
 301                                                     u8 band,
 302                                                     u8 path, u8 rate_section,
 303                                                     u8 txnum, u8 value)
 304 {
 305         struct rtl_priv *rtlpriv = rtl_priv(hw);
 306         struct rtl_phy *rtlphy = &rtlpriv->phy;
 307
 308         if (path > RF90_PATH_D) {
 309                 RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
 310                          "Invalid Rf Path %d in phy_SetTxPowerByRatBase()\n",
 311                           path);
 312                 return;
 313         }
 314
 315         if (band == BAND_ON_2_4G) {
 316                 switch (rate_section) {
 317                 case CCK:
 318                         rtlphy->txpwr_by_rate_base_24g[path][txnum][0] = value;
 319                         break;
 320                 case OFDM:
 321                         rtlphy->txpwr_by_rate_base_24g[path][txnum][1] = value;
 322                         break;
 323                 case HT_MCS0_MCS7:
 324                         rtlphy->txpwr_by_rate_base_24g[path][txnum][2] = value;
 325                         break;
 326                 case HT_MCS8_MCS15:
 327                         rtlphy->txpwr_by_rate_base_24g[path][txnum][3] = value;
 328                         break;
 329                 default:
 330                         RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
 331                                  "Invalid RateSection %d in Band 2.4G, Rf Path %d, %dTx in PHY_SetTxPowerByRateBase()\n",
 332                                  rate_section, path, txnum);
 333                         break;
 334                 };
 335         } else {
 336                 RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
 337                          "Invalid Band %d in PHY_SetTxPowerByRateBase()\n",
 338                          band);
 339         }
 340
 341 }
 342
 343 static u8 _rtl8723be_phy_get_txpower_by_rate_base(struct ieee80211_hw *hw,
 344                                                   u8 band, u8 path, u8 txnum,
 345                                                   u8 rate_section)
 346 {
 347         struct rtl_priv *rtlpriv = rtl_priv(hw);
 348         struct rtl_phy *rtlphy = &rtlpriv->phy;
 349         u8 value = 0;
 350         if (path > RF90_PATH_D) {
 351                 RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
 352                          "Invalid Rf Path %d in PHY_GetTxPowerByRateBase()\n",
 353                           path);
 354                 return 0;
 355         }
 356
 357         if (band == BAND_ON_2_4G) {
 358                 switch (rate_section) {
 359                 case CCK:
 360                         value = rtlphy->txpwr_by_rate_base_24g[path][txnum][0];
 361                         break;
 362                 case OFDM:
 363                         value = rtlphy->txpwr_by_rate_base_24g[path][txnum][1];
 364                         break;
 365                 case HT_MCS0_MCS7:
 366                         value = rtlphy->txpwr_by_rate_base_24g[path][txnum][2];
 367                         break;
 368                 case HT_MCS8_MCS15:
 369                         value = rtlphy->txpwr_by_rate_base_24g[path][txnum][3];
 370                         break;
 371                 default:
 372                         RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
 373                                  "Invalid RateSection %d in Band 2.4G, Rf Path %d, %dTx in PHY_GetTxPowerByRateBase()\n",
 374                                  rate_section, path, txnum);
 375                         break;
 376                 };
 377         } else {
 378                 RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
 379                          "Invalid Band %d in PHY_GetTxPowerByRateBase()\n",
 380                          band);
 381         }
 382
 383         return value;
 384 }
 385
 386 static void _rtl8723be_phy_store_txpower_by_rate_base(struct ieee80211_hw *hw)
 387 {
 388         struct rtl_priv *rtlpriv = rtl_priv(hw);
 389         struct rtl_phy *rtlphy = &rtlpriv->phy;
 390         u16 rawvalue = 0;
 391         u8 base = 0, path = 0;
 392
 393         for (path = RF90_PATH_A; path <= RF90_PATH_B; ++path) {
 394                 if (path == RF90_PATH_A) {
 395                         rawvalue = (u16)(rtlphy->tx_power_by_rate_offset
 396                                 [BAND_ON_2_4G][path][RF_1TX][3] >> 24) & 0xFF;
 397                         base = (rawvalue >> 4) * 10 + (rawvalue & 0xF);
 398                         _rtl8723be_phy_set_txpower_by_rate_base(hw,
 399                                 BAND_ON_2_4G, path, CCK, RF_1TX, base);
 400                 } else if (path == RF90_PATH_B) {
 401                         rawvalue = (u16)(rtlphy->tx_power_by_rate_offset
 402                                 [BAND_ON_2_4G][path][RF_1TX][3] >> 0) & 0xFF;
 403                         base = (rawvalue >> 4) * 10 + (rawvalue & 0xF);
 404                         _rtl8723be_phy_set_txpower_by_rate_base(hw,
 405                                                                 BAND_ON_2_4G,
 406                                                                 path, CCK,
 407                                                                 RF_1TX, base);
 408                 }
 409                 rawvalue = (u16)(rtlphy->tx_power_by_rate_offset
 410                                 [BAND_ON_2_4G][path][RF_1TX][1] >> 24) & 0xFF;
 411                 base = (rawvalue >> 4) * 10 + (rawvalue & 0xF);
 412                 _rtl8723be_phy_set_txpower_by_rate_base(hw, BAND_ON_2_4G,
 413                                                         path, OFDM, RF_1TX,
 414                                                         base);
 415
 416                 rawvalue = (u16)(rtlphy->tx_power_by_rate_offset
 417                                 [BAND_ON_2_4G][path][RF_1TX][5] >> 24) & 0xFF;
 418                 base = (rawvalue >> 4) * 10 + (rawvalue & 0xF);
 419                 _rtl8723be_phy_set_txpower_by_rate_base(hw, BAND_ON_2_4G,
 420                                                         path, HT_MCS0_MCS7,
 421                                                         RF_1TX, base);
 422
 423                 rawvalue = (u16)(rtlphy->tx_power_by_rate_offset
 424                                 [BAND_ON_2_4G][path][RF_2TX][7] >> 24) & 0xFF;
 425                 base = (rawvalue >> 4) * 10 + (rawvalue & 0xF);
 426                 _rtl8723be_phy_set_txpower_by_rate_base(hw, BAND_ON_2_4G,
 427                                                         path, HT_MCS8_MCS15,
 428                                                         RF_2TX, base);
 429         }
 430 }
 431
 432 static void _phy_convert_txpower_dbm_to_relative_value(u32 *data, u8 start,
 433                                                 u8 end, u8 base_val)
 434 {
 435         s8 i = 0;
 436         u8 temp_value = 0;
 437         u32 temp_data = 0;
 438
 439         for (i = 3; i >= 0; --i) {
 440                 if (i >= start && i <= end) {
 441                         /* Get the exact value */
 442                         temp_value = (u8)(*data >> (i * 8)) & 0xF;
 443                         temp_value += ((u8)((*data >> (i*8 + 4)) & 0xF)) * 10;
 444
 445                         /* Change the value to a relative value */
 446                         temp_value = (temp_value > base_val) ?
 447                                      temp_value - base_val :
 448                                      base_val - temp_value;
 449                 } else {
 450                         temp_value = (u8)(*data >> (i * 8)) & 0xFF;
 451                 }
 452                 temp_data <<= 8;
 453                 temp_data |= temp_value;
 454         }
 455         *data = temp_data;
 456 }
 457
 458 static void _rtl8723be_phy_convert_txpower_dbm_to_relative_value(
 459                                                         struct ieee80211_hw *hw)
 460 {
 461         struct rtl_priv *rtlpriv = rtl_priv(hw);
 462         struct rtl_phy *rtlphy = &rtlpriv->phy;
 463         u8 base = 0, rfpath = RF90_PATH_A;
 464
 465         base = _rtl8723be_phy_get_txpower_by_rate_base(hw,
 466                         BAND_ON_2_4G, rfpath, RF_1TX, CCK);
 467         _phy_convert_txpower_dbm_to_relative_value(
 468             &rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfpath][RF_1TX][2],
 469             1, 1, base);
 470         _phy_convert_txpower_dbm_to_relative_value(
 471             &rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfpath][RF_1TX][3],
 472             1, 3, base);
 473
 474         base = _rtl8723be_phy_get_txpower_by_rate_base(hw, BAND_ON_2_4G, rfpath,
 475                                                        RF_1TX, OFDM);
 476         _phy_convert_txpower_dbm_to_relative_value(
 477             &rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfpath][RF_1TX][0],
 478             0, 3, base);
 479         _phy_convert_txpower_dbm_to_relative_value(
 480             &rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfpath][RF_1TX][1],
 481             0, 3, base);
 482
 483         base = _rtl8723be_phy_get_txpower_by_rate_base(hw, BAND_ON_2_4G,
 484                                                 rfpath, RF_1TX, HT_MCS0_MCS7);
 485         _phy_convert_txpower_dbm_to_relative_value(
 486             &rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfpath][RF_1TX][4],
 487             0, 3, base);
 488         _phy_convert_txpower_dbm_to_relative_value(
 489             &rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfpath][RF_1TX][5],
 490             0, 3, base);
 491
 492         base = _rtl8723be_phy_get_txpower_by_rate_base(hw, BAND_ON_2_4G,
 493                                                        rfpath, RF_2TX,
 494                                                        HT_MCS8_MCS15);
 495         _phy_convert_txpower_dbm_to_relative_value(
 496             &rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfpath][RF_2TX][6],
 497             0, 3, base);
 498
 499         _phy_convert_txpower_dbm_to_relative_value(
 500             &rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfpath][RF_2TX][7],
 501             0, 3, base);
 502
 503         RT_TRACE(rtlpriv, COMP_POWER, DBG_TRACE,
 504             "<===_rtl8723be_phy_convert_txpower_dbm_to_relative_value()\n");
 505 }
 506
 507 static void phy_txpower_by_rate_config(struct ieee80211_hw *hw)
 508 {
 509         _rtl8723be_phy_store_txpower_by_rate_base(hw);
 510         _rtl8723be_phy_convert_txpower_dbm_to_relative_value(hw);
 511 }
 512
 513 static bool _rtl8723be_phy_bb8723b_config_parafile(struct ieee80211_hw *hw)
 514 {
 515         struct rtl_priv *rtlpriv = rtl_priv(hw);
 516         struct rtl_phy *rtlphy = &rtlpriv->phy;
 517         struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
 518         bool rtstatus;
 519
 520         /* switch ant to BT */
 521         if (rtlpriv->rtlhal.interface == INTF_USB) {
 522                 rtl_write_dword(rtlpriv, 0x948, 0x0);
 523         } else {
 524                 if (rtlpriv->btcoexist.btc_info.single_ant_path == 0)
 525                         rtl_write_dword(rtlpriv, 0x948, 0x280);
 526                 else
 527                         rtl_write_dword(rtlpriv, 0x948, 0x0);
 528         }
 529
 530         rtstatus = _rtl8723be_phy_config_bb_with_headerfile(hw,
 531                                                 BASEBAND_CONFIG_PHY_REG);
 532         if (!rtstatus) {
 533                 pr_err("Write BB Reg Fail!!\n");
 534                 return false;
 535         }
 536         _rtl8723be_phy_init_tx_power_by_rate(hw);
 537         if (!rtlefuse->autoload_failflag) {
 538                 rtlphy->pwrgroup_cnt = 0;
 539                 rtstatus = _rtl8723be_phy_config_bb_with_pgheaderfile(hw,
 540                                                 BASEBAND_CONFIG_PHY_REG);
 541         }
 542         phy_txpower_by_rate_config(hw);
 543         if (!rtstatus) {
 544                 pr_err("BB_PG Reg Fail!!\n");
 545                 return false;
 546         }
 547         rtstatus = _rtl8723be_phy_config_bb_with_headerfile(hw,
 548                                                 BASEBAND_CONFIG_AGC_TAB);
 549         if (!rtstatus) {
 550                 pr_err("AGC Table Fail\n");
 551                 return false;
 552         }
 553         rtlphy->cck_high_power = (bool)(rtl_get_bbreg(hw,
 554                                                       RFPGA0_XA_HSSIPARAMETER2,
 555                                                       0x200));
 556         return true;
 557 }
 558
 559 static bool rtl8723be_phy_config_with_headerfile(struct ieee80211_hw *hw,
 560                                                  u32 *array_table,
 561                                                  u16 arraylen,
 562                 void (*set_reg)(struct ieee80211_hw *hw, u32 regaddr, u32 data))
 563 {
 564         #define COND_ELSE  2
 565         #define COND_ENDIF 3
 566
 567         int i = 0;
 568         u8 cond;
 569         bool matched = true, skipped = false;
 570
 571         while ((i + 1) < arraylen) {
 572                 u32 v1 = array_table[i];
 573                 u32 v2 = array_table[i + 1];
 574
 575                 if (v1 & (BIT(31) | BIT(30))) {/*positive & negative condition*/
 576                         if (v1 & BIT(31)) {/* positive condition*/
 577                                 cond  = (u8)((v1 & (BIT(29) | BIT(28))) >> 28);
 578                                 if (cond == COND_ENDIF) { /*end*/
 579                                         matched = true;
 580                                         skipped = false;
 581                                 } else if (cond == COND_ELSE) { /*else*/
 582                                         matched = skipped ? false : true;
 583                                 } else {/*if , else if*/
 584                                         if (skipped) {
 585                                                 matched = false;
 586                                         } else {
 587                                                 if (_rtl8723be_check_positive(
 588                                                                 hw, v1, v2)) {
 589                                                         matched = true;
 590                                                         skipped = true;
 591                                                 } else {
 592                                                         matched = false;
 593                                                         skipped = false;
 594                                                 }
 595                                         }
 596                                 }
 597                         } else if (v1 & BIT(30)) { /*negative condition*/
 598                         /*do nothing*/
 599                         }
 600                 } else {
 601                         if (matched)
 602                                 set_reg(hw, v1, v2);
 603                 }
 604                 i = i + 2;
 605         }
 606
 607         return true;
 608 }
 609
 610 static bool _rtl8723be_phy_config_mac_with_headerfile(struct ieee80211_hw *hw)
 611 {
 612         struct rtl_priv *rtlpriv = rtl_priv(hw);
 613
 614         RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, "Read rtl8723beMACPHY_Array\n");
 615
 616         return rtl8723be_phy_config_with_headerfile(hw,
 617                         RTL8723BEMAC_1T_ARRAY, RTL8723BEMAC_1T_ARRAYLEN,
 618                         rtl_write_byte_with_val32);
 619 }
 620
 621 static bool _rtl8723be_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
 622                                                      u8 configtype)
 623 {
 624
 625         if (configtype == BASEBAND_CONFIG_PHY_REG)
 626                 return rtl8723be_phy_config_with_headerfile(hw,
 627                                 RTL8723BEPHY_REG_1TARRAY,
 628                                 RTL8723BEPHY_REG_1TARRAYLEN,
 629                                 _rtl8723be_config_bb_reg);
 630         else if (configtype == BASEBAND_CONFIG_AGC_TAB)
 631                 return rtl8723be_phy_config_with_headerfile(hw,
 632                                 RTL8723BEAGCTAB_1TARRAY,
 633                                 RTL8723BEAGCTAB_1TARRAYLEN,
 634                                 rtl_set_bbreg_with_dwmask);
 635
 636         return false;
 637 }
 638
 639 static u8 _rtl8723be_get_rate_section_index(u32 regaddr)
 640 {
 641         u8 index = 0;
 642
 643         switch (regaddr) {
 644         case RTXAGC_A_RATE18_06:
 645                 index = 0;
 646         break;
 647         case RTXAGC_A_RATE54_24:
 648                 index = 1;
 649         break;
 650         case RTXAGC_A_CCK1_MCS32:
 651                 index = 2;
 652         break;
 653         case RTXAGC_B_CCK11_A_CCK2_11:
 654                 index = 3;
 655         break;
 656         case RTXAGC_A_MCS03_MCS00:
 657                 index = 4;
 658         break;
 659         case RTXAGC_A_MCS07_MCS04:
 660                 index = 5;
 661         break;
 662         case RTXAGC_A_MCS11_MCS08:
 663                 index = 6;
 664         break;
 665         case RTXAGC_A_MCS15_MCS12:
 666                 index = 7;
 667         break;
 668         case RTXAGC_B_RATE18_06:
 669                 index = 0;
 670         break;
 671         case RTXAGC_B_RATE54_24:
 672                 index = 1;
 673         break;
 674         case RTXAGC_B_CCK1_55_MCS32:
 675                 index = 2;
 676         break;
 677         case RTXAGC_B_MCS03_MCS00:
 678                 index = 4;
 679         break;
 680         case RTXAGC_B_MCS07_MCS04:
 681                 index = 5;
 682         break;
 683         case RTXAGC_B_MCS11_MCS08:
 684                 index = 6;
 685         break;
 686         case RTXAGC_B_MCS15_MCS12:
 687                 index = 7;
 688         break;
 689         default:
 690                 regaddr &= 0xFFF;
 691                 if (regaddr >= 0xC20 && regaddr <= 0xC4C)
 692                         index = (u8)((regaddr - 0xC20) / 4);
 693                 else if (regaddr >= 0xE20 && regaddr <= 0xE4C)
 694                         index = (u8)((regaddr - 0xE20) / 4);
 695                 break;
 696         };
 697         return index;
 698 }
 699
 700 static void _rtl8723be_store_tx_power_by_rate(struct ieee80211_hw *hw,
 701                                               u32 band, u32 rfpath,
 702                                               u32 txnum, u32 regaddr,
 703                                               u32 bitmask, u32 data)
 704 {
 705         struct rtl_priv *rtlpriv = rtl_priv(hw);
 706         struct rtl_phy *rtlphy = &rtlpriv->phy;
 707         u8 rate_section = _rtl8723be_get_rate_section_index(regaddr);
 708
 709         if (band != BAND_ON_2_4G && band != BAND_ON_5G) {
 710                 RT_TRACE(rtlpriv, FPHY, PHY_TXPWR, "Invalid Band %d\n", band);
 711                 return;
 712         }
 713         if (rfpath > MAX_RF_PATH - 1) {
 714                 RT_TRACE(rtlpriv, FPHY, PHY_TXPWR,
 715                          "Invalid RfPath %d\n", rfpath);
 716                 return;
 717         }
 718         if (txnum > MAX_RF_PATH - 1) {
 719                 RT_TRACE(rtlpriv, FPHY, PHY_TXPWR, "Invalid TxNum %d\n", txnum);
 720                 return;
 721         }
 722
 723         rtlphy->tx_power_by_rate_offset[band][rfpath][txnum][rate_section] =
 724                                                                         data;
 725
 726 }
 727
 728 static bool _rtl8723be_phy_config_bb_with_pgheaderfile(struct ieee80211_hw *hw,
 729                                                        u8 configtype)
 730 {
 731         struct rtl_priv *rtlpriv = rtl_priv(hw);
 732         int i;
 733         u32 *phy_regarray_table_pg;
 734         u16 phy_regarray_pg_len;
 735         u32 v1 = 0, v2 = 0, v3 = 0, v4 = 0, v5 = 0, v6 = 0;
 736
 737         phy_regarray_pg_len = RTL8723BEPHY_REG_ARRAY_PGLEN;
 738         phy_regarray_table_pg = RTL8723BEPHY_REG_ARRAY_PG;
 739
 740         if (configtype == BASEBAND_CONFIG_PHY_REG) {
 741                 for (i = 0; i < phy_regarray_pg_len; i = i + 6) {
 742                         v1 = phy_regarray_table_pg[i];
 743                         v2 = phy_regarray_table_pg[i+1];
 744                         v3 = phy_regarray_table_pg[i+2];
 745                         v4 = phy_regarray_table_pg[i+3];
 746                         v5 = phy_regarray_table_pg[i+4];
 747                         v6 = phy_regarray_table_pg[i+5];
 748
 749                         if (v1 < 0xcdcdcdcd) {
 750                                 if (phy_regarray_table_pg[i] == 0xfe ||
 751                                     phy_regarray_table_pg[i] == 0xffe)
 752                                         mdelay(50);
 753                                 else
 754                                         _rtl8723be_store_tx_power_by_rate(hw,
 755                                                         v1, v2, v3, v4, v5, v6);
 756                                 continue;
 757                         }
 758                 }
 759         } else {
 760                 RT_TRACE(rtlpriv, COMP_SEND, DBG_TRACE,
 761                          "configtype != BaseBand_Config_PHY_REG\n");
 762         }
 763         return true;
 764 }
 765
 766 bool rtl8723be_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
 767                                              enum radio_path rfpath)
 768 {
 769         struct rtl_priv *rtlpriv = rtl_priv(hw);
 770         struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 771         bool ret = true;
 772
 773         RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "Radio No %x\n", rfpath);
 774         switch (rfpath) {
 775         case RF90_PATH_A:
 776                 ret =  rtl8723be_phy_config_with_headerfile(hw,
 777                                 RTL8723BE_RADIOA_1TARRAY,
 778                                 RTL8723BE_RADIOA_1TARRAYLEN,
 779                                 _rtl8723be_config_rf_radio_a);
 780
 781                 if (rtlhal->oem_id == RT_CID_819X_HP)
 782                         _rtl8723be_config_rf_radio_a(hw, 0x52, 0x7E4BD);
 783                 break;
 784         case RF90_PATH_B:
 785         case RF90_PATH_C:
 786                 break;
 787         case RF90_PATH_D:
 788                 RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
 789                          "switch case %#x not processed\n", rfpath);
 790                 break;
 791         }
 792         return ret;
 793 }
 794
 795 void rtl8723be_phy_get_hw_reg_originalvalue(struct ieee80211_hw *hw)
 796 {
 797         struct rtl_priv *rtlpriv = rtl_priv(hw);
 798         struct rtl_phy *rtlphy = &rtlpriv->phy;
 799
 800         rtlphy->default_initialgain[0] =
 801             (u8)rtl_get_bbreg(hw, ROFDM0_XAAGCCORE1, MASKBYTE0);
 802         rtlphy->default_initialgain[1] =
 803             (u8)rtl_get_bbreg(hw, ROFDM0_XBAGCCORE1, MASKBYTE0);
 804         rtlphy->default_initialgain[2] =
 805             (u8)rtl_get_bbreg(hw, ROFDM0_XCAGCCORE1, MASKBYTE0);
 806         rtlphy->default_initialgain[3] =
 807             (u8)rtl_get_bbreg(hw, ROFDM0_XDAGCCORE1, MASKBYTE0);
 808
 809         RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 810                  "Default initial gain (c50=0x%x, c58=0x%x, c60=0x%x, c68=0x%x\n",
 811                  rtlphy->default_initialgain[0],
 812                  rtlphy->default_initialgain[1],
 813                  rtlphy->default_initialgain[2],
 814                  rtlphy->default_initialgain[3]);
 815
 816         rtlphy->framesync = (u8)rtl_get_bbreg(hw, ROFDM0_RXDETECTOR3,
 817                                                MASKBYTE0);
 818         rtlphy->framesync_c34 = rtl_get_bbreg(hw, ROFDM0_RXDETECTOR2,
 819                                               MASKDWORD);
 820
 821         RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 822                  "Default framesync (0x%x) = 0x%x\n",
 823                   ROFDM0_RXDETECTOR3, rtlphy->framesync);
 824 }
 825
 826 static u8 _rtl8723be_phy_get_ratesection_intxpower_byrate(enum radio_path path,
 827                                                           u8 rate)
 828 {
 829         u8 rate_section = 0;
 830
 831         switch (rate) {
 832         case DESC92C_RATE1M:
 833                 rate_section = 2;
 834                 break;
 835
 836         case DESC92C_RATE2M:
 837         case DESC92C_RATE5_5M:
 838                 if (path == RF90_PATH_A)
 839                         rate_section = 3;
 840                 else if (path == RF90_PATH_B)
 841                         rate_section = 2;
 842                 break;
 843
 844         case DESC92C_RATE11M:
 845                 rate_section = 3;
 846                 break;
 847
 848         case DESC92C_RATE6M:
 849         case DESC92C_RATE9M:
 850         case DESC92C_RATE12M:
 851         case DESC92C_RATE18M:
 852                 rate_section = 0;
 853                 break;
 854
 855         case DESC92C_RATE24M:
 856         case DESC92C_RATE36M:
 857         case DESC92C_RATE48M:
 858         case DESC92C_RATE54M:
 859                 rate_section = 1;
 860                 break;
 861
 862         case DESC92C_RATEMCS0:
 863         case DESC92C_RATEMCS1:
 864         case DESC92C_RATEMCS2:
 865         case DESC92C_RATEMCS3:
 866                 rate_section = 4;
 867                 break;
 868
 869         case DESC92C_RATEMCS4:
 870         case DESC92C_RATEMCS5:
 871         case DESC92C_RATEMCS6:
 872         case DESC92C_RATEMCS7:
 873                 rate_section = 5;
 874                 break;
 875
 876         case DESC92C_RATEMCS8:
 877         case DESC92C_RATEMCS9:
 878         case DESC92C_RATEMCS10:
 879         case DESC92C_RATEMCS11:
 880                 rate_section = 6;
 881                 break;
 882
 883         case DESC92C_RATEMCS12:
 884         case DESC92C_RATEMCS13:
 885         case DESC92C_RATEMCS14:
 886         case DESC92C_RATEMCS15:
 887                 rate_section = 7;
 888                 break;
 889
 890         default:
 891                 WARN_ONCE(true, "rtl8723be: Rate_Section is Illegal\n");
 892                 break;
 893         }
 894
 895         return rate_section;
 896 }
 897
 898 static u8 _rtl8723be_get_txpower_by_rate(struct ieee80211_hw *hw,
 899                                          enum band_type band,
 900                                          enum radio_path rfpath, u8 rate)
 901 {
 902         struct rtl_priv *rtlpriv = rtl_priv(hw);
 903         struct rtl_phy *rtlphy = &rtlpriv->phy;
 904         u8 shift = 0, rate_section, tx_num;
 905         s8 tx_pwr_diff = 0;
 906
 907         rate_section = _rtl8723be_phy_get_ratesection_intxpower_byrate(rfpath,
 908                                                                        rate);
 909         tx_num = RF_TX_NUM_NONIMPLEMENT;
 910
 911         if (tx_num == RF_TX_NUM_NONIMPLEMENT) {
 912                 if (rate >= DESC92C_RATEMCS8 && rate <= DESC92C_RATEMCS15)
 913                         tx_num = RF_2TX;
 914                 else
 915                         tx_num = RF_1TX;
 916         }
 917
 918         switch (rate) {
 919         case DESC92C_RATE6M:
 920         case DESC92C_RATE24M:
 921         case DESC92C_RATEMCS0:
 922         case DESC92C_RATEMCS4:
 923         case DESC92C_RATEMCS8:
 924         case DESC92C_RATEMCS12:
 925                 shift = 0;
 926                 break;
 927         case DESC92C_RATE1M:
 928         case DESC92C_RATE2M:
 929         case DESC92C_RATE9M:
 930         case DESC92C_RATE36M:
 931         case DESC92C_RATEMCS1:
 932         case DESC92C_RATEMCS5:
 933         case DESC92C_RATEMCS9:
 934         case DESC92C_RATEMCS13:
 935                 shift = 8;
 936                 break;
 937         case DESC92C_RATE5_5M:
 938         case DESC92C_RATE12M:
 939         case DESC92C_RATE48M:
 940         case DESC92C_RATEMCS2:
 941         case DESC92C_RATEMCS6:
 942         case DESC92C_RATEMCS10:
 943         case DESC92C_RATEMCS14:
 944                 shift = 16;
 945                 break;
 946         case DESC92C_RATE11M:
 947         case DESC92C_RATE18M:
 948         case DESC92C_RATE54M:
 949         case DESC92C_RATEMCS3:
 950         case DESC92C_RATEMCS7:
 951         case DESC92C_RATEMCS11:
 952         case DESC92C_RATEMCS15:
 953                 shift = 24;
 954                 break;
 955         default:
 956                 WARN_ONCE(true, "rtl8723be: Rate_Section is Illegal\n");
 957                 break;
 958         }
 959         tx_pwr_diff = (u8)(rtlphy->tx_power_by_rate_offset[band][rfpath][tx_num]
 960                                           [rate_section] >> shift) & 0xff;
 961
 962         return  tx_pwr_diff;
 963 }
 964
 965 static u8 _rtl8723be_get_txpower_index(struct ieee80211_hw *hw, u8 path,
 966                                        u8 rate, u8 bandwidth, u8 channel)
 967 {
 968         struct rtl_priv *rtlpriv = rtl_priv(hw);
 969         struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
 970         u8 index = (channel - 1);
 971         u8 txpower = 0;
 972         u8 power_diff_byrate = 0;
 973
 974         if (channel > 14 || channel < 1) {
 975                 index = 0;
 976                 RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
 977                          "Illegal channel!\n");
 978         }
 979         if (RX_HAL_IS_CCK_RATE(rate))
 980                 txpower = rtlefuse->txpwrlevel_cck[path][index];
 981         else if (DESC92C_RATE6M <= rate)
 982                 txpower = rtlefuse->txpwrlevel_ht40_1s[path][index];
 983         else
 984                 RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
 985                          "invalid rate\n");
 986
 987         if (DESC92C_RATE6M <= rate && rate <= DESC92C_RATE54M &&
 988             !RX_HAL_IS_CCK_RATE(rate))
 989                 txpower += rtlefuse->txpwr_legacyhtdiff[0][TX_1S];
 990
 991         if (bandwidth == HT_CHANNEL_WIDTH_20) {
 992                 if (DESC92C_RATEMCS0 <= rate && rate <= DESC92C_RATEMCS15)
 993                         txpower += rtlefuse->txpwr_ht20diff[0][TX_1S];
 994                 if (DESC92C_RATEMCS8 <= rate && rate <= DESC92C_RATEMCS15)
 995                         txpower += rtlefuse->txpwr_ht20diff[0][TX_2S];
 996         } else if (bandwidth == HT_CHANNEL_WIDTH_20_40) {
 997                 if (DESC92C_RATEMCS0 <= rate && rate <= DESC92C_RATEMCS15)
 998                         txpower += rtlefuse->txpwr_ht40diff[0][TX_1S];
 999                 if (DESC92C_RATEMCS8 <= rate && rate <= DESC92C_RATEMCS15)
1000                         txpower += rtlefuse->txpwr_ht40diff[0][TX_2S];
1001         }
1002
1003         if (rtlefuse->eeprom_regulatory != 2)
1004                 power_diff_byrate = _rtl8723be_get_txpower_by_rate(hw,
1005                                                                    BAND_ON_2_4G,
1006                                                                    path, rate);
1007
1008         txpower += power_diff_byrate;
1009
1010         if (txpower > MAX_POWER_INDEX)
1011                 txpower = MAX_POWER_INDEX;
1012
1013         return txpower;
1014 }
1015
1016 static void _rtl8723be_phy_set_txpower_index(struct ieee80211_hw *hw,
1017                                              u8 power_index, u8 path, u8 rate)
1018 {
1019         struct rtl_priv *rtlpriv = rtl_priv(hw);
1020         if (path == RF90_PATH_A) {
1021                 switch (rate) {
1022                 case DESC92C_RATE1M:
1023                         rtl8723_phy_set_bb_reg(hw, RTXAGC_A_CCK1_MCS32,
1024                                                MASKBYTE1, power_index);
1025                         break;
1026                 case DESC92C_RATE2M:
1027                         rtl8723_phy_set_bb_reg(hw, RTXAGC_B_CCK11_A_CCK2_11,
1028                                                MASKBYTE1, power_index);
1029                         break;
1030                 case DESC92C_RATE5_5M:
1031                         rtl8723_phy_set_bb_reg(hw, RTXAGC_B_CCK11_A_CCK2_11,
1032                                                MASKBYTE2, power_index);
1033                         break;
1034                 case DESC92C_RATE11M:
1035                         rtl8723_phy_set_bb_reg(hw, RTXAGC_B_CCK11_A_CCK2_11,
1036                                                MASKBYTE3, power_index);
1037                         break;
1038
1039                 case DESC92C_RATE6M:
1040                         rtl8723_phy_set_bb_reg(hw, RTXAGC_A_RATE18_06,
1041                                                MASKBYTE0, power_index);
1042                         break;
1043                 case DESC92C_RATE9M:
1044                         rtl8723_phy_set_bb_reg(hw, RTXAGC_A_RATE18_06,
1045                                                MASKBYTE1, power_index);
1046                         break;
1047                 case DESC92C_RATE12M:
1048                         rtl8723_phy_set_bb_reg(hw, RTXAGC_A_RATE18_06,
1049                                                MASKBYTE2, power_index);
1050                         break;
1051                 case DESC92C_RATE18M:
1052                         rtl8723_phy_set_bb_reg(hw, RTXAGC_A_RATE18_06,
1053                                                MASKBYTE3, power_index);
1054                         break;
1055
1056                 case DESC92C_RATE24M:
1057                         rtl8723_phy_set_bb_reg(hw, RTXAGC_A_RATE54_24,
1058                                                MASKBYTE0, power_index);
1059                         break;
1060                 case DESC92C_RATE36M:
1061                         rtl8723_phy_set_bb_reg(hw, RTXAGC_A_RATE54_24,
1062                                                MASKBYTE1, power_index);
1063                         break;
1064                 case DESC92C_RATE48M:
1065                         rtl8723_phy_set_bb_reg(hw, RTXAGC_A_RATE54_24,
1066                                                MASKBYTE2, power_index);
1067                         break;
1068                 case DESC92C_RATE54M:
1069                         rtl8723_phy_set_bb_reg(hw, RTXAGC_A_RATE54_24,
1070                                                MASKBYTE3, power_index);
1071                         break;
1072
1073                 case DESC92C_RATEMCS0:
1074                         rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS03_MCS00,
1075                                                MASKBYTE0, power_index);
1076                         break;
1077                 case DESC92C_RATEMCS1:
1078                         rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS03_MCS00,
1079                                                MASKBYTE1, power_index);
1080                         break;
1081                 case DESC92C_RATEMCS2:
1082                         rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS03_MCS00,
1083                                                MASKBYTE2, power_index);
1084                         break;
1085                 case DESC92C_RATEMCS3:
1086                         rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS03_MCS00,
1087                                                MASKBYTE3, power_index);
1088                         break;
1089
1090                 case DESC92C_RATEMCS4:
1091                         rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS07_MCS04,
1092                                                MASKBYTE0, power_index);
1093                         break;
1094                 case DESC92C_RATEMCS5:
1095                         rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS07_MCS04,
1096                                                MASKBYTE1, power_index);
1097                         break;
1098                 case DESC92C_RATEMCS6:
1099                         rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS07_MCS04,
1100                                                MASKBYTE2, power_index);
1101                         break;
1102                 case DESC92C_RATEMCS7:
1103                         rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS07_MCS04,
1104                                                MASKBYTE3, power_index);
1105                         break;
1106
1107                 case DESC92C_RATEMCS8:
1108                         rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS11_MCS08,
1109                                                MASKBYTE0, power_index);
1110                         break;
1111                 case DESC92C_RATEMCS9:
1112                         rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS11_MCS08,
1113                                                MASKBYTE1, power_index);
1114                         break;
1115                 case DESC92C_RATEMCS10:
1116                         rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS11_MCS08,
1117                                                MASKBYTE2, power_index);
1118                         break;
1119                 case DESC92C_RATEMCS11:
1120                         rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS11_MCS08,
1121                                                MASKBYTE3, power_index);
1122                         break;
1123
1124                 default:
1125                         RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD, "Invalid Rate!!\n");
1126                         break;
1127                 }
1128         } else {
1129                 RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD, "Invalid RFPath!!\n");
1130         }
1131 }
1132
1133 void rtl8723be_phy_set_txpower_level(struct ieee80211_hw *hw, u8 channel)
1134 {
1135         struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
1136         u8 cck_rates[]  = {DESC92C_RATE1M, DESC92C_RATE2M,
1137                            DESC92C_RATE5_5M, DESC92C_RATE11M};
1138         u8 ofdm_rates[]  = {DESC92C_RATE6M, DESC92C_RATE9M,
1139                             DESC92C_RATE12M, DESC92C_RATE18M,
1140                             DESC92C_RATE24M, DESC92C_RATE36M,
1141                             DESC92C_RATE48M, DESC92C_RATE54M};
1142         u8 ht_rates_1t[]  = {DESC92C_RATEMCS0, DESC92C_RATEMCS1,
1143                              DESC92C_RATEMCS2, DESC92C_RATEMCS3,
1144                              DESC92C_RATEMCS4, DESC92C_RATEMCS5,
1145                              DESC92C_RATEMCS6, DESC92C_RATEMCS7};
1146         u8 i, size;
1147         u8 power_index;
1148
1149         if (!rtlefuse->txpwr_fromeprom)
1150                 return;
1151
1152         size = sizeof(cck_rates) / sizeof(u8);
1153         for (i = 0; i < size; i++) {
1154                 power_index = _rtl8723be_get_txpower_index(hw, RF90_PATH_A,
1155                                         cck_rates[i],
1156                                         rtl_priv(hw)->phy.current_chan_bw,
1157                                         channel);
1158                 _rtl8723be_phy_set_txpower_index(hw, power_index, RF90_PATH_A,
1159                                                  cck_rates[i]);
1160         }
1161         size = sizeof(ofdm_rates) / sizeof(u8);
1162         for (i = 0; i < size; i++) {
1163                 power_index = _rtl8723be_get_txpower_index(hw, RF90_PATH_A,
1164                                         ofdm_rates[i],
1165                                         rtl_priv(hw)->phy.current_chan_bw,
1166                                         channel);
1167                 _rtl8723be_phy_set_txpower_index(hw, power_index, RF90_PATH_A,
1168                                                  ofdm_rates[i]);
1169         }
1170         size = sizeof(ht_rates_1t) / sizeof(u8);
1171         for (i = 0; i < size; i++) {
1172                 power_index = _rtl8723be_get_txpower_index(hw, RF90_PATH_A,
1173                                         ht_rates_1t[i],
1174                                         rtl_priv(hw)->phy.current_chan_bw,
1175                                         channel);
1176                 _rtl8723be_phy_set_txpower_index(hw, power_index, RF90_PATH_A,
1177                                                  ht_rates_1t[i]);
1178         }
1179 }
1180
1181 void rtl8723be_phy_scan_operation_backup(struct ieee80211_hw *hw, u8 operation)
1182 {
1183         struct rtl_priv *rtlpriv = rtl_priv(hw);
1184         struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
1185         enum io_type iotype;
1186
1187         if (!is_hal_stop(rtlhal)) {
1188                 switch (operation) {
1189                 case SCAN_OPT_BACKUP_BAND0:
1190                         iotype = IO_CMD_PAUSE_BAND0_DM_BY_SCAN;
1191                         rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_IO_CMD,
1192                                                       (u8 *)&iotype);
1193
1194                         break;
1195                 case SCAN_OPT_RESTORE:
1196                         iotype = IO_CMD_RESUME_DM_BY_SCAN;
1197                         rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_IO_CMD,
1198                                                       (u8 *)&iotype);
1199                         break;
1200                 default:
1201                         pr_err("Unknown Scan Backup operation.\n");
1202                         break;
1203                 }
1204         }
1205 }
1206
1207 void rtl8723be_phy_set_bw_mode_callback(struct ieee80211_hw *hw)
1208 {
1209         struct rtl_priv *rtlpriv = rtl_priv(hw);
1210         struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
1211         struct rtl_phy *rtlphy = &rtlpriv->phy;
1212         struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
1213         u8 reg_bw_opmode;
1214         u8 reg_prsr_rsc;
1215
1216         RT_TRACE(rtlpriv, COMP_SCAN, DBG_TRACE,
1217                  "Switch to %s bandwidth\n",
1218                   rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20 ?
1219                   "20MHz" : "40MHz");
1220
1221         if (is_hal_stop(rtlhal)) {
1222                 rtlphy->set_bwmode_inprogress = false;
1223                 return;
1224         }
1225
1226         reg_bw_opmode = rtl_read_byte(rtlpriv, REG_BWOPMODE);
1227         reg_prsr_rsc = rtl_read_byte(rtlpriv, REG_RRSR + 2);
1228
1229         switch (rtlphy->current_chan_bw) {
1230         case HT_CHANNEL_WIDTH_20:
1231                 reg_bw_opmode |= BW_OPMODE_20MHZ;
1232                 rtl_write_byte(rtlpriv, REG_BWOPMODE, reg_bw_opmode);
1233                 break;
1234         case HT_CHANNEL_WIDTH_20_40:
1235                 reg_bw_opmode &= ~BW_OPMODE_20MHZ;
1236                 rtl_write_byte(rtlpriv, REG_BWOPMODE, reg_bw_opmode);
1237                 reg_prsr_rsc = (reg_prsr_rsc & 0x90) |
1238                                (mac->cur_40_prime_sc << 5);
1239                 rtl_write_byte(rtlpriv, REG_RRSR + 2, reg_prsr_rsc);
1240                 break;
1241         default:
1242                 pr_err("unknown bandwidth: %#X\n",
1243                        rtlphy->current_chan_bw);
1244                 break;
1245         }
1246
1247         switch (rtlphy->current_chan_bw) {
1248         case HT_CHANNEL_WIDTH_20:
1249                 rtl_set_bbreg(hw, RFPGA0_RFMOD, BRFMOD, 0x0);
1250                 rtl_set_bbreg(hw, RFPGA1_RFMOD, BRFMOD, 0x0);
1251         /*      rtl_set_bbreg(hw, RFPGA0_ANALOGPARAMETER2, BIT(10), 1);*/
1252                 break;
1253         case HT_CHANNEL_WIDTH_20_40:
1254                 rtl_set_bbreg(hw, RFPGA0_RFMOD, BRFMOD, 0x1);
1255                 rtl_set_bbreg(hw, RFPGA1_RFMOD, BRFMOD, 0x1);
1256
1257                 rtl_set_bbreg(hw, RCCK0_SYSTEM, BCCK_SIDEBAND,
1258                               (mac->cur_40_prime_sc >> 1));
1259                 rtl_set_bbreg(hw, ROFDM1_LSTF, 0xC00, mac->cur_40_prime_sc);
1260                 /*rtl_set_bbreg(hw, RFPGA0_ANALOGPARAMETER2, BIT(10), 0);*/
1261
1262                 rtl_set_bbreg(hw, 0x818, (BIT(26) | BIT(27)),
1263                               (mac->cur_40_prime_sc ==
1264                                HAL_PRIME_CHNL_OFFSET_LOWER) ? 2 : 1);
1265                 break;
1266         default:
1267                 pr_err("unknown bandwidth: %#X\n",
1268                        rtlphy->current_chan_bw);
1269                 break;
1270         }
1271         rtl8723be_phy_rf6052_set_bandwidth(hw, rtlphy->current_chan_bw);
1272         rtlphy->set_bwmode_inprogress = false;
1273         RT_TRACE(rtlpriv, COMP_SCAN, DBG_LOUD, "\n");
1274 }
1275
1276 void rtl8723be_phy_set_bw_mode(struct ieee80211_hw *hw,
1277                             enum nl80211_channel_type ch_type)
1278 {
1279         struct rtl_priv *rtlpriv = rtl_priv(hw);
1280         struct rtl_phy *rtlphy = &rtlpriv->phy;
1281         struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
1282         u8 tmp_bw = rtlphy->current_chan_bw;
1283
1284         if (rtlphy->set_bwmode_inprogress)
1285                 return;
1286         rtlphy->set_bwmode_inprogress = true;
1287         if ((!is_hal_stop(rtlhal)) && !(RT_CANNOT_IO(hw))) {
1288                 rtl8723be_phy_set_bw_mode_callback(hw);
1289         } else {
1290                 RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
1291                          "false driver sleep or unload\n");
1292                 rtlphy->set_bwmode_inprogress = false;
1293                 rtlphy->current_chan_bw = tmp_bw;
1294         }
1295 }
1296
1297 void rtl8723be_phy_sw_chnl_callback(struct ieee80211_hw *hw)
1298 {
1299         struct rtl_priv *rtlpriv = rtl_priv(hw);
1300         struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
1301         struct rtl_phy *rtlphy = &rtlpriv->phy;
1302         u32 delay = 0;
1303
1304         RT_TRACE(rtlpriv, COMP_SCAN, DBG_TRACE,
1305                  "switch to channel%d\n", rtlphy->current_channel);
1306         if (is_hal_stop(rtlhal))
1307                 return;
1308         do {
1309                 if (!rtlphy->sw_chnl_inprogress)
1310                         break;
1311                 if (!_rtl8723be_phy_sw_chnl_step_by_step(hw,
1312                                                          rtlphy->current_channel,
1313                                                          &rtlphy->sw_chnl_stage,
1314                                                          &rtlphy->sw_chnl_step,
1315                                                          &delay)) {
1316                         if (delay > 0)
1317                                 mdelay(delay);
1318                         else
1319                                 continue;
1320                 } else {
1321                         rtlphy->sw_chnl_inprogress = false;
1322                 }
1323                 break;
1324         } while (true);
1325         RT_TRACE(rtlpriv, COMP_SCAN, DBG_TRACE, "\n");
1326 }
1327
1328 u8 rtl8723be_phy_sw_chnl(struct ieee80211_hw *hw)
1329 {
1330         struct rtl_priv *rtlpriv = rtl_priv(hw);
1331         struct rtl_phy *rtlphy = &rtlpriv->phy;
1332         struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
1333
1334         if (rtlphy->sw_chnl_inprogress)
1335                 return 0;
1336         if (rtlphy->set_bwmode_inprogress)
1337                 return 0;
1338         WARN_ONCE((rtlphy->current_channel > 14),
1339                   "rtl8723be: WIRELESS_MODE_G but channel>14");
1340         rtlphy->sw_chnl_inprogress = true;
1341         rtlphy->sw_chnl_stage = 0;
1342         rtlphy->sw_chnl_step = 0;
1343         if (!(is_hal_stop(rtlhal)) && !(RT_CANNOT_IO(hw))) {
1344                 rtl8723be_phy_sw_chnl_callback(hw);
1345                 RT_TRACE(rtlpriv, COMP_CHAN, DBG_LOUD,
1346                          "sw_chnl_inprogress false schedule workitem current channel %d\n",
1347                          rtlphy->current_channel);
1348                 rtlphy->sw_chnl_inprogress = false;
1349         } else {
1350                 RT_TRACE(rtlpriv, COMP_CHAN, DBG_LOUD,
1351                          "sw_chnl_inprogress false driver sleep or unload\n");
1352                 rtlphy->sw_chnl_inprogress = false;
1353         }
1354         return 1;
1355 }
1356
1357 static bool _rtl8723be_phy_sw_chnl_step_by_step(struct ieee80211_hw *hw,
1358                                                 u8 channel, u8 *stage,
1359                                                 u8 *step, u32 *delay)
1360 {
1361         struct rtl_priv *rtlpriv = rtl_priv(hw);
1362         struct rtl_phy *rtlphy = &rtlpriv->phy;
1363         struct swchnlcmd precommoncmd[MAX_PRECMD_CNT];
1364         u32 precommoncmdcnt;
1365         struct swchnlcmd postcommoncmd[MAX_POSTCMD_CNT];
1366         u32 postcommoncmdcnt;
1367         struct swchnlcmd rfdependcmd[MAX_RFDEPENDCMD_CNT];
1368         u32 rfdependcmdcnt;
1369         struct swchnlcmd *currentcmd = NULL;
1370         u8 rfpath;
1371         u8 num_total_rfpath = rtlphy->num_total_rfpath;
1372
1373         precommoncmdcnt = 0;
1374         rtl8723_phy_set_sw_chnl_cmdarray(precommoncmd, precommoncmdcnt++,
1375                                          MAX_PRECMD_CNT,
1376                                          CMDID_SET_TXPOWEROWER_LEVEL,
1377                                          0, 0, 0);
1378         rtl8723_phy_set_sw_chnl_cmdarray(precommoncmd, precommoncmdcnt++,
1379                                          MAX_PRECMD_CNT, CMDID_END, 0, 0, 0);
1380
1381         postcommoncmdcnt = 0;
1382
1383         rtl8723_phy_set_sw_chnl_cmdarray(postcommoncmd, postcommoncmdcnt++,
1384                                          MAX_POSTCMD_CNT, CMDID_END,
1385                                             0, 0, 0);
1386
1387         rfdependcmdcnt = 0;
1388
1389         WARN_ONCE((channel < 1 || channel > 14),
1390                   "rtl8723be: illegal channel for Zebra: %d\n", channel);
1391
1392         rtl8723_phy_set_sw_chnl_cmdarray(rfdependcmd, rfdependcmdcnt++,
1393                                          MAX_RFDEPENDCMD_CNT,
1394                                          CMDID_RF_WRITEREG,
1395                                          RF_CHNLBW, channel, 10);
1396
1397         rtl8723_phy_set_sw_chnl_cmdarray(rfdependcmd, rfdependcmdcnt++,
1398                                          MAX_RFDEPENDCMD_CNT,
1399                                             CMDID_END, 0, 0, 0);
1400
1401         do {
1402                 switch (*stage) {
1403                 case 0:
1404                         currentcmd = &precommoncmd[*step];
1405                         break;
1406                 case 1:
1407                         currentcmd = &rfdependcmd[*step];
1408                         break;
1409                 case 2:
1410                         currentcmd = &postcommoncmd[*step];
1411                         break;
1412                 default:
1413                         pr_err("Invalid 'stage' = %d, Check it!\n",
1414                                *stage);
1415                         return true;
1416                 }
1417
1418                 if (currentcmd->cmdid == CMDID_END) {
1419                         if ((*stage) == 2) {
1420                                 return true;
1421                         } else {
1422                                 (*stage)++;
1423                                 (*step) = 0;
1424                                 continue;
1425                         }
1426                 }
1427
1428                 switch (currentcmd->cmdid) {
1429                 case CMDID_SET_TXPOWEROWER_LEVEL:
1430                         rtl8723be_phy_set_txpower_level(hw, channel);
1431                         break;
1432                 case CMDID_WRITEPORT_ULONG:
1433                         rtl_write_dword(rtlpriv, currentcmd->para1,
1434                                         currentcmd->para2);
1435                         break;
1436                 case CMDID_WRITEPORT_USHORT:
1437                         rtl_write_word(rtlpriv, currentcmd->para1,
1438                                        (u16)currentcmd->para2);
1439                         break;
1440                 case CMDID_WRITEPORT_UCHAR:
1441                         rtl_write_byte(rtlpriv, currentcmd->para1,
1442                                        (u8)currentcmd->para2);
1443                         break;
1444                 case CMDID_RF_WRITEREG:
1445                         for (rfpath = 0; rfpath < num_total_rfpath; rfpath++) {
1446                                 rtlphy->rfreg_chnlval[rfpath] =
1447                                     ((rtlphy->rfreg_chnlval[rfpath] &
1448                                       0xfffffc00) | currentcmd->para2);
1449
1450                                 rtl_set_rfreg(hw, (enum radio_path)rfpath,
1451                                               currentcmd->para1,
1452                                               RFREG_OFFSET_MASK,
1453                                               rtlphy->rfreg_chnlval[rfpath]);
1454                         }
1455                         break;
1456                 default:
1457                         RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
1458                                  "switch case %#x not processed\n",
1459                                  currentcmd->cmdid);
1460                         break;
1461                 }
1462
1463                 break;
1464         } while (true);
1465
1466         (*delay) = currentcmd->msdelay;
1467         (*step)++;
1468         return false;
1469 }
1470
1471 static u8 _rtl8723be_phy_path_a_iqk(struct ieee80211_hw *hw)
1472 {
1473         u32 reg_eac, reg_e94, reg_e9c, tmp;
1474         u8 result = 0x00;
1475
1476         /* leave IQK mode */
1477         rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
1478         /* switch to path A */
1479         rtl_set_bbreg(hw, 0x948, MASKDWORD, 0x00000000);
1480         /* enable path A PA in TXIQK mode */
1481         rtl_set_rfreg(hw, RF90_PATH_A, RF_WE_LUT, RFREG_OFFSET_MASK, 0x800a0);
1482         rtl_set_rfreg(hw, RF90_PATH_A, RF_RCK_OS, RFREG_OFFSET_MASK, 0x20000);
1483         rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G1, RFREG_OFFSET_MASK, 0x0003f);
1484         rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G2, RFREG_OFFSET_MASK, 0xc7f87);
1485
1486         /* 1. TX IQK */
1487         /* path-A IQK setting */
1488         /* IQK setting */
1489         rtl_set_bbreg(hw, RTX_IQK, MASKDWORD, 0x01007c00);
1490         rtl_set_bbreg(hw, RRX_IQK, MASKDWORD, 0x01004800);
1491         /* path-A IQK setting */
1492         rtl_set_bbreg(hw, RTX_IQK_TONE_A, MASKDWORD, 0x18008c1c);
1493         rtl_set_bbreg(hw, RRX_IQK_TONE_A, MASKDWORD, 0x38008c1c);
1494         rtl_set_bbreg(hw, RTX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
1495         rtl_set_bbreg(hw, RRX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
1496
1497         rtl_set_bbreg(hw, RTX_IQK_PI_A, MASKDWORD, 0x821403ea);
1498         rtl_set_bbreg(hw, RRX_IQK_PI_A, MASKDWORD, 0x28160000);
1499         rtl_set_bbreg(hw, RTX_IQK_PI_B, MASKDWORD, 0x82110000);
1500         rtl_set_bbreg(hw, RRX_IQK_PI_B, MASKDWORD, 0x28110000);
1501         /* LO calibration setting */
1502         rtl_set_bbreg(hw, RIQK_AGC_RSP, MASKDWORD, 0x00462911);
1503         /* enter IQK mode */
1504         rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
1505
1506         /* One shot, path A LOK & IQK */
1507         rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf9000000);
1508         rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf8000000);
1509
1510         mdelay(IQK_DELAY_TIME);
1511
1512         /* leave IQK mode */
1513         rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
1514
1515         /* Check failed */
1516         reg_eac = rtl_get_bbreg(hw, 0xeac, MASKDWORD);
1517         reg_e94 = rtl_get_bbreg(hw, 0xe94, MASKDWORD);
1518         reg_e9c = rtl_get_bbreg(hw, 0xe9c, MASKDWORD);
1519
1520         if (!(reg_eac & BIT(28)) &&
1521             (((reg_e94 & 0x03FF0000) >> 16) != 0x142) &&
1522             (((reg_e9c & 0x03FF0000) >> 16) != 0x42))
1523                 result |= 0x01;
1524         else /* if Tx not OK, ignore Rx */
1525                 return result;
1526
1527         /* Allen 20131125 */
1528         tmp = (reg_e9c & 0x03FF0000) >> 16;
1529         if ((tmp & 0x200) > 0)
1530                 tmp = 0x400 - tmp;
1531
1532         if (!(reg_eac & BIT(28)) &&
1533             (((reg_e94 & 0x03FF0000) >> 16) < 0x110) &&
1534             (((reg_e94 & 0x03FF0000) >> 16) > 0xf0) &&
1535             (tmp < 0xf))
1536                 result |= 0x01;
1537         else /* if Tx not OK, ignore Rx */
1538                 return result;
1539
1540         return result;
1541 }
1542
1543 /* bit0 = 1 => Tx OK, bit1 = 1 => Rx OK */
1544 static u8 _rtl8723be_phy_path_a_rx_iqk(struct ieee80211_hw *hw)
1545 {
1546         u32 reg_eac, reg_e94, reg_e9c, reg_ea4, u32tmp, tmp;
1547         u8 result = 0x00;
1548
1549         /* leave IQK mode */
1550         rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
1551
1552         /* switch to path A */
1553         rtl_set_bbreg(hw, 0x948, MASKDWORD, 0x00000000);
1554
1555         /* 1 Get TXIMR setting */
1556         /* modify RXIQK mode table */
1557         rtl_set_rfreg(hw, RF90_PATH_A, RF_WE_LUT, 0x80000, 0x1);
1558         rtl_set_rfreg(hw, RF90_PATH_A, RF_RCK_OS, RFREG_OFFSET_MASK, 0x30000);
1559         rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G1, RFREG_OFFSET_MASK, 0x0001f);
1560         /* LNA2 off, PA on for Dcut */
1561         rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G2, RFREG_OFFSET_MASK, 0xf7fb7);
1562         rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
1563
1564         /* IQK setting */
1565         rtl_set_bbreg(hw, RTX_IQK, MASKDWORD, 0x01007c00);
1566         rtl_set_bbreg(hw, RRX_IQK, MASKDWORD, 0x01004800);
1567
1568         /* path-A IQK setting */
1569         rtl_set_bbreg(hw, RTX_IQK_TONE_A, MASKDWORD, 0x18008c1c);
1570         rtl_set_bbreg(hw, RRX_IQK_TONE_A, MASKDWORD, 0x38008c1c);
1571         rtl_set_bbreg(hw, RTX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
1572         rtl_set_bbreg(hw, RRX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
1573
1574         rtl_set_bbreg(hw, RTX_IQK_PI_A, MASKDWORD, 0x82160ff0);
1575         rtl_set_bbreg(hw, RRX_IQK_PI_A, MASKDWORD, 0x28110000);
1576         rtl_set_bbreg(hw, RTX_IQK_PI_B, MASKDWORD, 0x82110000);
1577         rtl_set_bbreg(hw, RRX_IQK_PI_B, MASKDWORD, 0x28110000);
1578
1579         /* LO calibration setting */
1580         rtl_set_bbreg(hw, RIQK_AGC_RSP, MASKDWORD, 0x0046a911);
1581
1582         /* enter IQK mode */
1583         rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
1584
1585         /* One shot, path A LOK & IQK */
1586         rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf9000000);
1587         rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf8000000);
1588
1589         mdelay(IQK_DELAY_TIME);
1590
1591         /* leave IQK mode */
1592         rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
1593
1594         /* Check failed */
1595         reg_eac = rtl_get_bbreg(hw, RRX_POWER_AFTER_IQK_A_2, MASKDWORD);
1596         reg_e94 = rtl_get_bbreg(hw, RTX_POWER_BEFORE_IQK_A, MASKDWORD);
1597         reg_e9c = rtl_get_bbreg(hw, RTX_POWER_AFTER_IQK_A, MASKDWORD);
1598
1599         if (!(reg_eac & BIT(28)) &&
1600             (((reg_e94 & 0x03FF0000) >> 16) != 0x142) &&
1601             (((reg_e9c & 0x03FF0000) >> 16) != 0x42))
1602                 result |= 0x01;
1603         else /* if Tx not OK, ignore Rx */
1604                 return result;
1605
1606         /* Allen 20131125 */
1607         tmp = (reg_e9c & 0x03FF0000) >> 16;
1608         if ((tmp & 0x200) > 0)
1609                 tmp = 0x400 - tmp;
1610
1611         if (!(reg_eac & BIT(28)) &&
1612             (((reg_e94 & 0x03FF0000) >> 16) < 0x110) &&
1613             (((reg_e94 & 0x03FF0000) >> 16) > 0xf0) &&
1614             (tmp < 0xf))
1615                 result |= 0x01;
1616         else /* if Tx not OK, ignore Rx */
1617                 return result;
1618
1619         u32tmp = 0x80007C00 | (reg_e94 & 0x3FF0000) |
1620                  ((reg_e9c & 0x3FF0000) >> 16);
1621         rtl_set_bbreg(hw, RTX_IQK, MASKDWORD, u32tmp);
1622
1623         /* 1 RX IQK */
1624         /* modify RXIQK mode table */
1625         rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
1626         rtl_set_rfreg(hw, RF90_PATH_A, RF_WE_LUT, 0x80000, 0x1);
1627         rtl_set_rfreg(hw, RF90_PATH_A, RF_RCK_OS, RFREG_OFFSET_MASK, 0x30000);
1628         rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G1, RFREG_OFFSET_MASK, 0x0001f);
1629         /* LAN2 on, PA off for Dcut */
1630         rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G2, RFREG_OFFSET_MASK, 0xf7d77);
1631
1632         /* PA, PAD setting */
1633         rtl_set_rfreg(hw, RF90_PATH_A, 0xdf, RFREG_OFFSET_MASK, 0xf80);
1634         rtl_set_rfreg(hw, RF90_PATH_A, 0x55, RFREG_OFFSET_MASK, 0x4021f);
1635
1636         /* IQK setting */
1637         rtl_set_bbreg(hw, RRX_IQK, MASKDWORD, 0x01004800);
1638
1639         /* path-A IQK setting */
1640         rtl_set_bbreg(hw, RTX_IQK_TONE_A, MASKDWORD, 0x38008c1c);
1641         rtl_set_bbreg(hw, RRX_IQK_TONE_A, MASKDWORD, 0x18008c1c);
1642         rtl_set_bbreg(hw, RTX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
1643         rtl_set_bbreg(hw, RRX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
1644
1645         rtl_set_bbreg(hw, RTX_IQK_PI_A, MASKDWORD, 0x82110000);
1646         rtl_set_bbreg(hw, RRX_IQK_PI_A, MASKDWORD, 0x2816001f);
1647         rtl_set_bbreg(hw, RTX_IQK_PI_B, MASKDWORD, 0x82110000);
1648         rtl_set_bbreg(hw, RRX_IQK_PI_B, MASKDWORD, 0x28110000);
1649
1650         /* LO calibration setting */
1651         rtl_set_bbreg(hw, RIQK_AGC_RSP, MASKDWORD, 0x0046a8d1);
1652
1653         /* enter IQK mode */
1654         rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
1655
1656         /* One shot, path A LOK & IQK */
1657         rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf9000000);
1658         rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf8000000);
1659
1660         mdelay(IQK_DELAY_TIME);
1661
1662         /* leave IQK mode */
1663         rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
1664
1665         /* Check failed */
1666         reg_eac = rtl_get_bbreg(hw, RRX_POWER_AFTER_IQK_A_2, MASKDWORD);
1667         reg_ea4 = rtl_get_bbreg(hw, RRX_POWER_BEFORE_IQK_A_2, MASKDWORD);
1668
1669         /* leave IQK mode */
1670         rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
1671         rtl_set_rfreg(hw, RF90_PATH_A, 0xdf, RFREG_OFFSET_MASK, 0x780);
1672
1673         /* Allen 20131125 */
1674         tmp = (reg_eac & 0x03FF0000) >> 16;
1675         if ((tmp & 0x200) > 0)
1676                 tmp = 0x400 - tmp;
1677         /* if Tx is OK, check whether Rx is OK */
1678         if (!(reg_eac & BIT(27)) &&
1679             (((reg_ea4 & 0x03FF0000) >> 16) != 0x132) &&
1680             (((reg_eac & 0x03FF0000) >> 16) != 0x36))
1681                 result |= 0x02;
1682         else if (!(reg_eac & BIT(27)) &&
1683                  (((reg_ea4 & 0x03FF0000) >> 16) < 0x110) &&
1684                  (((reg_ea4 & 0x03FF0000) >> 16) > 0xf0) &&
1685                  (tmp < 0xf))
1686                 result |= 0x02;
1687
1688         return result;
1689 }
1690
1691 static u8 _rtl8723be_phy_path_b_iqk(struct ieee80211_hw *hw)
1692 {
1693         u32 reg_eac, reg_e94, reg_e9c, tmp;
1694         u8 result = 0x00;
1695
1696         /* leave IQK mode */
1697         rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
1698         /* switch to path B */
1699         rtl_set_bbreg(hw, 0x948, MASKDWORD, 0x00000280);
1700
1701         /* enable path B PA in TXIQK mode */
1702         rtl_set_rfreg(hw, RF90_PATH_A, 0xed, RFREG_OFFSET_MASK, 0x00020);
1703         rtl_set_rfreg(hw, RF90_PATH_A, 0x43, RFREG_OFFSET_MASK, 0x40fc1);
1704
1705         /* 1 Tx IQK */
1706         /* IQK setting */
1707         rtl_set_bbreg(hw, RTX_IQK, MASKDWORD, 0x01007c00);
1708         rtl_set_bbreg(hw, RRX_IQK, MASKDWORD, 0x01004800);
1709         /* path-A IQK setting */
1710         rtl_set_bbreg(hw, RTX_IQK_TONE_A, MASKDWORD, 0x18008c1c);
1711         rtl_set_bbreg(hw, RRX_IQK_TONE_A, MASKDWORD, 0x38008c1c);
1712         rtl_set_bbreg(hw, RTX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
1713         rtl_set_bbreg(hw, RRX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
1714
1715         rtl_set_bbreg(hw, RTX_IQK_PI_A, MASKDWORD, 0x821403ea);
1716         rtl_set_bbreg(hw, RRX_IQK_PI_A, MASKDWORD, 0x28110000);
1717         rtl_set_bbreg(hw, RTX_IQK_PI_B, MASKDWORD, 0x82110000);
1718         rtl_set_bbreg(hw, RRX_IQK_PI_B, MASKDWORD, 0x28110000);
1719
1720         /* LO calibration setting */
1721         rtl_set_bbreg(hw, RIQK_AGC_RSP, MASKDWORD, 0x00462911);
1722
1723         /* enter IQK mode */
1724         rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
1725
1726         /* One shot, path B LOK & IQK */
1727         rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf9000000);
1728         rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf8000000);
1729
1730         mdelay(IQK_DELAY_TIME);
1731
1732         /* leave IQK mode */
1733         rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
1734
1735         /* Check failed */
1736         reg_eac = rtl_get_bbreg(hw, RRX_POWER_AFTER_IQK_A_2, MASKDWORD);
1737         reg_e94 = rtl_get_bbreg(hw, RTX_POWER_BEFORE_IQK_A, MASKDWORD);
1738         reg_e9c = rtl_get_bbreg(hw, RTX_POWER_AFTER_IQK_A, MASKDWORD);
1739
1740         if (!(reg_eac & BIT(28)) &&
1741             (((reg_e94 & 0x03FF0000) >> 16) != 0x142) &&
1742             (((reg_e9c & 0x03FF0000) >> 16) != 0x42))
1743                 result |= 0x01;
1744         else
1745                 return result;
1746
1747         /* Allen 20131125 */
1748         tmp = (reg_e9c & 0x03FF0000) >> 16;
1749         if ((tmp & 0x200) > 0)
1750                 tmp = 0x400 - tmp;
1751
1752         if (!(reg_eac & BIT(28)) &&
1753             (((reg_e94 & 0x03FF0000) >> 16) < 0x110) &&
1754             (((reg_e94 & 0x03FF0000) >> 16) > 0xf0) &&
1755             (tmp < 0xf))
1756                 result |= 0x01;
1757         else
1758                 return result;
1759
1760         return result;
1761 }
1762
1763 /* bit0 = 1 => Tx OK, bit1 = 1 => Rx OK */
1764 static u8 _rtl8723be_phy_path_b_rx_iqk(struct ieee80211_hw *hw)
1765 {
1766         u32 reg_e94, reg_e9c, reg_ea4, reg_eac, u32tmp, tmp;
1767         u8 result = 0x00;
1768
1769         /* leave IQK mode */
1770         rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
1771         /* switch to path B */
1772         rtl_set_bbreg(hw, 0x948, MASKDWORD, 0x00000280);
1773
1774         /* 1 Get TXIMR setting */
1775         /* modify RXIQK mode table */
1776         rtl_set_rfreg(hw, RF90_PATH_A, RF_WE_LUT, RFREG_OFFSET_MASK, 0x800a0);
1777         rtl_set_rfreg(hw, RF90_PATH_A, RF_RCK_OS, RFREG_OFFSET_MASK, 0x30000);
1778         rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G1, RFREG_OFFSET_MASK, 0x0001f);
1779         rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G2, RFREG_OFFSET_MASK, 0xf7ff7);
1780
1781         /* open PA S1 & SMIXER */
1782         rtl_set_rfreg(hw, RF90_PATH_A, 0xed, RFREG_OFFSET_MASK, 0x00020);
1783         rtl_set_rfreg(hw, RF90_PATH_A, 0x43, RFREG_OFFSET_MASK, 0x60fed);
1784
1785         /* IQK setting */
1786         rtl_set_bbreg(hw, RTX_IQK, MASKDWORD, 0x01007c00);
1787         rtl_set_bbreg(hw, RRX_IQK, MASKDWORD, 0x01004800);
1788
1789         /* path-B IQK setting */
1790         rtl_set_bbreg(hw, RTX_IQK_TONE_A, MASKDWORD, 0x18008c1c);
1791         rtl_set_bbreg(hw, RRX_IQK_TONE_A, MASKDWORD, 0x38008c1c);
1792         rtl_set_bbreg(hw, RTX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
1793         rtl_set_bbreg(hw, RRX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
1794
1795         rtl_set_bbreg(hw, RTX_IQK_PI_A, MASKDWORD, 0x82160ff0);
1796         rtl_set_bbreg(hw, RRX_IQK_PI_A, MASKDWORD, 0x28110000);
1797         rtl_set_bbreg(hw, RTX_IQK_PI_B, MASKDWORD, 0x82110000);
1798         rtl_set_bbreg(hw, RRX_IQK_PI_B, MASKDWORD, 0x28110000);
1799
1800         /* LO calibration setting */
1801         rtl_set_bbreg(hw, RIQK_AGC_RSP, MASKDWORD, 0x0046a911);
1802         /* enter IQK mode */
1803         rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
1804
1805         /* One shot, path B TXIQK @ RXIQK */
1806         rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf9000000);
1807         rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf8000000);
1808
1809         mdelay(IQK_DELAY_TIME);
1810
1811         /* leave IQK mode */
1812         rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
1813         /* Check failed */
1814         reg_eac = rtl_get_bbreg(hw, RRX_POWER_AFTER_IQK_A_2, MASKDWORD);
1815         reg_e94 = rtl_get_bbreg(hw, RTX_POWER_BEFORE_IQK_A, MASKDWORD);
1816         reg_e9c = rtl_get_bbreg(hw, RTX_POWER_AFTER_IQK_A, MASKDWORD);
1817
1818         if (!(reg_eac & BIT(28)) &&
1819             (((reg_e94 & 0x03FF0000) >> 16) != 0x142) &&
1820             (((reg_e9c & 0x03FF0000) >> 16) != 0x42))
1821                 result |= 0x01;
1822         else    /* if Tx not OK, ignore Rx */
1823                 return result;
1824
1825         /* Allen 20131125 */
1826         tmp = (reg_e9c & 0x03FF0000) >> 16;
1827         if ((tmp & 0x200) > 0)
1828                 tmp = 0x400 - tmp;
1829
1830         if (!(reg_eac & BIT(28)) &&
1831             (((reg_e94 & 0x03FF0000) >> 16) < 0x110) &&
1832             (((reg_e94 & 0x03FF0000) >> 16) > 0xf0) &&
1833             (tmp < 0xf))
1834                 result |= 0x01;
1835         else
1836                 return result;
1837
1838         u32tmp = 0x80007C00 | (reg_e94 & 0x3FF0000)  |
1839                  ((reg_e9c & 0x3FF0000) >> 16);
1840         rtl_set_bbreg(hw, RTX_IQK, MASKDWORD, u32tmp);
1841
1842         /* 1 RX IQK */
1843
1844         /* <20121009, Kordan> RF Mode = 3 */
1845         rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
1846         rtl_set_rfreg(hw, RF90_PATH_A, RF_WE_LUT, 0x80000, 0x1);
1847         rtl_set_rfreg(hw, RF90_PATH_A, RF_RCK_OS, RFREG_OFFSET_MASK, 0x30000);
1848         rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G1, RFREG_OFFSET_MASK, 0x0001f);
1849         rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G2, RFREG_OFFSET_MASK, 0xf7d77);
1850         rtl_set_rfreg(hw, RF90_PATH_A, RF_WE_LUT, 0x80000, 0x0);
1851
1852         /* open PA S1 & close SMIXER */
1853         rtl_set_rfreg(hw, RF90_PATH_A, 0xed, RFREG_OFFSET_MASK, 0x00020);
1854         rtl_set_rfreg(hw, RF90_PATH_A, 0x43, RFREG_OFFSET_MASK, 0x60fbd);
1855
1856         /* IQK setting */
1857         rtl_set_bbreg(hw, RRX_IQK, MASKDWORD, 0x01004800);
1858
1859         /* path-B IQK setting */
1860         rtl_set_bbreg(hw, RTX_IQK_TONE_A, MASKDWORD, 0x38008c1c);
1861         rtl_set_bbreg(hw, RRX_IQK_TONE_A, MASKDWORD, 0x18008c1c);
1862         rtl_set_bbreg(hw, RTX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
1863         rtl_set_bbreg(hw, RRX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
1864
1865         rtl_set_bbreg(hw, RTX_IQK_PI_A, MASKDWORD, 0x82110000);
1866         rtl_set_bbreg(hw, RRX_IQK_PI_A, MASKDWORD, 0x2816001f);
1867         rtl_set_bbreg(hw, RTX_IQK_PI_B, MASKDWORD, 0x82110000);
1868         rtl_set_bbreg(hw, RRX_IQK_PI_B, MASKDWORD, 0x28110000);
1869
1870         /* LO calibration setting */
1871         rtl_set_bbreg(hw, RIQK_AGC_RSP, MASKDWORD, 0x0046a8d1);
1872         /* enter IQK mode */
1873         rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
1874
1875         /* One shot, path B LOK & IQK */
1876         rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf9000000);
1877         rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf8000000);
1878
1879         mdelay(IQK_DELAY_TIME);
1880
1881         /* leave IQK mode */
1882         rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
1883         /* Check failed */
1884         reg_eac = rtl_get_bbreg(hw, RRX_POWER_AFTER_IQK_A_2, MASKDWORD);
1885         reg_ea4 = rtl_get_bbreg(hw, RRX_POWER_BEFORE_IQK_A_2, MASKDWORD);
1886
1887         /* Allen 20131125 */
1888         tmp = (reg_eac & 0x03FF0000) >> 16;
1889         if ((tmp & 0x200) > 0)
1890                 tmp = 0x400 - tmp;
1891
1892         /* if Tx is OK, check whether Rx is OK */
1893         if (!(reg_eac & BIT(27)) &&
1894             (((reg_ea4 & 0x03FF0000) >> 16) != 0x132) &&
1895             (((reg_eac & 0x03FF0000) >> 16) != 0x36))
1896                 result |= 0x02;
1897         else if (!(reg_eac & BIT(27)) &&
1898                  (((reg_ea4 & 0x03FF0000) >> 16) < 0x110) &&
1899                  (((reg_ea4 & 0x03FF0000) >> 16) > 0xf0) &&
1900                  (tmp < 0xf))
1901                 result |= 0x02;
1902         else
1903                 return result;
1904
1905         return result;
1906 }
1907
1908 static void _rtl8723be_phy_path_b_fill_iqk_matrix(struct ieee80211_hw *hw,
1909                                                   bool b_iqk_ok,
1910                                                   long result[][8],
1911                                                   u8 final_candidate,
1912                                                   bool btxonly)
1913 {
1914         u32 oldval_1, x, tx1_a, reg;
1915         long y, tx1_c;
1916
1917         if (final_candidate == 0xFF) {
1918                 return;
1919         } else if (b_iqk_ok) {
1920                 oldval_1 = (rtl_get_bbreg(hw, ROFDM0_XBTXIQIMBALANCE,
1921                                           MASKDWORD) >> 22) & 0x3FF;
1922                 x = result[final_candidate][4];
1923                 if ((x & 0x00000200) != 0)
1924                         x = x | 0xFFFFFC00;
1925                 tx1_a = (x * oldval_1) >> 8;
1926                 rtl_set_bbreg(hw, ROFDM0_XBTXIQIMBALANCE, 0x3FF, tx1_a);
1927                 rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD, BIT(27),
1928                               ((x * oldval_1 >> 7) & 0x1));
1929                 y = result[final_candidate][5];
1930                 if ((y & 0x00000200) != 0)
1931                         y = y | 0xFFFFFC00;
1932                 tx1_c = (y * oldval_1) >> 8;
1933                 rtl_set_bbreg(hw, ROFDM0_XDTXAFE, 0xF0000000,
1934                               ((tx1_c & 0x3C0) >> 6));
1935                 rtl_set_bbreg(hw, ROFDM0_XBTXIQIMBALANCE, 0x003F0000,
1936                               (tx1_c & 0x3F));
1937                 rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD, BIT(25),
1938                               ((y * oldval_1 >> 7) & 0x1));
1939                 if (btxonly)
1940                         return;
1941                 reg = result[final_candidate][6];
1942                 rtl_set_bbreg(hw, ROFDM0_XBRXIQIMBALANCE, 0x3FF, reg);
1943                 reg = result[final_candidate][7] & 0x3F;
1944                 rtl_set_bbreg(hw, ROFDM0_XBRXIQIMBALANCE, 0xFC00, reg);
1945                 reg = (result[final_candidate][7] >> 6) & 0xF;
1946                 /* rtl_set_bbreg(hw, 0xca0, 0xF0000000, reg); */
1947         }
1948 }
1949
1950 static bool _rtl8723be_phy_simularity_compare(struct ieee80211_hw *hw,
1951                                               long result[][8], u8 c1, u8 c2)
1952 {
1953         u32 i, j, diff, simularity_bitmap, bound = 0;
1954
1955         u8 final_candidate[2] = {0xFF, 0xFF}; /* for path A and path B */
1956         bool bresult = true; /* is2t = true*/
1957         s32 tmp1 = 0, tmp2 = 0;
1958
1959         bound = 8;
1960
1961         simularity_bitmap = 0;
1962
1963         for (i = 0; i < bound; i++) {
1964                 if ((i == 1) || (i == 3) || (i == 5) || (i == 7)) {
1965                         if ((result[c1][i] & 0x00000200) != 0)
1966                                 tmp1 = result[c1][i] | 0xFFFFFC00;
1967                         else
1968                                 tmp1 = result[c1][i];
1969
1970                         if ((result[c2][i] & 0x00000200) != 0)
1971                                 tmp2 = result[c2][i] | 0xFFFFFC00;
1972                         else
1973                                 tmp2 = result[c2][i];
1974                 } else {
1975                         tmp1 = result[c1][i];
1976                         tmp2 = result[c2][i];
1977                 }
1978
1979                 diff = (tmp1 > tmp2) ? (tmp1 - tmp2) : (tmp2 - tmp1);
1980
1981                 if (diff > MAX_TOLERANCE) {
1982                         if ((i == 2 || i == 6) && !simularity_bitmap) {
1983                                 if (result[c1][i] + result[c1][i + 1] == 0)
1984                                         final_candidate[(i / 4)] = c2;
1985                                 else if (result[c2][i] + result[c2][i + 1] == 0)
1986                                         final_candidate[(i / 4)] = c1;
1987                                 else
1988                                         simularity_bitmap |= (1 << i);
1989                         } else
1990                                 simularity_bitmap |= (1 << i);
1991                 }
1992         }
1993
1994         if (simularity_bitmap == 0) {
1995                 for (i = 0; i < (bound / 4); i++) {
1996                         if (final_candidate[i] != 0xFF) {
1997                                 for (j = i * 4; j < (i + 1) * 4 - 2; j++)
1998                                         result[3][j] =
1999                                                 result[final_candidate[i]][j];
2000                                 bresult = false;
2001                         }
2002                 }
2003                 return bresult;
2004         } else {
2005                 if (!(simularity_bitmap & 0x03)) { /* path A TX OK */
2006                         for (i = 0; i < 2; i++)
2007                                 result[3][i] = result[c1][i];
2008                 }
2009                 if (!(simularity_bitmap & 0x0c)) { /* path A RX OK */
2010                         for (i = 2; i < 4; i++)
2011                                 result[3][i] = result[c1][i];
2012                 }
2013                 if (!(simularity_bitmap & 0x30)) { /* path B TX OK */
2014                         for (i = 4; i < 6; i++)
2015                                 result[3][i] = result[c1][i];
2016                 }
2017                 if (!(simularity_bitmap & 0xc0)) { /* path B RX OK */
2018                         for (i = 6; i < 8; i++)
2019                                 result[3][i] = result[c1][i];
2020                 }
2021                 return false;
2022         }
2023 }
2024
2025 static void _rtl8723be_phy_iq_calibrate(struct ieee80211_hw *hw,
2026                                         long result[][8], u8 t, bool is2t)
2027 {
2028         struct rtl_priv *rtlpriv = rtl_priv(hw);
2029         struct rtl_phy *rtlphy = &rtlpriv->phy;
2030         u32 i;
2031         u8 patha_ok, pathb_ok;
2032         u32 adda_reg[IQK_ADDA_REG_NUM] = {
2033                 0x85c, 0xe6c, 0xe70, 0xe74,
2034                 0xe78, 0xe7c, 0xe80, 0xe84,
2035                 0xe88, 0xe8c, 0xed0, 0xed4,
2036                 0xed8, 0xedc, 0xee0, 0xeec
2037         };
2038
2039         u32 iqk_mac_reg[IQK_MAC_REG_NUM] = {
2040                 0x522, 0x550, 0x551, 0x040
2041         };
2042         u32 iqk_bb_reg[IQK_BB_REG_NUM] = {
2043                 ROFDM0_TRXPATHENABLE, ROFDM0_TRMUXPAR,
2044                 RFPGA0_XCD_RFINTERFACESW, 0xb68, 0xb6c,
2045                 0x870, 0x860,
2046                 0x864, 0xa04
2047         };
2048         const u32 retrycount = 2;
2049
2050         u32 path_sel_bb;/* path_sel_rf */
2051
2052         u8 tmp_reg_c50, tmp_reg_c58;
2053
2054         tmp_reg_c50 = rtl_get_bbreg(hw, 0xc50, MASKBYTE0);
2055         tmp_reg_c58 = rtl_get_bbreg(hw, 0xc58, MASKBYTE0);
2056
2057         if (t == 0) {
2058                 rtl8723_save_adda_registers(hw, adda_reg,
2059                                             rtlphy->adda_backup, 16);
2060                 rtl8723_phy_save_mac_registers(hw, iqk_mac_reg,
2061                                                rtlphy->iqk_mac_backup);
2062                 rtl8723_save_adda_registers(hw, iqk_bb_reg,
2063                                             rtlphy->iqk_bb_backup,
2064                                             IQK_BB_REG_NUM);
2065         }
2066         rtl8723_phy_path_adda_on(hw, adda_reg, true, is2t);
2067         if (t == 0) {
2068                 rtlphy->rfpi_enable = (u8)rtl_get_bbreg(hw,
2069                                                 RFPGA0_XA_HSSIPARAMETER1,
2070                                                 BIT(8));
2071         }
2072
2073         path_sel_bb = rtl_get_bbreg(hw, 0x948, MASKDWORD);
2074
2075         rtl8723_phy_mac_setting_calibration(hw, iqk_mac_reg,
2076                                             rtlphy->iqk_mac_backup);
2077         /*BB Setting*/
2078         rtl_set_bbreg(hw, 0xa04, 0x0f000000, 0xf);
2079         rtl_set_bbreg(hw, 0xc04, MASKDWORD, 0x03a05600);
2080         rtl_set_bbreg(hw, 0xc08, MASKDWORD, 0x000800e4);
2081         rtl_set_bbreg(hw, 0x874, MASKDWORD, 0x22204000);
2082
2083         /* path A TX IQK */
2084         for (i = 0; i < retrycount; i++) {
2085                 patha_ok = _rtl8723be_phy_path_a_iqk(hw);
2086                 if (patha_ok == 0x01) {
2087                         RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
2088                                 "Path A Tx IQK Success!!\n");
2089                         result[t][0] = (rtl_get_bbreg(hw, 0xe94, MASKDWORD) &
2090                                         0x3FF0000) >> 16;
2091                         result[t][1] = (rtl_get_bbreg(hw, 0xe9c, MASKDWORD) &
2092                                         0x3FF0000) >> 16;
2093                         break;
2094                 } else {
2095                         RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
2096                                  "Path A Tx IQK Fail!!\n");
2097                 }
2098         }
2099         /* path A RX IQK */
2100         for (i = 0; i < retrycount; i++) {
2101                 patha_ok = _rtl8723be_phy_path_a_rx_iqk(hw);
2102                 if (patha_ok == 0x03) {
2103                         RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
2104                                  "Path A Rx IQK Success!!\n");
2105                         result[t][2] = (rtl_get_bbreg(hw, 0xea4, MASKDWORD) &
2106                                         0x3FF0000) >> 16;
2107                         result[t][3] = (rtl_get_bbreg(hw, 0xeac, MASKDWORD) &
2108                                         0x3FF0000) >> 16;
2109                         break;
2110                 }
2111                 RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
2112                          "Path A Rx IQK Fail!!\n");
2113         }
2114
2115         if (0x00 == patha_ok)
2116                 RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "Path A IQK Fail!!\n");
2117
2118         if (is2t) {
2119                 /* path B TX IQK */
2120                 for (i = 0; i < retrycount; i++) {
2121                         pathb_ok = _rtl8723be_phy_path_b_iqk(hw);
2122                         if (pathb_ok == 0x01) {
2123                                 RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
2124                                          "Path B Tx IQK Success!!\n");
2125                                 result[t][4] = (rtl_get_bbreg(hw, 0xe94,
2126                                                               MASKDWORD) &
2127                                                               0x3FF0000) >> 16;
2128                                 result[t][5] = (rtl_get_bbreg(hw, 0xe9c,
2129                                                               MASKDWORD) &
2130                                                               0x3FF0000) >> 16;
2131                                 break;
2132                         }
2133                         RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
2134                                  "Path B Tx IQK Fail!!\n");
2135                 }
2136                 /* path B RX IQK */
2137                 for (i = 0; i < retrycount; i++) {
2138                         pathb_ok = _rtl8723be_phy_path_b_rx_iqk(hw);
2139                         if (pathb_ok == 0x03) {
2140                                 RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
2141                                          "Path B Rx IQK Success!!\n");
2142                                 result[t][6] = (rtl_get_bbreg(hw, 0xea4,
2143                                                               MASKDWORD) &
2144                                                               0x3FF0000) >> 16;
2145                                 result[t][7] = (rtl_get_bbreg(hw, 0xeac,
2146                                                               MASKDWORD) &
2147                                                               0x3FF0000) >> 16;
2148                                 break;
2149                         }
2150                         RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
2151                                  "Path B Rx IQK Fail!!\n");
2152                 }
2153         }
2154
2155         /* Back to BB mode, load original value */
2156         rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0);
2157
2158         if (t != 0) {
2159                 rtl8723_phy_reload_adda_registers(hw, adda_reg,
2160                                                   rtlphy->adda_backup, 16);
2161                 rtl8723_phy_reload_mac_registers(hw, iqk_mac_reg,
2162                                                  rtlphy->iqk_mac_backup);
2163                 rtl8723_phy_reload_adda_registers(hw, iqk_bb_reg,
2164                                                   rtlphy->iqk_bb_backup,
2165                                                   IQK_BB_REG_NUM);
2166
2167                 rtl_set_bbreg(hw, 0x948, MASKDWORD, path_sel_bb);
2168                 /*rtl_set_rfreg(hw, RF90_PATH_B, 0xb0, 0xfffff, path_sel_rf);*/
2169
2170                 rtl_set_bbreg(hw, 0xc50, MASKBYTE0, 0x50);
2171                 rtl_set_bbreg(hw, 0xc50, MASKBYTE0, tmp_reg_c50);
2172                 if (is2t) {
2173                         rtl_set_bbreg(hw, 0xc58, MASKBYTE0, 0x50);
2174                         rtl_set_bbreg(hw, 0xc58, MASKBYTE0, tmp_reg_c58);
2175                 }
2176                 rtl_set_bbreg(hw, 0xe30, MASKDWORD, 0x01008c00);
2177                 rtl_set_bbreg(hw, 0xe34, MASKDWORD, 0x01008c00);
2178         }
2179         RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "8723be IQK Finish!!\n");
2180 }
2181
2182 static u8 _get_right_chnl_place_for_iqk(u8 chnl)
2183 {
2184         u8 channel_all[TARGET_CHNL_NUM_2G_5G] = {
2185                         1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12,
2186                         13, 14, 36, 38, 40, 42, 44, 46,
2187                         48, 50, 52, 54, 56, 58, 60, 62, 64,
2188                         100, 102, 104, 106, 108, 110,
2189                         112, 114, 116, 118, 120, 122,
2190                         124, 126, 128, 130, 132, 134, 136,
2191                         138, 140, 149, 151, 153, 155, 157,
2192                         159, 161, 163, 165};
2193         u8 place = chnl;
2194
2195         if (chnl > 14) {
2196                 for (place = 14; place < sizeof(channel_all); place++) {
2197                         if (channel_all[place] == chnl)
2198                                 return place - 13;
2199                 }
2200         }
2201         return 0;
2202 }
2203
2204 static void _rtl8723be_phy_lc_calibrate(struct ieee80211_hw *hw, bool is2t)
2205 {
2206         u8 tmpreg;
2207         u32 rf_a_mode = 0, rf_b_mode = 0, lc_cal;
2208         struct rtl_priv *rtlpriv = rtl_priv(hw);
2209
2210         tmpreg = rtl_read_byte(rtlpriv, 0xd03);
2211
2212         if ((tmpreg & 0x70) != 0)
2213                 rtl_write_byte(rtlpriv, 0xd03, tmpreg & 0x8F);
2214         else
2215                 rtl_write_byte(rtlpriv, REG_TXPAUSE, 0xFF);
2216
2217         if ((tmpreg & 0x70) != 0) {
2218                 rf_a_mode = rtl_get_rfreg(hw, RF90_PATH_A, 0x00, MASK12BITS);
2219
2220                 if (is2t)
2221                         rf_b_mode = rtl_get_rfreg(hw, RF90_PATH_B, 0x00,
2222                                                   MASK12BITS);
2223
2224                 rtl_set_rfreg(hw, RF90_PATH_A, 0x00, MASK12BITS,
2225                               (rf_a_mode & 0x8FFFF) | 0x10000);
2226
2227                 if (is2t)
2228                         rtl_set_rfreg(hw, RF90_PATH_B, 0x00, MASK12BITS,
2229                                       (rf_b_mode & 0x8FFFF) | 0x10000);
2230         }
2231         lc_cal = rtl_get_rfreg(hw, RF90_PATH_A, 0x18, MASK12BITS);
2232
2233         rtl_set_rfreg(hw, RF90_PATH_A, 0xb0, RFREG_OFFSET_MASK, 0xdfbe0);
2234         rtl_set_rfreg(hw, RF90_PATH_A, 0x18, MASK12BITS, 0x8c0a);
2235
2236         /* In order not to disturb BT music when wifi init.(1ant NIC only) */
2237         /*mdelay(100);*/
2238         /* In order not to disturb BT music when wifi init.(1ant NIC only) */
2239         mdelay(50);
2240
2241         rtl_set_rfreg(hw, RF90_PATH_A, 0xb0, RFREG_OFFSET_MASK, 0xdffe0);
2242
2243         if ((tmpreg & 0x70) != 0) {
2244                 rtl_write_byte(rtlpriv, 0xd03, tmpreg);
2245                 rtl_set_rfreg(hw, RF90_PATH_A, 0x00, MASK12BITS, rf_a_mode);
2246
2247                 if (is2t)
2248                         rtl_set_rfreg(hw, RF90_PATH_B, 0x00,
2249                                       MASK12BITS, rf_b_mode);
2250         } else {
2251                 rtl_write_byte(rtlpriv, REG_TXPAUSE, 0x00);
2252         }
2253         RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "\n");
2254 }
2255
2256 static void _rtl8723be_phy_set_rfpath_switch(struct ieee80211_hw *hw,
2257                                              bool bmain, bool is2t)
2258 {
2259         struct rtl_priv *rtlpriv = rtl_priv(hw);
2260         RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "\n");
2261
2262         if (bmain) /* left antenna */
2263                 rtl_set_bbreg(hw, 0x92C, MASKDWORD, 0x1);
2264         else
2265                 rtl_set_bbreg(hw, 0x92C, MASKDWORD, 0x2);
2266 }
2267
2268 #undef IQK_ADDA_REG_NUM
2269 #undef IQK_DELAY_TIME
2270 /* IQK is merge from Merge Temp */
2271 void rtl8723be_phy_iq_calibrate(struct ieee80211_hw *hw, bool b_recovery)
2272 {
2273         struct rtl_priv *rtlpriv = rtl_priv(hw);
2274         struct rtl_phy *rtlphy = &rtlpriv->phy;
2275         long result[4][8];
2276         u8 i, final_candidate, idx;
2277         bool b_patha_ok, b_pathb_ok;
2278         long reg_e94, reg_e9c, reg_ea4, reg_eac, reg_eb4, reg_ebc, reg_ec4;
2279         long reg_ecc, reg_tmp = 0;
2280         bool is12simular, is13simular, is23simular;
2281         u32 iqk_bb_reg[9] = {
2282                 ROFDM0_XARXIQIMBALANCE,
2283                 ROFDM0_XBRXIQIMBALANCE,
2284                 ROFDM0_ECCATHRESHOLD,
2285                 ROFDM0_AGCRSSITABLE,
2286                 ROFDM0_XATXIQIMBALANCE,
2287                 ROFDM0_XBTXIQIMBALANCE,
2288                 ROFDM0_XCTXAFE,
2289                 ROFDM0_XDTXAFE,
2290                 ROFDM0_RXIQEXTANTA
2291         };
2292         u32 path_sel_bb = 0; /* path_sel_rf = 0 */
2293
2294         if (rtlphy->lck_inprogress)
2295                 return;
2296
2297         spin_lock(&rtlpriv->locks.iqk_lock);
2298         rtlphy->lck_inprogress = true;
2299         spin_unlock(&rtlpriv->locks.iqk_lock);
2300
2301         if (b_recovery) {
2302                 rtl8723_phy_reload_adda_registers(hw, iqk_bb_reg,
2303                                                   rtlphy->iqk_bb_backup, 9);
2304                 goto label_done;
2305         }
2306         /* Save RF Path */
2307         path_sel_bb = rtl_get_bbreg(hw, 0x948, MASKDWORD);
2308         /* path_sel_rf = rtl_get_rfreg(hw, RF90_PATH_A, 0xb0, 0xfffff); */
2309
2310         for (i = 0; i < 8; i++) {
2311                 result[0][i] = 0;
2312                 result[1][i] = 0;
2313                 result[2][i] = 0;
2314                 result[3][i] = 0;
2315         }
2316         final_candidate = 0xff;
2317         b_patha_ok = false;
2318         b_pathb_ok = false;
2319         is12simular = false;
2320         is23simular = false;
2321         is13simular = false;
2322         for (i = 0; i < 3; i++) {
2323                 _rtl8723be_phy_iq_calibrate(hw, result, i, true);
2324                 if (i == 1) {
2325                         is12simular = _rtl8723be_phy_simularity_compare(hw,
2326                                                                         result,
2327                                                                         0, 1);
2328                         if (is12simular) {
2329                                 final_candidate = 0;
2330                                 break;
2331                         }
2332                 }
2333                 if (i == 2) {
2334                         is13simular = _rtl8723be_phy_simularity_compare(hw,
2335                                                                         result,
2336                                                                         0, 2);
2337                         if (is13simular) {
2338                                 final_candidate = 0;
2339                                 break;
2340                         }
2341                         is23simular = _rtl8723be_phy_simularity_compare(hw,
2342                                                                         result,
2343                                                                         1, 2);
2344                         if (is23simular) {
2345                                 final_candidate = 1;
2346                         } else {
2347                                 for (i = 0; i < 8; i++)
2348                                         reg_tmp += result[3][i];
2349
2350                                 if (reg_tmp != 0)
2351                                         final_candidate = 3;
2352                                 else
2353                                         final_candidate = 0xFF;
2354                         }
2355                 }
2356         }
2357         for (i = 0; i < 4; i++) {
2358                 reg_e94 = result[i][0];
2359                 reg_e9c = result[i][1];
2360                 reg_ea4 = result[i][2];
2361                 reg_eac = result[i][3];
2362                 reg_eb4 = result[i][4];
2363                 reg_ebc = result[i][5];
2364                 reg_ec4 = result[i][6];
2365                 reg_ecc = result[i][7];
2366         }
2367         if (final_candidate != 0xff) {
2368                 reg_e94 = result[final_candidate][0];
2369                 rtlphy->reg_e94 = reg_e94;
2370                 reg_e9c = result[final_candidate][1];
2371                 rtlphy->reg_e9c = reg_e9c;
2372                 reg_ea4 = result[final_candidate][2];
2373                 reg_eac = result[final_candidate][3];
2374                 reg_eb4 = result[final_candidate][4];
2375                 rtlphy->reg_eb4 = reg_eb4;
2376                 reg_ebc = result[final_candidate][5];
2377                 rtlphy->reg_ebc = reg_ebc;
2378                 reg_ec4 = result[final_candidate][6];
2379                 reg_ecc = result[final_candidate][7];
2380                 b_patha_ok = true;
2381                 b_pathb_ok = true;
2382         } else {
2383                 rtlphy->reg_e94 = 0x100;
2384                 rtlphy->reg_eb4 = 0x100;
2385                 rtlphy->reg_e9c = 0x0;
2386                 rtlphy->reg_ebc = 0x0;
2387         }
2388         if (reg_e94 != 0)
2389                 rtl8723_phy_path_a_fill_iqk_matrix(hw, b_patha_ok, result,
2390                                                    final_candidate,
2391                                                    (reg_ea4 == 0));
2392         if (reg_eb4 != 0)
2393                 _rtl8723be_phy_path_b_fill_iqk_matrix(hw, b_pathb_ok, result,
2394                                                       final_candidate,
2395                                                       (reg_ec4 == 0));
2396
2397         idx = _get_right_chnl_place_for_iqk(rtlphy->current_channel);
2398
2399         if (final_candidate < 4) {
2400                 for (i = 0; i < IQK_MATRIX_REG_NUM; i++)
2401                         rtlphy->iqk_matrix[idx].value[0][i] =
2402                                                 result[final_candidate][i];
2403                 rtlphy->iqk_matrix[idx].iqk_done = true;
2404
2405         }
2406         rtl8723_save_adda_registers(hw, iqk_bb_reg,
2407                                     rtlphy->iqk_bb_backup, 9);
2408
2409         rtl_set_bbreg(hw, 0x948, MASKDWORD, path_sel_bb);
2410         /* rtl_set_rfreg(hw, RF90_PATH_A, 0xb0, 0xfffff, path_sel_rf); */
2411
2412 label_done:
2413         spin_lock(&rtlpriv->locks.iqk_lock);
2414         rtlphy->lck_inprogress = false;
2415         spin_unlock(&rtlpriv->locks.iqk_lock);
2416 }
2417
2418 void rtl8723be_phy_lc_calibrate(struct ieee80211_hw *hw)
2419 {
2420         struct rtl_priv *rtlpriv = rtl_priv(hw);
2421         struct rtl_phy *rtlphy = &rtlpriv->phy;
2422         struct rtl_hal *rtlhal = &rtlpriv->rtlhal;
2423         u32 timeout = 2000, timecount = 0;
2424
2425         while (rtlpriv->mac80211.act_scanning && timecount < timeout) {
2426                 udelay(50);
2427                 timecount += 50;
2428         }
2429
2430         rtlphy->lck_inprogress = true;
2431         RTPRINT(rtlpriv, FINIT, INIT_IQK,
2432                 "LCK:Start!!! currentband %x delay %d ms\n",
2433                  rtlhal->current_bandtype, timecount);
2434
2435         _rtl8723be_phy_lc_calibrate(hw, false);
2436
2437         rtlphy->lck_inprogress = false;
2438 }
2439
2440 void rtl8723be_phy_set_rfpath_switch(struct ieee80211_hw *hw, bool bmain)
2441 {
2442         _rtl8723be_phy_set_rfpath_switch(hw, bmain, true);
2443 }
2444
2445 bool rtl8723be_phy_set_io_cmd(struct ieee80211_hw *hw, enum io_type iotype)
2446 {
2447         struct rtl_priv *rtlpriv = rtl_priv(hw);
2448         struct rtl_phy *rtlphy = &rtlpriv->phy;
2449         bool b_postprocessing = false;
2450
2451         RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
2452                  "-->IO Cmd(%#x), set_io_inprogress(%d)\n",
2453                   iotype, rtlphy->set_io_inprogress);
2454         do {
2455                 switch (iotype) {
2456                 case IO_CMD_RESUME_DM_BY_SCAN:
2457                         RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
2458                                  "[IO CMD] Resume DM after scan.\n");
2459                         b_postprocessing = true;
2460                         break;
2461                 case IO_CMD_PAUSE_BAND0_DM_BY_SCAN:
2462                         RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
2463                                  "[IO CMD] Pause DM before scan.\n");
2464                         b_postprocessing = true;
2465                         break;
2466                 default:
2467                         RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
2468                                  "switch case %#x not processed\n", iotype);
2469                         break;
2470                 }
2471         } while (false);
2472         if (b_postprocessing && !rtlphy->set_io_inprogress) {
2473                 rtlphy->set_io_inprogress = true;
2474                 rtlphy->current_io_type = iotype;
2475         } else {
2476                 return false;
2477         }
2478         rtl8723be_phy_set_io(hw);
2479         RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE, "IO Type(%#x)\n", iotype);
2480         return true;
2481 }
2482
2483 static void rtl8723be_phy_set_io(struct ieee80211_hw *hw)
2484 {
2485         struct rtl_priv *rtlpriv = rtl_priv(hw);
2486         struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
2487         struct rtl_phy *rtlphy = &rtlpriv->phy;
2488
2489         RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
2490                  "--->Cmd(%#x), set_io_inprogress(%d)\n",
2491                   rtlphy->current_io_type, rtlphy->set_io_inprogress);
2492         switch (rtlphy->current_io_type) {
2493         case IO_CMD_RESUME_DM_BY_SCAN:
2494                 dm_digtable->cur_igvalue = rtlphy->initgain_backup.xaagccore1;
2495                 /*rtl92c_dm_write_dig(hw);*/
2496                 rtl8723be_phy_set_txpower_level(hw, rtlphy->current_channel);
2497                 rtl_set_bbreg(hw, RCCK0_CCA, 0xff0000, 0x83);
2498                 break;
2499         case IO_CMD_PAUSE_BAND0_DM_BY_SCAN:
2500                 rtlphy->initgain_backup.xaagccore1 = dm_digtable->cur_igvalue;
2501                 dm_digtable->cur_igvalue = 0x17;
2502                 rtl_set_bbreg(hw, RCCK0_CCA, 0xff0000, 0x40);
2503                 break;
2504         default:
2505                 RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
2506                          "switch case %#x not processed\n",
2507                          rtlphy->current_io_type);
2508                 break;
2509         }
2510         rtlphy->set_io_inprogress = false;
2511         RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
2512                  "(%#x)\n", rtlphy->current_io_type);
2513 }
2514
2515 static void rtl8723be_phy_set_rf_on(struct ieee80211_hw *hw)
2516 {
2517         struct rtl_priv *rtlpriv = rtl_priv(hw);
2518
2519         rtl_write_byte(rtlpriv, REG_SPS0_CTRL, 0x2b);
2520         rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE3);
2521         rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE2);
2522         rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE3);
2523         rtl_write_byte(rtlpriv, REG_TXPAUSE, 0x00);
2524 }
2525
2526 static void _rtl8723be_phy_set_rf_sleep(struct ieee80211_hw *hw)
2527 {
2528         struct rtl_priv *rtlpriv = rtl_priv(hw);
2529
2530         rtl_write_byte(rtlpriv, REG_TXPAUSE, 0xFF);
2531         rtl_set_rfreg(hw, RF90_PATH_A, 0x00, RFREG_OFFSET_MASK, 0x00);
2532         rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE2);
2533         rtl_write_byte(rtlpriv, REG_SPS0_CTRL, 0x22);
2534 }
2535
2536 static bool _rtl8723be_phy_set_rf_power_state(struct ieee80211_hw *hw,
2537                                               enum rf_pwrstate rfpwr_state)
2538 {
2539         struct rtl_priv *rtlpriv = rtl_priv(hw);
2540         struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
2541         struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
2542         struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
2543         bool bresult = true;
2544         u8 i, queue_id;
2545         struct rtl8192_tx_ring *ring = NULL;
2546
2547         switch (rfpwr_state) {
2548         case ERFON:
2549                 if ((ppsc->rfpwr_state == ERFOFF) &&
2550                      RT_IN_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC)) {
2551                         bool rtstatus;
2552                         u32 initializecount = 0;
2553                         do {
2554                                 initializecount++;
2555                                 RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
2556                                          "IPS Set eRf nic enable\n");
2557                                 rtstatus = rtl_ps_enable_nic(hw);
2558                         } while (!rtstatus && (initializecount < 10));
2559                         RT_CLEAR_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
2560                 } else {
2561                         RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
2562                                  "Set ERFON sleeped:%d ms\n",
2563                                   jiffies_to_msecs(jiffies -
2564                                                    ppsc->last_sleep_jiffies));
2565                         ppsc->last_awake_jiffies = jiffies;
2566                         rtl8723be_phy_set_rf_on(hw);
2567                 }
2568                 if (mac->link_state == MAC80211_LINKED)
2569                         rtlpriv->cfg->ops->led_control(hw, LED_CTL_LINK);
2570                 else
2571                         rtlpriv->cfg->ops->led_control(hw, LED_CTL_NO_LINK);
2572
2573                 break;
2574
2575         case ERFOFF:
2576                 for (queue_id = 0, i = 0;
2577                      queue_id < RTL_PCI_MAX_TX_QUEUE_COUNT;) {
2578                         ring = &pcipriv->dev.tx_ring[queue_id];
2579                         /* Don't check BEACON Q.
2580                          * BEACON Q is always not empty,
2581                          * because '_rtl8723be_cmd_send_packet'
2582                          */
2583                         if (queue_id == BEACON_QUEUE ||
2584                             skb_queue_len(&ring->queue) == 0) {
2585                                 queue_id++;
2586                                 continue;
2587                         } else {
2588                                 RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
2589                                          "eRf Off/Sleep: %d times TcbBusyQueue[%d] =%d before doze!\n",
2590                                          (i + 1), queue_id,
2591                                          skb_queue_len(&ring->queue));
2592
2593                                 udelay(10);
2594                                 i++;
2595                         }
2596                         if (i >= MAX_DOZE_WAITING_TIMES_9x) {
2597                                 RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
2598                                          "ERFSLEEP: %d times TcbBusyQueue[%d] = %d !\n",
2599                                           MAX_DOZE_WAITING_TIMES_9x,
2600                                           queue_id,
2601                                           skb_queue_len(&ring->queue));
2602                                 break;
2603                         }
2604                 }
2605
2606                 if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_HALT_NIC) {
2607                         RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
2608                                  "IPS Set eRf nic disable\n");
2609                         rtl_ps_disable_nic(hw);
2610                         RT_SET_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
2611                 } else {
2612                         if (ppsc->rfoff_reason == RF_CHANGE_BY_IPS) {
2613                                 rtlpriv->cfg->ops->led_control(hw,
2614                                                                LED_CTL_NO_LINK);
2615                         } else {
2616                                 rtlpriv->cfg->ops->led_control(hw,
2617                                                              LED_CTL_POWER_OFF);
2618                         }
2619                 }
2620                 break;
2621
2622         case ERFSLEEP:
2623                 if (ppsc->rfpwr_state == ERFOFF)
2624                         break;
2625                 for (queue_id = 0, i = 0;
2626                      queue_id < RTL_PCI_MAX_TX_QUEUE_COUNT;) {
2627                         ring = &pcipriv->dev.tx_ring[queue_id];
2628                         if (skb_queue_len(&ring->queue) == 0) {
2629                                 queue_id++;
2630                                 continue;
2631                         } else {
2632                                 RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
2633                                          "eRf Off/Sleep: %d times TcbBusyQueue[%d] =%d before doze!\n",
2634                                          (i + 1), queue_id,
2635                                          skb_queue_len(&ring->queue));
2636
2637                                 udelay(10);
2638                                 i++;
2639                         }
2640                         if (i >= MAX_DOZE_WAITING_TIMES_9x) {
2641                                 RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
2642                                          "ERFSLEEP: %d times TcbBusyQueue[%d] = %d !\n",
2643                                          MAX_DOZE_WAITING_TIMES_9x,
2644                                          queue_id,
2645                                          skb_queue_len(&ring->queue));
2646                                 break;
2647                         }
2648                 }
2649                 RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
2650                          "Set ERFSLEEP awaked:%d ms\n",
2651                           jiffies_to_msecs(jiffies -
2652                                            ppsc->last_awake_jiffies));
2653                 ppsc->last_sleep_jiffies = jiffies;
2654                 _rtl8723be_phy_set_rf_sleep(hw);
2655                 break;
2656
2657         default:
2658                 RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
2659                          "switch case %#x not processed\n", rfpwr_state);
2660                 bresult = false;
2661                 break;
2662         }
2663         if (bresult)
2664                 ppsc->rfpwr_state = rfpwr_state;
2665         return bresult;
2666 }
2667
2668 bool rtl8723be_phy_set_rf_power_state(struct ieee80211_hw *hw,
2669                                       enum rf_pwrstate rfpwr_state)
2670 {
2671         struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
2672
2673         bool bresult = false;
2674
2675         if (rfpwr_state == ppsc->rfpwr_state)
2676                 return bresult;
2677         bresult = _rtl8723be_phy_set_rf_power_state(hw, rfpwr_state);
2678         return bresult;
2679 }