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         struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
  13         struct cfo_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
  30 static u8 odm_GetDefaultCrytaltalCap(void *pDM_VOID)
  31 {
  32         struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
  33
  34         struct adapter *Adapter = pDM_Odm->Adapter;
  35         struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
  36
  37         return pHalData->CrystalCap & 0x3f;
  38 }
  39
  40 static void odm_SetATCStatus(void *pDM_VOID, bool ATCStatus)
  41 {
  42         struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
  43         struct cfo_tracking *pCfoTrack = &pDM_Odm->DM_CfoTrack;
  44
  45         if (pCfoTrack->bATCStatus == ATCStatus)
  46                 return;
  47
  48         PHY_SetBBReg(
  49                 pDM_Odm->Adapter,
  50                 ODM_REG(BB_ATC, pDM_Odm),
  51                 ODM_BIT(BB_ATC, pDM_Odm),
  52                 ATCStatus
  53         );
  54         pCfoTrack->bATCStatus = ATCStatus;
  55 }
  56
  57 static bool odm_GetATCStatus(void *pDM_VOID)
  58 {
  59         bool ATCStatus;
  60         struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
  61
  62         ATCStatus = (bool)PHY_QueryBBReg(
  63                 pDM_Odm->Adapter,
  64                 ODM_REG(BB_ATC, pDM_Odm),
  65                 ODM_BIT(BB_ATC, pDM_Odm)
  66         );
  67         return ATCStatus;
  68 }
  69
  70 void ODM_CfoTrackingReset(void *pDM_VOID)
  71 {
  72         struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
  73         struct cfo_tracking *pCfoTrack = &pDM_Odm->DM_CfoTrack;
  74
  75         pCfoTrack->DefXCap = odm_GetDefaultCrytaltalCap(pDM_Odm);
  76         pCfoTrack->bAdjust = true;
  77
  78         odm_SetCrystalCap(pDM_Odm, pCfoTrack->DefXCap);
  79         odm_SetATCStatus(pDM_Odm, true);
  80 }
  81
  82 void ODM_CfoTrackingInit(void *pDM_VOID)
  83 {
  84         struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
  85         struct cfo_tracking *pCfoTrack = &pDM_Odm->DM_CfoTrack;
  86
  87         pCfoTrack->DefXCap =
  88                 pCfoTrack->CrystalCap = odm_GetDefaultCrytaltalCap(pDM_Odm);
  89         pCfoTrack->bATCStatus = odm_GetATCStatus(pDM_Odm);
  90         pCfoTrack->bAdjust = true;
  91 }
  92
  93 void ODM_CfoTracking(void *pDM_VOID)
  94 {
  95         struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
  96         struct cfo_tracking *pCfoTrack = &pDM_Odm->DM_CfoTrack;
  97         int CFO_kHz_A, CFO_ave = 0;
  98         int CFO_ave_diff;
  99         int CrystalCap = (int)pCfoTrack->CrystalCap;
 100         u8 Adjust_Xtal = 1;
 101
 102         /* 4 Support ability */
 103         if (!(pDM_Odm->SupportAbility & ODM_BB_CFO_TRACKING)) {
 104                 return;
 105         }
 106
 107         if (!pDM_Odm->bLinked || !pDM_Odm->bOneEntryOnly) {
 108                 /* 4 No link or more than one entry */
 109                 ODM_CfoTrackingReset(pDM_Odm);
 110         } else {
 111                 /* 3 1. CFO Tracking */
 112                 /* 4 1.1 No new packet */
 113                 if (pCfoTrack->packetCount == pCfoTrack->packetCount_pre) {
 114                         return;
 115                 }
 116                 pCfoTrack->packetCount_pre = pCfoTrack->packetCount;
 117
 118                 /* 4 1.2 Calculate CFO */
 119                 CFO_kHz_A =  (int)(pCfoTrack->CFO_tail[0] * 3125)  / 1280;
 120
 121                 CFO_ave = CFO_kHz_A;
 122
 123                 /* 4 1.3 Avoid abnormal large CFO */
 124                 CFO_ave_diff =
 125                         (pCfoTrack->CFO_ave_pre >= CFO_ave) ?
 126                         (pCfoTrack->CFO_ave_pre-CFO_ave) :
 127                         (CFO_ave-pCfoTrack->CFO_ave_pre);
 128
 129                 if (
 130                         CFO_ave_diff > 20 &&
 131                         pCfoTrack->largeCFOHit == 0 &&
 132                         !pCfoTrack->bAdjust
 133                 ) {
 134                         pCfoTrack->largeCFOHit = 1;
 135                         return;
 136                 } else
 137                         pCfoTrack->largeCFOHit = 0;
 138                 pCfoTrack->CFO_ave_pre = CFO_ave;
 139
 140                 /* 4 1.4 Dynamic Xtal threshold */
 141                 if (pCfoTrack->bAdjust == false) {
 142                         if (CFO_ave > CFO_TH_XTAL_HIGH || CFO_ave < (-CFO_TH_XTAL_HIGH))
 143                                 pCfoTrack->bAdjust = true;
 144                 } else {
 145                         if (CFO_ave < CFO_TH_XTAL_LOW && CFO_ave > (-CFO_TH_XTAL_LOW))
 146                                 pCfoTrack->bAdjust = false;
 147                 }
 148
 149                 /* 4 1.5 BT case: Disable CFO tracking */
 150                 if (pDM_Odm->bBtEnabled) {
 151                         pCfoTrack->bAdjust = false;
 152                         odm_SetCrystalCap(pDM_Odm, pCfoTrack->DefXCap);
 153                 }
 154
 155                 /* 4 1.6 Big jump */
 156                 if (pCfoTrack->bAdjust) {
 157                         if (CFO_ave > CFO_TH_XTAL_LOW)
 158                                 Adjust_Xtal = Adjust_Xtal+((CFO_ave-CFO_TH_XTAL_LOW)>>2);
 159                         else if (CFO_ave < (-CFO_TH_XTAL_LOW))
 160                                 Adjust_Xtal = Adjust_Xtal+((CFO_TH_XTAL_LOW-CFO_ave)>>2);
 161                 }
 162
 163                 /* 4 1.7 Adjust Crystal Cap. */
 164                 if (pCfoTrack->bAdjust) {
 165                         if (CFO_ave > CFO_TH_XTAL_LOW)
 166                                 CrystalCap = CrystalCap + Adjust_Xtal;
 167                         else if (CFO_ave < (-CFO_TH_XTAL_LOW))
 168                                 CrystalCap = CrystalCap - Adjust_Xtal;
 169
 170                         if (CrystalCap > 0x3f)
 171                                 CrystalCap = 0x3f;
 172                         else if (CrystalCap < 0)
 173                                 CrystalCap = 0;
 174
 175                         odm_SetCrystalCap(pDM_Odm, (u8)CrystalCap);
 176                 }
 177
 178                 /* 3 2. Dynamic ATC switch */
 179                 if (CFO_ave < CFO_TH_ATC && CFO_ave > -CFO_TH_ATC) {
 180                         odm_SetATCStatus(pDM_Odm, false);
 181                 } else {
 182                         odm_SetATCStatus(pDM_Odm, true);
 183                 }
 184         }
 185 }
 186
 187 void odm_parsing_cfo(void *dm_void, void *pkt_info_void, s8 *cfotail)
 188 {
 189         struct dm_odm_t *dm_odm = (struct dm_odm_t *)dm_void;
 190         struct odm_packet_info *pkt_info = pkt_info_void;
 191         struct cfo_tracking *cfo_track = &dm_odm->DM_CfoTrack;
 192         u8 i;
 193
 194         if (!(dm_odm->SupportAbility & ODM_BB_CFO_TRACKING))
 195                 return;
 196
 197         if (pkt_info->station_id != 0) {
 198                 /*
 199                  * 3 Update CFO report for path-A & path-B
 200                  * Only paht-A and path-B have CFO tail and short CFO
 201                  */
 202                 for (i = RF_PATH_A; i <= RF_PATH_B; i++)
 203                         cfo_track->CFO_tail[i] = (int)cfotail[i];
 204
 205                 /* 3 Update packet counter */
 206                 if (cfo_track->packetCount == 0xffffffff)
 207                         cfo_track->packetCount = 0;
 208                 else
 209                         cfo_track->packetCount++;
 210         }
 211 }