rtl8821ce/hal/phydm/phydm_antdect.c

   1 /******************************************************************************
   2  *
   3  * Copyright(c) 2007 - 2017  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 /* ************************************************************
  27  * include files
  28  * ************************************************************ */
  29
  30 #include "mp_precomp.h"
  31 #include "phydm_precomp.h"
  32
  33 #ifdef CONFIG_ANT_DETECTION
  34
  35 /* @IS_ANT_DETECT_SUPPORT_SINGLE_TONE(adapter)
  36  * IS_ANT_DETECT_SUPPORT_RSSI(adapter)
  37  * IS_ANT_DETECT_SUPPORT_PSD(adapter) */
  38
  39 /* @1 [1. Single Tone method] =================================================== */
  40
  41 /*@
  42  * Description:
  43  *      Set Single/Dual Antenna default setting for products that do not do detection in advance.
  44  *
  45  * Added by Joseph, 2012.03.22
  46  *   */
  47 void odm_sw_ant_div_construct_scan_chnl(
  48         void *adapter,
  49         u8 scan_chnl)
  50 {
  51 }
  52
  53 u8 odm_sw_ant_div_select_scan_chnl(
  54         void *adapter)
  55 {
  56         return 0;
  57 }
  58
  59 void odm_single_dual_antenna_default_setting(
  60         void *dm_void)
  61 {
  62         struct dm_struct *dm = (struct dm_struct *)dm_void;
  63         struct sw_antenna_switch *dm_swat_table = &dm->dm_swat_table;
  64         void *adapter = dm->adapter;
  65
  66         u8 bt_ant_num = BT_GetPgAntNum(adapter);
  67         /* Set default antenna A and B status */
  68         if (bt_ant_num == 2) {
  69                 dm_swat_table->ANTA_ON = true;
  70                 dm_swat_table->ANTB_ON = true;
  71
  72         } else if (bt_ant_num == 1) {
  73                 /* Set antenna A as default */
  74                 dm_swat_table->ANTA_ON = true;
  75                 dm_swat_table->ANTB_ON = false;
  76
  77         } else
  78                 RT_ASSERT(false, ("Incorrect antenna number!!\n"));
  79 }
  80
  81 /* @2 8723A ANT DETECT
  82  *
  83  * Description:
  84  *      Implement IQK single tone for RF DPK loopback and BB PSD scanning.
  85  *      This function is cooperated with BB team Neil.
  86  *
  87  * Added by Roger, 2011.12.15
  88  *   */
  89 boolean
  90 odm_single_dual_antenna_detection(
  91         void *dm_void,
  92         u8 mode)
  93 {
  94         struct dm_struct *dm = (struct dm_struct *)dm_void;
  95         void *adapter = dm->adapter;
  96         struct sw_antenna_switch *dm_swat_table = &dm->dm_swat_table;
  97         u32 current_channel, rf_loop_reg;
  98         u8 n;
  99         u32 reg88c, regc08, reg874, regc50, reg948, regb2c, reg92c, reg930, reg064, afe_rrx_wait_cca;
 100         u8 initial_gain = 0x5a;
 101         u32 PSD_report_tmp;
 102         u32 ant_a_report = 0x0, ant_b_report = 0x0, ant_0_report = 0x0;
 103         boolean is_result = true;
 104
 105         PHYDM_DBG(dm, DBG_ANT_DIV, "%s============>\n", __func__);
 106
 107         if (!(dm->support_ic_type & ODM_RTL8723B))
 108                 return is_result;
 109
 110         /* Retrieve antenna detection registry info, added by Roger, 2012.11.27. */
 111         if (!IS_ANT_DETECT_SUPPORT_SINGLE_TONE(((PADAPTER)adapter)))
 112                 return is_result;
 113
 114         /* @1 Backup Current RF/BB Settings */
 115
 116         current_channel = odm_get_rf_reg(dm, RF_PATH_A, ODM_CHANNEL, RFREGOFFSETMASK);
 117         rf_loop_reg = odm_get_rf_reg(dm, RF_PATH_A, RF_0x00, RFREGOFFSETMASK);
 118         if (dm->support_ic_type & ODM_RTL8723B) {
 119                 reg92c = odm_get_bb_reg(dm, REG_DPDT_CONTROL, MASKDWORD);
 120                 reg930 = odm_get_bb_reg(dm, rfe_ctrl_anta_src, MASKDWORD);
 121                 reg948 = odm_get_bb_reg(dm, REG_S0_S1_PATH_SWITCH, MASKDWORD);
 122                 regb2c = odm_get_bb_reg(dm, REG_AGC_TABLE_SELECT, MASKDWORD);
 123                 reg064 = odm_get_mac_reg(dm, REG_SYM_WLBT_PAPE_SEL, BIT(29));
 124                 odm_set_bb_reg(dm, REG_DPDT_CONTROL, 0x3, 0x1);
 125                 odm_set_bb_reg(dm, rfe_ctrl_anta_src, 0xff, 0x77);
 126                 odm_set_mac_reg(dm, REG_SYM_WLBT_PAPE_SEL, BIT(29), 0x1); /* @dbg 7 */
 127                 odm_set_bb_reg(dm, REG_S0_S1_PATH_SWITCH, 0x3c0, 0x0); /* @dbg 8 */
 128                 odm_set_bb_reg(dm, REG_AGC_TABLE_SELECT, BIT(31), 0x0);
 129         }
 130
 131         ODM_delay_us(10);
 132
 133         /* Store A path Register 88c, c08, 874, c50 */
 134         reg88c = odm_get_bb_reg(dm, REG_FPGA0_ANALOG_PARAMETER4, MASKDWORD);
 135         regc08 = odm_get_bb_reg(dm, REG_OFDM_0_TR_MUX_PAR, MASKDWORD);
 136         reg874 = odm_get_bb_reg(dm, REG_FPGA0_XCD_RF_INTERFACE_SW, MASKDWORD);
 137         regc50 = odm_get_bb_reg(dm, REG_OFDM_0_XA_AGC_CORE1, MASKDWORD);
 138
 139         /* Store AFE Registers */
 140         if (dm->support_ic_type & ODM_RTL8723B)
 141                 afe_rrx_wait_cca = odm_get_bb_reg(dm, REG_RX_WAIT_CCA, MASKDWORD);
 142
 143         /* Set PSD 128 pts */
 144         odm_set_bb_reg(dm, REG_FPGA0_PSD_FUNCTION, BIT(14) | BIT15, 0x0); /* @128 pts */
 145
 146         /* To SET CH1 to do */
 147         odm_set_rf_reg(dm, RF_PATH_A, ODM_CHANNEL, RFREGOFFSETMASK, 0x7401); /* @channel 1 */
 148
 149         /* @AFE all on step */
 150         if (dm->support_ic_type & ODM_RTL8723B)
 151                 odm_set_bb_reg(dm, REG_RX_WAIT_CCA, MASKDWORD, 0x01c00016);
 152
 153         /* @3 wire Disable */
 154         odm_set_bb_reg(dm, REG_FPGA0_ANALOG_PARAMETER4, MASKDWORD, 0xCCF000C0);
 155
 156         /* @BB IQK setting */
 157         odm_set_bb_reg(dm, REG_OFDM_0_TR_MUX_PAR, MASKDWORD, 0x000800E4);
 158         odm_set_bb_reg(dm, REG_FPGA0_XCD_RF_INTERFACE_SW, MASKDWORD, 0x22208000);
 159
 160         /* @IQK setting tone@ 4.34Mhz */
 161         odm_set_bb_reg(dm, REG_TX_IQK_TONE_A, MASKDWORD, 0x10008C1C);
 162         odm_set_bb_reg(dm, REG_TX_IQK, MASKDWORD, 0x01007c00);
 163
 164         /* Page B init */
 165         odm_set_bb_reg(dm, REG_CONFIG_ANT_A, MASKDWORD, 0x00080000);
 166         odm_set_bb_reg(dm, REG_CONFIG_ANT_A, MASKDWORD, 0x0f600000);
 167         odm_set_bb_reg(dm, REG_RX_IQK, MASKDWORD, 0x01004800);
 168         odm_set_bb_reg(dm, REG_RX_IQK_TONE_A, MASKDWORD, 0x10008c1f);
 169         if (dm->support_ic_type & ODM_RTL8723B) {
 170                 odm_set_bb_reg(dm, REG_TX_IQK_PI_A, MASKDWORD, 0x82150016);
 171                 odm_set_bb_reg(dm, REG_RX_IQK_PI_A, MASKDWORD, 0x28150016);
 172         }
 173         odm_set_bb_reg(dm, REG_IQK_AGC_RSP, MASKDWORD, 0x001028d0);
 174         odm_set_bb_reg(dm, REG_OFDM_0_XA_AGC_CORE1, 0x7f, initial_gain);
 175
 176         /* @IQK Single tone start */
 177         odm_set_bb_reg(dm, REG_FPGA0_IQK, 0xffffff00, 0x808000);
 178         odm_set_bb_reg(dm, REG_IQK_AGC_PTS, MASKDWORD, 0xf9000000);
 179         odm_set_bb_reg(dm, REG_IQK_AGC_PTS, MASKDWORD, 0xf8000000);
 180
 181         ODM_delay_us(10000);
 182
 183         /* PSD report of antenna A */
 184         PSD_report_tmp = 0x0;
 185         for (n = 0; n < 2; n++) {
 186                 PSD_report_tmp = phydm_get_psd_data(dm, 14, initial_gain);
 187                 if (PSD_report_tmp > ant_a_report)
 188                         ant_a_report = PSD_report_tmp;
 189         }
 190
 191         /* @change to Antenna B */
 192         if (dm->support_ic_type & ODM_RTL8723B) {
 193 #if 0
 194                 /* odm_set_bb_reg(dm, REG_DPDT_CONTROL, 0x3, 0x2); */
 195 #endif
 196                 odm_set_bb_reg(dm, REG_S0_S1_PATH_SWITCH, 0xfff, 0x280);
 197                 odm_set_bb_reg(dm, REG_AGC_TABLE_SELECT, BIT(31), 0x1);
 198         }
 199
 200         ODM_delay_us(10);
 201
 202         /* PSD report of antenna B */
 203         PSD_report_tmp = 0x0;
 204         for (n = 0; n < 2; n++) {
 205                 PSD_report_tmp = phydm_get_psd_data(dm, 14, initial_gain);
 206                 if (PSD_report_tmp > ant_b_report)
 207                         ant_b_report = PSD_report_tmp;
 208         }
 209
 210         /* @Close IQK Single Tone function */
 211         odm_set_bb_reg(dm, REG_FPGA0_IQK, 0xffffff00, 0x000000);
 212
 213         /* @1 Return to antanna A */
 214         if (dm->support_ic_type & ODM_RTL8723B) {
 215                 /* @external DPDT */
 216                 odm_set_bb_reg(dm, REG_DPDT_CONTROL, MASKDWORD, reg92c);
 217
 218                 /* @internal S0/S1 */
 219                 odm_set_bb_reg(dm, REG_S0_S1_PATH_SWITCH, MASKDWORD, reg948);
 220                 odm_set_bb_reg(dm, REG_AGC_TABLE_SELECT, MASKDWORD, regb2c);
 221                 odm_set_bb_reg(dm, rfe_ctrl_anta_src, MASKDWORD, reg930);
 222                 odm_set_mac_reg(dm, REG_SYM_WLBT_PAPE_SEL, BIT(29), reg064);
 223         }
 224
 225         odm_set_bb_reg(dm, REG_FPGA0_ANALOG_PARAMETER4, MASKDWORD, reg88c);
 226         odm_set_bb_reg(dm, REG_OFDM_0_TR_MUX_PAR, MASKDWORD, regc08);
 227         odm_set_bb_reg(dm, REG_FPGA0_XCD_RF_INTERFACE_SW, MASKDWORD, reg874);
 228         odm_set_bb_reg(dm, REG_OFDM_0_XA_AGC_CORE1, 0x7F, 0x40);
 229         odm_set_bb_reg(dm, REG_OFDM_0_XA_AGC_CORE1, MASKDWORD, regc50);
 230         odm_set_rf_reg(dm, RF_PATH_A, RF_CHNLBW, RFREGOFFSETMASK, current_channel);
 231         odm_set_rf_reg(dm, RF_PATH_A, RF_0x00, RFREGOFFSETMASK, rf_loop_reg);
 232
 233         /* Reload AFE Registers */
 234         if (dm->support_ic_type & ODM_RTL8723B)
 235                 odm_set_bb_reg(dm, REG_RX_WAIT_CCA, MASKDWORD, afe_rrx_wait_cca);
 236
 237         if (dm->support_ic_type & ODM_RTL8723B) {
 238                 PHYDM_DBG(dm, DBG_ANT_DIV, "psd_report_A[%d]= %d\n", 2416,
 239                           ant_a_report);
 240                 PHYDM_DBG(dm, DBG_ANT_DIV, "psd_report_B[%d]= %d\n", 2416,
 241                           ant_b_report);
 242
 243                 /* @2 Test ant B based on ant A is ON */
 244                 if (ant_a_report >= 100 && ant_b_report >= 100 && ant_a_report <= 135 && ant_b_report <= 135) {
 245                         u8 TH1 = 2, TH2 = 6;
 246
 247                         if ((ant_a_report - ant_b_report < TH1) || (ant_b_report - ant_a_report < TH1)) {
 248                                 dm_swat_table->ANTA_ON = true;
 249                                 dm_swat_table->ANTB_ON = true;
 250                                 PHYDM_DBG(dm, DBG_ANT_DIV, "%s: Dual Antenna\n",
 251                                           __func__);
 252                         } else if (((ant_a_report - ant_b_report >= TH1) && (ant_a_report - ant_b_report <= TH2)) ||
 253                                    ((ant_b_report - ant_a_report >= TH1) && (ant_b_report - ant_a_report <= TH2))) {
 254                                 dm_swat_table->ANTA_ON = false;
 255                                 dm_swat_table->ANTB_ON = false;
 256                                 is_result = false;
 257                                 PHYDM_DBG(dm, DBG_ANT_DIV,
 258                                           "%s: Need to check again\n",
 259                                           __func__);
 260                         } else {
 261                                 dm_swat_table->ANTA_ON = true;
 262                                 dm_swat_table->ANTB_ON = false;
 263                                 PHYDM_DBG(dm, DBG_ANT_DIV,
 264                                           "%s: Single Antenna\n", __func__);
 265                         }
 266                         dm->ant_detected_info.is_ant_detected = true;
 267                         dm->ant_detected_info.db_for_ant_a = ant_a_report;
 268                         dm->ant_detected_info.db_for_ant_b = ant_b_report;
 269                         dm->ant_detected_info.db_for_ant_o = ant_0_report;
 270
 271                 } else {
 272                         PHYDM_DBG(dm, DBG_ANT_DIV, "return false!!\n");
 273                         is_result = false;
 274                 }
 275         }
 276         return is_result;
 277 }
 278
 279 /* @1 [2. Scan AP RSSI method] ================================================== */
 280
 281 boolean
 282 odm_sw_ant_div_check_before_link(
 283         void *dm_void)
 284 {
 285 #if (RT_MEM_SIZE_LEVEL != RT_MEM_SIZE_MINIMUM)
 286
 287         struct dm_struct *dm = (struct dm_struct *)dm_void;
 288         void *adapter = dm->adapter;
 289         HAL_DATA_TYPE *hal_data = GET_HAL_DATA(((PADAPTER)adapter));
 290         //PMGNT_INFO            mgnt_info = &adapter->MgntInfo;
 291         PMGNT_INFO mgnt_info = &(((PADAPTER)(adapter))->MgntInfo);
 292         struct sw_antenna_switch *dm_swat_table = &dm->dm_swat_table;
 293         struct phydm_fat_struct *fat_tab = &dm->dm_fat_table;
 294         s8 score = 0;
 295         PRT_WLAN_BSS p_tmp_bss_desc, p_test_bss_desc;
 296         u8 power_target_L = 9, power_target_H = 16;
 297         u8 tmp_power_diff = 0, power_diff = 0, avg_power_diff = 0, max_power_diff = 0, min_power_diff = 0xff;
 298         u16 index, counter = 0;
 299         static u8 scan_channel;
 300         u32 tmp_swas_no_link_bk_reg948;
 301
 302         PHYDM_DBG(dm, DBG_ANT_DIV, "ANTA_ON = (( %d )) , ANTB_ON = (( %d ))\n",
 303                   dm->dm_swat_table.ANTA_ON, dm->dm_swat_table.ANTB_ON);
 304
 305         /* @if(HP id) */
 306         {
 307                 if (dm->dm_swat_table.rssi_ant_dect_result == true && dm->support_ic_type == ODM_RTL8723B) {
 308                         PHYDM_DBG(dm, DBG_ANT_DIV,
 309                                   "8723B RSSI-based Antenna Detection is done\n");
 310                         return false;
 311                 }
 312
 313                 if (dm->support_ic_type == ODM_RTL8723B) {
 314                         if (dm_swat_table->swas_no_link_bk_reg948 == 0xff)
 315                                 dm_swat_table->swas_no_link_bk_reg948 = odm_read_4byte(dm, REG_S0_S1_PATH_SWITCH);
 316                 }
 317         }
 318
 319         if (dm->adapter == NULL) { /* @For BSOD when plug/unplug fast.  //By YJ,120413 */
 320                 /* The ODM structure is not initialized. */
 321                 return false;
 322         }
 323
 324         /* Retrieve antenna detection registry info, added by Roger, 2012.11.27. */
 325         if (!IS_ANT_DETECT_SUPPORT_RSSI(((PADAPTER)adapter)))
 326                 return false;
 327         else
 328                 PHYDM_DBG(dm, DBG_ANT_DIV, "Antenna Detection: RSSI method\n");
 329
 330         /* Since driver is going to set BB register, it shall check if there is another thread controlling BB/RF. */
 331         odm_acquire_spin_lock(dm, RT_RF_STATE_SPINLOCK);
 332         if (hal_data->eRFPowerState != eRfOn || mgnt_info->RFChangeInProgress || mgnt_info->bMediaConnect) {
 333                 odm_release_spin_lock(dm, RT_RF_STATE_SPINLOCK);
 334
 335                 PHYDM_DBG(dm, DBG_ANT_DIV,
 336                           "%s: rf_change_in_progress(%x), e_rf_power_state(%x)\n",
 337                           __func__, mgnt_info->RFChangeInProgress,
 338                           hal_data->eRFPowerState);
 339
 340                 dm_swat_table->swas_no_link_state = 0;
 341
 342                 return false;
 343         } else
 344                 odm_release_spin_lock(dm, RT_RF_STATE_SPINLOCK);
 345         PHYDM_DBG(dm, DBG_ANT_DIV, "dm_swat_table->swas_no_link_state = %d\n",
 346                   dm_swat_table->swas_no_link_state);
 347         /* @1 Run AntDiv mechanism "Before Link" part. */
 348         if (dm_swat_table->swas_no_link_state == 0) {
 349                 /* @1 Prepare to do Scan again to check current antenna state. */
 350
 351                 /* Set check state to next step. */
 352                 dm_swat_table->swas_no_link_state = 1;
 353
 354                 /* @Copy Current Scan list. */
 355                 mgnt_info->tmpNumBssDesc = mgnt_info->NumBssDesc;
 356                 PlatformMoveMemory((void *)mgnt_info->tmpbssDesc, (void *)mgnt_info->bssDesc, sizeof(RT_WLAN_BSS) * MAX_BSS_DESC);
 357
 358                 /* @Go back to scan function again. */
 359                 PHYDM_DBG(dm, DBG_ANT_DIV, "%s: Scan one more time\n",
 360                           __func__);
 361                 mgnt_info->ScanStep = 0;
 362                 mgnt_info->bScanAntDetect = true;
 363                 scan_channel = odm_sw_ant_div_select_scan_chnl(adapter);
 364
 365                 if (dm->support_ic_type & (ODM_RTL8188E | ODM_RTL8821)) {
 366                         if (fat_tab->rx_idle_ant == MAIN_ANT)
 367                                 odm_update_rx_idle_ant(dm, AUX_ANT);
 368                         else
 369                                 odm_update_rx_idle_ant(dm, MAIN_ANT);
 370                         if (scan_channel == 0) {
 371                                 PHYDM_DBG(dm, DBG_ANT_DIV,
 372                                           "%s: No AP List Avaiable, Using ant(%s)\n",
 373                                           __func__,
 374                                           (fat_tab->rx_idle_ant == MAIN_ANT) ?
 375                                           "AUX_ANT" : "MAIN_ANT");
 376
 377                                 if (IS_5G_WIRELESS_MODE(mgnt_info->dot11CurrentWirelessMode)) {
 378                                         dm_swat_table->ant_5g = fat_tab->rx_idle_ant;
 379                                         PHYDM_DBG(dm, DBG_ANT_DIV, "dm_swat_table->ant_5g=%s\n", (fat_tab->rx_idle_ant == MAIN_ANT) ? "MAIN_ANT" : "AUX_ANT");
 380                                 } else {
 381                                         dm_swat_table->ant_2g = fat_tab->rx_idle_ant;
 382                                         PHYDM_DBG(dm, DBG_ANT_DIV, "dm_swat_table->ant_2g=%s\n", (fat_tab->rx_idle_ant == MAIN_ANT) ? "MAIN_ANT" : "AUX_ANT");
 383                                 }
 384                                 return false;
 385                         }
 386
 387                         PHYDM_DBG(dm, DBG_ANT_DIV,
 388                                   "%s: Change to %s for testing.\n", __func__,
 389                                   ((fat_tab->rx_idle_ant == MAIN_ANT) ?
 390                                   "MAIN_ANT" : "AUX_ANT"));
 391                 } else if (dm->support_ic_type & (ODM_RTL8723B)) {
 392                         /*Switch Antenna to another one.*/
 393
 394                         tmp_swas_no_link_bk_reg948 = odm_read_4byte(dm, REG_S0_S1_PATH_SWITCH);
 395
 396                         if (dm_swat_table->cur_antenna == MAIN_ANT && tmp_swas_no_link_bk_reg948 == 0x200) {
 397                                 odm_set_bb_reg(dm, REG_S0_S1_PATH_SWITCH, 0xfff, 0x280);
 398                                 odm_set_bb_reg(dm, REG_AGC_TABLE_SELECT, BIT(31), 0x1);
 399                                 dm_swat_table->cur_antenna = AUX_ANT;
 400                         } else {
 401                                 PHYDM_DBG(dm, DBG_ANT_DIV,
 402                                           "Reg[948]= (( %x )) was in wrong state\n",
 403                                           tmp_swas_no_link_bk_reg948);
 404                                 return false;
 405                         }
 406                         ODM_delay_us(10);
 407
 408                         PHYDM_DBG(dm, DBG_ANT_DIV,
 409                                   "%s: Change to (( %s-ant))  for testing.\n",
 410                                   __func__,
 411                                   (dm_swat_table->cur_antenna == MAIN_ANT) ?
 412                                   "MAIN" : "AUX");
 413                 }
 414
 415                 odm_sw_ant_div_construct_scan_chnl(adapter, scan_channel);
 416                 PlatformSetTimer(adapter, &mgnt_info->ScanTimer, 5);
 417
 418                 return true;
 419         } else { /* @dm_swat_table->swas_no_link_state == 1 */
 420                 /* @1 ScanComple() is called after antenna swiched. */
 421                 /* @1 Check scan result and determine which antenna is going */
 422                 /* @1 to be used. */
 423
 424                 PHYDM_DBG(dm, DBG_ANT_DIV, " tmp_num_bss_desc= (( %d ))\n",
 425                           mgnt_info->tmpNumBssDesc); /* @debug for Dino */
 426
 427                 for (index = 0; index < mgnt_info->tmpNumBssDesc; index++) {
 428                         p_tmp_bss_desc = &mgnt_info->tmpbssDesc[index]; /* @Antenna 1 */
 429                         p_test_bss_desc = &mgnt_info->bssDesc[index]; /* @Antenna 2 */
 430
 431                         if (PlatformCompareMemory(p_test_bss_desc->bdBssIdBuf, p_tmp_bss_desc->bdBssIdBuf, 6) != 0) {
 432                                 PHYDM_DBG(dm, DBG_ANT_DIV,
 433                                           "%s: ERROR!! This shall not happen.\n",
 434                                           __func__);
 435                                 continue;
 436                         }
 437
 438                         if (dm->support_ic_type != ODM_RTL8723B) {
 439                                 if (p_tmp_bss_desc->ChannelNumber == scan_channel) {
 440                                         if (p_tmp_bss_desc->RecvSignalPower > p_test_bss_desc->RecvSignalPower) {
 441                                                 PHYDM_DBG(dm, DBG_ANT_DIV, "%s: Compare scan entry: score++\n", __func__);
 442                                                 RT_PRINT_STR(COMP_SCAN, DBG_WARNING, "GetScanInfo(): new Bss SSID:", p_tmp_bss_desc->bdSsIdBuf, p_tmp_bss_desc->bdSsIdLen);
 443                                                 PHYDM_DBG(dm, DBG_ANT_DIV, "at ch %d, Original: %d, Test: %d\n\n", p_tmp_bss_desc->ChannelNumber, p_tmp_bss_desc->RecvSignalPower, p_test_bss_desc->RecvSignalPower);
 444
 445                                                 score++;
 446                                                 PlatformMoveMemory(p_test_bss_desc, p_tmp_bss_desc, sizeof(RT_WLAN_BSS));
 447                                         } else if (p_tmp_bss_desc->RecvSignalPower < p_test_bss_desc->RecvSignalPower) {
 448                                                 PHYDM_DBG(dm, DBG_ANT_DIV, "%s: Compare scan entry: score--\n", __func__);
 449                                                 RT_PRINT_STR(COMP_SCAN, DBG_WARNING, "GetScanInfo(): new Bss SSID:", p_tmp_bss_desc->bdSsIdBuf, p_tmp_bss_desc->bdSsIdLen);
 450                                                 PHYDM_DBG(dm, DBG_ANT_DIV, "at ch %d, Original: %d, Test: %d\n\n", p_tmp_bss_desc->ChannelNumber, p_tmp_bss_desc->RecvSignalPower, p_test_bss_desc->RecvSignalPower);
 451                                                 score--;
 452                                         } else {
 453                                                 if (p_test_bss_desc->bdTstamp - p_tmp_bss_desc->bdTstamp < 5000) {
 454                                                         RT_PRINT_STR(COMP_SCAN, DBG_WARNING, "GetScanInfo(): new Bss SSID:", p_tmp_bss_desc->bdSsIdBuf, p_tmp_bss_desc->bdSsIdLen);
 455                                                         PHYDM_DBG(dm, DBG_ANT_DIV, "at ch %d, Original: %d, Test: %d\n", p_tmp_bss_desc->ChannelNumber, p_tmp_bss_desc->RecvSignalPower, p_test_bss_desc->RecvSignalPower);
 456                                                         PHYDM_DBG(dm, DBG_ANT_DIV, "The 2nd Antenna didn't get this AP\n\n");
 457                                                 }
 458                                         }
 459                                 }
 460                         } else { /* @8723B */
 461                                 if (p_tmp_bss_desc->ChannelNumber == scan_channel) {
 462                                         PHYDM_DBG(dm, DBG_ANT_DIV, "channel_number == scan_channel->(( %d ))\n", p_tmp_bss_desc->ChannelNumber);
 463
 464                                         if (p_tmp_bss_desc->RecvSignalPower > p_test_bss_desc->RecvSignalPower) { /* Pow(Ant1) > Pow(Ant2) */
 465                                                 counter++;
 466                                                 tmp_power_diff = (u8)(p_tmp_bss_desc->RecvSignalPower - p_test_bss_desc->RecvSignalPower);
 467                                                 power_diff = power_diff + tmp_power_diff;
 468
 469                                                 PHYDM_DBG(dm, DBG_ANT_DIV, "Original: %d, Test: %d\n", p_tmp_bss_desc->RecvSignalPower, p_test_bss_desc->RecvSignalPower);
 470                                                 PHYDM_PRINT_ADDR(dm, DBG_ANT_DIV, "SSID:", p_tmp_bss_desc->bdSsIdBuf);
 471                                                 PHYDM_PRINT_ADDR(dm, DBG_ANT_DIV, "BSSID:", p_tmp_bss_desc->bdSsIdBuf);
 472
 473 #if 0
 474                                                 /* PHYDM_DBG(dm,DBG_ANT_DIV, "tmp_power_diff: (( %d)),max_power_diff: (( %d)),min_power_diff: (( %d))\n", tmp_power_diff,max_power_diff,min_power_diff); */
 475 #endif
 476                                                 if (tmp_power_diff > max_power_diff)
 477                                                         max_power_diff = tmp_power_diff;
 478                                                 if (tmp_power_diff < min_power_diff)
 479                                                         min_power_diff = tmp_power_diff;
 480 #if 0
 481                                                 /* PHYDM_DBG(dm,DBG_ANT_DIV, "max_power_diff: (( %d)),min_power_diff: (( %d))\n",max_power_diff,min_power_diff); */
 482 #endif
 483
 484                                                 PlatformMoveMemory(p_test_bss_desc, p_tmp_bss_desc, sizeof(RT_WLAN_BSS));
 485                                         } else if (p_test_bss_desc->RecvSignalPower > p_tmp_bss_desc->RecvSignalPower) { /* Pow(Ant1) < Pow(Ant2) */
 486                                                 counter++;
 487                                                 tmp_power_diff = (u8)(p_test_bss_desc->RecvSignalPower - p_tmp_bss_desc->RecvSignalPower);
 488                                                 power_diff = power_diff + tmp_power_diff;
 489                                                 PHYDM_DBG(dm, DBG_ANT_DIV, "Original: %d, Test: %d\n", p_tmp_bss_desc->RecvSignalPower, p_test_bss_desc->RecvSignalPower);
 490                                                 PHYDM_PRINT_ADDR(dm, DBG_ANT_DIV, "SSID:", p_tmp_bss_desc->bdSsIdBuf);
 491                                                 PHYDM_PRINT_ADDR(dm, DBG_ANT_DIV, "BSSID:", p_tmp_bss_desc->bdSsIdBuf);
 492                                                 if (tmp_power_diff > max_power_diff)
 493                                                         max_power_diff = tmp_power_diff;
 494                                                 if (tmp_power_diff < min_power_diff)
 495                                                         min_power_diff = tmp_power_diff;
 496                                         } else { /* Pow(Ant1) = Pow(Ant2) */
 497                                                 if (p_test_bss_desc->bdTstamp > p_tmp_bss_desc->bdTstamp) { /* Stamp(Ant1) < Stamp(Ant2) */
 498                                                         PHYDM_DBG(dm, DBG_ANT_DIV, "time_diff: %lld\n", (p_test_bss_desc->bdTstamp - p_tmp_bss_desc->bdTstamp) / 1000);
 499                                                         if (p_test_bss_desc->bdTstamp - p_tmp_bss_desc->bdTstamp > 5000) {
 500                                                                 counter++;
 501                                                                 PHYDM_DBG(dm, DBG_ANT_DIV, "Original: %d, Test: %d\n", p_tmp_bss_desc->RecvSignalPower, p_test_bss_desc->RecvSignalPower);
 502                                                                 PHYDM_PRINT_ADDR(dm, DBG_ANT_DIV, "SSID:", p_tmp_bss_desc->bdSsIdBuf);
 503                                                                 PHYDM_PRINT_ADDR(dm, DBG_ANT_DIV, "BSSID:", p_tmp_bss_desc->bdSsIdBuf);
 504                                                                 min_power_diff = 0;
 505                                                         }
 506                                                 } else
 507                                                         PHYDM_DBG(dm, DBG_ANT_DIV, "[Error !!!]: Time_diff: %lld\n", (p_test_bss_desc->bdTstamp - p_tmp_bss_desc->bdTstamp) / 1000);
 508                                         }
 509                                 }
 510                         }
 511                 }
 512
 513                 if (dm->support_ic_type & (ODM_RTL8188E | ODM_RTL8821)) {
 514                         if (mgnt_info->NumBssDesc != 0 && score < 0) {
 515                                 PHYDM_DBG(dm, DBG_ANT_DIV,
 516                                           "%s: Using ant(%s)\n", __func__,
 517                                           (fat_tab->rx_idle_ant == MAIN_ANT) ?
 518                                           "MAIN_ANT" : "AUX_ANT");
 519                         } else {
 520                                 PHYDM_DBG(dm, DBG_ANT_DIV,
 521                                           "%s: Remain ant(%s)\n", __func__,
 522                                           (fat_tab->rx_idle_ant == MAIN_ANT) ?
 523                                           "AUX_ANT" : "MAIN_ANT");
 524
 525                                 if (fat_tab->rx_idle_ant == MAIN_ANT)
 526                                         odm_update_rx_idle_ant(dm, AUX_ANT);
 527                                 else
 528                                         odm_update_rx_idle_ant(dm, MAIN_ANT);
 529                         }
 530
 531                         if (IS_5G_WIRELESS_MODE(mgnt_info->dot11CurrentWirelessMode)) {
 532                                 dm_swat_table->ant_5g = fat_tab->rx_idle_ant;
 533                                 PHYDM_DBG(dm, DBG_ANT_DIV,
 534                                           "dm_swat_table->ant_5g=%s\n",
 535                                           (fat_tab->rx_idle_ant == MAIN_ANT) ?
 536                                           "MAIN_ANT" : "AUX_ANT");
 537                         } else {
 538                                 dm_swat_table->ant_2g = fat_tab->rx_idle_ant;
 539                                 PHYDM_DBG(dm, DBG_ANT_DIV,
 540                                           "dm_swat_table->ant_2g=%s\n",
 541                                           (fat_tab->rx_idle_ant == MAIN_ANT) ?
 542                                           "MAIN_ANT" : "AUX_ANT");
 543                         }
 544                 } else if (dm->support_ic_type == ODM_RTL8723B) {
 545                         if (counter == 0) {
 546                                 if (dm->dm_swat_table.pre_aux_fail_detec == false) {
 547                                         dm->dm_swat_table.pre_aux_fail_detec = true;
 548                                         dm->dm_swat_table.rssi_ant_dect_result = false;
 549                                         PHYDM_DBG(dm, DBG_ANT_DIV, "counter=(( 0 )) , [[ Cannot find any AP with Aux-ant ]] ->  Scan Target-channel again\n");
 550
 551                                         /* @3 [ Scan again ] */
 552                                         odm_sw_ant_div_construct_scan_chnl(adapter, scan_channel);
 553                                         PlatformSetTimer(adapter, &mgnt_info->ScanTimer, 5);
 554                                         return true;
 555                                 } else { /* pre_aux_fail_detec == true */
 556                                         /* @2 [ Single Antenna ] */
 557                                         dm->dm_swat_table.pre_aux_fail_detec = false;
 558                                         dm->dm_swat_table.rssi_ant_dect_result = true;
 559                                         PHYDM_DBG(dm, DBG_ANT_DIV, "counter=(( 0 )) , [[  Still cannot find any AP ]]\n");
 560                                         PHYDM_DBG(dm, DBG_ANT_DIV, "%s: Single antenna\n", __func__);
 561                                 }
 562                                 dm->dm_swat_table.aux_fail_detec_counter++;
 563                         } else {
 564                                 dm->dm_swat_table.pre_aux_fail_detec = false;
 565
 566                                 if (counter == 3) {
 567                                         avg_power_diff = ((power_diff - max_power_diff - min_power_diff) >> 1) + ((max_power_diff + min_power_diff) >> 2);
 568                                         PHYDM_DBG(dm, DBG_ANT_DIV, "counter: (( %d )) ,  power_diff: (( %d ))\n", counter, power_diff);
 569                                         PHYDM_DBG(dm, DBG_ANT_DIV, "[ counter==3 ] Modified avg_power_diff: (( %d )) , max_power_diff: (( %d )) ,  min_power_diff: (( %d ))\n", avg_power_diff, max_power_diff, min_power_diff);
 570                                 } else if (counter >= 4) {
 571                                         avg_power_diff = (power_diff - max_power_diff - min_power_diff) / (counter - 2);
 572                                         PHYDM_DBG(dm, DBG_ANT_DIV, "counter: (( %d )) ,  power_diff: (( %d ))\n", counter, power_diff);
 573                                         PHYDM_DBG(dm, DBG_ANT_DIV, "[ counter>=4 ] Modified avg_power_diff: (( %d )) , max_power_diff: (( %d )) ,  min_power_diff: (( %d ))\n", avg_power_diff, max_power_diff, min_power_diff);
 574
 575                                 } else { /* @counter==1,2 */
 576                                         avg_power_diff = power_diff / counter;
 577                                         PHYDM_DBG(dm, DBG_ANT_DIV, "avg_power_diff: (( %d )) , counter: (( %d )) ,  power_diff: (( %d ))\n", avg_power_diff, counter, power_diff);
 578                                 }
 579
 580                                 /* @2 [ Retry ] */
 581                                 if (avg_power_diff >= power_target_L && avg_power_diff <= power_target_H) {
 582                                         dm->dm_swat_table.retry_counter++;
 583
 584                                         if (dm->dm_swat_table.retry_counter <= 3) {
 585                                                 dm->dm_swat_table.rssi_ant_dect_result = false;
 586                                                 PHYDM_DBG(dm, DBG_ANT_DIV, "[[ Low confidence result ]] avg_power_diff= (( %d ))  ->  Scan Target-channel again ]]\n", avg_power_diff);
 587
 588                                                 /* @3 [ Scan again ] */
 589                                                 odm_sw_ant_div_construct_scan_chnl(adapter, scan_channel);
 590                                                 PlatformSetTimer(adapter, &mgnt_info->ScanTimer, 5);
 591                                                 return true;
 592                                         } else {
 593                                                 dm->dm_swat_table.rssi_ant_dect_result = true;
 594                                                 PHYDM_DBG(dm, DBG_ANT_DIV, "[[ Still Low confidence result ]]  (( retry_counter > 3 ))\n");
 595                                                 PHYDM_DBG(dm, DBG_ANT_DIV, "%s: Single antenna\n", __func__);
 596                                         }
 597                                 }
 598                                 /* @2 [ Dual Antenna ] */
 599                                 else if ((mgnt_info->NumBssDesc != 0) && (avg_power_diff < power_target_L)) {
 600                                         dm->dm_swat_table.rssi_ant_dect_result = true;
 601                                         if (dm->dm_swat_table.ANTB_ON == false) {
 602                                                 dm->dm_swat_table.ANTA_ON = true;
 603                                                 dm->dm_swat_table.ANTB_ON = true;
 604                                         }
 605                                         PHYDM_DBG(dm, DBG_ANT_DIV, "%s: Dual antenna\n", __func__);
 606                                         dm->dm_swat_table.dual_ant_counter++;
 607
 608                                         /* set bt coexDM from 1ant coexDM to 2ant coexDM */
 609                                         BT_SetBtCoexAntNum(adapter, BT_COEX_ANT_TYPE_DETECTED, 2);
 610
 611                                         /* @3 [ Init antenna diversity ] */
 612                                         dm->support_ability |= ODM_BB_ANT_DIV;
 613                                         odm_ant_div_init(dm);
 614                                 }
 615                                 /* @2 [ Single Antenna ] */
 616                                 else if (avg_power_diff > power_target_H) {
 617                                         dm->dm_swat_table.rssi_ant_dect_result = true;
 618                                         if (dm->dm_swat_table.ANTB_ON == true) {
 619                                                 dm->dm_swat_table.ANTA_ON = true;
 620                                                 dm->dm_swat_table.ANTB_ON = false;
 621 #if 0
 622                                                 /* @bt_set_bt_coex_ant_num(adapter, BT_COEX_ANT_TYPE_DETECTED, 1); */
 623 #endif
 624                                         }
 625                                         PHYDM_DBG(dm, DBG_ANT_DIV, "%s: Single antenna\n", __func__);
 626                                         dm->dm_swat_table.single_ant_counter++;
 627                                 }
 628                         }
 629 #if 0
 630                         /* PHYDM_DBG(dm,DBG_ANT_DIV, "is_result=(( %d ))\n",dm->dm_swat_table.rssi_ant_dect_result); */
 631 #endif
 632                         PHYDM_DBG(dm, DBG_ANT_DIV,
 633                                   "dual_ant_counter = (( %d )), single_ant_counter = (( %d )) , retry_counter = (( %d )) , aux_fail_detec_counter = (( %d ))\n\n\n",
 634                                   dm->dm_swat_table.dual_ant_counter,
 635                                   dm->dm_swat_table.single_ant_counter,
 636                                   dm->dm_swat_table.retry_counter,
 637                                   dm->dm_swat_table.aux_fail_detec_counter);
 638
 639                         /* @2 recover the antenna setting */
 640
 641                         if (dm->dm_swat_table.ANTB_ON == false)
 642                                 odm_set_bb_reg(dm, REG_S0_S1_PATH_SWITCH, 0xfff, (dm_swat_table->swas_no_link_bk_reg948));
 643
 644                         PHYDM_DBG(dm, DBG_ANT_DIV,
 645                                   "is_result=(( %d )), Recover  Reg[948]= (( %x ))\n\n",
 646                                   dm->dm_swat_table.rssi_ant_dect_result,
 647                                   dm_swat_table->swas_no_link_bk_reg948);
 648                 }
 649
 650                 /* @Check state reset to default and wait for next time. */
 651                 dm_swat_table->swas_no_link_state = 0;
 652                 mgnt_info->bScanAntDetect = false;
 653
 654                 return false;
 655         }
 656
 657 #else
 658         return false;
 659 #endif
 660
 661         return false;
 662 }
 663
 664 /* @1 [3. PSD method] ========================================================== */
 665 void odm_single_dual_antenna_detection_psd(
 666         void *dm_void)
 667 {
 668         struct dm_struct *dm = (struct dm_struct *)dm_void;
 669         u32 channel_ori;
 670         u8 initial_gain = 0x36;
 671         u8 tone_idx;
 672         u8 tone_lenth_1 = 7, tone_lenth_2 = 4;
 673         u16 tone_idx_1[7] = {88, 104, 120, 8, 24, 40, 56};
 674         u16 tone_idx_2[4] = {8, 24, 40, 56};
 675         u32 psd_report_main[11] = {0}, psd_report_aux[11] = {0};
 676         /* u8   tone_lenth_1=4, tone_lenth_2=2; */
 677         /* u16  tone_idx_1[4]={88, 120, 24, 56}; */
 678         /* u16  tone_idx_2[2]={ 24,  56}; */
 679         /* u32  psd_report_main[6]={0}, psd_report_aux[6]={0}; */
 680
 681         u32 PSD_report_temp, max_psd_report_main = 0, max_psd_report_aux = 0;
 682         u32 PSD_power_threshold;
 683         u32 main_psd_result = 0, aux_psd_result = 0;
 684         u32 regc50, reg948, regb2c, regc14, reg908;
 685         u32 i = 0, test_num = 8;
 686
 687         if (dm->support_ic_type != ODM_RTL8723B)
 688                 return;
 689
 690         PHYDM_DBG(dm, DBG_ANT_DIV, "%s============>\n", __func__);
 691
 692         /* @2 [ Backup Current RF/BB Settings ] */
 693
 694         channel_ori = odm_get_rf_reg(dm, RF_PATH_A, ODM_CHANNEL, RFREGOFFSETMASK);
 695         reg948 = odm_get_bb_reg(dm, REG_S0_S1_PATH_SWITCH, MASKDWORD);
 696         regb2c = odm_get_bb_reg(dm, REG_AGC_TABLE_SELECT, MASKDWORD);
 697         regc50 = odm_get_bb_reg(dm, REG_OFDM_0_XA_AGC_CORE1, MASKDWORD);
 698         regc14 = odm_get_bb_reg(dm, R_0xc14, MASKDWORD);
 699         reg908 = odm_get_bb_reg(dm, R_0x908, MASKDWORD);
 700
 701         /* @2 [ setting for doing PSD function (CH4)] */
 702         odm_set_bb_reg(dm, REG_FPGA0_RFMOD, BIT(24), 0); /* @disable whole CCK block */
 703         odm_write_1byte(dm, REG_TXPAUSE, 0xFF); /* Turn off TX  ->  Pause TX Queue */
 704         odm_set_bb_reg(dm, R_0xc14, MASKDWORD, 0x0); /* @[ Set IQK Matrix = 0 ] equivalent to [ Turn off CCA] */
 705
 706         /* PHYTXON while loop */
 707         odm_set_bb_reg(dm, R_0x908, MASKDWORD, 0x803);
 708         while (odm_get_bb_reg(dm, R_0xdf4, BIT(6))) {
 709                 i++;
 710                 if (i > 1000000) {
 711                         PHYDM_DBG(dm, DBG_ANT_DIV,
 712                                   "Wait in %s() more than %d times!\n",
 713                                   __FUNCTION__, i);
 714                         break;
 715                 }
 716         }
 717
 718         odm_set_bb_reg(dm, R_0xc50, 0x7f, initial_gain);
 719         odm_set_rf_reg(dm, RF_PATH_A, ODM_CHANNEL, 0x7ff, 0x04); /* Set RF to CH4 & 40M */
 720         odm_set_bb_reg(dm, REG_FPGA0_ANALOG_PARAMETER4, 0xf00000, 0xf); /* @3 wire Disable    88c[23:20]=0xf */
 721         odm_set_bb_reg(dm, REG_FPGA0_PSD_FUNCTION, BIT(14) | BIT15, 0x0); /* 128 pt      */ /* Set PSD 128 ptss */
 722         ODM_delay_us(3000);
 723
 724         /* @2 [ Doing PSD Function in (CH4)] */
 725
 726         /* @Antenna A */
 727         PHYDM_DBG(dm, DBG_ANT_DIV, "Switch to Main-ant   (CH4)\n");
 728         odm_set_bb_reg(dm, R_0x948, 0xfff, 0x200);
 729         ODM_delay_us(10);
 730         PHYDM_DBG(dm, DBG_ANT_DIV, "dbg\n");
 731         for (i = 0; i < test_num; i++) {
 732                 for (tone_idx = 0; tone_idx < tone_lenth_1; tone_idx++) {
 733                         PSD_report_temp = phydm_get_psd_data(dm, tone_idx_1[tone_idx], initial_gain);
 734                         /* @if(  PSD_report_temp>psd_report_main[tone_idx]  ) */
 735                         psd_report_main[tone_idx] += PSD_report_temp;
 736                 }
 737         }
 738         /* @Antenna B */
 739         PHYDM_DBG(dm, DBG_ANT_DIV, "Switch to Aux-ant   (CH4)\n");
 740         odm_set_bb_reg(dm, R_0x948, 0xfff, 0x280);
 741         ODM_delay_us(10);
 742         for (i = 0; i < test_num; i++) {
 743                 for (tone_idx = 0; tone_idx < tone_lenth_1; tone_idx++) {
 744                         PSD_report_temp = phydm_get_psd_data(dm, tone_idx_1[tone_idx], initial_gain);
 745                         /* @if(  PSD_report_temp>psd_report_aux[tone_idx]  ) */
 746                         psd_report_aux[tone_idx] += PSD_report_temp;
 747                 }
 748         }
 749         /* @2 [ Doing PSD Function in (CH8)] */
 750
 751         odm_set_bb_reg(dm, REG_FPGA0_ANALOG_PARAMETER4, 0xf00000, 0x0); /* @3 wire enable    88c[23:20]=0x0 */
 752         ODM_delay_us(3000);
 753
 754         odm_set_bb_reg(dm, R_0xc50, 0x7f, initial_gain);
 755         odm_set_rf_reg(dm, RF_PATH_A, ODM_CHANNEL, 0x7ff, 0x04); /* Set RF to CH8 & 40M */
 756
 757         odm_set_bb_reg(dm, REG_FPGA0_ANALOG_PARAMETER4, 0xf00000, 0xf); /* @3 wire Disable    88c[23:20]=0xf */
 758         ODM_delay_us(3000);
 759
 760         /* @Antenna A */
 761         PHYDM_DBG(dm, DBG_ANT_DIV, "Switch to Main-ant   (CH8)\n");
 762         odm_set_bb_reg(dm, R_0x948, 0xfff, 0x200);
 763         ODM_delay_us(10);
 764
 765         for (i = 0; i < test_num; i++) {
 766                 for (tone_idx = 0; tone_idx < tone_lenth_2; tone_idx++) {
 767                         PSD_report_temp = phydm_get_psd_data(dm, tone_idx_2[tone_idx], initial_gain);
 768                         /* @if(  PSD_report_temp>psd_report_main[tone_idx]  ) */
 769                         psd_report_main[tone_lenth_1 + tone_idx] += PSD_report_temp;
 770                 }
 771         }
 772
 773         /* @Antenna B */
 774         PHYDM_DBG(dm, DBG_ANT_DIV, "Switch to Aux-ant   (CH8)\n");
 775         odm_set_bb_reg(dm, R_0x948, 0xfff, 0x280);
 776         ODM_delay_us(10);
 777
 778         for (i = 0; i < test_num; i++) {
 779                 for (tone_idx = 0; tone_idx < tone_lenth_2; tone_idx++) {
 780                         PSD_report_temp = phydm_get_psd_data(dm, tone_idx_2[tone_idx], initial_gain);
 781                         /* @if(  PSD_report_temp>psd_report_aux[tone_idx]  ) */
 782                         psd_report_aux[tone_lenth_1 + tone_idx] += PSD_report_temp;
 783                 }
 784         }
 785
 786         /* @2 [ Calculate Result ] */
 787
 788         PHYDM_DBG(dm, DBG_ANT_DIV, "\nMain PSD Result: (ALL)\n");
 789         for (tone_idx = 0; tone_idx < (tone_lenth_1 + tone_lenth_2); tone_idx++) {
 790                 PHYDM_DBG(dm, DBG_ANT_DIV, "[Tone-%d]: %d,\n", (tone_idx + 1),
 791                           psd_report_main[tone_idx]);
 792                 main_psd_result += psd_report_main[tone_idx];
 793                 if (psd_report_main[tone_idx] > max_psd_report_main)
 794                         max_psd_report_main = psd_report_main[tone_idx];
 795         }
 796         PHYDM_DBG(dm, DBG_ANT_DIV,
 797                   "--------------------------- \nTotal_Main= (( %d ))\n",
 798                   main_psd_result);
 799         PHYDM_DBG(dm, DBG_ANT_DIV, "MAX_Main = (( %d ))\n",
 800                   max_psd_report_main);
 801
 802         PHYDM_DBG(dm, DBG_ANT_DIV, "\nAux PSD Result: (ALL)\n");
 803         for (tone_idx = 0; tone_idx < (tone_lenth_1 + tone_lenth_2); tone_idx++) {
 804                 PHYDM_DBG(dm, DBG_ANT_DIV, "[Tone-%d]: %d,\n", (tone_idx + 1),
 805                           psd_report_aux[tone_idx]);
 806                 aux_psd_result += psd_report_aux[tone_idx];
 807                 if (psd_report_aux[tone_idx] > max_psd_report_aux)
 808                         max_psd_report_aux = psd_report_aux[tone_idx];
 809         }
 810         PHYDM_DBG(dm, DBG_ANT_DIV,
 811                   "--------------------------- \nTotal_Aux= (( %d ))\n",
 812                   aux_psd_result);
 813         PHYDM_DBG(dm, DBG_ANT_DIV, "MAX_Aux = (( %d ))\n\n",
 814                   max_psd_report_aux);
 815
 816         /* @main_psd_result=main_psd_result-max_psd_report_main; */
 817         /* @aux_psd_result=aux_psd_result-max_psd_report_aux; */
 818         PSD_power_threshold = (main_psd_result * 7) >> 3;
 819
 820         PHYDM_DBG(dm, DBG_ANT_DIV,
 821                   "[ Main_result, Aux_result ] = [ %d , %d ], PSD_power_threshold=(( %d ))\n",
 822                   main_psd_result, aux_psd_result, PSD_power_threshold);
 823
 824         /* @3 [ Dual Antenna ] */
 825         if (aux_psd_result >= PSD_power_threshold) {
 826                 if (dm->dm_swat_table.ANTB_ON == false) {
 827                         dm->dm_swat_table.ANTA_ON = true;
 828                         dm->dm_swat_table.ANTB_ON = true;
 829                 }
 830                 PHYDM_DBG(dm, DBG_ANT_DIV,
 831                           "odm_sw_ant_div_check_before_link(): Dual antenna\n");
 832
 833 #if 0
 834                 /* set bt coexDM from 1ant coexDM to 2ant coexDM */
 835                 /* @bt_set_bt_coex_ant_num(adapter, BT_COEX_ANT_TYPE_DETECTED, 2); */
 836 #endif
 837
 838                 /* @Init antenna diversity */
 839                 dm->support_ability |= ODM_BB_ANT_DIV;
 840                 odm_ant_div_init(dm);
 841         }
 842         /* @3 [ Single Antenna ] */
 843         else {
 844                 if (dm->dm_swat_table.ANTB_ON == true) {
 845                         dm->dm_swat_table.ANTA_ON = true;
 846                         dm->dm_swat_table.ANTB_ON = false;
 847                 }
 848                 PHYDM_DBG(dm, DBG_ANT_DIV,
 849                           "odm_sw_ant_div_check_before_link(): Single antenna\n");
 850         }
 851
 852         /* @2 [ Recover all parameters ] */
 853
 854         odm_set_rf_reg(dm, RF_PATH_A, RF_CHNLBW, RFREGOFFSETMASK, channel_ori);
 855         odm_set_bb_reg(dm, REG_FPGA0_ANALOG_PARAMETER4, 0xf00000, 0x0); /* @3 wire enable    88c[23:20]=0x0 */
 856         odm_set_bb_reg(dm, R_0xc50, 0x7f, regc50);
 857
 858         odm_set_bb_reg(dm, REG_S0_S1_PATH_SWITCH, MASKDWORD, reg948);
 859         odm_set_bb_reg(dm, REG_AGC_TABLE_SELECT, MASKDWORD, regb2c);
 860
 861         odm_set_bb_reg(dm, REG_FPGA0_RFMOD, BIT(24), 1); /* @enable whole CCK block */
 862         odm_write_1byte(dm, REG_TXPAUSE, 0x0); /* Turn on TX     */ /* Resume TX Queue */
 863         odm_set_bb_reg(dm, R_0xc14, MASKDWORD, regc14); /* @[ Set IQK Matrix = 0 ] equivalent to [ Turn on CCA] */
 864         odm_set_bb_reg(dm, R_0x908, MASKDWORD, reg908);
 865
 866         return;
 867 }
 868
 869 void odm_sw_ant_detect_init(void *dm_void)
 870 {
 871 #if (RTL8723B_SUPPORT == 1)
 872
 873         struct dm_struct *dm = (struct dm_struct *)dm_void;
 874         struct sw_antenna_switch *dm_swat_table = &dm->dm_swat_table;
 875
 876         if (dm->support_ic_type != ODM_RTL8723B)
 877                 return;
 878
 879         /* @dm_swat_table->pre_antenna = MAIN_ANT; */
 880         /* @dm_swat_table->cur_antenna = MAIN_ANT; */
 881         dm_swat_table->swas_no_link_state = 0;
 882         dm_swat_table->pre_aux_fail_detec = false;
 883         dm_swat_table->swas_no_link_bk_reg948 = 0xff;
 884
 885 #ifdef CONFIG_PSD_TOOL
 886         phydm_psd_init(dm);
 887 #endif
 888 #endif
 889 }
 890 #endif