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
 10static 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
 30static 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
 40static 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
 57static 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
 70void 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
 82void 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
 93void 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
187void 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}