1 // SPDX-License-Identifier: GPL-2.0
2 /* Copyright(c) 2009-2012  Realtek Corporation.*/
3 
4 #include "../wifi.h"
5 #include "../pci.h"
6 #include "../base.h"
7 #include "../core.h"
8 #include "reg.h"
9 #include "def.h"
10 #include "fw.h"
11 #include "../rtl8723com/fw_common.h"
12 
13 static bool _rtl8723e_check_fw_read_last_h2c(struct ieee80211_hw *hw,
14 					     u8 boxnum)
15 {
16 	struct rtl_priv *rtlpriv = rtl_priv(hw);
17 	u8 val_hmetfr, val_mcutst_1;
18 	bool result = false;
19 
20 	val_hmetfr = rtl_read_byte(rtlpriv, REG_HMETFR);
21 	val_mcutst_1 = rtl_read_byte(rtlpriv, (REG_MCUTST_1 + boxnum));
22 
23 	if (((val_hmetfr >> boxnum) & BIT(0)) == 0 && val_mcutst_1 == 0)
24 		result = true;
25 	return result;
26 }
27 
28 static void _rtl8723e_fill_h2c_command(struct ieee80211_hw *hw, u8 element_id,
29 				       u32 cmd_len, u8 *cmdbuffer)
30 {
31 	struct rtl_priv *rtlpriv = rtl_priv(hw);
32 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
33 	u8 boxnum;
34 	u16 box_reg = 0, box_extreg = 0;
35 	u8 u1b_tmp;
36 	bool isfw_read = false;
37 	u8 buf_index = 0;
38 	bool bwrite_sucess = false;
39 	u8 wait_h2c_limmit = 100;
40 	u8 wait_writeh2c_limmit = 100;
41 	u8 boxcontent[4], boxextcontent[2];
42 	u32 h2c_waitcounter = 0;
43 	unsigned long flag;
44 	u8 idx;
45 
46 	RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD, "come in\n");
47 
48 	while (true) {
49 		spin_lock_irqsave(&rtlpriv->locks.h2c_lock, flag);
50 		if (rtlhal->h2c_setinprogress) {
51 			RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
52 				 "H2C set in progress! Wait to set..element_id(%d).\n",
53 				 element_id);
54 
55 			while (rtlhal->h2c_setinprogress) {
56 				spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock,
57 						       flag);
58 				h2c_waitcounter++;
59 				RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
60 					 "Wait 100 us (%d times)...\n",
61 					  h2c_waitcounter);
62 				udelay(100);
63 
64 				if (h2c_waitcounter > 1000)
65 					return;
66 				spin_lock_irqsave(&rtlpriv->locks.h2c_lock,
67 						  flag);
68 			}
69 			spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
70 		} else {
71 			rtlhal->h2c_setinprogress = true;
72 			spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
73 			break;
74 		}
75 	}
76 
77 	while (!bwrite_sucess) {
78 		wait_writeh2c_limmit--;
79 		if (wait_writeh2c_limmit == 0) {
80 			pr_err("Write H2C fail because no trigger for FW INT!\n");
81 			break;
82 		}
83 
84 		boxnum = rtlhal->last_hmeboxnum;
85 		switch (boxnum) {
86 		case 0:
87 			box_reg = REG_HMEBOX_0;
88 			box_extreg = REG_HMEBOX_EXT_0;
89 			break;
90 		case 1:
91 			box_reg = REG_HMEBOX_1;
92 			box_extreg = REG_HMEBOX_EXT_1;
93 			break;
94 		case 2:
95 			box_reg = REG_HMEBOX_2;
96 			box_extreg = REG_HMEBOX_EXT_2;
97 			break;
98 		case 3:
99 			box_reg = REG_HMEBOX_3;
100 			box_extreg = REG_HMEBOX_EXT_3;
101 			break;
102 		default:
103 			pr_err("switch case %#x not processed\n",
104 			       boxnum);
105 			break;
106 		}
107 
108 		isfw_read = _rtl8723e_check_fw_read_last_h2c(hw, boxnum);
109 		while (!isfw_read) {
110 
111 			wait_h2c_limmit--;
112 			if (wait_h2c_limmit == 0) {
113 				RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
114 					 "Waiting too long for FW read clear HMEBox(%d)!\n",
115 					 boxnum);
116 				break;
117 			}
118 
119 			udelay(10);
120 
121 			isfw_read = _rtl8723e_check_fw_read_last_h2c(hw,
122 								boxnum);
123 			u1b_tmp = rtl_read_byte(rtlpriv, 0x1BF);
124 			RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
125 				 "Waiting for FW read clear HMEBox(%d)!!! 0x1BF = %2x\n",
126 				 boxnum, u1b_tmp);
127 		}
128 
129 		if (!isfw_read) {
130 			RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
131 				 "Write H2C register BOX[%d] fail!!!!! Fw do not read.\n",
132 				 boxnum);
133 			break;
134 		}
135 
136 		memset(boxcontent, 0, sizeof(boxcontent));
137 		memset(boxextcontent, 0, sizeof(boxextcontent));
138 		boxcontent[0] = element_id;
139 		RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
140 			 "Write element_id box_reg(%4x) = %2x\n",
141 			  box_reg, element_id);
142 
143 		switch (cmd_len) {
144 		case 1:
145 			boxcontent[0] &= ~(BIT(7));
146 			memcpy((u8 *)(boxcontent) + 1,
147 			       cmdbuffer + buf_index, 1);
148 
149 			for (idx = 0; idx < 4; idx++) {
150 				rtl_write_byte(rtlpriv, box_reg + idx,
151 					       boxcontent[idx]);
152 			}
153 			break;
154 		case 2:
155 			boxcontent[0] &= ~(BIT(7));
156 			memcpy((u8 *)(boxcontent) + 1,
157 			       cmdbuffer + buf_index, 2);
158 
159 			for (idx = 0; idx < 4; idx++) {
160 				rtl_write_byte(rtlpriv, box_reg + idx,
161 					       boxcontent[idx]);
162 			}
163 			break;
164 		case 3:
165 			boxcontent[0] &= ~(BIT(7));
166 			memcpy((u8 *)(boxcontent) + 1,
167 			       cmdbuffer + buf_index, 3);
168 
169 			for (idx = 0; idx < 4; idx++) {
170 				rtl_write_byte(rtlpriv, box_reg + idx,
171 					       boxcontent[idx]);
172 			}
173 			break;
174 		case 4:
175 			boxcontent[0] |= (BIT(7));
176 			memcpy((u8 *)(boxextcontent),
177 			       cmdbuffer + buf_index, 2);
178 			memcpy((u8 *)(boxcontent) + 1,
179 			       cmdbuffer + buf_index + 2, 2);
180 
181 			for (idx = 0; idx < 2; idx++) {
182 				rtl_write_byte(rtlpriv, box_extreg + idx,
183 					       boxextcontent[idx]);
184 			}
185 
186 			for (idx = 0; idx < 4; idx++) {
187 				rtl_write_byte(rtlpriv, box_reg + idx,
188 					       boxcontent[idx]);
189 			}
190 			break;
191 		case 5:
192 			boxcontent[0] |= (BIT(7));
193 			memcpy((u8 *)(boxextcontent),
194 			       cmdbuffer + buf_index, 2);
195 			memcpy((u8 *)(boxcontent) + 1,
196 			       cmdbuffer + buf_index + 2, 3);
197 
198 			for (idx = 0; idx < 2; idx++) {
199 				rtl_write_byte(rtlpriv, box_extreg + idx,
200 					       boxextcontent[idx]);
201 			}
202 
203 			for (idx = 0; idx < 4; idx++) {
204 				rtl_write_byte(rtlpriv, box_reg + idx,
205 					       boxcontent[idx]);
206 			}
207 			break;
208 		default:
209 			pr_err("switch case %#x not processed\n",
210 			       cmd_len);
211 			break;
212 		}
213 
214 		bwrite_sucess = true;
215 
216 		rtlhal->last_hmeboxnum = boxnum + 1;
217 		if (rtlhal->last_hmeboxnum == 4)
218 			rtlhal->last_hmeboxnum = 0;
219 
220 		RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
221 			 "pHalData->last_hmeboxnum  = %d\n",
222 			  rtlhal->last_hmeboxnum);
223 	}
224 
225 	spin_lock_irqsave(&rtlpriv->locks.h2c_lock, flag);
226 	rtlhal->h2c_setinprogress = false;
227 	spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
228 
229 	RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD, "go out\n");
230 }
231 
232 void rtl8723e_fill_h2c_cmd(struct ieee80211_hw *hw,
233 			   u8 element_id, u32 cmd_len, u8 *cmdbuffer)
234 {
235 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
236 	u32 tmp_cmdbuf[2];
237 
238 	if (!rtlhal->fw_ready) {
239 		WARN_ONCE(true,
240 			  "rtl8723ae: error H2C cmd because of Fw download fail!!!\n");
241 		return;
242 	}
243 	memset(tmp_cmdbuf, 0, 8);
244 	memcpy(tmp_cmdbuf, cmdbuffer, cmd_len);
245 	_rtl8723e_fill_h2c_command(hw, element_id, cmd_len,
246 				   (u8 *)&tmp_cmdbuf);
247 }
248 
249 void rtl8723e_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode)
250 {
251 	struct rtl_priv *rtlpriv = rtl_priv(hw);
252 	u8 u1_h2c_set_pwrmode[3] = { 0 };
253 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
254 
255 	RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD, "FW LPS mode = %d\n", mode);
256 
257 	SET_H2CCMD_PWRMODE_PARM_MODE(u1_h2c_set_pwrmode, mode);
258 	SET_H2CCMD_PWRMODE_PARM_SMART_PS(u1_h2c_set_pwrmode,
259 		(rtlpriv->mac80211.p2p) ? ppsc->smart_ps : 1);
260 	SET_H2CCMD_PWRMODE_PARM_BCN_PASS_TIME(u1_h2c_set_pwrmode,
261 					      ppsc->reg_max_lps_awakeintvl);
262 
263 	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
264 		      "rtl8723e_set_fw_rsvdpagepkt(): u1_h2c_set_pwrmode\n",
265 		      u1_h2c_set_pwrmode, 3);
266 	rtl8723e_fill_h2c_cmd(hw, H2C_SETPWRMODE, 3, u1_h2c_set_pwrmode);
267 }
268 
269 #define BEACON_PG		0 /* ->1 */
270 #define PSPOLL_PG		2
271 #define NULL_PG			3
272 #define PROBERSP_PG		4 /* ->5 */
273 
274 #define TOTAL_RESERVED_PKT_LEN	768
275 
276 static u8 reserved_page_packet[TOTAL_RESERVED_PKT_LEN] = {
277 	/* page 0 beacon */
278 	0x80, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0xFF, 0xFF,
279 	0xFF, 0xFF, 0x00, 0xE0, 0x4C, 0x76, 0x00, 0x42,
280 	0x00, 0x40, 0x10, 0x10, 0x00, 0x03, 0x50, 0x08,
281 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
282 	0x64, 0x00, 0x00, 0x04, 0x00, 0x0C, 0x6C, 0x69,
283 	0x6E, 0x6B, 0x73, 0x79, 0x73, 0x5F, 0x77, 0x6C,
284 	0x61, 0x6E, 0x01, 0x04, 0x82, 0x84, 0x8B, 0x96,
285 	0x03, 0x01, 0x01, 0x06, 0x02, 0x00, 0x00, 0x2A,
286 	0x01, 0x00, 0x32, 0x08, 0x24, 0x30, 0x48, 0x6C,
287 	0x0C, 0x12, 0x18, 0x60, 0x2D, 0x1A, 0x6C, 0x18,
288 	0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
289 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
290 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
291 	0x3D, 0x00, 0xDD, 0x06, 0x00, 0xE0, 0x4C, 0x02,
292 	0x01, 0x70, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
293 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
294 
295 	/* page 1 beacon */
296 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
297 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
298 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
299 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
300 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
301 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
302 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
303 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
304 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
305 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
306 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
307 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
308 	0x10, 0x00, 0x20, 0x8C, 0x00, 0x12, 0x10, 0x00,
309 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
310 	0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
311 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
312 
313 	/* page 2  ps-poll */
314 	0xA4, 0x10, 0x01, 0xC0, 0x00, 0x40, 0x10, 0x10,
315 	0x00, 0x03, 0x00, 0xE0, 0x4C, 0x76, 0x00, 0x42,
316 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
317 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
318 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
319 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
320 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
321 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
322 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
323 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
324 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
325 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
326 	0x18, 0x00, 0x20, 0x8C, 0x00, 0x12, 0x00, 0x00,
327 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80,
328 	0x80, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
329 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
330 
331 	/* page 3  null */
332 	0x48, 0x01, 0x00, 0x00, 0x00, 0x40, 0x10, 0x10,
333 	0x00, 0x03, 0x00, 0xE0, 0x4C, 0x76, 0x00, 0x42,
334 	0x00, 0x40, 0x10, 0x10, 0x00, 0x03, 0x00, 0x00,
335 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
336 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
337 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
338 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
339 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
340 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
341 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
342 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
343 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
344 	0x72, 0x00, 0x20, 0x8C, 0x00, 0x12, 0x00, 0x00,
345 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80,
346 	0x80, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
347 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
348 
349 	/* page 4  probe_resp */
350 	0x50, 0x00, 0x00, 0x00, 0x00, 0x40, 0x10, 0x10,
351 	0x00, 0x03, 0x00, 0xE0, 0x4C, 0x76, 0x00, 0x42,
352 	0x00, 0x40, 0x10, 0x10, 0x00, 0x03, 0x00, 0x00,
353 	0x9E, 0x46, 0x15, 0x32, 0x27, 0xF2, 0x2D, 0x00,
354 	0x64, 0x00, 0x00, 0x04, 0x00, 0x0C, 0x6C, 0x69,
355 	0x6E, 0x6B, 0x73, 0x79, 0x73, 0x5F, 0x77, 0x6C,
356 	0x61, 0x6E, 0x01, 0x04, 0x82, 0x84, 0x8B, 0x96,
357 	0x03, 0x01, 0x01, 0x06, 0x02, 0x00, 0x00, 0x2A,
358 	0x01, 0x00, 0x32, 0x08, 0x24, 0x30, 0x48, 0x6C,
359 	0x0C, 0x12, 0x18, 0x60, 0x2D, 0x1A, 0x6C, 0x18,
360 	0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
361 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
362 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
363 	0x3D, 0x00, 0xDD, 0x06, 0x00, 0xE0, 0x4C, 0x02,
364 	0x01, 0x70, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
365 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
366 
367 	/* page 5  probe_resp */
368 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
369 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
370 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
371 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
372 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
373 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
374 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
375 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
376 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
377 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
378 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
379 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
380 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
381 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
382 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
383 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
384 };
385 
386 void rtl8723e_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished)
387 {
388 	struct rtl_priv *rtlpriv = rtl_priv(hw);
389 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
390 	struct sk_buff *skb = NULL;
391 	u32 totalpacketlen;
392 	bool rtstatus;
393 	u8 u1rsvdpageloc[3] = { 0 };
394 	bool b_dlok = false;
395 	u8 *beacon;
396 	u8 *p_pspoll;
397 	u8 *nullfunc;
398 	u8 *p_probersp;
399 
400 	/*---------------------------------------------------------
401 	 *			(1) beacon
402 	 *---------------------------------------------------------
403 	 */
404 	beacon = &reserved_page_packet[BEACON_PG * 128];
405 	SET_80211_HDR_ADDRESS2(beacon, mac->mac_addr);
406 	SET_80211_HDR_ADDRESS3(beacon, mac->bssid);
407 
408 	/*-------------------------------------------------------
409 	 *			(2) ps-poll
410 	 *--------------------------------------------------------
411 	 */
412 	p_pspoll = &reserved_page_packet[PSPOLL_PG * 128];
413 	SET_80211_PS_POLL_AID(p_pspoll, (mac->assoc_id | 0xc000));
414 	SET_80211_PS_POLL_BSSID(p_pspoll, mac->bssid);
415 	SET_80211_PS_POLL_TA(p_pspoll, mac->mac_addr);
416 
417 	SET_H2CCMD_RSVDPAGE_LOC_PSPOLL(u1rsvdpageloc, PSPOLL_PG);
418 
419 	/*--------------------------------------------------------
420 	 *			(3) null data
421 	 *---------------------------------------------------------
422 	 */
423 	nullfunc = &reserved_page_packet[NULL_PG * 128];
424 	SET_80211_HDR_ADDRESS1(nullfunc, mac->bssid);
425 	SET_80211_HDR_ADDRESS2(nullfunc, mac->mac_addr);
426 	SET_80211_HDR_ADDRESS3(nullfunc, mac->bssid);
427 
428 	SET_H2CCMD_RSVDPAGE_LOC_NULL_DATA(u1rsvdpageloc, NULL_PG);
429 
430 	/*---------------------------------------------------------
431 	 *			(4) probe response
432 	 *----------------------------------------------------------
433 	 */
434 	p_probersp = &reserved_page_packet[PROBERSP_PG * 128];
435 	SET_80211_HDR_ADDRESS1(p_probersp, mac->bssid);
436 	SET_80211_HDR_ADDRESS2(p_probersp, mac->mac_addr);
437 	SET_80211_HDR_ADDRESS3(p_probersp, mac->bssid);
438 
439 	SET_H2CCMD_RSVDPAGE_LOC_PROBE_RSP(u1rsvdpageloc, PROBERSP_PG);
440 
441 	totalpacketlen = TOTAL_RESERVED_PKT_LEN;
442 
443 	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_LOUD,
444 		      "rtl8723e_set_fw_rsvdpagepkt(): HW_VAR_SET_TX_CMD: ALL\n",
445 		      &reserved_page_packet[0], totalpacketlen);
446 	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
447 		      "rtl8723e_set_fw_rsvdpagepkt(): HW_VAR_SET_TX_CMD: ALL\n",
448 		      u1rsvdpageloc, 3);
449 
450 	skb = dev_alloc_skb(totalpacketlen);
451 	skb_put_data(skb, &reserved_page_packet, totalpacketlen);
452 
453 	rtstatus = rtl_cmd_send_packet(hw, skb);
454 
455 	if (rtstatus)
456 		b_dlok = true;
457 
458 	if (b_dlok) {
459 		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
460 			 "Set RSVD page location to Fw.\n");
461 		RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
462 			      "H2C_RSVDPAGE:\n",
463 			      u1rsvdpageloc, 3);
464 		rtl8723e_fill_h2c_cmd(hw, H2C_RSVDPAGE,
465 				      sizeof(u1rsvdpageloc), u1rsvdpageloc);
466 	} else
467 		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
468 			 "Set RSVD page location to Fw FAIL!!!!!!.\n");
469 }
470 
471 void rtl8723e_set_fw_joinbss_report_cmd(struct ieee80211_hw *hw, u8 mstatus)
472 {
473 	u8 u1_joinbssrpt_parm[1] = { 0 };
474 
475 	SET_H2CCMD_JOINBSSRPT_PARM_OPMODE(u1_joinbssrpt_parm, mstatus);
476 
477 	rtl8723e_fill_h2c_cmd(hw, H2C_JOINBSSRPT, 1, u1_joinbssrpt_parm);
478 }
479 
480 static void rtl8723e_set_p2p_ctw_period_cmd(struct ieee80211_hw *hw,
481 					    u8 ctwindow)
482 {
483 	u8 u1_ctwindow_period[1] = { ctwindow};
484 
485 	rtl8723e_fill_h2c_cmd(hw, H2C_P2P_PS_CTW_CMD, 1, u1_ctwindow_period);
486 
487 }
488 
489 void rtl8723e_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
490 {
491 	struct rtl_priv *rtlpriv = rtl_priv(hw);
492 	struct rtl_ps_ctl *rtlps = rtl_psc(rtl_priv(hw));
493 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
494 	struct rtl_p2p_ps_info *p2pinfo = &(rtlps->p2p_ps_info);
495 	struct p2p_ps_offload_t *p2p_ps_offload = &rtlhal->p2p_ps_offload;
496 	u8	i;
497 	u16	ctwindow;
498 	u32	start_time, tsf_low;
499 
500 	switch (p2p_ps_state) {
501 	case P2P_PS_DISABLE:
502 		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_DISABLE\n");
503 		memset(p2p_ps_offload, 0, sizeof(*p2p_ps_offload));
504 		break;
505 	case P2P_PS_ENABLE:
506 		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_ENABLE\n");
507 		/* update CTWindow value. */
508 		if (p2pinfo->ctwindow > 0) {
509 			p2p_ps_offload->ctwindow_en = 1;
510 			ctwindow = p2pinfo->ctwindow;
511 			rtl8723e_set_p2p_ctw_period_cmd(hw, ctwindow);
512 		}
513 
514 		/* hw only support 2 set of NoA */
515 		for (i = 0 ; i < p2pinfo->noa_num ; i++) {
516 			/* To control the register setting for which NOA*/
517 			rtl_write_byte(rtlpriv, 0x5cf, (i << 4));
518 			if (i == 0)
519 				p2p_ps_offload->noa0_en = 1;
520 			else
521 				p2p_ps_offload->noa1_en = 1;
522 
523 			/* config P2P NoA Descriptor Register */
524 			rtl_write_dword(rtlpriv, 0x5E0,
525 					p2pinfo->noa_duration[i]);
526 			rtl_write_dword(rtlpriv, 0x5E4,
527 					p2pinfo->noa_interval[i]);
528 
529 			/*Get Current TSF value */
530 			tsf_low = rtl_read_dword(rtlpriv, REG_TSFTR);
531 
532 			start_time = p2pinfo->noa_start_time[i];
533 			if (p2pinfo->noa_count_type[i] != 1) {
534 				while (start_time <=
535 					(tsf_low+(50*1024))) {
536 					start_time +=
537 						p2pinfo->noa_interval[i];
538 					if (p2pinfo->noa_count_type[i] != 255)
539 						p2pinfo->noa_count_type[i]--;
540 				}
541 			}
542 			rtl_write_dword(rtlpriv, 0x5E8, start_time);
543 			rtl_write_dword(rtlpriv, 0x5EC,
544 				p2pinfo->noa_count_type[i]);
545 
546 		}
547 
548 		if ((p2pinfo->opp_ps == 1) || (p2pinfo->noa_num > 0)) {
549 			/* rst p2p circuit */
550 			rtl_write_byte(rtlpriv, REG_DUAL_TSF_RST, BIT(4));
551 
552 			p2p_ps_offload->offload_en = 1;
553 
554 			if (P2P_ROLE_GO == rtlpriv->mac80211.p2p) {
555 				p2p_ps_offload->role = 1;
556 				p2p_ps_offload->allstasleep = 0;
557 			} else {
558 				p2p_ps_offload->role = 0;
559 			}
560 
561 			p2p_ps_offload->discovery = 0;
562 		}
563 		break;
564 	case P2P_PS_SCAN:
565 		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_SCAN\n");
566 		p2p_ps_offload->discovery = 1;
567 		break;
568 	case P2P_PS_SCAN_DONE:
569 		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_SCAN_DONE\n");
570 		p2p_ps_offload->discovery = 0;
571 		p2pinfo->p2p_ps_state = P2P_PS_ENABLE;
572 		break;
573 	default:
574 		break;
575 	}
576 
577 	rtl8723e_fill_h2c_cmd(hw, H2C_P2P_PS_OFFLOAD, 1, (u8 *)p2p_ps_offload);
578 
579 }
580