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_VOID: pDM_Odm);
76 pCfoTrack->bAdjust = true;
77
78 odm_SetCrystalCap(pDM_VOID: pDM_Odm, CrystalCap: pCfoTrack->DefXCap);
79 odm_SetATCStatus(pDM_VOID: pDM_Odm, ATCStatus: 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_VOID: pDM_Odm);
89 pCfoTrack->bATCStatus = odm_GetATCStatus(pDM_VOID: 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_VOID: 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_VOID: pDM_Odm, CrystalCap: 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_VOID: pDM_Odm, CrystalCap: (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_VOID: pDM_Odm, ATCStatus: false);
181 } else {
182 odm_SetATCStatus(pDM_VOID: pDM_Odm, ATCStatus: 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}
212

source code of linux/drivers/staging/rtl8723bs/hal/odm_CfoTracking.c