drivers/staging/rtl8723bs/hal/odm_CfoTracking.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /******************************************************************************
   3  *
   4  * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
   5  *
   6  ******************************************************************************/
   7
   8 #include "odm_precomp.h"
   9
  10 static void odm_SetCrystalCap(void *pDM_VOID, u8 CrystalCap)
  11 {
  12         PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
  13         PCFO_TRACKING pCfoTrack = &pDM_Odm->DM_CfoTrack;
  14         bool bEEPROMCheck;
  15         struct adapter *Adapter = pDM_Odm->Adapter;
  16         struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
  17
  18         bEEPROMCheck = pHalData->EEPROMVersion >= 0x01;
  19
  20         if (pCfoTrack->CrystalCap == CrystalCap)
  21                 return;
  22
  23         pCfoTrack->CrystalCap = CrystalCap;
  24
  25         /*  0x2C[23:18] = 0x2C[17:12] = CrystalCap */
  26         CrystalCap = CrystalCap & 0x3F;
  27         PHY_SetBBReg(
  28                 pDM_Odm->Adapter,
  29                 REG_MAC_PHY_CTRL,
  30                 0x00FFF000,
  31                 (CrystalCap | (CrystalCap << 6))
  32         );
  33
  34         ODM_RT_TRACE(
  35                 pDM_Odm,
  36                 ODM_COMP_CFO_TRACKING,
  37                 ODM_DBG_LOUD,
  38                 (
  39                         "odm_SetCrystalCap(): CrystalCap = 0x%x\n",
  40                         CrystalCap
  41                 )
  42         );
  43 }
  44
  45 static u8 odm_GetDefaultCrytaltalCap(void *pDM_VOID)
  46 {
  47         PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
  48         u8 CrystalCap = 0x20;
  49
  50         struct adapter *Adapter = pDM_Odm->Adapter;
  51         struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
  52
  53         CrystalCap = pHalData->CrystalCap;
  54
  55         CrystalCap = CrystalCap & 0x3f;
  56
  57         return CrystalCap;
  58 }
  59
  60 static void odm_SetATCStatus(void *pDM_VOID, bool ATCStatus)
  61 {
  62         PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
  63         PCFO_TRACKING pCfoTrack = &pDM_Odm->DM_CfoTrack;
  64
  65         if (pCfoTrack->bATCStatus == ATCStatus)
  66                 return;
  67
  68         PHY_SetBBReg(
  69                 pDM_Odm->Adapter,
  70                 ODM_REG(BB_ATC, pDM_Odm),
  71                 ODM_BIT(BB_ATC, pDM_Odm),
  72                 ATCStatus
  73         );
  74         pCfoTrack->bATCStatus = ATCStatus;
  75 }
  76
  77 static bool odm_GetATCStatus(void *pDM_VOID)
  78 {
  79         bool ATCStatus;
  80         PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
  81
  82         ATCStatus = (bool)PHY_QueryBBReg(
  83                 pDM_Odm->Adapter,
  84                 ODM_REG(BB_ATC, pDM_Odm),
  85                 ODM_BIT(BB_ATC, pDM_Odm)
  86         );
  87         return ATCStatus;
  88 }
  89
  90 void ODM_CfoTrackingReset(void *pDM_VOID)
  91 {
  92         PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
  93         PCFO_TRACKING pCfoTrack = &pDM_Odm->DM_CfoTrack;
  94
  95         pCfoTrack->DefXCap = odm_GetDefaultCrytaltalCap(pDM_Odm);
  96         pCfoTrack->bAdjust = true;
  97
  98         odm_SetCrystalCap(pDM_Odm, pCfoTrack->DefXCap);
  99         odm_SetATCStatus(pDM_Odm, true);
 100 }
 101
 102 void ODM_CfoTrackingInit(void *pDM_VOID)
 103 {
 104         PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
 105         PCFO_TRACKING pCfoTrack = &pDM_Odm->DM_CfoTrack;
 106
 107         pCfoTrack->DefXCap =
 108                 pCfoTrack->CrystalCap = odm_GetDefaultCrytaltalCap(pDM_Odm);
 109         pCfoTrack->bATCStatus = odm_GetATCStatus(pDM_Odm);
 110         pCfoTrack->bAdjust = true;
 111         ODM_RT_TRACE(
 112                 pDM_Odm,
 113                 ODM_COMP_CFO_TRACKING,
 114                 ODM_DBG_LOUD,
 115                 ("ODM_CfoTracking_init() =========>\n")
 116         );
 117         ODM_RT_TRACE(
 118                 pDM_Odm,
 119                 ODM_COMP_CFO_TRACKING,
 120                 ODM_DBG_LOUD,
 121                 (
 122                         "ODM_CfoTracking_init(): bATCStatus = %d, CrystalCap = 0x%x\n",
 123                         pCfoTrack->bATCStatus,
 124                         pCfoTrack->DefXCap
 125                 )
 126         );
 127 }
 128
 129 void ODM_CfoTracking(void *pDM_VOID)
 130 {
 131         PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
 132         PCFO_TRACKING pCfoTrack = &pDM_Odm->DM_CfoTrack;
 133         int CFO_kHz_A, CFO_kHz_B, CFO_ave = 0;
 134         int CFO_ave_diff;
 135         int CrystalCap = (int)pCfoTrack->CrystalCap;
 136         u8 Adjust_Xtal = 1;
 137
 138         /* 4 Support ability */
 139         if (!(pDM_Odm->SupportAbility & ODM_BB_CFO_TRACKING)) {
 140                 ODM_RT_TRACE(
 141                         pDM_Odm,
 142                         ODM_COMP_CFO_TRACKING,
 143                         ODM_DBG_LOUD,
 144                         ("ODM_CfoTracking(): Return: SupportAbility ODM_BB_CFO_TRACKING is disabled\n")
 145                 );
 146                 return;
 147         }
 148
 149         ODM_RT_TRACE(
 150                 pDM_Odm,
 151                 ODM_COMP_CFO_TRACKING,
 152                 ODM_DBG_LOUD,
 153                 ("ODM_CfoTracking() =========>\n")
 154         );
 155
 156         if (!pDM_Odm->bLinked || !pDM_Odm->bOneEntryOnly) {
 157                 /* 4 No link or more than one entry */
 158                 ODM_CfoTrackingReset(pDM_Odm);
 159                 ODM_RT_TRACE(
 160                         pDM_Odm,
 161                         ODM_COMP_CFO_TRACKING,
 162                         ODM_DBG_LOUD,
 163                         (
 164                                 "ODM_CfoTracking(): Reset: bLinked = %d, bOneEntryOnly = %d\n",
 165                                 pDM_Odm->bLinked,
 166                                 pDM_Odm->bOneEntryOnly
 167                         )
 168                 );
 169         } else {
 170                 /* 3 1. CFO Tracking */
 171                 /* 4 1.1 No new packet */
 172                 if (pCfoTrack->packetCount == pCfoTrack->packetCount_pre) {
 173                         ODM_RT_TRACE(
 174                                 pDM_Odm,
 175                                 ODM_COMP_CFO_TRACKING,
 176                                 ODM_DBG_LOUD,
 177                                 (
 178                                         "ODM_CfoTracking(): packet counter doesn't change\n"
 179                                 )
 180                         );
 181                         return;
 182                 }
 183                 pCfoTrack->packetCount_pre = pCfoTrack->packetCount;
 184
 185                 /* 4 1.2 Calculate CFO */
 186                 CFO_kHz_A =  (int)(pCfoTrack->CFO_tail[0] * 3125)  / 1280;
 187                 CFO_kHz_B =  (int)(pCfoTrack->CFO_tail[1] * 3125)  / 1280;
 188
 189                 if (pDM_Odm->RFType < ODM_2T2R)
 190                         CFO_ave = CFO_kHz_A;
 191                 else
 192                         CFO_ave = (int)(CFO_kHz_A + CFO_kHz_B) >> 1;
 193                 ODM_RT_TRACE(
 194                         pDM_Odm,
 195                         ODM_COMP_CFO_TRACKING,
 196                         ODM_DBG_LOUD,
 197                         (
 198                                 "ODM_CfoTracking(): CFO_kHz_A = %dkHz, CFO_kHz_B = %dkHz, CFO_ave = %dkHz\n",
 199                                 CFO_kHz_A,
 200                                 CFO_kHz_B,
 201                                 CFO_ave
 202                         )
 203                 );
 204
 205                 /* 4 1.3 Avoid abnormal large CFO */
 206                 CFO_ave_diff =
 207                         (pCfoTrack->CFO_ave_pre >= CFO_ave) ?
 208                         (pCfoTrack->CFO_ave_pre-CFO_ave) :
 209                         (CFO_ave-pCfoTrack->CFO_ave_pre);
 210
 211                 if (
 212                         CFO_ave_diff > 20 &&
 213                         pCfoTrack->largeCFOHit == 0 &&
 214                         !pCfoTrack->bAdjust
 215                 ) {
 216                         ODM_RT_TRACE(pDM_Odm, ODM_COMP_CFO_TRACKING, ODM_DBG_LOUD, ("ODM_CfoTracking(): first large CFO hit\n"));
 217                         pCfoTrack->largeCFOHit = 1;
 218                         return;
 219                 } else
 220                         pCfoTrack->largeCFOHit = 0;
 221                 pCfoTrack->CFO_ave_pre = CFO_ave;
 222
 223                 /* 4 1.4 Dynamic Xtal threshold */
 224                 if (pCfoTrack->bAdjust == false) {
 225                         if (CFO_ave > CFO_TH_XTAL_HIGH || CFO_ave < (-CFO_TH_XTAL_HIGH))
 226                                 pCfoTrack->bAdjust = true;
 227                 } else {
 228                         if (CFO_ave < CFO_TH_XTAL_LOW && CFO_ave > (-CFO_TH_XTAL_LOW))
 229                                 pCfoTrack->bAdjust = false;
 230                 }
 231
 232                 /* 4 1.5 BT case: Disable CFO tracking */
 233                 if (pDM_Odm->bBtEnabled) {
 234                         pCfoTrack->bAdjust = false;
 235                         odm_SetCrystalCap(pDM_Odm, pCfoTrack->DefXCap);
 236                         ODM_RT_TRACE(
 237                                 pDM_Odm,
 238                                 ODM_COMP_CFO_TRACKING,
 239                                 ODM_DBG_LOUD,
 240                                 ("ODM_CfoTracking(): Disable CFO tracking for BT!!\n")
 241                         );
 242                 }
 243
 244                 /* 4 1.6 Big jump */
 245                 if (pCfoTrack->bAdjust) {
 246                         if (CFO_ave > CFO_TH_XTAL_LOW)
 247                                 Adjust_Xtal = Adjust_Xtal+((CFO_ave-CFO_TH_XTAL_LOW)>>2);
 248                         else if (CFO_ave < (-CFO_TH_XTAL_LOW))
 249                                 Adjust_Xtal = Adjust_Xtal+((CFO_TH_XTAL_LOW-CFO_ave)>>2);
 250
 251                         ODM_RT_TRACE(
 252                                 pDM_Odm,
 253                                 ODM_COMP_CFO_TRACKING,
 254                                 ODM_DBG_LOUD,
 255                                 (
 256                                         "ODM_CfoTracking(): Crystal cap offset = %d\n",
 257                                         Adjust_Xtal
 258                                 )
 259                         );
 260                 }
 261
 262                 /* 4 1.7 Adjust Crystal Cap. */
 263                 if (pCfoTrack->bAdjust) {
 264                         if (CFO_ave > CFO_TH_XTAL_LOW)
 265                                 CrystalCap = CrystalCap + Adjust_Xtal;
 266                         else if (CFO_ave < (-CFO_TH_XTAL_LOW))
 267                                 CrystalCap = CrystalCap - Adjust_Xtal;
 268
 269                         if (CrystalCap > 0x3f)
 270                                 CrystalCap = 0x3f;
 271                         else if (CrystalCap < 0)
 272                                 CrystalCap = 0;
 273
 274                         odm_SetCrystalCap(pDM_Odm, (u8)CrystalCap);
 275                 }
 276                 ODM_RT_TRACE(
 277                         pDM_Odm,
 278                         ODM_COMP_CFO_TRACKING,
 279                         ODM_DBG_LOUD,
 280                         (
 281                                 "ODM_CfoTracking(): Crystal cap = 0x%x, Default Crystal cap = 0x%x\n",
 282                                 pCfoTrack->CrystalCap,
 283                                 pCfoTrack->DefXCap
 284                         )
 285                 );
 286
 287                 /* 3 2. Dynamic ATC switch */
 288                 if (CFO_ave < CFO_TH_ATC && CFO_ave > -CFO_TH_ATC) {
 289                         odm_SetATCStatus(pDM_Odm, false);
 290                         ODM_RT_TRACE(
 291                                 pDM_Odm,
 292                                 ODM_COMP_CFO_TRACKING,
 293                                 ODM_DBG_LOUD,
 294                                 ("ODM_CfoTracking(): Disable ATC!!\n")
 295                         );
 296                 } else {
 297                         odm_SetATCStatus(pDM_Odm, true);
 298                         ODM_RT_TRACE(
 299                                 pDM_Odm,
 300                                 ODM_COMP_CFO_TRACKING,
 301                                 ODM_DBG_LOUD,
 302                                 ("ODM_CfoTracking(): Enable ATC!!\n")
 303                         );
 304                 }
 305         }
 306 }
 307
 308 void ODM_ParsingCFO(void *pDM_VOID, void *pPktinfo_VOID, s8 *pcfotail)
 309 {
 310         PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
 311         struct odm_packet_info *pPktinfo = (struct odm_packet_info *)pPktinfo_VOID;
 312         PCFO_TRACKING pCfoTrack = &pDM_Odm->DM_CfoTrack;
 313         u8 i;
 314
 315         if (!(pDM_Odm->SupportAbility & ODM_BB_CFO_TRACKING))
 316                 return;
 317
 318         if (pPktinfo->station_id != 0) {
 319                 /* 3 Update CFO report for path-A & path-B */
 320                 /*  Only paht-A and path-B have CFO tail and short CFO */
 321                 for (i = ODM_RF_PATH_A; i <= ODM_RF_PATH_B; i++)
 322                         pCfoTrack->CFO_tail[i] = (int)pcfotail[i];
 323
 324                 /* 3 Update packet counter */
 325                 if (pCfoTrack->packetCount == 0xffffffff)
 326                         pCfoTrack->packetCount = 0;
 327                 else
 328                         pCfoTrack->packetCount++;
 329         }
 330 }