1 // SPDX-License-Identifier: GPL-2.0
2 /* Copyright(c) 2009-2014  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 _rtl8723be_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;
18 	bool result = false;
19 
20 	val_hmetfr = rtl_read_byte(rtlpriv, REG_HMETFR);
21 	if (((val_hmetfr >> boxnum) & BIT(0)) == 0)
22 		result = true;
23 	return result;
24 }
25 
26 static void _rtl8723be_fill_h2c_command(struct ieee80211_hw *hw, u8 element_id,
27 					u32 cmd_len, u8 *p_cmdbuffer)
28 {
29 	struct rtl_priv *rtlpriv = rtl_priv(hw);
30 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
31 	u8 boxnum;
32 	u16 box_reg = 0, box_extreg = 0;
33 	u8 u1b_tmp;
34 	bool isfw_read = false;
35 	u8 buf_index = 0;
36 	bool bwrite_sucess = false;
37 	u8 wait_h2c_limmit = 100;
38 	u8 wait_writeh2c_limmit = 100;
39 	u8 boxcontent[4], boxextcontent[4];
40 	u32 h2c_waitcounter = 0;
41 	unsigned long flag;
42 	u8 idx;
43 
44 	RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD, "come in\n");
45 
46 	while (true) {
47 		spin_lock_irqsave(&rtlpriv->locks.h2c_lock, flag);
48 		if (rtlhal->h2c_setinprogress) {
49 			RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
50 				 "H2C set in progress! Wait to set..element_id(%d).\n",
51 				 element_id);
52 
53 			while (rtlhal->h2c_setinprogress) {
54 				spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock,
55 						       flag);
56 				h2c_waitcounter++;
57 				RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
58 					 "Wait 100 us (%d times)...\n",
59 					 h2c_waitcounter);
60 				udelay(100);
61 
62 				if (h2c_waitcounter > 1000)
63 					return;
64 				spin_lock_irqsave(&rtlpriv->locks.h2c_lock,
65 						  flag);
66 			}
67 			spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
68 		} else {
69 			rtlhal->h2c_setinprogress = true;
70 			spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
71 			break;
72 		}
73 	}
74 
75 	while (!bwrite_sucess) {
76 		wait_writeh2c_limmit--;
77 		if (wait_writeh2c_limmit == 0) {
78 			pr_err("Write H2C fail because no trigger for FW INT!\n");
79 			break;
80 		}
81 
82 		boxnum = rtlhal->last_hmeboxnum;
83 		switch (boxnum) {
84 		case 0:
85 			box_reg = REG_HMEBOX_0;
86 			box_extreg = REG_HMEBOX_EXT_0;
87 			break;
88 		case 1:
89 			box_reg = REG_HMEBOX_1;
90 			box_extreg = REG_HMEBOX_EXT_1;
91 			break;
92 		case 2:
93 			box_reg = REG_HMEBOX_2;
94 			box_extreg = REG_HMEBOX_EXT_2;
95 			break;
96 		case 3:
97 			box_reg = REG_HMEBOX_3;
98 			box_extreg = REG_HMEBOX_EXT_3;
99 			break;
100 		default:
101 			pr_err("switch case %#x not processed\n",
102 			       boxnum);
103 			break;
104 		}
105 
106 		isfw_read = _rtl8723be_check_fw_read_last_h2c(hw, boxnum);
107 		while (!isfw_read) {
108 			wait_h2c_limmit--;
109 			if (wait_h2c_limmit == 0) {
110 				RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
111 					 "Waiting too long for FW read clear HMEBox(%d)!\n",
112 					 boxnum);
113 				break;
114 			}
115 
116 			udelay(10);
117 
118 			isfw_read = _rtl8723be_check_fw_read_last_h2c(hw,
119 								boxnum);
120 			u1b_tmp = rtl_read_byte(rtlpriv, 0x130);
121 			RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
122 				 "Waiting for FW read clear HMEBox(%d)!!! 0x130 = %2x\n",
123 				 boxnum, u1b_tmp);
124 		}
125 
126 		if (!isfw_read) {
127 			RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
128 				 "Write H2C register BOX[%d] fail!!!!! Fw do not read.\n",
129 				 boxnum);
130 			break;
131 		}
132 
133 		memset(boxcontent, 0, sizeof(boxcontent));
134 		memset(boxextcontent, 0, sizeof(boxextcontent));
135 		boxcontent[0] = element_id;
136 		RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
137 			 "Write element_id box_reg(%4x) = %2x\n",
138 			  box_reg, element_id);
139 
140 		switch (cmd_len) {
141 		case 1:
142 		case 2:
143 		case 3:
144 			/*boxcontent[0] &= ~(BIT(7));*/
145 			memcpy((u8 *)(boxcontent) + 1,
146 			       p_cmdbuffer + buf_index, cmd_len);
147 
148 			for (idx = 0; idx < 4; idx++) {
149 				rtl_write_byte(rtlpriv, box_reg + idx,
150 					       boxcontent[idx]);
151 			}
152 			break;
153 		case 4:
154 		case 5:
155 		case 6:
156 		case 7:
157 			/*boxcontent[0] |= (BIT(7));*/
158 			memcpy((u8 *)(boxextcontent),
159 			       p_cmdbuffer + buf_index+3, cmd_len-3);
160 			memcpy((u8 *)(boxcontent) + 1,
161 			       p_cmdbuffer + buf_index, 3);
162 
163 			for (idx = 0; idx < 4; idx++) {
164 				rtl_write_byte(rtlpriv, box_extreg + idx,
165 					       boxextcontent[idx]);
166 			}
167 
168 			for (idx = 0; idx < 4; idx++) {
169 				rtl_write_byte(rtlpriv, box_reg + idx,
170 					       boxcontent[idx]);
171 			}
172 			break;
173 		default:
174 			pr_err("switch case %#x not processed\n",
175 			       cmd_len);
176 			break;
177 		}
178 
179 		bwrite_sucess = true;
180 
181 		rtlhal->last_hmeboxnum = boxnum + 1;
182 		if (rtlhal->last_hmeboxnum == 4)
183 			rtlhal->last_hmeboxnum = 0;
184 
185 		RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
186 			 "pHalData->last_hmeboxnum  = %d\n",
187 			  rtlhal->last_hmeboxnum);
188 	}
189 
190 	spin_lock_irqsave(&rtlpriv->locks.h2c_lock, flag);
191 	rtlhal->h2c_setinprogress = false;
192 	spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
193 
194 	RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD, "go out\n");
195 }
196 
197 void rtl8723be_fill_h2c_cmd(struct ieee80211_hw *hw, u8 element_id,
198 			    u32 cmd_len, u8 *p_cmdbuffer)
199 {
200 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
201 	u32 tmp_cmdbuf[2];
202 
203 	if (!rtlhal->fw_ready) {
204 		WARN_ONCE(true,
205 			  "rtl8723be: error H2C cmd because of Fw download fail!!!\n");
206 		return;
207 	}
208 
209 	memset(tmp_cmdbuf, 0, 8);
210 	memcpy(tmp_cmdbuf, p_cmdbuffer, cmd_len);
211 	_rtl8723be_fill_h2c_command(hw, element_id, cmd_len,
212 				    (u8 *)&tmp_cmdbuf);
213 	return;
214 }
215 
216 void rtl8723be_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode)
217 {
218 	struct rtl_priv *rtlpriv = rtl_priv(hw);
219 	u8 u1_h2c_set_pwrmode[H2C_PWEMODE_LENGTH] = { 0 };
220 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
221 	u8 rlbm, power_state = 0, byte5 = 0;
222 	u8 awake_intvl;	/* DTIM = (awake_intvl - 1) */
223 	struct rtl_btc_ops *btc_ops = rtlpriv->btcoexist.btc_ops;
224 	bool bt_ctrl_lps = (rtlpriv->cfg->ops->get_btc_status() ?
225 			    btc_ops->btc_is_bt_ctrl_lps(rtlpriv) : false);
226 	bool bt_lps_on = (rtlpriv->cfg->ops->get_btc_status() ?
227 			  btc_ops->btc_is_bt_lps_on(rtlpriv) : false);
228 
229 	if (bt_ctrl_lps)
230 		mode = (bt_lps_on ? FW_PS_MIN_MODE : FW_PS_ACTIVE_MODE);
231 
232 	RT_TRACE(rtlpriv, COMP_POWER, DBG_DMESG, "FW LPS mode = %d (coex:%d)\n",
233 		 mode, bt_ctrl_lps);
234 
235 	switch (mode) {
236 	case FW_PS_MIN_MODE:
237 		rlbm = 0;
238 		awake_intvl = 2;
239 		break;
240 	case FW_PS_MAX_MODE:
241 		rlbm = 1;
242 		awake_intvl = 2;
243 		break;
244 	case FW_PS_DTIM_MODE:
245 		rlbm = 2;
246 		awake_intvl = ppsc->reg_max_lps_awakeintvl;
247 		/* hw->conf.ps_dtim_period or mac->vif->bss_conf.dtim_period
248 		 * is only used in swlps.
249 		 */
250 		break;
251 	default:
252 		rlbm = 2;
253 		awake_intvl = 4;
254 		break;
255 	}
256 
257 	if (rtlpriv->mac80211.p2p) {
258 		awake_intvl = 2;
259 		rlbm = 1;
260 	}
261 
262 	if (mode == FW_PS_ACTIVE_MODE) {
263 		byte5 = 0x40;
264 		power_state = FW_PWR_STATE_ACTIVE;
265 	} else {
266 		if (bt_ctrl_lps) {
267 			byte5 = btc_ops->btc_get_lps_val(rtlpriv);
268 			power_state = btc_ops->btc_get_rpwm_val(rtlpriv);
269 
270 			if ((rlbm == 2) && (byte5 & BIT(4))) {
271 				/* Keep awake interval to 1 to prevent from
272 				 * decreasing coex performance
273 				 */
274 				awake_intvl = 2;
275 				rlbm = 2;
276 			}
277 		} else {
278 			byte5 = 0x40;
279 			power_state = FW_PWR_STATE_RF_OFF;
280 		}
281 	}
282 
283 	SET_H2CCMD_PWRMODE_PARM_MODE(u1_h2c_set_pwrmode, ((mode) ? 1 : 0));
284 	SET_H2CCMD_PWRMODE_PARM_RLBM(u1_h2c_set_pwrmode, rlbm);
285 	SET_H2CCMD_PWRMODE_PARM_SMART_PS(u1_h2c_set_pwrmode,
286 					 bt_ctrl_lps ? 0 : ppsc->smart_ps);
287 	SET_H2CCMD_PWRMODE_PARM_AWAKE_INTERVAL(u1_h2c_set_pwrmode,
288 					       awake_intvl);
289 	SET_H2CCMD_PWRMODE_PARM_ALL_QUEUE_UAPSD(u1_h2c_set_pwrmode, 0);
290 	SET_H2CCMD_PWRMODE_PARM_PWR_STATE(u1_h2c_set_pwrmode, power_state);
291 	SET_H2CCMD_PWRMODE_PARM_BYTE5(u1_h2c_set_pwrmode, byte5);
292 
293 	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
294 		      "rtl92c_set_fw_pwrmode(): u1_h2c_set_pwrmode\n",
295 		      u1_h2c_set_pwrmode, H2C_PWEMODE_LENGTH);
296 	if (rtlpriv->cfg->ops->get_btc_status())
297 		btc_ops->btc_record_pwr_mode(rtlpriv, u1_h2c_set_pwrmode,
298 					     H2C_PWEMODE_LENGTH);
299 	rtl8723be_fill_h2c_cmd(hw, H2C_8723B_SETPWRMODE, H2C_PWEMODE_LENGTH,
300 			       u1_h2c_set_pwrmode);
301 }
302 
303 void rtl8723be_set_fw_media_status_rpt_cmd(struct ieee80211_hw *hw, u8 mstatus)
304 {
305 	u8 parm[3] = { 0, 0, 0 };
306 	/* parm[0]: bit0=0-->Disconnect, bit0=1-->Connect
307 	 *          bit1=0-->update Media Status to MACID
308 	 *          bit1=1-->update Media Status from MACID to MACID_End
309 	 * parm[1]: MACID, if this is INFRA_STA, MacID = 0
310 	 * parm[2]: MACID_End
311 	*/
312 	SET_H2CCMD_MSRRPT_PARM_OPMODE(parm, mstatus);
313 	SET_H2CCMD_MSRRPT_PARM_MACID_IND(parm, 0);
314 
315 	rtl8723be_fill_h2c_cmd(hw, H2C_8723B_MSRRPT, 3, parm);
316 }
317 
318 #define BEACON_PG		0 /* ->1 */
319 #define PSPOLL_PG		2
320 #define NULL_PG			3
321 #define PROBERSP_PG		4 /* ->5 */
322 #define QOS_NULL_PG		6
323 #define BT_QOS_NULL_PG	7
324 
325 #define TOTAL_RESERVED_PKT_LEN	1024	/* can be up to 1280 (tx_bndy=245) */
326 
327 static u8 reserved_page_packet[TOTAL_RESERVED_PKT_LEN] = {
328 	/* page 0 beacon */
329 	0x80, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0xFF, 0xFF,
330 	0xFF, 0xFF, 0x00, 0xE0, 0x4C, 0x02, 0xB1, 0x78,
331 	0xEC, 0x1A, 0x59, 0x0B, 0xAD, 0xD4, 0x20, 0x00,
332 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
333 	0x64, 0x00, 0x10, 0x04, 0x00, 0x05, 0x54, 0x65,
334 	0x73, 0x74, 0x32, 0x01, 0x08, 0x82, 0x84, 0x0B,
335 	0x16, 0x24, 0x30, 0x48, 0x6C, 0x03, 0x01, 0x06,
336 	0x06, 0x02, 0x00, 0x00, 0x2A, 0x01, 0x02, 0x32,
337 	0x04, 0x0C, 0x12, 0x18, 0x60, 0x2D, 0x1A, 0x6C,
338 	0x09, 0x03, 0xFF, 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, 0x3D, 0x00, 0xDD, 0x07, 0x00, 0xE0, 0x4C,
342 	0x02, 0x02, 0x00, 0x00, 0xDD, 0x18, 0x00, 0x50,
343 	0xF2, 0x01, 0x01, 0x00, 0x00, 0x50, 0xF2, 0x04,
344 	0x01, 0x00, 0x00, 0x50, 0xF2, 0x04, 0x01, 0x00,
345 
346 	/* page 1 beacon */
347 	0x00, 0x50, 0xF2, 0x02, 0x00, 0x00, 0x00, 0x00,
348 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
349 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
350 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
351 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
352 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
353 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
354 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
355 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
356 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
357 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
358 	0x10, 0x00, 0x28, 0x8C, 0x00, 0x12, 0x00, 0x00,
359 	0x00, 0x00, 0x00, 0x00, 0x00, 0x81, 0x00, 0x00,
360 	0x00, 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 
364 	/* page 2  ps-poll */
365 	0xA4, 0x10, 0x01, 0xC0, 0xEC, 0x1A, 0x59, 0x0B,
366 	0xAD, 0xD4, 0x00, 0xE0, 0x4C, 0x02, 0xB1, 0x78,
367 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
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 	0x18, 0x00, 0x28, 0x8C, 0x00, 0x12, 0x00, 0x00,
377 	0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00,
378 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
379 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
380 	0x00, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
381 
382 	/* page 3  null */
383 	0x48, 0x01, 0x00, 0x00, 0xEC, 0x1A, 0x59, 0x0B,
384 	0xAD, 0xD4, 0x00, 0xE0, 0x4C, 0x02, 0xB1, 0x78,
385 	0xEC, 0x1A, 0x59, 0x0B, 0xAD, 0xD4, 0x00, 0x00,
386 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
387 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
388 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
389 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
390 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
391 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
392 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
393 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
394 	0x72, 0x00, 0x28, 0x8C, 0x00, 0x12, 0x00, 0x00,
395 	0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00,
396 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
397 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
398 	0x00, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
399 
400 	/* page 4  probe_resp */
401 	0x50, 0x00, 0x00, 0x00, 0x00, 0x40, 0x10, 0x10,
402 	0x00, 0x03, 0x00, 0xE0, 0x4C, 0x76, 0x00, 0x42,
403 	0x00, 0x40, 0x10, 0x10, 0x00, 0x03, 0x00, 0x00,
404 	0x9E, 0x46, 0x15, 0x32, 0x27, 0xF2, 0x2D, 0x00,
405 	0x64, 0x00, 0x00, 0x04, 0x00, 0x0C, 0x6C, 0x69,
406 	0x6E, 0x6B, 0x73, 0x79, 0x73, 0x5F, 0x77, 0x6C,
407 	0x61, 0x6E, 0x01, 0x04, 0x82, 0x84, 0x8B, 0x96,
408 	0x03, 0x01, 0x01, 0x06, 0x02, 0x00, 0x00, 0x2A,
409 	0x01, 0x00, 0x32, 0x08, 0x24, 0x30, 0x48, 0x6C,
410 	0x0C, 0x12, 0x18, 0x60, 0x2D, 0x1A, 0x6C, 0x18,
411 	0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
412 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
413 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
414 	0x3D, 0x00, 0xDD, 0x06, 0x00, 0xE0, 0x4C, 0x02,
415 	0x01, 0x70, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
416 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
417 
418 	/* page 5  probe_resp */
419 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
420 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
421 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
422 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
423 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
424 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
425 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
426 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
427 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
428 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
429 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
430 	0x1A, 0x00, 0x28, 0x8C, 0x00, 0x12, 0x00, 0x00,
431 	0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00,
432 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
433 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
434 	0x00, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
435 
436 	/* page 6 qos null data */
437 	0xC8, 0x01, 0x00, 0x00, 0x84, 0xC9, 0xB2, 0xA7,
438 	0xB3, 0x6E, 0x00, 0xE0, 0x4C, 0x02, 0x51, 0x02,
439 	0x84, 0xC9, 0xB2, 0xA7, 0xB3, 0x6E, 0x00, 0x00,
440 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
441 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
442 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
443 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
444 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
445 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
446 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
447 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
448 	0x1A, 0x00, 0x28, 0x8C, 0x00, 0x12, 0x00, 0x00,
449 	0x00, 0x00, 0x80, 0x00, 0x00, 0x01, 0x00, 0x00,
450 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
451 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
452 	0x00, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
453 
454 	/* page 7 BT-qos null data */
455 	0xC8, 0x01, 0x00, 0x00, 0x84, 0xC9, 0xB2, 0xA7,
456 	0xB3, 0x6E, 0x00, 0xE0, 0x4C, 0x02, 0x51, 0x02,
457 	0x84, 0xC9, 0xB2, 0xA7, 0xB3, 0x6E, 0x00, 0x00,
458 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
459 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
460 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
461 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
462 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
463 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
464 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
465 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
466 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
467 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
468 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
469 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
470 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
471 
472 };
473 
474 void rtl8723be_set_fw_rsvdpagepkt(struct ieee80211_hw *hw,
475 				  bool b_dl_finished)
476 {
477 	struct rtl_priv *rtlpriv = rtl_priv(hw);
478 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
479 	struct sk_buff *skb = NULL;
480 
481 	u32 totalpacketlen;
482 	bool rtstatus;
483 	u8 u1rsvdpageloc[5] = { 0 };
484 	bool b_dlok = false;
485 
486 	u8 *beacon;
487 	u8 *p_pspoll;
488 	u8 *nullfunc;
489 	u8 *p_probersp;
490 	u8 *qosnull;
491 	u8 *btqosnull;
492 	/*---------------------------------------------------------
493 	 *			(1) beacon
494 	 *---------------------------------------------------------
495 	 */
496 	beacon = &reserved_page_packet[BEACON_PG * 128];
497 	SET_80211_HDR_ADDRESS2(beacon, mac->mac_addr);
498 	SET_80211_HDR_ADDRESS3(beacon, mac->bssid);
499 
500 	/*-------------------------------------------------------
501 	 *			(2) ps-poll
502 	 *-------------------------------------------------------
503 	 */
504 	p_pspoll = &reserved_page_packet[PSPOLL_PG * 128];
505 	SET_80211_PS_POLL_AID(p_pspoll, (mac->assoc_id | 0xc000));
506 	SET_80211_PS_POLL_BSSID(p_pspoll, mac->bssid);
507 	SET_80211_PS_POLL_TA(p_pspoll, mac->mac_addr);
508 
509 	SET_H2CCMD_RSVDPAGE_LOC_PSPOLL(u1rsvdpageloc, PSPOLL_PG);
510 
511 	/*--------------------------------------------------------
512 	 *			(3) null data
513 	 *--------------------------------------------------------
514 	 */
515 	nullfunc = &reserved_page_packet[NULL_PG * 128];
516 	SET_80211_HDR_ADDRESS1(nullfunc, mac->bssid);
517 	SET_80211_HDR_ADDRESS2(nullfunc, mac->mac_addr);
518 	SET_80211_HDR_ADDRESS3(nullfunc, mac->bssid);
519 
520 	SET_H2CCMD_RSVDPAGE_LOC_NULL_DATA(u1rsvdpageloc, NULL_PG);
521 
522 	/*---------------------------------------------------------
523 	 *			(4) probe response
524 	 *---------------------------------------------------------
525 	 */
526 	p_probersp = &reserved_page_packet[PROBERSP_PG * 128];
527 	SET_80211_HDR_ADDRESS1(p_probersp, mac->bssid);
528 	SET_80211_HDR_ADDRESS2(p_probersp, mac->mac_addr);
529 	SET_80211_HDR_ADDRESS3(p_probersp, mac->bssid);
530 
531 	SET_H2CCMD_RSVDPAGE_LOC_PROBE_RSP(u1rsvdpageloc, PROBERSP_PG);
532 
533 	/*---------------------------------------------------------
534 	 *			(5) QoS Null
535 	 *---------------------------------------------------------
536 	 */
537 	qosnull = &reserved_page_packet[QOS_NULL_PG * 128];
538 	SET_80211_HDR_ADDRESS1(qosnull, mac->bssid);
539 	SET_80211_HDR_ADDRESS2(qosnull, mac->mac_addr);
540 	SET_80211_HDR_ADDRESS3(qosnull, mac->bssid);
541 
542 	SET_H2CCMD_RSVDPAGE_LOC_QOS_NULL_DATA(u1rsvdpageloc, QOS_NULL_PG);
543 
544 	/*---------------------------------------------------------
545 	 *			(5) QoS Null
546 	 *---------------------------------------------------------
547 	 */
548 	btqosnull = &reserved_page_packet[BT_QOS_NULL_PG * 128];
549 	SET_80211_HDR_ADDRESS1(btqosnull, mac->bssid);
550 	SET_80211_HDR_ADDRESS2(btqosnull, mac->mac_addr);
551 	SET_80211_HDR_ADDRESS3(btqosnull, mac->bssid);
552 
553 	SET_H2CCMD_RSVDPAGE_LOC_BT_QOS_NULL_DATA(u1rsvdpageloc, BT_QOS_NULL_PG);
554 
555 	totalpacketlen = TOTAL_RESERVED_PKT_LEN;
556 
557 	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_LOUD,
558 		      "rtl8723be_set_fw_rsvdpagepkt(): HW_VAR_SET_TX_CMD: ALL\n",
559 		      &reserved_page_packet[0], totalpacketlen);
560 	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
561 		      "rtl8723be_set_fw_rsvdpagepkt(): HW_VAR_SET_TX_CMD: ALL\n",
562 		      u1rsvdpageloc, sizeof(u1rsvdpageloc));
563 
564 	skb = dev_alloc_skb(totalpacketlen);
565 	skb_put_data(skb, &reserved_page_packet, totalpacketlen);
566 
567 	rtstatus = rtl_cmd_send_packet(hw, skb);
568 
569 	if (rtstatus)
570 		b_dlok = true;
571 
572 	if (b_dlok) {
573 		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
574 			 "Set RSVD page location to Fw.\n");
575 		RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG, "H2C_RSVDPAGE:\n",
576 			      u1rsvdpageloc, sizeof(u1rsvdpageloc));
577 		rtl8723be_fill_h2c_cmd(hw, H2C_8723B_RSVDPAGE,
578 				       sizeof(u1rsvdpageloc), u1rsvdpageloc);
579 	} else
580 		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
581 			 "Set RSVD page location to Fw FAIL!!!!!!.\n");
582 }
583 
584 /*Should check FW support p2p or not.*/
585 static void rtl8723be_set_p2p_ctw_period_cmd(struct ieee80211_hw *hw,
586 					     u8 ctwindow)
587 {
588 	u8 u1_ctwindow_period[1] = { ctwindow};
589 
590 	rtl8723be_fill_h2c_cmd(hw, H2C_8723B_P2P_PS_CTW_CMD, 1,
591 			       u1_ctwindow_period);
592 }
593 
594 void rtl8723be_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw,
595 				      u8 p2p_ps_state)
596 {
597 	struct rtl_priv *rtlpriv = rtl_priv(hw);
598 	struct rtl_ps_ctl *rtlps = rtl_psc(rtl_priv(hw));
599 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
600 	struct rtl_p2p_ps_info *p2pinfo = &(rtlps->p2p_ps_info);
601 	struct p2p_ps_offload_t *p2p_ps_offload = &rtlhal->p2p_ps_offload;
602 	u8 i;
603 	u16 ctwindow;
604 	u32 start_time, tsf_low;
605 
606 	switch (p2p_ps_state) {
607 	case P2P_PS_DISABLE:
608 		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_DISABLE\n");
609 		memset(p2p_ps_offload, 0, sizeof(*p2p_ps_offload));
610 		break;
611 	case P2P_PS_ENABLE:
612 		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_ENABLE\n");
613 		/* update CTWindow value. */
614 		if (p2pinfo->ctwindow > 0) {
615 			p2p_ps_offload->ctwindow_en = 1;
616 			ctwindow = p2pinfo->ctwindow;
617 			rtl8723be_set_p2p_ctw_period_cmd(hw, ctwindow);
618 		}
619 		/* hw only support 2 set of NoA */
620 		for (i = 0 ; i < p2pinfo->noa_num ; i++) {
621 			/* To control the register setting
622 			 * for which NOA
623 			 */
624 			rtl_write_byte(rtlpriv, 0x5cf, (i << 4));
625 			if (i == 0)
626 				p2p_ps_offload->noa0_en = 1;
627 			else
628 				p2p_ps_offload->noa1_en = 1;
629 
630 			/* config P2P NoA Descriptor Register */
631 			rtl_write_dword(rtlpriv, 0x5E0,
632 					p2pinfo->noa_duration[i]);
633 			rtl_write_dword(rtlpriv, 0x5E4,
634 					p2pinfo->noa_interval[i]);
635 
636 			/*Get Current TSF value */
637 			tsf_low = rtl_read_dword(rtlpriv, REG_TSFTR);
638 
639 			start_time = p2pinfo->noa_start_time[i];
640 			if (p2pinfo->noa_count_type[i] != 1) {
641 				while (start_time <= (tsf_low + (50 * 1024))) {
642 					start_time += p2pinfo->noa_interval[i];
643 					if (p2pinfo->noa_count_type[i] != 255)
644 						p2pinfo->noa_count_type[i]--;
645 				}
646 			}
647 			rtl_write_dword(rtlpriv, 0x5E8, start_time);
648 			rtl_write_dword(rtlpriv, 0x5EC,
649 					p2pinfo->noa_count_type[i]);
650 		}
651 
652 		if ((p2pinfo->opp_ps == 1) ||
653 		    (p2pinfo->noa_num > 0)) {
654 			/* rst p2p circuit */
655 			rtl_write_byte(rtlpriv, REG_DUAL_TSF_RST, BIT(4));
656 
657 			p2p_ps_offload->offload_en = 1;
658 
659 			if (P2P_ROLE_GO == rtlpriv->mac80211.p2p) {
660 				p2p_ps_offload->role = 1;
661 				p2p_ps_offload->allstasleep = 0;
662 			} else {
663 				p2p_ps_offload->role = 0;
664 			}
665 			p2p_ps_offload->discovery = 0;
666 		}
667 		break;
668 	case P2P_PS_SCAN:
669 		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_SCAN\n");
670 		p2p_ps_offload->discovery = 1;
671 		break;
672 	case P2P_PS_SCAN_DONE:
673 		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_SCAN_DONE\n");
674 		p2p_ps_offload->discovery = 0;
675 		p2pinfo->p2p_ps_state = P2P_PS_ENABLE;
676 		break;
677 	default:
678 		break;
679 	}
680 
681 	rtl8723be_fill_h2c_cmd(hw, H2C_8723B_P2P_PS_OFFLOAD, 1,
682 			       (u8 *)p2p_ps_offload);
683 }
684