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 } 212