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