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_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 | |
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_VOID: pDM_Odm); |
89 | pCfoTrack->bATCStatus = odm_GetATCStatus(pDM_VOID: 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_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 | |
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 | |